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


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Thu Jan 14 2021 03:57:44