wrap_python_robot_interface.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2013, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Ioan Sucan */
00036 
00037 #include <moveit/common_planning_interface_objects/common_objects.h>
00038 #include <moveit/robot_state/conversions.h>
00039 #include <moveit/py_bindings_tools/roscpp_initializer.h>
00040 #include <moveit/py_bindings_tools/py_conversions.h>
00041 #include <moveit/py_bindings_tools/serialize_msg.h>
00042 #include <moveit_msgs/RobotState.h>
00043 
00044 #include <stdexcept>
00045 #include <boost/python.hpp>
00046 #include <Python.h>
00047 
00050 namespace bp = boost::python;
00051 
00052 namespace moveit
00053 {
00054 class RobotInterfacePython : protected py_bindings_tools::ROScppInitializer
00055 {
00056 public:
00057   RobotInterfacePython(const std::string& robot_description) : py_bindings_tools::ROScppInitializer()
00058   {
00059     robot_model_ = planning_interface::getSharedRobotModel(robot_description);
00060     if (!robot_model_)
00061       throw std::runtime_error("RobotInterfacePython: invalid robot model");
00062     current_state_monitor_ = planning_interface::getSharedStateMonitor(robot_model_, planning_interface::getSharedTF());
00063   }
00064 
00065   const char* getRobotName() const
00066   {
00067     return robot_model_->getName().c_str();
00068   }
00069 
00070   bp::list getJointNames() const
00071   {
00072     return py_bindings_tools::listFromString(robot_model_->getJointModelNames());
00073   }
00074 
00075   bp::list getGroupJointNames(const std::string& group) const
00076   {
00077     const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(group);
00078     if (jmg)
00079       return py_bindings_tools::listFromString(jmg->getJointModelNames());
00080     else
00081       return bp::list();
00082   }
00083 
00084   bp::list getGroupJointTips(const std::string& group) const
00085   {
00086     const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(group);
00087     if (jmg)
00088     {
00089       std::vector<std::string> tips;
00090       jmg->getEndEffectorTips(tips);
00091       return py_bindings_tools::listFromString(tips);
00092     }
00093     else
00094       return bp::list();
00095   }
00096 
00097   bp::list getLinkNames() const
00098   {
00099     return py_bindings_tools::listFromString(robot_model_->getLinkModelNames());
00100   }
00101 
00102   bp::list getGroupLinkNames(const std::string& group) const
00103   {
00104     const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(group);
00105     if (jmg)
00106       return py_bindings_tools::listFromString(jmg->getLinkModelNames());
00107     else
00108       return bp::list();
00109   }
00110 
00111   bp::list getGroupNames() const
00112   {
00113     return py_bindings_tools::listFromString(robot_model_->getJointModelGroupNames());
00114   }
00115 
00116   bp::list getJointLimits(const std::string& name) const
00117   {
00118     bp::list result;
00119     const robot_model::JointModel* jm = robot_model_->getJointModel(name);
00120     if (jm)
00121     {
00122       const std::vector<moveit_msgs::JointLimits>& lim = jm->getVariableBoundsMsg();
00123       for (std::size_t i = 0; i < lim.size(); ++i)
00124       {
00125         bp::list l;
00126         l.append(lim[i].min_position);
00127         l.append(lim[i].max_position);
00128         result.append(l);
00129       }
00130     }
00131     return result;
00132   }
00133 
00134   const char* getPlanningFrame() const
00135   {
00136     return robot_model_->getModelFrame().c_str();
00137   }
00138 
00139   bp::list getLinkPose(const std::string& name)
00140   {
00141     bp::list l;
00142     if (!ensureCurrentState())
00143       return l;
00144     robot_state::RobotStatePtr state = current_state_monitor_->getCurrentState();
00145     const robot_model::LinkModel* lm = state->getLinkModel(name);
00146     if (lm)
00147     {
00148       const Eigen::Affine3d& t = state->getGlobalLinkTransform(lm);
00149       std::vector<double> v(7);
00150       v[0] = t.translation().x();
00151       v[1] = t.translation().y();
00152       v[2] = t.translation().z();
00153       Eigen::Quaterniond q(t.rotation());
00154       v[3] = q.x();
00155       v[4] = q.y();
00156       v[5] = q.z();
00157       v[6] = q.w();
00158       l = py_bindings_tools::listFromDouble(v);
00159     }
00160     return l;
00161   }
00162 
00163   bp::list getCurrentJointValues(const std::string& name)
00164   {
00165     bp::list l;
00166     if (!ensureCurrentState())
00167       return l;
00168     robot_state::RobotStatePtr state = current_state_monitor_->getCurrentState();
00169     const robot_model::JointModel* jm = state->getJointModel(name);
00170     if (jm)
00171     {
00172       const double* pos = state->getJointPositions(jm);
00173       const unsigned int sz = jm->getVariableCount();
00174       for (unsigned int i = 0; i < sz; ++i)
00175         l.append(pos[i]);
00176     }
00177 
00178     return l;
00179   }
00180 
00181   bool ensureCurrentState(double wait = 1.0)
00182   {
00183     if (!current_state_monitor_)
00184     {
00185       ROS_ERROR("Unable to get current robot state");
00186       return false;
00187     }
00188 
00189     // if needed, start the monitor and wait up to 1 second for a full robot state
00190     if (!current_state_monitor_->isActive())
00191     {
00192       current_state_monitor_->startStateMonitor();
00193       if (!current_state_monitor_->waitForCurrentState(wait))
00194         ROS_WARN("Joint values for monitored state are requested but the full state is not known");
00195     }
00196     return true;
00197   }
00198 
00199   std::string getCurrentState()
00200   {
00201     if (!ensureCurrentState())
00202       return "";
00203     robot_state::RobotStatePtr s = current_state_monitor_->getCurrentState();
00204     moveit_msgs::RobotState msg;
00205     robot_state::robotStateToRobotStateMsg(*s, msg);
00206     return py_bindings_tools::serializeMsg(msg);
00207   }
00208 
00209   bp::dict getCurrentVariableValues()
00210   {
00211     bp::dict d;
00212 
00213     if (!ensureCurrentState())
00214       return d;
00215 
00216     const std::map<std::string, double>& vars = current_state_monitor_->getCurrentStateValues();
00217     for (std::map<std::string, double>::const_iterator it = vars.begin(); it != vars.end(); ++it)
00218       d[it->first] = it->second;
00219 
00220     return d;
00221   }
00222 
00223   const char* getRobotRootLink() const
00224   {
00225     return robot_model_->getRootLinkName().c_str();
00226   }
00227 
00228   bool hasGroup(const std::string& group) const
00229   {
00230     return robot_model_->hasJointModelGroup(group);
00231   }
00232 
00233 private:
00234   robot_model::RobotModelConstPtr robot_model_;
00235   planning_scene_monitor::CurrentStateMonitorPtr current_state_monitor_;
00236 };
00237 }
00238 
00239 static void wrap_robot_interface()
00240 {
00241   using namespace moveit;
00242 
00243   bp::class_<RobotInterfacePython> RobotClass("RobotInterface", bp::init<std::string>());
00244 
00245   RobotClass.def("get_joint_names", &RobotInterfacePython::getJointNames);
00246   RobotClass.def("get_group_joint_names", &RobotInterfacePython::getGroupJointNames);
00247   RobotClass.def("get_group_joint_tips", &RobotInterfacePython::getGroupJointTips);
00248   RobotClass.def("get_group_names", &RobotInterfacePython::getGroupNames);
00249   RobotClass.def("get_link_names", &RobotInterfacePython::getLinkNames);
00250   RobotClass.def("get_group_link_names", &RobotInterfacePython::getGroupLinkNames);
00251   RobotClass.def("get_joint_limits", &RobotInterfacePython::getJointLimits);
00252   RobotClass.def("get_link_pose", &RobotInterfacePython::getLinkPose);
00253   RobotClass.def("get_planning_frame", &RobotInterfacePython::getPlanningFrame);
00254   RobotClass.def("get_current_state", &RobotInterfacePython::getCurrentState);
00255   RobotClass.def("get_current_variable_values", &RobotInterfacePython::getCurrentVariableValues);
00256   RobotClass.def("get_current_joint_values", &RobotInterfacePython::getCurrentJointValues);
00257   RobotClass.def("get_robot_root_link", &RobotInterfacePython::getRobotRootLink);
00258   RobotClass.def("has_group", &RobotInterfacePython::hasGroup);
00259   RobotClass.def("get_robot_name", &RobotInterfacePython::getRobotName);
00260 }
00261 
00262 BOOST_PYTHON_MODULE(_moveit_robot_interface)
00263 {
00264   wrap_robot_interface();
00265 }
00266 


planning_interface
Author(s): Ioan Sucan
autogenerated on Wed Jun 19 2019 19:24:53