BalanceToolkit.h

Go to the documentation of this file.

/*
 * 
 * Copyright (c) 2020 Matthew Millard <millard.matthew@gmail.com>
 *
 * Licensed under the zlib license. See LICENSE for more details.
 */
 
 
#ifndef BALANCETOOLKIT_H_
#define BALANCETOOLKIT_H_
 
 
#include <rbdl/rbdl_math.h>
#include <rbdl/Model.h>
#include <limits>
 
namespace RigidBodyDynamics {
  namespace Addons {
    namespace Balance{
 
    struct FootPlacementEstimatorInfo{
      double f               ;
      unsigned int iterations;
      double phi             ;
      Math::Vector3d r0F0    ;
      
      double projectionError ;
 
      Math::Vector3d  n      ;
      Math::Vector3d  u      ;
      Math::Vector3d  k      ;
      Math::Vector3d r0C0    ;
      Math::Vector3d v0C0    ;
      Math::Vector3d r0P0    ;
      Math::Vector3d HC0     ;
      Math::Matrix3d JC0     ;
      Math::Vector3d w0C0    ;
      Math::Vector3d HP0     ;
      Math::Matrix3d JP0     ;
      Math::Vector3d w0P0    ;
      double h               ;
      double nJC0n            ;
      double v0C0u;
      double v0C0k;
      double w0C0n;
      double w0F0nPlus;
      double l               ;
      double E               ;
      double Df_Dphi         ;
      double Df_Dw0C0n       ;
      double Df_Dh           ;
      double Df_Dv0C0u       ;
      double Df_Dv0C0k       ;
      double Df_DnJC0n           ;
      double Df_Dm           ;
      double Df_Dg           ;
      double Ds_Dl           ;
      double Ds_DnJC0n        ;
      double Ds_DE           ;
      double Ds_Dv0C0u       ;
      double Ds_Dv0C0k       ;
      double Ds_Dw0C0n       ;
      double Dphi_Dl         ;
      double Dphi_DnJC0n         ;
      double Dphi_DE         ;
      double Dphi_Dv0C0u     ;
      double Dphi_Dv0C0k     ;
      double Dphi_Dw0C0n     ; 
 
 
      FootPlacementEstimatorInfo():
        f(std::numeric_limits<double>::signaling_NaN()),
        iterations(std::numeric_limits<unsigned int>::signaling_NaN()),
        phi(std::numeric_limits<double>::signaling_NaN()),
        r0F0( Math::Vector3dZero ),
        projectionError(std::numeric_limits<double>::signaling_NaN()),
        n( Math::Vector3dZero ),
        u( Math::Vector3dZero ),
        k( Math::Vector3dZero ),
        r0C0( Math::Vector3dZero ),
        v0C0( Math::Vector3dZero ),
        r0P0( Math::Vector3dZero ),
        HC0( Math::Vector3dZero ),
        JC0( Math::Matrix3dZero ),
        w0C0( Math::Vector3dZero ),
        HP0( Math::Vector3dZero ),
        JP0( Math::Matrix3dZero ),
        w0P0( Math::Vector3dZero ),
        h(std::numeric_limits<double>::signaling_NaN()),
        nJC0n(std::numeric_limits<double>::signaling_NaN()),
        v0C0u(std::numeric_limits<double>::signaling_NaN()),
        v0C0k(std::numeric_limits<double>::signaling_NaN()),
        w0C0n(std::numeric_limits<double>::signaling_NaN()),
        w0F0nPlus(std::numeric_limits<double>::signaling_NaN()),
        l(std::numeric_limits<double>::signaling_NaN()),
        E(std::numeric_limits<double>::signaling_NaN()),
        Df_Dphi(std::numeric_limits<double>::signaling_NaN()),
        Df_Dw0C0n(std::numeric_limits<double>::signaling_NaN()),
        Df_Dh(std::numeric_limits<double>::signaling_NaN()),
        Df_Dv0C0u(std::numeric_limits<double>::signaling_NaN()),
        Df_Dv0C0k(std::numeric_limits<double>::signaling_NaN()),
        Df_DnJC0n(std::numeric_limits<double>::signaling_NaN()),
        Df_Dm(std::numeric_limits<double>::signaling_NaN()),
        Df_Dg(std::numeric_limits<double>::signaling_NaN()),
        Ds_Dl(std::numeric_limits<double>::signaling_NaN()),
        Ds_DnJC0n(std::numeric_limits<double>::signaling_NaN()),
        Ds_DE(std::numeric_limits<double>::signaling_NaN()),
        Ds_Dv0C0u(std::numeric_limits<double>::signaling_NaN()),
        Ds_Dv0C0k(std::numeric_limits<double>::signaling_NaN()),
        Ds_Dw0C0n(std::numeric_limits<double>::signaling_NaN()),
        Dphi_Dl(std::numeric_limits<double>::signaling_NaN()),
        Dphi_DnJC0n(std::numeric_limits<double>::signaling_NaN()),
        Dphi_DE(std::numeric_limits<double>::signaling_NaN()),
        Dphi_Dv0C0u(std::numeric_limits<double>::signaling_NaN()),
        Dphi_Dv0C0k(std::numeric_limits<double>::signaling_NaN()),
        Dphi_Dw0C0n(std::numeric_limits<double>::signaling_NaN()){}
 
    };
 
class BalanceToolkit
{
  public:
 
 
 
              
 
    static void CalculateFootPlacementEstimator(
      Model &model,
      Math::VectorNd &q,
      Math::VectorNd &qdot,
      Math::Vector3d &pointOnGroundPlane,
      Math::Vector3d &groundPlaneNormal,
      FootPlacementEstimatorInfo &fpeInfo,
      double smallAngularVelocity,
      bool evaluate_derivatives,
      bool update_kinematics);
 
};
 
    }
  }
}
 
#endif