quad_kd.h

#ifndef QUAD_KD_H
#define QUAD_KD_H
 
#include <math.h>
#include <rbdl/addons/urdfreader/urdfreader.h>
#include <rbdl/rbdl.h>
#include <rbdl/rbdl_utils.h>
#include <ros/ros.h>
#include <tf2/LinearMath/Quaternion.h>
 
#include <Eigen/Geometry>
#include <chrono>
#include <grid_map_core/GridMap.hpp>
#include <random>
#include <vector>
 
#include "quad_utils/function_timer.h"
#include "quad_utils/math_utils.h"
 
namespace quad_utils {
 
 
class QuadKD {
 public:
  QuadKD();
 
  QuadKD(std::string ns);
 
  void initModel(std::string ns);
 
  Eigen::Matrix4d createAffineMatrix(Eigen::Vector3d trans,
                                     Eigen::Vector3d rpy) const;
 
  Eigen::Matrix4d createAffineMatrix(Eigen::Vector3d trans,
                                     Eigen::AngleAxisd rot) const;
 
  void transformBodyToWorld(Eigen::Vector3d body_pos, Eigen::Vector3d body_rpy,
                            Eigen::Matrix4d transform_body,
                            Eigen::Matrix4d &transform_world) const;
 
  void transformWorldToBody(Eigen::Vector3d body_pos, Eigen::Vector3d body_rpy,
                            Eigen::Matrix4d transform_world,
                            Eigen::Matrix4d &transform_body) const;
 
  void bodyToFootFKBodyFrame(int leg_index, Eigen::Vector3d joint_state,
                             Eigen::Matrix4d &g_body_foot) const;
 
  void bodyToFootFKBodyFrame(int leg_index, Eigen::Vector3d joint_state,
                             Eigen::Vector3d &foot_pos_body) const;
 
  void worldToFootFKWorldFrame(int leg_index, Eigen::Vector3d body_pos,
                               Eigen::Vector3d body_rpy,
                               Eigen::Vector3d joint_state,
                               Eigen::Matrix4d &g_world_foot) const;
 
  void worldToFootFKWorldFrame(int leg_index, Eigen::Vector3d body_pos,
                               Eigen::Vector3d body_rpy,
                               Eigen::Vector3d joint_state,
                               Eigen::Vector3d &foot_pos_world) const;
 
  void worldToKneeFKWorldFrame(int leg_index, Eigen::Vector3d body_pos,
                               Eigen::Vector3d body_rpy,
                               Eigen::Vector3d joint_state,
                               Eigen::Matrix4d &g_world_knee) const;
 
  void worldToKneeFKWorldFrame(int leg_index, Eigen::Vector3d body_pos,
                               Eigen::Vector3d body_rpy,
                               Eigen::Vector3d joint_state,
                               Eigen::Vector3d &knee_pos_world) const;
 
  bool worldToFootIKWorldFrame(int leg_index, Eigen::Vector3d body_pos,
                               Eigen::Vector3d body_rpy,
                               Eigen::Vector3d foot_pos_world,
                               Eigen::Vector3d &joint_state) const;
 
  bool legbaseToFootIKLegbaseFrame(int leg_index,
                                   Eigen::Vector3d foot_pos_legbase,
                                   Eigen::Vector3d &joint_state) const;
 
  double getJointLowerLimit(int leg_index, int joint_index) const;
 
  double getJointUpperLimit(int leg_index, int joint_index) const;
 
  double getLinkLength(int leg_index, int link_index) const;
 
  void worldToLegbaseFKWorldFrame(int leg_index, Eigen::Vector3d body_pos,
                                  Eigen::Vector3d body_rpy,
                                  Eigen::Matrix4d &g_world_legbase) const;
 
  void worldToLegbaseFKWorldFrame(int leg_index, Eigen::Vector3d body_pos,
                                  Eigen::Vector3d body_rpy,
                                  Eigen::Vector3d &leg_base_pos_world) const;
 
  void worldToNominalHipFKWorldFrame(
      int leg_index, Eigen::Vector3d body_pos, Eigen::Vector3d body_rpy,
      Eigen::Vector3d &nominal_hip_pos_world) const;
 
  void getJacobianGenCoord(const Eigen::VectorXd &state,
                           Eigen::MatrixXd &jacobian) const;
 
  void getJacobianBodyAngVel(const Eigen::VectorXd &state,
                             Eigen::MatrixXd &jacobian) const;
 
  void getJacobianWorldAngVel(const Eigen::VectorXd &state,
                              Eigen::MatrixXd &jacobian) const;
 
  void getRotationMatrix(const Eigen::VectorXd &rpy,
                         Eigen::Matrix3d &rot) const;
 
  void computeInverseDynamics(const Eigen::VectorXd &state_pos,
                              const Eigen::VectorXd &state_vel,
                              const Eigen::VectorXd &foot_acc,
                              const Eigen::VectorXd &grf,
                              const std::vector<int> &contact_mode,
                              Eigen::VectorXd &tau) const;
 
  bool convertCentroidalToFullBody(const Eigen::VectorXd &body_state,
                                   const Eigen::VectorXd &foot_positions,
                                   const Eigen::VectorXd &foot_velocities,
                                   const Eigen::VectorXd &grfs,
                                   Eigen::VectorXd &joint_positions,
                                   Eigen::VectorXd &joint_velocities,
                                   Eigen::VectorXd &torques);
 
  bool applyMotorModel(const Eigen::VectorXd &torques,
                       Eigen::VectorXd &constrained_torques);
 
  bool applyMotorModel(const Eigen::VectorXd &torques,
                       const Eigen::VectorXd &joint_velocities,
                       Eigen::VectorXd &constrained_torques);
 
  bool isValidFullState(const Eigen::VectorXd &body_state,
                        const Eigen::VectorXd &joint_state,
                        const Eigen::VectorXd &torques,
                        const grid_map::GridMap &terrain,
                        Eigen::VectorXd &state_violation,
                        Eigen::VectorXd &control_violation);
 
  bool isValidCentroidalState(
      const Eigen::VectorXd &body_state, const Eigen::VectorXd &foot_positions,
      const Eigen::VectorXd &foot_velocities, const Eigen::VectorXd &grfs,
      const grid_map::GridMap &terrain, Eigen::VectorXd &joint_positions,
      Eigen::VectorXd &joint_velocities, Eigen::VectorXd &torques,
      Eigen::VectorXd &state_violation, Eigen::VectorXd &control_violation);
 
  inline double getGroundClearance(const Eigen::Vector3d &point,
                                   const grid_map::GridMap &terrain) {
    grid_map::Position pos = {point.x(), point.y()};
    return (point.z() - terrain.atPosition("z", pos));
  }
 
 private:
  const int num_feet_ = 4;
 
  std::vector<double> l0_vec_;
 
  double l1_;
 
  double l2_;
 
  Eigen::Vector3d abad_offset_;
 
  Eigen::Vector3d knee_offset_;
 
  Eigen::Vector3d foot_offset_;
 
  std::vector<Eigen::Vector3d> legbase_offsets_;
 
  std::vector<Eigen::Matrix4d> g_body_legbases_;
 
  const double joint_eps = 0.1;
 
  std::vector<std::vector<double>> joint_min_;
 
  std::vector<std::vector<double>> joint_max_;
 
  RigidBodyDynamics::Model *model_;
 
  std::vector<std::string> body_name_list_;
 
  std::vector<unsigned int> body_id_list_;
 
  std::vector<int> leg_idx_list_;
 
  const double abad_tau_max_ = 21;
 
  const double hip_tau_max_ = 21;
 
  const double knee_tau_max_ = 32;
 
  const Eigen::VectorXd tau_max_ =
      (Eigen::VectorXd(12) << abad_tau_max_, hip_tau_max_, knee_tau_max_,
       abad_tau_max_, hip_tau_max_, knee_tau_max_, abad_tau_max_, hip_tau_max_,
       knee_tau_max_, abad_tau_max_, hip_tau_max_, knee_tau_max_)
          .finished();
 
  const double abad_vel_max_ = 37.7;
 
  const double hip_vel_max_ = 37.7;
 
  const double knee_vel_max_ = 25.1;
 
  const Eigen::VectorXd vel_max_ =
      (Eigen::VectorXd(12) << abad_vel_max_, hip_vel_max_, knee_vel_max_,
       abad_vel_max_, hip_vel_max_, knee_vel_max_, abad_vel_max_, hip_vel_max_,
       knee_vel_max_, abad_vel_max_, hip_vel_max_, knee_vel_max_)
          .finished();
 
  const Eigen::VectorXd mm_slope_ = tau_max_.cwiseQuotient(vel_max_);
};
 
}  // namespace quad_utils
 
#endif  // QUAD_KD_H