robot.h

挖掘机机器人算法

/*!
  @file robot.h
  @brief Robots class definitions.
*/

#define pi				3.14159

//lengths of excavator's working components
#define a1		200
#define a2		3720
#define a3		1620
#define a4		978
#define thetaB	52			//parameter of buckets

//angles in excavator when computing Drive Space
#define BAC 1
#define ACI 1
#define FCD 1
#define JLD 1
#define CDL 1
#define GDN 1
#define LKG 1

//lengths in excavator when computing Drive Space
//for BOOM's cylinder
#define AB 1
#define AH 1
#define HE 1
#define BE 1
//for ARM's cylinder
#define FC 1
#define CI 1
#define FI 1
//for BUCKET's cylinder
#define JL 1
#define KL 1
#define KG 1
#define GD 1
#define KL 1
#define LD 1

//末端位置向量
typedef struct
{
	double x;
	double y;
	double z;
}position;
//关节转角向量
typedef struct
{
	double q1;
	double q2;
	double q3;
	double q4;
}q;
//关节速度向量
typedef struct
{
	double qd1;
	double qd2;
	double qd3;
	double qd4;
}qd;
//关节加速度向量
typedef struct
{
	double qdd1;
	double qdd2;
	double qdd3;
	double qdd4;
}qdd;
//关节转矩向量
typedef struct
{
	double T1;
	double T2;
	double T3;
	double T4;
}T;

class CExcavator
{
public:
	CExcavator();										//Constructor
	~CExcavator();                                       //Destructor
	position coordinate(q* joint_angle);			//返回末端直角坐标位置
	Pos kin_JointSpace(Space* kJointSpace);   
	Space kin_DriveSpace(Space* kDriveSpace); 
	Space inv_JointSpace(Pos* pose); 
	Space inv_DriveSpace(Space* iDriveSpace);

	//dynamics computation

};