trans.h

日本V01机器人运动学算法

//Laser scan head file

struct  trans;

struct  joint
{
	float j[6];

	joint operator+(joint J2);
	joint operator-(joint J2);
	joint operator*(joint D1);
	joint operator/(int n);
	trans DtoT(void);

};

struct  movement;

struct  trans
{
	float t[4][3];     // Translation matrix T.t[n o a p][x y z]

	friend joint operator*(trans T, joint A);
	movement operator*(movement B);
	movement operator%(movement B);
	trans operator=(trans Tr);
	trans operator*(trans T);

//	trans DtoT(void);
	trans rotate(trans *ts0p, char worldr);
	trans tdelta(trans *tmp);
	trans tdeltw(trans T);
	trans tdrive(float sita,float fi,float lamd );
	trans trans::Tinvert();
};

//...........................
struct  movement
{
	float p[3];

	friend movement getposit(joint M);
	friend movement getrotat(joint M);
	friend movement trans::operator*(movement B);
	friend movement trans::operator%(movement B);
	float    operator*(movement B);
	movement movement::operator%(movement B);
	movement movement::operator*(float k);
	movement movement::operator/(float k);
	movement movement::operator-(movement B);
	movement movement::operator+(movement B);
	movement movement::operator/(int n);
};
//----------------------

#define QUEUE_MAXSIZE 400     //queue which save the scanned trace point
#define NSEND         100

const trans T_SENSOR = {    // used in 6th and 11th func.
		//{ 0.999784673, -0.00350164,  0.02045,       -1.00 },
		//{ 0.003539239,  0.999992112,-0.001802,      29.713},
		//{-0.020446999,  0.001874254, 0.999790435,  146.676},
/*		{{  0.999784673, 0.003539239,-0.020446999  },
		 { -0.00350164,  0.999992112, 0.001874254  },
		 {  0.02045,    -0.001802,    0.999790435  },
		 {  -1.00,       29.713,      146.676      }}
		 };
*/
/*		{{  1,   0,    0  },
		 {  0,  -1,    0  },
		 {  0,   0,   -1  },
		 //	 {  -1.20,      22.35,     -123.53         }}
		   //{  -2.20,      22.35,     -128.53         }}
		 { // 0.9,      19,     -120.53      
			-1.5, -31.0, 113.0  }}
*/


		{
		 {  1,   0,    0  },
		 {  0,  -1,    0  },
		 {  0,   0,   -1  },
//		  { 0.99802,  0.04935,  -0.03907 },
//		  { 0.04939, -0.99878,   0},
//		  {-0.039022,-0.0019298,-0.9992365},

		  //plane
		  { -0.6,   -30.3,    112.4} }

		  //space
//		  { -0.6,   -30.3,    125.0} }

	  };



// *** the torch's tool coordinate of robot's six joint ***
/*
const float TORCH_X =    0.0;
const float TORCH_Y =    7.5;
const float TORCH_Z =  271.0; //268.9;
const movement TORCH={ {TORCH_X, TORCH_Y, TORCH_Z} };
const trans T_TORCH={ {{1,0,0},{0,1,0},{0,0,1},
					   {TORCH_X, TORCH_Y, TORCH_Z}} };
*/
//const movement UNITM ={ {0, 0, 0} };
const movement ZEROM ={ {0, 0, 0} };
const joint    ZEROJ ={ {0, 0, 0, 0, 0, 0} };
/*
//const int NN=2;
const float TRACE_LINE_LENGTH = 8.0;    // 10mm
const float SPEED = (20/6);                // 40mm/s<-[cm/min]
const float STEP = (SPEED*0.028);
const int   STEP_NUMBER = 5;   //TRACE_LINE_LENGTH/(SPEED*0.085);~=30
const float STEP_LENGTH = (0.5*STEP); //0.25; // =17.4mm/s /2, 1*, 3*STEP
const float STEP_DEGREE = (0.01*SPEED);         //   0.0001*, 0.0005*STEP
	//(0.01745*20DGR/s*0.028s*SPEED/10);// 20DGR/s if SPEED=10mm/s.
*/
//const int   STEP_LENGTH_ROBOT=(int)(STEP_LENGTH*32.0 + 0.5);