IKMethods逆向运动学

IK（Inverse Kinematic，逆向运动学，也称反向运动学）的实现代码

#include 
#include 
#include 
#include 
using namespace std;

#ifdef WIN32
#include 
#endif

#include 
#include 
#include 
#include 

#include “Jacobian.h“

void Arrow（const VectorR3& tail const VectorR3& head）;

extern RestPositionOn;
extern VectorR3 target[];

// Optimal damping values have to be determined in an ad hoc manner  （Yuck!）
// const double Jacobian::DefaultDampingLambda = 0.6;		// Optimal for the “Y“ shape （any lower gives jitter）
const double Jacobian::DefaultDampingLambda = 1.1;			// Optimal for the DLS “double Y“ shape （any lower gives jitter）
// const double Jacobian::DefaultDampingLambda = 0.7;			// Optimal for the DLS “double Y“ shape with distance clamping （lower gives jitter）

const double Jacobian::PseudoInverseThresholdFactor = 0.01;
const double Jacobian::MaxAngleJtranspose = 30.0*DegreesToRadians;
const double Jacobian::MaxAnglePseudoinverse = 5.0*DegreesToRadians;
const double Jacobian::MaxAngleDLS = 45.0*DegreesToRadians;
const double Jacobian::MaxAngleSDLS = 45.0*DegreesToRadians;
const double Jacobian::baseMaxTargetDist = 0.4;

Jacobian::Jacobian（Tree* tree）
{
	Jacobian::tree = tree;
	nEffector = tree->GetNumEffector（）;
	nJoint = tree->GetNumJoint（）;
	nRow = 3 * nEffector;
	nCol = nJoint;

	Jend.SetSize（nRow nCol）;				// The Jocobian matrix
	Jend.SetZero（）;
	Jtarget.SetSize（nRow nCol）;			// The Jacobian matrix based on target positions
	Jtarget.SetZero（）;
	SetJendActive（）;

	U.SetSize（nRow nRow）;				// The U matrix for SVD calculations
	w .SetLength（Min（nRow nCol））;
	V.SetSize（nCol nCol）;				// The V matrix for SVD calculations

	dS.SetLength（nRow）;			// （Target positions） - （End effector positions）
	dTheta.SetLength（nCol）;		// Changes in joint angles
	dPreTheta.SetLength（nCol）;

	// Used by Jacobian transpose method & DLS & SDLS
	dT.SetLength（nRow）;			// Linearized change in end effector positions based on dTheta

	// Used by the Selectively Damped Least Squares Method
	//dT.SetLength（nRow）;
	dSclamp.SetLength（nEffector）;
	errorArray.SetLength（nEffector）;
	Jnorms.SetSize（nEffector nCol）;		// Holds the norms of the active J matrix

	Reset（）;
}

void Jacobian::Reset（） 
{
	// Used by Damped Least Squares Method
	DampingLambda = DefaultDampingLambda;
	DampingLambdaSq = Square（DampingLambda）;
	// DampingLambdaSDLS = 1.5*DefaultDampingLambda;
	
	dSclamp.Fill（HUGE_VAL）;
}

// Compute the deltaS vector dS （the error in end effector positions
// Compute the J and K matrices （the Jacobians）
void Jacobian::ComputeJacobian（） 
{
	// Traverse tree to find all end effectors
	VectorR3 temp;
	double value;
	Node* n = tree->GetRoot（）;
	while （ n ） {	
		if （ n->IsEffector（） ） {
			int i = n->GetEffectorNum（）;
			const VectorR3& targetPos = target[i];

			// Compute the delta S value （differences from end effectors to target positions.
			temp = 

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       2517  2007-10-19 20:51  IkMethods\Debug\BuildLog.htm

     文件     116736  2004-08-04 10:25  IkMethods\Debug\glu32.dll

     文件     237568  2004-08-04 10:25  IkMethods\Debug\glut32.dll

     文件     389120  2007-10-19 20:51  IkMethods\Debug\IkMethods.exe

     文件    1522792  2007-10-19 20:51  IkMethods\Debug\IkMethods.ilk

     文件    1477632  2007-10-19 20:51  IkMethods\Debug\IkMethods.pdb

     文件     227525  2007-05-22 21:36  IkMethods\Debug\Jacobian.obj

     文件      45294  2007-05-16 15:50  IkMethods\Debug\LinearR2.obj

     文件     117493  2007-05-16 15:50  IkMethods\Debug\LinearR3.obj

     文件     103758  2007-05-16 15:50  IkMethods\Debug\LinearR4.obj

     文件     264740  2007-10-19 20:51  IkMethods\Debug\Main.obj

     文件     122404  2007-05-16 15:50  IkMethods\Debug\MatrixRmn.obj

     文件     159618  2007-05-16 15:50  IkMethods\Debug\Misc.obj

     文件     160848  2007-05-19 14:22  IkMethods\Debug\Node.obj

     文件      30378  2007-05-16 15:50  IkMethods\Debug\RgbImage.obj

     文件      61307  2007-05-19 14:22  IkMethods\Debug\Tree.obj

     文件    1002496  2007-10-19 20:51  IkMethods\Debug\vc70.idb

     文件     282624  2007-10-19 20:51  IkMethods\Debug\vc70.pdb

     文件      36840  2007-05-16 15:50  IkMethods\Debug\VectorRn.obj

     文件     389120  2007-05-20 08:53  IkMethods\Debug\复件 IkMethods.exe

     文件      74528  2005-08-10 10:19  IkMethods\GL\GL.h

     文件      12024  2005-08-10 10:19  IkMethods\GL\GLAux.h

     文件     203244  2005-08-10 10:19  IkMethods\GL\glext.h

     文件      18282  2005-08-10 10:19  IkMethods\GL\GLU.h

     文件      96289  2006-07-06 17:42  IkMethods\GL\glui.h

     文件      21494  2005-08-10 10:19  IkMethods\GL\glut.h

     文件      20932  2005-08-10 10:19  IkMethods\GL\wglext.h

     文件    8340728  2006-09-19 16:32  IkMethods\glui32.lib

     文件      85256  2005-07-28 11:46  IkMethods\glut32.lib

     文件     789504  2010-11-07 20:59  IkMethods\IkMethods.ncb

............此处省略33个文件信息