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() * o.constraint_region_pose_[j];
394  if (diff.translation().norm() < margin && diff.rotation().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_.inverse();
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  Eigen::Matrix3d diff = desired_rotation_matrix_.inverse() * o.desired_rotation_matrix_;
569  if (!diff.isIdentity(margin))
570  return false;
571  return fabs(absolute_x_axis_tolerance_ - o.absolute_x_axis_tolerance_) <= margin &&
572  fabs(absolute_y_axis_tolerance_ - o.absolute_y_axis_tolerance_) <= margin &&
573  fabs(absolute_z_axis_tolerance_ - o.absolute_z_axis_tolerance_) <= margin;
574  }
575  return false;
576 }
577 
579 {
580  link_model_ = nullptr;
581  desired_rotation_matrix_ = Eigen::Matrix3d::Identity();
582  desired_rotation_matrix_inv_ = Eigen::Matrix3d::Identity();
583  desired_rotation_frame_id_ = "";
584  mobile_frame_ = false;
585  absolute_z_axis_tolerance_ = absolute_y_axis_tolerance_ = absolute_x_axis_tolerance_ = 0.0;
586 }
587 
589 {
590  return link_model_;
591 }
592 
594 {
595  if (!link_model_)
596  return ConstraintEvaluationResult(true, 0.0);
597 
598  Eigen::Vector3d xyz;
599  if (mobile_frame_)
600  {
601  Eigen::Matrix3d tmp = state.getFrameTransform(desired_rotation_frame_id_).rotation() * desired_rotation_matrix_;
602  Eigen::Affine3d diff(tmp.inverse() * state.getGlobalLinkTransform(link_model_).rotation());
603  xyz = diff.rotation().eulerAngles(0, 1, 2);
604  // 0,1,2 corresponds to XYZ, the convention used in sampling constraints
605  }
606  else
607  {
608  Eigen::Affine3d diff(desired_rotation_matrix_inv_ * state.getGlobalLinkTransform(link_model_).rotation());
609  xyz =
610  diff.rotation().eulerAngles(0, 1, 2); // 0,1,2 corresponds to XYZ, the convention used in sampling constraints
611  }
612 
613  xyz(0) = std::min(fabs(xyz(0)), boost::math::constants::pi<double>() - fabs(xyz(0)));
614  xyz(1) = std::min(fabs(xyz(1)), boost::math::constants::pi<double>() - fabs(xyz(1)));
615  xyz(2) = std::min(fabs(xyz(2)), boost::math::constants::pi<double>() - fabs(xyz(2)));
616  bool result = xyz(2) < absolute_z_axis_tolerance_ + std::numeric_limits<double>::epsilon() &&
617  xyz(1) < absolute_y_axis_tolerance_ + std::numeric_limits<double>::epsilon() &&
618  xyz(0) < absolute_x_axis_tolerance_ + std::numeric_limits<double>::epsilon();
619 
620  if (verbose)
621  {
622  Eigen::Quaterniond q_act(state.getGlobalLinkTransform(link_model_).rotation());
623  Eigen::Quaterniond q_des(desired_rotation_matrix_);
624  ROS_INFO_NAMED("kinematic_constraints",
625  "Orientation constraint %s for link '%s'. Quaternion desired: %f %f %f %f, quaternion "
626  "actual: %f %f %f %f, error: x=%f, y=%f, z=%f, tolerance: x=%f, y=%f, z=%f",
627  result ? "satisfied" : "violated", link_model_->getName().c_str(), q_des.x(), q_des.y(), q_des.z(),
628  q_des.w(), q_act.x(), q_act.y(), q_act.z(), q_act.w(), xyz(0), xyz(1), xyz(2),
629  absolute_x_axis_tolerance_, absolute_y_axis_tolerance_, absolute_z_axis_tolerance_);
630  }
631 
632  return ConstraintEvaluationResult(result, constraint_weight_ * (xyz(0) + xyz(1) + xyz(2)));
633 }
634 
635 void OrientationConstraint::print(std::ostream& out) const
636 {
637  if (link_model_)
638  {
639  out << "Orientation constraint on link '" << link_model_->getName() << "'" << std::endl;
640  Eigen::Quaterniond q_des(desired_rotation_matrix_);
641  out << "Desired orientation:" << q_des.x() << "," << q_des.y() << "," << q_des.z() << "," << q_des.w() << std::endl;
642  }
643  else
644  out << "No constraint" << std::endl;
645 }
646 
647 VisibilityConstraint::VisibilityConstraint(const robot_model::RobotModelConstPtr& model)
648  : KinematicConstraint(model), collision_robot_(new collision_detection::CollisionRobotFCL(model))
649 {
651 }
652 
654 {
655  mobile_sensor_frame_ = false;
656  mobile_target_frame_ = false;
657  target_frame_id_ = "";
658  sensor_frame_id_ = "";
659  sensor_pose_ = Eigen::Affine3d::Identity();
661  target_pose_ = Eigen::Affine3d::Identity();
662  cone_sides_ = 0;
663  points_.clear();
664  target_radius_ = -1.0;
665  max_view_angle_ = 0.0;
666  max_range_angle_ = 0.0;
667 }
668 
669 bool VisibilityConstraint::configure(const moveit_msgs::VisibilityConstraint& vc, const robot_state::Transforms& tf)
670 {
671  clear();
672  target_radius_ = fabs(vc.target_radius);
673 
674  if (vc.target_radius <= std::numeric_limits<double>::epsilon())
675  ROS_WARN_NAMED("kinematic_constraints", "The radius of the target disc that must be visible should be strictly "
676  "positive");
677 
678  if (vc.cone_sides < 3)
679  {
680  ROS_WARN_NAMED("kinematic_constraints", "The number of sides for the visibility region must be 3 or more. "
681  "Assuming 3 sides instead of the specified %d",
682  vc.cone_sides);
683  cone_sides_ = 3;
684  }
685  else
686  cone_sides_ = vc.cone_sides;
687 
688  // compute the points on the base circle of the cone that make up the cone sides
689  points_.clear();
690  double delta = 2.0 * boost::math::constants::pi<double>() / (double)cone_sides_;
691  double a = 0.0;
692  for (unsigned int i = 0; i < cone_sides_; ++i, a += delta)
693  {
694  double x = sin(a) * target_radius_;
695  double y = cos(a) * target_radius_;
696  points_.push_back(Eigen::Vector3d(x, y, 0.0));
697  }
698 
699  tf::poseMsgToEigen(vc.target_pose.pose, target_pose_);
700 
701  if (tf.isFixedFrame(vc.target_pose.header.frame_id))
702  {
703  tf.transformPose(vc.target_pose.header.frame_id, target_pose_, target_pose_);
705  mobile_target_frame_ = false;
706  // transform won't change, so apply it now
707  for (std::size_t i = 0; i < points_.size(); ++i)
708  points_[i] = target_pose_ * points_[i];
709  }
710  else
711  {
712  target_frame_id_ = vc.target_pose.header.frame_id;
713  mobile_target_frame_ = true;
714  }
715 
716  tf::poseMsgToEigen(vc.sensor_pose.pose, sensor_pose_);
717 
718  if (tf.isFixedFrame(vc.sensor_pose.header.frame_id))
719  {
720  tf.transformPose(vc.sensor_pose.header.frame_id, sensor_pose_, sensor_pose_);
722  mobile_sensor_frame_ = false;
723  }
724  else
725  {
726  sensor_frame_id_ = vc.sensor_pose.header.frame_id;
727  mobile_sensor_frame_ = true;
728  }
729 
730  if (vc.weight <= std::numeric_limits<double>::epsilon())
731  {
732  ROS_WARN_NAMED("kinematic_constraints", "The weight of visibility constraint is near zero. Setting to 1.0.");
733  constraint_weight_ = 1.0;
734  }
735  else
736  constraint_weight_ = vc.weight;
737 
738  max_view_angle_ = vc.max_view_angle;
739  max_range_angle_ = vc.max_range_angle;
740  sensor_view_direction_ = vc.sensor_view_direction;
741 
742  return target_radius_ > std::numeric_limits<double>::epsilon();
743 }
744 
745 bool VisibilityConstraint::equal(const KinematicConstraint& other, double margin) const
746 {
747  if (other.getType() != type_)
748  return false;
749  const VisibilityConstraint& o = static_cast<const VisibilityConstraint&>(other);
750 
754  {
755  if (fabs(max_view_angle_ - o.max_view_angle_) > margin || fabs(target_radius_ - o.target_radius_) > margin)
756  return false;
757  Eigen::Affine3d diff = sensor_pose_.inverse() * o.sensor_pose_;
758  if (diff.translation().norm() > margin)
759  return false;
760  if (!diff.rotation().isIdentity(margin))
761  return false;
762  diff = target_pose_.inverse() * o.target_pose_;
763  if (diff.translation().norm() > margin)
764  return false;
765  if (!diff.rotation().isIdentity(margin))
766  return false;
767  return true;
768  }
769  return false;
770 }
771 
773 {
774  return target_radius_ > std::numeric_limits<double>::epsilon();
775 }
776 
778 {
779  // the current pose of the sensor
780 
781  const Eigen::Affine3d& sp =
783  const Eigen::Affine3d& tp =
785 
786  // transform the points on the disc to the desired target frame
787  const EigenSTL::vector_Vector3d* points = &points_;
788  std::unique_ptr<EigenSTL::vector_Vector3d> tempPoints;
790  {
791  tempPoints.reset(new EigenSTL::vector_Vector3d(points_.size()));
792  for (std::size_t i = 0; i < points_.size(); ++i)
793  tempPoints->at(i) = tp * points_[i];
794  points = tempPoints.get();
795  }
796 
797  // allocate memory for a mesh to represent the visibility cone
798  shapes::Mesh* m = new shapes::Mesh();
799  m->vertex_count = cone_sides_ + 2;
800  m->vertices = new double[m->vertex_count * 3];
801  m->triangle_count = cone_sides_ * 2;
802  m->triangles = new unsigned int[m->triangle_count * 3];
803  // we do NOT allocate normals because we do not compute them
804 
805  // the sensor origin
806  m->vertices[0] = sp.translation().x();
807  m->vertices[1] = sp.translation().y();
808  m->vertices[2] = sp.translation().z();
809 
810  // the center of the base of the cone approximation
811  m->vertices[3] = tp.translation().x();
812  m->vertices[4] = tp.translation().y();
813  m->vertices[5] = tp.translation().z();
814 
815  // the points that approximate the base disc
816  for (std::size_t i = 0; i < points->size(); ++i)
817  {
818  m->vertices[i * 3 + 6] = points->at(i).x();
819  m->vertices[i * 3 + 7] = points->at(i).y();
820  m->vertices[i * 3 + 8] = points->at(i).z();
821  }
822 
823  // add the triangles
824  std::size_t p3 = points->size() * 3;
825  for (std::size_t i = 1; i < points->size(); ++i)
826  {
827  // triangle forming a side of the cone, using the sensor origin
828  std::size_t i3 = (i - 1) * 3;
829  m->triangles[i3] = i + 1;
830  m->triangles[i3 + 1] = 0;
831  m->triangles[i3 + 2] = i + 2;
832  // triangle forming a part of the base of the cone, using the center of the base
833  std::size_t i6 = p3 + i3;
834  m->triangles[i6] = i + 1;
835  m->triangles[i6 + 1] = 1;
836  m->triangles[i6 + 2] = i + 2;
837  }
838 
839  // last triangles
840  m->triangles[p3 - 3] = points->size() + 1;
841  m->triangles[p3 - 2] = 0;
842  m->triangles[p3 - 1] = 2;
843  p3 *= 2;
844  m->triangles[p3 - 3] = points->size() + 1;
845  m->triangles[p3 - 2] = 1;
846  m->triangles[p3 - 1] = 2;
847 
848  return m;
849 }
850 
852  visualization_msgs::MarkerArray& markers) const
853 {
854  shapes::Mesh* m = getVisibilityCone(state);
855  visualization_msgs::Marker mk;
857  delete m;
858  mk.header.frame_id = robot_model_->getModelFrame();
859  mk.header.stamp = ros::Time::now();
860  mk.ns = "constraints";
861  mk.id = 1;
862  mk.action = visualization_msgs::Marker::ADD;
863  mk.pose.position.x = 0;
864  mk.pose.position.y = 0;
865  mk.pose.position.z = 0;
866  mk.pose.orientation.x = 0;
867  mk.pose.orientation.y = 0;
868  mk.pose.orientation.z = 0;
869  mk.pose.orientation.w = 1;
870  mk.lifetime = ros::Duration(60);
871  // this scale necessary to make results look reasonable
872  mk.scale.x = .01;
873  mk.color.a = 1.0;
874  mk.color.r = 1.0;
875  mk.color.g = 0.0;
876  mk.color.b = 0.0;
877 
878  markers.markers.push_back(mk);
879 
880  const Eigen::Affine3d& sp =
882  const Eigen::Affine3d& tp =
884 
885  visualization_msgs::Marker mka;
886  mka.type = visualization_msgs::Marker::ARROW;
887  mka.action = visualization_msgs::Marker::ADD;
888  mka.color = mk.color;
889  mka.pose = mk.pose;
890 
891  mka.header = mk.header;
892  mka.ns = mk.ns;
893  mka.id = 2;
894  mka.lifetime = mk.lifetime;
895  mka.scale.x = 0.05;
896  mka.scale.y = .15;
897  mka.scale.z = 0.0;
898  mka.points.resize(2);
899  Eigen::Vector3d d = tp.translation() + tp.rotation().col(2) * 0.5;
900  mka.points[0].x = tp.translation().x();
901  mka.points[0].y = tp.translation().y();
902  mka.points[0].z = tp.translation().z();
903  mka.points[1].x = d.x();
904  mka.points[1].y = d.y();
905  mka.points[1].z = d.z();
906  markers.markers.push_back(mka);
907 
908  mka.id = 3;
909  mka.color.b = 1.0;
910  mka.color.r = 0.0;
911 
912  d = sp.translation() + sp.rotation().col(2 - sensor_view_direction_) * 0.5;
913  mka.points[0].x = sp.translation().x();
914  mka.points[0].y = sp.translation().y();
915  mka.points[0].z = sp.translation().z();
916  mka.points[1].x = d.x();
917  mka.points[1].y = d.y();
918  mka.points[1].z = d.z();
919 
920  markers.markers.push_back(mka);
921 }
922 
924 {
925  if (target_radius_ <= std::numeric_limits<double>::epsilon())
926  return ConstraintEvaluationResult(true, 0.0);
927 
928  if (max_view_angle_ > 0.0 || max_range_angle_ > 0.0)
929  {
930  const Eigen::Affine3d& sp =
932  const Eigen::Affine3d& tp =
934 
935  // necessary to do subtraction as SENSOR_Z is 0 and SENSOR_X is 2
936  const Eigen::Vector3d& normal2 = sp.rotation().col(2 - sensor_view_direction_);
937 
938  if (max_view_angle_ > 0.0)
939  {
940  const Eigen::Vector3d& normal1 = tp.rotation().col(2) * -1.0; // along Z axis and inverted
941  double dp = normal2.dot(normal1);
942  double ang = acos(dp);
943  if (dp < 0.0)
944  {
945  if (verbose)
946  ROS_INFO_NAMED("kinematic_constraints", "Visibility constraint is violated because the sensor is looking at "
947  "the wrong side");
948  return ConstraintEvaluationResult(false, 0.0);
949  }
950  if (max_view_angle_ < ang)
951  {
952  if (verbose)
953  ROS_INFO_NAMED("kinematic_constraints", "Visibility constraint is violated because the view angle is %lf "
954  "(above the maximum allowed of %lf)",
955  ang, max_view_angle_);
956  return ConstraintEvaluationResult(false, 0.0);
957  }
958  }
959  if (max_range_angle_ > 0.0)
960  {
961  const Eigen::Vector3d& dir = (tp.translation() - sp.translation()).normalized();
962  double dp = normal2.dot(dir);
963  if (dp < 0.0)
964  {
965  if (verbose)
966  ROS_INFO_NAMED("kinematic_constraints", "Visibility constraint is violated because the sensor is looking at "
967  "the wrong side");
968  return ConstraintEvaluationResult(false, 0.0);
969  }
970 
971  double ang = acos(dp);
972  if (max_range_angle_ < ang)
973  {
974  if (verbose)
975  ROS_INFO_NAMED("kinematic_constraints", "Visibility constraint is violated because the range angle is %lf "
976  "(above the maximum allowed of %lf)",
977  ang, max_range_angle_);
978  return ConstraintEvaluationResult(false, 0.0);
979  }
980  }
981  }
982 
983  shapes::Mesh* m = getVisibilityCone(state);
984  if (!m)
985  return ConstraintEvaluationResult(false, 0.0);
986 
987  // add the visibility cone as an object
989  collision_world.getWorld()->addToObject("cone", shapes::ShapeConstPtr(m), Eigen::Affine3d::Identity());
990 
991  // check for collisions between the robot and the cone
995  acm.setDefaultEntry("cone", boost::bind(&VisibilityConstraint::decideContact, this, _1));
996  req.contacts = true;
997  req.verbose = verbose;
998  req.max_contacts = 1;
999  collision_world.checkRobotCollision(req, res, *collision_robot_, state, acm);
1000 
1001  if (verbose)
1002  {
1003  std::stringstream ss;
1004  m->print(ss);
1005  ROS_INFO_NAMED("kinematic_constraints", "Visibility constraint %ssatisfied. Visibility cone approximation:\n %s",
1006  res.collision ? "not " : "", ss.str().c_str());
1007  }
1008 
1009  return ConstraintEvaluationResult(!res.collision, res.collision ? res.contacts.begin()->second.front().depth : 0.0);
1010 }
1011 
1013 {
1016  return true;
1021  {
1022  ROS_DEBUG_NAMED("kinematic_constraints", "Accepted collision with either sensor or target");
1023  return true;
1024  }
1029  {
1030  ROS_DEBUG_NAMED("kinematic_constraints", "Accepted collision with either sensor or target");
1031  return true;
1032  }
1033  return false;
1034 }
1035 
1036 void VisibilityConstraint::print(std::ostream& out) const
1037 {
1038  if (enabled())
1039  {
1040  out << "Visibility constraint for sensor in frame '" << sensor_frame_id_ << "' using target in frame '"
1041  << target_frame_id_ << "'" << std::endl;
1042  out << "Target radius: " << target_radius_ << ", using " << cone_sides_ << " sides." << std::endl;
1043  }
1044  else
1045  out << "No constraint" << std::endl;
1046 }
1047 
1049 {
1050  all_constraints_ = moveit_msgs::Constraints();
1051  kinematic_constraints_.clear();
1052  joint_constraints_.clear();
1053  position_constraints_.clear();
1054  orientation_constraints_.clear();
1055  visibility_constraints_.clear();
1056 }
1057 
1058 bool KinematicConstraintSet::add(const std::vector<moveit_msgs::JointConstraint>& jc)
1059 {
1060  bool result = true;
1061  for (unsigned int i = 0; i < jc.size(); ++i)
1062  {
1064  bool u = ev->configure(jc[i]);
1065  result = result && u;
1066  kinematic_constraints_.push_back(KinematicConstraintPtr(ev));
1067  joint_constraints_.push_back(jc[i]);
1068  all_constraints_.joint_constraints.push_back(jc[i]);
1069  }
1070  return result;
1071 }
1072 
1073 bool KinematicConstraintSet::add(const std::vector<moveit_msgs::PositionConstraint>& pc,
1074  const robot_state::Transforms& tf)
1075 {
1076  bool result = true;
1077  for (unsigned int i = 0; i < pc.size(); ++i)
1078  {
1080  bool u = ev->configure(pc[i], tf);
1081  result = result && u;
1082  kinematic_constraints_.push_back(KinematicConstraintPtr(ev));
1083  position_constraints_.push_back(pc[i]);
1084  all_constraints_.position_constraints.push_back(pc[i]);
1085  }
1086  return result;
1087 }
1088 
1089 bool KinematicConstraintSet::add(const std::vector<moveit_msgs::OrientationConstraint>& oc,
1090  const robot_state::Transforms& tf)
1091 {
1092  bool result = true;
1093  for (unsigned int i = 0; i < oc.size(); ++i)
1094  {
1096  bool u = ev->configure(oc[i], tf);
1097  result = result && u;
1098  kinematic_constraints_.push_back(KinematicConstraintPtr(ev));
1099  orientation_constraints_.push_back(oc[i]);
1100  all_constraints_.orientation_constraints.push_back(oc[i]);
1101  }
1102  return result;
1103 }
1104 
1105 bool KinematicConstraintSet::add(const std::vector<moveit_msgs::VisibilityConstraint>& vc,
1106  const robot_state::Transforms& tf)
1107 {
1108  bool result = true;
1109  for (unsigned int i = 0; i < vc.size(); ++i)
1110  {
1112  bool u = ev->configure(vc[i], tf);
1113  result = result && u;
1114  kinematic_constraints_.push_back(KinematicConstraintPtr(ev));
1115  visibility_constraints_.push_back(vc[i]);
1116  all_constraints_.visibility_constraints.push_back(vc[i]);
1117  }
1118  return result;
1119 }
1120 
1121 bool KinematicConstraintSet::add(const moveit_msgs::Constraints& c, const robot_state::Transforms& tf)
1122 {
1123  bool j = add(c.joint_constraints);
1124  bool p = add(c.position_constraints, tf);
1125  bool o = add(c.orientation_constraints, tf);
1126  bool v = add(c.visibility_constraints, tf);
1127  return j && p && o && v;
1128 }
1129 
1131 {
1132  ConstraintEvaluationResult res(true, 0.0);
1133  for (unsigned int i = 0; i < kinematic_constraints_.size(); ++i)
1134  {
1135  ConstraintEvaluationResult r = kinematic_constraints_[i]->decide(state, verbose);
1136  if (!r.satisfied)
1137  res.satisfied = false;
1138  res.distance += r.distance;
1139  }
1140  return res;
1141 }
1142 
1144  std::vector<ConstraintEvaluationResult>& results,
1145  bool verbose) const
1146 {
1147  ConstraintEvaluationResult result(true, 0.0);
1148  results.resize(kinematic_constraints_.size());
1149  for (std::size_t i = 0; i < kinematic_constraints_.size(); ++i)
1150  {
1151  results[i] = kinematic_constraints_[i]->decide(state, verbose);
1152  result.satisfied = result.satisfied && results[i].satisfied;
1153  result.distance += results[i].distance;
1154  }
1155 
1156  return result;
1157 }
1158 
1159 void KinematicConstraintSet::print(std::ostream& out) const
1160 {
1161  out << kinematic_constraints_.size() << " kinematic constraints" << std::endl;
1162  for (unsigned int i = 0; i < kinematic_constraints_.size(); ++i)
1163  kinematic_constraints_[i]->print(out);
1164 }
1165 
1166 bool KinematicConstraintSet::equal(const KinematicConstraintSet& other, double margin) const
1167 {
1168  // each constraint in this matches some in the other
1169  for (unsigned int i = 0; i < kinematic_constraints_.size(); ++i)
1170  {
1171  bool found = false;
1172  for (unsigned int j = 0; !found && j < other.kinematic_constraints_.size(); ++j)
1173  found = kinematic_constraints_[i]->equal(*other.kinematic_constraints_[j], margin);
1174  if (!found)
1175  return false;
1176  }
1177  // each constraint in the other matches some constraint in this
1178  for (unsigned int i = 0; i < other.kinematic_constraints_.size(); ++i)
1179  {
1180  bool found = false;
1181  for (unsigned int j = 0; !found && j < kinematic_constraints_.size(); ++j)
1182  found = other.kinematic_constraints_[i]->equal(*kinematic_constraints_[j], margin);
1183  if (!found)
1184  return false;
1185  }
1186  return true;
1187 }
1188 
1189 } // 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:1375
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 Wed Jul 18 2018 02:48:31