kinematics.h

Go to the documentation of this file.

#ifndef INDUSTRIAL_MOVEIT_STOMP_MOVEIT_INCLUDE_STOMP_MOVEIT_UTILS_KINEMATICS_H_
#define INDUSTRIAL_MOVEIT_STOMP_MOVEIT_INCLUDE_STOMP_MOVEIT_UTILS_KINEMATICS_H_

#include <ros/console.h>
#include <Eigen/Geometry>
#include <Eigen/StdVector>
#include <eigen_conversions/eigen_msg.h>
#include <moveit/robot_state/robot_state.h>
#include <moveit/robot_state/conversions.h>
#include <pluginlib/class_list_macros.h>
#include <XmlRpcException.h>
#include <moveit_msgs/Constraints.h>
#include <moveit_msgs/PositionConstraint.h>
#include <moveit_msgs/OrientationConstraint.h>
#include <trac_ik/trac_ik.hpp>
#include <boost/optional.hpp>


namespace EigenSTL
{
  typedef std::vector<Eigen::Affine3d, Eigen::aligned_allocator<Eigen::Affine3d>> vector_Affine3d;
}

namespace stomp_moveit
{
namespace utils
{

namespace kinematics
{

  const static double EPSILON = 0.011;  
  const static double LAMBDA = 0.01;    
  MOVEIT_CLASS_FORWARD(IKSolver);

  class IKSolver
  {
  public:
    IKSolver(const moveit::core::RobotState& robot_state,std::string group_name,double max_time = 0.005);
    ~IKSolver();

    void setKinematicState(const moveit::core::RobotState& state);

    bool solve(const Eigen::VectorXd& seed,const Eigen::Affine3d& tool_pose,Eigen::VectorXd& solution,
               Eigen::VectorXd tol = Eigen::VectorXd::Constant(6,0.005)) ;

    bool solve(const std::vector<double>& seed, const Eigen::Affine3d& tool_pose,std::vector<double>& solution,
               std::vector<double> tol = std::vector<double>(6,0.005)) ;

    bool solve(const std::vector<double>& seed, const moveit_msgs::Constraints& tool_constraints,std::vector<double>& solution);

    bool solve(const Eigen::VectorXd& seed, const moveit_msgs::Constraints& tool_constraints,Eigen::VectorXd& solution);

  protected:

    std::shared_ptr<TRAC_IK::TRAC_IK> ik_solver_impl_;
    moveit::core::RobotModelConstPtr robot_model_;
    moveit::core::RobotStatePtr robot_state_;
    std::string group_name_;
    Eigen::Affine3d tf_base_to_root_;

  };

  bool isCartesianConstraints(const moveit_msgs::Constraints& c);

  boost::optional<moveit_msgs::Constraints> curateCartesianConstraints(const moveit_msgs::Constraints& c,const Eigen::Affine3d& ref_pose,
                                                                double default_pos_tol = 0.0005, double default_rot_tol = M_PI);

  bool decodeCartesianConstraint(moveit::core::RobotModelConstPtr model,const moveit_msgs::Constraints& constraints,
                                 Eigen::Affine3d& tool_pose, std::vector<double>& tolerance, std::string target_frame = "");

  bool decodeCartesianConstraint(moveit::core::RobotModelConstPtr model,const moveit_msgs::Constraints& constraints,
                                 Eigen::Affine3d& tool_pose, Eigen::VectorXd& tolerance,std::string target_frame = "");

  EigenSTL::vector_Affine3d sampleCartesianPoses(const moveit_msgs::Constraints& c,
                                                 const std::vector<double> sampling_resolution = {0.05, 0.05, 0.05, M_PI_2,M_PI_2,M_PI_2},
                                                 int max_samples =20 );


  void computeTwist(const Eigen::Affine3d& p0,
                                          const Eigen::Affine3d& pf,
                                          const Eigen::ArrayXi& nullity,Eigen::VectorXd& twist);

  void reduceJacobian(const Eigen::MatrixXd& jacb,
                                            const std::vector<int>& indices,Eigen::MatrixXd& jacb_reduced);

  void calculateDampedPseudoInverse(const Eigen::MatrixXd &jacb, Eigen::MatrixXd &jacb_pseudo_inv,
                                           double eps = 0.011, double lambda = 0.01);

  bool computeJacobianNullSpace(moveit::core::RobotStatePtr state,std::string group,std::string tool_link,
                                       const Eigen::ArrayXi& constrained_dofs,const Eigen::VectorXd& joint_pose,
                                       Eigen::MatrixXd& jacb_nullspace);

} // kinematics
} // utils
} // stomp_moveit



#endif /* INDUSTRIAL_MOVEIT_STOMP_MOVEIT_INCLUDE_STOMP_MOVEIT_UTILS_KINEMATICS_H_ */