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 */

#ifndef MOVEIT_KINEMATIC_CONSTRAINTS_KINEMATIC_CONSTRAINT_
#define MOVEIT_KINEMATIC_CONSTRAINTS_KINEMATIC_CONSTRAINT_

#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_state/robot_state.h>
#include <moveit/transforms/transforms.h>
#include <moveit/collision_detection/collision_world.h>
#include <moveit/macros/class_forward.h>

#include <geometric_shapes/bodies.h>
#include <moveit_msgs/Constraints.h>

#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);

class KinematicConstraint
{
public:
  enum ConstraintType
  {
    UNKNOWN_CONSTRAINT,
    JOINT_CONSTRAINT,
    POSITION_CONSTRAINT,
    ORIENTATION_CONSTRAINT,
    VISIBILITY_CONSTRAINT
  };

  KinematicConstraint(const robot_model::RobotModelConstPtr& model);
  virtual ~KinematicConstraint();

  virtual void clear() = 0;

  virtual ConstraintEvaluationResult decide(const robot_state::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& out = std::cout) const
  {
  }

  double getConstraintWeight() const
  {
    return constraint_weight_;
  }

  const robot_model::RobotModelConstPtr& getRobotModel() const
  {
    return robot_model_;
  }

protected:
  ConstraintType type_;                         
  robot_model::RobotModelConstPtr robot_model_; 
  double constraint_weight_; 
};

MOVEIT_CLASS_FORWARD(JointConstraint);

class JointConstraint : public KinematicConstraint
{
public:
  JointConstraint(const robot_model::RobotModelConstPtr& model)
    : KinematicConstraint(model), joint_model_(NULL), joint_variable_index_(-1)
  {
    type_ = JOINT_CONSTRAINT;
  }

  bool configure(const moveit_msgs::JointConstraint& jc);

  virtual bool equal(const KinematicConstraint& other, double margin) const;

  virtual ConstraintEvaluationResult decide(const robot_state::RobotState& state, bool verbose = false) const;
  virtual bool enabled() const;
  virtual void clear();
  virtual void print(std::ostream& out = std::cout) const;

  const robot_model::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 robot_model::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);

class OrientationConstraint : public KinematicConstraint
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

public:
  OrientationConstraint(const robot_model::RobotModelConstPtr& model) : KinematicConstraint(model), link_model_(NULL)
  {
    type_ = ORIENTATION_CONSTRAINT;
  }

  bool configure(const moveit_msgs::OrientationConstraint& oc, const robot_state::Transforms& tf);

  virtual bool equal(const KinematicConstraint& other, double margin) const;

  virtual void clear();
  virtual ConstraintEvaluationResult decide(const robot_state::RobotState& state, bool verbose = false) const;
  virtual bool enabled() const;
  virtual void print(std::ostream& out = std::cout) const;

  const robot_model::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& getDesiredRotationMatrix() const
  {
    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_;
  }

  void swapLinkModel(const robot_model::LinkModel* new_link, const Eigen::Matrix3d& update);

protected:
  const robot_model::LinkModel* link_model_;    
  Eigen::Matrix3d desired_rotation_matrix_;     
  Eigen::Matrix3d desired_rotation_matrix_inv_; 
  std::string desired_rotation_frame_id_;       
  bool mobile_frame_;                           
  double absolute_x_axis_tolerance_, absolute_y_axis_tolerance_,
      absolute_z_axis_tolerance_; 
};

MOVEIT_CLASS_FORWARD(PositionConstraint);

class PositionConstraint : public KinematicConstraint
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

public:
  PositionConstraint(const robot_model::RobotModelConstPtr& model) : KinematicConstraint(model), link_model_(NULL)
  {
    type_ = POSITION_CONSTRAINT;
  }

  bool configure(const moveit_msgs::PositionConstraint& pc, const robot_state::Transforms& tf);

  virtual bool equal(const KinematicConstraint& other, double margin) const;

  virtual void clear();
  virtual ConstraintEvaluationResult decide(const robot_state::RobotState& state, bool verbose = false) const;
  virtual bool enabled() const;
  virtual void print(std::ostream& out = std::cout) const;

  const robot_model::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_;
  }

  void swapLinkModel(const robot_model::LinkModel* new_link, const Eigen::Affine3d& update);

protected:
  Eigen::Vector3d offset_;                         
  bool has_offset_;                                
  std::vector<bodies::BodyPtr> constraint_region_; 
  EigenSTL::vector_Affine3d constraint_region_pose_; 
  bool mobile_frame_;                                
  std::string constraint_frame_id_;                  
  const robot_model::LinkModel* link_model_;         
};

MOVEIT_CLASS_FORWARD(VisibilityConstraint);

class VisibilityConstraint : public KinematicConstraint
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

public:
  VisibilityConstraint(const robot_model::RobotModelConstPtr& model);

  bool configure(const moveit_msgs::VisibilityConstraint& vc, const robot_state::Transforms& tf);

  virtual bool equal(const KinematicConstraint& other, double margin) const;
  virtual void clear();

  shapes::Mesh* getVisibilityCone(const robot_state::RobotState& state) const;

  void getMarkers(const robot_state::RobotState& state, visualization_msgs::MarkerArray& markers) const;

  virtual bool enabled() const;
  virtual ConstraintEvaluationResult decide(const robot_state::RobotState& state, bool verbose = false) const;
  virtual void print(std::ostream& out = std::cout) const;

protected:
  bool decideContact(const collision_detection::Contact& contact) const;

  collision_detection::CollisionRobotPtr collision_robot_; 
  bool mobile_sensor_frame_;    
  bool mobile_target_frame_;    
  std::string target_frame_id_; 
  std::string sensor_frame_id_; 
  Eigen::Affine3d sensor_pose_; 
  int sensor_view_direction_;   
  Eigen::Affine3d 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);

class KinematicConstraintSet
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

public:
  KinematicConstraintSet(const robot_model::RobotModelConstPtr& model) : robot_model_(model)
  {
  }

  ~KinematicConstraintSet()
  {
    clear();
  }

  void clear();

  bool add(const moveit_msgs::Constraints& c, const robot_state::Transforms& tf);

  bool add(const std::vector<moveit_msgs::JointConstraint>& jc);

  bool add(const std::vector<moveit_msgs::PositionConstraint>& pc, const robot_state::Transforms& tf);

  bool add(const std::vector<moveit_msgs::OrientationConstraint>& oc, const robot_state::Transforms& tf);

  bool add(const std::vector<moveit_msgs::VisibilityConstraint>& vc, const robot_state::Transforms& tf);

  ConstraintEvaluationResult decide(const robot_state::RobotState& state, bool verbose = false) const;

  ConstraintEvaluationResult decide(const robot_state::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::PositionConstraint>& getPositionConstraints() const
  {
    return position_constraints_;
  }

  const std::vector<moveit_msgs::OrientationConstraint>& getOrientationConstraints() const
  {
    return orientation_constraints_;
  }

  const std::vector<moveit_msgs::JointConstraint>& getJointConstraints() const
  {
    return joint_constraints_;
  }

  const std::vector<moveit_msgs::VisibilityConstraint>& getVisibilityConstraints() const
  {
    return visibility_constraints_;
  }

  const moveit_msgs::Constraints& getAllConstraints() const
  {
    return all_constraints_;
  }

  bool empty() const
  {
    return kinematic_constraints_.empty();
  }

protected:
  robot_model::RobotModelConstPtr robot_model_; 
  std::vector<KinematicConstraintPtr>
      kinematic_constraints_; 
  std::vector<moveit_msgs::JointConstraint> joint_constraints_; 
  std::vector<moveit_msgs::PositionConstraint> position_constraints_;       
  std::vector<moveit_msgs::OrientationConstraint> orientation_constraints_; 
  std::vector<moveit_msgs::VisibilityConstraint> visibility_constraints_;   
  moveit_msgs::Constraints all_constraints_; 
};
}

#endif