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<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"                                
      );
 
  ~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;
 
  rb::kin::ArmPose getJointPos(const int& jnt);
 
  rb::math::Matrix4 getJointFrame(const int& jnt);
 
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_;                        
};
 
}       // namespace kin
}       // namespace rb
 
#endif  // RB_ARTIC_H_