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 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
00074 clear();
00075
00076
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
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
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
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>();
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, "
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
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
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
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
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
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
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
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
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);
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
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
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
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
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
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
00820
00821
00822 m->vertices[0] = sp.translation().x();
00823 m->vertices[1] = sp.translation().y();
00824 m->vertices[2] = sp.translation().z();
00825
00826
00827 m->vertices[3] = tp.translation().x();
00828 m->vertices[4] = tp.translation().y();
00829 m->vertices[5] = tp.translation().z();
00830
00831
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
00840 std::size_t p3 = points->size() * 3;
00841 for (std::size_t i = 1; i < points->size(); ++i)
00842 {
00843
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
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
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
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
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;
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
01003 collision_detection::CollisionWorldFCL collision_world;
01004 collision_world.getWorld()->addToObject("cone", shapes::ShapeConstPtr(m), Eigen::Affine3d::Identity());
01005
01006
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
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
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 }