artic.h

Go to the documentation of this file.

#ifndef RB_ARTIC_H_
#define RB_ARTIC_H_

#include "kinematic_chain.h"


namespace rb 
{
namespace kin 
{
// Class of kinematics of articulated (6-axis) robot arm
class Artic : public KinematicChain
{
public:
  Artic();

  Artic(
      const rb::math::VectorX& a0,              
      const rb::math::VectorX& alpha0,          
      const rb::math::VectorX& d0,              
      const rb::math::VectorX& ini_theta,       
      const rb::math::VectorX& uplimit0,        
      const rb::math::VectorX& lowlimit0        
      );

  Artic(
      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"                                
      );

  ~Artic();

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

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

  IK_RESULT inverseKin(
      const double& x,            
      const double& y,            
      const double& z,            
      const double& roll,         
      const double& pitch,        
      const double& yaw,          
      rb::math::VectorX& joints,  
      ArmAxisValue& all_sols      
      );

  void solvePitchPitchIK(const double& th1_rad, const rb::math::Vector4& p0,
      const std::vector<bool>& config,
      ArmAxisValue& all_sols);

  void solveRowPitchRowIK(const double& th1_rad, const std::vector<bool>& config,
      const rb::math::Matrix4& flange_tr,
      ArmAxisValue& all_sols);

  IK_RESULT solutionCheck(ArmAxisValue& sols);

  void preCheck(const int& njoint, double& rad);

  ArmPose getArmPose(void) const;

  rb::math::VectorX getA(void) const;

  rb::math::VectorX getAlpha(void) const;

  rb::math::VectorX getD(void) const;

  rb::math::VectorX getTheta(void) const;

  void setUpLimit(rb::math::VectorX& up_lim);

  rb::math::VectorX getUpLimit(void) const;

  void setLowLimit(rb::math::VectorX& low_lim);

  rb::math::VectorX getLowLimit(void) const;

protected:
  rb::math::VectorX a;                          

  rb::math::VectorX alpha;                      

  rb::math::VectorX d;                          

  rb::math::VectorX theta;                      
  rb::math::VectorX up_lim_;                    
  rb::math::VectorX low_lim_;                   

private:
  rb::math::VectorX ini_theta_;                 
  rb::math::VectorX pre_theta_;                 
  rb::math::Matrix4 base_tcp_tf_;              
  ArmPose tcp_pose_;                           
  int pre_fit_solution_;                        

  // 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);
  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_ARTIC_H_