utils.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
41 
42 using namespace moveit::core;
43 
44 namespace kinematic_constraints
45 {
46 const std::string LOGNAME = "kinematic_constraint_utils";
47 
48 moveit_msgs::Constraints mergeConstraints(const moveit_msgs::Constraints& first, const moveit_msgs::Constraints& second)
49 {
50  moveit_msgs::Constraints r;
51 
52  // add all joint constraints that are in first but not in second
53  // and merge joint constraints that are for the same joint
54  for (std::size_t i = 0; i < first.joint_constraints.size(); ++i)
55  {
56  bool add = true;
57  for (std::size_t j = 0; j < second.joint_constraints.size(); ++j)
58  if (second.joint_constraints[j].joint_name == first.joint_constraints[i].joint_name)
59  {
60  add = false;
61  // now we merge
62  moveit_msgs::JointConstraint m;
63  const moveit_msgs::JointConstraint& a = first.joint_constraints[i];
64  const moveit_msgs::JointConstraint& b = second.joint_constraints[j];
65  double low = std::max(a.position - a.tolerance_below, b.position - b.tolerance_below);
66  double high = std::min(a.position + a.tolerance_above, b.position + b.tolerance_above);
67  if (low > high)
68  ROS_ERROR_NAMED("kinematic_constraints",
69  "Attempted to merge incompatible constraints for joint '%s'. Discarding constraint.",
70  a.joint_name.c_str());
71  else
72  {
73  m.joint_name = a.joint_name;
74  m.position =
75  std::max(low, std::min((a.position * a.weight + b.position * b.weight) / (a.weight + b.weight), high));
76  m.weight = (a.weight + b.weight) / 2.0;
77  m.tolerance_above = std::max(0.0, high - m.position);
78  m.tolerance_below = std::max(0.0, m.position - low);
79  r.joint_constraints.push_back(m);
80  }
81  break;
82  }
83  if (add)
84  r.joint_constraints.push_back(first.joint_constraints[i]);
85  }
86 
87  // add all joint constraints that are in second but not in first
88  for (std::size_t i = 0; i < second.joint_constraints.size(); ++i)
89  {
90  bool add = true;
91  for (std::size_t j = 0; j < first.joint_constraints.size(); ++j)
92  if (second.joint_constraints[i].joint_name == first.joint_constraints[j].joint_name)
93  {
94  add = false;
95  break;
96  }
97  if (add)
98  r.joint_constraints.push_back(second.joint_constraints[i]);
99  }
100 
101  // merge rest of constraints
102  r.position_constraints = first.position_constraints;
103  for (std::size_t i = 0; i < second.position_constraints.size(); ++i)
104  r.position_constraints.push_back(second.position_constraints[i]);
105 
106  r.orientation_constraints = first.orientation_constraints;
107  for (std::size_t i = 0; i < second.orientation_constraints.size(); ++i)
108  r.orientation_constraints.push_back(second.orientation_constraints[i]);
109 
110  r.visibility_constraints = first.visibility_constraints;
111  for (std::size_t i = 0; i < second.visibility_constraints.size(); ++i)
112  r.visibility_constraints.push_back(second.visibility_constraints[i]);
113 
114  return r;
115 }
116 
117 bool isEmpty(const moveit_msgs::Constraints& constr)
118 {
119  return constr.position_constraints.empty() && constr.orientation_constraints.empty() &&
120  constr.visibility_constraints.empty() && constr.joint_constraints.empty();
121 }
122 
123 std::size_t countIndividualConstraints(const moveit_msgs::Constraints& constr)
124 {
125  return constr.position_constraints.size() + constr.orientation_constraints.size() +
126  constr.visibility_constraints.size() + constr.joint_constraints.size();
127 }
128 
129 moveit_msgs::Constraints constructGoalConstraints(const robot_state::RobotState& state,
130  const robot_model::JointModelGroup* jmg, double tolerance)
131 {
132  return constructGoalConstraints(state, jmg, tolerance, tolerance);
133 }
134 
135 moveit_msgs::Constraints constructGoalConstraints(const robot_state::RobotState& state,
136  const robot_model::JointModelGroup* jmg, double tolerance_below,
137  double tolerance_above)
138 {
139  moveit_msgs::Constraints goal;
140  std::vector<double> vals;
141  state.copyJointGroupPositions(jmg, vals);
142  goal.joint_constraints.resize(vals.size());
143  for (std::size_t i = 0; i < vals.size(); ++i)
144  {
145  goal.joint_constraints[i].joint_name = jmg->getVariableNames()[i];
146  goal.joint_constraints[i].position = vals[i];
147  goal.joint_constraints[i].tolerance_above = tolerance_below;
148  goal.joint_constraints[i].tolerance_below = tolerance_above;
149  goal.joint_constraints[i].weight = 1.0;
150  }
151 
152  return goal;
153 }
154 
155 moveit_msgs::Constraints constructGoalConstraints(const std::string& link_name, const geometry_msgs::PoseStamped& pose,
156  double tolerance_pos, double tolerance_angle)
157 {
158  moveit_msgs::Constraints goal;
159 
160  goal.position_constraints.resize(1);
161  moveit_msgs::PositionConstraint& pcm = goal.position_constraints[0];
162  pcm.link_name = link_name;
163  pcm.target_point_offset.x = 0;
164  pcm.target_point_offset.y = 0;
165  pcm.target_point_offset.z = 0;
166  pcm.constraint_region.primitives.resize(1);
167  shape_msgs::SolidPrimitive& bv = pcm.constraint_region.primitives[0];
168  bv.type = shape_msgs::SolidPrimitive::SPHERE;
170  bv.dimensions[shape_msgs::SolidPrimitive::SPHERE_RADIUS] = tolerance_pos;
171 
172  pcm.header = pose.header;
173  pcm.constraint_region.primitive_poses.resize(1);
174  pcm.constraint_region.primitive_poses[0].position = pose.pose.position;
175 
176  // orientation of constraint region does not affect anything, since it is a sphere
177  pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
178  pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
179  pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
180  pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
181  pcm.weight = 1.0;
182 
183  goal.orientation_constraints.resize(1);
184  moveit_msgs::OrientationConstraint& ocm = goal.orientation_constraints[0];
185  ocm.link_name = link_name;
186  ocm.header = pose.header;
187  ocm.orientation = pose.pose.orientation;
188  ocm.absolute_x_axis_tolerance = tolerance_angle;
189  ocm.absolute_y_axis_tolerance = tolerance_angle;
190  ocm.absolute_z_axis_tolerance = tolerance_angle;
191  ocm.weight = 1.0;
192 
193  return goal;
194 }
195 
196 moveit_msgs::Constraints constructGoalConstraints(const std::string& link_name, const geometry_msgs::PoseStamped& pose,
197  const std::vector<double>& tolerance_pos,
198  const std::vector<double>& tolerance_angle)
199 {
200  moveit_msgs::Constraints goal = constructGoalConstraints(link_name, pose);
201  if (tolerance_pos.size() == 3)
202  {
203  shape_msgs::SolidPrimitive& bv = goal.position_constraints[0].constraint_region.primitives[0];
204  bv.type = shape_msgs::SolidPrimitive::BOX;
206  bv.dimensions[shape_msgs::SolidPrimitive::BOX_X] = tolerance_pos[0];
207  bv.dimensions[shape_msgs::SolidPrimitive::BOX_Y] = tolerance_pos[1];
208  bv.dimensions[shape_msgs::SolidPrimitive::BOX_Z] = tolerance_pos[2];
209  }
210  if (tolerance_angle.size() == 3)
211  {
212  moveit_msgs::OrientationConstraint& ocm = goal.orientation_constraints[0];
213  ocm.absolute_x_axis_tolerance = tolerance_angle[0];
214  ocm.absolute_y_axis_tolerance = tolerance_angle[1];
215  ocm.absolute_z_axis_tolerance = tolerance_angle[2];
216  }
217  return goal;
218 }
219 
220 moveit_msgs::Constraints constructGoalConstraints(const std::string& link_name,
221  const geometry_msgs::QuaternionStamped& quat, double tolerance)
222 {
223  moveit_msgs::Constraints goal;
224  goal.orientation_constraints.resize(1);
225  moveit_msgs::OrientationConstraint& ocm = goal.orientation_constraints[0];
226  ocm.link_name = link_name;
227  ocm.header = quat.header;
228  ocm.orientation = quat.quaternion;
229  ocm.absolute_x_axis_tolerance = tolerance;
230  ocm.absolute_y_axis_tolerance = tolerance;
231  ocm.absolute_z_axis_tolerance = tolerance;
232  ocm.weight = 1.0;
233  return goal;
234 }
235 
236 moveit_msgs::Constraints constructGoalConstraints(const std::string& link_name,
237  const geometry_msgs::PointStamped& goal_point, double tolerance)
238 {
239  geometry_msgs::Point p;
240  p.x = 0;
241  p.y = 0;
242  p.z = 0;
243  return constructGoalConstraints(link_name, p, goal_point, tolerance);
244 }
245 
246 moveit_msgs::Constraints constructGoalConstraints(const std::string& link_name,
247  const geometry_msgs::Point& reference_point,
248  const geometry_msgs::PointStamped& goal_point, double tolerance)
249 {
250  moveit_msgs::Constraints goal;
251  goal.position_constraints.resize(1);
252  moveit_msgs::PositionConstraint& pcm = goal.position_constraints[0];
253  pcm.link_name = link_name;
254  pcm.target_point_offset.x = reference_point.x;
255  pcm.target_point_offset.y = reference_point.y;
256  pcm.target_point_offset.z = reference_point.z;
257  pcm.constraint_region.primitives.resize(1);
258  pcm.constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE;
259  pcm.constraint_region.primitives[0].dimensions.resize(
261  pcm.constraint_region.primitives[0].dimensions[shape_msgs::SolidPrimitive::SPHERE_RADIUS] = tolerance;
262 
263  pcm.header = goal_point.header;
264  pcm.constraint_region.primitive_poses.resize(1);
265  pcm.constraint_region.primitive_poses[0].position = goal_point.point;
266 
267  // orientation of constraint region does not affect anything, since it is a sphere
268  pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
269  pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
270  pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
271  pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
272  pcm.weight = 1.0;
273 
274  return goal;
275 }
276 
277 static bool constructPoseStamped(XmlRpc::XmlRpcValue::iterator& it, geometry_msgs::PoseStamped& pose)
278 {
279  if (!isStruct(it->second, { "frame_id", "position", "orientation" }, it->first))
280  return false;
281  pose.header.frame_id = static_cast<std::string>(it->second["frame_id"]);
282 
283  if (!isArray(it->second["orientation"], 3, "orientation", "RPY values"))
284  return false;
285  auto& rpy = it->second["orientation"];
286  tf2::Quaternion q;
287  q.setRPY(parseDouble(rpy[0]), parseDouble(rpy[1]), parseDouble(rpy[2]));
288  pose.pose.orientation = toMsg(q);
289 
290  if (!isArray(it->second["position"], 3, "position", "xyz position"))
291  return false;
292  pose.pose.position.x = parseDouble(it->second["position"][0]);
293  pose.pose.position.y = parseDouble(it->second["position"][1]);
294  pose.pose.position.z = parseDouble(it->second["position"][2]);
295 
296  return true;
297 }
298 
299 static bool constructConstraint(XmlRpc::XmlRpcValue& params, moveit_msgs::JointConstraint& constraint)
300 {
301  for (XmlRpc::XmlRpcValue::iterator it = params.begin(); it != params.end(); ++it)
302  {
303  if (it->first == "type")
304  continue;
305  else if (it->first == "joint_name")
306  constraint.joint_name = static_cast<std::string>(it->second);
307  else if (it->first == "weight")
308  constraint.weight = parseDouble(it->second);
309  else if (it->first == "position")
310  {
311  constraint.position = parseDouble(it->second);
312  }
313  else if (it->first == "tolerance")
314  {
315  constraint.tolerance_below = parseDouble(it->second);
316  constraint.tolerance_above = parseDouble(it->second);
317  }
318  else if (it->first == "tolerances")
319  {
320  if (!isArray(it->second, 2, it->first, "lower/upper tolerances"))
321  return false;
322 
323  constraint.tolerance_below = parseDouble(it->second[0]);
324  constraint.tolerance_above = parseDouble(it->second[1]);
325  }
326  else if (it->first == "bounds")
327  {
328  if (!isArray(it->second, 2, it->first, "lower/upper bound"))
329  return false;
330 
331  const double lower_bound = parseDouble(it->second[0]);
332  const double upper_bound = parseDouble(it->second[1]);
333 
334  constraint.position = (lower_bound + upper_bound) / 2;
335  constraint.tolerance_below = constraint.position - lower_bound;
336  constraint.tolerance_above = upper_bound - constraint.position;
337  }
338  else
339  {
340  ROS_WARN_STREAM_NAMED(LOGNAME, "joint constraint contains unknown entity '" << it->first << "'");
341  }
342  }
343  return true;
344 }
345 
346 static bool constructConstraint(XmlRpc::XmlRpcValue& params, moveit_msgs::PositionConstraint& constraint)
347 {
348  for (XmlRpc::XmlRpcValue::iterator it = params.begin(); it != params.end(); ++it)
349  {
350  if (it->first == "type")
351  continue;
352  else if (it->first == "frame_id")
353  constraint.header.frame_id = static_cast<std::string>(it->second);
354  else if (it->first == "weight")
355  constraint.weight = parseDouble(it->second);
356  else if (it->first == "link_name")
357  constraint.link_name = static_cast<std::string>(it->second);
358  else if (it->first == "target_offset")
359  {
360  if (!isArray(it->second, 3, it->first, "x/y/z position"))
361  return false;
362 
363  constraint.target_point_offset.x = parseDouble(it->second[0]);
364  constraint.target_point_offset.y = parseDouble(it->second[1]);
365  constraint.target_point_offset.z = parseDouble(it->second[2]);
366  }
367  else if (it->first == "region")
368  {
369  if (!isStruct(it->second, { "x", "y", "z" }, "region"))
370  return false;
371 
372  constraint.constraint_region.primitive_poses.emplace_back();
373  constraint.constraint_region.primitives.emplace_back();
374 
375  geometry_msgs::Pose& region_pose = constraint.constraint_region.primitive_poses.back();
376  shape_msgs::SolidPrimitive& region_primitive = constraint.constraint_region.primitives.back();
377 
378  region_primitive.type = shape_msgs::SolidPrimitive::BOX;
379  region_primitive.dimensions.resize(3);
380 
381  std::function<void(XmlRpc::XmlRpcValue&, double&, double&)> parse_dimension =
382  [](XmlRpc::XmlRpcValue& it, double& center, double& dimension) {
383  center = (parseDouble(it[0]) + parseDouble(it[1])) / 2;
384  dimension = parseDouble(it[1]) - parseDouble(it[0]);
385  };
386 
387  parse_dimension(it->second["x"], region_pose.position.x,
388  region_primitive.dimensions[shape_msgs::SolidPrimitive::BOX_X]);
389  parse_dimension(it->second["y"], region_pose.position.y,
390  region_primitive.dimensions[shape_msgs::SolidPrimitive::BOX_Y]);
391  parse_dimension(it->second["z"], region_pose.position.z,
392  region_primitive.dimensions[shape_msgs::SolidPrimitive::BOX_Z]);
393 
394  region_pose.orientation.w = 1.0;
395  }
396  else
397  {
398  ROS_WARN_STREAM_NAMED(LOGNAME, "position constraint contains unknown entity '" << it->first << "'");
399  }
400  }
401  return true;
402 }
403 
404 static bool constructConstraint(XmlRpc::XmlRpcValue& params, moveit_msgs::OrientationConstraint& constraint)
405 {
406  for (XmlRpc::XmlRpcValue::iterator it = params.begin(); it != params.end(); ++it)
407  {
408  if (it->first == "type")
409  continue;
410  else if (it->first == "frame_id")
411  constraint.header.frame_id = static_cast<std::string>(it->second);
412  else if (it->first == "weight")
413  constraint.weight = parseDouble(it->second);
414  else if (it->first == "link_name")
415  constraint.link_name = static_cast<std::string>(it->second);
416  else if (it->first == "orientation")
417  {
418  if (!isArray(it->second, 3, it->first, "RPY values"))
419  return false;
420 
421  tf2::Quaternion q;
422  q.setRPY(parseDouble(it->second[0]), parseDouble(it->second[1]), parseDouble(it->second[2]));
423  constraint.orientation = toMsg(q);
424  }
425  else if (it->first == "tolerances")
426  {
427  if (!isArray(it->second, 3, it->first, "xyz tolerances"))
428  return false;
429 
430  constraint.absolute_x_axis_tolerance = parseDouble(it->second[0]);
431  constraint.absolute_y_axis_tolerance = parseDouble(it->second[1]);
432  constraint.absolute_z_axis_tolerance = parseDouble(it->second[2]);
433  }
434  else
435  {
436  ROS_WARN_STREAM_NAMED(LOGNAME, "orientation constraint contains unknown entity '" << it->first << "'");
437  }
438  }
439  return true;
440 }
441 
442 static bool constructConstraint(XmlRpc::XmlRpcValue& params, moveit_msgs::VisibilityConstraint& constraint)
443 {
444  for (XmlRpc::XmlRpcValue::iterator it = params.begin(); it != params.end(); ++it)
445  {
446  if (it->first == "type")
447  continue;
448  else if (it->first == "weight")
449  constraint.weight = parseDouble(it->second);
450  else if (it->first == "target_radius")
451  constraint.target_radius = parseDouble(it->second);
452  else if (it->first == "target_pose")
453  {
454  if (!constructPoseStamped(it, constraint.target_pose))
455  return false;
456  }
457  else if (it->first == "cone_sides")
458  constraint.cone_sides = static_cast<int>(it->second);
459  else if (it->first == "sensor_pose")
460  {
461  if (!constructPoseStamped(it, constraint.sensor_pose))
462  return false;
463  }
464  else if (it->first == "max_view_angle")
465  constraint.max_view_angle = parseDouble(it->second);
466  else if (it->first == "max_range_angle")
467  constraint.max_range_angle = parseDouble(it->second);
468  else
469  {
470  ROS_WARN_STREAM_NAMED(LOGNAME, "orientation constraint contains unknown entity '" << it->first << "'");
471  }
472  }
473 
474  constraint.sensor_view_direction = moveit_msgs::VisibilityConstraint::SENSOR_X;
475 
476  return true;
477 }
478 
479 static bool collectConstraints(XmlRpc::XmlRpcValue& params, moveit_msgs::Constraints& constraints)
480 {
481  if (params.getType() != XmlRpc::XmlRpcValue::TypeArray)
482  {
483  ROS_ERROR_NAMED(LOGNAME, "expected constraints as array");
484  return false;
485  }
486 
487  for (int i = 0; i < params.size(); ++i)
488  {
489  if (!params[i].hasMember("type"))
490  {
491  ROS_ERROR_NAMED(LOGNAME, "constraint parameter does not specify its type");
492  }
493  else if (params[i]["type"] == "joint")
494  {
495  constraints.joint_constraints.emplace_back();
496  if (!constructConstraint(params[i], constraints.joint_constraints.back()))
497  return false;
498  }
499  else if (params[i]["type"] == "position")
500  {
501  constraints.position_constraints.emplace_back();
502  if (!constructConstraint(params[i], constraints.position_constraints.back()))
503  return false;
504  }
505  else if (params[i]["type"] == "orientation")
506  {
507  constraints.orientation_constraints.emplace_back();
508  if (!constructConstraint(params[i], constraints.orientation_constraints.back()))
509  return false;
510  }
511  else if (params[i]["type"] == "visibility")
512  {
513  constraints.visibility_constraints.emplace_back();
514  if (!constructConstraint(params[i], constraints.visibility_constraints.back()))
515  return false;
516  }
517  }
518 
519  return true;
520 }
521 
522 bool constructConstraints(XmlRpc::XmlRpcValue& params, moveit_msgs::Constraints& constraints)
523 {
524  if (!isStruct(params, { "name", "constraints" }, "Parameter"))
525  return false;
526 
527  constraints.name = static_cast<std::string>(params["name"]);
528  return collectConstraints(params["constraints"], constraints);
529 }
530 } // namespace kinematic_constraints
void setRPY(const tf2Scalar &roll, const tf2Scalar &pitch, const tf2Scalar &yaw)
Core components of MoveIt!
bool isEmpty(const moveit_msgs::Constraints &constr)
Check if any constraints were specified.
Definition: utils.cpp:117
ValueStruct::iterator iterator
std::size_t countIndividualConstraints(const moveit_msgs::Constraints &constr)
Definition: utils.cpp:123
bool isArray(XmlRpc::XmlRpcValue &v, size_t size=0, const std::string &name="", const std::string &description="")
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...
bool add(const actionlib::TwoIntsGoal &req, actionlib::TwoIntsResult &res)
Representation and evaluation of kinematic constraints.
static bool constructConstraint(XmlRpc::XmlRpcValue &params, moveit_msgs::VisibilityConstraint &constraint)
Definition: utils.cpp:442
double parseDouble(XmlRpc::XmlRpcValue &v)
parse a double value from a scalar XmlRpc
Type const & getType() const
moveit_msgs::Constraints constructGoalConstraints(const robot_state::RobotState &state, const robot_model::JointModelGroup *jmg, double tolerance_below, double tolerance_above)
Generates a constraint message intended to be used as a goal constraint for a joint group...
Definition: utils.cpp:135
static bool collectConstraints(XmlRpc::XmlRpcValue &params, moveit_msgs::Constraints &constraints)
Definition: utils.cpp:479
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:666
const std::string LOGNAME
Definition: robot_model.cpp:53
bool constructConstraints(XmlRpc::XmlRpcValue &params, moveit_msgs::Constraints &constraints)
extract constraint message from XmlRpc node.
Definition: utils.cpp:522
B toMsg(const A &a)
bool isStruct(XmlRpc::XmlRpcValue &v, const std::vector< std::string > &keys={}, const std::string &name="")
#define ROS_ERROR_NAMED(name,...)
Representation of a robot&#39;s state. This includes position, velocity, acceleration and effort...
Definition: robot_state.h:123
moveit_msgs::Constraints mergeConstraints(const moveit_msgs::Constraints &first, const moveit_msgs::Constraints &second)
Merge two sets of constraints into one.
Definition: utils.cpp:48
r
static bool constructPoseStamped(XmlRpc::XmlRpcValue::iterator &it, geometry_msgs::PoseStamped &pose)
Definition: utils.cpp:277
#define ROS_WARN_STREAM_NAMED(name, args)


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Fri Oct 11 2019 03:56:04