Difference between revisions of "Program Examples:User Kinematics"

From SoftMC-Wiki
Jump to: navigation, search
(→‎User Kinematics)
 
(6 intermediate revisions by one other user not shown)
Line 1: Line 1:
 +
{{Languages|Program_Examples:User_Kinematics}}
 
= User Kinematics =
 
= User Kinematics =
  
−
{{Note| This topic is only for advance users. You must be expert in robotics and C-programming in order to benefit from this feature.}}
+
{{Note/Caution| This topic is only for advance users only. You must be expert in robotics and C-programming in order to benefit from this feature.}}
  
  
 +
An example of user-defined kinematics will be given using the SCORBOT robot. Attached files: [[File:OpenKinematics.zip | User.cpp, TUKIN.PRG ]].
  
  
 +
[[File:SCORBOTPHOTO.JPG]] [[File:SCORBOTDRAWING.PNG]]
  
−
See:
+
 
 +
In the Scorbot robot, the second, third, and fourth joint axes are parallel to one another and point into the paper at points A, B, and P, respectively. The first joint axis points up vertically, and the fifth joint axis intersects the fourth perpendicularly. Find the overall transformation matrix for the robot.
 +
 
 +
 
 +
<syntaxhighlight lang="C">
 +
 
 +
#include <stdio.h>
 +
#include <math.h>
 +
#include <time.h>
 +
#include <string.h>
 +
 
 +
 
 +
static const double PI = 3.14159265358979323846;
 +
#include "rbtUKin.typ"
 +
 
 +
// Example of SCORBOT, 5 Dof robot type: RRRRR
 +
 
 +
static double d1 = 170; // mm
 +
static double a1 = 50; // mm
 +
static double a2 = 300; // mm
 +
static double a3 = 250; // mm
 +
static double d5 = 90; // mm
 +
 
 +
static double rmin = 10; // arbitrary values, just for example
 +
static double rmax = 800;
 +
 
 +
static double a22, a32, a2a3; // Intermidiate values computed in SETUP
 +
 
 +
#define SIGN_EPS 1e-4
 +
inline double Sqrt(double x) { return (x<=0 ? 0: sqrt(x));}
 +
inline int    _signE(double x) { return (x < -SIGN_EPS ? -1 : (x > SIGN_EPS ? 1: 0));} // Special sign function, rejects anything which is less then SIGN_EPS
 +
 
 +
// Some definitions used for simpler computations:
 +
// A05 = [U V W], rotation matrix
 +
 
 +
#define Ux A05[0][0]
 +
#define Uy A05[0][1]
 +
#define Uz A05[0][2]
 +
 
 +
#define Vx A05[1][0]
 +
#define Vy A05[1][1]
 +
#define Vz A05[1][2]
 +
 
 +
#define Wx A05[2][0]
 +
#define Wy A05[2][1]
 +
#define Wz A05[2][2]
 +
 
 +
 
 +
// Cartesian point: X,Y,Z + orientation quaternion
 +
// Quaterniond is of the form: Cos(phi/2), n*Sin(Phi/2) where n is unit vector ||n|| =1
 +
#define X cpnt[0]
 +
#define Y cpnt[1]
 +
#define Z cpnt[2]
 +
 
 +
#define Ro cpnt[3]
 +
#define sinNx cpnt[4]
 +
#define sinNy cpnt[5]
 +
#define sinNz cpnt[6]
 +
 
 +
 
 +
 
 +
 
 +
// Joint coordinates
 +
#define THETA1 0
 +
#define THETA2 1
 +
#define THETA3 2
 +
#define THETA4 3
 +
#define THETA5 4
 +
 
 +
 
 +
// Some small eps values
 +
#define MAT_EPS   1e-4
 +
#define QUAT_EPS   1e-20
 +
#define SYS_ACCURACY 1e-13
 +
 
 +
 
 +
// Rotation Matrix to Quaternion:
 +
static void Mat2Quat(double A[3][3], double *cpnt)
 +
{
 +
// for cos(phi): q(1) = (trace(a) +1)/2;
 +
//      A32 - A23          A13 - A31          A21 - A12
 +
// Nx = ---------    Ny = ---------      Nz = ---------
 +
//      2sin(phi)          2sin(phi)          2sin(phi)
 +
// but we need cos(phi/2)!
 +
//
 +
// The quaternion is defined as: [cos(phi/2),C*sin(phi/2)]
 +
// Using: cos^2(x) = 1/2 + 1/2 cos(2x)
 +
 
 +
double cphi = (A[0][0] + A[1][1] + A[2][2] - 1.0)/2.0;
 +
Ro = Sqrt((1+cphi)/2.0);  // Ro = cos(phi/2)!
 +
// double sphi  = Sqrt(1 - cphi*cphi);
 +
// CS.x = sin(phi/2)*Nx = sin(phi/2) (A32-A23)/2sin(phi);
 +
// So the multiplier is: sin(phi/2)/2sin(phi) = 1/4cos(pih/2) !!!
 +
if (fabs(Ro) > QUAT_EPS)
 +
{
 +
double f = 1/(4*Ro);
 +
sinNx = (A[2][1]-A[1][2])*f;
 +
sinNy = (A[0][2]-A[2][0])*f;
 +
sinNz = (A[1][0]-A[0][1])*f;
 +
}
 +
else // phi = PI, n is not defined, we will try the best we can do
 +
{
 +
// A = 1 + 2*Ro*Cross(CS) + 2*Cross(CS)^2
 +
// .................................................................................................... 
 +
//          0  -z    y                  0  -z    y      0  -z    y    -z^2-y^2    x*y      x*z
 +
// Cross =  z    0  -x  --->  Cross^2 = z    0  -x  *  z    0  -x  =    x*y    -z^2-x^2    y*z
 +
//        -y    x    0                  -y    x    0    -y    x    0      x*z      z*y  -y^2-x^2
 +
//......................................................................................................
 +
// But Ro is zero, therefore:
 +
// A = 1 + 2*Cross(CS)^2
 +
// .................................................................................................... 
 +
//      1-2*z^2-2*y^2    2*x*y      2*x*z
 +
// A  =    2*x*y    1-2*z^2-2*x^2    2*y*z
 +
//        2*x*z      2*z*y  1-2*y^2-2*x^2
 +
//......................................................................................................
 +
// A[0][0] + A[1][1] - A[2][2] = 2-4z^2 - 2*(x^2+y^2) - 1 + 2*(x^2+y^2) = 1 - 4z^2
 +
//  z = (+/-)0.5*Sqrt(1 - (A[0][0] + A[1][1] - A[2][2]))
 +
//  y = (+/-)0.5*Sqrt(1-2*z^2-A[0][0])
 +
//  x = (+/-)0.5*Sqrt(1-2*y^2-A[1][1])
 +
//
 +
double _4z2  = 1 - (A[0][0] + A[1][1] - A[2][2]);
 +
double _2z2  = _4z2/2;
 +
double _2y2  = 1 - _2z2 - A[0][0];
 +
double _2x2  = 1 - _2y2 - A[2][2];
 +
// Assume all same sign
 +
sinNx = Sqrt(0.5*_2x2);
 +
sinNy = Sqrt(0.5*_2y2);
 +
sinNz = Sqrt(0.5*_2z2);
 +
 
 +
// Check the other eleemnts:
 +
int sxy = _signE(A[1][0]);
 +
int sxz = _signE(A[2][0]);
 +
int szy = _signE(A[2][1]);
 +
if      ( sxy && sxz )
 +
{
 +
// Assume x is positive!
 +
sinNy *= sxy;
 +
sinNz *= sxz;
 +
}
 +
else if ( sxy && szy )
 +
{
 +
// Assume y is positive!
 +
sinNx *= sxy;
 +
sinNz *= szy;
 +
}
 +
else if ( sxz && szy )
 +
{
 +
// Assume z is positive!
 +
sinNx *= sxz;
 +
sinNy *= szy;
 +
}
 +
else if (sxy)
 +
{
 +
// Assume x,z are positive!
 +
sinNy *= sxy;
 +
}
 +
else if (sxz)
 +
{
 +
// Assume x,y are positive!
 +
sinNz *= sxz;
 +
}
 +
else if (szy)
 +
{
 +
// Assume x,z are positive!
 +
sinNy *= szy;
 +
}
 +
}
 +
 
 +
}
 +
 
 +
// Quaternion to Rotation Matrix:
 +
static void Quat2Mat(double *cpnt, double A[3][3])
 +
{
 +
 
 +
// Q = unit + (2.0*Ro)*cross + 2.0*(cross*cross);
 +
 
 +
double Ro2 = (2.0*Ro);
 +
double Nz  = Ro2*sinNz;
 +
double Ny  = Ro2*sinNy;
 +
double Nx  = Ro2*sinNx;
 +
 
 +
double z2 = sinNz*sinNz;
 +
double y2 = sinNy*sinNy;
 +
double x2 = sinNx*sinNx;
 +
 
 +
double z2y2  = 2*(-z2-y2);
 +
double z2x2  = 2*(-z2-x2);
 +
double y2x2  = 2*(-y2-x2);
 +
 
 +
double xy    = 2*sinNx*sinNy;
 +
double xz    = 2*sinNx*sinNz;
 +
double zy    = 2*sinNz*sinNy;
 +
 
 +
A[0][0] = A[1][1] = A[2][2] = 1.0; // A = 1
 +
 
 +
// A = 1 + 2*Ro*Cross(CS)
 +
                    A[0][1] = -Nz; A[0][2] =  Ny;
 +
A[1][0] =  Nz;                A[1][2] = -Nx;
 +
A[2][0] = -Ny;    A[2][1] = Nx;                   
 +
 +
 
 +
// A = 1 + 2*Ro*Cross(CS) + 2*Cross(CS)^2
 +
// .................................................................................................... 
 +
//          0  -z    y                  0  -z    y      0  -z    y    -z^2-y^2    x*y      x*z
 +
// Cross =  z    0  -x  --->  Cross^2 = z    0  -x  *  z    0  -x  =    x*y    -z^2-x^2    y*z
 +
//        -y    x    0                  -y    x    0    -y    x    0      x*z      z*y  -y^2-x^2
 +
//......................................................................................................
 +
  A[0][0] += z2y2;  A[0][1] += xy;    A[0][2] +=  xz;
 +
A[1][0] += xy;    A[1][1] += z2x2;  A[1][2] +=  zy;
 +
A[2][0] += xz;    A[2][1] += zy;    A[2][2] +=  y2x2;               
 +
}
 +
 
 +
 
 +
// Setup function, called from ConfigGroup, used for some off-line computations
 +
int Setup()
 +
{
 +
a22  = a2*a2;
 +
a32  = a3*a3;
 +
a2a3 = a2*a3;
 +
return 0;
 +
}
 +
 
 +
 
 +
// configuration bits:  b0-1: Shoulder, b2-3: Elbow, b4-5: Wrist, b6-7:Turn1, b8-9: Turn2, ....
 +
int Config(double *jnt)
 +
{
 +
int cfg = 0;
 +
if (jnt[4] <  0) cfg |= 1 << 4;
 +
if (jnt[4] >  0) cfg |= 2 << 4;
 +
return cfg;
 +
}
 +
 
 +
// Inverse kinematics:
 +
int InverseKinematics(int cfg, double *cpnt, double *jnt)
 +
{
 +
double s1,c1;
 +
double s2,c2;
 +
double s3,c3;
 +
double s234,c234;
 +
double A05[3][3];
 +
 +
 
 +
 
 +
Quat2Mat(cpnt,A05);
 +
 
 +
double rxy2 = X*X + Y*Y;
 +
if (rxy2 > 0.0001) jnt[THETA1] = atan2(Y,X);
 +
 
 +
sincos(jnt[THETA1],&s1,&c1);
 +
 
 +
double s5 = Ux*s1 - Uy*c1;
 +
if(fabs(s5) > 1.001) return 5;
 +
else if (s5 > 1) s5 = 1;
 +
else if (s5 < -1) s5 = -1;
 +
 
 +
jnt[THETA5] = asin(s5);
 +
if (((cfg >> 4) & 3) == 2) jnt[THETA5] += PI;
 +
 
 +
if (Uz*Uz + (Ux*c1 + Uy*s1)*(Ux*c1 + Uy*s1) < 0.0001) return 4;
 +
 
 +
double Theta234 = atan2(-Uz, (Ux*c1 + Uy*s1));
 +
sincos(Theta234,&s234,&c234);
 +
 +
double k1 = X*c1 +Y*s1 -a1 + d5*s234;
 +
double k2 = -Z + d1 - d5*c234;
 +
 
 +
c3 = (k1*k1 + k2*k2 - a22 - a32)/(2*a2a3);
 +
 
 +
if (fabs(c3) > 1.001) return 3;
 +
else if (c3 > 1) c3 = 1;
 +
else if (c3 < -1) c3 = -1;
 +
 
 +
jnt[THETA3] = acos(c3);
 +
sincos(jnt[THETA3],&s3,&c3);
 +
 
 +
 
 +
double invdet = (a22 + a32 + 2*a2a3*c3);
 +
if (fabs(invdet) < 1e-10) return 2;
 +
 
 +
 
 +
c2 = (k1*(a2 + a3*c3) +k2*a3*s3)/invdet;
 +
s2 = (-k1*a3*s3 + k2*(a2+a3*c3))/invdet;
 +
 
 +
jnt[THETA2] = atan2(s2,c2);
 +
jnt[THETA4] = Theta234 - jnt[THETA2] - jnt[THETA3];
 +
 +
 
 +
return 0;
 +
}
 +
 
 +
// Direct Kinematics:
 +
int DirectKinematics(double *cpnt, double *jnt)
 +
{
 +
double c1,s1;
 +
double c2,s2;
 +
double c23,s23;
 +
double c234,s234;
 +
double c5,s5;
 +
double A05[3][3];
 +
 
 +
sincos(jnt[0],&s1,&c1);
 +
sincos(jnt[1],&s2,&c2);
 +
sincos(jnt[1]+jnt[2],&s23,&c23);
 +
sincos(jnt[1]+jnt[2]+jnt[3],&s234,&c234);
 +
sincos(jnt[4],&s5,&c5);
 +
 
 +
double L = (a1 + a2*c2 + a3*c23 - d5*s234);
 +
X  = c1*L; // X
 +
Y  = s1*L; // Y
 +
Z  = d1 - a2*s2 - a3*s23  - d5*c234; // Z
 +
 
 +
 
 +
Ux = c1*c234*c5 + s1*s5;
 +
Uy = s1*c234*c5 - c1*s5;
 +
Uz = -s234*c5;
 +
 
 +
Vx = -c1*c234*s5 + s1*c5;
 +
Vy = -s1*c234*s5 - c1*c5;
 +
Vz = s234*s5;
 +
 
 +
Wx = -c1*s234;
 +
Wy = -s1*s234;
 +
Wz = -c234;
 +
 
 +
Mat2Quat(A05,cpnt);
 +
 +
return 0;
 +
}
 +
 
 +
// Accessibility test:
 +
int Accessible(double *cpnt)
 +
{
 +
double l = sqrt(X*X + Y*Y + Z*Z);
 +
 
 +
if (l < rmin) return -1; // Too Close
 +
if (l > rmax) return  1; // Too Far
 +
return 0; // OK
 +
}
 +
 
 +
 
 +
 
 +
static RBT_UFUN user;
 +
 
 +
extern "C"
 +
{
 +
 
 +
int USR_VER(int,int,int) { return 10000;} // Version ID
 +
 
 +
// Here it is the link function that connects user kinematics to a user group.
 +
int USR_SCORBOT(int id, int, int)
 +
{
 +
user.ijac  = 0; // No Inverse Jacobian provided
 +
user.ikin  = InverseKinematics; // Inverse Kinematics
 +
user.dkin  = DirectKinematics; // Direct Kineamtics
 +
user.setup  = Setup; // Setup (Off-Line) computations
 +
user.cfg    = Config; // Configuration flags function
 +
user.acces  = Accessible; // Accessibility test
 +
 
 +
// Link to a group:
 +
return rbt_SetUserKin (id, user);
 +
}
 +
}
 +
 
 +
</syntaxhighlight>
 +
 
 +
=See Also:=
 
* [[User_Kinematics | User Kinematics]]
 
* [[User_Kinematics | User Kinematics]]
 
* [[File:Open_Kinemaitcs_Interface.PDF | User Kinematics manual]]
 
* [[File:Open_Kinemaitcs_Interface.PDF | User Kinematics manual]]
 +
* [[File:OpenKinematics.zip| Sources]]

Latest revision as of 07:40, 17 July 2017

Language: English  • 中文（简体）‎

User Kinematics

CAUTION.svgCAUTION
This topic is only for advance users only. You must be expert in robotics and C-programming in order to benefit from this feature.


An example of user-defined kinematics will be given using the SCORBOT robot. Attached files: File:OpenKinematics.zip.


SCORBOTPHOTO.JPG SCORBOTDRAWING.PNG


In the Scorbot robot, the second, third, and fourth joint axes are parallel to one another and point into the paper at points A, B, and P, respectively. The first joint axis points up vertically, and the fifth joint axis intersects the fourth perpendicularly. Find the overall transformation matrix for the robot. 


#include <stdio.h>
#include <math.h>
#include <time.h>
#include <string.h>


static const double PI 		= 3.14159265358979323846;
#include "rbtUKin.typ"

// Example of SCORBOT, 5 Dof robot type: RRRRR

static double d1 = 170; // mm
static double a1 = 50; // mm
static double a2 = 300; // mm
static double a3 = 250; // mm
static double d5 = 90; // mm

static double rmin = 10;		// arbitrary values, just for example
static double rmax = 800;

static double a22, a32, a2a3; // Intermidiate values computed in SETUP

#define SIGN_EPS 1e-4
inline double Sqrt(double x) { return (x<=0 ? 0: sqrt(x));}
inline int    _signE(double x) { return (x < -SIGN_EPS ? -1 : (x > SIGN_EPS ? 1: 0));} // Special sign function, rejects anything which is less then SIGN_EPS

// Some definitions used for simpler computations:
// A05 = [U V W], rotation matrix

#define Ux A05[0][0]
#define Uy A05[0][1]	
#define Uz A05[0][2]	

#define Vx A05[1][0]
#define Vy A05[1][1]	
#define Vz A05[1][2]	

#define Wx A05[2][0]
#define Wy A05[2][1]	
#define Wz A05[2][2]	


// Cartesian point: X,Y,Z + orientation quaternion
// Quaterniond is of the form: Cos(phi/2), n*Sin(Phi/2) where n is unit vector ||n|| =1
#define X cpnt[0]
#define Y cpnt[1]
#define Z cpnt[2]

#define Ro cpnt[3]
#define sinNx cpnt[4]
#define sinNy cpnt[5]
#define sinNz cpnt[6]




// Joint coordinates
#define THETA1 0
#define THETA2 1
#define THETA3 2
#define THETA4 3
#define THETA5 4


// Some small eps values
#define MAT_EPS 	   1e-4
#define QUAT_EPS 	   1e-20
#define SYS_ACCURACY 1e-13


// Rotation Matrix to Quaternion:
static void Mat2Quat(double A[3][3], double *cpnt)
{
// for cos(phi):	q(1) = (trace(a) +1)/2;
//      A32 - A23          A13 - A31           A21 - A12
//	Nx = ---------     Ny = ---------      Nz = ---------
//      2sin(phi)          2sin(phi)           2sin(phi)
// but we need cos(phi/2)!
//
// The quaternion is defined as: [cos(phi/2),C*sin(phi/2)]
// Using: cos^2(x) = 1/2 + 1/2 cos(2x)

	double cphi = (A[0][0] + A[1][1] + A[2][2] - 1.0)/2.0;
	Ro = Sqrt((1+cphi)/2.0);  // Ro = cos(phi/2)!
//	double sphi  = Sqrt(1 - cphi*cphi);
// CS.x = sin(phi/2)*Nx = sin(phi/2) (A32-A23)/2sin(phi); 
// So the multiplier is: sin(phi/2)/2sin(phi) = 1/4cos(pih/2) !!!
	if (fabs(Ro) > QUAT_EPS)
	{
		double f = 1/(4*Ro);
		sinNx = (A[2][1]-A[1][2])*f;
		sinNy = (A[0][2]-A[2][0])*f;
		sinNz = (A[1][0]-A[0][1])*f;
	}
	else // phi = PI, n is not defined, we will try the best we can do
	{
	// A = 1 + 2*Ro*Cross(CS) + 2*Cross(CS)^2
	// ....................................................................................................   
	//          0   -z    y                   0   -z    y      0   -z    y    -z^2-y^2     x*y      x*z
	//	Cross =  z    0   -x   --->  Cross^2 = z    0   -x  *   z    0   -x  =    x*y    -z^2-x^2    y*z
	//         -y    x    0                  -y    x    0     -y    x    0       x*z       z*y   -y^2-x^2
	//......................................................................................................
	// But Ro is zero, therefore:
	// A = 1 + 2*Cross(CS)^2
	// ....................................................................................................   
	//       1-2*z^2-2*y^2     2*x*y      2*x*z
	//	A  =    2*x*y     1-2*z^2-2*x^2    2*y*z
	//         2*x*z       2*z*y   1-2*y^2-2*x^2
	//......................................................................................................
	// A[0][0] + A[1][1] - A[2][2] = 2-4z^2 - 2*(x^2+y^2) - 1 + 2*(x^2+y^2) = 1 - 4z^2
	//  z = (+/-)0.5*Sqrt(1 - (A[0][0] + A[1][1] - A[2][2]))
	//  y = (+/-)0.5*Sqrt(1-2*z^2-A[0][0])
	//  x = (+/-)0.5*Sqrt(1-2*y^2-A[1][1])
	//
		double _4z2   = 1 - (A[0][0] + A[1][1] - A[2][2]);
		double _2z2   = _4z2/2;
		double _2y2   = 1 - _2z2 - A[0][0];
		double _2x2   = 1 - _2y2 - A[2][2];
	// Assume all same sign
		sinNx = Sqrt(0.5*_2x2);
		sinNy = Sqrt(0.5*_2y2);
		sinNz = Sqrt(0.5*_2z2);

	// Check the other eleemnts:
		int sxy = _signE(A[1][0]);
		int sxz = _signE(A[2][0]);
		int szy = _signE(A[2][1]);
		if      ( sxy && sxz ) 
		{
			// Assume x is positive!
			sinNy *= sxy;
			sinNz *= sxz;
		}		
		else if ( sxy && szy )
		{
			// Assume y is positive!
			sinNx *= sxy;
			sinNz *= szy;
		}
		else if ( sxz && szy )
		{
			// Assume z is positive!
			sinNx *= sxz;
			sinNy *= szy;
		}
		else if (sxy)
		{
			// Assume x,z are positive!
			sinNy *= sxy;
		}
		else if (sxz)
		{
			// Assume x,y are positive!
			sinNz *= sxz;
		}
		else if (szy)
		{
			// Assume x,z are positive!
			sinNy *= szy;
		}
	}

}

// Quaternion to Rotation Matrix:
static void Quat2Mat(double *cpnt, double A[3][3])
{

//	Q = unit + (2.0*Ro)*cross + 2.0*(cross*cross);

	double Ro2 = (2.0*Ro);
	double Nz  = Ro2*sinNz;
	double Ny  = Ro2*sinNy;
	double Nx  = Ro2*sinNx;

	double z2 	 = sinNz*sinNz;
	double y2 	 = sinNy*sinNy;
	double x2 	 = sinNx*sinNx;

	double z2y2  = 2*(-z2-y2);
	double z2x2  = 2*(-z2-x2);
	double y2x2  = 2*(-y2-x2);

	double xy     = 2*sinNx*sinNy;
	double xz     = 2*sinNx*sinNz;
	double zy     = 2*sinNz*sinNy;

	A[0][0] = A[1][1] = A[2][2] = 1.0; // A = 1

	// A = 1 + 2*Ro*Cross(CS)	
                    	A[0][1] = -Nz; A[0][2] =  Ny;
	A[1][0] =  Nz;                 	A[1][2] = -Nx;
	A[2][0] = -Ny;    A[2][1] = Nx;                    
	

	// A = 1 + 2*Ro*Cross(CS) + 2*Cross(CS)^2
// ....................................................................................................   
//          0   -z    y                   0   -z    y      0   -z    y    -z^2-y^2     x*y      x*z
//	Cross =  z    0   -x   --->  Cross^2 = z    0   -x  *   z    0   -x  =    x*y    -z^2-x^2    y*z
//         -y    x    0                  -y    x    0     -y    x    0       x*z       z*y   -y^2-x^2
//......................................................................................................
   A[0][0] += z2y2;  A[0][1] += xy;    A[0][2] +=  xz;
	A[1][0] += xy;    A[1][1] += z2x2;  A[1][2] +=  zy;
	A[2][0] += xz;    A[2][1] += zy;    A[2][2] +=  y2x2;                 
}


// Setup function, called from ConfigGroup, used for some off-line computations
int Setup()
{
	a22  = a2*a2;
	a32  = a3*a3;
	a2a3 = a2*a3;
	return 0;
}


// configuration bits:  b0-1: Shoulder, b2-3: Elbow, b4-5: Wrist, b6-7:Turn1, b8-9: Turn2, ....
int Config(double *jnt)
{
	int cfg = 0;
	if (jnt[4] <  0) cfg |= 1 << 4;
	if (jnt[4] >  0) cfg |= 2 << 4;
	return cfg;
}

// Inverse kinematics:
int InverseKinematics(int cfg, double *cpnt, double *jnt)
{
	double s1,c1;
	double s2,c2;
	double s3,c3;
	double s234,c234;
	double A05[3][3];
	


	Quat2Mat(cpnt,A05);

	double rxy2 = X*X + Y*Y;
	if (rxy2 > 0.0001) jnt[THETA1] = atan2(Y,X); 

	sincos(jnt[THETA1],&s1,&c1);

	double s5 = Ux*s1 - Uy*c1;
	if(fabs(s5) > 1.001) return 5;
	else if (s5 > 1)	s5 = 1;
	else if (s5 < -1)	s5 = -1;

	jnt[THETA5] = asin(s5);
	if (((cfg >> 4) & 3) == 2) jnt[THETA5] += PI;		

	if (Uz*Uz + (Ux*c1 + Uy*s1)*(Ux*c1 + Uy*s1) < 0.0001) return 4;

	double Theta234 = atan2(-Uz, (Ux*c1 + Uy*s1));
	sincos(Theta234,&s234,&c234);
	
	double k1 = X*c1 +Y*s1 -a1 + d5*s234;
	double k2 = -Z + d1 - d5*c234;

	c3 = (k1*k1 + k2*k2 - a22 - a32)/(2*a2a3);

	if (fabs(c3) > 1.001)		return 3;
	else if (c3 > 1)	c3 = 1;
	else if (c3 < -1)	c3 = -1;

	jnt[THETA3] = acos(c3);	
	sincos(jnt[THETA3],&s3,&c3);


	double invdet = (a22 + a32 + 2*a2a3*c3);
	if (fabs(invdet) < 1e-10) return 2;


	c2 = (k1*(a2 + a3*c3) +k2*a3*s3)/invdet;
	s2 = (-k1*a3*s3 + k2*(a2+a3*c3))/invdet;

	jnt[THETA2] = atan2(s2,c2);	
	jnt[THETA4] = Theta234 - jnt[THETA2] - jnt[THETA3];		
	

	return 0;
}

// Direct Kinematics:
int DirectKinematics(double *cpnt, double *jnt)
{
	double c1,s1;
	double c2,s2;
	double c23,s23;
	double c234,s234;
	double c5,s5;
	double A05[3][3];

	sincos(jnt[0],&s1,&c1);
	sincos(jnt[1],&s2,&c2);
	sincos(jnt[1]+jnt[2],&s23,&c23);
	sincos(jnt[1]+jnt[2]+jnt[3],&s234,&c234);
	sincos(jnt[4],&s5,&c5);

	double L = (a1 + a2*c2 + a3*c23 - d5*s234);
	X  = c1*L; 											// X
	Y  = s1*L; 											// Y
	Z  = d1 - a2*s2 - a3*s23  - d5*c234;		// Z


	Ux = c1*c234*c5 + s1*s5;
	Uy = s1*c234*c5 - c1*s5;
	Uz = -s234*c5;

	Vx = -c1*c234*s5 + s1*c5;
	Vy = -s1*c234*s5 - c1*c5;
	Vz = s234*s5;

	Wx = -c1*s234;
	Wy = -s1*s234;
	Wz = -c234;

	Mat2Quat(A05,cpnt);
	
	return 0;
}

// Accessibility test:
int Accessible(double *cpnt)
{
	double l = sqrt(X*X + Y*Y + Z*Z);

	if (l < rmin) return -1; // Too Close
	if (l > rmax) return  1; // Too Far
	return 0;					 // OK
}



static RBT_UFUN user;

extern "C"
{

	int USR_VER(int,int,int) { return 10000;} // Version ID

// Here it is the link function that connects user kinematics to a user group.
	int USR_SCORBOT(int id, int, int)
	{
			user.ijac   = 0;						// No Inverse Jacobian provided
			user.ikin   = InverseKinematics;	// Inverse Kinematics
			user.dkin   = DirectKinematics;	// Direct Kineamtics
			user.setup  = Setup;					// Setup (Off-Line) computations
			user.cfg    = Config;				// Configuration flags function
			user.acces  = Accessible;			// Accessibility test

			// Link to a group:
			return rbt_SetUserKin (id, user);
	}
}

See Also:

Retrieved from "http://softmc.servotronix.com/index.php?title=Program_Examples:User_Kinematics&oldid=129892"