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


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Jan 15 2018 03:50:44