Skip to content

Instantly share code, notes, and snippets.

@howiemnet
Created April 1, 2015 06:23
Show Gist options
  • Save howiemnet/83bdaa2bb26c1967d589 to your computer and use it in GitHub Desktop.
Save howiemnet/83bdaa2bb26c1967d589 to your computer and use it in GitHub Desktop.
Puma Jacobian functions from RCCL / RCI, (C) 1987, 1991 J Lloyd and V Hayward
/*-=-=-=-=-=-=-=-=-=-=-=- RCCL/RCI Notice of Copyright -=-=-=-=-=-=-=-=-=-=-*
* *
* Copyright (C) 1987, 1991 by John E. Lloyd and Vincent Hayward. *
* *
* Information on copyright permissions, as well as a complete *
* disclaimer of warranty, is given in the file COPYRIGHT. *
* *
*-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/
/*=~=~=~=~ McGill-Computer-Vision-and-Robotics-Laboratory--C-Module ~=~=~=~=
*
* MODULE NAME: pumaJacob.c
*
* DESCRIPTION: Jacobian computations for the PUMA robots.
* For description, see 'man pumaJacob'
*
* AUTHOR/DATE: John Lloyd, Spring 1987, after Vincent Hayward, Purdue, 1983
* Rewrote routines to interface with new RCI, winter 1988.
*
*~=~=~=~=~=~= McGill-Computer-Vision-and-Robotics-Laboratory ~=~=~=~=~=~=*/
#include <math.h>
#include "../h/fastmath.h"
#include "../h/cdefs.h"
#include "../h/robot_jls.h"
#include "../h/robotMath.h"
#include "../h/puma_jls.h"
#include "../h/puma_kynvar.h"
#include "../h/robotTypes.h"
#define EPSILONF 2.0e-6
#define J1 0
#define J2 1
#define J3 2
#define J4 3
#define J5 4
#define J6 5
/* ---- Definition of link parameters to make code more readable ---- */
#define A2 gen->dh[1].a /* basic A matrix parameters */
#define D3 gen->dh[2].d
#define D4 gen->dh[3].d
#define A3 gen->dh[2].a
#define D32 pkyn->d32
#define A22A32D42 pkyn->a22a32d42 /* A2*A2 - A3*A3 - D4*D4 */
#define ATANA3D4 pkyn->atana3d4 /* atan2(A3, D4) */
#define EPSILON_A2D4 pkyn->epsilon_a2d4 /* Precision of (A2+D4)/2 */
#define EPSILON_D32 pkyn->epsilon_d32 /* Precision of D32 */
/* ---- Definition of var structure parameters ---- */
#define S1 pvar->s[J1]
#define C1 pvar->c[J1]
#define S2 pvar->s[J2]
#define C2 pvar->c[J2]
#define S23 pvar->s23
#define C23 pvar->c23
#define S3 pvar->s[J3]
#define C3 pvar->c[J3]
#define S4 pvar->s[J4]
#define C4 pvar->c[J4]
#define S5 pvar->s[J5]
#define C5 pvar->c[J5]
#define S6 pvar->s[J6]
#define C6 pvar->c[J6]
#define U5 pvar->u5
pumaFwdJacobT(t, f, j6, kyn)
float *t;
FORCE *f;
float *j6;
KYN *kyn;
{
PUMA_VAR var;
pumaUpdateVar (&var, j6, kyn);
return (pumaFwdJacobTVar (t, f, &var, kyn));
}
pumaFwdJacob(dc, dj, j6, kyn)
DIFF *dc;
float *dj;
float *j6;
KYN *kyn;
{
PUMA_VAR var;
pumaUpdateVar (&var, j6, kyn);
return (pumaFwdJacobVar (dc, dj, &var, kyn));
}
pumaInvJacobT(f, t, j6, kyn, epsilon)
FORCE *f;
float *t;
float *j6;
KYN *kyn;
double epsilon;
{
PUMA_VAR var;
pumaUpdateVar (&var, j6, kyn);
return (pumaInvJacobTVar (f, t, &var, kyn, epsilon));
}
pumaInvJacob(dj, dc, j6, kyn, epsilon)
float *dj;
DIFF *dc;
float *j6;
KYN *kyn;
double epsilon;
{
PUMA_VAR var;
pumaUpdateVar (&var, j6, kyn);
return (pumaInvJacobVar (dj, dc, &var, kyn, epsilon));
}
pumaFwdJacobTVar(j, f, var, kyn) /*::*/
float *j;
FORCE *f;
void *var;
KYN *kyn;
{
register PUMA_KYN *pkyn =(PUMA_KYN*)kyn->ext;
register PUMA_VAR *pvar = (PUMA_VAR*)var;
GEN_KYN *gen = kyn->gen;
FORCE fl4;
fl4.f.x = U5.n.x * f->f.x + U5.o.x * f->f.y + U5.a.x * f->f.z;
fl4.f.y = U5.n.y * f->f.x + U5.o.y * f->f.y + U5.a.y * f->f.z;
fl4.f.z = U5.n.z * f->f.x + U5.o.z * f->f.y + U5.a.z * f->f.z;
fl4.m.x = U5.n.x * f->m.x + U5.o.x * f->m.y + U5.a.x * f->m.z;
fl4.m.y = U5.n.y * f->m.x + U5.o.y * f->m.y + U5.a.y * f->m.z;
fl4.m.z = U5.n.z * f->m.x + U5.o.z * f->m.y + U5.a.z * f->m.z;
if (pkyn->oldCoordDefs)
{ j[J1] = pvar->j11 * fl4.f.x + pvar->j21 * fl4.f.y +
pvar->j31 * fl4.f.z + pvar->j41 * fl4.m.x -
C23 * fl4.m.y + pvar->j61 * fl4.m.z;
j[J2] = pvar->j12 * fl4.f.x + pvar->j22 * fl4.f.y +
pvar->j32 * fl4.f.z;
j[J3] = pvar->j13 * fl4.f.x + A3 * fl4.f.y +
pvar->j33 * fl4.f.z + S4 * fl4.m.x + C4 * fl4.m.z;
j[J2] += j[J3];
j[J4] = - fl4.m.y;
j[J5] = fl4.m.z;
j[J6] = S5 * fl4.m.x - C5 * fl4.m.y;
}
else
{ j[J1] = pvar->j11 * fl4.f.x + pvar->j21 * fl4.f.y +
pvar->j31 * fl4.f.z + pvar->j41 * fl4.m.x +
C23 * fl4.m.y + pvar->j61 * fl4.m.z;
j[J2] = pvar->j12 * fl4.f.x + pvar->j32 * fl4.f.z +
pvar->j22 * fl4.f.y;
j[J3] = pvar->j13 * fl4.f.x + A3 * fl4.f.y +
pvar->j33 * fl4.f.z - S4 * fl4.m.x + C4 * fl4.m.z;
j[J2] += j[J3];
j[J4] = fl4.m.y;
j[J5] = fl4.m.z;
j[J6] = - S5 * fl4.m.x + C5 * fl4.m.y;
}
}
pumaFwdJacobVar(d, q, var, kyn) /*::*/
register DIFF_PTR d;
register float *q;
void *var;
KYN *kyn;
{
register PUMA_KYN *pkyn =(PUMA_KYN*)kyn->ext;
register PUMA_VAR *pvar = (PUMA_VAR*)var;
GEN_KYN *gen = kyn->gen;
DIFF dl4;
float q23 = q[J2] + q[J3];
if (pkyn->oldCoordDefs)
{ dl4.t.x = pvar->j11 * q[J1] + pvar->j12 * q[J2] + pvar->j13 * q23;
dl4.t.y = pvar->j21 * q[J1] + pvar->j22 * q[J2] + A3 * q23;
dl4.t.z = pvar->j31 * q[J1] + pvar->j32 * q[J2] + pvar->j33 * q23;
dl4.r.x = pvar->j41 * q[J1] + S4 * q23 + S5 * q[J6];
dl4.r.y = -C23 * q[J1] - q[J4] - C5 * q[J6];
dl4.r.z = pvar->j61 * q[J1] + C4 * q23 + q[J5];
}
else
{ dl4.t.x = pvar->j11 * q[J1] + pvar->j12 * q[J2] + pvar->j13 * q23;
dl4.t.y = pvar->j21 * q[J1] + pvar->j22 * q[J2] + A3 * q23;
dl4.t.z = pvar->j31 * q[J1] + pvar->j32 * q[J2] + pvar->j33 * q23;
dl4.r.x = pvar->j41 * q[J1] - S4 * q23 - S5 * q[J6];
dl4.r.y = C23 * q[J1] + q[J4] + C5 * q[J6];
dl4.r.z = pvar->j61 * q[J1] + C4 * q23 + q[J5];
}
d->t.x = U5.n.x * dl4.t.x + U5.n.y * dl4.t.y + U5.n.z * dl4.t.z;
d->t.y = U5.o.x * dl4.t.x + U5.o.y * dl4.t.y + U5.o.z * dl4.t.z;
d->t.z = U5.a.x * dl4.t.x + U5.a.y * dl4.t.y + U5.a.z * dl4.t.z;
d->r.x = U5.n.x * dl4.r.x + U5.n.y * dl4.r.y + U5.n.z * dl4.r.z;
d->r.y = U5.o.x * dl4.r.x + U5.o.y * dl4.r.y + U5.o.z * dl4.r.z;
d->r.z = U5.a.x * dl4.r.x + U5.a.y * dl4.r.y + U5.a.z * dl4.r.z;
}
pumaInvJacobVar(q, d, var, kyn, epsilon) /*::*/
register float *q;
register DIFF_PTR d;
void *var;
KYN *kyn;
double epsilon;
{
register PUMA_KYN *pkyn =(PUMA_KYN*)kyn->ext;
register PUMA_VAR *pvar = (PUMA_VAR*)var;
GEN_KYN *gen = kyn->gen;
DIFF dl4;
int code = 0;
register float threshold;
float q23, vx;
dl4.t.x = U5.n.x * d->t.x + U5.o.x * d->t.y + U5.a.x * d->t.z;
dl4.t.y = U5.n.y * d->t.x + U5.o.y * d->t.y + U5.a.y * d->t.z;
dl4.t.z = U5.n.z * d->t.x + U5.o.z * d->t.y + U5.a.z * d->t.z;
dl4.r.x = U5.n.x * d->r.x + U5.o.x * d->r.y + U5.a.x * d->r.z;
dl4.r.y = U5.n.y * d->r.x + U5.o.y * d->r.y + U5.a.y * d->r.z;
dl4.r.z = U5.n.z * d->r.x + U5.o.z * d->r.y + U5.a.z * d->r.z;
/* Massage epsilon into something reasonable */
if (epsilon < EPSILONF)
{ epsilon = EPSILONF;
}
if (pkyn->oldCoordDefs)
{
/* theta1 */
threshold = epsilon * pvar->h1_norm;
if (FABS(pvar->h1) < threshold)
{ code |= PUMA_ALIGN_DEG;
q[J1] = (S4 * dl4.t.x + C4 * dl4.t.z) * pvar->h1 /
(threshold * threshold);
}
else
{ q[J1] = (S4 * dl4.t.x + C4 * dl4.t.z) / pvar->h1;
}
/* theta2 */
vx = C4 * dl4.t.x - S4 * dl4.t.z;
threshold = epsilon * pkyn->sqrt_a32d42;
if (FABS(pvar->h3) < threshold)
{ code |= PUMA_ELBOW_DEG;
q[J2] = (- D4 * dl4.t.y + A3 * vx + q[J1] * D3 * pvar->h2 )
* (pvar->h3) / (A2 * threshold * threshold);
}
else
{ q[J2] = (- D4 * dl4.t.y + A3 * vx + q[J1] * D3 * pvar->h2 )
/ (A2 * pvar->h3);
}
/* theta3 */
q23 = ( vx - A2 * S3 * q[J2] + D3 * C23 * q[J1] ) / D4;
q[J3] = q23 - q[J2];
/* theta6 */
threshold = epsilon;
if (FABS(S5) < threshold)
{ code |= PUMA_WRIST_DEG;
q[J6] = (dl4.r.x + S23 * C4 * q[J1] -
S4 * q23) * S5 / (threshold * threshold);
}
else
{ q[J6] = (dl4.r.x + S23 * C4 * q[J1] -
S4 * q23) / S5;
}
/* theta4 */
q[J4] = - dl4.r.y - C23 * q[J1] - C5 * q[J6];
/* theta5 */
q[J5] = dl4.r.z - S23 * S4 * q[J1] - C4 * q23;
}
else
{
/* theta1 */
threshold = epsilon * pvar->h1_norm;
if (FABS(pvar->h1) < threshold)
{ code |= PUMA_ALIGN_DEG;
q[J1] = (S4 * dl4.t.x - C4 * dl4.t.z) * pvar->h1 /
(threshold * threshold);
}
else
{ q[J1] = (S4 * dl4.t.x - C4 * dl4.t.z) / pvar->h1;
}
/* theta2 */
vx = C4 * dl4.t.x + S4 * dl4.t.z;
threshold = epsilon * pkyn->sqrt_a32d42;
if (FABS(pvar->h3) < threshold)
{ code |= PUMA_ELBOW_DEG;
q[J2] = (D4 * dl4.t.y + A3 * vx - q[J1] * D3 * pvar->h2 )
* (pvar->h3) / (A2 * threshold * threshold);
}
else
{ q[J2] = (D4 * dl4.t.y + A3 * vx - q[J1] * D3 * pvar->h2 )
/ (A2 * pvar->h3);
}
/* theta3 */
q23 = ( -vx + A2 * S3 * q[J2] + D3 * C23 * q[J1] ) / D4;
q[J3] = q23 - q[J2];
/* theta6 */
threshold = epsilon;
if (FABS(S5) < threshold)
{ code |= PUMA_WRIST_DEG;
q[J6] = (- dl4.r.x + S23 * C4 * q[J1] -
S4 * q23) * S5 / (threshold * threshold);
}
else
{ q[J6] = (- dl4.r.x + S23 * C4 * q[J1] -
S4 * q23) / S5;
}
/* theta4 */
q[J4] = dl4.r.y - C23 * q[J1] - C5 * q[J6];
/* theta5 */
q[J5] = dl4.r.z - S23 * S4 * q[J1] - C4 * q23;
}
return (code);
}
pumaInvJacobTVar(f, t, var, kyn, epsilon)
register FORCE *f;
register float *t;
void *var;
KYN *kyn;
double epsilon;
{
register PUMA_KYN *pkyn =(PUMA_KYN*)kyn->ext;
register PUMA_VAR *pvar = (PUMA_VAR*)var;
GEN_KYN *gen = kyn->gen;
FORCE fl4;
int code = 0;
float tor2, threshold, vx, vz, wx, wy, wz;
/* Set threshold to something reasonable */
if (epsilon < EPSILONF)
{ epsilon = EPSILONF;
}
tor2 = t[J2] - t[J3];
if (pkyn->oldCoordDefs)
{
fl4.m.z = t[J5];
fl4.m.y = -t[J4];
threshold = epsilon;
if (FABS(S5) < threshold)
{ code |= PUMA_WRIST_DEG;
fl4.m.x = (- C5 * t[J4] + t[J6]) * S5 /
(threshold * threshold);
}
else
{ fl4.m.x = (- C5 * t[J4] + t[J6]) / S5;
}
vx = t[J1] + C23 * fl4.m.y +
S23 * (C4 * fl4.m.x - S4 * fl4.m.z);
vz = t[J3] - fl4.m.x * S4 - fl4.m.z * C4;
threshold = epsilon * pkyn->sqrt_a32d42;
if (FABS(pvar->h3) < threshold)
{ code |= PUMA_ELBOW_DEG;
wz = (tor2 * D4 - A2 * S3 * vz)
* (pvar->h3) / (A2 * threshold * threshold);
}
else
{ wz = (tor2 * D4 - A2 * S3 * vz) / (A2 * pvar->h3);
}
wx = (A3 * wz + vz) / D4;
threshold = epsilon * pvar->h1_norm;
if (FABS(pvar->h1) < threshold)
{ code |= PUMA_ALIGN_DEG;
wy = (D3 * (S23 * wz + C23 * wx) + vx)
* pvar->h1 / (threshold * threshold);
}
else
{ wy = (D3 * (S23 * wz + C23 * wx) + vx)
/ pvar->h1;
}
fl4.f.y = -wz;
fl4.f.x = C4 * wx + S4 * wy;
fl4.f.z = - S4 * wx + C4 * wy;
}
else
{ fl4.m.z = t[J5];
fl4.m.y = t[J4];
threshold = epsilon;
if (FABS(S5) < threshold)
{ code |= PUMA_WRIST_DEG;
fl4.m.x = (C5 * t[J4] - t[J6]) * S5 /
(threshold * threshold);
}
else
{ fl4.m.x = (C5 * t[J4] - t[J6]) / S5;
}
vx = t[J1] - (C23 * fl4.m.y +
S23 * (C4 * fl4.m.x + S4 * fl4.m.z));
vz = t[J3] + fl4.m.x * S4 - fl4.m.z * C4;
threshold = epsilon * pkyn->sqrt_a32d42;
if (FABS(pvar->h3) < threshold)
{ code |= PUMA_ELBOW_DEG;
fl4.f.y = (tor2 * D4 + A2 * S3 * vz)
* (pvar->h3) / (A2 * threshold * threshold);
}
else
{ fl4.f.y = (tor2 * D4 + A2 * S3 * vz) / (A2 * pvar->h3);
}
wx = (A3 * fl4.f.y - vz) / D4;
threshold = epsilon * pvar->h1_norm;
if (FABS(pvar->h1) < threshold)
{ code |= PUMA_ALIGN_DEG;
wy = (D3 * (S23 * fl4.f.y - C23 * wx) + vx)
* pvar->h1 / (threshold * threshold);
}
else
{ wy = (D3 * (S23 * fl4.f.y - C23 * wx) + vx)
/ pvar->h1;
}
fl4.f.x = C4 * wx + S4 * wy;
fl4.f.z = S4 * wx - C4 * wy;
}
f->m.x = U5.n.x * fl4.m.x + U5.n.y * fl4.m.y + U5.n.z * fl4.m.z;
f->m.y = U5.o.x * fl4.m.x + U5.o.y * fl4.m.y + U5.o.z * fl4.m.z;
f->m.z = U5.a.x * fl4.m.x + U5.a.y * fl4.m.y + U5.a.z * fl4.m.z;
f->f.x = U5.n.x * fl4.f.x + U5.n.y * fl4.f.y + U5.n.z * fl4.f.z;
f->f.y = U5.o.x * fl4.f.x + U5.o.y * fl4.f.y + U5.o.z * fl4.f.z;
f->f.z = U5.a.x * fl4.f.x + U5.a.y * fl4.f.y + U5.a.z * fl4.f.z;
return (code);
}
/* Jacobian updating functions */
pumaUpdateVar (var, ang, kyn)
void *var;
register float *ang;
KYN *kyn;
{
register PUMA_VAR *pvar = (PUMA_VAR*)var;
float ang23 = ang[J2] + ang[J3];
SINCOS (ang[J1], &S1, &C1);
SINCOS (ang[J2], &S2, &C2);
SINCOS (ang[J3], &S3, &C3);
SINCOS (ang[J4], &S4, &C4);
SINCOS (ang[J5], &S5, &C5);
SINCOS (ang[J6], &S6, &C6);
SINCOS (ang23, &S23, &C23);
pumaUpdateVarXsincos (var, ang, kyn);
}
pumaUpdateVarXsincos (var, ang, kyn)
void *var;
register float *ang;
KYN *kyn;
{
register PUMA_KYN *pkyn =(PUMA_KYN*)kyn->ext;
register PUMA_VAR *pvar = (PUMA_VAR*)var;
register GEN_KYN *gen = kyn->gen;
if (pkyn->oldCoordDefs)
{
/* update h values */
pvar->h2 = C23 * A3 +
S23 * D4;
pvar->h1 = pvar->h2 + C2 * A2;
pvar->h3 = S3 * A3 - C3 * D4;
pvar->h1_norm = D4 + A2;
/* update terms of the jacobian */
pvar->j11 = pvar->h1 * S4 -
D3 * C23 * C4;
pvar->j21 = S23 * D3;
pvar->j31 = pvar->h1 * C4 + D3 * C23 * S4;
pvar->j41 = -S23 * C4;
pvar->j61 = S23 * S4;
pvar->j12 = A2 * S3 * C4;
pvar->j22 = A2 * C3;
pvar->j32 = -A2 * S3 * S4;
pvar->j13 = C4 * D4;
pvar->j33 = -S4 * D4;
/* update the U5 coefficients */
U5.n.x = C5 * C6;
U5.n.y = S5 * C6;
U5.n.z = S6;
U5.o.x = -C5 * S6;
U5.o.y = -S5 * S6;
U5.o.z = C6;
U5.a.x = S5;
U5.a.y = -C5;
U5.a.z = 0.;
}
else
{ /* update h values */
pvar->h2 = C23 * A3 +
- S23 * D4;
pvar->h1 = pvar->h2 + C2 * A2;
pvar->h3 = S3 * A3 + C3 * D4;
pvar->h1_norm = sqrt (pkyn->d42a22a32 +
2.0 * A2 * (A3 * C3 - D4 * S3));
/* update terms of the jacobian */
pvar->j11 = pvar->h1 * S4 +
D3 * C23 * C4;
pvar->j21 = -S23 * D3;
pvar->j31 = -pvar->h1 * C4 + D3 *
C23 * S4;
pvar->j41 = S23 * C4;
pvar->j61 = S23 * S4;
pvar->j12 = A2 * S3 * C4;
pvar->j22 = A2 * C3;
pvar->j32 = A2 * S3 * S4;
pvar->j13 = -C4 * D4;
pvar->j33 = -S4 * D4;
/* update the U5 coefficients */
U5.n.x = C5 * C6;
U5.n.y = S5 * C6;
U5.n.z = -S6;
U5.o.x = -C5 * S6;
U5.o.y = -S5 * S6;
U5.o.z = -C6;
U5.a.x = -S5;
U5.a.y = C5;
U5.a.z = 0.;
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment