40 #include <tf/transform_datatypes.h> 43 #include <boost/algorithm/string/join.hpp> 47 const std::string
LOGNAME =
"kinematic_constraint_utils";
49 moveit_msgs::Constraints
mergeConstraints(
const moveit_msgs::Constraints& first,
const moveit_msgs::Constraints& second)
51 moveit_msgs::Constraints
r;
55 for (std::size_t i = 0; i < first.joint_constraints.size(); ++i)
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)
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);
70 "Attempted to merge incompatible constraints for joint '%s'. Discarding constraint.",
71 a.joint_name.c_str());
74 m.joint_name = a.joint_name;
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);
85 r.joint_constraints.push_back(first.joint_constraints[i]);
89 for (std::size_t i = 0; i < second.joint_constraints.size(); ++i)
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)
99 r.joint_constraints.push_back(second.joint_constraints[i]);
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]);
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]);
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]);
118 bool isEmpty(
const moveit_msgs::Constraints& constr)
120 return constr.position_constraints.empty() && constr.orientation_constraints.empty() &&
121 constr.visibility_constraints.empty() && constr.joint_constraints.empty();
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_below;
149 goal.joint_constraints[i].tolerance_below = tolerance_above;
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;
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;
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(
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;
281 return static_cast<double>(v);
283 return static_cast<int>(v);
289 const std::string& description =
"")
309 for (
const std::string& key : keys)
313 ROS_WARN_STREAM_NAMED(LOGNAME, name <<
" is not a struct with keys " << boost::join(keys,
",") <<
" (misses " 323 if (!
isStruct(it->second, {
"frame_id",
"position",
"orientation" }, it->first))
325 pose.header.frame_id =
static_cast<std::string
>(it->second[
"frame_id"]);
327 if (!
isArray(it->second[
"orientation"], 3,
"orientation",
"RPY values"))
329 pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(
parseDouble(it->second[
"orientation"][0]),
333 if (!
isArray(it->second[
"position"], 3,
"position",
"xyz position"))
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]);
346 if (it->first ==
"type")
348 else if (it->first ==
"joint_name")
349 constraint.joint_name =
static_cast<std::string
>(it->second);
350 else if (it->first ==
"weight")
352 else if (it->first ==
"position")
356 else if (it->first ==
"tolerance")
358 constraint.tolerance_below =
parseDouble(it->second);
359 constraint.tolerance_above =
parseDouble(it->second);
361 else if (it->first ==
"tolerances")
363 if (!
isArray(it->second, 2, it->first,
"lower/upper tolerances"))
366 constraint.tolerance_below =
parseDouble(it->second[0]);
367 constraint.tolerance_above =
parseDouble(it->second[1]);
369 else if (it->first ==
"bounds")
371 if (!
isArray(it->second, 2, it->first,
"lower/upper bound"))
374 const double lower_bound =
parseDouble(it->second[0]);
375 const double upper_bound =
parseDouble(it->second[1]);
377 constraint.position = (lower_bound + upper_bound) / 2;
378 constraint.tolerance_below = constraint.position - lower_bound;
379 constraint.tolerance_above = upper_bound - constraint.position;
393 if (it->first ==
"type")
395 else if (it->first ==
"frame_id")
396 constraint.header.frame_id =
static_cast<std::string
>(it->second);
397 else if (it->first ==
"weight")
399 else if (it->first ==
"link_name")
400 constraint.link_name =
static_cast<std::string
>(it->second);
401 else if (it->first ==
"target_offset")
403 if (!
isArray(it->second, 3, it->first,
"x/y/z position"))
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]);
410 else if (it->first ==
"region")
412 if (!
isStruct(it->second, {
"x",
"y",
"z" },
"region"))
415 constraint.constraint_region.primitive_poses.emplace_back();
416 constraint.constraint_region.primitives.emplace_back();
418 geometry_msgs::Pose& region_pose = constraint.constraint_region.primitive_poses.back();
419 shape_msgs::SolidPrimitive& region_primitive = constraint.constraint_region.primitives.back();
421 region_primitive.type = shape_msgs::SolidPrimitive::BOX;
422 region_primitive.dimensions.resize(3);
424 std::function<void(XmlRpc::XmlRpcValue&, double&, double&)> parse_dimension =
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]);
437 region_pose.orientation.w = 1.0;
451 if (it->first ==
"type")
453 else if (it->first ==
"frame_id")
454 constraint.header.frame_id =
static_cast<std::string
>(it->second);
455 else if (it->first ==
"weight")
457 else if (it->first ==
"link_name")
458 constraint.link_name =
static_cast<std::string
>(it->second);
459 else if (it->first ==
"orientation")
461 if (!
isArray(it->second, 3, it->first,
"RPY values"))
464 constraint.orientation = tf::createQuaternionMsgFromRollPitchYaw(
467 else if (it->first ==
"tolerances")
469 if (!
isArray(it->second, 3, it->first,
"xyz tolerances"))
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]);
478 ROS_WARN_STREAM_NAMED(LOGNAME,
"orientation constraint contains unknown entity '" << it->first <<
"'");
488 if (it->first ==
"type")
490 else if (it->first ==
"weight")
492 else if (it->first ==
"target_radius")
493 constraint.target_radius =
parseDouble(it->second);
494 else if (it->first ==
"target_pose")
499 else if (it->first ==
"cone_sides")
500 constraint.cone_sides =
static_cast<int>(it->second);
501 else if (it->first ==
"sensor_pose")
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);
512 ROS_WARN_STREAM_NAMED(LOGNAME,
"orientation constraint contains unknown entity '" << it->first <<
"'");
516 constraint.sensor_view_direction = moveit_msgs::VisibilityConstraint::SENSOR_X;
529 for (
int i = 0; i < params.
size(); ++i)
531 if (!params[i].hasMember(
"type"))
533 ROS_ERROR_NAMED(LOGNAME,
"constraint parameter does not specify its type");
535 else if (params[i][
"type"] ==
"joint")
537 constraints.joint_constraints.emplace_back();
541 else if (params[i][
"type"] ==
"position")
543 constraints.position_constraints.emplace_back();
547 else if (params[i][
"type"] ==
"orientation")
549 constraints.orientation_constraints.emplace_back();
553 else if (params[i][
"type"] ==
"visibility")
555 constraints.visibility_constraints.emplace_back();
566 if (!
isStruct(params, {
"name",
"constraints" },
"Parameter"))
569 constraints.name =
static_cast<std::string
>(params[
"name"]);
bool isEmpty(const moveit_msgs::Constraints &constr)
Check if any constraints were specified.
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
std::size_t countIndividualConstraints(const moveit_msgs::Constraints &constr)
static bool isStruct(XmlRpc::XmlRpcValue &v, const std::set< std::string > &keys, const std::string &name="")
static bool isArray(XmlRpc::XmlRpcValue &v, size_t size, const std::string &name="", const std::string &description="")
static bool constructConstraint(XmlRpc::XmlRpcValue ¶ms, moveit_msgs::JointConstraint &constraint)
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...
const std::string LOGNAME
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...
static bool collectConstraints(XmlRpc::XmlRpcValue ¶ms, moveit_msgs::Constraints &constraints)
static double parseDouble(XmlRpc::XmlRpcValue &v)
bool constructConstraints(XmlRpc::XmlRpcValue ¶ms, moveit_msgs::Constraints &constraints)
extract constraint message from XmlRpc node.
bool hasMember(const std::string &name) const
#define ROS_ERROR_NAMED(name,...)
Representation of a robot's state. This includes position, velocity, acceleration and effort...
moveit_msgs::Constraints mergeConstraints(const moveit_msgs::Constraints &first, const moveit_msgs::Constraints &second)
Merge two sets of constraints into one.
static bool constructPoseStamped(XmlRpc::XmlRpcValue::iterator &it, geometry_msgs::PoseStamped &pose)
#define ROS_WARN_STREAM_NAMED(name, args)