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 <geometric_shapes/bodies.h>
#include <moveit_msgs/Constraints.h>

#include <iostream>
#include <vector>
#include <boost/shared_ptr.hpp>

namespace kinematic_constraints
{

struct ConstraintEvaluationResult
{
  ConstraintEvaluationResult(bool result_satisfied = false, double dist = 0.0) : satisfied(result_satisfied), distance(dist)
  {
  }

  bool   satisfied;             
  double distance;              
};

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_; 
};

typedef boost::shared_ptr<KinematicConstraint> KinematicConstraintPtr; 
typedef boost::shared_ptr<const KinematicConstraint> KinematicConstraintConstPtr; 
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_; 
};

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_; 
};


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_; 
};

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_; 
};

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_; 
};

typedef boost::shared_ptr<KinematicConstraintSet> KinematicConstraintSetPtr; 
typedef boost::shared_ptr<const KinematicConstraintSet> KinematicConstraintSetConstPtr; 
}


#endif