kinematics.h

Go to the documentation of this file.

/*******************************************************************/
/*    AUTHOR: Paal Arthur S. Thorseth                              */
/*    ORGN:   Dept of Eng Cybernetics, NTNU Trondheim              */
/*    FILE:   kinematics.h                                         */
/*    DATE:   Mar 1, 2021                                          */
/*                                                                 */
/* Copyright (C) 2021 Paal Arthur S. Thorseth,                     */
/*                    Adrian B. Ghansah                            */
/*                                                                 */
/* This program is free software: you can redistribute it          */
/* and/or modify it under the terms of the GNU General             */
/* Public License as published by the Free Software Foundation,    */
/* either version 3 of the License, or (at your option) any        */
/* later version.                                                  */
/*                                                                 */
/* This program is distributed in the hope that it will be useful, */
/* but WITHOUT ANY WARRANTY; without even the implied warranty     */
/* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.         */
/* See the GNU General Public License for more details.            */
/*                                                                 */
/* You should have received a copy of the GNU General Public       */
/* License along with this program. If not, see                    */
/* <https://www.gnu.org/licenses/>.                                */
/*                                                                 */
/*******************************************************************/
 
#pragma once
 
// C++ Standard Library
#include <cmath>
#include <algorithm>
 
// ROS
#include "ros/ros.h" 
 
// ROS Package Libraries
#include <math_utils/angle_utils.h>
 
// Eigen
#include <Eigen/Core>
 
// Kindr
#include <kindr/Core>
 
using GeneralizedCoordinates = Eigen::Matrix<double, 18, 1>;
using FootstepPositions = Eigen::Matrix<Eigen::Vector3d, 4, 1>;
using JointSpaceVector = Eigen::Matrix<double, 12, 1>;
using Twist = Eigen::Matrix<double, 6, 1>;
using Vector3d = Eigen::Vector3d;
using TransMatrix = kindr::HomTransformMatrixD;
 
class Kinematics
{
    public: enum LegType { frontLeft = 1, frontRight = 2, rearLeft = 3, rearRight = 4, NONE };
 
    public: enum BodyType { base = 1, hip = 2, thigh = 3, leg = 4, foot = 5 };
 
    public: Kinematics();
 
    public: virtual ~Kinematics();
 
    public: bool SolveForwardKinematics(const GeneralizedCoordinates &_q, 
                                        FootstepPositions &_f_pos);
 
    public: bool SolveInverseKinematics(const Twist &_q_b, 
                                        const FootstepPositions &_f_pos, 
                                        JointSpaceVector &_q_r);
 
    public: Vector3d SolveSingleLegForwardKinematics(const Vector3d &_h_pos, 
                                                     const double &_theta_hy, 
                                                     const double &_theta_hp, 
                                                     const double &_theta_kp); 
 
    public: Vector3d SolveSingleLegForwardKinematics(const Vector3d &_joint_angles);
 
    public: Vector3d SolveSingleLegForwardKinematics(const LegType &_leg_type, const Vector3d &_joint_angles);
 
    public: Vector3d SolveSingleLegHipToFootForwardKinematics(const LegType &_leg_type, const Vector3d &_joint_angles);
 
    public: bool SolveSingleLegInverseKinematics(const bool &_offset, 
                                                 const Vector3d &_h_pos, 
                                                 const Vector3d &_f_pos, 
                                                 Vector3d &_joint_angles);
 
    public: bool SolveSingleLegInverseKinematics(const bool &_offset, 
                                                 const Vector3d &_f_pos, 
                                                 Vector3d &_joint_angles);
 
    public: TransMatrix GetDhTransform(const double &_a, 
                                       const double &_alpha, 
                                       const double &_d, 
                                       const double &_theta);
 
    public: TransMatrix GetHipToBodyTransform(const LegType &_leg,
                                              const BodyType &_body,
                                              const double &_theta_hy, 
                                              const double &_theta_hp, 
                                              const double &_theta_kp);
    
    public: Eigen::Matrix<double, 3, 3> GetRotationMatrixWToB(const double &_roll,
                                                              const double &_pitch,
                                                              const double &_yaw);
 
    public: Eigen::Matrix<double, 3, 3> GetRotationMatrixWToBDiff(const double &_roll,
                                                                  const double &_pitch,
                                                                  const double &_yaw,
                                                                  const double &_roll_rate,
                                                                  const double &_pitch_rate,
                                                                  const double &_yaw_rate);
 
    public: Eigen::Matrix<double, 3, 3> GetRotationMatrixWToC(const double &_roll,
                                                              const double &_pitch,
                                                              const double &_yaw);
 
    public: Eigen::Matrix<double, 3, 3> GetRotationMatrixWToCDiff(const double &_roll,
                                                                  const double &_pitch,
                                                                  const double &_yaw,
                                                                  const double &_roll_rate,
                                                                  const double &_pitch_rate,
                                                                  const double &_yaw_rate);
 
    public: Eigen::Matrix<double, 3, 3> GetRotationMatrixCToB(const double &_roll,
                                                              const double &_pitch,
                                                              const double &_yaw);
 
    public: Eigen::Matrix<double, 3, 3> GetRotationMatrixCToBDiff(const double &_roll,
                                                                  const double &_pitch,
                                                                  const double &_yaw,
                                                                  const double &_roll_rate,
                                                                  const double &_pitch_rate,
                                                                  const double &_yaw_rate);
 
    public: Eigen::Matrix<double, 3, 1> GetPositionBaseToBodyInB(const LegType &_leg,
                                                                 const BodyType &_body, 
                                                                 const Eigen::Matrix<double, 18, 1> &_q);
 
    public: Eigen::Matrix<double, 3, 3> GetTranslationJacobianInB(const LegType &_leg,
                                                                  const BodyType &_body,
                                                                  const double &_theta_hy, 
                                                                  const double &_theta_hp, 
                                                                  const double &_theta_kp);
 
    public: Eigen::Matrix<double, 3, 12> GetTranslationJacobianInB(const LegType &_leg, 
                                                                   const BodyType &_body,
                                                                   const Eigen::Matrix<double, 12, 1> &_q_r);
 
    public: Eigen::Matrix<double, 3, 18> GetTranslationJacobianInW(const LegType &_leg,
                                                                   const BodyType &_body,
                                                                   const Eigen::Matrix<double, 18, 1> &_q);
 
    public: Eigen::Matrix<double, 3, 18> GetTranslationJacobianInC(const LegType &_leg,
                                                                   const BodyType &_body,
                                                                   const Eigen::Matrix<double, 18, 1> &_q);
 
    public: Eigen::Matrix<double, 3, 3> GetTranslationJacobianInBDiff(const LegType &_leg,
                                                                       const BodyType &_body,
                                                                       const double &_theta_hy, 
                                                                       const double &_theta_hp, 
                                                                       const double &_theta_kp,
                                                                       const double &_dot_theta_hy,
                                                                       const double &_dot_theta_hp,
                                                                       const double &_dot_theta_kp);
 
    public: Eigen::Matrix<double, 3, 12> GetTranslationJacobianInBDiff(const LegType &_leg,
                                                                       const BodyType &_body,
                                                                       const Eigen::Matrix<double, 12, 1> &_q_r,
                                                                       const Eigen::Matrix<double, 12, 1> &_dot_q_r);
 
    public: Eigen::Matrix<double, 3, 18> GetTranslationJacobianInWDiff(const LegType &_leg,
                                                                       const BodyType &_body,
                                                                       const Eigen::Matrix<double, 18, 1> &_q,
                                                                       const Eigen::Matrix<double, 18, 1> &_u);
 
    public: Eigen::Matrix<double, 3, 18> GetTranslationJacobianInCDiff(const LegType &_leg,
                                                                       const BodyType &_body,
                                                                       const Eigen::Matrix<double, 18, 1> &_q,
                                                                       const Eigen::Matrix<double, 18, 1> &_u);
 
 
    public: Eigen::Matrix<double, 3, 3> GetRotationJacobianInB(const LegType &_leg,
                                                               const BodyType &_body,
                                                               const double &_theta_hy, 
                                                               const double &_theta_hp, 
                                                               const double &_theta_kp);
 
    public: Eigen::Matrix<double, 3, 12> GetRotationJacobianInB(const LegType &_leg, 
                                                                const BodyType &_body,
                                                                const Eigen::Matrix<double, 12, 1> &_q_r);
 
    public: Eigen::Matrix<double, 3, 18> GetRotationJacobianInW(const LegType &_leg,
                                                                const BodyType &_body,
                                                                const Eigen::Matrix<double, 18, 1> &_q);
 
    public: Eigen::Matrix<double, 3, 18> GetRotationJacobianInC(const LegType &_leg,
                                                                const BodyType &_body,
                                                                const Eigen::Matrix<double, 18, 1> &_q);
 
    public: Eigen::Matrix<double, 3, 3> GetRotationJacobianInBDiff(const LegType &_leg,
                                                                   const BodyType &_body,
                                                                   const double &_theta_hy, 
                                                                   const double &_theta_hp, 
                                                                   const double &_theta_kp,
                                                                   const double &_dot_theta_hy,
                                                                   const double &_dot_theta_hp,
                                                                   const double &_dot_theta_kp);
 
    public: Eigen::Matrix<double, 3, 12> GetRotationJacobianInBDiff(const LegType &_leg,
                                                                    const BodyType &_body,
                                                                    const Eigen::Matrix<double, 12, 1> &_q_r,
                                                                    const Eigen::Matrix<double, 12, 1> &_dot_q_r);
 
    public: Eigen::Matrix<double, 3, 18> GetRotationJacobianInWDiff(const LegType &_leg,
                                                                    const BodyType &_body,
                                                                    const Eigen::Matrix<double, 18, 1> &_q,
                                                                    const Eigen::Matrix<double, 18, 1> &_u);
 
    public: Eigen::Matrix<double, 3, 18> GetRotationJacobianInCDiff(const LegType &_leg,
                                                                    const BodyType &_body,
                                                                    const Eigen::Matrix<double, 18, 1> &_q,
                                                                    const Eigen::Matrix<double, 18, 1> &_u);
 
    public: Eigen::Matrix<double, 6, 18> GetJacobianInW(const LegType &_leg,
                                                        const BodyType &_body,
                                                        const Eigen::Matrix<double, 18, 1> &_q);
 
    public: Eigen::Matrix<double, 6, 18> GetJacobianInWDiff(const LegType &_leg,
                                                            const BodyType &_body,
                                                            const Eigen::Matrix<double, 18, 1> &_q,
                                                            const Eigen::Matrix<double, 18, 1> &_u);
 
    public: Eigen::Matrix<double, Eigen::Dynamic, 18> GetContactJacobianInW(std::vector<LegType> &_legs,
                                                                            const Eigen::Matrix<double, 18, 1> &_q);
 
    public: Eigen::Matrix<double, Eigen::Dynamic, 18> GetContactJacobianInWDiff(std::vector<LegType> &_legs,
                                                                                const Eigen::Matrix<double, 18, 1> &_q,
                                                                                const Eigen::Matrix<double, 18, 1> &_u);
 
    public: Eigen::Matrix<double, Eigen::Dynamic, 18> GetSwingJacobianInW(std::vector<LegType> &_legs,
                                                                          const Eigen::Matrix<double, 18, 1> &_q);
 
    public: Eigen::Matrix<double, Eigen::Dynamic, 18> GetSwingJacobianInWDiff(std::vector<LegType> &_legs,
                                                                              const Eigen::Matrix<double, 18, 1> &_q,
                                                                              const Eigen::Matrix<double, 18, 1> &_u);
 
    public: Eigen::Matrix<double, 18, 18> GetSingleBodyMassMatrix(const LegType &_leg,
                                                                  const BodyType &_body,
                                                                  const Eigen::Matrix<double, 18, 1> &_q);
 
    public: Eigen::Matrix<double, 18, 18> GetMassMatrix(const Eigen::Matrix<double, 18, 1> &_q);
 
 
    public: Eigen::Matrix<double, 3, 3> GetSingleLegMassMatrix(const Eigen::Matrix<double, 3, 1> &_q);
 
    public: Eigen::Matrix<double, 18, 1> GetSingleBodyCoriolisAndCentrifugalTerms(const LegType &_leg,
                                                                                  const BodyType &_body,
                                                                                  const Eigen::Matrix<double, 18, 1> &_q,
                                                                                  const Eigen::Matrix<double, 18, 1> &_u);
 
    public: Eigen::Matrix<double, 18, 1> GetCoriolisAndCentrifugalTerms(const Eigen::Matrix<double, 18, 1> &_q,
                                                                        const Eigen::Matrix<double, 18, 1> &_u);
 
 
    public: Eigen::Matrix<double, 3, 1> GetSingleLegCoriolisAndCentrifugalTerms(const Eigen::Matrix<double, 3, 1> &_q,
                                                                                const Eigen::Matrix<double, 3, 1> &_u);
 
    public: Eigen::Matrix<double, 18, 1> GetSingleBodyGravitationalTerms(const LegType &_leg,
                                                                         const BodyType &_body,
                                                                         const Eigen::Matrix<double, 18, 1> &_q);
 
    public: Eigen::Matrix<double, 18, 1> GetGravitationalTerms(const Eigen::Matrix<double, 18, 1> &_q);
 
    public: Eigen::Matrix<double, 3, 1> GetSingleLegGravitationalTerms(const Eigen::Matrix<double, 3, 1> &_q);
 
    public: bool ValidateSolution(const JointSpaceVector &_q_r);
 
    public: bool ValidateSolution(const Eigen::Matrix<double, 3, 1> &_q_r);
 
    private: Eigen::Matrix<double, 3, 3> GetInertiaMatrix(const double &_I_XX,
                                                          const double &_I_YY,
                                                          const double &_I_ZZ,
                                                          const double &_I_XY,
                                                          const double &_I_XZ,
                                                          const double &_I_YZ);
 
    private: double GetBodyMass(BodyType _body);
 
    private: Eigen::Matrix<double, 3, 3> GetBodyInertia(BodyType _body);
 
    private: Eigen::Matrix<double, 3, 3> GetBodyInertia(BodyType _body, LegType _leg);
 
    public: bool GetflOffset() {return flOffset;}
 
    public: bool GetfrOffset() {return frOffset;}
 
    public: bool GetrlOffset() {return rlOffset;}
 
    public: bool GetrrOffset() {return rrOffset;}
 
    public: Eigen::Matrix<double, 3, 1> GetDistanceFromBaseToFrontLeftHipInB() {return positionBaseToFrontLeftInB;}
 
    public: Eigen::Matrix<double, 3, 1> GetDistanceFromBaseToFrontRightHipInB() {return positionBaseToFrontRightInB;}
 
    public: Eigen::Matrix<double, 3, 1> GetDistanceFromBaseToRearLeftHipInB() {return positionBaseToRearLeftInB;}
    
    public: Eigen::Matrix<double, 3, 1> GetDistanceFromBaseToRearRightHipInB() {return positionBaseToRearRightInB;}
 
    private: JointSpaceVector min_angles;
 
    private: JointSpaceVector max_angles;
 
    private: Vector3d positionBaseToFrontLeftInB;
 
    private: Vector3d positionBaseToFrontRightInB;
 
    private: Vector3d positionBaseToRearLeftInB;
 
    private: Vector3d positionBaseToRearRightInB;
 
    private: double L1;
 
    private: double L2;
 
    private: double L3;
 
    private: double LC1;
 
    private: double LC2;
 
    private: double LC3;
 
    private: bool flOffset;
 
    private: bool frOffset;
 
    private: bool rlOffset;
 
    private: bool rrOffset;
 
    private: double M0;
 
    private: double M1;
 
    private: double M2; 
 
    private: double M3;
 
    private: Eigen::Matrix<double, 3, 3> I0;
 
    private: Eigen::Matrix<double, 3, 3> I1_fl_rr;
 
    private: Eigen::Matrix<double, 3, 3> I1_fr_rl;
 
    private: Eigen::Matrix<double, 3, 3> I2;
 
    private: Eigen::Matrix<double, 3, 3> I3;
};