Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

XWalkMC.cc

Go to the documentation of this file.
00001 #include "Shared/RobotInfo.h"
00002 #if defined(TGT_HAS_LEGS) && !defined(TGT_IS_AIBO)
00003 
00004 #include "XWalkMC.h"
00005 #include "Events/LocomotionEvent.h"
00006 #include "Motion/IKGradientSolver.h"
00007 #include "Motion/IKThreeLink.h"
00008 #include "Motion/Kinematics.h"
00009 #include "Shared/newmat/newmatap.h"
00010 #include <cmath>
00011 
00012 using namespace std; 
00013 
00014 KinematicJoint* XWalkMC::kine=NULL;
00015 KinematicJoint* XWalkMC::childMap[NumReferenceFrames];
00016 
00017 XWalkMC::XWalkMC() : MotionCommand(), XWalkParameters(), dirty(false),
00018                      targetVel(), targetAngVel(0), targetDisp(), targetAngDisp(0),
00019                      velocityMode(true), displacementTime(0), plantingLegs(false), initialPlacement(true), p(),
00020                      transitions(), active(), startTime(get_time()), period(0), 
00021                      globPhase(0), rotationCenter(), contactMsg() {
00022   if (kine==NULL) {
00023     kine = new KinematicJoint;
00024     kine->loadFile(::config->makePath(config->motion.kinematics).c_str());
00025     kine->buildChildMap(childMap,0,NumReferenceFrames);
00026   }
00027   loadFile(::config->motion.makePath("xwalk.plist").c_str());
00028   p.loadFile(::config->motion.makePath("xwalk.plist").c_str());
00029   spiderSettings(*this,p);
00030   if(kine!=NULL && kine->getBranches().size()>0)
00031     updateNeutralPos((unsigned int)startTime);
00032   setTargetVelocity(targetVel[0],targetVel[1],targetAngVel);
00033 }
00034 
00035 void XWalkMC::setTargetVelocity(float x, float y, float a) {
00036   // std::cout << (void*)(this) << ":id=" << getID() << "   setTargetVelocity(" << x << ", " << y << ", " << a << ", " << setVelocityMode << ")"
00037   //  << "   active.size()=" << active.size() << "   startTime=" << startTime << std::endl;
00038   velocityMode = true;
00039   if(targetVel[0]==x && targetVel[1]==y && targetAngVel==a)
00040     return;
00041   
00042   unsigned int sysTime = get_time();
00043   float time = (sysTime-startTime)/1000.f;
00044   computePhase(time);
00045   
00046   float speed=targetVel.norm(), origSpeed=speed;
00047   bool inAir[NumLegs];
00048   float phases[NumLegs];
00049   fmat::Column<3> curPos[NumLegs];
00050   for(unsigned int leg=0; leg<NumLegs; ++leg) {
00051     computeLegPhase(leg,inAir[leg],phases[leg]);
00052     computeCurrentPosition(leg, inAir[leg], speed, phases[leg], curPos[leg]);
00053   }
00054   fmat::Column<2> origOff;
00055   float origOffA=0;
00056   if(adaptiveLegOrder)
00057     computeCurrentBodyOffset(phases, speed, origOff[0], origOff[1], origOffA);
00058   
00059   /*std::cout << "Original phases:";
00060    for(unsigned int leg=0; leg<NumLegs; ++leg)
00061    std::cout << " " << phases[leg];
00062    std::cout << std::endl;*/
00063   
00064   unsigned int origOrder[NumLegs];
00065   for(unsigned int leg=0; leg<NumLegs; ++leg)
00066     origOrder[leg]=legStates[leg].reorder;
00067   
00068   fmat::Column<3> origVel=fmat::pack(targetVel,targetAngVel);
00069   targetVel[0]=x;
00070   targetVel[1]=y;
00071   targetAngVel=a;
00072   fmat::Column<3> newVel=fmat::pack(targetVel,targetAngVel);
00073   LocomotionEvent e(EventBase::locomotionEGID, getID(), EventBase::statusETID);
00074   e.setXYA(x, y, a);
00075   postEvent(e);
00076   
00077   speed=targetVel.norm();
00078   float dirAlignment = fmat::dotProduct(origVel,newVel)/origSpeed/speed;
00079   
00080   bool standStill = resetPeriod(time,speed);
00081   
00082   if(standStill) {
00083     // no-op
00084   } else if(!adaptiveLegOrder) {
00085     for(unsigned int leg=0; leg<NumLegs; ++leg)
00086       legStates[leg].reorder = leg;
00087   } else if(std::abs(dirAlignment-1) > EPSILON) {
00088     // rotate leg ordering by direction of motion so gait is consistent relative to direction
00089     // e.g. ripple gait always moves from back to front, *in direction of motion*
00090     
00091     // find nominal "forward" leg ordering
00092     std::pair<float,unsigned int> fAngle[NumLegs];
00093     for(unsigned int leg=0; leg<NumLegs; ++leg) {
00094       // hack ... fix it later
00095             #ifdef TGT_IS_MANTIS
00096             static const int JointsPerLeg = 4;
00097             #endif
00098             fmat::Column<3> ep = childMap[LegOffset+leg*JointsPerLeg]->getT(*kine).column(3);
00099       fAngle[leg] = std::pair<float,unsigned int>(std::atan2(ep[1],ep[0]),leg);
00100     }
00101     std::stable_sort(fAngle,fAngle+NumLegs);
00102     
00103     // target heading
00104     float dir = 0;
00105     if (speed>EPSILON)
00106       dir += std::atan2(y,x);
00107     if (std::abs(targetAngVel) > speed*EPSILON) {
00108       fmat::Column<2> v=rotationCenter-targetVel;
00109       dir += std::atan2(v[1],v[0]) - std::atan2(rotationCenter[1],rotationCenter[0]);
00110     }
00111     
00112     // rotate legs by target heading
00113     std::pair<float,unsigned int> tAngle[NumLegs];
00114     for(unsigned int leg=0; leg<NumLegs; ++leg) {
00115       tAngle[leg] = std::pair<float,unsigned int>(fAngle[leg].first-dir,fAngle[leg].second);
00116       if(tAngle[leg].first<(float)M_PI)
00117         tAngle[leg].first+=2*(float)M_PI;
00118       if(tAngle[leg].first>(float)M_PI)
00119         tAngle[leg].first-=2*(float)M_PI;
00120     }
00121     std::stable_sort(tAngle,tAngle+NumLegs);
00122     
00123     // pull out reordered mapping
00124     for(unsigned int leg=0; leg<NumLegs; ++leg)
00125       legStates[fAngle[leg].second].reorder = tAngle[leg].second;
00126   }
00127   
00128   if(initialPlacement)
00129     return;
00130   
00131   bool legsReordered=false;
00132   for(unsigned int leg=0; leg<NumLegs; ++leg) {
00133     if(origOrder[leg]!=legStates[leg].reorder) {
00134       legsReordered=true;
00135       break;
00136     }
00137   }
00138   
00139   if(legsReordered) {
00140     unsigned int bestLeg=-1U;
00141     //float bestLegPhase=0;
00142     // fix phase stuff, want to align global phase so the same leg(s) are in the air
00143     for(unsigned int leg=0; leg<NumLegs; ++leg) {
00144       if(inAir[leg]) {
00145         // found leg in air, use it
00146         bestLeg=leg;
00147         break;
00148       } else {
00149         // also keep track of next flight (max phase)
00150         // if we don't find a leg in the air, we'll use this instead
00151         /*if(phases[leg]>bestLegPhase) {
00152          bestLegPhase=phases[leg];
00153          bestLeg=leg;
00154          }*/
00155       }
00156     }
00157     if(bestLeg==-1U) {
00158       // keep global phase match, might not have crossed gait change threshold anyway...
00159     } else {
00160       float legphase;
00161       bool inAirTmp;
00162       computeLegPhase(bestLeg,inAirTmp,legphase);
00163       
00164       float tgtPhase = phases[bestLeg];
00165       //cout << origVel << " dot " << fmat::pack(targetVel,targetAngVel) << " = " << fmat::dotProduct(origVel,fmat::pack(targetVel,targetAngVel)) << endl;
00166       if(dirAlignment<0)
00167         tgtPhase = 1-tgtPhase; // reverse direction, reverse phase
00168       
00169       float shift;
00170       if(inAirTmp)
00171         shift = (legphase-tgtPhase)*p.legParams[bestLeg].flightDuration;
00172       else {
00173         float strideTime = period - p.legParams[bestLeg].flightDuration/1000.f;
00174         shift = (1-tgtPhase)*p.legParams[bestLeg].flightDuration - (p.legParams[bestLeg].flightDuration+legphase*strideTime*1000.f);
00175       }
00176       startTime-=shift; // convert and round to nearest millisecond
00177       time = (sysTime-startTime)/1000.f;
00178       computePhase(time);
00179     }
00180     
00181     bool inAirTmp; // ignored, don't want to overwrite original inAir entries
00182     //std::cout << "Reordered phases on "<< bestLeg << ": " << phases[bestLeg] << endl;
00183     for(unsigned int leg=0; leg<NumLegs; ++leg)
00184       computeLegPhase(leg,inAirTmp,phases[leg]);
00185     //std::cout << "Now: " << phases[bestLeg] << endl;
00186     /*for(unsigned int leg=0; leg<NumLegs; ++leg)
00187      std::cout << " " << phases[leg];
00188      std::cout << std::endl;*/
00189     
00190   }
00191   
00192   for(unsigned int leg=0; leg<NumLegs; ++leg)
00193     resetLegState(leg,phases[leg],curPos[leg],inAir[leg],speed);
00194   
00195   // if we shifted global phase to keep a leg in the air, need to fix the global offset
00196   if(legsReordered) {
00197     fmat::Column<2> off;
00198     float offA=0;
00199     computeCurrentBodyOffset(phases, speed, off[0], off[1], offA);
00200     off-=origOff;
00201     for(unsigned int leg=0; leg<NumLegs; ++leg)
00202       if(!inAir[leg])
00203         legStates[leg].downPos-=off;
00204     // todo: if we add a 'twist' parameter to do periodic offA adjustments, will need to something for that here
00205   }
00206 }
00207 
00208 void XWalkMC::setTargetVelocity(float xvel, float yvel, float avel, float time) {
00209   plantingLegs = false;
00210   targetDisp = fmat::pack(xvel*time,yvel*time);
00211   targetAngDisp = avel*time;
00212   displacementTime = get_time();
00213   if ( xvel == 0 && yvel == 0 && avel == 0 ) {
00214     setTargetVelocity(0, 0, 0);
00215   } else {
00216     setTargetVelocity(xvel,yvel,avel);
00217     velocityMode = false;
00218   }
00219 }
00220 
00221 void XWalkMC::setTargetDisplacement(float xdisp, float ydisp, float adisp, float time) {
00222   // std::cout << (void*)(this) << ":id=" << getID() <<"   setTargetDisplacement(" << xdisp << ", " << ydisp << ", " << adisp << ")" << std::endl;
00223   if ( xdisp == 0 && ydisp == 0 && adisp == 0 ) {
00224     velocityMode = false;
00225     plantingLegs = false;
00226     targetDisp = fmat::pack(xdisp,ydisp);
00227     targetAngDisp = adisp;
00228     displacementTime = get_time();
00229     setTargetVelocity(0, 0, 0);
00230     return;
00231   }
00232   const float tx = std::abs(xdisp/getMaxXVel());
00233   const float ty = std::abs(ydisp/getMaxYVel());
00234   const float ta = std::abs(adisp/getMaxAVel());
00235   const float maxTime =  std::max(time,std::max(tx,std::max(ty,ta)));
00236   setTargetVelocity(xdisp/maxTime, ydisp/maxTime, adisp/maxTime, maxTime);
00237   velocityMode = false;
00238 }
00239 
00240 int XWalkMC::updateOutputs() {
00241   unsigned int curTime=get_time();
00242   unsigned int dispTime = curTime-displacementTime;
00243   float dispSecs = dispTime / 1000.f;
00244   
00245   if(!isDirty()) {
00246     if ( plantingLegs ) {
00247       postEvent(EventBase(EventBase::motmanEGID, getID(), EventBase::statusETID, dispTime));
00248       plantingLegs = false;
00249     }
00250     return 0;
00251   }
00252   
00253   if ( ! velocityMode )  { // we're trying to travel a specific displacement
00254     if ( fabs(dispSecs*targetVel[0]) >= fabs(targetDisp[0]) &&
00255         fabs(dispSecs*targetVel[1]) >= fabs(targetDisp[1]) &&
00256         fabs(dispSecs*targetAngVel) >= fabs(targetAngDisp) ) {
00257       plantingLegs = true;
00258       setTargetVelocity(0, 0, 0);
00259       //return JointsPerLeg*NumLegs;
00260     }
00261   }
00262   
00263   // for z position, need to drop the x,y point along gravity vector to the ground plane
00264   fmat::Column<3> ground, gravity;
00265   packGroundGravity(ground, gravity);
00266   
00267   float speed = targetVel.norm();
00268   float dt = (curTime-startTime)/1000.f;
00269   
00270   kine->pullChildrenQFromArray(state->outputs, 0, NumOutputs);
00271   
00272   if(active.size()>0) {
00273     updateNeutralPos(curTime);
00274     // could probably be more selective about when we do the rest of this...
00275     /*computePhase(dt);
00276      resetPeriod(dt,speed);
00277      dt = (curTime-startTime)/1000.f;
00278      for(unsigned int leg=0; leg<NumLegs; ++leg) {
00279      float phase;
00280      computeLegPhase(leg,legStates[leg].inAir,phase);
00281      //fmat::Column<3> curPos = childMap[FootFrameOffset+leg]->getWorldPosition();
00282      //resetLegState(leg,phase,curPos,legStates[leg].inAir,speed);
00283      }*/
00284   }
00285   
00286   IKSolver::Point tgts[NumLegs];
00287   
00288   if(initialPlacement)
00289     updateOutputsInitial(curTime,ground,gravity,tgts);
00290   else
00291     updateOutputsWalking(dt,ground,gravity,speed,tgts);
00292   
00293   sendLoadPredictions(tgts);
00294   
00295   /*if(targetVelX==0 && targetVelY==0 && targetVelA==0) {
00296    if(!resetOnStop)
00297    return 0;
00298    }*/
00299     #ifdef TGT_IS_MANTIS
00300     static const int JointsPerLeg = 4; // hack! fix it later
00301     #endif
00302   return JointsPerLeg*NumLegs;
00303 }
00304 
00305 void XWalkMC::start() {
00306   MotionCommand::start();
00307   kine->pullChildrenQFromArray(state->outputs, 0, NumOutputs);
00308   #ifdef TGT_IS_BIPED
00309   initialPlacement=false;
00310   #else
00311   initialPlacement=true;
00312   #endif
00313   startTime=get_time();
00314   if(period == 0)
00315     for(unsigned int leg=0; leg<NumLegs; ++leg)
00316       period += legParams[leg].totalDuration() * 2 / 1000.f;
00317   computePhase(0);
00318   for(unsigned int leg=0; leg<NumLegs; ++leg) {
00319     fmat::Column<3> curPos = childMap[FootFrameOffset+leg]->getWorldPosition();
00320     legStates[leg].liftPos = fmat::SubVector<2>(curPos);
00321     legStates[leg].inAir=true;
00322     legStates[leg].support=0;
00323     legStates[leg].initialHeight = curPos[2];
00324     resetLegState(leg,0,curPos,true,0);
00325   }
00326 }
00327 
00328 void XWalkMC::stop() {
00329   DriverMessaging::LoadPrediction loads;
00330   for(unsigned int i=0; i<NumLegs; ++i) {
00331     KinematicJoint * kj = childMap[FootFrameOffset+i];
00332     while(kj!=NULL) {
00333       if(kj->outputOffset<NumOutputs)
00334         loads.loads[kj->outputOffset.get()] = 0;
00335       kj=kj->getParent();
00336     }
00337   }
00338   DriverMessaging::postMessage(loads);
00339   
00340   // clear contact points too
00341   if(contactMsg.size()>0) {
00342     contactMsg.clear();
00343     contactMsg.flushOnMotionUpdate=false; // send immediately (if no other motion, might not send)
00344     DriverMessaging::postMessage(contactMsg);
00345   }
00346   
00347   MotionCommand::stop();
00348 }
00349 
00350 void XWalkMC::zeroVelocities() {
00351   unsigned int t=get_time();
00352   setTargetVelocity(0,0,0);
00353   LocomotionEvent e(EventBase::locomotionEGID, getID(), EventBase::deactivateETID, t-displacementTime); // velocities default to 0
00354   postEvent(e);
00355   displacementTime = t;
00356 }
00357 
00358 
00359 #if 0
00360 void XWalkMC::lockLegsInPlace() {
00361   setTargetVelocity(0,0,0);
00362   
00363   float minHeight=std::numeric_limits<float>::max();
00364   for(unsigned int leg=0; leg<NumLegs; ++leg) {
00365     bool inAir;
00366     float phases;
00367     computeLegPhase(leg,inAir,phases);
00368     fmat::Column<3> curPos;
00369     computeCurrentPosition(leg, inAir, 0, phases, curPos);
00370     
00371     KinematicJoint * kj = childMap[FootFrameOffset+leg];
00372     float legr=0; // this will be the maximum distance from elevator to foot when leg is stretched out
00373     while(kj!=NULL && kj->outputOffset!=LegOffset+leg*JointsPerLeg) {
00374       legr += kj->r;
00375       kj = kj->getParent();
00376     }
00377     fmat::Transform baseToElv = childMap[LegOffset+leg*JointsPerLeg+1]->getFullInvT();
00378     fmat::Column<2> curPosElv(baseToElv*curPos);
00379     float curr = curPosElv.norm(); // this is the current distance from elevator to foot
00380     
00381     //float ang = baseToElv;
00382     
00383     float tgtheight = 0;
00384   }
00385 }
00386 #endif
00387 
00388 void XWalkMC::updateNeutralPos(unsigned int curTime) {
00389 //  return;   ***** DEBUG HACK *****
00390   std::set<ParameterTransition*> deactivated;
00391   for(std::set<ParameterTransition*>::const_iterator it=active.begin(); it!=active.end(); ++it)
00392     if(!(*it)->update(curTime))
00393       deactivated.insert(*it);
00394   for(std::set<ParameterTransition*>::const_iterator it=deactivated.begin(); it!=deactivated.end(); ++it)
00395     active.erase(*it);
00396   
00397   // each mid-stride position is base primarily on the location of the first leg joint
00398   // the strideBias then offsets the x position, and the stance width offsets the y position
00399   for(unsigned int leg=0; leg<NumLegs; ++leg) {
00400     LegParameters& legParam = p.legParams[leg];
00401         #ifdef TGT_IS_MANTIS
00402         static const int JointsPerLeg = 4; // hack! fix it later
00403         #endif
00404         KinematicJoint * legRoot = childMap[LegOffset+JointsPerLeg*leg];
00405     fmat::Column<3> fstPos = legRoot->getWorldPosition();
00406     legStates[leg].neutralPos[0] = fstPos[0] + legParam.strideBias;
00407     legStates[leg].neutralPos[1] = ((fstPos[1]<0) ? -*legParam.stanceWidth : *legParam.stanceWidth);
00408   }
00409     return;
00410 }
00411 
00412 bool XWalkMC::resetPeriod(float time, float speed) {
00413   bool standStill=false;
00414   
00415   if (std::abs(targetAngVel) <= speed*EPSILON) {
00416     // straight line motion
00417     if(speed>EPSILON) {
00418       // Since all legs on the ground move the same speed, spending more time on ground means taking longer stride
00419       // Since all legs share the same period, period - airtime = groundtime, so minimum air time is maximum ground time
00420       // Thus the minimum air time determines the period which produces the correct speed at the maximal stride
00421       float minAirTime = std::numeric_limits<float>::infinity();
00422       for(unsigned int leg=0; leg<NumLegs; ++leg) {
00423         float t = legParams[leg].totalDuration();
00424         //float t = legParams[leg].flightDuration; // *TRANSITION_STRIDE* //
00425         if(t<minAirTime)
00426           minAirTime=t;
00427       }
00428       minAirTime/=1000.f; // convert to seconds
00429       
00430       // Maximum stride length is determined by direction of motion (max stride x vs. y)
00431       // find intersection of targetVel with max-stride-elipse
00432       fmat::Column<2> d = fmat::pack(strideLenX*targetVel[1], strideLenY*targetVel[0]); // conveniently, don't need to normalize
00433       float groundTime = strideLenX*strideLenY / std::sqrt(d[0]*d[0] + d[1]*d[1]); // in seconds
00434       period = groundTime + minAirTime;
00435       //std::cout << "new period " << period << " " << groundTime << ' ' << targetVel << " " << speed << std::endl;
00436     } else {
00437       period = nominalPeriod();
00438       standStill=true;
00439     }
00440     
00441   } else { // arcing motion
00442     // turning, need to find center of rotation, handle legs correctly (inner legs don't move as fast...)
00443     rotationCenter[0] = -targetVel[1];
00444     rotationCenter[1] = targetVel[0];
00445     rotationCenter/=targetAngVel;
00446     
00447     // pick the shortest period so other legs will not exceed maximum stride length
00448     // This isn't quite right, is finding groundTime based on straight line motion
00449     // instead of arcing motion, yet arcing motion is what's used later.
00450     period = std::numeric_limits<float>::infinity();
00451     for(unsigned int leg=0; leg<NumLegs; ++leg) {
00452       fmat::Column<2> radial = legStates[leg].neutralPos - rotationCenter;
00453       if(radial.norm() < EPSILON) // neutral point is *on* rotation center, ignore it
00454         continue;
00455       // this does some fancy calculation to get ground time
00456       // basically an optimization of (x/a)^2 + (y/b)^2 = 1/t 
00457       fmat::Column<2> ivel = fmat::pack(-radial[1],radial[0])*targetAngVel; // instantaneous velocity
00458       fmat::Column<2> d = fmat::pack(strideLenX*ivel[1], strideLenY*ivel[0]); // conveniently, don't need to normalize
00459       float groundTime = strideLenX*strideLenY / d.norm(); // in seconds
00460       float airTime = legParams[leg].totalDuration() / 1000.f;
00461       //float airTime = legParams[leg].flightDuration / 1000.f; // *TRANSITION_STRIDE* //
00462       if(groundTime + airTime < period)
00463         period = groundTime + airTime;
00464       //std::cout << "testing leg " << leg << " " << period << " " << groundTime << ' ' << ivel << std::endl;
00465     }
00466     //std::cout << "new period " << period << std::endl;
00467   }
00468   
00469   if(!initialPlacement) {
00470     float origPhase=globPhase;
00471     computePhase(time);
00472     float shift = (origPhase-globPhase)*period;
00473     startTime-=shift*1000.f; // convert and round to nearest millisecond
00474     globPhase = origPhase;
00475     // to verify:
00476     //time = (sysTime-startTime)/1000.f;
00477     //computePhase(time);
00478     //cout << globPhase << " vs " << origPhase << endl;
00479   }
00480   
00481   return standStill;
00482 }
00483 
00484 void XWalkMC::resetLegState(unsigned int leg, float phase, const fmat::Column<3>& curPos, bool inAir, float speed) {
00485   fmat::Column<3> tgt;
00486   if(inAir) {
00487     legStates[leg].downPos = legStates[leg].neutralPos;
00488     // now take a half reverse-step to find optimal new downPos
00489     computeCurrentPosition(leg, false, speed, -0.5f, tgt);
00490     legStates[leg].downPos = fmat::SubVector<2>(tgt);
00491     // adjust liftPos to ensure smooth motion to new target
00492     /* Ideally, solve the inAir leg position equation used by computeCurrentPosition for lift, holding current position:
00493      (lift-down) * phase + down = cur
00494      However, as we get close to down, leg reorders can cause large, instantaneous movements (asking for numerical stability)
00495      Instead, if less than half of flight remains, rotate remaining distance in direction of downPos */
00496     fmat::Column<2> cur(curPos); // ugly, need ConstSubVector...
00497     if(phase>.8) {
00498       // close to lift, solve in terms of down:  (cur - down) / phase + down
00499       legStates[leg].liftPos = (cur-legStates[leg].downPos)/phase + legStates[leg].downPos;
00500     } else {
00501       // close to down, solve in terms of lift: |cur - lift| / (1-phase) * phase * normalize(down - cur)
00502       fmat::Column<2> downDir = legStates[leg].downPos - cur;
00503       downDir/=downDir.norm();
00504       (cur-legStates[leg].liftPos).norm()/(1-phase)*phase * downDir;
00505     }
00506   } else {
00507     legStates[leg].downPos = fmat::Column<2>(curPos);
00508     // now take reverse-step of the current phase to find new downPos
00509     computeCurrentPosition(leg, false, speed, -phase, tgt);
00510     legStates[leg].downPos = fmat::SubVector<2>(tgt);
00511   }
00512 }
00513 
00514 
00515 void XWalkMC::updateOutputsInitial(unsigned int curTime, const fmat::Column<3>& ground, const fmat::Column<3>& gravity, IKSolver::Point tgts[]) {
00516   initialPlacement = false;
00517   bool contactMsgDirty=false; // set to true if there's actually a *new* contact in contactMsg
00518   
00519   float phases[NumLegs];
00520   bool inAirFixed = true;
00521   for(unsigned int leg=0; leg<NumLegs; ++leg) {
00522     computeLegPhase(leg,inAirFixed,phases[leg]);
00523   }
00524   
00525   fmat::Column<2> off;
00526   float offa=0;
00527   computeCurrentBodyOffset(phases, 0, off[0], off[1], offa);
00528   fmat::Matrix<2,2> offaM = fmat::rotation2D(offa);
00529   
00530   for(unsigned int leg=0; leg<NumLegs; ++leg) {
00531     legStates[leg].support=0;
00532     tgts[leg][0]=legStates[leg].neutralPos[0];
00533     tgts[leg][1]=legStates[leg].neutralPos[1];
00534     tgts[leg][2]=p.legParams[leg].flightHeight;
00535     (fmat::SubVector<2>)tgts[leg] = offaM * fmat::SubVector<2>(tgts[leg]) + off;
00536     
00537     float raiseDur = p.legParams[leg].raiseDuration*2;
00538     float lowerDur = p.legParams[leg].lowerDuration*2;
00539     float flightDur = p.legParams[leg].flightDuration*2;
00540     
00541     // move linearly to flight height above belly
00542     // note assumption that z=0 is belly of robot...
00543     fmat::Column<3> curP(legStates[leg].liftPos);
00544     curP[2] = legStates[leg].initialHeight;
00545     float phase;
00546     if(curP[2]>tgts[leg][2]) {
00547       // type A: already above ground, go straight to target
00548       phase = (curTime-startTime)/(raiseDur+flightDur);
00549       if(phase<1) {
00550         tgts[leg] *= phase;
00551         tgts[leg] += curP*(1-phase);
00552         initialPlacement = true;
00553       }
00554     } else {
00555       // type B: raise straight up first to lower onto belly, then move to target
00556       phase = (curTime-startTime)/raiseDur;
00557       if(phase<1) {
00558         // moving up
00559         tgts[leg][0] = curP[0];
00560         tgts[leg][1] = curP[1];
00561         tgts[leg][2] *= phase;
00562         tgts[leg][2] += curP[2]*(1-phase);
00563         initialPlacement = true;
00564       } else {
00565         // moving to target, assuming we're already at height
00566         phase = (curTime-startTime-raiseDur)/flightDur;
00567         if(phase<1) {
00568           curP[2] = tgts[leg][2];
00569           tgts[leg] *= phase;
00570           tgts[leg] += curP*(1-phase);
00571           initialPlacement = true;
00572         }
00573       }
00574     }
00575     if(phase>=1) {
00576       // above target, now lower back down to ground, then increase support
00577       phase = (curTime-startTime-raiseDur-flightDur)/(lowerDur);
00578       
00579       if(p.groundPlane[3] > 0) {
00580         phase += 1;
00581         //fmat::Column<3> physPos = childMap[FootFrameOffset+leg]->getWorldPosition();
00582         fmat::Column<3>& physPos = tgts[leg]; // use the target position instead of sensing, trade off robustness to interference vs. precision
00583         legStates[leg].liftPos = fmat::SubVector<2>(physPos);
00584         legStates[leg].initialHeight = physPos[2];
00585       }
00586       
00587       if(phase<1) {
00588         // lower to ground plane
00589         curP=tgts[leg];
00590         tgts[leg][2]=0;
00591         tgts[leg] *= phase;
00592         tgts[leg] += curP*(1-phase);
00593         // compute and store actual position in case we hit joint limits or obstacle
00594         //fmat::Column<3> physPos = childMap[FootFrameOffset+leg]->getWorldPosition();
00595         fmat::Column<3>& physPos = tgts[leg]; // use the target position instead of sensing, trade off robustness to interference vs. precision
00596         legStates[leg].liftPos = fmat::SubVector<2>(physPos);
00597         initialPlacement = true;
00598       } else {
00599         phase-=1;
00600         // lower to support height, lifting body (now support activated)
00601         if(phase<1) {
00602           if(legParams[leg].usable) {
00603             if(legStates[leg].inAir && !contactMsgDirty) {
00604               contactMsg.clear();
00605               contactMsgDirty=true;
00606             }
00607             contactMsg.addEntry(FootFrameOffset+leg, fmat::Column<3>()); //need to add all contacts, not just new ones
00608           }
00609 
00610           legStates[leg].inAir=false;
00611           legStates[leg].support=.5f; // not 1 so isDirty will still return true, not 0 so pressures/loads will be computed
00612           if(p.groundPlane[3] <= 0)
00613             curP[2]=0;
00614           tgts[leg]=curP;
00615           projectToGround(ground, p.groundPlane[3], gravity, tgts[leg]);
00616           tgts[leg] *= phase;
00617           tgts[leg] += curP*(1-phase);
00618           initialPlacement = true;
00619         }
00620       }
00621     }
00622   }
00623   if(contactMsgDirty) {
00624     contactMsg.flushOnMotionUpdate=true; // delay message to sync with corresponding joint angles
00625     DriverMessaging::postMessage(contactMsg);
00626   }
00627   if(initialPlacement) {
00628     for(unsigned int leg=0; leg<NumLegs; ++leg)
00629       solveIK(leg,tgts[leg]);
00630   } else {
00631     // sync time to end of leg 0 flight
00632     unsigned int timingleg = legStates[0].reorder==-1U ? 0 : legStates[0].reorder;
00633     float endflightphase = p.legParams[timingleg].flightPhase + p.legParams[0].flightDuration/1000.f/period;
00634     float shift = (endflightphase-globPhase)*period;
00635     startTime-=shift*1000.f; // convert and round to nearest millisecond
00636     computePhase((curTime-startTime)/1000.f);
00637     kine->pullChildrenQFromArray(state->outputs, 0, NumOutputs);
00638     for(unsigned int leg=0; leg<NumLegs; ++leg) {
00639       bool inAir; // ignored, all feet are on ground
00640       float phase;
00641       computeLegPhase(leg,inAir,phase);
00642       // todo: need to project back to xy plane along gravity vector
00643       fmat::Column<3> physPos = childMap[FootFrameOffset+leg]->getWorldPosition();
00644     //  offaM * fmat::SubVector<2>(tgts[leg]) + off;
00645       (fmat::SubVector<2>)physPos = offaM.transpose()*fmat::SubVector<2>(physPos) - offaM.transpose()*off;
00646       resetLegState(leg,phase,physPos,false,targetVel.norm());
00647       legStates[leg].support=1;
00648       legStates[leg].onGround=true;
00649     }
00650     LocomotionEvent e(EventBase::locomotionEGID,getID(),EventBase::activateETID,0);
00651     e.setXYA(targetVel[0], targetVel[1], targetAngVel);
00652     postEvent(e);
00653   }
00654   displacementTime=get_time();
00655 }
00656 
00657 void XWalkMC::updateOutputsWalking(float dt, const fmat::Column<3>& ground, const fmat::Column<3>& gravity, float speed, IKSolver::Point tgts[]) {
00658   /*TimeET walkTime;
00659    static float walkAvg=0;
00660    static int walkcnt=0;
00661    static float ikAvg=0;
00662    static int ikcnt=0;*/
00663   
00664   bool inAir[NumLegs];
00665   float phases[NumLegs];
00666   
00667   // compute xy leg positions
00668   
00669   computePhase(dt);
00670   
00671   for(unsigned int leg=0; leg<NumLegs; ++leg) {
00672     computeLegPhase(leg,inAir[leg],phases[leg]);
00673     //cout << leg << " inAir " << inAir[leg] << " phase " << phases[leg] << endl;
00674     if(legStates[leg].inAir!=inAir[leg]) {
00675       // this is a bit of a hack, walk sometimes lifts legs during "pure" parameter transition
00676       // so only initiate a leg lift if we are actually moving.  Always pass touch-downs through.
00677       if(speed>0 || std::abs(targetAngVel)>EPSILON || !inAir[leg]) {
00678         legStates[leg].inAir=inAir[leg];
00679       } else { // hold the leg on the ground
00680         inAir[leg] = false;
00681       }
00682       if(inAir[leg]) {
00683         legStates[leg].downPos = legStates[leg].neutralPos;
00684         computeCurrentPosition(leg, false, speed, -0.5f, tgts[leg]);
00685         legStates[leg].downPos = fmat::SubVector<2>(tgts[leg]);
00686       }
00687     }
00688   }
00689   
00690   fmat::Column<2> off;
00691   float offa=0;
00692   computeCurrentBodyOffset(phases, speed, off[0], off[1], offa);
00693   fmat::Matrix<2,2> offaM = fmat::rotation2D(offa);
00694   
00695   for(unsigned int leg=0; leg<NumLegs; ++leg) {
00696     // get some statistics on leg position
00697     /*if(legStates[leg].support==1) {
00698      float dist = p.groundPlane[3] - fmat::dotProduct(childMap[FootFrameOffset+leg]->getWorldPosition(),ground);
00699      float align = fmat::dotProduct(gravity,ground);
00700      if(align<EPSILON) // we're trying to walk up a perfectly vertical cliff...
00701      align=EPSILON; //pretend it's near vertical
00702      cout << leg << ' ' << phases[leg] << ' ' << (dist/align) << endl;
00703      }*/
00704     
00705     computeCurrentPosition(leg, inAir[leg], speed, phases[leg], tgts[leg]);
00706     if(!inAir[leg])
00707       legStates[leg].liftPos = fmat::SubVector<2>(tgts[leg]);
00708     (fmat::SubVector<2>)tgts[leg] = offaM * fmat::SubVector<2>(tgts[leg]) + off;
00709   }
00710   
00711   // do the "extra", non-leg joints before leg IK in case we're changing earlier joints (e.g. rotor joints on Chiara)
00712   for(plist::DictionaryOf<SinusoidalParameters>::const_iterator it=nonLegJoints.begin(); it!=nonLegJoints.end(); ++it) {
00713     unsigned int idx = capabilities.findOutputOffset(it->first.c_str());
00714     if(idx!=-1U) {
00715       float val = (*it->second)(globPhase,phases);
00716       motman->setOutput(this,idx,val);
00717       if(KinematicJoint * kj = childMap[idx])
00718         kj->freezeQ(val);
00719     }
00720   }
00721   
00722   bool contactChanged=false; // in the following loop, will set this flag if a foot has transitioned to/from full support (LegState::onGround toggled)
00723 
00724   // for each leg, project foot position to ground plane along gravity vector,
00725   // solve inverse kinematics, and send joint angles to motion manager
00726   for(unsigned int leg=0; leg<NumLegs; ++leg) {
00727     projectToGround(ground, p.groundPlane[3], gravity, tgts[leg]);
00728     
00729     if(inAir[leg]) {
00730       tgts[leg] += ground*(p.legParams[leg].flightHeight + bounce(globPhase,phases));
00731       legStates[leg].support=0;
00732     } else {
00733       float strideTime = period - p.legParams[leg].flightDuration/1000.f;
00734       float height = p.legParams[leg].flightHeight;
00735       float t = (1-phases[leg])*strideTime;
00736       float lowerDur = p.legParams[leg].lowerDuration/1000.f;
00737       float raiseDur = p.legParams[leg].raiseDuration/1000.f;
00738       if(t>strideTime - lowerDur && legStates[leg].support<1) { // don't jump into lowering if not previously raised
00739         float acc = 4 * p.legParams[leg].flightHeight / lowerDur / lowerDur;
00740         t = strideTime - t;
00741         if(t<lowerDur/2) {
00742           height -= acc * t * t / 2;
00743         } else {
00744           t = lowerDur - t;
00745           height = acc * t * t / 2;
00746         }
00747         tgts[leg] += ground*height;
00748         legStates[leg].support=0;//1-height/p.legParams[leg].flightHeight;
00749       } else if(t<raiseDur && (speed>EPSILON || std::abs(targetAngVel)>EPSILON)) { // don't initiate a raise if not walking
00750         float acc = 4 * p.legParams[leg].flightHeight / raiseDur / raiseDur;
00751         if(t<raiseDur/2) {
00752           height -= acc * t * t / 2;
00753         } else {
00754           t = raiseDur - t;
00755           height = acc * t * t / 2;
00756         }
00757         tgts[leg] += ground*height;
00758         legStates[leg].support=0;//1-height/p.legParams[leg].flightHeight;
00759       } else {
00760         legStates[leg].support=1;
00761       }
00762       tgts[leg] += ground*bounce(globPhase,phases);
00763       
00764       if( (legStates[leg].support>=1) != legStates[leg].onGround) {
00765         contactChanged=true;
00766         /*static unsigned int times[NumLegs];
00767         static fmat::Column<3> downs[NumLegs];
00768         if(legStates[leg].support>=1) {
00769           times[leg]=get_time();
00770           downs[leg]=tgts[leg];
00771           float stride = (legStates[leg].neutralPos - legStates[leg].downPos).norm();
00772           float airTime = (legParams[leg].flightDuration) / 1000.f;
00773           //float airTime = (legParams[leg].raiseDuration + legParams[leg].flightDuration + legParams[leg].lowerDuration) / 1000.f; // *TRANSITION_STRIDE* //
00774           std::cout << "leg " << leg << " non-air stride " << stride*2 << " speed " << stride*2/(period-airTime) << std::endl;
00775         } else {
00776           float stride = (downs[leg] - tgts[leg]).norm();
00777           std::cout << "leg " << leg << " ground stride " << stride << " speed " << stride/(get_time()-times[leg])*1000 << std::endl;
00778         }*/
00779       }
00780       legStates[leg].onGround = (legStates[leg].support>=1);
00781     }
00782     
00783     // inverse kinematics to get leg to target, and send values to motion manager
00784     //TimeET ikTime;
00785     solveIK(leg,tgts[leg]);
00786     //ikAvg+=ikTime.Age().Value();
00787     //++ikcnt;
00788   }
00789   
00790   if(contactChanged) {
00791     // send driver message reporting leg contacts
00792     if(DriverMessaging::hasListener(DriverMessaging::FixedPoints::NAME)) {
00793       contactMsg.clear();
00794       for(unsigned int leg=0; leg<NumLegs; ++leg) {
00795         if(legStates[leg].onGround && legParams[leg].usable)
00796           contactMsg.addEntry(FootFrameOffset+leg, fmat::Column<3>());
00797       }
00798       contactMsg.flushOnMotionUpdate=true; // delay message to sync with corresponding joint angles
00799       postMessage(contactMsg);
00800     }
00801   }
00802   
00803   /*walkAvg+=walkTime.Age().Value();
00804    ++walkcnt;
00805    std::cout << "Calculation time " << ikAvg/ikcnt << ' ' << walkAvg/walkcnt << std::endl;*/
00806 }
00807 
00808 void XWalkMC::sendLoadPredictions(IKSolver::Point tgts[]) {
00809   // predict servo loads by solving for pressure distribution over the supporting feet, then
00810   // using Jacobian of each leg to find it's servo's loads
00811   DriverMessaging::LoadPrediction loads;
00812   
00813   // first, find center of mass and count ground legs
00814   KinematicJoint * base = childMap[BaseFrameOffset];
00815   std::set<KinematicJoint*> nonsupports(base->getBranches().begin(),base->getBranches().end());
00816   std::map<unsigned int, KinematicJoint*> supports;
00817   for(unsigned int leg=0; leg<NumLegs; ++leg) {
00818     if(legStates[leg].inAir || legStates[leg].support<EPSILON || !legParams[leg].usable) {
00819       // todo: these servos are marked unloaded, but do carry some load from the leg itself... could process that
00820             #ifdef TGT_IS_MANTIS
00821             static const int JointsPerLeg = 4; // hack! fix it later
00822             #endif
00823             KinematicJoint * j = childMap[LegOffset+JointsPerLeg*leg];
00824       while(j->getParent()!=base) {
00825         if(j->outputOffset<NumOutputs)
00826           loads.loads[j->outputOffset.get()]=0;
00827         j = j->getParent();
00828       }
00829     } else {
00830             #ifdef TGT_IS_MANTIS
00831             static const int JointsPerLeg = 4; // hack! fix it later
00832             #endif
00833             KinematicJoint * j = childMap[LegOffset+JointsPerLeg*leg];
00834       while(j->getParent()!=base)
00835         j = j->getParent();
00836       supports[leg]=j;
00837       nonsupports.erase(j);
00838     }
00839   }
00840   if(supports.size()>0) {
00841     fmat::Column<4> com = base->getMassVector();
00842     for(std::set<KinematicJoint*>::const_iterator it=nonsupports.begin(); it!=nonsupports.end(); ++it)
00843       com += (*it)->getTq() * (*it)->sumCenterOfMass();
00844     //cout << "Center of Mass " << com[3] << ": " << com/com[3] << endl;
00845     
00846     std::vector<fmat::Column<2> > contacts;
00847     // todo: should probably use forward kinematics of effector instead of target point in case of IK failure (e.g. joint limit)
00848     for(std::map<unsigned int, KinematicJoint*>::const_iterator it=supports.begin(); it!=supports.end(); ++it)
00849       contacts.push_back(fmat::SubVector<2>(tgts[it->first]));
00850     // todo: project contacts into plane normal to gravity vector
00851     
00852     std::vector<float> pressures;
00853     std::vector<unsigned int> negatives;
00854     do {
00855       computePressure(com[3],com[0]/com[3],com[1]/com[3],contacts,pressures);
00856       
00857       // drop negative pressures and recompute
00858       // this drops all negatives all at once... might be better to drop only the most negative and then recompute?
00859       negatives.clear();
00860       std::map<unsigned int, KinematicJoint*>::const_iterator it=supports.end();
00861       for(unsigned int i=pressures.size(); i!=0;) {
00862         --it;
00863         if(pressures[--i]<0) {
00864           negatives.push_back(it->first);
00865           contacts.erase(contacts.begin()+i);
00866         }
00867       }
00868       for(unsigned int i=0; i<negatives.size(); ++i)
00869         supports.erase(negatives[i]);
00870     } while(!negatives.empty());
00871     
00872     // now for each support, find servo torques in that chain
00873     std::vector<float>::const_iterator pressureIt=pressures.begin();
00874     for(std::map<unsigned int, KinematicJoint*>::const_iterator it=supports.begin(); it!=supports.end(); ++it,++pressureIt) {
00875       std::vector<fmat::Column<6> > j;
00876       KinematicJoint * kj = childMap[FootFrameOffset+it->first];
00877       // todo: should probably use forward kinematics of effector instead of target point in case of IK failure (e.g. joint limit)
00878       kj->getFullJacobian(tgts[it->first],j);
00879       while(kj!=NULL) {
00880         if(kj->outputOffset<NumOutputs) {
00881           // todo: pressure should be along gravity, not z axis
00882           loads.loads[kj->outputOffset.get()] = (*pressureIt)*j[kj->getDepth()][2]/1000;
00883         }
00884         kj=kj->getParent();
00885       }
00886     }
00887     DriverMessaging::postMessage(loads);
00888   }
00889 }
00890 
00891 
00892 void XWalkMC::computePhase(float time) {
00893   if(!isDirty())
00894     return;
00895   globPhase = time/period;
00896   globPhase -= std::floor(globPhase);
00897 }
00898 
00899 void XWalkMC::computeLegPhase(unsigned int leg, bool& inAir, float& phase) {
00900   unsigned int timingLeg = leg;
00901   if(adaptiveLegOrder && legStates[leg].reorder!=-1U)
00902     timingLeg = legStates[leg].reorder;
00903   
00904   float legphase = globPhase - p.legParams[timingLeg].flightPhase;
00905   if(legphase<0)
00906     legphase+=1;
00907   float flightphase = p.legParams[leg].flightDuration/1000.f/period;
00908   inAir=(legphase<flightphase);
00909   phase = (inAir) ? 1-legphase/flightphase : (legphase-flightphase)/(1-flightphase);
00910   if(phase<0)
00911     phase+=1;
00912 }
00913 
00914 void XWalkMC::computeCurrentPosition(unsigned int leg, bool inAir, float speed, float phase, fmat::Column<3>& tgt) {
00915   const fmat::Column<2>& sp = legStates[leg].downPos;
00916   
00917   if(inAir) {
00918     (fmat::SubVector<2>)(tgt) = sp + phase*(legStates[leg].liftPos - sp);
00919     //cout << leg << " down " << sp <<  " lift " << legStates[leg].liftPos << " target " << tgt << endl;
00920     
00921   } else { 
00922     float nonFlightTime = period - legParams[leg].flightDuration/1000.f;
00923     // the raise and lower are considered part of the "ground" phase
00924     //nonFlightTime -= (legParams[leg].raiseDuration + legParams[leg].lowerDuration) / 1000.f; // *TRANSITION_STRIDE* //
00925     
00926     if (std::abs(targetAngVel) <= speed*EPSILON) { // straight line motion
00927       (fmat::SubVector<2>)(tgt) = sp + -phase*nonFlightTime*targetVel;
00928     } else { // arcing motion
00929       // rotate target point about the previously determined center of rotation
00930       (fmat::SubVector<2>)(tgt) = fmat::rotation2D(-phase*nonFlightTime*targetAngVel) * (sp-rotationCenter) + rotationCenter;
00931     }
00932   }
00933 }
00934 
00935 void XWalkMC::computeCurrentBodyOffset(float* legPhases, float speed, float& offx, float& offy, float& offa) {
00936   offx = surge(globPhase,legPhases);
00937   offy = sway(globPhase,legPhases);
00938   offa = 0;
00939   if(rotateBodyMotion && (offx!=0 || offy!=0) && std::abs(speed)>EPSILON) {
00940     float s = targetVel[1]/speed, c = targetVel[0]/speed;
00941     float tmpx = c*offx - s*offy;
00942     float tmpy = s*offx + c*offy;
00943     offx=tmpx;
00944     offy=tmpy;
00945   }
00946   offx-=p.offsetX;
00947   offy-=p.offsetY;
00948   offa-=p.offsetA;
00949 }
00950 
00951 void XWalkMC::solveIK(unsigned int leg, const IKSolver::Point& tgt) {
00952   if(!legParams[leg].usable)
00953     return;
00954 
00955   KinematicJoint * eff = childMap[FootFrameOffset+leg];
00956 
00957 #ifndef TGT_IS_BIPED
00958   eff = childMap[FootFrameOffset+leg];
00959   eff->getIK().solve(IKSolver::Point(), *eff, tgt);
00960 #else
00961   /*float theta = 3.14159/2.;
00962   if(abs(tgt[1])!=23)
00963       theta = atan(tgt[2]/(abs(tgt[1]-23));
00964     if(leg)
00965     {
00966         if(tgt[1] != -23.)
00967             theta = atan(groundPlane[3]/(tgt[1]+23.));
00968         else
00969             theta = -3.14159/2.;
00970     }
00971     else
00972     {
00973         if(tgt[1] != 23.)
00974             theta = atan(groundPlane[3]/(tgt[1]-23.));
00975         else
00976             theta = 3.14159/2.;
00977     }*/
00978 
00979   IKSolver::Point new_tgt;
00980   new_tgt[0] = tgt[0];
00981   new_tgt[1] = tgt[1];
00982   new_tgt[2] = tgt[2]+55;
00983     /*if(theta < 0)
00984       new_tgt[1] = tgt[1]+44.0*cos(theta);
00985     else
00986         new_tgt[1] = tgt[1]-44.0*cos(theta);
00987   new_tgt[2] = tgt[2]+11.0+44.0*sin(theta+3.14159/2);*/
00988   
00989   fmat::Column<3> anklePos = childMap[LegOffset+leg*JointsPerLeg + LegAnkleOffset]->getPosition();
00990 
00991   eff = childMap[LegOffset+LegKneeOffset+leg*JointsPerLeg];
00992   eff->getIK().solve(anklePos, *eff, new_tgt);
00993   float temp = eff->getQ() + eff->getParent()->getQ();     //instead of finding the orientation of the knee I mirrored the hip joints to force the legs to be parellel to the ground.  Doesn't quite work with sidestepping motion, might also solve turning issues?
00994   float other_tmp = eff->getParent()->getParent()->getQ();
00995   eff = childMap[FootFrameOffset+leg];
00996   eff->getParent()->tryQ(other_tmp*-1);
00997   eff->getParent()->getParent()->tryQ(temp);
00998   /*if(leg)
00999   {
01000       float ankleAng = eff->getParent()
01001       eff->getParent()->tryQ(theta-1.57);
01002       eff->getParent()->getParent()->tryQ(0);
01003   }
01004   else
01005   {
01006       eff->getParent()->tryQ((theta-1.57)*-1);
01007       eff->getParent()->getParent()->tryQ(0);
01008   }*/
01009 #endif
01010 
01011   /*if(leg==1)
01012    std::cout << "Leg " << leg << " end pos: " << eff->getWorldPosition() << std::endl;*/
01013   for(; eff!=NULL; eff=eff->getParent()) {
01014     if(eff->outputOffset!=plist::OutputSelector::UNUSED && eff->outputOffset<NumOutputs) {
01015       /*if(leg==1)
01016        std::cout << eff->outputOffset.get() << " set to " << eff->getQ() << std::endl;*/
01017 #ifndef PLATFORM_APERIOS
01018       if(!std::isnan(eff->getQ()))
01019 #endif
01020       {
01021         motman->setOutput(this, eff->outputOffset, eff->getQ());
01022       }
01023     }
01024   }
01025 }
01026 
01027 void XWalkMC::computePressure(float mass, float massx, float massy, const std::vector<fmat::Column<2> >& contacts, std::vector<float>& pressures) {
01028   const float gAcc = 9.80665f;
01029   NEWMAT::ColumnVector weight(3);
01030   const float force = mass*gAcc;
01031   weight(1) = force;
01032   weight(2) = massx/1000.f*force;
01033   weight(3) = massy/1000.f*force;
01034   
01035   //cout << mass << " @ " << massx <<',' << massy << endl;
01036   
01037   // now build matrix of statics constraints:
01038   NEWMAT::Matrix statics(3,contacts.size());
01039   for(unsigned int i=0; i<contacts.size(); ++i) {
01040     statics(1,i+1) = 1;
01041     statics(2,i+1) = contacts[i][0]/1000.f;
01042     statics(3,i+1) = contacts[i][1]/1000.f;
01043   }
01044   
01045   NEWMAT::ColumnVector f;
01046   if(statics.ncols() == 0) {
01047     // no legs on the ground?  wtf.
01048   } else if(statics.ncols() < 3) {
01049     // over constrained, linear least squares
01050     NEWMAT::Matrix U, V;
01051     NEWMAT::DiagonalMatrix Q;
01052     NEWMAT::SVD(statics,Q,U,V,true,true);
01053     for(int i=1; i<=Q.ncols(); ++i) {
01054       if(Q(i)<EPSILON)
01055         Q(i)=0;
01056       else
01057         Q(i)=1/Q(i);
01058     }
01059     f = V*Q*U.t()*weight;
01060   } else if(statics.ncols() == 3) {
01061     // square matrix, easy invert
01062     try {
01063       f = statics.i()*weight;
01064     } catch(...) {
01065       // hmm, if that failed probably singular, fall back on SVD
01066       NEWMAT::Matrix U, V;
01067       NEWMAT::DiagonalMatrix Q;
01068       NEWMAT::SVD(statics,Q,U,V,true,true);
01069       for(int i=1; i<=Q.ncols(); ++i) {
01070         if(Q(i)<EPSILON)
01071           Q(i)=0;
01072         else
01073           Q(i)=1/Q(i);
01074       }
01075       f = V*Q*U.t()*weight;
01076     }
01077   } else {
01078     // under constrained, Moore-Penrose psuedo inverse
01079     NEWMAT::Matrix U, V;
01080     NEWMAT::DiagonalMatrix Q;
01081     NEWMAT::SVD(statics.t(),Q,U,V,true,true);
01082     for(int i=1; i<=Q.ncols(); ++i) {
01083       if(Q(i)<EPSILON)
01084         Q(i)=0;
01085       else
01086         Q(i)=1/Q(i);
01087     }
01088     f = U*Q*V.t()*weight;
01089   }
01090   
01091   pressures.resize(contacts.size());
01092   for(unsigned int i=0; i<contacts.size(); ++i)
01093     pressures[i]=f(i+1);
01094 }
01095 
01096 
01097 void XWalkMC::spiderSettings(plist::DictionaryBase& src, plist::DictionaryBase& dst) {
01098   for(plist::DictionaryBase::const_iterator itS=src.begin(), itD=dst.begin(); itS!=src.end(); ++itS, ++itD) {
01099     if(plist::DictionaryBase* dcol=dynamic_cast<plist::DictionaryBase*>(itS->second)) {
01100       spiderSettings(*dcol,dynamic_cast<plist::DictionaryBase&>(*itD->second));
01101     } else if(plist::ArrayBase* acol=dynamic_cast<plist::ArrayBase*>(itS->second)) {
01102       spiderSettings(*acol,dynamic_cast<plist::ArrayBase&>(*itD->second));
01103     } else if(plist::PrimitiveBase* prim=dynamic_cast<plist::PrimitiveBase*>(itS->second)) {
01104       plist::PrimitiveBase& primDst = dynamic_cast<plist::PrimitiveBase&>(*itD->second);
01105       ParameterTransition * trans = new ParameterTransition(*prim,primDst,active,transitionDuration);
01106       transitions.insert(trans);
01107     }
01108   }
01109 }
01110 
01111 void XWalkMC::spiderSettings(plist::ArrayBase& src, plist::ArrayBase& dst) {
01112   for(unsigned int i=0; i<src.size(); ++i) {
01113     if(plist::DictionaryBase* dcol=dynamic_cast<plist::DictionaryBase*>(&src[i])) {
01114       spiderSettings(*dcol,dynamic_cast<plist::DictionaryBase&>(dst[i]));
01115     } else if(plist::ArrayBase* acol=dynamic_cast<plist::ArrayBase*>(&src[i])) {
01116       spiderSettings(*acol,dynamic_cast<plist::ArrayBase&>(dst[i]));
01117     } else if(plist::PrimitiveBase* prim=dynamic_cast<plist::PrimitiveBase*>(&src[i])) {
01118       plist::PrimitiveBase& primDst = dynamic_cast<plist::PrimitiveBase&>(dst[i]);
01119       ParameterTransition * trans = new ParameterTransition(*prim,primDst,active,transitionDuration);
01120       transitions.insert(trans);
01121     }
01122   }
01123 }
01124 
01125 
01126 void XWalkMC::ParameterTransition::plistValueChanged(const plist::PrimitiveBase& /*pl*/) {
01127   decelerate=false;
01128   lastUpdate=startTime=get_time();
01129   float dur = duration/2.f;
01130   cura = 2 * ( (src.castTo<float>()-dst.castTo<float>())/2 - curd * dur ) / (dur*dur);
01131   active.insert(this);
01132 }
01133 
01134 bool XWalkMC::ParameterTransition::update(unsigned int curTime) {
01135   if(curTime>startTime+duration) {
01136     curd=0;
01137     dst=src;
01138     return false;
01139   }
01140   float srcV = src.castTo<float>(), dstV = dst.castTo<float>();
01141   float dt = (startTime+duration)-lastUpdate; // time remaining
01142   float f = dstV + curd*dt + 0.5f * -cura*dt*dt; // predicted position if we start/continue decelerating now...
01143   if( (f>=srcV && dstV<=srcV) || (f<=srcV && dstV>=srcV) )
01144     decelerate=true;
01145   if(decelerate) {
01146     // would overshoot, decelerate
01147     float dist = srcV-dstV;
01148     // this would keep the target time constant, but discontinuity in velocity
01149     //curd = 2 * dist / dt;
01150     //float acc = ( dist - 2 * curd * dt ) / (dt*dt);
01151     // instead, this lets time flex, keeps other parameters continuous
01152     float acc =  - curd*curd / (2 * dist);
01153     startTime = (unsigned int)(curTime + 2 * dist / curd - duration + .5f);
01154     // std::cout << "  ParameterTransition::update: startTime=" << startTime << std::endl;
01155     dt = curTime-lastUpdate; // now compute current position given current time
01156     f = plist::Primitive<float>(dstV + curd*dt + acc * dt * dt / 2);
01157     // don't overshoot:
01158     if( (f>=srcV && dstV<=srcV) || (f<=srcV && dstV>=srcV) ) {
01159       curd=0;
01160       dst=src;
01161       return false;
01162     }
01163     dst = f;
01164     curd += acc*dt;
01165   } else {
01166     // accelerate
01167     dt = curTime-lastUpdate+.5f;
01168     dst = plist::Primitive<float>(dstV + curd*dt + cura*dt*dt/2);
01169     curd += cura*dt;
01170   }
01171   lastUpdate=curTime;
01172   return true;
01173 }
01174 
01175 /*! @file
01176  * @brief 
01177  * @author Ethan Tira-Thompson (ejt) (Creator)
01178  */
01179 
01180 #endif // TGT_HAS_LEGS

Tekkotsu v5.1CVS
Generated Mon May 9 04:58:53 2016 by Doxygen 1.6.3