p_itrans.cpp

日本V01机器人运动学算法

/***********************************************/
/* WFMC Positioner Invert_Kinematics Function  */
/*		Author:	Qiu Tao				           */
/*      Date: 1999-12-30			           */
/***********************************************/

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

PJoint P_InvertTransform(const PJoint &Jp,const TransMatrix &T,ErrMessage &Err)
{
	PJoint Jt;
	double temp;

	/*----- joint 1 -----*/
	Jt.pJoint[0]=T.tmData[2][3];

	if(Jt.pJoint[0]<JMIN[6])
	{
		Err.emJoint[6]=-1;
		return Jp;
	}
	else if(Jt.pJoint[0]>JMAX[6])
	{
		Err.emJoint[6]=1;
		return Jp;
	}
	else
	{
		Err.emJoint[6]=0;
	}

	/*----- joint 2 -----*/
	Jt.pJoint[1]=atan2(T.tmData[2][2],T.tmData[0][2]);

	if(Jt.pJoint[1]<JMIN[7])
	{
		Err.emJoint[7]=-1;
		return Jp;
	}
	else if(Jt.pJoint[1]>JMAX[7])
	{
		Err.emJoint[7]=1;
		return Jp;
	}
	else
	{
		Err.emJoint[7]=0;
	}

	/*----- joint 3 -----*/
	Jt.pJoint[2]=atan2(-T.tmData[1][0],-T.tmData[1][1]);
	temp=Jp.pJoint[2]-Jt.pJoint[2];

	if(temp>HPI)		Jt.pJoint[2]+=DPI;
	else if(temp<-HPI)  Jt.pJoint[2]-=DPI;

	if(Jt.pJoint[2]<JMIN[8])
	{
		Err.emJoint[8]=-1;
		return Jp;
	}
	else if(Jt.pJoint[2]>JMAX[8])
	{
		Err.emJoint[8]=1;
		return Jp;
	}
	else
	{
		Err.emJoint[8]=0;
	}

	return Jt;
}