local_footstep_planner.h

#ifndef LOCAL_FOOTSTEP_PLANNER_H
#define LOCAL_FOOTSTEP_PLANNER_H
 
#include <eigen_conversions/eigen_msg.h>
#include <local_planner/local_planner_modes.h>
#include <nav_msgs/Path.h>
#include <quad_msgs/FootPlanDiscrete.h>
#include <quad_msgs/FootState.h>
#include <quad_msgs/MultiFootPlanContinuous.h>
#include <quad_msgs/MultiFootPlanDiscrete.h>
#include <quad_msgs/MultiFootState.h>
#include <quad_msgs/RobotPlan.h>
#include <quad_msgs/RobotState.h>
#include <quad_utils/fast_terrain_map.h>
#include <quad_utils/function_timer.h>
#include <quad_utils/math_utils.h>
#include <quad_utils/quad_kd.h>
#include <quad_utils/ros_utils.h>
#include <ros/ros.h>
 
#include <eigen3/Eigen/Eigen>
#include <grid_map_core/grid_map_core.hpp>
#include <grid_map_ros/GridMapRosConverter.hpp>
#include <grid_map_ros/grid_map_ros.hpp>
 
 
class LocalFootstepPlanner {
 public:
  LocalFootstepPlanner();
 
  void setTemporalParams(double dt, int period, int horizon_length,
                         const std::vector<double> &duty_cycles,
                         const std::vector<double> &phase_offsets);
 
  void setSpatialParams(double ground_clearance, double hip_clearance,
                        double grf_weight, double standing_error_threshold,
                        std::shared_ptr<quad_utils::QuadKD> kinematics,
                        double foothold_search_radius,
                        double foothold_obj_threshold,
                        std::string obj_fun_layer, double toe_radius);
 
  void getFootPositionsBodyFrame(const Eigen::VectorXd &body_plan,
                                 const Eigen::VectorXd &foot_positions_world,
                                 Eigen::VectorXd &foot_positions_body);
 
  void getFootPositionsBodyFrame(const Eigen::MatrixXd &body_plan,
                                 const Eigen::MatrixXd &foot_positions_world,
                                 Eigen::MatrixXd &foot_positions_body);
 
  void updateMap(const FastTerrainMap &terrain);
 
  void updateMap(const grid_map::GridMap &terrain);
 
  void computeContactSchedule(int current_plan_index,
                              const Eigen::MatrixXd &body_plan,
                              const Eigen::VectorXi &ref_primitive_plan_,
                              int control_mode,
                              std::vector<std::vector<bool>> &contact_schedule);
 
  void computeFootPlan(int current_plan_index,
                       const std::vector<std::vector<bool>> &contact_schedule,
                       const Eigen::MatrixXd &body_plan,
                       const Eigen::MatrixXd &grf_plan,
                       const Eigen::MatrixXd &ref_body_plan,
                       const Eigen::VectorXd &foot_positions_current,
                       const Eigen::VectorXd &foot_velocities_current,
                       double first_element_duration,
                       quad_msgs::MultiFootState &past_footholds_msg,
                       Eigen::MatrixXd &foot_positions,
                       Eigen::MatrixXd &foot_velocities,
                       Eigen::MatrixXd &foot_accelerations);
 
  void loadFootPlanMsgs(
      const std::vector<std::vector<bool>> &contact_schedule,
      int current_plan_index, double first_element_duration,
      const Eigen::MatrixXd &foot_positions,
      const Eigen::MatrixXd &foot_velocities,
      const Eigen::MatrixXd &foot_accelerations,
      quad_msgs::MultiFootPlanDiscrete &future_footholds_msg,
      quad_msgs::MultiFootPlanContinuous &foot_plan_continuous_msg);
 
  inline void printContactSchedule(
      const std::vector<std::vector<bool>> &contact_schedule) {
    for (int i = 0; i < contact_schedule.size(); i++) {
      for (int j = 0; j < contact_schedule.at(i).size(); j++) {
        if (contact_schedule[i][j]) {
          printf("1 ");
        } else {
          printf("0 ");
        }
      }
      printf("\n");
    }
  }
 
  inline double getTerrainHeight(double x, double y) {
    grid_map::Position pos = {x, y};
    double height = this->terrain_grid_.atPosition(
        "z_smooth", terrain_grid_.getClosestPositionInMap(pos),
        grid_map::InterpolationMethods::INTER_LINEAR);
    return (height);
  }
 
  inline double getTerrainSlope(double x, double y, double dx, double dy) {
    std::array<double, 3> surf_norm =
        this->terrain_.getSurfaceNormalFiltered(x, y);
 
    double denom = dx * dx + dy * dy;
    if (denom <= 0 || surf_norm[2] <= 0) {
      double default_pitch = 0;
      return default_pitch;
    } else {
      double v_proj = (dx * surf_norm[0] + dy * surf_norm[1]) / sqrt(denom);
      double pitch = atan2(v_proj, surf_norm[2]);
 
      return pitch;
    }
  }
 
  inline void getTerrainSlope(double x, double y, double yaw, double &roll,
                              double &pitch) {
    std::array<double, 3> surf_norm =
        this->terrain_.getSurfaceNormalFiltered(x, y);
 
    Eigen::Vector3d norm_vec(surf_norm.data());
    Eigen::Vector3d axis = Eigen::Vector3d::UnitZ().cross(norm_vec);
    double ang = acos(
        std::max(std::min(Eigen::Vector3d::UnitZ().dot(norm_vec), 1.), -1.));
 
    if (ang < 1e-3) {
      roll = 0;
      pitch = 0;
      return;
    } else {
      Eigen::Matrix3d rot(Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()));
      axis = rot.transpose() * (axis / axis.norm());
      tf2::Quaternion quat(tf2::Vector3(axis(0), axis(1), axis(2)), ang);
      tf2::Matrix3x3 m(quat);
      double tmp;
      m.getRPY(roll, pitch, tmp);
    }
  }
 
  inline void setTerrainMap(const grid_map::GridMap &grid_map) {
    terrain_grid_ = grid_map;
  }
 
  // Compute future states by integrating linear states (hold orientation
  // states)
  inline Eigen::VectorXd computeFutureBodyPlan(
      double step, const Eigen::VectorXd &body_plan) {
    // Initialize vector
    Eigen::VectorXd future_body_plan = body_plan;
 
    // Integrate the linear state
    future_body_plan.segment(0, 3) =
        future_body_plan.segment(0, 3) + body_plan.segment(6, 3) * step * dt_;
 
    return future_body_plan;
  }
 
 private:
  void updateContinuousPlan();
 
  void cubicHermiteSpline(double pos_prev, double vel_prev, double pos_next,
                          double vel_next, double phase, double duration,
                          double &pos, double &vel, double &acc);
 
  Eigen::Vector3d getNearestValidFoothold(
      const Eigen::Vector3d &foot_position,
      const Eigen::Vector3d &foot_position_prev_solve) const;
 
  Eigen::Vector3d welzlMinimumCircle(std::vector<Eigen::Vector2d> P,
                                     std::vector<Eigen::Vector2d> R);
 
  double computeSwingApex(int leg_idx, const Eigen::VectorXd &body_plan,
                          const Eigen::Vector3d &foot_position_prev,
                          const Eigen::Vector3d &foot_position_next);
 
  inline Eigen::Vector3d getFootData(const Eigen::MatrixXd &foot_state_vars,
                                     int horizon_index, int foot_index) {
    return foot_state_vars.block<1, 3>(horizon_index, 3 * foot_index);
  }
 
  inline bool isContact(const std::vector<std::vector<bool>> &contact_schedule,
                        int horizon_index, int foot_index) {
    return (contact_schedule.at(horizon_index).at(foot_index));
  }
 
  inline bool isNewContact(
      const std::vector<std::vector<bool>> &contact_schedule, int horizon_index,
      int foot_index) {
    if (horizon_index == 0) return false;
 
    return (!isContact(contact_schedule, horizon_index - 1, foot_index) &&
            isContact(contact_schedule, horizon_index, foot_index));
  }
 
  inline bool isNewLiftoff(
      const std::vector<std::vector<bool>> &contact_schedule, int horizon_index,
      int foot_index) {
    if (horizon_index == 0) return false;
 
    return (isContact(contact_schedule, horizon_index - 1, foot_index) &&
            !isContact(contact_schedule, horizon_index, foot_index));
  }
 
  inline int getNextContactIndex(
      const std::vector<std::vector<bool>> &contact_schedule, int horizon_index,
      int foot_index) {
    // Loop through the rest of this contact schedule, if a new contact is found
    // return its index
    for (int i_touchdown = horizon_index; i_touchdown < horizon_length_;
         i_touchdown++) {
      if (isNewContact(contact_schedule, i_touchdown, foot_index)) {
        return i_touchdown;
      }
    }
 
    // If no contact is found, return the last index in the horizon
    return (horizon_length_ - 1);
  }
 
  inline int getNextLiftoffIndex(
      const std::vector<std::vector<bool>> &contact_schedule, int horizon_index,
      int foot_index) {
    // Loop through the rest of this contact schedule, if a new liftoff is found
    // return its index
    for (int i_liftoff = horizon_index; i_liftoff < horizon_length_;
         i_liftoff++) {
      if (isNewLiftoff(contact_schedule, i_liftoff, foot_index)) {
        return i_liftoff;
      }
    }
 
    // If no contact is found, return the last index in the horizon
    return (horizon_length_ - 1);
  }
 
  FastTerrainMap terrain_;
 
  grid_map::GridMap terrain_grid_;
 
  const int num_feet_ = 4;
 
  double dt_;
 
  int period_;
 
  int horizon_length_;
 
  std::vector<double> phase_offsets_ = {0, 0.5, 0.5, 0.0};
 
  std::vector<double> duty_cycles_ = {0.5, 0.5, 0.5, 0.5};
 
  std::vector<std::vector<bool>> nominal_contact_schedule_;
 
  double ground_clearance_;
 
  double hip_clearance_;
 
  double grf_weight_;
 
  const int CONNECT_STANCE = 0;
 
  const int LEAP_STANCE = 1;
 
  const int FLIGHT = 2;
 
  const int LAND_STANCE = 3;
 
  std::shared_ptr<quad_utils::QuadKD> quadKD_;
 
  double standing_error_threshold_ = 0;
 
  double foothold_search_radius_;
 
  double foothold_obj_threshold_;
 
  std::string obj_fun_layer_;
 
  double toe_radius_;
};
 
#endif  // LOCAL_FOOTSTEP_PLANNER_H