kinematic_chain.h

Go to the documentation of this file.

#ifndef RB_KINEMATIC_CHAIN_H_
#define RB_KINEMATIC_CHAIN_H_
#include <iostream>
#include <iomanip>
#include <string>
#include <vector>
#include <array>

#include "link.h"

#include <Eigen/StdVector>

namespace rb 
{
namespace kin 
{
enum IK_RESULT
{
  IK_COMPLETE,        
  IK_NO_SOLUTION,     
  IK_ANGLE_LIMIT,     
  IK_SINGULAR,        
  IK_INPUT_INVALID    
};

struct ArmPose
{
  double x;   
  double y;   
  double z;   

  double a;   
  double b;   
  double c;   
};

std::ostream& operator<< (std::ostream& ost, const ArmPose& pose);

#if __cplusplus == 201103L

template<typename T>
  void ostrVec(std::ostream& ost, std::vector<T> vec_inp, int width=12)
  {
    ost << '\n';
    for(auto& inp : vec_inp)
      ost << std::right << std::setw(width) << inp;
    return;
  }
#endif

struct ArmAxisValue
{
  rb::math::MatrixX axis_value;
  int fit;                                        
  std::array<bool, 8> solution_check = {{0}};     
  std::array<bool, 8> singular_check = {{0}};     
  std::array<bool, 8> limit_check = {{0}};        

  ArmAxisValue()
  {
    axis_value = rb::math::MatrixX::Constant(8, 6, 0.0);
    fit = 0;
    for (int i = 0; i < 8; i++)
    {
      solution_check[i] = singular_check[i] = limit_check[i] = 0;
    }
  }
};


class KinematicChain
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  KinematicChain();

  KinematicChain(
      std::vector<rb::kin::Link*>& links,
      rb::math::Matrix4  base=rb::math::Matrix4::Identity(),
      rb::math::Matrix4  tool=rb::math::Matrix4::Identity(),
      rb::math::Vector3  gravity={0., 0., rb::math::GRAVITY},
      std::string manufactor="None",
      std::string model="None"
      );

  virtual ~KinematicChain();

  virtual ArmPose forwardKin(const rb::math::VectorX& q, const bool update=true);

  virtual IK_RESULT inverseKin(
          const rb::math::Matrix4& world_tcp_tf,  
          rb::math::VectorX& joints,              
          ArmAxisValue& all_sols                  
          ) = 0;

  rb::math::Matrix4 homoTrans(double& A, double& alpha, double& D, const double& theta);

  bool setTool(const ArmPose& tool_pose);

  bool getTool(ArmPose& tool_pose) const;

  void setBase(const rb::math::Matrix4& base);

  rb::math::Matrix4 getBase(void) const;

  rb::math::Matrix4 getTCP(void) const;

  void setDOF(void);

  int getDOF(void) const;

protected:
  std::vector<rb::kin::Link*> links_;
  //std::vector<rb::kin::Link*, Eigen::aligned_allocator<rb::kin::Link*> > links_;

  std::vector<rb::math::Matrix4*> frames_;
  //std::vector<rb::math::Matrix4*, Eigen::aligned_allocator<rb::math::Matrix4*> > frames_;

  int dof_;                                   
  rb::math::Matrix4 base_tf_;                 
  rb::math::Matrix4 tool_tf_;                 
  rb::math::Matrix4 world_tcp_tf_;            
  rb::math::Vector3 gravity_;                 

  std::string manufactor_;                    
  std::string model_;                         

private:
  // private functions
  // TODO: could move functions for matrix manipulating, such as rpy2tr & tr2rpy to math.h
  void tr2rpy(const rb::math::Matrix4& m, double& roll_z, double& pitch_y, double& yaw_x) const;
  void rpy2tr(double& roll_z, double& pitch_y, double& yaw_x, rb::math::Matrix4& tool_mat);
  rb::math::Matrix4 rotateX(const double& deg);
  rb::math::Matrix4 rotateY(const double& deg);
  rb::math::Matrix4 rotateZ(const double& deg);
};

}       // namespace kin
}       // namespace rb

#endif  // RB_KINEMATIC_CHAIN_H_