Hexapod.cpp

00001 #include "Hexapod.h"
00002 #include "COM/LOGGER.h"
00003 
00004 using namespace std;
00005 static Logger *Ordi = Logger::Instance();
00006 void hexapod::step (float dt)
00007 {
00008     int ii, modt;
00009     float absspeed, speedsgn, ssfrac, ssfrac2;
00010     float cycletime, xpos, ypos, zpos, target[3];
00011     float legraise;
00012     float turn_dist, thtpos, rpos, maxdist;
00013     float dist, tht0, turn_step, speed_step;
00014 
00015     // clamp speed and turning, just in case
00016   //  hexlock.lock();
00017     if (speed > MAX_SPEED) speed = MAX_SPEED;
00018     if (speed < -MAX_SPEED) speed = -MAX_SPEED;
00019     if (turning > 1.0) turning = 1.0;
00020     if (turning < -1.0) turning = -1.0;
00021     if (standheight < -2.0) standheight = -2.0;
00022     if (standheight > 2.0) standheight = 2.0;
00023     
00024     // make sure speed doesnt change too rapidly
00025     speed_step = -(smoothspeed - speed);// speed - smoothspeed
00026     if (speed_step > SPEED_SLEW*dt) speed_step = SPEED_SLEW*dt;
00027     if (speed_step < -SPEED_SLEW*dt) speed_step = -SPEED_SLEW*dt;
00028     smoothspeed += speed_step;
00029     // cap the rate at which turning can change
00030     
00031     turn_step = -(smoothturning - turning);
00032     if (turn_step > TURN_SLEW*dt) turn_step = TURN_SLEW*dt;
00033     if (turn_step < -TURN_SLEW*dt) turn_step = -TURN_SLEW*dt;
00034     smoothturning += turn_step;
00035 
00036     
00037     //hexlock.unlock();
00038 
00039     // to control walking, modify speed and turning
00040     absspeed = fabs(smoothspeed);
00041     speedsgn = 1.0;
00042     if (smoothspeed < 0.0) speedsgn = -1.0;
00043 
00044     // walking speed is influenced by leg sweep and movement speed
00045     legraise = 1.0;
00046     if (absspeed < 0.05) 
00047         legraise = absspeed/0.05;
00048     if (absspeed < 0.2)
00049     {
00050         sweepmodifier = absspeed*0.8/0.2;
00051         speedmodifier = 0.25;
00052     } else if (absspeed < 0.8) {
00053         sweepmodifier = 0.8;
00054         speedmodifier = absspeed/sweepmodifier;
00055     } else if (absspeed < 1.0) {
00056         sweepmodifier = absspeed;
00057         speedmodifier = 1.0;
00058     } else {
00059         sweepmodifier = 1.0;
00060         speedmodifier = absspeed;
00061     }
00062     speedmodifier *= speedsgn;
00063 
00064     if (ssrunning)
00065     {
00066         sstime += dt;
00067         ssfrac = sstime/SS_DURATION;
00068         for (ii=0; ii<6; ii++)
00069         {
00070             // compute final target
00071             target[0] = legpos[ii][0]*1.5;
00072             if (ii == 0 || ii == 2) target[1] = 14.0;
00073             if (ii == 1) target[1] = 18.0;
00074             if (ii == 3 || ii == 5) target[1] = -14.0;
00075             if (ii == 4) target[1] = -18.0;
00076             target[0] = legpos1[ii][0];
00077             target[1] = legpos1[ii][1];
00078             target[2] = -10. + standheight;
00079             // given final target, turn into current target
00080             if (ssfrac < 0.5)
00081             {
00082                 ssfrac2 = ssfrac*2.0;
00083                 if (ii % 2 == 0)
00084                 {
00085                     target[0] = ssx0[ii] + ssfrac2*(target[0]-ssx0[ii]);
00086                     target[1] = ssy0[ii] + ssfrac2*(target[1]-ssy0[ii]);
00087                     target[2] = ssz0[ii] + ssfrac2*(target[2]-ssz0[ii]) + 2.0*pow(sin(3.1416*ssfrac2),2);
00088                 } else {
00089                     target[0] = ssx0[ii];
00090                     target[1] = ssy0[ii];
00091                     target[2] = ssz0[ii];
00092                 }
00093             } else {
00094                 ssfrac2 = (ssfrac-0.5)*2.0;
00095                 if (ii % 2 == 0)
00096                 {
00097                     // don't modify targets
00098                 } else {
00099                     target[0] = ssx0[ii] + ssfrac2*(target[0]-ssx0[ii]);
00100                     target[1] = ssy0[ii] + ssfrac2*(target[1]-ssy0[ii]);
00101                     target[2] = ssz0[ii] + ssfrac2*(target[2]-ssz0[ii]) +2.0*pow(sin(3.1416*ssfrac2),2);
00102                 }
00103             }
00104             IKSolve(ii,target);
00105         }
00106         if (sstime > SS_DURATION)
00107         {
00108             ssrunning = false;
00109             sstime = 0.0;
00110         }
00111     } else {
00112     
00113 
00114     // based on current turning, compute turning math
00115     if (fabs(smoothturning) <= TURN_TOL)turn_dist = tan((1.0-TURN_TOL)*3.1416/2.0)*50.;
00116     else if (fabs(smoothturning) > TURN_TOL)turn_dist = tan((1.0-smoothturning)*3.1416/2.0)*50.;
00117     // compute dist between turn_dist and farthest leg - blindage pour eviter out of range
00118     maxdist = 0.0;
00119     for (ii=0; ii<6; ii++)
00120     {
00121         dist = sqrt(pow(legpos1[ii][0],2) + pow(legpos1[ii][1]-turn_dist,2));
00122         if (dist > maxdist) maxdist = dist;
00123     }
00124     // each leg can only sweep so much, so use this farthest leg
00125     // to determine the angle of sweep that every leg must do
00126     maxsweep = 8.*sweepmodifier/maxdist;
00127     if (turn_dist < 0.0) maxsweep = -maxsweep;// maxsweep is the angle of sweep for every leg, in radians
00128 
00129     // increment dead-reckoning position
00130     // turning radius is "turn_dist"
00131     dr_ang += dt*speedmodifier*maxsweep*2.0*0.83775/fdf;
00132     if (dr_ang > PI) dr_ang -= 2.*PI;
00133     if (dr_ang < -PI) dr_ang += 2.*PI;
00134     dr_xpos += maxsweep*turn_dist*dt*speedmodifier*2.0*cos(dr_ang)*0.83775/fdf;
00135     dr_ypos += maxsweep*turn_dist*dt*speedmodifier*2.0*sin(dr_ang)*0.83775/fdf;
00136 
00137     // dr_ang has about 20% error, likely systematic
00138     // dr_xpos,dr_ypos have more like 5% error, also likely systematic
00139 
00140     // increment fake time - pour reset bezier sur le gait
00141     time += dt*speedmodifier;
00142     if (time > 1.0) time -= 1.0;
00143     if (time < 0.0) time += 1.0;
00144 
00145     // loop through each leg to figure out where it should be right now
00146     for (ii=0; ii<6; ii++)
00147     {
00148         // where is this leg in the cycle of stepping?
00149         // the 0.5*ii is to completely de-sync legs - pour avoir deux groupes de jambes distincs
00150         // the other part is to adjust it more
00151         cycletime = fmod(time + 0.5*ii + (legpos[ii][0]-legpos[0][0])*0.0125, 1.0);
00152         
00153         // use bezier curve to either be up or down
00154         // b2d_walk_down goes between +/- 0.83775
00155         if (cycletime < fdf) b_walk_down.get_pos(cycletime/fdf, &thtpos, &zpos);
00156         else b_walk_up.get_pos((cycletime-fdf)/(1.-fdf), &thtpos, &zpos);
00157         // convert thtpos into angle?
00158         thtpos *= maxsweep;
00159 
00160         // convert rpos to xpos,ypos
00161         dist = sqrt(pow(legpos1[ii][0],2) + pow(legpos1[ii][1]-turn_dist,2));
00162         tht0 = atan2(legpos1[ii][1]-turn_dist,legpos1[ii][0]);
00163         xpos = dist*cos(thtpos+tht0);
00164         ypos = turn_dist + dist*sin(thtpos+tht0);
00165 
00166         // set up the IK target
00167         target[0] = xpos;
00168         target[1] = ypos;
00169         target[2] = -10.0 + zpos*3.0*legraise + standheight;
00170         if (standheight < 0.0) target[2] -= 1.7*zpos*standheight;
00171 
00172 
00173         // perform IK solve
00174         IKSolve(ii,target);
00175         // TODO: add error handling if IK fails to converge
00176     }
00177 
00178     }
00179     setServoAngles();
00180 }
00181 
00182 void hexapod::safeStand ()
00183 {
00184     int ii;
00185     float fkangles[3], fkpos[3];
00186 
00187     // initialize
00188     sstime = 0.0;
00189     ssrunning = true;
00190     // store FK leg positions
00191     for (ii=0; ii<6; ii++)
00192     {
00193         fkangles[0] = angle[ii*3+0];
00194         fkangles[1] = angle[ii*3+1];
00195         fkangles[2] = angle[ii*3+2];
00196         FKSolve(ii,fkangles,fkpos);
00197         ssx0[ii] = fkpos[0];
00198         ssy0[ii] = fkpos[1];
00199         ssz0[ii] = fkpos[2];
00200     }
00201 }
00202 
00203 hexapod::hexapod(bool debugflag)
00204 {
00205     int ii;
00206 
00207     debug = debugflag;
00208 
00209     // init constants for each leg
00210     length[0] = 7.53; // in cm
00211     length[1] = 10;
00212     length[2] = 12.97;
00213     //angles de decalage causer par la forme de la structure
00214     coxaangle=0.703310743; //deja en radians
00215     femurangle = 0; 
00216     tibiaangle = 0.258856711;
00217 //angles maximums - maximums and minimums -coax femur tibia
00218     anglelb[0] = 46*DEGTORAD;
00219     angleub[0] = 134*DEGTORAD;
00220     anglelb[1] = 10*DEGTORAD; // 90-44 
00221     angleub[1] = 170*DEGTORAD;
00222     anglelb[2] = 10*DEGTORAD;
00223     angleub[2] = 160*DEGTORAD;
00224     for (ii=0; ii<3; ii++)
00225     {
00226         anglelb[ii] -= 90.*DEGTORAD; // because servos are straight at 90 degrees
00227         angleub[ii] -= 90.*DEGTORAD;
00228 
00229     }
00230 // root of leg in xyz
00231     legpos[0][0] = 4.52;
00232     legpos[0][1] = 4.958;
00233     legpos[0][2] = 2.1;
00234     legpos[1][0] = 0.0;
00235     legpos[1][1] = 6.38;
00236     legpos[1][2] = legpos[0][2];
00237     // symmetry
00238     legpos[2][0] = -legpos[0][0];
00239     legpos[2][1] = legpos[0][1];
00240     legpos[2][2] = legpos[0][2];
00241     legpos[3][0] = legpos[0][0];
00242     legpos[3][1] = -legpos[0][1];
00243     legpos[3][2] = legpos[0][2];
00244     legpos[4][0] = legpos[1][0];
00245     legpos[4][1] = -legpos[1][1];
00246     legpos[4][2] = legpos[1][2];
00247     legpos[5][0] = legpos[2][0];
00248     legpos[5][1] = legpos[3][1];
00249     legpos[5][2] = legpos[0][2];
00250 
00251     // which way do the coxa servos point
00252     legang[0] = 45.0*DEGTORAD;
00253     legang[1] = 90.0*DEGTORAD;
00254     legang[2] = 135.*DEGTORAD;
00255     legang[3] =-45.*DEGTORAD;
00256     legang[4] =-90.0*DEGTORAD;
00257     legang[5] =-135.0*DEGTORAD;
00258 
00259     // default target for each leg
00260     for (ii=0; ii<6; ii++)
00261     {
00262         legpos1[ii][0] = legpos[ii][0] + 12.0 * cos(legang[ii]);
00263         legpos1[ii][1] = legpos[ii][1] + 12.0 * sin(legang[ii]);
00264         legpos1[ii][2] = -10.0;
00265     }
00266 
00267     // initialize bezier curve gait
00268     // goes from +-1 in x and 0 to 1 in z
00269     b_walk_up.add_point(-0.83775,0);
00270     b_walk_up.add_point(-1.11701,0);
00271     b_walk_up.add_point(-1.39626,0);
00272     b_walk_up.add_point(0,3.2);
00273     b_walk_up.add_point(1.39626,0);
00274     b_walk_up.add_point(1.11701,0);
00275     b_walk_up.add_point(0.83775,0);
00276 
00277     b_walk_down.add_point(0.83775,0);
00278     b_walk_down.add_point(-0.83775,0);
00279 
00280     //hexlock.lock();
00281     speed = 0.0;
00282     turning = 0.0;
00283     standheight = 2.0;
00284    // hexlock.unlock();
00285     smoothspeed = 0.0;
00286     time = 0.0;
00287     fdf = 0.50;
00288 
00289     dr_xpos = 0.0;
00290     dr_ypos = 0.0;
00291     dr_ang = 0.0;
00292 }
00293 /*
00294 void hexapod::setAngles ()
00295 {
00296     int ii;
00297     for (ii=0; ii<6; ii++)
00298     {
00299         angle[ii*3] = (servoangle[ii*3]-150.)*DEGTORAD;
00300         angle[ii*3+1] = (servoangle[ii*3+1]-150.)*DEGTORAD;
00301         angle[ii*3+2] = (servoangle[ii*3+2]-150.)*DEGTORAD;
00302     }
00303 }
00304 */
00305 void hexapod::setServoAngles ()
00306 {
00307     int ii;
00308     for (ii=0; ii<6; ii++)
00309     {
00310         servoangle[ii*3] = angle[ii*3]*RADTODEG + 90.;
00311         servoangle[ii*3+1] = angle[ii*3+1]*RADTODEG + 90.;
00312         servoangle[ii*3+2] = angle[ii*3+2]*RADTODEG + 90.;
00313     }
00314 }
00315 
00316 // target is a float[3] giving absolute position
00317 // return whether or not the solver was successful
00318 bool hexapod::IKSolve (int leg, float *target)
00319 {
00320     int iter;
00321     bool converged;
00322     float diff;
00323     float targetr, targetz, targetang;
00324     float fkpos[3], fkangles[3], J[2][2], inv[2][2], delta[2], p[2];
00325     float posr, posz, ang1, ang2, det;
00326 
00327     //1. convert absolute position to polar around leg root
00328     targetz = target[2] - legpos[leg][2];
00329     targetr = sqrt(pow(target[0]-legpos[leg][0],2) + pow(target[1]-legpos[leg][1],2));
00330     targetang = atan2(target[1]-legpos[leg][1],target[0]-legpos[leg][0]) - legang[leg]; // atan2 [-pi:pi]
00331 
00332     //2. easy part: can the coxa servo get to the right angle?
00333     if (targetang > angleub[0] || targetang < anglelb[0]) return false;
00334     // else, go ahead and set coxa servo. One out of three angles done!
00335     angle[leg*3] = targetang;
00336 
00337     //3. begin 2-joint IK solver using jacobian pseudo-inverse
00338     
00339     // whenever we call FKSolve, need to convert to polar coords around leg root
00340     fkangles[0] = angle[leg*3]; // already solved for
00341     //3.1 starting point is influenced by actual current point
00342     // but this makes it safer in case the leg has somehow gone out of bounds
00343     fkangles[1] = angle[leg*3+1]*0.5;
00344     fkangles[2] = angle[leg*3+2]*0.5;
00345     FKSolve(leg, fkangles, fkpos);
00346     posz = fkpos[2] - legpos[leg][2];
00347     posr = sqrt(pow(fkpos[0]-legpos[leg][0],2) + pow(fkpos[1]-legpos[leg][1],2));
00348     diff = sqrt(pow(targetr-posr,2) + pow(targetz-posz,2));
00349     
00350     //3.2 ITERATE
00351     converged = false;
00352     for (iter=0; iter<MAXITER && !converged; iter++)
00353     {
00354         // compute jacobian
00355         p[0] = targetr - posr;
00356         p[1] = targetz - posz;
00357         ang1 = fkangles[1]-femurangle;
00358         ang2 = fkangles[2]-tibiaangle;
00359         J[0][0] = -length[1]*sin(ang1) - length[2]*sin(ang1+ang2); // dr/dang1
00360         J[1][0] = -length[2]*sin(ang1+ang2); // dr/dang2
00361         J[0][1] = length[1]*cos(ang1) + length[2]*cos(ang1+ang2); // dz/dang2
00362         J[1][1] = length[2]*cos(ang1+ang2); // dz/dang2
00363         // compute inverse
00364         det = 1.0/(J[0][0]*J[1][1]-J[0][1]*J[1][0]);
00365         inv[0][0] = J[1][1]*det;
00366         inv[1][0] = -J[1][0]*det;
00367         inv[0][1] = -J[0][1]*det;
00368         inv[1][1] = J[0][0]*det;
00369         delta[0] = p[0]*inv[0][0] + p[1]*inv[0][1];
00370         delta[1] = p[0]*inv[1][0] + p[1]*inv[1][1];
00371         fkangles[1] += delta[0]*0.5;
00372         fkangles[2] += delta[1]*0.5;
00373         // enforce bounds
00374         if (fkangles[1] >= angleub[1]) // maxium
00375             {fkangles[1] = angleub[1]-ANGEPS; if (debug) Ordi->log("ang1ub "+Ordi->log_itos(leg)+" " +Ordi->log_itos( angleub[1]) );}
00376         if (fkangles[1] <= anglelb[1]) // minimums
00377             {fkangles[1] = anglelb[1]+ANGEPS; if (debug) Ordi->log("ang1lb "+Ordi->log_itos(leg)+" " +Ordi->log_itos( anglelb[1]) );}
00378         if (fkangles[2] >= angleub[2]) // maxium
00379             {fkangles[2] = angleub[2]-ANGEPS; if (debug) Ordi->log("ang2ub "+Ordi->log_itos(leg)+" " +Ordi->log_itos( angleub[2]) );}
00380         if (fkangles[2] <= anglelb[2]) // minimums
00381             {fkangles[2] = anglelb[2]+ANGEPS; if (debug)Ordi->log("ang2lb "+Ordi->log_itos(leg)+" " +Ordi->log_itos( anglelb[2]) );}
00382         // FK
00383         FKSolve(leg, fkangles, fkpos);
00384         posz = fkpos[2] - legpos[leg][2];
00385         posr = sqrt(pow(fkpos[0]-legpos[leg][0],2) + pow(fkpos[1]-legpos[leg][1],2));
00386         // convergence criteria
00387         diff = sqrt(pow(targetr-posr,2) + pow(targetz-posz,2));
00388         //cout << iter << " " << diff << " " << posr << " " << posz << endl;
00389         if (diff < TOLERANCE) converged = true; // 1 mm tolerance
00390     }
00391 
00392     // converged?
00393     if (converged)
00394     {
00395         angle[leg*3+1] = fkangles[1];
00396         angle[leg*3+2] = fkangles[2];
00397         Logger::Instance()->log("IK solver reussi a trouver solution");
00398     }
00399     return converged;
00400 }
00401 
00402 // forward kinematics in absolute coordinate system
00403 // given a float[3] angles, compute position and put in float[3] pos
00404 // input angles are in radians and are offset properly
00405 void hexapod::FKSolve (int leg, float *angles, float *pos)
00406 {
00407     float r, ang0, ang1, ang2;
00408 
00409     //ajustement celon la structure
00410     ang0 = angles[0]+legang[leg];
00411     ang1 = angles[1]-femurangle;
00412     ang2 = angles[2]-tibiaangle;
00413     // conversion en coordonnes x,y,z pour savoire en fonction des angles la position du pied en fct de la root de la pate
00414     r = length[0] + length[1]*cos(ang1) + length[2]*cos(ang1+ang2);
00415     pos[0] = legpos[leg][0] + r*cos(ang0);
00416     pos[1] = legpos[leg][1] + r*sin(ang0);
00417     pos[2] = legpos[leg][2] + length[1]*sin(ang1) + length[2]*sin(ang1+ang2);
00418 }
00419 
00420 void hexapod::stand () // mettre le pied a 10cm en face de la root de l'hexapode, le corp a hauteur 10 cm du sol
00421 {
00422     int ii;
00423     float target[3];
00424     for (ii=0; ii<6; ii++)
00425     {
00426         target[0] = legpos[ii][0] + 10.0*cos(legang[ii]);
00427         target[1] = legpos[ii][1] + 10.0*sin(legang[ii]);
00428         target[2] = -10.0;
00429         IKSolve(ii,target);
00430     }
00431     setServoAngles();
00432 }
00433 
00434 void hexapod::sit ()// mettre le pied a 10cm en face de la root de l'hexapode, le corp a hauteur 5 cm du sol
00435 {
00436     int ii;
00437     float target[3];
00438     for (ii=0; ii<6; ii++)
00439     {
00440         target[0] = legpos[ii][0] + 10.0*cos(legang[ii]);
00441         target[1] = legpos[ii][1] + 10.0*sin(legang[ii]);
00442         target[2] = -5.0;
00443         IKSolve(ii,target);
00444     }
00445     setServoAngles();
00446 }