48 const std::string
LOGNAME =
"kinematic_constraint_utils";
50 moveit_msgs::Constraints
mergeConstraints(
const moveit_msgs::Constraints& first,
const moveit_msgs::Constraints& second)
52 moveit_msgs::Constraints
r;
56 for (
const moveit_msgs::JointConstraint& jc_first : first.joint_constraints)
59 for (
const moveit_msgs::JointConstraint& jc_second : second.joint_constraints)
60 if (jc_second.joint_name == jc_first.joint_name)
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);
71 "Attempted to merge incompatible constraints for joint '%s'. Discarding constraint.",
72 a.joint_name.c_str());
75 m.joint_name = a.joint_name;
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);
86 r.joint_constraints.push_back(jc_first);
90 for (
const moveit_msgs::JointConstraint& jc_second : second.joint_constraints)
93 for (
const moveit_msgs::JointConstraint& jc_first : first.joint_constraints)
94 if (jc_second.joint_name == jc_first.joint_name)
100 r.joint_constraints.push_back(jc_second);
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);
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);
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);
119 bool isEmpty(
const moveit_msgs::Constraints& constr)
126 return constr.position_constraints.size() + constr.orientation_constraints.size() +
127 constr.visibility_constraints.size() + constr.joint_constraints.size();
138 double tolerance_above)
140 moveit_msgs::Constraints goal;
141 std::vector<double> vals;
143 goal.joint_constraints.resize(vals.size());
144 for (std::size_t i = 0; i < vals.size(); ++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;
157 double tolerance_pos,
double tolerance_angle)
159 moveit_msgs::Constraints goal;
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;
173 pcm.header = pose.header;
174 pcm.constraint_region.primitive_poses.resize(1);
175 pcm.constraint_region.primitive_poses[0].position = pose.pose.position;
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;
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;
198 const std::vector<double>& tolerance_pos,
199 const std::vector<double>& tolerance_angle)
202 if (tolerance_pos.size() == 3)
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];
211 if (tolerance_angle.size() == 3)
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];
222 const geometry_msgs::QuaternionStamped& quat,
double tolerance)
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;
238 const geometry_msgs::PointStamped& goal_point,
double tolerance)
240 geometry_msgs::Point p;
248 const geometry_msgs::Point& reference_point,
249 const geometry_msgs::PointStamped& goal_point,
double tolerance)
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;
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;
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;
280 if (!
isStruct(it->second, {
"frame_id",
"position",
"orientation" }, it->first))
282 pose.header.frame_id =
static_cast<std::string
>(it->second[
"frame_id"]);
284 if (!
isArray(it->second[
"orientation"], 3,
"orientation",
"RPY values"))
286 auto& rpy = it->second[
"orientation"];
289 pose.pose.orientation =
toMsg(q);
291 if (!
isArray(it->second[
"position"], 3,
"position",
"xyz position"))
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]);
304 if (it->first ==
"type")
306 else if (it->first ==
"joint_name")
307 constraint.joint_name =
static_cast<std::string
>(it->second);
308 else if (it->first ==
"weight")
310 else if (it->first ==
"position")
314 else if (it->first ==
"tolerance")
316 constraint.tolerance_below =
parseDouble(it->second);
317 constraint.tolerance_above =
parseDouble(it->second);
319 else if (it->first ==
"tolerances")
321 if (!
isArray(it->second, 2, it->first,
"lower/upper tolerances"))
324 constraint.tolerance_below =
parseDouble(it->second[0]);
325 constraint.tolerance_above =
parseDouble(it->second[1]);
327 else if (it->first ==
"bounds")
329 if (!
isArray(it->second, 2, it->first,
"lower/upper bound"))
332 const double lower_bound =
parseDouble(it->second[0]);
333 const double upper_bound =
parseDouble(it->second[1]);
335 constraint.position = (lower_bound + upper_bound) / 2;
336 constraint.tolerance_below = constraint.position - lower_bound;
337 constraint.tolerance_above = upper_bound - constraint.position;
351 if (it->first ==
"type")
353 else if (it->first ==
"frame_id")
354 constraint.header.frame_id =
static_cast<std::string
>(it->second);
355 else if (it->first ==
"weight")
357 else if (it->first ==
"link_name")
358 constraint.link_name =
static_cast<std::string
>(it->second);
359 else if (it->first ==
"target_offset")
361 if (!
isArray(it->second, 3, it->first,
"x/y/z position"))
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]);
368 else if (it->first ==
"region")
370 if (!
isStruct(it->second, {
"x",
"y",
"z" },
"region"))
373 constraint.constraint_region.primitive_poses.emplace_back();
374 constraint.constraint_region.primitives.emplace_back();
376 geometry_msgs::Pose& region_pose = constraint.constraint_region.primitive_poses.back();
377 shape_msgs::SolidPrimitive& region_primitive = constraint.constraint_region.primitives.back();
379 region_primitive.type = shape_msgs::SolidPrimitive::BOX;
380 region_primitive.dimensions.resize(3);
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]);
395 region_pose.orientation.w = 1.0;
409 if (it->first ==
"type")
411 else if (it->first ==
"frame_id")
412 constraint.header.frame_id =
static_cast<std::string
>(it->second);
413 else if (it->first ==
"weight")
415 else if (it->first ==
"link_name")
416 constraint.link_name =
static_cast<std::string
>(it->second);
417 else if (it->first ==
"orientation")
419 if (!
isArray(it->second, 3, it->first,
"RPY values"))
424 constraint.orientation =
toMsg(q);
426 else if (it->first ==
"tolerances")
428 if (!
isArray(it->second, 3, it->first,
"xyz tolerances"))
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]);
447 if (it->first ==
"type")
449 else if (it->first ==
"weight")
451 else if (it->first ==
"target_radius")
452 constraint.target_radius =
parseDouble(it->second);
453 else if (it->first ==
"target_pose")
458 else if (it->first ==
"cone_sides")
459 constraint.cone_sides =
static_cast<int>(it->second);
460 else if (it->first ==
"sensor_pose")
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);
475 constraint.sensor_view_direction = moveit_msgs::VisibilityConstraint::SENSOR_X;
488 for (
int i = 0; i < params.
size(); ++i)
490 if (!params[i].hasMember(
"type"))
494 else if (params[i][
"type"] ==
"joint")
496 constraints.joint_constraints.emplace_back();
500 else if (params[i][
"type"] ==
"position")
502 constraints.position_constraints.emplace_back();
506 else if (params[i][
"type"] ==
"orientation")
508 constraints.orientation_constraints.emplace_back();
512 else if (params[i][
"type"] ==
"visibility")
514 constraints.visibility_constraints.emplace_back();
525 if (!
isStruct(params, {
"name",
"constraints" },
"Parameter"))
528 constraints.name =
static_cast<std::string
>(params[
"name"]);
534 for (
auto& c : constraints.position_constraints)
544 if (c.link_name != robot_link->
getName())
547 Eigen::Vector3d offset_link_name(c.target_point_offset.x, c.target_point_offset.y, c.target_point_offset.z);
548 Eigen::Vector3d offset_robot_link = robot_link_to_link_name * offset_link_name;
550 c.link_name = robot_link->
getName();
551 tf2::toMsg(offset_robot_link, c.target_point_offset);
555 for (
auto& c : constraints.orientation_constraints)
566 if (c.link_name != robot_link->
getName())
568 c.link_name = robot_link->
getName();
569 Eigen::Quaterniond link_name_to_robot_link(
transform.linear().transpose() *
571 Eigen::Quaterniond quat_target;
573 c.orientation =
tf2::toMsg(quat_target * link_name_to_robot_link);