Skip to content

Commit

Permalink
Implement RobotDynamicsEstimatorDevice
Browse files Browse the repository at this point in the history
  • Loading branch information
@isorrentino
isorrentino committed Nov 8, 2023
1 parent 395aa48 commit 67a8bf6
Show file tree
Hide file tree
Showing 10 changed files with 1,238 additions and 0 deletions.
1 change: 1 addition & 0 deletions devices/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@
# BSD-3-Clause license.

add_subdirectory(FloatingBaseEstimatorDevice)
add_subdirectory(RobotDynamicsEstimatorDevice)
add_subdirectory(YarpRobotLoggerDevice)
add_subdirectory(VectorsCollectionWrapper)

Expand Down
19 changes: 19 additions & 0 deletions devices/RobotDynamicsEstimatorDevice/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
@@ -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()
10 changes: 10 additions & 0 deletions devices/RobotDynamicsEstimatorDevice/app/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
@@ -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 ()
13 changes: 13 additions & 0 deletions ...torDevice/app/robots/ergoCubGazeboV1/interface-robot-dynamics-estimator/all_joints_mc.xml
View file
Original file line number Diff line number Diff line change
@@ -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>
14 changes: 14 additions & 0 deletions ...imatorDevice/app/robots/ergoCubGazeboV1/interface-robot-dynamics-estimator/ft_clients.xml
View file
Original file line number Diff line number Diff line change
@@ -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>
20 changes: 20 additions & 0 deletions ...atorDevice/app/robots/ergoCubGazeboV1/interface-robot-dynamics-estimator/mas-remapper.xml
View file
Original file line number Diff line number Diff line change
@@ -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>
15 changes: 15 additions & 0 deletions ...botDynamicsEstimatorDevice/app/robots/ergoCubGazeboV1/launch-robot-dynamics-estimator.xml
View file
Original file line number Diff line number Diff line change
@@ -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>
172 changes: 172 additions & 0 deletions devices/RobotDynamicsEstimatorDevice/app/robots/ergoCubGazeboV1/robot-dynamics-estimator.xml
View file
Original file line number Diff line number Diff line change
@@ -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_sensor", "r_foot_front_ft_sensor", "r_foot_rear_ft_sensor")</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_sensor</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_sensor</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_sensor</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_sensor</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_sensor</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_sensor</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>
Loading

0 comments on commit 67a8bf6

Please sign in to comment.