Tools/Actorics/Kinematics.h

Go to the documentation of this file.

/**
* @file Kinematics.h
*
* Class Kinematics provides methods for robots kinematic calculations
*
* @author Max Risler
* @author Uwe Düffert
*/

#ifndef __Kinematics_h_
#define __Kinematics_h_

#include "Tools/Actorics/RobotVertices.h"
#include "Representations/Motion/JointData.h"
#include "Representations/Perception/SensorData.h"
#include "Representations/Perception/CameraMatrix.h"

#define LEFTLEG(leg) ((leg==Kinematics::fl)||(leg==Kinematics::hl))
#define FORELEG(leg) ((leg==Kinematics::fr)||(leg==Kinematics::fl)||(leg==Kinematics::basefront))
#define LEGBASE(leg) ((leg==Kinematics::basefront)||(leg==Kinematics::basehind))

/**
* @class Kinematics
* Provides Methods for robots kinematic calculations
* @author Max Risler
*/
class Kinematics
{
public:
  enum LegIndex
  {
    fr,fl,hr,hl,basefront,basehind
  };

  enum GroundMode
  {
    constantMode, dynamicMode, staticMode, superMode, flyMode
  };

  /**
  * Calculates leg positon relative to the legs shoulder from given leg joint angles
  * @param leg The leg the that we will calculate with.
  * @param j1,j2,j3 Leg joint angles in rad
  * @param x,y,z Reference to variables for leg position
  * @param correctCalculation shall we calculate correctly and assume that knee is not in x-middle of leg?
  */
  static void legPositionFromJoints(LegIndex leg, double j1, double j2, double j3, double &x, double &y, double &z, bool correctCalculation=true);

  /**
  * Calculates knee positon relative to the legs shoulder from given leg joint angles
  * @param leg The leg the that we will calculate with.
  * @param j1,j2 Leg joint angles in rad
  * @param x,y,z Reference to variables for leg position
  * @param correctCalculation shall we calculate correctly and assume that knee is not in x-middle of leg?
  */
  static void kneePositionFromJoints(LegIndex leg, double j1, double j2, double &x, double &y, double &z, bool correctCalculation=true);

  /**
  * Calculates joint angles from given leg position
  * @param leg The leg the that we will calculate with.
  * @param x,y,z Leg position
  * @param j1,j2,j3 Reference to variables for leg joint angles in rad
  * @param bodyTilt The pre-calculated desired tilt of the robots body
  * @param correctCalculation shall we calculate correctly and assume that knee is not in x-middle of leg?
  * @return false if no possible joint position could be calculated
  */
  static bool jointsFromLegPosition(LegIndex leg, double x, double y, double z, double &j1, double &j2, double &j3, double bodyTilt=0, bool correctCalculation=true);

  /**
  * Calculates all robot vertices relative to the neck from given leg joint angles
  * @param robotVertices the data struct to be filled
  * @param jointData the source joint angles
  */
  static void getRelativeRobotVertices(RobotVertices& robotVertices, const JointData& jointData);

  /**
  * Calculates the transformation of relativeRobotVertices needed to make the robot stand on the grond
  * @param tilt The tilt angle to be calculated
  * @param roll The roll angle to be calculated
  * @param relativeRobotVertices vertices of the robot relative to the neck, e.g. calculated by getRelativeRobotVertices()
  * @param mode Mode of assumption about kind of ground contact
  * @param expectedBodyTilt You may provide an expected tilt angle of the body
  * @param expectedBodyRoll You may provide an expected roll angle of the body
  */
  static void getRobotTransformation(double& tilt,double& roll, const RobotVertices& relativeRobotVertices, const GroundMode mode, double expectedBodyTilt=0, double expectedBodyRoll=0);

  /**
  * Calculates all robot vertices to ground from given leg joint angles
  * @param robotVertices the data struct to be filled
  * @param mode Mode of assumption about kind of ground contact
  * @param jointData the source joint angles
  * @param expectedBodyTilt You may provide an expected tilt angle of the body
  * @param expectedBodyRoll You may provide an expected roll angle of the body
  */
  static void getAbsoluteRobotVertices(RobotVertices& robotVertices, const GroundMode mode, const JointData& jointData, double expectedBodyTilt=0, double expectedBodyRoll=0);

  /**
  * 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.
  * @param rob The robot vertices that are calculated.
  * @param sensorData The joint positions of the robot.
  */
  static void calcRelativeRobotVertices(RobotVertices& rob, const SensorData& sensorData);

  /**
  * The function calculates the body tilt and the position of the feet and the neck of the robot.
  * bodyRoll == 0 is assumed.
  * @param sensorData The joint positions of the robot.
    * @param rob The position of the feet, knees, and the neck.
  */
  static void calcNeckAndLegPositions(const SensorData& sensorData, RobotVertices& rob);

  /**
  * The function calculates the camera matrix based on sensor data and the position of the neck.
  * @param tilt head tilt arc.
  * @param pan head pan arc.
  * @param roll head roll or tilt2 arc.
  * @param bodyTilt The height of the neck.
  * @param neckHeight The height of the neck.
  * @param cameraMatrix The resulting camera matrix.
  */
  static void calculateCameraMatrix(const double tilt, const double pan, const double roll, const double bodyTilt, const double neckHeight, CameraMatrix& cameraMatrix);

  /**
  * The function calculates the height of the Tilt2Center based on the sensor data.
  * @param headHeight The resulting headheight
  * @param bodyTilt The bodytilt the headheight is calculated for
  * @param neckHeight The neckheight the headheight is calculated for
  * @param sensorData The joint angles the headheight is calculated for.
  */
  static void calcHeadHeight(double& headHeight, const double bodyTilt, const double neckHeight, const SensorData& sensorData);

  /**
  * The function calculates the height of the nose based on the sensor data.
  * @param noseHeight The resulting noseheight
  * @param absHeadTilt The absolute headtilt the noseheight is calculated for
  * @param headHeight The headheight the noseheight is calculated for
  * @param bodyTilt The bodytilt the noseheight is calculated for
  * @param headRoll The headRoll the noseheight is calculated for
  */
  static void calcNoseHeight(double& noseHeight, const double absHeadTilt, const double headHeight, const double bodyTilt, const double headRoll);

  /**
  * The function calculates the absolute roll (Tilt in the head) based on the sensor data.
  * @param absRoll The resulting absolute roll
  * @param bodyTilt The bodytilt the tilt is calculated for.
  * @param sensorData The joint angles the tikt is calculated for.
  */
  static void calcAbsRoll(double& absRoll, const double bodyTilt, const SensorData& sensorData);

  /**
  * The function calculates the highest possible necktilt to reach the absolute headtilt.
  * @param absRoll The wanted absolute headroll
  * @param bodyTilt The bodytilt the tilt is calculated for.
  */
  static double calcHighestPossibleTilt(const double absRoll, const double bodyTilt);

  /**
  * The function calculates the lowest possible necktilt to reach the absolute headtilt.
  * @param absRoll The absolute roll
  * @param bodyTilt The bodytilt the tilt is calculated for.
  */
  static double calcLowestPossibleTilt(const double absRoll, const double bodyTilt);

    /**
    * The function calculates joints so that the head has a @param absRoll 
    * @param neckTilt The best fitting headTilt to reach absRoll
    * @param absRoll The resulting absolute headRoll
    * @param bodyTilt The body tilt.
  */
  static void calcAbsRollJoints(double& neckTilt, double& absRoll, const double bodyTilt);

private:
  static char *stringFromLeg(LegIndex leg);
  static void transformToLegBase(LegIndex leg,double &x, double &y, double &z, double &lowerleg);
  static 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);
};

#endif // __Kinematics_h_

---