search

icub-tech-iit / ergocub-software

Main collector of ergoCub specific SW
https://icub-tech-iit.github.io/ergocub-software/
BSD 3-Clause "New" or "Revised" License
13 stars 18 forks source link

Unable to launch `launch_wholebodydynamics_ecub.xml` in simulation #246

Closed SimoneMic closed 5 months ago

SimoneMic commented 5 months ago

Hello,

I am unable to launch the wholebodydynamics in gazebo, with the current robotology-superbuild and ergocub-software master branches.

While launching the YRI, I am getting this warning:

[WARNING] subModel no  0  has the maximum number of variables (6). The frame  r_hand_palm  will not be added to it.
[WARNING] subModel no  0  has the maximum number of variables (6). The frame  root_link  will not be added to it.
[WARNING] subModel no  0  has the maximum number of variables (6). The frame  l_foot_front  will not be added to it.
[WARNING] subModel no  0  has the maximum number of variables (6). The frame  l_foot_rear  will not be added to it.
[WARNING] subModel no  0  has the maximum number of variables (6). The frame  r_foot_front  will not be added to it.
[WARNING] subModel no  0  has the maximum number of variables (6). The frame  r_foot_rear  will not be added to it.
[WARNING] subModel no  0  has the maximum number of variables (6). The frame  l_upper_leg  will not be added to it.
[WARNING] subModel no  0  has the maximum number of variables (6). The frame  r_upper_leg  will not be added to it.

And I think that it leads to this following error:

[ERROR] Model :: getLinkIndex : Impossible to find link l_foot_front in the Model
[ERROR] wholeBodyDynamics : Link  l_foot_front  not found in the model.
[ERROR] wholeBodyDynamics: Problem in opening external wrenches port.
[ERROR] |yarp.dev.PolyDriver|wholebodydynamics| Driver <wholebodydynamics> was found but could not open

Hover, the frames are correctly present in the urdf, and the YARP_ROBOT_NAME variable is properly set.

I have exactly no clue on what could be the solution here.

Here's the full log for completion:

ecub_docker@simone-Alienware-m16-R1:~$ launch_wbd_interface 
[ERROR] |yarp.os.Property| cannot read from yarprobotinterface.ini
[DEBUG] Reading file /home/ecub_docker/robotology-superbuild/src/ergocub-software/urdf/ergoCub/conf/launch_wholebodydynamics_ecub.xml
[WARNING] Invalid syntax while loading /home/ecub_docker/robotology-superbuild/src/ergocub-software/urdf/ergoCub/conf/launch_wholebodydynamics_ecub.xml at line 1 . Expected document of type robot . Found devices
[DEBUG] yarprobotinterface: using xml parser for DTD v3.x
[DEBUG] Reading file /home/ecub_docker/robotology-superbuild/src/ergocub-software/urdf/ergoCub/conf/launch_wholebodydynamics_ecub.xml
[INFO] Yarprobotinterface was started using the following enable_tags: 
[INFO] Yarprobotinterface was started using the following disable_tags: 
[DEBUG] List of all enable attributes found in the include tags: 
[DEBUG] List of all disable attributes found in the include tags: 
[DEBUG] Preprocessor complete in:  8.91685e-05 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|/ergoCubGazeboV1/yarprobotinterface| Port /ergoCubGazeboV1/yarprobotinterface active at tcp://10.0.3.1:10053/
[INFO] startup phase starting...
[INFO] Opening device torso_mc with parameters [("robotName" = "ergoCubGazeboV1"), ("remote" = "/ergocubSim/torso"), ("local" = "/wholeBodyDynamics/torso")]
[DEBUG] |yarp.dev.PolyDriver|torso_mc| Parameters are (device remote_controlboard) (id torso_mc) (local "/wholeBodyDynamics/torso") (remote "/ergocubSim/torso") (robotName ergoCubGazeboV1)
[INFO] |yarp.os.Port|/wholeBodyDynamics/torso/rpc:o| Port /wholeBodyDynamics/torso/rpc:o active at tcp://10.0.3.1:10054/
[INFO] |yarp.os.Port|/wholeBodyDynamics/torso/command:o| Port /wholeBodyDynamics/torso/command:o active at tcp://10.0.3.1:10055/
[INFO] |yarp.os.Port|/wholeBodyDynamics/torso/stateExt:i| Port /wholeBodyDynamics/torso/stateExt:i active at tcp://10.0.3.1:10056/
[INFO] |yarp.os.impl.PortCoreOutputUnit|/wholeBodyDynamics/torso/rpc:o| Sending output from /wholeBodyDynamics/torso/rpc:o to /ergocubSim/torso/rpc:i using tcp
[INFO] |yarp.os.impl.PortCoreOutputUnit|/wholeBodyDynamics/torso/command:o| Sending output from /wholeBodyDynamics/torso/command:o to /ergocubSim/torso/command:i using udp
[INFO] |yarp.os.impl.PortCoreInputUnit|/wholeBodyDynamics/torso/stateExt:i| Receiving input from /ergocubSim/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" = "ergoCubGazeboV1"), ("remote" = "/ergocubSim/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 "/ergocubSim/left_arm") (robotName ergoCubGazeboV1)
[INFO] |yarp.os.Port|/wholeBodyDynamics/left_arm/rpc:o| Port /wholeBodyDynamics/left_arm/rpc:o active at tcp://10.0.3.1:10057/
[INFO] |yarp.os.Port|/wholeBodyDynamics/left_arm/command:o| Port /wholeBodyDynamics/left_arm/command:o active at tcp://10.0.3.1:10058/
[INFO] |yarp.os.Port|/wholeBodyDynamics/left_arm/stateExt:i| Port /wholeBodyDynamics/left_arm/stateExt:i active at tcp://10.0.3.1:10059/
[INFO] |yarp.os.impl.PortCoreOutputUnit|/wholeBodyDynamics/left_arm/rpc:o| Sending output from /wholeBodyDynamics/left_arm/rpc:o to /ergocubSim/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 /ergocubSim/left_arm/command:i using udp
[INFO] |yarp.os.impl.PortCoreInputUnit|/wholeBodyDynamics/left_arm/stateExt:i| Receiving input from /ergocubSim/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" = "ergoCubGazeboV1"), ("remote" = "/ergocubSim/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 "/ergocubSim/right_arm") (robotName ergoCubGazeboV1)
[INFO] |yarp.os.Port|/wholeBodyDynamics/right_arm/rpc:o| Port /wholeBodyDynamics/right_arm/rpc:o active at tcp://10.0.3.1:10060/
[INFO] |yarp.os.Port|/wholeBodyDynamics/right_arm/command:o| Port /wholeBodyDynamics/right_arm/command:o active at tcp://10.0.3.1:10061/
[INFO] |yarp.os.Port|/wholeBodyDynamics/right_arm/stateExt:i| Port /wholeBodyDynamics/right_arm/stateExt:i active at tcp://10.0.3.1:10062/
[INFO] |yarp.os.impl.PortCoreOutputUnit|/wholeBodyDynamics/right_arm/rpc:o| Sending output from /wholeBodyDynamics/right_arm/rpc:o to /ergocubSim/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 /ergocubSim/right_arm/command:i using udp
[INFO] |yarp.os.impl.PortCoreInputUnit|/wholeBodyDynamics/right_arm/stateExt:i| Receiving input from /ergocubSim/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" = "ergoCubGazeboV1"), ("remote" = "/ergocubSim/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 "/ergocubSim/left_leg") (robotName ergoCubGazeboV1)
[INFO] |yarp.os.Port|/wholeBodyDynamics/left_leg/rpc:o| Port /wholeBodyDynamics/left_leg/rpc:o active at tcp://10.0.3.1:10063/
[INFO] |yarp.os.Port|/wholeBodyDynamics/left_leg/command:o| Port /wholeBodyDynamics/left_leg/command:o active at tcp://10.0.3.1:10064/
[INFO] |yarp.os.Port|/wholeBodyDynamics/left_leg/stateExt:i| Port /wholeBodyDynamics/left_leg/stateExt:i active at tcp://10.0.3.1:10065/
[INFO] |yarp.os.impl.PortCoreOutputUnit|/wholeBodyDynamics/left_leg/rpc:o| Sending output from /wholeBodyDynamics/left_leg/rpc:o to /ergocubSim/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 /ergocubSim/left_leg/command:i using udp
[INFO] |yarp.os.impl.PortCoreInputUnit|/wholeBodyDynamics/left_leg/stateExt:i| Receiving input from /ergocubSim/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" = "ergoCubGazeboV1"), ("remote" = "/ergocubSim/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 "/ergocubSim/right_leg") (robotName ergoCubGazeboV1)
[INFO] |yarp.os.Port|/wholeBodyDynamics/right_leg/rpc:o| Port /wholeBodyDynamics/right_leg/rpc:o active at tcp://10.0.3.1:10066/
[INFO] |yarp.os.Port|/wholeBodyDynamics/right_leg/command:o| Port /wholeBodyDynamics/right_leg/command:o active at tcp://10.0.3.1:10067/
[INFO] |yarp.os.Port|/wholeBodyDynamics/right_leg/stateExt:i| Port /wholeBodyDynamics/right_leg/stateExt:i active at tcp://10.0.3.1:10068/
[INFO] |yarp.os.impl.PortCoreOutputUnit|/wholeBodyDynamics/right_leg/rpc:o| Sending output from /wholeBodyDynamics/right_leg/rpc:o to /ergocubSim/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 /ergocubSim/right_leg/command:i using udp
[INFO] |yarp.os.impl.PortCoreInputUnit|/wholeBodyDynamics/right_leg/stateExt:i| Receiving input from /ergocubSim/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" = "ergoCubGazeboV1"), ("remote" = "/ergocubSim/head"), ("local" = "/wholeBodyDynamics/head")]
[DEBUG] |yarp.dev.PolyDriver|head_mc| Parameters are (device remote_controlboard) (id head_mc) (local "/wholeBodyDynamics/head") (remote "/ergocubSim/head") (robotName ergoCubGazeboV1)
[INFO] |yarp.os.Port|/wholeBodyDynamics/head/rpc:o| Port /wholeBodyDynamics/head/rpc:o active at tcp://10.0.3.1:10069/
[INFO] |yarp.os.Port|/wholeBodyDynamics/head/command:o| Port /wholeBodyDynamics/head/command:o active at tcp://10.0.3.1:10070/
[INFO] |yarp.os.Port|/wholeBodyDynamics/head/stateExt:i| Port /wholeBodyDynamics/head/stateExt:i active at tcp://10.0.3.1:10071/
[INFO] |yarp.os.impl.PortCoreOutputUnit|/wholeBodyDynamics/head/rpc:o| Sending output from /wholeBodyDynamics/head/rpc:o to /ergocubSim/head/rpc:i using tcp
[INFO] |yarp.os.impl.PortCoreOutputUnit|/wholeBodyDynamics/head/command:o| Sending output from /wholeBodyDynamics/head/command:o to /ergocubSim/head/command:i using udp
[INFO] |yarp.os.impl.PortCoreInputUnit|/wholeBodyDynamics/head/stateExt:i| Receiving input from /ergocubSim/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 head_inertial_hardware_device with parameters [("robotName" = "ergoCubGazeboV1"), ("remote" = "/ergocubSim/head/inertials"), ("local" = "/wholeBodyDynamics/imu")]
[DEBUG] |yarp.dev.PolyDriver|head_inertial_hardware_device| Parameters are (device multipleanalogsensorsclient) (id head_inertial_hardware_device) (local "/wholeBodyDynamics/imu") (remote "/ergocubSim/head/inertials") (robotName ergoCubGazeboV1)
[INFO] |yarp.os.Port|/wholeBodyDynamics/imu/rpc:i| Port /wholeBodyDynamics/imu/rpc:i active at tcp://10.0.3.1:10072/
[INFO] |yarp.os.Port|/wholeBodyDynamics/imu/measures:i| Port /wholeBodyDynamics/imu/measures:i active at tcp://10.0.3.1:10073/
[INFO] |yarp.os.impl.PortCoreOutputUnit|/wholeBodyDynamics/imu/rpc:i| Sending output from /wholeBodyDynamics/imu/rpc:i to /ergocubSim/head/inertials/rpc:o using tcp
[INFO] |yarp.os.impl.PortCoreInputUnit|/wholeBodyDynamics/imu/measures:i| Receiving input from /ergocubSim/head/inertials/measures:o to /wholeBodyDynamics/imu/measures:i using tcp
[DEBUG] |yarp.device.multipleanalogsensorsclient| Open complete
[INFO] |yarp.dev.PolyDriver|head_inertial_hardware_device| Created device <multipleanalogsensorsclient>. See C++ class MultipleAnalogSensorsClient for documentation.
[INFO] Opening device waist_inertial_hardware_device with parameters [("robotName" = "ergoCubGazeboV1"), ("remote" = "/ergocubSim/waist/inertials"), ("local" = "/wholeBodyDynamics/waist_imu")]
[DEBUG] |yarp.dev.PolyDriver|waist_inertial_hardware_device| Parameters are (device multipleanalogsensorsclient) (id waist_inertial_hardware_device) (local "/wholeBodyDynamics/waist_imu") (remote "/ergocubSim/waist/inertials") (robotName ergoCubGazeboV1)
[INFO] |yarp.os.Port|/wholeBodyDynamics/waist_imu/rpc:i| Port /wholeBodyDynamics/waist_imu/rpc:i active at tcp://10.0.3.1:10074/
[INFO] |yarp.os.Port|/wholeBodyDynamics/waist_imu/measures:i| Port /wholeBodyDynamics/waist_imu/measures:i active at tcp://10.0.3.1:10075/
[INFO] |yarp.os.impl.PortCoreOutputUnit|/wholeBodyDynamics/waist_imu/rpc:i| Sending output from /wholeBodyDynamics/waist_imu/rpc:i to /ergocubSim/waist/inertials/rpc:o using tcp
[INFO] |yarp.os.impl.PortCoreInputUnit|/wholeBodyDynamics/waist_imu/measures:i| Receiving input from /ergocubSim/waist/inertials/measures:o to /wholeBodyDynamics/waist_imu/measures:i using tcp
[DEBUG] |yarp.device.multipleanalogsensorsclient| Open complete
[INFO] |yarp.dev.PolyDriver|waist_inertial_hardware_device| Created device <multipleanalogsensorsclient>. See C++ class MultipleAnalogSensorsClient for documentation.
[INFO] Opening device ergocub_left_arm_ft with parameters [("robotName" = "ergoCubGazeboV1"), ("remote" = "/ergocubSim/left_arm/FT"), ("local" = "/wholeBodyDynamics/left_arm_ft_sensor")]
[DEBUG] |yarp.dev.PolyDriver|ergocub_left_arm_ft| Parameters are (device multipleanalogsensorsclient) (id ergocub_left_arm_ft) (local "/wholeBodyDynamics/left_arm_ft_sensor") (remote "/ergocubSim/left_arm/FT") (robotName ergoCubGazeboV1)
[INFO] |yarp.os.Port|/wholeBodyDynamics/left_arm_ft_sensor/rpc:i| Port /wholeBodyDynamics/left_arm_ft_sensor/rpc:i active at tcp://10.0.3.1:10076/
[INFO] |yarp.os.Port|/wholeBodyDynamics/left_arm_ft_sensor/measures:i| Port /wholeBodyDynamics/left_arm_ft_sensor/measures:i active at tcp://10.0.3.1:10077/
[INFO] |yarp.os.impl.PortCoreOutputUnit|/wholeBodyDynamics/left_arm_ft_sensor/rpc:i| Sending output from /wholeBodyDynamics/left_arm_ft_sensor/rpc:i to /ergocubSim/left_arm/FT/rpc:o using tcp
[INFO] |yarp.os.impl.PortCoreInputUnit|/wholeBodyDynamics/left_arm_ft_sensor/measures:i| Receiving input from /ergocubSim/left_arm/FT/measures:o to /wholeBodyDynamics/left_arm_ft_sensor/measures:i using tcp
[DEBUG] |yarp.device.multipleanalogsensorsclient| Open complete
[INFO] |yarp.dev.PolyDriver|ergocub_left_arm_ft| Created device <multipleanalogsensorsclient>. See C++ class MultipleAnalogSensorsClient for documentation.
[INFO] Opening device ergocub_right_arm_ft with parameters [("robotName" = "ergoCubGazeboV1"), ("remote" = "/ergocubSim/right_arm/FT"), ("local" = "/wholeBodyDynamics/right_arm_ft_sensor")]
[DEBUG] |yarp.dev.PolyDriver|ergocub_right_arm_ft| Parameters are (device multipleanalogsensorsclient) (id ergocub_right_arm_ft) (local "/wholeBodyDynamics/right_arm_ft_sensor") (remote "/ergocubSim/right_arm/FT") (robotName ergoCubGazeboV1)
[INFO] |yarp.os.Port|/wholeBodyDynamics/right_arm_ft_sensor/rpc:i| Port /wholeBodyDynamics/right_arm_ft_sensor/rpc:i active at tcp://10.0.3.1:10078/
[INFO] |yarp.os.Port|/wholeBodyDynamics/right_arm_ft_sensor/measures:i| Port /wholeBodyDynamics/right_arm_ft_sensor/measures:i active at tcp://10.0.3.1:10079/
[INFO] |yarp.os.impl.PortCoreOutputUnit|/wholeBodyDynamics/right_arm_ft_sensor/rpc:i| Sending output from /wholeBodyDynamics/right_arm_ft_sensor/rpc:i to /ergocubSim/right_arm/FT/rpc:o using tcp
[INFO] |yarp.os.impl.PortCoreInputUnit|/wholeBodyDynamics/right_arm_ft_sensor/measures:i| Receiving input from /ergocubSim/right_arm/FT/measures:o to /wholeBodyDynamics/right_arm_ft_sensor/measures:i using tcp
[DEBUG] |yarp.device.multipleanalogsensorsclient| Open complete
[INFO] |yarp.dev.PolyDriver|ergocub_right_arm_ft| Created device <multipleanalogsensorsclient>. See C++ class MultipleAnalogSensorsClient for documentation.
[INFO] Opening device ergocub_left_leg_ft with parameters [("robotName" = "ergoCubGazeboV1"), ("remote" = "/ergocubSim/left_leg/FT"), ("local" = "/wholeBodyDynamics/left_leg_ft_sensor")]
[DEBUG] |yarp.dev.PolyDriver|ergocub_left_leg_ft| Parameters are (device multipleanalogsensorsclient) (id ergocub_left_leg_ft) (local "/wholeBodyDynamics/left_leg_ft_sensor") (remote "/ergocubSim/left_leg/FT") (robotName ergoCubGazeboV1)
[INFO] |yarp.os.Port|/wholeBodyDynamics/left_leg_ft_sensor/rpc:i| Port /wholeBodyDynamics/left_leg_ft_sensor/rpc:i active at tcp://10.0.3.1:10080/
[INFO] |yarp.os.Port|/wholeBodyDynamics/left_leg_ft_sensor/measures:i| Port /wholeBodyDynamics/left_leg_ft_sensor/measures:i active at tcp://10.0.3.1:10081/
[INFO] |yarp.os.impl.PortCoreOutputUnit|/wholeBodyDynamics/left_leg_ft_sensor/rpc:i| Sending output from /wholeBodyDynamics/left_leg_ft_sensor/rpc:i to /ergocubSim/left_leg/FT/rpc:o using tcp
[INFO] |yarp.os.impl.PortCoreInputUnit|/wholeBodyDynamics/left_leg_ft_sensor/measures:i| Receiving input from /ergocubSim/left_leg/FT/measures:o to /wholeBodyDynamics/left_leg_ft_sensor/measures:i using tcp
[DEBUG] |yarp.device.multipleanalogsensorsclient| Open complete
[INFO] |yarp.dev.PolyDriver|ergocub_left_leg_ft| Created device <multipleanalogsensorsclient>. See C++ class MultipleAnalogSensorsClient for documentation.
[INFO] Opening device ergocub_right_leg_ft with parameters [("robotName" = "ergoCubGazeboV1"), ("remote" = "/ergocubSim/right_leg/FT"), ("local" = "/wholeBodyDynamics/right_leg_ft_sensor")]
[DEBUG] |yarp.dev.PolyDriver|ergocub_right_leg_ft| Parameters are (device multipleanalogsensorsclient) (id ergocub_right_leg_ft) (local "/wholeBodyDynamics/right_leg_ft_sensor") (remote "/ergocubSim/right_leg/FT") (robotName ergoCubGazeboV1)
[INFO] |yarp.os.Port|/wholeBodyDynamics/right_leg_ft_sensor/rpc:i| Port /wholeBodyDynamics/right_leg_ft_sensor/rpc:i active at tcp://10.0.3.1:10082/
[INFO] |yarp.os.Port|/wholeBodyDynamics/right_leg_ft_sensor/measures:i| Port /wholeBodyDynamics/right_leg_ft_sensor/measures:i active at tcp://10.0.3.1:10083/
[INFO] |yarp.os.impl.PortCoreOutputUnit|/wholeBodyDynamics/right_leg_ft_sensor/rpc:i| Sending output from /wholeBodyDynamics/right_leg_ft_sensor/rpc:i to /ergocubSim/right_leg/FT/rpc:o using tcp
[INFO] |yarp.os.impl.PortCoreInputUnit|/wholeBodyDynamics/right_leg_ft_sensor/measures:i| Receiving input from /ergocubSim/right_leg/FT/measures:o to /wholeBodyDynamics/right_leg_ft_sensor/measures:i using tcp
[DEBUG] |yarp.device.multipleanalogsensorsclient| Open complete
[INFO] |yarp.dev.PolyDriver|ergocub_right_leg_ft| Created device <multipleanalogsensorsclient>. See C++ class MultipleAnalogSensorsClient for documentation.
[INFO] Opening device wholebodydynamics with parameters [("robotName" = "ergoCubGazeboV1"), ("axesNames" = "(torso_roll,torso_pitch,torso_yaw,l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,l_elbow,l_wrist_pitch,l_wrist_yaw,r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,r_elbow,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_palm,r_hand_palm,root_link,l_foot_front,l_foot_rear,r_foot_front,r_foot_rear,l_upper_leg,r_upper_leg)"), ("imuFrameName" = "head_imu_0"), ("useJointVelocity" = "true"), ("useJointAcceleration" = "true"), ("imuFilterCutoffInHz" = "3.0"), ("forceTorqueFilterCutoffInHz" = "3.0"), ("jointVelFilterCutoffInHz" = "3.0"), ("jointAccFilterCutoffInHz" = "3.0"), ("startWithZeroFTSensorOffsets" = "true"), ("useSkinForContacts" = "false"), ("publishNetExternalWrenches" = "true"), ("disableSensorReadCheckAtStartup" = "true"), ("GRAVITY_COMPENSATION" [group] = "(enableGravityCompensation true) (gravityCompensationBaseLink root_link) (gravityCompensationAxesNames (torso_roll,torso_pitch,torso_yaw,l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,l_elbow,r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,r_elbow))"), ("HW_USE_MAS_IMU" [group] = "(accelerometer head_imu_0) (gyroscope head_imu_0)"), ("WBD_OUTPUT_EXTERNAL_WRENCH_PORTS" [group] = "(/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)) (/wholeBodyDynamics/left_arm/cartesianEndEffectorWrench:o (l_hand_palm,l_hand_palm,root_link)) (/wholeBodyDynamics/right_arm/cartesianEndEffectorWrench:o (r_hand_palm,r_hand_palm,root_link))"), ("multipleAnalogSensorsNames" [group] = "(SixAxisForceTorqueSensorsNames ("l_arm_ft", "r_arm_ft", "l_leg_ft", "r_leg_ft", "l_foot_front_ft", "r_foot_front_ft", "l_foot_rear_ft", "r_foot_rear_ft"))")]
[DEBUG] |yarp.dev.PolyDriver|wholebodydynamics| Parameters are (GRAVITY_COMPENSATION (enableGravityCompensation true) (gravityCompensationBaseLink root_link) (gravityCompensationAxesNames (torso_roll torso_pitch torso_yaw l_shoulder_pitch l_shoulder_roll l_shoulder_yaw l_elbow r_shoulder_pitch r_shoulder_roll r_shoulder_yaw r_elbow))) (HW_USE_MAS_IMU (accelerometer head_imu_0) (gyroscope head_imu_0)) (WBD_OUTPUT_EXTERNAL_WRENCH_PORTS ("/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)) ("/wholeBodyDynamics/left_arm/cartesianEndEffectorWrench:o" (l_hand_palm l_hand_palm root_link)) ("/wholeBodyDynamics/right_arm/cartesianEndEffectorWrench:o" (r_hand_palm r_hand_palm root_link))) (axesNames (torso_roll torso_pitch torso_yaw l_shoulder_pitch l_shoulder_roll l_shoulder_yaw l_elbow l_wrist_pitch l_wrist_yaw r_shoulder_pitch r_shoulder_roll r_shoulder_yaw r_elbow 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_palm r_hand_palm root_link l_foot_front l_foot_rear r_foot_front r_foot_rear l_upper_leg r_upper_leg)) (device wholebodydynamics) (disableSensorReadCheckAtStartup true) (fixedFrameGravity (0 0 -9.81000000000000049738)) (forceTorqueFilterCutoffInHz 3.0) (id wholebodydynamics) (imuFilterCutoffInHz 3.0) (imuFrameName head_imu_0) (jointAccFilterCutoffInHz 3.0) (jointVelFilterCutoffInHz 3.0) (modelFile model.urdf) (multipleAnalogSensorsNames (SixAxisForceTorqueSensorsNames (l_arm_ft r_arm_ft l_leg_ft r_leg_ft l_foot_front_ft r_foot_front_ft l_foot_rear_ft r_foot_rear_ft))) (publishNetExternalWrenches true) (robotName ergoCubGazeboV1) (startWithZeroFTSensorOffsets true) (useJointAcceleration true) (useJointVelocity true) (useSkinForContacts false)
[DEBUG] wholeBodyDynamics Statistics: Opening estimator.
[INFO] wholeBodyDynamics : Loading model from  /home/ecub_docker/robotology-superbuild/build/install/share/ergoCub/robots/ergoCubGazeboV1_1/model.urdf
[WARNING] wholeBodyDynamics : the loaded model has 0 FT sensors, so the estimation will use just the model.
[WARNING] wholeBodyDynamics : If you instead want to add the FT sensors to your model, please check iDynTree documentation on how to add sensors to models.
[DEBUG] wholeBodyDynamics Statistics: Estimator opened in  0.00610352 s.
[DEBUG] wholeBodyDynamics Statistics: Loading settings from configuration files.
[WARNING] wholeBodyDynamics : The devicePeriodInSeconds parameter is not found. The default one is used. Period: 0.01 seconds.
[WARNING] wholeBodyDynamics: estimateVelocityAccelerationOptionName bool parameter missing, please specify it.
[WARNING] wholeBodyDynamics: setting estimateVelocityAccelerationOptionName to the default value of true, but this is a deprecated behaviour that will be removed in the future.
[INFO] wholeBodyDynamics : Using the following filter cutoff frequencies,
[INFO] wholeBodyDynamics: imuFilterCutoffInHz:  3  Hz.
[INFO] wholeBodyDynamics: forceTorqueFilterCutoffInHz:  3  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.000850439 s.
[DEBUG] wholeBodyDynamics Statistics: Opening RPC port.
[INFO] |yarp.os.Port|/wholeBodyDynamics/rpc| Port /wholeBodyDynamics/rpc active at tcp://10.0.3.1:10084/
[DEBUG] wholeBodyDynamics Statistics: RPC port opened in  0.00281167 s.
[DEBUG] wholeBodyDynamics Statistics: Opening settings port.
[INFO] |yarp.os.Port|/wholeBodyDynamics/settings| Port /wholeBodyDynamics/settings active at tcp://10.0.3.1:10085/
[DEBUG] wholeBodyDynamics Statistics: Settings port opened in  0.00223303 s.
[DEBUG] wholeBodyDynamics Statistics: Opening remapper control board.
[DEBUG] |yarp.dev.PolyDriver|controlboardremapper| Parameters are (axesNames (torso_roll torso_pitch torso_yaw l_shoulder_pitch l_shoulder_roll l_shoulder_yaw l_elbow l_wrist_pitch l_wrist_yaw r_shoulder_pitch r_shoulder_roll r_shoulder_yaw r_elbow 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.00615335 s.
[DEBUG] wholeBodyDynamics Statistics: Opening remapper virtual sensors.
[DEBUG] |yarp.dev.PolyDriver|virtualAnalogRemapper| Parameters are (axesNames (torso_roll torso_pitch torso_yaw l_shoulder_pitch l_shoulder_roll l_shoulder_yaw l_elbow l_wrist_pitch l_wrist_yaw r_shoulder_pitch r_shoulder_roll r_shoulder_yaw r_elbow 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.00642109 s.
[DEBUG] wholeBodyDynamics Statistics: Opening contact frames.
[WARNING] subModel no  0  has the maximum number of variables (6). The frame  r_hand_palm  will not be added to it.
[WARNING] subModel no  0  has the maximum number of variables (6). The frame  root_link  will not be added to it.
[WARNING] subModel no  0  has the maximum number of variables (6). The frame  l_foot_front  will not be added to it.
[WARNING] subModel no  0  has the maximum number of variables (6). The frame  l_foot_rear  will not be added to it.
[WARNING] subModel no  0  has the maximum number of variables (6). The frame  r_foot_front  will not be added to it.
[WARNING] subModel no  0  has the maximum number of variables (6). The frame  r_foot_rear  will not be added to it.
[WARNING] subModel no  0  has the maximum number of variables (6). The frame  l_upper_leg  will not be added to it.
[WARNING] subModel no  0  has the maximum number of variables (6). The frame  r_upper_leg  will not be added to it.
[DEBUG] wholeBodyDynamics Statistics: Contact frames opened in  0.000279188 s.
[DEBUG] wholeBodyDynamics Statistics: Opening external wrenches ports.
[ERROR] Model :: getLinkIndex : Impossible to find link l_foot_front in the Model
[ERROR] wholeBodyDynamics : Link  l_foot_front  not found in the model.
[ERROR] wholeBodyDynamics: Problem in opening external wrenches port.
[ERROR] |yarp.dev.PolyDriver|wholebodydynamics| Driver <wholebodydynamics> was found but could not open
[WARNING] Cannot open device wholebodydynamics
[WARNING] Cannot open device wholebodydynamics
[WARNING] There was some problem opening one or more devices. Please check the log and your configuration
[ERROR] One or more devices failed opening... see previous log messages for more info
[INFO] Closing device wholebodydynamics
traversaro commented 5 months ago

The core problem is:

[WARNING] wholeBodyDynamics : the loaded model has 0 FT sensors, so the estimation will use just the model.
[WARNING] wholeBodyDynamics : If you instead want to add the FT sensors to your model, please check iDynTree documentation on how to add sensors to models.

Basically, iDynTree is loading a model, but without any FT sensor. I suspect this may be a regression of https://github.com/robotology/whole-body-estimators/pull/188 . Just to try, can you check if everything is working with whole-body-estimators v0.10.0 ? You should just do:

cd robotology-superbuild
cd src/whole-body-estimators
git checkout v0.10.0

and rebuild the robotology-superbuild.

traversaro commented 5 months ago

Thanks for reporting the issue.

traversaro commented 5 months ago

@SimoneMic which models are you launching? I tried to replicate this, but I could not start any ergocub models as it was making gazebo crash.

SimoneMic commented 5 months ago

Hi @traversaro I tried it on ergoCubGazeboV1 and 1_1.

Now it works with 0.10.0!

traversaro commented 5 months ago

Hi @traversaro I tried it on ergoCubGazeboV1 and 1_1.

Now it works with 0.10.0!

False alarm, I had an old version of gazebo-yarp-plugins. If it works with 0.10.0. then probably there is a regression in 0.11.0 .

traversaro commented 5 months ago

The bug is in https://github.com/robotology/whole-body-estimators/pull/188/files#diff-938e8a6e8fc4690975a0955e2f3a91a8a23c313180dda2dea0fc6cbea41046d2R386 . Basically, we switched to use the `` method, but by doing so we are not executing anymore https://github.com/robotology/idyntree/blob/42e779bbecb98ab934450afee04d100c6274cee7/src/estimation/src/ExtWrenchesAndJointTorquesEstimator.cpp#L146-L160, i.e. we are also removing the FT sensors fixed joint from the model, and then the loading fails as the FT sensors are not there.

traversaro commented 5 months ago

@SimoneMic the issue should have been fixed in the latest master of whole-body-estimators, and releases as v0.11.1 . I will re-open the issue, if you can confirm that the issue has been fixed, I guess we can close it.

SimoneMic commented 5 months ago

Yes we can close this, thanks a lot!