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

InvKinWalkingEngine Class Reference

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

#include <InvKinWalkingEngine.h>

Inheritance diagram for InvKinWalkingEngine:

Inheritance graph
[legend]
Collaboration diagram for InvKinWalkingEngine:

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.

InvKinWalkingParameters currentParameters
 Current parameters of this walk.

InvKinWalkingParameters * requestedParameters
 Pointer to the parameter set requested in setParameters.

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

InvKinWalkingParameters 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 positionInWalkCycle
 Stores precise version of currentStep during interpolation.


Public Member Functions

 InvKinWalkingEngine (const WalkingEngineInterfaces &interfaces)
 Constructor.

 ~InvKinWalkingEngine ()
 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 setParameters (InvKinWalkingParameters *p, int changeSteps=32)
 Sets the engine's parameters.

const InvKinWalkingParameters & getParameters () const
 Gets the engine's parameters.


Private Member Functions

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)

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

void calculateFootPositions ()
 calculate current foot positions

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

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

void limitToMaxSpeed (double &stepSizeX, double &stepSizeY, double &stepSizeR)
 limit step to maximum step size corrects odometry accordingly

double getLegPositionCurve (double &rz, double index)
 gets (hopefully optimized) relative foot position from a lookup table with interpolation


Private Attributes

unsigned long lastParametersFromPackageTimeStamp
Pose2D currentRequest
 currently executed motion request speeds in mm/s

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

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]
 foot z positions

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 footOnGround, legSpeedX, and legSpeedY.

InvKinWalkingEngine::~InvKinWalkingEngine (   ) 
 

Destructor.

Definition at line 30 of file InvKinWalkingEngine.cpp.

Member Function Documentation

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(), MotionInfo::executedMotionRequest, InvKinWalkingParameters::firstStep, RobotConfiguration::getRobotCalibration(), getRobotConfiguration(), initParametersInterpolation(), InvKinWalkingParameters::leaveAnytime, MotionInfo::motionIsStable, MotionRequest::motionType, InvKinWalkingParameters::neckHeight, MotionInfo::neckHeight, nextParametersInterpolation(), MotionInfo::positionInWalkCycle, Pose2D::rotation, smoothMotionRequest(), InvKinWalkingParameters::stepLen, Pose2D::translation, WalkRequest::walkParams, MotionRequest::walkRequest, WalkRequest::walkType, Vector2< double >::x, and Vector2< double >::y.

Referenced by MSH2004InvKinWalkingEngine::executeParameterized(), ParamRearOnlyInvKinWalkingEngine::executeParameterized(), ParamInvKinWalkingEngine::executeParameterized(), and BB2004InvKinWalkingEngine::executeParameterized().

Here is the call graph for this function:

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, and initParametersInterpolation().

Referenced by MSH2004InvKinWalkingEngine::handleMessage(), ParamRearOnlyInvKinWalkingEngine::handleMessage(), ParamInvKinWalkingEngine::handleMessage(), and BB2004InvKinWalkingEngine::handleMessage().

Here is the call graph for this function:

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 InvKinWalkingParameters::firstStep, initParametersInterpolation(), requestedParameters, and InvKinWalkingParameters::stepLen.

Referenced by MSH2004InvKinWalkingEngine::executeParameterized(), ParamRearOnlyInvKinWalkingEngine::executeParameterized(), ParamInvKinWalkingEngine::executeParameterized(), BB2004InvKinWalkingEngine::executeParameterized(), and BB2004InvKinWalkingEngine::setParameters().

Here is the call graph for this function:

const InvKinWalkingParameters& InvKinWalkingEngine::getParameters (   )  const [inline]
 

Gets the engine's parameters.

Definition at line 58 of file InvKinWalkingEngine.h.

Referenced by BB2004InvKinWalkingEngine::updateOdometry().

void InvKinWalkingEngine::initParametersInterpolation (  int  changeSteps  )  [private]
 

Initialize interpolation of WalkingParameterSets.

Definition at line 564 of file InvKinWalkingEngine.cpp.

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

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(), InvKinWalkingParameters::init(), InvKinWalkingParameters::interpolate(), and InvKinWalkingParameters::stepLen.

Referenced by executeParameterized().

Here is the call graph for this function:

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, InvKinWalkingParameters::CorrectionValues::forward, InvKinWalkingParameters::legSpeedFactorR, InvKinWalkingParameters::legSpeedFactorX, InvKinWalkingParameters::legSpeedFactorY, legSpeedX, legSpeedY, limitToMaxSpeed(), Pose2D::rotation, InvKinWalkingParameters::CorrectionValues::rotationCenter, InvKinWalkingParameters::CorrectionValues::sideward, Pose2D::translation, InvKinWalkingParameters::CorrectionValues::turning, Vector2< double >::x, and Vector2< double >::y.

Referenced by executeParameterized(), and nextParametersInterpolation().

Here is the call graph for this function:

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::footMode, FORELEG, InvKinWalkingParameters::freeFormQuadPos, getLegPositionCurve(), InvKinWalkingParameters::groundTime, InvKinWalkingParameters::liftTime, InvKinWalkingParameters::loweringTime, pi, InvKinWalkingParameters::stepAir, InvKinWalkingParameters::stepLen, InvKinWalkingParameters::stepLift, and InvKinWalkingParameters::stepLower.

Referenced by calculateFootPositions().

Here is the call graph for this function:

void InvKinWalkingEngine::calculateData (  JointData &  j  )  [private]
 

calculate current joint data values

Definition at line 254 of file InvKinWalkingEngine.cpp.

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

Referenced by executeParameterized().

Here is the call graph for this function:

void InvKinWalkingEngine::calculateFootPositions (   )  [private]
 

calculate current foot positions

Definition at line 487 of file InvKinWalkingEngine.cpp.

References InvKinWalkingParameters::bodyShiftOffset, InvKinWalkingParameters::bodyShiftX, InvKinWalkingParameters::bodyShiftY, calculateRelativeFootPosition(), footOnGround, InvKinWalkingParameters::foreCenterX, InvKinWalkingParameters::foreFootLift, InvKinWalkingParameters::foreFootTilt, InvKinWalkingParameters::foreHeight, FORELEG, InvKinWalkingParameters::foreWidth, InvKinWalkingParameters::hindCenterX, InvKinWalkingParameters::hindFootLift, InvKinWalkingParameters::hindFootTilt, InvKinWalkingParameters::hindHeight, InvKinWalkingParameters::hindWidth, LEFTLEG, InvKinWalkingParameters::legPhaseIndex, legSpeedX, legSpeedY, and InvKinWalkingParameters::stepLen.

Referenced by calculateData().

Here is the call graph for this function:

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::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 Pose2D::getAngle(), InvKinWalkingParameters::maxRotationChange, InvKinWalkingParameters::maxSpeedXChange, InvKinWalkingParameters::maxSpeedYChange, Pose2D::rotation, Pose2D::translation, Vector2< double >::x, and Vector2< double >::y.

Referenced by executeParameterized().

Here is the call graph for this function:

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, InvKinWalkingParameters::CorrectionValues::forward, InvKinWalkingParameters::maxStepSizeR, InvKinWalkingParameters::maxStepSizeX, InvKinWalkingParameters::maxStepSizeY, Pose2D::rotation, InvKinWalkingParameters::CorrectionValues::sideward, Pose2D::translation, InvKinWalkingParameters::CorrectionValues::turning, Vector2< double >::x, and Vector2< double >::y.

Referenced by calculateLegSpeeds().

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.

Referenced by calculateRelativeFootPosition().

Member Data Documentation

InvKinWalkingParameters InvKinWalkingEngine::currentParameters [private]
 

Current parameters of this walk.

Definition at line 65 of file InvKinWalkingEngine.h.

InvKinWalkingParameters* InvKinWalkingEngine::requestedParameters [private]
 

Pointer to the parameter set requested in setParameters.

Definition at line 68 of file InvKinWalkingEngine.h.

Referenced by setParameters().

InvKinWalkingParameters InvKinWalkingEngine::nextParameters [private]
 

Next parameters of this walk, target of current interpolation.

Definition at line 71 of file InvKinWalkingEngine.h.

InvKinWalkingParameters InvKinWalkingEngine::lastParameters [private]
 

Last parameters of this walk, origin of current interpolation.

Definition at line 74 of file InvKinWalkingEngine.h.

int InvKinWalkingEngine::paramInterpolCount [private]
 

Counts parameter set interpolation steps.

Definition at line 77 of file InvKinWalkingEngine.h.

int InvKinWalkingEngine::paramInterpolLength [private]
 

Stores the length of the current parameter set interpolation.

Definition at line 80 of file InvKinWalkingEngine.h.

double InvKinWalkingEngine::positionInWalkCycle [private]
 

Stores precise version of currentStep during interpolation.

Definition at line 83 of file InvKinWalkingEngine.h.

unsigned long InvKinWalkingEngine::lastParametersFromPackageTimeStamp [private]
 

Definition at line 92 of file InvKinWalkingEngine.h.

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

bool InvKinWalkingEngine::footOnGround[4] [private]
 

foot is on ground

Definition at line 99 of file InvKinWalkingEngine.h.

Referenced by calculateFootPositions(), and InvKinWalkingEngine().

double InvKinWalkingEngine::x[4] [private]
 

foot x positions

Definition at line 100 of file InvKinWalkingEngine.h.

double InvKinWalkingEngine::y[4] [private]
 

foot y positions

Definition at line 101 of file InvKinWalkingEngine.h.

double InvKinWalkingEngine::z[4] [private]
 

foot z positions

Definition at line 102 of file InvKinWalkingEngine.h.

Pose2D InvKinWalkingEngine::currentRequest [private]
 

currently executed motion request speeds in mm/s

Definition at line 108 of file InvKinWalkingEngine.h.

Pose2D InvKinWalkingEngine::odometry [private]
 

odometry resulting from current request speed in mm/tick

Definition at line 113 of file InvKinWalkingEngine.h.

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