Implement joint torque control device and friction estimation through…

… PINN (ami-iit#866)

CHANGELOG.md

@@ -26,6 +26,7 @@ All notable changes to this project are documented in this file.
 - Add launch parameter to `blf-logger-with-audio.sh` script to set logger launch file (https://github.com/ami-iit/bipedal-locomotion-framework/pull/867)
 - Add `getJointLimits` function to set get actuated joints position limits (https://github.com/ami-iit/bipedal-locomotion-framework/pull/868)
 - Add the possibility to disable streaming of joint encoder acceleration measurements (https://github.com/ami-iit/bipedal-locomotion-framework/pull/876)
+- Implement joint torque control device and friction estimation through PINN (https://github.com/ami-iit/bipedal-locomotion-framework/pull/866)
 - Add a task in the IK to set the joint velocity limits (https://github.com/ami-iit/bipedal-locomotion-framework/pull/879)
 
 ### Changed

cmake/AddBipedalLocomotionYARPDevice.cmake

@@ -52,6 +52,8 @@ function(add_bipedal_yarp_device)
     target_link_libraries(${name} PUBLIC ${public_link_libraries})
     target_link_libraries(${name} PRIVATE ${private_link_libraries})
     target_compile_features(${name} PUBLIC cxx_std_17)
+    target_compile_definitions(${name} PRIVATE -D_USE_MATH_DEFINES)
+
 
     # Specify include directories for both compilation and installation process.
     # The $<INSTALL_PREFIX> generator expression is useful to ensure to create
@@ -73,3 +75,4 @@ function(add_bipedal_yarp_device)
   endif()
 
 endfunction()
+

cmake/BipedalLocomotionDependencyClassifier.cmake

@@ -39,3 +39,4 @@ function(dependency_classifier package)
   endif()
 
 endfunction()
+

cmake/BipedalLocomotionFrameworkDependencies.cmake

@@ -277,6 +277,10 @@ framework_dependent_option(FRAMEWORK_COMPILE_YarpRobotLoggerDevice
   "Do you want to generate and compile the YarpRobotLoggerDevice?" ON
   "FRAMEWORK_COMPILE_RobotInterface;FRAMEWORK_COMPILE_YarpImplementation;FRAMEWORK_COMPILE_Perception;FRAMEWORK_COMPILE_YarpUtilities;FRAMEWORK_USE_robometry" OFF)
 
+framework_dependent_option(FRAMEWORK_COMPILE_JointTorqueControlDevice
+  "Do you want to generate and compile the YarpRobotLoggerDevice?" ON
+  "FRAMEWORK_COMPILE_YarpImplementation;FRAMEWORK_COMPILE_YarpUtilities;FRAMEWORK_USE_onnxruntime" OFF)
+
 framework_dependent_option(FRAMEWORK_COMPILE_VectorsCollectionWrapper
   "Do you want to generate and compile the VectorsCollectionWrapper?" ON
   "FRAMEWORK_COMPILE_YarpImplementation;FRAMEWORK_COMPILE_YarpUtilities" OFF)

cmake/BipedalLocomotionFrameworkFindDependencies.cmake

@@ -124,3 +124,4 @@ macro(FRAMEWORK_DEPENDENT_OPTION _option _doc _default _deps _force)
   endif()
 
 endmacro()
+

devices/CMakeLists.txt

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

devices/JointTorqueControlDevice/CMakeLists.txt

@@ -0,0 +1,35 @@
+# 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_JointTorqueControlDevice)
+  add_bipedal_locomotion_yarp_thrift(
+      NAME JointTorqueControlCommands
+      THRIFT thrifts/JointTorqueControlCommands.thrift
+      INSTALLATION_FOLDER JointTorqueControlDevice)
+
+  add_bipedal_yarp_device(
+      NAME JointTorqueControlDevice
+      TYPE BipedalLocomotion::JointTorqueControlDevice
+      SOURCES src/JointTorqueControlDevice.cpp
+              src/PassThroughControlBoard.cpp
+              src/PINNFrictionEstimator.cpp
+      PUBLIC_HEADERS include/BipedalLocomotion/JointTorqueControlDevice.h
+                     include/BipedalLocomotion/PassThroughControlBoard.h
+                     include/BipedalLocomotion/PINNFrictionEstimator.h
+      PRIVATE_LINK_LIBRARIES
+      BipedalLocomotion::YarpUtilities
+      BipedalLocomotion::SystemYarpImplementation
+      BipedalLocomotion::RobotInterfaceYarpImplementation
+      BipedalLocomotion::ParametersHandlerYarpImplementation
+      BipedalLocomotion::VectorsCollection
+      BipedalLocomotion::JointTorqueControlCommands
+      BipedalLocomotion::Math
+      PUBLIC_LINK_LIBRARIES
+      onnxruntime::onnxruntime
+      Eigen3::Eigen
+      ${YARP_LIBRARIES}
+      ${iDynTree_LIBRARIES}
+      CONFIGURE_PACKAGE_NAME joint_torque_control_device)
+endif()
+

devices/JointTorqueControlDevice/README.md

@@ -0,0 +1,46 @@
+# JointTorqueControlDevice
+
+The **JointTorqueControlDevice** is a YARP device to run joint torque control and generate motor current set-points.
+
+## :running: How to use the device
+
+### Run the device on a fake robot
+
+The `example` folder contains a simple example on how to run the `JointTorqueControlDevice` to hijack an existing control board device.
+The `jtcvcExample.xml` launches four devices:
+* `original_controlboard` of type `fakeMotionControlboard`, the original controlboard
+* `original_nws_yarp` of type `controlBoard_nws_yarp`, the network wrapper server to expose `original_controlboard` on YARP ports (for example to access it via yarpmotorgui)
+* `hijacked_controlboard` of type `JointTorqueControlDevice`, the controlboard wrapped by `JointTorqueControlDevice` to implement the `ITorqueControl` methods on the top of `ICurrentControl`
+* `hijacked_nws_yarp` of type `controlBoard_nws_yarp`, the network wrapper server to expose `hijacked_controlboard` on YARP ports (for example to access it via yarpmotorgui)
+
+This permit to show how the hijacking  works. First of all, open three terminals in the `example` directory.
+
+In one, launch the `jtcvcExample.xml` :
+~~~
+yarprobotinterface --config ./jtcvcExample.xml
+~~~
+
+In another, launch the `yarpmotorgui` to monitor the state of `original_controlboard` :
+~~~
+yarpmotorgui --from ./yarpmotorgui.ini
+~~~
+
+In the third, launch the `yarpmotorgui` to monitor the state of `hijacked_controlboard` :
+~~~
+yarpmotorgui --from ./yarpmotorgui_hijacked.ini
+~~~
+
+If everything started correctly, if on the  `hijacked_controlboard` `yarpmotorgui` you change a joint controlmode to torque, you should see that on `original_controlboard`'s `yarpmotorgui` the same joint will switch to current control mode. If you change the desired torque, you should see also the measured current change.
+
+### Run the device on the real robot
+
+To run the device on a robot make sure to define the configuration files for the robot in the `app/robots` folder.
+
+- **ergoCubSN001**
+
+  - Launch `yarprobotinterface` on the robot.
+  - launch `JointTorqueControlDevice`
+    ```
+    YARP_ROBOT_NAME=ergoCubSN001 yarprobotinterface --config launch-joint-torque-control.xml
+    ```
+

devices/JointTorqueControlDevice/app/CMakeLists.txt

@@ -0,0 +1,11 @@
+# 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/JointTorqueControlDevice/example/jtcvcExample.xml

@@ -0,0 +1,77 @@
+<!-- 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" ?>
+<!DOCTYPE robot PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
+
+<robot name="jtcvc_example">
+
+    <!-- Simulated controlboard -->
+    <!-- On a real robot, it should be something like embObjMotionControl -->
+    <device name="original_controlboard" type="fakeMotionControl">
+        <group name="GENERAL">
+            <param name="Joints">3</param>
+            <param name="AxisName">"joint0" "joint1"  "joint2"</param>
+        </group>
+    </device>
+
+    <!-- Expose original controlboard on YARP network under the name /jtcvc_example/original -->
+    <device xmlns:xi="http://www.w3.org/2001/XInclude" name="original_nws_yarp" type="controlBoard_nws_yarp">
+        <param name="period"> 0.002  </param>
+        <param name="name"> /jtcvc_example/original </param>
+        <action phase="startup" level="10" type="attach">
+            <param name="device"> original_controlboard </param>
+        </action>
+        <action phase="shutdown" level="15" type="detach" />
+    </device>
+
+    <!-- Controlboard hijacked by JointTorqueControlDevice -->
+    <!---->
+    <device name="hijacked_controlboard" type="JointTorqueControlDevice">
+        <group name="TORQUE_CURRENT_PARAMS">
+            <param name="kt">(   0.142674       -0.158045       0.132443       )</param>
+            <param name="kfc">(   0.5            0.5            0.5  )</param>
+            <param name="kp">(   0.0            0.0            0.0  )</param>
+            <param name="max_curr">(   2.0            2.0            2.0 )</param>
+            <param name="friction_model"> ("FRICTION_COULOMB_VISCOUS", "FRICTION_COULOMB_VISCOUS". "FRICTION_COULOMB_VISCOUS") </param>
+        </group>
+
+        <group name="FRICTION_COULOMB_VISCOUS">
+            <param name="kv">(   0.000457       0.000458       0.000980   )</param>
+            <param name="kc">(   0.0            0.0            0.0 )</param>
+            <param name="ka">(   0.0            0.0            0.0 )</param>
+        </group>
+
+        <group name="FRICTION_COULOMB_VISCOUS_STRIBECK">
+            <param name="kv">(   0.000457       0.000458       0.000980   )</param>
+            <param name="kc">(   0.0            0.0            0.0 )</param>
+            <param name="ks">(   0.000457       0.000458       0.000980   )</param>
+            <param name="ka">(   0.0            0.0            0.0 )</param>
+            <param name="vs">(   0.000457       0.000458       0.000980   )</param>
+        </group>
+
+        <!-- Warning, still in milliseconds -->
+        <param name="period"> 2 </param>
+
+        <action phase="startup" level="5" type="attach">
+            <paramlist name="networks">
+                <elem name="original_controlboard"> original_controlboard </elem>
+            </paramlist>
+        </action>
+
+        <action phase="shutdown" level="20" type="detach" />
+    </device>
+
+    <!-- Expose the controlboard hijacked by jointTorqueCurrentControlViaCurrent on YARP network under the name /jtcvc_example/hijacked -->
+    <device xmlns:xi="http://www.w3.org/2001/XInclude" name="hijacked_nws_yarp" type="controlBoard_nws_yarp">
+        <param name="period"> 0.002  </param>
+        <param name="name"> /jtcvc_example/hijacked </param>
+        <action phase="startup" level="10" type="attach">
+            <param name="device"> hijacked_controlboard </param>
+        </action>
+        <action phase="shutdown" level="15" type="detach" />
+    </device>
+
+</robot>
+

devices/JointTorqueControlDevice/example/yarpmotorgui.ini

@@ -0,0 +1,2 @@
+names ("/jtcvc_example/original")
+

devices/JointTorqueControlDevice/example/yarpmotorgui_hijacked.ini

@@ -0,0 +1,2 @@
+names ("/jtcvc_example/hijacked")
+