Continue implementing Controller in C++ and create MpcWrapper C++ class

CMakeLists.txt

@@ -61,6 +61,7 @@ set(${PROJECT_NAME}_HEADERS
   include/qrw/Joystick.hpp
   include/qrw/Filter.hpp
   include/qrw/Controller.hpp
+  include/qrw/MpcWrapper.hpp
   include/other/st_to_cc.hpp
   )
 
@@ -79,6 +80,7 @@ set(${PROJECT_NAME}_SOURCES
   src/Joystick.cpp
   src/Filter.cpp
   src/Controller.cpp
+  src/MpcWrapper.cpp
   )
 
 add_library(${PROJECT_NAME} SHARED ${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_HEADERS})

include/qrw/Controller.hpp

@@ -17,8 +17,17 @@
 #include <fstream>
 #include <iostream>
 #include <string>
-#include "qrw/Params.hpp"
 #include "qrw/Types.h"
+#include "qrw/Params.hpp"
+#include "qrw/Joystick.hpp"
+#include "qrw/Estimator.hpp"
+#include "qrw/Gait.hpp"
+#include "qrw/FootstepPlanner.hpp"
+#include "qrw/StatePlanner.hpp"
+#include "qrw/MpcWrapper.hpp"
+#include "qrw/FootTrajectoryGenerator.hpp"
+#include "qrw/QPWBC.hpp"
+#include "qrw/Filter.hpp"
 
 class Controller {
  public:
@@ -54,21 +63,56 @@ class Controller {
   ////////////////////////////////////////////////////////////////////////////////////////////////
   void compute(std::shared_ptr<odri_control_interface::Robot> robot);
 
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  ///
+  /// \brief Perform a security check before sending commands to the robot
+  ///
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  void security_check();
+
   // Commands to be sent to the robot
   Vector12 P;
   Vector12 D;
   Vector12 q_des;
   Vector12 v_des;
   Vector12 tau_ff;
-  double FF;
+  Vector12 FF;
 
   // Control info
   Params* params_;  // Params object to store parameters
   bool error;
+  int error_flag;
+  Vector12 error_value;
   
  private:
   
-  double test = 0;
+  int k;
+  int k_mpc;
+
+  // Classes of the different control blocks
+  Joystick joystick = Joystick();
+  Estimator estimator = Estimator();
+  Gait gait = Gait();
+  FootstepPlanner footstepPlanner = FootstepPlanner();
+  StatePlanner statePlanner = StatePlanner();
+  MpcWrapper mpcWrapper = MpcWrapper();
+  FootTrajectoryGenerator footTrajectoryGenerator = FootTrajectoryGenerator();
+  WbcWrapper wbcWrapper = WbcWrapper();
+
+  // Filters
+  Filter filter_mpc_q = Filter();
+  Filter filter_mpc_v = Filter();
+  Filter filter_mpc_vref = Filter();
+  Vector18 q_filt_mpc;
+  Vector6 h_v_filt_mpc;
+  Vector6 vref_filt_mpc;
+
+  // Various
+  Matrix34 o_targetFootstep;
+  Vector18 q_wbc;
+  Vector18 dq_wbc;
+  Vector12 xgoals;
+
 };
 
 #endif  // CONTROLLER_H_INCLUDED

include/qrw/Joystick.hpp

@@ -44,10 +44,18 @@ class Joystick {
   ////////////////////////////////////////////////////////////////////////////////////////////////
   VectorN handle_v_switch(double k, VectorN const& k_switch, MatrixN const& v_switch);
 
+  void update_v_ref(int k, int velID);
+
+  Vector6 getVRef() { return v_ref_; }
+  int getJoystickCode() { return joystick_code_; }
+  bool getStop() { return stop_; }
+
  private:
   Vector6 A3_;     // Third order coefficient of the polynomial that generates the velocity profile
   Vector6 A2_;     // Second order coefficient of the polynomial that generates the velocity profile
   Vector6 v_ref_;  // Reference velocity resulting of the polynomial interpolation
+  int joystick_code_ = 0;
+  bool stop_ = false;
 };
 
 #endif  // JOYSTICK_H_INCLUDED

include/qrw/MpcWrapper.hpp

+///////////////////////////////////////////////////////////////////////////////////////////////////
+///
+/// \brief This is the header for MpcWrapper class
+///
+/// \details Handle the communication between the main control loop and the MPC
+///
+//////////////////////////////////////////////////////////////////////////////////////////////////
+
+#ifndef MPCWRAPPER_H_INCLUDED
+#define MPCWRAPPER_H_INCLUDED
+
+#include "pinocchio/math/rpy.hpp"
+#include <Eigen/Core>
+#include <Eigen/Dense>
+#include "qrw/Types.h"
+#include "qrw/MPC.hpp"
+
+class MpcWrapper {
+ public:
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  ///
+  /// \brief Constructor
+  ///
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  MpcWrapper();
+
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  ///
+  /// \brief Initialize with given data
+  ///
+  /// \param[in] params Object that stores parameters
+  ///
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  void initialize(Params& params);
+
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  ///
+  /// \brief Destructor
+  ///
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  ~MpcWrapper() {}  // Empty destructor
+
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  ///
+  /// \brief Send data to the MPC to solve one iteration of MPC
+  ///
+  /// \param[in] robot Pointer to the robot interface
+  ///
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  void solve(int k, MatrixN const& xref, MatrixN const& fsteps, MatrixN const& gait, Matrix34 const& l_fsteps_target);
+
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  ///
+  /// \brief Return the latest available result of the MPC
+  ///
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  Vector12 get_latest_result();
+
+ private:
+  
+  Params* params_;
+  MPC mpc_;
+
+  int test = 0;
+
+};
+
+#endif  // MPCWRAPPER_H_INCLUDED

src/Controller.cpp

@@ -6,16 +6,177 @@ Controller::Controller()
       q_des(Vector12::Zero()),
       v_des(Vector12::Zero()),
       tau_ff(Vector12::Zero()),
-      FF(0.0),
-      error(false) {}
+      FF(Vector12::Zero()),
+      error(false),
+      error_flag(0),
+      error_value(Vector12::Zero()),
+      k(0),
+      k_mpc(0),
+      q_filt_mpc(Vector18::Zero()),
+      h_v_filt_mpc(Vector6::Zero()),
+      vref_filt_mpc(Vector6::Zero()),
+      o_targetFootstep(Matrix34::Zero()),
+      q_wbc(Vector18::Zero()),
+      dq_wbc(Vector18::Zero()),
+      xgoals(Vector12::Zero()) {}
 
 void Controller::initialize(Params& params) {
   // Params store parameters
   params_ = &params;
 
+  // Initialization of the control blocks
+  statePlanner.initialize(params);
+  gait.initialize(params);
+  footstepPlanner.initialize(params, gait);
+  mpcWrapper.initialize(params);
+  footTrajectoryGenerator.initialize(params, gait);
+  estimator.initialize(params);
+  wbcWrapper.initialize(params);
+
+  filter_mpc_q.initialize(params);
+  filter_mpc_v.initialize(params);
+  filter_mpc_vref.initialize(params);
+
+  // Other variables
+  k_mpc = static_cast<int>(params.dt_mpc / params.dt_wbc);
+  P = (Vector3(params.Kp_main.data())).replicate<4, 1>();
+  D = (Vector3(params.Kd_main.data())).replicate<4, 1>();
+  FF = params.Kff_main * Vector12::Ones();
 }
 
 void Controller::compute(std::shared_ptr<odri_control_interface::Robot> robot) {
-
   std::cout << "Pass" << std::endl;
+
+  // Update the reference velocity coming from the gamepad
+  joystick.update_v_ref(k, params_->velID);
+
+  // Process state estimator
+  estimator.run_filter(gait.getCurrentGait(),
+                       footTrajectoryGenerator.getFootPosition(),
+                       robot->imu->GetLinearAcceleration(),
+                       robot->imu->GetGyroscope(),
+                       robot->imu->GetAttitudeEuler(),
+                       robot->joints->GetPositions(),
+                       robot->joints->GetVelocities(),
+                       Vector3::Zero(),
+                       Vector3::Zero());
+
+  // Update state vectors of the robot (q and v) + transformation matrices between world and horizontal frames
+  estimator.updateState(joystick.getVRef(), gait);
+
+  // Retrieve data from estimator
+  /*oRh = self.estimator.getoRh()
+  hRb = self.estimator.gethRb()
+  oTh = self.estimator.getoTh().reshape((3, 1))
+  self.a_ref[0:6, 0] = self.estimator.getARef()
+  self.v_ref[0:6, 0] = self.estimator.getVRef()
+  self.h_v[0:6, 0] = self.estimator.getHV()
+  self.h_v_windowed[0:6, 0] = self.estimator.getHVWindowed()
+  self.q[:, 0] = self.estimator.getQUpdated()
+  self.v[:, 0] = self.estimator.getVUpdated()
+  self.yaw_estim = self.estimator.getYawEstim()
+  */
+
+  // Update gait
+  gait.updateGait(k, k_mpc, joystick.getJoystickCode());
+
+  // Quantities go through a 1st order low pass filter with fc = 15 Hz (avoid >25Hz foldback)
+  q_filt_mpc.head(6) = filter_mpc_q.filter(estimator.getQUpdated().head(6), true);
+  q_filt_mpc.tail(12) = estimator.getQUpdated().tail(12);
+  h_v_filt_mpc = filter_mpc_v.filter(estimator.getHV().head(6), false);
+  vref_filt_mpc = filter_mpc_vref.filter(estimator.getVRef().head(6), false);
+
+  // Compute target footstep based on current and reference velocities
+  o_targetFootstep = footstepPlanner.updateFootsteps((k % k_mpc == 0) && (k != 0),
+                                                      static_cast<int>(k_mpc - (k % k_mpc)),
+                                                      estimator.getQUpdated().head(18),
+                                                      estimator.getHVWindowed().head(6),
+                                                      estimator.getVRef().head(6));
+
+  // Run state planner (outputs the reference trajectory of the base)
+  statePlanner.computeReferenceStates(q_filt_mpc.head(6), h_v_filt_mpc, vref_filt_mpc, 0.0);
+
+  // Solve MPC problem once every k_mpc iterations of the main loop
+  if (k % k_mpc == 0)
+  {
+    mpcWrapper.solve(k, statePlanner.getReferenceStates(), footstepPlanner.getFootsteps(),
+                      gait.getCurrentGait(), Matrix34::Zero());
+  }
+
+  // Update pos, vel and acc references for feet
+  footTrajectoryGenerator.update(k, o_targetFootstep);
+
+  // Whole Body Control
+  // If nothing wrong happened yet in the WBC controller
+  if (!error && !joystick.getStop())
+  {
+      // Update configuration vector for wbc
+      q_wbc(3, 0) = q_filt_mpc(3, 0);  // Roll
+      q_wbc(4, 0) = q_filt_mpc(4, 0);  // Pitch
+      q_wbc.tail(12) = wbcWrapper.get_qdes();  // with reference angular positions of previous loop
+
+      // Update velocity vector for wbc
+      dq_wbc.head(6) = estimator.getVFilt().head(6);  //  Velocities in base frame (not horizontal frame!)
+      dq_wbc.tail(12) = wbcWrapper.get_vdes();  // with reference angular velocities of previous loop
+
+      // Desired position, orientation and velocities of the base
+      xgoals.tail(6) = vref_filt_mpc;  // Velocities (in horizontal frame!)
+
+      // Run InvKin + WBC QP
+      wbcWrapper.compute(
+        q_wbc, dq_wbc, mpcWrapper.get_latest_result().block(12, 0, 12, 1), gait.getCurrentGait().row(0),
+        footTrajectoryGenerator.getFootAccelerationBaseFrame(estimator.gethRb() * estimator.getoRh().transpose(),
+                                                             Vector3::Zero(), Vector3::Zero()),
+        footTrajectoryGenerator.getFootVelocityBaseFrame(estimator.gethRb() * estimator.getoRh().transpose(),
+                                                         Vector3::Zero(), Vector3::Zero()),
+        footTrajectoryGenerator.getFootPositionBaseFrame(estimator.gethRb() * estimator.getoRh().transpose(),
+                                                         estimator.getoTh() + Vector3(0.0, 0.0, params_->h_ref)),
+        xgoals);
+
+      // Quantities sent to the control board
+      q_des = wbcWrapper.get_qdes();
+      v_des = wbcWrapper.get_vdes();
+      tau_ff = wbcWrapper.get_tau_ff();
+  }
+
+  // Security check
+  security_check();
+
+  // Increment loop counter
+  k++;
+}
+
+void Controller::security_check()
+{
+  if (error_flag == 0 && !error && !joystick.getStop())
+  {
+    error_flag = estimator.security_check(tau_ff);
+    if (error_flag != 0)
+    {
+      error = true;
+      switch (error_flag)
+      {
+        case 1:
+          error_value = estimator.getQFilt().tail(12) * 180 / 3.1415;
+          break;
+        case 2:
+          error_value = estimator.getVSecu();
+          break;
+        default:
+          error_value = tau_ff;
+      }
+    }
+  }
+
+  // If something wrong happened in the controller we stick to a security controller
+  if (error || joystick.getStop())
+  {
+    // Quantities sent to the control board
+    P = Vector12::Zero();
+    D = 0.1 * Vector12::Ones();
+    q_des = Vector12::Zero();
+    v_des = Vector12::Zero();
+    FF = Vector12::Zero();
+    tau_ff = Vector12::Zero();
+  }
 }
\ No newline at end of file

src/Joystick.cpp

@@ -16,3 +16,8 @@ VectorN Joystick::handle_v_switch(double k, VectorN const& k_switch, MatrixN con
   }
   return v_ref_;
 }
+
+void Joystick::update_v_ref(int k, int velID)
+{
+  // TODO
+}

src/MpcWrapper.cpp

+#include "qrw/MpcWrapper.hpp"
+
+MpcWrapper::MpcWrapper()
+    : test(0) {}
+
+void MpcWrapper::initialize(Params& params) {
+  
+  params_ = &params;
+  mpc_ = MPC(params);
+
+}
+
+void MpcWrapper::solve(int k, MatrixN const& xref, MatrixN const& fsteps,
+                       MatrixN const& gait, Matrix34 const& l_fsteps_target) {
+  
+  std::cout << "Pass" << std::endl;
+
+}
+
+Vector12 MpcWrapper::get_latest_result()
+{
+    return Vector12::Zero();
+}