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

				// Control.cpp: implementation of the CControl class.
				//
				//////////////////////////////////////////////////////////////////////
				
				#include "stdafx.h"
				#include "MySDOpenGL.h"
				#include "Control.h"
				
				#ifdef _DEBUG
				#undef THIS_FILE
				static char THIS_FILE[]=__FILE__;
				#define new DEBUG_NEW
				#endif
				
				//////////////////////////////////////////////////////////////////////
				// Construction/Destruction
				//////////////////////////////////////////////////////////////////////
				
				CControl::CControl()
				{
				
				}
				
				CControl::~CControl()
				{
				
				}
				
				//////////////////////////////////////////////////////////////////////
				//计算控制规则
				//由给定的当前速度，当前位置，及期望加速度，速度，位置，定常增益矩阵，
				//指向动力学类的指针，计算出应加力矩
				//
				// 参数：
				// 1、dx    -当前速度
				// 2、x     -当前位置
				//
				// 返回值：CMatrix 型，应加力矩
				//////////////////////////////////////////////////////////////////////
				CMatrix CControl::ControlTorqueCL(CMatrix dx,CMatrix x)
				{CMatrix result;
				 CMatrix de;
				 CMatrix e;
				         de=dx-Designdx;
						 e=x-Designx;
				
				         result=lpDynamics->M*(DesignDdx-Kv*de-Kp*e)+lpDynamics->C*dx+lpDynamics->N;
				
				
				        return result;
				
				}