Program Examples:User Kinematics
User Kinematics
#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);
}
}