constrained_ik.h

Go to the documentation of this file.

#ifndef CONSTRAINED_IK_H
#define CONSTRAINED_IK_H

#include "constrained_ik/basic_kin.h"
#include "constraint_group.h"
#include "solver_state.h"
#include "constrained_ik/constrained_ik_utils.h"
#include <string>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <urdf/model.h>
#include <constrained_ik/enum_types.h>
#include <moveit/planning_scene/planning_scene.h>
#include <constrained_ik/constraint_results.h>
#include <pluginlib/class_loader.h>

namespace constrained_ik
{

enum SolverStatus
{
  Converged, 
  NotConverged, 
  Failed, 
};

class Constrained_IK
{

public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  Constrained_IK();

  virtual bool linkTransforms(const Eigen::VectorXd &joints,
                      std::vector<KDL::Frame> &poses,
                      const std::vector<std::string> link_names = std::vector<std::string>()) const
  {return kin_.linkTransforms(joints, poses, link_names);}

  virtual void addConstraint(Constraint* constraint, ConstraintTypes constraint_type)
  {
    switch(constraint_type)
    {
      case constraint_types::Primary:
        primary_constraints_.add(constraint);
        break;
      case constraint_types::Auxiliary:
        auxiliary_constraints_.add(constraint);
        break;
    }
  }

  virtual void addConstraintsFromParamServer(const std::string &parameter_name);

  virtual bool calcInvKin(const Eigen::Affine3d &goal,
                          const Eigen::VectorXd &joint_seed,
                          Eigen::VectorXd &joint_angles) const;


  virtual bool calcInvKin(const Eigen::Affine3d &goal,
                          const Eigen::VectorXd &joint_seed,
                          const planning_scene::PlanningSceneConstPtr planning_scene,
                          Eigen::VectorXd &joint_angles) const;

  virtual initialization_state::InitializationState checkInitialized() const
  {
    if (initialized_)
    {
      if (!primary_constraints_.empty() && !auxiliary_constraints_.empty())
      {
        return initialization_state::PrimaryAndAuxiliary;
      }
      else if (!primary_constraints_.empty() && auxiliary_constraints_.empty())
      {
        return initialization_state::PrimaryOnly;
      }
      else if (primary_constraints_.empty() && !auxiliary_constraints_.empty())
      {
        return initialization_state::AuxiliaryOnly;
      }
    }
    else
    {
      return initialization_state::NothingInitialized;
    }
  }

  virtual void clearConstraintList();

  virtual bool getJointNames(std::vector<std::string> &names) const {return kin_.getJointNames(names);}

  virtual inline const basic_kin::BasicKin& getKin() const {return kin_;}

  virtual bool getLinkNames(std::vector<std::string> &names) const {return kin_.getLinkNames(names);}

  virtual ConstrainedIKConfiguration getSolverConfiguration() const {return config_;}

  virtual void setSolverConfiguration(const ConstrainedIKConfiguration &config);

  virtual void loadDefaultSolverConfiguration();

  virtual void init(const basic_kin::BasicKin &kin);

  virtual unsigned int numJoints() const {return kin_.numJoints();}

  static double rangedAngle(double angle);

  virtual Eigen::MatrixXd calcNullspaceProjection(const Eigen::MatrixXd &J) const;

  virtual Eigen::MatrixXd calcNullspaceProjectionTheRightWay(const Eigen::MatrixXd &A) const;

  virtual Eigen::MatrixXd calcDampedPseudoinverse(const Eigen::MatrixXd &J) const;

  bool isInitialized() const { return initialized_; }

 protected:
  // solver configuration parameters
  ros::NodeHandle nh_;                
  ConstrainedIKConfiguration config_; 
  // constraints
  ConstraintGroup primary_constraints_;   
  ConstraintGroup auxiliary_constraints_; 
  // state/counter data
  bool initialized_;        
  basic_kin::BasicKin kin_; 
  virtual constrained_ik::ConstraintResults evalConstraint(constraint_types::ConstraintTypes constraint_type, const constrained_ik::SolverState &state) const;

  virtual void clipToJointLimits(Eigen::VectorXd &joints) const;

  virtual SolverStatus checkStatus(const constrained_ik::SolverState &state, const constrained_ik::ConstraintResults &primary, const constrained_ik::ConstraintResults &auxiliary) const;

  virtual constrained_ik::SolverState getState(const Eigen::Affine3d &goal, const Eigen::VectorXd &joint_seed) const;

  virtual void updateState(constrained_ik::SolverState &state, const Eigen::VectorXd &joints) const;

}; // class Constrained_IK

} // namespace constrained_ik

typedef constrained_ik::SolverStatus SolverStatus; 
#endif // CONSTRAINED_IK_H