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(), GT2005WalkingEngine::readDebugData(), and GT2005WalkingEngine::readResampleData().

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(), GT2005WalkingEngine::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 GT2005SensorDataProcessor::calculateBodyPostureFromLegSensors().

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 GT2005SensorDataProcessor::buildCameraMatrix().

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:

- Tools/Actorics/Kinematics.h
- Tools/Actorics/Kinematics.cpp