43 #include <moveit_msgs/RobotState.h>
44 #include <visualization_msgs/MarkerArray.h>
47 #include <boost/python.hpp>
57 class RobotInterfacePython :
protected py_bindings_tools::ROScppInitializer
60 RobotInterfacePython(
const std::string& robot_description,
const std::string& ns =
"")
61 : py_bindings_tools::ROScppInitializer()
65 throw std::runtime_error(
"RobotInterfacePython: invalid robot model");
66 current_state_monitor_ =
70 const char* getRobotName()
const
72 return robot_model_->getName().c_str();
75 bp::list getActiveJointNames()
const
80 bp::list getGroupActiveJointNames(
const std::string&
group)
const
89 bp::list getJointNames()
const
94 bp::list getGroupJointNames(
const std::string&
group)
const
103 bp::list getGroupJointTips(
const std::string&
group)
const
108 std::vector<std::string> tips;
116 bp::list getLinkNames()
const
121 bp::list getGroupLinkNames(
const std::string&
group)
const
130 bp::list getGroupNames()
const
135 bp::list getJointLimits(
const std::string& name)
const
142 for (
const moveit_msgs::JointLimits& joint_limit : lim)
145 l.append(joint_limit.min_position);
146 l.append(joint_limit.max_position);
153 const char* getPlanningFrame()
const
155 return robot_model_->getModelFrame().c_str();
158 bp::list getLinkPose(
const std::string& name)
161 if (!ensureCurrentState())
163 moveit::core::RobotStatePtr state = current_state_monitor_->getCurrentState();
168 const Eigen::Isometry3d&
t = state->getGlobalLinkTransform(lm);
169 std::vector<double>
v(7);
170 v[0] =
t.translation().x();
171 v[1] =
t.translation().y();
172 v[2] =
t.translation().z();
173 Eigen::Quaterniond
q(
t.linear());
183 bp::list getDefaultStateNames(
const std::string&
group)
191 l.append(known_state);
197 bp::list getCurrentJointValues(
const std::string& name)
200 if (!ensureCurrentState())
202 moveit::core::RobotStatePtr state = current_state_monitor_->getCurrentState();
206 const double* pos = state->getJointPositions(jm);
208 for (
unsigned int i = 0; i < sz; ++i)
215 bp::dict getJointValues(
const std::string&
group,
const std::string& named_state)
219 return boost::python::dict();
220 std::map<std::string, double>
values;
225 bool ensureCurrentState(
double wait = 1.0)
227 if (!current_state_monitor_)
229 ROS_ERROR(
"Unable to get current robot state");
234 if (!current_state_monitor_->isActive())
237 current_state_monitor_->startStateMonitor();
238 if (!current_state_monitor_->waitForCompleteState(wait))
239 ROS_WARN(
"Joint values for monitored state are requested but the full state is not known");
244 py_bindings_tools::ByteString getCurrentState()
246 if (!ensureCurrentState())
247 return py_bindings_tools::ByteString(
"");
248 moveit::core::RobotStatePtr
s = current_state_monitor_->getCurrentState();
249 moveit_msgs::RobotState msg;
254 bp::tuple getEndEffectorParentGroup(
const std::string&
group)
260 return boost::python::make_tuple(
"",
"");
262 return boost::python::make_tuple(parent_group.first, parent_group.second);
265 py_bindings_tools::ByteString getRobotMarkersPythonDictList(bp::dict& values, bp::list& links)
267 moveit::core::RobotStatePtr state;
268 if (ensureCurrentState())
270 state = current_state_monitor_->getCurrentState();
274 state = std::make_shared<moveit::core::RobotState>(robot_model_);
277 bp::list k =
values.keys();
279 sensor_msgs::JointState joint_state;
280 joint_state.name.resize(l);
281 joint_state.position.resize(l);
282 for (
int i = 0; i < l; ++i)
284 joint_state.name[i] = bp::extract<std::string>(k[i]);
285 joint_state.position[i] = bp::extract<double>(values[k[i]]);
287 state->setVariableValues(joint_state);
288 visualization_msgs::MarkerArray msg;
294 py_bindings_tools::ByteString getRobotMarkersPythonDict(bp::dict& values)
297 return getRobotMarkersPythonDictList(values, links);
300 py_bindings_tools::ByteString getRobotMarkersFromMsg(
const py_bindings_tools::ByteString& state_str)
302 moveit_msgs::RobotState state_msg;
307 visualization_msgs::MarkerArray msg;
308 state.getRobotMarkers(msg, state.getRobotModel()->getLinkModelNames());
313 py_bindings_tools::ByteString getRobotMarkers()
315 if (!ensureCurrentState())
316 return py_bindings_tools::ByteString();
317 moveit::core::RobotStatePtr
s = current_state_monitor_->getCurrentState();
318 visualization_msgs::MarkerArray msg;
319 s->getRobotMarkers(msg,
s->getRobotModel()->getLinkModelNames());
324 py_bindings_tools::ByteString getRobotMarkersPythonList(
const bp::list& links)
326 if (!ensureCurrentState())
327 return py_bindings_tools::ByteString(
"");
328 moveit::core::RobotStatePtr
s = current_state_monitor_->getCurrentState();
329 visualization_msgs::MarkerArray msg;
335 py_bindings_tools::ByteString getRobotMarkersGroup(
const std::string&
group)
337 if (!ensureCurrentState())
338 return py_bindings_tools::ByteString(
"");
339 moveit::core::RobotStatePtr
s = current_state_monitor_->getCurrentState();
341 visualization_msgs::MarkerArray msg;
350 py_bindings_tools::ByteString getRobotMarkersGroupPythonDict(
const std::string&
group, bp::dict& values)
354 return py_bindings_tools::ByteString(
"");
356 return getRobotMarkersPythonDictList(values, links);
359 bp::dict getCurrentVariableValues()
363 if (!ensureCurrentState())
366 const std::map<std::string, double>& vars = current_state_monitor_->getCurrentStateValues();
367 for (
const std::pair<const std::string, double>& var : vars)
368 d[var.first] = var.second;
373 const char* getRobotRootLink()
const
375 return robot_model_->getRootLinkName().c_str();
378 bool hasGroup(
const std::string&
group)
const
380 return robot_model_->hasJointModelGroup(
group);
384 moveit::core::RobotModelConstPtr robot_model_;
385 planning_scene_monitor::CurrentStateMonitorPtr current_state_monitor_;
390 static void wrap_robot_interface()
394 bp::class_<RobotInterfacePython> robot_class(
"RobotInterface", bp::init<std::string, bp::optional<std::string>>());
396 robot_class.def(
"get_joint_names", &RobotInterfacePython::getJointNames);
397 robot_class.def(
"get_group_joint_names", &RobotInterfacePython::getGroupJointNames);
398 robot_class.def(
"get_active_joint_names", &RobotInterfacePython::getActiveJointNames);
399 robot_class.def(
"get_group_active_joint_names", &RobotInterfacePython::getGroupActiveJointNames);
400 robot_class.def(
"get_group_default_states", &RobotInterfacePython::getDefaultStateNames);
401 robot_class.def(
"get_group_joint_tips", &RobotInterfacePython::getGroupJointTips);
402 robot_class.def(
"get_group_names", &RobotInterfacePython::getGroupNames);
403 robot_class.def(
"get_link_names", &RobotInterfacePython::getLinkNames);
404 robot_class.def(
"get_group_link_names", &RobotInterfacePython::getGroupLinkNames);
405 robot_class.def(
"get_joint_limits", &RobotInterfacePython::getJointLimits);
406 robot_class.def(
"get_link_pose", &RobotInterfacePython::getLinkPose);
407 robot_class.def(
"get_planning_frame", &RobotInterfacePython::getPlanningFrame);
408 robot_class.def(
"get_current_state", &RobotInterfacePython::getCurrentState);
409 robot_class.def(
"get_current_variable_values", &RobotInterfacePython::getCurrentVariableValues);
410 robot_class.def(
"get_current_joint_values", &RobotInterfacePython::getCurrentJointValues);
411 robot_class.def(
"get_joint_values", &RobotInterfacePython::getJointValues);
412 robot_class.def(
"get_robot_root_link", &RobotInterfacePython::getRobotRootLink);
413 robot_class.def(
"has_group", &RobotInterfacePython::hasGroup);
414 robot_class.def(
"get_robot_name", &RobotInterfacePython::getRobotName);
415 robot_class.def(
"get_robot_markers", &RobotInterfacePython::getRobotMarkers);
416 robot_class.def(
"get_robot_markers", &RobotInterfacePython::getRobotMarkersPythonList);
417 robot_class.def(
"get_robot_markers", &RobotInterfacePython::getRobotMarkersFromMsg);
418 robot_class.def(
"get_robot_markers", &RobotInterfacePython::getRobotMarkersPythonDictList);
419 robot_class.def(
"get_robot_markers", &RobotInterfacePython::getRobotMarkersPythonDict);
420 robot_class.def(
"get_group_markers", &RobotInterfacePython::getRobotMarkersGroup);
421 robot_class.def(
"get_group_markers", &RobotInterfacePython::getRobotMarkersGroupPythonDict);
422 robot_class.def(
"get_parent_group", &RobotInterfacePython::getEndEffectorParentGroup);
427 wrap_robot_interface();