dynamics.cpp

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

// Dynamics.cpp: implementation of the CDynamics class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "MySDOpenGL.h"
#include "Dynamics.h"

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

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

CDynamics::CDynamics()
{ 
	A=NULL;
	R=NULL;
	m_n=1;
	BOOL bSuccess =Init( m_n);
	ASSERT(bSuccess);


}

//////////////////////////////////////////////////////////////////////
// 指定关节数目构造函数
//
// 参数：
//  int n - 关节数目
//////////////////////////////////////////////////////////////////////
CDynamics::CDynamics(int n)
{
    A=NULL;
	R=NULL;
	BOOL bSuccess =Init( n);
	ASSERT(bSuccess);

}


CDynamics::~CDynamics()
{
	if (R)
	{
		delete[] R;
		R = NULL;
	}

	if (A)
	{
		delete[] A;
		A = NULL;
	}


}

//////////////////////////////////////////////////////////////////////
// 重载运算符=，给动力学类赋值
//
// 参数：
// 1. const CDynamics& other - 用于给动力学类赋值的源动力学类
//
// 返回值：CDynamics型的引用，所引用的动力学类与other相等
//////////////////////////////////////////////////////////////////////
CDynamics & CDynamics::operator=(const CDynamics& other)
{
	BOOL bSuccess = Init(other.m_n);
	ASSERT(bSuccess);

	m_n=other.m_n;
	A=other.A;
	R=other.R;
	M=other.M;
	C=other.C;
	N=other.N;

	return *this ;
}


////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////
// 初始化函数
//
// 参数：
// int n - 指定的关节数目
//
// 返回值：BOOL 型，初始化是否成功
//////////////////////////////////////////////////////////////////////
BOOL CDynamics::Init(int n)
{
	if (R)
	{
		delete[] R;
		R = NULL;
	}

	if (A)
	{
		delete[] A;
		A = NULL;
	}

	m_n=n;
	if (n < 0)
		return FALSE;

	R = new CRigid[n];
	A = new CMatrix[n*n];

	if (R == NULL)
		return FALSE;					// 内存分配失败
	if (IsBadReadPtr(R, sizeof(CRigid) * n))
		return FALSE;

	if (A == NULL)
		return FALSE;					// 内存分配失败
	if (IsBadReadPtr(A, sizeof(CMatrix) * n*n))
		return FALSE;



	return TRUE;
}

//////////////////////////////////////////////////////////////////////
// 由给定的各关节转角theta，生成各关节间的伴随变换矩阵A [Aij表示为(*(A+i*m_n+i))]
//
// 参数：
// theta    -各关节转角
//
// 返回值：BOOL 型，转化是否成功
//////////////////////////////////////////////////////////////////////
BOOL CDynamics::MakeA(CMatrix theta)
{int i,j,k;
CMatrix temM;

	for(i=0;i<m_n;i++)
	{
		(*(A+i*m_n+i)).MakeUnitMatrix(6);
		(*(R+i)).MakeTwistExp(theta.GetElement(i,0));
	}
	for(j=0;j<m_n;j++)
	{   temM.MakeUnitMatrix(4);
		for(i=j+1;i<m_n;i++)
		{
//			for(k=j+1;k<=i;k++)
                temM= temM*(*(R+i)).TwistExp;
          (*(A+i*m_n+j))=InvertRigidAdjoint(temM);
		}
	}
    return TRUE;
}

//////////////////////////////////////////////////////////////////////
// 由给定的各关节转角theta和theta的一阶导数deltatheta，生成动力学方程
//  中的惯性矩阵M（θ），哥氏力离心力矩阵C（θ，θ.）
//
// 参数：
// 1、theta        -各关节转角
// 2、deltatheta   -各关节转角的一阶导数
//
// 返回值：BOOL 型，转化是否成功
//////////////////////////////////////////////////////////////////////
BOOL CDynamics::MakeMC(CMatrix theta,CMatrix deltatheta)
{int i,j,l,k;
 double temp;
 CMatrix temxi;
 CMatrix temA;


     MakeA(theta);

	 M.MakeUnitMatrix(m_n);
	 C.MakeUnitMatrix(m_n);

	 for(i=0;i<m_n;i++)
		 for(j=0;j<m_n;j++)
		 {   temp=0;
			 l=max(i,j);
			 
			 for(k=l;k<m_n;k++)
			 {temxi=(*(R+i)).xi.Transpose();

//			 temxi.Transpose();
//CString	m_strMatrix3 = temxi.ToString(" ");
//	AfxMessageBox(m_strMatrix3, MB_OK|MB_ICONINFORMATION);
			  temA=(*(A+k*m_n+i)).Transpose();
//			  temA.Transpose();
//CString	m_strMatrix3 = temA.ToString(" ");
//	AfxMessageBox(m_strMatrix3, MB_OK|MB_ICONINFORMATION);

//              temp+=(temxi*((*(R+j)).xi)).GetElement(0,0);
			  temp+=(temxi*temA*(*(R+k)).GInertiaM*(*(A+k*m_n+j))*(*(R+j)).xi).GetElement(0,0);
			 }
			 *(M.m_pData+i*m_n+j)=temp;

              temp=0;
			 for(k=0;k<m_n;k++)
			 {
//                 temp+=MakeEMEK(i,j,k);
				 temp+=(MakeEMEK(i,j,k)+MakeEMEK(i,k,j)-MakeEMEK(k,j,i))*deltatheta.GetElement(k,0);
			 }
             C.m_pData[i*m_n+j]=0.5*temp;
		 }

    return TRUE;
}

//////////////////////////////////////////////////////////////////////
// 生成Mij对θk的偏导
//
// 参数：
// i,j,k    -序号
//
// 返回值：double 型，偏导结果
//////////////////////////////////////////////////////////////////////
double CDynamics::MakeEMEK(int i,int j,int k)
{int l;
 double result,temp; 
 CMatrix temM1;
 CMatrix temM2;

 CMatrix temA;   
 CMatrix temxi;
 BOOL flagi=true,flagj=true;
 
     temp=0;
	 if(k>=i)
     temM1=LiBrackets((*(A+k*m_n+i))*(*(R+i)).xi,(*(R+k)).xi).Transpose();
     else 
          flagi=false;
	 if(k>=j)
     temM2=LiBrackets((*(A+k*m_n+j))*(*(R+j)).xi,(*(R+k)).xi);
	 else 
         flagj=false;

	 for( l=max(i,j);l<m_n;l++)
	 { if(l<k) continue;
//CString	m_strMatrix3 = temM.ToString(" ");
//	AfxMessageBox(m_strMatrix3, MB_OK|MB_ICONINFORMATION);
	 if(flagi){ 
		  temA=(*(A+l*m_n+k)).Transpose();
// CString	m_strMatrix3 =temA.ToString(" ");
//	AfxMessageBox(m_strMatrix3, MB_OK|MB_ICONINFORMATION);
//       temM*temA;
		  temp+=(temM1*temA*(*(R+l)).GInertiaM*(*(A+l*m_n+j))*(*(R+j)).xi).GetElement(0,0);
	 }

     if(flagj){
          temxi=(*(R+i)).xi.Transpose();
	      temA=(*(A+l*m_n+i)).Transpose();
          temp+=(temxi*temA*(*(R+l)).GInertiaM*(*(A+l*m_n+k))*temM2).GetElement(0,0);
	 }

	 }


	 result=temp;
     return result;
}

//////////////////////////////////////////////////////////////////////
// 由给定的各关节转角theta和theta的一阶导数deltatheta，关节驱动力force，
// 计算theta的二阶导数ddtheta
//
// 参数：
// 1、theta        -各关节转角
// 2、deltatheta   -各关节转角的一阶导数
// 3、force        -关节驱动力
//
// 返回值：CMatrix 型，theta的二阶导数
//////////////////////////////////////////////////////////////////////
CMatrix CDynamics::CaculateDdtheta(CMatrix theta,CMatrix deltatheta,CMatrix force)
{CMatrix result;
 CMatrix temM;
     temM=M;
	 temM.InvertGaussJordan();

	 result=temM*(force-N-C*deltatheta);
     return result;
        
}

//////////////////////////////////////////////////////////////////////
// 由给定的各关节转角theta的二阶导数ddtheta和theta的一阶导数deltatheta,
// 计算关节驱动力
//
// 参数：
// 1、DDtheta        -各关节转角的二阶导数
// 2、deltatheta   -各关节转角的一阶导数
//
// 返回值：CMatrix 型，关节驱动力
//////////////////////////////////////////////////////////////////////
CMatrix CDynamics::CaculateForce(CMatrix ddtheta,CMatrix deltatheta)
{CMatrix result;

	 result=M*ddtheta+C*deltatheta+N;
     return result;
        
}