Re: Planar Hexapod Kinematics
Hi!
What I experienced with my Clavel-style kinematic is that I couldnt afford
the kinematics functions being called as often as in the case of trivkins.
For The genhexkins there was implementd a NO_FORWARD_TRAFO or something that
prevents emcmot do calculate the actual TCP-Position out of the Motor
Positions in each cycle. I dont remember exactly. You should have a look at
that. Instead I think it is sufficient to assume the actual position is the
same as the commanded position. So in emcmot this is simply assigned. As far
as I understood it, emcmot made the control part in cartesian world
coordinates. In their simple case these were the same as the drive
coordinates. In the case of Parallelkinematics it is very demanding to make
the control (PID) in world coordinates and transform back into drive
coordinates. Many people would like to do that, because it would result in
better behaviour, but it is simply too time consuming if the forward trafo
is iterative. In your case, it isnt iterative, but still, methinks, in
emcmot there are some trafo things that can simply be omitted in order to
speed things up. Maybe your following errors have something to do with it.
Till
Lukas du Plessis wrote:
> 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
--
Dipl.-Ing. Till Franitza
Institute for Control Engineering of Machine Tools and
Manufacturing Units (ISW)
c.o. FISW
Rosenbergstr. 28
D- 70174 Stuttgart
Germany
phon: ++49 (0)711 22 99 2-28
fax: ++49 (0)711 22 99 2-22
email: till.franitza-at-isw.uni-stuttgart.de
web: www.isw.uni-stuttgart.de
Date Index |
Thread Index |
Back to archive index |
Back to Mailing List Page
Problems or questions? Contact