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>
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 KinematicConstraint::KinematicConstraint(const robot_model::RobotModelConstPtr& model)
62  : type_(UNKNOWN_CONSTRAINT), robot_model_(model), constraint_weight_(std::numeric_limits<double>::epsilon())
63 {
64 }
65 
67 
68 bool JointConstraint::configure(const moveit_msgs::JointConstraint& jc)
69 {
70  // clearing before we configure to get rid of any old data
71  clear();
72 
73  // testing tolerances first
74  if (jc.tolerance_above < 0.0 || jc.tolerance_below < 0.0)
75  {
76  ROS_WARN_NAMED("kinematic_constraints", "JointConstraint tolerance values must be positive.");
77  joint_model_ = nullptr;
78  return false;
79  }
80 
81  joint_variable_name_ = jc.joint_name;
82  local_variable_name_.clear();
83  if (robot_model_->hasJointModel(joint_variable_name_))
84  joint_model_ = robot_model_->getJointModel(joint_variable_name_);
85  else
86  {
87  std::size_t pos = jc.joint_name.find_last_of("/");
88  if (pos != std::string::npos)
89  {
90  joint_model_ = robot_model_->getJointModel(jc.joint_name.substr(0, pos));
91  if (pos + 1 < jc.joint_name.length())
92  local_variable_name_ = jc.joint_name.substr(pos + 1);
93  }
94  else
95  joint_model_ = robot_model_->getJointModel(jc.joint_name);
96  }
97 
98  if (joint_model_)
99  {
100  if (local_variable_name_.empty())
101  {
102  // check if the joint has 1 DOF (the only kind we can handle)
103  if (joint_model_->getVariableCount() == 0)
104  {
105  ROS_ERROR_NAMED("kinematic_constraints", "Joint '%s' has no parameters to constrain", jc.joint_name.c_str());
106  joint_model_ = nullptr;
107  }
108  else if (joint_model_->getVariableCount() > 1)
109  {
110  ROS_ERROR_NAMED("kinematic_constraints", "Joint '%s' has more than one parameter to constrain. "
111  "This type of constraint is not supported.",
112  jc.joint_name.c_str());
113  joint_model_ = nullptr;
114  }
115  }
116  else
117  {
118  int found = -1;
119  const std::vector<std::string>& local_var_names = joint_model_->getLocalVariableNames();
120  for (std::size_t i = 0; i < local_var_names.size(); ++i)
121  if (local_var_names[i] == local_variable_name_)
122  {
123  found = i;
124  break;
125  }
126  if (found < 0)
127  {
128  ROS_ERROR_NAMED("kinematic_constraints", "Local variable name '%s' is not known to joint '%s'",
129  local_variable_name_.c_str(), joint_model_->getName().c_str());
130  joint_model_ = nullptr;
131  }
132  }
133  }
134 
135  if (joint_model_)
136  {
137  joint_is_continuous_ = false;
138  joint_tolerance_above_ = jc.tolerance_above;
139  joint_tolerance_below_ = jc.tolerance_below;
140  joint_variable_index_ = robot_model_->getVariableIndex(joint_variable_name_);
141 
142  // check if we have to wrap angles when computing distances
143  joint_is_continuous_ = false;
144  if (joint_model_->getType() == robot_model::JointModel::REVOLUTE)
145  {
146  const robot_model::RevoluteJointModel* rjoint = static_cast<const robot_model::RevoluteJointModel*>(joint_model_);
147  if (rjoint->isContinuous())
148  joint_is_continuous_ = true;
149  }
150  else if (joint_model_->getType() == robot_model::JointModel::PLANAR)
151  {
152  if (local_variable_name_ == "theta")
153  joint_is_continuous_ = true;
154  }
155 
156  if (joint_is_continuous_)
157  {
158  joint_position_ = normalizeAngle(jc.position);
159  }
160  else
161  {
162  joint_position_ = jc.position;
163  const robot_model::VariableBounds& bounds = joint_model_->getVariableBounds(joint_variable_name_);
164 
165  if (bounds.min_position_ > joint_position_ + joint_tolerance_above_)
166  {
167  joint_position_ = bounds.min_position_;
168  joint_tolerance_above_ = std::numeric_limits<double>::epsilon();
169  ROS_WARN_NAMED("kinematic_constraints", "Joint %s is constrained to be below the minimum bounds. "
170  "Assuming minimum bounds instead.",
171  jc.joint_name.c_str());
172  }
173  else if (bounds.max_position_ < joint_position_ - joint_tolerance_below_)
174  {
175  joint_position_ = bounds.max_position_;
176  joint_tolerance_below_ = std::numeric_limits<double>::epsilon();
177  ROS_WARN_NAMED("kinematic_constraints", "Joint %s is constrained to be above the maximum bounds. "
178  "Assuming maximum bounds instead.",
179  jc.joint_name.c_str());
180  }
181  }
182 
183  if (jc.weight <= std::numeric_limits<double>::epsilon())
184  {
185  ROS_WARN_NAMED("kinematic_constraints",
186  "The weight on constraint for joint '%s' is very near zero. Setting to 1.0.",
187  jc.joint_name.c_str());
188  constraint_weight_ = 1.0;
189  }
190  else
191  constraint_weight_ = jc.weight;
192  }
193  return joint_model_ != nullptr;
194 }
195 
196 bool JointConstraint::equal(const KinematicConstraint& other, double margin) const
197 {
198  if (other.getType() != type_)
199  return false;
200  const JointConstraint& o = static_cast<const JointConstraint&>(other);
201  if (o.joint_model_ == joint_model_ && o.local_variable_name_ == local_variable_name_)
202  return fabs(joint_position_ - o.joint_position_) <= margin &&
203  fabs(joint_tolerance_above_ - o.joint_tolerance_above_) <= margin &&
204  fabs(joint_tolerance_below_ - o.joint_tolerance_below_) <= margin;
205  return false;
206 }
207 
209 {
210  if (!joint_model_)
211  return ConstraintEvaluationResult(true, 0.0);
212 
213  double current_joint_position = state.getVariablePosition(joint_variable_index_);
214  double dif = 0.0;
215 
216  // compute signed shortest distance for continuous joints
217  if (joint_is_continuous_)
218  {
219  dif = normalizeAngle(current_joint_position) - joint_position_;
220 
221  if (dif > boost::math::constants::pi<double>())
222  dif = 2.0 * boost::math::constants::pi<double>() - dif;
223  else if (dif < -boost::math::constants::pi<double>())
224  dif += 2.0 * boost::math::constants::pi<double>(); // we include a sign change to have dif > 0
225  }
226  else
227  dif = current_joint_position - joint_position_;
228 
229  // check bounds
230  bool result = dif <= (joint_tolerance_above_ + 2.0 * std::numeric_limits<double>::epsilon()) &&
231  dif >= (-joint_tolerance_below_ - 2.0 * std::numeric_limits<double>::epsilon());
232  if (verbose)
233  ROS_INFO_NAMED("kinematic_constraints", "Constraint %s:: Joint name: '%s', actual value: %f, desired value: %f, "
234  "tolerance_above: %f, tolerance_below: %f",
235  result ? "satisfied" : "violated", joint_variable_name_.c_str(), current_joint_position,
236  joint_position_, joint_tolerance_above_, joint_tolerance_below_);
237  return ConstraintEvaluationResult(result, constraint_weight_ * fabs(dif));
238 }
239 
241 {
242  return joint_model_;
243 }
244 
246 {
247  joint_model_ = nullptr;
248  joint_variable_index_ = -1;
249  joint_is_continuous_ = false;
250  local_variable_name_ = "";
251  joint_variable_name_ = "";
252  joint_position_ = joint_tolerance_below_ = joint_tolerance_above_ = 0.0;
253 }
254 
255 void JointConstraint::print(std::ostream& out) const
256 {
257  if (joint_model_)
258  {
259  out << "Joint constraint for joint " << joint_variable_name_ << ": " << std::endl;
260  out << " value = ";
261  out << joint_position_ << "; ";
262  out << " tolerance below = ";
263  out << joint_tolerance_below_ << "; ";
264  out << " tolerance above = ";
265  out << joint_tolerance_above_ << "; ";
266  out << std::endl;
267  }
268  else
269  out << "No constraint" << std::endl;
270 }
271 
272 bool PositionConstraint::configure(const moveit_msgs::PositionConstraint& pc, const robot_state::Transforms& tf)
273 {
274  // clearing before we configure to get rid of any old data
275  clear();
276 
277  link_model_ = robot_model_->getLinkModel(pc.link_name);
278  if (link_model_ == nullptr)
279  {
280  ROS_WARN_NAMED("kinematic_constraints",
281  "Position constraint link model %s not found in kinematic model. Constraint invalid.",
282  pc.link_name.c_str());
283  return false;
284  }
285 
286  if (pc.header.frame_id.empty())
287  {
288  ROS_WARN_NAMED("kinematic_constraints", "No frame specified for position constraint on link '%s'!",
289  pc.link_name.c_str());
290  return false;
291  }
292 
293  offset_ = Eigen::Vector3d(pc.target_point_offset.x, pc.target_point_offset.y, pc.target_point_offset.z);
294  has_offset_ = offset_.squaredNorm() > std::numeric_limits<double>::epsilon();
295 
296  if (tf.isFixedFrame(pc.header.frame_id))
297  {
298  constraint_frame_id_ = tf.getTargetFrame();
299  mobile_frame_ = false;
300  }
301  else
302  {
303  constraint_frame_id_ = pc.header.frame_id;
304  mobile_frame_ = true;
305  }
306 
307  // load primitive shapes, first clearing any we already have
308  for (std::size_t i = 0; i < pc.constraint_region.primitives.size(); ++i)
309  {
310  std::unique_ptr<shapes::Shape> shape(shapes::constructShapeFromMsg(pc.constraint_region.primitives[i]));
311  if (shape)
312  {
313  if (pc.constraint_region.primitive_poses.size() <= i)
314  {
315  ROS_WARN_NAMED("kinematic_constraints", "Constraint region message does not contain enough primitive poses");
316  continue;
317  }
318  constraint_region_.push_back(bodies::BodyPtr(bodies::createBodyFromShape(shape.get())));
319  Eigen::Affine3d t;
320  tf::poseMsgToEigen(pc.constraint_region.primitive_poses[i], t);
321  constraint_region_pose_.push_back(t);
322  if (mobile_frame_)
323  constraint_region_.back()->setPose(constraint_region_pose_.back());
324  else
325  {
326  tf.transformPose(pc.header.frame_id, constraint_region_pose_.back(), constraint_region_pose_.back());
327  constraint_region_.back()->setPose(constraint_region_pose_.back());
328  }
329  }
330  else
331  ROS_WARN_NAMED("kinematic_constraints", "Could not construct primitive shape %zu", i);
332  }
333 
334  // load meshes
335  for (std::size_t i = 0; i < pc.constraint_region.meshes.size(); ++i)
336  {
337  std::unique_ptr<shapes::Shape> shape(shapes::constructShapeFromMsg(pc.constraint_region.meshes[i]));
338  if (shape)
339  {
340  if (pc.constraint_region.mesh_poses.size() <= i)
341  {
342  ROS_WARN_NAMED("kinematic_constraints", "Constraint region message does not contain enough primitive poses");
343  continue;
344  }
345  constraint_region_.push_back(bodies::BodyPtr(bodies::createBodyFromShape(shape.get())));
346  Eigen::Affine3d t;
347  tf::poseMsgToEigen(pc.constraint_region.mesh_poses[i], t);
348  constraint_region_pose_.push_back(t);
349  if (mobile_frame_)
350  constraint_region_.back()->setPose(constraint_region_pose_.back());
351  else
352  {
353  tf.transformPose(pc.header.frame_id, constraint_region_pose_.back(), constraint_region_pose_.back());
354  constraint_region_.back()->setPose(constraint_region_pose_.back());
355  }
356  }
357  else
358  {
359  ROS_WARN_NAMED("kinematic_constraints", "Could not construct mesh shape %zu", i);
360  }
361  }
362 
363  if (pc.weight <= std::numeric_limits<double>::epsilon())
364  {
365  ROS_WARN_NAMED("kinematic_constraints",
366  "The weight on position constraint for link '%s' is near zero. Setting to 1.0.",
367  pc.link_name.c_str());
368  constraint_weight_ = 1.0;
369  }
370  else
371  constraint_weight_ = pc.weight;
372 
373  return !constraint_region_.empty();
374 }
375 
376 bool PositionConstraint::equal(const KinematicConstraint& other, double margin) const
377 {
378  if (other.getType() != type_)
379  return false;
380  const PositionConstraint& o = static_cast<const PositionConstraint&>(other);
381 
382  if (link_model_ == o.link_model_ && robot_state::Transforms::sameFrame(constraint_frame_id_, o.constraint_frame_id_))
383  {
384  if ((offset_ - o.offset_).norm() > margin)
385  return false;
386  std::vector<bool> other_region_matches_this(constraint_region_.size(), false);
387  for (std::size_t i = 0; i < constraint_region_.size(); ++i)
388  {
389  bool some_match = false;
390  // need to check against all other regions
391  for (std::size_t j = 0; j < o.constraint_region_.size(); ++j)
392  {
393  Eigen::Affine3d diff = constraint_region_pose_[i].inverse(Eigen::Isometry) * o.constraint_region_pose_[j];
394  if (diff.translation().norm() < margin && diff.linear().isIdentity(margin) &&
395  constraint_region_[i]->getType() == o.constraint_region_[j]->getType() &&
396  fabs(constraint_region_[i]->computeVolume() - o.constraint_region_[j]->computeVolume()) < margin)
397  {
398  some_match = true;
399  // can't break, as need to do matches the other way as well
400  other_region_matches_this[j] = true;
401  }
402  }
403  if (!some_match)
404  return false;
405  }
406  for (std::size_t i = 0; i < o.constraint_region_.size(); ++i)
407  if (!other_region_matches_this[i])
408  return false;
409  return true;
410  }
411  return false;
412 }
413 
414 // helper function to avoid code duplication
415 static inline ConstraintEvaluationResult finishPositionConstraintDecision(const Eigen::Vector3d& pt,
416  const Eigen::Vector3d& desired,
417  const std::string& name, double weight,
418  bool result, bool verbose)
419 {
420  double dx = desired.x() - pt.x();
421  double dy = desired.y() - pt.y();
422  double dz = desired.z() - pt.z();
423  if (verbose)
424  {
426  "kinematic_constraints", "Position constraint %s on link '%s'. Desired: %f, %f, %f, current: %f, %f, %f",
427  result ? "satisfied" : "violated", name.c_str(), desired.x(), desired.y(), desired.z(), pt.x(), pt.y(), pt.z());
428  ROS_INFO_NAMED("kinematic_constraints", "Differences %g %g %g", dx, dy, dz);
429  }
430  return ConstraintEvaluationResult(result, weight * sqrt(dx * dx + dy * dy + dz * dz));
431 }
432 
434 {
435  if (!link_model_ || constraint_region_.empty())
436  return ConstraintEvaluationResult(true, 0.0);
437 
438  Eigen::Vector3d pt = state.getGlobalLinkTransform(link_model_) * offset_;
439  if (mobile_frame_)
440  {
441  for (std::size_t i = 0; i < constraint_region_.size(); ++i)
442  {
443  Eigen::Affine3d tmp = state.getFrameTransform(constraint_frame_id_) * constraint_region_pose_[i];
444  bool result = constraint_region_[i]->cloneAt(tmp)->containsPoint(pt, verbose);
445  if (result || (i + 1 == constraint_region_pose_.size()))
446  return finishPositionConstraintDecision(pt, tmp.translation(), link_model_->getName(), constraint_weight_,
447  result, verbose);
448  else
449  finishPositionConstraintDecision(pt, tmp.translation(), link_model_->getName(), constraint_weight_, result,
450  verbose);
451  }
452  }
453  else
454  {
455  for (std::size_t i = 0; i < constraint_region_.size(); ++i)
456  {
457  bool result = constraint_region_[i]->containsPoint(pt, true);
458  if (result || (i + 1 == constraint_region_.size()))
459  return finishPositionConstraintDecision(pt, constraint_region_[i]->getPose().translation(),
460  link_model_->getName(), constraint_weight_, result, verbose);
461  else
462  finishPositionConstraintDecision(pt, constraint_region_[i]->getPose().translation(), link_model_->getName(),
463  constraint_weight_, result, verbose);
464  }
465  }
466  return ConstraintEvaluationResult(false, 0.0);
467 }
468 
469 void PositionConstraint::print(std::ostream& out) const
470 {
471  if (enabled())
472  out << "Position constraint on link '" << link_model_->getName() << "'" << std::endl;
473  else
474  out << "No constraint" << std::endl;
475 }
476 
478 {
479  offset_ = Eigen::Vector3d(0.0, 0.0, 0.0);
480  has_offset_ = false;
481  constraint_region_.clear();
482  constraint_region_pose_.clear();
483  mobile_frame_ = false;
484  constraint_frame_id_ = "";
485  link_model_ = nullptr;
486 }
487 
489 {
490  return link_model_ && !constraint_region_.empty();
491 }
492 
493 bool OrientationConstraint::configure(const moveit_msgs::OrientationConstraint& oc, const robot_state::Transforms& tf)
494 {
495  // clearing out any old data
496  clear();
497 
498  link_model_ = robot_model_->getLinkModel(oc.link_name);
499  if (!link_model_)
500  {
501  ROS_WARN_NAMED("kinematic_constraints", "Could not find link model for link name %s", oc.link_name.c_str());
502  return false;
503  }
504  Eigen::Quaterniond q;
505  tf::quaternionMsgToEigen(oc.orientation, q);
506  if (fabs(q.norm() - 1.0) > 1e-3)
507  {
508  ROS_WARN_NAMED("kinematic_constraints", "Orientation constraint for link '%s' is probably incorrect: %f, %f, %f, "
509  "%f. Assuming identity instead.",
510  oc.link_name.c_str(), oc.orientation.x, oc.orientation.y, oc.orientation.z, oc.orientation.w);
511  q = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0);
512  }
513 
514  if (oc.header.frame_id.empty())
515  ROS_WARN_NAMED("kinematic_constraints", "No frame specified for position constraint on link '%s'!",
516  oc.link_name.c_str());
517 
518  if (tf.isFixedFrame(oc.header.frame_id))
519  {
520  tf.transformQuaternion(oc.header.frame_id, q, q);
521  desired_rotation_frame_id_ = tf.getTargetFrame();
522  desired_rotation_matrix_ = Eigen::Matrix3d(q);
523  desired_rotation_matrix_inv_ = desired_rotation_matrix_.transpose();
524  mobile_frame_ = false;
525  }
526  else
527  {
528  desired_rotation_frame_id_ = oc.header.frame_id;
529  desired_rotation_matrix_ = Eigen::Matrix3d(q);
530  mobile_frame_ = true;
531  }
532  std::stringstream matrix_str;
533  matrix_str << desired_rotation_matrix_;
534  ROS_DEBUG_NAMED("kinematic_constraints", "The desired rotation matrix for link '%s' in frame %s is:\n%s",
535  oc.link_name.c_str(), desired_rotation_frame_id_.c_str(), matrix_str.str().c_str());
536 
537  if (oc.weight <= std::numeric_limits<double>::epsilon())
538  {
539  ROS_WARN_NAMED("kinematic_constraints",
540  "The weight on position constraint for link '%s' is near zero. Setting to 1.0.",
541  oc.link_name.c_str());
542  constraint_weight_ = 1.0;
543  }
544  else
545  constraint_weight_ = oc.weight;
546  absolute_x_axis_tolerance_ = fabs(oc.absolute_x_axis_tolerance);
547  if (absolute_x_axis_tolerance_ < std::numeric_limits<double>::epsilon())
548  ROS_WARN_NAMED("kinematic_constraints", "Near-zero value for absolute_x_axis_tolerance");
549  absolute_y_axis_tolerance_ = fabs(oc.absolute_y_axis_tolerance);
550  if (absolute_y_axis_tolerance_ < std::numeric_limits<double>::epsilon())
551  ROS_WARN_NAMED("kinematic_constraints", "Near-zero value for absolute_y_axis_tolerance");
552  absolute_z_axis_tolerance_ = fabs(oc.absolute_z_axis_tolerance);
553  if (absolute_z_axis_tolerance_ < std::numeric_limits<double>::epsilon())
554  ROS_WARN_NAMED("kinematic_constraints", "Near-zero value for absolute_z_axis_tolerance");
555 
556  return link_model_ != nullptr;
557 }
558 
559 bool OrientationConstraint::equal(const KinematicConstraint& other, double margin) const
560 {
561  if (other.getType() != type_)
562  return false;
563  const OrientationConstraint& o = static_cast<const OrientationConstraint&>(other);
564 
565  if (o.link_model_ == link_model_ &&
566  robot_state::Transforms::sameFrame(desired_rotation_frame_id_, o.desired_rotation_frame_id_))
567  {
568  if (!desired_rotation_matrix_.isApprox(o.desired_rotation_matrix_))
569  return false;
570  return fabs(absolute_x_axis_tolerance_ - o.absolute_x_axis_tolerance_) <= margin &&
571  fabs(absolute_y_axis_tolerance_ - o.absolute_y_axis_tolerance_) <= margin &&
572  fabs(absolute_z_axis_tolerance_ - o.absolute_z_axis_tolerance_) <= margin;
573  }
574  return false;
575 }
576 
578 {
579  link_model_ = nullptr;
580  desired_rotation_matrix_ = Eigen::Matrix3d::Identity();
581  desired_rotation_matrix_inv_ = Eigen::Matrix3d::Identity();
582  desired_rotation_frame_id_ = "";
583  mobile_frame_ = false;
584  absolute_z_axis_tolerance_ = absolute_y_axis_tolerance_ = absolute_x_axis_tolerance_ = 0.0;
585 }
586 
588 {
589  return link_model_;
590 }
591 
593 {
594  if (!link_model_)
595  return ConstraintEvaluationResult(true, 0.0);
596 
597  Eigen::Vector3d xyz;
598  if (mobile_frame_)
599  {
600  Eigen::Matrix3d tmp = state.getFrameTransform(desired_rotation_frame_id_).linear() * desired_rotation_matrix_;
601  Eigen::Affine3d diff(tmp.transpose() * state.getGlobalLinkTransform(link_model_).linear());
602  xyz = diff.linear().eulerAngles(0, 1, 2);
603  // 0,1,2 corresponds to XYZ, the convention used in sampling constraints
604  }
605  else
606  {
607  Eigen::Affine3d diff(desired_rotation_matrix_inv_ * state.getGlobalLinkTransform(link_model_).linear());
608  xyz = diff.linear().eulerAngles(0, 1, 2); // 0,1,2 corresponds to XYZ, the convention used in sampling constraints
609  }
610 
611  xyz(0) = std::min(fabs(xyz(0)), boost::math::constants::pi<double>() - fabs(xyz(0)));
612  xyz(1) = std::min(fabs(xyz(1)), boost::math::constants::pi<double>() - fabs(xyz(1)));
613  xyz(2) = std::min(fabs(xyz(2)), boost::math::constants::pi<double>() - fabs(xyz(2)));
614  bool result = xyz(2) < absolute_z_axis_tolerance_ + std::numeric_limits<double>::epsilon() &&
615  xyz(1) < absolute_y_axis_tolerance_ + std::numeric_limits<double>::epsilon() &&
616  xyz(0) < absolute_x_axis_tolerance_ + std::numeric_limits<double>::epsilon();
617 
618  if (verbose)
619  {
620  Eigen::Quaterniond q_act(state.getGlobalLinkTransform(link_model_).linear());
621  Eigen::Quaterniond q_des(desired_rotation_matrix_);
622  ROS_INFO_NAMED("kinematic_constraints",
623  "Orientation constraint %s for link '%s'. Quaternion desired: %f %f %f %f, quaternion "
624  "actual: %f %f %f %f, error: x=%f, y=%f, z=%f, tolerance: x=%f, y=%f, z=%f",
625  result ? "satisfied" : "violated", link_model_->getName().c_str(), q_des.x(), q_des.y(), q_des.z(),
626  q_des.w(), q_act.x(), q_act.y(), q_act.z(), q_act.w(), xyz(0), xyz(1), xyz(2),
627  absolute_x_axis_tolerance_, absolute_y_axis_tolerance_, absolute_z_axis_tolerance_);
628  }
629 
630  return ConstraintEvaluationResult(result, constraint_weight_ * (xyz(0) + xyz(1) + xyz(2)));
631 }
632 
633 void OrientationConstraint::print(std::ostream& out) const
634 {
635  if (link_model_)
636  {
637  out << "Orientation constraint on link '" << link_model_->getName() << "'" << std::endl;
638  Eigen::Quaterniond q_des(desired_rotation_matrix_);
639  out << "Desired orientation:" << q_des.x() << "," << q_des.y() << "," << q_des.z() << "," << q_des.w() << std::endl;
640  }
641  else
642  out << "No constraint" << std::endl;
643 }
644 
645 VisibilityConstraint::VisibilityConstraint(const robot_model::RobotModelConstPtr& model)
646  : KinematicConstraint(model), collision_robot_(new collision_detection::CollisionRobotFCL(model))
647 {
649 }
650 
652 {
653  mobile_sensor_frame_ = false;
654  mobile_target_frame_ = false;
655  target_frame_id_ = "";
656  sensor_frame_id_ = "";
657  sensor_pose_ = Eigen::Affine3d::Identity();
659  target_pose_ = Eigen::Affine3d::Identity();
660  cone_sides_ = 0;
661  points_.clear();
662  target_radius_ = -1.0;
663  max_view_angle_ = 0.0;
664  max_range_angle_ = 0.0;
665 }
666 
667 bool VisibilityConstraint::configure(const moveit_msgs::VisibilityConstraint& vc, const robot_state::Transforms& tf)
668 {
669  clear();
670  target_radius_ = fabs(vc.target_radius);
671 
672  if (vc.target_radius <= std::numeric_limits<double>::epsilon())
673  ROS_WARN_NAMED("kinematic_constraints",
674  "The radius of the target disc that must be visible should be strictly positive");
675 
676  if (vc.cone_sides < 3)
677  {
678  ROS_WARN_NAMED("kinematic_constraints", "The number of sides for the visibility region must be 3 or more. "
679  "Assuming 3 sides instead of the specified %d",
680  vc.cone_sides);
681  cone_sides_ = 3;
682  }
683  else
684  cone_sides_ = vc.cone_sides;
685 
686  // compute the points on the base circle of the cone that make up the cone sides
687  points_.clear();
688  double delta = 2.0 * boost::math::constants::pi<double>() / (double)cone_sides_;
689  double a = 0.0;
690  for (unsigned int i = 0; i < cone_sides_; ++i, a += delta)
691  {
692  double x = sin(a) * target_radius_;
693  double y = cos(a) * target_radius_;
694  points_.push_back(Eigen::Vector3d(x, y, 0.0));
695  }
696 
697  tf::poseMsgToEigen(vc.target_pose.pose, target_pose_);
698 
699  if (tf.isFixedFrame(vc.target_pose.header.frame_id))
700  {
701  tf.transformPose(vc.target_pose.header.frame_id, target_pose_, target_pose_);
703  mobile_target_frame_ = false;
704  // transform won't change, so apply it now
705  for (std::size_t i = 0; i < points_.size(); ++i)
706  points_[i] = target_pose_ * points_[i];
707  }
708  else
709  {
710  target_frame_id_ = vc.target_pose.header.frame_id;
711  mobile_target_frame_ = true;
712  }
713 
714  tf::poseMsgToEigen(vc.sensor_pose.pose, sensor_pose_);
715 
716  if (tf.isFixedFrame(vc.sensor_pose.header.frame_id))
717  {
718  tf.transformPose(vc.sensor_pose.header.frame_id, sensor_pose_, sensor_pose_);
720  mobile_sensor_frame_ = false;
721  }
722  else
723  {
724  sensor_frame_id_ = vc.sensor_pose.header.frame_id;
725  mobile_sensor_frame_ = true;
726  }
727 
728  if (vc.weight <= std::numeric_limits<double>::epsilon())
729  {
730  ROS_WARN_NAMED("kinematic_constraints", "The weight of visibility constraint is near zero. Setting to 1.0.");
731  constraint_weight_ = 1.0;
732  }
733  else
734  constraint_weight_ = vc.weight;
735 
736  max_view_angle_ = vc.max_view_angle;
737  max_range_angle_ = vc.max_range_angle;
738  sensor_view_direction_ = vc.sensor_view_direction;
739 
740  return target_radius_ > std::numeric_limits<double>::epsilon();
741 }
742 
743 bool VisibilityConstraint::equal(const KinematicConstraint& other, double margin) const
744 {
745  if (other.getType() != type_)
746  return false;
747  const VisibilityConstraint& o = static_cast<const VisibilityConstraint&>(other);
748 
752  {
753  if (fabs(max_view_angle_ - o.max_view_angle_) > margin || fabs(target_radius_ - o.target_radius_) > margin)
754  return false;
755  Eigen::Affine3d diff = sensor_pose_.inverse(Eigen::Isometry) * o.sensor_pose_;
756  if (diff.translation().norm() > margin)
757  return false;
758  if (!diff.linear().isIdentity(margin))
759  return false;
760  diff = target_pose_.inverse(Eigen::Isometry) * o.target_pose_;
761  if (diff.translation().norm() > margin)
762  return false;
763  if (!diff.linear().isIdentity(margin))
764  return false;
765  return true;
766  }
767  return false;
768 }
769 
771 {
772  return target_radius_ > std::numeric_limits<double>::epsilon();
773 }
774 
776 {
777  // the current pose of the sensor
778 
779  const Eigen::Affine3d& sp =
781  const Eigen::Affine3d& tp =
783 
784  // transform the points on the disc to the desired target frame
785  const EigenSTL::vector_Vector3d* points = &points_;
786  std::unique_ptr<EigenSTL::vector_Vector3d> tempPoints;
788  {
789  tempPoints.reset(new EigenSTL::vector_Vector3d(points_.size()));
790  for (std::size_t i = 0; i < points_.size(); ++i)
791  tempPoints->at(i) = tp * points_[i];
792  points = tempPoints.get();
793  }
794 
795  // allocate memory for a mesh to represent the visibility cone
796  shapes::Mesh* m = new shapes::Mesh();
797  m->vertex_count = cone_sides_ + 2;
798  m->vertices = new double[m->vertex_count * 3];
799  m->triangle_count = cone_sides_ * 2;
800  m->triangles = new unsigned int[m->triangle_count * 3];
801  // we do NOT allocate normals because we do not compute them
802 
803  // the sensor origin
804  m->vertices[0] = sp.translation().x();
805  m->vertices[1] = sp.translation().y();
806  m->vertices[2] = sp.translation().z();
807 
808  // the center of the base of the cone approximation
809  m->vertices[3] = tp.translation().x();
810  m->vertices[4] = tp.translation().y();
811  m->vertices[5] = tp.translation().z();
812 
813  // the points that approximate the base disc
814  for (std::size_t i = 0; i < points->size(); ++i)
815  {
816  m->vertices[i * 3 + 6] = points->at(i).x();
817  m->vertices[i * 3 + 7] = points->at(i).y();
818  m->vertices[i * 3 + 8] = points->at(i).z();
819  }
820 
821  // add the triangles
822  std::size_t p3 = points->size() * 3;
823  for (std::size_t i = 1; i < points->size(); ++i)
824  {
825  // triangle forming a side of the cone, using the sensor origin
826  std::size_t i3 = (i - 1) * 3;
827  m->triangles[i3] = i + 1;
828  m->triangles[i3 + 1] = 0;
829  m->triangles[i3 + 2] = i + 2;
830  // triangle forming a part of the base of the cone, using the center of the base
831  std::size_t i6 = p3 + i3;
832  m->triangles[i6] = i + 1;
833  m->triangles[i6 + 1] = 1;
834  m->triangles[i6 + 2] = i + 2;
835  }
836 
837  // last triangles
838  m->triangles[p3 - 3] = points->size() + 1;
839  m->triangles[p3 - 2] = 0;
840  m->triangles[p3 - 1] = 2;
841  p3 *= 2;
842  m->triangles[p3 - 3] = points->size() + 1;
843  m->triangles[p3 - 2] = 1;
844  m->triangles[p3 - 1] = 2;
845 
846  return m;
847 }
848 
850  visualization_msgs::MarkerArray& markers) const
851 {
852  shapes::Mesh* m = getVisibilityCone(state);
853  visualization_msgs::Marker mk;
855  delete m;
856  mk.header.frame_id = robot_model_->getModelFrame();
857  mk.header.stamp = ros::Time::now();
858  mk.ns = "constraints";
859  mk.id = 1;
860  mk.action = visualization_msgs::Marker::ADD;
861  mk.pose.position.x = 0;
862  mk.pose.position.y = 0;
863  mk.pose.position.z = 0;
864  mk.pose.orientation.x = 0;
865  mk.pose.orientation.y = 0;
866  mk.pose.orientation.z = 0;
867  mk.pose.orientation.w = 1;
868  mk.lifetime = ros::Duration(60);
869  // this scale necessary to make results look reasonable
870  mk.scale.x = .01;
871  mk.color.a = 1.0;
872  mk.color.r = 1.0;
873  mk.color.g = 0.0;
874  mk.color.b = 0.0;
875 
876  markers.markers.push_back(mk);
877 
878  const Eigen::Affine3d& sp =
880  const Eigen::Affine3d& tp =
882 
883  visualization_msgs::Marker mka;
884  mka.type = visualization_msgs::Marker::ARROW;
885  mka.action = visualization_msgs::Marker::ADD;
886  mka.color = mk.color;
887  mka.pose = mk.pose;
888 
889  mka.header = mk.header;
890  mka.ns = mk.ns;
891  mka.id = 2;
892  mka.lifetime = mk.lifetime;
893  mka.scale.x = 0.05;
894  mka.scale.y = .15;
895  mka.scale.z = 0.0;
896  mka.points.resize(2);
897  Eigen::Vector3d d = tp.translation() + tp.linear().col(2) * 0.5;
898  mka.points[0].x = tp.translation().x();
899  mka.points[0].y = tp.translation().y();
900  mka.points[0].z = tp.translation().z();
901  mka.points[1].x = d.x();
902  mka.points[1].y = d.y();
903  mka.points[1].z = d.z();
904  markers.markers.push_back(mka);
905 
906  mka.id = 3;
907  mka.color.b = 1.0;
908  mka.color.r = 0.0;
909 
910  d = sp.translation() + sp.linear().col(2 - sensor_view_direction_) * 0.5;
911  mka.points[0].x = sp.translation().x();
912  mka.points[0].y = sp.translation().y();
913  mka.points[0].z = sp.translation().z();
914  mka.points[1].x = d.x();
915  mka.points[1].y = d.y();
916  mka.points[1].z = d.z();
917 
918  markers.markers.push_back(mka);
919 }
920 
922 {
923  if (target_radius_ <= std::numeric_limits<double>::epsilon())
924  return ConstraintEvaluationResult(true, 0.0);
925 
926  if (max_view_angle_ > 0.0 || max_range_angle_ > 0.0)
927  {
928  const Eigen::Affine3d& sp =
930  const Eigen::Affine3d& tp =
932 
933  // necessary to do subtraction as SENSOR_Z is 0 and SENSOR_X is 2
934  const Eigen::Vector3d& normal2 = sp.linear().col(2 - sensor_view_direction_);
935 
936  if (max_view_angle_ > 0.0)
937  {
938  const Eigen::Vector3d& normal1 = tp.linear().col(2) * -1.0; // along Z axis and inverted
939  double dp = normal2.dot(normal1);
940  double ang = acos(dp);
941  if (dp < 0.0)
942  {
943  if (verbose)
944  ROS_INFO_NAMED("kinematic_constraints", "Visibility constraint is violated because the sensor is looking at "
945  "the wrong side");
946  return ConstraintEvaluationResult(false, 0.0);
947  }
948  if (max_view_angle_ < ang)
949  {
950  if (verbose)
951  ROS_INFO_NAMED("kinematic_constraints", "Visibility constraint is violated because the view angle is %lf "
952  "(above the maximum allowed of %lf)",
953  ang, max_view_angle_);
954  return ConstraintEvaluationResult(false, 0.0);
955  }
956  }
957  if (max_range_angle_ > 0.0)
958  {
959  const Eigen::Vector3d& dir = (tp.translation() - sp.translation()).normalized();
960  double dp = normal2.dot(dir);
961  if (dp < 0.0)
962  {
963  if (verbose)
964  ROS_INFO_NAMED("kinematic_constraints", "Visibility constraint is violated because the sensor is looking at "
965  "the wrong side");
966  return ConstraintEvaluationResult(false, 0.0);
967  }
968 
969  double ang = acos(dp);
970  if (max_range_angle_ < ang)
971  {
972  if (verbose)
973  ROS_INFO_NAMED("kinematic_constraints", "Visibility constraint is violated because the range angle is %lf "
974  "(above the maximum allowed of %lf)",
975  ang, max_range_angle_);
976  return ConstraintEvaluationResult(false, 0.0);
977  }
978  }
979  }
980 
981  shapes::Mesh* m = getVisibilityCone(state);
982  if (!m)
983  return ConstraintEvaluationResult(false, 0.0);
984 
985  // add the visibility cone as an object
987  collision_world.getWorld()->addToObject("cone", shapes::ShapeConstPtr(m), Eigen::Affine3d::Identity());
988 
989  // check for collisions between the robot and the cone
993  acm.setDefaultEntry("cone", boost::bind(&VisibilityConstraint::decideContact, this, _1));
994  req.contacts = true;
995  req.verbose = verbose;
996  req.max_contacts = 1;
997  collision_world.checkRobotCollision(req, res, *collision_robot_, state, acm);
998 
999  if (verbose)
1000  {
1001  std::stringstream ss;
1002  m->print(ss);
1003  ROS_INFO_NAMED("kinematic_constraints", "Visibility constraint %ssatisfied. Visibility cone approximation:\n %s",
1004  res.collision ? "not " : "", ss.str().c_str());
1005  }
1006 
1007  return ConstraintEvaluationResult(!res.collision, res.collision ? res.contacts.begin()->second.front().depth : 0.0);
1008 }
1009 
1011 {
1014  return true;
1019  {
1020  ROS_DEBUG_NAMED("kinematic_constraints", "Accepted collision with either sensor or target");
1021  return true;
1022  }
1027  {
1028  ROS_DEBUG_NAMED("kinematic_constraints", "Accepted collision with either sensor or target");
1029  return true;
1030  }
1031  return false;
1032 }
1033 
1034 void VisibilityConstraint::print(std::ostream& out) const
1035 {
1036  if (enabled())
1037  {
1038  out << "Visibility constraint for sensor in frame '" << sensor_frame_id_ << "' using target in frame '"
1039  << target_frame_id_ << "'" << std::endl;
1040  out << "Target radius: " << target_radius_ << ", using " << cone_sides_ << " sides." << std::endl;
1041  }
1042  else
1043  out << "No constraint" << std::endl;
1044 }
1045 
1047 {
1048  all_constraints_ = moveit_msgs::Constraints();
1049  kinematic_constraints_.clear();
1050  joint_constraints_.clear();
1051  position_constraints_.clear();
1052  orientation_constraints_.clear();
1053  visibility_constraints_.clear();
1054 }
1055 
1056 bool KinematicConstraintSet::add(const std::vector<moveit_msgs::JointConstraint>& jc)
1057 {
1058  bool result = true;
1059  for (unsigned int i = 0; i < jc.size(); ++i)
1060  {
1062  bool u = ev->configure(jc[i]);
1063  result = result && u;
1064  kinematic_constraints_.push_back(KinematicConstraintPtr(ev));
1065  joint_constraints_.push_back(jc[i]);
1066  all_constraints_.joint_constraints.push_back(jc[i]);
1067  }
1068  return result;
1069 }
1070 
1071 bool KinematicConstraintSet::add(const std::vector<moveit_msgs::PositionConstraint>& pc,
1072  const robot_state::Transforms& tf)
1073 {
1074  bool result = true;
1075  for (unsigned int i = 0; i < pc.size(); ++i)
1076  {
1078  bool u = ev->configure(pc[i], tf);
1079  result = result && u;
1080  kinematic_constraints_.push_back(KinematicConstraintPtr(ev));
1081  position_constraints_.push_back(pc[i]);
1082  all_constraints_.position_constraints.push_back(pc[i]);
1083  }
1084  return result;
1085 }
1086 
1087 bool KinematicConstraintSet::add(const std::vector<moveit_msgs::OrientationConstraint>& oc,
1088  const robot_state::Transforms& tf)
1089 {
1090  bool result = true;
1091  for (unsigned int i = 0; i < oc.size(); ++i)
1092  {
1094  bool u = ev->configure(oc[i], tf);
1095  result = result && u;
1096  kinematic_constraints_.push_back(KinematicConstraintPtr(ev));
1097  orientation_constraints_.push_back(oc[i]);
1098  all_constraints_.orientation_constraints.push_back(oc[i]);
1099  }
1100  return result;
1101 }
1102 
1103 bool KinematicConstraintSet::add(const std::vector<moveit_msgs::VisibilityConstraint>& vc,
1104  const robot_state::Transforms& tf)
1105 {
1106  bool result = true;
1107  for (unsigned int i = 0; i < vc.size(); ++i)
1108  {
1110  bool u = ev->configure(vc[i], tf);
1111  result = result && u;
1112  kinematic_constraints_.push_back(KinematicConstraintPtr(ev));
1113  visibility_constraints_.push_back(vc[i]);
1114  all_constraints_.visibility_constraints.push_back(vc[i]);
1115  }
1116  return result;
1117 }
1118 
1119 bool KinematicConstraintSet::add(const moveit_msgs::Constraints& c, const robot_state::Transforms& tf)
1120 {
1121  bool j = add(c.joint_constraints);
1122  bool p = add(c.position_constraints, tf);
1123  bool o = add(c.orientation_constraints, tf);
1124  bool v = add(c.visibility_constraints, tf);
1125  return j && p && o && v;
1126 }
1127 
1129 {
1130  ConstraintEvaluationResult res(true, 0.0);
1131  for (unsigned int i = 0; i < kinematic_constraints_.size(); ++i)
1132  {
1133  ConstraintEvaluationResult r = kinematic_constraints_[i]->decide(state, verbose);
1134  if (!r.satisfied)
1135  res.satisfied = false;
1136  res.distance += r.distance;
1137  }
1138  return res;
1139 }
1140 
1142  std::vector<ConstraintEvaluationResult>& results,
1143  bool verbose) const
1144 {
1145  ConstraintEvaluationResult result(true, 0.0);
1146  results.resize(kinematic_constraints_.size());
1147  for (std::size_t i = 0; i < kinematic_constraints_.size(); ++i)
1148  {
1149  results[i] = kinematic_constraints_[i]->decide(state, verbose);
1150  result.satisfied = result.satisfied && results[i].satisfied;
1151  result.distance += results[i].distance;
1152  }
1153 
1154  return result;
1155 }
1156 
1157 void KinematicConstraintSet::print(std::ostream& out) const
1158 {
1159  out << kinematic_constraints_.size() << " kinematic constraints" << std::endl;
1160  for (unsigned int i = 0; i < kinematic_constraints_.size(); ++i)
1161  kinematic_constraints_[i]->print(out);
1162 }
1163 
1164 bool KinematicConstraintSet::equal(const KinematicConstraintSet& other, double margin) const
1165 {
1166  // each constraint in this matches some in the other
1167  for (unsigned int i = 0; i < kinematic_constraints_.size(); ++i)
1168  {
1169  bool found = false;
1170  for (unsigned int j = 0; !found && j < other.kinematic_constraints_.size(); ++j)
1171  found = kinematic_constraints_[i]->equal(*other.kinematic_constraints_[j], margin);
1172  if (!found)
1173  return false;
1174  }
1175  // each constraint in the other matches some constraint in this
1176  for (unsigned int i = 0; i < other.kinematic_constraints_.size(); ++i)
1177  {
1178  bool found = false;
1179  for (unsigned int j = 0; !found && j < kinematic_constraints_.size(); ++j)
1180  found = other.kinematic_constraints_[i]->equal(*kinematic_constraints_[j], margin);
1181  if (!found)
1182  return false;
1183  }
1184  return true;
1185 }
1186 
1187 } // end of namespace kinematic_constraints
d
collision_detection::CollisionRobotPtr collision_robot_
A copy of the collision robot maintained for collision checking the cone against robot links...
bool decideContact(const collision_detection::Contact &contact) const
Function that gets passed into collision checking to allow some collisions.
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 absolute_z_axis_tolerance_
Storage for the tolerances.
Class for constraints on the orientation of a link.
const std::string & getTargetFrame() const
Get the planning frame corresponding to this set of transforms.
Definition: transforms.cpp:77
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.
#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.
const Eigen::Affine3d & getGlobalLinkTransform(const std::string &link_name)
Definition: robot_state.h:1368
bool configure(const moveit_msgs::PositionConstraint &pc, const robot_state::Transforms &tf)
Configure the constraint based on a moveit_msgs::PositionConstraint.
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.
void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e)
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 ids of the bodies in contact, plus information about the contacts themse...
double max_view_angle_
Storage for the max view angle.
Eigen::Affine3d target_pose_
The target pose transformed into the transform frame.
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
double joint_tolerance_below_
Position and tolerance values.
A class that contains many different constraints, and can check RobotState *versus the full set...
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:64
const Eigen::Affine3d & getFrameTransform(const std::string &id)
Get the transformation matrix from the model frame to the frame identified by id. ...
virtual ConstraintEvaluationResult decide(const robot_state::RobotState &state, bool verbose=false) const
Decide whether the constraint is satisfied in the indicated state.
virtual void print(std::ostream &out=std::cout) const
bool configure(const moveit_msgs::VisibilityConstraint &vc, const robot_state::Transforms &tf)
Configure the constraint based on a moveit_msgs::VisibilityConstraint.
virtual bool enabled() const
This function returns true if this constraint is configured and able to decide whether states do meet...
Representation and evaluation of kinematic constraints.
virtual void print(std::ostream &out=std::cout) const
Print the constraint data.
unsigned int * triangles
Provides an implementation of a snapshot of a transform tree that can be easily queried for transform...
Definition: transforms.h:61
void print(std::ostream &out=std::cout) const
Print the constraint data.
Generic interface to collision detection.
bool equal(const KinematicConstraintSet &other, double margin) const
Whether or not another KinematicConstraintSet is equal to this one.
double distance
The distance evaluation from the constraint or constraints.
bool isContinuous() const
Check if this joint wraps around.
double y
double target_radius_
Storage for the target radius.
virtual void print(std::ostream &out=std::cout) const
Print the constraint data.
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.
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:150
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,...)
virtual bool equal(const KinematicConstraint &other, double margin) const
Check if two joint constraints are the same.
unsigned int vertex_count
std::size_t max_contacts
Overall maximum number of contacts to compute.
bool verbose
Flag indicating whether information about detected collisions should be reported. ...
Eigen::Affine3d sensor_pose_
The sensor pose transformed into the transform frame.
std::string desired_rotation_frame_id_
The target frame of the transform tree.
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...
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...
bool mobile_target_frame_
True if the target is a non-fixed frame relative to the transform frame.
void transformPose(const std::string &from_frame, const Eigen::Affine3d &t_in, Eigen::Affine3d &t_out) const
Transform a pose in from_frame to the target_frame.
Definition: transforms.h:173
Body * createBodyFromShape(const shapes::Shape *shape)
virtual ConstraintEvaluationResult decide(const robot_state::RobotState &state, bool verbose=false) const
Decide whether the constraint is satisfied in the indicated state.
const robot_model::LinkModel * link_model_
The link model constraint subject.
double epsilon
virtual void print(std::ostream &out=std::cout) const
Print the constraint data.
virtual void clear()
Clear the stored constraint.
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.
virtual bool equal(const KinematicConstraint &other, double margin) const
Check if two orientation constraints are the same.
Base class for representing a kinematic constraint.
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
EigenSTL::vector_Affine3d constraint_region_pose_
The constraint region pose vector.
VisibilityConstraint(const robot_model::RobotModelConstPtr &model)
Constructor.
virtual bool equal(const KinematicConstraint &other, double margin) const
Check if two constraints are the same. For position constraints this means that:
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...
virtual void checkRobotCollision(const CollisionRequest &req, CollisionResult &res, const CollisionRobot &robot, const robot_state::RobotState &state) const
Check whether the robot model is in collision with the world. Any collisions between a robot link and...
Shape * constructShapeFromMsg(const shape_msgs::SolidPrimitive &shape_msg)
Struct for containing the results of constraint evaluation.
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().
virtual void clear()
Clear the stored constraint.
unsigned int cone_sides_
Storage for the cone sides.
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:92
std::string body_name_1
The id of the first body involved in the contact.
INLINE Rall1d< T, V, S > acos(const Rall1d< T, V, S > &x)
virtual bool enabled() const
This function returns true if this constraint is configured and able to decide whether states do meet...
double max_range_angle_
Storage for the max range angle.
shapes::Mesh * getVisibilityCone(const robot_state::RobotState &state) const
Gets a trimesh shape representing the visibility cone.
std::vector< KinematicConstraintPtr > kinematic_constraints_
Shared pointers to all the member constraints.
virtual bool equal(const KinematicConstraint &other, double margin) const
Check if two constraints are the same.
void quaternionMsgToEigen(const geometry_msgs::Quaternion &m, Eigen::Quaterniond &e)
#define ROS_ERROR_NAMED(name,...)
const robot_model::JointModel * joint_model_
The joint from the kinematic model for this constraint.
static Time now()
virtual void print(std::ostream &out=std::cout) const
Print the constraint data.
Representation of a robot&#39;s state. This includes position, velocity, acceleration and effort...
Definition: robot_state.h:123
BodyType body_type_1
The type of the first body involved in the contact.
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:241
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)
virtual bool enabled() const
This function returns true if this constraint is configured and able to decide whether states do meet...
virtual void clear()
Clear the stored constraint.
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...
virtual ConstraintEvaluationResult decide(const robot_state::RobotState &state, bool verbose=false) const
Decide whether the constraint is satisfied in the indicated state.
ConstraintType getType() const
Get the type of constraint.
double x
r
static double normalizeAngle(double angle)
virtual bool enabled() const
This function returns true if this constraint is configured and able to decide whether states do meet...
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
bool configure(const moveit_msgs::JointConstraint &jc)
Configure the constraint based on a moveit_msgs::JointConstraint.
virtual void clear()
Clear the stored constraint.
ConstraintEvaluationResult decide(const robot_state::RobotState &state, bool verbose=false) const
Determines whether all constraints are satisfied by state, returning a single evaluation result...
virtual ConstraintEvaluationResult decide(const robot_state::RobotState &state, bool verbose=false) const
Decide whether the constraint is satisfied in the indicated state.
std::shared_ptr< const Shape > ShapeConstPtr


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sun Sep 15 2019 03:57:56