Merge pull request #525 from isorrentino/addLoggingAllFTsiCubGazeboV3

Add logging of fts and cartesian wrenches on left and right arm and right upper leg when using iCubGazeboV3

CHANGELOG.md

@@ -10,12 +10,14 @@ All notable changes to this project are documented in this file.
 - Implement the `VectorsCollectionWrapper` device for collection of arbitrary vector ports (https://github.com/ami-iit/bipedal-locomotion-framework/pull/512)
 - Add reading of right upper leg FT for `iCubGenova09` and associated cartesian wrench in `YarpRobotLoggerDevice` configuration files (https://github.com/ami-iit/bipedal-locomotion-framework/pull/513)
 - Add reading of right and left arms FT for `iCubGenova09` in `YarpRobotLoggerDevice` configuration files (https://github.com/ami-iit/bipedal-locomotion-framework/pull/515)
+- Add reading of right and left arms and right upper leg FTs and cartesian wrenches for `iCubGazeboV3` in `YarpRobotLoggerDevice` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/525)
 
 ### Changed
 - An error it will be returned if the user tries to change the clock type once the `clock()` has been already called once (https://github.com/ami-iit/bipedal-locomotion-framework/pull/500)
 - Log the arms external wrenches on the YarpRobotLogger for iCubGenova09 (https://github.com/ami-iit/bipedal-locomotion-framework/pull/502)
 - IK and TSID now uses the weight provider to specify the weight associated to a task (https://github.com/ami-iit/bipedal-locomotion-framework/pull/506)
 - The `Planners`, `System`, `RobotInterface` and `YarpImplementation` components are no more mandatory to compile the python bindings (https://github.com/ami-iit/bipedal-locomotion-framework/pull/511)
+- Reorganize the multiple FT sensor and external wrench files into a single file in the YarpRobotLoggerDevice (https://github.com/ami-iit/bipedal-locomotion-framework/pull/525)
 
 ### Fix
 - Remove outdated includes in YarpRobotLoggerDevice.cpp (https://github.com/ami-iit/bipedal-locomotion-framework/pull/502)

devices/YarpRobotLoggerDevice/app/robots/iCubGazeboV2_5/interface/ft_clients.xml

@@ -0,0 +1,28 @@
+<!-- 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_front_ft_client" type="analogsensorclient">
+        <param name="remote">/icubSim/left_foot_front/analog:o</param>
+        <param name="local">/yarp_robot_logger/left_front_ft</param>
+    </device>
+
+    <device xmlns:xi="http://www.w3.org/2001/XInclude" name="left_rear_ft_client" type="analogsensorclient">
+        <param name="remote">/icubSim/left_foot_rear/analog:o</param>
+        <param name="local">/yarp_robot_logger/left_rear_ft</param>
+    </device>
+
+    <device xmlns:xi="http://www.w3.org/2001/XInclude" name="right_front_ft_client" type="analogsensorclient">
+        <param name="remote">/icubSim/right_foot_front/analog:o</param>
+        <param name="local">/yarp_robot_logger/right_front_ft</param>
+    </device>
+
+    <device xmlns:xi="http://www.w3.org/2001/XInclude" name="right_rear_ft_client" type="analogsensorclient">
+        <param name="remote">/icubSim/right_foot_rear/analog:o</param>
+        <param name="local">/yarp_robot_logger/right_rear_ft</param>
+    </device>
+
+</devices>

devices/YarpRobotLoggerDevice/app/robots/iCubGazeboV2_5/interface/wrench_clients.xml

@@ -0,0 +1,28 @@
+<!-- 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_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>
+    </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>
+    </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>
+    </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>
+    </device>
+
+</devices>

devices/YarpRobotLoggerDevice/app/robots/iCubGazeboV3/interface/ft_clients.xml

@@ -0,0 +1,43 @@
+<!-- Copyright (C) 2022 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_arm_ft_client" type="analogsensorclient">
+        <param name="remote">/icubSim/left_arm/analog:o</param>
+        <param name="local">/yarp_robot_logger/left_arm</param>
+    </device>
+
+    <device xmlns:xi="http://www.w3.org/2001/XInclude" name="right_arm_ft_client" type="analogsensorclient">
+        <param name="remote">/icubSim/right_arm/analog:o</param>
+        <param name="local">/yarp_robot_logger/right_arm</param>
+    </device>
+
+    <device xmlns:xi="http://www.w3.org/2001/XInclude" name="left_front_ft_client" type="analogsensorclient">
+        <param name="remote">/icubSim/left_foot_front/analog:o</param>
+        <param name="local">/yarp_robot_logger/left_front_ft</param>
+    </device>
+
+    <device xmlns:xi="http://www.w3.org/2001/XInclude" name="left_rear_ft_client" type="analogsensorclient">
+        <param name="remote">/icubSim/left_foot_rear/analog:o</param>
+        <param name="local">/yarp_robot_logger/left_rear_ft</param>
+    </device>
+
+    <device xmlns:xi="http://www.w3.org/2001/XInclude" name="right_upper_leg_ft_client" type="analogsensorclient">
+        <param name="remote">/icubSim/right_leg/analog:o</param>
+        <param name="local">/yarp_robot_logger/right_upper_leg_ft</param>
+    </device>
+
+    <device xmlns:xi="http://www.w3.org/2001/XInclude" name="right_front_ft_client" type="analogsensorclient">
+        <param name="remote">/icubSim/right_foot_front/analog:o</param>
+        <param name="local">/yarp_robot_logger/right_front_ft</param>
+    </device>
+
+    <device xmlns:xi="http://www.w3.org/2001/XInclude" name="right_rear_ft_client" type="analogsensorclient">
+        <param name="remote">/icubSim/right_foot_rear/analog:o</param>
+        <param name="local">/yarp_robot_logger/right_rear_ft</param>
+    </device>
+
+</devices>

devices/YarpRobotLoggerDevice/app/robots/iCubGazeboV3/interface/wrench_clients.xml

@@ -0,0 +1,43 @@
+<!-- Copyright (C) 2022 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_arm_wrench_client" type="genericSensorClient">
+        <param name="remote">/wholeBodyDynamics/left_arm/cartesianEndEffectorWrench:o</param>
+        <param name="local">/yarp_robot_logger/left_arm_wrench</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>
+    </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>
+    </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>
+    </device>
+
+    <device xmlns:xi="http://www.w3.org/2001/XInclude" name="right_upper_leg_wrench_client" type="genericSensorClient">
+        <param name="remote">/wholeBodyDynamics/right_upper_leg/cartesianEndEffectorWrench:o</param>
+        <param name="local">/yarp_robot_logger/right_upper_leg_wrench_client</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>
+    </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>
+    </device>
+
+</devices>

devices/YarpRobotLoggerDevice/app/robots/iCubGazeboV3/launch-yarp-robot-logger.xml

@@ -11,14 +11,8 @@ GNU Lesser General Public License v2.1 or any later version. -->
         <xi:include href="interface/all_joints_mc.xml" />
         <xi:include href="interface/head_imu.xml" />
         <xi:include href="interface/mas-remapper.xml" />
-        <xi:include href="interface/left_front_ft_client.xml" />
-        <xi:include href="interface/left_rear_ft_client.xml" />
-        <xi:include href="interface/right_front_ft_client.xml" />
-        <xi:include href="interface/right_rear_ft_client.xml" />
-        <xi:include href="interface/left_front_wrench_client.xml" />
-        <xi:include href="interface/left_rear_wrench_client.xml" />
-        <xi:include href="interface/right_front_wrench_client.xml" />
-        <xi:include href="interface/right_rear_wrench_client.xml" />
+        <xi:include href="interface/ft_clients.xml" />
+        <xi:include href="interface/wrench_clients.xml" />
         <xi:include href="./yarp-robot-logger.xml" />
     </devices>
 </robot>

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

@@ -26,7 +26,7 @@ GNU Lesser General Public License v2.1 or any later version. -->
     <param name="check_for_nan">false</param>
     <param name="stream_joint_states">true</param>
     <param name="stream_motor_states">false</param>
-    <param name="stream_inertials">true</param>
+    <param name="stream_inertials">false</param>
     <param name="stream_cartesian_wrenches">true</param>
     <param name="stream_forcetorque_sensors">true</param>
 
@@ -35,11 +35,11 @@ GNU Lesser General Public License v2.1 or any later version. -->
     </group>
 
     <group name="SixAxisForceTorqueSensors">
-      <param name="sixaxis_forcetorque_sensors_list">("left_front_ft_client", "left_rear_ft_client", "right_front_ft_client", "right_rear_ft_client")</param>
+      <param name="sixaxis_forcetorque_sensors_list">("left_front_ft_client", "left_rear_ft_client", "right_front_ft_client", "right_rear_ft_client", "right_arm_ft_client", "left_arm_ft_client", "right_upper_leg_ft_client")</param>
     </group>
 
     <group name="CartesianWrenches">
-      <param name="cartesian_wrenches_list">("left_front_wrench_client", "left_rear_wrench_client", "right_front_wrench_client", "right_rear_wrench_client")</param>
+      <param name="cartesian_wrenches_list">("left_front_wrench_client", "left_rear_wrench_client", "right_front_wrench_client", "right_rear_wrench_client", "right_arm_wrench_client", "left_arm_wrench_client", "right_upper_leg_wrench_client")</param>
     </group>
 
     <group name="InertialSensors">
@@ -58,10 +58,16 @@ GNU Lesser General Public License v2.1 or any later version. -->
       <elem name="left_rear_ft_client">left_rear_ft_client</elem>
       <elem name="right_front_ft_client">right_front_ft_client</elem>
       <elem name="right_rear_ft_client">right_rear_ft_client</elem>
+      <elem name="right_upper_leg_ft_client">right_upper_leg_ft_client</elem>
+      <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_front_wrench_client">left_front_wrench_client</elem>
       <elem name="left_rear_wrench_client">left_rear_wrench_client</elem>
       <elem name="right_front_wrench_client">right_front_wrench_client</elem>
       <elem name="right_rear_wrench_client">right_rear_wrench_client</elem>
+      <elem name="right_arm_wrench_client">right_arm_wrench_client</elem>
+      <elem name="left_arm_wrench_client">left_arm_wrench_client</elem>
+      <elem name="right_upper_leg_wrench_client">right_upper_leg_wrench_client</elem>
       <elem name="mas-remapper">mas-remapper</elem>
     </paramlist>
   </action>

devices/YarpRobotLoggerDevice/app/robots/iCubGenova02/interface/ft_clients.xml

@@ -0,0 +1,29 @@
+<!-- Copyright (C) 2019-2021 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-ft-client" type="multipleanalogsensorsclient">
+        <param name="remote">/icub/left_foot</param>
+        <param name="local">/yarp_robot_logger/left_foot</param>
+    </device>
+
+    <device  xmlns:xi="http://www.w3.org/2001/XInclude" name="left-leg-ft-client" type="multipleanalogsensorsclient">
+        <param name="remote">/icub/left_leg</param>
+        <param name="local">/yarp_robot_logger/left_leg</param>
+    </device>
+
+    <device  xmlns:xi="http://www.w3.org/2001/XInclude" name="right-foot-ft-client" type="multipleanalogsensorsclient">
+        <param name="remote">/icub/right_foot</param>
+        <param name="local">/yarp_robot_logger/right_foot</param>
+    </device>
+
+    <device  xmlns:xi="http://www.w3.org/2001/XInclude" name="right-leg-ft-client" type="multipleanalogsensorsclient">
+        <param name="remote">/icub/right_leg</param>
+        <param name="local">/yarp_robot_logger/right_leg</param>
+    </device>
+
+</devices>