p_ptrans.cpp

日本V01机器人运动学算法

/************************************/
/* WFMC positioner Kinematics Fun.	*/
/*		Author:	Qiu Tao				*/
/*      Date: 1999-12-30			*/
/************************************/

#include "mp_para.h"
#include "define.h"
#include <math.h>

TransMatrix P_PositiveTransform(const PJoint &J)
{
	TransMatrix T;
	double s2,c2,s3,c3;

	s2=sin(J.pJoint[1]+HPI);
	c2=cos(J.pJoint[1]+HPI);
	s3=sin(J.pJoint[2]);
	c3=cos(J.pJoint[2]);

/*---------- column 1 ----------*/
	T.tmData[0][0]=c2*c3;
	T.tmData[1][0]=-s3;
	T.tmData[2][0]=s2*c3;

/*---------- column 2 ----------*/
	T.tmData[0][1]=-c2*s3;
	T.tmData[1][1]=-c3;
	T.tmData[2][1]=-s2*s3;

/*---------- column 3 ----------*/
	T.tmData[0][2]=s2;
	T.tmData[1][2]=0.0;
	T.tmData[2][2]=-c2;

/*---------- column 4 ----------*/
	T.tmData[0][3]=0.0;
	T.tmData[1][3]=0.0;
	T.tmData[2][3]=J.pJoint[0];

	return T;
}