Planar Hexapod Kinematics
- Subject: Planar Hexapod Kinematics
- From: Lukas du Plessis <lplessis-at-eng.up.ac.za>
- Date: Mon, 26 Mar 2001 10:29:34 +0100
- Content-Transfer-Encoding: 7bit
- Content-Type: text/plain; charset=us-ascii
Hi everybody
We are experiencing problems with implementing the kinematics of our
planar hexapod. When using only the inverse kinematics, by passing
EMCMOT_NO_FORWARD_KINEMATICS=1 to insmod when insmoding emcfreqmot.o, we
encounter following- errors. Our .ini file is set up as suggested by
Ray to enforce 200 steps per mm (see below)
___________________________________________________snippet from .ini
file
; Trajectory planner section
--------------------------------------------------
[TRAJ]
AXES = 3
; COORDINATES = X Y Z R P W
COORDINATES = X Y Z
HOME = 0 0 0
LINEAR_UNITS = 1.00000000
ANGULAR_UNITS = 1.0
CYCLE_TIME = 0.010
DEFAULT_VELOCITY = 0.42418
MAX_VELOCITY = 30.48
DEFAULT_ACCELERATION = 5.0
MAX_ACCELERATION = 5.0
..
; Axes sections
---------------------------------------------------------------
; First axis
[AXIS_0]
TYPE = LINEAR
UNITS = 1.0000000000
HOME = 0.000
MAX_VELOCITY = 30.48
P = 200.000
I = 0.000
D = 0.000
FF0 = 0.000
FF1 = 0.000
FF2 = 0.000
BACKLASH = 0.000
BIAS = 0.000
MAX_ERROR = 0.000
DEADBAND = 0.01
CYCLE_TIME = 0.001000
INPUT_SCALE = 200.0 0
OUTPUT_SCALE = 200.0 0
MIN_LIMIT = -1000.0
MAX_LIMIT = 1000.0
MIN_OUTPUT = -10
MAX_OUTPUT = 10
FERROR = 25.400
MIN_FERROR = 0.254
HOMING_VEL = 0.1
HOME_OFFSET = 1
ENABLE_POLARITY = 0
MIN_LIMIT_SWITCH_POLARITY = 0
MAX_LIMIT_SWITCH_POLARITY = 0
HOME_SWITCH_POLARITY = 1
HOMING_POLARITY = 1
JOGGING_POLARITY = 1
FAULT_POLARITY = 1
___________________________________________________end snippet from .ini
file
We then coded a very crued forward kinematics routine, using the law of
cosines and the law of sines to obtain the leg lengths for a given
position and orientation of the moving platform. In particular we used
the following layout:
A
_____________ B
/\ |
/
\ |
joints[0] / \
joints[1] | joints[2]
/
\ |
/________\
________|
C
D E
We defined AB=EX_1, CD=EX_2, DE=EX_5, (Cx, Cy)=(EX_4,
EX_3)
and further AB^2=EX_1_2, CD^2=EX_2_2, DE^2=EX_5_2
The angles are given by the three letters, eg. the angle between vectors
CA and CD in the above figure is defined as ACD.
Finally the midpoint of AB is the point of interest, therefore its
position is given by (pos->tran.x , pos->tran.y), and the orientation of
moving platform AB is phi.
As a start, we decided to keep the orientation (phi) separate, and try
to get the mechanism moving as desired by specifying phi=0. Once this
is working, we will specify phi=pos->c
We modified trivkins.c as follows:
___________________________________________________beginning of modified
trivkins.c
/*
trivkins.c
Modification history:
13-Mar-2000 WPS added unused attribute to ident to avoid 'defined but
not used' compiler warning.
11-Aug-1999 FMP added kinematicsType()
9-Aug-1999 FMP added kinematicsHome(), changed naming from invK to
kinI
18-Dec-1997 FMP changed to PmPose
16-Oct-1997 FMP created
*/
#include "emcmot.h" /* these decls */
#include <math.h>
#define DIRECT
/* ident tag */
#ifndef __GNUC__
#ifndef __attribute__
#define __attribute__(x)
#endif
#endif
double PI =3.1415926535359;
double EX_1 =305;
double EX_1_2 =93025;
double EX_2 =250;
double EX_2_2 =62500;
double EX_3 =-400;
double EX_4 =-100;
double EX_5 =200;
double EX_5_2 =40000;
static char __attribute__((unused)) ident[] = "$Id: trivkins.c,v 1.4
2000/12/21 16:22:11 wshackle Exp $";
int kinematicsForward(const double * joints,
EmcPose * pos,
const KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags)
{
double acd;
double adc;
double ade;
double ae;
double aed;
double aeb;
double bef;
double xb;
double yb;
double xa;
double ya;
double phi;
#ifndef DIRECT
if(joints[0] == 0)
{ joints[0] = 403.430601;
joints[1] = 501.503988;
joints[2] = 446.101166;
}
#endif
// rcs_print("%f/n/r",joints[0]);
acd = acos( ( joints[0] * joints[0] + EX_2_2 - joints[1] * joints[1])
/ (2.0 * joints[0] * EX_2) );
xa = EX_4 + joints[0] * cos(acd);
ya = EX_3 + joints[0] * sin(acd);
adc = asin( ( joints[0] / joints[1] ) * sin(acd) );
ade = PI - adc;
ae = sqrt( joints[1]*joints[1] + EX_5_2 - 2*joints[1]*EX_5*cos(ade));
aed = asin( ( joints[1]/ae ) * sin(ade) );
aeb = acos( ( ae*ae + joints[2]*joints[2] - EX_1_2 ) / (
2*ae*joints[2] ) );
bef = PI - aed - aeb;
xb = EX_4 + EX_2 + EX_5 + joints[2]*cos(bef);
yb = EX_3 + joints[2]*sin(bef);
phi = atan2((yb - ya), (xb - xa));
pos->tran.x = xa + 0.5*EX_1*cos(phi);
pos->tran.y = ya + 0.5*EX_1*sin(phi);
pos->tran.z = 0;
#ifdef DIRECT
pos->tran.x = joints[0];
pos->tran.y = joints[1];
pos->tran.z = joints[2];
#endif
pos->a = joints[3];
pos->b = joints[4];
pos->c = joints[5];
return 0;
}
int kinematicsInverse(const EmcPose * pos,
double * joints,
const KINEMATICS_INVERSE_FLAGS * iflags,
KINEMATICS_FORWARD_FLAGS * fflags)
{
double phi = 0.0;
joints[0] = sqrt( pow( (pos->tran.x - 0.5 * EX_1 * cos (phi) -
EX_4 ),2) + pow((pos->tran.y - 0.5 * EX_1 * sin (phi) -
EX_3),2) );
joints[1] = sqrt( pow( (pos->tran.x - 0.5 * EX_1 * cos (phi) - EX_4 -
EX_2 ),2) + pow((pos->tran.y - 0.5 * EX_1 * sin (phi) - EX_3),2)
);
joints[2] = sqrt( pow( (pos->tran.x + 0.5 * EX_1 * cos (phi) - EX_4 -
EX_2 - EX_5),2) + pow((pos->tran.y + 0.5 * EX_1 * sin (phi) - EX_3),2)
);
#ifdef DIRECT
joints[0] = pos->tran.x;
joints[1] = pos->tran.y;
joints[2] = pos->tran.z;
#endif
joints[3] = pos->a;
joints[4] = pos->b;
joints[5] = pos->c;
return 0;
}
/* implemented for these kinematics as giving joints preference */
int kinematicsHome(EmcPose * world,
double * joint,
KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags)
{
*fflags = 0;
*iflags = 0;
joint[0] = 403.430601;
joint[1] = 501.503988;
joint[2] = 446.101166;
return kinematicsForward(joint, world, fflags, iflags);
}
KINEMATICS_TYPE kinematicsType()
{
return KINEMATICS_IDENTITY;
}
___________________________________________________end of modified
trivkins.c
Note that we added
if(joints[0] == 0)
{ joints[0] = 403.430601;
joints[1] = 501.503988;
joints[2] = 446.101166;
}
and
joint[0] = 403.430601;
joint[1] = 501.503988;
joint[2] = 446.101166;
because zero values result in NAN using the forward kinematics we coded.
We encountered the following problems:
Homing:
Is homing separated from the kinematics? When we highlighted
the X-axis in tkemc and clicked home, joints[0] of the
mechanism moved to the home switch. I would have thought that
homing while on the X-axis display would cause all three axis
to move, until an external Home switch is hit.
Homing while on the Y-axis display causes joints[1] of the
mechanism to run away regardless of the state of its home
switch.
Jogging:
Jogging while on the X-axis display, causes only joints[0] to
move, but the X- and Y- displays vary. We were expecting
joints[0], joints[1] and joints[2] to move and only the
Display to vary.
MDI: Since we couldn't home joints[1] and joints[2], an MDI command
results in following errors.
I am sorry for the long post, but if anyone is willing to help, it will
be greatly appreciated.
Lukas
Date Index |
Thread Index |
Back to archive index |
Back to Mailing List Page
Problems or questions? Contact