sddynamics.cpp

机器人动力学源码,可以根据机器人当前状态精神其加速度

// SDDynamics.cpp: implementation of the CSDDynamics class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "MySDOpenGL.h"
#include "SDDynamics.h"

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

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

CSDDynamics::CSDDynamics()
{

}

//CSDDynamics::CSDDynamics(int n)
//{  // A=NULL;
//	R=NULL;
//	BOOL bSuccess =Init( n);
//	ASSERT(bSuccess);
 //   CDynamics(n);
//}


CSDDynamics::~CSDDynamics()
{

}

BOOL CSDDynamics::MakeN(CMatrix theta,CMatrix deltatheta,CMatrix beata,double gravity)
{double theta2=theta.GetElement(1,0)*3.14/180;
 double theta3=theta.GetElement(2,0)*3.14/180; 
 double r2=(*(R+1)).r.z;
 double l1=(*(R+1)).l;
 double m2=(*(R+1)).m;

 double r3=(*(R+2)).r.z;
 double l3=(*(R+2)).l;
 double m3=(*(R+2)).m;

 double m4=(*(R+3)).m;

 double g=gravity;

 double vN[]={0,-m2*g*r2*cos(theta2)-m3*g*(l1*cos(theta2)+r3*cos(theta2+theta3))-m4*g*(l1*cos(theta2)+l3*cos(theta2+theta3)),-m3*g*r3*cos(theta2+theta3)-m4*g*l3*cos(theta2+theta3),0};
 CMatrix temN(4,1,vN);
        N=temN+beata*deltatheta;
    return true;
}