diff --git a/CMakeLists.txt b/CMakeLists.txt
index 0f04cd6f97bb81306c5a35d9108cc12bd52cd055..7335a564167991b77a83d55ab459baee09c2d479 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -53,6 +53,7 @@ set(${PROJECT_NAME}_HEADERS
include/qrw/Types.h
include/qrw/InvKin.hpp
include/qrw/QPWBC.hpp
+ include/qrw/Params.hpp
include/other/st_to_cc.hpp
)
@@ -66,6 +67,7 @@ set(${PROJECT_NAME}_SOURCES
src/StatePlanner.cpp
src/InvKin.cpp
src/QPWBC.cpp
+ src/Params.cpp
)
add_library(${PROJECT_NAME} SHARED ${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_HEADERS})
@@ -99,6 +101,11 @@ find_package(osqp REQUIRED)
# Link the OSQP shared library
target_link_libraries(${PROJECT_NAME} PRIVATE osqp::osqp)
+#find_package(yaml-cpp REQUIRED)
+#target_link_libraries(${PROJECT_NAME} PRIVATE yaml-cpp::yaml-cpp)
+add_project_dependency(yaml-cpp CONFIG REQUIRED)
+target_link_libraries(${PROJECT_NAME} PUBLIC ${YAML_CPP_LIBRARIES})
+
if(SUFFIX_SO_VERSION)
set_target_properties(${PROJECT_NAME} PROPERTIES SOVERSION ${PROJECT_VERSION})
endif()
@@ -108,6 +115,7 @@ endif()
target_compile_options(${PROJECT_NAME} PUBLIC -DNDEBUG -O3)
+target_compile_definitions(${PROJECT_NAME} PUBLIC CONFIG_SOLO12_YAML="${PROJECT_SOURCE_DIR}/src/config_solo12.yaml")
# Main Executable
add_executable(${PROJECT_NAMESPACE}-${PROJECT_NAME} src/main.cpp)
diff --git a/README.md b/README.md
index a6700fc8d4a07c783bc6744c921f36e10ca890c8..54b9d904f5d510109fa3412f6d2c7037017a34cd 100644
--- a/README.md
+++ b/README.md
@@ -30,6 +30,8 @@ Implementation of a reactive walking controller for quadruped robots. Architectu
* cmake .. -DCMAKE_BUILD_TYPE=RELEASE -DCMAKE_INSTALL_PREFIX=~/install -DPYTHON_EXECUTABLE=$(which python3.6) -DPYTHON_STANDARD_LAYOUT=ON (to install in ~/install folder)
* make install
+* Install YAML parser for C++: [https://github.com/jbeder/yaml-cpp]
+
* Install package that handles the gamepad: `pip3 install --user inputs`
* Install eiquadprog: `sudo apt install robotpkg-eiquadprog`
diff --git a/include/qrw/Params.hpp b/include/qrw/Params.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..92e1bd420e649dfcdbfcc95dd21a1379dda326bb
--- /dev/null
+++ b/include/qrw/Params.hpp
@@ -0,0 +1,91 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////
+///
+/// \brief This is the header for Params class
+///
+/// \details Planner that outputs the reference trajectory of the base based on the reference
+/// velocity given by the user and the current position/velocity of the base
+///
+//////////////////////////////////////////////////////////////////////////////////////////////////
+
+#ifndef PARAMS_H_INCLUDED
+#define PARAMS_H_INCLUDED
+
+#include "qrw/Types.h"
+#include <yaml-cpp/yaml.h>
+
+class Params
+{
+public:
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Empty constructor
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ Params();
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Destructor.
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ~Params() {}
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Initializer
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void initialize(const std::string& file_path);
+
+
+ // See .yaml file for meaning of parameters
+ std::string interface;
+ bool SIMULATION;
+ bool LOGGING;
+ bool PLOTTING;
+ double dt_wbc;
+ int N_gait;
+ int envID;
+ int velID;
+ double dt_mpc;
+ double T_gait;
+ double T_mpc;
+ int N_SIMULATION;
+ bool type_MPC;
+ bool use_flat_plane;
+ bool predefined_vel;
+ bool kf_enabled;
+ bool enable_pyb_GUI;
+
+};
+
+namespace yaml_control_interface
+{
+#define assert_yaml_parsing(yaml_node, parent_node_name, child_node_name) \
+ if (!yaml_node[child_node_name]) \
+ { \
+ std::ostringstream oss; \
+ oss << "Error: Wrong parsing of the YAML file from src file: [" \
+ << __FILE__ << "], in function: [" << __FUNCTION__ << "], line: [" \
+ << __LINE__ << ". Node [" << child_node_name \
+ << "] does not exists under the node [" << parent_node_name \
+ << "]."; \
+ throw std::runtime_error(oss.str()); \
+ } \
+ assert(true)
+
+#define assert_file_exists(filename) \
+ std::ifstream f(filename.c_str()); \
+ if (!f.good()) \
+ { \
+ std::ostringstream oss; \
+ oss << "Error: Problem opening the file [" << filename \
+ << "], from src file: [" << __FILE__ << "], in function: [" \
+ << __FUNCTION__ << "], line: [" << __LINE__ \
+ << ". The file may not exists."; \
+ throw std::runtime_error(oss.str()); \
+ } \
+ assert(true)
+} // end of yaml_control_interface namespace
+
+#endif // PARAMS_H_INCLUDED
diff --git a/python/gepadd.cpp b/python/gepadd.cpp
index a6685eac72825b4eb9d146b7a54d5996c343fcdf..7903d8b6f70a9768e0864fe9c2220aea1a1c5b6f 100644
--- a/python/gepadd.cpp
+++ b/python/gepadd.cpp
@@ -7,6 +7,7 @@
#include "qrw/FootstepPlanner.hpp"
#include "qrw/FootTrajectoryGenerator.hpp"
#include "qrw/QPWBC.hpp"
+#include "qrw/Params.hpp"
#include <boost/python.hpp>
#include <eigenpy/eigenpy.hpp>
@@ -207,6 +208,9 @@ struct InvKinPythonVisitor : public bp::def_visitor<InvKinPythonVisitor<InvKin>>
void exposeInvKin() { InvKinPythonVisitor<InvKin>::expose(); }
+/////////////////////////////////
+/// Binding QPWBC class
+/////////////////////////////////
template <typename QPWBC>
struct QPWBCPythonVisitor : public bp::def_visitor<QPWBCPythonVisitor<QPWBC>>
{
@@ -230,9 +234,55 @@ struct QPWBCPythonVisitor : public bp::def_visitor<QPWBCPythonVisitor<QPWBC>>
ENABLE_SPECIFIC_MATRIX_TYPE(matXd);
}
};
-
void exposeQPWBC() { QPWBCPythonVisitor<QPWBC>::expose(); }
+/////////////////////////////////
+/// Binding Params class
+/////////////////////////////////
+template <typename Params>
+struct ParamsPythonVisitor : public bp::def_visitor<ParamsPythonVisitor<Params>>
+{
+ template <class PyClassParams>
+ void visit(PyClassParams& cl) const
+ {
+ cl.def(bp::init<>(bp::arg(""), "Default constructor."))
+
+ .def("initialize", &Params::initialize, bp::args("file_path"),
+ "Initialize Params from Python.\n")
+
+ // Read Params from Python
+ .def_readwrite("interface", &Params::interface)
+ .def_readwrite("SIMULATION", &Params::SIMULATION)
+ .def_readwrite("LOGGING", &Params::LOGGING)
+ .def_readwrite("PLOTTING", &Params::PLOTTING)
+ .def_readwrite("dt_wbc", &Params::dt_wbc)
+ .def_readwrite("N_gait", &Params::N_gait)
+ .def_readwrite("envID", &Params::envID)
+ .def_readwrite("velID", &Params::velID)
+ .def_readwrite("dt_mpc", &Params::dt_mpc)
+ .def_readwrite("T_gait", &Params::T_gait)
+ .def_readwrite("T_mpc", &Params::T_mpc)
+ .def_readwrite("N_SIMULATION", &Params::N_SIMULATION)
+ .def_readwrite("type_MPC", &Params::type_MPC)
+ .def_readwrite("use_flat_plane", &Params::use_flat_plane)
+ .def_readwrite("predefined_vel", &Params::predefined_vel)
+ .def_readwrite("kf_enabled", &Params::kf_enabled)
+ .def_readwrite("enable_pyb_GUI", &Params::enable_pyb_GUI);
+
+ }
+
+ static void expose()
+ {
+ bp::class_<Params>("Params", bp::no_init).def(ParamsPythonVisitor<Params>());
+
+ ENABLE_SPECIFIC_MATRIX_TYPE(MatrixN);
+ }
+};
+void exposeParams() { ParamsPythonVisitor<Params>::expose(); }
+
+/////////////////////////////////
+/// Exposing classes
+/////////////////////////////////
BOOST_PYTHON_MODULE(libquadruped_reactive_walking)
{
boost::python::def("add", gepetto::example::add);
@@ -247,4 +297,5 @@ BOOST_PYTHON_MODULE(libquadruped_reactive_walking)
exposeFootTrajectoryGenerator();
exposeInvKin();
exposeQPWBC();
+ exposeParams();
}
\ No newline at end of file
diff --git a/scripts/Controller.py b/scripts/Controller.py
index 7ad95641b2e04f0be6b125fa295970953f03d3a9..b29c2683ea134201602546241a2f71db770fee58 100644
--- a/scripts/Controller.py
+++ b/scripts/Controller.py
@@ -49,8 +49,7 @@ class dummyDevice:
class Controller:
def __init__(self, q_init, envID, velID, dt_wbc, dt_mpc, k_mpc, t, T_gait, T_mpc, N_SIMULATION, type_MPC,
- pyb_feedback, on_solo8, use_flat_plane, predefined_vel, enable_pyb_GUI, kf_enabled, N0_gait,
- isSimulation):
+ use_flat_plane, predefined_vel, enable_pyb_GUI, kf_enabled, N0_gait, isSimulation):
"""Function that runs a simulation scenario based on a reference velocity profile, an environment and
various parameters to define the gait
@@ -65,8 +64,6 @@ class Controller:
T_mpc (float): duration of mpc prediction horizon
N_SIMULATION (int): number of iterations of inverse dynamics during the simulation
type_mpc (bool): True to have PA's MPC, False to have Thomas's MPC
- pyb_feedback (bool): whether PyBullet feedback is enabled or not
- on_solo8 (bool): whether we are working on solo8 or not
use_flat_plane (bool): to use either a flat ground or a rough ground
predefined_vel (bool): to use either a predefined velocity profile or a gamepad
enable_pyb_GUI (bool): to display PyBullet GUI or not
@@ -130,6 +127,9 @@ class Controller:
self.gait = lqrw.Gait()
self.gait.initialize(dt_mpc, T_gait, T_mpc)
+ """from IPython import embed
+ embed()"""
+
shoulders = np.zeros((3, 4))
shoulders[0, :] = [0.1946, 0.1946, -0.1946, -0.1946]
shoulders[1, :] = [0.14695, -0.14695, 0.14695, -0.14695]
@@ -163,8 +163,6 @@ class Controller:
self.T_mpc = T_mpc
self.N_SIMULATION = N_SIMULATION
self.type_MPC = type_MPC
- self.pyb_feedback = pyb_feedback
- self.on_solo8 = on_solo8
self.use_flat_plane = use_flat_plane
self.predefined_vel = predefined_vel
self.enable_pyb_GUI = enable_pyb_GUI
diff --git a/scripts/LoggerControl.py b/scripts/LoggerControl.py
index 18d1f8a066f23e4c2268c9f52a1e3bd06d44dfd4..05c15924b20f0844bc28ce817d1d97fe97c7379c 100644
--- a/scripts/LoggerControl.py
+++ b/scripts/LoggerControl.py
@@ -679,6 +679,8 @@ class LoggerControl():
ax = plt.subplot(3, 2, index6[j])
h1, = plt.plot(log_t_pred, self.mpc_x_f[0, j, :], "b", linewidth=2)
h2, = plt.plot(log_t_ref, self.planner_xref[0, j, :], linestyle="--", marker='x', color="g", linewidth=2)
+ h3, = plt.plot(np.array([k*self.dt for k in range(self.mpc_x_f.shape[0])]),
+ self.planner_xref[:, j, 0], linestyle=None, marker='x', color="r", linewidth=1)
axs.append(ax)
h1s.append(h1)
h2s.append(h2)
@@ -704,6 +706,8 @@ class LoggerControl():
ax = plt.subplot(3, 2, index6[j])
h1, = plt.plot(log_t_pred, self.mpc_x_f[0, j, :], "b", linewidth=2)
h2, = plt.plot(log_t_ref, self.planner_xref[0, j, :], linestyle="--", marker='x', color="g", linewidth=2)
+ h3, = plt.plot(np.array([k*self.dt for k in range(self.mpc_x_f.shape[0])]),
+ self.planner_xref[:, j+6, 0], linestyle=None, marker='x', color="r", linewidth=1)
axs_vel.append(ax)
h1s_vel.append(h1)
h2s_vel.append(h2)
diff --git a/scripts/main_solo12_control.py b/scripts/main_solo12_control.py
index ac28183d7c7d922952a5ffcd548c76715a2400b2..911ee957713a29658d27917144503d8af18e7160 100644
--- a/scripts/main_solo12_control.py
+++ b/scripts/main_solo12_control.py
@@ -12,21 +12,17 @@ import numpy as np
import argparse
from LoggerSensors import LoggerSensors
from LoggerControl import LoggerControl
+import libquadruped_reactive_walking as lqrw
+params = lqrw.Params() # Object that holds all controller parameters
-SIMULATION = True
-LOGGING = False
-PLOTTING = True
-
-if SIMULATION:
+if params.SIMULATION:
from PyBulletSimulator import PyBulletSimulator
else:
# from pynput import keyboard
from solopython.solo12 import Solo12
from solopython.utils.qualisysClient import QualisysClient
-DT = 0.0020
-N0_gait = 100 # Same value than N0_gait in include/config.h
key_pressed = False
@@ -72,8 +68,8 @@ def clone_movements(name_interface_clone, q_init, cloneP, cloneD, cloneQdes, clo
print("-- Launching clone interface --")
- print(name_interface_clone, DT)
- clone = Solo12(name_interface_clone, dt=DT)
+ print(name_interface_clone, params.dt_wbc)
+ clone = Solo12(name_interface_clone, dt=params.dt_wbc)
clone.Init(calibrateEncoders=True, q_init=q_init)
while cloneRunning.value and not clone.hardware.IsTimeout():
@@ -102,60 +98,33 @@ def control_loop(name_interface, name_interface_clone=None):
name_interface_clone (string): name of the interface that will mimic the movements of the first
"""
- ################################
- # PARAMETERS OF THE CONTROLLER #
- ################################
-
- envID = 0 # Identifier of the environment to choose in which one the simulation will happen
- velID = 2 # Identifier of the reference velocity profile to choose which one will be sent to the robot
-
- dt_wbc = DT # Time step of the whole body control
- dt_mpc = 0.02 # Time step of the model predictive control
- k_mpc = int(dt_mpc / dt_wbc)
- t = 0.0 # Time
- T_gait = 0.32 # Duration of one gait period
- T_mpc = 0.32 # Duration of the prediction horizon
- N_SIMULATION = 12000 # number of simulated wbc time steps
-
- # Which MPC solver you want to use
- # True to have PA's MPC, to False to have Thomas's MPC
- type_MPC = True
-
- # Whether PyBullet feedback is enabled or not
- pyb_feedback = True
-
- # Whether we are working with solo8 or not
- on_solo8 = False
-
- # If True the ground is flat, otherwise it has bumps
- use_flat_plane = True
-
- # If we are using a predefined reference velocity (True) or a joystick (False)
- predefined_vel = True
-
- # Use complementary filter (False) or kalman filter (True) for the estimator
- kf_enabled = False
+ # Check .yaml file for parameters of the controller
# Enable or disable PyBullet GUI
- enable_pyb_GUI = True
- if not SIMULATION:
+ enable_pyb_GUI = params.enable_pyb_GUI
+ if not params.SIMULATION:
enable_pyb_GUI = False
+ # Time variable to keep track of time
+ t = 0.0
+
# Default position after calibration
q_init = np.array([0.0, 0.7, -1.4, -0.0, 0.7, -1.4, 0.0, -0.7, +1.4, -0.0, -0.7, +1.4])
# Run a scenario and retrieve data thanks to the logger
- controller = Controller(q_init, envID, velID, dt_wbc, dt_mpc, k_mpc, t, T_gait, T_mpc, N_SIMULATION, type_MPC,
- pyb_feedback, on_solo8, use_flat_plane, predefined_vel, enable_pyb_GUI, kf_enabled,
- N0_gait, SIMULATION)
+ controller = Controller(q_init, params.envID, params.velID, params.dt_wbc, params.dt_mpc,
+ int(params.dt_mpc / params.dt_wbc), t, params.T_gait,
+ params.T_mpc, params.N_SIMULATION, params.type_MPC, params.use_flat_plane,
+ params.predefined_vel, enable_pyb_GUI, params.kf_enabled, params.N_gait,
+ params.SIMULATION)
####
- if SIMULATION:
+ if params.SIMULATION:
device = PyBulletSimulator()
qc = None
else:
- device = Solo12(name_interface, dt=DT)
+ device = Solo12(name_interface, dt=params.dt_wbc)
qc = QualisysClient(ip="140.93.16.160", body_id=0)
if name_interface_clone is not None:
@@ -172,21 +141,22 @@ def control_loop(name_interface, name_interface_clone=None):
clone.start()
print(cloneResult.value)
- if LOGGING or PLOTTING:
- loggerSensors = LoggerSensors(device, qualisys=qc, logSize=N_SIMULATION-3)
- loggerControl = LoggerControl(dt_wbc, N0_gait, joystick=controller.joystick, estimator=controller.estimator,
- loop=controller, gait=controller.gait, statePlanner=controller.statePlanner,
+ if params.LOGGING or params.PLOTTING:
+ loggerSensors = LoggerSensors(device, qualisys=qc, logSize=params.N_SIMULATION-3)
+ loggerControl = LoggerControl(params.dt_wbc, params.N_gait, joystick=controller.joystick,
+ estimator=controller.estimator, loop=controller,
+ gait=controller.gait, statePlanner=controller.statePlanner,
footstepPlanner=controller.footstepPlanner,
footTrajectoryGenerator=controller.footTrajectoryGenerator,
- logSize=N_SIMULATION-3)
+ logSize=params.N_SIMULATION-3)
# Number of motors
nb_motors = device.nb_motors
# Initiate communication with the device and calibrate encoders
- if SIMULATION:
- device.Init(calibrateEncoders=True, q_init=q_init, envID=envID,
- use_flat_plane=use_flat_plane, enable_pyb_GUI=enable_pyb_GUI, dt=dt_wbc)
+ if params.SIMULATION:
+ device.Init(calibrateEncoders=True, q_init=q_init, envID=params.envID,
+ use_flat_plane=params.use_flat_plane, enable_pyb_GUI=enable_pyb_GUI, dt=params.dt_wbc)
else:
device.Init(calibrateEncoders=True, q_init=q_init)
@@ -195,7 +165,7 @@ def control_loop(name_interface, name_interface_clone=None):
# CONTROL LOOP ***************************************************
t = 0.0
- t_max = (N_SIMULATION-2) * dt_wbc
+ t_max = (params.N_SIMULATION-2) * params.dt_wbc
while ((not device.hardware.IsTimeout()) and (t < t_max) and (not controller.myController.error)):
@@ -207,7 +177,7 @@ def control_loop(name_interface, name_interface_clone=None):
# Check that the initial position of actuators is not too far from the
# desired position of actuators to avoid breaking the robot
- if (t <= 10 * DT):
+ if (t <= 10 * params.dt_wbc):
if np.max(np.abs(controller.result.q_des - device.q_mes)) > 0.15:
print("DIFFERENCE: ", controller.result.q_des - device.q_mes)
print("q_des: ", controller.result.q_des)
@@ -221,7 +191,7 @@ def control_loop(name_interface, name_interface_clone=None):
device.SetDesiredJointTorque(controller.result.tau_ff.ravel())
# Call logger
- if LOGGING or PLOTTING:
+ if params.LOGGING or params.PLOTTING:
loggerSensors.sample(device, qc)
loggerControl.sample(controller.joystick, controller.estimator,
controller, controller.gait, controller.statePlanner,
@@ -259,12 +229,12 @@ def control_loop(name_interface, name_interface_clone=None):
cpt_frames += 1"""
- t += DT # Increment loop time
+ t += params.dt_wbc # Increment loop time
# ****************************************************************
# Stop clone interface running in parallel process
- if not SIMULATION and name_interface_clone is not None:
+ if not params.SIMULATION and name_interface_clone is not None:
cloneResult.value = False
# Stop MPC running in a parallel process
@@ -291,7 +261,7 @@ def control_loop(name_interface, name_interface_clone=None):
if ((device.cpt % 1000) == 0):
device.Print()
- t += DT
+ t += params.dt_wbc
# FINAL SHUTDOWN *************************************************
@@ -319,15 +289,15 @@ def control_loop(name_interface, name_interface_clone=None):
plt.show(block=True)
# Plot recorded data
- if PLOTTING:
+ if params.PLOTTING:
loggerControl.plotAll(loggerSensors)
# Save the logs of the Logger object
- if LOGGING:
+ if params.LOGGING:
loggerControl.saveAll(loggerSensors)
print("Log saved")
- if SIMULATION and enable_pyb_GUI:
+ if params.SIMULATION and enable_pyb_GUI:
# Disconnect the PyBullet server (also close the GUI)
device.Stop()
diff --git a/src/Params.cpp b/src/Params.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a39c95c28d108db1ecba7ddf4e4ee5c13159b1de
--- /dev/null
+++ b/src/Params.cpp
@@ -0,0 +1,89 @@
+#include "qrw/Params.hpp"
+
+using namespace yaml_control_interface;
+
+Params::Params()
+ : interface("")
+ , SIMULATION(false)
+ , LOGGING(false)
+ , PLOTTING(false)
+ , dt_wbc(0.0)
+ , N_gait(0)
+ , envID(0)
+ , velID(0)
+ , dt_mpc(0.0)
+ , T_gait(0.0)
+ , T_mpc(0.0)
+ , N_SIMULATION(0)
+ , type_MPC(false)
+ , use_flat_plane(false)
+ , predefined_vel(false)
+ , kf_enabled(false)
+ , enable_pyb_GUI(false)
+{
+ initialize(CONFIG_SOLO12_YAML);
+}
+
+void Params::initialize(const std::string& file_path)
+{
+ // Load YAML file
+ assert_file_exists(file_path);
+ YAML::Node param = YAML::LoadFile(file_path);
+
+ // Check if YAML node is detected and retrieve it
+ assert_yaml_parsing(param, file_path, "robot");
+ const YAML::Node& robot_node = param["robot"];
+
+ // Retrieve robot parameters
+ assert_yaml_parsing(robot_node, "robot", "interface");
+ interface = robot_node["interface"].as<std::string>();
+
+ assert_yaml_parsing(robot_node, "robot", "SIMULATION");
+ SIMULATION = robot_node["SIMULATION"].as<bool>();
+
+ assert_yaml_parsing(robot_node, "robot", "LOGGING");
+ LOGGING = robot_node["LOGGING"].as<bool>();
+
+ assert_yaml_parsing(robot_node, "robot", "PLOTTING");
+ PLOTTING = robot_node["PLOTTING"].as<bool>();
+
+ assert_yaml_parsing(robot_node, "robot", "dt_wbc");
+ dt_wbc = robot_node["dt_wbc"].as<double>();
+
+ assert_yaml_parsing(robot_node, "robot", "N_gait");
+ N_gait = robot_node["N_gait"].as<int>();
+
+ assert_yaml_parsing(robot_node, "robot", "envID");
+ envID = robot_node["envID"].as<int>();
+
+ assert_yaml_parsing(robot_node, "robot", "velID");
+ velID = robot_node["velID"].as<int>();
+
+ assert_yaml_parsing(robot_node, "robot", "dt_mpc");
+ dt_mpc = robot_node["dt_mpc"].as<double>();
+
+ assert_yaml_parsing(robot_node, "robot", "T_gait");
+ T_gait = robot_node["T_gait"].as<double>();
+
+ assert_yaml_parsing(robot_node, "robot", "T_mpc");
+ T_mpc = robot_node["T_mpc"].as<double>();
+
+ assert_yaml_parsing(robot_node, "robot", "N_SIMULATION");
+ N_SIMULATION = robot_node["N_SIMULATION"].as<int>();
+
+ assert_yaml_parsing(robot_node, "robot", "type_MPC");
+ type_MPC = robot_node["type_MPC"].as<bool>();
+
+ assert_yaml_parsing(robot_node, "robot", "use_flat_plane");
+ use_flat_plane = robot_node["use_flat_plane"].as<bool>();
+
+ assert_yaml_parsing(robot_node, "robot", "predefined_vel");
+ predefined_vel = robot_node["predefined_vel"].as<bool>();
+
+ assert_yaml_parsing(robot_node, "robot", "kf_enabled");
+ kf_enabled = robot_node["kf_enabled"].as<bool>();
+
+ assert_yaml_parsing(robot_node, "robot", "enable_pyb_GUI");
+ enable_pyb_GUI = robot_node["enable_pyb_GUI"].as<bool>();
+
+}
diff --git a/src/config_solo12.yaml b/src/config_solo12.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..b01c79042ef2f70ad78299e231b1b245edcfaaf9
--- /dev/null
+++ b/src/config_solo12.yaml
@@ -0,0 +1,19 @@
+robot:
+ interface: enp2s0
+ SIMULATION: true # Enable/disable PyBullet simulation or running on real robot
+ LOGGING: false # Enable/disable logging during the experiment
+ PLOTTING: true # Enable/disable automatic plotting at the end of the experiment
+ dt_wbc: 0.002 # Time step of the whole body control
+ N_gait: 100 # Number of rows in the gait matrix. Arbitrary value that should be set high enough
+ # so that there is always at least one empty line at the end of the gait matrix
+ envID: 0 # Identifier of the environment to choose in which one the simulation will happen
+ velID: 2 # Identifier of the reference velocity profile to choose which one will be sent to the robot
+ dt_mpc: 0.02 # Time step of the model predictive control
+ T_gait: 0.32 # Duration of one gait period
+ T_mpc: 0.32 # Duration of the prediction horizon
+ N_SIMULATION: 3000 # Number of simulated wbc time steps
+ type_MPC: true # Which MPC solver you want to use: True to have PA's MPC, to False to have Thomas's MPC
+ use_flat_plane: true # If True the ground is flat, otherwise it has bumps
+ predefined_vel: true # If we are using a predefined reference velocity (True) or a joystick (False)
+ kf_enabled: false # Use complementary filter (False) or kalman filter (True) for the estimator
+ enable_pyb_GUI: true # Enable/disable PyBullet GUI