robot.cpp

挖掘机机器人算法

//implementation of class robot
#include "robot.h"
#include <math.h>

#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif

CExcavator::CExcavator()
{
}

CExcavator::~CExcavator()
{
}

Pos CExcavator::kin_JointSpace(Space* kJointSpace)
{	
	//compute position&pose from Joint Space
	Pos Ende;
	double theta1,theta2,theta3,theta4;

	theta1=kJointSpace->element1*pi/180;
	theta2=kJointSpace->element2*pi/180;
	theta3=kJointSpace->element3*pi/180;
	theta4=kJointSpace->element4*pi/180;

	Ende.x=cos(theta4)*(length_bucket*cos(theta2+theta3+theta4)+length_arm*cos(theta2+theta3)+length_boom*cos(theta2)+offset);
	Ende.y=sin(theta4)*(length_bucket*cos(theta2+theta3+theta4)+length_arm*cos(theta2+theta3)+length_boom*cos(theta2)+offset);
	Ende.z=length_bucket*sin((theta2+theta3+theta4))+length_arm*sin(theta2+theta3)+length_boom*sin(theta2);
	Ende.pose=pi+theta2+theta3+theta4;

	return Ende;
}

Space kin_DriveSpace(Space* kDriveSpace)
{
	//convert Drive Space into Jiont Space
	//Ignore swinging
	Space kJS;
	double lBE,lFI,lJK;	//the lenghts of the three actuators.
	double h1,h2,h3;
	double e1,e2;

	lBE=kDriveSpace->element2;
	lFI=kDriveSpace->element3;
	lJK=kDriveSpace->element4;
	
	//for BOOM's angle
	h1=AB*AB+AH*AH+HE*HE-lBE*lBE;
	kJS.element2=-BAC-atan(AH/HE)*180/pi+atan(h1*h1/sqrt(4*AB*AB*(AH*AH+HE*HE)-h1*h1))*180/pi;

	//for ARM's angle
	h2=FC*FC+CI*CI-lFI*lFI;
	kJS.element3=3*180-ACI-FCD-atan(sqrt(4*FC*FC*CI*CI-pow(h2,4))/h2/h2)*180/pi;

	//for BUCKET's angle
	h3=JL*JL+KL*KL-lJK*lJK;
	e1=JLD-atan(sqrt(4*JL*JL*KL*KL-pow(h3,4))/h3/h3)*180/pi;
	e2=acos((KG*KG+GD*GD-KL*KL-LD*LD+2*KL*LD*cos(e1*pi/180))/2/KG/GD)*180/pi;
	kJS.element4=180+CDL+GDN-e1-e2-LKG;

	return kJS;
}

Space CExcavator::inv_JointSpace(Pos* pose)
{
	//calculate the inverse kinematics using geometry method.
	//only considered two kinds situation:point B inside the triangle ACD and outside it.
	Space iJS;
	double x,y,z,xc,yc,zc;		//cordinations of B and C
	double theta,theta1,theta2,theta3,theta4;
	double gama,egi,alfa,beta;
	double ab,ac;

	x=pose->x;
	y=pose->y;
	z=pose->z;
	theta=pose->pose;

	theta1=atan(y/x);
	xc=x-length_bucket*cos(theta*pi/180)*cos(theta1);
	yc=y-length_bucket*cos(theta*pi/180)*sin(theta1);
	zc=z-length_bucket*sin(theta*pi/180);
	gama=atan2(z,sqrt(x*2+y*2));
	ab=sqrt(x*2+y*2+z*2);
	if((z/sqrt(x*2+y*2))<=(zc/sqrt(xc*2+yc*2)))
	{
		//Point B is outside the triangle ACD
		egi=fabs(theta*pi/180-gama);
		ac=sqrt(ab*2+length_bucket*length_bucket-2*length_bucket*ab*cos(egi));
		alfa=acos((length_boom*length_boom+ac*ac-length_arm*length_arm)/(2*length_boom*ac));
		beta=acos((ac*ac+ab*ab-length_bucket*length_bucket)/(2*ab*ac));
		theta2=gama+alfa+beta;    
	}
	else
	{
		//Point B is inside the triangle ACD
		egi=2*pi-fabs(theta*pi/180-gama);
		ac=sqrt(ab*2+length_bucket*length_bucket-2*length_bucket*ab*cos(egi));
		alfa=acos((length_boom*length_boom+ac*ac-length_arm*length_arm)/(2*length_boom*ac));
		beta=acos((ac*ac+ab*ab-length_bucket*length_bucket)/(2*ab*ac));
		theta2=gama+alfa-beta;   
	}	
	theta3=acos((length_boom*length_boom+length_arm*length_arm-ac*ac)/(2*length_boom*length_arm))-pi;//0～-180
	theta4=theta*pi/180-theta2-theta3;

	iJS.element1=theta1*180/pi;
	iJS.element2=theta2*180/pi;
	iJS.element3=theta3*180/pi;
	iJS.element4=theta4*180/pi;

	return iJS;
}

Space CExcavator::inv_DriveSpace(Space* iDriveSpace)
{
	//convert Drive Space into Jiont Space
	//Ignore swinging
	Space actuators;
	double h3;
	double theta1,theta2,theta3,theta4;	//the angles in joint space. 

	theta1=iDriveSpace->element1;
	theta2=iDriveSpace->element2;
	theta3=iDriveSpace->element3;
	theta4=iDriveSpace->element4;

	//h3=JL*JL+KL*KL-JK*JK;	//??have a problem:how could such a equation be sovled??
	actuators.element1=0;
	actuators.element2=pow((AB*sin(theta2+BAC)+AH),2)+pow((AB*cos(theta2+BAC)-HE),2);
	actuators.element3=FC*FC+CI*CI-2*FC*CI*cos(3*pi-ACI-FCD-theta3);
	//actuators.element4=JL*JL+JK*KL-2*JL*KL*cos(atan(sqrt(4*JL*JL*KL*KL-pow(h3,4))/pow(h3,2)));

	return actuators;
}