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 
42 #include <tf2_eigen/tf2_eigen.h>
43 
44 using namespace moveit::core;
45 
46 namespace kinematic_constraints
47 {
48 const std::string LOGNAME = "kinematic_constraint_utils";
49 
50 moveit_msgs::Constraints mergeConstraints(const moveit_msgs::Constraints& first, const moveit_msgs::Constraints& second)
51 {
52  moveit_msgs::Constraints r;
53 
54  // add all joint constraints that are in first but not in second
55  // and merge joint constraints that are for the same joint
56  for (const moveit_msgs::JointConstraint& jc_first : first.joint_constraints)
57  {
58  bool add = true;
59  for (const moveit_msgs::JointConstraint& jc_second : second.joint_constraints)
60  if (jc_second.joint_name == jc_first.joint_name)
61  {
62  add = false;
63  // now we merge
64  moveit_msgs::JointConstraint m;
65  const moveit_msgs::JointConstraint& a = jc_first;
66  const moveit_msgs::JointConstraint& b = jc_second;
67  double low = std::max(a.position - a.tolerance_below, b.position - b.tolerance_below);
68  double high = std::min(a.position + a.tolerance_above, b.position + b.tolerance_above);
69  if (low > high)
70  ROS_ERROR_NAMED("kinematic_constraints",
71  "Attempted to merge incompatible constraints for joint '%s'. Discarding constraint.",
72  a.joint_name.c_str());
73  else
74  {
75  m.joint_name = a.joint_name;
76  m.position =
77  std::max(low, std::min((a.position * a.weight + b.position * b.weight) / (a.weight + b.weight), high));
78  m.weight = (a.weight + b.weight) / 2.0;
79  m.tolerance_above = std::max(0.0, high - m.position);
80  m.tolerance_below = std::max(0.0, m.position - low);
81  r.joint_constraints.push_back(m);
82  }
83  break;
84  }
85  if (add)
86  r.joint_constraints.push_back(jc_first);
87  }
88 
89  // add all joint constraints that are in second but not in first
90  for (const moveit_msgs::JointConstraint& jc_second : second.joint_constraints)
91  {
92  bool add = true;
93  for (const moveit_msgs::JointConstraint& jc_first : first.joint_constraints)
94  if (jc_second.joint_name == jc_first.joint_name)
95  {
96  add = false;
97  break;
98  }
99  if (add)
100  r.joint_constraints.push_back(jc_second);
101  }
102 
103  // merge rest of constraints
104  r.position_constraints = first.position_constraints;
105  for (const moveit_msgs::PositionConstraint& position_constraint : second.position_constraints)
106  r.position_constraints.push_back(position_constraint);
107 
108  r.orientation_constraints = first.orientation_constraints;
109  for (const moveit_msgs::OrientationConstraint& orientation_constraint : second.orientation_constraints)
110  r.orientation_constraints.push_back(orientation_constraint);
111 
112  r.visibility_constraints = first.visibility_constraints;
113  for (const moveit_msgs::VisibilityConstraint& visibility_constraint : second.visibility_constraints)
114  r.visibility_constraints.push_back(visibility_constraint);
115 
116  return r;
117 }
118 
119 bool isEmpty(const moveit_msgs::Constraints& constr)
120 {
121  return moveit::core::isEmpty(constr);
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 moveit::core::RobotState& state,
131  const moveit::core::JointModelGroup* jmg, double tolerance)
132 {
133  return constructGoalConstraints(state, jmg, tolerance, tolerance);
134 }
135 
136 moveit_msgs::Constraints constructGoalConstraints(const moveit::core::RobotState& state,
137  const moveit::core::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_above;
149  goal.joint_constraints[i].tolerance_below = tolerance_below;
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;
170  bv.dimensions.resize(geometric_shapes::solidPrimitiveDimCount<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;
206  bv.dimensions.resize(geometric_shapes::solidPrimitiveDimCount<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(
261  geometric_shapes::solidPrimitiveDimCount<shape_msgs::SolidPrimitive::SPHERE>());
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 
278 static bool constructPoseStamped(XmlRpc::XmlRpcValue::iterator& it, geometry_msgs::PoseStamped& pose)
279 {
280  if (!isStruct(it->second, { "frame_id", "position", "orientation" }, it->first))
281  return false;
282  pose.header.frame_id = static_cast<std::string>(it->second["frame_id"]);
283 
284  if (!isArray(it->second["orientation"], 3, "orientation", "RPY values"))
285  return false;
286  auto& rpy = it->second["orientation"];
287  tf2::Quaternion q;
288  q.setRPY(parseDouble(rpy[0]), parseDouble(rpy[1]), parseDouble(rpy[2]));
289  pose.pose.orientation = toMsg(q);
290 
291  if (!isArray(it->second["position"], 3, "position", "xyz position"))
292  return false;
293  pose.pose.position.x = parseDouble(it->second["position"][0]);
294  pose.pose.position.y = parseDouble(it->second["position"][1]);
295  pose.pose.position.z = parseDouble(it->second["position"][2]);
296 
297  return true;
298 }
299 
300 static bool constructConstraint(XmlRpc::XmlRpcValue& params, moveit_msgs::JointConstraint& constraint)
301 {
302  for (XmlRpc::XmlRpcValue::iterator it = params.begin(); it != params.end(); ++it)
303  {
304  if (it->first == "type")
305  continue;
306  else if (it->first == "joint_name")
307  constraint.joint_name = static_cast<std::string>(it->second);
308  else if (it->first == "weight")
309  constraint.weight = parseDouble(it->second);
310  else if (it->first == "position")
311  {
312  constraint.position = parseDouble(it->second);
313  }
314  else if (it->first == "tolerance")
315  {
316  constraint.tolerance_below = parseDouble(it->second);
317  constraint.tolerance_above = parseDouble(it->second);
318  }
319  else if (it->first == "tolerances")
320  {
321  if (!isArray(it->second, 2, it->first, "lower/upper tolerances"))
322  return false;
323 
324  constraint.tolerance_below = parseDouble(it->second[0]);
325  constraint.tolerance_above = parseDouble(it->second[1]);
326  }
327  else if (it->first == "bounds")
328  {
329  if (!isArray(it->second, 2, it->first, "lower/upper bound"))
330  return false;
331 
332  const double lower_bound = parseDouble(it->second[0]);
333  const double upper_bound = parseDouble(it->second[1]);
334 
335  constraint.position = (lower_bound + upper_bound) / 2;
336  constraint.tolerance_below = constraint.position - lower_bound;
337  constraint.tolerance_above = upper_bound - constraint.position;
338  }
339  else
340  {
341  ROS_WARN_STREAM_NAMED(LOGNAME, "joint constraint contains unknown entity '" << it->first << "'");
342  }
343  }
344  return true;
345 }
346 
347 static bool constructConstraint(XmlRpc::XmlRpcValue& params, moveit_msgs::PositionConstraint& constraint)
348 {
349  for (XmlRpc::XmlRpcValue::iterator it = params.begin(); it != params.end(); ++it)
350  {
351  if (it->first == "type")
352  continue;
353  else if (it->first == "frame_id")
354  constraint.header.frame_id = static_cast<std::string>(it->second);
355  else if (it->first == "weight")
356  constraint.weight = parseDouble(it->second);
357  else if (it->first == "link_name")
358  constraint.link_name = static_cast<std::string>(it->second);
359  else if (it->first == "target_offset")
360  {
361  if (!isArray(it->second, 3, it->first, "x/y/z position"))
362  return false;
363 
364  constraint.target_point_offset.x = parseDouble(it->second[0]);
365  constraint.target_point_offset.y = parseDouble(it->second[1]);
366  constraint.target_point_offset.z = parseDouble(it->second[2]);
367  }
368  else if (it->first == "region")
369  {
370  if (!isStruct(it->second, { "x", "y", "z" }, "region"))
371  return false;
372 
373  constraint.constraint_region.primitive_poses.emplace_back();
374  constraint.constraint_region.primitives.emplace_back();
375 
376  geometry_msgs::Pose& region_pose = constraint.constraint_region.primitive_poses.back();
377  shape_msgs::SolidPrimitive& region_primitive = constraint.constraint_region.primitives.back();
378 
379  region_primitive.type = shape_msgs::SolidPrimitive::BOX;
380  region_primitive.dimensions.resize(3);
381 
382  std::function<void(XmlRpc::XmlRpcValue&, double&, double&)> parse_dimension =
383  [](XmlRpc::XmlRpcValue& it, double& center, double& dimension) {
384  center = (parseDouble(it[0]) + parseDouble(it[1])) / 2;
385  dimension = parseDouble(it[1]) - parseDouble(it[0]);
386  };
387 
388  parse_dimension(it->second["x"], region_pose.position.x,
389  region_primitive.dimensions[shape_msgs::SolidPrimitive::BOX_X]);
390  parse_dimension(it->second["y"], region_pose.position.y,
391  region_primitive.dimensions[shape_msgs::SolidPrimitive::BOX_Y]);
392  parse_dimension(it->second["z"], region_pose.position.z,
393  region_primitive.dimensions[shape_msgs::SolidPrimitive::BOX_Z]);
394 
395  region_pose.orientation.w = 1.0;
396  }
397  else
398  {
399  ROS_WARN_STREAM_NAMED(LOGNAME, "position constraint contains unknown entity '" << it->first << "'");
400  }
401  }
402  return true;
403 }
404 
405 static bool constructConstraint(XmlRpc::XmlRpcValue& params, moveit_msgs::OrientationConstraint& constraint)
406 {
407  for (XmlRpc::XmlRpcValue::iterator it = params.begin(); it != params.end(); ++it)
408  {
409  if (it->first == "type")
410  continue;
411  else if (it->first == "frame_id")
412  constraint.header.frame_id = static_cast<std::string>(it->second);
413  else if (it->first == "weight")
414  constraint.weight = parseDouble(it->second);
415  else if (it->first == "link_name")
416  constraint.link_name = static_cast<std::string>(it->second);
417  else if (it->first == "orientation")
418  {
419  if (!isArray(it->second, 3, it->first, "RPY values"))
420  return false;
421 
422  tf2::Quaternion q;
423  q.setRPY(parseDouble(it->second[0]), parseDouble(it->second[1]), parseDouble(it->second[2]));
424  constraint.orientation = toMsg(q);
425  }
426  else if (it->first == "tolerances")
427  {
428  if (!isArray(it->second, 3, it->first, "xyz tolerances"))
429  return false;
430 
431  constraint.absolute_x_axis_tolerance = parseDouble(it->second[0]);
432  constraint.absolute_y_axis_tolerance = parseDouble(it->second[1]);
433  constraint.absolute_z_axis_tolerance = parseDouble(it->second[2]);
434  }
435  else
436  {
437  ROS_WARN_STREAM_NAMED(LOGNAME, "orientation constraint contains unknown entity '" << it->first << "'");
438  }
439  }
440  return true;
441 }
442 
443 static bool constructConstraint(XmlRpc::XmlRpcValue& params, moveit_msgs::VisibilityConstraint& constraint)
444 {
445  for (XmlRpc::XmlRpcValue::iterator it = params.begin(); it != params.end(); ++it)
446  {
447  if (it->first == "type")
448  continue;
449  else if (it->first == "weight")
450  constraint.weight = parseDouble(it->second);
451  else if (it->first == "target_radius")
452  constraint.target_radius = parseDouble(it->second);
453  else if (it->first == "target_pose")
454  {
455  if (!constructPoseStamped(it, constraint.target_pose))
456  return false;
457  }
458  else if (it->first == "cone_sides")
459  constraint.cone_sides = static_cast<int>(it->second);
460  else if (it->first == "sensor_pose")
461  {
462  if (!constructPoseStamped(it, constraint.sensor_pose))
463  return false;
464  }
465  else if (it->first == "max_view_angle")
466  constraint.max_view_angle = parseDouble(it->second);
467  else if (it->first == "max_range_angle")
468  constraint.max_range_angle = parseDouble(it->second);
469  else
470  {
471  ROS_WARN_STREAM_NAMED(LOGNAME, "orientation constraint contains unknown entity '" << it->first << "'");
472  }
473  }
474 
475  constraint.sensor_view_direction = moveit_msgs::VisibilityConstraint::SENSOR_X;
476 
477  return true;
478 }
479 
480 static bool collectConstraints(XmlRpc::XmlRpcValue& params, moveit_msgs::Constraints& constraints)
481 {
482  if (params.getType() != XmlRpc::XmlRpcValue::TypeArray)
483  {
484  ROS_ERROR_NAMED(LOGNAME, "expected constraints as array");
485  return false;
486  }
487 
488  for (int i = 0; i < params.size(); ++i) // NOLINT(modernize-loop-convert)
489  {
490  if (!params[i].hasMember("type"))
491  {
492  ROS_ERROR_NAMED(LOGNAME, "constraint parameter does not specify its type");
493  }
494  else if (params[i]["type"] == "joint")
495  {
496  constraints.joint_constraints.emplace_back();
497  if (!constructConstraint(params[i], constraints.joint_constraints.back()))
498  return false;
499  }
500  else if (params[i]["type"] == "position")
501  {
502  constraints.position_constraints.emplace_back();
503  if (!constructConstraint(params[i], constraints.position_constraints.back()))
504  return false;
505  }
506  else if (params[i]["type"] == "orientation")
507  {
508  constraints.orientation_constraints.emplace_back();
509  if (!constructConstraint(params[i], constraints.orientation_constraints.back()))
510  return false;
511  }
512  else if (params[i]["type"] == "visibility")
513  {
514  constraints.visibility_constraints.emplace_back();
515  if (!constructConstraint(params[i], constraints.visibility_constraints.back()))
516  return false;
517  }
518  }
519 
520  return true;
521 }
522 
523 bool constructConstraints(XmlRpc::XmlRpcValue& params, moveit_msgs::Constraints& constraints)
524 {
525  if (!isStruct(params, { "name", "constraints" }, "Parameter"))
526  return false;
527 
528  constraints.name = static_cast<std::string>(params["name"]);
529  return collectConstraints(params["constraints"], constraints);
530 }
531 } // namespace kinematic_constraints
532 
534  moveit_msgs::Constraints& constraints)
535 {
536  for (auto& c : constraints.position_constraints)
537  {
538  bool frame_found;
539  const moveit::core::LinkModel* robot_link;
540  const Eigen::Isometry3d& transform = state.getFrameInfo(c.link_name, robot_link, frame_found);
541  if (!frame_found)
542  return false;
543 
544  // If the frame of the constraint is not part of the robot link model (but an attached body or subframe),
545  // the constraint needs to be expressed in the frame of a robot link.
546  if (c.link_name != robot_link->getName())
547  {
548  Eigen::Isometry3d robot_link_to_link_name = state.getGlobalLinkTransform(robot_link).inverse() * transform;
549  Eigen::Vector3d offset_link_name(c.target_point_offset.x, c.target_point_offset.y, c.target_point_offset.z);
550  Eigen::Vector3d offset_robot_link = robot_link_to_link_name * offset_link_name;
551 
552  c.link_name = robot_link->getName();
553  tf2::toMsg(offset_robot_link, c.target_point_offset);
554  }
555  }
556 
557  for (auto& c : constraints.orientation_constraints)
558  {
559  bool frame_found;
560  const moveit::core::LinkModel* robot_link;
561  // getFrameInfo() returns a valid isometry by contract
562  const Eigen::Isometry3d& transform = state.getFrameInfo(c.link_name, robot_link, frame_found);
563  if (!frame_found)
564  return false;
565 
566  // If the frame of the constraint is not part of the robot link model (but an attached body or subframe),
567  // the constraint needs to be expressed in the frame of a robot link.
568  if (c.link_name != robot_link->getName())
569  {
570  c.link_name = robot_link->getName();
571  Eigen::Quaterniond link_name_to_robot_link(transform.linear().transpose() *
572  state.getGlobalLinkTransform(robot_link).linear());
573  Eigen::Quaterniond quat_target;
574  tf2::fromMsg(c.orientation, quat_target);
575  c.orientation = tf2::toMsg(quat_target * link_name_to_robot_link);
576  }
577  }
578  return true;
579 }
XmlRpc::XmlRpcValue::size
int size() const
moveit::core::LinkModel
A link from the robot. Contains the constant transform applied to the link and its geometry.
Definition: link_model.h:71
moveit::core
Core components of MoveIt.
Definition: kinematics_base.h:83
xmlrpc_casts.h
moveit::core::JointModelGroup
Definition: joint_model_group.h:134
tf2_eigen.h
tf2::fromMsg
void fromMsg(const A &, B &b)
moveit::core::RobotState::getFrameInfo
const Eigen::Isometry3d & getFrameInfo(const std::string &frame_id, const LinkModel *&robot_link, bool &frame_found) const
Get the transformation matrix from the model frame (root of model) to the frame identified by frame_i...
Definition: robot_state.cpp:1160
kinematic_constraints::resolveConstraintFrames
bool resolveConstraintFrames(const moveit::core::RobotState &state, moveit_msgs::Constraints &constraints)
Resolves frames used in constraints to links in the robot model.
Definition: utils.cpp:533
kinematic_constraints
Representation and evaluation of kinematic constraints.
Definition: kinematic_constraint.h:52
tf2::Quaternion::setRPY
void setRPY(const tf2Scalar &roll, const tf2Scalar &pitch, const tf2Scalar &yaw)
kinematic_constraints::constructConstraints
bool constructConstraints(XmlRpc::XmlRpcValue &params, moveit_msgs::Constraints &constraints)
extract constraint message from XmlRpc node.
Definition: utils.cpp:523
ROS_ERROR_NAMED
#define ROS_ERROR_NAMED(name,...)
moveit::core::RobotState
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:155
moveit::core::RobotState::copyJointGroupPositions
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:757
moveit::core::RobotState::getGlobalLinkTransform
const Eigen::Isometry3d & getGlobalLinkTransform(const std::string &link_name)
Get the link transform w.r.t. the root link (model frame) of the RobotModel. This is typically the ro...
Definition: robot_state.h:1453
transform
template Halfspace< double > transform(const Halfspace< double > &a, const Transform3< double > &tf)
kinematic_constraints::collectConstraints
static bool collectConstraints(XmlRpc::XmlRpcValue &params, moveit_msgs::Constraints &constraints)
Definition: utils.cpp:480
moveit::core::JointModelGroup::getVariableNames
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...
Definition: joint_model_group.h:254
utils.h
moveit::core::parseDouble
double parseDouble(XmlRpc::XmlRpcValue &v)
parse a double value from a scalar XmlRpc
Definition: xmlrpc_casts.cpp:109
kinematic_constraints::constructPoseStamped
static bool constructPoseStamped(XmlRpc::XmlRpcValue::iterator &it, geometry_msgs::PoseStamped &pose)
Definition: utils.cpp:278
kinematic_constraints::mergeConstraints
moveit_msgs::Constraints mergeConstraints(const moveit_msgs::Constraints &first, const moveit_msgs::Constraints &second)
Merge two sets of constraints into one.
Definition: utils.cpp:50
kinematic_constraints::constructConstraint
static bool constructConstraint(XmlRpc::XmlRpcValue &params, moveit_msgs::VisibilityConstraint &constraint)
Definition: utils.cpp:443
moveit::core::LOGNAME
const std::string LOGNAME
Definition: joint_model_group.cpp:165
XmlRpc::XmlRpcValue::iterator
ValueStruct::iterator iterator
XmlRpc::XmlRpcValue::end
iterator end()
r
S r
XmlRpc::XmlRpcValue::getType
const Type & getType() const
kinematic_constraints::countIndividualConstraints
std::size_t countIndividualConstraints(const moveit_msgs::Constraints &constr)
Definition: utils.cpp:124
moveit::core::isArray
bool isArray(XmlRpc::XmlRpcValue &v, size_t size=0, const std::string &name="", const std::string &description="")
Definition: xmlrpc_casts.cpp:119
kinematic_constraints::constructGoalConstraints
moveit_msgs::Constraints constructGoalConstraints(const moveit::core::RobotState &state, const moveit::core::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
ROS_WARN_STREAM_NAMED
#define ROS_WARN_STREAM_NAMED(name, args)
XmlRpc::XmlRpcValue::TypeArray
TypeArray
solid_primitive_dims.h
add
bool add(const actionlib::TwoIntsGoal &req, actionlib::TwoIntsResult &res)
moveit::core::isStruct
bool isStruct(XmlRpc::XmlRpcValue &v, const std::vector< std::string > &keys={}, const std::string &name="")
Definition: xmlrpc_casts.cpp:131
tf2::Quaternion
tolerance
S tolerance()
tf2_geometry_msgs.h
toMsg
B toMsg(const A &a)
XmlRpc::XmlRpcValue::begin
iterator begin()
moveit::core::LinkModel::getName
const std::string & getName() const
The name of this link.
Definition: link_model.h:80
message_checks.h
XmlRpc::XmlRpcValue
fcl::Vector3d
Vec3fX< details::Vec3Data< double > > Vector3d
Definition: fcl_compat.h:89
moveit::core::isEmpty
bool isEmpty(const moveit_msgs::PlanningScene &msg)
Check if a message includes any information about a planning scene, or whether it is empty.
Definition: message_checks.cpp:107


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Tue Oct 12 2021 02:24:45