kinematic_constraint.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/kinematic_constraint.h>
00038 #include <geometric_shapes/body_operations.h>
00039 #include <geometric_shapes/shape_operations.h>
00040 #include <moveit/robot_state/conversions.h>
00041 #include <moveit/collision_detection_fcl/collision_robot_fcl.h>
00042 #include <moveit/collision_detection_fcl/collision_world_fcl.h>
00043 #include <boost/scoped_ptr.hpp>
00044 #include <boost/math/constants/constants.hpp>
00045 #include <eigen_conversions/eigen_msg.h>
00046 #include <boost/bind.hpp>
00047 #include <limits>
00048 
00049 namespace kinematic_constraints
00050 {
00051 static double normalizeAngle(double angle)
00052 {
00053   double v = fmod(angle, 2.0 * boost::math::constants::pi<double>());
00054   if (v < -boost::math::constants::pi<double>())
00055     v += 2.0 * boost::math::constants::pi<double>();
00056   else if (v > boost::math::constants::pi<double>())
00057     v -= 2.0 * boost::math::constants::pi<double>();
00058   return v;
00059 }
00060 }
00061 
00062 kinematic_constraints::KinematicConstraint::KinematicConstraint(const robot_model::RobotModelConstPtr& model)
00063   : type_(UNKNOWN_CONSTRAINT), robot_model_(model), constraint_weight_(std::numeric_limits<double>::epsilon())
00064 {
00065 }
00066 
00067 kinematic_constraints::KinematicConstraint::~KinematicConstraint()
00068 {
00069 }
00070 
00071 bool kinematic_constraints::JointConstraint::configure(const moveit_msgs::JointConstraint& jc)
00072 {
00073   // clearing before we configure to get rid of any old data
00074   clear();
00075 
00076   // testing tolerances first
00077   if (jc.tolerance_above < 0.0 || jc.tolerance_below < 0.0)
00078   {
00079     logWarn("JointConstraint tolerance values must be positive.");
00080     joint_model_ = NULL;
00081     return false;
00082   }
00083 
00084   joint_variable_name_ = jc.joint_name;
00085   local_variable_name_.clear();
00086   if (robot_model_->hasJointModel(joint_variable_name_))
00087     joint_model_ = robot_model_->getJointModel(joint_variable_name_);
00088   else
00089   {
00090     std::size_t pos = jc.joint_name.find_last_of("/");
00091     if (pos != std::string::npos)
00092     {
00093       joint_model_ = robot_model_->getJointModel(jc.joint_name.substr(0, pos));
00094       if (pos + 1 < jc.joint_name.length())
00095         local_variable_name_ = jc.joint_name.substr(pos + 1);
00096     }
00097     else
00098       joint_model_ = robot_model_->getJointModel(jc.joint_name);
00099   }
00100 
00101   if (joint_model_)
00102   {
00103     if (local_variable_name_.empty())
00104     {
00105       // check if the joint has 1 DOF (the only kind we can handle)
00106       if (joint_model_->getVariableCount() == 0)
00107       {
00108         logError("Joint '%s' has no parameters to constrain", jc.joint_name.c_str());
00109         joint_model_ = NULL;
00110       }
00111       else if (joint_model_->getVariableCount() > 1)
00112       {
00113         logError("Joint '%s' has more than one parameter to constrain. This type of constraint is not supported.",
00114                  jc.joint_name.c_str());
00115         joint_model_ = NULL;
00116       }
00117     }
00118     else
00119     {
00120       int found = -1;
00121       const std::vector<std::string>& local_var_names = joint_model_->getLocalVariableNames();
00122       for (std::size_t i = 0; i < local_var_names.size(); ++i)
00123         if (local_var_names[i] == local_variable_name_)
00124         {
00125           found = i;
00126           break;
00127         }
00128       if (found < 0)
00129       {
00130         logError("Local variable name '%s' is not known to joint '%s'", local_variable_name_.c_str(),
00131                  joint_model_->getName().c_str());
00132         joint_model_ = NULL;
00133       }
00134     }
00135   }
00136 
00137   if (joint_model_)
00138   {
00139     joint_is_continuous_ = false;
00140     joint_tolerance_above_ = jc.tolerance_above;
00141     joint_tolerance_below_ = jc.tolerance_below;
00142     joint_variable_index_ = robot_model_->getVariableIndex(joint_variable_name_);
00143 
00144     // check if we have to wrap angles when computing distances
00145     joint_is_continuous_ = false;
00146     if (joint_model_->getType() == robot_model::JointModel::REVOLUTE)
00147     {
00148       const robot_model::RevoluteJointModel* rjoint = static_cast<const robot_model::RevoluteJointModel*>(joint_model_);
00149       if (rjoint->isContinuous())
00150         joint_is_continuous_ = true;
00151     }
00152     else if (joint_model_->getType() == robot_model::JointModel::PLANAR)
00153     {
00154       if (local_variable_name_ == "theta")
00155         joint_is_continuous_ = true;
00156     }
00157 
00158     if (joint_is_continuous_)
00159     {
00160       joint_position_ = normalizeAngle(jc.position);
00161     }
00162     else
00163     {
00164       joint_position_ = jc.position;
00165       const robot_model::VariableBounds& bounds = joint_model_->getVariableBounds(joint_variable_name_);
00166 
00167       if (bounds.min_position_ > joint_position_ + joint_tolerance_above_)
00168       {
00169         joint_position_ = bounds.min_position_;
00170         joint_tolerance_above_ = std::numeric_limits<double>::epsilon();
00171         logWarn("Joint %s is constrained to be below the minimum bounds. Assuming minimum bounds instead.",
00172                 jc.joint_name.c_str());
00173       }
00174       else if (bounds.max_position_ < joint_position_ - joint_tolerance_below_)
00175       {
00176         joint_position_ = bounds.max_position_;
00177         joint_tolerance_below_ = std::numeric_limits<double>::epsilon();
00178         logWarn("Joint %s is constrained to be above the maximum bounds. Assuming maximum bounds instead.",
00179                 jc.joint_name.c_str());
00180       }
00181     }
00182 
00183     if (jc.weight <= std::numeric_limits<double>::epsilon())
00184     {
00185       logWarn("The weight on constraint for joint '%s' is very near zero.  Setting to 1.0.", jc.joint_name.c_str());
00186       constraint_weight_ = 1.0;
00187     }
00188     else
00189       constraint_weight_ = jc.weight;
00190   }
00191   return joint_model_ != NULL;
00192 }
00193 
00194 bool kinematic_constraints::JointConstraint::equal(const KinematicConstraint& other, double margin) const
00195 {
00196   if (other.getType() != type_)
00197     return false;
00198   const JointConstraint& o = static_cast<const JointConstraint&>(other);
00199   if (o.joint_model_ == joint_model_ && o.local_variable_name_ == local_variable_name_)
00200     return fabs(joint_position_ - o.joint_position_) <= margin &&
00201            fabs(joint_tolerance_above_ - o.joint_tolerance_above_) <= margin &&
00202            fabs(joint_tolerance_below_ - o.joint_tolerance_below_) <= margin;
00203   return false;
00204 }
00205 
00206 kinematic_constraints::ConstraintEvaluationResult
00207 kinematic_constraints::JointConstraint::decide(const robot_state::RobotState& state, bool verbose) const
00208 {
00209   if (!joint_model_)
00210     return ConstraintEvaluationResult(true, 0.0);
00211 
00212   double current_joint_position = state.getVariablePosition(joint_variable_index_);
00213   double dif = 0.0;
00214 
00215   // compute signed shortest distance for continuous joints
00216   if (joint_is_continuous_)
00217   {
00218     dif = normalizeAngle(current_joint_position) - joint_position_;
00219 
00220     if (dif > boost::math::constants::pi<double>())
00221       dif = 2.0 * boost::math::constants::pi<double>() - dif;
00222     else if (dif < -boost::math::constants::pi<double>())
00223       dif += 2.0 * boost::math::constants::pi<double>();  // we include a sign change to have dif > 0
00224   }
00225   else
00226     dif = current_joint_position - joint_position_;
00227 
00228   // check bounds
00229   bool result = dif <= (joint_tolerance_above_ + 2.0 * std::numeric_limits<double>::epsilon()) &&
00230                 dif >= (-joint_tolerance_below_ - 2.0 * std::numeric_limits<double>::epsilon());
00231   if (verbose)
00232     logInform("Constraint %s:: Joint name: '%s', actual value: %f, desired value: %f, tolerance_above: %f, "
00233               "tolerance_below: %f",
00234               result ? "satisfied" : "violated", joint_variable_name_.c_str(), current_joint_position, joint_position_,
00235               joint_tolerance_above_, joint_tolerance_below_);
00236   return ConstraintEvaluationResult(result, constraint_weight_ * fabs(dif));
00237 }
00238 
00239 bool kinematic_constraints::JointConstraint::enabled() const
00240 {
00241   return joint_model_;
00242 }
00243 
00244 void kinematic_constraints::JointConstraint::clear()
00245 {
00246   joint_model_ = NULL;
00247   joint_variable_index_ = -1;
00248   joint_is_continuous_ = false;
00249   local_variable_name_ = "";
00250   joint_variable_name_ = "";
00251   joint_position_ = joint_tolerance_below_ = joint_tolerance_above_ = 0.0;
00252 }
00253 
00254 void kinematic_constraints::JointConstraint::print(std::ostream& out) const
00255 {
00256   if (joint_model_)
00257   {
00258     out << "Joint constraint for joint " << joint_variable_name_ << ": " << std::endl;
00259     out << "  value = ";
00260     out << joint_position_ << "; ";
00261     out << "  tolerance below = ";
00262     out << joint_tolerance_below_ << "; ";
00263     out << "  tolerance above = ";
00264     out << joint_tolerance_above_ << "; ";
00265     out << std::endl;
00266   }
00267   else
00268     out << "No constraint" << std::endl;
00269 }
00270 
00271 bool kinematic_constraints::PositionConstraint::configure(const moveit_msgs::PositionConstraint& pc,
00272                                                           const robot_state::Transforms& tf)
00273 {
00274   // clearing before we configure to get rid of any old data
00275   clear();
00276 
00277   link_model_ = robot_model_->getLinkModel(pc.link_name);
00278   if (link_model_ == NULL)
00279   {
00280     logWarn("Position constraint link model %s not found in kinematic model.  Constraint invalid.",
00281             pc.link_name.c_str());
00282     return false;
00283   }
00284 
00285   if (pc.header.frame_id.empty())
00286   {
00287     logWarn("No frame specified for position constraint on link '%s'!", pc.link_name.c_str());
00288     return false;
00289   }
00290 
00291   offset_ = Eigen::Vector3d(pc.target_point_offset.x, pc.target_point_offset.y, pc.target_point_offset.z);
00292   has_offset_ = offset_.squaredNorm() > std::numeric_limits<double>::epsilon();
00293 
00294   if (tf.isFixedFrame(pc.header.frame_id))
00295   {
00296     constraint_frame_id_ = tf.getTargetFrame();
00297     mobile_frame_ = false;
00298   }
00299   else
00300   {
00301     constraint_frame_id_ = pc.header.frame_id;
00302     mobile_frame_ = true;
00303   }
00304 
00305   // load primitive shapes, first clearing any we already have
00306   for (std::size_t i = 0; i < pc.constraint_region.primitives.size(); ++i)
00307   {
00308     boost::scoped_ptr<shapes::Shape> shape(shapes::constructShapeFromMsg(pc.constraint_region.primitives[i]));
00309     if (shape)
00310     {
00311       if (pc.constraint_region.primitive_poses.size() <= i)
00312       {
00313         logWarn("Constraint region message does not contain enough primitive poses");
00314         continue;
00315       }
00316       constraint_region_.push_back(bodies::BodyPtr(bodies::createBodyFromShape(shape.get())));
00317       Eigen::Affine3d t;
00318       tf::poseMsgToEigen(pc.constraint_region.primitive_poses[i], t);
00319       constraint_region_pose_.push_back(t);
00320       if (mobile_frame_)
00321         constraint_region_.back()->setPose(constraint_region_pose_.back());
00322       else
00323       {
00324         tf.transformPose(pc.header.frame_id, constraint_region_pose_.back(), constraint_region_pose_.back());
00325         constraint_region_.back()->setPose(constraint_region_pose_.back());
00326       }
00327     }
00328     else
00329       logWarn("Could not construct primitive shape %d", i);
00330   }
00331 
00332   // load meshes
00333   for (std::size_t i = 0; i < pc.constraint_region.meshes.size(); ++i)
00334   {
00335     boost::scoped_ptr<shapes::Shape> shape(shapes::constructShapeFromMsg(pc.constraint_region.meshes[i]));
00336     if (shape)
00337     {
00338       if (pc.constraint_region.mesh_poses.size() <= i)
00339       {
00340         logWarn("Constraint region message does not contain enough primitive poses");
00341         continue;
00342       }
00343       constraint_region_.push_back(bodies::BodyPtr(bodies::createBodyFromShape(shape.get())));
00344       Eigen::Affine3d t;
00345       tf::poseMsgToEigen(pc.constraint_region.mesh_poses[i], t);
00346       constraint_region_pose_.push_back(t);
00347       if (mobile_frame_)
00348         constraint_region_.back()->setPose(constraint_region_pose_.back());
00349       else
00350       {
00351         tf.transformPose(pc.header.frame_id, constraint_region_pose_.back(), constraint_region_pose_.back());
00352         constraint_region_.back()->setPose(constraint_region_pose_.back());
00353       }
00354     }
00355     else
00356     {
00357       logWarn("Could not construct mesh shape %d", i);
00358     }
00359   }
00360 
00361   if (pc.weight <= std::numeric_limits<double>::epsilon())
00362   {
00363     logWarn("The weight on position constraint for link '%s' is near zero.  Setting to 1.0.", pc.link_name.c_str());
00364     constraint_weight_ = 1.0;
00365   }
00366   else
00367     constraint_weight_ = pc.weight;
00368 
00369   return !constraint_region_.empty();
00370 }
00371 
00372 void kinematic_constraints::PositionConstraint::swapLinkModel(const robot_model::LinkModel* new_link,
00373                                                               const Eigen::Affine3d& update)
00374 {
00375   if (!enabled())
00376     return;
00377   link_model_ = new_link;
00378   for (std::size_t i = 0; i < constraint_region_pose_.size(); ++i)
00379     constraint_region_pose_[i] = constraint_region_pose_[i] * update;
00380 }
00381 
00382 bool kinematic_constraints::PositionConstraint::equal(const KinematicConstraint& other, double margin) const
00383 {
00384   if (other.getType() != type_)
00385     return false;
00386   const PositionConstraint& o = static_cast<const PositionConstraint&>(other);
00387 
00388   if (link_model_ == o.link_model_ && robot_state::Transforms::sameFrame(constraint_frame_id_, o.constraint_frame_id_))
00389   {
00390     if ((offset_ - o.offset_).norm() > margin)
00391       return false;
00392     std::vector<bool> other_region_matches_this(constraint_region_.size(), false);
00393     for (std::size_t i = 0; i < constraint_region_.size(); ++i)
00394     {
00395       bool some_match = false;
00396       // need to check against all other regions
00397       for (std::size_t j = 0; j < o.constraint_region_.size(); ++j)
00398       {
00399         Eigen::Affine3d diff = constraint_region_pose_[i].inverse() * o.constraint_region_pose_[j];
00400         if (diff.translation().norm() < margin && diff.rotation().isIdentity(margin) &&
00401             constraint_region_[i]->getType() == o.constraint_region_[j]->getType() &&
00402             fabs(constraint_region_[i]->computeVolume() - o.constraint_region_[j]->computeVolume()) < margin)
00403         {
00404           some_match = true;
00405           // can't break, as need to do matches the other way as well
00406           other_region_matches_this[j] = true;
00407         }
00408       }
00409       if (!some_match)
00410         return false;
00411     }
00412     for (std::size_t i = 0; i < o.constraint_region_.size(); ++i)
00413       if (!other_region_matches_this[i])
00414         return false;
00415     return true;
00416   }
00417   return false;
00418 }
00419 
00420 namespace kinematic_constraints
00421 {
00422 // helper function to avoid code duplication
00423 static inline kinematic_constraints::ConstraintEvaluationResult
00424 finishPositionConstraintDecision(const Eigen::Vector3d& pt, const Eigen::Vector3d& desired, const std::string& name,
00425                                  double weight, bool result, bool verbose)
00426 {
00427   double dx = desired.x() - pt.x();
00428   double dy = desired.y() - pt.y();
00429   double dz = desired.z() - pt.z();
00430   if (verbose)
00431   {
00432     logInform("Position constraint %s on link '%s'. Desired: %f, %f, %f, current: %f, %f, %f",
00433               result ? "satisfied" : "violated", name.c_str(), desired.x(), desired.y(), desired.z(), pt.x(), pt.y(),
00434               pt.z());
00435     logInform("Differences %g %g %g", dx, dy, dz);
00436   }
00437   return ConstraintEvaluationResult(result, weight * sqrt(dx * dx + dy * dy + dz * dz));
00438 }
00439 }
00440 
00441 kinematic_constraints::ConstraintEvaluationResult
00442 kinematic_constraints::PositionConstraint::decide(const robot_state::RobotState& state, bool verbose) const
00443 {
00444   if (!link_model_ || constraint_region_.empty())
00445     return ConstraintEvaluationResult(true, 0.0);
00446 
00447   Eigen::Vector3d pt = state.getGlobalLinkTransform(link_model_) * offset_;
00448   if (mobile_frame_)
00449   {
00450     for (std::size_t i = 0; i < constraint_region_.size(); ++i)
00451     {
00452       Eigen::Affine3d tmp = state.getFrameTransform(constraint_frame_id_) * constraint_region_pose_[i];
00453       bool result = constraint_region_[i]->cloneAt(tmp)->containsPoint(pt, verbose);
00454       if (result || (i + 1 == constraint_region_pose_.size()))
00455         return finishPositionConstraintDecision(pt, tmp.translation(), link_model_->getName(), constraint_weight_,
00456                                                 result, verbose);
00457       else
00458         finishPositionConstraintDecision(pt, tmp.translation(), link_model_->getName(), constraint_weight_, result,
00459                                          verbose);
00460     }
00461   }
00462   else
00463   {
00464     for (std::size_t i = 0; i < constraint_region_.size(); ++i)
00465     {
00466       bool result = constraint_region_[i]->containsPoint(pt, true);
00467       if (result || (i + 1 == constraint_region_.size()))
00468         return finishPositionConstraintDecision(pt, constraint_region_[i]->getPose().translation(),
00469                                                 link_model_->getName(), constraint_weight_, result, verbose);
00470       else
00471         finishPositionConstraintDecision(pt, constraint_region_[i]->getPose().translation(), link_model_->getName(),
00472                                          constraint_weight_, result, verbose);
00473     }
00474   }
00475   return ConstraintEvaluationResult(false, 0.0);
00476 }
00477 
00478 void kinematic_constraints::PositionConstraint::print(std::ostream& out) const
00479 {
00480   if (enabled())
00481     out << "Position constraint on link '" << link_model_->getName() << "'" << std::endl;
00482   else
00483     out << "No constraint" << std::endl;
00484 }
00485 
00486 void kinematic_constraints::PositionConstraint::clear()
00487 {
00488   offset_ = Eigen::Vector3d(0.0, 0.0, 0.0);
00489   has_offset_ = false;
00490   constraint_region_.clear();
00491   constraint_region_pose_.clear();
00492   mobile_frame_ = false;
00493   constraint_frame_id_ = "";
00494   link_model_ = NULL;
00495 }
00496 
00497 bool kinematic_constraints::PositionConstraint::enabled() const
00498 {
00499   return link_model_ && !constraint_region_.empty();
00500 }
00501 
00502 bool kinematic_constraints::OrientationConstraint::configure(const moveit_msgs::OrientationConstraint& oc,
00503                                                              const robot_state::Transforms& tf)
00504 {
00505   // clearing out any old data
00506   clear();
00507 
00508   link_model_ = robot_model_->getLinkModel(oc.link_name);
00509   if (!link_model_)
00510   {
00511     logWarn("Could not find link model for link name %s", oc.link_name.c_str());
00512     return false;
00513   }
00514   Eigen::Quaterniond q;
00515   tf::quaternionMsgToEigen(oc.orientation, q);
00516   if (fabs(q.norm() - 1.0) > 1e-3)
00517   {
00518     logWarn("Orientation constraint for link '%s' is probably incorrect: %f, %f, %f, %f. Assuming identity instead.",
00519             oc.link_name.c_str(), oc.orientation.x, oc.orientation.y, oc.orientation.z, oc.orientation.w);
00520     q = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0);
00521   }
00522 
00523   if (oc.header.frame_id.empty())
00524     logWarn("No frame specified for position constraint on link '%s'!", oc.link_name.c_str());
00525 
00526   if (tf.isFixedFrame(oc.header.frame_id))
00527   {
00528     tf.transformQuaternion(oc.header.frame_id, q, q);
00529     desired_rotation_frame_id_ = tf.getTargetFrame();
00530     desired_rotation_matrix_ = Eigen::Matrix3d(q);
00531     desired_rotation_matrix_inv_ = desired_rotation_matrix_.inverse();
00532     mobile_frame_ = false;
00533   }
00534   else
00535   {
00536     desired_rotation_frame_id_ = oc.header.frame_id;
00537     desired_rotation_matrix_ = Eigen::Matrix3d(q);
00538     mobile_frame_ = true;
00539   }
00540   std::stringstream matrix_str;
00541   matrix_str << desired_rotation_matrix_;
00542   logDebug("The desired rotation matrix for link '%s' in frame %s is:\n%s", oc.link_name.c_str(),
00543            desired_rotation_frame_id_.c_str(), matrix_str.str().c_str());
00544 
00545   if (oc.weight <= std::numeric_limits<double>::epsilon())
00546   {
00547     logWarn("The weight on position constraint for link '%s' is near zero.  Setting to 1.0.", oc.link_name.c_str());
00548     constraint_weight_ = 1.0;
00549   }
00550   else
00551     constraint_weight_ = oc.weight;
00552   absolute_x_axis_tolerance_ = fabs(oc.absolute_x_axis_tolerance);
00553   if (absolute_x_axis_tolerance_ < std::numeric_limits<double>::epsilon())
00554     logWarn("Near-zero value for absolute_x_axis_tolerance");
00555   absolute_y_axis_tolerance_ = fabs(oc.absolute_y_axis_tolerance);
00556   if (absolute_y_axis_tolerance_ < std::numeric_limits<double>::epsilon())
00557     logWarn("Near-zero value for absolute_y_axis_tolerance");
00558   absolute_z_axis_tolerance_ = fabs(oc.absolute_z_axis_tolerance);
00559   if (absolute_z_axis_tolerance_ < std::numeric_limits<double>::epsilon())
00560     logWarn("Near-zero value for absolute_z_axis_tolerance");
00561 
00562   return link_model_ != NULL;
00563 }
00564 
00565 void kinematic_constraints::OrientationConstraint::swapLinkModel(const robot_model::LinkModel* new_link,
00566                                                                  const Eigen::Matrix3d& update)
00567 {
00568   if (!enabled())
00569     return;
00570   link_model_ = new_link;
00571   desired_rotation_matrix_ = desired_rotation_matrix_ * update;
00572   desired_rotation_matrix_inv_ = desired_rotation_matrix_.inverse();
00573 }
00574 
00575 bool kinematic_constraints::OrientationConstraint::equal(const KinematicConstraint& other, double margin) const
00576 {
00577   if (other.getType() != type_)
00578     return false;
00579   const OrientationConstraint& o = static_cast<const OrientationConstraint&>(other);
00580 
00581   if (o.link_model_ == link_model_ &&
00582       robot_state::Transforms::sameFrame(desired_rotation_frame_id_, o.desired_rotation_frame_id_))
00583   {
00584     Eigen::Matrix3d diff = desired_rotation_matrix_.inverse() * o.desired_rotation_matrix_;
00585     if (!diff.isIdentity(margin))
00586       return false;
00587     return fabs(absolute_x_axis_tolerance_ - o.absolute_x_axis_tolerance_) <= margin &&
00588            fabs(absolute_y_axis_tolerance_ - o.absolute_y_axis_tolerance_) <= margin &&
00589            fabs(absolute_z_axis_tolerance_ - o.absolute_z_axis_tolerance_) <= margin;
00590   }
00591   return false;
00592 }
00593 
00594 void kinematic_constraints::OrientationConstraint::clear()
00595 {
00596   link_model_ = NULL;
00597   desired_rotation_matrix_ = Eigen::Matrix3d::Identity();
00598   desired_rotation_matrix_inv_ = Eigen::Matrix3d::Identity();
00599   desired_rotation_frame_id_ = "";
00600   mobile_frame_ = false;
00601   absolute_z_axis_tolerance_ = absolute_y_axis_tolerance_ = absolute_x_axis_tolerance_ = 0.0;
00602 }
00603 
00604 bool kinematic_constraints::OrientationConstraint::enabled() const
00605 {
00606   return link_model_;
00607 }
00608 
00609 kinematic_constraints::ConstraintEvaluationResult
00610 kinematic_constraints::OrientationConstraint::decide(const robot_state::RobotState& state, bool verbose) const
00611 {
00612   if (!link_model_)
00613     return ConstraintEvaluationResult(true, 0.0);
00614 
00615   Eigen::Vector3d xyz;
00616   if (mobile_frame_)
00617   {
00618     Eigen::Matrix3d tmp = state.getFrameTransform(desired_rotation_frame_id_).rotation() * desired_rotation_matrix_;
00619     Eigen::Affine3d diff(tmp.inverse() * state.getGlobalLinkTransform(link_model_).rotation());
00620     xyz = diff.rotation().eulerAngles(0, 1, 2);
00621     // 0,1,2 corresponds to XYZ, the convention used in sampling constraints
00622   }
00623   else
00624   {
00625     Eigen::Affine3d diff(desired_rotation_matrix_inv_ * state.getGlobalLinkTransform(link_model_).rotation());
00626     xyz =
00627         diff.rotation().eulerAngles(0, 1, 2);  // 0,1,2 corresponds to XYZ, the convention used in sampling constraints
00628   }
00629 
00630   xyz(0) = std::min(fabs(xyz(0)), boost::math::constants::pi<double>() - fabs(xyz(0)));
00631   xyz(1) = std::min(fabs(xyz(1)), boost::math::constants::pi<double>() - fabs(xyz(1)));
00632   xyz(2) = std::min(fabs(xyz(2)), boost::math::constants::pi<double>() - fabs(xyz(2)));
00633   bool result = xyz(2) < absolute_z_axis_tolerance_ + std::numeric_limits<double>::epsilon() &&
00634                 xyz(1) < absolute_y_axis_tolerance_ + std::numeric_limits<double>::epsilon() &&
00635                 xyz(0) < absolute_x_axis_tolerance_ + std::numeric_limits<double>::epsilon();
00636 
00637   if (verbose)
00638   {
00639     Eigen::Quaterniond q_act(state.getGlobalLinkTransform(link_model_).rotation());
00640     Eigen::Quaterniond q_des(desired_rotation_matrix_);
00641     logInform("Orientation constraint %s for link '%s'. Quaternion desired: %f %f %f %f, quaternion actual: %f %f %f "
00642               "%f, error: x=%f, y=%f, z=%f, tolerance: x=%f, y=%f, z=%f",
00643               result ? "satisfied" : "violated", link_model_->getName().c_str(), q_des.x(), q_des.y(), q_des.z(),
00644               q_des.w(), q_act.x(), q_act.y(), q_act.z(), q_act.w(), xyz(0), xyz(1), xyz(2), absolute_x_axis_tolerance_,
00645               absolute_y_axis_tolerance_, absolute_z_axis_tolerance_);
00646   }
00647 
00648   return ConstraintEvaluationResult(result, constraint_weight_ * (xyz(0) + xyz(1) + xyz(2)));
00649 }
00650 
00651 void kinematic_constraints::OrientationConstraint::print(std::ostream& out) const
00652 {
00653   if (link_model_)
00654   {
00655     out << "Orientation constraint on link '" << link_model_->getName() << "'" << std::endl;
00656     Eigen::Quaterniond q_des(desired_rotation_matrix_);
00657     out << "Desired orientation:" << q_des.x() << "," << q_des.y() << "," << q_des.z() << "," << q_des.w() << std::endl;
00658   }
00659   else
00660     out << "No constraint" << std::endl;
00661 }
00662 
00663 kinematic_constraints::VisibilityConstraint::VisibilityConstraint(const robot_model::RobotModelConstPtr& model)
00664   : KinematicConstraint(model), collision_robot_(new collision_detection::CollisionRobotFCL(model))
00665 {
00666   type_ = VISIBILITY_CONSTRAINT;
00667 }
00668 
00669 void kinematic_constraints::VisibilityConstraint::clear()
00670 {
00671   mobile_sensor_frame_ = false;
00672   mobile_target_frame_ = false;
00673   target_frame_id_ = "";
00674   sensor_frame_id_ = "";
00675   sensor_pose_ = Eigen::Affine3d::Identity();
00676   sensor_view_direction_ = 0;
00677   target_pose_ = Eigen::Affine3d::Identity();
00678   cone_sides_ = 0;
00679   points_.clear();
00680   target_radius_ = -1.0;
00681   max_view_angle_ = 0.0;
00682   max_range_angle_ = 0.0;
00683 }
00684 
00685 bool kinematic_constraints::VisibilityConstraint::configure(const moveit_msgs::VisibilityConstraint& vc,
00686                                                             const robot_state::Transforms& tf)
00687 {
00688   clear();
00689   target_radius_ = fabs(vc.target_radius);
00690 
00691   if (vc.target_radius <= std::numeric_limits<double>::epsilon())
00692     logWarn("The radius of the target disc that must be visible should be strictly positive");
00693 
00694   if (vc.cone_sides < 3)
00695   {
00696     logWarn("The number of sides for the visibility region must be 3 or more. Assuming 3 sides instead of the "
00697             "specified %d",
00698             vc.cone_sides);
00699     cone_sides_ = 3;
00700   }
00701   else
00702     cone_sides_ = vc.cone_sides;
00703 
00704   // compute the points on the base circle of the cone that make up the cone sides
00705   points_.clear();
00706   double delta = 2.0 * boost::math::constants::pi<double>() / (double)cone_sides_;
00707   double a = 0.0;
00708   for (unsigned int i = 0; i < cone_sides_; ++i, a += delta)
00709   {
00710     double x = sin(a) * target_radius_;
00711     double y = cos(a) * target_radius_;
00712     points_.push_back(Eigen::Vector3d(x, y, 0.0));
00713   }
00714 
00715   tf::poseMsgToEigen(vc.target_pose.pose, target_pose_);
00716 
00717   if (tf.isFixedFrame(vc.target_pose.header.frame_id))
00718   {
00719     tf.transformPose(vc.target_pose.header.frame_id, target_pose_, target_pose_);
00720     target_frame_id_ = tf.getTargetFrame();
00721     mobile_target_frame_ = false;
00722     // transform won't change, so apply it now
00723     for (std::size_t i = 0; i < points_.size(); ++i)
00724       points_[i] = target_pose_ * points_[i];
00725   }
00726   else
00727   {
00728     target_frame_id_ = vc.target_pose.header.frame_id;
00729     mobile_target_frame_ = true;
00730   }
00731 
00732   tf::poseMsgToEigen(vc.sensor_pose.pose, sensor_pose_);
00733 
00734   if (tf.isFixedFrame(vc.sensor_pose.header.frame_id))
00735   {
00736     tf.transformPose(vc.sensor_pose.header.frame_id, sensor_pose_, sensor_pose_);
00737     sensor_frame_id_ = tf.getTargetFrame();
00738     mobile_sensor_frame_ = false;
00739   }
00740   else
00741   {
00742     sensor_frame_id_ = vc.sensor_pose.header.frame_id;
00743     mobile_sensor_frame_ = true;
00744   }
00745 
00746   if (vc.weight <= std::numeric_limits<double>::epsilon())
00747   {
00748     logWarn("The weight of visibility constraint is near zero.  Setting to 1.0.");
00749     constraint_weight_ = 1.0;
00750   }
00751   else
00752     constraint_weight_ = vc.weight;
00753 
00754   max_view_angle_ = vc.max_view_angle;
00755   max_range_angle_ = vc.max_range_angle;
00756   sensor_view_direction_ = vc.sensor_view_direction;
00757 
00758   return target_radius_ > std::numeric_limits<double>::epsilon();
00759 }
00760 
00761 bool kinematic_constraints::VisibilityConstraint::equal(const KinematicConstraint& other, double margin) const
00762 {
00763   if (other.getType() != type_)
00764     return false;
00765   const VisibilityConstraint& o = static_cast<const VisibilityConstraint&>(other);
00766 
00767   if (robot_state::Transforms::sameFrame(target_frame_id_, o.target_frame_id_) &&
00768       robot_state::Transforms::sameFrame(sensor_frame_id_, o.sensor_frame_id_) && cone_sides_ == o.cone_sides_ &&
00769       sensor_view_direction_ == o.sensor_view_direction_)
00770   {
00771     if (fabs(max_view_angle_ - o.max_view_angle_) > margin || fabs(target_radius_ - o.target_radius_) > margin)
00772       return false;
00773     Eigen::Affine3d diff = sensor_pose_.inverse() * o.sensor_pose_;
00774     if (diff.translation().norm() > margin)
00775       return false;
00776     if (!diff.rotation().isIdentity(margin))
00777       return false;
00778     diff = target_pose_.inverse() * o.target_pose_;
00779     if (diff.translation().norm() > margin)
00780       return false;
00781     if (!diff.rotation().isIdentity(margin))
00782       return false;
00783     return true;
00784   }
00785   return false;
00786 }
00787 
00788 bool kinematic_constraints::VisibilityConstraint::enabled() const
00789 {
00790   return target_radius_ > std::numeric_limits<double>::epsilon();
00791 }
00792 
00793 shapes::Mesh* kinematic_constraints::VisibilityConstraint::getVisibilityCone(const robot_state::RobotState& state) const
00794 {
00795   // the current pose of the sensor
00796 
00797   const Eigen::Affine3d& sp =
00798       mobile_sensor_frame_ ? state.getFrameTransform(sensor_frame_id_) * sensor_pose_ : sensor_pose_;
00799   const Eigen::Affine3d& tp =
00800       mobile_target_frame_ ? state.getFrameTransform(target_frame_id_) * target_pose_ : target_pose_;
00801 
00802   // transform the points on the disc to the desired target frame
00803   const EigenSTL::vector_Vector3d* points = &points_;
00804   boost::scoped_ptr<EigenSTL::vector_Vector3d> tempPoints;
00805   if (mobile_target_frame_)
00806   {
00807     tempPoints.reset(new EigenSTL::vector_Vector3d(points_.size()));
00808     for (std::size_t i = 0; i < points_.size(); ++i)
00809       tempPoints->at(i) = tp * points_[i];
00810     points = tempPoints.get();
00811   }
00812 
00813   // allocate memory for a mesh to represent the visibility cone
00814   shapes::Mesh* m = new shapes::Mesh();
00815   m->vertex_count = cone_sides_ + 2;
00816   m->vertices = new double[m->vertex_count * 3];
00817   m->triangle_count = cone_sides_ * 2;
00818   m->triangles = new unsigned int[m->triangle_count * 3];
00819   // we do NOT allocate normals because we do not compute them
00820 
00821   // the sensor origin
00822   m->vertices[0] = sp.translation().x();
00823   m->vertices[1] = sp.translation().y();
00824   m->vertices[2] = sp.translation().z();
00825 
00826   // the center of the base of the cone approximation
00827   m->vertices[3] = tp.translation().x();
00828   m->vertices[4] = tp.translation().y();
00829   m->vertices[5] = tp.translation().z();
00830 
00831   // the points that approximate the base disc
00832   for (std::size_t i = 0; i < points->size(); ++i)
00833   {
00834     m->vertices[i * 3 + 6] = points->at(i).x();
00835     m->vertices[i * 3 + 7] = points->at(i).y();
00836     m->vertices[i * 3 + 8] = points->at(i).z();
00837   }
00838 
00839   // add the triangles
00840   std::size_t p3 = points->size() * 3;
00841   for (std::size_t i = 1; i < points->size(); ++i)
00842   {
00843     // triangle forming a side of the cone, using the sensor origin
00844     std::size_t i3 = (i - 1) * 3;
00845     m->triangles[i3] = i + 1;
00846     m->triangles[i3 + 1] = 0;
00847     m->triangles[i3 + 2] = i + 2;
00848     // triangle forming a part of the base of the cone, using the center of the base
00849     std::size_t i6 = p3 + i3;
00850     m->triangles[i6] = i + 1;
00851     m->triangles[i6 + 1] = 1;
00852     m->triangles[i6 + 2] = i + 2;
00853   }
00854 
00855   // last triangles
00856   m->triangles[p3 - 3] = points->size() + 1;
00857   m->triangles[p3 - 2] = 0;
00858   m->triangles[p3 - 1] = 2;
00859   p3 *= 2;
00860   m->triangles[p3 - 3] = points->size() + 1;
00861   m->triangles[p3 - 2] = 1;
00862   m->triangles[p3 - 1] = 2;
00863 
00864   return m;
00865 }
00866 
00867 void kinematic_constraints::VisibilityConstraint::getMarkers(const robot_state::RobotState& state,
00868                                                              visualization_msgs::MarkerArray& markers) const
00869 {
00870   shapes::Mesh* m = getVisibilityCone(state);
00871   visualization_msgs::Marker mk;
00872   shapes::constructMarkerFromShape(m, mk);
00873   delete m;
00874   mk.header.frame_id = robot_model_->getModelFrame();
00875   mk.header.stamp = ros::Time::now();
00876   mk.ns = "constraints";
00877   mk.id = 1;
00878   mk.action = visualization_msgs::Marker::ADD;
00879   mk.pose.position.x = 0;
00880   mk.pose.position.y = 0;
00881   mk.pose.position.z = 0;
00882   mk.pose.orientation.x = 0;
00883   mk.pose.orientation.y = 0;
00884   mk.pose.orientation.z = 0;
00885   mk.pose.orientation.w = 1;
00886   mk.lifetime = ros::Duration(60);
00887   // this scale necessary to make results look reasonable
00888   mk.scale.x = .01;
00889   mk.color.a = 1.0;
00890   mk.color.r = 1.0;
00891   mk.color.g = 0.0;
00892   mk.color.b = 0.0;
00893 
00894   markers.markers.push_back(mk);
00895 
00896   const Eigen::Affine3d& sp =
00897       mobile_sensor_frame_ ? state.getFrameTransform(sensor_frame_id_) * sensor_pose_ : sensor_pose_;
00898   const Eigen::Affine3d& tp =
00899       mobile_target_frame_ ? state.getFrameTransform(target_frame_id_) * target_pose_ : target_pose_;
00900 
00901   visualization_msgs::Marker mka;
00902   mka.type = visualization_msgs::Marker::ARROW;
00903   mka.action = visualization_msgs::Marker::ADD;
00904   mka.color = mk.color;
00905   mka.pose = mk.pose;
00906 
00907   mka.header = mk.header;
00908   mka.ns = mk.ns;
00909   mka.id = 2;
00910   mka.lifetime = mk.lifetime;
00911   mka.scale.x = 0.05;
00912   mka.scale.y = .15;
00913   mka.scale.z = 0.0;
00914   mka.points.resize(2);
00915   Eigen::Vector3d d = tp.translation() + tp.rotation().col(2) * 0.5;
00916   mka.points[0].x = tp.translation().x();
00917   mka.points[0].y = tp.translation().y();
00918   mka.points[0].z = tp.translation().z();
00919   mka.points[1].x = d.x();
00920   mka.points[1].y = d.y();
00921   mka.points[1].z = d.z();
00922   markers.markers.push_back(mka);
00923 
00924   mka.id = 3;
00925   mka.color.b = 1.0;
00926   mka.color.r = 0.0;
00927 
00928   d = sp.translation() + sp.rotation().col(2 - sensor_view_direction_) * 0.5;
00929   mka.points[0].x = sp.translation().x();
00930   mka.points[0].y = sp.translation().y();
00931   mka.points[0].z = sp.translation().z();
00932   mka.points[1].x = d.x();
00933   mka.points[1].y = d.y();
00934   mka.points[1].z = d.z();
00935 
00936   markers.markers.push_back(mka);
00937 }
00938 
00939 kinematic_constraints::ConstraintEvaluationResult
00940 kinematic_constraints::VisibilityConstraint::decide(const robot_state::RobotState& state, bool verbose) const
00941 {
00942   if (target_radius_ <= std::numeric_limits<double>::epsilon())
00943     return ConstraintEvaluationResult(true, 0.0);
00944 
00945   if (max_view_angle_ > 0.0 || max_range_angle_ > 0.0)
00946   {
00947     const Eigen::Affine3d& sp =
00948         mobile_sensor_frame_ ? state.getFrameTransform(sensor_frame_id_) * sensor_pose_ : sensor_pose_;
00949     const Eigen::Affine3d& tp =
00950         mobile_target_frame_ ? state.getFrameTransform(target_frame_id_) * target_pose_ : target_pose_;
00951 
00952     // necessary to do subtraction as SENSOR_Z is 0 and SENSOR_X is 2
00953     const Eigen::Vector3d& normal2 = sp.rotation().col(2 - sensor_view_direction_);
00954 
00955     if (max_view_angle_ > 0.0)
00956     {
00957       const Eigen::Vector3d& normal1 = tp.rotation().col(2) * -1.0;  // along Z axis and inverted
00958       double dp = normal2.dot(normal1);
00959       double ang = acos(dp);
00960       if (dp < 0.0)
00961       {
00962         if (verbose)
00963           logInform("Visibility constraint is violated because the sensor is looking at the wrong side");
00964         return ConstraintEvaluationResult(false, 0.0);
00965       }
00966       if (max_view_angle_ < ang)
00967       {
00968         if (verbose)
00969           logInform("Visibility constraint is violated because the view angle is %lf (above the maximum allowed of "
00970                     "%lf)",
00971                     ang, max_view_angle_);
00972         return ConstraintEvaluationResult(false, 0.0);
00973       }
00974     }
00975     if (max_range_angle_ > 0.0)
00976     {
00977       const Eigen::Vector3d& dir = (tp.translation() - sp.translation()).normalized();
00978       double dp = normal2.dot(dir);
00979       if (dp < 0.0)
00980       {
00981         if (verbose)
00982           logInform("Visibility constraint is violated because the sensor is looking at the wrong side");
00983         return ConstraintEvaluationResult(false, 0.0);
00984       }
00985 
00986       double ang = acos(dp);
00987       if (max_range_angle_ < ang)
00988       {
00989         if (verbose)
00990           logInform("Visibility constraint is violated because the range angle is %lf (above the maximum allowed of "
00991                     "%lf)",
00992                     ang, max_range_angle_);
00993         return ConstraintEvaluationResult(false, 0.0);
00994       }
00995     }
00996   }
00997 
00998   shapes::Mesh* m = getVisibilityCone(state);
00999   if (!m)
01000     return ConstraintEvaluationResult(false, 0.0);
01001 
01002   // add the visibility cone as an object
01003   collision_detection::CollisionWorldFCL collision_world;
01004   collision_world.getWorld()->addToObject("cone", shapes::ShapeConstPtr(m), Eigen::Affine3d::Identity());
01005 
01006   // check for collisions between the robot and the cone
01007   collision_detection::CollisionRequest req;
01008   collision_detection::CollisionResult res;
01009   collision_detection::AllowedCollisionMatrix acm;
01010   acm.setDefaultEntry("cone", boost::bind(&VisibilityConstraint::decideContact, this, _1));
01011   req.contacts = true;
01012   req.verbose = verbose;
01013   req.max_contacts = 1;
01014   collision_world.checkRobotCollision(req, res, *collision_robot_, state, acm);
01015 
01016   if (verbose)
01017   {
01018     std::stringstream ss;
01019     m->print(ss);
01020     logInform("Visibility constraint %ssatisfied. Visibility cone approximation:\n %s", res.collision ? "not " : "",
01021               ss.str().c_str());
01022   }
01023 
01024   return ConstraintEvaluationResult(!res.collision, res.collision ? res.contacts.begin()->second.front().depth : 0.0);
01025 }
01026 
01027 bool kinematic_constraints::VisibilityConstraint::decideContact(const collision_detection::Contact& contact) const
01028 {
01029   if (contact.body_type_1 == collision_detection::BodyTypes::ROBOT_ATTACHED ||
01030       contact.body_type_2 == collision_detection::BodyTypes::ROBOT_ATTACHED)
01031     return true;
01032   if (contact.body_type_1 == collision_detection::BodyTypes::ROBOT_LINK &&
01033       contact.body_type_2 == collision_detection::BodyTypes::WORLD_OBJECT &&
01034       (robot_state::Transforms::sameFrame(contact.body_name_1, sensor_frame_id_) ||
01035        robot_state::Transforms::sameFrame(contact.body_name_1, target_frame_id_)))
01036   {
01037     logDebug("Accepted collision with either sensor or target");
01038     return true;
01039   }
01040   if (contact.body_type_2 == collision_detection::BodyTypes::ROBOT_LINK &&
01041       contact.body_type_1 == collision_detection::BodyTypes::WORLD_OBJECT &&
01042       (robot_state::Transforms::sameFrame(contact.body_name_2, sensor_frame_id_) ||
01043        robot_state::Transforms::sameFrame(contact.body_name_2, target_frame_id_)))
01044   {
01045     logDebug("Accepted collision with either sensor or target");
01046     return true;
01047   }
01048   return false;
01049 }
01050 
01051 void kinematic_constraints::VisibilityConstraint::print(std::ostream& out) const
01052 {
01053   if (enabled())
01054   {
01055     out << "Visibility constraint for sensor in frame '" << sensor_frame_id_ << "' using target in frame '"
01056         << target_frame_id_ << "'" << std::endl;
01057     out << "Target radius: " << target_radius_ << ", using " << cone_sides_ << " sides." << std::endl;
01058   }
01059   else
01060     out << "No constraint" << std::endl;
01061 }
01062 
01063 void kinematic_constraints::KinematicConstraintSet::clear()
01064 {
01065   all_constraints_ = moveit_msgs::Constraints();
01066   kinematic_constraints_.clear();
01067   joint_constraints_.clear();
01068   position_constraints_.clear();
01069   orientation_constraints_.clear();
01070   visibility_constraints_.clear();
01071 }
01072 
01073 bool kinematic_constraints::KinematicConstraintSet::add(const std::vector<moveit_msgs::JointConstraint>& jc)
01074 {
01075   bool result = true;
01076   for (unsigned int i = 0; i < jc.size(); ++i)
01077   {
01078     JointConstraint* ev = new JointConstraint(robot_model_);
01079     bool u = ev->configure(jc[i]);
01080     result = result && u;
01081     kinematic_constraints_.push_back(KinematicConstraintPtr(ev));
01082     joint_constraints_.push_back(jc[i]);
01083     all_constraints_.joint_constraints.push_back(jc[i]);
01084   }
01085   return result;
01086 }
01087 
01088 bool kinematic_constraints::KinematicConstraintSet::add(const std::vector<moveit_msgs::PositionConstraint>& pc,
01089                                                         const robot_state::Transforms& tf)
01090 {
01091   bool result = true;
01092   for (unsigned int i = 0; i < pc.size(); ++i)
01093   {
01094     PositionConstraint* ev = new PositionConstraint(robot_model_);
01095     bool u = ev->configure(pc[i], tf);
01096     result = result && u;
01097     kinematic_constraints_.push_back(KinematicConstraintPtr(ev));
01098     position_constraints_.push_back(pc[i]);
01099     all_constraints_.position_constraints.push_back(pc[i]);
01100   }
01101   return result;
01102 }
01103 
01104 bool kinematic_constraints::KinematicConstraintSet::add(const std::vector<moveit_msgs::OrientationConstraint>& oc,
01105                                                         const robot_state::Transforms& tf)
01106 {
01107   bool result = true;
01108   for (unsigned int i = 0; i < oc.size(); ++i)
01109   {
01110     OrientationConstraint* ev = new OrientationConstraint(robot_model_);
01111     bool u = ev->configure(oc[i], tf);
01112     result = result && u;
01113     kinematic_constraints_.push_back(KinematicConstraintPtr(ev));
01114     orientation_constraints_.push_back(oc[i]);
01115     all_constraints_.orientation_constraints.push_back(oc[i]);
01116   }
01117   return result;
01118 }
01119 
01120 bool kinematic_constraints::KinematicConstraintSet::add(const std::vector<moveit_msgs::VisibilityConstraint>& vc,
01121                                                         const robot_state::Transforms& tf)
01122 {
01123   bool result = true;
01124   for (unsigned int i = 0; i < vc.size(); ++i)
01125   {
01126     VisibilityConstraint* ev = new VisibilityConstraint(robot_model_);
01127     bool u = ev->configure(vc[i], tf);
01128     result = result && u;
01129     kinematic_constraints_.push_back(KinematicConstraintPtr(ev));
01130     visibility_constraints_.push_back(vc[i]);
01131     all_constraints_.visibility_constraints.push_back(vc[i]);
01132   }
01133   return result;
01134 }
01135 
01136 bool kinematic_constraints::KinematicConstraintSet::add(const moveit_msgs::Constraints& c,
01137                                                         const robot_state::Transforms& tf)
01138 {
01139   bool j = add(c.joint_constraints);
01140   bool p = add(c.position_constraints, tf);
01141   bool o = add(c.orientation_constraints, tf);
01142   bool v = add(c.visibility_constraints, tf);
01143   return j && p && o && v;
01144 }
01145 
01146 kinematic_constraints::ConstraintEvaluationResult
01147 kinematic_constraints::KinematicConstraintSet::decide(const robot_state::RobotState& state, bool verbose) const
01148 {
01149   ConstraintEvaluationResult res(true, 0.0);
01150   for (unsigned int i = 0; i < kinematic_constraints_.size(); ++i)
01151   {
01152     ConstraintEvaluationResult r = kinematic_constraints_[i]->decide(state, verbose);
01153     if (!r.satisfied)
01154       res.satisfied = false;
01155     res.distance += r.distance;
01156   }
01157   return res;
01158 }
01159 
01160 kinematic_constraints::ConstraintEvaluationResult kinematic_constraints::KinematicConstraintSet::decide(
01161     const robot_state::RobotState& state, std::vector<ConstraintEvaluationResult>& results, bool verbose) const
01162 {
01163   ConstraintEvaluationResult result(true, 0.0);
01164   results.resize(kinematic_constraints_.size());
01165   for (std::size_t i = 0; i < kinematic_constraints_.size(); ++i)
01166   {
01167     results[i] = kinematic_constraints_[i]->decide(state, verbose);
01168     result.satisfied = result.satisfied && results[i].satisfied;
01169     result.distance += results[i].distance;
01170   }
01171 
01172   return result;
01173 }
01174 
01175 void kinematic_constraints::KinematicConstraintSet::print(std::ostream& out) const
01176 {
01177   out << kinematic_constraints_.size() << " kinematic constraints" << std::endl;
01178   for (unsigned int i = 0; i < kinematic_constraints_.size(); ++i)
01179     kinematic_constraints_[i]->print(out);
01180 }
01181 
01182 bool kinematic_constraints::KinematicConstraintSet::equal(const KinematicConstraintSet& other, double margin) const
01183 {
01184   // each constraint in this matches some in the other
01185   for (unsigned int i = 0; i < kinematic_constraints_.size(); ++i)
01186   {
01187     bool found = false;
01188     for (unsigned int j = 0; !found && j < other.kinematic_constraints_.size(); ++j)
01189       found = kinematic_constraints_[i]->equal(*other.kinematic_constraints_[j], margin);
01190     if (!found)
01191       return false;
01192   }
01193   // each constraint in the other matches some constraint in this
01194   for (unsigned int i = 0; i < other.kinematic_constraints_.size(); ++i)
01195   {
01196     bool found = false;
01197     for (unsigned int j = 0; !found && j < kinematic_constraints_.size(); ++j)
01198       found = other.kinematic_constraints_[i]->equal(*kinematic_constraints_[j], margin);
01199     if (!found)
01200       return false;
01201   }
01202   return true;
01203 }


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Jun 19 2019 19:23:49