Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <moveit/kinematic_constraints/utils.h>
00038 #include <shape_tools/solid_primitive_dims.h>
00039
00040 moveit_msgs::Constraints kinematic_constraints::mergeConstraints(const moveit_msgs::Constraints &first, const moveit_msgs::Constraints &second)
00041 {
00042 moveit_msgs::Constraints r;
00043
00044
00045
00046 for (std::size_t i = 0 ; i < first.joint_constraints.size() ; ++i)
00047 {
00048 bool add = true;
00049 for (std::size_t j = 0 ; j < second.joint_constraints.size() ; ++j)
00050 if (second.joint_constraints[j].joint_name == first.joint_constraints[i].joint_name)
00051 {
00052 add = false;
00053
00054 moveit_msgs::JointConstraint m;
00055 const moveit_msgs::JointConstraint &a = first.joint_constraints[i];
00056 const moveit_msgs::JointConstraint &b = second.joint_constraints[j];
00057 double low = std::max(a.position - a.tolerance_below, b.position - b.tolerance_below);
00058 double high = std::min(a.position + a.tolerance_above, b.position + b.tolerance_above);
00059 if (low > high)
00060 logError("Attempted to merge incompatible constraints for joint '%s'. Discarding constraint.", a.joint_name.c_str());
00061 else
00062 {
00063 m.joint_name = a.joint_name;
00064 m.position = std::max(low, std::min((a.position * a.weight + b.position * b.weight) / (a.weight + b.weight), high));
00065 m.weight = (a.weight + b.weight) / 2.0;
00066 m.tolerance_above = std::max(0.0, high - m.position);
00067 m.tolerance_below = std::max(0.0, m.position - low);
00068 r.joint_constraints.push_back(m);
00069 }
00070 break;
00071 }
00072 if (add)
00073 r.joint_constraints.push_back(first.joint_constraints[i]);
00074 }
00075
00076
00077 for (std::size_t i = 0 ; i < second.joint_constraints.size() ; ++i)
00078 {
00079 bool add = true;
00080 for (std::size_t j = 0 ; j < first.joint_constraints.size() ; ++j)
00081 if (second.joint_constraints[i].joint_name == first.joint_constraints[j].joint_name)
00082 {
00083 add = false;
00084 break;
00085 }
00086 if (add)
00087 r.joint_constraints.push_back(second.joint_constraints[i]);
00088 }
00089
00090
00091 r.position_constraints = first.position_constraints;
00092 for (std::size_t i = 0 ; i < second.position_constraints.size() ; ++i)
00093 r.position_constraints.push_back(second.position_constraints[i]);
00094
00095 r.orientation_constraints = first.orientation_constraints;
00096 for (std::size_t i = 0 ; i < second.orientation_constraints.size() ; ++i)
00097 r.orientation_constraints.push_back(second.orientation_constraints[i]);
00098
00099 r.visibility_constraints = first.visibility_constraints;
00100 for (std::size_t i = 0 ; i < second.visibility_constraints.size() ; ++i)
00101 r.visibility_constraints.push_back(second.visibility_constraints[i]);
00102
00103 return r;
00104 }
00105
00106 bool kinematic_constraints::isEmpty(const moveit_msgs::Constraints &constr)
00107 {
00108 return constr.position_constraints.empty() && constr.orientation_constraints.empty() &&
00109 constr.visibility_constraints.empty() && constr.joint_constraints.empty();
00110 }
00111
00112 std::size_t kinematic_constraints::countIndividualConstraints(const moveit_msgs::Constraints &constr)
00113 {
00114 return constr.position_constraints.size() + constr.orientation_constraints.size() +
00115 constr.visibility_constraints.size() + constr.joint_constraints.size();
00116 }
00117
00118 moveit_msgs::Constraints kinematic_constraints::constructGoalConstraints(const robot_state::RobotState &state, const robot_model::JointModelGroup *jmg,
00119 double tolerance)
00120 {
00121 return constructGoalConstraints(state, jmg, tolerance, tolerance);
00122 }
00123
00124 moveit_msgs::Constraints kinematic_constraints::constructGoalConstraints(const robot_state::RobotState &state, const robot_model::JointModelGroup *jmg,
00125 double tolerance_below, double tolerance_above)
00126 {
00127 moveit_msgs::Constraints goal;
00128 std::vector<double> vals;
00129 state.copyJointGroupPositions(jmg, vals);
00130 goal.joint_constraints.resize(vals.size());
00131 for (std::size_t i = 0 ; i < vals.size() ; ++i)
00132 {
00133 goal.joint_constraints[i].joint_name = jmg->getVariableNames()[i];
00134 goal.joint_constraints[i].position = vals[i];
00135 goal.joint_constraints[i].tolerance_above = tolerance_below;
00136 goal.joint_constraints[i].tolerance_below = tolerance_above;
00137 goal.joint_constraints[i].weight = 1.0;
00138 }
00139
00140 return goal;
00141 }
00142
00143 moveit_msgs::Constraints kinematic_constraints::constructGoalConstraints(const std::string &link_name, const geometry_msgs::PoseStamped &pose,
00144 double tolerance_pos, double tolerance_angle)
00145 {
00146 moveit_msgs::Constraints goal;
00147
00148 goal.position_constraints.resize(1);
00149 moveit_msgs::PositionConstraint &pcm = goal.position_constraints[0];
00150 pcm.link_name = link_name;
00151 pcm.target_point_offset.x = 0;
00152 pcm.target_point_offset.y = 0;
00153 pcm.target_point_offset.z = 0;
00154 pcm.constraint_region.primitives.resize(1);
00155 shape_msgs::SolidPrimitive &bv = pcm.constraint_region.primitives[0];
00156 bv.type = shape_msgs::SolidPrimitive::SPHERE;
00157 bv.dimensions.resize(shape_tools::SolidPrimitiveDimCount<shape_msgs::SolidPrimitive::SPHERE>::value);
00158 bv.dimensions[shape_msgs::SolidPrimitive::SPHERE_RADIUS] = tolerance_pos;
00159
00160 pcm.header = pose.header;
00161 pcm.constraint_region.primitive_poses.resize(1);
00162 pcm.constraint_region.primitive_poses[0].position = pose.pose.position;
00163
00164
00165 pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
00166 pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
00167 pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
00168 pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
00169 pcm.weight = 1.0;
00170
00171 goal.orientation_constraints.resize(1);
00172 moveit_msgs::OrientationConstraint &ocm = goal.orientation_constraints[0];
00173 ocm.link_name = link_name;
00174 ocm.header = pose.header;
00175 ocm.orientation = pose.pose.orientation;
00176 ocm.absolute_x_axis_tolerance = tolerance_angle;
00177 ocm.absolute_y_axis_tolerance = tolerance_angle;
00178 ocm.absolute_z_axis_tolerance = tolerance_angle;
00179 ocm.weight = 1.0;
00180
00181 return goal;
00182 }
00183
00184 moveit_msgs::Constraints kinematic_constraints::constructGoalConstraints(const std::string &link_name, const geometry_msgs::PoseStamped &pose,
00185 const std::vector<double> &tolerance_pos, const std::vector<double> &tolerance_angle)
00186 {
00187 moveit_msgs::Constraints goal = constructGoalConstraints(link_name, pose);
00188 if (tolerance_pos.size() == 3)
00189 {
00190 shape_msgs::SolidPrimitive &bv = goal.position_constraints[0].constraint_region.primitives[0];
00191 bv.type = shape_msgs::SolidPrimitive::BOX;
00192 bv.dimensions.resize(shape_tools::SolidPrimitiveDimCount<shape_msgs::SolidPrimitive::BOX>::value);
00193 bv.dimensions[shape_msgs::SolidPrimitive::BOX_X] = tolerance_pos[0];
00194 bv.dimensions[shape_msgs::SolidPrimitive::BOX_Y] = tolerance_pos[1];
00195 bv.dimensions[shape_msgs::SolidPrimitive::BOX_Z] = tolerance_pos[2];
00196 }
00197 if (tolerance_angle.size() == 3)
00198 {
00199 moveit_msgs::OrientationConstraint &ocm = goal.orientation_constraints[0];
00200 ocm.absolute_x_axis_tolerance = tolerance_angle[0];
00201 ocm.absolute_y_axis_tolerance = tolerance_angle[1];
00202 ocm.absolute_z_axis_tolerance = tolerance_angle[2];
00203 }
00204 return goal;
00205 }
00206
00207 moveit_msgs::Constraints kinematic_constraints::constructGoalConstraints(const std::string &link_name, const geometry_msgs::QuaternionStamped &quat, double tolerance)
00208 {
00209 moveit_msgs::Constraints goal;
00210 goal.orientation_constraints.resize(1);
00211 moveit_msgs::OrientationConstraint &ocm = goal.orientation_constraints[0];
00212 ocm.link_name = link_name;
00213 ocm.header = quat.header;
00214 ocm.orientation = quat.quaternion;
00215 ocm.absolute_x_axis_tolerance = tolerance;
00216 ocm.absolute_y_axis_tolerance = tolerance;
00217 ocm.absolute_z_axis_tolerance = tolerance;
00218 ocm.weight = 1.0;
00219 return goal;
00220 }
00221
00222 moveit_msgs::Constraints kinematic_constraints::constructGoalConstraints(const std::string &link_name, const geometry_msgs::PointStamped &goal_point, double tolerance)
00223 {
00224 geometry_msgs::Point p;
00225 p.x = 0;
00226 p.y = 0;
00227 p.z = 0;
00228 return constructGoalConstraints(link_name, p, goal_point, tolerance );
00229 }
00230
00231 moveit_msgs::Constraints kinematic_constraints::constructGoalConstraints(const std::string &link_name, const geometry_msgs::Point &reference_point, const geometry_msgs::PointStamped &goal_point, double tolerance)
00232 {
00233 moveit_msgs::Constraints goal;
00234 goal.position_constraints.resize(1);
00235 moveit_msgs::PositionConstraint &pcm = goal.position_constraints[0];
00236 pcm.link_name = link_name;
00237 pcm.target_point_offset.x = reference_point.x;
00238 pcm.target_point_offset.y = reference_point.y;
00239 pcm.target_point_offset.z = reference_point.z;
00240 pcm.constraint_region.primitives.resize(1);
00241 pcm.constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE;
00242 pcm.constraint_region.primitives[0].dimensions.resize(shape_tools::SolidPrimitiveDimCount<shape_msgs::SolidPrimitive::SPHERE>::value);
00243 pcm.constraint_region.primitives[0].dimensions[shape_msgs::SolidPrimitive::SPHERE_RADIUS] = tolerance;
00244
00245 pcm.header = goal_point.header;
00246 pcm.constraint_region.primitive_poses.resize(1);
00247 pcm.constraint_region.primitive_poses[0].position = goal_point.point;
00248
00249
00250 pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
00251 pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
00252 pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
00253 pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
00254 pcm.weight = 1.0;
00255
00256 return goal;
00257 }