Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

WheeledWalkMC.cc

Go to the documentation of this file.
00001 #include "Shared/RobotInfo.h"
00002 #ifdef TGT_HAS_WHEELS
00003 
00004 #include "WheeledWalkMC.h"
00005 #include "Kinematics.h"
00006 #include "Events/LocomotionEvent.h"
00007 #include "Shared/Config.h"
00008 #include <sys/stat.h>
00009 
00010 using namespace std; 
00011 
00012 void WheeledWalkMC::resetConfig() {
00013   updateWheelConfig();  // calculates maxVel values
00014   // default to going half maximum speed
00015   preferredXVel = getMaxXVel() / 2;
00016   preferredYVel = getMaxYVel() / 2;
00017   preferredAngVel = getMaxAVel() / 2;
00018   // check if a config file is available to alter preferred speeds
00019   std::string file = config->motion.makePath("wheeledwalk.plist");
00020   struct stat statbuf;
00021   if(::stat(file.c_str(),&statbuf)==0)
00022     loadFile(file.c_str()); // override with settings from file
00023 }
00024 
00025 int WheeledWalkMC::updateOutputs() {
00026   //std::cout << "WheeledWalkMC::updateOutputs " << (void*)this << "  displacementMode=" << displacementMode
00027   //          << " dirty=" << dirty << std::endl;
00028   unsigned int t = get_time();
00029   unsigned int dur = t - travelStartTime;
00030   if (!dirty) { // Not dirty means current walk values previously sent to motman, and haven't changed
00031     if (displacementMode) {
00032 #if defined(TGT_IS_CREATE)
00033       float distTraveled = state->sensors[EncoderDistanceOffset] - travelStartDist;
00034       float angTraveled = state->sensors[EncoderAngleOffset]*M_PI/180.0 - travelStartAngle;
00035       /* 
00036          The Create updates its EncoderDistance value slowly enough
00037          that it jumps in increments of 7 to 20 mm for typical travel
00038          speeds.  To minimize the under/overshoot we should stop when we are
00039          within jump/2 of the target displacement.  At default speed, jump is 14,
00040          so use a fudgeFactor of 7 for now.  Should determine dynamically.
00041       */
00042       float distFudgeFactor = 7;  // should determine empirically since this varies by speed
00043       float angFudgeFactor = 2.5 / 180 * M_PI;
00044 #elif defined(TGT_IS_CREATE2)
00045       // *** replace this with encoder calculation
00046       float distTraveled = state->sensors[EncoderDistanceOffset] - travelStartDist;
00047       float angTraveled = state->sensors[EncoderAngleOffset]*M_PI/180.0 - travelStartAngle;
00048       float distFudgeFactor = 0;
00049       float angFudgeFactor = 0;
00050 #elif defined(TGT_IS_KOBUKI)
00051       unsigned short currentTickLeft = state->sensors[LeftEncoderOffset];
00052       float leftDiffTicks = (float)(short)((currentTickLeft - lastTickLeft) & 0xffff);
00053       lastTickLeft = currentTickLeft;
00054       unsigned short currentTickRight = state->sensors[RightEncoderOffset];
00055       float rightDiffTicks = (double)(short)((currentTickRight - lastTickRight) & 0xffff);
00056       lastTickRight = currentTickRight; 
00057 
00058       float dx = RobotInfo:: wheelRadius * RobotInfo::tickToRad * (leftDiffTicks + rightDiffTicks)/2.0;
00059       float dtheta = RobotInfo::wheelRadius * RobotInfo::tickToRad * (rightDiffTicks - leftDiffTicks)/RobotInfo::wheelBase;
00060       float distTraveled = float(dx + lastDistTraveled);
00061       float angTraveled = float(dtheta + lastAngTraveled);
00062       lastDistTraveled = distTraveled;
00063       lastAngTraveled = angTraveled;
00064       float distFudgeFactor = 0;
00065       float angFudgeFactor = 0 / 180 * M_PI;
00066 #else
00067 #  error "Wheeled walk can't do odometry: target is not a CREATE or KOBUKI"
00068 #endif
00069       //std::cout << "wwmc:  dist = " << dist << " targetDist = " << targetDist << "     angdist = " << angdist*180/M_PI << std::endl;
00070       // Must use zeroVelocities() here, not
00071       // setTargetVelocity, because we must post a
00072       // locomotionEvent to let Mirage know we're stopping.
00073       // std::cout<< ">>> Debug: angTraveled = " << angTraveled << "   targetAngDist = " << targetAngDist << std::endl;
00074       if ( (targetDist != 0 && fabs(distTraveled)+distFudgeFactor >= fabs(targetDist)) ||
00075            (targetDist == 0 && fabs(angTraveled)+angFudgeFactor >= fabs(targetAngDist)) ) {
00076         zeroVelocities();
00077         postEvent(EventBase(EventBase::motmanEGID, getID(), EventBase::statusETID, dur));
00078       }
00079     }
00080     if(targetVel[0]==0 && targetVel[1]==0 && targetAngVel==0)
00081       return 0;
00082   }
00083   if (dirty) {
00084     travelStartTime = t;
00085 #if defined(TGT_IS_CREATE) || defined(TGT_IS_CREATE2)
00086     travelStartDist = state->sensors[EncoderDistanceOffset];
00087     travelStartAngle = state->sensors[EncoderAngleOffset]*M_PI/180.0;
00088 #elif defined(TGT_IS_KOBUKI)
00089     travelStartDist = 0;
00090     travelStartAngle = 0;
00091 #endif
00092     LocomotionEvent e(EventBase::locomotionEGID, getID(), EventBase::statusETID, dur);
00093     e.setXYA(targetVel[0], targetVel[1], targetAngVel);
00094     postEvent(e);
00095   }
00096   for(unsigned int i=0; i<NumWheels; ++i) {
00097     if(wheels[i].valid) {
00098       motman->setOutput(this, WheelOffset+i, wheels[i].targetVel);
00099     }
00100   }
00101   dirty=false;
00102   return NumWheels;
00103 }
00104 
00105 void WheeledWalkMC::start() {
00106   dirty = true;
00107   travelStartTime = get_time();
00108 #if defined(TGT_IS_CREATE) || defined(TGT_IS_CREATE2)
00109   travelStartDist = state->sensors[EncoderDistanceOffset];
00110   travelStartAngle = state->sensors[EncoderAngleOffset]*M_PI/180.0;
00111 #elif defined(TGT_IS_KOBUKI)
00112   travelStartDist = 0;
00113   travelStartAngle = 0;
00114 #endif
00115 }
00116 
00117 void WheeledWalkMC::stop() {
00118   zeroVelocities();
00119   MotionCommand::stop();
00120 }
00121 
00122 int WheeledWalkMC::isAlive() {
00123   return !displacementMode || getTravelTime()<=targetDur || dirty;
00124 }
00125 
00126 void WheeledWalkMC::zeroVelocities() {
00127   if(targetVel[0]==0 && targetVel[1]==0 && targetAngVel==0)
00128     return;
00129   unsigned int t = getTravelTime(); // cache because travelStartTime is about to be reset
00130   setTargetVelocity(0, 0, 0);
00131   for (unsigned int i = WheelOffset; i < WheelOffset + NumWheels; i++)
00132     motman->setOutput(this, i, 0.0f);
00133   postEvent(LocomotionEvent(EventBase::locomotionEGID, getID(), EventBase::statusETID, t));
00134 }
00135 
00136 void WheeledWalkMC::setTargetVelocity(float xvel, float yvel, float avel) {
00137   // std::cout << "setTargetVelocity xvel=" << xvel << " avel=" << avel << std::endl;
00138   displacementMode=false;
00139   if(std::abs(targetVel[0]-xvel)<0.01f && std::abs(targetVel[1]-yvel)<0.01f && std::abs(targetAngVel-avel)<0.001f)
00140     return; // don't bother with updates (and in particular, associated LocomotionEvents) from small rounding errors
00141   targetVel[0] = xvel;
00142   targetVel[1] = yvel;
00143   targetAngVel = avel;
00144   updateWheelVels();
00145 }
00146 
00147 void WheeledWalkMC::setTargetVelocity(float xvel, float yvel, float avel, float time) {
00148   displacementMode=true;
00149   if(time<=0 || (xvel==0 && yvel==0 && avel==0) ) {
00150     targetVel[0] = targetVel[1] = targetAngVel = 0;
00151     targetDur = 0;
00152   } else {
00153     targetVel[0] = xvel;
00154     targetVel[1] = yvel;
00155     targetAngVel = avel;
00156     targetAngDist = avel * time;  // need this for updateWheelVels
00157     targetDur = static_cast<unsigned int>(time*1000 + 0.5);
00158   }
00159   updateWheelVels();
00160 }
00161 
00162 void WheeledWalkMC::setTargetDisplacement(float xdisp, float ydisp, float adisp, float time) {
00163   // std::cout << (void*)(this) << ":id=" << getID() <<"   setTargetDisplacement(" << xdisp << ", " << ydisp << ", " << adisp << ")" << std::endl;
00164   if ( xdisp == 0 && ydisp == 0 && adisp == 0 ) {
00165     setTargetVelocity(0, 0, 0, 0);
00166     return;
00167   }
00168   
00169   // select preferred velocity if time==0, otherwise cap at maximum
00170   float xCap=getMaxXVel(), yCap=getMaxYVel(), aCap=getMaxAVel();
00171   if(time==0) {
00172     if(xCap>preferredXVel) xCap = preferredXVel;
00173     if(yCap>preferredYVel) yCap = preferredYVel;
00174     if(aCap>preferredAngVel) aCap = preferredAngVel;
00175   }
00176   xCap = 100; // ***** GREASY HACK TO MAKE CREATE SPEED WORK
00177   
00178   // test each cap, may be constrained - set to 0
00179   const float tx = xCap>0 ? std::abs(xdisp/xCap) : 0;
00180   const float ty = yCap>0 ? std::abs(ydisp/yCap) : 0;
00181   const float ta = aCap>0 ? std::abs(adisp/aCap) : 0;
00182   float maxTime =  std::max(time,std::max(tx,std::max(ty,ta)));
00183   
00184   // round up to closest FrameTime * NumFrames multiple for even better precision
00185   maxTime = std::ceil(maxTime * 1000 / (FrameTime*NumFrames)) * (FrameTime*NumFrames) / 1000.f;
00186   //std::cout << "xCap=" << xCap << " tx=" << tx << " xdisp=" << xdisp << " maxTime=" << maxTime << std::endl;
00187   //std::cout << "maxVel is " << maxVel[0] << " " << maxVel[1] << " " << maxVel[2] << " preferreXVel=" << preferredXVel << std::endl;
00188   targetDist = xdisp;
00189   targetAngDist = adisp;
00190   setTargetVelocity(xdisp/maxTime, ydisp/maxTime, adisp/maxTime, maxTime);
00191 }
00192 
00193 void WheeledWalkMC::setTargetDisplacement(float xdisp, float ydisp, float adisp, float xvel, float yvel, float avel) {
00194   //std::cout << "WheeledWalkMC::setTargetDisplacement xdisp=" << xdisp << " xvel=" << xvel
00195   //          << "  adisp=" << adisp << " avel=" << avel << std::endl;
00196   targetVel = fmat::pack(xdisp>=0 ? xvel : -xvel, ydisp>=0 ? yvel : -yvel);
00197   targetAngVel = adisp >= 0 ? avel : -avel;
00198   targetDist = xdisp;
00199   targetAngDist = adisp;
00200   displacementMode = true;
00201   updateWheelVels();
00202   dirty = true;
00203 }
00204 void WheeledWalkMC::updateWheelVels() {
00205   if ( (displacementMode && std::abs(targetAngDist) < 0.01) || std::abs(targetAngVel) <= targetVel.norm()*EPSILON) {
00206     // straight line motion
00207 #ifdef TGT_IS_KOBUKI
00208     //std::cout << "targetVel[0] = " << targetVel[0];
00209     wheels[SpeedOffset].targetVel = targetVel[0];
00210     wheels[RadiusOffset].targetVel = 0;
00211 #else
00212     for(unsigned int i=0; i<NumWheels; ++i)
00213       if (wheels[i].valid)
00214         wheels[i].targetVel = fmat::dotProduct(wheels[i].direction,targetVel);
00215 #endif
00216   } else if ( (displacementMode && std::abs(targetDist) < 1) || std::abs(targetVel[0]) <= EPSILON) {
00217       // pure turn  
00218 #ifdef TGT_IS_KOBUKI
00219       wheels[SpeedOffset].targetVel = RobotInfo::wheelBase / 2 * targetAngVel;
00220       wheels[RadiusOffset].targetVel = 1;
00221 #else
00222     for (unsigned int i=0; i<NumWheels; ++i)
00223       if (wheels[i].valid) {
00224         fmat::Column<2> p = wheels[i].position - rotationCenter;
00225         wheels[i].targetVel = fmat::dotProduct(wheels[i].direction,fmat::pack(-p[1],p[0])) * targetAngVel;
00226       }
00227 #endif
00228     } else {
00229       // arc
00230 #ifdef TGT_IS_KOBUKI
00231       float turnRadius = targetVel[0] / targetAngVel;
00232       if ( turnRadius > 0.0 )
00233         wheels[SpeedOffset].targetVel = (turnRadius + RobotInfo::wheelBase/2) * targetAngVel;
00234       else
00235         wheels[SpeedOffset].targetVel = (turnRadius - RobotInfo::wheelBase/2) * targetAngVel;
00236       wheels[RadiusOffset].targetVel = turnRadius;
00237 #else
00238     // arcing motion, rotate target point about the previously determined center of rotation
00239     // std::cout <<"targetVel[0] = " << targetVel[0] << " targetVel[1] = " << targetVel[1]
00240     //          << " targetAngVel = " << targetAngVel << std::endl;
00241     fmat::Column<2> arcCenter = fmat::pack(-targetVel[1],targetVel[0]) / targetAngVel + rotationCenter;
00242     for(unsigned int i=0; i<NumWheels; ++i) {
00243       if(!wheels[i].valid)
00244         continue;
00245       fmat::Column<2> p = wheels[i].position - arcCenter;
00246       wheels[i].targetVel = fmat::dotProduct(wheels[i].direction,fmat::pack(-p[1],p[0])) * targetAngVel;
00247     }
00248 #endif
00249   }
00250   dirty = true;
00251 }
00252 
00253 void WheeledWalkMC::updateWheelConfig() {
00254 #ifdef TGT_IS_KOBUKI
00255   // Kobuki cannot control individual wheel speeds, so the algorithm below doesn't apply
00256   wheels[0].valid = wheels[1].valid = true;
00257   maxVel = fmat::pack(700.f, 0.f);
00258   maxAngVel = 6.0f;
00259 #else
00260   fmat::Column<2> wheelSum;
00261   unsigned int avail=0;
00262   
00263   // find position and orientation of each joint
00264   for(unsigned int i=0; i<NumWheels; ++i) {
00265     fmat::Transform t = kine->linkToBase(WheelOffset+i);
00266     wheels[i].direction = fmat::pack(t(1,2),-t(0,2));
00267     fmat::fmatReal dm=wheels[i].direction.norm();
00268     if(dm<std::numeric_limits<float>::epsilon()*10) {
00269       std::cerr << "WARNING: " << outputNames[WheelOffset+i] << " has a vertical axle, ignoring it" << std::endl;
00270       wheels[i].direction=0.f;
00271       continue; // wheel is on its side, does not contribute
00272     } else if(dm<1-std::numeric_limits<float>::epsilon()*10) {
00273       // this could be supported, just need to compute position at contact with ground
00274       std::cerr << "WARNING: " << outputNames[WheelOffset+i] << " has a non-planar axle, this may affect motion accuracy" << std::endl;
00275     }
00276     wheels[i].direction/=dm;
00277     wheels[i].position = fmat::SubVector<2>(t.translation());
00278     wheelSum+=wheels[i].position;
00279     ++avail;
00280     wheels[i].valid=true;
00281   }
00282   rotationCenter = wheelSum / avail;
00283   
00284   maxVel = (NumWheels==0) ? 0 : std::numeric_limits<fmat::fmatReal>::infinity();
00285   maxAngVel=0.f;
00286   for(unsigned int i=0; i<NumWheels; ++i) {
00287     if(!wheels[i].valid)
00288       continue;
00289     
00290     // maximum velocity is the minimum full-power wheel contribution
00291     float v = std::min(std::abs(outputRanges[WheelOffset+i][0]),std::abs(outputRanges[WheelOffset+i][1]));
00292     fmat::Column<2> d = wheels[i].direction*v;
00293     maxVel[0] = std::min(maxVel[0],d[0]);
00294     maxVel[1] = std::min(maxVel[1],d[1]);
00295     
00296     // maximum rotation is sum of contributions
00297     fmat::Column<2> curCof = (wheelSum - wheels[i].position) / (avail-1);
00298     fmat::Column<2> norm = (wheels[i].position - curCof);
00299     norm -= fmat::dotProduct(wheels[i].direction,norm) * wheels[i].direction;
00300     float base = norm.norm();
00301     const float MAX_ROT = M_PI*3; // limit crazy rotational speeds in case of short base
00302     float av = (base*MAX_ROT > v ) ? v / base : 0; // send limit to zero because centrally applied instead of inf. rotation
00303     maxAngVel += av;
00304   }
00305 #endif  
00306   if(targetVel[0]!=0 || targetVel[1]!=0 || targetAngVel!=0)
00307     updateWheelVels();
00308 }
00309 
00310 /*! @file
00311  * @brief Impliments WheeledWalkMC, which provides a 'WalkMC' implementation for wheeled robots (diff-drive or theoretically holonomic configurations as well)
00312  * @author Ethan Tira-Thompson (ejt) (Creator)
00313  */
00314 
00315 #endif

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