talos-base-estimator.hh

Go to the documentation of this file.

/*
 * Copyright 2017, Thomas Flayols, LAAS-CNRS
 *
 * This file is part of sot-torque-control.
 * sot-torque-control is free software: you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public License
 * as published by the Free Software Foundation, either version 3 of
 * the License, or (at your option) any later version.
 * sot-torque-control 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 Lesser General Public License for more details.  You should
 * have received a copy of the GNU Lesser General Public License along
 * with sot-torque-control.  If not, see <http://www.gnu.org/licenses/>.
 */
 
#ifndef __sot_torque_control_base_estimator_H__
#define __sot_torque_control_base_estimator_H__
 
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
 
#if defined(WIN32)
#if defined(base_estimator_EXPORTS)
#define TALOS_BASE_ESTIMATOR_EXPORT __declspec(dllexport)
#else
#define TALOS_BASE_ESTIMATOR_EXPORT __declspec(dllimport)
#endif
#else
#define TALOS_BASE_ESTIMATOR_EXPORT
#endif
 
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <pinocchio/fwd.hpp>
// include pinocchio first
#include <dynamic-graph/signal-helper.h>
 
#include <map>
#include <sot/core/matrix-geometry.hh>
#include <sot/core/robot-utils.hh>
 
#include "boost/assign.hpp"
// #include <boost/random/normal_distribution.hpp>
#include <boost/math/distributions/normal.hpp>  // for normal_distribution
 
/* Pinocchio */
#include <pinocchio/algorithm/kinematics.hpp>
#include <pinocchio/multibody/model.hpp>
#include <pinocchio/parsers/urdf.hpp>
 
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
 
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
 
void se3Interp(const pinocchio::SE3& s1, const pinocchio::SE3& s2,
               const double alpha, pinocchio::SE3& s12);
 
void rpyToMatrix(double r, double p, double y, Eigen::Matrix3d& R);
 
void rpyToMatrix(const Eigen::Vector3d& rpy, Eigen::Matrix3d& R);
 
void matrixToRpy(const Eigen::Matrix3d& M, Eigen::Vector3d& rpy);
 
// void quanternionMult(const Eigen::Vector4d & q1, const Eigen::Vector4d & q2,
// Eigen::Vector4d & q12);
 
// void pointRotationByQuaternion(const Eigen::Vector3d & point,const
// Eigen::Vector4d & quat, Eigen::Vector3d & rotatedPoint);
 
double eulerMean(double a1, double a2);
 
double wEulerMean(double a1, double a2, double w1, double w2);
 
class TALOS_BASE_ESTIMATOR_EXPORT TalosBaseEstimator
    : public ::dynamicgraph::Entity {
  typedef TalosBaseEstimator EntityClassName;
  typedef pinocchio::SE3 SE3;
  typedef Eigen::Vector2d Vector2;
  typedef Eigen::Vector3d Vector3;
  typedef Eigen::Vector4d Vector4;
  typedef Vector6d Vector6;
  typedef Vector7d Vector7;
  typedef Eigen::Matrix3d Matrix3;
  typedef boost::math::normal normal;
 
  DYNAMIC_GRAPH_ENTITY_DECL();
 
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  /* --- CONSTRUCTOR ---- */
  TalosBaseEstimator(const std::string& name);
 
  void init(const double& dt, const std::string& urdfFile);
 
  void set_fz_stable_windows_size(const int& ws);
  void set_alpha_w_filter(const double& a);
  void set_alpha_DC_acc(const double& a);
  void set_alpha_DC_vel(const double& a);
  void reset_foot_positions();
  void set_imu_weight(const double& w);
  void set_zmp_std_dev_right_foot(const double& std_dev);
  void set_zmp_std_dev_left_foot(const double& std_dev);
  void set_normal_force_std_dev_right_foot(const double& std_dev);
  void set_normal_force_std_dev_left_foot(const double& std_dev);
  void set_stiffness_right_foot(const dynamicgraph::Vector& k);
  void set_stiffness_left_foot(const dynamicgraph::Vector& k);
  void set_right_foot_sizes(const dynamicgraph::Vector& s);
  void set_left_foot_sizes(const dynamicgraph::Vector& s);
  void set_zmp_margin_right_foot(const double& margin);
  void set_zmp_margin_left_foot(const double& margin);
  void set_normal_force_margin_right_foot(const double& margin);
  void set_normal_force_margin_left_foot(const double& margin);
 
  void reset_foot_positions_impl(const Vector6& ftlf, const Vector6& ftrf);
  void compute_zmp(const Vector6& w, Vector2& zmp);
  double compute_zmp_weight(const Vector2& zmp, const Vector4& foot_sizes,
                            double std_dev, double margin);
  double compute_force_weight(double fz, double std_dev, double margin);
  void kinematics_estimation(const Vector6& ft, const Vector6& K,
                             const SE3& oMfs,
                             const pinocchio::FrameIndex foot_id, SE3& oMff,
                             SE3& oMfa, SE3& fsMff);
 
  /* --- SIGNALS --- */
  DECLARE_SIGNAL_IN(joint_positions, dynamicgraph::Vector);
  DECLARE_SIGNAL_IN(joint_velocities, dynamicgraph::Vector);
  DECLARE_SIGNAL_IN(imu_quaternion, dynamicgraph::Vector);
  DECLARE_SIGNAL_IN(forceLLEG, dynamicgraph::Vector);
  DECLARE_SIGNAL_IN(forceRLEG, dynamicgraph::Vector);
  DECLARE_SIGNAL_IN(
      dforceLLEG,
      dynamicgraph::Vector);  
  DECLARE_SIGNAL_IN(
      dforceRLEG,
      dynamicgraph::Vector);  
  DECLARE_SIGNAL_IN(
      w_lf_in, double);  
  DECLARE_SIGNAL_IN(
      w_rf_in, double);  
  DECLARE_SIGNAL_IN(
      K_fb_feet_poses,
      double);  
  DECLARE_SIGNAL_IN(lf_ref_xyzquat, dynamicgraph::Vector);
  DECLARE_SIGNAL_IN(rf_ref_xyzquat, dynamicgraph::Vector);
  DECLARE_SIGNAL_IN(accelerometer, dynamicgraph::Vector);
  DECLARE_SIGNAL_IN(gyroscope, dynamicgraph::Vector);
 
  DECLARE_SIGNAL_INNER(kinematics_computations, dynamicgraph::Vector);
 
  DECLARE_SIGNAL_OUT(
      q, dynamicgraph::Vector);  
  DECLARE_SIGNAL_OUT(v, dynamicgraph::Vector);  
  DECLARE_SIGNAL_OUT(
      v_kin, dynamicgraph::Vector);  
  DECLARE_SIGNAL_OUT(
      v_flex,
      dynamicgraph::Vector);  
  DECLARE_SIGNAL_OUT(
      v_imu,
      dynamicgraph::Vector);  
  DECLARE_SIGNAL_OUT(
      v_gyr, dynamicgraph::Vector);  
  DECLARE_SIGNAL_OUT(lf_xyzquat, dynamicgraph::Vector);  
  DECLARE_SIGNAL_OUT(rf_xyzquat, dynamicgraph::Vector);  
  DECLARE_SIGNAL_OUT(a_ac,
                     dynamicgraph::Vector);  
  DECLARE_SIGNAL_OUT(v_ac,
                     dynamicgraph::Vector);  
 
  DECLARE_SIGNAL_OUT(
      q_lf,
      dynamicgraph::Vector);  
  DECLARE_SIGNAL_OUT(
      q_rf,
      dynamicgraph::Vector);  
  DECLARE_SIGNAL_OUT(
      q_imu,
      dynamicgraph::Vector);  
  DECLARE_SIGNAL_OUT(
      w_lf, double);  
  DECLARE_SIGNAL_OUT(
      w_rf, double);  
  DECLARE_SIGNAL_OUT(
      w_lf_filtered,
      double);  
  DECLARE_SIGNAL_OUT(
      w_rf_filtered,
      double);  
 
  /* --- COMMANDS --- */
  /* --- ENTITY INHERITANCE --- */
  virtual void display(std::ostream& os) const;
 
 protected:
  bool
      m_initSucceeded;  
  bool
      m_reset_foot_pos;  
  double m_dt;           
  RobotUtilShrPtr m_robot_util;
 
  bool m_left_foot_is_stable;  
  bool m_right_foot_is_stable;   
  int m_fz_stable_windows_size;  
  int m_lf_fz_stable_cpt;  
  int m_rf_fz_stable_cpt;  
  /* Estimator parameters */
  double m_w_imu;           
  double m_zmp_std_dev_rf;  
  double m_zmp_std_dev_lf;  
  double m_fz_std_dev_rf;   
  double m_fz_std_dev_lf;   
  Vector4 m_left_foot_sizes;   
  Vector4 m_right_foot_sizes;  
  double m_zmp_margin_lf;      
  double m_zmp_margin_rf;      
  double m_fz_margin_lf;       
  double m_fz_margin_rf;       
  Vector6 m_K_rf;              
  Vector6 m_K_lf;              
 
  Eigen::VectorXd m_v_kin;   
  Eigen::VectorXd m_v_flex;  
  Eigen::VectorXd m_v_imu;  
  Eigen::VectorXd m_v_gyr;  
 
  Vector3
      m_v_ac;  
  Vector3 m_a_ac;  
 
  double m_alpha_DC_acc;  
  double m_alpha_DC_vel;  
 
  double m_alpha_w_filter;  
 
  double m_w_lf_filtered;  
  double m_w_rf_filtered;  
 
  pinocchio::Model m_model;    
  pinocchio::Data* m_data;     
  pinocchio::SE3 m_torsoMimu;  
  pinocchio::SE3
      m_oMff_lf;  
  pinocchio::SE3
      m_oMff_rf;  
  SE3 m_oMlfs;    
  SE3 m_oMrfs;    
  Vector7 m_oMlfs_xyzquat;
  Vector7 m_oMrfs_xyzquat;
  SE3 m_oMlfs_default_ref;  
  SE3 m_oMrfs_default_ref;  
  normal m_normal;          
 
  bool m_isFirstIterFlag;  
 
  SE3 m_sole_M_ftSens;  
 
  pinocchio::FrameIndex m_right_foot_id;
  pinocchio::FrameIndex m_left_foot_id;
  pinocchio::FrameIndex m_torso_id;
  pinocchio::FrameIndex m_IMU_frame_id;
  Eigen::VectorXd
      m_q_pin;  
  Eigen::VectorXd m_q_sot;  
  Eigen::VectorXd
      m_v_pin;  
  Eigen::VectorXd m_v_sot;  
  Matrix3 m_oRtorso;  
  Matrix3 m_oRff;     
 
  /* Filter buffers*/
  Vector3 m_last_vel;
  Vector3 m_last_DCvel;
  Vector3 m_last_DCacc;
 
};  // class TalosBaseEstimator
 
}  // namespace talos_balance
}  // namespace sot
}  // namespace dynamicgraph
 
#endif  // #ifndef __sot_torque_control_free_flyer_locator_H__