humanoid_functions.h

Go to the documentation of this file.

/**************************************************************************************************
 Software License Agreement (BSD License)

 Copyright (c) 2011-2013, LAR toolkit developers - University of Aveiro - http://lars.mec.ua.pt
 All rights reserved.

 Redistribution and use in source and binary forms, with or without modification, are permitted
 provided that the following conditions are met:

  *Redistributions of source code must retain the above copyright notice, this list of
   conditions and the following disclaimer.
  *Redistributions in binary form must reproduce the above copyright notice, this list of
   conditions and the following disclaimer in the documentation and/or other materials provided
   with the distribution.
  *Neither the name of the University of Aveiro nor the names of its contributors may be used to
   endorse or promote products derived from this software without specific prior written permission.
 
 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR
 IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
 FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
 CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
 IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
 OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
***************************************************************************************************/
#ifndef _HUMANOID_FUNCTIONS_H_
#define _HUMANOID_FUNCTIONS_H_

//Defines for left or right side limbs
#define LEFT     1
#define RIGHT   -1

// Define for dimension extraction
#define ARM_LENGTH      100
#define FOREARM_LENGTH  110
#define LEG_LENGTH      200
#define THIGH_LENGTH    210
#define FOOT_LENGTH     220
#define ANKLE_HEIGHT    230
#define FOOT_WIDTH      240

// #define PFLN {printf("Passing through File [%s] @ Line [%d].\n", __FILE__, __LINE__);}

//includes
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <iostream>

#include <Eigen/Dense>

class humanoid
{
  public:
    humanoid(void);
    
    ~humanoid(void);
    
    //FUNCTIONS FOR INVERSE/DIRECT KINEMATICS
    void Dir_Kin_3DOF_Arm(double t1, double t2, double t3, short int SIDE, double *end_pos_n_RPY);
    
    void Inv_Kin_3DOF_Arm(double X, double Y, double Z, double *req_angle);
    
    void CalculateArmJacobian(double theta_1, double theta_2, double theta_3, Eigen::Matrix3d *jacobian);
    
    void CalculateArmAlternateJacobian_No_Theta3_Singularity(double theta_1, double theta_2, double theta_3, Eigen::MatrixXd *jacobian);
    
    void Dir_Kin_4DOF_Detached_Leg(double t1, double t2, double t3, double t4, short int SIDE, double *end_pos_n_RPY);
    
    void Inv_Kin_3DOF_Detached_Leg(double X, double Y, double Z, double *req_angle);
    
    void Calculate_Detached_Leg_3DOF_Jacobian(double theta_1, double theta_2, double theta_3, Eigen::Matrix3d *jacobian);
    
    void Calculate_Detached_Leg_3DOF_COG(double theta_1, double theta_2, double theta_3, int SIDE, Eigen::Vector3d *COG);
    
    //THESE FUNCTIONS TRANSFORM JOINT ANGLE VALUES TO THEIR EQUIVALENT SERVOMOTOR VALUES
    /* ++++++++++++++++++++++++++++++++++++++++++
        ++++++++++++++++++ HEAD ++++++++++++++++++
      ++++++++++++++++++++++++++++++++++++++++++*/
    short unsigned int Head_Pan_Tilt_Conversion(double joint_angle);
    
    /* ++++++++++++++++++++++++++++++++++++++++++
        ++++++++++++++++++ ARMS ++++++++++++++++++
        ++++++++++++++++++++++++++++++++++++++++++*/
    short unsigned int Shoulder_Flexion_Extension_Conversion(short int SIDE, double joint_angle);

    short unsigned int Shoulder_Abduction_Adduction_Conversion(short int SIDE, double joint_angle);

    short unsigned int Elbow_Flexion_Extension_Conversion(short int SIDE, double joint_angle);

    /* ++++++++++++++++++++++++++++++++++++++++++
        ++++++++++++++++++ TORSO +++++++++++++++++
        ++++++++++++++++++++++++++++++++++++++++++*/
    short unsigned int Torso_Rotation_Conversion(double joint_angle);

    short unsigned int Torso_Flexion_Extension_Conversion(double joint_angle);

    short unsigned int Torso_Lateral_Flexion_Extension_Conversion(double joint_angle);
  
    /* ++++++++++++++++++++++++++++++++++++++++++
        ++++++++++++++++++ LEGS ++++++++++++++++++
        ++++++++++++++++++++++++++++++++++++++++++*/
    short unsigned int Ankle_Inversion_Eversion_Conversion(short int SIDE, double joint_angle);
    
    short unsigned int Ankle_Flexion_Conversion(short int SIDE, double joint_angle);

    short unsigned int Knee_Conversion(short int SIDE, double joint_angle);

    short unsigned int Hip_Abduction_Hiperabduction_Conversion(short int SIDE, double joint_angle);
    
    short unsigned int Hip_Flexion_Conversion(short int SIDE, double joint_angle);
    
    //THESE FUNCTIONS TRANSFORM SERVOMOTOR VALUES TO THEIR EQUIVALENT JOINT ANGLE VALUES 
    /* ++++++++++++++++++++++++++++++++++++++++++
        ++++++++++++++++++ HEAD ++++++++++++++++++
      ++++++++++++++++++++++++++++++++++++++++++*/
    double Head_Pan_Tilt_ServoValue_Conversion(short unsigned int servo_value);
    
    /* ++++++++++++++++++++++++++++++++++++++++++
        ++++++++++++++++++ ARMS ++++++++++++++++++
        ++++++++++++++++++++++++++++++++++++++++++*/
    double Shoulder_Flexion_Extension_ServoValue_Conversion(short int SIDE, short unsigned int servo_value);

    double Shoulder_Abduction_Adduction_ServoValue_Conversion(short int SIDE, short unsigned int servo_value);

    double Elbow_Flexion_Extension_ServoValue_Conversion(short int SIDE, short unsigned int servo_value);
    
    /* ++++++++++++++++++++++++++++++++++++++++++
       ++++++++++++++++++ TORSO +++++++++++++++++
       ++++++++++++++++++++++++++++++++++++++++++*/
    double Torso_Rotation_ServoValue_Conversion(short unsigned int servo_value);

    double Torso_Flexion_Extension_ServoValue_Conversion(short unsigned int servo_value);

    double Torso_Lateral_Flexion_Extension_ServoValue_Conversion(short unsigned int servo_value);
    
    /* ++++++++++++++++++++++++++++++++++++++++++
        ++++++++++++++++++ LEGS ++++++++++++++++++
        ++++++++++++++++++++++++++++++++++++++++++*/
    double Ankle_Inversion_Eversion_ServoValue_Conversion(short int SIDE, short unsigned int servo_value);
    
    double Ankle_Flexion_ServoValue_Conversion(short int SIDE, short unsigned int servo_value);

    double Knee_Flexion_ServoValue_Conversion(short int SIDE, short unsigned int servo_value);

    double Hip_Abduction_Hiperabduction_ServoValue_Conversion(short int SIDE, short unsigned int servo_value);
    
    double Hip_Flexion_ServoValue_Conversion(short int SIDE, short unsigned int servo_value);
    
    /* ++++++++++++++++++++++++++++++++++++++++++
        +++++++++ GET ROBOT DIMENSIONS ++++++++++
        ++++++++++++++++++++++++++++++++++++++++++*/
    double GetRobotDimension(int dimension);
    
  private:
    
    /*~~~~~~~~~~~~~~~~~~~~~~~~~ 
    || miscellaneous variables ||
      ~~~~~~~~~~~~~~~~~~~~~~~~~*/
    double PI;
    
    /*~~~~~~~~~~~~~~~~~~~~~~~~~~~ 
    ||      robot dimensions     ||
      ~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
    struct{
    double arm_length;
    
    double forearm_length;
    
    double shin_length;
    
    double thigh_length;
    
    double foot_length;
    
    double foot_width;
    
    double ankle_height;
    } robot_dimensions;
    
    //   ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 
    // || robot joint limitation variables ||
    //   ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
    struct{
    // <<<< HEAD >>>>   
    // Motion range [0...10] vertical to tilted
    
    double head_min_tilt;
    
    double head_max_tilt;
    
    /* <<<< ARMS >>>>*/
    /* Shoulder flexion
    Motion range [-40...140] back to front
    */
    double shoulder_min_flexion;
    
    double shoulder_max_flexion;
    
    /* Shoulder abduction
    Motion range [0...180] back to front
    */
    double shoulder_min_abduction;
    
    double shoulder_max_abduction;
    
    /* Elbow flexion
    Motion range [0...120] streched to flected
    */
    double elbow_min_flexion;
    
    double elbow_max_flexion;
    
    /* <<<< TORSO >>>>*/
    /* Torso rotation
    Motion range [-90...90] left to right
    */    
    double torso_min_rotation;
    
    double torso_max_rotation;
    
    /* Torso flexion
    Motion range [-15...90] left to right
    */    
    double torso_min_flexion;
    
    double torso_max_flexion;
    
    /* Torso lateral flexion
    Motion range [-90...90] left to right
    */
    double torso_min_lateral_flexion;
    
    double torso_max_lateral_flexion;
    
    /* Ankle inversion
    Motion range [-30...45] inside to outside
    */
    double ankle_min_inversion;
    
    double ankle_max_inversion;
    
    /* Ankle flexion
    Motion range [-40...20] inside to outside
    */
    double ankle_min_flexion;
    
    double ankle_max_flexion;
    
    /* Knee flexion
    Motion range [0...130] inside to outside
    */
    double knee_min_flexion;
    
    double knee_max_flexion;
    
    /* Hip abduction
    Motion range [-40...45] inside to outside
    */
    double hip_min_abduction;
    
    double hip_max_abduction;
    
    /* hip flexion
    Motion range [-30...120] inside to outside
    */
    double hip_min_flexion;
    
    double hip_max_flexion;
    } robot_joint_limits;
    
    struct{
      double head_servo_offset;

      double shoulder_flexion_servo_offset_left;

      double shoulder_flexion_servo_offset_right;

      double shoulder_abduction_servo_offset_left;

      double shoulder_abduction_servo_offset_right;

      double elbow_flexion_servo_offset_left;

      double elbow_flexion_servo_offset_right;

      double torso_rotation_servo_offset;

      double torso_flexion_servo_offset;

      double torso_lateral_flexion_servo_offset;

      double knee_flexion_b_value_left;

      double knee_flexion_b_value_right;

      double hip_flexion_b_value_left;

      double hip_flexion_b_value_right;
    } robot_servo_conversion;
    
    struct{
      double foot_mass;
      
      double shin_mass;
      
      double thigh_mass;
      
    } robot_element_mass;
    
    struct{
      Eigen::Vector3d foot_COM;
      
      Eigen::Vector3d shin_COM;
      
      Eigen::Vector3d thigh_COM;
      
    } robot_relative_COM;
    
};

#endif