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& out = std::cout) const
148  {
149  (void)out;
150  }
151 
159  double getConstraintWeight() const
160  {
161  return constraint_weight_;
162  }
163 
170  const moveit::core::RobotModelConstPtr& getRobotModel() const
171  {
172  return robot_model_;
173  }
174 
175 protected:
177  moveit::core::RobotModelConstPtr robot_model_;
178  double constraint_weight_;
180 };
181 
182 MOVEIT_CLASS_FORWARD(JointConstraint); // Defines JointConstraintPtr, ConstPtr, WeakPtr... etc
183 
203 class JointConstraint : public KinematicConstraint
204 {
205 public:
211  JointConstraint(const moveit::core::RobotModelConstPtr& model)
213  {
215  }
216 
230  bool configure(const moveit_msgs::JointConstraint& jc);
231 
247  bool equal(const KinematicConstraint& other, double margin) const override;
248 
249  ConstraintEvaluationResult decide(const moveit::core::RobotState& state, bool verbose = false) const override;
250  bool enabled() const override;
251  void clear() override;
252  void print(std::ostream& out = std::cout) const override;
253 
260  {
261  return joint_model_;
262  }
271  const std::string& getLocalVariableName() const
272  {
273  return local_variable_name_;
274  }
275 
283  const std::string& getJointVariableName() const
284  {
285  return joint_variable_name_;
286  }
287 
288  int getJointVariableIndex() const
289  {
290  return joint_variable_index_;
291  }
292 
299  double getDesiredJointPosition() const
300  {
301  return joint_position_;
302  }
303 
310  double getJointToleranceAbove() const
311  {
312  return joint_tolerance_above_;
313  }
314 
321  double getJointToleranceBelow() const
322  {
323  return joint_tolerance_below_;
324  }
325 
326 protected:
328  bool joint_is_continuous_;
329  std::string local_variable_name_;
330  std::string joint_variable_name_;
333 };
334 
335 MOVEIT_CLASS_FORWARD(OrientationConstraint); // Defines OrientationConstraintPtr, ConstPtr, WeakPtr... etc
336 
349 {
350 public:
351  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
352 
353 public:
359  OrientationConstraint(const moveit::core::RobotModelConstPtr& model)
360  : KinematicConstraint(model), link_model_(nullptr)
361  {
363  }
364 
378  bool configure(const moveit_msgs::OrientationConstraint& oc, const moveit::core::Transforms& tf);
379 
396  bool equal(const KinematicConstraint& other, double margin) const override;
397 
398  void clear() override;
399  ConstraintEvaluationResult decide(const moveit::core::RobotState& state, bool verbose = false) const override;
400  bool enabled() const override;
401  void print(std::ostream& out = std::cout) const override;
402 
410  {
411  return link_model_;
412  }
413 
420  const std::string& getReferenceFrame() const
421  {
423  }
424 
431  bool mobileReferenceFrame() const
432  {
433  return mobile_frame_;
434  }
435 
443  const Eigen::Matrix3d& getDesiredRotationMatrix() const
444  {
445  // validity of the rotation matrix is enforced in configure()
447  }
448 
455  double getXAxisTolerance() const
456  {
458  }
459 
466  double getYAxisTolerance() const
467  {
469  }
470 
477  double getZAxisTolerance() const
478  {
480  }
481 
482  int getParameterizationType() const
483  {
484  return parameterization_type_;
485  }
486 
487 protected:
489  Eigen::Matrix3d desired_rotation_matrix_;
491  Eigen::Matrix3d desired_rotation_matrix_inv_;
493  std::string desired_rotation_frame_id_;
494  bool mobile_frame_;
498 };
499 
500 MOVEIT_CLASS_FORWARD(PositionConstraint); // Defines PositionConstraintPtr, ConstPtr, WeakPtr... etc
501 
516 {
517 public:
518  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
519 
520 public:
526  PositionConstraint(const moveit::core::RobotModelConstPtr& model) : KinematicConstraint(model), link_model_(nullptr)
527  {
529  }
530 
547  bool configure(const moveit_msgs::PositionConstraint& pc, const moveit::core::Transforms& tf);
548 
572  bool equal(const KinematicConstraint& other, double margin) const override;
573 
574  void clear() override;
575  ConstraintEvaluationResult decide(const moveit::core::RobotState& state, bool verbose = false) const override;
576  bool enabled() const override;
577  void print(std::ostream& out = std::cout) const override;
578 
586  {
587  return link_model_;
588  }
589 
596  const Eigen::Vector3d& getLinkOffset() const
597  {
598  return offset_;
599  }
600 
608  bool hasLinkOffset() const
609  {
610  if (!enabled())
611  return false;
612  return has_offset_;
613  }
614 
621  const std::vector<bodies::BodyPtr>& getConstraintRegions() const
622  {
623  return constraint_region_;
624  }
625 
632  const std::string& getReferenceFrame() const
633  {
634  return constraint_frame_id_;
635  }
636 
644  bool mobileReferenceFrame() const
645  {
646  if (!enabled())
647  return false;
648  return mobile_frame_;
649  }
650 
651 protected:
653  bool has_offset_;
654  std::vector<bodies::BodyPtr> constraint_region_;
657  bool mobile_frame_;
658  std::string constraint_frame_id_;
660 };
661 
662 MOVEIT_CLASS_FORWARD(VisibilityConstraint); // Defines VisibilityConstraintPtr, ConstPtr, WeakPtr... etc
663 
762 {
763 public:
764  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
765 
766 public:
772  VisibilityConstraint(const moveit::core::RobotModelConstPtr& model);
773 
786  bool configure(const moveit_msgs::VisibilityConstraint& vc, const moveit::core::Transforms& tf);
787 
805  bool equal(const KinematicConstraint& other, double margin) const override;
806  void clear() override;
807 
816 
828  void getMarkers(const moveit::core::RobotState& state, visualization_msgs::MarkerArray& markers) const;
829 
830  bool enabled() const override;
831  ConstraintEvaluationResult decide(const moveit::core::RobotState& state, bool verbose = false) const override;
832  void print(std::ostream& out = std::cout) const override;
833 
834 protected:
844  bool decideContact(const collision_detection::Contact& contact) const;
845 
846  collision_detection::CollisionEnvPtr collision_env_;
848  bool mobile_sensor_frame_;
849  bool mobile_target_frame_;
850  std::string target_frame_id_;
851  std::string sensor_frame_id_;
852  Eigen::Isometry3d sensor_pose_;
854  Eigen::Isometry3d target_pose_;
855  unsigned int cone_sides_;
857  double target_radius_;
858  double max_view_angle_;
859  double max_range_angle_;
860 };
861 
862 MOVEIT_CLASS_FORWARD(KinematicConstraintSet); // Defines KinematicConstraintSetPtr, ConstPtr, WeakPtr... etc
863 
872 class KinematicConstraintSet
873 {
874 public:
875  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
876 
877 public:
883  KinematicConstraintSet(const moveit::core::RobotModelConstPtr& model) : robot_model_(model)
884  {
885  }
886 
888  {
889  clear();
890  }
891 
893  void clear();
894 
905  bool add(const moveit_msgs::Constraints& c, const moveit::core::Transforms& tf);
906 
914  bool add(const std::vector<moveit_msgs::JointConstraint>& jc);
915 
923  bool add(const std::vector<moveit_msgs::PositionConstraint>& pc, const moveit::core::Transforms& tf);
924 
932  bool add(const std::vector<moveit_msgs::OrientationConstraint>& oc, const moveit::core::Transforms& tf);
933 
941  bool add(const std::vector<moveit_msgs::VisibilityConstraint>& vc, const moveit::core::Transforms& tf);
942 
954  ConstraintEvaluationResult decide(const moveit::core::RobotState& state, bool verbose = false) const;
955 
975  std::vector<ConstraintEvaluationResult>& results, bool verbose = false) const;
976 
992  bool equal(const KinematicConstraintSet& other, double margin) const;
993 
999  void print(std::ostream& out = std::cout) const;
1000 
1007  const std::vector<moveit_msgs::PositionConstraint>& getPositionConstraints() const
1008  {
1009  return position_constraints_;
1010  }
1011 
1018  const std::vector<moveit_msgs::OrientationConstraint>& getOrientationConstraints() const
1019  {
1020  return orientation_constraints_;
1021  }
1022 
1029  const std::vector<moveit_msgs::JointConstraint>& getJointConstraints() const
1030  {
1031  return joint_constraints_;
1032  }
1033 
1040  const std::vector<moveit_msgs::VisibilityConstraint>& getVisibilityConstraints() const
1041  {
1042  return visibility_constraints_;
1043  }
1044 
1051  const moveit_msgs::Constraints& getAllConstraints() const
1052  {
1053  return all_constraints_;
1054  }
1055 
1062  bool empty() const
1063  {
1064  return kinematic_constraints_.empty();
1065  }
1066 
1067 protected:
1068  moveit::core::RobotModelConstPtr robot_model_;
1069  std::vector<KinematicConstraintPtr>
1072  std::vector<moveit_msgs::JointConstraint> joint_constraints_;
1074  std::vector<moveit_msgs::PositionConstraint> position_constraints_;
1076  std::vector<moveit_msgs::OrientationConstraint> orientation_constraints_;
1078  std::vector<moveit_msgs::VisibilityConstraint> visibility_constraints_;
1080  moveit_msgs::Constraints all_constraints_;
1081 };
1082 } // 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:209
kinematic_constraints::KinematicConstraintSet::robot_model_
moveit::core::RobotModelConstPtr robot_model_
The kinematic model used for by the Set.
Definition: kinematic_constraint.h:1100
kinematic_constraints::OrientationConstraint::getDesiredRotationMatrix
const Eigen::Matrix3d & getDesiredRotationMatrix() const
The rotation target in the reference frame.
Definition: kinematic_constraint.h:475
kinematic_constraints::JointConstraint::getJointVariableIndex
int getJointVariableIndex() const
Definition: kinematic_constraint.h:320
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:891
kinematic_constraints::OrientationConstraint::getZAxisTolerance
double getZAxisTolerance() const
Gets the Z axis tolerance.
Definition: kinematic_constraint.h:509
EigenSTL::vector_Isometry3d
std::vector< Eigen::Isometry3d, Eigen::aligned_allocator< Eigen::Isometry3d > > vector_Isometry3d
kinematic_constraints::JointConstraint::joint_tolerance_above_
double joint_tolerance_above_
Definition: kinematic_constraint.h:364
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:1061
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:889
kinematic_constraints::OrientationConstraint::mobileReferenceFrame
bool mobileReferenceFrame() const
Whether or not a mobile reference frame is being employed.
Definition: kinematic_constraint.h:463
kinematic_constraints::KinematicConstraintSet::all_constraints_
moveit_msgs::Constraints all_constraints_
Messages corresponding to all internal constraints.
Definition: kinematic_constraint.h:1112
kinematic_constraints::OrientationConstraint::absolute_z_axis_tolerance_
double absolute_z_axis_tolerance_
Storage for the tolerances.
Definition: kinematic_constraint.h:529
kinematic_constraints::PositionConstraint::getReferenceFrame
const std::string & getReferenceFrame() const
Returns the reference frame.
Definition: kinematic_constraint.h:664
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:363
kinematic_constraints::KinematicConstraintSet::getOrientationConstraints
const std::vector< moveit_msgs::OrientationConstraint > & getOrientationConstraints() const
Get all orientation constraints in the set.
Definition: kinematic_constraint.h:1050
kinematic_constraints::PositionConstraint::constraint_frame_id_
std::string constraint_frame_id_
The constraint frame id.
Definition: kinematic_constraint.h:690
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:303
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:881
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
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:640
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:1108
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:342
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:362
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:1106
kinematic_constraints::KinematicConstraint::print
virtual void print(std::ostream &out=std::cout) const
Print the constraint data.
Definition: kinematic_constraint.h:179
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:315
robot_model.h
kinematic_constraints::VisibilityConstraint::target_pose_
Eigen::Isometry3d target_pose_
The target pose transformed into the transform frame.
Definition: kinematic_constraint.h:886
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:361
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:887
kinematic_constraints::VisibilityConstraint::sensor_pose_
Eigen::Isometry3d sensor_pose_
The sensor pose transformed into the transform frame.
Definition: kinematic_constraint.h:884
kinematic_constraints::KinematicConstraintSet::kinematic_constraints_
std::vector< KinematicConstraintPtr > kinematic_constraints_
Shared pointers to all the member constraints.
Definition: kinematic_constraint.h:1102
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:243
kinematic_constraints::OrientationConstraint::getParameterizationType
int getParameterizationType() const
Definition: kinematic_constraint.h:514
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:528
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:521
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:208
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:653
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:523
kinematic_constraints::JointConstraint::joint_model_
const moveit::core::JointModel * joint_model_
The joint from the kinematic model for this constraint.
Definition: kinematic_constraint.h:359
kinematic_constraints::KinematicConstraintSet::getPositionConstraints
const std::vector< moveit_msgs::PositionConstraint > & getPositionConstraints() const
Get all position constraints in the set.
Definition: kinematic_constraint.h:1039
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:890
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:617
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:202
collision_env.h
kinematic_constraints::JointConstraint::joint_tolerance_below_
double joint_tolerance_below_
Position and tolerance values.
Definition: kinematic_constraint.h:364
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
EigenSTL::vector_Vector3d
std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > vector_Vector3d
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:628
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:525
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:691
kinematic_constraints::VisibilityConstraint::points_
EigenSTL::vector_Vector3d points_
A set of points along the base of the circle.
Definition: kinematic_constraint.h:888
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:919
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:904
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:380
kinematic_constraints::JointConstraint::joint_is_continuous_
bool joint_is_continuous_
Whether or not the joint is continuous.
Definition: kinematic_constraint.h:360
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:793
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:487
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:883
kinematic_constraints::KinematicConstraintSet::empty
bool empty() const
Returns whether or not there are any constraints in the set.
Definition: kinematic_constraint.h:1094
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:391
kinematic_constraints::PositionConstraint::mobile_frame_
bool mobile_frame_
Whether or not a mobile frame is employed.
Definition: kinematic_constraint.h:689
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:688
kinematic_constraints::OrientationConstraint::link_model_
const moveit::core::LinkModel * link_model_
The target link model.
Definition: kinematic_constraint.h:520
kinematic_constraints::OrientationConstraint::mobile_frame_
bool mobile_frame_
Whether or not the header frame is mobile or fixed.
Definition: kinematic_constraint.h:526
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:880
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:684
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:1110
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:191
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:498
bodies.h
kinematic_constraints::JointConstraint::getJointToleranceBelow
double getJointToleranceBelow() const
Gets the lower tolerance component of the joint constraint.
Definition: kinematic_constraint.h:353
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:441
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:210
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:1083
kinematic_constraints::OrientationConstraint::absolute_y_axis_tolerance_
double absolute_y_axis_tolerance_
Definition: kinematic_constraint.h:528
kinematic_constraints::OrientationConstraint::parameterization_type_
int parameterization_type_
Parameterization type for orientation tolerance.
Definition: kinematic_constraint.h:527
kinematic_constraints::VisibilityConstraint::sensor_view_direction_
int sensor_view_direction_
Storage for the sensor view direction.
Definition: kinematic_constraint.h:885
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:882
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:676
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:452
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:331
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:878
kinematic_constraints::JointConstraint::getJointModel
const moveit::core::JointModel * getJointModel() const
Get the joint model for which this constraint operates.
Definition: kinematic_constraint.h:291
kinematic_constraints::PositionConstraint::constraint_region_
std::vector< bodies::BodyPtr > constraint_region_
The constraint region vector.
Definition: kinematic_constraint.h:686
kinematic_constraints::KinematicConstraintSet::KinematicConstraintSet
KinematicConstraintSet(const moveit::core::RobotModelConstPtr &model)
Constructor.
Definition: kinematic_constraint.h:915
kinematic_constraints::PositionConstraint::has_offset_
bool has_offset_
Whether the offset is substantially different than 0.0.
Definition: kinematic_constraint.h:685
kinematic_constraints::PositionConstraint::PositionConstraint
PositionConstraint(const moveit::core::RobotModelConstPtr &model)
Constructor.
Definition: kinematic_constraint.h:558
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:1072
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:1104
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:547
kinematic_constraints::JointConstraint::joint_position_
double joint_position_
Definition: kinematic_constraint.h:364
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 Sun Mar 3 2024 03:23:35