kinematic_constraint.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
43 #include <boost/math/constants/constants.hpp>
44 #include <tf2_eigen/tf2_eigen.h>
45 #include <boost/bind.hpp>
46 #include <limits>
47 #include <memory>
48 
49 namespace kinematic_constraints
50 {
51 static double normalizeAngle(double angle)
52 {
53  double v = fmod(angle, 2.0 * boost::math::constants::pi<double>());
54  if (v < -boost::math::constants::pi<double>())
55  v += 2.0 * boost::math::constants::pi<double>();
56  else if (v > boost::math::constants::pi<double>())
57  v -= 2.0 * boost::math::constants::pi<double>();
58  return v;
59 }
60 
61 // Normalizes an angle to the interval [-pi, +pi] and then take the absolute value
62 // The returned values will be in the following range [0, +pi]
63 static double normalizeAbsoluteAngle(const double angle)
64 {
65  const double normalized_angle = std::fmod(std::abs(angle), 2 * M_PI);
66  return std::min(2 * M_PI - normalized_angle, normalized_angle);
67 }
68 
76 template <typename Derived>
77 std::tuple<Eigen::Matrix<typename Eigen::MatrixBase<Derived>::Scalar, 3, 1>, bool>
78 CalcEulerAngles(const Eigen::MatrixBase<Derived>& R)
79 {
80  using std::atan2;
81  using std::sqrt;
82  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 3, 3)
83  using Index = EIGEN_DEFAULT_DENSE_INDEX_TYPE;
84  using Scalar = typename Eigen::MatrixBase<Derived>::Scalar;
85  const Index i = 0;
86  const Index j = 1;
87  const Index k = 2;
88  Eigen::Matrix<Scalar, 3, 1> res;
89  const Scalar rsum = sqrt((R(i, i) * R(i, i) + R(i, j) * R(i, j) + R(j, k) * R(j, k) + R(k, k) * R(k, k)) / 2);
90  res[1] = atan2(R(i, k), rsum);
91  // There is a singularity when cos(beta) == 0
92  if (rsum > 4 * Eigen::NumTraits<Scalar>::epsilon())
93  { // cos(beta) != 0
94  res[0] = atan2(-R(j, k), R(k, k));
95  res[2] = atan2(-R(i, j), R(i, i));
96  return { res, true };
97  }
98  else if (R(i, k) > 0)
99  { // cos(beta) == 0 and sin(beta) == 1
100  const Scalar spos = R(j, i) + R(k, j); // 2*sin(alpha + gamma)
101  const Scalar cpos = R(j, j) - R(k, i); // 2*cos(alpha + gamma)
102  res[0] = atan2(spos, cpos);
103  res[2] = 0;
104  return { res, false };
105  } // cos(beta) == 0 and sin(beta) == -1
106  const Scalar sneg = R(k, j) - R(j, i); // 2*sin(alpha + gamma)
107  const Scalar cneg = R(j, j) + R(k, i); // 2*cos(alpha + gamma)
108  res[0] = atan2(sneg, cneg);
109  res[2] = 0;
110  return { res, false };
111 }
112 
113 KinematicConstraint::KinematicConstraint(const moveit::core::RobotModelConstPtr& model)
114  : type_(UNKNOWN_CONSTRAINT), robot_model_(model), constraint_weight_(std::numeric_limits<double>::epsilon())
115 {
116 }
117 
119 
120 bool JointConstraint::configure(const moveit_msgs::JointConstraint& jc)
121 {
122  // clearing before we configure to get rid of any old data
123  clear();
124 
125  // testing tolerances first
126  if (jc.tolerance_above < 0.0 || jc.tolerance_below < 0.0)
127  {
128  ROS_WARN_NAMED("kinematic_constraints", "JointConstraint tolerance values must be positive.");
129  joint_model_ = nullptr;
130  return false;
131  }
132 
133  joint_variable_name_ = jc.joint_name;
134  local_variable_name_.clear();
135  if (robot_model_->hasJointModel(joint_variable_name_))
136  joint_model_ = robot_model_->getJointModel(joint_variable_name_);
137  else
138  {
139  std::size_t pos = jc.joint_name.find_last_of('/');
140  if (pos != std::string::npos)
141  {
142  joint_model_ = robot_model_->getJointModel(jc.joint_name.substr(0, pos));
143  if (pos + 1 < jc.joint_name.length())
144  local_variable_name_ = jc.joint_name.substr(pos + 1);
145  }
146  else
147  joint_model_ = robot_model_->getJointModel(jc.joint_name);
148  }
149 
150  if (joint_model_)
151  {
152  if (local_variable_name_.empty())
153  {
154  // check if the joint has 1 DOF (the only kind we can handle)
155  if (joint_model_->getVariableCount() == 0)
156  {
157  ROS_ERROR_NAMED("kinematic_constraints", "Joint '%s' has no parameters to constrain", jc.joint_name.c_str());
158  joint_model_ = nullptr;
159  }
160  else if (joint_model_->getVariableCount() > 1)
161  {
162  ROS_ERROR_NAMED("kinematic_constraints",
163  "Joint '%s' has more than one parameter to constrain. "
164  "This type of constraint is not supported.",
165  jc.joint_name.c_str());
166  joint_model_ = nullptr;
167  }
168  }
169  else
170  {
171  int found = -1;
172  const std::vector<std::string>& local_var_names = joint_model_->getLocalVariableNames();
173  for (std::size_t i = 0; i < local_var_names.size(); ++i)
174  if (local_var_names[i] == local_variable_name_)
175  {
176  found = i;
177  break;
178  }
179  if (found < 0)
180  {
181  ROS_ERROR_NAMED("kinematic_constraints", "Local variable name '%s' is not known to joint '%s'",
182  local_variable_name_.c_str(), joint_model_->getName().c_str());
183  joint_model_ = nullptr;
184  }
185  }
186  }
187 
188  if (joint_model_)
189  {
190  joint_is_continuous_ = false;
191  joint_tolerance_above_ = jc.tolerance_above;
192  joint_tolerance_below_ = jc.tolerance_below;
193  joint_variable_index_ = robot_model_->getVariableIndex(joint_variable_name_);
194 
195  // check if we have to wrap angles when computing distances
196  joint_is_continuous_ = false;
197  if (joint_model_->getType() == robot_model::JointModel::REVOLUTE)
198  {
199  const robot_model::RevoluteJointModel* rjoint = static_cast<const robot_model::RevoluteJointModel*>(joint_model_);
200  if (rjoint->isContinuous())
201  joint_is_continuous_ = true;
202  }
203  else if (joint_model_->getType() == robot_model::JointModel::PLANAR)
204  {
205  if (local_variable_name_ == "theta")
206  joint_is_continuous_ = true;
207  }
208 
209  if (joint_is_continuous_)
210  {
211  joint_position_ = normalizeAngle(jc.position);
212  }
213  else
214  {
215  joint_position_ = jc.position;
216  const robot_model::VariableBounds& bounds = joint_model_->getVariableBounds(joint_variable_name_);
217 
218  if (bounds.min_position_ > joint_position_ + joint_tolerance_above_)
219  {
220  joint_position_ = bounds.min_position_;
221  joint_tolerance_above_ = std::numeric_limits<double>::epsilon();
222  ROS_WARN_NAMED("kinematic_constraints",
223  "Joint %s is constrained to be below the minimum bounds. "
224  "Assuming minimum bounds instead.",
225  jc.joint_name.c_str());
226  }
227  else if (bounds.max_position_ < joint_position_ - joint_tolerance_below_)
228  {
229  joint_position_ = bounds.max_position_;
230  joint_tolerance_below_ = std::numeric_limits<double>::epsilon();
231  ROS_WARN_NAMED("kinematic_constraints",
232  "Joint %s is constrained to be above the maximum bounds. "
233  "Assuming maximum bounds instead.",
234  jc.joint_name.c_str());
235  }
236  }
237 
238  if (jc.weight <= std::numeric_limits<double>::epsilon())
239  {
240  ROS_WARN_NAMED("kinematic_constraints",
241  "The weight on constraint for joint '%s' is very near zero. Setting to 1.0.",
242  jc.joint_name.c_str());
243  constraint_weight_ = 1.0;
244  }
245  else
246  constraint_weight_ = jc.weight;
247  }
248  return joint_model_ != nullptr;
249 }
250 
251 bool JointConstraint::equal(const KinematicConstraint& other, double margin) const
252 {
253  if (other.getType() != type_)
254  return false;
255  const JointConstraint& o = static_cast<const JointConstraint&>(other);
256  if (o.joint_model_ == joint_model_ && o.local_variable_name_ == local_variable_name_)
257  return fabs(joint_position_ - o.joint_position_) <= margin &&
258  fabs(joint_tolerance_above_ - o.joint_tolerance_above_) <= margin &&
259  fabs(joint_tolerance_below_ - o.joint_tolerance_below_) <= margin;
260  return false;
261 }
262 
264 {
265  if (!joint_model_)
266  return ConstraintEvaluationResult(true, 0.0);
267 
268  double current_joint_position = state.getVariablePosition(joint_variable_index_);
269  double dif = 0.0;
270 
271  // compute signed shortest distance for continuous joints
272  if (joint_is_continuous_)
273  {
274  dif = normalizeAngle(current_joint_position) - joint_position_;
275 
276  if (dif > boost::math::constants::pi<double>())
277  dif = 2.0 * boost::math::constants::pi<double>() - dif;
278  else if (dif < -boost::math::constants::pi<double>())
279  dif += 2.0 * boost::math::constants::pi<double>(); // we include a sign change to have dif > 0
280  }
281  else
282  dif = current_joint_position - joint_position_;
283 
284  // check bounds
285  bool result = dif <= (joint_tolerance_above_ + 2.0 * std::numeric_limits<double>::epsilon()) &&
286  dif >= (-joint_tolerance_below_ - 2.0 * std::numeric_limits<double>::epsilon());
287  if (verbose)
288  ROS_INFO_NAMED("kinematic_constraints",
289  "Constraint %s:: Joint name: '%s', actual value: %f, desired value: %f, "
290  "tolerance_above: %f, tolerance_below: %f",
291  result ? "satisfied" : "violated", joint_variable_name_.c_str(), current_joint_position,
292  joint_position_, joint_tolerance_above_, joint_tolerance_below_);
293  return ConstraintEvaluationResult(result, constraint_weight_ * fabs(dif));
294 }
295 
297 {
298  return joint_model_;
299 }
300 
302 {
303  joint_model_ = nullptr;
304  joint_variable_index_ = -1;
305  joint_is_continuous_ = false;
306  local_variable_name_ = "";
307  joint_variable_name_ = "";
308  joint_position_ = joint_tolerance_below_ = joint_tolerance_above_ = 0.0;
309 }
310 
311 void JointConstraint::print(std::ostream& out) const
312 {
313  if (joint_model_)
314  {
315  out << "Joint constraint for joint " << joint_variable_name_ << ": " << std::endl;
316  out << " value = ";
317  out << joint_position_ << "; ";
318  out << " tolerance below = ";
319  out << joint_tolerance_below_ << "; ";
320  out << " tolerance above = ";
321  out << joint_tolerance_above_ << "; ";
322  out << std::endl;
323  }
324  else
325  out << "No constraint" << std::endl;
326 }
327 
328 bool PositionConstraint::configure(const moveit_msgs::PositionConstraint& pc, const robot_state::Transforms& tf)
329 {
330  // clearing before we configure to get rid of any old data
331  clear();
332 
333  link_model_ = robot_model_->getLinkModel(pc.link_name);
334  if (link_model_ == nullptr)
335  {
336  ROS_WARN_NAMED("kinematic_constraints",
337  "Position constraint link model %s not found in kinematic model. Constraint invalid.",
338  pc.link_name.c_str());
339  return false;
340  }
341 
342  if (pc.header.frame_id.empty())
343  {
344  ROS_WARN_NAMED("kinematic_constraints", "No frame specified for position constraint on link '%s'!",
345  pc.link_name.c_str());
346  return false;
347  }
348 
349  offset_ = Eigen::Vector3d(pc.target_point_offset.x, pc.target_point_offset.y, pc.target_point_offset.z);
350  has_offset_ = offset_.squaredNorm() > std::numeric_limits<double>::epsilon();
351 
352  if (tf.isFixedFrame(pc.header.frame_id))
353  {
354  constraint_frame_id_ = tf.getTargetFrame();
355  mobile_frame_ = false;
356  }
357  else
358  {
359  constraint_frame_id_ = pc.header.frame_id;
360  mobile_frame_ = true;
361  }
362 
363  // load primitive shapes, first clearing any we already have
364  for (std::size_t i = 0; i < pc.constraint_region.primitives.size(); ++i)
365  {
366  std::unique_ptr<shapes::Shape> shape(shapes::constructShapeFromMsg(pc.constraint_region.primitives[i]));
367  if (shape)
368  {
369  if (pc.constraint_region.primitive_poses.size() <= i)
370  {
371  ROS_WARN_NAMED("kinematic_constraints", "Constraint region message does not contain enough primitive poses");
372  continue;
373  }
374  constraint_region_.push_back(bodies::BodyPtr(bodies::createBodyFromShape(shape.get())));
375  Eigen::Isometry3d t;
376  tf2::fromMsg(pc.constraint_region.primitive_poses[i], t);
377  constraint_region_pose_.push_back(t);
378  if (mobile_frame_)
379  constraint_region_.back()->setPose(constraint_region_pose_.back());
380  else
381  {
382  tf.transformPose(pc.header.frame_id, constraint_region_pose_.back(), constraint_region_pose_.back());
383  constraint_region_.back()->setPose(constraint_region_pose_.back());
384  }
385  }
386  else
387  ROS_WARN_NAMED("kinematic_constraints", "Could not construct primitive shape %zu", i);
388  }
389 
390  // load meshes
391  for (std::size_t i = 0; i < pc.constraint_region.meshes.size(); ++i)
392  {
393  std::unique_ptr<shapes::Shape> shape(shapes::constructShapeFromMsg(pc.constraint_region.meshes[i]));
394  if (shape)
395  {
396  if (pc.constraint_region.mesh_poses.size() <= i)
397  {
398  ROS_WARN_NAMED("kinematic_constraints", "Constraint region message does not contain enough primitive poses");
399  continue;
400  }
401  constraint_region_.push_back(bodies::BodyPtr(bodies::createBodyFromShape(shape.get())));
402  Eigen::Isometry3d t;
403  tf2::fromMsg(pc.constraint_region.mesh_poses[i], t);
404  constraint_region_pose_.push_back(t);
405  if (mobile_frame_)
406  constraint_region_.back()->setPose(constraint_region_pose_.back());
407  else
408  {
409  tf.transformPose(pc.header.frame_id, constraint_region_pose_.back(), constraint_region_pose_.back());
410  constraint_region_.back()->setPose(constraint_region_pose_.back());
411  }
412  }
413  else
414  {
415  ROS_WARN_NAMED("kinematic_constraints", "Could not construct mesh shape %zu", i);
416  }
417  }
418 
419  if (pc.weight <= std::numeric_limits<double>::epsilon())
420  {
421  ROS_WARN_NAMED("kinematic_constraints",
422  "The weight on position constraint for link '%s' is near zero. Setting to 1.0.",
423  pc.link_name.c_str());
424  constraint_weight_ = 1.0;
425  }
426  else
427  constraint_weight_ = pc.weight;
428 
429  return !constraint_region_.empty();
430 }
431 
432 bool PositionConstraint::equal(const KinematicConstraint& other, double margin) const
433 {
434  if (other.getType() != type_)
435  return false;
436  const PositionConstraint& o = static_cast<const PositionConstraint&>(other);
437 
438  if (link_model_ == o.link_model_ && robot_state::Transforms::sameFrame(constraint_frame_id_, o.constraint_frame_id_))
439  {
440  if ((offset_ - o.offset_).norm() > margin)
441  return false;
442  std::vector<bool> other_region_matches_this(constraint_region_.size(), false);
443  for (std::size_t i = 0; i < constraint_region_.size(); ++i)
444  {
445  bool some_match = false;
446  // need to check against all other regions
447  for (std::size_t j = 0; j < o.constraint_region_.size(); ++j)
448  {
449  Eigen::Isometry3d diff = constraint_region_pose_[i].inverse() * o.constraint_region_pose_[j];
450  if (diff.translation().norm() < margin && diff.rotation().isIdentity(margin) &&
451  constraint_region_[i]->getType() == o.constraint_region_[j]->getType() &&
452  fabs(constraint_region_[i]->computeVolume() - o.constraint_region_[j]->computeVolume()) < margin)
453  {
454  some_match = true;
455  // can't break, as need to do matches the other way as well
456  other_region_matches_this[j] = true;
457  }
458  }
459  if (!some_match)
460  return false;
461  }
462  for (std::size_t i = 0; i < o.constraint_region_.size(); ++i)
463  if (!other_region_matches_this[i])
464  return false;
465  return true;
466  }
467  return false;
468 }
469 
470 // helper function to avoid code duplication
472  const Eigen::Vector3d& desired,
473  const std::string& name, double weight,
474  bool result, bool verbose)
475 {
476  double dx = desired.x() - pt.x();
477  double dy = desired.y() - pt.y();
478  double dz = desired.z() - pt.z();
479  if (verbose)
480  {
481  ROS_INFO_NAMED("kinematic_constraints",
482  "Position constraint %s on link '%s'. Desired: %f, %f, %f, current: %f, %f, %f",
483  result ? "satisfied" : "violated", name.c_str(), desired.x(), desired.y(), desired.z(), pt.x(),
484  pt.y(), pt.z());
485  ROS_INFO_NAMED("kinematic_constraints", "Differences %g %g %g", dx, dy, dz);
486  }
487  return ConstraintEvaluationResult(result, weight * sqrt(dx * dx + dy * dy + dz * dz));
488 }
489 
491 {
492  if (!link_model_ || constraint_region_.empty())
493  return ConstraintEvaluationResult(true, 0.0);
494 
495  Eigen::Vector3d pt = state.getGlobalLinkTransform(link_model_) * offset_;
496  if (mobile_frame_)
497  {
498  for (std::size_t i = 0; i < constraint_region_.size(); ++i)
499  {
500  Eigen::Isometry3d tmp = state.getFrameTransform(constraint_frame_id_) * constraint_region_pose_[i];
501  bool result = constraint_region_[i]->cloneAt(tmp)->containsPoint(pt, verbose);
502  if (result || (i + 1 == constraint_region_pose_.size()))
503  return finishPositionConstraintDecision(pt, tmp.translation(), link_model_->getName(), constraint_weight_,
504  result, verbose);
505  else
506  finishPositionConstraintDecision(pt, tmp.translation(), link_model_->getName(), constraint_weight_, result,
507  verbose);
508  }
509  }
510  else
511  {
512  for (std::size_t i = 0; i < constraint_region_.size(); ++i)
513  {
514  bool result = constraint_region_[i]->containsPoint(pt, true);
515  if (result || (i + 1 == constraint_region_.size()))
516  return finishPositionConstraintDecision(pt, constraint_region_[i]->getPose().translation(),
517  link_model_->getName(), constraint_weight_, result, verbose);
518  else
519  finishPositionConstraintDecision(pt, constraint_region_[i]->getPose().translation(), link_model_->getName(),
520  constraint_weight_, result, verbose);
521  }
522  }
523  return ConstraintEvaluationResult(false, 0.0);
524 }
525 
526 void PositionConstraint::print(std::ostream& out) const
527 {
528  if (enabled())
529  out << "Position constraint on link '" << link_model_->getName() << "'" << std::endl;
530  else
531  out << "No constraint" << std::endl;
532 }
533 
535 {
536  offset_ = Eigen::Vector3d(0.0, 0.0, 0.0);
537  has_offset_ = false;
538  constraint_region_.clear();
539  constraint_region_pose_.clear();
540  mobile_frame_ = false;
541  constraint_frame_id_ = "";
542  link_model_ = nullptr;
543 }
544 
546 {
547  return link_model_ && !constraint_region_.empty();
548 }
549 
550 bool OrientationConstraint::configure(const moveit_msgs::OrientationConstraint& oc, const robot_state::Transforms& tf)
551 {
552  // clearing out any old data
553  clear();
554 
555  link_model_ = robot_model_->getLinkModel(oc.link_name);
556  if (!link_model_)
557  {
558  ROS_WARN_NAMED("kinematic_constraints", "Could not find link model for link name %s", oc.link_name.c_str());
559  return false;
560  }
561  Eigen::Quaterniond q;
562  tf2::fromMsg(oc.orientation, q);
563  if (fabs(q.norm() - 1.0) > 1e-3)
564  {
565  ROS_WARN_NAMED("kinematic_constraints",
566  "Orientation constraint for link '%s' is probably incorrect: %f, %f, %f, "
567  "%f. Assuming identity instead.",
568  oc.link_name.c_str(), oc.orientation.x, oc.orientation.y, oc.orientation.z, oc.orientation.w);
569  q = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0);
570  }
571 
572  if (oc.header.frame_id.empty())
573  ROS_WARN_NAMED("kinematic_constraints", "No frame specified for position constraint on link '%s'!",
574  oc.link_name.c_str());
575 
576  if (tf.isFixedFrame(oc.header.frame_id))
577  {
578  tf.transformQuaternion(oc.header.frame_id, q, q);
579  desired_rotation_frame_id_ = tf.getTargetFrame();
580  desired_rotation_matrix_ = Eigen::Matrix3d(q);
581  desired_rotation_matrix_inv_ = desired_rotation_matrix_.transpose();
582  mobile_frame_ = false;
583  }
584  else
585  {
586  desired_rotation_frame_id_ = oc.header.frame_id;
587  desired_rotation_matrix_ = Eigen::Matrix3d(q);
588  mobile_frame_ = true;
589  }
590  std::stringstream matrix_str;
591  matrix_str << desired_rotation_matrix_;
592  ROS_DEBUG_NAMED("kinematic_constraints", "The desired rotation matrix for link '%s' in frame %s is:\n%s",
593  oc.link_name.c_str(), desired_rotation_frame_id_.c_str(), matrix_str.str().c_str());
594 
595  if (oc.weight <= std::numeric_limits<double>::epsilon())
596  {
597  ROS_WARN_NAMED("kinematic_constraints",
598  "The weight on position constraint for link '%s' is near zero. Setting to 1.0.",
599  oc.link_name.c_str());
600  constraint_weight_ = 1.0;
601  }
602  else
603  constraint_weight_ = oc.weight;
604  absolute_x_axis_tolerance_ = fabs(oc.absolute_x_axis_tolerance);
605  if (absolute_x_axis_tolerance_ < std::numeric_limits<double>::epsilon())
606  ROS_WARN_NAMED("kinematic_constraints", "Near-zero value for absolute_x_axis_tolerance");
607  absolute_y_axis_tolerance_ = fabs(oc.absolute_y_axis_tolerance);
608  if (absolute_y_axis_tolerance_ < std::numeric_limits<double>::epsilon())
609  ROS_WARN_NAMED("kinematic_constraints", "Near-zero value for absolute_y_axis_tolerance");
610  absolute_z_axis_tolerance_ = fabs(oc.absolute_z_axis_tolerance);
611  if (absolute_z_axis_tolerance_ < std::numeric_limits<double>::epsilon())
612  ROS_WARN_NAMED("kinematic_constraints", "Near-zero value for absolute_z_axis_tolerance");
613 
614  return link_model_ != nullptr;
615 }
616 
617 bool OrientationConstraint::equal(const KinematicConstraint& other, double margin) const
618 {
619  if (other.getType() != type_)
620  return false;
621  const OrientationConstraint& o = static_cast<const OrientationConstraint&>(other);
622 
623  if (o.link_model_ == link_model_ &&
624  robot_state::Transforms::sameFrame(desired_rotation_frame_id_, o.desired_rotation_frame_id_))
625  {
626  if (!desired_rotation_matrix_.isApprox(o.desired_rotation_matrix_))
627  return false;
628  return fabs(absolute_x_axis_tolerance_ - o.absolute_x_axis_tolerance_) <= margin &&
629  fabs(absolute_y_axis_tolerance_ - o.absolute_y_axis_tolerance_) <= margin &&
630  fabs(absolute_z_axis_tolerance_ - o.absolute_z_axis_tolerance_) <= margin;
631  }
632  return false;
633 }
634 
636 {
637  link_model_ = nullptr;
638  desired_rotation_matrix_ = Eigen::Matrix3d::Identity();
639  desired_rotation_matrix_inv_ = Eigen::Matrix3d::Identity();
640  desired_rotation_frame_id_ = "";
641  mobile_frame_ = false;
642  absolute_z_axis_tolerance_ = absolute_y_axis_tolerance_ = absolute_x_axis_tolerance_ = 0.0;
643 }
644 
646 {
647  return link_model_;
648 }
649 
651 {
652  if (!link_model_)
653  return ConstraintEvaluationResult(true, 0.0);
654 
655  std::tuple<Eigen::Vector3d, bool> euler_angles_error;
656  if (mobile_frame_)
657  {
658  // getFrameTransform() returns a valid isometry by contract
659  Eigen::Matrix3d tmp = state.getFrameTransform(desired_rotation_frame_id_).linear() * desired_rotation_matrix_;
660  // getGlobalLinkTransform() returns a valid isometry by contract
661  Eigen::Isometry3d diff(tmp.transpose() * state.getGlobalLinkTransform(link_model_).linear()); // valid isometry
662  euler_angles_error = CalcEulerAngles(diff.linear());
663  }
664  else
665  {
666  // diff is valid isometry by construction
667  Eigen::Isometry3d diff(desired_rotation_matrix_inv_ * state.getGlobalLinkTransform(link_model_).linear());
668  euler_angles_error = CalcEulerAngles(diff.linear());
669  }
670 
671  // Converting from a rotation matrix to an intrinsic XYZ euler angles have 2 singularities:
672  // pitch ~= pi/2 ==> roll + yaw = theta
673  // pitch ~= -pi/2 ==> roll - yaw = theta
674  // in those cases CalcEulerAngles will set roll (xyz(0)) to theta and yaw (xyz(2)) to zero, so for us to be able to
675  // capture yaw tolerance violation we do the following, if theta violate the absolute yaw tolerance we think of it as
676  // pure yaw rotation and set roll to zero
677  auto& xyz = std::get<Eigen::Vector3d>(euler_angles_error);
678  if (!std::get<bool>(euler_angles_error))
679  {
680  if (normalizeAbsoluteAngle(xyz(0)) > absolute_z_axis_tolerance_ + std::numeric_limits<double>::epsilon())
681  {
682  xyz(2) = xyz(0);
683  xyz(0) = 0;
684  }
685  }
686  // Account for angle wrapping
687  xyz = xyz.unaryExpr(&normalizeAbsoluteAngle);
688 
689  // 0,1,2 corresponds to XYZ, the convention used in sampling constraints
690  bool result = xyz(2) < absolute_z_axis_tolerance_ + std::numeric_limits<double>::epsilon() &&
691  xyz(1) < absolute_y_axis_tolerance_ + std::numeric_limits<double>::epsilon() &&
692  xyz(0) < absolute_x_axis_tolerance_ + std::numeric_limits<double>::epsilon();
693 
694  if (verbose)
695  {
696  Eigen::Quaterniond q_act(state.getGlobalLinkTransform(link_model_).rotation());
697  Eigen::Quaterniond q_des(desired_rotation_matrix_);
698  ROS_INFO_NAMED("kinematic_constraints",
699  "Orientation constraint %s for link '%s'. Quaternion desired: %f %f %f %f, quaternion "
700  "actual: %f %f %f %f, error: x=%f, y=%f, z=%f, tolerance: x=%f, y=%f, z=%f",
701  result ? "satisfied" : "violated", link_model_->getName().c_str(), q_des.x(), q_des.y(), q_des.z(),
702  q_des.w(), q_act.x(), q_act.y(), q_act.z(), q_act.w(), xyz(0), xyz(1), xyz(2),
703  absolute_x_axis_tolerance_, absolute_y_axis_tolerance_, absolute_z_axis_tolerance_);
704  }
705 
706  return ConstraintEvaluationResult(result, constraint_weight_ * (xyz(0) + xyz(1) + xyz(2)));
707 }
708 
709 void OrientationConstraint::print(std::ostream& out) const
710 {
711  if (link_model_)
712  {
713  out << "Orientation constraint on link '" << link_model_->getName() << "'" << std::endl;
714  Eigen::Quaterniond q_des(desired_rotation_matrix_);
715  out << "Desired orientation:" << q_des.x() << "," << q_des.y() << "," << q_des.z() << "," << q_des.w() << std::endl;
716  }
717  else
718  out << "No constraint" << std::endl;
719 }
720 
721 VisibilityConstraint::VisibilityConstraint(const robot_model::RobotModelConstPtr& model)
722  : KinematicConstraint(model), collision_robot_(new collision_detection::CollisionRobotFCL(model))
723 {
725 }
726 
728 {
729  mobile_sensor_frame_ = false;
730  mobile_target_frame_ = false;
731  target_frame_id_ = "";
732  sensor_frame_id_ = "";
733  sensor_pose_ = Eigen::Isometry3d::Identity();
735  target_pose_ = Eigen::Isometry3d::Identity();
736  cone_sides_ = 0;
737  points_.clear();
738  target_radius_ = -1.0;
739  max_view_angle_ = 0.0;
740  max_range_angle_ = 0.0;
741 }
742 
743 bool VisibilityConstraint::configure(const moveit_msgs::VisibilityConstraint& vc, const robot_state::Transforms& tf)
744 {
745  clear();
746  target_radius_ = fabs(vc.target_radius);
747 
748  if (vc.target_radius <= std::numeric_limits<double>::epsilon())
749  ROS_WARN_NAMED("kinematic_constraints",
750  "The radius of the target disc that must be visible should be strictly positive");
751 
752  if (vc.cone_sides < 3)
753  {
754  ROS_WARN_NAMED("kinematic_constraints",
755  "The number of sides for the visibility region must be 3 or more. "
756  "Assuming 3 sides instead of the specified %d",
757  vc.cone_sides);
758  cone_sides_ = 3;
759  }
760  else
761  cone_sides_ = vc.cone_sides;
762 
763  // compute the points on the base circle of the cone that make up the cone sides
764  points_.clear();
765  double delta = 2.0 * boost::math::constants::pi<double>() / (double)cone_sides_;
766  double a = 0.0;
767  for (unsigned int i = 0; i < cone_sides_; ++i, a += delta)
768  {
769  double x = sin(a) * target_radius_;
770  double y = cos(a) * target_radius_;
771  points_.push_back(Eigen::Vector3d(x, y, 0.0));
772  }
773 
774  tf2::fromMsg(vc.target_pose.pose, target_pose_);
775 
776  if (tf.isFixedFrame(vc.target_pose.header.frame_id))
777  {
778  tf.transformPose(vc.target_pose.header.frame_id, target_pose_, target_pose_);
780  mobile_target_frame_ = false;
781  // transform won't change, so apply it now
782  for (std::size_t i = 0; i < points_.size(); ++i)
783  points_[i] = target_pose_ * points_[i];
784  }
785  else
786  {
787  target_frame_id_ = vc.target_pose.header.frame_id;
788  mobile_target_frame_ = true;
789  }
790 
791  tf2::fromMsg(vc.sensor_pose.pose, sensor_pose_);
792 
793  if (tf.isFixedFrame(vc.sensor_pose.header.frame_id))
794  {
795  tf.transformPose(vc.sensor_pose.header.frame_id, sensor_pose_, sensor_pose_);
797  mobile_sensor_frame_ = false;
798  }
799  else
800  {
801  sensor_frame_id_ = vc.sensor_pose.header.frame_id;
802  mobile_sensor_frame_ = true;
803  }
804 
805  if (vc.weight <= std::numeric_limits<double>::epsilon())
806  {
807  ROS_WARN_NAMED("kinematic_constraints", "The weight of visibility constraint is near zero. Setting to 1.0.");
808  constraint_weight_ = 1.0;
809  }
810  else
811  constraint_weight_ = vc.weight;
812 
813  max_view_angle_ = vc.max_view_angle;
814  max_range_angle_ = vc.max_range_angle;
815  sensor_view_direction_ = vc.sensor_view_direction;
816 
817  return target_radius_ > std::numeric_limits<double>::epsilon();
818 }
819 
820 bool VisibilityConstraint::equal(const KinematicConstraint& other, double margin) const
821 {
822  if (other.getType() != type_)
823  return false;
824  const VisibilityConstraint& o = static_cast<const VisibilityConstraint&>(other);
825 
829  {
830  if (fabs(max_view_angle_ - o.max_view_angle_) > margin || fabs(target_radius_ - o.target_radius_) > margin)
831  return false;
832  Eigen::Isometry3d diff = sensor_pose_.inverse() * o.sensor_pose_;
833  if (diff.translation().norm() > margin)
834  return false;
835  if (!diff.rotation().isIdentity(margin))
836  return false;
837  diff = target_pose_.inverse() * o.target_pose_;
838  if (diff.translation().norm() > margin)
839  return false;
840  if (!diff.rotation().isIdentity(margin))
841  return false;
842  return true;
843  }
844  return false;
845 }
846 
848 {
849  return target_radius_ > std::numeric_limits<double>::epsilon();
850 }
851 
853 {
854  // the current pose of the sensor
855 
856  const Eigen::Isometry3d& sp =
858  const Eigen::Isometry3d& tp =
860 
861  // transform the points on the disc to the desired target frame
862  const EigenSTL::vector_Vector3d* points = &points_;
863  std::unique_ptr<EigenSTL::vector_Vector3d> temp_points;
865  {
866  temp_points.reset(new EigenSTL::vector_Vector3d(points_.size()));
867  for (std::size_t i = 0; i < points_.size(); ++i)
868  temp_points->at(i) = tp * points_[i];
869  points = temp_points.get();
870  }
871 
872  // allocate memory for a mesh to represent the visibility cone
873  shapes::Mesh* m = new shapes::Mesh();
874  m->vertex_count = cone_sides_ + 2;
875  m->vertices = new double[m->vertex_count * 3];
876  m->triangle_count = cone_sides_ * 2;
877  m->triangles = new unsigned int[m->triangle_count * 3];
878  // we do NOT allocate normals because we do not compute them
879 
880  // the sensor origin
881  m->vertices[0] = sp.translation().x();
882  m->vertices[1] = sp.translation().y();
883  m->vertices[2] = sp.translation().z();
884 
885  // the center of the base of the cone approximation
886  m->vertices[3] = tp.translation().x();
887  m->vertices[4] = tp.translation().y();
888  m->vertices[5] = tp.translation().z();
889 
890  // the points that approximate the base disc
891  for (std::size_t i = 0; i < points->size(); ++i)
892  {
893  m->vertices[i * 3 + 6] = points->at(i).x();
894  m->vertices[i * 3 + 7] = points->at(i).y();
895  m->vertices[i * 3 + 8] = points->at(i).z();
896  }
897 
898  // add the triangles
899  std::size_t p3 = points->size() * 3;
900  for (std::size_t i = 1; i < points->size(); ++i)
901  {
902  // triangle forming a side of the cone, using the sensor origin
903  std::size_t i3 = (i - 1) * 3;
904  m->triangles[i3] = i + 1;
905  m->triangles[i3 + 1] = 0;
906  m->triangles[i3 + 2] = i + 2;
907  // triangle forming a part of the base of the cone, using the center of the base
908  std::size_t i6 = p3 + i3;
909  m->triangles[i6] = i + 1;
910  m->triangles[i6 + 1] = 1;
911  m->triangles[i6 + 2] = i + 2;
912  }
913 
914  // last triangles
915  m->triangles[p3 - 3] = points->size() + 1;
916  m->triangles[p3 - 2] = 0;
917  m->triangles[p3 - 1] = 2;
918  p3 *= 2;
919  m->triangles[p3 - 3] = points->size() + 1;
920  m->triangles[p3 - 2] = 1;
921  m->triangles[p3 - 1] = 2;
922 
923  return m;
924 }
925 
927  visualization_msgs::MarkerArray& markers) const
928 {
929  shapes::Mesh* m = getVisibilityCone(state);
930  visualization_msgs::Marker mk;
932  delete m;
933  mk.header.frame_id = robot_model_->getModelFrame();
934  mk.header.stamp = ros::Time::now();
935  mk.ns = "constraints";
936  mk.id = 1;
937  mk.action = visualization_msgs::Marker::ADD;
938  mk.pose.position.x = 0;
939  mk.pose.position.y = 0;
940  mk.pose.position.z = 0;
941  mk.pose.orientation.x = 0;
942  mk.pose.orientation.y = 0;
943  mk.pose.orientation.z = 0;
944  mk.pose.orientation.w = 1;
945  mk.lifetime = ros::Duration(60);
946  // this scale necessary to make results look reasonable
947  mk.scale.x = .01;
948  mk.color.a = 1.0;
949  mk.color.r = 1.0;
950  mk.color.g = 0.0;
951  mk.color.b = 0.0;
952 
953  markers.markers.push_back(mk);
954 
955  const Eigen::Isometry3d& sp =
957  const Eigen::Isometry3d& tp =
959 
960  visualization_msgs::Marker mka;
961  mka.type = visualization_msgs::Marker::ARROW;
962  mka.action = visualization_msgs::Marker::ADD;
963  mka.color = mk.color;
964  mka.pose = mk.pose;
965 
966  mka.header = mk.header;
967  mka.ns = mk.ns;
968  mka.id = 2;
969  mka.lifetime = mk.lifetime;
970  mka.scale.x = 0.05;
971  mka.scale.y = .15;
972  mka.scale.z = 0.0;
973  mka.points.resize(2);
974  Eigen::Vector3d d = tp.translation() + tp.rotation().col(2) * 0.5;
975  mka.points[0].x = tp.translation().x();
976  mka.points[0].y = tp.translation().y();
977  mka.points[0].z = tp.translation().z();
978  mka.points[1].x = d.x();
979  mka.points[1].y = d.y();
980  mka.points[1].z = d.z();
981  markers.markers.push_back(mka);
982 
983  mka.id = 3;
984  mka.color.b = 1.0;
985  mka.color.r = 0.0;
986 
987  d = sp.translation() + sp.rotation().col(2 - sensor_view_direction_) * 0.5;
988  mka.points[0].x = sp.translation().x();
989  mka.points[0].y = sp.translation().y();
990  mka.points[0].z = sp.translation().z();
991  mka.points[1].x = d.x();
992  mka.points[1].y = d.y();
993  mka.points[1].z = d.z();
994 
995  markers.markers.push_back(mka);
996 }
997 
999 {
1000  if (target_radius_ <= std::numeric_limits<double>::epsilon())
1001  return ConstraintEvaluationResult(true, 0.0);
1002 
1003  if (max_view_angle_ > 0.0 || max_range_angle_ > 0.0)
1004  {
1005  const Eigen::Isometry3d& sp =
1007  const Eigen::Isometry3d& tp =
1009 
1010  // necessary to do subtraction as SENSOR_Z is 0 and SENSOR_X is 2
1011  const Eigen::Vector3d& normal2 = sp.rotation().col(2 - sensor_view_direction_);
1012 
1013  if (max_view_angle_ > 0.0)
1014  {
1015  const Eigen::Vector3d& normal1 = tp.rotation().col(2) * -1.0; // along Z axis and inverted
1016  double dp = normal2.dot(normal1);
1017  double ang = acos(dp);
1018  if (dp < 0.0)
1019  {
1020  if (verbose)
1021  ROS_INFO_NAMED("kinematic_constraints", "Visibility constraint is violated because the sensor is looking at "
1022  "the wrong side");
1023  return ConstraintEvaluationResult(false, 0.0);
1024  }
1025  if (max_view_angle_ < ang)
1026  {
1027  if (verbose)
1028  ROS_INFO_NAMED("kinematic_constraints",
1029  "Visibility constraint is violated because the view angle is %lf "
1030  "(above the maximum allowed of %lf)",
1031  ang, max_view_angle_);
1032  return ConstraintEvaluationResult(false, 0.0);
1033  }
1034  }
1035  if (max_range_angle_ > 0.0)
1036  {
1037  const Eigen::Vector3d& dir = (tp.translation() - sp.translation()).normalized();
1038  double dp = normal2.dot(dir);
1039  if (dp < 0.0)
1040  {
1041  if (verbose)
1042  ROS_INFO_NAMED("kinematic_constraints", "Visibility constraint is violated because the sensor is looking at "
1043  "the wrong side");
1044  return ConstraintEvaluationResult(false, 0.0);
1045  }
1046 
1047  double ang = acos(dp);
1048  if (max_range_angle_ < ang)
1049  {
1050  if (verbose)
1051  ROS_INFO_NAMED("kinematic_constraints",
1052  "Visibility constraint is violated because the range angle is %lf "
1053  "(above the maximum allowed of %lf)",
1054  ang, max_range_angle_);
1055  return ConstraintEvaluationResult(false, 0.0);
1056  }
1057  }
1058  }
1059 
1060  shapes::Mesh* m = getVisibilityCone(state);
1061  if (!m)
1062  return ConstraintEvaluationResult(false, 0.0);
1063 
1064  // add the visibility cone as an object
1066  collision_world.getWorld()->addToObject("cone", shapes::ShapeConstPtr(m), Eigen::Isometry3d::Identity());
1067 
1068  // check for collisions between the robot and the cone
1072  acm.setDefaultEntry("cone", boost::bind(&VisibilityConstraint::decideContact, this, _1));
1073  req.contacts = true;
1074  req.verbose = verbose;
1075  req.max_contacts = 1;
1076  collision_world.checkRobotCollision(req, res, *collision_robot_, state, acm);
1077 
1078  if (verbose)
1079  {
1080  std::stringstream ss;
1081  m->print(ss);
1082  ROS_INFO_NAMED("kinematic_constraints", "Visibility constraint %ssatisfied. Visibility cone approximation:\n %s",
1083  res.collision ? "not " : "", ss.str().c_str());
1084  }
1085 
1086  return ConstraintEvaluationResult(!res.collision, res.collision ? res.contacts.begin()->second.front().depth : 0.0);
1087 }
1088 
1090 {
1093  return true;
1098  {
1099  ROS_DEBUG_NAMED("kinematic_constraints", "Accepted collision with either sensor or target");
1100  return true;
1101  }
1106  {
1107  ROS_DEBUG_NAMED("kinematic_constraints", "Accepted collision with either sensor or target");
1108  return true;
1109  }
1110  return false;
1111 }
1112 
1113 void VisibilityConstraint::print(std::ostream& out) const
1114 {
1115  if (enabled())
1116  {
1117  out << "Visibility constraint for sensor in frame '" << sensor_frame_id_ << "' using target in frame '"
1118  << target_frame_id_ << "'" << std::endl;
1119  out << "Target radius: " << target_radius_ << ", using " << cone_sides_ << " sides." << std::endl;
1120  }
1121  else
1122  out << "No constraint" << std::endl;
1123 }
1124 
1126 {
1127  all_constraints_ = moveit_msgs::Constraints();
1128  kinematic_constraints_.clear();
1129  joint_constraints_.clear();
1130  position_constraints_.clear();
1131  orientation_constraints_.clear();
1132  visibility_constraints_.clear();
1133 }
1134 
1135 bool KinematicConstraintSet::add(const std::vector<moveit_msgs::JointConstraint>& jc)
1136 {
1137  bool result = true;
1138  for (unsigned int i = 0; i < jc.size(); ++i)
1139  {
1141  bool u = ev->configure(jc[i]);
1142  result = result && u;
1143  kinematic_constraints_.push_back(KinematicConstraintPtr(ev));
1144  joint_constraints_.push_back(jc[i]);
1145  all_constraints_.joint_constraints.push_back(jc[i]);
1146  }
1147  return result;
1148 }
1149 
1150 bool KinematicConstraintSet::add(const std::vector<moveit_msgs::PositionConstraint>& pc,
1151  const robot_state::Transforms& tf)
1152 {
1153  bool result = true;
1154  for (unsigned int i = 0; i < pc.size(); ++i)
1155  {
1157  bool u = ev->configure(pc[i], tf);
1158  result = result && u;
1159  kinematic_constraints_.push_back(KinematicConstraintPtr(ev));
1160  position_constraints_.push_back(pc[i]);
1161  all_constraints_.position_constraints.push_back(pc[i]);
1162  }
1163  return result;
1164 }
1165 
1166 bool KinematicConstraintSet::add(const std::vector<moveit_msgs::OrientationConstraint>& oc,
1167  const robot_state::Transforms& tf)
1168 {
1169  bool result = true;
1170  for (unsigned int i = 0; i < oc.size(); ++i)
1171  {
1173  bool u = ev->configure(oc[i], tf);
1174  result = result && u;
1175  kinematic_constraints_.push_back(KinematicConstraintPtr(ev));
1176  orientation_constraints_.push_back(oc[i]);
1177  all_constraints_.orientation_constraints.push_back(oc[i]);
1178  }
1179  return result;
1180 }
1181 
1182 bool KinematicConstraintSet::add(const std::vector<moveit_msgs::VisibilityConstraint>& vc,
1183  const robot_state::Transforms& tf)
1184 {
1185  bool result = true;
1186  for (unsigned int i = 0; i < vc.size(); ++i)
1187  {
1189  bool u = ev->configure(vc[i], tf);
1190  result = result && u;
1191  kinematic_constraints_.push_back(KinematicConstraintPtr(ev));
1192  visibility_constraints_.push_back(vc[i]);
1193  all_constraints_.visibility_constraints.push_back(vc[i]);
1194  }
1195  return result;
1196 }
1197 
1198 bool KinematicConstraintSet::add(const moveit_msgs::Constraints& c, const robot_state::Transforms& tf)
1199 {
1200  bool j = add(c.joint_constraints);
1201  bool p = add(c.position_constraints, tf);
1202  bool o = add(c.orientation_constraints, tf);
1203  bool v = add(c.visibility_constraints, tf);
1204  return j && p && o && v;
1205 }
1206 
1208 {
1209  ConstraintEvaluationResult res(true, 0.0);
1210  for (unsigned int i = 0; i < kinematic_constraints_.size(); ++i)
1211  {
1212  ConstraintEvaluationResult r = kinematic_constraints_[i]->decide(state, verbose);
1213  if (!r.satisfied)
1214  res.satisfied = false;
1215  res.distance += r.distance;
1216  }
1217  return res;
1218 }
1219 
1221  std::vector<ConstraintEvaluationResult>& results,
1222  bool verbose) const
1223 {
1224  ConstraintEvaluationResult result(true, 0.0);
1225  results.resize(kinematic_constraints_.size());
1226  for (std::size_t i = 0; i < kinematic_constraints_.size(); ++i)
1227  {
1228  results[i] = kinematic_constraints_[i]->decide(state, verbose);
1229  result.satisfied = result.satisfied && results[i].satisfied;
1230  result.distance += results[i].distance;
1231  }
1232 
1233  return result;
1234 }
1235 
1236 void KinematicConstraintSet::print(std::ostream& out) const
1237 {
1238  out << kinematic_constraints_.size() << " kinematic constraints" << std::endl;
1239  for (unsigned int i = 0; i < kinematic_constraints_.size(); ++i)
1240  kinematic_constraints_[i]->print(out);
1241 }
1242 
1243 bool KinematicConstraintSet::equal(const KinematicConstraintSet& other, double margin) const
1244 {
1245  // each constraint in this matches some in the other
1246  for (unsigned int i = 0; i < kinematic_constraints_.size(); ++i)
1247  {
1248  bool found = false;
1249  for (unsigned int j = 0; !found && j < other.kinematic_constraints_.size(); ++j)
1250  found = kinematic_constraints_[i]->equal(*other.kinematic_constraints_[j], margin);
1251  if (!found)
1252  return false;
1253  }
1254  // each constraint in the other matches some constraint in this
1255  for (unsigned int i = 0; i < other.kinematic_constraints_.size(); ++i)
1256  {
1257  bool found = false;
1258  for (unsigned int j = 0; !found && j < kinematic_constraints_.size(); ++j)
1259  found = other.kinematic_constraints_[i]->equal(*kinematic_constraints_[j], margin);
1260  if (!found)
1261  return false;
1262  }
1263  return true;
1264 }
1265 
1266 } // end of namespace kinematic_constraints
ConstraintEvaluationResult decide(const robot_state::RobotState &state, bool verbose=false) const override
Decide whether the constraint is satisfied in the indicated state.
ConstraintType getType() const
Get the type of constraint.
d
collision_detection::CollisionRobotPtr collision_robot_
A copy of the collision robot maintained for collision checking the cone against robot links...
Eigen::Isometry3d sensor_pose_
The sensor pose transformed into the transform frame.
bool configure(const moveit_msgs::OrientationConstraint &oc, const robot_state::Transforms &tf)
Configure the constraint based on a moveit_msgs::OrientationConstraint.
#define ROS_INFO_NAMED(name,...)
double getVariablePosition(const std::string &variable) const
Get the position of a particular variable. An exception is thrown if the variable is not known...
Definition: robot_state.h:246
ConstraintEvaluationResult decide(const robot_state::RobotState &state, bool verbose=false) const override
Decide whether the constraint is satisfied in the indicated state.
double absolute_z_axis_tolerance_
Storage for the tolerances.
Class for constraints on the orientation of a link.
ConstraintType type_
The type of the constraint.
int sensor_view_direction_
Storage for the sensor view direction.
Class for constraints on the visibility relationship between a sensor and a target.
Vec3fX< details::Vec3Data< double > > Vector3d
Definition: fcl_compat.h:90
#define ROS_WARN_NAMED(name,...)
std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > vector_Vector3d
bool mobile_sensor_frame_
True if the sensor is a non-fixed frame relative to the transform frame.
bool enabled() const override
This function returns true if this constraint is configured and able to decide whether states do meet...
bool configure(const moveit_msgs::PositionConstraint &pc, const robot_state::Transforms &tf)
Configure the constraint based on a moveit_msgs::PositionConstraint.
ConstraintEvaluationResult decide(const robot_state::RobotState &state, bool verbose=false) const override
Decide whether the constraint is satisfied in the indicated state.
BodyType body_type_2
The type of the second body involved in the contact.
EigenSTL::vector_Vector3d points_
A set of points along the base of the circle.
bool enabled() const override
This function returns true if this constraint is configured and able to decide whether states do meet...
std::tuple< Eigen::Matrix< typename Eigen::MatrixBase< Derived >::Scalar, 3, 1 >, bool > CalcEulerAngles(const Eigen::MatrixBase< Derived > &R)
bool enabled() const override
This function returns true if this constraint is configured and able to decide whether states do meet...
virtual void clear()=0
Clear the stored constraint.
Class for handling single DOF joint constraints.
std::shared_ptr< Body > BodyPtr
ContactMap contacts
A map returning the pairs of body ids in contact, plus their contact details.
double max_view_angle_
Storage for the max view angle.
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
void clear() override
Clear the stored constraint.
double joint_tolerance_below_
Position and tolerance values.
A class that contains many different constraints, and can check RobotState *versus the full set...
void getMarkers(const robot_state::RobotState &state, visualization_msgs::MarkerArray &markers) const
Adds markers associated with the visibility cone, sensor and target to the visualization array...
static bool sameFrame(const std::string &frame1, const std::string &frame2)
Check if two frames end up being the same once the missing / are added as prefix (if they are missing...
Definition: transforms.cpp:57
virtual bool isFixedFrame(const std::string &frame) const
Check whether a frame stays constant as the state of the robot model changes. This is true for any tr...
Definition: transforms.cpp:81
bool equal(const KinematicConstraint &other, double margin) const override
Check if two constraints are the same. For position constraints this means that:
bool equal(const KinematicConstraint &other, double margin) const override
Check if two orientation constraints are the same.
void print(std::ostream &out=std::cout) const
Print the constraint data.
bool configure(const moveit_msgs::VisibilityConstraint &vc, const robot_state::Transforms &tf)
Configure the constraint based on a moveit_msgs::VisibilityConstraint.
#define M_PI
bool add(const actionlib::TwoIntsGoal &req, actionlib::TwoIntsResult &res)
Representation and evaluation of kinematic constraints.
geometry_msgs::TransformStamped t
unsigned int * triangles
Provides an implementation of a snapshot of a transform tree that can be easily queried for transform...
Definition: transforms.h:60
Generic interface to collision detection.
double distance
The distance evaluation from the constraint or constraints.
double y
double target_radius_
Storage for the target radius.
const robot_model::LinkModel * link_model_
The target link model.
bool collision
True if collision was found, false otherwise.
Eigen::Matrix3d desired_rotation_matrix_
The desired rotation matrix in the tf frame.
std::string sensor_frame_id_
The sensor frame id.
bool add(const moveit_msgs::Constraints &c, const robot_state::Transforms &tf)
Add all known constraints.
KinematicConstraint(const robot_model::RobotModelConstPtr &model)
Constructor.
bool satisfied
Whether or not the constraint or constraints were satisfied.
#define ROS_DEBUG_NAMED(name,...)
unsigned int vertex_count
void checkRobotCollision(const CollisionRequest &req, CollisionResult &res, const CollisionRobot &robot, const robot_state::RobotState &state) const override
Check whether the robot model is in collision with the world. Any collisions between a robot link and...
void print(std::ostream &out=std::cout) const override
Print the constraint data.
std::size_t max_contacts
Overall maximum number of contacts to compute.
bool verbose
Flag indicating whether information about detected collisions should be reported. ...
void print(std::ostream &out=std::cout) const override
Print the constraint data.
bool isContinuous() const
Check if this joint wraps around.
std::string desired_rotation_frame_id_
The target frame of the transform tree.
static double normalizeAbsoluteAngle(const double angle)
std::string local_variable_name_
The local variable name for a multi DOF joint, if any.
std::vector< bodies::BodyPtr > constraint_region_
The constraint region vector.
unsigned int triangle_count
Definition of a structure for the allowed collision matrix. All elements in the collision world are r...
bool mobile_target_frame_
True if the target is a non-fixed frame relative to the transform frame.
Body * createBodyFromShape(const shapes::Shape *shape)
bool equal(const KinematicConstraintSet &other, double margin) const
Whether or not another KinematicConstraintSet is equal to this one.
void fromMsg(const A &, B &b)
const robot_model::LinkModel * link_model_
The link model constraint subject.
double epsilon
const std::string & getTargetFrame() const
Get the planning frame corresponding to this set of transforms.
Definition: transforms.cpp:66
void transformQuaternion(const std::string &from_frame, const Eigen::Quaterniond &q_in, Eigen::Quaterniond &q_out) const
Transform a quaternion in from_frame to the target_frame.
Definition: transforms.h:149
robot_model::RobotModelConstPtr robot_model_
The kinematic model associated with this constraint.
std::string constraint_frame_id_
The constraint frame id.
double * vertices
std::string body_name_2
The id of the second body involved in the contact.
Eigen::Vector3d offset_
The target offset.
Base class for representing a kinematic constraint.
void print(std::ostream &out=std::cout) const override
Print the constraint data.
void clear() override
Clear the stored constraint.
EigenSTL::vector_Isometry3d constraint_region_pose_
The constraint region pose vector.
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
bool contacts
If true, compute contacts. Otherwise only a binary collision yes/no is reported.
VisibilityConstraint(const robot_model::RobotModelConstPtr &model)
Constructor.
bool constructMarkerFromShape(const Shape *shape, visualization_msgs::Marker &mk, bool use_mesh_triangle_list=false)
virtual bool enabled() const =0
This function returns true if this constraint is configured and able to decide whether states do meet...
bool decideContact(const collision_detection::Contact &contact) const
Function that gets passed into collision checking to allow some collisions.
Shape * constructShapeFromMsg(const shape_msgs::SolidPrimitive &shape_msg)
Struct for containing the results of constraint evaluation.
const Eigen::Isometry3d & getFrameTransform(const std::string &frame_id)
Get the transformation matrix from the model frame to the frame identified by frame_id.
ConstraintEvaluationResult decide(const robot_state::RobotState &state, bool verbose=false) const
Determines whether all constraints are satisfied by state, returning a single evaluation result...
Class for constraints on the XYZ position of a link.
std::string target_frame_id_
The target frame id.
void setDefaultEntry(const std::string &name, bool allowed)
Set the default value for entries that include name. If such a default value is set, queries to getAllowedCollision() that include name will return this value instead, even if a pair that includes name was previously specified with setEntry().
unsigned int cone_sides_
Storage for the cone sides.
Eigen::Isometry3d target_pose_
The target pose transformed into the transform frame.
bool equal(const KinematicConstraint &other, double margin) const override
Check if two joint constraints are the same.
std::string body_name_1
The id of the first body involved in the contact.
void clear() override
Clear the stored constraint.
ConstraintEvaluationResult decide(const robot_state::RobotState &state, bool verbose=false) const override
Decide whether the constraint is satisfied in the indicated state.
INLINE Rall1d< T, V, S > acos(const Rall1d< T, V, S > &x)
double max_range_angle_
Storage for the max range angle.
const Eigen::Isometry3d & getGlobalLinkTransform(const std::string &link_name)
Definition: robot_state.h:1474
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
std::vector< KinematicConstraintPtr > kinematic_constraints_
Shared pointers to all the member constraints.
#define ROS_ERROR_NAMED(name,...)
const robot_model::JointModel * joint_model_
The joint from the kinematic model for this constraint.
static Time now()
Representation of a robot&#39;s state. This includes position, velocity, acceleration and effort...
Definition: robot_state.h:128
BodyType body_type_1
The type of the first body involved in the contact.
void print(std::ostream &out=std::cout) const override
Print the constraint data.
static ConstraintEvaluationResult finishPositionConstraintDecision(const Eigen::Vector3d &pt, const Eigen::Vector3d &desired, const std::string &name, double weight, bool result, bool verbose)
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
void print(std::ostream &out=std::cout) const override
void clear()
Clear the stored constraints.
double constraint_weight_
The weight of a constraint is a multiplicative factor associated to the distance computed by the deci...
shapes::Mesh * getVisibilityCone(const robot_state::RobotState &state) const
Gets a trimesh shape representing the visibility cone.
void transformPose(const std::string &from_frame, const Eigen::Isometry3d &t_in, Eigen::Isometry3d &t_out) const
Transform a pose in from_frame to the target_frame.
Definition: transforms.h:172
double x
r
static double normalizeAngle(double angle)
bool equal(const KinematicConstraint &other, double margin) const override
Check if two constraints are the same.
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
bool enabled() const override
This function returns true if this constraint is configured and able to decide whether states do meet...
bool configure(const moveit_msgs::JointConstraint &jc)
Configure the constraint based on a moveit_msgs::JointConstraint.
void clear() override
Clear the stored constraint.
std::shared_ptr< const Shape > ShapeConstPtr


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed May 5 2021 02:53:34