InvKinWalkingEngine Class Reference

#include <InvKinWalkingEngine.h>

Inheritance diagram for InvKinWalkingEngine:

WalkingEngine Module WalkingEngineInterfaces RobotDimensions MessageHandler List of all members.

parameter set interpolation

void initParametersInterpolation (int changeSteps)
void nextParametersInterpolation (bool walk)
InvKinWalkingParameters currentParameters
InvKinWalkingParametersrequestedParameters
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 InvKinWalkingParametersgetParameters () 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 &currentRequest)
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]

Detailed Description

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

Author:
Max Risler

Definition at line 24 of file InvKinWalkingEngine.h.


Constructor & Destructor Documentation

InvKinWalkingEngine::InvKinWalkingEngine ( const WalkingEngineInterfaces interfaces  ) 

Constructor.

Parameters:
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.


Member Function Documentation

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

Parameters:
index exact table index 0.0..0.25
rz the returned relative z coordinate of the foot
Returns:
the returned relative x coordinate (signless) 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

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

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

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


Member Data Documentation

InvKinWalkingParameters InvKinWalkingEngine::currentParameters [private]

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

InvKinWalkingParameters InvKinWalkingEngine::lastParameters [private]

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]

Definition at line 92 of file InvKinWalkingEngine.h.

Referenced by executeParameterized().

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

InvKinWalkingParameters InvKinWalkingEngine::nextParameters [private]

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

InvKinWalkingParameters* InvKinWalkingEngine::requestedParameters [private]

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


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