WheeledWalkMC.cc

Go to the documentation of this file.

#include "Shared/RobotInfo.h"
#ifdef TGT_HAS_WHEELS

#include "WheeledWalkMC.h"
#include "Kinematics.h"
#include "Events/LocomotionEvent.h"
#include "Shared/Config.h"
#include <sys/stat.h>

using namespace std; 

static const float EPSILON=1e-4f;

void WheeledWalkMC::resetConfig() {
  updateWheelInfo();
  // default to going half maximum speed
  preferredXVel = getMaxXVel() / 2;
  preferredYVel = getMaxYVel() / 2;
  preferredAngVel = getMaxAVel() / 2;
  // check if a config file is available
  std::string file = config->motion.makePath("wheeledwalk.plist");
  struct stat statbuf;
  if(::stat(file.c_str(),&statbuf)==0)
    loadFile(file.c_str()); // override with settings from file
}

int WheeledWalkMC::updateOutputs() {
  unsigned int t = get_time();
  unsigned int dur = t - travelTime;
  if(!dirty) {
    if(displacementMode && dur>=travelDur) {
      // Must use zeroVelocities() here, not
      // setTargetVelocity, because we must post a
      // locomotionEvent to let Mirage know we're stopping.
        zeroVelocities();
      postEvent(EventBase(EventBase::motmanEGID, getID(), EventBase::statusETID, dur));
    }
    if(targetVel[0]==0 && targetVel[1]==0 && targetAngVel==0)
      return 0;
  }
  if(dirty) {
    LocomotionEvent e(EventBase::locomotionEGID, getID(), EventBase::statusETID, dur);
    e.setXYA(targetVel[0], targetVel[1], targetAngVel);
    travelTime = t;
    postEvent(e);
  }
  for(unsigned int i=0; i<NumWheels; ++i) {
    if(wheels[i].valid) {
      motman->setOutput(this, WheelOffset+i, wheels[i].targetVel);
    }
  }
  dirty=false;
  return NumWheels;
}

void WheeledWalkMC::start() {
  dirty=true;
  travelTime=get_time();
}

void WheeledWalkMC::stop() {
  zeroVelocities();
  MotionCommand::stop();
}

void WheeledWalkMC::zeroVelocities() {
  if(targetVel[0]==0 && targetVel[1]==0 && targetAngVel==0)
    return;
  unsigned int t = getTravelTime(); // cache because travelTime is about to be reset
  setTargetVelocity(0, 0, 0);
  for (unsigned int i = WheelOffset; i < WheelOffset + NumWheels; i++)
    motman->setOutput(this, i, 0.0f);
  postEvent(LocomotionEvent(EventBase::locomotionEGID, getID(), EventBase::statusETID, t));
}

void WheeledWalkMC::setTargetVelocity(float xvel, float yvel, float avel) {
  displacementMode=false;
  if(std::abs(targetVel[0]-xvel)<0.01f && std::abs(targetVel[1]-yvel)<0.01f && std::abs(targetAngVel-avel)<0.001f)
    return; // don't bother with updates (and in particular, associated LocomotionEvents) from small rounding errors
  targetVel[0] = xvel;
  targetVel[1] = yvel;
  targetAngVel = avel;
  updateWheelVels();
}

void WheeledWalkMC::setTargetVelocity(float xvel, float yvel, float avel, float time) {
  displacementMode=true;
  if(time<=0 || (xvel==0 && yvel==0 && avel==0) ) {
    targetVel[0] = targetVel[1] = targetAngVel = 0;
    travelDur = 0;
  } else {
    targetVel[0] = xvel;
    targetVel[1] = yvel;
    targetAngVel = avel;
    travelDur = static_cast<unsigned int>(time*1000 + 0.5);
  }
  updateWheelVels();
}

void WheeledWalkMC::setTargetDisplacement(float xdisp, float ydisp, float adisp, float time) {
  // std::cout << (void*)(this) << ":id=" << getID() <<"   setTargetDisplacement(" << xdisp << ", " << ydisp << ", " << adisp << ")" << std::endl;
  if ( xdisp == 0 && ydisp == 0 && adisp == 0 ) {
    setTargetVelocity(0, 0, 0, 0);
    return;
  }
  
  // select preferred velocity if time==0, otherwise cap at maximum
  float xCap=getMaxXVel(), yCap=getMaxYVel(), aCap=getMaxAVel();
  if(time==0) {
    if(xCap>preferredXVel) xCap = preferredXVel;
    if(yCap>preferredYVel) yCap = preferredYVel;
    if(aCap>preferredAngVel) aCap = preferredAngVel;
  }
  
  // test each cap, may be constrained - set to 0
  const float tx = xCap>0 ? std::abs(xdisp/xCap) : 0;
  const float ty = yCap>0 ? std::abs(ydisp/yCap) : 0;
  const float ta = aCap>0 ? std::abs(adisp/aCap) : 0;
  float maxTime =  std::max(time,std::max(tx,std::max(ty,ta)));
  
  // round up to closest FrameTime * NumFrames multiple for even better precision
  maxTime = std::ceil(maxTime * 1000 / (FrameTime*NumFrames)) * (FrameTime*NumFrames) / 1000.f;
  
  setTargetVelocity(xdisp/maxTime, ydisp/maxTime, adisp/maxTime, maxTime);
}

void WheeledWalkMC::updateWheelInfo() {
  fmat::Column<2> wheelSum;
  unsigned int avail=0;
  
  // find position and orientation of each joint
  for(unsigned int i=0; i<NumWheels; ++i) {
    fmat::Transform t = kine->linkToBase(WheelOffset+i);
    wheels[i].direction = fmat::pack(t(1,2),-t(0,2));
    fmat::fmatReal dm=wheels[i].direction.norm();
    if(dm<std::numeric_limits<float>::epsilon()*10) {
      std::cerr << "WARNING: " << outputNames[WheelOffset+i] << " has a vertical axle, ignoring it" << std::endl;
      wheels[i].direction=0.f;
      continue; // wheel is on its side, does not contribute
    } else if(dm<1-std::numeric_limits<float>::epsilon()*10) {
      // this could be supported, just need to compute position at contact with ground
      std::cerr << "WARNING: " << outputNames[WheelOffset+i] << " has a non-planar axle, this may affect motion accuracy" << std::endl;
    }
    wheels[i].direction/=dm;
    wheels[i].position = fmat::SubVector<2>(t.translation());
    wheelSum+=wheels[i].position;
    ++avail;
    wheels[i].valid=true;
  }
  center = wheelSum / avail;
  
  maxVel = (NumWheels==0) ? 0 : std::numeric_limits<fmat::fmatReal>::infinity();
  maxAngVel=0.f;
  for(unsigned int i=0; i<NumWheels; ++i) {
    if(!wheels[i].valid)
      continue;
    
    // maximum velocity is the minimum full-power wheel contribution
    float v = std::min(std::abs(outputRanges[WheelOffset+i][0]),std::abs(outputRanges[WheelOffset+i][1]));
    fmat::Column<2> d = wheels[i].direction*v;
    maxVel[0] = std::min(maxVel[0],d[0]);
    maxVel[1] = std::min(maxVel[1],d[1]);
    
    // maximum rotation is sum of contributions
    fmat::Column<2> curCof = (wheelSum - wheels[i].position) / (avail-1);
    fmat::Column<2> norm = (wheels[i].position - curCof);
    norm -= fmat::dotProduct(wheels[i].direction,norm) * wheels[i].direction;
    float base = norm.norm();
    const float MAX_ROT = M_PI*3; // limit crazy rotational speeds in case of short base
    float av = (base*MAX_ROT > v ) ? v / base : 0; // send limit to zero because centrally applied instead of inf. rotation
    maxAngVel += av;
  }
  
  if(targetVel[0]!=0 || targetVel[1]!=0 || targetAngVel!=0)
    updateWheelVels();
  
  /*
  for(unsigned int i=0; i<NumWheels; ++i)
    std::cout << "Wheel " << i << " at " << wheels[i].position << " facing " << wheels[i].direction << std::endl;
  std::cout << "Max velocity: " << maxVel[0] << ' ' << maxVel[1] << ' ' << maxAngVel << std::endl;
  */
}

void WheeledWalkMC::updateWheelVels() {
  if (std::abs(targetAngVel) <= targetVel.norm()*EPSILON) {
    // straight line motion
    for(unsigned int i=0; i<NumWheels; ++i) {
      if(!wheels[i].valid)
        continue;
      wheels[i].targetVel = fmat::dotProduct(wheels[i].direction,targetVel);
    }
  } else {
    // arcing motion, rotate target point about the previously determined center of rotation
    fmat::Column<2> rotationCenter = fmat::pack(-targetVel[1],targetVel[0]) / targetAngVel + center;
    for(unsigned int i=0; i<NumWheels; ++i) {
      if(!wheels[i].valid)
        continue;
      fmat::Column<2> p = wheels[i].position - rotationCenter;
      wheels[i].targetVel = fmat::dotProduct(wheels[i].direction,fmat::pack(-p[1],p[0])) * targetAngVel;
    }
  }
  dirty = true;
} 

/*! @file
 * @brief Impliments WheeledWalkMC, which provides a 'WalkMC' implementation for wheeled robots (diff-drive or theoretically holonomic configurations as well)
 * @author Ethan Tira-Thompson (ejt) (Creator)
 */

#endif