Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

XWalkMC Class Reference

Extreme walk engine handles legged locomotion on hexapods and beyond. More...

#include <XWalkMC.h>

Inheritance diagram for XWalkMC:

Detailed Description

Extreme walk engine handles legged locomotion on hexapods and beyond.

Definition at line 17 of file XWalkMC.h.

List of all members.

Classes

struct  LegState
 contains cached information about each leg's stride More...
class  ParameterTransition

Public Member Functions

 XWalkMC ()
 constructor
const fmat::Column< 2 > & getTargetVelocity () const
 Returns the current x and y velocities in mm/sec.
float getTargetAngVelocity () const
 Returns the current angular velocity in radians/sec.
virtual void getTargetVelocity (float &xvel, float &yvel, float &avel)
 Specify the desired body velocity in x and y (millimeters per second) and angular velocity (radians per second).
virtual void setTargetVelocity (float xvel, float yvel, float avel)
 Specify the desired body velocity in x and y (millimeters per second) and angular velocity (radians per second); does not stop automatically.
virtual void setTargetVelocity (float xvel, float yvel, float avel, float time)
 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.
virtual void setTargetDisplacement (float xdisp, float ydisp, float adisp, float time=0)
 Specify the desired body displacement in x and y (millimeters) and angle (radians), optionally specify time (seconds).
virtual void setTargetDisplacement (float xdisp, float ydisp, float adisp, float xvel, float yvel, float avel)
virtual int updateOutputs ()
 is called once per update cycle, can do any processing you need to change your priorities or set output commands on the MotionManager
void setDirty ()
virtual int isDirty ()
 not used by MotionManager at the moment, but could be used to reduce recomputation, and you may find it useful
virtual int isAlive ()
 used to prune "dead" motions from the MotionManager
virtual void start ()
 called after this is added to MotionManager; don't override this, use doStart instead
virtual void stop ()
 called after this is removed from MotionManager; don't override this, use doStop instead
virtual void zeroVelocities ()
 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.

Protected Member Functions

void updateNeutralPos (unsigned int curTime)
 recomputes each leg's LegState::neutralPos based on current values in p
bool resetPeriod (float time, float speed)
void resetLegState (unsigned int leg, float phase, const fmat::Column< 3 > &curPos, bool inAir, float speed)
 recomputes LegState::downPos and LegState::liftPos based on specified state variables, such that the direction of stride rotates about the current position
void updateOutputsInitial (unsigned int curTime, const fmat::Column< 3 > &ground, const fmat::Column< 3 > &gravity, IKSolver::Point tgts[])
void updateOutputsWalking (float dt, const fmat::Column< 3 > &ground, const fmat::Column< 3 > &gravity, float speed, IKSolver::Point tgts[])
void sendLoadPredictions (IKSolver::Point tgts[])
void computePhase (float time)
 updates globPhase based on specified stride time (relative to startTime)
void computeLegPhase (unsigned int leg, bool &inAir, float &phase)
 computes current leg state based on globPhase
void computeCurrentPosition (unsigned int leg, bool inAir, float speed, float phase, fmat::Column< 3 > &tgt)
 computes the leg position (in xy plane only) based on leg state
void computeCurrentBodyOffset (float *legPhases, float speed, float &offx, float &offy, float &offa)
 computes body position based on settings in p, which should then be added to leg positions before projecting onto ground plane
void solveIK (unsigned int leg, const IKSolver::Point &tgt)
 solves inverse kinematics and send affected output values to motion manager
void computePressure (float mass, float massx, float massy, const std::vector< fmat::Column< 2 > > &contacts, std::vector< float > &pressures)
void spiderSettings (plist::DictionaryBase &src, plist::DictionaryBase &dst)
 recursively processes contents of src, creating a ParameterTransition for each entry to smoothly update corresponding entries in dst
void spiderSettings (plist::ArrayBase &src, plist::ArrayBase &dst)
 recursively processes contents of src, creating a ParameterTransition for each entry to smoothly update corresponding entries in dst

Protected Attributes

bool dirty
 true if update required
fmat::Column< 2 > targetVel
 the requested xy velocity of the body (ignoring parameterized body motion, like sway or surge), millimeters per second
float targetAngVel
 the requested angular velocity of the body, radians per second
fmat::Column< 2 > targetDisp
 the requesed xy displacement of the body, in millimeters
float targetAngDisp
 the requested angular displacement of the body in radians
bool velocityMode
 True if we just want to maintain a velocity; false if we're trying to achieve a displacement.
unsigned int displacementTime
 time in msecs since setTargetDisplacement called
bool plantingLegs
 True if we've finished a displacement and are waiting for all legs to come to ground before posting a status event.
bool initialPlacement
 set to true when legs are in an unknown configuration (e.g. following start())
XWalkParameters p
 current parameter values, subject to smoothed transition from those as members of the superclass (see ParameterTransition)
std::set< ParameterTransition * > transitions
 full collection of parameter listeners
std::set< ParameterTransition * > active
 collection of those listeners which are actively transitioning their corresponding parameters in p
float startTime
 must be float (not unsigned int) because value can go negative: phase offset of "walk time" from system get_time() (milliseconds)
float period
 the time between leg lifts (milliseconds), calculated value to yield target speed (targetVel)
float globPhase
 the current phase within the walk, in turn controls phase of individual legs via updateLegPhase()
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
DriverMessaging::FixedPoints contactMsg
 list of current contact points, for better Mirage simulation
LegState legStates [NumLegs]
 storage of cached stride information for each leg

Static Protected Attributes

static KinematicJointkine = NULL
static KinematicJointchildMap [NumReferenceFrames]

Constructor & Destructor Documentation

XWalkMC::XWalkMC (  ) 

constructor

Definition at line 17 of file XWalkMC.cc.


Member Function Documentation

void XWalkMC::computeCurrentBodyOffset ( float legPhases,
float  speed,
float offx,
float offy,
float offa 
) [protected]

computes body position based on settings in p, which should then be added to leg positions before projecting onto ground plane

Definition at line 935 of file XWalkMC.cc.

Referenced by setTargetVelocity(), updateOutputsInitial(), and updateOutputsWalking().

void XWalkMC::computeCurrentPosition ( unsigned int  leg,
bool  inAir,
float  speed,
float  phase,
fmat::Column< 3 > &  tgt 
) [protected]

computes the leg position (in xy plane only) based on leg state

Definition at line 914 of file XWalkMC.cc.

Referenced by resetLegState(), setTargetVelocity(), and updateOutputsWalking().

void XWalkMC::computeLegPhase ( unsigned int  leg,
bool &  inAir,
float phase 
) [protected]

computes current leg state based on globPhase

Definition at line 899 of file XWalkMC.cc.

Referenced by setTargetVelocity(), updateOutputsInitial(), and updateOutputsWalking().

void XWalkMC::computePhase ( float  time  )  [protected]

updates globPhase based on specified stride time (relative to startTime)

Definition at line 892 of file XWalkMC.cc.

Referenced by resetPeriod(), setTargetVelocity(), start(), updateOutputsInitial(), and updateOutputsWalking().

void XWalkMC::computePressure ( float  mass,
float  massx,
float  massy,
const std::vector< fmat::Column< 2 > > &  contacts,
std::vector< float > &  pressures 
) [protected]
Parameters:
mass being supported in kilograms
massx center of mass along x axis, in millimeters
massy center of mass along y axis, in millimeters
contacts position of each support contact, in millimeters
[out] pressures will be resized and overwritten with pressure at each corresponding point in contacts, in newtons

Note the z position is irrelevant, all points should be projected to the same plane, normal to gravity vector.
Thanks Jacqueline K. Libby for helping figure out the free-body diagram/computation upon which this function is based.

Definition at line 1027 of file XWalkMC.cc.

Referenced by sendLoadPredictions().

float XWalkMC::getTargetAngVelocity (  )  const

Returns the current angular velocity in radians/sec.

Definition at line 26 of file XWalkMC.h.

virtual void XWalkMC::getTargetVelocity ( float xvel,
float yvel,
float avel 
) [virtual]

Specify the desired body velocity in x and y (millimeters per second) and angular velocity (radians per second).

Definition at line 29 of file XWalkMC.h.

const fmat::Column<2>& XWalkMC::getTargetVelocity (  )  const

Returns the current x and y velocities in mm/sec.

Definition at line 23 of file XWalkMC.h.

virtual int XWalkMC::isAlive (  )  [virtual]

used to prune "dead" motions from the MotionManager

note that a motion could be "paused" or inactive and therefore not dirty, but still alive, biding its time to "strike" ;)

Returns:
zero if the motion is still processing, non-zero otherwise

Implements MotionCommand.

Definition at line 58 of file XWalkMC.h.

virtual int XWalkMC::isDirty (  )  [virtual]

not used by MotionManager at the moment, but could be used to reduce recomputation, and you may find it useful

Returns:
zero if none of the commands have changed since last getJointCmd(), else non-zero

Implements MotionCommand.

Definition at line 51 of file XWalkMC.h.

Referenced by computePhase(), and updateOutputs().

void XWalkMC::resetLegState ( unsigned int  leg,
float  phase,
const fmat::Column< 3 > &  curPos,
bool  inAir,
float  speed 
) [protected]

recomputes LegState::downPos and LegState::liftPos based on specified state variables, such that the direction of stride rotates about the current position

Definition at line 484 of file XWalkMC.cc.

Referenced by start(), and updateOutputsInitial().

bool XWalkMC::resetPeriod ( float  time,
float  speed 
) [protected]

Definition at line 412 of file XWalkMC.cc.

Referenced by setTargetVelocity().

void XWalkMC::sendLoadPredictions ( IKSolver::Point  tgts[]  )  [protected]

Definition at line 808 of file XWalkMC.cc.

Referenced by updateOutputs().

void XWalkMC::setDirty (  ) 

Reimplemented in WaypointWalkMC.

Definition at line 49 of file XWalkMC.h.

virtual void XWalkMC::setTargetDisplacement ( float  xdisp,
float  ydisp,
float  adisp,
float  xvel,
float  yvel,
float  avel 
) [virtual]

Definition at line 45 of file XWalkMC.h.

void XWalkMC::setTargetDisplacement ( float  xdisp,
float  ydisp,
float  adisp,
float  time = 0 
) [virtual]

Specify the desired body displacement in x and y (millimeters) and angle (radians), optionally specify time (seconds).

Corresponding velocity will be limited to max velocity, so setting time=0 implies max speed

Definition at line 221 of file XWalkMC.cc.

void XWalkMC::setTargetVelocity ( float  xvel,
float  yvel,
float  avel,
float  time 
) [virtual]

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.

Definition at line 208 of file XWalkMC.cc.

void XWalkMC::setTargetVelocity ( float  xvel,
float  yvel,
float  avel 
) [virtual]

Specify the desired body velocity in x and y (millimeters per second) and angular velocity (radians per second); does not stop automatically.

Definition at line 35 of file XWalkMC.cc.

Referenced by setTargetDisplacement(), setTargetVelocity(), updateOutputs(), XWalkMC(), and zeroVelocities().

void XWalkMC::solveIK ( unsigned int  leg,
const IKSolver::Point tgt 
) [protected]

solves inverse kinematics and send affected output values to motion manager

Definition at line 951 of file XWalkMC.cc.

Referenced by updateOutputsInitial(), and updateOutputsWalking().

void XWalkMC::spiderSettings ( plist::ArrayBase src,
plist::ArrayBase dst 
) [protected]

recursively processes contents of src, creating a ParameterTransition for each entry to smoothly update corresponding entries in dst

Definition at line 1111 of file XWalkMC.cc.

void XWalkMC::spiderSettings ( plist::DictionaryBase src,
plist::DictionaryBase dst 
) [protected]

recursively processes contents of src, creating a ParameterTransition for each entry to smoothly update corresponding entries in dst

Definition at line 1097 of file XWalkMC.cc.

Referenced by spiderSettings(), and XWalkMC().

void XWalkMC::start (  )  [virtual]

called after this is added to MotionManager; don't override this, use doStart instead

Reimplemented from MotionCommand.

Definition at line 305 of file XWalkMC.cc.

void XWalkMC::stop (  )  [virtual]

called after this is removed from MotionManager; don't override this, use doStop instead

Reimplemented from MotionCommand.

Definition at line 328 of file XWalkMC.cc.

void XWalkMC::updateNeutralPos ( unsigned int  curTime  )  [protected]

recomputes each leg's LegState::neutralPos based on current values in p

Definition at line 388 of file XWalkMC.cc.

Referenced by updateOutputs(), and XWalkMC().

int XWalkMC::updateOutputs (  )  [virtual]

is called once per update cycle, can do any processing you need to change your priorities or set output commands on the MotionManager

Returns:
zero if no changes were made, non-zero otherwise
See also:
RobotInfo::NumFrames
RobotInfo::FrameTime

Implements MotionCommand.

Reimplemented in WaypointWalkMC.

Definition at line 240 of file XWalkMC.cc.

void XWalkMC::updateOutputsInitial ( unsigned int  curTime,
const fmat::Column< 3 > &  ground,
const fmat::Column< 3 > &  gravity,
IKSolver::Point  tgts[] 
) [protected]

Definition at line 515 of file XWalkMC.cc.

Referenced by updateOutputs().

void XWalkMC::updateOutputsWalking ( float  dt,
const fmat::Column< 3 > &  ground,
const fmat::Column< 3 > &  gravity,
float  speed,
IKSolver::Point  tgts[] 
) [protected]

Definition at line 657 of file XWalkMC.cc.

Referenced by updateOutputs().

void XWalkMC::zeroVelocities (  )  [virtual]

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.

Definition at line 350 of file XWalkMC.cc.


Member Data Documentation

std::set<ParameterTransition*> XWalkMC::active [protected]

collection of those listeners which are actively transitioning their corresponding parameters in p

Definition at line 151 of file XWalkMC.h.

Referenced by isDirty(), spiderSettings(), updateNeutralPos(), and updateOutputs().

list of current contact points, for better Mirage simulation

Definition at line 156 of file XWalkMC.h.

Referenced by stop(), updateOutputsInitial(), and updateOutputsWalking().

bool XWalkMC::dirty [protected]

true if update required

Definition at line 140 of file XWalkMC.h.

Referenced by isDirty(), and setDirty().

unsigned int XWalkMC::displacementTime [protected]

time in msecs since setTargetDisplacement called

Definition at line 146 of file XWalkMC.h.

Referenced by setTargetDisplacement(), setTargetVelocity(), updateOutputs(), updateOutputsInitial(), and zeroVelocities().

the current phase within the walk, in turn controls phase of individual legs via updateLegPhase()

Definition at line 154 of file XWalkMC.h.

Referenced by computeCurrentBodyOffset(), computeLegPhase(), computePhase(), resetPeriod(), updateOutputsInitial(), and updateOutputsWalking().

bool XWalkMC::initialPlacement [protected]

set to true when legs are in an unknown configuration (e.g. following start())

Definition at line 148 of file XWalkMC.h.

Referenced by resetPeriod(), setTargetVelocity(), start(), updateOutputs(), and updateOutputsInitial().

KinematicJoint * XWalkMC::kine = NULL [static, protected]

Definition at line 177 of file XWalkMC.h.

Referenced by start(), updateOutputs(), updateOutputsInitial(), and XWalkMC().

current parameter values, subject to smoothed transition from those as members of the superclass (see ParameterTransition)

Definition at line 149 of file XWalkMC.h.

Referenced by computeCurrentBodyOffset(), computeLegPhase(), setTargetVelocity(), updateNeutralPos(), updateOutputsInitial(), updateOutputsWalking(), and XWalkMC().

float XWalkMC::period [protected]

the time between leg lifts (milliseconds), calculated value to yield target speed (targetVel)

Definition at line 153 of file XWalkMC.h.

Referenced by computeCurrentPosition(), computeLegPhase(), computePhase(), resetPeriod(), setTargetVelocity(), start(), updateOutputsInitial(), and updateOutputsWalking().

bool XWalkMC::plantingLegs [protected]

True if we've finished a displacement and are waiting for all legs to come to ground before posting a status event.

Definition at line 147 of file XWalkMC.h.

Referenced by setTargetDisplacement(), setTargetVelocity(), and updateOutputs().

if the target angular velocity (targetAngVel) is producing a reasonable curvature vs. targetVel, this is the point about which the body is arcing

Definition at line 155 of file XWalkMC.h.

Referenced by computeCurrentPosition(), resetPeriod(), and setTargetVelocity().

must be float (not unsigned int) because value can go negative: phase offset of "walk time" from system get_time() (milliseconds)

Definition at line 152 of file XWalkMC.h.

Referenced by resetPeriod(), setTargetVelocity(), start(), updateOutputs(), updateOutputsInitial(), and XWalkMC().

the requested angular displacement of the body in radians

Definition at line 144 of file XWalkMC.h.

Referenced by setTargetDisplacement(), setTargetVelocity(), and updateOutputs().

the requested angular velocity of the body, radians per second

Definition at line 142 of file XWalkMC.h.

Referenced by computeCurrentPosition(), getTargetAngVelocity(), getTargetVelocity(), isDirty(), resetPeriod(), setTargetVelocity(), updateOutputs(), updateOutputsInitial(), updateOutputsWalking(), and XWalkMC().

the requesed xy displacement of the body, in millimeters

Definition at line 143 of file XWalkMC.h.

Referenced by setTargetDisplacement(), setTargetVelocity(), and updateOutputs().

the requested xy velocity of the body (ignoring parameterized body motion, like sway or surge), millimeters per second

Definition at line 141 of file XWalkMC.h.

Referenced by computeCurrentBodyOffset(), computeCurrentPosition(), getTargetVelocity(), isDirty(), resetPeriod(), setTargetVelocity(), updateOutputs(), updateOutputsInitial(), and XWalkMC().

std::set<ParameterTransition*> XWalkMC::transitions [protected]

full collection of parameter listeners

Definition at line 150 of file XWalkMC.h.

Referenced by spiderSettings().

bool XWalkMC::velocityMode [protected]

True if we just want to maintain a velocity; false if we're trying to achieve a displacement.

Definition at line 145 of file XWalkMC.h.

Referenced by setTargetDisplacement(), setTargetVelocity(), and updateOutputs().


The documentation for this class was generated from the following files:

Tekkotsu v5.1CVS
Generated Mon May 9 04:59:19 2016 by Doxygen 1.6.3