utils.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Ioan Sucan */
00036 
00037 #include <moveit/kinematic_constraints/utils.h>
00038 #include <geometric_shapes/solid_primitive_dims.h>
00039 #include <eigen_conversions/eigen_msg.h>
00040 
00041 moveit_msgs::Constraints kinematic_constraints::mergeConstraints(const moveit_msgs::Constraints& first,
00042                                                                  const moveit_msgs::Constraints& second)
00043 {
00044   moveit_msgs::Constraints r;
00045 
00046   // add all joint constraints that are in first but not in second
00047   // and merge joint constraints that are for the same joint
00048   for (std::size_t i = 0; i < first.joint_constraints.size(); ++i)
00049   {
00050     bool add = true;
00051     for (std::size_t j = 0; j < second.joint_constraints.size(); ++j)
00052       if (second.joint_constraints[j].joint_name == first.joint_constraints[i].joint_name)
00053       {
00054         add = false;
00055         // now we merge
00056         moveit_msgs::JointConstraint m;
00057         const moveit_msgs::JointConstraint& a = first.joint_constraints[i];
00058         const moveit_msgs::JointConstraint& b = second.joint_constraints[j];
00059         double low = std::max(a.position - a.tolerance_below, b.position - b.tolerance_below);
00060         double high = std::min(a.position + a.tolerance_above, b.position + b.tolerance_above);
00061         if (low > high)
00062           logError("Attempted to merge incompatible constraints for joint '%s'. Discarding constraint.",
00063                    a.joint_name.c_str());
00064         else
00065         {
00066           m.joint_name = a.joint_name;
00067           m.position =
00068               std::max(low, std::min((a.position * a.weight + b.position * b.weight) / (a.weight + b.weight), high));
00069           m.weight = (a.weight + b.weight) / 2.0;
00070           m.tolerance_above = std::max(0.0, high - m.position);
00071           m.tolerance_below = std::max(0.0, m.position - low);
00072           r.joint_constraints.push_back(m);
00073         }
00074         break;
00075       }
00076     if (add)
00077       r.joint_constraints.push_back(first.joint_constraints[i]);
00078   }
00079 
00080   // add all joint constraints that are in second but not in first
00081   for (std::size_t i = 0; i < second.joint_constraints.size(); ++i)
00082   {
00083     bool add = true;
00084     for (std::size_t j = 0; j < first.joint_constraints.size(); ++j)
00085       if (second.joint_constraints[i].joint_name == first.joint_constraints[j].joint_name)
00086       {
00087         add = false;
00088         break;
00089       }
00090     if (add)
00091       r.joint_constraints.push_back(second.joint_constraints[i]);
00092   }
00093 
00094   // merge rest of constraints
00095   r.position_constraints = first.position_constraints;
00096   for (std::size_t i = 0; i < second.position_constraints.size(); ++i)
00097     r.position_constraints.push_back(second.position_constraints[i]);
00098 
00099   r.orientation_constraints = first.orientation_constraints;
00100   for (std::size_t i = 0; i < second.orientation_constraints.size(); ++i)
00101     r.orientation_constraints.push_back(second.orientation_constraints[i]);
00102 
00103   r.visibility_constraints = first.visibility_constraints;
00104   for (std::size_t i = 0; i < second.visibility_constraints.size(); ++i)
00105     r.visibility_constraints.push_back(second.visibility_constraints[i]);
00106 
00107   return r;
00108 }
00109 
00110 bool kinematic_constraints::isEmpty(const moveit_msgs::Constraints& constr)
00111 {
00112   return constr.position_constraints.empty() && constr.orientation_constraints.empty() &&
00113          constr.visibility_constraints.empty() && constr.joint_constraints.empty();
00114 }
00115 
00116 std::size_t kinematic_constraints::countIndividualConstraints(const moveit_msgs::Constraints& constr)
00117 {
00118   return constr.position_constraints.size() + constr.orientation_constraints.size() +
00119          constr.visibility_constraints.size() + constr.joint_constraints.size();
00120 }
00121 
00122 moveit_msgs::Constraints kinematic_constraints::constructGoalConstraints(const robot_state::RobotState& state,
00123                                                                          const robot_model::JointModelGroup* jmg,
00124                                                                          double tolerance)
00125 {
00126   return constructGoalConstraints(state, jmg, tolerance, tolerance);
00127 }
00128 
00129 moveit_msgs::Constraints kinematic_constraints::constructGoalConstraints(const robot_state::RobotState& state,
00130                                                                          const robot_model::JointModelGroup* jmg,
00131                                                                          double tolerance_below, double tolerance_above)
00132 {
00133   moveit_msgs::Constraints goal;
00134   std::vector<double> vals;
00135   state.copyJointGroupPositions(jmg, vals);
00136   goal.joint_constraints.resize(vals.size());
00137   for (std::size_t i = 0; i < vals.size(); ++i)
00138   {
00139     goal.joint_constraints[i].joint_name = jmg->getVariableNames()[i];
00140     goal.joint_constraints[i].position = vals[i];
00141     goal.joint_constraints[i].tolerance_above = tolerance_below;
00142     goal.joint_constraints[i].tolerance_below = tolerance_above;
00143     goal.joint_constraints[i].weight = 1.0;
00144   }
00145 
00146   return goal;
00147 }
00148 
00149 moveit_msgs::Constraints kinematic_constraints::constructGoalConstraints(const std::string& link_name,
00150                                                                          const geometry_msgs::PoseStamped& pose,
00151                                                                          double tolerance_pos, double tolerance_angle)
00152 {
00153   moveit_msgs::Constraints goal;
00154 
00155   goal.position_constraints.resize(1);
00156   moveit_msgs::PositionConstraint& pcm = goal.position_constraints[0];
00157   pcm.link_name = link_name;
00158   pcm.target_point_offset.x = 0;
00159   pcm.target_point_offset.y = 0;
00160   pcm.target_point_offset.z = 0;
00161   pcm.constraint_region.primitives.resize(1);
00162   shape_msgs::SolidPrimitive& bv = pcm.constraint_region.primitives[0];
00163   bv.type = shape_msgs::SolidPrimitive::SPHERE;
00164   bv.dimensions.resize(geometric_shapes::SolidPrimitiveDimCount<shape_msgs::SolidPrimitive::SPHERE>::value);
00165   bv.dimensions[shape_msgs::SolidPrimitive::SPHERE_RADIUS] = tolerance_pos;
00166 
00167   pcm.header = pose.header;
00168   pcm.constraint_region.primitive_poses.resize(1);
00169   pcm.constraint_region.primitive_poses[0].position = pose.pose.position;
00170 
00171   // orientation of constraint region does not affect anything, since it is a sphere
00172   pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
00173   pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
00174   pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
00175   pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
00176   pcm.weight = 1.0;
00177 
00178   goal.orientation_constraints.resize(1);
00179   moveit_msgs::OrientationConstraint& ocm = goal.orientation_constraints[0];
00180   ocm.link_name = link_name;
00181   ocm.header = pose.header;
00182   ocm.orientation = pose.pose.orientation;
00183   ocm.absolute_x_axis_tolerance = tolerance_angle;
00184   ocm.absolute_y_axis_tolerance = tolerance_angle;
00185   ocm.absolute_z_axis_tolerance = tolerance_angle;
00186   ocm.weight = 1.0;
00187 
00188   return goal;
00189 }
00190 
00191 moveit_msgs::Constraints kinematic_constraints::constructGoalConstraints(const std::string& link_name,
00192                                                                          const geometry_msgs::PoseStamped& pose,
00193                                                                          const std::vector<double>& tolerance_pos,
00194                                                                          const std::vector<double>& tolerance_angle)
00195 {
00196   moveit_msgs::Constraints goal = constructGoalConstraints(link_name, pose);
00197   if (tolerance_pos.size() == 3)
00198   {
00199     shape_msgs::SolidPrimitive& bv = goal.position_constraints[0].constraint_region.primitives[0];
00200     bv.type = shape_msgs::SolidPrimitive::BOX;
00201     bv.dimensions.resize(geometric_shapes::SolidPrimitiveDimCount<shape_msgs::SolidPrimitive::BOX>::value);
00202     bv.dimensions[shape_msgs::SolidPrimitive::BOX_X] = tolerance_pos[0];
00203     bv.dimensions[shape_msgs::SolidPrimitive::BOX_Y] = tolerance_pos[1];
00204     bv.dimensions[shape_msgs::SolidPrimitive::BOX_Z] = tolerance_pos[2];
00205   }
00206   if (tolerance_angle.size() == 3)
00207   {
00208     moveit_msgs::OrientationConstraint& ocm = goal.orientation_constraints[0];
00209     ocm.absolute_x_axis_tolerance = tolerance_angle[0];
00210     ocm.absolute_y_axis_tolerance = tolerance_angle[1];
00211     ocm.absolute_z_axis_tolerance = tolerance_angle[2];
00212   }
00213   return goal;
00214 }
00215 
00216 moveit_msgs::Constraints kinematic_constraints::constructGoalConstraints(const std::string& link_name,
00217                                                                          const geometry_msgs::QuaternionStamped& quat,
00218                                                                          double tolerance)
00219 {
00220   moveit_msgs::Constraints goal;
00221   goal.orientation_constraints.resize(1);
00222   moveit_msgs::OrientationConstraint& ocm = goal.orientation_constraints[0];
00223   ocm.link_name = link_name;
00224   ocm.header = quat.header;
00225   ocm.orientation = quat.quaternion;
00226   ocm.absolute_x_axis_tolerance = tolerance;
00227   ocm.absolute_y_axis_tolerance = tolerance;
00228   ocm.absolute_z_axis_tolerance = tolerance;
00229   ocm.weight = 1.0;
00230   return goal;
00231 }
00232 
00233 moveit_msgs::Constraints kinematic_constraints::constructGoalConstraints(const std::string& link_name,
00234                                                                          const geometry_msgs::PointStamped& goal_point,
00235                                                                          double tolerance)
00236 {
00237   geometry_msgs::Point p;
00238   p.x = 0;
00239   p.y = 0;
00240   p.z = 0;
00241   return constructGoalConstraints(link_name, p, goal_point, tolerance);
00242 }
00243 
00244 moveit_msgs::Constraints kinematic_constraints::constructGoalConstraints(const std::string& link_name,
00245                                                                          const geometry_msgs::Point& reference_point,
00246                                                                          const geometry_msgs::PointStamped& goal_point,
00247                                                                          double tolerance)
00248 {
00249   moveit_msgs::Constraints goal;
00250   goal.position_constraints.resize(1);
00251   moveit_msgs::PositionConstraint& pcm = goal.position_constraints[0];
00252   pcm.link_name = link_name;
00253   pcm.target_point_offset.x = reference_point.x;
00254   pcm.target_point_offset.y = reference_point.y;
00255   pcm.target_point_offset.z = reference_point.z;
00256   pcm.constraint_region.primitives.resize(1);
00257   pcm.constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE;
00258   pcm.constraint_region.primitives[0].dimensions.resize(
00259       geometric_shapes::SolidPrimitiveDimCount<shape_msgs::SolidPrimitive::SPHERE>::value);
00260   pcm.constraint_region.primitives[0].dimensions[shape_msgs::SolidPrimitive::SPHERE_RADIUS] = tolerance;
00261 
00262   pcm.header = goal_point.header;
00263   pcm.constraint_region.primitive_poses.resize(1);
00264   pcm.constraint_region.primitive_poses[0].position = goal_point.point;
00265 
00266   // orientation of constraint region does not affect anything, since it is a sphere
00267   pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
00268   pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
00269   pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
00270   pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
00271   pcm.weight = 1.0;
00272 
00273   return goal;
00274 }


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Jul 24 2017 02:20:44