---

lukaskins.c

Go to the documentation of this file.

/*
  lukaskins.c

  Modification history:

  15-May-2001  FMP changed kinematicsType to KINEMATICS_BOTH
  4-May-2001  FMP fixed undef's, which were defs
  16-Apr-2001  FMP created from Lukas du Plessis' figure
  */

/*
  These kinematics are for Lukas du Plessis' planar parallel mechanism:

       A--------B
      / \        \       ^
     /   \        \      |
    /     \        \     y
   /       \        \    |
  C---------D--------E   +-- x -->

  where C is the origin;
  the x and y directions are to the left and up, respectively, and z is
  out of the page;
  AC, AD, and BE are struts whose length can be controlled;
  C, D, and E are fixed points; and
  AB is a fixed-length strut.

  The kinematics use variables named from the figure, where single capital
  letters indicate vertex points (e.g., A, B); single capital letters
  followed by lowercase x or y indicate the x or y coordinate of the vertex
  point, respectively, with respect to the origin C; double capital letters
  indicate the length of sides (e.g., AB, BE); and triple lowercase letters
  indicate angles (e.g., bae, the angle between BA and AC).

  Changing the lengths of AC, AD, and BE result in points A and B moving
  about. Point B is the controlled point, and the three Cartesian variables
  are Bx, By, and the angle of AB relative to the horizontal (x) axis,
  called theta. This is the angle of rotation of B about the z axis,
  In the above figure theta (not labeled) is 0.

  The arguments to the kinematics functions are named "pos" for the 
  Cartesian EmcPose type, and "joints" for the joint (strut length) array.
  The association between these names and names from the figure are:

  pos->tran.x = Bx
  pos->tran.y = By
  pos->c      = theta

  The inverse kinematics have no singularities. Any values for Bx, By, and
  theta will yield numerical results. Of course, these may be beyond the
  strut length limits, but there are no singular effects like infinite speed.

  The forward kinematics have two singularities. The first occurs when
  struts AC and AD are not long enough to span base length CD and the
  triangle inquality is violated. The second occurs when struts AB and BE
  are not long enough to span the distance between points A and E.

  The forward kinematics flags, referred to in kinematicsForward and
  set in kinematicsInverse, let the forward kinematics select between
  multiple valid solutions of B and theta for a given set of strut
  values. The two types of solutions correspond to

  a) "head" point A is above (as in figure) or below x axis;
  b) "elbow" point B is up (as in figure) or down.

  Bit 0 of fflags selects between (a), with 0 = above, 1 = below;
  bit 1 of fflags selects between (b), with 0 = up, 1 = down.

  The inverse kinematics flags let the inverse kinematics select between
  multiple valid solutions of strut lengths for given Cartesian values
  for B and theta. There are no multiple solutions: B and theta constrain
  the strut lengths completely. So, the inverse flags are ignored.
 */

#include "emcmot.h"             /* these decls */
#include <math.h>

/* ident tag */
#ifndef __GNUC__
#ifndef __attribute__
#define __attribute__(x)
#endif
#endif

/* define the constant strut lengths here */
#define CD 10.0
#define DE 4.0
#define AB 8.7

#define sq(x) ((x)*(x))

/*
  forward kinematics takes three strut lengths and computes Bx, By,
  and theta, as pos->tran.x,y, and pos->c, respectively. The forward
  flags are used to resolve head above/below and elbow up/down. The
  inverse flags are not set since there are no ambiguities going from
  world to joint coordinates.
*/
int kinematicsForward(const double * joints,
                      EmcPose * pos,
                      const KINEMATICS_FORWARD_FLAGS * fflags,
                      KINEMATICS_INVERSE_FLAGS * iflags)
{
#define AC (joints[0])
#define AD (joints[1])
#define BE (joints[2])
#define Bx (pos->tran.x)
#define By (pos->tran.y)
#define theta (pos->c)
  double temp;
  double CE;
  double AE;
  double Ax, Ay;
  double bae;
  double aec;

  Ax = (sq(AC) + sq(CD) - sq(AD)) / (2.0 * CD);
  temp = sq(AC) - sq(Ax);
  if (temp < 0.0) {
    return -1;
  }
  Ay = sqrt(temp);
  if (*fflags % 2) {
    Ay = -Ay;                   /* head below */
  }

  CE = CD + DE;
  AE = sqrt(sq(Ay) + sq(CE - Ax));
  temp = (sq(AB) + sq(AE) - sq(BE)) / (2.0 * AB * AE);
  if (temp < -1.0 ||
      temp > +1.0) {
    return -1;
  }
  bae = acos(temp);
  if ((*fflags >> 1) % 2) {
    bae = -bae;                 /* elbow down */
  }
  /* else elbow up */
  aec = atan2(Ay, CE - Ax);
  theta = bae - aec;
  Bx = Ax + AB * cos(theta);
  By = Ay + AB * sin(theta);

  /* pos->tran.x,y are Bx, By resp.; pos->c is theta; they're
     already done via #define, so no assignment necessary. We'll
     set the others to 0. */
  pos->tran.z = 0.0;
  pos->a = 0.0;
  pos->b = 0.0;

  return 0;

#undef AC
#undef AD
#undef BE
#undef Bx
#undef By
#undef theta
}

int kinematicsInverse(const EmcPose * pos,
                      double * joints,
                      const KINEMATICS_INVERSE_FLAGS * iflags,
                      KINEMATICS_FORWARD_FLAGS * fflags)
{
#define AC (joints[0])
#define AD (joints[1])
#define BE (joints[2])
#define Bx (pos->tran.x)
#define By (pos->tran.y)
#define theta (pos->c)
  double CE;
  double Ax;
  double Ay;

  CE = CD + DE;
  BE = sqrt(sq(Bx - CE) + sq(By));
  Ax = Bx - AB * cos(theta);
  Ay = By - AB * sin(theta);
  AC = sqrt(sq(Ax) + sq(Ay));
  AD = sqrt(sq(CD - Ax) + sq(Ay));

  /* AC, AD, and BE are joints[0,1,2], resp.; they're already 
     done via #define, so no assignment necessary */

  /* set fflags to appropriately for given B and theta */
  *fflags = 0;
  if (Ay < 0.0) {
    *fflags = 1;                /* head below */
  }
  /* use sign of cross product EB X BA for elbow; positive is
     elbow up */
  if ((Bx - CE) * (Ay - By) - By * (Ax - Bx) < 0.0) {
    *fflags += 2;
  }
    
  return 0;

#undef AC
#undef AD
#undef BE
#undef Bx
#undef By
#undef theta
}

/* the inverse kinematics have no singularities, so kinematicsHome gives
   the world position preferences, and fills in the joint values */
int kinematicsHome(EmcPose * world,
                   double * joint,
                   KINEMATICS_FORWARD_FLAGS * fflags,
                   KINEMATICS_INVERSE_FLAGS * iflags)
{
  *fflags = 0;
  *iflags = 0;

  return kinematicsInverse(world, joint, iflags, fflags);
}

KINEMATICS_TYPE kinematicsType()
{
  return KINEMATICS_BOTH;
}

---