screws.cpp

机器人运动学动力学及控制的一个小程序,供有兴趣的人参考

// Screws.cpp: implementation of the CScrews class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "MySDOpenGL.h"
#include "Screws.h"
#include <math.h>

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

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

CScrews::CScrews()
{

}

CScrews::~CScrews()
{

}

//////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////
// 将三维向量转化为齐次坐标
//
// 参数：
// 1. CVector3 *vc - 需要转换的三维矢量
// 2. int point    - 分三种情况 point<0 转换成三行一列矩阵
//                              point=0 转换成点的四行一列矩阵
//                              point=1 转换成矢量的四行一列矩阵
//
// 返回值：转换成的矩阵
//////////////////////////////////////////////////////////////////////
	CMatrix Vector3ToMatrix41(CVector3 *vc,int point)
{   if(point<0)
	{double v1[3]={vc->x,vc->y,vc->z};
    CMatrix result1(3,1,v1);
    return result1;
	}
	else
	{
	double v[4]={vc->x,vc->y,vc->z,point};
    CMatrix result(4,1,v);
    return result;
	}
}

//////////////////////////////////////////////////////////////////////
// 将齐次坐标转化为三维向量
//
// 参数：
// 1. int num    - 开始行
// 2. int col    - 开始列
// 3. CMatrix m  - 需转换的矩阵                       
//                              
//
// 返回值：转换成的矢量
//////////////////////////////////////////////////////////////////////
   CVector3 MatrixToVector3(int num,int col,CMatrix m)
   {CVector3 result;
//    double *vector=new double[num];
      double vector[6];
               m.GetColVector(col,vector);
			   result.x=vector[num+0];
			   result.y=vector[num+1];
			   result.z=vector[num+2];

//               delete vector;

               return result;

   }
//////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////
// 由给定矢量生成为反对称矩阵 ω^
//
// 参数：
// 1. CVector3 omiga   - 矢量 ω                    
//                              
//
// 返回值：转换成的反对称矩阵 ω^
//////////////////////////////////////////////////////////////////////
CMatrix AxisToSkew(CVector3 omiga)
{   double  v[9]={0,-omiga.z,omiga.y,omiga.z,0,-omiga.x,-omiga.y,omiga.x,0};
	CMatrix result(3,v);

	return result;
    
}

//////////////////////////////////////////////////////////////////////
// 由给定ω,q,h生成为运动旋量坐标 ξ
//
// 参数：
// 1. CVector3 omiga   - 矢量 ω                    
// 2. CVector3 q       - 轴线上的点q
// 3. double   h       - 节距 h   当fabs(h)>1e8时为平移旋量          
//
// 返回值：转换成的运动旋量坐标 ξ
//////////////////////////////////////////////////////////////////////
CMatrix AxisToTwist(CVector3 omiga,CVector3 q,double h)
{  	 CMatrix result(6,1);

	if(fabs(h)<1e8 ){
	 double  v[3]={q.x,q.y,q.z};
     CMatrix mq(3,1,v);
     CMatrix momigaso=AxisToSkew(omiga);
	 CMatrix momiga=Vector3ToMatrix41(&omiga,-1);
             mq=-momigaso*mq+momiga*h;
             result.m_pData[0]=mq.m_pData[0];
             result.m_pData[1]=mq.m_pData[1];
             result.m_pData[2]=mq.m_pData[2];
             result.m_pData[3]=omiga.x;
             result.m_pData[4]=omiga.y;
             result.m_pData[5]=omiga.z;
}
else{
             result.m_pData[0]=omiga.x;
             result.m_pData[1]=omiga.y;
             result.m_pData[2]=omiga.z;
	         result.m_pData[3]=0;
             result.m_pData[4]=0;
             result.m_pData[5]=0;


}
     return  result;

}

//////////////////////////////////////////////////////////////////////
// 由给定ω,θ生成为反对称矩阵指数 exp(ωθ)
//
// 参数：
// 1. CVector3 omiga   - 矢量 ω                    
// 2. double theta     - 旋转角度θ
//
// 返回值：转换成的反对称矩阵指数 exp(ωθ)
//////////////////////////////////////////////////////////////////////
CMatrix SkewExp(CVector3 omiga,double theta)
{	 CMatrix result(3);
             result.MakeUnitMatrix(3);
     CMatrix momiga=AxisToSkew(omiga);

             result=result+momiga*sin(theta*PI/180)+momiga*momiga*(1-cos(theta*PI/180));
     return  result;         
}

//////////////////////////////////////////////////////////////////////
// 由给定ω,θ,q,h生成为运动旋量指数 exp(ξθ)
//
// 参数：
// 1. CVector3 omiga   - 矢量 ω                    
// 2. double   theta   - 旋转角度θ
// 3. CVector3 q       - 轴线上的点q
// 4. double   h       - 节距 h   当fabs(h)>1e8时为平移旋量指数
//
// 返回值：转换成的运动旋量指数 exp(ξθ)
//////////////////////////////////////////////////////////////////////
CMatrix TwistExp1(CVector3 omiga,double theta,CVector3 q,double h)
{    CMatrix result(4);
             result.MakeUnitMatrix(4);

   if(fabs(h)<1e8 ){
     CMatrix SkewE=SkewExp(omiga,theta);
// CString	m_strMatrix3 = SkewE.ToString(" ");
//	AfxMessageBox(m_strMatrix3, MB_OK|MB_ICONINFORMATION);
     CMatrix temp(3);
             temp.MakeUnitMatrix(3);
	 CMatrix mq=Vector3ToMatrix41(&q,-1);
	 CMatrix momiga=Vector3ToMatrix41(&omiga,-1);

			  mq=(temp-SkewE)*mq+momiga*h*theta;
// CString	m_strMatrix3 = mq.ToString(" ");
//	AfxMessageBox(m_strMatrix3, MB_OK|MB_ICONINFORMATION);

	 for(int i=0;i<3;i++)
		 for(int j=0;j<3;j++)
             result.m_pData[i*4+j]=SkewE.m_pData[i*3+j];

     for(i=0;i<3;i++)   
		     result.m_pData[i*4+3]=mq.m_pData[i];
   }
   else{
             result.m_pData[0*4+3]=omiga.x*theta;
             result.m_pData[1*4+3]=omiga.y*theta;
             result.m_pData[2*4+3]=omiga.z*theta;

   }
	 return result;
}

//////////////////////////////////////////////////////////////////////
// 由给定ξ,θ生成为运动旋量指数 exp(ξθ)
//
// 参数：
// 1. CMatrix xi       - 运动旋量坐标ξ                    
// 2. double   theta   - 旋转角度θ
//
// 返回值：转换成的运动旋量指数 exp(ξθ)
//////////////////////////////////////////////////////////////////////
 CMatrix TwistExp2(CMatrix xi,double theta)
 {   CMatrix result(4);
             result.MakeUnitMatrix(4);
 
	 CVector3 omiga=MatrixToVector3(3,0,xi);
	 CVector3 v=MatrixToVector3(0,0,xi);
if(omiga!=0){
     CVector3 tempwxv=omiga^v;
     CVector3 tempwwv=omiga*(omiga*v)*theta;
     CMatrix temp1=Vector3ToMatrix41(&tempwxv,-1);
     CMatrix temp2=Vector3ToMatrix41(&tempwwv,-1);

     CMatrix SkewE=SkewExp(omiga,theta);

     CMatrix temp(3);
             temp.MakeUnitMatrix(3);
			 
     CMatrix p=(temp-SkewE)*temp1+temp2;

	 for(int i=0;i<3;i++)
		 for(int j=0;j<3;j++)
             result.m_pData[i*4+j]=SkewE.m_pData[i*3+j];

     for(i=0;i<3;i++)   
		     result.m_pData[i*4+3]=p.m_pData[i];
	 }
else{
             result.m_pData[0*4+3]=v.x*theta;
             result.m_pData[1*4+3]=v.y*theta;
             result.m_pData[2*4+3]=v.z*theta;

	 }
      return result;
 }

//////////////////////////////////////////////////////////////////////
// 由给定ξ转换为4x4运动旋量 ξ^
//
// 参数：
// 1. CMatrix xi       - 运动旋量坐标ξ                    
//
// 返回值：转换成的为4x4运动旋量 ξ^
//////////////////////////////////////////////////////////////////////
 CMatrix TwistToHomogeneous(CMatrix xi)
 {   CMatrix result(4);

     CVector3 omiga;
	          omiga.x=xi.m_pData[3];
	          omiga.y=xi.m_pData[4];
	          omiga.z=xi.m_pData[5];
     CMatrix  omigaso=AxisToSkew(omiga);

     for(int i=0;i<3;i++)
		 for(int j=0;j<3;j++)
             result.m_pData[i*4+j]=omigaso.m_pData[i*3+j];

     for(i=0;i<3;i++)   
		     result.m_pData[i*4+3]=xi.m_pData[i];

	 return result;

 }

//////////////////////////////////////////////////////////////////////
// 由给定4x4运动旋量ξ^转换为运动旋量坐标ξ 
//
// 参数：
// 1. CMatrix xise      - 运动旋量ξ^                    
//
// 返回值：转换成的运动旋量坐标 ξ
//////////////////////////////////////////////////////////////////////
 CMatrix HomogeneousToTwist(CMatrix xise)
 {   CMatrix result(6,1);

     for(int i=0;i<3;i++)   
		     result.m_pData[i]=xise.m_pData[i*4+3];

             result.m_pData[3]=xise.GetElement(2,1);
             result.m_pData[4]=xise.GetElement(0,2);
             result.m_pData[5]=xise.GetElement(1,0);
     return result;
 }

//////////////////////////////////////////////////////////////////////
// 由给定ξ1,ξ2生成李氏括号[ξ1,ξ2] 
//
// 参数：
// 1. CMatrix xi1      - 运动旋量坐标ξ1
// 2. CMatrix xi2      - 运动旋量坐标ξ2
//
// 返回值：转换成的李氏括号[ξ1,ξ2]
//////////////////////////////////////////////////////////////////////
 CMatrix LiBrackets(CMatrix xi1,CMatrix xi2)
 {   CMatrix result(6,1);

     CMatrix  sexi1=TwistToHomogeneous(xi1);
     CMatrix  sexi2=TwistToHomogeneous(xi2);
     CMatrix  seresult=sexi1*sexi2-sexi2*sexi1;
              result=HomogeneousToTwist(seresult);
     return result;
   
 }

//////////////////////////////////////////////////////////////////////
// 由给定运动旋量指数 exp(ξθ)取得位置矢量p 
//
// 参数：
// 1. CMatrix g      - 运动旋量指数 exp(ξθ)
//
// 返回值：转换成的位置矢量p
//////////////////////////////////////////////////////////////////////
 CVector3 RigidPosition(CMatrix g)
 {   CVector3 result;
              result=MatrixToVector3(0,3,g);
     return result;
 }
 
//////////////////////////////////////////////////////////////////////
// 由给定运动旋量指数 exp(ξθ)取得反对称矩阵指数 exp(ωθ) 
//
// 参数：
// 1. CMatrix g      - 运动旋量指数 exp(ξθ)
//
// 返回值：转换成的反对称矩阵指数 exp(ωθ)
//////////////////////////////////////////////////////////////////////
 CMatrix RigidOrientation(CMatrix g)
 {   CMatrix result(3);

     for(int i=0;i<3;i++)
		 for(int j=0;j<3;j++)
             result.m_pData[i*3+j]=g.m_pData[i*4+j];
     return result;
 }

//////////////////////////////////////////////////////////////////////
// 由给定运动旋量指数 exp(ξθ)生成伴随矩阵Adg
//
// 参数：
// 1. CMatrix g      - 运动旋量指数 exp(ξθ)
//
// 返回值：转换成的伴随矩阵Adg
//////////////////////////////////////////////////////////////////////
 CMatrix RigidAdjoint(CMatrix g)
 {   CMatrix result(6);

     CVector3 p=RigidPosition(g);
     CMatrix pso=AxisToSkew(p);
     CMatrix R=RigidOrientation(g);
     CMatrix pR=pso*R;
// CString	m_strMatrix3 = g.ToString(" ");
//	AfxMessageBox(m_strMatrix3, MB_OK|MB_ICONINFORMATION);
    
	 int i,j;
     for(i=0;i<3;i++)
		 for(j=0;j<3;j++)
             result.m_pData[i*6+j]=R.m_pData[i*3+j];

     for(i=0;i<3;i++)
		 for(j=0;j<3;j++)
             result.m_pData[(i+3)*6+j+3]=R.m_pData[i*3+j];

     for(i=0;i<3;i++)
		 for(j=0;j<3;j++)
             result.m_pData[i*6+j+3]=pR.m_pData[i*3+j];

     return result;
 }

//////////////////////////////////////////////////////////////////////
// 由给定运动旋量指数 exp(ξθ)生成伴随矩阵逆阵Adg-1
//
// 参数：
// 1. CMatrix g      - 运动旋量指数 exp(ξθ)
//
// 返回值：转换成的伴随矩阵逆阵Adg-1
//////////////////////////////////////////////////////////////////////
 CMatrix InvertRigidAdjoint(CMatrix g)
 {   CMatrix result(6);

     CVector3 p=RigidPosition(g);
     CMatrix pso=AxisToSkew(p);
     CMatrix R=RigidOrientation(g);

//	   CString	m_strMatrix3 =  RT.ToString(" ");
//	AfxMessageBox(m_strMatrix3, MB_OK|MB_ICONINFORMATION);

	 CMatrix RT=R.Transpose();
     CMatrix RTp=-RT*pso;

//	  	m_strMatrix3 =  RT.ToString(" ");
//	AfxMessageBox(m_strMatrix3, MB_OK|MB_ICONINFORMATION);

	 int i,j;
     for(i=0;i<3;i++)
		 for(j=0;j<3;j++)
             result.m_pData[i*6+j]=RT.m_pData[i*3+j];

     for(i=0;i<3;i++)
		 for(j=0;j<3;j++)
             result.m_pData[(i+3)*6+j+3]=RT.m_pData[i*3+j];

     for(i=0;i<3;i++)
		 for(j=0;j<3;j++)
             result.m_pData[i*6+j+3]=RTp.m_pData[i*3+j];

     return result;

 }