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

# Tools/Actorics/Kinematics.h

Go to the documentation of this file.

    00001 /**
    00002 * @file Kinematics.h
    00003 *
    00004 * Class Kinematics provides methods for robots kinematic calculations
    00005 *
    00006 * @author Max Risler
    00007 * @author Uwe Düffert
    00008 */
    00009
    00010 #ifndef __Kinematics_h_
    00011 #define __Kinematics_h_
    00012
    00013 #include "Tools/Actorics/RobotVertices.h"
    00014 #include "Representations/Motion/JointData.h"
    00015 #include "Representations/Perception/SensorData.h"
    00016 #include "Representations/Perception/CameraMatrix.h"
    00017
    00018 #define LEFTLEG(leg) ((leg==Kinematics::fl)||(leg==Kinematics::hl))
    00019 #define FORELEG(leg) ((leg==Kinematics::fr)||(leg==Kinematics::fl)||(leg==Kinematics::basefront))
    00020 #define LEGBASE(leg) ((leg==Kinematics::basefront)||(leg==Kinematics::basehind))
    00021
    00022 /**
    00023 * @class Kinematics
    00024 * Provides Methods for robots kinematic calculations
    00025 * @author Max Risler
    00026 */
    00027 class Kinematics
    00028 {
    00029 public:
    00030   enum LegIndex
    00031   {
    00032     fr,fl,hr,hl,basefront,basehind
    00033   };
    00034
    00035   enum GroundMode
    00036   {
    00037     constantMode, dynamicMode, staticMode, superMode, flyMode
    00038   };
    00039
    00040   /**
    00041   * Calculates leg positon relative to the legs shoulder from given leg joint angles
    00042   * @param leg The leg the that we will calculate with.
    00043   * @param j1,j2,j3 Leg joint angles in rad
    00044   * @param x,y,z Reference to variables for leg position
    00045   * @param correctCalculation shall we calculate correctly and assume that knee is not in x-middle of leg?
    00046   */
    00047   static void legPositionFromJoints(LegIndex leg, double j1, double j2, double j3, double &x, double &y, double &z, bool correctCalculation=true);
    00048
    00049   /**
    00050   * Calculates knee positon relative to the legs shoulder from given leg joint angles
    00051   * @param leg The leg the that we will calculate with.
    00052   * @param j1,j2 Leg joint angles in rad
    00053   * @param x,y,z Reference to variables for leg position
    00054   * @param correctCalculation shall we calculate correctly and assume that knee is not in x-middle of leg?
    00055   */
    00056   static void kneePositionFromJoints(LegIndex leg, double j1, double j2, double &x, double &y, double &z, bool correctCalculation=true);
    00057
    00058   /**
    00059   * Calculates joint angles from given leg position
    00060   * @param leg The leg the that we will calculate with.
    00061   * @param x,y,z Leg position
    00062   * @param j1,j2,j3 Reference to variables for leg joint angles in rad
    00063   * @param bodyTilt The pre-calculated desired tilt of the robots body
    00064   * @param correctCalculation shall we calculate correctly and assume that knee is not in x-middle of leg?
    00065   * @return false if no possible joint position could be calculated
    00066   */
    00067   static bool jointsFromLegPosition(LegIndex leg, double x, double y, double z, double &j1, double &j2, double &j3, double bodyTilt=0, bool correctCalculation=true);
    00068
    00069   /**
    00070   * Calculates all robot vertices relative to the neck from given leg joint angles
    00071   * @param robotVertices the data struct to be filled
    00072   * @param jointData the source joint angles
    00073   */
    00074   static void getRelativeRobotVertices(RobotVertices& robotVertices, const JointData& jointData);
    00075
    00076   /**
    00077   * Calculates the transformation of relativeRobotVertices needed to make the robot stand on the grond
    00078   * @param tilt The tilt angle to be calculated
    00079   * @param roll The roll angle to be calculated
    00080   * @param relativeRobotVertices vertices of the robot relative to the neck, e.g. calculated by getRelativeRobotVertices()
    00081   * @param mode Mode of assumption about kind of ground contact
    00082   * @param expectedBodyTilt You may provide an expected tilt angle of the body
    00083   * @param expectedBodyRoll You may provide an expected roll angle of the body
    00084   */
    00085   static void getRobotTransformation(double& tilt,double& roll, const RobotVertices& relativeRobotVertices, const GroundMode mode, double expectedBodyTilt=0, double expectedBodyRoll=0);
    00086
    00087   /**
    00088   * Calculates all robot vertices to ground from given leg joint angles
    00089   * @param robotVertices the data struct to be filled
    00090   * @param mode Mode of assumption about kind of ground contact
    00091   * @param jointData the source joint angles
    00092   * @param expectedBodyTilt You may provide an expected tilt angle of the body
    00093   * @param expectedBodyRoll You may provide an expected roll angle of the body
    00094   */
    00095   static void getAbsoluteRobotVertices(RobotVertices& robotVertices, const GroundMode mode, const JointData& jointData, double expectedBodyTilt=0, double expectedBodyRoll=0);
    00096
    00097   /**
    00098   * The function calculates the relative positions of possible floor contact points.
    00099   * In contrast to getRelativeRobotVertices(), it works correctly.
    00100   * All coordinates are relative to the neck joint.
    00101   * @param rob The robot vertices that are calculated.
    00102   * @param sensorData The joint positions of the robot.
    00103   */
    00104   static void calcRelativeRobotVertices(RobotVertices& rob, const SensorData& sensorData);
    00105
    00106   /**
    00107   * The function calculates the body tilt and the position of the feet and the neck of the robot.
    00108   * bodyRoll == 0 is assumed.
    00109   * @param sensorData The joint positions of the robot.
    00110     * @param rob The position of the feet, knees, and the neck.
    00111   */
    00112   static void calcNeckAndLegPositions(const SensorData& sensorData, RobotVertices& rob);
    00113
    00114   /**
    00115   * The function calculates the camera matrix based on sensor data and the position of the neck.
    00116   * @param tilt head tilt arc.
    00117   * @param pan head pan arc.
    00118   * @param roll head roll or tilt2 arc.
    00119   * @param bodyTilt The height of the neck.
    00120   * @param neckHeight The height of the neck.
    00121   * @param cameraMatrix The resulting camera matrix.
    00122   */
    00123   static void calculateCameraMatrix(const double tilt, const double pan, const double roll, const double bodyTilt, const double neckHeight, CameraMatrix& cameraMatrix);
    00124
    00125   /**
    00126   * The function calculates the height of the Tilt2Center based on the sensor data.
    00127   * @param headHeight The resulting headheight
    00128   * @param bodyTilt The bodytilt the headheight is calculated for
    00129   * @param neckHeight The neckheight the headheight is calculated for
    00130   * @param sensorData The joint angles the headheight is calculated for.
    00131   */
    00132   static void calcHeadHeight(double& headHeight, const double bodyTilt, const double neckHeight, const SensorData& sensorData);
    00133
    00134   /**
    00135   * The function calculates the height of the nose based on the sensor data.
    00136   * @param noseHeight The resulting noseheight
    00137   * @param absHeadTilt The absolute headtilt the noseheight is calculated for
    00138   * @param headHeight The headheight the noseheight is calculated for
    00139   * @param bodyTilt The bodytilt the noseheight is calculated for
    00140   * @param headRoll The headRoll the noseheight is calculated for
    00141   */
    00142   static void calcNoseHeight(double& noseHeight, const double absHeadTilt, const double headHeight, const double bodyTilt, const double headRoll);
    00143
    00144   /**
    00145   * The function calculates the absolute roll (Tilt in the head) based on the sensor data.
    00146   * @param absRoll The resulting absolute roll
    00147   * @param bodyTilt The bodytilt the tilt is calculated for.
    00148   * @param sensorData The joint angles the tikt is calculated for.
    00149   */
    00150   static void calcAbsRoll(double& absRoll, const double bodyTilt, const SensorData& sensorData);
    00151
    00152   /**
    00153   * The function calculates the highest possible necktilt to reach the absolute headtilt.
    00154   * @param absRoll The wanted absolute headroll
    00155   * @param bodyTilt The bodytilt the tilt is calculated for.
    00156   */
    00157   static double calcHighestPossibleTilt(const double absRoll, const double bodyTilt);
    00158
    00159   /**
    00160   * The function calculates the lowest possible necktilt to reach the absolute headtilt.
    00161   * @param absRoll The absolute roll
    00162   * @param bodyTilt The bodytilt the tilt is calculated for.
    00163   */
    00164   static double calcLowestPossibleTilt(const double absRoll, const double bodyTilt);
    00165
    00166     /**
    00167     * The function calculates joints so that the head has a @param absRoll
    00168     * @param neckTilt The best fitting headTilt to reach absRoll
    00169     * @param absRoll The resulting absolute headRoll
    00170     * @param bodyTilt The body tilt.
    00171   */
    00172   static void calcAbsRollJoints(double& neckTilt, double& absRoll, const double bodyTilt);
    00173
    00174 private:
    00175   static char *stringFromLeg(LegIndex leg);
    00176   static void transformToLegBase(LegIndex leg,double &x, double &y, double &z, double &lowerleg);
    00177   static void calcLegPosition(Vector3<double>& kneePosition, Vector3<double>& footPosition,
    00178     long joint1, long joint2, long joint3,
    00179     const Vector3<double> shoulder,
    00180     const Vector3<double>& upper, const Vector3<double>& lower);
    00181 };
    00182
    00183 #endif // __Kinematics_h_