lukaskins.c File Reference

00103 {
00104 #define AC (joints[0])
00105 #define AD (joints[1])
00106 #define BE (joints[2])
00107 #define Bx (pos->tran.x)
00108 #define By (pos->tran.y)
00109 #define theta (pos->c)
00110   double temp;
00111   double CE;
00112   double AE;
00113   double Ax, Ay;
00114   double bae;
00115   double aec;
00116 
00117   Ax = (sq(AC) + sq(CD) - sq(AD)) / (2.0 * CD);
00118   temp = sq(AC) - sq(Ax);
00119   if (temp < 0.0) {
00120     return -1;
00121   }
00122   Ay = sqrt(temp);
00123   if (*fflags % 2) {
00124     Ay = -Ay;                   /* head below */
00125   }
00126 
00127   CE = CD + DE;
00128   AE = sqrt(sq(Ay) + sq(CE - Ax));
00129   temp = (sq(AB) + sq(AE) - sq(BE)) / (2.0 * AB * AE);
00130   if (temp < -1.0 ||
00131       temp > +1.0) {
00132     return -1;
00133   }
00134   bae = acos(temp);
00135   if ((*fflags >> 1) % 2) {
00136     bae = -bae;                 /* elbow down */
00137   }
00138   /* else elbow up */
00139   aec = atan2(Ay, CE - Ax);
00140   theta = bae - aec;
00141   Bx = Ax + AB * cos(theta);
00142   By = Ay + AB * sin(theta);
00143 
00144   /* pos->tran.x,y are Bx, By resp.; pos->c is theta; they're
00145      already done via #define, so no assignment necessary. We'll
00146      set the others to 0. */
00147   pos->tran.z = 0.0;
00148   pos->a = 0.0;
00149   pos->b = 0.0;
00150 
00151   return 0;
00152 
00153 #undef AC
00154 #undef AD
00155 #undef BE
00156 #undef Bx
00157 #undef By
00158 #undef theta
00159 }

00213 {
00214   *fflags = 0;
00215   *iflags = 0;
00216 
00217   return kinematicsInverse(world, joint, iflags, fflags);
00218 }

00165 {
00166 #define AC (joints[0])
00167 #define AD (joints[1])
00168 #define BE (joints[2])
00169 #define Bx (pos->tran.x)
00170 #define By (pos->tran.y)
00171 #define theta (pos->c)
00172   double CE;
00173   double Ax;
00174   double Ay;
00175 
00176   CE = CD + DE;
00177   BE = sqrt(sq(Bx - CE) + sq(By));
00178   Ax = Bx - AB * cos(theta);
00179   Ay = By - AB * sin(theta);
00180   AC = sqrt(sq(Ax) + sq(Ay));
00181   AD = sqrt(sq(CD - Ax) + sq(Ay));
00182 
00183   /* AC, AD, and BE are joints[0,1,2], resp.; they're already 
00184      done via #define, so no assignment necessary */
00185 
00186   /* set fflags to appropriately for given B and theta */
00187   *fflags = 0;
00188   if (Ay < 0.0) {
00189     *fflags = 1;                /* head below */
00190   }
00191   /* use sign of cross product EB X BA for elbow; positive is
00192      elbow up */
00193   if ((Bx - CE) * (Ay - By) - By * (Ax - Bx) < 0.0) {
00194     *fflags += 2;
00195   }
00196     
00197   return 0;
00198 
00199 #undef AC
00200 #undef AD
00201 #undef BE
00202 #undef Bx
00203 #undef By
00204 #undef theta
00205 }

00221 {
00222   return KINEMATICS_BOTH;
00223 }

lukaskins.c File Reference

Defines

Functions

Define Documentation

Function Documentation

Defines
#define	CD 10.0
#define	DE 4.0
#define	AB 8.7
#define	sq(x) ((x)*(x))
#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)
#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)
Functions
int	kinematicsForward (const double joints, EmcPose pos, const KINEMATICS_FORWARD_FLAGS fflags, KINEMATICS_INVERSE_FLAGS iflags)
int	kinematicsInverse (const EmcPose pos, double joints, const KINEMATICS_INVERSE_FLAGS iflags, KINEMATICS_FORWARD_FLAGS fflags)
int	kinematicsHome (EmcPose world, double joint, KINEMATICS_FORWARD_FLAGS fflags, KINEMATICS_INVERSE_FLAGS iflags)
KINEMATICS_TYPE	kinematicsType ()