Program Examples:User Kinematics
| Language: | English • 中文（简体） |
|---|
User Kinematics
 | 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.
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);
}
}