Merge branch 'ami-iit:master' into feature/StableCentroidalMPC

ami-iit · Feb 26, 2024 · ddffe34 · ddffe34
2 parents 2fadb08 + eb4b82d
commit ddffe34
Show file tree

Hide file tree

Showing 73 changed files with 1,583 additions and 659 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -1,6 +1,28 @@
 # Changelog
 All notable changes to this project are documented in this file.
 
+## [unreleased]
+### Added
+- Set submodel states from IMUs in RDE and add friction torques as measurement (https://github.com/ami-iit/bipedal-locomotion-framework/pull/793)
+- Add streaming of arm fts in YarpRobotLoggerDevice (https://github.com/ami-iit/bipedal-locomotion-framework/pull/803)
+- 🤖  [ `ergoCubGazeboV1_1`] Add configuration files to log data with the `YarpRobotLoggerDevice` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/806, https://github.com/ami-iit/bipedal-locomotion-framework/pull/808)
+
+### Changed
+- 🤖 [ergoCubSN001] Add logging of the wrist and fix the name of the waist imu (https://github.com/ami-iit/bipedal-locomotion-framework/pull/810)
+
+
+### Fixed
+- Fix the barrier logic for threads synchronization (https://github.com/ami-iit/bipedal-locomotion-framework/pull/811)
+
+### Removed
+
+## [0.18.0] - 2024-01-23
+### Changed
+- 🤖 [`ergoCubSN001`] Update `YarpRobotLoggerDevice` configuration file to exclude the head-imu and include the arms FTs  (https://github.com/ami-iit/bipedal-locomotion-framework/pull/798)
+- Avoid to call `BufferedPort::prepare` every time `VectorsCollectionServer::populateData` is called (https://github.com/ami-iit/bipedal-locomotion-framework/pull/790)
+- Check that the size the gains vectors in the `IK::JointTrackingTask`, `IK::JointLimitsTask` and `TSID::JointLimitsTask` are correct (https://github.com/ami-iit/bipedal-locomotion-framework/pull/797)
+- Check that the size of the weights in `MultiStateWeightProvider` are all the same (https://github.com/ami-iit/bipedal-locomotion-framework/pull/797)
+
 ## [0.17.0] - 2023-12-23
 ### Added
 - Implement `ContactList::getNextContact` and `ContactList::getActiveContact` in `Contact` component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/764)
@@ -517,7 +539,9 @@ All notable changes to this project are documented in this file.
 - Added `mas-imu-test` application to check the output of MAS IMUs (https://github.com/ami-iit/bipedal-locomotion-framework/pull/62)
 - Implement motor currents reading in `YarpSensorBridge`. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/187)
 
-[unreleased]: https://github.com/ami-iit/bipedal-locomotion-framework/compare/v0.16.1...master
+[unreleased]: https://github.com/ami-iit/bipedal-locomotion-framework/compare/v0.18.0...master
+[0.18.0]: https://github.com/ami-iit/bipedal-locomotion-framework/compare/v0.17.0...v0.18.0
+[0.17.0]: https://github.com/ami-iit/bipedal-locomotion-framework/compare/v0.16.1...v0.17.0
 [0.16.1]: https://github.com/ami-iit/bipedal-locomotion-framework/compare/v0.16.0...v0.16.1
 [0.16.0]: https://github.com/ami-iit/bipedal-locomotion-framework/compare/v0.15.0...v0.16.0
 [0.15.0]: https://github.com/ami-iit/bipedal-locomotion-framework/compare/v0.14.1...v0.15.0

diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -6,7 +6,7 @@ cmake_minimum_required(VERSION 3.16.0)
 
 ## MAIN project
 project(BipedalLocomotionFramework
-  VERSION 0.17.0)
+  VERSION 0.18.100)
 
 list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake)
 

diff --git a/bindings/python/RobotDynamicsEstimator/src/RobotDynamicsEstimator.cpp b/bindings/python/RobotDynamicsEstimator/src/RobotDynamicsEstimator.cpp
@@ -49,7 +49,7 @@ void CreateRobotDynamicsEstimator(pybind11::module& module)
         .def_readwrite("ft_wrenches", &RobotDynamicsEstimatorInput::ftWrenches)
         .def_readwrite("linear_accelerations", &RobotDynamicsEstimatorInput::linearAccelerations)
         .def_readwrite("angular_velocities", &RobotDynamicsEstimatorInput::angularVelocities)
-        .def_readwrite("friction", &RobotDynamicsEstimatorInput::friction);
+        .def_readwrite("friction_torques", &RobotDynamicsEstimatorInput::frictionTorques);
 
     py::class_<RobotDynamicsEstimatorOutput>(module, "RobotDynamicsEstimatorOutput")
             .def(py::init())
@@ -58,7 +58,9 @@ void CreateRobotDynamicsEstimator(pybind11::module& module)
             .def_readwrite("tau_F", &RobotDynamicsEstimatorOutput::tau_F)
             .def_readwrite("ft_wrenches", &RobotDynamicsEstimatorOutput::ftWrenches)
             .def_readwrite("ft_wrenches_biases", &RobotDynamicsEstimatorOutput::ftWrenchesBiases)
+            .def_readwrite("linear_accelerations", &RobotDynamicsEstimatorOutput::linearAccelerations)
             .def_readwrite("accelerometer_biases", &RobotDynamicsEstimatorOutput::accelerometerBiases)
+            .def_readwrite("angular_velocities", &RobotDynamicsEstimatorOutput::angularVelocities)
             .def_readwrite("gyroscope_biases", &RobotDynamicsEstimatorOutput::gyroscopeBiases)
             .def_readwrite("contact_wrenches", &RobotDynamicsEstimatorOutput::contactWrenches)
             .def(py::pickle([](const RobotDynamicsEstimatorOutput &output) { //__getstate__
@@ -67,7 +69,9 @@ void CreateRobotDynamicsEstimator(pybind11::module& module)
                                       output.tau_F,
                                       output.ftWrenches,
                                       output.ftWrenchesBiases,
+                                      output.linearAccelerations,
                                       output.accelerometerBiases,
+                                      output.angularVelocities,
                                       output.gyroscopeBiases,
                                       output.contactWrenches);
                 },
@@ -80,9 +84,11 @@ void CreateRobotDynamicsEstimator(pybind11::module& module)
                       rde.tau_F = t[2].cast<Eigen::VectorXd>();
                       rde.ftWrenches = t[3].cast<std::map<std::string, Eigen::VectorXd>>();
                       rde.ftWrenchesBiases = t[4].cast<std::map<std::string, Eigen::VectorXd>>();
-                      rde.accelerometerBiases = t[5].cast<std::map<std::string, Eigen::VectorXd>>();
-                      rde.gyroscopeBiases = t[6].cast<std::map<std::string, Eigen::VectorXd>>();
-                      rde.contactWrenches = t[7].cast<std::map<std::string, Eigen::VectorXd>>();
+                      rde.linearAccelerations = t[5].cast<std::map<std::string, Eigen::VectorXd>>();
+                      rde.accelerometerBiases = t[6].cast<std::map<std::string, Eigen::VectorXd>>();
+                      rde.angularVelocities = t[7].cast<std::map<std::string, Eigen::VectorXd>>();
+                      rde.gyroscopeBiases = t[8].cast<std::map<std::string, Eigen::VectorXd>>();
+                      rde.contactWrenches = t[9].cast<std::map<std::string, Eigen::VectorXd>>();
 
                       return rde;
                  }

diff --git a/bindings/python/YarpUtilities/src/VectorsCollection.cpp b/bindings/python/YarpUtilities/src/VectorsCollection.cpp
@@ -43,7 +43,8 @@ void CreateVectorsCollectionServer(pybind11::module& module)
                 const std::string& key,
                 Eigen::Ref<const Eigen::VectorXd> data) -> bool {
                  return impl.populateData(key, data);
-             });
+             })
+        .def("prepare_data", &VectorsCollectionServer::prepareData);
 }
 } // namespace YarpUtilities
 } // namespace bindings

diff --git a/devices/YarpRobotLoggerDevice/README.md b/devices/YarpRobotLoggerDevice/README.md
@@ -80,6 +80,7 @@ m_vectorsCollectionServer.finalizeMetadata(); // this should be called only once
 In the main loop, add the following code to prepare and populate the data:
 
 ```c++
+m_vectorsCollectionServer.prepareData(); // required to prepare the data to be sent
 m_vectorsCollectionServer.clearData(); // optional see the documentation
 
 // DCM
@@ -118,6 +119,7 @@ self.vectors_collection_server.finalize_metadata() # this should be called only
 ```
 In the main loop, add the following code to prepare and populate the data:
 ```python
+self.vectors_collection_server.prepare_data() # required to prepare the data to be sent
 self.vectors_collection_server.clear_data() # optional see the documentation
 self.vectors_collection_server.populate_data("dcm::position::measured", <signal>)
 self.vectors_collection_server.populate_data("dcm::position::desired", <signal>)

diff --git a/...gerDevice/app/robots/ergoCubGazeboV1_1/blf-yarp-robot-logger-interfaces/all_joints_mc.xml b/...gerDevice/app/robots/ergoCubGazeboV1_1/blf-yarp-robot-logger-interfaces/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>
diff --git a/...LoggerDevice/app/robots/ergoCubGazeboV1_1/blf-yarp-robot-logger-interfaces/ft_clients.xml b/...LoggerDevice/app/robots/ergoCubGazeboV1_1/blf-yarp-robot-logger-interfaces/ft_clients.xml
@@ -0,0 +1,36 @@
+<!-- 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/FT</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/FT</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_leg_ft_client" type="multipleanalogsensorsclient">
+  <param name="remote">/ergocubSim/left_leg/FT</param>
+  <param name="local">/yarp_robot_logger/left_leg</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_leg_ft_client" type="multipleanalogsensorsclient">
+  <param name="remote">/ergocubSim/right_leg/FT</param>
+  <param name="local">/yarp_robot_logger/right_leg</param>
+  <param name="timeout">0.5</param>
+  <param name="carrier">fast_tcp</param>
+</device>
+
+</devices>
diff --git a/...oggerDevice/app/robots/ergoCubGazeboV1_1/blf-yarp-robot-logger-interfaces/imu_clients.xml b/...oggerDevice/app/robots/ergoCubGazeboV1_1/blf-yarp-robot-logger-interfaces/imu_clients.xml
@@ -0,0 +1,22 @@
+<!-- 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="left_foot_imu_client" type="multipleanalogsensorsclient">
+    <param name="remote">/ergocubSim/left_foot_heel_tiptoe/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_heel_tiptoe/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>
diff --git a/...ggerDevice/app/robots/ergoCubGazeboV1_1/blf-yarp-robot-logger-interfaces/mas-remapper.xml b/...ggerDevice/app/robots/ergoCubGazeboV1_1/blf-yarp-robot-logger-interfaces/mas-remapper.xml
@@ -0,0 +1,39 @@
+<!-- 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">
+    (l_foot_front_ft_imu, l_foot_rear_ft_imu, r_foot_front_ft_imu, r_foot_rear_ft_imu)
+  </param>
+  <param name="ThreeAxisLinearAccelerometersNames">
+    (l_foot_front_ft_imu, l_foot_rear_ft_imu, r_foot_front_ft_imu, r_foot_rear_ft_imu)
+  </param>
+  <param name="ThreeAxisMagnetometersNames">
+    (l_foot_front_ft_imu, l_foot_rear_ft_imu, r_foot_front_ft_imu, r_foot_rear_ft_imu)
+  </param>
+  <param name="OrientationSensorsNames">
+    (l_foot_front_ft_imu, l_foot_rear_ft_imu, r_foot_front_ft_imu, r_foot_rear_ft_imu)
+  </param>
+  <param name="SixAxisForceTorqueSensorsNames">
+    (l_arm_ft, r_arm_ft, l_leg_ft, l_foot_front_ft, l_foot_rear_ft, r_leg_ft, r_foot_front_ft, r_foot_rear_ft)
+  </param>
+  <param name="TemperatureSensorsNames">
+    (l_arm_ft, r_arm_ft, l_leg_ft, l_foot_front_ft, l_foot_rear_ft, r_leg_ft, r_foot_front_ft, r_foot_rear_ft)
+  </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_leg_ft_client">left_leg_ft_client</elem>
+      <elem name="right_leg_ft_client">right_leg_ft_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>
diff --git a/...erDevice/app/robots/ergoCubGazeboV1_1/blf-yarp-robot-logger-interfaces/wrench_clients.xml b/...erDevice/app/robots/ergoCubGazeboV1_1/blf-yarp-robot-logger-interfaces/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>
diff --git a/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/launch-yarp-robot-logger.xml b/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/launch-yarp-robot-logger.xml
@@ -0,0 +1,17 @@
+<!-- Copyright (C) 2019-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" ?>
+<!DOCTYPE robot PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
+
+<robot name="ergoCubGazeboV1_1" portprefix="YarpRobotLogger" build="1" xmlns:xi="http://www.w3.org/2001/XInclude">
+    <devices>
+        <!-- interface -->
+        <xi:include href="blf-yarp-robot-logger-interfaces/all_joints_mc.xml" />
+        <xi:include href="blf-yarp-robot-logger-interfaces/ft_clients.xml" />
+        <xi:include href="blf-yarp-robot-logger-interfaces/imu_clients.xml" />
+        <xi:include href="blf-yarp-robot-logger-interfaces/mas-remapper.xml" />
+        <xi:include href="./yarp-robot-logger.xml" />
+    </devices>
+</robot>