Homepage Demos Overview Downloads Tutorials Reference
Credits

WalkMC Class Reference

#include <WalkMC.h>

Inheritance diagram for WalkMC:

Inheritance graph
[legend]
List of all members.

Detailed Description

A nice walking class from Carnegie Mellon University's 2001 Robosoccer team, modified to fit this framework, see their license.

Moves the feet through a looping path in order to walk - default parameters use a walk low to the ground so you don't walk over the ball.

There are around 50 parameters which control the walk - these are loaded from a file and can modify almost every aspect of the the gait. It's a binary file format, I recommend using our Walk Edit menu to edit the parameters in real time and get immediate feedback. It's a tricky job to find a good set of parameters.

And then, once you have it walking, there's a whole different problem of actually moving at the speed that's requested. That's what the calibration parameters do - map the requested target speed to the command to pass to the engine so the resulting motion will hopefully match what you asked for.

You'll probably want to take a look at the setTargetVelocity() function to control the direction of the walk.

This class is in some dire need of some cleanup - we (Tekkotsu) didn't write it, never really bothered to get a deep understanding of it, but have none the less hacked around and added stuff on top of it. So pardon the mess, unless you're feeling ambitious to write your own ;)

This portion of the code falls under CMPack's license:

  =========================================================================
    CMPack'02 Source Code Release for OPEN-R SDK v1.0
    Copyright (C) 2002 Multirobot Lab [Project Head: Manuela Veloso]
    School of Computer Science, Carnegie Mellon University
  -------------------------------------------------------------------------
    This software is distributed under the GNU General Public License,
    version 2.  If you do not have a copy of this licence, visit
    www.gnu.org, or write: Free Software Foundation, 59 Temple Place,
    Suite 330 Boston, MA 02111-1307 USA.  This program is distributed
    in the hope that it will be useful, but WITHOUT ANY WARRANTY,
    including MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  -------------------------------------------------------------------------
    Additionally licensed to Sony Corporation under the following terms:

    This software is provided by the copyright holders AS IS and any
    express or implied warranties, including, but not limited to, the
    implied warranties of merchantability and fitness for a particular
    purpose are disclaimed.  In no event shall authors be liable for
    any direct, indirect, incidental, special, exemplary, or consequential
    damages (including, but not limited to, procurement of substitute
    goods or services; loss of use, data, or profits; or business
    interruption) however caused and on any theory of liability, whether
    in contract, strict liability, or tort (including negligence or
    otherwise) arising in any way out of the use of this software, even if
    advised of the possibility of such damage.
  =========================================================================

Bug:
the legs try (briefly) to straighten out when first starting to move

Definition at line 73 of file WalkMC.h.

Public Types

typedef SplinePath< vector3d,
double > 
splinepath
 for convenience

typedef HermiteSplineSegment<
vector3d, double > 
spline
 for convenience


Public Member Functions

 WalkMC (const char *pfile=NULL)
 constructor

virtual void DoStart ()
 sends an activate LocomotionEvent

virtual void DoStop ()
 sends a deactivate LocomotionEvent

virtual int updateOutputs ()
 calculates current positions of the paws

virtual int isDirty ()
 Returns true if we are walking. This modified isDirty allows the AIBO to slow down to a stop rather than stopping immediately.

virtual int isAlive ()
 always true - never autoprunes

virtual unsigned int getBinSize () const
 calculates space needed to save - if you can't precisely add up the size, overestimate and things will still work.

virtual unsigned int LoadBuffer (const char buf[], unsigned int len)
 Load from a saved buffer.

virtual unsigned int SaveBuffer (char buf[], unsigned int len) const
 Save to a given buffer.

void setTargetVelocity (double dx, double dy, double da)
 set the direction to walk - can specify x (forward), y (left), and angular (counterclockwise) velocities

const vector3d & getTargetVelocity ()
 returns current velocity we're trying to go

const vector3d & getCurVelocity () const
 returns the velocity we're actually moving (subject to clipping at max_accel_xya), doesn't reflect value of getPaused()...

unsigned int getStartTravelTime ()
 returns the time at which we started traveling along the current vector

unsigned int getTravelTime ()
 returns the amount of time we've been traveling along the current vector

void setPaused (bool p)
 if set to true, will stop moving

bool getPaused () const
 if is true, we aren't moving

void setHeight (double h)
 sets WalkParam::body_height of wp

double getHeight ()
 gets WalkParam::body_height of wp

void setAngle (double a)
 sets WalkParam::body_angle of wp

double getAngle ()
 gets WalkParam::body_angle of wp

void setHop (double h)
 sets WalkParam::hop of wp

double getHop ()
 gets WalkParam::hop of wp

void setSway (double h)
 sets WalkParam::sway of wp

double getSway ()
 gets WalkParam::sway of wp

void setPeriod (long p)
 sets WalkParam::period of wp

long getPeriod ()
 gets WalkParam::period of wp

void setSlowMo (float p)
 sets slowmo

float * getSlowMo ()
 gets slowmo

WalkParamgetWP ()
CalibrationParamgetCP ()
void resetLegPos ()
 takes current leg positions from WorldState and tries to match the point in the cycle most like it


Static Public Attributes

const float MAX_DX = 180
 ==180 mm/sec

const float MAX_DY = 140
 ==140 mm/sec

const float MAX_DA = 1.8
 ==1.8 rad/sec

const vector3d max_accel_xya
 maximum acceleration of x, y, and a velocity


Protected Member Functions

void init (const char *pfile)
 does some setup stuff, calls LoadFile(pfile)


Static Protected Member Functions

void applyCalibration (const float mat[3][11], const vector3d &in, vector3d &out)
 converts in to calibration parameters and multiplies through the calibration matrix


Protected Attributes

OutputCmd cmds [NumOutputs][NumFrames]
 holds current joint commands

bool isPaused
 true if we are paused

WalkParam wp
 current walking parameters (note that it's not static - different WalkMC's can have different setting, handy...

CalibrationParam cp
 calibration parameters for current walk.

LegWalkState legw [NumLegs]
 current state of each leg

vector3d legpos [NumLegs]
 current position of each leg

splinepath body_loc
 the path the body goes through while walking (?)

splinepath body_angle
 the path the body goes through while walking (?)

vector3d pos_delta
 how much we've moved

double angle_delta
 how much we've turned

unsigned int travelTime
 the time of the last call to setTargetVelocity - handy to check the time we've been traveling current vector

int time
 time of last call to updateJointCmds() (scaled by slowmo)

int TimeStep
 time to pretend passes between each call to updateJointCmds() - usually RobotInfo::FrameTime

float slowmo
 scales time values to make the walk move in slow motion for analysis (or fast forward)

int CycleOffset
int TimeOffset
bool NewCycleOffset
vector3d vel_xya
 the current velocity we're moving

vector3d target_vel_xya
 the velocity that was requested

vector3d last_target_vel_xya
 the velocity that was last sent to motion


Member Typedef Documentation

typedef HermiteSplineSegment<vector3d,double> WalkMC::spline
 

for convenience

Definition at line 76 of file WalkMC.h.

typedef SplinePath<vector3d,double> WalkMC::splinepath
 

for convenience

Definition at line 75 of file WalkMC.h.


Constructor & Destructor Documentation

WalkMC::WalkMC const char *  pfile = NULL  ) 
 

constructor

Definition at line 60 of file WalkMC.cc.


Member Function Documentation

void WalkMC::applyCalibration const float  mat[3][11],
const vector3d &  in,
vector3d &  out
[static, protected]
 

converts in to calibration parameters and multiplies through the calibration matrix

Definition at line 425 of file WalkMC.cc.

Referenced by updateOutputs().

void WalkMC::DoStart  )  [virtual]
 

sends an activate LocomotionEvent

Reimplemented from MotionCommand.

Definition at line 89 of file WalkMC.cc.

void WalkMC::DoStop  )  [virtual]
 

sends a deactivate LocomotionEvent

Reimplemented from MotionCommand.

Definition at line 97 of file WalkMC.cc.

double WalkMC::getAngle  )  [inline]
 

gets WalkParam::body_angle of wp

Definition at line 161 of file WalkMC.h.

unsigned int WalkMC::getBinSize  )  const [virtual]
 

calculates space needed to save - if you can't precisely add up the size, overestimate and things will still work.

Returns:
number of bytes read/written, 0 if error (or empty)

Implements LoadSave.

Definition at line 142 of file WalkMC.cc.

CalibrationParam& WalkMC::getCP  )  [inline]
 

Definition at line 172 of file WalkMC.h.

Referenced by StartupBehavior::SetupWalkEdit().

const vector3d& WalkMC::getCurVelocity  )  const [inline]
 

returns the velocity we're actually moving (subject to clipping at max_accel_xya), doesn't reflect value of getPaused()...

Definition at line 150 of file WalkMC.h.

double WalkMC::getHeight  )  [inline]
 

gets WalkParam::body_height of wp

Definition at line 159 of file WalkMC.h.

double WalkMC::getHop  )  [inline]
 

gets WalkParam::hop of wp

Definition at line 163 of file WalkMC.h.

bool WalkMC::getPaused  )  const [inline]
 

if is true, we aren't moving

Definition at line 157 of file WalkMC.h.

long WalkMC::getPeriod  )  [inline]
 

gets WalkParam::period of wp

Definition at line 167 of file WalkMC.h.

float* WalkMC::getSlowMo  )  [inline]
 

gets slowmo

Definition at line 169 of file WalkMC.h.

Referenced by StartupBehavior::SetupWalkEdit().

unsigned int WalkMC::getStartTravelTime  )  [inline]
 

returns the time at which we started traveling along the current vector

Definition at line 152 of file WalkMC.h.

Referenced by getTravelTime().

double WalkMC::getSway  )  [inline]
 

gets WalkParam::sway of wp

Definition at line 165 of file WalkMC.h.

const vector3d& WalkMC::getTargetVelocity  )  [inline]
 

returns current velocity we're trying to go

Definition at line 148 of file WalkMC.h.

unsigned int WalkMC::getTravelTime  )  [inline]
 

returns the amount of time we've been traveling along the current vector

Definition at line 154 of file WalkMC.h.

Referenced by updateOutputs().

WalkParam& WalkMC::getWP  )  [inline]
 

Definition at line 171 of file WalkMC.h.

Referenced by StartupBehavior::SetupWalkEdit().

void WalkMC::init const char *  pfile  )  [protected]
 

does some setup stuff, calls LoadFile(pfile)

Definition at line 106 of file WalkMC.cc.

Referenced by WalkMC().

virtual int WalkMC::isAlive  )  [inline, virtual]
 

always true - never autoprunes

Implements MotionCommand.

Definition at line 139 of file WalkMC.h.

int WalkMC::isDirty  )  [virtual]
 

Returns true if we are walking. This modified isDirty allows the AIBO to slow down to a stop rather than stopping immediately.

Implements MotionCommand.

Definition at line 129 of file WalkMC.cc.

Referenced by updateOutputs().

unsigned int WalkMC::LoadBuffer const char  buf[],
unsigned int  len
[virtual]
 

Load from a saved buffer.

Parameters:
buf pointer to the memory where you should begin loading
len length of buf available (this isn't all yours, might be more stuff saved after yours)
Returns:
the number of bytes actually used

Implements LoadSave.

Definition at line 150 of file WalkMC.cc.

void WalkMC::resetLegPos  ) 
 

takes current leg positions from WorldState and tries to match the point in the cycle most like it

Definition at line 393 of file WalkMC.cc.

Referenced by init(), and updateOutputs().

unsigned int WalkMC::SaveBuffer char  buf[],
unsigned int  len
const [virtual]
 

Save to a given buffer.

Parameters:
buf pointer to the memory where you should begin writing
len length of buf available. (this isn't all yours, constrain yourself to what you returned in getBinSize() )
Returns:
the number of bytes actually used

Implements LoadSave.

Definition at line 181 of file WalkMC.cc.

void WalkMC::setAngle double  a  )  [inline]
 

sets WalkParam::body_angle of wp

Definition at line 160 of file WalkMC.h.

void WalkMC::setHeight double  h  )  [inline]
 

sets WalkParam::body_height of wp

Definition at line 158 of file WalkMC.h.

void WalkMC::setHop double  h  )  [inline]
 

sets WalkParam::hop of wp

Definition at line 162 of file WalkMC.h.

void WalkMC::setPaused bool  p  )  [inline]
 

if set to true, will stop moving

Definition at line 156 of file WalkMC.h.

void WalkMC::setPeriod long  p  )  [inline]
 

sets WalkParam::period of wp

Definition at line 166 of file WalkMC.h.

void WalkMC::setSlowMo float  p  )  [inline]
 

sets slowmo

Definition at line 168 of file WalkMC.h.

void WalkMC::setSway double  h  )  [inline]
 

sets WalkParam::sway of wp

Definition at line 164 of file WalkMC.h.

void WalkMC::setTargetVelocity double  dx,
double  dy,
double  da
 

set the direction to walk - can specify x (forward), y (left), and angular (counterclockwise) velocities

Definition at line 200 of file WalkMC.cc.

Referenced by WalkToTargetMachine::processEvent(), DriveMeBehavior::processEvent(), and ChaseBallBehavior::processEvent().

int WalkMC::updateOutputs  )  [virtual]
 

calculates current positions of the paws

Implements MotionCommand.

Definition at line 219 of file WalkMC.cc.


Member Data Documentation

double WalkMC::angle_delta [protected]
 

how much we've turned

Definition at line 205 of file WalkMC.h.

Referenced by updateOutputs().

splinepath WalkMC::body_angle [protected]
 

the path the body goes through while walking (?)

Definition at line 202 of file WalkMC.h.

Referenced by init().

splinepath WalkMC::body_loc [protected]
 

the path the body goes through while walking (?)

Definition at line 201 of file WalkMC.h.

Referenced by init().

OutputCmd WalkMC::cmds[NumOutputs][NumFrames] [protected]
 

holds current joint commands

Definition at line 186 of file WalkMC.h.

Referenced by updateOutputs().

CalibrationParam WalkMC::cp [protected]
 

calibration parameters for current walk.

Definition at line 198 of file WalkMC.h.

Referenced by getBinSize(), getCP(), LoadBuffer(), SaveBuffer(), setTargetVelocity(), and updateOutputs().

int WalkMC::CycleOffset [protected]
 

The CycleOffset variable is used to ensure that each time the AIBO starts walking, it starts at the same point in the walk cycle as where it stopped. This measure is intended to decrease the amount of jerking (and hence deviation) that occurs when the AIBO starts walking forward and then suddenly stops.

Definition at line 218 of file WalkMC.h.

Referenced by updateOutputs().

bool WalkMC::isPaused [protected]
 

true if we are paused

Definition at line 195 of file WalkMC.h.

Referenced by getPaused(), isDirty(), and setPaused().

vector3d WalkMC::last_target_vel_xya [protected]
 

the velocity that was last sent to motion

Definition at line 234 of file WalkMC.h.

Referenced by updateOutputs().

vector3d WalkMC::legpos[NumLegs] [protected]
 

current position of each leg

Definition at line 200 of file WalkMC.h.

Referenced by resetLegPos(), and updateOutputs().

LegWalkState WalkMC::legw[NumLegs] [protected]
 

current state of each leg

Definition at line 199 of file WalkMC.h.

Referenced by init(), and updateOutputs().

const vector3d WalkMC::max_accel_xya [static]
 

maximum acceleration of x, y, and a velocity

Referenced by updateOutputs().

const float WalkMC::MAX_DA = 1.8 [static]
 

==1.8 rad/sec

Definition at line 53 of file WalkMC.cc.

Referenced by updateOutputs().

const float WalkMC::MAX_DX = 180 [static]
 

==180 mm/sec

Definition at line 51 of file WalkMC.cc.

Referenced by updateOutputs().

const float WalkMC::MAX_DY = 140 [static]
 

==140 mm/sec

Definition at line 52 of file WalkMC.cc.

Referenced by updateOutputs().

bool WalkMC::NewCycleOffset [protected]
 

Every time we stop, we know we'll have a new CycleOffset, and we'll need to compute a new TimeOffset. This boolean says as much.

Definition at line 229 of file WalkMC.h.

Referenced by updateOutputs().

vector3d WalkMC::pos_delta [protected]
 

how much we've moved

Definition at line 204 of file WalkMC.h.

Referenced by updateOutputs().

float WalkMC::slowmo [protected]
 

scales time values to make the walk move in slow motion for analysis (or fast forward)

Definition at line 210 of file WalkMC.h.

Referenced by getSlowMo(), setSlowMo(), and updateOutputs().

vector3d WalkMC::target_vel_xya [protected]
 

the velocity that was requested

Definition at line 233 of file WalkMC.h.

Referenced by DoStart(), DoStop(), getTargetVelocity(), isDirty(), setTargetVelocity(), and updateOutputs().

int WalkMC::time [protected]
 

time of last call to updateJointCmds() (scaled by slowmo)

Definition at line 208 of file WalkMC.h.

Referenced by updateOutputs().

int WalkMC::TimeOffset [protected]
 

Each CycleOffset corresponds to a different TimeOffset once the robot starts walking again. Consider this example: the robot stops 2/3 of the way through the cycle, then starts again 1/3 of the way through the cycle on the absolute clock. The time offset to advance the clock to the right part of the cycle is 1/3 of a cycle, so we set TimeOffset to 1/3 cycle and add that to every clock value used in the walk code.

Definition at line 226 of file WalkMC.h.

Referenced by updateOutputs().

int WalkMC::TimeStep [protected]
 

time to pretend passes between each call to updateJointCmds() - usually RobotInfo::FrameTime

Definition at line 209 of file WalkMC.h.

Referenced by updateOutputs().

unsigned int WalkMC::travelTime [protected]
 

the time of the last call to setTargetVelocity - handy to check the time we've been traveling current vector

Definition at line 207 of file WalkMC.h.

Referenced by DoStart(), DoStop(), getStartTravelTime(), and updateOutputs().

vector3d WalkMC::vel_xya [protected]
 

the current velocity we're moving

Definition at line 232 of file WalkMC.h.

Referenced by getCurVelocity(), isDirty(), and updateOutputs().

WalkParam WalkMC::wp [protected]
 

current walking parameters (note that it's not static - different WalkMC's can have different setting, handy...

Definition at line 197 of file WalkMC.h.

Referenced by getAngle(), getBinSize(), getHeight(), getHop(), getPeriod(), getSway(), getWP(), init(), LoadBuffer(), resetLegPos(), SaveBuffer(), setAngle(), setHeight(), setHop(), setPeriod(), setSway(), and updateOutputs().


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

Tekkotsu v2.1
Generated Tue Mar 16 23:22:32 2004 by Doxygen 1.3.5