Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

[iCubGazeboV3 - iCubGazeboV2_5] Wrong estimated external wrenches #156

Closed
GiulioRomualdi opened this issue Aug 30, 2022 · 8 comments
Closed

[iCubGazeboV3 - iCubGazeboV2_5] Wrong estimated external wrenches #156

GiulioRomualdi opened this issue Aug 30, 2022 · 8 comments

Comments

@GiulioRomualdi
Copy link
Member

I spawned iCubGazeboV3 in gazebo and I ran wholebody dynamics with yarprobotinterface --config launch-wholebodydynamics.xml.
The robot was in double support and the feet were in contact with the ground

However when I tried to read the contact forces with yarp read ... /wholeBodyDynamics/left_foot_front/cartesianEndEffectorWrench:o I got the following result.

-6.61824911262959858504 22.7513259601726787196 -19.8714123575537797706 -3.94523009229582255486 -0.386628332966471921495 0.835996243860451682117
-6.70601412783409056573 22.8582984108054354522 -19.8699358998094659512 -3.95174154470659821925 -0.390805641636702727482 0.845556781194174433836
-6.79496840578879535144 22.9110749466858649726 -19.8054163161445622166 -3.95648009198770456152 -0.399091541687972561903 0.850312911042904850945

I asked @paolo-viceconte to replicate the same experiment and he got the same result
My setup is

cc @traversaro
@GiulioRomualdi GiulioRomualdi changed the title [iCubGazeboV3] Uncorrect estimated external wrench [iCubGazeboV3] Wrong estimated external wrenches Aug 30, 2022
@GiulioRomualdi
Copy link
Member Author

This is the output of whole-body-dynamics

[ERROR] |yarp.os.Property| cannot read from yarprobotinterface.ini
[DEBUG] Reading file /home/gromualdi/robot-code/robotology-superbuild/build/install/share/yarp/robots/iCubGazeboV3/launch-wholebodydynamics.xml
[WARNING] Invalid syntax while loading /home/gromualdi/robot-code/robotology-superbuild/build/install/share/yarp/robots/iCubGazeboV3/launch-wholebodydynamics.xml at line 1 . Expected document of type robot . Found devices
[DEBUG] yarprobotinterface: using xml parser for DTD v3.x
[DEBUG] Reading file /home/gromualdi/robot-code/robotology-superbuild/build/install/share/yarp/robots/iCubGazeboV3/launch-wholebodydynamics.xml
[DEBUG] Preprocessor complete in:  0.00042963 s
[WARNING] Invalid syntax while loading  at line 4 . "robot" element should contain the "portprefix" attribute. Using "name" attribute
[WARNING] *************************************************************************************
* yarprobotinterface 'portprefix' parameter does not follow convention,  *
* it MUST start with a leading '/' since it is used as the full prefix port name    *
*     name:    full port prefix name with leading '/',  e.g.  /robotName      *
* A temporary automatic fix will be done for you, but please fix your config file   *
*************************************************************************************
[INFO] |yarp.os.Port|/iCubGazeboV3/yarprobotinterface| Port /iCubGazeboV3/yarprobotinterface active at tcp://10.240.9.207:10056/
[INFO] startup phase starting...
[INFO] Opening device torso_mc with parameters [("robotName" = "iCubGazeboV3"), ("remote" = "/icubSim/torso"), ("local" = "/wholeBodyDynamics/torso")]
[DEBUG] |yarp.dev.PolyDriver|torso_mc| Parameters are (device remote_controlboard) (id torso_mc) (local "/wholeBodyDynamics/torso") (remote "/icubSim/torso") (robotName iCubGazeboV3)
[INFO] |yarp.os.Port|/wholeBodyDynamics/torso/rpc:o| Port /wholeBodyDynamics/torso/rpc:o active at tcp://10.240.9.207:10057/
[INFO] |yarp.os.Port|/wholeBodyDynamics/torso/command:o| Port /wholeBodyDynamics/torso/command:o active at tcp://10.240.9.207:10058/
[INFO] |yarp.os.Port|/wholeBodyDynamics/torso/stateExt:i| Port /wholeBodyDynamics/torso/stateExt:i active at tcp://10.240.9.207:10059/
[INFO] |yarp.os.impl.PortCoreOutputUnit|/wholeBodyDynamics/torso/rpc:o| Sending output from /wholeBodyDynamics/torso/rpc:o to /icubSim/torso/rpc:i using tcp
[INFO] |yarp.os.impl.PortCoreOutputUnit|/wholeBodyDynamics/torso/command:o| Sending output from /wholeBodyDynamics/torso/command:o to /icubSim/torso/command:i using udp
[INFO] |yarp.os.impl.PortCoreInputUnit|/wholeBodyDynamics/torso/stateExt:i| Receiving input from /icubSim/torso/stateExt:o to /wholeBodyDynamics/torso/stateExt:i using udp
[INFO] |yarp.dev.PolyDriver|torso_mc| Created device <remote_controlboard>. See C++ class RemoteControlBoard for documentation.
[INFO] Opening device left_arm_mc with parameters [("robotName" = "iCubGazeboV3"), ("remote" = "/icubSim/left_arm"), ("local" = "/wholeBodyDynamics/left_arm")]
[DEBUG] |yarp.dev.PolyDriver|left_arm_mc| Parameters are (device remote_controlboard) (id left_arm_mc) (local "/wholeBodyDynamics/left_arm") (remote "/icubSim/left_arm") (robotName iCubGazeboV3)
[INFO] |yarp.os.Port|/wholeBodyDynamics/left_arm/rpc:o| Port /wholeBodyDynamics/left_arm/rpc:o active at tcp://10.240.9.207:10060/
[INFO] |yarp.os.Port|/wholeBodyDynamics/left_arm/command:o| Port /wholeBodyDynamics/left_arm/command:o active at tcp://10.240.9.207:10061/
[INFO] |yarp.os.Port|/wholeBodyDynamics/left_arm/stateExt:i| Port /wholeBodyDynamics/left_arm/stateExt:i active at tcp://10.240.9.207:10062/
[INFO] |yarp.os.impl.PortCoreOutputUnit|/wholeBodyDynamics/left_arm/rpc:o| Sending output from /wholeBodyDynamics/left_arm/rpc:o to /icubSim/left_arm/rpc:i using tcp
[INFO] |yarp.os.impl.PortCoreOutputUnit|/wholeBodyDynamics/left_arm/command:o| Sending output from /wholeBodyDynamics/left_arm/command:o to /icubSim/left_arm/command:i using udp
[INFO] |yarp.os.impl.PortCoreInputUnit|/wholeBodyDynamics/left_arm/stateExt:i| Receiving input from /icubSim/left_arm/stateExt:o to /wholeBodyDynamics/left_arm/stateExt:i using udp
[INFO] |yarp.dev.PolyDriver|left_arm_mc| Created device <remote_controlboard>. See C++ class RemoteControlBoard for documentation.
[INFO] Opening device right_arm_mc with parameters [("robotName" = "iCubGazeboV3"), ("remote" = "/icubSim/right_arm"), ("local" = "/wholeBodyDynamics/right_arm")]
[DEBUG] |yarp.dev.PolyDriver|right_arm_mc| Parameters are (device remote_controlboard) (id right_arm_mc) (local "/wholeBodyDynamics/right_arm") (remote "/icubSim/right_arm") (robotName iCubGazeboV3)
[INFO] |yarp.os.Port|/wholeBodyDynamics/right_arm/rpc:o| Port /wholeBodyDynamics/right_arm/rpc:o active at tcp://10.240.9.207:10063/
[INFO] |yarp.os.Port|/wholeBodyDynamics/right_arm/command:o| Port /wholeBodyDynamics/right_arm/command:o active at tcp://10.240.9.207:10064/
[INFO] |yarp.os.Port|/wholeBodyDynamics/right_arm/stateExt:i| Port /wholeBodyDynamics/right_arm/stateExt:i active at tcp://10.240.9.207:10065/
[INFO] |yarp.os.impl.PortCoreOutputUnit|/wholeBodyDynamics/right_arm/rpc:o| Sending output from /wholeBodyDynamics/right_arm/rpc:o to /icubSim/right_arm/rpc:i using tcp
[INFO] |yarp.os.impl.PortCoreOutputUnit|/wholeBodyDynamics/right_arm/command:o| Sending output from /wholeBodyDynamics/right_arm/command:o to /icubSim/right_arm/command:i using udp
[INFO] |yarp.os.impl.PortCoreInputUnit|/wholeBodyDynamics/right_arm/stateExt:i| Receiving input from /icubSim/right_arm/stateExt:o to /wholeBodyDynamics/right_arm/stateExt:i using udp
[INFO] |yarp.dev.PolyDriver|right_arm_mc| Created device <remote_controlboard>. See C++ class RemoteControlBoard for documentation.
[INFO] Opening device left_leg_mc with parameters [("robotName" = "iCubGazeboV3"), ("remote" = "/icubSim/left_leg"), ("local" = "/wholeBodyDynamics/left_leg")]
[DEBUG] |yarp.dev.PolyDriver|left_leg_mc| Parameters are (device remote_controlboard) (id left_leg_mc) (local "/wholeBodyDynamics/left_leg") (remote "/icubSim/left_leg") (robotName iCubGazeboV3)
[INFO] |yarp.os.Port|/wholeBodyDynamics/left_leg/rpc:o| Port /wholeBodyDynamics/left_leg/rpc:o active at tcp://10.240.9.207:10066/
[INFO] |yarp.os.Port|/wholeBodyDynamics/left_leg/command:o| Port /wholeBodyDynamics/left_leg/command:o active at tcp://10.240.9.207:10067/
[INFO] |yarp.os.Port|/wholeBodyDynamics/left_leg/stateExt:i| Port /wholeBodyDynamics/left_leg/stateExt:i active at tcp://10.240.9.207:10068/
[INFO] |yarp.os.impl.PortCoreOutputUnit|/wholeBodyDynamics/left_leg/rpc:o| Sending output from /wholeBodyDynamics/left_leg/rpc:o to /icubSim/left_leg/rpc:i using tcp
[INFO] |yarp.os.impl.PortCoreOutputUnit|/wholeBodyDynamics/left_leg/command:o| Sending output from /wholeBodyDynamics/left_leg/command:o to /icubSim/left_leg/command:i using udp
[INFO] |yarp.os.impl.PortCoreInputUnit|/wholeBodyDynamics/left_leg/stateExt:i| Receiving input from /icubSim/left_leg/stateExt:o to /wholeBodyDynamics/left_leg/stateExt:i using udp
[INFO] |yarp.dev.PolyDriver|left_leg_mc| Created device <remote_controlboard>. See C++ class RemoteControlBoard for documentation.
[INFO] Opening device right_leg_mc with parameters [("robotName" = "iCubGazeboV3"), ("remote" = "/icubSim/right_leg"), ("local" = "/wholeBodyDynamics/right_leg")]
[DEBUG] |yarp.dev.PolyDriver|right_leg_mc| Parameters are (device remote_controlboard) (id right_leg_mc) (local "/wholeBodyDynamics/right_leg") (remote "/icubSim/right_leg") (robotName iCubGazeboV3)
[INFO] |yarp.os.Port|/wholeBodyDynamics/right_leg/rpc:o| Port /wholeBodyDynamics/right_leg/rpc:o active at tcp://10.240.9.207:10069/
[INFO] |yarp.os.Port|/wholeBodyDynamics/right_leg/command:o| Port /wholeBodyDynamics/right_leg/command:o active at tcp://10.240.9.207:10070/
[INFO] |yarp.os.Port|/wholeBodyDynamics/right_leg/stateExt:i| Port /wholeBodyDynamics/right_leg/stateExt:i active at tcp://10.240.9.207:10071/
[INFO] |yarp.os.impl.PortCoreOutputUnit|/wholeBodyDynamics/right_leg/rpc:o| Sending output from /wholeBodyDynamics/right_leg/rpc:o to /icubSim/right_leg/rpc:i using tcp
[INFO] |yarp.os.impl.PortCoreOutputUnit|/wholeBodyDynamics/right_leg/command:o| Sending output from /wholeBodyDynamics/right_leg/command:o to /icubSim/right_leg/command:i using udp
[INFO] |yarp.os.impl.PortCoreInputUnit|/wholeBodyDynamics/right_leg/stateExt:i| Receiving input from /icubSim/right_leg/stateExt:o to /wholeBodyDynamics/right_leg/stateExt:i using udp
[INFO] |yarp.dev.PolyDriver|right_leg_mc| Created device <remote_controlboard>. See C++ class RemoteControlBoard for documentation.
[INFO] Opening device head_mc with parameters [("robotName" = "iCubGazeboV3"), ("remote" = "/icubSim/head"), ("local" = "/wholeBodyDynamics/head")]
[DEBUG] |yarp.dev.PolyDriver|head_mc| Parameters are (device remote_controlboard) (id head_mc) (local "/wholeBodyDynamics/head") (remote "/icubSim/head") (robotName iCubGazeboV3)
[INFO] |yarp.os.Port|/wholeBodyDynamics/head/rpc:o| Port /wholeBodyDynamics/head/rpc:o active at tcp://10.240.9.207:10072/
[INFO] |yarp.os.Port|/wholeBodyDynamics/head/command:o| Port /wholeBodyDynamics/head/command:o active at tcp://10.240.9.207:10073/
[INFO] |yarp.os.Port|/wholeBodyDynamics/head/stateExt:i| Port /wholeBodyDynamics/head/stateExt:i active at tcp://10.240.9.207:10074/
[INFO] |yarp.os.impl.PortCoreOutputUnit|/wholeBodyDynamics/head/rpc:o| Sending output from /wholeBodyDynamics/head/rpc:o to /icubSim/head/rpc:i using tcp
[INFO] |yarp.os.impl.PortCoreOutputUnit|/wholeBodyDynamics/head/command:o| Sending output from /wholeBodyDynamics/head/command:o to /icubSim/head/command:i using udp
[INFO] |yarp.os.impl.PortCoreInputUnit|/wholeBodyDynamics/head/stateExt:i| Receiving input from /icubSim/head/stateExt:o to /wholeBodyDynamics/head/stateExt:i using udp
[INFO] |yarp.dev.PolyDriver|head_mc| Created device <remote_controlboard>. See C++ class RemoteControlBoard for documentation.
[INFO] Opening device inertial with parameters [("robotName" = "iCubGazeboV3"), ("remote" = "/icubSim/inertial"), ("local" = "/wholeBodyDynamics/imu")]
[DEBUG] |yarp.dev.PolyDriver|inertial| Parameters are (device genericSensorClient) (id inertial) (local "/wholeBodyDynamics/imu") (remote "/icubSim/inertial") (robotName iCubGazeboV3)
[INFO] |yarp.os.Port|/wholeBodyDynamics/imu| Port /wholeBodyDynamics/imu active at tcp://10.240.9.207:10075/
[INFO] |yarp.os.impl.PortCoreInputUnit|/wholeBodyDynamics/imu| Receiving input from /icubSim/inertial to /wholeBodyDynamics/imu using fast_tcp
[INFO] |yarp.dev.PolyDriver|inertial| Created device <genericSensorClient>. See C++ class yarp::dev::GenericSensorClient for documentation.
[INFO] Opening device left_upper_arm_strain with parameters [("robotName" = "iCubGazeboV3"), ("remote" = "/icubSim/left_arm/analog:o"), ("local" = "/wholeBodyDynamics/l_arm_ft_sensor")]
[DEBUG] |yarp.dev.PolyDriver|left_upper_arm_strain| Parameters are (device analogsensorclient) (id left_upper_arm_strain) (local "/wholeBodyDynamics/l_arm_ft_sensor") (remote "/icubSim/left_arm/analog:o") (robotName iCubGazeboV3)
[WARNING] |yarp.device.analogsensorclient| The 'inertial' device is deprecated in favour of 'multipleanalogsensorsclient'
[WARNING] |yarp.device.analogsensorclient| The old device is no longer supported, and it will be deprecated in YARP 3.7 and removed in YARP 4.
[WARNING] |yarp.device.analogsensorclient| Please update your scripts.
[INFO] |yarp.os.Port|/wholeBodyDynamics/l_arm_ft_sensor| Port /wholeBodyDynamics/l_arm_ft_sensor active at tcp://10.240.9.207:10076/
[INFO] |yarp.os.Port|/wholeBodyDynamics/l_arm_ft_sensor/rpc:o| Port /wholeBodyDynamics/l_arm_ft_sensor/rpc:o active at tcp://10.240.9.207:10077/
[INFO] |yarp.os.impl.PortCoreInputUnit|/wholeBodyDynamics/l_arm_ft_sensor| Receiving input from /icubSim/left_arm/analog:o to /wholeBodyDynamics/l_arm_ft_sensor using udp
[INFO] |yarp.os.impl.PortCoreOutputUnit|/wholeBodyDynamics/l_arm_ft_sensor/rpc:o| Sending output from /wholeBodyDynamics/l_arm_ft_sensor/rpc:o to /icubSim/left_arm/analog:o/rpc:i using tcp
[INFO] |yarp.dev.PolyDriver|left_upper_arm_strain| Created device <analogsensorclient>. See C++ class AnalogSensorClient for documentation.
[INFO] Opening device right_upper_arm_strain with parameters [("robotName" = "iCubGazeboV3"), ("remote" = "/icubSim/right_arm/analog:o"), ("local" = "/wholeBodyDynamics/r_arm_ft_sensor")]
[DEBUG] |yarp.dev.PolyDriver|right_upper_arm_strain| Parameters are (device analogsensorclient) (id right_upper_arm_strain) (local "/wholeBodyDynamics/r_arm_ft_sensor") (remote "/icubSim/right_arm/analog:o") (robotName iCubGazeboV3)
[WARNING] |yarp.device.analogsensorclient| The 'inertial' device is deprecated in favour of 'multipleanalogsensorsclient'
[WARNING] |yarp.device.analogsensorclient| The old device is no longer supported, and it will be deprecated in YARP 3.7 and removed in YARP 4.
[WARNING] |yarp.device.analogsensorclient| Please update your scripts.
[INFO] |yarp.os.Port|/wholeBodyDynamics/r_arm_ft_sensor| Port /wholeBodyDynamics/r_arm_ft_sensor active at tcp://10.240.9.207:10078/
[INFO] |yarp.os.Port|/wholeBodyDynamics/r_arm_ft_sensor/rpc:o| Port /wholeBodyDynamics/r_arm_ft_sensor/rpc:o active at tcp://10.240.9.207:10079/
[INFO] |yarp.os.impl.PortCoreInputUnit|/wholeBodyDynamics/r_arm_ft_sensor| Receiving input from /icubSim/right_arm/analog:o to /wholeBodyDynamics/r_arm_ft_sensor using udp
[INFO] |yarp.os.impl.PortCoreOutputUnit|/wholeBodyDynamics/r_arm_ft_sensor/rpc:o| Sending output from /wholeBodyDynamics/r_arm_ft_sensor/rpc:o to /icubSim/right_arm/analog:o/rpc:i using tcp
[INFO] |yarp.dev.PolyDriver|right_upper_arm_strain| Created device <analogsensorclient>. See C++ class AnalogSensorClient for documentation.
[INFO] Opening device left_lower_leg_front_strain with parameters [("robotName" = "iCubGazeboV3"), ("remote" = "/icubSim/left_foot_front/analog:o"), ("local" = "/wholeBodyDynamics/l_foot_front_ft_sensor")]
[DEBUG] |yarp.dev.PolyDriver|left_lower_leg_front_strain| Parameters are (device analogsensorclient) (id left_lower_leg_front_strain) (local "/wholeBodyDynamics/l_foot_front_ft_sensor") (remote "/icubSim/left_foot_front/analog:o") (robotName iCubGazeboV3)
[WARNING] |yarp.device.analogsensorclient| The 'inertial' device is deprecated in favour of 'multipleanalogsensorsclient'
[WARNING] |yarp.device.analogsensorclient| The old device is no longer supported, and it will be deprecated in YARP 3.7 and removed in YARP 4.
[WARNING] |yarp.device.analogsensorclient| Please update your scripts.
[INFO] |yarp.os.Port|/wholeBodyDynamics/l_foot_front_ft_sensor| Port /wholeBodyDynamics/l_foot_front_ft_sensor active at tcp://10.240.9.207:10080/
[INFO] |yarp.os.Port|/wholeBodyDynamics/l_foot_front_ft_sensor/rpc:o| Port /wholeBodyDynamics/l_foot_front_ft_sensor/rpc:o active at tcp://10.240.9.207:10081/
[INFO] |yarp.os.impl.PortCoreInputUnit|/wholeBodyDynamics/l_foot_front_ft_sensor| Receiving input from /icubSim/left_foot_front/analog:o to /wholeBodyDynamics/l_foot_front_ft_sensor using udp
[INFO] |yarp.os.impl.PortCoreOutputUnit|/wholeBodyDynamics/l_foot_front_ft_sensor/rpc:o| Sending output from /wholeBodyDynamics/l_foot_front_ft_sensor/rpc:o to /icubSim/left_foot_front/analog:o/rpc:i using tcp
[INFO] |yarp.dev.PolyDriver|left_lower_leg_front_strain| Created device <analogsensorclient>. See C++ class AnalogSensorClient for documentation.
[INFO] Opening device left_lower_leg_rear_strain with parameters [("robotName" = "iCubGazeboV3"), ("remote" = "/icubSim/left_foot_rear/analog:o"), ("local" = "/wholeBodyDynamics/l_foot_rear_ft_sensor")]
[DEBUG] |yarp.dev.PolyDriver|left_lower_leg_rear_strain| Parameters are (device analogsensorclient) (id left_lower_leg_rear_strain) (local "/wholeBodyDynamics/l_foot_rear_ft_sensor") (remote "/icubSim/left_foot_rear/analog:o") (robotName iCubGazeboV3)
[WARNING] |yarp.device.analogsensorclient| The 'inertial' device is deprecated in favour of 'multipleanalogsensorsclient'
[WARNING] |yarp.device.analogsensorclient| The old device is no longer supported, and it will be deprecated in YARP 3.7 and removed in YARP 4.
[WARNING] |yarp.device.analogsensorclient| Please update your scripts.
[INFO] |yarp.os.Port|/wholeBodyDynamics/l_foot_rear_ft_sensor| Port /wholeBodyDynamics/l_foot_rear_ft_sensor active at tcp://10.240.9.207:10082/
[INFO] |yarp.os.Port|/wholeBodyDynamics/l_foot_rear_ft_sensor/rpc:o| Port /wholeBodyDynamics/l_foot_rear_ft_sensor/rpc:o active at tcp://10.240.9.207:10083/
[INFO] |yarp.os.impl.PortCoreInputUnit|/wholeBodyDynamics/l_foot_rear_ft_sensor| Receiving input from /icubSim/left_foot_rear/analog:o to /wholeBodyDynamics/l_foot_rear_ft_sensor using udp
[INFO] |yarp.os.impl.PortCoreOutputUnit|/wholeBodyDynamics/l_foot_rear_ft_sensor/rpc:o| Sending output from /wholeBodyDynamics/l_foot_rear_ft_sensor/rpc:o to /icubSim/left_foot_rear/analog:o/rpc:i using tcp
[INFO] |yarp.dev.PolyDriver|left_lower_leg_rear_strain| Created device <analogsensorclient>. See C++ class AnalogSensorClient for documentation.
[INFO] Opening device left_upper_leg_strain with parameters [("robotName" = "iCubGazeboV3"), ("remote" = "/icubSim/left_leg/analog:o"), ("local" = "/wholeBodyDynamics/l_leg_ft_sensor")]
[DEBUG] |yarp.dev.PolyDriver|left_upper_leg_strain| Parameters are (device analogsensorclient) (id left_upper_leg_strain) (local "/wholeBodyDynamics/l_leg_ft_sensor") (remote "/icubSim/left_leg/analog:o") (robotName iCubGazeboV3)
[WARNING] |yarp.device.analogsensorclient| The 'inertial' device is deprecated in favour of 'multipleanalogsensorsclient'
[WARNING] |yarp.device.analogsensorclient| The old device is no longer supported, and it will be deprecated in YARP 3.7 and removed in YARP 4.
[WARNING] |yarp.device.analogsensorclient| Please update your scripts.
[INFO] |yarp.os.Port|/wholeBodyDynamics/l_leg_ft_sensor| Port /wholeBodyDynamics/l_leg_ft_sensor active at tcp://10.240.9.207:10084/
[INFO] |yarp.os.Port|/wholeBodyDynamics/l_leg_ft_sensor/rpc:o| Port /wholeBodyDynamics/l_leg_ft_sensor/rpc:o active at tcp://10.240.9.207:10085/
[INFO] |yarp.os.impl.PortCoreInputUnit|/wholeBodyDynamics/l_leg_ft_sensor| Receiving input from /icubSim/left_leg/analog:o to /wholeBodyDynamics/l_leg_ft_sensor using udp
[INFO] |yarp.os.impl.PortCoreOutputUnit|/wholeBodyDynamics/l_leg_ft_sensor/rpc:o| Sending output from /wholeBodyDynamics/l_leg_ft_sensor/rpc:o to /icubSim/left_leg/analog:o/rpc:i using tcp
[INFO] |yarp.dev.PolyDriver|left_upper_leg_strain| Created device <analogsensorclient>. See C++ class AnalogSensorClient for documentation.
[INFO] Opening device right_lower_leg_front_strain with parameters [("robotName" = "iCubGazeboV3"), ("remote" = "/icubSim/right_foot_front/analog:o"), ("local" = "/wholeBodyDynamics/r_foot_front_ft_sensor")]
[DEBUG] |yarp.dev.PolyDriver|right_lower_leg_front_strain| Parameters are (device analogsensorclient) (id right_lower_leg_front_strain) (local "/wholeBodyDynamics/r_foot_front_ft_sensor") (remote "/icubSim/right_foot_front/analog:o") (robotName iCubGazeboV3)
[WARNING] |yarp.device.analogsensorclient| The 'inertial' device is deprecated in favour of 'multipleanalogsensorsclient'
[WARNING] |yarp.device.analogsensorclient| The old device is no longer supported, and it will be deprecated in YARP 3.7 and removed in YARP 4.
[WARNING] |yarp.device.analogsensorclient| Please update your scripts.
[INFO] |yarp.os.Port|/wholeBodyDynamics/r_foot_front_ft_sensor| Port /wholeBodyDynamics/r_foot_front_ft_sensor active at tcp://10.240.9.207:10086/
[INFO] |yarp.os.Port|/wholeBodyDynamics/r_foot_front_ft_sensor/rpc:o| Port /wholeBodyDynamics/r_foot_front_ft_sensor/rpc:o active at tcp://10.240.9.207:10087/
[INFO] |yarp.os.impl.PortCoreInputUnit|/wholeBodyDynamics/r_foot_front_ft_sensor| Receiving input from /icubSim/right_foot_front/analog:o to /wholeBodyDynamics/r_foot_front_ft_sensor using udp
[INFO] |yarp.os.impl.PortCoreOutputUnit|/wholeBodyDynamics/r_foot_front_ft_sensor/rpc:o| Sending output from /wholeBodyDynamics/r_foot_front_ft_sensor/rpc:o to /icubSim/right_foot_front/analog:o/rpc:i using tcp
[INFO] |yarp.dev.PolyDriver|right_lower_leg_front_strain| Created device <analogsensorclient>. See C++ class AnalogSensorClient for documentation.
[INFO] Opening device right_lower_leg_rear_strain with parameters [("robotName" = "iCubGazeboV3"), ("remote" = "/icubSim/right_foot_rear/analog:o"), ("local" = "/wholeBodyDynamics/r_foot_rear_ft_sensor")]
[DEBUG] |yarp.dev.PolyDriver|right_lower_leg_rear_strain| Parameters are (device analogsensorclient) (id right_lower_leg_rear_strain) (local "/wholeBodyDynamics/r_foot_rear_ft_sensor") (remote "/icubSim/right_foot_rear/analog:o") (robotName iCubGazeboV3)
[WARNING] |yarp.device.analogsensorclient| The 'inertial' device is deprecated in favour of 'multipleanalogsensorsclient'
[WARNING] |yarp.device.analogsensorclient| The old device is no longer supported, and it will be deprecated in YARP 3.7 and removed in YARP 4.
[WARNING] |yarp.device.analogsensorclient| Please update your scripts.
[INFO] |yarp.os.Port|/wholeBodyDynamics/r_foot_rear_ft_sensor| Port /wholeBodyDynamics/r_foot_rear_ft_sensor active at tcp://10.240.9.207:10088/
[INFO] |yarp.os.Port|/wholeBodyDynamics/r_foot_rear_ft_sensor/rpc:o| Port /wholeBodyDynamics/r_foot_rear_ft_sensor/rpc:o active at tcp://10.240.9.207:10089/
[INFO] |yarp.os.impl.PortCoreInputUnit|/wholeBodyDynamics/r_foot_rear_ft_sensor| Receiving input from /icubSim/right_foot_rear/analog:o to /wholeBodyDynamics/r_foot_rear_ft_sensor using udp
[INFO] |yarp.os.impl.PortCoreOutputUnit|/wholeBodyDynamics/r_foot_rear_ft_sensor/rpc:o| Sending output from /wholeBodyDynamics/r_foot_rear_ft_sensor/rpc:o to /icubSim/right_foot_rear/analog:o/rpc:i using tcp
[INFO] |yarp.dev.PolyDriver|right_lower_leg_rear_strain| Created device <analogsensorclient>. See C++ class AnalogSensorClient for documentation.
[INFO] Opening device right_upper_leg_strain with parameters [("robotName" = "iCubGazeboV3"), ("remote" = "/icubSim/right_leg/analog:o"), ("local" = "/wholeBodyDynamics/r_leg_ft_sensor")]
[DEBUG] |yarp.dev.PolyDriver|right_upper_leg_strain| Parameters are (device analogsensorclient) (id right_upper_leg_strain) (local "/wholeBodyDynamics/r_leg_ft_sensor") (remote "/icubSim/right_leg/analog:o") (robotName iCubGazeboV3)
[WARNING] |yarp.device.analogsensorclient| The 'inertial' device is deprecated in favour of 'multipleanalogsensorsclient'
[WARNING] |yarp.device.analogsensorclient| The old device is no longer supported, and it will be deprecated in YARP 3.7 and removed in YARP 4.
[WARNING] |yarp.device.analogsensorclient| Please update your scripts.
[INFO] |yarp.os.Port|/wholeBodyDynamics/r_leg_ft_sensor| Port /wholeBodyDynamics/r_leg_ft_sensor active at tcp://10.240.9.207:10090/
[INFO] |yarp.os.Port|/wholeBodyDynamics/r_leg_ft_sensor/rpc:o| Port /wholeBodyDynamics/r_leg_ft_sensor/rpc:o active at tcp://10.240.9.207:10091/
[INFO] |yarp.os.impl.PortCoreInputUnit|/wholeBodyDynamics/r_leg_ft_sensor| Receiving input from /icubSim/right_leg/analog:o to /wholeBodyDynamics/r_leg_ft_sensor using udp
[INFO] |yarp.os.impl.PortCoreOutputUnit|/wholeBodyDynamics/r_leg_ft_sensor/rpc:o| Sending output from /wholeBodyDynamics/r_leg_ft_sensor/rpc:o to /icubSim/right_leg/analog:o/rpc:i using tcp
[INFO] |yarp.dev.PolyDriver|right_upper_leg_strain| Created device <analogsensorclient>. See C++ class AnalogSensorClient for documentation.
[INFO] Opening device wholebodydynamics with parameters [("robotName" = "iCubGazeboV3"), ("axesNames" = "(torso_pitch,torso_roll,torso_yaw, neck_pitch, neck_roll,neck_yaw, l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,l_elbow,l_wrist_prosup,l_wrist_pitch,l_wrist_yaw, r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,r_elbow,r_wrist_prosup,r_wrist_pitch,r_wrist_yaw, l_hip_pitch,l_hip_roll,l_hip_yaw,l_knee,l_ankle_pitch,l_ankle_roll, r_hip_pitch,r_hip_roll,r_hip_yaw,r_knee,r_ankle_pitch,r_ankle_roll)"), ("modelFile" = "model.urdf"), ("fixedFrameGravity" = "(0,0,-9.81)"), ("defaultContactFrames" = "(l_hand,r_hand,root_link,l_upper_leg,l_foot_front,l_foot_rear,r_upper_leg,r_foot_front,r_foot_rear)"), ("imuFrameName" = "imu_frame"), ("useJointVelocity" = "true"), ("useJointAcceleration" = "true"), ("imuFilterCutoffInHz" = "3.0"), ("forceTorqueFilterCutoffInHz" = "2.0"), ("jointVelFilterCutoffInHz" = "3.0"), ("jointAccFilterCutoffInHz" = "3.0"), ("startWithZeroFTSensorOffsets" = "true"), ("useSkinForContacts" = "false"), ("publishNetExternalWrenches" = "true"), ("devicePeriodInSeconds" = "0.01"), ("estimateJointVelocityAcceleration" = "true"), ("processNoiseCovariance" = "(1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1)"), ("measurementNoiseCovariance" = "(1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3)"), ("stateCovariance" = "(5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1)"), ("GRAVITY_COMPENSATION" [group] = "(enableGravityCompensation true) (gravityCompensationBaseLink root_link) (gravityCompensationAxesNames (torso_pitch,torso_roll,torso_yaw,neck_pitch,neck_roll,neck_yaw,l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,l_elbow,r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,r_elbow))"), ("WBD_OUTPUT_EXTERNAL_WRENCH_PORTS" [group] = "(/wholeBodyDynamics/left_arm/cartesianEndEffectorWrench:o (l_hand,l_hand,root_link)) (/wholeBodyDynamics/right_arm/cartesianEndEffectorWrench:o (r_hand,r_hand,root_link)) (/wholeBodyDynamics/left_upper_leg/cartesianEndEffectorWrench:o (l_upper_leg,l_upper_leg,root_link)) (/wholeBodyDynamics/right_upper_leg/cartesianEndEffectorWrench:o (r_upper_leg,r_upper_leg,root_link)) (/wholeBodyDynamics/left_foot_front/cartesianEndEffectorWrench:o (l_foot_front,l_sole,l_sole)) (/wholeBodyDynamics/left_foot_rear/cartesianEndEffectorWrench:o (l_foot_rear,l_sole,l_sole)) (/wholeBodyDynamics/right_foot_front/cartesianEndEffectorWrench:o (r_foot_front,r_sole,r_sole)) (/wholeBodyDynamics/right_foot_rear/cartesianEndEffectorWrench:o (r_foot_rear,r_sole,r_sole))")]
[DEBUG] |yarp.dev.PolyDriver|wholebodydynamics| Parameters are (GRAVITY_COMPENSATION (enableGravityCompensation true) (gravityCompensationBaseLink root_link) (gravityCompensationAxesNames (torso_pitch torso_roll torso_yaw neck_pitch neck_roll neck_yaw l_shoulder_pitch l_shoulder_roll l_shoulder_yaw l_elbow r_shoulder_pitch r_shoulder_roll r_shoulder_yaw r_elbow))) (WBD_OUTPUT_EXTERNAL_WRENCH_PORTS ("/wholeBodyDynamics/left_arm/cartesianEndEffectorWrench:o" (l_hand l_hand root_link)) ("/wholeBodyDynamics/right_arm/cartesianEndEffectorWrench:o" (r_hand r_hand root_link)) ("/wholeBodyDynamics/left_upper_leg/cartesianEndEffectorWrench:o" (l_upper_leg l_upper_leg root_link)) ("/wholeBodyDynamics/right_upper_leg/cartesianEndEffectorWrench:o" (r_upper_leg r_upper_leg root_link)) ("/wholeBodyDynamics/left_foot_front/cartesianEndEffectorWrench:o" (l_foot_front l_sole l_sole)) ("/wholeBodyDynamics/left_foot_rear/cartesianEndEffectorWrench:o" (l_foot_rear l_sole l_sole)) ("/wholeBodyDynamics/right_foot_front/cartesianEndEffectorWrench:o" (r_foot_front r_sole r_sole)) ("/wholeBodyDynamics/right_foot_rear/cartesianEndEffectorWrench:o" (r_foot_rear r_sole r_sole))) (axesNames (torso_pitch torso_roll torso_yaw neck_pitch neck_roll neck_yaw l_shoulder_pitch l_shoulder_roll l_shoulder_yaw l_elbow l_wrist_prosup l_wrist_pitch l_wrist_yaw r_shoulder_pitch r_shoulder_roll r_shoulder_yaw r_elbow r_wrist_prosup r_wrist_pitch r_wrist_yaw l_hip_pitch l_hip_roll l_hip_yaw l_knee l_ankle_pitch l_ankle_roll r_hip_pitch r_hip_roll r_hip_yaw r_knee r_ankle_pitch r_ankle_roll)) (defaultContactFrames (l_hand r_hand root_link l_upper_leg l_foot_front l_foot_rear r_upper_leg r_foot_front r_foot_rear)) (device wholebodydynamics) (devicePeriodInSeconds 0.0100000000000000002082) (estimateJointVelocityAcceleration true) (fixedFrameGravity (0 0 -9.81000000000000049738)) (forceTorqueFilterCutoffInHz 2.0) (id wholebodydynamics) (imuFilterCutoffInHz 3.0) (imuFrameName imu_frame) (jointAccFilterCutoffInHz 3.0) (jointVelFilterCutoffInHz 3.0) (measurementNoiseCovariance (0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082)) (modelFile model.urdf) (processNoiseCovariance (0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 10.0 10.0 10.0 10.0 10.0 10.0 10.0 10.0 10.0 10.0 10.0 10.0 10.0 10.0 10.0 10.0 10.0 10.0 10.0 10.0 10.0 10.0 10.0 10.0 10.0 10.0 10.0 10.0 10.0 10.0 10.0 10.0)) (publishNetExternalWrenches true) (robotName iCubGazeboV3) (startWithZeroFTSensorOffsets true) (stateCovariance (0.000500000000000000010408 0.000500000000000000010408 0.000500000000000000010408 0.000500000000000000010408 0.000500000000000000010408 0.000500000000000000010408 0.000500000000000000010408 0.000500000000000000010408 0.000500000000000000010408 0.000500000000000000010408 0.000500000000000000010408 0.000500000000000000010408 0.000500000000000000010408 0.000500000000000000010408 0.000500000000000000010408 0.000500000000000000010408 0.000500000000000000010408 0.000500000000000000010408 0.000500000000000000010408 0.000500000000000000010408 0.000500000000000000010408 0.000500000000000000010408 0.000500000000000000010408 0.000500000000000000010408 0.000500000000000000010408 0.000500000000000000010408 0.000500000000000000010408 0.000500000000000000010408 0.000500000000000000010408 0.000500000000000000010408 0.000500000000000000010408 0.000500000000000000010408 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.00100000000000000002082 0.100000000000000005551 0.100000000000000005551 0.100000000000000005551 0.100000000000000005551 0.100000000000000005551 0.100000000000000005551 0.100000000000000005551 0.100000000000000005551 0.100000000000000005551 0.100000000000000005551 0.100000000000000005551 0.100000000000000005551 0.100000000000000005551 0.100000000000000005551 0.100000000000000005551 0.100000000000000005551 0.100000000000000005551 0.100000000000000005551 0.100000000000000005551 0.100000000000000005551 0.100000000000000005551 0.100000000000000005551 0.100000000000000005551 0.100000000000000005551 0.100000000000000005551 0.100000000000000005551 0.100000000000000005551 0.100000000000000005551 0.100000000000000005551 0.100000000000000005551 0.100000000000000005551 0.100000000000000005551)) (useJointAcceleration true) (useJointVelocity true) (useSkinForContacts false)
[DEBUG] wholeBodyDynamics Statistics: Opening estimator.
[INFO] wholeBodyDynamics : Loading model from  /home/gromualdi/robot-code/robotology-superbuild/build/install/share/iCub/robots/iCubGazeboV3/model.urdf
[DEBUG] wholeBodyDynamics Statistics: Estimator opened in  0.0240269 s.
[DEBUG] wholeBodyDynamics Statistics: Loading settings from configuration files.
[INFO] wholeBodyDynamics : Using the following filter cutoff frequencies,
[INFO] wholeBodyDynamics: imuFilterCutoffInHz:  3  Hz.
[INFO] wholeBodyDynamics: forceTorqueFilterCutoffInHz:  2  Hz.
[INFO] wholeBodyDynamics: jointVelFilterCutoffInHz:  3  Hz.
[INFO] wholeBodyDynamics: jointAccFilterCutoffInHz:  3  Hz.
[INFO] wholeBodyDynamics: setting startWithZeroFTSensorOffsets was set to true , FT sensor offsets will be automatically reset to zero.
[DEBUG] wholeBodyDynamics Statistics: Settings loaded in  0.0279691 s.
[DEBUG] wholeBodyDynamics Statistics: Opening RPC port.
[INFO] |yarp.os.Port|/wholeBodyDynamics/rpc| Port /wholeBodyDynamics/rpc active at tcp://10.240.9.207:10092/
[DEBUG] wholeBodyDynamics Statistics: RPC port opened in  0.017766 s.
[DEBUG] wholeBodyDynamics Statistics: Opening settings port.
[INFO] |yarp.os.Port|/wholeBodyDynamics/settings| Port /wholeBodyDynamics/settings active at tcp://10.240.9.207:10093/
[DEBUG] wholeBodyDynamics Statistics: Settings port opened in  0.0205419 s.
[DEBUG] wholeBodyDynamics Statistics: Opening remapper control board.
[DEBUG] |yarp.dev.PolyDriver|controlboardremapper| Parameters are (axesNames (torso_pitch torso_roll torso_yaw neck_pitch neck_roll neck_yaw l_shoulder_pitch l_shoulder_roll l_shoulder_yaw l_elbow l_wrist_prosup l_wrist_pitch l_wrist_yaw r_shoulder_pitch r_shoulder_roll r_shoulder_yaw r_elbow r_wrist_prosup r_wrist_pitch r_wrist_yaw l_hip_pitch l_hip_roll l_hip_yaw l_knee l_ankle_pitch l_ankle_roll r_hip_pitch r_hip_roll r_hip_yaw r_knee r_ankle_pitch r_ankle_roll)) (device controlboardremapper)
[INFO] |yarp.dev.PolyDriver|controlboardremapper| Created device <controlboardremapper>. See C++ class ControlBoardRemapper for documentation.
[DEBUG] wholeBodyDynamics Statistics: Remapper control board opened in  0.0740056 s.
[DEBUG] wholeBodyDynamics Statistics: Opening remapper virtual sensors.
[DEBUG] |yarp.dev.PolyDriver|virtualAnalogRemapper| Parameters are (axesNames (torso_pitch torso_roll torso_yaw neck_pitch neck_roll neck_yaw l_shoulder_pitch l_shoulder_roll l_shoulder_yaw l_elbow l_wrist_prosup l_wrist_pitch l_wrist_yaw r_shoulder_pitch r_shoulder_roll r_shoulder_yaw r_elbow r_wrist_prosup r_wrist_pitch r_wrist_yaw l_hip_pitch l_hip_roll l_hip_yaw l_knee l_ankle_pitch l_ankle_roll r_hip_pitch r_hip_roll r_hip_yaw r_knee r_ankle_pitch r_ankle_roll)) (device virtualAnalogRemapper)
[INFO] |yarp.dev.PolyDriver|virtualAnalogRemapper| Created device <virtualAnalogRemapper>. See C++ class yarp::dev::VirtualAnalogRemapper for documentation.
[DEBUG] wholeBodyDynamics Statistics: Remapper virtual sensors opened in  0.081136 s.
[DEBUG] wholeBodyDynamics Statistics: Opening contact frames.
[DEBUG] wholeBodyDynamics Statistics: Contact frames opened in  0.00633693 s.
[DEBUG] wholeBodyDynamics Statistics: Opening external wrenches ports.
[INFO] |yarp.os.Port|/wholeBodyDynamics/left_arm/cartesianEndEffectorWrench:o| Port /wholeBodyDynamics/left_arm/cartesianEndEffectorWrench:o active at tcp://10.240.9.207:10094/
[INFO] |yarp.os.Port|/wholeBodyDynamics/right_arm/cartesianEndEffectorWrench:o| Port /wholeBodyDynamics/right_arm/cartesianEndEffectorWrench:o active at tcp://10.240.9.207:10095/
[INFO] |yarp.os.Port|/wholeBodyDynamics/left_upper_leg/cartesianEndEffectorWrench:o| Port /wholeBodyDynamics/left_upper_leg/cartesianEndEffectorWrench:o active at tcp://10.240.9.207:10096/
[INFO] |yarp.os.Port|/wholeBodyDynamics/right_upper_leg/cartesianEndEffectorWrench:o| Port /wholeBodyDynamics/right_upper_leg/cartesianEndEffectorWrench:o active at tcp://10.240.9.207:10097/
[INFO] |yarp.os.Port|/wholeBodyDynamics/left_foot_front/cartesianEndEffectorWrench:o| Port /wholeBodyDynamics/left_foot_front/cartesianEndEffectorWrench:o active at tcp://10.240.9.207:10098/
[INFO] |yarp.os.Port|/wholeBodyDynamics/left_foot_rear/cartesianEndEffectorWrench:o| Port /wholeBodyDynamics/left_foot_rear/cartesianEndEffectorWrench:o active at tcp://10.240.9.207:10099/
[INFO] |yarp.os.Port|/wholeBodyDynamics/right_foot_front/cartesianEndEffectorWrench:o| Port /wholeBodyDynamics/right_foot_front/cartesianEndEffectorWrench:o active at tcp://10.240.9.207:10100/
[INFO] |yarp.os.Port|/wholeBodyDynamics/right_foot_rear/cartesianEndEffectorWrench:o| Port /wholeBodyDynamics/right_foot_rear/cartesianEndEffectorWrench:o active at tcp://10.240.9.207:10101/
[INFO] |yarp.os.Port|/wholeBodyDynamics/netExternalWrenches:o| Port /wholeBodyDynamics/netExternalWrenches:o active at tcp://10.240.9.207:10102/
[DEBUG] wholeBodyDynamics Statistics: External port wrenches opened in  0.14664 s.
[DEBUG] wholeBodyDynamics Statistics: Opening multiple analog sensors remapper.
[DEBUG] |yarp.dev.PolyDriver|multipleanalogsensorsremapper| Parameters are (device multipleanalogsensorsremapper)
[INFO] |yarp.dev.PolyDriver|multipleanalogsensorsremapper| Created device <multipleanalogsensorsremapper>. See C++ class MultipleAnalogSensorsRemapper for documentation.
[DEBUG] wholeBodyDynamics Statistics: Multiple analog sensors remapper opened in  0.0741644 s.
[DEBUG] wholeBodyDynamics Statistics: Configuration finished. Waiting attachAll to be called.
[INFO] |yarp.dev.PolyDriver|wholebodydynamics| Created device <wholebodydynamics>. See C++ class yarp::dev::WholeBodyDynamicsDevice for documentation.
[INFO] Entering action level 15 of phase startup
[INFO] Executing attach action, level 15 on device wholebodydynamics with parameters [("networks" = "(left_leg right_leg torso right_arm left_arm head imu l_arm_ft_sensor r_arm_ft_sensor l_leg_ft_sensor l_foot_front_ft_sensor l_foot_rear_ft_sensor r_leg_ft_sensor r_foot_front_ft_sensor r_foot_rear_ft_sensor)"), ("left_leg" = "left_leg_mc"), ("right_leg" = "right_leg_mc"), ("torso" = "torso_mc"), ("right_arm" = "right_arm_mc"), ("left_arm" = "left_arm_mc"), ("head" = "head_mc"), ("imu" = "inertial"), ("l_arm_ft_sensor" = "left_upper_arm_strain"), ("r_arm_ft_sensor" = "right_upper_arm_strain"), ("l_leg_ft_sensor" = "left_upper_leg_strain"), ("l_foot_front_ft_sensor" = "left_lower_leg_front_strain"), ("l_foot_rear_ft_sensor" = "left_lower_leg_rear_strain"), ("r_leg_ft_sensor" = "right_upper_leg_strain"), ("r_foot_front_ft_sensor" = "right_lower_leg_front_strain"), ("r_foot_rear_ft_sensor" = "right_lower_leg_rear_strain")]
[DEBUG] wholeBodyDynamics Statistics: attachAll started. Attaching all control board.
[DEBUG] wholeBodyDynamics Statistics: Attaching all control board took  0.0160208 s.
[DEBUG] wholeBodyDynamics Statistics: Attaching all virtual analog sensors.
[DEBUG] wholeBodyDynamics Statistics: Attaching all virtual analog sensors took  3.45707e-05 s.
[DEBUG] wholeBodyDynamics Statistics: Attaching all FTs.
[INFO] Starting attach MAS and analog ft
[DEBUG] wholeBodyDynamicsDevice :: number of ft sensors found in both ft + mas 8 where analog are  8  and mas are  0
[DEBUG] wholeBodyDynamics Statistics: Attaching all Fts took  0.000161409 s.
[DEBUG] wholeBodyDynamics Statistics: Attaching all IMUs.
[DEBUG] wholeBodyDynamics Statistics: Attaching all IMUs took  3.12328e-05 s.
[DEBUG] wholeBodyDynamics Statistics: Calibrating offsets.
[DEBUG] wholeBodyDynamics Statistics: Calibrating took  1.14441e-05 s.
[DEBUG] wholeBodyDynamics Statistics: Starting
[INFO] All actions for action level 15 of startup phase started. Waiting for unfinished actions.
[INFO] All actions for action level 15 of startup phase finished.
[INFO] startup phase finished.
[INFO] run phase starting...
[DEBUG] yarprobotinterface running happily

@GiulioRomualdi
Copy link
Member Author

I've just noticed that I've the same problem with iCubGazeboV2_5 so I will rename the issue

@GiulioRomualdi GiulioRomualdi changed the title [iCubGazeboV3] Wrong estimated external wrenches [iCubGazeboV3 - iCubGazeboV2_5] Wrong estimated external wrenches Aug 30, 2022
@GiulioRomualdi
Copy link
Member Author

Since the version v0.7.0 is working without any problem I played with git bisect This is the first breaking commit 9bd947c

commit 9bd947c6477e686b3b0bba435b4ef64a74bfd88a
Author: Carlotta Sartore <56030908+CarlottaSartore@users.noreply.github.com>
Date:   Fri Jul 29 11:37:19 2022 +0200

    Introduce MAS Read in whole body estimator  (#153)
    
    * wholeBodyDynamicsDevice: Read from MAS FT sensors
    
    * WIP fix segfaults - to check valid measures
    
    * Fix bugs in reading from MAS interface
    
    * Remove stray commented line
    
    * Update CHANGELOG
    
    Co-authored-by: Prashanth <prashanth.ramadoss@iit.it>
    Co-authored-by: iCubGenova09 <icubgenova09.user@example.com>

 CHANGELOG.md                                       |   1 +
 .../wholeBodyDynamics/WholeBodyDynamicsDevice.cpp  | 103 ++++++++++++++++-----
 .../wholeBodyDynamics/WholeBodyDynamicsDevice.h    |   7 ++

@traversaro
Copy link
Member

My intuition is that there is something wrong in the ft serialization, we had a similar problem (if I recall correctly) when debugging https://github.com/robotology/whole-body-estimators/pull/153/files in the first place.

@traversaro
Copy link
Member

In the iCubGazeboV2_5 test did you changed the YARP_ROBOT_NAME ?

@traversaro
Copy link
Member

traversaro commented Aug 30, 2022 •
edited
Loading

A possible bug is in

whole-body-estimators/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp

Line 2058 in 63785f1

auto ftID = std::distance(ftMultipleAnalogSensorNames.begin(), ftMasITer);
, that should be:

auto ftID = std::distance(ftAnalogSensorNames.begin(), ftAnalogIter);

instead.

traversaro added a commit that referenced this issue Aug 30, 2022
@traversaro
Fix for #156
1f8aa9b
@traversaro
Copy link
Member

PR: #157 . @GiulioRomualdi

HosameldinMohamed pushed a commit that referenced this issue Aug 31, 2022
@traversaro
Fix for #156 (#157)
3bf3f03 
* Fix for #156

* Update WholeBodyDynamicsDevice.cpp

* Update CHANGELOG.md

* Update CHANGELOG.md
@GiulioRomualdi
Copy link
Member Author

Fixed in #157 thank you @traversaro

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Assignees
No one assigned
Labels
None yet
Projects
None yet
Milestone
No milestone
Development

No branches or pull requests
2 participants
@traversaro @GiulioRomualdi