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_

/*
* Change log :
* 
* $Log: Kinematics.h,v $
* Revision 1.5  2004/09/09 10:15:58  spranger
* fixed doxygen-errors
*
* Revision 1.4  2004/05/26 17:40:31  dueffert
* HeadState is replaced by BodyPosture and MotionInfo
*
* Revision 1.3  2004/05/26 17:32:17  dueffert
* better data types used
*
* Revision 1.2  2004/05/26 15:54:02  dueffert
* camera matrix calculation cleaned up
*
* Revision 1.1.1.1  2004/05/22 17:35:57  cvsadm
* created new repository GT2004_WM
*
* Revision 1.11  2004/03/22 15:56:14  hodie
* Fixed calcAbsRollJoints
*
* Revision 1.10  2004/03/20 09:55:27  roefer
* Preparation for improved odometry
*
* Revision 1.9  2004/03/18 18:07:21  hodie
* Some more absolute HeadControlModes
* Absolute HeadPathPlanner
*
* Revision 1.8  2004/03/17 16:21:12  hodie
* Some more absolute HeadControlModes
* Absolute HeadPathPlanner
*
* Revision 1.7  2004/03/15 15:09:46  hodie
* new absRoll and absStraightRoll
* some new functions and tools
* modified lookArounds
*
* Revision 1.6  2004/03/04 15:56:22  hodie
* added calc -AbsTilt, -HeadHeight, and -NoseHeight
*
* Revision 1.5  2004/02/04 13:41:33  roefer
* Some place holders for BB2004 modules added
*
* Revision 1.4  2004/01/28 21:55:50  roefer
* RobotDimensions revised
*
* Revision 1.3  2003/12/19 15:55:13  dueffert
* added default parameter for correct kinematics, so old version can still be used
*
* Revision 1.2  2003/12/09 14:21:48  dueffert
* flyMode added to visualize theoretical footPositions in RadarViewer better
*
* Revision 1.1  2003/10/07 10:13:21  cvsadm
* Created GT2004 (M.J.)
*
* Revision 1.3  2003/09/30 10:51:11  dueffert
* typos fixed
*
* Revision 1.2  2003/09/26 15:28:10  juengel
* Renamed DataTypes to representations.
*
* Revision 1.1  2003/09/26 11:40:40  juengel
* - sorted tools
* - clean-up in DataTypes
*
* Revision 1.4  2003/09/16 14:57:41  dueffert
* tilt calculation in forward kinematics added
*
* Revision 1.3  2003/09/16 11:08:52  dueffert
* doxygen comments improved
*
* Revision 1.2  2003/09/15 20:38:08  dueffert
* forward kinematics improved
*
* Revision 1.1.1.1  2003/07/02 09:40:28  cvsadm
* created new repository for the competitions in Padova from the 
* tamara CVS (Tuesday 2:00 pm)
*
* removed unused solutions
*
* Revision 1.8  2003/03/03 17:34:57  risler
* legPositionFromJoints adjusted to changed leg coordinate system
*
* Revision 1.7  2003/01/17 16:23:06  dueffert
* started to add ground based calculations
*
* Revision 1.6  2002/11/27 13:52:17  dueffert
* doxygen docu corrected
*
* Revision 1.5  2002/11/25 12:32:56  dueffert
* bodyTilt should be used to calculate leg positions from joints and vice versa
*
* Revision 1.4  2002/11/19 15:43:03  dueffert
* doxygen comments corrected
*
* Revision 1.3  2002/10/14 13:14:24  dueffert
* doxygen comments corrected
*
* Revision 1.2  2002/09/22 18:40:53  risler
* added new math functions, removed GTMath library
*
* Revision 1.1  2002/09/10 15:53:58  cvsadm
* Created new project GT2003 (M.L.)
* - Cleaned up the /Src/DataTypes directory
* - Removed challenge related source code
* - Removed processing of incoming audio data
* - Renamed AcousticMessage to SoundRequest
*
* Revision 1.1.1.1  2002/05/10 12:40:32  cvsadm
* Moved GT2002 Project from ute to tamara.
*
* Revision 1.1  2002/04/04 11:02:16  risler
* added class Kinematics
*
*
*/

---