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

Kinematics Class Reference

Provides Methods for robots kinematic calculations. More...

#include <Kinematics.h>

List of all members.

Public Types

enum  LegIndex {
  fr, fl, hr,
  hl, basefront, basehind
}
enum  GroundMode {
  constantMode, dynamicMode, staticMode,
  superMode, flyMode
}

Static Public Member Functions

void legPositionFromJoints (LegIndex leg, double j1, double j2, double j3, double &x, double &y, double &z, bool correctCalculation=true)
 Calculates leg positon relative to the legs shoulder from given leg joint angles.

void kneePositionFromJoints (LegIndex leg, double j1, double j2, double &x, double &y, double &z, bool correctCalculation=true)
 Calculates knee positon relative to the legs shoulder from given leg joint angles.

bool jointsFromLegPosition (LegIndex leg, double x, double y, double z, double &j1, double &j2, double &j3, double bodyTilt=0, bool correctCalculation=true)
 Calculates joint angles from given leg position.

void getRelativeRobotVertices (RobotVertices &robotVertices, const JointData &jointData)
 Calculates all robot vertices relative to the neck from given leg joint angles.

void getRobotTransformation (double &tilt, double &roll, const RobotVertices &relativeRobotVertices, const GroundMode mode, double expectedBodyTilt=0, double expectedBodyRoll=0)
 Calculates the transformation of relativeRobotVertices needed to make the robot stand on the grond.

void getAbsoluteRobotVertices (RobotVertices &robotVertices, const GroundMode mode, const JointData &jointData, double expectedBodyTilt=0, double expectedBodyRoll=0)
 Calculates all robot vertices to ground from given leg joint angles.

void calcRelativeRobotVertices (RobotVertices &rob, const SensorData &sensorData)
 The function calculates the relative positions of possible floor contact points.

void calcNeckAndLegPositions (const SensorData &sensorData, RobotVertices &rob)
 The function calculates the body tilt and the position of the feet and the neck of the robot.

void calculateCameraMatrix (const double tilt, const double pan, const double roll, const double bodyTilt, const double neckHeight, CameraMatrix &cameraMatrix)
 The function calculates the camera matrix based on sensor data and the position of the neck.

void calcHeadHeight (double &headHeight, const double bodyTilt, const double neckHeight, const SensorData &sensorData)
 The function calculates the height of the Tilt2Center based on the sensor data.

void calcNoseHeight (double &noseHeight, const double absHeadTilt, const double headHeight, const double bodyTilt, const double headRoll)
 The function calculates the height of the nose based on the sensor data.

void calcAbsRoll (double &absRoll, const double bodyTilt, const SensorData &sensorData)
 The function calculates the absolute roll (Tilt in the head) based on the sensor data.

double calcHighestPossibleTilt (const double absRoll, const double bodyTilt)
 The function calculates the highest possible necktilt to reach the absolute headtilt.

double calcLowestPossibleTilt (const double absRoll, const double bodyTilt)
 The function calculates the lowest possible necktilt to reach the absolute headtilt.

void calcAbsRollJoints (double &neckTilt, double &absRoll, const double bodyTilt)
 The function calculates joints so that the head has a.


Static Private Member Functions

char * stringFromLeg (LegIndex leg)
void transformToLegBase (LegIndex leg, double &x, double &y, double &z, double &lowerleg)
void calcLegPosition (Vector3< double > &kneePosition, Vector3< double > &footPosition, long joint1, long joint2, long joint3, const Vector3< double > shoulder, const Vector3< double > &upper, const Vector3< double > &lower)

Detailed Description

Provides Methods for robots kinematic calculations.

Author:
Max Risler

Definition at line 27 of file Kinematics.h.

Member Enumeration Documentation

enum Kinematics::LegIndex
 

Enumeration values:
fr 
fl 
hr 
hl 
basefront 
basehind 

Definition at line 30 of file Kinematics.h.

enum Kinematics::GroundMode
 

Enumeration values:
constantMode 
dynamicMode 
staticMode 
superMode 
flyMode 

Definition at line 35 of file Kinematics.h.

Member Function Documentation

void Kinematics::legPositionFromJoints (  LegIndex  leg,
double  j1,
double  j2,
double  j3,
double &  x,
double &  y,
double &  z,
bool  correctCalculation = true
)  [static]
 

Calculates leg positon relative to the legs shoulder from given leg joint angles.

Parameters:
leg The leg the that we will calculate with.
j1,j2,j3 Leg joint angles in rad
x,y,z Reference to variables for leg position
correctCalculation shall we calculate correctly and assume that knee is not in x-middle of leg?

Definition at line 205 of file Kinematics.cpp.

References FORELEG, getRobotConfiguration(), RobotConfiguration::getRobotDimensions(), LEFTLEG, LEGBASE, RobotDimensions::lowerForeLegLength, RobotDimensions::lowerHindLegLength, RobotDimensions::upperLegLength, RobotDimensions::zeroFrontKneeArc, RobotDimensions::zeroHindKneeArc, and RobotDimensions::zeroShoulderArc.

Referenced by getRelativeRobotVertices().

Here is the call graph for this function:

void Kinematics::kneePositionFromJoints (  LegIndex  leg,
double  j1,
double  j2,
double &  x,
double &  y,
double &  z,
bool  correctCalculation = true
)  [static]
 

Calculates knee positon relative to the legs shoulder from given leg joint angles.

Parameters:
leg The leg the that we will calculate with.
j1,j2 Leg joint angles in rad
x,y,z Reference to variables for leg position
correctCalculation shall we calculate correctly and assume that knee is not in x-middle of leg?

Definition at line 246 of file Kinematics.cpp.

References FORELEG, getRobotConfiguration(), RobotConfiguration::getRobotDimensions(), LEFTLEG, LEGBASE, RobotDimensions::upperLegLength, and RobotDimensions::zeroShoulderArc.

Referenced by getRelativeRobotVertices().

Here is the call graph for this function:

bool Kinematics::jointsFromLegPosition (  LegIndex  leg,
double  x,
double  y,
double  z,
double &  j1,
double &  j2,
double &  j3,
double  bodyTilt = 0,
bool  correctCalculation = true
)  [static]
 

Calculates joint angles from given leg position.

Parameters:
leg The leg the that we will calculate with.
x,y,z Leg position
j1,j2,j3 Reference to variables for leg joint angles in rad
bodyTilt The pre-calculated desired tilt of the robots body
correctCalculation shall we calculate correctly and assume that knee is not in x-middle of leg?
Returns:
false if no possible joint position could be calculated

Definition at line 292 of file Kinematics.cpp.

References FORELEG, getRobotConfiguration(), RobotConfiguration::getRobotDimensions(), RobotDimensions::jointLimitLeg1FN, RobotDimensions::jointLimitLeg1FP, RobotDimensions::jointLimitLeg1HN, RobotDimensions::jointLimitLeg1HP, RobotDimensions::jointLimitLeg2N, RobotDimensions::jointLimitLeg2P, RobotDimensions::jointLimitLeg3N, RobotDimensions::jointLimitLeg3P, pi_2, transformToLegBase(), RobotDimensions::upperLegLength, RobotDimensions::zeroFrontKneeArc, RobotDimensions::zeroHindKneeArc, and RobotDimensions::zeroShoulderArc.

Referenced by InvKinWalkingEngine::calculateData(), and GT2004WalkingEngine::calculateData().

Here is the call graph for this function:

void Kinematics::getRelativeRobotVertices (  RobotVertices &  robotVertices,
const JointData &  jointData
)  [static]
 

Calculates all robot vertices relative to the neck from given leg joint angles.

Parameters:
robotVertices the data struct to be filled
jointData the source joint angles

Definition at line 17 of file Kinematics.cpp.

References JointData::data, fl, RobotVertices::footPosition, fr, fromMicroRad(), getRobotConfiguration(), hl, hr, RobotVertices::kneePosition, kneePositionFromJoints(), legPositionFromJoints(), RobotVertices::shoulderPosition, Vector3< double >::x, Vector3< double >::y, and Vector3< double >::z.

Referenced by getAbsoluteRobotVertices().

Here is the call graph for this function:

void Kinematics::getRobotTransformation (  double &  tilt,
double &  roll,
const RobotVertices &  relativeRobotVertices,
const GroundMode  mode,
double  expectedBodyTilt = 0,
double  expectedBodyRoll = 0
)  [static]
 

Calculates the transformation of relativeRobotVertices needed to make the robot stand on the grond.

Parameters:
tilt The tilt angle to be calculated
roll The roll angle to be calculated
relativeRobotVertices vertices of the robot relative to the neck, e.g. calculated by getRelativeRobotVertices()
mode Mode of assumption about kind of ground contact
expectedBodyTilt You may provide an expected tilt angle of the body
expectedBodyRoll You may provide an expected roll angle of the body

Todo:
calulate lower feet diagonale calculate lower supporting feet triangle by checking on which side of the diagonale the center of gravity is

thats quite tricky, but we may just check falling to side/front/hind and return staticMode solution otherwise

Definition at line 74 of file Kinematics.cpp.

References Vector2< V >::angle(), constantMode, dynamicMode, flyMode, RobotVertices::footPosition, RobotVertices::kneePosition, staticMode, superMode, Vector3< double >::x, and Vector3< double >::z.

Referenced by calcNeckAndLegPositions(), and getAbsoluteRobotVertices().

Here is the call graph for this function:

void Kinematics::getAbsoluteRobotVertices (  RobotVertices &  robotVertices,
const GroundMode  mode,
const JointData &  jointData,
double  expectedBodyTilt = 0,
double  expectedBodyRoll = 0
)  [static]
 

Calculates all robot vertices to ground from given leg joint angles.

Parameters:
robotVertices the data struct to be filled
mode Mode of assumption about kind of ground contact
jointData the source joint angles
expectedBodyTilt You may provide an expected tilt angle of the body
expectedBodyRoll You may provide an expected roll angle of the body

Definition at line 146 of file Kinematics.cpp.

References Vector2< V >::abs(), Vector2< V >::angle(), RobotVertices::bodyRoll, RobotVertices::bodyTilt, flyMode, RobotVertices::footPosition, getRelativeRobotVertices(), getRobotConfiguration(), RobotConfiguration::getRobotDimensions(), getRobotTransformation(), RobotVertices::kneePosition, RobotVertices::neck, RobotVertices::shoulderPosition, Vector3< double >::x, Vector3< double >::y, Vector3< double >::z, and RobotDimensions::zeroBodyTilt.

Here is the call graph for this function:

void Kinematics::calcRelativeRobotVertices (  RobotVertices &  rob,
const SensorData &  sensorData
)  [static]
 

The function calculates the relative positions of possible floor contact points.

In contrast to getRelativeRobotVertices(), it works correctly. All coordinates are relative to the neck joint.

Parameters:
rob The robot vertices that are calculated.
sensorData The joint positions of the robot.

Definition at line 442 of file Kinematics.cpp.

References calcLegPosition(), SensorData::data, fl, RobotVertices::footPosition, RobotDimensions::footRadius, fr, SensorData::frameNumber, RobotVertices::frameNumber, getRobotConfiguration(), RobotConfiguration::getRobotDimensions(), hl, hr, RobotVertices::kneePosition, RobotDimensions::kneeRadius, RobotDimensions::lengthBetweenLegs, RobotDimensions::lowerForeLegLength, RobotDimensions::lowerForeLegLengthX, RobotDimensions::lowerForeLegLengthZ, RobotDimensions::lowerHindLegLength, RobotDimensions::lowerHindLegLengthX, RobotDimensions::lowerHindLegLengthZ, RobotDimensions::neckToLegsLengthX, RobotDimensions::neckToLegsLengthZ, RobotVertices::shoulderPosition, RobotDimensions::shoulderWidth, RobotDimensions::specsKneeArc, RobotDimensions::upperLegLengthX, RobotDimensions::upperLegLengthY, RobotDimensions::upperLegLengthZ, and Vector3< double >::z.

Referenced by calcNeckAndLegPositions().

Here is the call graph for this function:

void Kinematics::calcNeckAndLegPositions (  const SensorData &  sensorData,
RobotVertices &  rob
)  [static]
 

The function calculates the body tilt and the position of the feet and the neck of the robot.

bodyRoll == 0 is assumed.

Parameters:
sensorData The joint positions of the robot.
rob The position of the feet, knees, and the neck.

Definition at line 487 of file Kinematics.cpp.

References RobotVertices::bodyRoll, RobotVertices::bodyTilt, calcRelativeRobotVertices(), dynamicMode, RobotVertices::footPosition, getRobotConfiguration(), RobotConfiguration::getRobotDimensions(), getRobotTransformation(), RobotVertices::kneePosition, RobotVertices::neckHeight, Pose3D::rotateY(), RobotVertices::shoulderPosition, RobotDimensions::shoulderRadius, and Vector3< double >::z.

Referenced by GT2004SensorDataProcessor::calculateBodyPostureFromLegSensors(), BB2004Calibrator::evolve(), and BB2004InvKinWalkingEngine::updateOdometry().

Here is the call graph for this function:

void Kinematics::calculateCameraMatrix (  const double  tilt,
const double  pan,
const double  roll,
const double  bodyTilt,
const double  neckHeight,
CameraMatrix &  cameraMatrix
)  [static]
 

The function calculates the camera matrix based on sensor data and the position of the neck.

Parameters:
tilt head tilt arc.
pan head pan arc.
roll head roll or tilt2 arc.
bodyTilt The height of the neck.
neckHeight The height of the neck.
cameraMatrix The resulting camera matrix.

Todo:
test if it is useful (faster) to expand these formulas:

Definition at line 529 of file Kinematics.cpp.

References RobotCalibration::bodyRollOffset, RobotCalibration::bodyTiltOffset, RobotDimensions::distanceNeckToPanCenter, RobotDimensions::distancePanCenterToCameraX, RobotDimensions::distancePanCenterToCameraZ, RobotConfiguration::getRobotCalibration(), getRobotConfiguration(), RobotConfiguration::getRobotDesign(), RobotConfiguration::getRobotDimensions(), RobotCalibration::headRollOffset, RobotCalibration::headTiltOffset, RobotCalibration::panFactor, RobotCalibration::tilt2Factor, and RobotCalibration::tiltFactor.

Referenced by GT2004SensorDataProcessor::buildCameraMatrix(), and BB2004Calibrator::evolve().

Here is the call graph for this function:

void Kinematics::calcHeadHeight (  double &  headHeight,
const double  bodyTilt,
const double  neckHeight,
const SensorData &  sensorData
)  [static]
 

The function calculates the height of the Tilt2Center based on the sensor data.

Parameters:
headHeight The resulting headheight
bodyTilt The bodytilt the headheight is calculated for
neckHeight The neckheight the headheight is calculated for
sensorData The joint angles the headheight is calculated for.

Definition at line 561 of file Kinematics.cpp.

References SensorData::data, RobotDimensions::distanceNeckToPanCenter, fromMicroRad(), getRobotConfiguration(), RobotConfiguration::getRobotDimensions(), and pi.

Here is the call graph for this function:

void Kinematics::calcNoseHeight (  double &  noseHeight,
const double  absHeadTilt,
const double  headHeight,
const double  bodyTilt,
const double  headRoll
)  [static]
 

The function calculates the height of the nose based on the sensor data.

Parameters:
noseHeight The resulting noseheight
absHeadTilt The absolute headtilt the noseheight is calculated for
headHeight The headheight the noseheight is calculated for
bodyTilt The bodytilt the noseheight is calculated for
headRoll The headRoll the noseheight is calculated for

Definition at line 574 of file Kinematics.cpp.

References RobotDimensions::distancePanCenterToCameraX, getRobotConfiguration(), and RobotConfiguration::getRobotDimensions().

Here is the call graph for this function:

void Kinematics::calcAbsRoll (  double &  absRoll,
const double  bodyTilt,
const SensorData &  sensorData
)  [static]
 

The function calculates the absolute roll (Tilt in the head) based on the sensor data.

Parameters:
absRoll The resulting absolute roll
bodyTilt The bodytilt the tilt is calculated for.
sensorData The joint angles the tikt is calculated for.

Definition at line 584 of file Kinematics.cpp.

References SensorData::data, and fromMicroRad().

Here is the call graph for this function:

double Kinematics::calcHighestPossibleTilt (  const double  absRoll,
const double  bodyTilt
)  [static]
 

The function calculates the highest possible necktilt to reach the absolute headtilt.

Parameters:
absRoll The wanted absolute headroll
bodyTilt The bodytilt the tilt is calculated for.

Definition at line 589 of file Kinematics.cpp.

References getRobotConfiguration(), RobotConfiguration::getRobotDimensions(), RobotDimensions::jointLimitHeadTiltN, and RobotDimensions::jointLimitNeckTiltP.

Referenced by calcAbsRollJoints().

Here is the call graph for this function:

double Kinematics::calcLowestPossibleTilt (  const double  absRoll,
const double  bodyTilt
)  [static]
 

The function calculates the lowest possible necktilt to reach the absolute headtilt.

Parameters:
absRoll The absolute roll
bodyTilt The bodytilt the tilt is calculated for.

Definition at line 603 of file Kinematics.cpp.

References getRobotConfiguration(), RobotConfiguration::getRobotDimensions(), RobotDimensions::jointLimitHeadTiltP, and RobotDimensions::jointLimitNeckTiltN.

Referenced by calcAbsRollJoints().

Here is the call graph for this function:

void Kinematics::calcAbsRollJoints (  double &  neckTilt,
double &  absRoll,
const double  bodyTilt
)  [static]
 

The function calculates joints so that the head has a.

Parameters:
absRoll 
neckTilt The best fitting headTilt to reach absRoll
absRoll The resulting absolute headRoll
bodyTilt The body tilt.

Definition at line 618 of file Kinematics.cpp.

References calcHighestPossibleTilt(), calcLowestPossibleTilt(), getRobotConfiguration(), RobotConfiguration::getRobotDimensions(), RobotDimensions::jointLimitHeadTiltN, and RobotDimensions::jointLimitHeadTiltP.

Here is the call graph for this function:

char * Kinematics::stringFromLeg (  LegIndex  leg  )  [static, private]
 

Definition at line 423 of file Kinematics.cpp.

References basefront, basehind, fl, fr, hl, and hr.

void Kinematics::transformToLegBase (  LegIndex  leg,
double &  x,
double &  y,
double &  z,
double &  lowerleg
)  [static, private]
 

Definition at line 273 of file Kinematics.cpp.

References RobotDimensions::bodyWidth, FORELEG, getRobotConfiguration(), RobotConfiguration::getRobotDimensions(), LEFTLEG, LEGBASE, RobotDimensions::lowerForeLegLength, and RobotDimensions::lowerHindLegLength.

Referenced by jointsFromLegPosition().

Here is the call graph for this function:

void Kinematics::calcLegPosition (  Vector3< double > &  kneePosition,
Vector3< double > &  footPosition,
long  joint1,
long  joint2,
long  joint3,
const Vector3< double >  shoulder,
const Vector3< double > &  upper,
const Vector3< double > &  lower
)  [static, private]
 

Definition at line 519 of file Kinematics.cpp.

References e, Pose3D::rotateY(), Pose3D::translate(), and Pose3D::translation.

Referenced by calcRelativeRobotVertices().

Here is the call graph for this function:

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