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 
40 #include <tf/transform_datatypes.h>
41 #include <xmlrpcpp/XmlRpcValue.h>
42 
43 #include <boost/algorithm/string/join.hpp>
44 
45 namespace kinematic_constraints
46 {
47 const std::string LOGNAME = "kinematic_constraint_utils";
48 
49 moveit_msgs::Constraints mergeConstraints(const moveit_msgs::Constraints& first, const moveit_msgs::Constraints& second)
50 {
51  moveit_msgs::Constraints r;
52 
53  // add all joint constraints that are in first but not in second
54  // and merge joint constraints that are for the same joint
55  for (std::size_t i = 0; i < first.joint_constraints.size(); ++i)
56  {
57  bool add = true;
58  for (std::size_t j = 0; j < second.joint_constraints.size(); ++j)
59  if (second.joint_constraints[j].joint_name == first.joint_constraints[i].joint_name)
60  {
61  add = false;
62  // now we merge
63  moveit_msgs::JointConstraint m;
64  const moveit_msgs::JointConstraint& a = first.joint_constraints[i];
65  const moveit_msgs::JointConstraint& b = second.joint_constraints[j];
66  double low = std::max(a.position - a.tolerance_below, b.position - b.tolerance_below);
67  double high = std::min(a.position + a.tolerance_above, b.position + b.tolerance_above);
68  if (low > high)
69  ROS_ERROR_NAMED("kinematic_constraints",
70  "Attempted to merge incompatible constraints for joint '%s'. Discarding constraint.",
71  a.joint_name.c_str());
72  else
73  {
74  m.joint_name = a.joint_name;
75  m.position =
76  std::max(low, std::min((a.position * a.weight + b.position * b.weight) / (a.weight + b.weight), high));
77  m.weight = (a.weight + b.weight) / 2.0;
78  m.tolerance_above = std::max(0.0, high - m.position);
79  m.tolerance_below = std::max(0.0, m.position - low);
80  r.joint_constraints.push_back(m);
81  }
82  break;
83  }
84  if (add)
85  r.joint_constraints.push_back(first.joint_constraints[i]);
86  }
87 
88  // add all joint constraints that are in second but not in first
89  for (std::size_t i = 0; i < second.joint_constraints.size(); ++i)
90  {
91  bool add = true;
92  for (std::size_t j = 0; j < first.joint_constraints.size(); ++j)
93  if (second.joint_constraints[i].joint_name == first.joint_constraints[j].joint_name)
94  {
95  add = false;
96  break;
97  }
98  if (add)
99  r.joint_constraints.push_back(second.joint_constraints[i]);
100  }
101 
102  // merge rest of constraints
103  r.position_constraints = first.position_constraints;
104  for (std::size_t i = 0; i < second.position_constraints.size(); ++i)
105  r.position_constraints.push_back(second.position_constraints[i]);
106 
107  r.orientation_constraints = first.orientation_constraints;
108  for (std::size_t i = 0; i < second.orientation_constraints.size(); ++i)
109  r.orientation_constraints.push_back(second.orientation_constraints[i]);
110 
111  r.visibility_constraints = first.visibility_constraints;
112  for (std::size_t i = 0; i < second.visibility_constraints.size(); ++i)
113  r.visibility_constraints.push_back(second.visibility_constraints[i]);
114 
115  return r;
116 }
117 
118 bool isEmpty(const moveit_msgs::Constraints& constr)
119 {
120  return constr.position_constraints.empty() && constr.orientation_constraints.empty() &&
121  constr.visibility_constraints.empty() && constr.joint_constraints.empty();
122 }
123 
124 std::size_t countIndividualConstraints(const moveit_msgs::Constraints& constr)
125 {
126  return constr.position_constraints.size() + constr.orientation_constraints.size() +
127  constr.visibility_constraints.size() + constr.joint_constraints.size();
128 }
129 
130 moveit_msgs::Constraints constructGoalConstraints(const robot_state::RobotState& state,
131  const robot_model::JointModelGroup* jmg, double tolerance)
132 {
133  return constructGoalConstraints(state, jmg, tolerance, tolerance);
134 }
135 
136 moveit_msgs::Constraints constructGoalConstraints(const robot_state::RobotState& state,
137  const robot_model::JointModelGroup* jmg, double tolerance_below,
138  double tolerance_above)
139 {
140  moveit_msgs::Constraints goal;
141  std::vector<double> vals;
142  state.copyJointGroupPositions(jmg, vals);
143  goal.joint_constraints.resize(vals.size());
144  for (std::size_t i = 0; i < vals.size(); ++i)
145  {
146  goal.joint_constraints[i].joint_name = jmg->getVariableNames()[i];
147  goal.joint_constraints[i].position = vals[i];
148  goal.joint_constraints[i].tolerance_above = tolerance_below;
149  goal.joint_constraints[i].tolerance_below = tolerance_above;
150  goal.joint_constraints[i].weight = 1.0;
151  }
152 
153  return goal;
154 }
155 
156 moveit_msgs::Constraints constructGoalConstraints(const std::string& link_name, const geometry_msgs::PoseStamped& pose,
157  double tolerance_pos, double tolerance_angle)
158 {
159  moveit_msgs::Constraints goal;
160 
161  goal.position_constraints.resize(1);
162  moveit_msgs::PositionConstraint& pcm = goal.position_constraints[0];
163  pcm.link_name = link_name;
164  pcm.target_point_offset.x = 0;
165  pcm.target_point_offset.y = 0;
166  pcm.target_point_offset.z = 0;
167  pcm.constraint_region.primitives.resize(1);
168  shape_msgs::SolidPrimitive& bv = pcm.constraint_region.primitives[0];
169  bv.type = shape_msgs::SolidPrimitive::SPHERE;
171  bv.dimensions[shape_msgs::SolidPrimitive::SPHERE_RADIUS] = tolerance_pos;
172 
173  pcm.header = pose.header;
174  pcm.constraint_region.primitive_poses.resize(1);
175  pcm.constraint_region.primitive_poses[0].position = pose.pose.position;
176 
177  // orientation of constraint region does not affect anything, since it is a sphere
178  pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
179  pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
180  pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
181  pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
182  pcm.weight = 1.0;
183 
184  goal.orientation_constraints.resize(1);
185  moveit_msgs::OrientationConstraint& ocm = goal.orientation_constraints[0];
186  ocm.link_name = link_name;
187  ocm.header = pose.header;
188  ocm.orientation = pose.pose.orientation;
189  ocm.absolute_x_axis_tolerance = tolerance_angle;
190  ocm.absolute_y_axis_tolerance = tolerance_angle;
191  ocm.absolute_z_axis_tolerance = tolerance_angle;
192  ocm.weight = 1.0;
193 
194  return goal;
195 }
196 
197 moveit_msgs::Constraints constructGoalConstraints(const std::string& link_name, const geometry_msgs::PoseStamped& pose,
198  const std::vector<double>& tolerance_pos,
199  const std::vector<double>& tolerance_angle)
200 {
201  moveit_msgs::Constraints goal = constructGoalConstraints(link_name, pose);
202  if (tolerance_pos.size() == 3)
203  {
204  shape_msgs::SolidPrimitive& bv = goal.position_constraints[0].constraint_region.primitives[0];
205  bv.type = shape_msgs::SolidPrimitive::BOX;
207  bv.dimensions[shape_msgs::SolidPrimitive::BOX_X] = tolerance_pos[0];
208  bv.dimensions[shape_msgs::SolidPrimitive::BOX_Y] = tolerance_pos[1];
209  bv.dimensions[shape_msgs::SolidPrimitive::BOX_Z] = tolerance_pos[2];
210  }
211  if (tolerance_angle.size() == 3)
212  {
213  moveit_msgs::OrientationConstraint& ocm = goal.orientation_constraints[0];
214  ocm.absolute_x_axis_tolerance = tolerance_angle[0];
215  ocm.absolute_y_axis_tolerance = tolerance_angle[1];
216  ocm.absolute_z_axis_tolerance = tolerance_angle[2];
217  }
218  return goal;
219 }
220 
221 moveit_msgs::Constraints constructGoalConstraints(const std::string& link_name,
222  const geometry_msgs::QuaternionStamped& quat, double tolerance)
223 {
224  moveit_msgs::Constraints goal;
225  goal.orientation_constraints.resize(1);
226  moveit_msgs::OrientationConstraint& ocm = goal.orientation_constraints[0];
227  ocm.link_name = link_name;
228  ocm.header = quat.header;
229  ocm.orientation = quat.quaternion;
230  ocm.absolute_x_axis_tolerance = tolerance;
231  ocm.absolute_y_axis_tolerance = tolerance;
232  ocm.absolute_z_axis_tolerance = tolerance;
233  ocm.weight = 1.0;
234  return goal;
235 }
236 
237 moveit_msgs::Constraints constructGoalConstraints(const std::string& link_name,
238  const geometry_msgs::PointStamped& goal_point, double tolerance)
239 {
240  geometry_msgs::Point p;
241  p.x = 0;
242  p.y = 0;
243  p.z = 0;
244  return constructGoalConstraints(link_name, p, goal_point, tolerance);
245 }
246 
247 moveit_msgs::Constraints constructGoalConstraints(const std::string& link_name,
248  const geometry_msgs::Point& reference_point,
249  const geometry_msgs::PointStamped& goal_point, double tolerance)
250 {
251  moveit_msgs::Constraints goal;
252  goal.position_constraints.resize(1);
253  moveit_msgs::PositionConstraint& pcm = goal.position_constraints[0];
254  pcm.link_name = link_name;
255  pcm.target_point_offset.x = reference_point.x;
256  pcm.target_point_offset.y = reference_point.y;
257  pcm.target_point_offset.z = reference_point.z;
258  pcm.constraint_region.primitives.resize(1);
259  pcm.constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE;
260  pcm.constraint_region.primitives[0].dimensions.resize(
262  pcm.constraint_region.primitives[0].dimensions[shape_msgs::SolidPrimitive::SPHERE_RADIUS] = tolerance;
263 
264  pcm.header = goal_point.header;
265  pcm.constraint_region.primitive_poses.resize(1);
266  pcm.constraint_region.primitive_poses[0].position = goal_point.point;
267 
268  // orientation of constraint region does not affect anything, since it is a sphere
269  pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
270  pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
271  pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
272  pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
273  pcm.weight = 1.0;
274 
275  return goal;
276 }
277 
279 {
281  return static_cast<double>(v);
282  else if (v.getType() == XmlRpc::XmlRpcValue::TypeInt)
283  return static_cast<int>(v);
284  else
285  return 0.0;
286 }
287 
288 static bool isArray(XmlRpc::XmlRpcValue& v, size_t size, const std::string& name = "",
289  const std::string& description = "")
290 {
291  if (v.getType() != XmlRpc::XmlRpcValue::TypeArray || static_cast<size_t>(v.size()) != size)
292  {
293  if (!name.empty())
294  ROS_WARN_STREAM_NAMED(LOGNAME, name << " is not an array[" << size << "] of " << description);
295  return false;
296  }
297  return true;
298 }
299 
300 static bool isStruct(XmlRpc::XmlRpcValue& v, const std::set<std::string>& keys, const std::string& name = "")
301 {
303  {
304  if (!name.empty())
305  ROS_WARN_STREAM_NAMED(LOGNAME, name << " is not a struct with keys " << boost::join(keys, ","));
306  return false;
307  }
308 
309  for (const std::string& key : keys)
310  if (!v.hasMember(key))
311  {
312  if (!name.empty())
313  ROS_WARN_STREAM_NAMED(LOGNAME, name << " is not a struct with keys " << boost::join(keys, ",") << " (misses "
314  << key << ")");
315  return false;
316  }
317 
318  return true;
319 }
320 
321 static bool constructPoseStamped(XmlRpc::XmlRpcValue::iterator& it, geometry_msgs::PoseStamped& pose)
322 {
323  if (!isStruct(it->second, { "frame_id", "position", "orientation" }, it->first))
324  return false;
325  pose.header.frame_id = static_cast<std::string>(it->second["frame_id"]);
326 
327  if (!isArray(it->second["orientation"], 3, "orientation", "RPY values"))
328  return false;
329  pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(parseDouble(it->second["orientation"][0]),
330  parseDouble(it->second["orientation"][1]),
331  parseDouble(it->second["orientation"][2]));
332 
333  if (!isArray(it->second["position"], 3, "position", "xyz position"))
334  return false;
335  pose.pose.position.x = parseDouble(it->second["position"][0]);
336  pose.pose.position.y = parseDouble(it->second["position"][1]);
337  pose.pose.position.z = parseDouble(it->second["position"][2]);
338 
339  return true;
340 }
341 
342 static bool constructConstraint(XmlRpc::XmlRpcValue& params, moveit_msgs::JointConstraint& constraint)
343 {
344  for (XmlRpc::XmlRpcValue::iterator it = params.begin(); it != params.end(); ++it)
345  {
346  if (it->first == "type")
347  continue;
348  else if (it->first == "joint_name")
349  constraint.joint_name = static_cast<std::string>(it->second);
350  else if (it->first == "weight")
351  constraint.weight = parseDouble(it->second);
352  else if (it->first == "position")
353  {
354  constraint.position = parseDouble(it->second);
355  }
356  else if (it->first == "tolerance")
357  {
358  constraint.tolerance_below = parseDouble(it->second);
359  constraint.tolerance_above = parseDouble(it->second);
360  }
361  else if (it->first == "tolerances")
362  {
363  if (!isArray(it->second, 2, it->first, "lower/upper tolerances"))
364  return false;
365 
366  constraint.tolerance_below = parseDouble(it->second[0]);
367  constraint.tolerance_above = parseDouble(it->second[1]);
368  }
369  else if (it->first == "bounds")
370  {
371  if (!isArray(it->second, 2, it->first, "lower/upper bound"))
372  return false;
373 
374  const double lower_bound = parseDouble(it->second[0]);
375  const double upper_bound = parseDouble(it->second[1]);
376 
377  constraint.position = (lower_bound + upper_bound) / 2;
378  constraint.tolerance_below = constraint.position - lower_bound;
379  constraint.tolerance_above = upper_bound - constraint.position;
380  }
381  else
382  {
383  ROS_WARN_STREAM_NAMED(LOGNAME, "joint constraint contains unknown entity '" << it->first << "'");
384  }
385  }
386  return true;
387 }
388 
389 static bool constructConstraint(XmlRpc::XmlRpcValue& params, moveit_msgs::PositionConstraint& constraint)
390 {
391  for (XmlRpc::XmlRpcValue::iterator it = params.begin(); it != params.end(); ++it)
392  {
393  if (it->first == "type")
394  continue;
395  else if (it->first == "frame_id")
396  constraint.header.frame_id = static_cast<std::string>(it->second);
397  else if (it->first == "weight")
398  constraint.weight = parseDouble(it->second);
399  else if (it->first == "link_name")
400  constraint.link_name = static_cast<std::string>(it->second);
401  else if (it->first == "target_offset")
402  {
403  if (!isArray(it->second, 3, it->first, "x/y/z position"))
404  return false;
405 
406  constraint.target_point_offset.x = parseDouble(it->second[0]);
407  constraint.target_point_offset.y = parseDouble(it->second[1]);
408  constraint.target_point_offset.z = parseDouble(it->second[2]);
409  }
410  else if (it->first == "region")
411  {
412  if (!isStruct(it->second, { "x", "y", "z" }, "region"))
413  return false;
414 
415  constraint.constraint_region.primitive_poses.emplace_back();
416  constraint.constraint_region.primitives.emplace_back();
417 
418  geometry_msgs::Pose& region_pose = constraint.constraint_region.primitive_poses.back();
419  shape_msgs::SolidPrimitive& region_primitive = constraint.constraint_region.primitives.back();
420 
421  region_primitive.type = shape_msgs::SolidPrimitive::BOX;
422  region_primitive.dimensions.resize(3);
423 
424  std::function<void(XmlRpc::XmlRpcValue&, double&, double&)> parse_dimension =
425  [](XmlRpc::XmlRpcValue& it, double& center, double& dimension) {
426  center = (parseDouble(it[0]) + parseDouble(it[1])) / 2;
427  dimension = parseDouble(it[1]) - parseDouble(it[0]);
428  };
429 
430  parse_dimension(it->second["x"], region_pose.position.x,
431  region_primitive.dimensions[shape_msgs::SolidPrimitive::BOX_X]);
432  parse_dimension(it->second["y"], region_pose.position.y,
433  region_primitive.dimensions[shape_msgs::SolidPrimitive::BOX_Y]);
434  parse_dimension(it->second["z"], region_pose.position.z,
435  region_primitive.dimensions[shape_msgs::SolidPrimitive::BOX_Z]);
436 
437  region_pose.orientation.w = 1.0;
438  }
439  else
440  {
441  ROS_WARN_STREAM_NAMED(LOGNAME, "position constraint contains unknown entity '" << it->first << "'");
442  }
443  }
444  return true;
445 }
446 
447 static bool constructConstraint(XmlRpc::XmlRpcValue& params, moveit_msgs::OrientationConstraint& constraint)
448 {
449  for (XmlRpc::XmlRpcValue::iterator it = params.begin(); it != params.end(); ++it)
450  {
451  if (it->first == "type")
452  continue;
453  else if (it->first == "frame_id")
454  constraint.header.frame_id = static_cast<std::string>(it->second);
455  else if (it->first == "weight")
456  constraint.weight = parseDouble(it->second);
457  else if (it->first == "link_name")
458  constraint.link_name = static_cast<std::string>(it->second);
459  else if (it->first == "orientation")
460  {
461  if (!isArray(it->second, 3, it->first, "RPY values"))
462  return false;
463 
464  constraint.orientation = tf::createQuaternionMsgFromRollPitchYaw(
465  parseDouble(it->second[0]), parseDouble(it->second[1]), parseDouble(it->second[2]));
466  }
467  else if (it->first == "tolerances")
468  {
469  if (!isArray(it->second, 3, it->first, "xyz tolerances"))
470  return false;
471 
472  constraint.absolute_x_axis_tolerance = parseDouble(it->second[0]);
473  constraint.absolute_y_axis_tolerance = parseDouble(it->second[1]);
474  constraint.absolute_z_axis_tolerance = parseDouble(it->second[2]);
475  }
476  else
477  {
478  ROS_WARN_STREAM_NAMED(LOGNAME, "orientation constraint contains unknown entity '" << it->first << "'");
479  }
480  }
481  return true;
482 }
483 
484 static bool constructConstraint(XmlRpc::XmlRpcValue& params, moveit_msgs::VisibilityConstraint& constraint)
485 {
486  for (XmlRpc::XmlRpcValue::iterator it = params.begin(); it != params.end(); ++it)
487  {
488  if (it->first == "type")
489  continue;
490  else if (it->first == "weight")
491  constraint.weight = parseDouble(it->second);
492  else if (it->first == "target_radius")
493  constraint.target_radius = parseDouble(it->second);
494  else if (it->first == "target_pose")
495  {
496  if (!constructPoseStamped(it, constraint.target_pose))
497  return false;
498  }
499  else if (it->first == "cone_sides")
500  constraint.cone_sides = static_cast<int>(it->second);
501  else if (it->first == "sensor_pose")
502  {
503  if (!constructPoseStamped(it, constraint.sensor_pose))
504  return false;
505  }
506  else if (it->first == "max_view_angle")
507  constraint.max_view_angle = parseDouble(it->second);
508  else if (it->first == "max_range_angle")
509  constraint.max_range_angle = parseDouble(it->second);
510  else
511  {
512  ROS_WARN_STREAM_NAMED(LOGNAME, "orientation constraint contains unknown entity '" << it->first << "'");
513  }
514  }
515 
516  constraint.sensor_view_direction = moveit_msgs::VisibilityConstraint::SENSOR_X;
517 
518  return true;
519 }
520 
521 static bool collectConstraints(XmlRpc::XmlRpcValue& params, moveit_msgs::Constraints& constraints)
522 {
523  if (params.getType() != XmlRpc::XmlRpcValue::TypeArray)
524  {
525  ROS_ERROR_NAMED(LOGNAME, "expected constraints as array");
526  return false;
527  }
528 
529  for (int i = 0; i < params.size(); ++i)
530  {
531  if (!params[i].hasMember("type"))
532  {
533  ROS_ERROR_NAMED(LOGNAME, "constraint parameter does not specify its type");
534  }
535  else if (params[i]["type"] == "joint")
536  {
537  constraints.joint_constraints.emplace_back();
538  if (!constructConstraint(params[i], constraints.joint_constraints.back()))
539  return false;
540  }
541  else if (params[i]["type"] == "position")
542  {
543  constraints.position_constraints.emplace_back();
544  if (!constructConstraint(params[i], constraints.position_constraints.back()))
545  return false;
546  }
547  else if (params[i]["type"] == "orientation")
548  {
549  constraints.orientation_constraints.emplace_back();
550  if (!constructConstraint(params[i], constraints.orientation_constraints.back()))
551  return false;
552  }
553  else if (params[i]["type"] == "visibility")
554  {
555  constraints.visibility_constraints.emplace_back();
556  if (!constructConstraint(params[i], constraints.visibility_constraints.back()))
557  return false;
558  }
559  }
560 
561  return true;
562 }
563 
564 bool constructConstraints(XmlRpc::XmlRpcValue& params, moveit_msgs::Constraints& constraints)
565 {
566  if (!isStruct(params, { "name", "constraints" }, "Parameter"))
567  return false;
568 
569  constraints.name = static_cast<std::string>(params["name"]);
570  return collectConstraints(params["constraints"], constraints);
571 }
572 }
bool isEmpty(const moveit_msgs::Constraints &constr)
Check if any constraints were specified.
Definition: utils.cpp:118
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...
ValueStruct::iterator iterator
int size() const
std::size_t countIndividualConstraints(const moveit_msgs::Constraints &constr)
Definition: utils.cpp:124
static bool isStruct(XmlRpc::XmlRpcValue &v, const std::set< std::string > &keys, const std::string &name="")
Definition: utils.cpp:300
static bool isArray(XmlRpc::XmlRpcValue &v, size_t size, const std::string &name="", const std::string &description="")
Definition: utils.cpp:288
static bool constructConstraint(XmlRpc::XmlRpcValue &params, moveit_msgs::JointConstraint &constraint)
Definition: utils.cpp:342
Type const & getType() const
Representation and evaluation of kinematic constraints.
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: utils.cpp:47
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:136
static bool collectConstraints(XmlRpc::XmlRpcValue &params, moveit_msgs::Constraints &constraints)
Definition: utils.cpp:521
static double parseDouble(XmlRpc::XmlRpcValue &v)
Definition: utils.cpp:278
bool constructConstraints(XmlRpc::XmlRpcValue &params, moveit_msgs::Constraints &constraints)
extract constraint message from XmlRpc node.
Definition: utils.cpp:564
bool hasMember(const std::string &name) const
#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:49
r
static bool constructPoseStamped(XmlRpc::XmlRpcValue::iterator &it, geometry_msgs::PoseStamped &pose)
Definition: utils.cpp:321
#define ROS_WARN_STREAM_NAMED(name, args)


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sun Sep 15 2019 03:57:56