kinematic_constraint.h

Go to the documentation of this file.

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2011, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/
 
/* Author: Ioan Sucan */
 
#pragma once
 
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_state/robot_state.h>
#include <moveit/transforms/transforms.h>
#include <moveit/collision_detection/collision_env.h>
#include <moveit/macros/class_forward.h>
 
#include <geometric_shapes/bodies.h>
#include <moveit_msgs/msg/constraints.hpp>
 
#include <iostream>
#include <vector>
 
namespace kinematic_constraints
{
struct ConstraintEvaluationResult
{
  ConstraintEvaluationResult(bool result_satisfied = false, double dist = 0.0)
    : satisfied(result_satisfied), distance(dist)
  {
  }
 
  bool satisfied;  
  double distance; 
};
 
MOVEIT_CLASS_FORWARD(KinematicConstraint);  // Defines KinematicConstraintPtr, ConstPtr, WeakPtr... etc
 
class KinematicConstraint
{
public:
  enum ConstraintType
  {
    UNKNOWN_CONSTRAINT,
    JOINT_CONSTRAINT,
    POSITION_CONSTRAINT,
    ORIENTATION_CONSTRAINT,
    VISIBILITY_CONSTRAINT
  };
 
  KinematicConstraint(const moveit::core::RobotModelConstPtr& model);
  virtual ~KinematicConstraint();
 
  virtual void clear() = 0;
 
  virtual ConstraintEvaluationResult decide(const moveit::core::RobotState& state, bool verbose = false) const = 0;
 
  virtual bool enabled() const = 0;
 
  virtual bool equal(const KinematicConstraint& other, double margin) const = 0;
 
  ConstraintType getType() const
  {
    return type_;
  }
 
  virtual void print(std::ostream& /*unused*/ = std::cout) const
  {
  }
 
  double getConstraintWeight() const
  {
    return constraint_weight_;
  }
 
  const moveit::core::RobotModelConstPtr& getRobotModel() const
  {
    return robot_model_;
  }
 
protected:
  ConstraintType type_;                          
  moveit::core::RobotModelConstPtr robot_model_; 
  double constraint_weight_; 
};
 
MOVEIT_CLASS_FORWARD(JointConstraint);  // Defines JointConstraintPtr, ConstPtr, WeakPtr... etc
 
class JointConstraint : public KinematicConstraint
{
public:
  JointConstraint(const moveit::core::RobotModelConstPtr& model)
    : KinematicConstraint(model), joint_model_(nullptr), joint_variable_index_(-1)
  {
    type_ = JOINT_CONSTRAINT;
  }
 
  bool configure(const moveit_msgs::msg::JointConstraint& jc);
 
  bool equal(const KinematicConstraint& other, double margin) const override;
 
  ConstraintEvaluationResult decide(const moveit::core::RobotState& state, bool verbose = false) const override;
  bool enabled() const override;
  void clear() override;
  void print(std::ostream& out = std::cout) const override;
 
  const moveit::core::JointModel* getJointModel() const
  {
    return joint_model_;
  }
  const std::string& getLocalVariableName() const
  {
    return local_variable_name_;
  }
 
  const std::string& getJointVariableName() const
  {
    return joint_variable_name_;
  }
 
  int getJointVariableIndex() const
  {
    return joint_variable_index_;
  }
 
  double getDesiredJointPosition() const
  {
    return joint_position_;
  }
 
  double getJointToleranceAbove() const
  {
    return joint_tolerance_above_;
  }
 
  double getJointToleranceBelow() const
  {
    return joint_tolerance_below_;
  }
 
protected:
  const moveit::core::JointModel* joint_model_; 
  bool joint_is_continuous_;                    
  std::string local_variable_name_;             
  std::string joint_variable_name_;             
  int joint_variable_index_; 
  double joint_position_, joint_tolerance_above_, joint_tolerance_below_; 
};
 
MOVEIT_CLASS_FORWARD(OrientationConstraint);  // Defines OrientationConstraintPtr, ConstPtr, WeakPtr... etc
 
class OrientationConstraint : public KinematicConstraint
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
public:
  OrientationConstraint(const moveit::core::RobotModelConstPtr& model)
    : KinematicConstraint(model), link_model_(nullptr)
  {
    type_ = ORIENTATION_CONSTRAINT;
  }
 
  bool configure(const moveit_msgs::msg::OrientationConstraint& oc, const moveit::core::Transforms& tf);
 
  bool equal(const KinematicConstraint& other, double margin) const override;
 
  void clear() override;
  ConstraintEvaluationResult decide(const moveit::core::RobotState& state, bool verbose = false) const override;
  bool enabled() const override;
  void print(std::ostream& out = std::cout) const override;
 
  const moveit::core::LinkModel* getLinkModel() const
  {
    return link_model_;
  }
 
  const std::string& getReferenceFrame() const
  {
    return desired_rotation_frame_id_;
  }
 
  bool mobileReferenceFrame() const
  {
    return mobile_frame_;
  }
 
  const Eigen::Matrix3d& getDesiredRotationMatrixInRefFrame() const
  {
    // validity of the rotation matrix is enforced in configure()
    return desired_R_in_frame_id_;
  }
 
  const Eigen::Matrix3d& getDesiredRotationMatrix() const
  {
    // validity of the rotation matrix is enforced in configure()
    return desired_rotation_matrix_;
  }
 
  double getXAxisTolerance() const
  {
    return absolute_x_axis_tolerance_;
  }
 
  double getYAxisTolerance() const
  {
    return absolute_y_axis_tolerance_;
  }
 
  double getZAxisTolerance() const
  {
    return absolute_z_axis_tolerance_;
  }
 
  int getParameterizationType() const
  {
    return parameterization_type_;
  }
 
protected:
  const moveit::core::LinkModel* link_model_;   
  Eigen::Matrix3d desired_R_in_frame_id_;       
  Eigen::Matrix3d desired_rotation_matrix_;     
  Eigen::Matrix3d desired_rotation_matrix_inv_; 
  std::string desired_rotation_frame_id_;       
  bool mobile_frame_;                           
  int parameterization_type_;                   
  double absolute_x_axis_tolerance_, absolute_y_axis_tolerance_,
      absolute_z_axis_tolerance_; 
};
 
MOVEIT_CLASS_FORWARD(PositionConstraint);  // Defines PositionConstraintPtr, ConstPtr, WeakPtr... etc
 
class PositionConstraint : public KinematicConstraint
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
public:
  PositionConstraint(const moveit::core::RobotModelConstPtr& model) : KinematicConstraint(model), link_model_(nullptr)
  {
    type_ = POSITION_CONSTRAINT;
  }
 
  bool configure(const moveit_msgs::msg::PositionConstraint& pc, const moveit::core::Transforms& tf);
 
  bool equal(const KinematicConstraint& other, double margin) const override;
 
  void clear() override;
  ConstraintEvaluationResult decide(const moveit::core::RobotState& state, bool verbose = false) const override;
  bool enabled() const override;
  void print(std::ostream& out = std::cout) const override;
 
  const moveit::core::LinkModel* getLinkModel() const
  {
    return link_model_;
  }
 
  const Eigen::Vector3d& getLinkOffset() const
  {
    return offset_;
  }
 
  bool hasLinkOffset() const
  {
    if (!enabled())
      return false;
    return has_offset_;
  }
 
  const std::vector<bodies::BodyPtr>& getConstraintRegions() const
  {
    return constraint_region_;
  }
 
  const std::string& getReferenceFrame() const
  {
    return constraint_frame_id_;
  }
 
  bool mobileReferenceFrame() const
  {
    if (!enabled())
      return false;
    return mobile_frame_;
  }
 
protected:
  Eigen::Vector3d offset_;                         
  bool has_offset_;                                
  std::vector<bodies::BodyPtr> constraint_region_; 
  EigenSTL::vector_Isometry3d constraint_region_pose_;
  bool mobile_frame_;                         
  std::string constraint_frame_id_;           
  const moveit::core::LinkModel* link_model_; 
};
 
MOVEIT_CLASS_FORWARD(VisibilityConstraint);  // Defines VisibilityConstraintPtr, ConstPtr, WeakPtr... etc
 
class VisibilityConstraint : public KinematicConstraint
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
public:
  VisibilityConstraint(const moveit::core::RobotModelConstPtr& model);
 
  bool configure(const moveit_msgs::msg::VisibilityConstraint& vc, const moveit::core::Transforms& tf);
 
  bool equal(const KinematicConstraint& other, double margin) const override;
  void clear() override;
 
  shapes::Mesh* getVisibilityCone(const Eigen::Isometry3d& tform_world_to_sensor,
                                  const Eigen::Isometry3d& tform_world_to_target) const;
 
  void getMarkers(const moveit::core::RobotState& state, visualization_msgs::msg::MarkerArray& markers) const;
 
  bool enabled() const override;
  ConstraintEvaluationResult decide(const moveit::core::RobotState& state, bool verbose = false) const override;
  void print(std::ostream& out = std::cout) const override;
 
protected:
  bool decideContact(const collision_detection::Contact& contact) const;
 
  moveit::core::RobotModelConstPtr robot_model_; 
  std::string target_frame_id_;      
  std::string sensor_frame_id_;      
  Eigen::Isometry3d sensor_pose_;    
  int sensor_view_direction_;        
  Eigen::Isometry3d target_pose_;    
  unsigned int cone_sides_;          
  EigenSTL::vector_Vector3d points_; 
  double target_radius_;             
  double max_view_angle_;            
  double max_range_angle_;           
};
 
MOVEIT_CLASS_FORWARD(KinematicConstraintSet);  // Defines KinematicConstraintSetPtr, ConstPtr, WeakPtr... etc
 
class KinematicConstraintSet
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
public:
  KinematicConstraintSet(const moveit::core::RobotModelConstPtr& model) : robot_model_(model)
  {
  }
 
  ~KinematicConstraintSet()
  {
    clear();
  }
 
  void clear();
 
  bool add(const moveit_msgs::msg::Constraints& c, const moveit::core::Transforms& tf);
 
  bool add(const std::vector<moveit_msgs::msg::JointConstraint>& jc);
 
  bool add(const std::vector<moveit_msgs::msg::PositionConstraint>& pc, const moveit::core::Transforms& tf);
 
  bool add(const std::vector<moveit_msgs::msg::OrientationConstraint>& oc, const moveit::core::Transforms& tf);
 
  bool add(const std::vector<moveit_msgs::msg::VisibilityConstraint>& vc, const moveit::core::Transforms& tf);
 
  ConstraintEvaluationResult decide(const moveit::core::RobotState& state, bool verbose = false) const;
 
  ConstraintEvaluationResult decide(const moveit::core::RobotState& state,
                                    std::vector<ConstraintEvaluationResult>& results, bool verbose = false) const;
 
  bool equal(const KinematicConstraintSet& other, double margin) const;
 
  void print(std::ostream& out = std::cout) const;
 
  const std::vector<moveit_msgs::msg::PositionConstraint>& getPositionConstraints() const
  {
    return position_constraints_;
  }
 
  const std::vector<moveit_msgs::msg::OrientationConstraint>& getOrientationConstraints() const
  {
    return orientation_constraints_;
  }
 
  const std::vector<moveit_msgs::msg::JointConstraint>& getJointConstraints() const
  {
    return joint_constraints_;
  }
 
  const std::vector<moveit_msgs::msg::VisibilityConstraint>& getVisibilityConstraints() const
  {
    return visibility_constraints_;
  }
 
  const moveit_msgs::msg::Constraints& getAllConstraints() const
  {
    return all_constraints_;
  }
 
  bool empty() const
  {
    return kinematic_constraints_.empty();
  }
 
protected:
  moveit::core::RobotModelConstPtr robot_model_; 
  std::vector<KinematicConstraintPtr>
      kinematic_constraints_; 
  std::vector<moveit_msgs::msg::JointConstraint> joint_constraints_; 
  std::vector<moveit_msgs::msg::PositionConstraint> position_constraints_; 
  std::vector<moveit_msgs::msg::OrientationConstraint> orientation_constraints_; 
  std::vector<moveit_msgs::msg::VisibilityConstraint> visibility_constraints_;   
  moveit_msgs::msg::Constraints all_constraints_; 
};
}  // namespace kinematic_constraints