GT2004WalkingEngine Class Reference

#include <GT2004WalkingEngine.h>

Inheritance diagram for GT2004WalkingEngine:

WalkingEngine Module WalkingEngineInterfaces RobotDimensions MessageHandler List of all members.

parameter set interpolation

void initParametersInterpolation (int changeSteps)
void nextParametersInterpolation (bool walk)
GT2004ParameterscurrentParameters
GT2004Parameters fixedParameters
GT2004ParametersparametersToChange
GT2004Parameters nextParameters
GT2004Parameters lastParameters
int paramInterpolCount
int paramInterpolLength
double currentStepPercentage

Public Member Functions

 GT2004WalkingEngine (const WalkingEngineInterfaces &interfaces)
 ~GT2004WalkingEngine ()
virtual bool executeParameterized (JointData &jointData, const WalkRequest &walkRequest, double positionInWalkingCycle)
virtual bool handleMessage (InMessage &message)
void setNextParameters (int steps)

Private Member Functions

void init ()
void calculateParams ()
void smoothMotionRequest (const Pose2D &request, Pose2D &currentRequest)
void calculateLegSpeeds ()
void calculateRelativeFootPosition (int step, int leg, double &rx, double &ry, double &rz)
int calculateLegJoints (Kinematics::LegIndex leg, double &j1, double &j2, double &j3, double bodyTilt=0)
void calculateData (JointData &j)
void calculateFootPositions ()

Private Attributes

GT2004ParametersSet paramSet
bool useFixedParameters
bool recalcEngineParameters
unsigned long lastParametersFromPackageTimeStamp
double neckHeight
double bodyTilt
Pose2D currentRequest
Pose2D odometry
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
current walk values
Vector2< double > legSpeed [4]
 speed of leg on ground (step size in mm)
Vector3< double > footPos [4]

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().

GT2004WalkingEngine::~GT2004WalkingEngine (  ) 

Destructor

Definition at line 27 of file GT2004WalkingEngine.cpp.


Member Function Documentation

void GT2004WalkingEngine::calculateData ( JointData j  )  [private]

calculate current joint data values

Definition at line 539 of file GT2004WalkingEngine.cpp.

References bodyTilt, calculateFootPositions(), currentParameters, JointData::data, JointData::earL, JointData::earR, footPos, GT2004Parameters::headPan, JointData::headPan, GT2004Parameters::headRoll, JointData::headTilt, GT2004Parameters::headTilt, jointDataInvalidValue, Kinematics::jointsFromLegPosition(), JointData::legFR1, JointData::legFR2, JointData::legFR3, GT2004Parameters::mouth, JointData::mouth, JointData::neckTilt, JointData::tailPan, JointData::tailTilt, and toMicroRad().

Referenced by executeParameterized().

void GT2004WalkingEngine::calculateFootPositions (  )  [private]

calculate current foot positions

Definition at line 378 of file GT2004WalkingEngine.cpp.

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

Referenced by calculateData().

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::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, currentParameters, currentRequest, Kinematics::fl, GT2004Parameters::foreCenterX, GT2004Parameters::foreWidth, Kinematics::fr, GT2004Parameters::hindCenterX, GT2004Parameters::hindWidth, Kinematics::hl, Kinematics::hr, legSpeed, RobotDimensions::lengthBetweenLegs, GT2004ParametersSet::mergedParameters, RobotDimensions::motionCycleTime, odometry, paramSet, recalcEngineParameters, GT2004Parameters::requestedMotion, Pose2D::rotation, GT2004Parameters::stepLen, Pose2D::translation, useFixedParameters, Vector2< V >::x, and Vector2< V >::y.

Referenced by executeParameterized().

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, bodyTilt, currentParameters, GT2004Parameters::foreHeight, GT2004Parameters::groundPhase, groundTime, GT2004Parameters::hindHeight, GT2004Parameters::legPhase, legPhaseIndex, RobotDimensions::lengthBetweenLegs, GT2004Parameters::liftPhase, liftTime, GT2004Parameters::loweringPhase, loweringTime, neckHeight, stepAir, GT2004Parameters::stepLen, stepLift, and stepLower.

Referenced by calculateLegSpeeds(), and init().

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::circle, currentParameters, GT2004Parameters::footMode, FORELEG, GT2004Parameters::freeFormQuad, groundTime, GT2004Parameters::halfCircle, liftTime, loweringTime, GT2004Parameters::optimized, pi, GT2004Parameters::rectangle, GT2004Parameters::rounded, stepAir, GT2004Parameters::stepLen, stepLift, and stepLower.

Referenced by calculateFootPositions().

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< V >::abs(), bodyTilt, MotionInfo::bodyTilt, RobotCalibration::bodyTiltOffset, calculateData(), calculateLegSpeeds(), Pose2D::conc(), currentParameters, currentRequest, currentStepPercentage, MotionInfo::executedMotionRequest, fixedParameters, RobotConfiguration::getRobotCalibration(), getRobotConfiguration(), WalkingEngineInterfaces::gt2004Parameters, GT2004Parameters::index, initParametersInterpolation(), WalkingEngineInterfaces::lastMotionType, lastParameters, lastParametersFromPackageTimeStamp, GT2004ParametersSet::mergedParameters, WalkingEngineInterfaces::motionInfo, MotionInfo::motionIsStable, MotionRequest::motionType, neckHeight, MotionInfo::neckHeight, nextParameters, nextParametersInterpolation(), GT2004ParametersSet::numberOfParameters, odometry, WalkingEngineInterfaces::odometryData, parametersToChange, paramInterpolCount, paramInterpolLength, paramSet, WalkingEngineInterfaces::pidData, MotionInfo::positionInWalkCycle, recalcEngineParameters, Pose2D::rotation, PIDData::setLegFJ1Values(), PIDData::setLegFJ2Values(), PIDData::setLegFJ3Values(), PIDData::setLegHJ1Values(), PIDData::setLegHJ2Values(), PIDData::setLegHJ3Values(), setNextParameters(), smoothMotionRequest(), Pose2D::translation, useFixedParameters, MotionRequest::walk, WalkingEngineInterfaces::walkParameterTimeStamp, WalkRequest::walkParams, MotionRequest::walkRequest, and WalkRequest::walkType.

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, currentParameters, fixedParameters, InMessage::getMessageID(), idGT2004EvolutionRequest, idGT2004Parameters, idInvKinWalkingParameters, idText, GT2004Parameters::index, initParametersInterpolation(), lastParameters, GT2004ParametersSet::load(), GT2004ParametersSet::mergedParameters, nextParameters, GT2004ParametersSet::numberOfParameters, OUTPUT, parametersToChange, paramInterpolCount, paramInterpolLength, paramSet, recalcEngineParameters, setNextParameters(), text, and useFixedParameters.

void GT2004WalkingEngine::init (  )  [private]

initialise fixed parameters

Definition at line 31 of file GT2004WalkingEngine.cpp.

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

Referenced by GT2004WalkingEngine().

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 currentParameters, currentStepPercentage, GT2004Parameters::index, GT2004Parameters::interpolate(), lastParameters, GT2004ParametersSet::mirrorThis(), nextParameters, parametersToChange, paramInterpolCount, paramInterpolLength, paramSet, recalcEngineParameters, and GT2004Parameters::stepLen.

Referenced by executeParameterized().

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().

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 currentRequest, Pose2D::rotation, Pose2D::translation, Vector2< V >::x, and Vector2< V >::y.

Referenced by executeParameterized().


Member Data Documentation

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().

double GT2004WalkingEngine::bodyTilt [private]

body tilt angle

Definition at line 123 of file GT2004WalkingEngine.h.

Referenced by calculateData(), calculateParams(), and executeParameterized().

GT2004Parameters* GT2004WalkingEngine::currentParameters [private]

Points to parameter set currently in use

Definition at line 68 of file GT2004WalkingEngine.h.

Referenced by calculateData(), calculateFootPositions(), calculateLegSpeeds(), calculateParams(), calculateRelativeFootPosition(), executeParameterized(), handleMessage(), init(), and nextParametersInterpolation().

Pose2D GT2004WalkingEngine::currentRequest [private]

currently executed motion request speeds in mm/s

Definition at line 135 of file GT2004WalkingEngine.h.

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

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::firstStep [private]

time index for first time all feet are on ground

Definition at line 108 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().

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

foot positions

Definition at line 129 of file GT2004WalkingEngine.h.

Referenced by calculateData(), and calculateFootPositions().

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().

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().

unsigned long GT2004WalkingEngine::lastParametersFromPackageTimeStamp [private]

Definition at line 117 of file GT2004WalkingEngine.h.

Referenced by executeParameterized().

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().

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().

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().

double GT2004WalkingEngine::neckHeight [private]

neck height in mm while walking

Definition at line 120 of file GT2004WalkingEngine.h.

Referenced by calculateParams(), and executeParameterized().

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().

Pose2D GT2004WalkingEngine::odometry [private]

odometry resulting from current request speed in mm/tick

Definition at line 140 of file GT2004WalkingEngine.h.

Referenced by calculateLegSpeeds(), and executeParameterized().

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().

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().

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().

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().

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::stepLift[2] [private]

time index for lifting foot

Definition at line 104 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().

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().


The documentation for this class was generated from the following files:
Generated on Thu Dec 7 01:36:24 2006 for DT2005.panorama by  doxygen 1.4.7