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 
118 KinematicConstraint::~KinematicConstraint() = default;
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() == moveit::core::JointModel::REVOLUTE)
198  {
199  const moveit::core::RevoluteJointModel* rjoint =
200  static_cast<const moveit::core::RevoluteJointModel*>(joint_model_);
201  if (rjoint->isContinuous())
202  joint_is_continuous_ = true;
203  }
204  else if (joint_model_->getType() == moveit::core::JointModel::PLANAR)
205  {
206  if (local_variable_name_ == "theta")
207  joint_is_continuous_ = true;
208  }
209 
210  if (joint_is_continuous_)
211  {
212  joint_position_ = normalizeAngle(jc.position);
213  }
214  else
215  {
216  joint_position_ = jc.position;
217  const moveit::core::VariableBounds& bounds = joint_model_->getVariableBounds(joint_variable_name_);
218 
219  if (bounds.min_position_ > joint_position_ + joint_tolerance_above_)
220  {
221  joint_position_ = bounds.min_position_;
222  joint_tolerance_above_ = std::numeric_limits<double>::epsilon();
223  ROS_WARN_NAMED("kinematic_constraints",
224  "Joint %s is constrained to be below the minimum bounds. "
225  "Assuming minimum bounds instead.",
226  jc.joint_name.c_str());
227  }
228  else if (bounds.max_position_ < joint_position_ - joint_tolerance_below_)
229  {
230  joint_position_ = bounds.max_position_;
231  joint_tolerance_below_ = std::numeric_limits<double>::epsilon();
232  ROS_WARN_NAMED("kinematic_constraints",
233  "Joint %s is constrained to be above the maximum bounds. "
234  "Assuming maximum bounds instead.",
235  jc.joint_name.c_str());
236  }
237  }
238 
239  if (jc.weight <= std::numeric_limits<double>::epsilon())
240  {
241  ROS_WARN_NAMED("kinematic_constraints",
242  "The weight on constraint for joint '%s' is very near zero. Setting to 1.0.",
243  jc.joint_name.c_str());
244  constraint_weight_ = 1.0;
245  }
246  else
247  constraint_weight_ = jc.weight;
248  }
249  return joint_model_ != nullptr;
250 }
251 
252 bool JointConstraint::equal(const KinematicConstraint& other, double margin) const
253 {
254  if (other.getType() != type_)
255  return false;
256  const JointConstraint& o = static_cast<const JointConstraint&>(other);
257  if (o.joint_model_ == joint_model_ && o.local_variable_name_ == local_variable_name_)
258  return fabs(joint_position_ - o.joint_position_) <= margin &&
259  fabs(joint_tolerance_above_ - o.joint_tolerance_above_) <= margin &&
260  fabs(joint_tolerance_below_ - o.joint_tolerance_below_) <= margin;
261  return false;
262 }
263 
264 ConstraintEvaluationResult JointConstraint::decide(const moveit::core::RobotState& state, bool verbose) const
265 {
266  if (!joint_model_)
267  return ConstraintEvaluationResult(true, 0.0);
268 
269  double current_joint_position = state.getVariablePosition(joint_variable_index_);
270  double dif = 0.0;
271 
272  // compute signed shortest distance for continuous joints
273  if (joint_is_continuous_)
274  {
275  dif = normalizeAngle(current_joint_position) - joint_position_;
276 
277  if (dif > boost::math::constants::pi<double>())
278  dif = 2.0 * boost::math::constants::pi<double>() - dif;
279  else if (dif < -boost::math::constants::pi<double>())
280  dif += 2.0 * boost::math::constants::pi<double>(); // we include a sign change to have dif > 0
281  }
282  else
283  dif = current_joint_position - joint_position_;
284 
285  // check bounds
286  bool result = dif <= (joint_tolerance_above_ + 2.0 * std::numeric_limits<double>::epsilon()) &&
287  dif >= (-joint_tolerance_below_ - 2.0 * std::numeric_limits<double>::epsilon());
288  if (verbose)
289  ROS_INFO_NAMED("kinematic_constraints",
290  "Constraint %s:: Joint name: '%s', actual value: %f, desired value: %f, "
291  "tolerance_above: %f, tolerance_below: %f",
292  result ? "satisfied" : "violated", joint_variable_name_.c_str(), current_joint_position,
293  joint_position_, joint_tolerance_above_, joint_tolerance_below_);
294  return ConstraintEvaluationResult(result, constraint_weight_ * fabs(dif));
295 }
296 
297 bool JointConstraint::enabled() const
298 {
299  return joint_model_;
300 }
301 
302 void JointConstraint::clear()
303 {
304  joint_model_ = nullptr;
305  joint_variable_index_ = -1;
306  joint_is_continuous_ = false;
307  local_variable_name_ = "";
308  joint_variable_name_ = "";
309  joint_position_ = joint_tolerance_below_ = joint_tolerance_above_ = 0.0;
310 }
311 
312 void JointConstraint::print(std::ostream& out) const
313 {
314  if (joint_model_)
315  {
316  out << "Joint constraint for joint " << joint_variable_name_ << ": " << std::endl;
317  out << " value = ";
318  out << joint_position_ << "; ";
319  out << " tolerance below = ";
320  out << joint_tolerance_below_ << "; ";
321  out << " tolerance above = ";
322  out << joint_tolerance_above_ << "; ";
323  out << std::endl;
324  }
325  else
326  out << "No constraint" << std::endl;
327 }
328 
329 bool PositionConstraint::configure(const moveit_msgs::PositionConstraint& pc, const moveit::core::Transforms& tf)
330 {
331  // clearing before we configure to get rid of any old data
332  clear();
333 
334  link_model_ = robot_model_->getLinkModel(pc.link_name);
335  if (link_model_ == nullptr)
336  {
337  ROS_WARN_NAMED("kinematic_constraints",
338  "Position constraint link model %s not found in kinematic model. Constraint invalid.",
339  pc.link_name.c_str());
340  return false;
341  }
342 
343  if (pc.header.frame_id.empty())
344  {
345  ROS_WARN_NAMED("kinematic_constraints", "No frame specified for position constraint on link '%s'!",
346  pc.link_name.c_str());
347  return false;
348  }
349 
350  offset_ = Eigen::Vector3d(pc.target_point_offset.x, pc.target_point_offset.y, pc.target_point_offset.z);
351  has_offset_ = offset_.squaredNorm() > std::numeric_limits<double>::epsilon();
352 
353  if (tf.isFixedFrame(pc.header.frame_id))
354  {
355  constraint_frame_id_ = tf.getTargetFrame();
356  mobile_frame_ = false;
357  }
358  else
359  {
360  constraint_frame_id_ = pc.header.frame_id;
361  mobile_frame_ = true;
362  }
363 
364  // load primitive shapes, first clearing any we already have
365  for (std::size_t i = 0; i < pc.constraint_region.primitives.size(); ++i)
366  {
367  std::unique_ptr<shapes::Shape> shape(shapes::constructShapeFromMsg(pc.constraint_region.primitives[i]));
368  if (shape)
369  {
370  if (pc.constraint_region.primitive_poses.size() <= i)
371  {
372  ROS_WARN_NAMED("kinematic_constraints", "Constraint region message does not contain enough primitive poses");
373  continue;
374  }
375  Eigen::Isometry3d t;
376  tf2::fromMsg(pc.constraint_region.primitive_poses[i], t);
377  ASSERT_ISOMETRY(t) // unsanitized input, could contain a non-isometry
378  constraint_region_pose_.push_back(t);
379  if (!mobile_frame_)
380  tf.transformPose(pc.header.frame_id, constraint_region_pose_.back(), constraint_region_pose_.back());
381 
382  const bodies::BodyPtr body(bodies::createEmptyBodyFromShapeType(shape->type));
383  body->setDimensionsDirty(shape.get());
384  body->setPoseDirty(constraint_region_pose_.back());
385  body->updateInternalData();
386  constraint_region_.push_back(body);
387  }
388  else
389  ROS_WARN_NAMED("kinematic_constraints", "Could not construct primitive shape %zu", i);
390  }
391 
392  // load meshes
393  for (std::size_t i = 0; i < pc.constraint_region.meshes.size(); ++i)
394  {
395  std::unique_ptr<shapes::Shape> shape(shapes::constructShapeFromMsg(pc.constraint_region.meshes[i]));
396  if (shape)
397  {
398  if (pc.constraint_region.mesh_poses.size() <= i)
399  {
400  ROS_WARN_NAMED("kinematic_constraints", "Constraint region message does not contain enough primitive poses");
401  continue;
402  }
403  Eigen::Isometry3d t;
404  tf2::fromMsg(pc.constraint_region.mesh_poses[i], t);
405  ASSERT_ISOMETRY(t) // unsanitized input, could contain a non-isometry
406  constraint_region_pose_.push_back(t);
407  if (!mobile_frame_)
408  tf.transformPose(pc.header.frame_id, constraint_region_pose_.back(), constraint_region_pose_.back());
409  const bodies::BodyPtr body(bodies::createEmptyBodyFromShapeType(shape->type));
410  body->setDimensionsDirty(shape.get());
411  body->setPoseDirty(constraint_region_pose_.back());
412  body->updateInternalData();
413  constraint_region_.push_back(body);
414  }
415  else
416  {
417  ROS_WARN_NAMED("kinematic_constraints", "Could not construct mesh shape %zu", i);
418  }
419  }
420 
421  if (pc.weight <= std::numeric_limits<double>::epsilon())
422  {
423  ROS_WARN_NAMED("kinematic_constraints",
424  "The weight on position constraint for link '%s' is near zero. Setting to 1.0.",
425  pc.link_name.c_str());
426  constraint_weight_ = 1.0;
427  }
428  else
429  constraint_weight_ = pc.weight;
430 
431  return !constraint_region_.empty();
432 }
433 
434 bool PositionConstraint::equal(const KinematicConstraint& other, double margin) const
435 {
436  if (other.getType() != type_)
437  return false;
438  const PositionConstraint& o = static_cast<const PositionConstraint&>(other);
439 
440  if (link_model_ == o.link_model_ && moveit::core::Transforms::sameFrame(constraint_frame_id_, o.constraint_frame_id_))
441  {
442  if ((offset_ - o.offset_).norm() > margin)
443  return false;
444  std::vector<bool> other_region_matches_this(constraint_region_.size(), false);
445  for (std::size_t i = 0; i < constraint_region_.size(); ++i)
446  {
447  bool some_match = false;
448  // need to check against all other regions
449  for (std::size_t j = 0; j < o.constraint_region_.size(); ++j)
450  {
451  // constraint_region_pose_ contain only valid isometries, so diff is also a valid isometry
452  Eigen::Isometry3d diff = constraint_region_pose_[i].inverse() * o.constraint_region_pose_[j];
453  if (diff.translation().norm() < margin && diff.linear().isIdentity(margin) &&
454  constraint_region_[i]->getType() == o.constraint_region_[j]->getType() &&
455  fabs(constraint_region_[i]->computeVolume() - o.constraint_region_[j]->computeVolume()) < margin)
456  {
457  some_match = true;
458  // can't break, as need to do matches the other way as well
459  other_region_matches_this[j] = true;
460  }
461  }
462  if (!some_match)
463  return false;
464  }
465  for (std::size_t i = 0; i < o.constraint_region_.size(); ++i)
466  if (!other_region_matches_this[i])
467  return false;
468  return true;
469  }
470  return false;
471 }
472 
473 // helper function to avoid code duplication
474 static inline ConstraintEvaluationResult finishPositionConstraintDecision(const Eigen::Vector3d& pt,
475  const Eigen::Vector3d& desired,
476  const std::string& name, double weight,
477  bool result, bool verbose)
478 {
479  double dx = desired.x() - pt.x();
480  double dy = desired.y() - pt.y();
481  double dz = desired.z() - pt.z();
482  if (verbose)
483  {
484  ROS_INFO_NAMED("kinematic_constraints",
485  "Position constraint %s on link '%s'. Desired: %f, %f, %f, current: %f, %f, %f",
486  result ? "satisfied" : "violated", name.c_str(), desired.x(), desired.y(), desired.z(), pt.x(),
487  pt.y(), pt.z());
488  ROS_INFO_NAMED("kinematic_constraints", "Differences %g %g %g", dx, dy, dz);
489  }
490  return ConstraintEvaluationResult(result, weight * sqrt(dx * dx + dy * dy + dz * dz));
491 }
492 
493 ConstraintEvaluationResult PositionConstraint::decide(const moveit::core::RobotState& state, bool verbose) const
494 {
495  if (!link_model_ || constraint_region_.empty())
496  return ConstraintEvaluationResult(true, 0.0);
497 
498  Eigen::Vector3d pt = state.getGlobalLinkTransform(link_model_) * offset_;
499  if (mobile_frame_)
500  {
501  for (std::size_t i = 0; i < constraint_region_.size(); ++i)
502  {
503  Eigen::Isometry3d tmp = state.getFrameTransform(constraint_frame_id_) * constraint_region_pose_[i];
504  bool result = constraint_region_[i]->cloneAt(tmp)->containsPoint(pt, verbose);
505  if (result || (i + 1 == constraint_region_pose_.size()))
506  return finishPositionConstraintDecision(pt, tmp.translation(), link_model_->getName(), constraint_weight_,
507  result, verbose);
508  else
509  finishPositionConstraintDecision(pt, tmp.translation(), link_model_->getName(), constraint_weight_, result,
510  verbose);
511  }
512  }
513  else
514  {
515  for (std::size_t i = 0; i < constraint_region_.size(); ++i)
516  {
517  bool result = constraint_region_[i]->containsPoint(pt, true);
518  if (result || (i + 1 == constraint_region_.size()))
519  return finishPositionConstraintDecision(pt, constraint_region_[i]->getPose().translation(),
520  link_model_->getName(), constraint_weight_, result, verbose);
521  else
522  finishPositionConstraintDecision(pt, constraint_region_[i]->getPose().translation(), link_model_->getName(),
523  constraint_weight_, result, verbose);
524  }
525  }
526  return ConstraintEvaluationResult(false, 0.0);
527 }
528 
529 void PositionConstraint::print(std::ostream& out) const
530 {
531  if (enabled())
532  out << "Position constraint on link '" << link_model_->getName() << "'" << std::endl;
533  else
534  out << "No constraint" << std::endl;
535 }
536 
537 void PositionConstraint::clear()
538 {
539  offset_ = Eigen::Vector3d(0.0, 0.0, 0.0);
540  has_offset_ = false;
541  constraint_region_.clear();
542  constraint_region_pose_.clear();
543  mobile_frame_ = false;
544  constraint_frame_id_ = "";
545  link_model_ = nullptr;
546 }
547 
548 bool PositionConstraint::enabled() const
549 {
550  return link_model_ && !constraint_region_.empty();
551 }
552 
553 bool OrientationConstraint::configure(const moveit_msgs::OrientationConstraint& oc, const moveit::core::Transforms& tf)
554 {
555  // clearing out any old data
556  clear();
557 
558  link_model_ = robot_model_->getLinkModel(oc.link_name);
559  if (!link_model_)
560  {
561  ROS_WARN_NAMED("kinematic_constraints", "Could not find link model for link name %s", oc.link_name.c_str());
562  return false;
563  }
564  Eigen::Quaterniond q;
565  tf2::fromMsg(oc.orientation, q);
566  if (fabs(q.norm() - 1.0) > 1e-3)
567  {
568  ROS_WARN_NAMED("kinematic_constraints",
569  "Orientation constraint for link '%s' is probably incorrect: %f, %f, %f, "
570  "%f. Assuming identity instead.",
571  oc.link_name.c_str(), oc.orientation.x, oc.orientation.y, oc.orientation.z, oc.orientation.w);
572  q = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0);
573  }
574 
575  if (oc.header.frame_id.empty())
576  ROS_WARN_NAMED("kinematic_constraints", "No frame specified for position constraint on link '%s'!",
577  oc.link_name.c_str());
578 
579  if (tf.isFixedFrame(oc.header.frame_id))
580  {
581  tf.transformQuaternion(oc.header.frame_id, q, q);
582  desired_rotation_frame_id_ = tf.getTargetFrame();
583  desired_rotation_matrix_ = Eigen::Matrix3d(q);
584  desired_rotation_matrix_inv_ = desired_rotation_matrix_.transpose();
585  mobile_frame_ = false;
586  }
587  else
588  {
589  desired_rotation_frame_id_ = oc.header.frame_id;
590  desired_rotation_matrix_ = Eigen::Matrix3d(q);
591  mobile_frame_ = true;
592  }
593  std::stringstream matrix_str;
594  matrix_str << desired_rotation_matrix_;
595  ROS_DEBUG_NAMED("kinematic_constraints", "The desired rotation matrix for link '%s' in frame %s is:\n%s",
596  oc.link_name.c_str(), desired_rotation_frame_id_.c_str(), matrix_str.str().c_str());
597 
598  if (oc.weight <= std::numeric_limits<double>::epsilon())
599  {
600  ROS_WARN_NAMED("kinematic_constraints",
601  "The weight on position constraint for link '%s' is near zero. Setting to 1.0.",
602  oc.link_name.c_str());
603  constraint_weight_ = 1.0;
604  }
605  else
606  {
607  constraint_weight_ = oc.weight;
608  }
609 
610  parameterization_type_ = oc.parameterization;
611  // validate the parameterization, set to default value if invalid
612  if (parameterization_type_ != moveit_msgs::OrientationConstraint::XYZ_EULER_ANGLES &&
613  parameterization_type_ != moveit_msgs::OrientationConstraint::ROTATION_VECTOR)
614  {
615  ROS_WARN_NAMED("kinematic_constraints",
616  "Unknown parameterization for orientation constraint tolerance, using default (XYZ_EULER_ANGLES).");
617  parameterization_type_ = moveit_msgs::OrientationConstraint::XYZ_EULER_ANGLES;
618  }
619 
620  absolute_x_axis_tolerance_ = fabs(oc.absolute_x_axis_tolerance);
621  if (absolute_x_axis_tolerance_ < std::numeric_limits<double>::epsilon())
622  ROS_WARN_NAMED("kinematic_constraints", "Near-zero value for absolute_x_axis_tolerance");
623  absolute_y_axis_tolerance_ = fabs(oc.absolute_y_axis_tolerance);
624  if (absolute_y_axis_tolerance_ < std::numeric_limits<double>::epsilon())
625  ROS_WARN_NAMED("kinematic_constraints", "Near-zero value for absolute_y_axis_tolerance");
626  absolute_z_axis_tolerance_ = fabs(oc.absolute_z_axis_tolerance);
627  if (absolute_z_axis_tolerance_ < std::numeric_limits<double>::epsilon())
628  ROS_WARN_NAMED("kinematic_constraints", "Near-zero value for absolute_z_axis_tolerance");
629 
630  return link_model_ != nullptr;
631 }
632 
633 bool OrientationConstraint::equal(const KinematicConstraint& other, double margin) const
634 {
635  if (other.getType() != type_)
636  return false;
637  const OrientationConstraint& o = static_cast<const OrientationConstraint&>(other);
638 
639  if (o.link_model_ == link_model_ &&
640  moveit::core::Transforms::sameFrame(desired_rotation_frame_id_, o.desired_rotation_frame_id_))
641  {
642  if (!desired_rotation_matrix_.isApprox(o.desired_rotation_matrix_))
643  return false;
644  return fabs(absolute_x_axis_tolerance_ - o.absolute_x_axis_tolerance_) <= margin &&
645  fabs(absolute_y_axis_tolerance_ - o.absolute_y_axis_tolerance_) <= margin &&
646  fabs(absolute_z_axis_tolerance_ - o.absolute_z_axis_tolerance_) <= margin;
647  }
648  return false;
649 }
650 
651 void OrientationConstraint::clear()
652 {
653  link_model_ = nullptr;
654  desired_rotation_matrix_ = Eigen::Matrix3d::Identity();
655  desired_rotation_matrix_inv_ = Eigen::Matrix3d::Identity();
656  desired_rotation_frame_id_ = "";
657  mobile_frame_ = false;
658  absolute_z_axis_tolerance_ = absolute_y_axis_tolerance_ = absolute_x_axis_tolerance_ = 0.0;
659 }
660 
661 bool OrientationConstraint::enabled() const
662 {
663  return link_model_;
664 }
665 
666 ConstraintEvaluationResult OrientationConstraint::decide(const moveit::core::RobotState& state, bool verbose) const
667 {
668  if (!link_model_)
669  return ConstraintEvaluationResult(true, 0.0);
670 
671  Eigen::Isometry3d diff;
672  if (mobile_frame_)
673  {
674  // getFrameTransform() returns a valid isometry by contract
675  Eigen::Matrix3d tmp = state.getFrameTransform(desired_rotation_frame_id_).linear() * desired_rotation_matrix_;
676  // getGlobalLinkTransform() returns a valid isometry by contract
677  diff = Eigen::Isometry3d(tmp.transpose() * state.getGlobalLinkTransform(link_model_).linear()); // valid isometry
678  }
679  else
680  {
681  // diff is valid isometry by construction
682  diff = Eigen::Isometry3d(desired_rotation_matrix_inv_ * state.getGlobalLinkTransform(link_model_).linear());
683  }
684 
685  // This needs to live outside the if-block scope (as xyz_rotation points to its data).
686  std::tuple<Eigen::Vector3d, bool> euler_angles_error;
687  Eigen::Vector3d xyz_rotation;
688  if (parameterization_type_ == moveit_msgs::OrientationConstraint::XYZ_EULER_ANGLES)
689  {
690  euler_angles_error = CalcEulerAngles(diff.linear());
691  // Converting from a rotation matrix to intrinsic XYZ Euler angles has 2 singularities:
692  // pitch ~= pi/2 ==> roll + yaw = theta
693  // pitch ~= -pi/2 ==> roll - yaw = theta
694  // in those cases CalcEulerAngles will set roll (xyz_rotation(0)) to theta and yaw (xyz_rotation(2)) to zero, so for
695  // us to be able to capture yaw tolerance violations we do the following: If theta violates the absolute yaw
696  // tolerance we think of it as a pure yaw rotation and set roll to zero.
697  xyz_rotation = std::get<Eigen::Vector3d>(euler_angles_error);
698  if (!std::get<bool>(euler_angles_error))
699  {
700  if (normalizeAbsoluteAngle(xyz_rotation(0)) > absolute_z_axis_tolerance_ + std::numeric_limits<double>::epsilon())
701  {
702  xyz_rotation(2) = xyz_rotation(0);
703  xyz_rotation(0) = 0;
704  }
705  }
706  // Account for angle wrapping
707  xyz_rotation = xyz_rotation.unaryExpr(&normalizeAbsoluteAngle);
708  }
709  else if (parameterization_type_ == moveit_msgs::OrientationConstraint::ROTATION_VECTOR)
710  {
711  Eigen::AngleAxisd aa(diff.linear());
712  xyz_rotation = aa.axis() * aa.angle();
713  xyz_rotation(0) = fabs(xyz_rotation(0));
714  xyz_rotation(1) = fabs(xyz_rotation(1));
715  xyz_rotation(2) = fabs(xyz_rotation(2));
716  }
717  else
718  {
719  /* The parameterization type should be validated in configure, so this should never happen. */
720  ROS_ERROR_STREAM_NAMED("kinematic_constraints",
721  "The parameterization type for the orientation constraints is invalid.");
722  }
723 
724  bool result = xyz_rotation(2) < absolute_z_axis_tolerance_ + std::numeric_limits<double>::epsilon() &&
725  xyz_rotation(1) < absolute_y_axis_tolerance_ + std::numeric_limits<double>::epsilon() &&
726  xyz_rotation(0) < absolute_x_axis_tolerance_ + std::numeric_limits<double>::epsilon();
727 
728  if (verbose)
729  {
730  Eigen::Quaterniond q_act(state.getGlobalLinkTransform(link_model_).linear());
731  Eigen::Quaterniond q_des(desired_rotation_matrix_);
732  ROS_INFO_NAMED("kinematic_constraints",
733  "Orientation constraint %s for link '%s'. Quaternion desired: %f %f %f %f, quaternion "
734  "actual: %f %f %f %f, error: x=%f, y=%f, z=%f, tolerance: x=%f, y=%f, z=%f",
735  result ? "satisfied" : "violated", link_model_->getName().c_str(), q_des.x(), q_des.y(), q_des.z(),
736  q_des.w(), q_act.x(), q_act.y(), q_act.z(), q_act.w(), xyz_rotation(0), xyz_rotation(1),
737  xyz_rotation(2), absolute_x_axis_tolerance_, absolute_y_axis_tolerance_, absolute_z_axis_tolerance_);
738  }
739 
740  return ConstraintEvaluationResult(result, constraint_weight_ * (xyz_rotation(0) + xyz_rotation(1) + xyz_rotation(2)));
741 }
742 
743 void OrientationConstraint::print(std::ostream& out) const
744 {
745  if (link_model_)
746  {
747  out << "Orientation constraint on link '" << link_model_->getName() << "'" << std::endl;
748  Eigen::Quaterniond q_des(desired_rotation_matrix_);
749  out << "Desired orientation:" << q_des.x() << "," << q_des.y() << "," << q_des.z() << "," << q_des.w() << std::endl;
750  }
751  else
752  out << "No constraint" << std::endl;
753 }
754 
755 VisibilityConstraint::VisibilityConstraint(const moveit::core::RobotModelConstPtr& model)
756  : KinematicConstraint(model), collision_env_(new collision_detection::CollisionEnvFCL(model))
757 {
759 }
760 
762 {
763  mobile_sensor_frame_ = false;
764  mobile_target_frame_ = false;
765  target_frame_id_ = "";
766  sensor_frame_id_ = "";
767  sensor_pose_ = Eigen::Isometry3d::Identity();
769  target_pose_ = Eigen::Isometry3d::Identity();
770  cone_sides_ = 0;
771  points_.clear();
772  target_radius_ = -1.0;
773  max_view_angle_ = 0.0;
774  max_range_angle_ = 0.0;
775 }
776 
777 bool VisibilityConstraint::configure(const moveit_msgs::VisibilityConstraint& vc, const moveit::core::Transforms& tf)
778 {
779  clear();
780  target_radius_ = fabs(vc.target_radius);
781 
782  if (vc.target_radius <= std::numeric_limits<double>::epsilon())
783  ROS_WARN_NAMED("kinematic_constraints",
784  "The radius of the target disc that must be visible should be strictly positive");
785 
786  if (vc.cone_sides < 3)
787  {
788  ROS_WARN_NAMED("kinematic_constraints",
789  "The number of sides for the visibility region must be 3 or more. "
790  "Assuming 3 sides instead of the specified %d",
791  vc.cone_sides);
792  cone_sides_ = 3;
793  }
794  else
795  cone_sides_ = vc.cone_sides;
796 
797  // compute the points on the base circle of the cone that make up the cone sides
798  points_.clear();
799  double delta = 2.0 * boost::math::constants::pi<double>() / (double)cone_sides_;
800  double a = 0.0;
801  for (unsigned int i = 0; i < cone_sides_; ++i, a += delta)
802  {
803  double x = sin(a) * target_radius_;
804  double y = cos(a) * target_radius_;
805  points_.push_back(Eigen::Vector3d(x, y, 0.0));
806  }
807 
808  tf2::fromMsg(vc.target_pose.pose, target_pose_);
809  ASSERT_ISOMETRY(target_pose_) // unsanitized input, could contain a non-isometry
810 
811  if (tf.isFixedFrame(vc.target_pose.header.frame_id))
812  {
813  tf.transformPose(vc.target_pose.header.frame_id, target_pose_, target_pose_);
815  mobile_target_frame_ = false;
816  // transform won't change, so apply it now
817  for (Eigen::Vector3d& point : points_)
819  }
820  else
821  {
822  target_frame_id_ = vc.target_pose.header.frame_id;
823  mobile_target_frame_ = true;
824  }
825 
826  tf2::fromMsg(vc.sensor_pose.pose, sensor_pose_);
827  ASSERT_ISOMETRY(sensor_pose_) // unsanitized input, could contain a non-isometry
828 
829  if (tf.isFixedFrame(vc.sensor_pose.header.frame_id))
830  {
831  tf.transformPose(vc.sensor_pose.header.frame_id, sensor_pose_, sensor_pose_);
833  mobile_sensor_frame_ = false;
834  }
835  else
836  {
837  sensor_frame_id_ = vc.sensor_pose.header.frame_id;
838  mobile_sensor_frame_ = true;
839  }
840 
841  if (vc.weight <= std::numeric_limits<double>::epsilon())
842  {
843  ROS_WARN_NAMED("kinematic_constraints", "The weight of visibility constraint is near zero. Setting to 1.0.");
844  constraint_weight_ = 1.0;
845  }
846  else
847  constraint_weight_ = vc.weight;
848 
849  max_view_angle_ = vc.max_view_angle;
850  max_range_angle_ = vc.max_range_angle;
851  sensor_view_direction_ = vc.sensor_view_direction;
852 
853  return target_radius_ > std::numeric_limits<double>::epsilon();
854 }
855 
856 bool VisibilityConstraint::equal(const KinematicConstraint& other, double margin) const
857 {
858  if (other.getType() != type_)
859  return false;
860  const VisibilityConstraint& o = static_cast<const VisibilityConstraint&>(other);
861 
862  if (moveit::core::Transforms::sameFrame(target_frame_id_, o.target_frame_id_) &&
863  moveit::core::Transforms::sameFrame(sensor_frame_id_, o.sensor_frame_id_) && cone_sides_ == o.cone_sides_ &&
864  sensor_view_direction_ == o.sensor_view_direction_)
865  {
866  if (fabs(max_view_angle_ - o.max_view_angle_) > margin || fabs(target_radius_ - o.target_radius_) > margin)
867  return false;
868  // sensor_pose_ is valid isometry, checked in configure()
869  Eigen::Isometry3d diff = sensor_pose_.inverse() * o.sensor_pose_; // valid isometry
870  if (diff.translation().norm() > margin)
871  return false;
872  if (!diff.linear().isIdentity(margin))
873  return false;
874  // target_pose_ is valid isometry, checked in configure()
875  diff = target_pose_.inverse() * o.target_pose_; // valid isometry
876  if (diff.translation().norm() > margin)
877  return false;
878  if (!diff.linear().isIdentity(margin))
879  return false;
880  return true;
881  }
882  return false;
883 }
884 
886 {
887  return target_radius_ > std::numeric_limits<double>::epsilon();
888 }
889 
891 {
892  // the current pose of the sensor
893 
894  const Eigen::Isometry3d& sp =
896  const Eigen::Isometry3d& tp =
898 
899  // transform the points on the disc to the desired target frame
900  const EigenSTL::vector_Vector3d* points = &points_;
901  std::unique_ptr<EigenSTL::vector_Vector3d> temp_points;
903  {
904  temp_points = std::make_unique<EigenSTL::vector_Vector3d>(points_.size());
905  for (std::size_t i = 0; i < points_.size(); ++i)
906  temp_points->at(i) = tp * points_[i];
907  points = temp_points.get();
908  }
909 
910  // allocate memory for a mesh to represent the visibility cone
911  shapes::Mesh* m = new shapes::Mesh();
912  m->vertex_count = cone_sides_ + 2;
913  m->vertices = new double[m->vertex_count * 3];
914  m->triangle_count = cone_sides_ * 2;
915  m->triangles = new unsigned int[m->triangle_count * 3];
916  // we do NOT allocate normals because we do not compute them
917 
918  // the sensor origin
919  m->vertices[0] = sp.translation().x();
920  m->vertices[1] = sp.translation().y();
921  m->vertices[2] = sp.translation().z();
922 
923  // the center of the base of the cone approximation
924  m->vertices[3] = tp.translation().x();
925  m->vertices[4] = tp.translation().y();
926  m->vertices[5] = tp.translation().z();
927 
928  // the points that approximate the base disc
929  for (std::size_t i = 0; i < points->size(); ++i)
930  {
931  m->vertices[i * 3 + 6] = points->at(i).x();
932  m->vertices[i * 3 + 7] = points->at(i).y();
933  m->vertices[i * 3 + 8] = points->at(i).z();
934  }
935 
936  // add the triangles
937  std::size_t p3 = points->size() * 3;
938  for (std::size_t i = 1; i < points->size(); ++i)
939  {
940  // triangle forming a side of the cone, using the sensor origin
941  std::size_t i3 = (i - 1) * 3;
942  m->triangles[i3] = i + 1;
943  m->triangles[i3 + 1] = 0;
944  m->triangles[i3 + 2] = i + 2;
945  // triangle forming a part of the base of the cone, using the center of the base
946  std::size_t i6 = p3 + i3;
947  m->triangles[i6] = i + 1;
948  m->triangles[i6 + 1] = 1;
949  m->triangles[i6 + 2] = i + 2;
950  }
951 
952  // last triangles
953  m->triangles[p3 - 3] = points->size() + 1;
954  m->triangles[p3 - 2] = 0;
955  m->triangles[p3 - 1] = 2;
956  p3 *= 2;
957  m->triangles[p3 - 3] = points->size() + 1;
958  m->triangles[p3 - 2] = 1;
959  m->triangles[p3 - 1] = 2;
960 
961  return m;
962 }
963 
965  visualization_msgs::MarkerArray& markers) const
966 {
967  shapes::Mesh* m = getVisibilityCone(state);
968  visualization_msgs::Marker mk;
970  delete m;
971  mk.header.frame_id = robot_model_->getModelFrame();
972  mk.header.stamp = ros::Time::now();
973  mk.ns = "constraints";
974  mk.id = 1;
975  mk.action = visualization_msgs::Marker::ADD;
976  mk.pose.position.x = 0;
977  mk.pose.position.y = 0;
978  mk.pose.position.z = 0;
979  mk.pose.orientation.x = 0;
980  mk.pose.orientation.y = 0;
981  mk.pose.orientation.z = 0;
982  mk.pose.orientation.w = 1;
983  mk.lifetime = ros::Duration(60);
984  // this scale necessary to make results look reasonable
985  mk.scale.x = .01;
986  mk.color.a = 1.0;
987  mk.color.r = 1.0;
988  mk.color.g = 0.0;
989  mk.color.b = 0.0;
990 
991  markers.markers.push_back(mk);
992 
993  // getFrameTransform() returns a valid isometry by contract
994  // sensor_pose_ is valid isometry (checked in configure())
995  const Eigen::Isometry3d& sp =
997  // target_pose_ is valid isometry (checked in configure())
998  const Eigen::Isometry3d& tp =
1000 
1001  visualization_msgs::Marker mka;
1002  mka.type = visualization_msgs::Marker::ARROW;
1003  mka.action = visualization_msgs::Marker::ADD;
1004  mka.color = mk.color;
1005  mka.pose = mk.pose;
1006 
1007  mka.header = mk.header;
1008  mka.ns = mk.ns;
1009  mka.id = 2;
1010  mka.lifetime = mk.lifetime;
1011  mka.scale.x = 0.05;
1012  mka.scale.y = .15;
1013  mka.scale.z = 0.0;
1014  mka.points.resize(2);
1015  Eigen::Vector3d d = tp.translation() + tp.linear().col(2) * 0.5;
1016  mka.points[0].x = tp.translation().x();
1017  mka.points[0].y = tp.translation().y();
1018  mka.points[0].z = tp.translation().z();
1019  mka.points[1].x = d.x();
1020  mka.points[1].y = d.y();
1021  mka.points[1].z = d.z();
1022  markers.markers.push_back(mka);
1023 
1024  mka.id = 3;
1025  mka.color.b = 1.0;
1026  mka.color.r = 0.0;
1027 
1028  d = sp.translation() + sp.linear().col(2 - sensor_view_direction_) * 0.5;
1029  mka.points[0].x = sp.translation().x();
1030  mka.points[0].y = sp.translation().y();
1031  mka.points[0].z = sp.translation().z();
1032  mka.points[1].x = d.x();
1033  mka.points[1].y = d.y();
1034  mka.points[1].z = d.z();
1035 
1036  markers.markers.push_back(mka);
1037 }
1038 
1040 {
1041  if (target_radius_ <= std::numeric_limits<double>::epsilon())
1042  return ConstraintEvaluationResult(true, 0.0);
1043 
1044  if (max_view_angle_ > 0.0 || max_range_angle_ > 0.0)
1045  {
1046  // getFrameTransform() returns a valid isometry by contract
1047  // sensor_pose_ is valid isometry (checked in configure())
1048  const Eigen::Isometry3d& sp =
1050  // target_pose_ is valid isometry (checked in configure())
1051  const Eigen::Isometry3d& tp =
1053 
1054  // necessary to do subtraction as SENSOR_Z is 0 and SENSOR_X is 2
1055  const Eigen::Vector3d& normal2 = sp.linear().col(2 - sensor_view_direction_);
1056 
1057  if (max_view_angle_ > 0.0)
1058  {
1059  const Eigen::Vector3d& normal1 = tp.linear().col(2) * -1.0; // along Z axis and inverted
1060  double dp = normal2.dot(normal1);
1061  double ang = acos(dp);
1062  if (dp < 0.0)
1063  {
1064  if (verbose)
1065  ROS_INFO_NAMED("kinematic_constraints", "Visibility constraint is violated because the sensor is looking at "
1066  "the wrong side");
1067  return ConstraintEvaluationResult(false, 0.0);
1068  }
1069  if (max_view_angle_ < ang)
1070  {
1071  if (verbose)
1072  ROS_INFO_NAMED("kinematic_constraints",
1073  "Visibility constraint is violated because the view angle is %lf "
1074  "(above the maximum allowed of %lf)",
1075  ang, max_view_angle_);
1076  return ConstraintEvaluationResult(false, 0.0);
1077  }
1078  }
1079  if (max_range_angle_ > 0.0)
1080  {
1081  const Eigen::Vector3d& dir = (tp.translation() - sp.translation()).normalized();
1082  double dp = normal2.dot(dir);
1083  if (dp < 0.0)
1084  {
1085  if (verbose)
1086  ROS_INFO_NAMED("kinematic_constraints", "Visibility constraint is violated because the sensor is looking at "
1087  "the wrong side");
1088  return ConstraintEvaluationResult(false, 0.0);
1089  }
1090 
1091  double ang = acos(dp);
1092  if (max_range_angle_ < ang)
1093  {
1094  if (verbose)
1095  ROS_INFO_NAMED("kinematic_constraints",
1096  "Visibility constraint is violated because the range angle is %lf "
1097  "(above the maximum allowed of %lf)",
1098  ang, max_range_angle_);
1099  return ConstraintEvaluationResult(false, 0.0);
1100  }
1101  }
1102  }
1103 
1104  shapes::Mesh* m = getVisibilityCone(state);
1105  if (!m)
1106  return ConstraintEvaluationResult(false, 0.0);
1107 
1108  // add the visibility cone as an object
1109  collision_env_->getWorld()->addToObject("cone", shapes::ShapeConstPtr(m), Eigen::Isometry3d::Identity());
1110 
1111  // check for collisions between the robot and the cone
1115  acm.setDefaultEntry("cone", boost::bind(&VisibilityConstraint::decideContact, this, _1));
1116  req.contacts = true;
1117  req.verbose = verbose;
1118  req.max_contacts = 1;
1119  collision_env_->checkRobotCollision(req, res, state, acm);
1120 
1121  if (verbose)
1122  {
1123  std::stringstream ss;
1124  m->print(ss);
1125  ROS_INFO_NAMED("kinematic_constraints", "Visibility constraint %ssatisfied. Visibility cone approximation:\n %s",
1126  res.collision ? "not " : "", ss.str().c_str());
1127  }
1128 
1129  collision_env_->getWorld()->removeObject("cone");
1130 
1131  return ConstraintEvaluationResult(!res.collision, res.collision ? res.contacts.begin()->second.front().depth : 0.0);
1132 }
1133 
1135 {
1138  return true;
1143  {
1144  ROS_DEBUG_NAMED("kinematic_constraints", "Accepted collision with either sensor or target");
1145  return true;
1146  }
1151  {
1152  ROS_DEBUG_NAMED("kinematic_constraints", "Accepted collision with either sensor or target");
1153  return true;
1154  }
1155  return false;
1156 }
1157 
1158 void VisibilityConstraint::print(std::ostream& out) const
1159 {
1160  if (enabled())
1161  {
1162  out << "Visibility constraint for sensor in frame '" << sensor_frame_id_ << "' using target in frame '"
1163  << target_frame_id_ << "'" << std::endl;
1164  out << "Target radius: " << target_radius_ << ", using " << cone_sides_ << " sides." << std::endl;
1165  }
1166  else
1167  out << "No constraint" << std::endl;
1168 }
1169 
1171 {
1172  all_constraints_ = moveit_msgs::Constraints();
1173  kinematic_constraints_.clear();
1174  joint_constraints_.clear();
1175  position_constraints_.clear();
1176  orientation_constraints_.clear();
1177  visibility_constraints_.clear();
1178 }
1179 
1180 bool KinematicConstraintSet::add(const std::vector<moveit_msgs::JointConstraint>& jc)
1181 {
1182  bool result = true;
1183  for (const moveit_msgs::JointConstraint& joint_constraint : jc)
1184  {
1185  JointConstraint* ev = new JointConstraint(robot_model_);
1186  bool u = ev->configure(joint_constraint);
1187  result = result && u;
1188  kinematic_constraints_.push_back(KinematicConstraintPtr(ev));
1189  joint_constraints_.push_back(joint_constraint);
1190  all_constraints_.joint_constraints.push_back(joint_constraint);
1191  }
1192  return result;
1193 }
1194 
1195 bool KinematicConstraintSet::add(const std::vector<moveit_msgs::PositionConstraint>& pc,
1196  const moveit::core::Transforms& tf)
1197 {
1198  bool result = true;
1199  for (const moveit_msgs::PositionConstraint& position_constraint : pc)
1200  {
1201  PositionConstraint* ev = new PositionConstraint(robot_model_);
1202  bool u = ev->configure(position_constraint, tf);
1203  result = result && u;
1204  kinematic_constraints_.push_back(KinematicConstraintPtr(ev));
1205  position_constraints_.push_back(position_constraint);
1206  all_constraints_.position_constraints.push_back(position_constraint);
1207  }
1208  return result;
1209 }
1210 
1211 bool KinematicConstraintSet::add(const std::vector<moveit_msgs::OrientationConstraint>& oc,
1213 {
1214  bool result = true;
1215  for (const moveit_msgs::OrientationConstraint& orientation_constraint : oc)
1216  {
1218  bool u = ev->configure(orientation_constraint, tf);
1219  result = result && u;
1220  kinematic_constraints_.push_back(KinematicConstraintPtr(ev));
1221  orientation_constraints_.push_back(orientation_constraint);
1222  all_constraints_.orientation_constraints.push_back(orientation_constraint);
1223  }
1224  return result;
1225 }
1226 
1227 bool KinematicConstraintSet::add(const std::vector<moveit_msgs::VisibilityConstraint>& vc,
1228  const moveit::core::Transforms& tf)
1229 {
1230  bool result = true;
1231  for (const moveit_msgs::VisibilityConstraint& visibility_constraint : vc)
1232  {
1234  bool u = ev->configure(visibility_constraint, tf);
1235  result = result && u;
1236  kinematic_constraints_.push_back(KinematicConstraintPtr(ev));
1237  visibility_constraints_.push_back(visibility_constraint);
1238  all_constraints_.visibility_constraints.push_back(visibility_constraint);
1239  }
1240  return result;
1241 }
1242 
1243 bool KinematicConstraintSet::add(const moveit_msgs::Constraints& c, const moveit::core::Transforms& tf)
1244 {
1245  bool j = add(c.joint_constraints);
1246  bool p = add(c.position_constraints, tf);
1247  bool o = add(c.orientation_constraints, tf);
1248  bool v = add(c.visibility_constraints, tf);
1249  return j && p && o && v;
1250 }
1251 
1253 {
1254  ConstraintEvaluationResult res(true, 0.0);
1255  for (const KinematicConstraintPtr& kinematic_constraint : kinematic_constraints_)
1256  {
1257  ConstraintEvaluationResult r = kinematic_constraint->decide(state, verbose);
1258  if (!r.satisfied)
1259  res.satisfied = false;
1260  res.distance += r.distance;
1261  }
1262  return res;
1263 }
1264 
1265 ConstraintEvaluationResult KinematicConstraintSet::decide(const moveit::core::RobotState& state,
1266  std::vector<ConstraintEvaluationResult>& results,
1267  bool verbose) const
1268 {
1269  ConstraintEvaluationResult result(true, 0.0);
1270  results.resize(kinematic_constraints_.size());
1271  for (std::size_t i = 0; i < kinematic_constraints_.size(); ++i)
1272  {
1273  results[i] = kinematic_constraints_[i]->decide(state, verbose);
1274  result.satisfied = result.satisfied && results[i].satisfied;
1275  result.distance += results[i].distance;
1276  }
1277 
1278  return result;
1279 }
1280 
1281 void KinematicConstraintSet::print(std::ostream& out) const
1282 {
1283  out << kinematic_constraints_.size() << " kinematic constraints" << std::endl;
1284  for (const KinematicConstraintPtr& kinematic_constraint : kinematic_constraints_)
1285  kinematic_constraint->print(out);
1286 }
1287 
1288 bool KinematicConstraintSet::equal(const KinematicConstraintSet& other, double margin) const
1289 {
1290  // each constraint in this matches some in the other
1291  for (const KinematicConstraintPtr& kinematic_constraint : kinematic_constraints_)
1292  {
1293  bool found = false;
1294  for (unsigned int j = 0; !found && j < other.kinematic_constraints_.size(); ++j)
1295  found = kinematic_constraint->equal(*other.kinematic_constraints_[j], margin);
1296  if (!found)
1297  return false;
1298  }
1299  // each constraint in the other matches some constraint in this
1300  for (const KinematicConstraintPtr& kinematic_constraint : other.kinematic_constraints_)
1301  {
1302  bool found = false;
1303  for (unsigned int j = 0; !found && j < kinematic_constraints_.size(); ++j)
1304  found = kinematic_constraint->equal(*kinematic_constraints_[j], margin);
1305  if (!found)
1306  return false;
1307  }
1308  return true;
1309 }
1310 
1311 } // end of namespace kinematic_constraints
kinematic_constraints::normalizeAbsoluteAngle
static double normalizeAbsoluteAngle(const double angle)
Definition: kinematic_constraint.cpp:95
kinematic_constraints::KinematicConstraint::robot_model_
moveit::core::RobotModelConstPtr robot_model_
The kinematic model associated with this constraint.
Definition: kinematic_constraint.h:208
kinematic_constraints::KinematicConstraintSet::robot_model_
moveit::core::RobotModelConstPtr robot_model_
The kinematic model used for by the Set.
Definition: kinematic_constraint.h:1099
shapes::Mesh::print
void print(std::ostream &out=std::cout) const override
collision_detection::AllowedCollisionMatrix::setDefaultEntry
void setDefaultEntry(const std::string &name, bool allowed)
Set the default value for entries that include name. If such a default value is set,...
Definition: collision_matrix.cpp:251
collision_detection::Contact::body_type_1
BodyType body_type_1
The type of the first body involved in the contact.
Definition: include/moveit/collision_detection/collision_common.h:122
kinematic_constraints::VisibilityConstraint::max_range_angle_
double max_range_angle_
Storage for the max range angle.
Definition: kinematic_constraint.h:890
Eigen
print
void print(PropagationDistanceField &pdf, int numX, int numY, int numZ)
Definition: test_distance_field.cpp:69
kinematic_constraint.h
shapes::Mesh::triangles
unsigned int * triangles
epsilon
double epsilon
moveit::core::JointModel::PLANAR
@ PLANAR
Definition: joint_model.h:182
kinematic_constraints::KinematicConstraintSet::equal
bool equal(const KinematicConstraintSet &other, double margin) const
Whether or not another KinematicConstraintSet is equal to this one.
Definition: kinematic_constraint.cpp:1320
moveit::core::Transforms::transformPose
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:238
moveit::core::VariableBounds
Definition: joint_model.h:118
collision_detection::BodyTypes::WORLD_OBJECT
@ WORLD_OBJECT
A body in the environment.
Definition: include/moveit/collision_detection/collision_common.h:97
kinematic_constraints::VisibilityConstraint::target_radius_
double target_radius_
Storage for the target radius.
Definition: kinematic_constraint.h:888
kinematic_constraints::finishPositionConstraintDecision
static ConstraintEvaluationResult finishPositionConstraintDecision(const Eigen::Vector3d &pt, const Eigen::Vector3d &desired, const std::string &name, double weight, bool result, bool verbose)
Definition: kinematic_constraint.cpp:506
moveit::core::VariableBounds::max_position_
double max_position_
Definition: joint_model.h:167
kinematic_constraints::KinematicConstraintSet::all_constraints_
moveit_msgs::Constraints all_constraints_
Messages corresponding to all internal constraints.
Definition: kinematic_constraint.h:1111
tf2_eigen.h
tf2::fromMsg
void fromMsg(const A &, B &b)
moveit::core::Transforms::getTargetFrame
const std::string & getTargetFrame() const
Get the planning frame corresponding to this set of transforms.
Definition: transforms.cpp:131
kinematic_constraints
Representation and evaluation of kinematic constraints.
Definition: kinematic_constraint.h:52
check_isometry.h
kinematic_constraints::KinematicConstraintSet::decide
ConstraintEvaluationResult decide(const moveit::core::RobotState &state, bool verbose=false) const
Determines whether all constraints are satisfied by state, returning a single evaluation result.
Definition: kinematic_constraint.cpp:1284
kinematic_constraints::VisibilityConstraint::mobile_target_frame_
bool mobile_target_frame_
True if the target is a non-fixed frame relative to the transform frame.
Definition: kinematic_constraint.h:880
shape_operations.h
shapes::Mesh::triangle_count
unsigned int triangle_count
kinematic_constraints::VisibilityConstraint::decideContact
bool decideContact(const collision_detection::Contact &contact) const
Function that gets passed into collision checking to allow some collisions.
Definition: kinematic_constraint.cpp:1166
kinematic_constraints::KinematicConstraintSet::orientation_constraints_
std::vector< moveit_msgs::OrientationConstraint > orientation_constraints_
Messages corresponding to all internal orientation constraints.
Definition: kinematic_constraint.h:1107
ROS_ERROR_NAMED
#define ROS_ERROR_NAMED(name,...)
moveit::core::RobotState
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:155
moveit::core::RobotState::getGlobalLinkTransform
const Eigen::Isometry3d & getGlobalLinkTransform(const std::string &link_name)
Get the link transform w.r.t. the root link (model frame) of the RobotModel. This is typically the ro...
Definition: robot_state.h:1426
moveit::core::VariableBounds::min_position_
double min_position_
Definition: joint_model.h:166
kinematic_constraints::KinematicConstraintSet::position_constraints_
std::vector< moveit_msgs::PositionConstraint > position_constraints_
Messages corresponding to all internal position constraints.
Definition: kinematic_constraint.h:1105
shapes::Mesh
collision_detection::Contact
Definition of a contact point.
Definition: include/moveit/collision_detection/collision_common.h:105
moveit::core::RevoluteJointModel::isContinuous
bool isContinuous() const
Check if this joint wraps around.
Definition: revolute_joint_model.h:169
conversions.h
collision_detection::CollisionRequest
Representation of a collision checking request.
Definition: include/moveit/collision_detection/collision_common.h:216
kinematic_constraints::VisibilityConstraint::target_pose_
Eigen::Isometry3d target_pose_
The target pose transformed into the transform frame.
Definition: kinematic_constraint.h:885
kinematic_constraints::VisibilityConstraint::cone_sides_
unsigned int cone_sides_
Storage for the cone sides
Definition: kinematic_constraint.h:886
kinematic_constraints::VisibilityConstraint::sensor_pose_
Eigen::Isometry3d sensor_pose_
The sensor pose transformed into the transform frame.
Definition: kinematic_constraint.h:883
kinematic_constraints::KinematicConstraintSet::kinematic_constraints_
std::vector< KinematicConstraintPtr > kinematic_constraints_
Shared pointers to all the member constraints.
Definition: kinematic_constraint.h:1101
collision_detection::Contact::body_name_2
std::string body_name_2
The id of the second body involved in the contact.
Definition: include/moveit/collision_detection/collision_common.h:125
moveit::core::JointModel::REVOLUTE
@ REVOLUTE
Definition: joint_model.h:180
collision_detection::Contact::body_type_2
BodyType body_type_2
The type of the second body involved in the contact.
Definition: include/moveit/collision_detection/collision_common.h:128
collision_detection::Contact::body_name_1
std::string body_name_1
The id of the first body involved in the contact.
Definition: include/moveit/collision_detection/collision_common.h:119
kinematic_constraints::VisibilityConstraint::configure
bool configure(const moveit_msgs::VisibilityConstraint &vc, const moveit::core::Transforms &tf)
Configure the constraint based on a moveit_msgs::VisibilityConstraint.
Definition: kinematic_constraint.cpp:809
collision_detection::AllowedCollisionMatrix
Definition of a structure for the allowed collision matrix. All elements in the collision world are r...
Definition: collision_matrix.h:112
kinematic_constraints::VisibilityConstraint::VisibilityConstraint
VisibilityConstraint(const moveit::core::RobotModelConstPtr &model)
Constructor.
Definition: kinematic_constraint.cpp:787
kinematic_constraints::KinematicConstraint::type_
ConstraintType type_
The type of the constraint.
Definition: kinematic_constraint.h:207
name
std::string name
ROS_INFO_NAMED
#define ROS_INFO_NAMED(name,...)
kinematic_constraints::VisibilityConstraint::enabled
bool enabled() const override
This function returns true if this constraint is configured and able to decide whether states do meet...
Definition: kinematic_constraint.cpp:917
kinematic_constraints::VisibilityConstraint::max_view_angle_
double max_view_angle_
Storage for the max view angle.
Definition: kinematic_constraint.h:889
kinematic_constraints::KinematicConstraintSet::add
bool add(const moveit_msgs::Constraints &c, const moveit::core::Transforms &tf)
Add all known constraints.
Definition: kinematic_constraint.cpp:1275
ROS_DEBUG_NAMED
#define ROS_DEBUG_NAMED(name,...)
collision_detection::CollisionResult
Representation of a collision checking result.
Definition: include/moveit/collision_detection/collision_common.h:177
shapes::Mesh::vertex_count
unsigned int vertex_count
y
double y
bodies::createEmptyBodyFromShapeType
Body * createEmptyBodyFromShapeType(const shapes::ShapeType &shapeType)
ROS_ERROR_STREAM_NAMED
#define ROS_ERROR_STREAM_NAMED(name, args)
collision_detection::CollisionRequest::max_contacts
std::size_t max_contacts
Overall maximum number of contacts to compute.
Definition: include/moveit/collision_detection/collision_common.h:245
ASSERT_ISOMETRY
#define ASSERT_ISOMETRY(transform)
kinematic_constraints::VisibilityConstraint::clear
void clear() override
Clear the stored constraint.
Definition: kinematic_constraint.cpp:793
collision_detection::CollisionRequest::verbose
bool verbose
Flag indicating whether information about detected collisions should be reported.
Definition: include/moveit/collision_detection/collision_common.h:258
moveit::core::RobotState::getVariablePosition
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:273
kinematic_constraints::normalizeAngle
static double normalizeAngle(double angle)
Definition: kinematic_constraint.cpp:83
kinematic_constraints::KinematicConstraintSet::clear
void clear()
Clear the stored constraints.
Definition: kinematic_constraint.cpp:1202
kinematic_constraints::VisibilityConstraint::points_
EigenSTL::vector_Vector3d points_
A set of points along the base of the circle.
Definition: kinematic_constraint.h:887
kinematic_constraints::VisibilityConstraint::print
void print(std::ostream &out=std::cout) const override
Print the constraint data.
Definition: kinematic_constraint.cpp:1190
d
d
collision_detection::BodyTypes::ROBOT_ATTACHED
@ ROBOT_ATTACHED
A body attached to a robot link.
Definition: include/moveit/collision_detection/collision_common.h:94
body_operations.h
shapes::constructMarkerFromShape
bool constructMarkerFromShape(const Shape *shape, visualization_msgs::Marker &mk, bool use_mesh_triangle_list=false)
kinematic_constraints::KinematicConstraintSet
A class that contains many different constraints, and can check RobotState *versus the full set....
Definition: kinematic_constraint.h:903
kinematic_constraints::OrientationConstraint
Class for constraints on the orientation of a link.
Definition: kinematic_constraint.h:379
shapes::Mesh::vertices
double * vertices
collision_detection::BodyTypes::ROBOT_LINK
@ ROBOT_LINK
A link on the robot.
Definition: include/moveit/collision_detection/collision_common.h:91
r
S r
kinematic_constraints::VisibilityConstraint::equal
bool equal(const KinematicConstraint &other, double margin) const override
Check if two constraints are the same.
Definition: kinematic_constraint.cpp:888
kinematic_constraints::VisibilityConstraint
Class for constraints on the visibility relationship between a sensor and a target.
Definition: kinematic_constraint.h:792
moveit::core::Transforms::sameFrame
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:122
point
std::chrono::system_clock::time_point point
bodies::BodyPtr
std::shared_ptr< Body > BodyPtr
collision_detection::CollisionResult::contacts
ContactMap contacts
A map returning the pairs of body ids in contact, plus their contact details.
Definition: include/moveit/collision_detection/collision_common.h:209
kinematic_constraints::VisibilityConstraint::sensor_frame_id_
std::string sensor_frame_id_
The sensor frame id.
Definition: kinematic_constraint.h:882
kinematic_constraints::VisibilityConstraint::decide
ConstraintEvaluationResult decide(const moveit::core::RobotState &state, bool verbose=false) const override
Decide whether the constraint is satisfied in the indicated state.
Definition: kinematic_constraint.cpp:1071
moveit::core::RobotState::getFrameTransform
const Eigen::Isometry3d & getFrameTransform(const std::string &frame_id, bool *frame_found=nullptr)
Get the transformation matrix from the model frame (root of model) to the frame identified by frame_i...
Definition: robot_state.cpp:1122
shapes::constructShapeFromMsg
Shape * constructShapeFromMsg(const shape_msgs::Mesh &shape_msg)
moveit::core::Transforms
Provides an implementation of a snapshot of a transform tree that can be easily queried for transform...
Definition: transforms.h:122
collision_env_fcl.h
kinematic_constraints::VisibilityConstraint::mobile_sensor_frame_
bool mobile_sensor_frame_
True if the sensor is a non-fixed frame relative to the transform frame.
Definition: kinematic_constraint.h:879
kinematic_constraints::KinematicConstraint::KinematicConstraint
KinematicConstraint(const moveit::core::RobotModelConstPtr &model)
Constructor.
Definition: kinematic_constraint.cpp:145
std
kinematic_constraints::KinematicConstraintSet::visibility_constraints_
std::vector< moveit_msgs::VisibilityConstraint > visibility_constraints_
Messages corresponding to all internal visibility constraints.
Definition: kinematic_constraint.h:1109
collision_detection::CollisionRequest::contacts
bool contacts
If true, compute contacts. Otherwise only a binary collision yes/no is reported.
Definition: include/moveit/collision_detection/collision_common.h:242
kinematic_constraints::CalcEulerAngles
std::tuple< Eigen::Matrix< typename Eigen::MatrixBase< Derived >::Scalar, 3, 1 >, bool > CalcEulerAngles(const Eigen::MatrixBase< Derived > &R)
Definition: kinematic_constraint.cpp:110
kinematic_constraints::VisibilityConstraint::getVisibilityCone
shapes::Mesh * getVisibilityCone(const moveit::core::RobotState &state) const
Gets a trimesh shape representing the visibility cone.
Definition: kinematic_constraint.cpp:922
ROS_WARN_NAMED
#define ROS_WARN_NAMED(name,...)
shapes::ShapeConstPtr
std::shared_ptr< const Shape > ShapeConstPtr
collision_detection::CollisionResult::collision
bool collision
True if collision was found, false otherwise.
Definition: include/moveit/collision_detection/collision_common.h:200
kinematic_constraints::OrientationConstraint::configure
bool configure(const moveit_msgs::OrientationConstraint &oc, const moveit::core::Transforms &tf)
Configure the constraint based on a moveit_msgs::OrientationConstraint.
Definition: kinematic_constraint.cpp:585
x
double x
moveit::core::RevoluteJointModel
A revolute joint.
Definition: revolute_joint_model.h:110
kinematic_constraints::KinematicConstraint::constraint_weight_
double constraint_weight_
The weight of a constraint is a multiplicative factor associated to the distance computed by the deci...
Definition: kinematic_constraint.h:209
kinematic_constraints::KinematicConstraintSet::print
void print(std::ostream &out=std::cout) const
Print the constraint data.
Definition: kinematic_constraint.cpp:1313
kinematic_constraints::VisibilityConstraint::sensor_view_direction_
int sensor_view_direction_
Storage for the sensor view direction.
Definition: kinematic_constraint.h:884
kinematic_constraints::VisibilityConstraint::getMarkers
void getMarkers(const moveit::core::RobotState &state, visualization_msgs::MarkerArray &markers) const
Adds markers associated with the visibility cone, sensor and target to the visualization array.
Definition: kinematic_constraint.cpp:996
kinematic_constraints::VisibilityConstraint::target_frame_id_
std::string target_frame_id_
The target frame id.
Definition: kinematic_constraint.h:881
kinematic_constraints::KinematicConstraint::VISIBILITY_CONSTRAINT
@ VISIBILITY_CONSTRAINT
Definition: kinematic_constraint.h:119
kinematic_constraints::VisibilityConstraint::collision_env_
collision_detection::CollisionEnvPtr collision_env_
A copy of the collision robot maintained for collision checking the cone against robot links.
Definition: kinematic_constraint.h:877
ros::Duration
moveit::core::Transforms::isFixedFrame
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:150
moveit::core::Transforms::transformQuaternion
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:213
t
geometry_msgs::TransformStamped t
kinematic_constraints::KinematicConstraintSet::joint_constraints_
std::vector< moveit_msgs::JointConstraint > joint_constraints_
Messages corresponding to all internal joint constraints.
Definition: kinematic_constraint.h:1103
collision_detection::CollisionResult::distance
double distance
Closest distance between two bodies.
Definition: include/moveit/collision_detection/collision_common.h:203
fcl::Vector3d
Vec3fX< details::Vec3Data< double > > Vector3d
Definition: fcl_compat.h:89
ros::Time::now
static Time now()
verbose
bool verbose
kinematic_constraints::ConstraintEvaluationResult
Struct for containing the results of constraint evaluation.
Definition: kinematic_constraint.h:87
collision_detection
Definition: collision_detector_allocator_allvalid.h:42


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Jul 21 2021 02:23:41