Main Page | Namespace List | Class Hierarchy | Alphabetical List | Class List | File List | Namespace Members | Class Members | File Members | Related Pages

GT2004WalkingEngine Class Reference

Walking engine based on calculation of rectangular foot movement and inverse kinematics. More...

#include <GT2004WalkingEngine.h>

Inheritance diagram for GT2004WalkingEngine:

Inheritance graph
[legend]
Collaboration diagram for GT2004WalkingEngine:

Collaboration graph
[legend]
List of all members.

parameter set interpolation

void initParametersInterpolation (int changeSteps)
 Initialize interpolation of WalkingParameterSets.

void nextParametersInterpolation (bool walk)
 Calculate next step in parameterset interpolation and increase currentStep if walk is true.

GT2004ParameterscurrentParameters
 Points to parameter set currently in use.

GT2004Parameters fixedParameters
 Fixed parameter set for InvKin compatibility.

GT2004ParametersparametersToChange
 Pointer to the parameter set that shall be changed by interpolation.

GT2004Parameters nextParameters
 Next parameters of this walk, target of current interpolation.

GT2004Parameters lastParameters
 Last parameters of this walk, origin of current interpolation.

int paramInterpolCount
 Counts parameter set interpolation steps.

int paramInterpolLength
 Stores the length of the current parameter set interpolation.

double currentStepPercentage
 Stores precise version of currentStep for all kinds of interpolation.


Public Member Functions

 GT2004WalkingEngine (const WalkingEngineInterfaces &interfaces)
 Constructor.

 ~GT2004WalkingEngine ()
 Destructor.

virtual bool executeParameterized (JointData &jointData, const WalkRequest &walkRequest, double positionInWalkingCycle)
 Executes the engine.

virtual bool handleMessage (InMessage &message)
 Called from a MessageQueue to distribute messages.

void setNextParameters (int steps)
 find correct table entry to update (in turnOnly and withWalk): find the entry most similar to nextParameters, copy the old value to lastParameters, set parametersToChange and start interpolation


Private Member Functions

void init ()
 initialise fixed parameters

void calculateParams ()
 calculate all engine wide parameters like neckHeight from currentParameters this is essential after every change in currentParameters!

void smoothMotionRequest (const Pose2D &request, Pose2D &currentRequest)
 smooth motion request current request is adjusted according to motion request eliminating quick changes

void calculateLegSpeeds ()
 calculates new leg speeds according to current motion request

void calculateRelativeFootPosition (int step, int leg, double &rx, double &ry, double &rz)
 calculate relative foot position for one leg rx is relative to current step size (range -1.0..1.0) ry is an absolute offset to y foot position rz is relative to step lift parameter (range 0..1.0)

int calculateLegJoints (Kinematics::LegIndex leg, double &j1, double &j2, double &j3, double bodyTilt=0)
 calculate angles for one leg in current step

void calculateData (JointData &j)
 calculate current joint data values

void calculateFootPositions ()
 calculate current foot positions


Private Attributes

GT2004ParametersSet paramSet
 a set of GT2004Parameters optimized for different requests

bool useFixedParameters
 decides whether we use merging of rotationOnly and withWalk or fixedParameters

bool recalcEngineParameters
 true if stuff like legSpeed has to be recalculated because of changed request or parameterset

unsigned long lastParametersFromPackageTimeStamp
double neckHeight
 neck height in mm while walking

double bodyTilt
 body tilt angle

Pose2D currentRequest
 currently executed motion request speeds in mm/s

Pose2D odometry
 odometry resulting from current request speed in mm/tick

fixed gait parameters resulting from merged parameter set
int groundTime [2]
 number of ticks foot is on ground

int liftTime [2]
 number of ticks for lifting foot

int loweringTime [2]
 number of ticks for lowering foot

int airTime [2]
 number of ticks foot is in air

int stepLift [2]
 time index for lifting foot

int stepAir [2]
 time index when foot is fully lifted

int stepLower [2]
 time index for lowering foot

int legPhaseIndex [4]
 leg phases, time indexes for lifting each leg

int firstStep
 time index for first time all feet are on ground

current walk values
Vector2< double > legSpeed [4]
 speed of leg on ground (step size in mm)

Vector3< double > footPos [4]
 foot positions


Detailed Description

Walking engine based on calculation of rectangular foot movement and inverse kinematics.

Author:
Uwe Dueffert

Definition at line 28 of file GT2004WalkingEngine.h.


Constructor & Destructor Documentation

GT2004WalkingEngine::GT2004WalkingEngine const WalkingEngineInterfaces interfaces  ) 
 

Constructor.

Parameters:
interfaces The paramters of the WalkingEngine module.

Definition at line 17 of file GT2004WalkingEngine.cpp.

References init().

Here is the call graph for this function:

GT2004WalkingEngine::~GT2004WalkingEngine  ) 
 

Destructor.

Definition at line 27 of file GT2004WalkingEngine.cpp.


Member Function Documentation

bool GT2004WalkingEngine::executeParameterized JointData jointData,
const WalkRequest walkRequest,
double  positionInWalkingCycle
[virtual]
 

Executes the engine.

Implements WalkingEngine.

Definition at line 449 of file GT2004WalkingEngine.cpp.

References Vector2< double >::abs(), MotionInfo::bodyTilt, RobotCalibration::bodyTiltOffset, calculateData(), calculateLegSpeeds(), Pose2D::conc(), currentRequest, currentStepPercentage, MotionInfo::executedMotionRequest, fixedParameters, RobotConfiguration::getRobotCalibration(), getRobotConfiguration(), GT2004Parameters::index, initParametersInterpolation(), lastParameters, lastParametersFromPackageTimeStamp, GT2004ParametersSet::mergedParameters, MotionInfo::motionIsStable, MotionRequest::motionType, MotionInfo::neckHeight, nextParameters, nextParametersInterpolation(), parametersToChange, paramInterpolCount, paramInterpolLength, paramSet, MotionInfo::positionInWalkCycle, recalcEngineParameters, Pose2D::rotation, PIDData::setLegFJ1Values(), PIDData::setLegFJ2Values(), PIDData::setLegFJ3Values(), PIDData::setLegHJ1Values(), PIDData::setLegHJ2Values(), PIDData::setLegHJ3Values(), setNextParameters(), smoothMotionRequest(), Pose2D::translation, useFixedParameters, WalkRequest::walkParams, MotionRequest::walkRequest, and WalkRequest::walkType.

Here is the call graph for this function:

bool GT2004WalkingEngine::handleMessage InMessage message  )  [virtual]
 

Called from a MessageQueue to distribute messages.

Parameters:
message The message that can be read.
Returns:
true if the message was read (handled).

Reimplemented from Module.

Definition at line 573 of file GT2004WalkingEngine.cpp.

References InMessage::bin, fixedParameters, InMessage::getMessageID(), idGT2004EvolutionRequest, idGT2004Parameters, idInvKinWalkingParameters, idText, GT2004Parameters::index, initParametersInterpolation(), lastParameters, GT2004ParametersSet::load(), GT2004ParametersSet::mergedParameters, nextParameters, OUTPUT, parametersToChange, paramInterpolCount, paramInterpolLength, paramSet, recalcEngineParameters, setNextParameters(), and useFixedParameters.

Here is the call graph for this function:

void GT2004WalkingEngine::setNextParameters int  steps  ) 
 

find correct table entry to update (in turnOnly and withWalk): find the entry most similar to nextParameters, copy the old value to lastParameters, set parametersToChange and start interpolation

Parameters:
steps Number of interpolation steps from old to new parameters

Definition at line 66 of file GT2004WalkingEngine.cpp.

References GT2004ParametersSet::getParameters(), GT2004Parameters::index, initParametersInterpolation(), lastParameters, nextParameters, parametersToChange, and paramSet.

Referenced by executeParameterized(), and handleMessage().

Here is the call graph for this function:

void GT2004WalkingEngine::initParametersInterpolation int  changeSteps  )  [private]
 

Initialize interpolation of WalkingParameterSets.

Definition at line 634 of file GT2004WalkingEngine.cpp.

References nextParameters, parametersToChange, paramInterpolCount, paramInterpolLength, and recalcEngineParameters.

Referenced by executeParameterized(), handleMessage(), and setNextParameters().

void GT2004WalkingEngine::nextParametersInterpolation bool  walk  )  [private]
 

Calculate next step in parameterset interpolation and increase currentStep if walk is true.

Definition at line 645 of file GT2004WalkingEngine.cpp.

References currentStepPercentage, GT2004Parameters::index, GT2004Parameters::interpolate(), lastParameters, GT2004ParametersSet::mirrorThis(), nextParameters, parametersToChange, paramInterpolCount, paramInterpolLength, paramSet, recalcEngineParameters, and GT2004Parameters::stepLen.

Referenced by executeParameterized().

Here is the call graph for this function:

void GT2004WalkingEngine::init  )  [private]
 

initialise fixed parameters

Definition at line 31 of file GT2004WalkingEngine.cpp.

References calculateParams(), GT2004ParametersSet::mergedParameters, and paramSet.

Referenced by GT2004WalkingEngine().

Here is the call graph for this function:

void GT2004WalkingEngine::calculateParams  )  [private]
 

calculate all engine wide parameters like neckHeight from currentParameters this is essential after every change in currentParameters!

Definition at line 37 of file GT2004WalkingEngine.cpp.

References airTime, GT2004Parameters::foreHeight, GT2004Parameters::groundPhase, groundTime, GT2004Parameters::hindHeight, GT2004Parameters::legPhase, legPhaseIndex, GT2004Parameters::liftPhase, liftTime, GT2004Parameters::loweringPhase, loweringTime, stepAir, GT2004Parameters::stepLen, stepLift, and stepLower.

Referenced by calculateLegSpeeds(), and init().

void GT2004WalkingEngine::smoothMotionRequest const Pose2D request,
Pose2D currentRequest
[private]
 

smooth motion request current request is adjusted according to motion request eliminating quick changes

Definition at line 73 of file GT2004WalkingEngine.cpp.

References Pose2D::rotation, Pose2D::translation, Vector2< double >::x, and Vector2< double >::y.

Referenced by executeParameterized().

void GT2004WalkingEngine::calculateLegSpeeds  )  [private]
 

calculates new leg speeds according to current motion request

Definition at line 99 of file GT2004WalkingEngine.cpp.

References GT2004ParametersSet::calculateMergedParameterSet(), calculateParams(), GT2004Parameters::correctedMotion, currentRequest, GT2004Parameters::foreCenterX, GT2004Parameters::foreWidth, GT2004Parameters::hindCenterX, GT2004Parameters::hindWidth, legSpeed, GT2004ParametersSet::mergedParameters, paramSet, recalcEngineParameters, GT2004Parameters::requestedMotion, Pose2D::rotation, GT2004Parameters::stepLen, Pose2D::translation, useFixedParameters, Vector2< double >::x, and Vector2< double >::y.

Referenced by executeParameterized().

Here is the call graph for this function:

void GT2004WalkingEngine::calculateRelativeFootPosition int  step,
int  leg,
double &  rx,
double &  ry,
double &  rz
[private]
 

calculate relative foot position for one leg rx is relative to current step size (range -1.0..1.0) ry is an absolute offset to y foot position rz is relative to step lift parameter (range 0..1.0)

Definition at line 193 of file GT2004WalkingEngine.cpp.

References airTime, GT2004Parameters::footMode, FORELEG, groundTime, liftTime, loweringTime, pi, stepAir, GT2004Parameters::stepLen, stepLift, and stepLower.

Referenced by calculateFootPositions().

int GT2004WalkingEngine::calculateLegJoints Kinematics::LegIndex  leg,
double &  j1,
double &  j2,
double &  j3,
double  bodyTilt = 0
[private]
 

calculate angles for one leg in current step

void GT2004WalkingEngine::calculateData JointData j  )  [private]
 

calculate current joint data values

Definition at line 539 of file GT2004WalkingEngine.cpp.

References calculateFootPositions(), JointData::data, footPos, GT2004Parameters::headPan, GT2004Parameters::headRoll, GT2004Parameters::headTilt, Kinematics::jointsFromLegPosition(), GT2004Parameters::mouth, and toMicroRad().

Referenced by executeParameterized().

Here is the call graph for this function:

void GT2004WalkingEngine::calculateFootPositions  )  [private]
 

calculate current foot positions

Definition at line 378 of file GT2004WalkingEngine.cpp.

References Vector2< double >::abs(), calculateRelativeFootPosition(), currentStepPercentage, footPos, GT2004Parameters::foreCenterX, GT2004Parameters::foreFootLift, GT2004Parameters::foreFootTilt, GT2004Parameters::foreHeight, FORELEG, GT2004Parameters::foreWidth, GT2004Parameters::hindCenterX, GT2004Parameters::hindFootLift, GT2004Parameters::hindFootTilt, GT2004Parameters::hindHeight, GT2004Parameters::hindWidth, LEFTLEG, legPhaseIndex, legSpeed, GT2004Parameters::stepLen, Vector2< double >::x, Vector3< double >::x, Vector3< double >::y, and Vector3< double >::z.

Referenced by calculateData().

Here is the call graph for this function:


Member Data Documentation

GT2004ParametersSet GT2004WalkingEngine::paramSet [private]
 

a set of GT2004Parameters optimized for different requests

Definition at line 63 of file GT2004WalkingEngine.h.

Referenced by calculateLegSpeeds(), executeParameterized(), handleMessage(), init(), nextParametersInterpolation(), and setNextParameters().

GT2004Parameters* GT2004WalkingEngine::currentParameters [private]
 

Points to parameter set currently in use.

Definition at line 68 of file GT2004WalkingEngine.h.

GT2004Parameters GT2004WalkingEngine::fixedParameters [private]
 

Fixed parameter set for InvKin compatibility.

Definition at line 71 of file GT2004WalkingEngine.h.

Referenced by executeParameterized(), and handleMessage().

GT2004Parameters* GT2004WalkingEngine::parametersToChange [private]
 

Pointer to the parameter set that shall be changed by interpolation.

Definition at line 74 of file GT2004WalkingEngine.h.

Referenced by executeParameterized(), handleMessage(), initParametersInterpolation(), nextParametersInterpolation(), and setNextParameters().

GT2004Parameters GT2004WalkingEngine::nextParameters [private]
 

Next parameters of this walk, target of current interpolation.

Definition at line 77 of file GT2004WalkingEngine.h.

Referenced by executeParameterized(), handleMessage(), initParametersInterpolation(), nextParametersInterpolation(), and setNextParameters().

GT2004Parameters GT2004WalkingEngine::lastParameters [private]
 

Last parameters of this walk, origin of current interpolation.

Definition at line 80 of file GT2004WalkingEngine.h.

Referenced by executeParameterized(), handleMessage(), nextParametersInterpolation(), and setNextParameters().

int GT2004WalkingEngine::paramInterpolCount [private]
 

Counts parameter set interpolation steps.

Definition at line 83 of file GT2004WalkingEngine.h.

Referenced by executeParameterized(), handleMessage(), initParametersInterpolation(), and nextParametersInterpolation().

int GT2004WalkingEngine::paramInterpolLength [private]
 

Stores the length of the current parameter set interpolation.

Definition at line 86 of file GT2004WalkingEngine.h.

Referenced by executeParameterized(), handleMessage(), initParametersInterpolation(), and nextParametersInterpolation().

double GT2004WalkingEngine::currentStepPercentage [private]
 

Stores precise version of currentStep for all kinds of interpolation.

Definition at line 89 of file GT2004WalkingEngine.h.

Referenced by calculateFootPositions(), executeParameterized(), and nextParametersInterpolation().

int GT2004WalkingEngine::groundTime[2] [private]
 

number of ticks foot is on ground

Definition at line 100 of file GT2004WalkingEngine.h.

Referenced by calculateParams(), and calculateRelativeFootPosition().

int GT2004WalkingEngine::liftTime[2] [private]
 

number of ticks for lifting foot

Definition at line 101 of file GT2004WalkingEngine.h.

Referenced by calculateParams(), and calculateRelativeFootPosition().

int GT2004WalkingEngine::loweringTime[2] [private]
 

number of ticks for lowering foot

Definition at line 102 of file GT2004WalkingEngine.h.

Referenced by calculateParams(), and calculateRelativeFootPosition().

int GT2004WalkingEngine::airTime[2] [private]
 

number of ticks foot is in air

Definition at line 103 of file GT2004WalkingEngine.h.

Referenced by calculateParams(), and calculateRelativeFootPosition().

int GT2004WalkingEngine::stepLift[2] [private]
 

time index for lifting foot

Definition at line 104 of file GT2004WalkingEngine.h.

Referenced by calculateParams(), and calculateRelativeFootPosition().

int GT2004WalkingEngine::stepAir[2] [private]
 

time index when foot is fully lifted

Definition at line 105 of file GT2004WalkingEngine.h.

Referenced by calculateParams(), and calculateRelativeFootPosition().

int GT2004WalkingEngine::stepLower[2] [private]
 

time index for lowering foot

Definition at line 106 of file GT2004WalkingEngine.h.

Referenced by calculateParams(), and calculateRelativeFootPosition().

int GT2004WalkingEngine::legPhaseIndex[4] [private]
 

leg phases, time indexes for lifting each leg

Definition at line 107 of file GT2004WalkingEngine.h.

Referenced by calculateFootPositions(), and calculateParams().

int GT2004WalkingEngine::firstStep [private]
 

time index for first time all feet are on ground

Definition at line 108 of file GT2004WalkingEngine.h.

bool GT2004WalkingEngine::useFixedParameters [private]
 

decides whether we use merging of rotationOnly and withWalk or fixedParameters

Definition at line 112 of file GT2004WalkingEngine.h.

Referenced by calculateLegSpeeds(), executeParameterized(), and handleMessage().

bool GT2004WalkingEngine::recalcEngineParameters [private]
 

true if stuff like legSpeed has to be recalculated because of changed request or parameterset

Definition at line 115 of file GT2004WalkingEngine.h.

Referenced by calculateLegSpeeds(), executeParameterized(), handleMessage(), initParametersInterpolation(), and nextParametersInterpolation().

unsigned long GT2004WalkingEngine::lastParametersFromPackageTimeStamp [private]
 

Definition at line 117 of file GT2004WalkingEngine.h.

Referenced by executeParameterized().

double GT2004WalkingEngine::neckHeight [private]
 

neck height in mm while walking

Definition at line 120 of file GT2004WalkingEngine.h.

double GT2004WalkingEngine::bodyTilt [private]
 

body tilt angle

Definition at line 123 of file GT2004WalkingEngine.h.

Vector2<double> GT2004WalkingEngine::legSpeed[4] [private]
 

speed of leg on ground (step size in mm)

Definition at line 127 of file GT2004WalkingEngine.h.

Referenced by calculateFootPositions(), and calculateLegSpeeds().

Vector3<double> GT2004WalkingEngine::footPos[4] [private]
 

foot positions

Definition at line 129 of file GT2004WalkingEngine.h.

Referenced by calculateData(), and calculateFootPositions().

Pose2D GT2004WalkingEngine::currentRequest [private]
 

currently executed motion request speeds in mm/s

Definition at line 135 of file GT2004WalkingEngine.h.

Referenced by calculateLegSpeeds(), and executeParameterized().

Pose2D GT2004WalkingEngine::odometry [private]
 

odometry resulting from current request speed in mm/tick

Definition at line 140 of file GT2004WalkingEngine.h.


The documentation for this class was generated from the following files:
Generated on Thu Sep 23 20:08:12 2004 for GT2004 by doxygen 1.3.6