kinematic_constraint.h
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 
37 #pragma once
38 
44 
46 #include <moveit_msgs/Constraints.h>
47 
48 #include <iostream>
49 #include <vector>
50 
53 {
56 {
65  ConstraintEvaluationResult(bool result_satisfied = false, double dist = 0.0)
66  : satisfied(result_satisfied), distance(dist)
67  {
68  }
69 
70  bool satisfied;
71  double distance;
72 };
73 
74 MOVEIT_CLASS_FORWARD(KinematicConstraint); // Defines KinematicConstraintPtr, ConstPtr, WeakPtr... etc
75 
78 {
79 public:
81  enum ConstraintType
82  {
88  };
89 
95  KinematicConstraint(const moveit::core::RobotModelConstPtr& model);
96  virtual ~KinematicConstraint();
97 
99  virtual void clear() = 0;
100 
109  virtual ConstraintEvaluationResult decide(const moveit::core::RobotState& state, bool verbose = false) const = 0;
110 
116  virtual bool enabled() const = 0;
117 
129  virtual bool equal(const KinematicConstraint& other, double margin) const = 0;
130 
137  ConstraintType getType() const
138  {
139  return type_;
140  }
141 
147  virtual void print(std::ostream& /*unused*/ = std::cout) const
148  {
149  }
150 
158  double getConstraintWeight() const
159  {
160  return constraint_weight_;
161  }
162 
169  const moveit::core::RobotModelConstPtr& getRobotModel() const
170  {
171  return robot_model_;
172  }
173 
174 protected:
176  moveit::core::RobotModelConstPtr robot_model_;
177  double constraint_weight_;
179 };
180 
181 MOVEIT_CLASS_FORWARD(JointConstraint); // Defines JointConstraintPtr, ConstPtr, WeakPtr... etc
182 
203 {
204 public:
210  JointConstraint(const moveit::core::RobotModelConstPtr& model)
212  {
214  }
215 
229  bool configure(const moveit_msgs::JointConstraint& jc);
230 
246  bool equal(const KinematicConstraint& other, double margin) const override;
247 
248  ConstraintEvaluationResult decide(const moveit::core::RobotState& state, bool verbose = false) const override;
249  bool enabled() const override;
250  void clear() override;
251  void print(std::ostream& out = std::cout) const override;
252 
259  {
260  return joint_model_;
261  }
270  const std::string& getLocalVariableName() const
271  {
272  return local_variable_name_;
273  }
274 
282  const std::string& getJointVariableName() const
283  {
284  return joint_variable_name_;
285  }
286 
287  int getJointVariableIndex() const
288  {
289  return joint_variable_index_;
290  }
291 
298  double getDesiredJointPosition() const
299  {
300  return joint_position_;
301  }
302 
309  double getJointToleranceAbove() const
310  {
311  return joint_tolerance_above_;
312  }
313 
320  double getJointToleranceBelow() const
321  {
322  return joint_tolerance_below_;
323  }
324 
325 protected:
327  bool joint_is_continuous_;
328  std::string local_variable_name_;
329  std::string joint_variable_name_;
332 };
333 
334 MOVEIT_CLASS_FORWARD(OrientationConstraint); // Defines OrientationConstraintPtr, ConstPtr, WeakPtr... etc
335 
348 {
349 public:
351 
352 public:
358  OrientationConstraint(const moveit::core::RobotModelConstPtr& model)
359  : KinematicConstraint(model), link_model_(nullptr)
360  {
362  }
363 
377  bool configure(const moveit_msgs::OrientationConstraint& oc, const moveit::core::Transforms& tf);
378 
395  bool equal(const KinematicConstraint& other, double margin) const override;
396 
397  void clear() override;
398  ConstraintEvaluationResult decide(const moveit::core::RobotState& state, bool verbose = false) const override;
399  bool enabled() const override;
400  void print(std::ostream& out = std::cout) const override;
401 
409  {
410  return link_model_;
411  }
412 
419  const std::string& getReferenceFrame() const
420  {
422  }
423 
430  bool mobileReferenceFrame() const
431  {
432  return mobile_frame_;
433  }
434 
442  const Eigen::Matrix3d& getDesiredRotationMatrix() const
443  {
444  // validity of the rotation matrix is enforced in configure()
446  }
447 
454  double getXAxisTolerance() const
455  {
457  }
458 
465  double getYAxisTolerance() const
466  {
468  }
469 
476  double getZAxisTolerance() const
477  {
479  }
480 
481  int getParameterizationType() const
482  {
483  return parameterization_type_;
484  }
485 
486 protected:
488  Eigen::Matrix3d desired_rotation_matrix_;
490  Eigen::Matrix3d desired_rotation_matrix_inv_;
492  std::string desired_rotation_frame_id_;
493  bool mobile_frame_;
497 };
498 
499 MOVEIT_CLASS_FORWARD(PositionConstraint); // Defines PositionConstraintPtr, ConstPtr, WeakPtr... etc
500 
515 {
516 public:
518 
519 public:
525  PositionConstraint(const moveit::core::RobotModelConstPtr& model) : KinematicConstraint(model), link_model_(nullptr)
526  {
528  }
529 
546  bool configure(const moveit_msgs::PositionConstraint& pc, const moveit::core::Transforms& tf);
547 
571  bool equal(const KinematicConstraint& other, double margin) const override;
572 
573  void clear() override;
574  ConstraintEvaluationResult decide(const moveit::core::RobotState& state, bool verbose = false) const override;
575  bool enabled() const override;
576  void print(std::ostream& out = std::cout) const override;
577 
585  {
586  return link_model_;
587  }
588 
595  const Eigen::Vector3d& getLinkOffset() const
596  {
597  return offset_;
598  }
599 
607  bool hasLinkOffset() const
608  {
609  if (!enabled())
610  return false;
611  return has_offset_;
612  }
613 
620  const std::vector<bodies::BodyPtr>& getConstraintRegions() const
621  {
622  return constraint_region_;
623  }
624 
631  const std::string& getReferenceFrame() const
632  {
633  return constraint_frame_id_;
634  }
635 
643  bool mobileReferenceFrame() const
644  {
645  if (!enabled())
646  return false;
647  return mobile_frame_;
648  }
649 
650 protected:
652  bool has_offset_;
653  std::vector<bodies::BodyPtr> constraint_region_;
655  EigenSTL::vector_Isometry3d constraint_region_pose_;
656  bool mobile_frame_;
657  std::string constraint_frame_id_;
659 };
660 
661 MOVEIT_CLASS_FORWARD(VisibilityConstraint); // Defines VisibilityConstraintPtr, ConstPtr, WeakPtr... etc
662 
761 {
762 public:
764 
765 public:
771  VisibilityConstraint(const moveit::core::RobotModelConstPtr& model);
772 
785  bool configure(const moveit_msgs::VisibilityConstraint& vc, const moveit::core::Transforms& tf);
786 
804  bool equal(const KinematicConstraint& other, double margin) const override;
805  void clear() override;
806 
815 
827  void getMarkers(const moveit::core::RobotState& state, visualization_msgs::MarkerArray& markers) const;
828 
829  bool enabled() const override;
830  ConstraintEvaluationResult decide(const moveit::core::RobotState& state, bool verbose = false) const override;
831  void print(std::ostream& out = std::cout) const override;
832 
833 protected:
843  bool decideContact(const collision_detection::Contact& contact) const;
844 
845  collision_detection::CollisionEnvPtr collision_env_;
847  bool mobile_sensor_frame_;
848  bool mobile_target_frame_;
849  std::string target_frame_id_;
850  std::string sensor_frame_id_;
851  Eigen::Isometry3d sensor_pose_;
853  Eigen::Isometry3d target_pose_;
854  unsigned int cone_sides_;
855  EigenSTL::vector_Vector3d points_;
856  double target_radius_;
857  double max_view_angle_;
858  double max_range_angle_;
859 };
860 
861 MOVEIT_CLASS_FORWARD(KinematicConstraintSet); // Defines KinematicConstraintSetPtr, ConstPtr, WeakPtr... etc
862 
871 class KinematicConstraintSet
872 {
873 public:
875 
876 public:
882  KinematicConstraintSet(const moveit::core::RobotModelConstPtr& model) : robot_model_(model)
883  {
884  }
885 
887  {
888  clear();
889  }
890 
892  void clear();
893 
904  bool add(const moveit_msgs::Constraints& c, const moveit::core::Transforms& tf);
905 
913  bool add(const std::vector<moveit_msgs::JointConstraint>& jc);
914 
922  bool add(const std::vector<moveit_msgs::PositionConstraint>& pc, const moveit::core::Transforms& tf);
923 
931  bool add(const std::vector<moveit_msgs::OrientationConstraint>& oc, const moveit::core::Transforms& tf);
932 
940  bool add(const std::vector<moveit_msgs::VisibilityConstraint>& vc, const moveit::core::Transforms& tf);
941 
953  ConstraintEvaluationResult decide(const moveit::core::RobotState& state, bool verbose = false) const;
954 
974  std::vector<ConstraintEvaluationResult>& results, bool verbose = false) const;
975 
991  bool equal(const KinematicConstraintSet& other, double margin) const;
992 
998  void print(std::ostream& out = std::cout) const;
999 
1006  const std::vector<moveit_msgs::PositionConstraint>& getPositionConstraints() const
1007  {
1008  return position_constraints_;
1009  }
1010 
1017  const std::vector<moveit_msgs::OrientationConstraint>& getOrientationConstraints() const
1018  {
1019  return orientation_constraints_;
1020  }
1021 
1028  const std::vector<moveit_msgs::JointConstraint>& getJointConstraints() const
1029  {
1030  return joint_constraints_;
1031  }
1032 
1039  const std::vector<moveit_msgs::VisibilityConstraint>& getVisibilityConstraints() const
1040  {
1041  return visibility_constraints_;
1042  }
1043 
1050  const moveit_msgs::Constraints& getAllConstraints() const
1051  {
1052  return all_constraints_;
1053  }
1054 
1061  bool empty() const
1062  {
1063  return kinematic_constraints_.empty();
1064  }
1065 
1066 protected:
1067  moveit::core::RobotModelConstPtr robot_model_;
1068  std::vector<KinematicConstraintPtr>
1071  std::vector<moveit_msgs::JointConstraint> joint_constraints_;
1073  std::vector<moveit_msgs::PositionConstraint> position_constraints_;
1075  std::vector<moveit_msgs::OrientationConstraint> orientation_constraints_;
1077  std::vector<moveit_msgs::VisibilityConstraint> visibility_constraints_;
1079  moveit_msgs::Constraints all_constraints_;
1080 };
1081 } // namespace kinematic_constraints
kinematic_constraints::OrientationConstraint::decide
ConstraintEvaluationResult decide(const moveit::core::RobotState &state, bool verbose=false) const override
Decide whether the constraint is satisfied in the indicated state.
Definition: kinematic_constraint.cpp:698
moveit::core::LinkModel
A link from the robot. Contains the constant transform applied to the link and its geometry.
Definition: link_model.h:71
kinematic_constraints::KinematicConstraint::POSITION_CONSTRAINT
@ POSITION_CONSTRAINT
Definition: kinematic_constraint.h:117
kinematic_constraints::KinematicConstraint::robot_model_
moveit::core::RobotModelConstPtr robot_model_
The kinematic model associated with this constraint.
Definition: kinematic_constraint.h:208
kinematic_constraints::KinematicConstraintSet::robot_model_
moveit::core::RobotModelConstPtr robot_model_
The kinematic model used for by the Set.
Definition: kinematic_constraint.h:1099
kinematic_constraints::OrientationConstraint::getDesiredRotationMatrix
const Eigen::Matrix3d & getDesiredRotationMatrix() const
The rotation target in the reference frame.
Definition: kinematic_constraint.h:474
kinematic_constraints::JointConstraint::getJointVariableIndex
int getJointVariableIndex() const
Definition: kinematic_constraint.h:319
kinematic_constraints::JointConstraint::enabled
bool enabled() const override
This function returns true if this constraint is configured and able to decide whether states do meet...
Definition: kinematic_constraint.cpp:329
kinematic_constraints::VisibilityConstraint::max_range_angle_
double max_range_angle_
Storage for the max range angle.
Definition: kinematic_constraint.h:890
kinematic_constraints::OrientationConstraint::getZAxisTolerance
double getZAxisTolerance() const
Gets the Z axis tolerance.
Definition: kinematic_constraint.h:508
kinematic_constraints::JointConstraint::joint_tolerance_above_
double joint_tolerance_above_
Definition: kinematic_constraint.h:363
kinematic_constraints::ConstraintEvaluationResult::ConstraintEvaluationResult
ConstraintEvaluationResult(bool result_satisfied=false, double dist=0.0)
Constructor.
Definition: kinematic_constraint.h:129
kinematic_constraints::KinematicConstraintSet::getJointConstraints
const std::vector< moveit_msgs::JointConstraint > & getJointConstraints() const
Get all joint constraints in the set.
Definition: kinematic_constraint.h:1060
kinematic_constraints::KinematicConstraint::enabled
virtual bool enabled() const =0
This function returns true if this constraint is configured and able to decide whether states do meet...
kinematic_constraints::KinematicConstraintSet::equal
bool equal(const KinematicConstraintSet &other, double margin) const
Whether or not another KinematicConstraintSet is equal to this one.
Definition: kinematic_constraint.cpp:1320
kinematic_constraints::VisibilityConstraint::target_radius_
double target_radius_
Storage for the target radius.
Definition: kinematic_constraint.h:888
kinematic_constraints::OrientationConstraint::mobileReferenceFrame
bool mobileReferenceFrame() const
Whether or not a mobile reference frame is being employed.
Definition: kinematic_constraint.h:462
kinematic_constraints::KinematicConstraintSet::all_constraints_
moveit_msgs::Constraints all_constraints_
Messages corresponding to all internal constraints.
Definition: kinematic_constraint.h:1111
kinematic_constraints::OrientationConstraint::absolute_z_axis_tolerance_
double absolute_z_axis_tolerance_
Storage for the tolerances.
Definition: kinematic_constraint.h:528
kinematic_constraints::PositionConstraint::getReferenceFrame
const std::string & getReferenceFrame() const
Returns the reference frame.
Definition: kinematic_constraint.h:663
kinematic_constraints::JointConstraint::joint_variable_index_
int joint_variable_index_
The index of the joint variable name in the full robot state.
Definition: kinematic_constraint.h:362
kinematic_constraints::KinematicConstraintSet::getOrientationConstraints
const std::vector< moveit_msgs::OrientationConstraint > & getOrientationConstraints() const
Get all orientation constraints in the set.
Definition: kinematic_constraint.h:1049
kinematic_constraints::PositionConstraint::constraint_frame_id_
std::string constraint_frame_id_
The constraint frame id.
Definition: kinematic_constraint.h:689
kinematic_constraints
Representation and evaluation of kinematic constraints.
Definition: kinematic_constraint.h:52
kinematic_constraints::JointConstraint::getLocalVariableName
const std::string & getLocalVariableName() const
Gets the local variable name if this constraint was configured for a part of a multi-DOF joint.
Definition: kinematic_constraint.h:302
kinematic_constraints::KinematicConstraintSet::decide
ConstraintEvaluationResult decide(const moveit::core::RobotState &state, bool verbose=false) const
Determines whether all constraints are satisfied by state, returning a single evaluation result.
Definition: kinematic_constraint.cpp:1284
kinematic_constraints::VisibilityConstraint::mobile_target_frame_
bool mobile_target_frame_
True if the target is a non-fixed frame relative to the transform frame.
Definition: kinematic_constraint.h:880
kinematic_constraints::OrientationConstraint::equal
bool equal(const KinematicConstraint &other, double margin) const override
Check if two orientation constraints are the same.
Definition: kinematic_constraint.cpp:665
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
ROS/KDL based interface for the inverse kinematics of the PR2 arm.
kinematic_constraints::JointConstraint::equal
bool equal(const KinematicConstraint &other, double margin) const override
Check if two joint constraints are the same.
Definition: kinematic_constraint.cpp:284
kinematic_constraints::PositionConstraint::hasLinkOffset
bool hasLinkOffset() const
If the constraint is enabled and the link offset is substantially different than zero.
Definition: kinematic_constraint.h:639
kinematic_constraints::JointConstraint::decide
ConstraintEvaluationResult decide(const moveit::core::RobotState &state, bool verbose=false) const override
Decide whether the constraint is satisfied in the indicated state.
Definition: kinematic_constraint.cpp:296
kinematic_constraints::VisibilityConstraint::decideContact
bool decideContact(const collision_detection::Contact &contact) const
Function that gets passed into collision checking to allow some collisions.
Definition: kinematic_constraint.cpp:1166
class_forward.h
kinematic_constraints::KinematicConstraintSet::orientation_constraints_
std::vector< moveit_msgs::OrientationConstraint > orientation_constraints_
Messages corresponding to all internal orientation constraints.
Definition: kinematic_constraint.h:1107
kinematic_constraints::JointConstraint::configure
bool configure(const moveit_msgs::JointConstraint &jc)
Configure the constraint based on a moveit_msgs::JointConstraint.
Definition: kinematic_constraint.cpp:152
kinematic_constraints::JointConstraint::getJointToleranceAbove
double getJointToleranceAbove() const
Gets the upper tolerance component of the joint constraint.
Definition: kinematic_constraint.h:341
kinematic_constraints::PositionConstraint::print
void print(std::ostream &out=std::cout) const override
Print the constraint data.
Definition: kinematic_constraint.cpp:561
moveit::core::RobotState
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:155
kinematic_constraints::JointConstraint::joint_variable_name_
std::string joint_variable_name_
The joint variable name.
Definition: kinematic_constraint.h:361
kinematic_constraints::ConstraintEvaluationResult::satisfied
bool satisfied
Whether or not the constraint or constraints were satisfied.
Definition: kinematic_constraint.h:134
kinematic_constraints::KinematicConstraintSet::position_constraints_
std::vector< moveit_msgs::PositionConstraint > position_constraints_
Messages corresponding to all internal position constraints.
Definition: kinematic_constraint.h:1105
shapes::Mesh
collision_detection::Contact
Definition of a contact point.
Definition: include/moveit/collision_detection/collision_common.h:105
kinematic_constraints::JointConstraint::getJointVariableName
const std::string & getJointVariableName() const
Gets the joint variable name, as known to the moveit::core::RobotModel.
Definition: kinematic_constraint.h:314
robot_model.h
kinematic_constraints::VisibilityConstraint::target_pose_
Eigen::Isometry3d target_pose_
The target pose transformed into the transform frame.
Definition: kinematic_constraint.h:885
kinematic_constraints::JointConstraint::local_variable_name_
std::string local_variable_name_
The local variable name for a multi DOF joint, if any.
Definition: kinematic_constraint.h:360
kinematic_constraints::PositionConstraint::equal
bool equal(const KinematicConstraint &other, double margin) const override
Check if two constraints are the same. For position constraints this means that:
Definition: kinematic_constraint.cpp:466
kinematic_constraints::VisibilityConstraint::cone_sides_
unsigned int cone_sides_
Storage for the cone sides
Definition: kinematic_constraint.h:886
kinematic_constraints::VisibilityConstraint::sensor_pose_
Eigen::Isometry3d sensor_pose_
The sensor pose transformed into the transform frame.
Definition: kinematic_constraint.h:883
kinematic_constraints::KinematicConstraintSet::kinematic_constraints_
std::vector< KinematicConstraintPtr > kinematic_constraints_
Shared pointers to all the member constraints.
Definition: kinematic_constraint.h:1101
transforms.h
kinematic_constraints::VisibilityConstraint::configure
bool configure(const moveit_msgs::VisibilityConstraint &vc, const moveit::core::Transforms &tf)
Configure the constraint based on a moveit_msgs::VisibilityConstraint.
Definition: kinematic_constraint.cpp:809
kinematic_constraints::JointConstraint::JointConstraint
JointConstraint(const moveit::core::RobotModelConstPtr &model)
Constructor.
Definition: kinematic_constraint.h:242
kinematic_constraints::OrientationConstraint::getParameterizationType
int getParameterizationType() const
Definition: kinematic_constraint.h:513
kinematic_constraints::KinematicConstraint::JOINT_CONSTRAINT
@ JOINT_CONSTRAINT
Definition: kinematic_constraint.h:116
kinematic_constraints::VisibilityConstraint::VisibilityConstraint
VisibilityConstraint(const moveit::core::RobotModelConstPtr &model)
Constructor.
Definition: kinematic_constraint.cpp:787
kinematic_constraints::OrientationConstraint::absolute_x_axis_tolerance_
double absolute_x_axis_tolerance_
Definition: kinematic_constraint.h:527
kinematic_constraints::OrientationConstraint::desired_rotation_matrix_
Eigen::Matrix3d desired_rotation_matrix_
The desired rotation matrix in the tf frame. Guaranteed to be valid rotation matrix.
Definition: kinematic_constraint.h:520
kinematic_constraints::ConstraintEvaluationResult::distance
double distance
The distance evaluation from the constraint or constraints.
Definition: kinematic_constraint.h:135
kinematic_constraints::KinematicConstraint::type_
ConstraintType type_
The type of the constraint.
Definition: kinematic_constraint.h:207
kinematic_constraints::PositionConstraint::enabled
bool enabled() const override
This function returns true if this constraint is configured and able to decide whether states do meet...
Definition: kinematic_constraint.cpp:580
kinematic_constraints::PositionConstraint::getConstraintRegions
const std::vector< bodies::BodyPtr > & getConstraintRegions() const
Returns all the constraint regions.
Definition: kinematic_constraint.h:652
kinematic_constraints::OrientationConstraint::desired_rotation_matrix_inv_
Eigen::Matrix3d desired_rotation_matrix_inv_
The inverse of the desired rotation matrix, precomputed for efficiency. Guaranteed to be valid rotati...
Definition: kinematic_constraint.h:522
kinematic_constraints::KinematicConstraint::print
virtual void print(std::ostream &=std::cout) const
Print the constraint data.
Definition: kinematic_constraint.h:179
kinematic_constraints::JointConstraint::joint_model_
const moveit::core::JointModel * joint_model_
The joint from the kinematic model for this constraint.
Definition: kinematic_constraint.h:358
kinematic_constraints::KinematicConstraintSet::getPositionConstraints
const std::vector< moveit_msgs::PositionConstraint > & getPositionConstraints() const
Get all position constraints in the set.
Definition: kinematic_constraint.h:1038
kinematic_constraints::VisibilityConstraint::enabled
bool enabled() const override
This function returns true if this constraint is configured and able to decide whether states do meet...
Definition: kinematic_constraint.cpp:917
kinematic_constraints::VisibilityConstraint::max_view_angle_
double max_view_angle_
Storage for the max view angle.
Definition: kinematic_constraint.h:889
kinematic_constraints::KinematicConstraintSet::add
bool add(const moveit_msgs::Constraints &c, const moveit::core::Transforms &tf)
Add all known constraints.
Definition: kinematic_constraint.cpp:1275
kinematic_constraints::PositionConstraint::getLinkModel
const moveit::core::LinkModel * getLinkModel() const
Returns the associated link model, or NULL if not enabled.
Definition: kinematic_constraint.h:616
kinematic_constraints::KinematicConstraint::UNKNOWN_CONSTRAINT
@ UNKNOWN_CONSTRAINT
Definition: kinematic_constraint.h:115
kinematic_constraints::KinematicConstraint::getRobotModel
const moveit::core::RobotModelConstPtr & getRobotModel() const
Definition: kinematic_constraint.h:201
collision_env.h
kinematic_constraints::JointConstraint::joint_tolerance_below_
double joint_tolerance_below_
Position and tolerance values.
Definition: kinematic_constraint.h:363
kinematic_constraints::PositionConstraint::decide
ConstraintEvaluationResult decide(const moveit::core::RobotState &state, bool verbose=false) const override
Decide whether the constraint is satisfied in the indicated state.
Definition: kinematic_constraint.cpp:525
kinematic_constraints::PositionConstraint::clear
void clear() override
Clear the stored constraint.
Definition: kinematic_constraint.cpp:569
kinematic_constraints::VisibilityConstraint::clear
void clear() override
Clear the stored constraint.
Definition: kinematic_constraint.cpp:793
kinematic_constraints::PositionConstraint::getLinkOffset
const Eigen::Vector3d & getLinkOffset() const
Returns the target offset.
Definition: kinematic_constraint.h:627
kinematic_constraints::OrientationConstraint::print
void print(std::ostream &out=std::cout) const override
Print the constraint data.
Definition: kinematic_constraint.cpp:775
kinematic_constraints::OrientationConstraint::desired_rotation_frame_id_
std::string desired_rotation_frame_id_
The target frame of the transform tree.
Definition: kinematic_constraint.h:524
kinematic_constraints::KinematicConstraintSet::clear
void clear()
Clear the stored constraints.
Definition: kinematic_constraint.cpp:1202
kinematic_constraints::PositionConstraint::link_model_
const moveit::core::LinkModel * link_model_
The link model constraint subject.
Definition: kinematic_constraint.h:690
kinematic_constraints::VisibilityConstraint::points_
EigenSTL::vector_Vector3d points_
A set of points along the base of the circle.
Definition: kinematic_constraint.h:887
kinematic_constraints::VisibilityConstraint::print
void print(std::ostream &out=std::cout) const override
Print the constraint data.
Definition: kinematic_constraint.cpp:1190
kinematic_constraints::KinematicConstraintSet::~KinematicConstraintSet
~KinematicConstraintSet()
Definition: kinematic_constraint.h:918
kinematic_constraints::PositionConstraint::configure
bool configure(const moveit_msgs::PositionConstraint &pc, const moveit::core::Transforms &tf)
Configure the constraint based on a moveit_msgs::PositionConstraint.
Definition: kinematic_constraint.cpp:361
kinematic_constraints::JointConstraint::clear
void clear() override
Clear the stored constraint.
Definition: kinematic_constraint.cpp:334
kinematic_constraints::KinematicConstraint::equal
virtual bool equal(const KinematicConstraint &other, double margin) const =0
Check if two constraints are the same. This means that the types are the same, the subject of the con...
kinematic_constraints::KinematicConstraintSet
A class that contains many different constraints, and can check RobotState *versus the full set....
Definition: kinematic_constraint.h:903
kinematic_constraints::KinematicConstraint::decide
virtual ConstraintEvaluationResult decide(const moveit::core::RobotState &state, bool verbose=false) const =0
Decide whether the constraint is satisfied in the indicated state.
kinematic_constraints::OrientationConstraint
Class for constraints on the orientation of a link.
Definition: kinematic_constraint.h:379
kinematic_constraints::JointConstraint::joint_is_continuous_
bool joint_is_continuous_
Whether or not the joint is continuous.
Definition: kinematic_constraint.h:359
kinematic_constraints::VisibilityConstraint::equal
bool equal(const KinematicConstraint &other, double margin) const override
Check if two constraints are the same.
Definition: kinematic_constraint.cpp:888
kinematic_constraints::VisibilityConstraint
Class for constraints on the visibility relationship between a sensor and a target.
Definition: kinematic_constraint.h:792
kinematic_constraints::KinematicConstraint::getType
ConstraintType getType() const
Get the type of constraint.
Definition: kinematic_constraint.h:169
kinematic_constraints::OrientationConstraint::getXAxisTolerance
double getXAxisTolerance() const
Gets the X axis tolerance.
Definition: kinematic_constraint.h:486
kinematic_constraints::MOVEIT_CLASS_FORWARD
MOVEIT_CLASS_FORWARD(KinematicConstraint)
kinematic_constraints::VisibilityConstraint::sensor_frame_id_
std::string sensor_frame_id_
The sensor frame id.
Definition: kinematic_constraint.h:882
kinematic_constraints::KinematicConstraintSet::empty
bool empty() const
Returns whether or not there are any constraints in the set.
Definition: kinematic_constraint.h:1093
kinematic_constraints::VisibilityConstraint::decide
ConstraintEvaluationResult decide(const moveit::core::RobotState &state, bool verbose=false) const override
Decide whether the constraint is satisfied in the indicated state.
Definition: kinematic_constraint.cpp:1071
kinematic_constraints::KinematicConstraint
Base class for representing a kinematic constraint.
Definition: kinematic_constraint.h:109
kinematic_constraints::OrientationConstraint::OrientationConstraint
OrientationConstraint(const moveit::core::RobotModelConstPtr &model)
Constructor.
Definition: kinematic_constraint.h:390
kinematic_constraints::PositionConstraint::mobile_frame_
bool mobile_frame_
Whether or not a mobile frame is employed.
Definition: kinematic_constraint.h:688
kinematic_constraints::KinematicConstraint::clear
virtual void clear()=0
Clear the stored constraint.
moveit::core::Transforms
Provides an implementation of a snapshot of a transform tree that can be easily queried for transform...
Definition: transforms.h:122
kinematic_constraints::PositionConstraint::constraint_region_pose_
EigenSTL::vector_Isometry3d constraint_region_pose_
The constraint region pose vector. All isometries are guaranteed to be valid.
Definition: kinematic_constraint.h:687
kinematic_constraints::OrientationConstraint::link_model_
const moveit::core::LinkModel * link_model_
The target link model.
Definition: kinematic_constraint.h:519
kinematic_constraints::OrientationConstraint::mobile_frame_
bool mobile_frame_
Whether or not the header frame is mobile or fixed.
Definition: kinematic_constraint.h:525
kinematic_constraints::VisibilityConstraint::mobile_sensor_frame_
bool mobile_sensor_frame_
True if the sensor is a non-fixed frame relative to the transform frame.
Definition: kinematic_constraint.h:879
kinematic_constraints::KinematicConstraint::KinematicConstraint
KinematicConstraint(const moveit::core::RobotModelConstPtr &model)
Constructor.
Definition: kinematic_constraint.cpp:145
kinematic_constraints::KinematicConstraint::~KinematicConstraint
virtual ~KinematicConstraint()
kinematic_constraints::KinematicConstraint::ORIENTATION_CONSTRAINT
@ ORIENTATION_CONSTRAINT
Definition: kinematic_constraint.h:118
kinematic_constraints::PositionConstraint::offset_
Eigen::Vector3d offset_
The target offset.
Definition: kinematic_constraint.h:683
kinematic_constraints::JointConstraint::print
void print(std::ostream &out=std::cout) const override
Print the constraint data.
Definition: kinematic_constraint.cpp:344
kinematic_constraints::KinematicConstraintSet::visibility_constraints_
std::vector< moveit_msgs::VisibilityConstraint > visibility_constraints_
Messages corresponding to all internal visibility constraints.
Definition: kinematic_constraint.h:1109
kinematic_constraints::KinematicConstraint::getConstraintWeight
double getConstraintWeight() const
The weight of a constraint is a multiplicative factor associated to the distance computed by the deci...
Definition: kinematic_constraint.h:190
kinematic_constraints::VisibilityConstraint::getVisibilityCone
shapes::Mesh * getVisibilityCone(const moveit::core::RobotState &state) const
Gets a trimesh shape representing the visibility cone.
Definition: kinematic_constraint.cpp:922
kinematic_constraints::OrientationConstraint::getYAxisTolerance
double getYAxisTolerance() const
Gets the Y axis tolerance.
Definition: kinematic_constraint.h:497
bodies.h
kinematic_constraints::JointConstraint::getJointToleranceBelow
double getJointToleranceBelow() const
Gets the lower tolerance component of the joint constraint.
Definition: kinematic_constraint.h:352
kinematic_constraints::OrientationConstraint::configure
bool configure(const moveit_msgs::OrientationConstraint &oc, const moveit::core::Transforms &tf)
Configure the constraint based on a moveit_msgs::OrientationConstraint.
Definition: kinematic_constraint.cpp:585
kinematic_constraints::OrientationConstraint::getLinkModel
const moveit::core::LinkModel * getLinkModel() const
Gets the subject link model.
Definition: kinematic_constraint.h:440
kinematic_constraints::KinematicConstraint::constraint_weight_
double constraint_weight_
The weight of a constraint is a multiplicative factor associated to the distance computed by the deci...
Definition: kinematic_constraint.h:209
kinematic_constraints::KinematicConstraintSet::print
void print(std::ostream &out=std::cout) const
Print the constraint data.
Definition: kinematic_constraint.cpp:1313
kinematic_constraints::KinematicConstraintSet::getAllConstraints
const moveit_msgs::Constraints & getAllConstraints() const
Get all constraints in the set.
Definition: kinematic_constraint.h:1082
kinematic_constraints::OrientationConstraint::absolute_y_axis_tolerance_
double absolute_y_axis_tolerance_
Definition: kinematic_constraint.h:527
kinematic_constraints::OrientationConstraint::parameterization_type_
int parameterization_type_
Parameterization type for orientation tolerance.
Definition: kinematic_constraint.h:526
kinematic_constraints::VisibilityConstraint::sensor_view_direction_
int sensor_view_direction_
Storage for the sensor view direction.
Definition: kinematic_constraint.h:884
kinematic_constraints::VisibilityConstraint::getMarkers
void getMarkers(const moveit::core::RobotState &state, visualization_msgs::MarkerArray &markers) const
Adds markers associated with the visibility cone, sensor and target to the visualization array.
Definition: kinematic_constraint.cpp:996
kinematic_constraints::VisibilityConstraint::target_frame_id_
std::string target_frame_id_
The target frame id.
Definition: kinematic_constraint.h:881
kinematic_constraints::PositionConstraint::mobileReferenceFrame
bool mobileReferenceFrame() const
If enabled and the specified frame is a mobile frame, return true. Otherwise, returns false.
Definition: kinematic_constraint.h:675
kinematic_constraints::JointConstraint
Class for handling single DOF joint constraints.
Definition: kinematic_constraint.h:234
kinematic_constraints::OrientationConstraint::getReferenceFrame
const std::string & getReferenceFrame() const
The target frame of the planning_models::Transforms class, for interpreting the rotation frame.
Definition: kinematic_constraint.h:451
kinematic_constraints::KinematicConstraint::VISIBILITY_CONSTRAINT
@ VISIBILITY_CONSTRAINT
Definition: kinematic_constraint.h:119
kinematic_constraints::KinematicConstraint::ConstraintType
ConstraintType
Enum for representing a constraint.
Definition: kinematic_constraint.h:113
kinematic_constraints::JointConstraint::getDesiredJointPosition
double getDesiredJointPosition() const
Gets the desired position component of the constraint.
Definition: kinematic_constraint.h:330
kinematic_constraints::VisibilityConstraint::collision_env_
collision_detection::CollisionEnvPtr collision_env_
A copy of the collision robot maintained for collision checking the cone against robot links.
Definition: kinematic_constraint.h:877
kinematic_constraints::JointConstraint::getJointModel
const moveit::core::JointModel * getJointModel() const
Get the joint model for which this constraint operates.
Definition: kinematic_constraint.h:290
kinematic_constraints::PositionConstraint::constraint_region_
std::vector< bodies::BodyPtr > constraint_region_
The constraint region vector.
Definition: kinematic_constraint.h:685
kinematic_constraints::KinematicConstraintSet::KinematicConstraintSet
KinematicConstraintSet(const moveit::core::RobotModelConstPtr &model)
Constructor.
Definition: kinematic_constraint.h:914
kinematic_constraints::PositionConstraint::has_offset_
bool has_offset_
Whether the offset is substantially different than 0.0.
Definition: kinematic_constraint.h:684
kinematic_constraints::PositionConstraint::PositionConstraint
PositionConstraint(const moveit::core::RobotModelConstPtr &model)
Constructor.
Definition: kinematic_constraint.h:557
kinematic_constraints::OrientationConstraint::clear
void clear() override
Clear the stored constraint.
Definition: kinematic_constraint.cpp:683
kinematic_constraints::KinematicConstraintSet::getVisibilityConstraints
const std::vector< moveit_msgs::VisibilityConstraint > & getVisibilityConstraints() const
Get all visibility constraints in the set.
Definition: kinematic_constraint.h:1071
robot_state.h
kinematic_constraints::KinematicConstraintSet::joint_constraints_
std::vector< moveit_msgs::JointConstraint > joint_constraints_
Messages corresponding to all internal joint constraints.
Definition: kinematic_constraint.h:1103
moveit::core::JointModel
A joint from the robot. Models the transform that this joint applies in the kinematic chain....
Definition: joint_model.h:173
kinematic_constraints::PositionConstraint
Class for constraints on the XYZ position of a link.
Definition: kinematic_constraint.h:546
kinematic_constraints::JointConstraint::joint_position_
double joint_position_
Definition: kinematic_constraint.h:363
fcl::Vector3d
Vec3fX< details::Vec3Data< double > > Vector3d
Definition: fcl_compat.h:89
kinematic_constraints::OrientationConstraint::enabled
bool enabled() const override
This function returns true if this constraint is configured and able to decide whether states do meet...
Definition: kinematic_constraint.cpp:693
kinematic_constraints::ConstraintEvaluationResult
Struct for containing the results of constraint evaluation.
Definition: kinematic_constraint.h:87


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Tue Oct 12 2021 02:24:44