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