Tekkotsu Homepage
Dev. Resources


Go to the documentation of this file.
00001 //-*-c++-*-
00002 #ifndef INCLUDED_XWalkMC_h_
00003 #define INCLUDED_XWalkMC_h_
00005 #include "Motion/XWalkParameters.h"
00006 #include "Motion/MotionCommand.h"
00007 #include "Motion/MotionManager.h"
00008 #include "Motion/KinematicJoint.h"
00009 #include "Motion/IKSolver.h"
00010 #include "Shared/fmatSpatial.h"
00011 #include "Shared/Config.h"
00012 #include "Shared/get_time.h"
00013 #include "IPC/DriverMessaging.h"
00014 #include <set>
00016 //! Extreme walk engine handles legged locomotion on hexapods and beyond
00017 class XWalkMC : public MotionCommand, public XWalkParameters {
00018 public:
00019   //! constructor
00020   XWalkMC();
00022   //! Returns the current x and y velocities in mm/sec
00023   const fmat::Column<2>& getTargetVelocity() const { return targetVel; };
00025   //! Returns the current angular velocity in radians/sec
00026   float getTargetAngVelocity() const { return targetAngVel; }
00028   //! Specify the desired body velocity in x and y (millimeters per second) and angular velocity (radians per second)
00029   virtual void getTargetVelocity(float &xvel, float &yvel, float &avel) {
00030     xvel = targetVel[0];
00031     yvel = targetVel[1];
00032     avel = targetAngVel;
00033   }
00035   //! Specify the desired body velocity in x and y (millimeters per second) and angular velocity (radians per second); does not stop automatically
00036   virtual void setTargetVelocity(float xvel, float yvel, float avel);
00038   //! Specify the desired body velocity in x and y (millimeters per second) and angular velocity (radians per second), and amount of time (seconds) before stopping
00039   virtual void setTargetVelocity(float xvel, float yvel, float avel, float time);
00041   //! Specify the desired body displacement in x and y (millimeters) and angle (radians), optionally specify time (seconds)
00042   /*! Corresponding velocity will be limited to max velocity, so setting time=0 implies max speed */
00043   virtual void setTargetDisplacement(float xdisp, float ydisp, float adisp, float time=0);
00045   virtual void setTargetDisplacement(float xdisp, float ydisp, float adisp, float xvel, float yvel, float avel) {}
00047   virtual int updateOutputs();
00049   void setDirty() { dirty = true; }
00051   virtual int isDirty() {
00052     for(unsigned int i=0; i<NumLegs; ++i)
00053       if(legStates[i].support<1)
00054         return dirty=true;
00055     return dirty = ( (targetVel.norm()>=EPSILON || std::abs(targetAngVel)>=EPSILON || !active.empty()) );
00056   }
00058   virtual int isAlive() { return true; }
00060   virtual void start();
00062   virtual void stop();
00064   //! Posts a LocomotionEvent and sets velocities to zero. Also forces an output frame setting wheel velocities to zero; needed because if we remove a motion command there may be nothing left to zero the velocities.
00065   virtual void zeroVelocities();
00068 #if 0
00069   //! Adjusts body height to lock knees and minimize joint torque to hold position, doesn't move feet
00070   /*! Implies a call to setTargetVelocity(0,0,0); later calls to set a non-zero velocity will cancel the leg lock. */
00071   void lockLegsInPlace();
00072 #endif
00074 protected:
00075   //! recomputes each leg's LegState::neutralPos based on current values in #p
00076   void updateNeutralPos(unsigned int curTime);
00077   // updates period based on stride length parameter, returns true if no significant motion is currently requested
00078   bool resetPeriod(float time, float speed);
00079   //! recomputes LegState::downPos and LegState::liftPos based on specified state variables, such that the direction of stride rotates about the current position
00080   void resetLegState(unsigned int leg, float phase, const fmat::Column<3>& curPos, bool inAir, float speed);
00082   void updateOutputsInitial(unsigned int curTime, const fmat::Column<3>& ground, const fmat::Column<3>& gravity, IKSolver::Point tgts[]);
00083   void updateOutputsWalking(float dt, const fmat::Column<3>& ground, const fmat::Column<3>& gravity, float speed, IKSolver::Point tgts[]);
00084   void sendLoadPredictions(IKSolver::Point tgts[]);
00086   //! updates #globPhase based on specified stride time (relative to #startTime)
00087   void computePhase(float time);
00088   //! computes current leg state based on #globPhase
00089   void computeLegPhase(unsigned int leg, bool& inAir, float& phase);
00090   //! computes the leg position (in xy plane only) based on leg state
00091   void computeCurrentPosition(unsigned int leg, bool inAir, float speed, float phase, fmat::Column<3>& tgt);
00092   //! computes body position based on settings in #p, which should then be added to leg positions before projecting onto ground plane
00093   void computeCurrentBodyOffset(float* legPhases, float speed, float& offx, float& offy, float& offa);
00094   //! solves inverse kinematics and send affected output values to motion manager
00095   void solveIK(unsigned int leg, const IKSolver::Point& tgt);
00097   // Given the mass, its position x and y, and a list of contact points, stores results into @a pressures
00098   /*! @param mass being supported in kilograms
00099    *  @param massx center of mass along x axis, in millimeters
00100    *  @param massy center of mass along y axis, in millimeters
00101    *  @param contacts position of each support contact, in millimeters
00102    *  @param[out] pressures will be resized and overwritten with pressure at each corresponding point in @a contacts, in newtons
00103    *  
00104    *  Note the z position is irrelevant, all points should be projected to the same plane, normal to gravity vector.<br>
00105    *  Thanks Jacqueline K. Libby for helping figure out the free-body diagram/computation upon which this function is based. */
00106   void computePressure(float mass, float massx, float massy, const std::vector<fmat::Column<2> >& contacts, std::vector<float>& pressures);
00108   //! recursively processes contents of @a src, creating a ParameterTransition for each entry to smoothly update corresponding entries in @a dst
00109   void spiderSettings(plist::DictionaryBase& src, plist::DictionaryBase& dst);
00110   //! recursively processes contents of @a src, creating a ParameterTransition for each entry to smoothly update corresponding entries in @a dst
00111   void spiderSettings(plist::ArrayBase& src, plist::ArrayBase& dst);
00113   //! 
00114   class ParameterTransition : plist::PrimitiveListener {
00115   public:
00116     //! constructor, specify source to be monitored, destination to be kept in sync, a collection to be updated when transitions are in progress, and desired duration of transitions
00117     ParameterTransition(const plist::PrimitiveBase& srcVal, plist::PrimitiveBase& dstVal, std::set<ParameterTransition*>& activeTrans, unsigned int dur) : src(srcVal), dst(dstVal), active(activeTrans), curd(0), cura(0),
00118     startTime(), duration(dur), lastUpdate(), decelerate(false)
00119     {
00120       dst=src;
00121       src.addPrimitiveListener(this);
00122     }
00123     //! destructor
00124     ~ParameterTransition() { src.removePrimitiveListener(this); }
00125     virtual void plistValueChanged(const plist::PrimitiveBase& /*pl*/);
00126     bool update(unsigned int curTime);
00128     const plist::PrimitiveBase& src; //!< the parameter which is being monitored for changes (a member of the superclass of XWalkMC)
00129     plist::PrimitiveBase& dst; //!< the parameter which is updated with smoothly transitioning values based on #src
00130     std::set<ParameterTransition*>& active; //!< collection which ParameterTransition will insert itself when src has been changed and transition is in progress
00131     float curd; //!< current velocity of parameter (d for derivative)
00132     float cura; //!< current acceleration of parameter
00133     unsigned int startTime; //!< time at which src was changed
00134     unsigned int duration; //!< how much time to use in transitioning (#cura is then based on difference between #src and #dst, and this)
00135     unsigned int lastUpdate; //!< time of last call to update() or reset()
00136     bool decelerate; //!< set to true if in the deceleration phase of transitioning
00137   };
00140   bool dirty; //!< true if update required
00141   fmat::Column<2> targetVel; //!< the requested xy velocity of the body (ignoring parameterized body motion, like sway or surge), millimeters per second
00142   float targetAngVel; //!< the requested angular velocity of the body, radians per second
00143   fmat::Column<2> targetDisp; //!< the requesed xy displacement of the body, in millimeters
00144   float targetAngDisp; //!< the requested angular displacement of the body in radians
00145   bool velocityMode; //!< True if we just want to maintain a velocity; false if we're trying to achieve a displacement
00146   unsigned int displacementTime; //!< time in msecs since setTargetDisplacement called
00147   bool plantingLegs; //!< True if we've finished a displacement and are waiting for all legs to come to ground before posting a status event
00148   bool initialPlacement; //!< set to true when legs are in an unknown configuration (e.g. following start())
00149   XWalkParameters p; //!< current parameter values, subject to smoothed transition from those as members of the superclass (see ParameterTransition)
00150   std::set<ParameterTransition*> transitions; //!< full collection of parameter listeners
00151   std::set<ParameterTransition*> active; //!< collection of those listeners which are actively transitioning their corresponding parameters in #p
00152   float startTime; //!< must be float (not unsigned int) because value can go negative: phase offset of "walk time" from system get_time() (milliseconds)
00153   float period; //!< the time between leg lifts (milliseconds), calculated value to yield target speed (#targetVel)
00154   float globPhase; //!< the current phase within the walk, in turn controls phase of individual legs via updateLegPhase()
00155   fmat::Column<2> rotationCenter; //!< if the target angular velocity (#targetAngVel) is producing a reasonable curvature vs. targetVel, this is the point about which the body is arcing
00156   DriverMessaging::FixedPoints contactMsg; //!< list of current contact points, for better Mirage simulation
00158   //! contains cached information about each leg's stride
00159   struct LegState {
00160     //! constructor
00161     LegState() : inAir(false), onGround(false), initialHeight(), support(1), reorder(-1U), neutralPos(), liftPos(), downPos() {}
00163     //! True if the leg is currently in the return flight phase, false if its xy motion is linked to the ground.
00164     /*! Note that if this is false, the leg may be in the process of raising or lowering to the ground, but not actually providing support */
00165     bool inAir;
00166     //! True if the leg is currently providing support on the ground, false if in the air (including raise or lower)
00167     bool onGround;
00168     float initialHeight; //!< during initial placement, stores the height of the leg when start() was called
00169     float support; //!< at the beginning and end of support phase, leg is lifted... this indicates how much lift has occurred/remains (range 0: in air, to 1: on ground)
00170     unsigned int reorder; //!< if a valid leg index (i.e. not -1U), indicates the index of the leg from which the phase offset parameter (XWalkParameters::LegParameters::flightPhase) should be used
00171     fmat::Column<2> neutralPos; //!< indicates the mid-stride foot position
00172     fmat::Column<2> liftPos; //!< indicates the position the leg was lifted from during flight, or the last target point during support
00173     fmat::Column<2> downPos; //!< indicates the position the leg will be placed following flight, or would have been placed to maintain continuity if target velocities change (see resetLegState())
00174   };
00175   LegState legStates[NumLegs]; //!< storage of cached stride information for each leg
00177   static KinematicJoint* kine;
00178   static KinematicJoint* childMap[NumReferenceFrames];
00179 };
00181 /*! @file
00182  * @brief Defines XWalkMC, which DESCRIPTION
00183  * @author Ethan Tira-Thompson (ejt) (Creator)
00184  */
00186 #endif

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