Tekkotsu Homepage | Demos | Overview | Downloads | Dev. Resources | Reference | Credits |
XWalkMC.hGo to the documentation of this file.00001 //-*-c++-*- 00002 #ifndef INCLUDED_XWalkMC_h_ 00003 #define INCLUDED_XWalkMC_h_ 00004 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> 00015 00016 //! Extreme walk engine handles legged locomotion on hexapods and beyond 00017 class XWalkMC : public MotionCommand, public XWalkParameters { 00018 public: 00019 //! constructor 00020 XWalkMC(); 00021 00022 //! Returns the current x and y velocities in mm/sec 00023 const fmat::Column<2>& getTargetVelocity() const { return targetVel; }; 00024 00025 //! Returns the current angular velocity in radians/sec 00026 float getTargetAngVelocity() const { return targetAngVel; } 00027 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 } 00034 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); 00037 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); 00040 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); 00044 00045 virtual void setTargetDisplacement(float xdisp, float ydisp, float adisp, float xvel, float yvel, float avel) {} 00046 00047 virtual int updateOutputs(); 00048 00049 void setDirty() { dirty = true; } 00050 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 } 00057 00058 virtual int isAlive() { return true; } 00059 00060 virtual void start(); 00061 00062 virtual void stop(); 00063 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(); 00066 00067 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 00073 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); 00081 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[]); 00085 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); 00096 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); 00107 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); 00112 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); 00127 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 }; 00138 00139 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 00157 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() {} 00162 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 00176 00177 static KinematicJoint* kine; 00178 static KinematicJoint* childMap[NumReferenceFrames]; 00179 }; 00180 00181 /*! @file 00182 * @brief Defines XWalkMC, which DESCRIPTION 00183 * @author Ethan Tira-Thompson (ejt) (Creator) 00184 */ 00185 00186 #endif |
Tekkotsu v5.1CVS |
Generated Mon May 9 04:58:53 2016 by Doxygen 1.6.3 |