diff --git a/CMakeLists.txt b/CMakeLists.txt
index d63e6a89a3ac70d7e25e96d638c94af7a065ae3f..7c3bd94f02e782f1007345bb6f6750f5c27ce828 100644
--- a/CMakeLists.txt
+++ b/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})
diff --git a/include/qrw/Controller.hpp b/include/qrw/Controller.hpp
index 6deae104d5142a7c7c4e67e84ec79db3f0d7b4b1..78b1b24e7bbe1d9cb13aa5fa19ad1085cbe12c10 100644
--- a/include/qrw/Controller.hpp
+++ b/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
diff --git a/include/qrw/Joystick.hpp b/include/qrw/Joystick.hpp
index d6a7868dd31b1d1d944ffb60084ccccd02870a12..6b02fb8085e4640cb3fd83270772dabf8310f0eb 100644
--- a/include/qrw/Joystick.hpp
+++ b/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
diff --git a/include/qrw/MpcWrapper.hpp b/include/qrw/MpcWrapper.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..64853b85c7deac2d056199a345937de390701ff7
--- /dev/null
+++ b/include/qrw/MpcWrapper.hpp
@@ -0,0 +1,68 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////
+///
+/// \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
diff --git a/src/Controller.cpp b/src/Controller.cpp
index 3f2c7cef0ea7ee292d89e8d6fd51d512e06ca260..c0a667de1e4219b4688fa8fb1eaa13b6bc457e2a 100644
--- a/src/Controller.cpp
+++ b/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_ = ¶ms;
+ // 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
diff --git a/src/Joystick.cpp b/src/Joystick.cpp
index 92edaf958af64b647b7b10ac08912744fb805fea..6c82af9f50f680eb464642953642485687a3d8f1 100644
--- a/src/Joystick.cpp
+++ b/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
+}
diff --git a/src/MpcWrapper.cpp b/src/MpcWrapper.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..be0c4f6e8dd1335a89dedbbf6fac0f087d26ae6a
--- /dev/null
+++ b/src/MpcWrapper.cpp
@@ -0,0 +1,23 @@
+#include "qrw/MpcWrapper.hpp"
+
+MpcWrapper::MpcWrapper()
+ : test(0) {}
+
+void MpcWrapper::initialize(Params& params) {
+
+ params_ = ¶ms;
+ 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();
+}