conversions.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2013, Ioan A. Sucan
5 * Copyright (c) 2011-2013, Willow Garage, Inc.
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of the Willow Garage nor the names of its
19 * contributors may be used to endorse or promote products derived
20 * from this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 *********************************************************************/
35 
36 /* Author: Ioan Sucan, Dave Coleman */
37 
41 #include <boost/lexical_cast.hpp>
42 
43 namespace moveit
44 {
45 namespace core
46 {
47 // ********************************************
48 // * Internal (hidden) functions
49 // ********************************************
50 
51 namespace
52 {
53 static bool _jointStateToRobotState(const sensor_msgs::JointState& joint_state, RobotState& state)
54 {
55  if (joint_state.name.size() != joint_state.position.size())
56  {
57  logError("Different number of names and positions in JointState message: %u, %u",
58  (unsigned int)joint_state.name.size(), (unsigned int)joint_state.position.size());
59  return false;
60  }
61 
62  state.setVariableValues(joint_state);
63 
64  return true;
65 }
66 
67 static bool _multiDOFJointsToRobotState(const sensor_msgs::MultiDOFJointState& mjs, RobotState& state,
68  const Transforms* tf)
69 {
70  std::size_t nj = mjs.joint_names.size();
71  if (nj != mjs.transforms.size())
72  {
73  logError("Different number of names, values or frames in MultiDOFJointState message.");
74  return false;
75  }
76 
77  bool error = false;
78  Eigen::Affine3d inv_t;
79  bool use_inv_t = false;
80 
81  if (nj > 0 && !Transforms::sameFrame(mjs.header.frame_id, state.getRobotModel()->getModelFrame()))
82  {
83  if (tf)
84  try
85  {
86  // find the transform that takes the given frame_id to the desired fixed frame
87  const Eigen::Affine3d& t2fixed_frame = tf->getTransform(mjs.header.frame_id);
88  // we update the value of the transform so that it transforms from the known fixed frame to the desired child
89  // link
90  inv_t = t2fixed_frame.inverse();
91  use_inv_t = true;
92  }
93  catch (std::exception& ex)
94  {
95  logError("Caught %s", ex.what());
96  error = true;
97  }
98  else
99  error = true;
100 
101  if (error)
102  logWarn("The transform for multi-dof joints was specified in frame '%s' but it was not possible to transform "
103  "that to frame '%s'",
104  mjs.header.frame_id.c_str(), state.getRobotModel()->getModelFrame().c_str());
105  }
106 
107  for (std::size_t i = 0; i < nj; ++i)
108  {
109  const std::string& joint_name = mjs.joint_names[i];
110  if (!state.getRobotModel()->hasJointModel(joint_name))
111  {
112  logWarn("No joint matching multi-dof joint '%s'", joint_name.c_str());
113  error = true;
114  continue;
115  }
116  Eigen::Affine3d transf;
117  tf::transformMsgToEigen(mjs.transforms[i], transf);
118  // if frames do not mach, attempt to transform
119  if (use_inv_t)
120  transf = transf * inv_t;
121 
122  state.setJointPositions(joint_name, transf);
123  }
124 
125  return !error;
126 }
127 
128 static inline void _robotStateToMultiDOFJointState(const RobotState& state, sensor_msgs::MultiDOFJointState& mjs)
129 {
130  const std::vector<const JointModel*>& js = state.getRobotModel()->getMultiDOFJointModels();
131  mjs.joint_names.clear();
132  mjs.transforms.clear();
133  for (std::size_t i = 0; i < js.size(); ++i)
134  {
135  geometry_msgs::Transform p;
136  if (state.dirtyJointTransform(js[i]))
137  {
138  Eigen::Affine3d t;
139  t.setIdentity();
140  js[i]->computeTransform(state.getJointPositions(js[i]), t);
142  }
143  else
144  tf::transformEigenToMsg(state.getJointTransform(js[i]), p);
145  mjs.joint_names.push_back(js[i]->getName());
146  mjs.transforms.push_back(p);
147  }
148  mjs.header.frame_id = state.getRobotModel()->getModelFrame();
149 }
150 
151 class ShapeVisitorAddToCollisionObject : public boost::static_visitor<void>
152 {
153 public:
154  ShapeVisitorAddToCollisionObject(moveit_msgs::CollisionObject* obj) : boost::static_visitor<void>(), obj_(obj)
155  {
156  }
157 
158  void addToObject(const shapes::ShapeMsg& sm, const geometry_msgs::Pose& pose)
159  {
160  pose_ = &pose;
161  boost::apply_visitor(*this, sm);
162  }
163 
164  void operator()(const shape_msgs::Plane& shape_msg) const
165  {
166  obj_->planes.push_back(shape_msg);
167  obj_->plane_poses.push_back(*pose_);
168  }
169 
170  void operator()(const shape_msgs::Mesh& shape_msg) const
171  {
172  obj_->meshes.push_back(shape_msg);
173  obj_->mesh_poses.push_back(*pose_);
174  }
175 
176  void operator()(const shape_msgs::SolidPrimitive& shape_msg) const
177  {
178  obj_->primitives.push_back(shape_msg);
179  obj_->primitive_poses.push_back(*pose_);
180  }
181 
182 private:
183  moveit_msgs::CollisionObject* obj_;
184  const geometry_msgs::Pose* pose_;
185 };
186 
187 static void _attachedBodyToMsg(const AttachedBody& attached_body, moveit_msgs::AttachedCollisionObject& aco)
188 {
189  aco.link_name = attached_body.getAttachedLinkName();
190  aco.detach_posture = attached_body.getDetachPosture();
191  const std::set<std::string>& touch_links = attached_body.getTouchLinks();
192  aco.touch_links.clear();
193  for (std::set<std::string>::const_iterator it = touch_links.begin(); it != touch_links.end(); ++it)
194  aco.touch_links.push_back(*it);
195  aco.object.header.frame_id = aco.link_name;
196  aco.object.id = attached_body.getName();
197 
198  aco.object.operation = moveit_msgs::CollisionObject::ADD;
199  const std::vector<shapes::ShapeConstPtr>& ab_shapes = attached_body.getShapes();
200  const EigenSTL::vector_Affine3d& ab_tf = attached_body.getFixedTransforms();
201  ShapeVisitorAddToCollisionObject sv(&aco.object);
202  aco.object.primitives.clear();
203  aco.object.meshes.clear();
204  aco.object.planes.clear();
205  aco.object.primitive_poses.clear();
206  aco.object.mesh_poses.clear();
207  aco.object.plane_poses.clear();
208  for (std::size_t j = 0; j < ab_shapes.size(); ++j)
209  {
210  shapes::ShapeMsg sm;
211  if (shapes::constructMsgFromShape(ab_shapes[j].get(), sm))
212  {
213  geometry_msgs::Pose p;
214  tf::poseEigenToMsg(ab_tf[j], p);
215  sv.addToObject(sm, p);
216  }
217  }
218 }
219 
220 static void _msgToAttachedBody(const Transforms* tf, const moveit_msgs::AttachedCollisionObject& aco, RobotState& state)
221 {
222  if (aco.object.operation == moveit_msgs::CollisionObject::ADD)
223  {
224  if (!aco.object.primitives.empty() || !aco.object.meshes.empty() || !aco.object.planes.empty())
225  {
226  if (aco.object.primitives.size() != aco.object.primitive_poses.size())
227  {
228  logError("Number of primitive shapes does not match number of poses in collision object message");
229  return;
230  }
231 
232  if (aco.object.meshes.size() != aco.object.mesh_poses.size())
233  {
234  logError("Number of meshes does not match number of poses in collision object message");
235  return;
236  }
237 
238  if (aco.object.planes.size() != aco.object.plane_poses.size())
239  {
240  logError("Number of planes does not match number of poses in collision object message");
241  return;
242  }
243 
244  const LinkModel* lm = state.getLinkModel(aco.link_name);
245  if (lm)
246  {
247  std::vector<shapes::ShapeConstPtr> shapes;
249 
250  for (std::size_t i = 0; i < aco.object.primitives.size(); ++i)
251  {
252  shapes::Shape* s = shapes::constructShapeFromMsg(aco.object.primitives[i]);
253  if (s)
254  {
255  Eigen::Affine3d p;
256  tf::poseMsgToEigen(aco.object.primitive_poses[i], p);
257  shapes.push_back(shapes::ShapeConstPtr(s));
258  poses.push_back(p);
259  }
260  }
261  for (std::size_t i = 0; i < aco.object.meshes.size(); ++i)
262  {
263  shapes::Shape* s = shapes::constructShapeFromMsg(aco.object.meshes[i]);
264  if (s)
265  {
266  Eigen::Affine3d p;
267  tf::poseMsgToEigen(aco.object.mesh_poses[i], p);
268  shapes.push_back(shapes::ShapeConstPtr(s));
269  poses.push_back(p);
270  }
271  }
272  for (std::size_t i = 0; i < aco.object.planes.size(); ++i)
273  {
274  shapes::Shape* s = shapes::constructShapeFromMsg(aco.object.planes[i]);
275  if (s)
276  {
277  Eigen::Affine3d p;
278  tf::poseMsgToEigen(aco.object.plane_poses[i], p);
279 
280  shapes.push_back(shapes::ShapeConstPtr(s));
281  poses.push_back(p);
282  }
283  }
284 
285  // transform poses to link frame
286  if (!Transforms::sameFrame(aco.object.header.frame_id, aco.link_name))
287  {
288  Eigen::Affine3d t0;
289  if (state.knowsFrameTransform(aco.object.header.frame_id))
290  t0 = state.getFrameTransform(aco.object.header.frame_id);
291  else if (tf && tf->canTransform(aco.object.header.frame_id))
292  t0 = tf->getTransform(aco.object.header.frame_id);
293  else
294  {
295  t0.setIdentity();
296  logError("Cannot properly transform from frame '%s'. The pose of the attached body may be incorrect",
297  aco.object.header.frame_id.c_str());
298  }
299  Eigen::Affine3d t = state.getGlobalLinkTransform(lm).inverse() * t0;
300  for (std::size_t i = 0; i < poses.size(); ++i)
301  poses[i] = t * poses[i];
302  }
303 
304  if (shapes.empty())
305  logError("There is no geometry to attach to link '%s' as part of attached body '%s'", aco.link_name.c_str(),
306  aco.object.id.c_str());
307  else
308  {
309  if (state.clearAttachedBody(aco.object.id))
310  logDebug("The robot state already had an object named '%s' attached to link '%s'. The object was replaced.",
311  aco.object.id.c_str(), aco.link_name.c_str());
312  state.attachBody(aco.object.id, shapes, poses, aco.touch_links, aco.link_name, aco.detach_posture);
313  logDebug("Attached object '%s' to link '%s'", aco.object.id.c_str(), aco.link_name.c_str());
314  }
315  }
316  }
317  else
318  logError("The attached body for link '%s' has no geometry", aco.link_name.c_str());
319  }
320  else if (aco.object.operation == moveit_msgs::CollisionObject::REMOVE)
321  {
322  if (!state.clearAttachedBody(aco.object.id))
323  logError("The attached body '%s' can not be removed because it does not exist", aco.link_name.c_str());
324  }
325  else
326  logError("Unknown collision object operation: %d", aco.object.operation);
327 }
328 
329 static bool _robotStateMsgToRobotStateHelper(const Transforms* tf, const moveit_msgs::RobotState& robot_state,
330  RobotState& state, bool copy_attached_bodies)
331 {
332  bool valid;
333  const moveit_msgs::RobotState& rs = robot_state;
334 
335  if (!rs.is_diff && rs.joint_state.name.empty() && rs.multi_dof_joint_state.joint_names.empty())
336  {
337  logError("Found empty JointState message");
338  return false;
339  }
340 
341  bool result1 = _jointStateToRobotState(robot_state.joint_state, state);
342  bool result2 = _multiDOFJointsToRobotState(robot_state.multi_dof_joint_state, state, tf);
343  valid = result1 || result2;
344 
345  if (valid && copy_attached_bodies)
346  {
347  if (!robot_state.is_diff)
348  state.clearAttachedBodies();
349  for (std::size_t i = 0; i < robot_state.attached_collision_objects.size(); ++i)
350  _msgToAttachedBody(tf, robot_state.attached_collision_objects[i], state);
351  }
352 
353  return valid;
354 }
355 }
356 }
357 }
358 
359 // ********************************************
360 
361 // ********************************************
362 // * Exposed functions
363 // ********************************************
364 
365 bool moveit::core::jointStateToRobotState(const sensor_msgs::JointState& joint_state, RobotState& state)
366 {
367  bool result = _jointStateToRobotState(joint_state, state);
368  state.update();
369  return result;
370 }
371 
372 bool moveit::core::robotStateMsgToRobotState(const moveit_msgs::RobotState& robot_state, RobotState& state,
373  bool copy_attached_bodies)
374 {
375  bool result = _robotStateMsgToRobotStateHelper(NULL, robot_state, state, copy_attached_bodies);
376  state.update();
377  return result;
378 }
379 
380 bool moveit::core::robotStateMsgToRobotState(const Transforms& tf, const moveit_msgs::RobotState& robot_state,
381  RobotState& state, bool copy_attached_bodies)
382 {
383  bool result = _robotStateMsgToRobotStateHelper(&tf, robot_state, state, copy_attached_bodies);
384  state.update();
385  return result;
386 }
387 
388 void moveit::core::robotStateToRobotStateMsg(const RobotState& state, moveit_msgs::RobotState& robot_state,
389  bool copy_attached_bodies)
390 {
391  robotStateToJointStateMsg(state, robot_state.joint_state);
392  _robotStateToMultiDOFJointState(state, robot_state.multi_dof_joint_state);
393 
394  if (copy_attached_bodies)
395  {
396  std::vector<const AttachedBody*> attached_bodies;
397  state.getAttachedBodies(attached_bodies);
398  attachedBodiesToAttachedCollisionObjectMsgs(attached_bodies, robot_state.attached_collision_objects);
399  }
400 }
401 
403  const std::vector<const AttachedBody*>& attached_bodies,
404  std::vector<moveit_msgs::AttachedCollisionObject>& attached_collision_objs)
405 {
406  attached_collision_objs.resize(attached_bodies.size());
407  for (std::size_t i = 0; i < attached_bodies.size(); ++i)
408  _attachedBodyToMsg(*attached_bodies[i], attached_collision_objs[i]);
409 }
410 
411 void moveit::core::robotStateToJointStateMsg(const RobotState& state, sensor_msgs::JointState& joint_state)
412 {
413  const std::vector<const JointModel*>& js = state.getRobotModel()->getSingleDOFJointModels();
414  joint_state = sensor_msgs::JointState();
415 
416  for (std::size_t i = 0; i < js.size(); ++i)
417  {
418  joint_state.name.push_back(js[i]->getName());
419  joint_state.position.push_back(state.getVariablePosition(js[i]->getFirstVariableIndex()));
420  if (state.hasVelocities())
421  joint_state.velocity.push_back(state.getVariableVelocity(js[i]->getFirstVariableIndex()));
422  }
423 
424  // if inconsistent number of velocities are specified, discard them
425  if (joint_state.velocity.size() != joint_state.position.size())
426  joint_state.velocity.clear();
427 
428  joint_state.header.frame_id = state.getRobotModel()->getModelFrame();
429 }
430 
431 bool moveit::core::jointTrajPointToRobotState(const trajectory_msgs::JointTrajectory& trajectory, std::size_t point_id,
432  RobotState& state)
433 {
434  if (trajectory.points.empty() || point_id > trajectory.points.size() - 1)
435  {
436  logError("Invalid point_id");
437  return false;
438  }
439  if (trajectory.joint_names.empty())
440  {
441  logError("No joint names specified");
442  return false;
443  }
444 
445  state.setVariablePositions(trajectory.joint_names, trajectory.points[point_id].positions);
446  if (!trajectory.points[point_id].velocities.empty())
447  state.setVariableVelocities(trajectory.joint_names, trajectory.points[point_id].velocities);
448  if (!trajectory.points[point_id].accelerations.empty())
449  state.setVariableAccelerations(trajectory.joint_names, trajectory.points[point_id].accelerations);
450  if (!trajectory.points[point_id].effort.empty())
451  state.setVariableEffort(trajectory.joint_names, trajectory.points[point_id].effort);
452 
453  return true;
454 }
455 
456 void moveit::core::robotStateToStream(const RobotState& state, std::ostream& out, bool include_header,
457  const std::string& separator)
458 {
459  // Output name of variables
460  if (include_header)
461  {
462  for (std::size_t i = 0; i < state.getVariableCount(); ++i)
463  {
464  out << state.getVariableNames()[i];
465 
466  // Output comma except at end
467  if (i < state.getVariableCount() - 1)
468  out << separator;
469  }
470  out << std::endl;
471  }
472 
473  // Output values of joints
474  for (std::size_t i = 0; i < state.getVariableCount(); ++i)
475  {
476  out << state.getVariablePositions()[i];
477 
478  // Output comma except at end
479  if (i < state.getVariableCount() - 1)
480  out << separator;
481  }
482  out << std::endl;
483 }
484 
485 void moveit::core::robotStateToStream(const RobotState& state, std::ostream& out,
486  const std::vector<std::string>& joint_groups_ordering, bool include_header,
487  const std::string& separator)
488 {
489  std::stringstream headers;
490  std::stringstream joints;
491 
492  for (std::size_t j = 0; j < joint_groups_ordering.size(); ++j)
493  {
494  const JointModelGroup* jmg = state.getRobotModel()->getJointModelGroup(joint_groups_ordering[j]);
495 
496  // Output name of variables
497  if (include_header)
498  {
499  for (std::size_t i = 0; i < jmg->getVariableCount(); ++i)
500  {
501  headers << jmg->getVariableNames()[i] << separator;
502  }
503  }
504 
505  // Copy the joint positions for each joint model group
506  std::vector<double> group_variable_positions;
507  state.copyJointGroupPositions(jmg, group_variable_positions);
508 
509  // Output values of joints
510  for (std::size_t i = 0; i < jmg->getVariableCount(); ++i)
511  {
512  joints << group_variable_positions[i] << separator;
513  }
514  }
515 
516  // Push all headers and joints to our output stream
517  if (include_header)
518  out << headers.str() << std::endl;
519  out << joints.str() << std::endl;
520 }
521 
522 void moveit::core::streamToRobotState(RobotState& state, const std::string& line, const std::string& separator)
523 {
524  std::stringstream lineStream(line);
525  std::string cell;
526 
527  // For each item/column
528  for (std::size_t i = 0; i < state.getVariableCount(); ++i)
529  {
530  // Get a variable
531  if (!std::getline(lineStream, cell, separator[0]))
532  logError("Missing variable %i", i);
533 
534  state.getVariablePositions()[i] = boost::lexical_cast<double>(cell.c_str());
535  }
536 }
void getAttachedBodies(std::vector< const AttachedBody * > &attached_bodies) const
Get all bodies attached to the model corresponding to this state.
Core components of MoveIt!
void attachedBodiesToAttachedCollisionObjectMsgs(const std::vector< const AttachedBody * > &attached_bodies, std::vector< moveit_msgs::AttachedCollisionObject > &attached_collision_objs)
Convert AttachedBodies to AttachedCollisionObjects.
const std::vector< std::string > & getVariableNames() const
Get the names of the variables that make up the joints included in this group. The number of returned...
std::vector< Eigen::Affine3d, Eigen::aligned_allocator< Eigen::Affine3d > > vector_Affine3d
void poseEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Pose &m)
void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e)
void robotStateToStream(const RobotState &state, std::ostream &out, bool include_header=true, const std::string &separator=",")
Convert a MoveIt! robot state to common separated values (CSV) on a single line that is outputted to ...
void setVariableVelocities(const double *velocity)
Given an array with velocity values for all variables, set those values as the velocities in this sta...
Definition: robot_state.h:243
XmlRpcServer s
void transformEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Transform &m)
bool hasVelocities() const
By default, if velocities are never set or initialized, the state remembers that there are no velocit...
Definition: robot_state.h:222
bool jointTrajPointToRobotState(const trajectory_msgs::JointTrajectory &trajectory, std::size_t point_id, RobotState &state)
Convert a joint trajectory point to a MoveIt! robot state.
static bool sameFrame(const std::string &frame1, const std::string &frame2)
Check if two frames end up being the same once the missing / are added as prefix (if they are missing...
Definition: transforms.cpp:59
std::string getName(void *handle)
void robotStateToJointStateMsg(const RobotState &state, sensor_msgs::JointState &joint_state)
Convert a MoveIt! robot state to a joint state message.
double * getVariablePositions()
Get a raw pointer to the positions of the variables stored in this state. Use carefully. If you change these values externally you need to make sure you trigger a forced update for the state by calling update(true).
Definition: robot_state.h:140
void setVariablePositions(const double *position)
It is assumed positions is an array containing the new positions for all variables in this state...
Provides an implementation of a snapshot of a transform tree that can be easily queried for transform...
Definition: transforms.h:61
void copyJointGroupPositions(const std::string &joint_group_name, std::vector< double > &gstate) const
For a given group, copy the position values of the variables that make up the group into another loca...
Definition: robot_state.h:632
unsigned int getVariableCount() const
Get the number of variables that describe this joint group. This includes variables necessary for mim...
void update(bool force=false)
Update all transforms.
bool robotStateMsgToRobotState(const Transforms &tf, const moveit_msgs::RobotState &robot_state, RobotState &state, bool copy_attached_bodies=true)
Convert a robot state msg (with accompanying extra transforms) to a MoveIt! robot state...
Shape * constructShapeFromMsg(const shape_msgs::SolidPrimitive &shape_msg)
const RobotModelConstPtr & getRobotModel() const
Get the robot model this state is constructed for.
Definition: robot_state.h:97
const geometry_msgs::Pose * pose_
void transformMsgToEigen(const geometry_msgs::Transform &m, Eigen::Affine3d &e)
void robotStateToRobotStateMsg(const RobotState &state, moveit_msgs::RobotState &robot_state, bool copy_attached_bodies=true)
Convert a MoveIt! robot state to a robot state message.
bool constructMsgFromShape(const Shape *shape, ShapeMsg &shape_msg)
bool jointStateToRobotState(const sensor_msgs::JointState &joint_state, RobotState &state)
Convert a joint state to a MoveIt! robot state.
void setVariableAccelerations(const double *acceleration)
Given an array with acceleration values for all variables, set those values as the accelerations in t...
Definition: robot_state.h:332
Representation of a robot&#39;s state. This includes position, velocity, acceleration and effort...
Definition: robot_state.h:82
double getVariablePosition(const std::string &variable) const
Get the position of a particular variable. An exception is thrown if the variable is not known...
Definition: robot_state.h:200
void setVariableEffort(const double *effort)
Given an array with effort values for all variables, set those values as the effort in this state...
Definition: robot_state.h:423
const std::vector< std::string > & getVariableNames() const
Get the names of the variables that make up this state, in the order they are stored in memory...
Definition: robot_state.h:109
Main namespace for MoveIt!
moveit_msgs::CollisionObject * obj_
std::size_t getVariableCount() const
Get the number of variables that make up this state.
Definition: robot_state.h:103
double getVariableVelocity(const std::string &variable) const
Get the velocity of a particular variable. An exception is thrown if the variable is not known...
Definition: robot_state.h:286
void streamToRobotState(RobotState &state, const std::string &line, const std::string &separator=",")
Convert a string of joint values from a file (CSV) or input source into a RobotState.
boost::variant< shape_msgs::SolidPrimitive, shape_msgs::Mesh, shape_msgs::Plane > ShapeMsg
std::shared_ptr< const Shape > ShapeConstPtr


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Jan 15 2018 03:50:44