Restructuring ReducedModelControllers

.gitignore

@@ -1,6 +1,6 @@
 # build folders
 build*
-
+.vscode*
 # emacs
 *~
 \#*\#

CHANGELOG.md

@@ -3,6 +3,7 @@ All notable changes to this project are documented in this file.
 
 ## [unreleased]
 ### Added
+- Added the ``BaseCentroidalMPC`` and ``StableCentroidalMPC`` to the ReducedModelControllers component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/703)
 - 🤖 Add the configuration files to use `YarpRobotLogger` with  `ergoCubGazeboV1` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/690)
 - Implement `RK4` integrator in `ContinuousDynamicalSystem` component and expose the python bindings (https://github.com/ami-iit/bipedal-locomotion-framework/pull/711)
 - Implement `blf-balancing-torque-control` in `utilities` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/707)

devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1/interface/all_joints_mc.xml

@@ -0,0 +1,13 @@
+<!-- Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+This software may be modified and distributed under the terms of the
+BSD-3-Clause license. -->
+<?xml version="1.0" encoding="UTF-8" ?>
+<device xmlns:xi="http://www.w3.org/2001/XInclude" name="all_joints_mc" type="remotecontrolboardremapper">
+    <param name="remoteControlBoards">("/ergocubSim/head", "/ergocubSim/torso", "/ergocubSim/left_arm", "/ergocubSim/right_arm", "/ergocubSim/left_leg", "/ergocubSim/right_leg")</param>
+    <param name="axesNames">("neck_pitch", "neck_roll", "neck_yaw", "camera_tilt", "torso_pitch", "torso_roll", "torso_yaw", "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", "l_wrist_roll", "l_wrist_pitch",  "l_wrist_yaw",  "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", "r_wrist_roll", "r_wrist_pitch",  "r_wrist_yaw", "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll", "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll")</param>
+    <param name="localPortPrefix">/yarp_robot_logger/joints</param>
+
+    <group name="REMOTE_CONTROLBOARD_OPTIONS">
+      <param name="carrier">udp</param>
+    </group>
+</device>

devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1/interface/ft_clients.xml

@@ -0,0 +1,35 @@
+<!-- Copyright (C) 2022 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+     This software may be modified and distributed under the terms of the
+     BSD-3-Clause license. -->
+<?xml version="1.0" encoding="UTF-8" ?>
+
+<devices>
+  <device xmlns:xi="http://www.w3.org/2001/XInclude" name="left_arm_ft_client" type="multipleanalogsensorsclient">
+    <param name="remote">/ergocubSim/left_arm</param>
+    <param name="local">/yarp_robot_logger/left_arm</param>
+    <param name="timeout">0.5</param>
+    <param name="carrier">fast_tcp</param>
+  </device>
+
+  <device xmlns:xi="http://www.w3.org/2001/XInclude" name="right_arm_ft_client" type="multipleanalogsensorsclient">
+    <param name="remote">/ergocubSim/right_arm</param>
+    <param name="local">/yarp_robot_logger/right_arm</param>
+    <param name="timeout">0.5</param>
+    <param name="carrier">fast_tcp</param>
+  </device>
+
+  <device xmlns:xi="http://www.w3.org/2001/XInclude" name="left_foot_ft_client" type="multipleanalogsensorsclient">
+    <param name="remote">/ergocubSim/left_foot_heel_tiptoe</param>
+    <param name="local">/yarp_robot_logger/left_foot_heel_tiptoe</param>
+    <param name="timeout">0.5</param>
+    <param name="carrier">fast_tcp</param>
+  </device>
+
+  <device xmlns:xi="http://www.w3.org/2001/XInclude" name="right_foot_ft_client" type="multipleanalogsensorsclient">
+    <param name="remote">/ergocubSim/right_foot_heel_tiptoe</param>
+    <param name="local">/yarp_robot_logger/right_foot_heel_tiptoe</param>
+    <param name="timeout">0.5</param>
+    <param name="carrier">fast_tcp</param>
+  </device>
+
+</devices>

devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1/interface/imu_clients.xml

@@ -0,0 +1,45 @@
+<!-- Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+     This software may be modified and distributed under the terms of the
+     GNU Lesser General Public License v2.1 or any later version. -->
+<?xml version="1.0" encoding="UTF-8" ?>
+
+<devices>
+
+  <device xmlns:xi="http://www.w3.org/2001/XInclude" name="head_imu_acc" type="multipleanalogsensorsclient">
+    <param name="remote">/ergocubSim/head/inertials</param>
+    <param name="local">/yarp_robot_logger/head/inertials</param>
+    <param name="timeout">0.5</param>
+    <param name="carrier">fast_tcp</param>
+  </device>
+
+  <device xmlns:xi="http://www.w3.org/2001/XInclude" name="left_arm_imu_client" type="multipleanalogsensorsclient">
+    <param name="remote">/ergocubSim/left_arm/imu</param>
+    <param name="local">/yarp_robot_logger/left_arm/imu</param>
+    <param name="timeout">0.5</param>
+    <param name="carrier">fast_tcp</param>
+  </device>
+
+  <device xmlns:xi="http://www.w3.org/2001/XInclude" name="right_arm_imu_client" type="multipleanalogsensorsclient">
+    <param name="remote">/ergocubSim/right_arm/imu</param>
+    <param name="local">/yarp_robot_logger/right_arm/imu</param>
+    <param name="timeout">0.5</param>
+    <param name="carrier">fast_tcp</param>
+  </device>
+
+  <device xmlns:xi="http://www.w3.org/2001/XInclude" name="left_foot_imu_client" type="multipleanalogsensorsclient">
+    <param name="remote">/ergocubSim/left_foot/imu</param>
+    <param name="local">/yarp_robot_logger/left_foot/imu</param>
+    <param name="timeout">0.5</param>
+    <param name="carrier">fast_tcp</param>
+  </device>
+
+  <device xmlns:xi="http://www.w3.org/2001/XInclude" name="right_foot_imu_client" type="multipleanalogsensorsclient">
+    <param name="remote">/ergocubSim/right_foot/imu</param>
+    <param name="local">/yarp_robot_logger/right_foot/imu</param>
+    <param name="timeout">0.5</param>
+    <param name="carrier">fast_tcp</param>
+  </device>
+
+</devices>
+
+

devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1/interface/mas-remapper.xml

@@ -0,0 +1,42 @@
+<!-- Copyright (C) 2019-2021 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+     This software may be modified and distributed under the terms of the
+     BSD-3-Clause license. -->
+
+<?xml version="1.0" encoding="UTF-8" ?>
+<device  xmlns:xi="http://www.w3.org/2001/XInclude" name="mas-remapper" type="multipleanalogsensorsremapper">
+  <param name="period">10</param>
+  <param name="ThreeAxisGyroscopesNames">
+    (rfeimu_gyro, l_arm_ft_gyro, r_arm_ft_gyro, l_foot_front_ft_gyro, l_foot_rear_ft_gyro, r_foot_front_ft_gyro, r_foot_rear_ft_gyro)
+  </param>
+  <param name="ThreeAxisLinearAccelerometersNames">
+    (rfeimu_acc, l_arm_ft_acc, r_arm_ft_acc, l_foot_front_ft_acc, l_foot_rear_ft_acc, r_foot_front_ft_acc, r_foot_rear_ft_acc)
+  </param>
+  <param name="ThreeAxisMagnetometersNames">
+    (rfeimu_mag, l_arm_ft_mag, r_arm_ft_mag, l_foot_front_ft_mag, l_foot_rear_ft_mag, r_foot_front_ft_mag, r_foot_rear_ft_mag)
+  </param>
+  <param name="OrientationSensorsNames">
+    (rfeimu_eul, l_arm_ft_eul, r_arm_ft_eul, l_foot_front_ft_eul, l_foot_rear_ft_eul, r_foot_front_ft_eul, r_foot_rear_ft_eul)
+  </param>
+  <param name="SixAxisForceTorqueSensorsNames">
+    (l_arm_ft_sensor, r_arm_ft_sensor, l_foot_front_ft_sensor, l_foot_rear_ft_sensor, r_foot_front_ft_sensor, r_foot_rear_ft_sensor)
+  </param>
+  <param name="TemperatureSensorsNames">
+    (l_arm_ft_sensor, r_arm_ft_sensor, l_foot_front_ft_sensor, l_foot_rear_ft_sensor, r_foot_front_ft_sensor, r_foot_rear_ft_sensor)
+  </param>
+
+  <action phase="startup" level="5" type="attach">
+    <paramlist name="networks">
+      <elem name="left_arm_ft_client">left_arm_ft_client</elem>
+      <elem name="right_arm_ft_client">right_arm_ft_client</elem>
+      <elem name="left_foot_ft_client">left_foot_ft_client</elem>
+      <elem name="right_foot_ft_client">right_foot_ft_client</elem>
+      <elem name="head_imu_acc">head_imu_acc</elem>
+      <elem name="left_arm_imu_client">left_arm_imu_client</elem>
+      <elem name="right_arm_imu_client">right_arm_imu_client</elem>
+      <elem name="left_foot_imu_client">left_foot_imu_client</elem>
+      <elem name="right_foot_imu_client">right_foot_imu_client</elem>
+    </paramlist>
+  </action>
+
+  <action phase="shutdown" level="5" type="detach" />
+</device>

devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1/interface/webcams.xml

@@ -0,0 +1,28 @@
+<!-- Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+This software may be modified and distributed under the terms of the
+BSD-3-Clause license. -->
+<?xml version="1.0" encoding="UTF-8" ?>
+
+<devices>
+
+  <!-- <device xmlns:xi="http://www.w3.org/2001/XInclude" name="jabra" type="opencv_grabber"> -->
+  <!--   <param name="camera">0</param> -->
+  <!--   <param name="width">1280</param> -->
+  <!--   <param name="height">720</param> -->
+  <!-- </device> -->
+
+  <!-- Enable the following block if you want to collect retrieve the cameras of the robot -->
+
+  <!-- <device xmlns:xi="http://www.w3.org/2001/XInclude" name="left_eye" type="remote_grabber"> -->
+  <!--   <param name="local">/yarp_robot_logger/cam/left</param> -->
+  <!--   <param name="remote">/icub/cam/left</param> -->
+  <!--   <param name="stream">mjpeg</param> -->
+  <!-- </device> -->
+
+  <!-- <device xmlns:xi="http://www.w3.org/2001/XInclude" name="right_eye" type="remote_grabber"> -->
+  <!--   <param name="local">/yarp_robot_logger/cam/right</param> -->
+  <!--   <param name="remote">/icub/cam/right</param> -->
+  <!--   <param name="stream">mjpeg</param> -->
+  <!-- </device> -->
+
+</devices>

devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1/interface/wrench_clients.xml

@@ -0,0 +1,44 @@
+<!-- Copyright (C) 2022 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+     This software may be modified and distributed under the terms of the
+     BSD-3-Clause license. -->
+<?xml version="1.0" encoding="UTF-8" ?>
+
+<devices>
+
+  <device xmlns:xi="http://www.w3.org/2001/XInclude" name="left_arm_wrench_client" type="genericSensorClient">
+    <param name="remote">/wholeBodyDynamics/left_arm/cartesianEndEffectorWrench:o</param>
+    <param name="local">/yarp_robot_logger/left_arm_wrench</param>
+    <param name="carrier">udp</param>
+  </device>
+
+  <device xmlns:xi="http://www.w3.org/2001/XInclude" name="right_arm_wrench_client" type="genericSensorClient">
+    <param name="remote">/wholeBodyDynamics/right_arm/cartesianEndEffectorWrench:o</param>
+    <param name="local">/yarp_robot_logger/right_arm_wrench</param>
+    <param name="carrier">udp</param>
+  </device>
+
+  <device xmlns:xi="http://www.w3.org/2001/XInclude" name="left_front_wrench_client" type="genericSensorClient">
+    <param name="remote">/wholeBodyDynamics/left_foot_front/cartesianEndEffectorWrench:o</param>
+    <param name="local">/yarp_robot_logger/left_front_wrench</param>
+    <param name="carrier">udp</param>
+  </device>
+
+  <device xmlns:xi="http://www.w3.org/2001/XInclude" name="left_rear_wrench_client" type="genericSensorClient">
+    <param name="remote">/wholeBodyDynamics/left_foot_rear/cartesianEndEffectorWrench:o</param>
+    <param name="local">/yarp_robot_logger/left_rear_wrench</param>
+    <param name="carrier">udp</param>
+  </device>
+
+  <device xmlns:xi="http://www.w3.org/2001/XInclude" name="right_front_wrench_client" type="genericSensorClient">
+    <param name="remote">/wholeBodyDynamics/right_foot_front/cartesianEndEffectorWrench:o</param>
+    <param name="local">/yarp_robot_logger/right_front_wrench</param>
+    <param name="carrier">udp</param>
+  </device>
+
+  <device xmlns:xi="http://www.w3.org/2001/XInclude" name="right_rear_wrench_client" type="genericSensorClient">
+    <param name="remote">/wholeBodyDynamics/right_foot_rear/cartesianEndEffectorWrench:o</param>
+    <param name="local">/yarp_robot_logger/right_rear_wrench</param>
+    <param name="carrier">udp</param>
+  </device>
+
+</devices>

devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1/yarp-robot-logger.xml

@@ -27,10 +27,11 @@ BSD-3-Clause license. -->
     </group-->
 
     <!--<group name="Walking">
-      <param name="local">"/yarp-robot-logger/exogenous_signals/walking:i"</param>
-      <param name="remote">"/walking-coordinator/logger/data:o"</param>
-      <param name="signal_name">"walking"</param>
-      <param name="carrier">"udp"</param>
+    <param name="vectors_collection_exogenous_inputs">("Walking")</param>
+    <param name="vectors_exogenous_inputs">()</param>
+
+    <group name="Walking">
+
     </group>-->
 
     <!--<group name="RobotDynamicsEstimator">
@@ -46,6 +47,7 @@ BSD-3-Clause license. -->
       <param name="signal_name">"FixedBaseTSID"</param>
       <param name="carrier">"udp"</param>
     </group>-->
+    </group>
   </group>
 
   <group name="RobotCameraBridge">
@@ -80,7 +82,6 @@ BSD-3-Clause license. -->
     <paramlist name="networks">
       <elem name="all_joints">all_joints_mc</elem>
       <elem name="mas-remapper">mas-remapper</elem>
-
     </paramlist>
   </action>
 

devices/YarpRobotLoggerDevice/app/robots/ergoCubSN000/yarp-robot-logger.xml

@@ -106,4 +106,4 @@ BSD-3-Clause license. -->
   <action phase="shutdown" level="2" type="detach" />
   <!-- FINISH ATTACH-->
 
-</device>
+</device>

devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h

@@ -183,4 +183,4 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver,
 
 } // namespace BipedalLocomotion
 
-#endif // BIPEDAL_LOCOMOTION_FRAMEWORK_YARP_ROBOT_LOGGER_DEVICE_H
+#endif // BIPEDAL_LOCOMOTION_FRAMEWORK_YARP_ROBOT_LOGGER_DEVICE_H

devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp

@@ -1652,4 +1652,4 @@ bool YarpRobotLoggerDevice::close()
     }
 
     return true;
-}
+}

src/ReducedModelControllers/CMakeLists.txt

@@ -8,10 +8,12 @@ if(FRAMEWORK_COMPILE_ReducedModelControllers)
 
   add_bipedal_locomotion_library(
     NAME                   ReducedModelControllers
-    PUBLIC_HEADERS         ${H_PREFIX}/CentroidalMPC.h
-    SOURCES                src/CentroidalMPC.cpp
+    PUBLIC_HEADERS         ${H_PREFIX}/BaseCentroidalMPC.h ${H_PREFIX}/CentroidalMPC.h ${H_PREFIX}/StableCentroidalMPC.h
+    SOURCES                src/CentroidalMPC.cpp src/StableCentroidalMPC.cpp
     PUBLIC_LINK_LIBRARIES  Eigen3::Eigen BipedalLocomotion::ParametersHandler BipedalLocomotion::System BipedalLocomotion::Contacts
     PRIVATE_LINK_LIBRARIES casadi BipedalLocomotion::Math BipedalLocomotion::TextLogging BipedalLocomotion::CasadiConversions
     SUBDIRECTORIES         tests)
 
 endif()
+
+