jstewartiter.cpp

矩阵运算库

// JStewartIter.cpp: implementation of the JStewartIter class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"

#include <math.h>
#include <JMath.h>

#include "JlsForm01.h"
#include "JStewartIter.h"




int		JStewart6(		const JVector * P, const JVector * Q, const double * L,
						double * x,
						int max_iter,      double tol							)
{
	//
	//	Parameters:
	//		P			(in)		joints on the base					dim[ 6 ]
	//		Q			(in)		joints on the platform				dim[ 6 ]
	//		L			(in)		length of the links					dim[ 6 ]
	//		x			(in/out)	location & attitude					dim[ 7 ]
	//		max_iter	(in)		maximum number of iteration
	//		tol			(in)		tolerance
	//

	int		i, j ;
	int		count = 0 ;
	double	error ;

	JMatrix		di( 3, 1 ),   diT( 1, 3 ),  dix( 3, 7 ) ;
	double		Ai ;
	JMatrix		Aix( 1, 7 ) ;
	JMatrix		F( 7, 1 ),    DF( 7, 7 ) ;

	do
	{
		//  A
		for ( i=0 ;  i<6 ;  i++ )
		{
			//  di ( 3, 1 )
			JlsForm01A( x, Q+i, P+i, &di ) ;

			//  diT ( 1, 3 )
			for ( j=0 ;  j<3 ;  j++ )	diT[0][j] = di[j][0] ;

			//  dix ( 3, 7 )
			JlsForm01Ax( x, Q+i, P+i, &dix ) ;

			//  Ai
			Ai  =  di[0][0] * di[0][0]  +  di[1][0] * di[1][0]  +  di[2][0] * di[2][0]  -  L[i] * L[i] ;

			//  Aix ( 1, 7 )
			Aix.EqMul( diT, dix ) ;		Aix *= 2 ;

			//  F
			F[i][0] = Ai ;

			//  DF
			for ( j=0 ;  j<7 ;  j++ )	DF[i][j] = Aix[0][j] ;
		}

		//  C
		{
			F[6][0]  =  x[3] * x[3]  +  x[4] * x[4]  +  x[5] * x[5]  +  x[6] * x[6]  -  1  ;

			DF[6][0] = 0 ;			DF[6][1] = 0 ;			DF[6][2] = 0 ;
			DF[6][3] = 2 * x[3] ;	DF[6][4] = 2 * x[4] ;	DF[6][5] = 2 * x[5] ;	DF[6][6] = 2 * x[6] ;
		}

		//  solve dx
		{
			F *= -1 ;		DF.LinSol( F ) ;
		}

		//  update			x = x + dx
		{
			for ( i=0 ;  i<7 ;  i++ )	x[i] += F[i][0] ;
		}

		//  the error		err = norm(dx) / norm(x)
		{
			error = 0 ;
			for ( i=0 ;  i<7 ;  i++ )	error += x[i] * x[i] ;
			error = F.NormF() / sqrt( error ) ;
		}

		//  c
		count ++ ;
	}
	while (  error > tol  &&  count < max_iter  ) ;

	if ( error < tol )	return	1 ;
	else				return	0 ;
}




static void	JStewartFormA(	const JVector * P, const JVector * Q, const double * L,
							int NL,            const double * x,
							JMatrix * AxTA,    JMatrix * AxTAx                      )
{
	//
	//	A = norm(d) - L * L
	//	d = r + m * Q - P
	//
	//	Parameters:
	//		P		(in)		joints on the base					dim[ NL ]
	//		Q		(in)		joints on the platform				dim[ NL ]
	//		L		(in)		length of the links					dim[ NL ]
	//		NL		(in)		number of links
	//		x		(in)		location & attitude					dim[ 7 ]
	//		AxTA	(out)		dim[7][1]
	//		AxTAx	(out)		dim[7][7]
	//

	//  aux
	int		i, j, k ;

	//  aux
	JMatrix		di( 3, 1 ),     diT( 1, 3 ),    dix( 3, 7 ),    dixT( 7, 3 ) ;
	JMatrix		dix0x( 3, 7 ),  dix1x( 3, 7 ),  dix2x( 3, 7 ),
				dix3x( 3, 7 ),	dix4x( 3, 7 ),  dix5x( 3, 7 ),  dix6x( 3, 7 ) ;
	JMatrix		* dixjx[7] = { &dix0x, &dix1x, &dix2x, &dix3x,  &dix4x,  &dix5x,  &dix6x } ;
	JMatrix		dixTdix( 7, 7 ),  diTdixjx( 1, 7 ) ;

	//  aux
	double		Ai ;
	JMatrix		Aix( 1, 7 ),    AixT( 7, 1 ),   AixTx( 7, 7 ),  AixTAix( 7, 7 ) ;


	//  zero
	AxTA->Zero() ;		AxTAx->Zero() ;


	//  accumulate
	for ( i=0 ;  i<NL ;  i++ )
	{
		//  di ( 3, 1 )
		JlsForm01A( x, Q+i, P+i, &di ) ;

		//  diT ( 1, 3 )
		for ( j=0 ;  j<3 ;  j++ )	diT[0][j] = di[j][0] ;

		//  dix ( 3, 7 )
		JlsForm01Ax( x, Q+i, P+i, &dix ) ;

		//  dixT ( 7, 3 )
		for ( j=0 ;  j<7 ;  j++ )
		{
			for ( k=0 ;  k<3 ;  k++ )	dixT[j][k] = dix[k][j] ;
		}

		//  dixjx ( 1, 7 )
		JlsForm01Axjx( x, Q+i, P+i, dixjx ) ;

		//  dixTdix ( 7, 7 )
		dixTdix.EqMul( dixT, dix ) ;


		//  Ai
		Ai  =  di[0][0] * di[0][0]  +  di[1][0] * di[1][0]  +  di[2][0] * di[2][0]  -  L[i] * L[i] ;

		//  Aix ( 1, 7 )
		Aix.EqMul( diT, dix ) ;		Aix *= 2 ;

		//  AixT ( 7, 1 )
		for ( j=0 ;  j<7 ;  j++ )	AixT[j][0] = Aix[0][j] ;

		//  AixTx ( 7, 7 )
		{
			for ( j=0 ;  j<7 ;  j++ )
			{
				diTdixjx.EqMul( diT, *dixjx[j] ) ;

				for ( k=0 ;  k<7 ;  k++ )	AixTx[j][k] = diTdixjx[0][k] ;
			}

			AixTx += dixTdix ;		AixTx *= 2 ;
		}

		//  AixTAix ( 7, 7 )
		AixTAix.EqMul( AixT, Aix ) ;


		//  AxTA
		AixT *= Ai ;				*AxTA += AixT ;

		//  AxTAx
		AixTx *= Ai ;				AixTx += AixTAix ;
		*AxTAx += AixTx ;
	}
}




int		JStewartN(	const JVector * P, const JVector * Q, const double * L,
					int NL,            double * x,
					int max_iter,      double tol							)
{
	//
	//	Parameters:
	//		P			(in)		joints on the base					dim[ NL ]
	//		Q			(in)		joints on the platform				dim[ NL ]
	//		L			(in)		length of the links					dim[ NL ]
	//		NL			(in)		number of links
	//		x			(in/out)	location & attitude					dim[ 7 ]
	//		max_iter	(in)		maximum number of iteration
	//		tol			(in)		tolerance
	//

	//  aux
	int		i, j ;
	int		count = 0 ;
	double	error ;

	JMatrix		AxTA( 7, 1 ),   AxTAx( 7, 7 ) ;
	JMatrix		Cx  ( 1, 7 ),   CxTx ( 7, 7 ) ;
	JMatrix		F   ( 8, 1 ),   DF   ( 8, 8 ) ;

	//  const matrix CxTx
	{
		CxTx.Zero() ;
		CxTx[3][3] = 2 ;		CxTx[4][4] = 2 ;		CxTx[5][5] = 2 ;		CxTx[6][6] = 2 ;
	}

	//  y
	double	y[8] ;
	for ( i=0 ;  i<7 ;  i++ )	y[i] = x[i] ;			y[7] = 0 ;

	//  iter
	do
	{
		//  A
		JStewartFormA( P, Q, L, NL, y, &AxTA, &AxTAx ) ;

		//  C
		{
			Cx[0][0] = 0 ;			Cx[0][1] = 0 ;			Cx[0][2] = 0 ;
			Cx[0][3] = 2 * y[3] ;	Cx[0][4] = 2 * y[4] ;	Cx[0][5] = 2 * y[5] ;	Cx[0][6] = 2 * y[6] ;
		}

		//  F
		{
			for ( i=0 ;  i<7 ;  i++ )	F[i][0] = AxTA[i][0] * 2 + Cx[0][i] * y[7] ;

			F[7][0]  =  y[3] * y[3]  +  y[4] * y[4]  +  y[5] * y[5]  +  y[6] * y[6]  -  1  ;
		}

		//  DF
		{
			for ( i=0 ;  i<7 ;  i++ )
			{
				for ( j=0 ;  j<7 ;  j++ )	DF[i][j] = AxTAx[i][j] * 2 + CxTx[i][j] * y[7] ;
			}

			for ( i=0 ;  i<7 ;  i++ )		DF[i][7] = DF[7][i] = Cx[0][i] ;

			DF[7][7] = 0 ;
		}

		//  solve dy
		{
			F *= -1 ;		DF.LinSol( F ) ;
		}

		//  update			y = y + dy
		{
			for ( i=0 ;  i<8 ;  i++ )	y[i] += F[i][0] ;
		}

		//  the error		err = norm(dy) / norm(y)
		{
			error = 0 ;
			for ( i=0 ;  i<8 ;  i++ )	error += y[i] * y[i] ;
			error = F.NormF() / sqrt( error ) ;
		}

		//  c
		count ++ ;
	}
	while (  error > tol  &&  count < max_iter  ) ;

	//  assign
	for ( i=0 ;  i<7 ;  i++ )	x[i] = y[i] ;

	if ( error < tol )	return	1 ;
	else				return	0 ;
}