basic_kin.h

Go to the documentation of this file.

#ifndef BASIC_KIN_H
#define BASIC_KIN_H

#include <vector>
#include <string>
#include <boost/scoped_ptr.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <kdl/tree.hpp>
#include <kdl/chain.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/chainjnttojacsolver.hpp>
#include <moveit/robot_model/joint_model_group.h>

namespace constrained_ik
{
namespace basic_kin
{

class BasicKin
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  BasicKin() :
    initialized_(false),
    group_(NULL)
  {

  }

  bool calcFwdKin(const Eigen::VectorXd &joint_angles, Eigen::Affine3d &pose) const;

  //TODO test
  bool calcFwdKin(const Eigen::VectorXd &joint_angles,
                  const std::string &base,
                  const std::string &tip,
                  KDL::Frame &pose) const;

  bool calcJacobian(const Eigen::VectorXd &joint_angles, Eigen::MatrixXd &jacobian) const;

  bool checkInitialized() const { return initialized_; }

  //TODO test
  bool checkJoints(const Eigen::VectorXd &vec) const;

  //TODO test
  bool getJointNames(std::vector<std::string> &names) const;

  Eigen::MatrixXd getLimits() const { return joint_limits_; }

  //TODO test
  bool getLinkNames(std::vector<std::string> &names) const;

  bool init(const moveit::core::JointModelGroup* group);

  const moveit::core::JointModelGroup* getJointModelGroup() const {return group_;}

  unsigned int numJoints() const { return robot_chain_.getNrOfJoints(); }

  bool getSubChain(const std::string link_name, KDL::Chain &chain) const;

  bool linkTransforms(const Eigen::VectorXd &joint_angles,
                      std::vector<KDL::Frame> &poses,
                      const std::vector<std::string> &link_names = std::vector<std::string>()) const;

  std::string getRobotBaseLinkName() const { return base_name_; }

  std::string getRobotTipLinkName() const { return tip_name_; }

  BasicKin& operator=(const BasicKin& rhs);

  bool solvePInv(const Eigen::MatrixXd &A, const Eigen::VectorXd &b, Eigen::VectorXd &x) const;

static  bool dampedPInv(const Eigen::MatrixXd &A, Eigen::MatrixXd &P, const double eps = 0.011, const double lambda = 0.01);

static void KDLToEigen(const KDL::Frame &frame, Eigen::Affine3d &transform);

static void KDLToEigen(const KDL::Jacobian &jacobian, Eigen::MatrixXd &matrix);

static void EigenToKDL(const Eigen::VectorXd &vec, KDL::JntArray &joints) {joints.data = vec;}

private:
  bool initialized_;                                             
  const moveit::core::JointModelGroup* group_;                   
  KDL::Chain  robot_chain_;                                      
  KDL::Tree   kdl_tree_;                                         
  std::string base_name_;                                        
  std::string tip_name_;                                         
  std::vector<std::string> joint_list_;                          
  std::vector<std::string> link_list_;                           
  Eigen::Matrix<double, Eigen::Dynamic, 2> joint_limits_;        
  boost::scoped_ptr<KDL::ChainFkSolverPos_recursive> fk_solver_; 
  boost::scoped_ptr<KDL::ChainJntToJacSolver> jac_solver_;       
  int getJointNum(const std::string &joint_name) const;

  int getLinkNum(const std::string &link_name) const;

}; // class BasicKin

} // namespace basic_kin
} // namespace constrained_ik


#endif // BASIC_KIN_H