Skip to content

Commit 552b9e5

Browse files
isorrentinoisorrentino
authored
Implement RobotDynamicsEstimatorDevice - PR6 (#756)
1 parent 735ac75 commit 552b9e5

File tree

12 files changed

+1385
-0
lines changed

12 files changed

+1385
-0
lines changed

‎CHANGELOG.md‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,7 @@ All notable changes to this project are documented in this file.
1818
- Finalize `RobotDynamicsEstimator` library and add complete library test (https://github.com/ami-iit/bipedal-locomotion-framework/pull/744)
1919
- Add Python bindings for `RobotDynamicsEstimator` library (https://github.com/ami-iit/bipedal-locomotion-framework/pull/755)
2020
- Add possibility to set the regularization on the mass matrix for the `TSID::JointDynamicsTask` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/722)
21+
- Implement `RobotDynamicsEstimatorDevice` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/756)
2122

2223
### Changed
2324
- Remove the possibility to disable the telemetry in `System::AdvanceableRunner` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/726)

‎devices/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -3,6 +3,7 @@
33
# BSD-3-Clause license.
44

55
add_subdirectory(FloatingBaseEstimatorDevice)
6+
add_subdirectory(RobotDynamicsEstimatorDevice)
67
add_subdirectory(YarpRobotLoggerDevice)
78
add_subdirectory(VectorsCollectionWrapper)
89

‎devices/RobotDynamicsEstimatorDevice/CMakeLists.txt‎

Lines changed: 19 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,19 @@
1+
# Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT). All rights reserved.
2+
# This software may be modified and distributed under the terms of the
3+
# BSD-3-Clause license.
4+
5+
if(FRAMEWORK_COMPILE_YarpImplementation AND FRAMEWORK_COMPILE_RobotDynamicsEstimator AND FRAMEWORK_COMPILE_ManifConversions)
6+
# Warning: the CONFIGURE_PACKAGE_NAME option should be different from the plugin NAME
7+
add_bipedal_yarp_device(
8+
NAME RobotDynamicsEstimatorDevice
9+
TYPE BipedalLocomotion::RobotDynamicsEstimatorDevice
10+
SOURCES src/RobotDynamicsEstimatorDevice.cpp
11+
PUBLIC_HEADERS include/BipedalLocomotion/RobotDynamicsEstimatorDevice.h
12+
PUBLIC_LINK_LIBRARIES ${YARP_LIBRARIES} ${iDynTree_LIBRARIES} BipedalLocomotion::YarpUtilities
13+
BipedalLocomotion::RobotInterfaceYarpImplementation
14+
BipedalLocomotion::ParametersHandlerYarpImplementation
15+
BipedalLocomotion::RobotDynamicsEstimator
16+
BipedalLocomotion::ManifConversions
17+
BipedalLocomotion::VectorsCollection
18+
CONFIGURE_PACKAGE_NAME robot_dynamics_estimator_device)
19+
endif()

‎devices/RobotDynamicsEstimatorDevice/README.md‎

Lines changed: 27 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,27 @@
1+
# RobotDynamicsEstimatorDevice
2+
3+
The **RobotDynamicsEstimatorDevice** is a YARP device based on `RobotDynamicsEstimator` to estimate joint torques from a YARP-based robot.
4+
5+
6+
## :running: How to use the device
7+
8+
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.
9+
10+
- **ergoCubGazeboV1**
11+
12+
- open the robot model in Gazebo
13+
- launch `RobotDynamicsEstimatorDevice`
14+
```
15+
YARP_ROBOT_NAME=ergoCubGazeboV1 yarprobotinterface --config launch-robot-dynamics-estimator.xml
16+
```
17+
in case you started gazebo with the real-time clock option (`gazebo -slibgazebo_yarp_clock.so`) add `YARP_CLOCK=/clock` to the previous command.
18+
```
19+
YARP_CLOCK=/clock YARP_ROBOT_NAME=ergoCubGazeboV1 yarprobotinterface --config launch-robot-dynamics-estimator.xml
20+
```
21+
- **ergoCubSN000**
22+
23+
- Launch `yarprobotinterface` on the robot.
24+
- launch `RobotDynamicsEstimatorDevice`
25+
```
26+
YARP_ROBOT_NAME=ergoCubSN000 yarprobotinterface --config launch-robot-dynamics-estimator.xml
27+
```

‎devices/RobotDynamicsEstimatorDevice/app/CMakeLists.txt‎

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,10 @@
1+
# Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved.
2+
# This software may be modified and distributed under the terms of the
3+
# BSD-3-Clause license.
4+
5+
# Get list of models
6+
subdirlist(subdirs ${CMAKE_CURRENT_SOURCE_DIR}/robots/)
7+
# Install each model
8+
foreach (dir ${subdirs})
9+
yarp_install(DIRECTORY robots/${dir} DESTINATION ${YARP_ROBOTS_INSTALL_DIR})
10+
endforeach ()

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

Lines changed: 13 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,13 @@
1+
<!-- Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved.
2+
This software may be modified and distributed under the terms of the
3+
BSD-3-Clause license. -->
4+
<?xml version="1.0" encoding="UTF-8" ?>
5+
<device xmlns:xi="http://www.w3.org/2001/XInclude" name="all_joints_mc" type="remotecontrolboardremapper">
6+
<param name="remoteControlBoards">("/ergocubSim/right_leg")</param>
7+
<param name="axesNames">("r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll")</param>
8+
<param name="localPortPrefix">/robot_dynamics_estimator/joints</param>
9+
10+
<group name="REMOTE_CONTROLBOARD_OPTIONS">
11+
<param name="carrier">udp</param>
12+
</group>
13+
</device>

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

Lines changed: 14 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,14 @@
1+
<!-- Copyright (C) 2022 Istituto Italiano di Tecnologia (IIT). All rights reserved.
2+
This software may be modified and distributed under the terms of the
3+
BSD-3-Clause license. -->
4+
<?xml version="1.0" encoding="UTF-8" ?>
5+
6+
<devices>
7+
<device xmlns:xi="http://www.w3.org/2001/XInclude" name="right_leg_ft_client" type="multipleanalogsensorsclient">
8+
<param name="remote">/ergocubSim/right_leg/FT</param>
9+
<param name="local">/robot_dynamics_estimator/right_leg_hip</param>
10+
<param name="timeout">0.5</param>
11+
<param name="carrier">fast_tcp</param>
12+
</device>
13+
14+
</devices>

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

Lines changed: 20 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,20 @@
1+
<!-- Copyright (C) 2019-2021 Istituto Italiano di Tecnologia (IIT). All rights reserved.
2+
This software may be modified and distributed under the terms of the
3+
BSD-3-Clause license. -->
4+
5+
<?xml version="1.0" encoding="UTF-8" ?>
6+
<device xmlns:xi="http://www.w3.org/2001/XInclude" name="mas-remapper" type="multipleanalogsensorsremapper">
7+
<param name="period">10</param>
8+
9+
<param name="SixAxisForceTorqueSensorsNames">
10+
(r_leg_ft, r_foot_front_ft, r_foot_rear_ft)
11+
</param>
12+
13+
<action phase="startup" level="5" type="attach">
14+
<paramlist name="networks">
15+
<elem name="right_leg_ft_client">right_leg_ft_client</elem>
16+
</paramlist>
17+
</action>
18+
19+
<action phase="shutdown" level="5" type="detach" />
20+
</device>

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

Lines changed: 15 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,15 @@
1+
<!-- Copyright (C) 2019-2022 Istituto Italiano di Tecnologia (IIT). All rights reserved.
2+
This software may be modified and distributed under the terms of the
3+
BSD-3-Clause license. -->
4+
5+
<?xml version="1.0" encoding="UTF-8" ?>
6+
<!DOCTYPE robot PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
7+
8+
<robot name="ergoCubGazeboV1" portprefix="RobotDynamicsEstimator" build="1" xmlns:xi="http://www.w3.org/2001/XInclude">
9+
<devices>
10+
<xi:include href="interface-robot-dynamics-estimator/all_joints_mc.xml" />
11+
<xi:include href="interface-robot-dynamics-estimator/mas-remapper.xml" />
12+
<xi:include href="interface-robot-dynamics-estimator/ft_clients.xml" />
13+
<xi:include href="./robot-dynamics-estimator.xml" />
14+
</devices>
15+
</robot>

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

Lines changed: 172 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,172 @@
1+
<!-- Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved.
2+
This software may be modified and distributed under the terms of the
3+
BSD-3-Clause license. -->
4+
5+
<?xml version="1.0" encoding="UTF-8" ?>
6+
<device xmlns:xi="http://www.w3.org/2001/XInclude" name="robot-dynamics-estimator" type="RobotDynamicsEstimatorDevice">
7+
<group name="GENERAL">
8+
<param name="sampling_time">0.01</param>
9+
<param name="port_prefix">/robot-dynamics-estimator</param>
10+
</group>
11+
12+
<group name="MODEL">
13+
<param name="model_file">model.urdf</param>
14+
<param name="base_link">root_link</param>
15+
<param name="joint_list">("r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll")</param>
16+
<param name="gear_ratio">(100.0, -160.0, 100.0, 100.0, 100.0, 160.0)</param>
17+
<param name="torque_constant">(0.111, 0.047, 0.047, 0.111, 0.111, 0.025)</param>
18+
19+
<group name="FT">
20+
<param name="names">("r_leg_ft", "r_foot_front_ft", "r_foot_rear_ft")</param>
21+
<param name="frames">("r_leg_ft", "r_foot_front_ft", "r_foot_rear_ft")</param>
22+
<param name="associated_joints">("r_leg_ft_sensor", "r_foot_front_ft_sensor", "r_foot_rear_ft_sensor")</param>
23+
</group>
24+
25+
<group name="ACCELEROMETER">
26+
<param name="names">()</param>
27+
<param name="frames">()</param>
28+
</group>
29+
30+
<group name="GYROSCOPE">
31+
<param name="names">()</param>
32+
<param name="frames">()</param>
33+
</group>
34+
35+
<group name="EXTERNAL_CONTACT">
36+
<param name="frames">()</param>
37+
</group>
38+
</group>
39+
40+
<group name="RobotSensorBridge">
41+
<param name="check_for_nan">false</param>
42+
<param name="stream_joint_states">true</param>
43+
<param name="stream_inertials">false</param>
44+
<param name="stream_forcetorque_sensors">true</param>
45+
<param name="stream_motor_states">true</param>
46+
47+
<group name="RemoteControlBoardRemapper">
48+
<param name="joints_list">("r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll")</param>
49+
</group>
50+
51+
<group name="SixAxisForceTorqueSensors">
52+
<param name="sixaxis_forcetorque_sensors_list">("r_leg_ft", "r_foot_front_ft", "r_foot_rear_ft")</param>
53+
</group>
54+
</group>
55+
56+
<group name="UKF">
57+
<param name="alpha">1.0</param>
58+
<param name="beta">2.0</param>
59+
<param name="kappa">0.0</param>
60+
<group name="UKF_STATE">
61+
<!--param name="dynamics_list">
62+
("JOINT_VELOCITY", "MOTOR_TORQUE", "FRICTION_TORQUE", "RIGHT_LEG_FT", "RIGHT_FOOT_FRONT_FT", "RIGHT_FOOT_REAR_FT",
63+
"RIGHT_LEG_FT_BIAS", "RIGHT_FOOT_FRONT_FT_BIAS", "RIGHT_FOOT_REAR_FT_BIAS",
64+
"RIGHT_LEG_ACC_BIAS", "RIGHT_FOOT_FRONT_ACC_BIAS", "RIGHT_FOOT_REAR_ACC_BIAS",
65+
"RIGHT_LEG_GYRO_BIAS", "RIGHT_FOOT_FRONT_GYRO_BIAS", "RIGHT_FOOT_REAR_GYRO_BIAS")
66+
</param-->
67+
<param name="dynamics_list">
68+
("JOINT_VELOCITY", "MOTOR_TORQUE", "FRICTION_TORQUE", "RIGHT_LEG_FT", "RIGHT_FOOT_FRONT_FT", "RIGHT_FOOT_REAR_FT")
69+
</param>
70+
<!-- Available models = ["ZeroVelocityStateDynamics", "JointVelocityStateDynamics", "FrictionTorqueStateDynamics"] -->
71+
<group name="JOINT_VELOCITY">
72+
<param name="name">ds</param>
73+
<param name="elements">("r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll")</param>
74+
<param name="covariance">(1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3)</param>
75+
<param name="initial_covariance">(0.01, 0.01, 0.01, 0.01, 0.01, 0.01)</param>
76+
<param name="dynamic_model">JointVelocityStateDynamics</param>
77+
</group>
78+
<group name="MOTOR_TORQUE">
79+
<param name="name">tau_m</param>
80+
<param name="elements">("r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll")</param>
81+
<param name="covariance">(1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3)</param>
82+
<param name="initial_covariance">(0.01, 0.01, 0.01, 0.01, 0.01, 0.01)</param>
83+
<param name="dynamic_model">ZeroVelocityStateDynamics</param>
84+
</group>
85+
<group name="FRICTION_TORQUE">
86+
<param name="name">tau_F</param>
87+
<param name="elements">("r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll")</param>
88+
<param name="covariance">(1e-1, 1e-1, 1e-1, 1e-1, 1e-1, 1e-1)</param>
89+
<param name="initial_covariance">(0.01, 0.01, 0.01, 0.01, 0.01, 0.01)</param>
90+
<param name="friction_k0">(0.001, 0.001, 0.001, 0.001, 0.001, 0.001)</param>
91+
<param name="friction_k1">(0.01, 0.01, 0.01, 0.01, 0.01, 0.01)</param>
92+
<param name="friction_k2">(0.01, 0.01, 0.01, 0.01, 0.01, 0.01)</param>
93+
<param name="dynamic_model">FrictionTorqueStateDynamics</param>
94+
</group>
95+
<group name="RIGHT_LEG_FT">
96+
<param name="name">r_leg_ft</param>
97+
<param name="covariance">(1e-3, 1e-3, 1e-3, 1e-4, 1e-4, 1e-4)</param>
98+
<param name="initial_covariance">(0.01, 0.01, 0.01, 0.01, 0.01, 0.01)</param>
99+
<param name="dynamic_model">ZeroVelocityStateDynamics</param>
100+
</group>
101+
<group name="RIGHT_FOOT_FRONT_FT">
102+
<param name="name">r_foot_front_ft</param>
103+
<param name="covariance">(1e-3, 1e-3, 1e-3, 1e-6, 1e-6, 1e-6)</param>
104+
<param name="initial_covariance">(0.01, 0.01, 0.01, 0.01, 0.01, 0.01)</param>
105+
<param name="dynamic_model">ZeroVelocityStateDynamics</param>
106+
</group>
107+
<group name="RIGHT_FOOT_REAR_FT">
108+
<param name="name">r_foot_rear_ft</param>
109+
<param name="covariance">(1e-3, 1e-3, 1e-3, 1e-6, 1e-6, 1e-6)</param>
110+
<param name="initial_covariance">(0.01, 0.01, 0.01, 0.01, 0.01, 0.01)</param>
111+
<param name="dynamic_model">ZeroVelocityStateDynamics</param>
112+
</group>
113+
</group>
114+
115+
<group name="UKF_MEASUREMENT">
116+
<!--param name="dynamics_list">
117+
("JOINT_VELOCITY", "MOTOR_CURRENT",
118+
"RIGHT_LEG_FT", "RIGHT_FOOT_FRONT_FT", "RIGHT_FOOT_REAR_FT",
119+
"RIGHT_LEG_ACC", "RIGHT_FOOT_FRONT_ACC", "RIGHT_FOOT_REAR_ACC",
120+
"RIGHT_LEG_GYRO", "RIGHT_FOOT_FRONT_GYRO", "RIGHT_FOOT_REAR_GYRO")
121+
</param-->
122+
<param name="dynamics_list">
123+
("JOINT_VELOCITY", "MOTOR_CURRENT",
124+
"RIGHT_LEG_FT", "RIGHT_FOOT_FRONT_FT", "RIGHT_FOOT_REAR_FT")
125+
</param>
126+
<!-- Available models = ["ConstantMeasurementModel", "AccelerometerMeasurementDynamics",
127+
"GyroscopeMeasurementDynamics", "MotorCurrentMeasurementDynamics"] -->
128+
<group name="JOINT_VELOCITY">
129+
<param name="name">ds</param>
130+
<param name="covariance">(1e-1, 1e-1, 1e-1, 1e-1, 1e-1, 1e-1)</param>
131+
<param name="dynamic_model">ConstantMeasurementModel</param>
132+
</group>
133+
<group name="MOTOR_CURRENT">
134+
<param name="name">i_m</param>
135+
<param name="covariance">(1e-2, 1e-2, 1e-1, 1e-1, 1e-1, 1e-1)</param>
136+
<param name="gear_ratio">(100.0, -160.0, 100.0, 100.0, 100.0, 160.0)</param>
137+
<param name="torque_constant">(0.111, 0.047, 0.047, 0.111, 0.111, 0.025)</param>
138+
<param name="dynamic_model">MotorCurrentMeasurementDynamics</param>
139+
</group>
140+
<group name="RIGHT_LEG_FT">
141+
<param name="name">r_leg_ft</param>
142+
<param name="covariance">(1e-1, 1e-1, 1e-1, 1e-1, 1e-1, 1e-1)</param>
143+
<param name="use_bias">false</param>
144+
<param name="dynamic_model">ConstantMeasurementModel</param>
145+
</group>
146+
<group name="RIGHT_FOOT_FRONT_FT">
147+
<param name="name">r_foot_front_ft</param>
148+
<param name="covariance">(1e-1, 1e-1, 1e-1, 1e-1, 1e-1, 1e-1)</param>
149+
<param name="use_bias">false</param>
150+
<param name="dynamic_model">ConstantMeasurementModel</param>
151+
</group>
152+
<group name="RIGHT_FOOT_REAR_FT">
153+
<param name="name">r_foot_rear_ft</param>
154+
<param name="covariance">(1e-1, 1e-1, 1e-1, 1e-1, 1e-1, 1e-1)</param>
155+
<param name="use_bias">false</param>
156+
<param name="dynamic_model">ConstantMeasurementModel</param>
157+
</group>
158+
</group>
159+
</group>
160+
161+
<!-- ATTACH -->
162+
<action phase="startup" level="15" type="attach">
163+
<paramlist name="networks">
164+
<elem name="all_joints">all_joints_mc</elem>
165+
<elem name="mas-remapper">mas-remapper</elem>
166+
</paramlist>
167+
</action>
168+
169+
<action phase="shutdown" level="2" type="detach" />
170+
<!-- FINISH ATTACH-->
171+
172+
</device>

0 commit comments

Comments
 (0)