rigid.cpp

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

// Rigid.cpp: implementation of the CRigid class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "MySDOpenGL.h"
#include "Rigid.h"

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

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

CRigid::CRigid()
{
       GInertiaM.MakeUnitMatrix(6);
}

CRigid::~CRigid()
{

}
//////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////
// 重载运算符=，给刚体类赋值
//
// 参数：
// 1. const CMatrix& other - 用于给刚体类赋值的源刚体类
//
// 返回值：CRigid型的引用，所引用的刚体类与other相等
//////////////////////////////////////////////////////////////////////
CRigid & CRigid::operator=(const CRigid& other)
{
	xi=other.xi;
	q=other.q;
	r=other.r;
	p=other.p;
	l=other.l;
	m=other.m;
    InertiaT=other.InertiaT;
    GInertiaM=other.GInertiaM;
    TwistExp=other.TwistExp;


	// finally return a reference to ourselves
	return *this ;
}


//////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////
// 由给定的Q，R，M，惯性张量生成广义惯性矩阵GInertiaM
//
// 参数：
// Q，R，M，InertiaT在外面给定
//
// 返回值：无
//////////////////////////////////////////////////////////////////////
void  CRigid::MakeGInertiaM()
{//CVector3 p;
          p=q+r;
 CMatrix  mpso; 
 CMatrix  mpso2; 

		  mpso=AxisToSkew(p);
          mpso2=mpso*mpso;
          mpso=-mpso*m;
          mpso2=mpso2*m;

 CMatrix  mI;
          mI.MakeUnitMatrix(3);
		  mI=mI*m;

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


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

     for(i=0;i<3;i++)
		 for(j=0;j<3;j++)
             GInertiaM.m_pData[(i+3)*6+j]=-mpso.m_pData[i*3+j];

     for(i=0;i<3;i++)
		 for(j=0;j<3;j++)
             GInertiaM.m_pData[(i+3)*6+j+3]=InertiaT.m_pData[i*3+j]-mpso2.m_pData[i*3+j];

}

//////////////////////////////////////////////////////////////////////
// 由给定的xi，theta生成运动旋量指数TwistExp
//
// 参数：
// 1. CMatrix xi       - 运动旋量坐标ξ 在外面给定
// 2. double   theta   - 旋转角度θ
//
// 返回值：无
//////////////////////////////////////////////////////////////////////	
void   CRigid::MakeTwistExp(double theta)
{
     TwistExp=TwistExp2(xi,theta);
}