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 <memory>
 
#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<std::unique_ptr<rb::kin::Link>>& links,
      rb::math::Matrix4  base=rb::math::Matrix4::Identity(),
      rb::math::Matrix4  tool=rb::math::Matrix4::Identity(),
      rb::math::Vector3  gravity=rb::math::Vector3(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<std::unique_ptr<rb::kin::Link>> links_;
 
  std::vector<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_;                         
 
};
 
}       // namespace kin
}       // namespace rb
 
#endif  // RB_KINEMATIC_CHAIN_H_