Stable BaseEstimatorFromFootIMU.

CHANGELOG.md

@@ -6,6 +6,7 @@ All notable changes to this project are documented in this file.
 - Implement `ContactList::getNextContact` and `ContactList::getActiveContact` in `Contact` component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/764)
 - Implement `MANN::generateDummyMANNOutput` and `MANN::generateDummyMANNInput` in `ML` component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/771)
 - Add `MANNAutoregressive` and `MANNTrajectoryGenerator` examples (https://github.com/ami-iit/bipedal-locomotion-framework/pull/771)
+- Add `System::TimeProfiler` class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/826)
 
 ### Changed
 - Restructure of the `CentroidalMPC` class in `ReducedModelControllers` component. Specifically, the `CentroidalMPC` now provides a contact phase list instead of indicating the next active contact. Additionally, users can now switch between `ipopt` and `sqpmethod` to solve the optimization problem. Furthermore, the update allows for setting the warm-start for the non-linear solver. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/766)

bindings/python/YarpUtilities/src/VectorsCollection.cpp

@@ -7,31 +7,44 @@
 
 #include <pybind11/pybind11.h>
 #include <pybind11/stl.h>
+#include <pybind11/eigen.h>
 
-#include <BipedalLocomotion/YarpUtilities/VectorsCollection.h>
+#include <BipedalLocomotion/YarpUtilities/VectorsCollectionServer.h>
 
-#include <BipedalLocomotion/bindings/YarpUtilities/VectorsCollection.h>
 #include <BipedalLocomotion/bindings/YarpUtilities/BufferedPort.h>
+#include <BipedalLocomotion/bindings/YarpUtilities/VectorsCollection.h>
 
 namespace BipedalLocomotion
 {
 namespace bindings
 {
 namespace YarpUtilities
 {
-void CreateVectorsCollection(pybind11::module& module)
+void CreateVectorsCollectionServer(pybind11::module& module)
 {
     namespace py = ::pybind11;
 
     using namespace ::BipedalLocomotion::YarpUtilities;
 
-    py::class_<VectorsCollection>(module, "VectorsCollection")
+    py::class_<VectorsCollectionServer>(module, "VectorsCollectionServer")
         .def(py::init())
-        .def_readwrite("vectors", &VectorsCollection::vectors)
-        .def("__repr__", &VectorsCollection::toString)
-        .def("to_string", &VectorsCollection::toString);
-
-    CreateBufferedPort<VectorsCollection>(module, "BufferedPortVectorsCollection");
+        .def(
+            "initialize",
+            [](VectorsCollectionServer& impl,
+               std::shared_ptr<const ::BipedalLocomotion::ParametersHandler::IParametersHandler>
+                   handler) -> bool { return impl.initialize(handler); },
+            py::arg("handler"))
+        .def("populate_metadata", &VectorsCollectionServer::populateMetadata)
+        .def("finalize_metadata", &VectorsCollectionServer::finalizeMetadata)
+        .def("clear_data", &VectorsCollectionServer::clearData)
+        .def("send_data", &VectorsCollectionServer::sendData, py::arg("force_strict") = false)
+        .def("populate_data",
+             [](VectorsCollectionServer& impl,
+                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

devices/YarpRobotLoggerDevice/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_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_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>

devices/YarpRobotLoggerDevice/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>

devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/blf-yarp-robot-logger-interfaces/mas-remapper.xml

@@ -0,0 +1,36 @@
+<!-- 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="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="ThreeAxisGyroscopesNames">
+    (l_foot_front_ft_sensor_imu, l_foot_rear_ft_sensor_imu, r_foot_front_ft_sensor_imu, r_foot_rear_ft_sensor_imu)
+  </param>
+  <param name="ThreeAxisLinearAccelerometersNames">
+    (l_foot_front_ft_sensor_imu, l_foot_rear_ft_sensor_imu, r_foot_front_ft_sensor_imu, r_foot_rear_ft_sensor_imu)
+  </param>
+  <param name="ThreeAxisMagnetometersNames">
+    (l_foot_front_ft_sensor_imu, l_foot_rear_ft_sensor_imu, r_foot_front_ft_sensor_imu, r_foot_rear_ft_sensor_imu)
+  </param>
+  <param name="OrientationSensorsNames">
+    (l_foot_front_ft_sensor_imu, l_foot_rear_ft_sensor_imu, r_foot_front_ft_sensor_imu, r_foot_rear_ft_sensor_imu)
+  </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>

devices/YarpRobotLoggerDevice/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>

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>

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

@@ -0,0 +1,104 @@
+<!-- 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="yarp-robot-logger" type="YarpRobotLoggerDevice">
+  <param name="robot">ergocubSim</param>
+  <param name="sampling_period_in_s">0.002</param>
+  <param name="rgb_cameras_fps">(30)</param>
+  <param name="video_codec_code">mp4v</param>
+  <param name="port_prefix">/yarp-robot-logger</param>
+  <param name="text_logging_subnames">("ergoCubGazeboV1_1/yarprobotinterface")</param>
+
+  <group name="Telemetry">
+    <param name="save_period">600.0</param>
+  </group>
+
+  <group name="ExogenousSignals">
+    <param name="vectors_collection_exogenous_inputs">("walking", "fee")</param>
+    <param name="vectors_exogenous_inputs">()</param>
+
+    <!--group name="Balancing">
+      <param name="local">"/yarp-robot-logger/exogenous_signals/balancing:i"</param>
+      <param name="remote">"/balancing_controller/logger/data:o"</param>
+      <param name="signal_name">"balancing"</param>
+      <param name="carrier">"udp"</param>
+    </group-->
+
+    <group name="walking">
+      <param name="local">"/yarp-robot-logger/exogenous_signals/walking"</param>
+      <param name="remote">"/walking-coordinator/logger"</param>
+      <param name="signal_name">"walking"</param>
+      <param name="carrier">"udp"</param>
+    </group>
+
+    <group name="fee">
+      <param name="local">"/yarp-robot-logger/exogenous_signals/fee"</param>
+      <param name="remote">"/fee-estimator/data"</param>
+      <param name="signal_name">"fee"</param>
+      <param name="carrier">"udp"</param>
+    </group>
+
+    <!--<group name="RobotDynamicsEstimator">
+      <param name="local">"/yarp-robot-logger/exogenous_signals/rde:i"</param>
+      <param name="remote">"/robot-dynamics-estimator/data:o"</param>
+      <param name="signal_name">"rde"</param>
+      <param name="carrier">"udp"</param>
+    </group>-->
+
+    <!--<group name="FixedBaseTSID">
+      <param name="local">"/yarp-robot-logger/exogenous_signals/FixedBaseTSID:i"</param>
+      <param name="remote">"/FixedBaseTSID"</param>
+      <param name="signal_name">"FixedBaseTSID"</param>
+      <param name="carrier">"udp"</param>
+    </group>-->
+  </group>
+
+  <group name="RobotCameraBridge">
+      <param name="stream_cameras">false</param>
+      <group name="Cameras">
+        <param name="rgb_cameras_list">()</param>
+      </group>
+  </group>
+
+  <group name="RobotSensorBridge">
+    <param name="check_for_nan">false</param>
+    <param name="stream_joint_states">true</param>
+    <param name="stream_inertials">true</param>
+    <param name="stream_cartesian_wrenches">false</param>
+    <param name="stream_forcetorque_sensors">true</param>
+    <param name="stream_pids">false</param>
+    <param name="stream_motor_PWM">false</param>
+    <param name="stream_temperatures">false</param>
+
+    <group name="RemoteControlBoardRemapper">
+      <param name="joints_list">("neck_pitch", "neck_roll", "neck_yaw", "camera_tilt", "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>
+    </group>
+
+    <group name="SixAxisForceTorqueSensors">
+      <param name="sixaxis_forcetorque_sensors_list">("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>
+    </group>
+
+    <group name="InertialSensors">
+      <param name="accelerometers_list">(l_foot_front_ft_sensor_imu, l_foot_rear_ft_sensor_imu, r_foot_front_ft_sensor_imu, r_foot_rear_ft_sensor_imu)</param>
+      <param name="gyroscopes_list">(l_foot_front_ft_sensor_imu, l_foot_rear_ft_sensor_imu, r_foot_front_ft_sensor_imu, r_foot_rear_ft_sensor_imu)</param>
+      <param name="magnetometers_list">(l_foot_front_ft_sensor_imu, l_foot_rear_ft_sensor_imu, r_foot_front_ft_sensor_imu, r_foot_rear_ft_sensor_imu)</param>
+      <param name="orientation_sensors_list">(l_foot_front_ft_sensor_imu, l_foot_rear_ft_sensor_imu, r_foot_front_ft_sensor_imu, r_foot_rear_ft_sensor_imu)</param>
+    </group>
+
+  </group>
+
+  <!-- ATTACH -->
+  <action phase="startup" level="15" type="attach">
+    <paramlist name="networks">
+      <elem name="all_joints">all_joints_mc</elem>
+      <elem name="mas-remapper">mas-remapper</elem>
+
+    </paramlist>
+  </action>
+
+  <action phase="shutdown" level="2" type="detach" />
+  <!-- FINISH ATTACH-->
+
+</device>