#include <GT2004WalkingEngine.h>
Inheritance diagram for GT2004WalkingEngine:
parameter set interpolation | |
void | initParametersInterpolation (int changeSteps) |
void | nextParametersInterpolation (bool walk) |
GT2004Parameters * | currentParameters |
GT2004Parameters | fixedParameters |
GT2004Parameters * | parametersToChange |
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 ¤tRequest) |
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] |
Definition at line 28 of file GT2004WalkingEngine.h.
GT2004WalkingEngine::GT2004WalkingEngine | ( | const WalkingEngineInterfaces & | interfaces | ) |
Constructor.
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.
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
message | The message that can be read. |
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
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().
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().
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.
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().
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] |
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().
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().
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().
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().