Implement RobotDynamicsEstimatorDevice - PR6

CHANGELOG.md

@@ -18,6 +18,7 @@ All notable changes to this project are documented in this file.
 - Finalize `RobotDynamicsEstimator` library and add complete library test (https://github.com/ami-iit/bipedal-locomotion-framework/pull/744)
 - Add Python bindings for `RobotDynamicsEstimator` library (https://github.com/ami-iit/bipedal-locomotion-framework/pull/755)
 - Add possibility to set the regularization on the mass matrix for the `TSID::JointDynamicsTask` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/722)
+- Implement `RobotDynamicsEstimatorDevice` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/756)
 
 ### Changed
 - Remove the possibility to disable the telemetry in `System::AdvanceableRunner` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/726)

devices/CMakeLists.txt

@@ -3,6 +3,7 @@
 # BSD-3-Clause license.
 
 add_subdirectory(FloatingBaseEstimatorDevice)
+add_subdirectory(RobotDynamicsEstimatorDevice)
 add_subdirectory(YarpRobotLoggerDevice)
 add_subdirectory(VectorsCollectionWrapper)
 

devices/RobotDynamicsEstimatorDevice/CMakeLists.txt

@@ -0,0 +1,19 @@
+# Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+# This software may be modified and distributed under the terms of the
+# BSD-3-Clause license.
+
+if(FRAMEWORK_COMPILE_YarpImplementation AND FRAMEWORK_COMPILE_RobotDynamicsEstimator AND FRAMEWORK_COMPILE_ManifConversions)
+ # Warning: the CONFIGURE_PACKAGE_NAME option should be different from the plugin NAME
+ add_bipedal_yarp_device(
+ NAME RobotDynamicsEstimatorDevice
+ TYPE BipedalLocomotion::RobotDynamicsEstimatorDevice
+ SOURCES src/RobotDynamicsEstimatorDevice.cpp
+ PUBLIC_HEADERS include/BipedalLocomotion/RobotDynamicsEstimatorDevice.h
+ PUBLIC_LINK_LIBRARIES ${YARP_LIBRARIES} ${iDynTree_LIBRARIES} BipedalLocomotion::YarpUtilities
+ BipedalLocomotion::RobotInterfaceYarpImplementation
+ BipedalLocomotion::ParametersHandlerYarpImplementation
+ BipedalLocomotion::RobotDynamicsEstimator
+ BipedalLocomotion::ManifConversions
+ BipedalLocomotion::VectorsCollection
+ CONFIGURE_PACKAGE_NAME robot_dynamics_estimator_device)
+endif()

devices/RobotDynamicsEstimatorDevice/README.md

@@ -0,0 +1,27 @@
+# RobotDynamicsEstimatorDevice
+
+The **RobotDynamicsEstimatorDevice** is a YARP device based on `RobotDynamicsEstimator` to estimate joint torques from a YARP-based robot.
+
+
+## :running: How to use the device
+
+To run the device on a robot (whether in simulation or on a real robot), make sure to define the configuration files for the robot in the `app/robots` folder.
+
+- **ergoCubGazeboV1**
+
+ - open the robot model in Gazebo
+ - launch `RobotDynamicsEstimatorDevice`
+ ```
+ YARP_ROBOT_NAME=ergoCubGazeboV1 yarprobotinterface --config launch-robot-dynamics-estimator.xml
+ ```
+ in case you started gazebo with the real-time clock option (`gazebo -slibgazebo_yarp_clock.so`) add `YARP_CLOCK=/clock` to the previous command.
+ ```
+ YARP_CLOCK=/clock YARP_ROBOT_NAME=ergoCubGazeboV1 yarprobotinterface --config launch-robot-dynamics-estimator.xml
+ ```
+- **ergoCubSN000**
+
+ - Launch `yarprobotinterface` on the robot.
+ - launch `RobotDynamicsEstimatorDevice`
+ ```
+ YARP_ROBOT_NAME=ergoCubSN000 yarprobotinterface --config launch-robot-dynamics-estimator.xml
+ ```

devices/RobotDynamicsEstimatorDevice/app/CMakeLists.txt

@@ -0,0 +1,10 @@
+# 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.
+
+# Get list of models
+subdirlist(subdirs ${CMAKE_CURRENT_SOURCE_DIR}/robots/)
+# Install each model
+foreach (dir ${subdirs})
+ yarp_install(DIRECTORY robots/${dir} DESTINATION ${YARP_ROBOTS_INSTALL_DIR})
+endforeach ()

devices/RobotDynamicsEstimatorDevice/app/robots/ergoCubGazeboV1/interface-robot-dynamics-estimator/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/right_leg")</param>
+ <param name="axesNames">("r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll")</param>
+ <param name="localPortPrefix">/robot_dynamics_estimator/joints</param>
+
+ <group name="REMOTE_CONTROLBOARD_OPTIONS">
+ <param name="carrier">udp</param>
+ </group>
+</device>

devices/RobotDynamicsEstimatorDevice/app/robots/ergoCubGazeboV1/interface-robot-dynamics-estimator/ft_clients.xml

@@ -0,0 +1,14 @@
+<!-- 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="right_leg_ft_client" type="multipleanalogsensorsclient">
+ <param name="remote">/ergocubSim/right_leg/FT</param>
+ <param name="local">/robot_dynamics_estimator/right_leg_hip</param>
+ <param name="timeout">0.5</param>
+ <param name="carrier">fast_tcp</param>
+ </device>
+
+</devices>

devices/RobotDynamicsEstimatorDevice/app/robots/ergoCubGazeboV1/interface-robot-dynamics-estimator/mas-remapper.xml

@@ -0,0 +1,20 @@
+<!-- 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">
+ (r_leg_ft, r_foot_front_ft, r_foot_rear_ft)
+ </param>
+
+ <action phase="startup" level="5" type="attach">
+ <paramlist name="networks">
+ <elem name="right_leg_ft_client">right_leg_ft_client</elem>
+ </paramlist>
+ </action>
+
+ <action phase="shutdown" level="5" type="detach" />
+</device>

devices/RobotDynamicsEstimatorDevice/app/robots/ergoCubGazeboV1/launch-robot-dynamics-estimator.xml

@@ -0,0 +1,15 @@
+<!-- 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" portprefix="RobotDynamicsEstimator" build="1" xmlns:xi="http://www.w3.org/2001/XInclude">
+ <devices>
+ <xi:include href="interface-robot-dynamics-estimator/all_joints_mc.xml" />
+ <xi:include href="interface-robot-dynamics-estimator/mas-remapper.xml" />
+ <xi:include href="interface-robot-dynamics-estimator/ft_clients.xml" />
+ <xi:include href="./robot-dynamics-estimator.xml" />
+ </devices>
+</robot>

devices/RobotDynamicsEstimatorDevice/app/robots/ergoCubGazeboV1/robot-dynamics-estimator.xml

@@ -0,0 +1,172 @@
+<!-- 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="robot-dynamics-estimator" type="RobotDynamicsEstimatorDevice">
+ <group name="GENERAL">
+ <param name="sampling_time">0.01</param>
+ <param name="port_prefix">/robot-dynamics-estimator</param>
+ </group>
+
+ <group name="MODEL">
+ <param name="model_file">model.urdf</param>
+ <param name="base_link">root_link</param>
+ <param name="joint_list">("r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll")</param>
+ <param name="gear_ratio">(100.0, -160.0, 100.0, 100.0, 100.0, 160.0)</param>
+ <param name="torque_constant">(0.111, 0.047, 0.047, 0.111, 0.111, 0.025)</param>
+
+ <group name="FT">
+ <param name="names">("r_leg_ft", "r_foot_front_ft", "r_foot_rear_ft")</param>
+ <param name="frames">("r_leg_ft", "r_foot_front_ft", "r_foot_rear_ft")</param>
+ <param name="associated_joints">("r_leg_ft_sensor", "r_foot_front_ft_sensor", "r_foot_rear_ft_sensor")</param>
+ </group>
+
+ <group name="ACCELEROMETER">
+ <param name="names">()</param>
+ <param name="frames">()</param>
+ </group>
+
+ <group name="GYROSCOPE">
+ <param name="names">()</param>
+ <param name="frames">()</param>
+ </group>
+
+ <group name="EXTERNAL_CONTACT">
+ <param name="frames">()</param>
+ </group>
+ </group>
+
+ <group name="RobotSensorBridge">
+ <param name="check_for_nan">false</param>
+ <param name="stream_joint_states">true</param>
+ <param name="stream_inertials">false</param>
+ <param name="stream_forcetorque_sensors">true</param>
+ <param name="stream_motor_states">true</param>
+
+ <group name="RemoteControlBoardRemapper">
+ <param name="joints_list">("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">("r_leg_ft", "r_foot_front_ft", "r_foot_rear_ft")</param>
+ </group>
+ </group>
+
+ <group name="UKF">
+ <param name="alpha">1.0</param>
+ <param name="beta">2.0</param>
+ <param name="kappa">0.0</param>
+ <group name="UKF_STATE">
+ <!--param name="dynamics_list">
+ ("JOINT_VELOCITY", "MOTOR_TORQUE", "FRICTION_TORQUE", "RIGHT_LEG_FT", "RIGHT_FOOT_FRONT_FT", "RIGHT_FOOT_REAR_FT",
+ "RIGHT_LEG_FT_BIAS", "RIGHT_FOOT_FRONT_FT_BIAS", "RIGHT_FOOT_REAR_FT_BIAS",
+ "RIGHT_LEG_ACC_BIAS", "RIGHT_FOOT_FRONT_ACC_BIAS", "RIGHT_FOOT_REAR_ACC_BIAS",
+ "RIGHT_LEG_GYRO_BIAS", "RIGHT_FOOT_FRONT_GYRO_BIAS", "RIGHT_FOOT_REAR_GYRO_BIAS")
+ </param-->
+ <param name="dynamics_list">
+ ("JOINT_VELOCITY", "MOTOR_TORQUE", "FRICTION_TORQUE", "RIGHT_LEG_FT", "RIGHT_FOOT_FRONT_FT", "RIGHT_FOOT_REAR_FT")
+ </param>
+ <!-- Available models = ["ZeroVelocityStateDynamics", "JointVelocityStateDynamics", "FrictionTorqueStateDynamics"] -->
+ <group name="JOINT_VELOCITY">
+ <param name="name">ds</param>
+ <param name="elements">("r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll")</param>
+ <param name="covariance">(1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3)</param>
+ <param name="initial_covariance">(0.01, 0.01, 0.01, 0.01, 0.01, 0.01)</param>
+ <param name="dynamic_model">JointVelocityStateDynamics</param>
+ </group>
+ <group name="MOTOR_TORQUE">
+ <param name="name">tau_m</param>
+ <param name="elements">("r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll")</param>
+ <param name="covariance">(1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3)</param>
+ <param name="initial_covariance">(0.01, 0.01, 0.01, 0.01, 0.01, 0.01)</param>
+ <param name="dynamic_model">ZeroVelocityStateDynamics</param>
+ </group>
+ <group name="FRICTION_TORQUE">
+ <param name="name">tau_F</param>
+ <param name="elements">("r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll")</param>
+ <param name="covariance">(1e-1, 1e-1, 1e-1, 1e-1, 1e-1, 1e-1)</param>
+ <param name="initial_covariance">(0.01, 0.01, 0.01, 0.01, 0.01, 0.01)</param>
+ <param name="friction_k0">(0.001, 0.001, 0.001, 0.001, 0.001, 0.001)</param>
+ <param name="friction_k1">(0.01, 0.01, 0.01, 0.01, 0.01, 0.01)</param>
+ <param name="friction_k2">(0.01, 0.01, 0.01, 0.01, 0.01, 0.01)</param>
+ <param name="dynamic_model">FrictionTorqueStateDynamics</param>
+ </group>
+ <group name="RIGHT_LEG_FT">
+ <param name="name">r_leg_ft</param>
+ <param name="covariance">(1e-3, 1e-3, 1e-3, 1e-4, 1e-4, 1e-4)</param>
+ <param name="initial_covariance">(0.01, 0.01, 0.01, 0.01, 0.01, 0.01)</param>
+ <param name="dynamic_model">ZeroVelocityStateDynamics</param>
+ </group>
+ <group name="RIGHT_FOOT_FRONT_FT">
+ <param name="name">r_foot_front_ft</param>
+ <param name="covariance">(1e-3, 1e-3, 1e-3, 1e-6, 1e-6, 1e-6)</param>
+ <param name="initial_covariance">(0.01, 0.01, 0.01, 0.01, 0.01, 0.01)</param>
+ <param name="dynamic_model">ZeroVelocityStateDynamics</param>
+ </group>
+ <group name="RIGHT_FOOT_REAR_FT">
+ <param name="name">r_foot_rear_ft</param>
+ <param name="covariance">(1e-3, 1e-3, 1e-3, 1e-6, 1e-6, 1e-6)</param>
+ <param name="initial_covariance">(0.01, 0.01, 0.01, 0.01, 0.01, 0.01)</param>
+ <param name="dynamic_model">ZeroVelocityStateDynamics</param>
+ </group>
+ </group>
+
+ <group name="UKF_MEASUREMENT">
+ <!--param name="dynamics_list">
+ ("JOINT_VELOCITY", "MOTOR_CURRENT",
+ "RIGHT_LEG_FT", "RIGHT_FOOT_FRONT_FT", "RIGHT_FOOT_REAR_FT",
+ "RIGHT_LEG_ACC", "RIGHT_FOOT_FRONT_ACC", "RIGHT_FOOT_REAR_ACC",
+ "RIGHT_LEG_GYRO", "RIGHT_FOOT_FRONT_GYRO", "RIGHT_FOOT_REAR_GYRO")
+ </param-->
+ <param name="dynamics_list">
+ ("JOINT_VELOCITY", "MOTOR_CURRENT",
+ "RIGHT_LEG_FT", "RIGHT_FOOT_FRONT_FT", "RIGHT_FOOT_REAR_FT")
+ </param>
+ <!-- Available models = ["ConstantMeasurementModel", "AccelerometerMeasurementDynamics",
+ "GyroscopeMeasurementDynamics", "MotorCurrentMeasurementDynamics"] -->
+ <group name="JOINT_VELOCITY">
+ <param name="name">ds</param>
+ <param name="covariance">(1e-1, 1e-1, 1e-1, 1e-1, 1e-1, 1e-1)</param>
+ <param name="dynamic_model">ConstantMeasurementModel</param>
+ </group>
+ <group name="MOTOR_CURRENT">
+ <param name="name">i_m</param>
+ <param name="covariance">(1e-2, 1e-2, 1e-1, 1e-1, 1e-1, 1e-1)</param>
+ <param name="gear_ratio">(100.0, -160.0, 100.0, 100.0, 100.0, 160.0)</param>
+ <param name="torque_constant">(0.111, 0.047, 0.047, 0.111, 0.111, 0.025)</param>
+ <param name="dynamic_model">MotorCurrentMeasurementDynamics</param>
+ </group>
+ <group name="RIGHT_LEG_FT">
+ <param name="name">r_leg_ft</param>
+ <param name="covariance">(1e-1, 1e-1, 1e-1, 1e-1, 1e-1, 1e-1)</param>
+ <param name="use_bias">false</param>
+ <param name="dynamic_model">ConstantMeasurementModel</param>
+ </group>
+ <group name="RIGHT_FOOT_FRONT_FT">
+ <param name="name">r_foot_front_ft</param>
+ <param name="covariance">(1e-1, 1e-1, 1e-1, 1e-1, 1e-1, 1e-1)</param>
+ <param name="use_bias">false</param>
+ <param name="dynamic_model">ConstantMeasurementModel</param>
+ </group>
+ <group name="RIGHT_FOOT_REAR_FT">
+ <param name="name">r_foot_rear_ft</param>
+ <param name="covariance">(1e-1, 1e-1, 1e-1, 1e-1, 1e-1, 1e-1)</param>
+ <param name="use_bias">false</param>
+ <param name="dynamic_model">ConstantMeasurementModel</param>
+ </group>
+ </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>