#include <InvKinWalkingEngine.h>
Inheritance diagram for InvKinWalkingEngine:
parameter set interpolation | |
void | initParametersInterpolation (int changeSteps) |
void | nextParametersInterpolation (bool walk) |
InvKinWalkingParameters | currentParameters |
InvKinWalkingParameters * | requestedParameters |
InvKinWalkingParameters | nextParameters |
InvKinWalkingParameters | lastParameters |
int | paramInterpolCount |
int | paramInterpolLength |
double | positionInWalkCycle |
Public Member Functions | |
InvKinWalkingEngine (const WalkingEngineInterfaces &interfaces) | |
~InvKinWalkingEngine () | |
virtual bool | executeParameterized (JointData &jointData, const WalkRequest &walkRequest, double positionInWalkingCycle) |
virtual bool | handleMessage (InMessage &message) |
void | setParameters (InvKinWalkingParameters *p, int changeSteps=32) |
const InvKinWalkingParameters & | getParameters () const |
Private Member Functions | |
void | calculateLegSpeeds () |
void | calculateRelativeFootPosition (int step, int leg, double &rx, double &ry, double &rz) |
void | calculateData (JointData &j) |
void | calculateFootPositions () |
int | calculateLegJoints (Kinematics::LegIndex leg, double &j1, double &j2, double &j3, double bodyTilt=0) |
void | smoothMotionRequest (const Pose2D &request, Pose2D ¤tRequest) |
void | limitToMaxSpeed (double &stepSizeX, double &stepSizeY, double &stepSizeR) |
double | getLegPositionCurve (double &rz, double index) |
Private Attributes | |
unsigned long | lastParametersFromPackageTimeStamp |
Pose2D | currentRequest |
Pose2D | odometry |
current walk values | |
double | legSpeedX [4] |
speed of leg in x direction (step size in mm) | |
double | legSpeedY [4] |
speed of leg in y direction (step size in mm) | |
bool | footOnGround [4] |
foot is on ground | |
double | x [4] |
foot x positions | |
double | y [4] |
foot y positions | |
double | z [4] |
Definition at line 24 of file InvKinWalkingEngine.h.
InvKinWalkingEngine::InvKinWalkingEngine | ( | const WalkingEngineInterfaces & | interfaces | ) |
Constructor.
interfaces | The paramters of the WalkingEngine module. |
Definition at line 17 of file InvKinWalkingEngine.cpp.
References currentRequest, footOnGround, legSpeedX, and legSpeedY.
InvKinWalkingEngine::~InvKinWalkingEngine | ( | ) |
Destructor
Definition at line 30 of file InvKinWalkingEngine.cpp.
void InvKinWalkingEngine::calculateData | ( | JointData & | j | ) | [private] |
calculate current joint data values
Definition at line 254 of file InvKinWalkingEngine.cpp.
References InvKinWalkingParameters::bodyTilt, calculateFootPositions(), currentParameters, JointData::data, JointData::earL, JointData::earR, InvKinWalkingParameters::headPan, JointData::headPan, InvKinWalkingParameters::headRoll, JointData::headTilt, InvKinWalkingParameters::headTilt, jointDataInvalidValue, Kinematics::jointsFromLegPosition(), JointData::legFR1, JointData::legFR2, JointData::legFR3, InvKinWalkingParameters::mouth, JointData::mouth, JointData::neckTilt, JointData::tailPan, JointData::tailTilt, toMicroRad(), x, y, and z.
Referenced by executeParameterized().
void InvKinWalkingEngine::calculateFootPositions | ( | ) | [private] |
calculate current foot positions
Definition at line 487 of file InvKinWalkingEngine.cpp.
References InvKinWalkingParameters::bodyShiftOffset, InvKinWalkingParameters::bodyShiftX, InvKinWalkingParameters::bodyShiftY, calculateRelativeFootPosition(), currentParameters, footOnGround, InvKinWalkingParameters::foreCenterX, InvKinWalkingParameters::foreFootLift, InvKinWalkingParameters::foreFootTilt, InvKinWalkingParameters::foreHeight, FORELEG, InvKinWalkingParameters::foreWidth, InvKinWalkingParameters::hindCenterX, InvKinWalkingParameters::hindFootLift, InvKinWalkingParameters::hindFootTilt, InvKinWalkingParameters::hindHeight, InvKinWalkingParameters::hindWidth, int(), LEFTLEG, InvKinWalkingParameters::legPhaseIndex, legSpeedX, legSpeedY, positionInWalkCycle, InvKinWalkingParameters::stepLen, x, y, and z.
Referenced by calculateData().
int InvKinWalkingEngine::calculateLegJoints | ( | Kinematics::LegIndex | leg, | |
double & | j1, | |||
double & | j2, | |||
double & | j3, | |||
double | bodyTilt = 0 | |||
) | [private] |
calculate angles for one leg in current step
void InvKinWalkingEngine::calculateLegSpeeds | ( | ) | [private] |
calculates new leg speeds according to current motion request
Definition at line 117 of file InvKinWalkingEngine.cpp.
References InvKinWalkingParameters::CorrectionValues::backward, InvKinWalkingParameters::correctionValues, InvKinWalkingParameters::counterRotation, currentParameters, currentRequest, Kinematics::fl, InvKinWalkingParameters::CorrectionValues::forward, Kinematics::fr, Kinematics::hl, Kinematics::hr, InvKinWalkingParameters::legSpeedFactorR, InvKinWalkingParameters::legSpeedFactorX, InvKinWalkingParameters::legSpeedFactorY, legSpeedX, legSpeedY, limitToMaxSpeed(), RobotDimensions::motionCycleTime, odometry, Pose2D::rotation, InvKinWalkingParameters::CorrectionValues::rotationCenter, InvKinWalkingParameters::CorrectionValues::sideward, Pose2D::translation, InvKinWalkingParameters::CorrectionValues::turning, Vector2< V >::x, and Vector2< V >::y.
Referenced by executeParameterized(), and nextParametersInterpolation().
void InvKinWalkingEngine::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 303 of file InvKinWalkingEngine.cpp.
References InvKinWalkingParameters::airTime, InvKinWalkingParameters::circle, currentParameters, d, InvKinWalkingParameters::footMode, FORELEG, InvKinWalkingParameters::freeFormQuad, InvKinWalkingParameters::freeFormQuadPos, getLegPositionCurve(), InvKinWalkingParameters::groundTime, InvKinWalkingParameters::halfCircle, InvKinWalkingParameters::liftTime, InvKinWalkingParameters::loweringTime, InvKinWalkingParameters::optimized, pi, InvKinWalkingParameters::rectangle, InvKinWalkingParameters::rounded, InvKinWalkingParameters::stepAir, InvKinWalkingParameters::stepLen, InvKinWalkingParameters::stepLift, InvKinWalkingParameters::stepLower, and x.
Referenced by calculateFootPositions().
bool InvKinWalkingEngine::executeParameterized | ( | JointData & | jointData, | |
const WalkRequest & | walkRequest, | |||
double | positionInWalkingCycle | |||
) | [virtual] |
Executes the engine
Implements WalkingEngine.
Definition at line 52 of file InvKinWalkingEngine.cpp.
References InvKinWalkingParameters::allFeetOnGround(), InvKinWalkingParameters::bodyTilt, MotionInfo::bodyTilt, RobotCalibration::bodyTiltOffset, InvKinWalkingParameters::bodyTiltOffset, calculateData(), calculateLegSpeeds(), Pose2D::conc(), InvKinWalkingParameters::copyValuesFrom(), currentParameters, currentRequest, MotionInfo::executedMotionRequest, InvKinWalkingParameters::firstStep, RobotConfiguration::getRobotCalibration(), getRobotConfiguration(), initParametersInterpolation(), WalkingEngineInterfaces::invKinWalkingParameters, WalkingEngineInterfaces::lastMotionType, lastParametersFromPackageTimeStamp, InvKinWalkingParameters::leaveAnytime, WalkingEngineInterfaces::motionInfo, MotionInfo::motionIsStable, MotionRequest::motionType, InvKinWalkingParameters::neckHeight, MotionInfo::neckHeight, nextParameters, nextParametersInterpolation(), odometry, WalkingEngineInterfaces::odometryData, MotionInfo::positionInWalkCycle, positionInWalkCycle, Pose2D::rotation, smoothMotionRequest(), InvKinWalkingParameters::stepLen, Pose2D::translation, MotionRequest::walk, WalkingEngineInterfaces::walkParameterTimeStamp, WalkRequest::walkParams, MotionRequest::walkRequest, WalkRequest::walkType, Vector2< V >::x, and Vector2< V >::y.
Referenced by MSH2004InvKinWalkingEngine::executeParameterized(), ParamRearOnlyInvKinWalkingEngine::executeParameterized(), ParamInvKinWalkingEngine::executeParameterized(), and BB2004InvKinWalkingEngine::executeParameterized().
double InvKinWalkingEngine::getLegPositionCurve | ( | double & | rz, | |
double | index | |||
) | [private] |
gets (hopefully optimized) relative foot position from a lookup table with interpolation
index | exact table index 0.0..0.25 | |
rz | the returned relative z coordinate of the foot |
Definition at line 288 of file InvKinWalkingEngine.cpp.
References int().
Referenced by calculateRelativeFootPosition().
const InvKinWalkingParameters& InvKinWalkingEngine::getParameters | ( | ) | const [inline] |
Gets the engine's parameters
Definition at line 58 of file InvKinWalkingEngine.h.
References currentParameters.
Referenced by BB2004InvKinWalkingEngine::updateOdometry().
bool InvKinWalkingEngine::handleMessage | ( | InMessage & | message | ) | [virtual] |
Called from a MessageQueue to distribute messages
message | The message that can be read. |
Reimplemented from Module.
Definition at line 552 of file InvKinWalkingEngine.cpp.
References InMessage::bin, InMessage::getMessageID(), idInvKinWalkingParameters, initParametersInterpolation(), and nextParameters.
Referenced by MSH2004InvKinWalkingEngine::handleMessage(), ParamRearOnlyInvKinWalkingEngine::handleMessage(), ParamInvKinWalkingEngine::handleMessage(), and BB2004InvKinWalkingEngine::handleMessage().
void InvKinWalkingEngine::initParametersInterpolation | ( | int | changeSteps | ) | [private] |
Initialize interpolation of WalkingParameterSets
Definition at line 564 of file InvKinWalkingEngine.cpp.
References currentParameters, lastParameters, nextParameters, paramInterpolCount, and paramInterpolLength.
Referenced by executeParameterized(), handleMessage(), and setParameters().
void InvKinWalkingEngine::limitToMaxSpeed | ( | double & | stepSizeX, | |
double & | stepSizeY, | |||
double & | stepSizeR | |||
) | [private] |
limit step to maximum step size corrects odometry accordingly
Definition at line 198 of file InvKinWalkingEngine.cpp.
References InvKinWalkingParameters::CorrectionValues::backward, InvKinWalkingParameters::correctionValues, currentParameters, InvKinWalkingParameters::CorrectionValues::forward, InvKinWalkingParameters::maxStepSizeR, InvKinWalkingParameters::maxStepSizeX, InvKinWalkingParameters::maxStepSizeY, odometry, Pose2D::rotation, InvKinWalkingParameters::CorrectionValues::sideward, Pose2D::translation, InvKinWalkingParameters::CorrectionValues::turning, Vector2< V >::x, and Vector2< V >::y.
Referenced by calculateLegSpeeds().
void InvKinWalkingEngine::nextParametersInterpolation | ( | bool | walk | ) | [private] |
Calculate next step in parameterset interpolation and increase currentStep if walk is true
Definition at line 575 of file InvKinWalkingEngine.cpp.
References calculateLegSpeeds(), currentParameters, InvKinWalkingParameters::init(), InvKinWalkingParameters::interpolate(), lastParameters, nextParameters, paramInterpolCount, paramInterpolLength, positionInWalkCycle, and InvKinWalkingParameters::stepLen.
Referenced by executeParameterized().
void InvKinWalkingEngine::setParameters | ( | InvKinWalkingParameters * | p, | |
int | changeSteps = 32 | |||
) |
Sets the engine's parameters
p | The new parameter set | |
changeSteps | Number of interpolation steps from old to new parameters |
Definition at line 34 of file InvKinWalkingEngine.cpp.
References currentParameters, InvKinWalkingParameters::firstStep, initParametersInterpolation(), nextParameters, positionInWalkCycle, requestedParameters, and InvKinWalkingParameters::stepLen.
Referenced by MSH2004InvKinWalkingEngine::executeParameterized(), ParamRearOnlyInvKinWalkingEngine::executeParameterized(), ParamInvKinWalkingEngine::executeParameterized(), BB2004InvKinWalkingEngine::executeParameterized(), and BB2004InvKinWalkingEngine::setParameters().
void InvKinWalkingEngine::smoothMotionRequest | ( | const Pose2D & | request, | |
Pose2D & | currentRequest | |||
) | [private] |
smooth motion request current request is adjusted according to motion request eliminating quick changes
Definition at line 174 of file InvKinWalkingEngine.cpp.
References currentParameters, currentRequest, Pose2D::getAngle(), InvKinWalkingParameters::maxRotationChange, InvKinWalkingParameters::maxSpeedXChange, InvKinWalkingParameters::maxSpeedYChange, Pose2D::rotation, Pose2D::translation, Vector2< V >::x, and Vector2< V >::y.
Referenced by executeParameterized().
Current parameters of this walk
Definition at line 65 of file InvKinWalkingEngine.h.
Referenced by calculateData(), calculateFootPositions(), calculateLegSpeeds(), calculateRelativeFootPosition(), executeParameterized(), getParameters(), initParametersInterpolation(), limitToMaxSpeed(), nextParametersInterpolation(), setParameters(), and smoothMotionRequest().
Pose2D InvKinWalkingEngine::currentRequest [private] |
currently executed motion request speeds in mm/s
Definition at line 108 of file InvKinWalkingEngine.h.
Referenced by calculateLegSpeeds(), executeParameterized(), InvKinWalkingEngine(), and smoothMotionRequest().
bool InvKinWalkingEngine::footOnGround[4] [private] |
foot is on ground
Definition at line 99 of file InvKinWalkingEngine.h.
Referenced by calculateFootPositions(), and InvKinWalkingEngine().
Last parameters of this walk, origin of current interpolation
Definition at line 74 of file InvKinWalkingEngine.h.
Referenced by initParametersInterpolation(), and nextParametersInterpolation().
unsigned long InvKinWalkingEngine::lastParametersFromPackageTimeStamp [private] |
double InvKinWalkingEngine::legSpeedX[4] [private] |
speed of leg in x direction (step size in mm)
Definition at line 96 of file InvKinWalkingEngine.h.
Referenced by calculateFootPositions(), calculateLegSpeeds(), and InvKinWalkingEngine().
double InvKinWalkingEngine::legSpeedY[4] [private] |
speed of leg in y direction (step size in mm)
Definition at line 97 of file InvKinWalkingEngine.h.
Referenced by calculateFootPositions(), calculateLegSpeeds(), and InvKinWalkingEngine().
Next parameters of this walk, target of current interpolation
Definition at line 71 of file InvKinWalkingEngine.h.
Referenced by executeParameterized(), handleMessage(), initParametersInterpolation(), nextParametersInterpolation(), and setParameters().
Pose2D InvKinWalkingEngine::odometry [private] |
odometry resulting from current request speed in mm/tick
Definition at line 113 of file InvKinWalkingEngine.h.
Referenced by calculateLegSpeeds(), executeParameterized(), and limitToMaxSpeed().
int InvKinWalkingEngine::paramInterpolCount [private] |
Counts parameter set interpolation steps
Definition at line 77 of file InvKinWalkingEngine.h.
Referenced by initParametersInterpolation(), and nextParametersInterpolation().
int InvKinWalkingEngine::paramInterpolLength [private] |
Stores the length of the current parameter set interpolation
Definition at line 80 of file InvKinWalkingEngine.h.
Referenced by initParametersInterpolation(), and nextParametersInterpolation().
double InvKinWalkingEngine::positionInWalkCycle [private] |
Stores precise version of currentStep during interpolation
Definition at line 83 of file InvKinWalkingEngine.h.
Referenced by calculateFootPositions(), executeParameterized(), nextParametersInterpolation(), and setParameters().
Pointer to the parameter set requested in setParameters
Definition at line 68 of file InvKinWalkingEngine.h.
Referenced by setParameters().
double InvKinWalkingEngine::x[4] [private] |
foot x positions
Definition at line 100 of file InvKinWalkingEngine.h.
Referenced by calculateData(), calculateFootPositions(), and calculateRelativeFootPosition().
double InvKinWalkingEngine::y[4] [private] |
foot y positions
Definition at line 101 of file InvKinWalkingEngine.h.
Referenced by calculateData(), and calculateFootPositions().
double InvKinWalkingEngine::z[4] [private] |
foot z positions
Definition at line 102 of file InvKinWalkingEngine.h.
Referenced by calculateData(), and calculateFootPositions().