GaitedFootstepMC.h

Go to the documentation of this file.

//-*-c++-*-
#ifndef INCLUDED_GaitedFootstepMC_h_
#define INCLUDED_GaitedFootstepMC_h_

#include "Motion/MotionCommand.h"
#include "Motion/MotionManager.h"
#include "Motion/XWalkMC.h"
#include "Planners/GaitedFootsteps.h"
#include "Shared/get_time.h"

//! Executes a series of footsteps, probably generated by applying astar() to a GaitedFootsteps domain
class GaitedFootstepMC : public MotionCommand {
public:
  //! Constructor
  GaitedFootstepMC()
    : MotionCommand(), xp(), ground(), gravity(), steps(), curStep(0),
    xySpeed(fmat::pack(30,30)), aSpeed(0.2f), newStep(false), stepStartTime(0), stepDuration(0), contactMsg()
  {
    setGait(::config->motion.makePath("xwalk.plist"), true);
    if(kine==NULL) {
      kine = new KinematicJoint;
      kine->loadFile(::config->makePath(config->motion.kinematics).c_str());
      kine->buildChildMap(childMap,0,NumReferenceFrames);
    }
  }
  
  virtual int updateOutputs();
  virtual int isDirty() { return curStep+1 < steps.size(); }
  virtual int isAlive() { return curStep+1 < steps.size(); }
  
  //! Assign gait settings
  void setGait(const XWalkParameters& p, bool resetSpeeds=true) {
    xp=p;
    xp.packGroundGravity(ground,gravity);
    if(resetSpeeds)
      setSpeed(xp.getMaxXVel(),xp.getMaxYVel(),xp.getMaxAVel());
  }
  
  //! Load gait parameters from a file
  void setGait(const std::string& file, bool resetSpeeds=true) {
    xp.loadFile(file.c_str());
    if(resetSpeeds)
      setSpeed(xp.getMaxXVel(),xp.getMaxYVel(),xp.getMaxAVel());
  }
  
  //! Specify steps to perform
  void setSteps(const std::vector<GaitedFootsteps::State>& st) {
    steps=st;
    stepStartTime = get_time() / 1000.0;
    stepDuration = 0;
    curStep=-1u;
    advanceStep();
  }
  
  //! Specify speeds for translation (mm/s) and rotation (rad/s)
  /*! Does not take affect until next step */
  void setSpeed(fmat::fmatReal x, fmat::fmatReal y, fmat::fmatReal a) {
    xySpeed[0]=x; xySpeed[1]=y; aSpeed=a;
  }

protected:
  //! updates active step and returns offset of body position from current step's initial location
  fmat::Column<2> curBodyOffset(double t);
  
  //! increments #curStep and dependent values
  bool advanceStep(); 
  
  //! solves inverse kinematics and send affected output values to motion manager
  void solveIK(unsigned int leg, const IKSolver::Point& tgt);
  
  virtual void doStart() {}
  virtual void doStop() {}

  XWalkParameters xp;
  fmat::Column<3> ground;
  fmat::Column<3> gravity;
  
  std::vector<GaitedFootsteps::State> steps;
  unsigned int curStep; //!< current #step index, indicates transition to curStep+1, never increments past steps.size()-1
  fmat::Column<2> xySpeed;
  fmat::fmatReal aSpeed;
  bool newStep;
  double stepStartTime; //!< clock time in second of beginning of step
  double stepDuration; //!< duration in seconds
  bool support[NumLegs]; //!< set to true if the leg is in support phase (i.e. not moving to new contact)
  DriverMessaging::FixedPoints contactMsg; //!< list of current contact points, for better Mirage simulation

  static KinematicJoint* kine;
  static KinematicJoint* childMap[NumReferenceFrames];
};

/*! @file
 * @brief Defines GaitedFootstepMC, which executes a series of footsteps, probably generated by applying astar() to a GaitedFootsteps domain
 * @author Ethan Tira-Thompson (ejt) (Creator)
 */

#endif