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 #ifndef MOVEIT_KINEMATIC_CONSTRAINTS_KINEMATIC_CONSTRAINT_
38 #define MOVEIT_KINEMATIC_CONSTRAINTS_KINEMATIC_CONSTRAINT_
39 
45 
47 #include <moveit_msgs/Constraints.h>
48 
49 #include <iostream>
50 #include <vector>
51 
54 {
57 {
66  ConstraintEvaluationResult(bool result_satisfied = false, double dist = 0.0)
67  : satisfied(result_satisfied), distance(dist)
68  {
69  }
70 
71  bool satisfied;
72  double distance;
73 };
74 
76 
79 {
80 public:
83  {
88  VISIBILITY_CONSTRAINT
89  };
90 
96  KinematicConstraint(const robot_model::RobotModelConstPtr& model);
97  virtual ~KinematicConstraint();
98 
100  virtual void clear() = 0;
101 
110  virtual ConstraintEvaluationResult decide(const robot_state::RobotState& state, bool verbose = false) const = 0;
111 
117  virtual bool enabled() const = 0;
118 
130  virtual bool equal(const KinematicConstraint& other, double margin) const = 0;
131 
139  {
140  return type_;
141  }
142 
148  virtual void print(std::ostream& = std::cout) const
149  {
150  }
151 
159  double getConstraintWeight() const
160  {
161  return constraint_weight_;
162  }
163 
170  const robot_model::RobotModelConstPtr& getRobotModel() const
171  {
172  return robot_model_;
173  }
174 
175 protected:
177  robot_model::RobotModelConstPtr robot_model_;
180 };
181 
183 
204 {
205 public:
211  JointConstraint(const robot_model::RobotModelConstPtr& model)
212  : KinematicConstraint(model), joint_model_(NULL), joint_variable_index_(-1)
213  {
214  type_ = JOINT_CONSTRAINT;
215  }
216 
230  bool configure(const moveit_msgs::JointConstraint& jc);
231 
247  virtual bool equal(const KinematicConstraint& other, double margin) const;
248 
249  virtual ConstraintEvaluationResult decide(const robot_state::RobotState& state, bool verbose = false) const;
250  virtual bool enabled() const;
251  virtual void clear();
252  virtual void print(std::ostream& out = std::cout) const;
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 
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:
329  std::string local_variable_name_;
330  std::string joint_variable_name_;
332  double joint_position_, joint_tolerance_above_, joint_tolerance_below_;
333 };
334 
336 
349 {
350 public:
352 
353 public:
359  OrientationConstraint(const robot_model::RobotModelConstPtr& model) : KinematicConstraint(model), link_model_(NULL)
360  {
361  type_ = ORIENTATION_CONSTRAINT;
362  }
363 
377  bool configure(const moveit_msgs::OrientationConstraint& oc, const robot_state::Transforms& tf);
378 
395  virtual bool equal(const KinematicConstraint& other, double margin) const;
396 
397  virtual void clear();
398  virtual ConstraintEvaluationResult decide(const robot_state::RobotState& state, bool verbose = false) const;
399  virtual bool enabled() const;
400  virtual void print(std::ostream& out = std::cout) const;
401 
409  {
410  return link_model_;
411  }
412 
419  const std::string& getReferenceFrame() const
420  {
421  return desired_rotation_frame_id_;
422  }
423 
430  bool mobileReferenceFrame() const
431  {
432  return mobile_frame_;
433  }
434 
440  const Eigen::Matrix3d& getDesiredRotationMatrix() const
441  {
442  return desired_rotation_matrix_;
443  }
444 
451  double getXAxisTolerance() const
452  {
453  return absolute_x_axis_tolerance_;
454  }
455 
462  double getYAxisTolerance() const
463  {
464  return absolute_y_axis_tolerance_;
465  }
466 
473  double getZAxisTolerance() const
474  {
475  return absolute_z_axis_tolerance_;
476  }
477 
478 protected:
480  Eigen::Matrix3d desired_rotation_matrix_;
486  double absolute_x_axis_tolerance_, absolute_y_axis_tolerance_,
488 };
489 
491 
506 {
507 public:
509 
510 public:
516  PositionConstraint(const robot_model::RobotModelConstPtr& model) : KinematicConstraint(model), link_model_(NULL)
517  {
518  type_ = POSITION_CONSTRAINT;
519  }
520 
537  bool configure(const moveit_msgs::PositionConstraint& pc, const robot_state::Transforms& tf);
538 
562  virtual bool equal(const KinematicConstraint& other, double margin) const;
563 
564  virtual void clear();
565  virtual ConstraintEvaluationResult decide(const robot_state::RobotState& state, bool verbose = false) const;
566  virtual bool enabled() const;
567  virtual void print(std::ostream& out = std::cout) const;
568 
576  {
577  return link_model_;
578  }
579 
586  const Eigen::Vector3d& getLinkOffset() const
587  {
588  return offset_;
589  }
590 
598  bool hasLinkOffset() const
599  {
600  if (!enabled())
601  return false;
602  return has_offset_;
603  }
604 
611  const std::vector<bodies::BodyPtr>& getConstraintRegions() const
612  {
613  return constraint_region_;
614  }
615 
622  const std::string& getReferenceFrame() const
623  {
624  return constraint_frame_id_;
625  }
626 
634  bool mobileReferenceFrame() const
635  {
636  if (!enabled())
637  return false;
638  return mobile_frame_;
639  }
640 
641 protected:
642  Eigen::Vector3d offset_;
643  bool has_offset_;
644  std::vector<bodies::BodyPtr> constraint_region_;
647  std::string constraint_frame_id_;
649 };
650 
652 
751 {
752 public:
754 
755 public:
761  VisibilityConstraint(const robot_model::RobotModelConstPtr& model);
762 
775  bool configure(const moveit_msgs::VisibilityConstraint& vc, const robot_state::Transforms& tf);
776 
794  virtual bool equal(const KinematicConstraint& other, double margin) const;
795  virtual void clear();
796 
804  shapes::Mesh* getVisibilityCone(const robot_state::RobotState& state) const;
805 
817  void getMarkers(const robot_state::RobotState& state, visualization_msgs::MarkerArray& markers) const;
818 
819  virtual bool enabled() const;
820  virtual ConstraintEvaluationResult decide(const robot_state::RobotState& state, bool verbose = false) const;
821  virtual void print(std::ostream& out = std::cout) const;
822 
823 protected:
833  bool decideContact(const collision_detection::Contact& contact) const;
834 
835  collision_detection::CollisionRobotPtr collision_robot_;
839  std::string target_frame_id_;
840  std::string sensor_frame_id_;
841  Eigen::Affine3d sensor_pose_;
843  Eigen::Affine3d target_pose_;
844  unsigned int cone_sides_;
846  double target_radius_;
849 };
850 
852 
862 {
863 public:
865 
866 public:
872  KinematicConstraintSet(const robot_model::RobotModelConstPtr& model) : robot_model_(model)
873  {
874  }
875 
877  {
878  clear();
879  }
880 
882  void clear();
883 
894  bool add(const moveit_msgs::Constraints& c, const robot_state::Transforms& tf);
895 
903  bool add(const std::vector<moveit_msgs::JointConstraint>& jc);
904 
912  bool add(const std::vector<moveit_msgs::PositionConstraint>& pc, const robot_state::Transforms& tf);
913 
921  bool add(const std::vector<moveit_msgs::OrientationConstraint>& oc, const robot_state::Transforms& tf);
922 
930  bool add(const std::vector<moveit_msgs::VisibilityConstraint>& vc, const robot_state::Transforms& tf);
931 
943  ConstraintEvaluationResult decide(const robot_state::RobotState& state, bool verbose = false) const;
944 
964  std::vector<ConstraintEvaluationResult>& results, bool verbose = false) const;
965 
981  bool equal(const KinematicConstraintSet& other, double margin) const;
982 
988  void print(std::ostream& out = std::cout) const;
989 
996  const std::vector<moveit_msgs::PositionConstraint>& getPositionConstraints() const
997  {
998  return position_constraints_;
999  }
1000 
1007  const std::vector<moveit_msgs::OrientationConstraint>& getOrientationConstraints() const
1008  {
1009  return orientation_constraints_;
1010  }
1011 
1018  const std::vector<moveit_msgs::JointConstraint>& getJointConstraints() const
1019  {
1020  return joint_constraints_;
1021  }
1022 
1029  const std::vector<moveit_msgs::VisibilityConstraint>& getVisibilityConstraints() const
1030  {
1031  return visibility_constraints_;
1032  }
1033 
1040  const moveit_msgs::Constraints& getAllConstraints() const
1041  {
1042  return all_constraints_;
1043  }
1044 
1051  bool empty() const
1052  {
1053  return kinematic_constraints_.empty();
1054  }
1055 
1056 protected:
1057  robot_model::RobotModelConstPtr robot_model_;
1058  std::vector<KinematicConstraintPtr>
1061  std::vector<moveit_msgs::JointConstraint> joint_constraints_;
1063  std::vector<moveit_msgs::PositionConstraint> position_constraints_;
1065  std::vector<moveit_msgs::OrientationConstraint> orientation_constraints_;
1067  std::vector<moveit_msgs::VisibilityConstraint> visibility_constraints_;
1069  moveit_msgs::Constraints all_constraints_;
1070 };
1071 }
1072 
1073 #endif
bool empty() const
Returns whether or not there are any constraints in the set.
ConstraintEvaluationResult(bool result_satisfied=false, double dist=0.0)
Constructor.
std::vector< moveit_msgs::JointConstraint > joint_constraints_
Messages corresponding to all internal joint constraints.
collision_detection::CollisionRobotPtr collision_robot_
A copy of the collision robot maintained for collision checking the cone against robot links...
const moveit_msgs::Constraints & getAllConstraints() const
Get all constraints in the set.
double absolute_z_axis_tolerance_
Storage for the tolerances.
Class for constraints on the orientation of a link.
virtual void print(std::ostream &=std::cout) const
Print the constraint data.
ConstraintType type_
The type of the constraint.
bool hasLinkOffset() const
If the constraint is enabled and the link offset is substantially different than zero.
int sensor_view_direction_
Storage for the sensor view direction.
std::vector< Eigen::Affine3d, Eigen::aligned_allocator< Eigen::Affine3d > > vector_Affine3d
Class for constraints on the visibility relationship between a sensor and a target.
bool mobile_frame_
Whether or not a mobile frame is employed.
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.
double getDesiredJointPosition() const
Gets the desired position component of the constraint.
EigenSTL::vector_Vector3d points_
A set of points along the base of the circle.
const robot_model::LinkModel * getLinkModel() const
Gets the subject link model.
MOVEIT_CLASS_FORWARD(KinematicConstraint)
const std::vector< bodies::BodyPtr > & getConstraintRegions() const
Returns all the constraint regions.
Class for handling single DOF joint constraints.
double max_view_angle_
Storage for the max view angle.
Eigen::Affine3d target_pose_
The target pose transformed into the transform frame.
double joint_tolerance_below_
Position and tolerance values.
A class that contains many different constraints, and can check RobotState *versus the full set...
const std::string & getReferenceFrame() const
Returns the reference frame.
const robot_model::LinkModel * getLinkModel() const
Returns the associated link model, or NULL if not enabled.
std::vector< moveit_msgs::PositionConstraint > position_constraints_
Messages corresponding to all internal position constraints.
const std::string & getJointVariableName() const
Gets the joint variable name, as known to the robot_model::RobotModel.
const std::vector< moveit_msgs::PositionConstraint > & getPositionConstraints() const
Get all position constraints in the set.
Representation and evaluation of kinematic constraints.
Provides an implementation of a snapshot of a transform tree that can be easily queried for transform...
Definition: transforms.h:61
std::vector< moveit_msgs::VisibilityConstraint > visibility_constraints_
Messages corresponding to all internal visibility constraints.
robot_model::RobotModelConstPtr robot_model_
The kinematic model used for by the Set.
double distance
The distance evaluation from the constraint or constraints.
double target_radius_
Storage for the target radius.
const robot_model::LinkModel * link_model_
The target link model.
Eigen::Matrix3d desired_rotation_matrix_
The desired rotation matrix in the tf frame.
const Eigen::Vector3d & getLinkOffset() const
Returns the target offset.
double getJointToleranceBelow() const
Gets the lower tolerance component of the joint constraint.
const std::string & getLocalVariableName() const
Gets the local variable name if this constraint was configured for a part of a multi-DOF joint...
Eigen::Matrix3d desired_rotation_matrix_inv_
The inverse of the desired rotation matrix, precomputed for efficiency.
std::string sensor_frame_id_
The sensor frame id.
bool satisfied
Whether or not the constraint or constraints were satisfied.
double getZAxisTolerance() const
Gets the Z axis tolerance.
ConstraintType
Enum for representing a constraint.
bool verbose
int joint_variable_index_
The index of the joint variable name in the full robot state.
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.
const std::vector< moveit_msgs::JointConstraint > & getJointConstraints() const
Get all joint constraints in the set.
const std::string & getReferenceFrame() const
The target frame of the planning_models::Transforms class, for interpreting the rotation frame...
bool mobile_target_frame_
True if the target is a non-fixed frame relative to the transform frame.
ROS/KDL based interface for the inverse kinematics of the PR2 arm.
const robot_model::LinkModel * link_model_
The link model constraint subject.
robot_model::RobotModelConstPtr robot_model_
The kinematic model associated with this constraint.
std::string constraint_frame_id_
The constraint frame id.
Eigen::Vector3d offset_
The target offset.
double getConstraintWeight() const
The weight of a constraint is a multiplicative factor associated to the distance computed by the deci...
Base class for representing a kinematic constraint.
bool has_offset_
Whether the offset is substantially different than 0.0.
moveit_msgs::Constraints all_constraints_
Messages corresponding to all internal constraints.
EigenSTL::vector_Affine3d constraint_region_pose_
The constraint region pose vector.
double getJointToleranceAbove() const
Gets the upper tolerance component of the joint constraint.
A joint from the robot. Models the transform that this joint applies in the kinematic chain...
Definition: joint_model.h:108
Struct for containing the results of constraint evaluation.
Class for constraints on the XYZ position of a link.
const std::vector< moveit_msgs::VisibilityConstraint > & getVisibilityConstraints() const
Get all visibility constraints in the set.
std::string target_frame_id_
The target frame id.
unsigned int cone_sides_
Storage for the cone sides.
KinematicConstraintSet(const robot_model::RobotModelConstPtr &model)
Constructor.
std::string joint_variable_name_
The joint variable name.
double max_range_angle_
Storage for the max range angle.
const robot_model::JointModel * getJointModel() const
Get the joint model for which this constraint operates.
const robot_model::RobotModelConstPtr & getRobotModel() const
bool mobileReferenceFrame() const
Whether or not a mobile reference frame is being employed.
double getXAxisTolerance() const
Gets the X axis tolerance.
std::vector< KinematicConstraintPtr > kinematic_constraints_
Shared pointers to all the member constraints.
bool mobile_frame_
Whether or not the header frame is mobile or fixed.
OrientationConstraint(const robot_model::RobotModelConstPtr &model)
Constructor.
std::vector< moveit_msgs::OrientationConstraint > orientation_constraints_
Messages corresponding to all internal orientation constraints.
const robot_model::JointModel * joint_model_
The joint from the kinematic model for this constraint.
double getYAxisTolerance() const
Gets the Y axis tolerance.
Representation of a robot&#39;s state. This includes position, velocity, acceleration and effort...
Definition: robot_state.h:123
A link from the robot. Contains the constant transform applied to the link and its geometry...
Definition: link_model.h:72
bool mobileReferenceFrame() const
If enabled and the specified frame is a mobile frame, return true. Otherwise, returns false...
double constraint_weight_
The weight of a constraint is a multiplicative factor associated to the distance computed by the deci...
const Eigen::Matrix3d & getDesiredRotationMatrix() const
The rotation target in the reference frame.
JointConstraint(const robot_model::RobotModelConstPtr &model)
Constructor.
ConstraintType getType() const
Get the type of constraint.
PositionConstraint(const robot_model::RobotModelConstPtr &model)
Constructor.
void print(PropagationDistanceField &pdf, int numX, int numY, int numZ)
bool joint_is_continuous_
Whether or not the joint is continuous.
const std::vector< moveit_msgs::OrientationConstraint > & getOrientationConstraints() const
Get all orientation constraints in the set.


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Jul 10 2019 04:03:05