search

robotology / icub-models-generator

Resources and programs to generated models (URDF, SDF) of the iCub robot
14 stars 23 forks source link

Migrate iCub3 models to use gazebo_yarp_robotinterface plugin #222

Closed traversaro closed 2 years ago

traversaro commented 2 years ago

This PR migrates the iCub3 models to use the gazebo_yarp_robotinterface plugin, and to remove the need to use the wrappers by the Gazebo plugins.

Furthermore, the use of gazebo_yarp_robotinterface would simplify aligning the simulated and the real robot, see for example https://github.com/robotology/icub-models-generator/compare/migrateicub3robint?expand=1#diff-1c5f5e2280364c4f70d59f6a0c5d5d9e1f5be5da33f10b4d652b1b5a700e24b8 that was directly taken from https://github.com/ami-iit/robots-configuration/blob/devel_iCubGenova09/iCubGenova09/icub_wbd.xml, with the appropriate modifications.

This PR is not ready to merge as it is, as it is currently:

traversaro commented 2 years ago

This change would be beneficial so that we could run wholebodydynamics directly inside the Gazebo simulator, without the need and the complexity of running an external YARP_CLOCK=/clock yarprobotinterface --config launch-wholebodydynamics.xml, that results in trick behavior such as the one described in https://github.com/robotology/icub-models/issues/164 .

traversaro commented 2 years ago

I would substitute the FTs ini file with <yarpConfigurationString>(yarpDeviceName chest-inertial) (disableImplicitNetworkWrapper)</yarpConfigurationString> inside the urdf

I agree, but I wanted to minimize the changes w.r.t. to the old way of doing. Once this gets trough, we can get rid of the many .ini files.

traversaro commented 2 years ago

Requires https://github.com/robotology/gazebo-yarp-plugins/pull/628, as on iCub 3 as of June 2022 the FT sensors are only exposed via MAS interfaces

This change was released in https://github.com/robotology/gazebo-yarp-plugins/releases/tag/v4.5.0 , this was fixed.

traversaro commented 2 years ago

Regarding the last two bullet points, can we have a short meeting @S-Dafarra and who do you think it is useful to have to sort it out, so that I can complete this PR?

S-Dafarra commented 2 years ago

[ ] It is not complete the simulation on FT sensors side: in particular, it is not clear to me the serialization of sensors in the foot FT sensors devices.

This might be useful: https://github.com/ami-iit/robots-configuration/blob/ac3d78863ee73566c9a810e6eff782ef644e5230/iCubGenova09/hardware/FT/right_leg-eb12-j3_5-strain.xml

S-Dafarra commented 2 years ago
  • It is not clear to me how the RGB/D sensors are exposed on the real robot.

Related configuration file on the robot: https://github.com/ami-iit/robots-configuration/blob/ac3d78863ee73566c9a810e6eff782ef644e5230/iCubGenova09/sensors/RealSense_conf.ini

traversaro commented 2 years ago

[ ] It is not complete the simulation on FT sensors side: in particular, it is not clear to me the serialization of sensors in the foot FT sensors devices.

This might be useful: https://github.com/ami-iit/robots-configuration/blob/ac3d78863ee73566c9a810e6eff782ef644e5230/iCubGenova09/hardware/FT/right_leg-eb12-j3_5-strain.xml

Thanks, this was fixed in:

traversaro commented 2 years ago

It is not clear to me how the RGB/D sensors are exposed on the real robot.

As the chest RGBD sensor location is in the process of being redesigned, we decided to leave it aside, and we add it back when it is ready and sistematically used.

traversaro commented 2 years ago

I rendered this branch in https://github.com/robotology/icub-models/tree/debug222, so this branch can be used to test the updated iCub3 model. @S-Dafarra @mebbaid do you think someone can test it against some complex iCub3 scenario such as walking? Thanks!

S-Dafarra commented 2 years ago

I rendered this branch in https://github.com/robotology/icub-models/tree/debug222, so this branch can be used to test the updated iCub3 model. @S-Dafarra @mebbaid do you think someone can test it against some complex iCub3 scenario such as walking? Thanks!

@mebbaid could you try it?

mebbaid commented 2 years ago

I rendered this branch in https://github.com/robotology/icub-models/tree/debug222, so this branch can be used to test the updated iCub3 model. @S-Dafarra @mebbaid do you think someone can test it against some complex iCub3 scenario such as walking? Thanks!

@mebbaid could you try it?

I will try it at at the end of today

mebbaid commented 2 years ago

I rendered this branch in https://github.com/robotology/icub-models/tree/debug222, so this branch can be used to test the updated iCub3 model. @S-Dafarra @mebbaid do you think someone can test it against some complex iCub3 scenario such as walking? Thanks!

@mebbaid could you try it?

I will try it at at the end of today

I checked out to the branch debug222 in icub-models, recompiled icub-models and dropped an iCubGazeboV3 in gazebo, but gazebo kept crashing. There is no specific error message in the terminal. The output of the terminal where i launched gazebo is below

(base) mebbaid@IITICUBLAP213:~/robot-code/element_walking-with-payload/cpp_playground/ParametrizedCentroidalMPC$ export YARP_CLOCK=/clock && YARP_CLOCK=/clock gazebo -slibgazebo_yarp_clock.so 
[INFO] GazeboYarpClock loaded. Clock port will be  /clock
[INFO] |yarp.os.Port|/clock| Port /clock active at tcp://10.0.0.123:10363/
[INFO] |yarp.os.Port|/clock/rpc| Port /clock/rpc active at tcp://10.0.0.123:10364/
Resetting YARP clock to default
[INFO] |yarp.os.Port|/IITICUBLAP213/gzserver/441791/clock:i| Port /IITICUBLAP213/gzserver/441791/clock:i active at tcp://10.0.0.123:10504/
[INFO] |yarp.os.Network| Success: port-to-port persistent connection added.
[INFO] |yarp.os.Time| Waiting for clock server to start broadcasting data ...
[INFO] |yarp.os.impl.PortCoreInputUnit|/IITICUBLAP213/gzserver/441791/clock:i| Receiving input from /clock to /IITICUBLAP213/gzserver/441791/clock:i using tcp
[INFO] |yarp.os.impl.PortCoreOutputUnit|/clock| Sending output from /clock to /IITICUBLAP213/gzserver/441791/clock:i using tcp
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpDepthCamera| CC: m_driverParameters:  (CAMERA_PARAM (focalLengthX 570.34222412109375) (focalLengthY 570.34222412109375) (tangentialPointX 0.0) (tangentialPointY 0.0)) (device gazebo_depthCamera) (disableImplicitNetworkWrapper) (gazeboYarpPluginsRobotName iCub) (gazeboYarpPluginsSensorName realsense_chest_depth) (sensorScopedName "default::iCub::chest::realsense_chest_depth") (yarpDeviceName icub_depth_camera)
[DEBUG] |yarp.dev.PolyDriver|gazebo_depthCamera| Parameters are (CAMERA_PARAM (focalLengthX 570.34222412109375) (focalLengthY 570.34222412109375) (tangentialPointX 0.0) (tangentialPointY 0.0)) (device gazebo_depthCamera) (disableImplicitNetworkWrapper) (gazeboYarpPluginsRobotName iCub) (gazeboYarpPluginsSensorName realsense_chest_depth) (sensorScopedName "default::iCub::chest::realsense_chest_depth") (yarpDeviceName icub_depth_camera)
[INFO] |yarp.dev.PolyDriver|gazebo_depthCamera| Created device <gazebo_depthCamera>. See C++ class GazeboYarpDepthCameraDriver for documentation.
[INFO] |gazebo-yarp-plugins.plugins.GazeboYarpDepthCamera| Registered YARP device with instance name: default::iCub::chest::realsense_chest_depth::icub_depth_camera
[DEBUG] GazeboYarpCamera Plugin: sensor scoped name is  default::iCub::chest::realsense_chest_rgb
[DEBUG] |yarp.dev.PolyDriver|gazebo_camera| Parameters are (device gazebo_camera) (disableImplicitNetworkWrapper) (framerate 30) (gazeboYarpPluginsRobotName iCub) (gazeboYarpPluginsSensorName realsense_chest_rgb) (sensorScopedName "default::iCub::chest::realsense_chest_rgb") (stamp 1) (yarpDeviceName icub_rgb_camera)
[INFO] |yarp.dev.PolyDriver|gazebo_camera| Created device <gazebo_camera>. See C++ class GazeboYarpCameraDriver for documentation.
[INFO] Loaded GazeboYarpCamera Plugin correctly
[INFO] GazeboYarpCamera: Register YARP device with instance name: default::iCub::chest::realsense_chest_rgb::icub_rgb_camera
[DEBUG] |yarp.dev.PolyDriver|gazebo_imu| Parameters are (device gazebo_imu) (sensorScopedName "default::iCub::chest::chest_imu_acc_1x1") (sensor_name chest_imu_acc_1x1)
[INFO] |yarp.dev.PolyDriver|gazebo_imu| Created device <gazebo_imu>. See C++ class GazeboYarpIMUDriver for documentation.
[INFO] Registered YARP device with instance name: default::iCub::chest::chest_imu_acc_1x1::chest_inertial_hardware_device
[DEBUG] |yarp.dev.PolyDriver|gazebo_imu| Parameters are (device gazebo_imu) (sensorScopedName "default::iCub::head::head_imu_acc_1x1") (sensor_name head_imu_acc_1x1)
[INFO] |yarp.dev.PolyDriver|gazebo_imu| Created device <gazebo_imu>. See C++ class GazeboYarpIMUDriver for documentation.
[INFO] Registered YARP device with instance name: default::iCub::head::head_imu_acc_1x1::head_inertial_hardware_device
[DEBUG] |yarp.dev.PolyDriver|gazebo_forcetorque| Parameters are (device gazebo_forcetorque) (disableImplicitNetworkWrapper) (gazeboYarpPluginsRobotName iCub) (gazeboYarpPluginsSensorName l_leg_ft_sensor) (sensorScopedName "default::iCub::l_leg_ft_sensor::l_leg_ft_sensor") (yarpDeviceName icub_left_leg_ft)
[INFO] |yarp.dev.PolyDriver|gazebo_forcetorque| Created device <gazebo_forcetorque>. See C++ class GazeboYarpForceTorqueDriver for documentation.
[INFO] Registered YARP device with instance name: default::iCub::l_leg_ft_sensor::l_leg_ft_sensor::icub_left_leg_ft
[DEBUG] |yarp.dev.PolyDriver|gazebo_forcetorque| Parameters are (device gazebo_forcetorque) (disableImplicitNetworkWrapper) (gazeboYarpPluginsRobotName iCub) (gazeboYarpPluginsSensorName l_foot_front_ft_sensor) (sensorScopedName "default::iCub::l_foot_front_ft_sensor::l_foot_front_ft_sensor") (yarpDeviceName icub_left_foot_front_ft)
[INFO] |yarp.dev.PolyDriver|gazebo_forcetorque| Created device <gazebo_forcetorque>. See C++ class GazeboYarpForceTorqueDriver for documentation.
[INFO] Registered YARP device with instance name: default::iCub::l_foot_front_ft_sensor::l_foot_front_ft_sensor::icub_left_foot_front_ft
[DEBUG] |yarp.dev.PolyDriver|gazebo_forcetorque| Parameters are (device gazebo_forcetorque) (disableImplicitNetworkWrapper) (gazeboYarpPluginsRobotName iCub) (gazeboYarpPluginsSensorName l_foot_rear_ft_sensor) (sensorScopedName "default::iCub::l_foot_rear_ft_sensor::l_foot_rear_ft_sensor") (yarpDeviceName icub_left_foot_rear_ft)
[INFO] |yarp.dev.PolyDriver|gazebo_forcetorque| Created device <gazebo_forcetorque>. See C++ class GazeboYarpForceTorqueDriver for documentation.
[INFO] Registered YARP device with instance name: default::iCub::l_foot_rear_ft_sensor::l_foot_rear_ft_sensor::icub_left_foot_rear_ft
[DEBUG] |yarp.dev.PolyDriver|gazebo_forcetorque| Parameters are (device gazebo_forcetorque) (disableImplicitNetworkWrapper) (gazeboYarpPluginsRobotName iCub) (gazeboYarpPluginsSensorName r_leg_ft_sensor) (sensorScopedName "default::iCub::r_leg_ft_sensor::r_leg_ft_sensor") (yarpDeviceName icub_right_leg_ft)
[INFO] |yarp.dev.PolyDriver|gazebo_forcetorque| Created device <gazebo_forcetorque>. See C++ class GazeboYarpForceTorqueDriver for documentation.
[INFO] Registered YARP device with instance name: default::iCub::r_leg_ft_sensor::r_leg_ft_sensor::icub_right_leg_ft
[DEBUG] |yarp.dev.PolyDriver|gazebo_forcetorque| Parameters are (device gazebo_forcetorque) (disableImplicitNetworkWrapper) (gazeboYarpPluginsRobotName iCub) (gazeboYarpPluginsSensorName r_foot_front_ft_sensor) (sensorScopedName "default::iCub::r_foot_front_ft_sensor::r_foot_front_ft_sensor") (yarpDeviceName icub_right_foot_front_ft)
[INFO] |yarp.dev.PolyDriver|gazebo_forcetorque| Created device <gazebo_forcetorque>. See C++ class GazeboYarpForceTorqueDriver for documentation.
[INFO] Registered YARP device with instance name: default::iCub::r_foot_front_ft_sensor::r_foot_front_ft_sensor::icub_right_foot_front_ft
[DEBUG] |yarp.dev.PolyDriver|gazebo_forcetorque| Parameters are (device gazebo_forcetorque) (disableImplicitNetworkWrapper) (gazeboYarpPluginsRobotName iCub) (gazeboYarpPluginsSensorName r_foot_rear_ft_sensor) (sensorScopedName "default::iCub::r_foot_rear_ft_sensor::r_foot_rear_ft_sensor") (yarpDeviceName icub_right_foot_rear_ft)
[INFO] |yarp.dev.PolyDriver|gazebo_forcetorque| Created device <gazebo_forcetorque>. See C++ class GazeboYarpForceTorqueDriver for documentation.
[INFO] Registered YARP device with instance name: default::iCub::r_foot_rear_ft_sensor::r_foot_rear_ft_sensor::icub_right_foot_rear_ft
[DEBUG] |yarp.dev.PolyDriver|gazebo_forcetorque| Parameters are (device gazebo_forcetorque) (disableImplicitNetworkWrapper) (gazeboYarpPluginsRobotName iCub) (gazeboYarpPluginsSensorName l_arm_ft_sensor) (sensorScopedName "default::iCub::l_arm_ft_sensor::l_arm_ft_sensor") (yarpDeviceName icub_left_arm_ft)
[INFO] |yarp.dev.PolyDriver|gazebo_forcetorque| Created device <gazebo_forcetorque>. See C++ class GazeboYarpForceTorqueDriver for documentation.
[INFO] Registered YARP device with instance name: default::iCub::l_arm_ft_sensor::l_arm_ft_sensor::icub_left_arm_ft
[DEBUG] |yarp.dev.PolyDriver|gazebo_forcetorque| Parameters are (device gazebo_forcetorque) (disableImplicitNetworkWrapper) (gazeboYarpPluginsRobotName iCub) (gazeboYarpPluginsSensorName r_arm_ft_sensor) (sensorScopedName "default::iCub::r_arm_ft_sensor::r_arm_ft_sensor") (yarpDeviceName icub_right_arm_ft)
[INFO] |yarp.dev.PolyDriver|gazebo_forcetorque| Created device <gazebo_forcetorque>. See C++ class GazeboYarpForceTorqueDriver for documentation.
[INFO] Registered YARP device with instance name: default::iCub::r_arm_ft_sensor::r_arm_ft_sensor::icub_right_arm_ft
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| m_parameters: (IMPEDANCE_CONTROL (controlUnits metric_units) (controlLaw joint_pid_gazebo_v1) (stiffness 0.0 0.0 0.0) (damping 0.0 0.0 0.0)) (LIMITS (jntPosMax 43.0 45.0 23.0) (jntPosMin -43.0 -18.0 -23.0) (jntVelMax 100.0 100.0 100.0)) (POSITION_CONTROL (controlUnits metric_units) (controlLaw joint_pid_gazebo_v1) (kp 70.0 70.0 70.0) (kd 0.149999999999999994449 0.149999999999999994449 0.149999999999999994449) (ki 0.170000000000000012212 0.170000000000000012212 0.170000000000000012212) (maxInt 9999 9999 9999) (maxOutput 9999 9999 9999) (shift 0.0 0.0 0.0) (ko 0.0 0.0 0.0) (stictionUp 0.0 0.0 0.0) (stictionDwn 0.0 0.0 0.0)) (TRAJECTORY_GENERATION (trajectory_type minimum_jerk)) (VELOCITY_CONTROL (velocityControlImplementationType integrator_and_position_pid) (controlUnits metric_units) (controlLaw joint_pid_gazebo_v1) (kp 8.72600000000000086686 8.72600000000000086686 8.72600000000000086686) (kd 0.0350000000000000033307 0.0350000000000000033307 0.0350000000000000033307) (ki 0.00200000000000000004163 0.00200000000000000004163 0.00200000000000000004163) (maxInt 9999 9999 9999) (maxOutput 9999 9999 9999) (shift 0.0 0.0 0.0) (ko 0.0 0.0 0.0) (stictionUp 0.0 0.0 0.0) (stictionDwn 0.0 0.0 0.0)) (device gazebo_controlboard) (disableImplicitNetworkWrapper) (gazeboYarpPluginsRobotName iCub) (jointNames torso_yaw torso_pitch torso_roll) (max_damping 100.0 100.0 100.0) (max_stiffness 1000.0 1000.0 1000.0) (min_damping 0.0 0.0 0.0) (min_stiffness 0.0 0.0 0.0) (name "iCub::torso_hardware_device") (robotScopedName iCub) (yarpDeviceName torso_hardware_device)
[DEBUG] |yarp.dev.PolyDriver|gazebo_controlboard| Parameters are (IMPEDANCE_CONTROL (controlUnits metric_units) (controlLaw joint_pid_gazebo_v1) (stiffness 0.0 0.0 0.0) (damping 0.0 0.0 0.0)) (LIMITS (jntPosMax 43.0 45.0 23.0) (jntPosMin -43.0 -18.0 -23.0) (jntVelMax 100.0 100.0 100.0)) (POSITION_CONTROL (controlUnits metric_units) (controlLaw joint_pid_gazebo_v1) (kp 70.0 70.0 70.0) (kd 0.149999999999999994449 0.149999999999999994449 0.149999999999999994449) (ki 0.170000000000000012212 0.170000000000000012212 0.170000000000000012212) (maxInt 9999 9999 9999) (maxOutput 9999 9999 9999) (shift 0.0 0.0 0.0) (ko 0.0 0.0 0.0) (stictionUp 0.0 0.0 0.0) (stictionDwn 0.0 0.0 0.0)) (TRAJECTORY_GENERATION (trajectory_type minimum_jerk)) (VELOCITY_CONTROL (velocityControlImplementationType integrator_and_position_pid) (controlUnits metric_units) (controlLaw joint_pid_gazebo_v1) (kp 8.72600000000000086686 8.72600000000000086686 8.72600000000000086686) (kd 0.0350000000000000033307 0.0350000000000000033307 0.0350000000000000033307) (ki 0.00200000000000000004163 0.00200000000000000004163 0.00200000000000000004163) (maxInt 9999 9999 9999) (maxOutput 9999 9999 9999) (shift 0.0 0.0 0.0) (ko 0.0 0.0 0.0) (stictionUp 0.0 0.0 0.0) (stictionDwn 0.0 0.0 0.0)) (device gazebo_controlboard) (disableImplicitNetworkWrapper) (gazeboYarpPluginsRobotName iCub) (jointNames torso_yaw torso_pitch torso_roll) (max_damping 100.0 100.0 100.0) (max_stiffness 1000.0 1000.0 1000.0) (min_damping 0.0 0.0 0.0) (min_stiffness 0.0 0.0 0.0) (name "iCub::torso_hardware_device") (robotScopedName iCub) (yarpDeviceName torso_hardware_device)
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| found:  torso_yaw torso_yaw
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| found:  torso_pitch torso_pitch
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| found:  torso_roll torso_roll
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| trajectory_type: minimum_jerk
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| Trajectory successfully generated.
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| refSpeed: [   10.000000     10.000000   10.000000  ] 
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| refAcceleration: [   10.000000  10.000000   10.000000  ] 
[WARNING] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| No positionToleranceLinear value found in ini file, default one will be used!
[WARNING] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| No positionToleranceRevolute value found in ini file, default one will be used!
[WARNING] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| No max torques value found in ini file, default one will be used!
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| max_torques: [   2000.000000    2000.000000     2000.000000  ]
[INFO] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| min_stiffness param found!
[INFO] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| max_stiffness param found!
[INFO] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| min_damping param found!
[INFO] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| max_damping param found!
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| min_stiffness: [   0.000000     0.000000    0.000000  ]
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| max_stiffness: [   1000.000000  1000.000000     1000.000000  ]
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| min_damping: [   0.000000   0.000000    0.000000  ]
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| max_damping: [   100.000000     100.000000  100.000000  ]
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| Initializing Trajectory Generator with current values
[INFO] |yarp.dev.PolyDriver|gazebo_controlboard| Created device <gazebo_controlboard>. See C++ class GazeboYarpControlBoardDriver for documentation.
[INFO] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| Registered YARP device with instance name: iCub::torso_hardware_device
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| m_parameters: (IMPEDANCE_CONTROL (controlUnits metric_units) (controlLaw joint_pid_gazebo_v1) (stiffness 0.0 0.0 0.0 0.0 0.0 0.0) (damping 0.0 0.0 0.0 0.0 0.0 0.0)) (LIMITS (jntPosMax 115.0 112.0 80.0 5.0 45.0 25.0) (jntPosMin -45.0 -10.0 -80.0 -70.0 -45.0 -25.0) (jntVelMax 100.0 100.0 100.0 100.0 100.0 100.0)) (POSITION_CONTROL (controlUnits metric_units) (controlLaw joint_pid_gazebo_v1) (kp 70.0 70.0 40.0 100.0 100.0 100.0) (kd 0.149999999999999994449 0.149999999999999994449 0.349999999999999977796 0.149999999999999994449 0.149999999999999994449 0.149999999999999994449) (ki 0.170000000000000012212 0.170000000000000012212 0.349999999999999977796 0.349999999999999977796 0.349999999999999977796 0.349999999999999977796) (maxInt 9999 9999 9999 9999 9999 9999) (maxOutput 9999 9999 9999 9999 9999 9999) (shift 0.0 0.0 0.0 0.0 0.0 0.0) (ko 0.0 0.0 0.0 0.0 0.0 0.0) (stictionUp 0.0 0.0 0.0 0.0 0.0 0.0) (stictionDwn 0.0 0.0 0.0 0.0 0.0 0.0)) (TRAJECTORY_GENERATION (trajectory_type minimum_jerk)) (VELOCITY_CONTROL (velocityControlImplementationType integrator_and_position_pid) (controlUnits metric_units) (controlLaw joint_pid_gazebo_v1) (kp 8.72600000000000086686 8.72600000000000086686 8.72600000000000086686 8.72600000000000086686 8.72600000000000086686 8.72600000000000086686) (kd 0.348999999999999976907 0.348999999999999976907 0.348999999999999976907 0.348999999999999976907 0.348999999999999976907 0.348999999999999976907) (ki 0.175999999999999989786 0.175999999999999989786 0.175999999999999989786 0.175999999999999989786 0.175999999999999989786 0.175999999999999989786) (maxInt 9999 9999 9999 9999 9999 9999) (maxOutput 9999 9999 9999 9999 9999 9999) (shift 0.0 0.0 0.0 0.0 0.0 0.0) (ko 0.0 0.0 0.0 0.0 0.0 0.0) (stictionUp 0.0 0.0 0.0 0.0 0.0 0.0) (stictionDwn 0.0 0.0 0.0 0.0 0.0 0.0)) (device gazebo_controlboard) (disableImplicitNetworkWrapper) (gazeboYarpPluginsRobotName iCub) (jointNames l_hip_pitch l_hip_roll l_hip_yaw l_knee l_ankle_pitch l_ankle_roll) (max_damping 100.0 100.0 100.0 100.0 100.0 100.0) (max_stiffness 1000.0 1000.0 1000.0 1000.0 1000.0 1000.0) (min_damping 0.0 0.0 0.0 0.0 0.0 0.0) (min_stiffness 0.0 0.0 0.0 0.0 0.0 0.0) (name "iCub::left_leg_hardware_device") (robotScopedName iCub) (yarpDeviceName left_leg_hardware_device)
[DEBUG] |yarp.dev.PolyDriver|gazebo_controlboard| Parameters are (IMPEDANCE_CONTROL (controlUnits metric_units) (controlLaw joint_pid_gazebo_v1) (stiffness 0.0 0.0 0.0 0.0 0.0 0.0) (damping 0.0 0.0 0.0 0.0 0.0 0.0)) (LIMITS (jntPosMax 115.0 112.0 80.0 5.0 45.0 25.0) (jntPosMin -45.0 -10.0 -80.0 -70.0 -45.0 -25.0) (jntVelMax 100.0 100.0 100.0 100.0 100.0 100.0)) (POSITION_CONTROL (controlUnits metric_units) (controlLaw joint_pid_gazebo_v1) (kp 70.0 70.0 40.0 100.0 100.0 100.0) (kd 0.149999999999999994449 0.149999999999999994449 0.349999999999999977796 0.149999999999999994449 0.149999999999999994449 0.149999999999999994449) (ki 0.170000000000000012212 0.170000000000000012212 0.349999999999999977796 0.349999999999999977796 0.349999999999999977796 0.349999999999999977796) (maxInt 9999 9999 9999 9999 9999 9999) (maxOutput 9999 9999 9999 9999 9999 9999) (shift 0.0 0.0 0.0 0.0 0.0 0.0) (ko 0.0 0.0 0.0 0.0 0.0 0.0) (stictionUp 0.0 0.0 0.0 0.0 0.0 0.0) (stictionDwn 0.0 0.0 0.0 0.0 0.0 0.0)) (TRAJECTORY_GENERATION (trajectory_type minimum_jerk)) (VELOCITY_CONTROL (velocityControlImplementationType integrator_and_position_pid) (controlUnits metric_units) (controlLaw joint_pid_gazebo_v1) (kp 8.72600000000000086686 8.72600000000000086686 8.72600000000000086686 8.72600000000000086686 8.72600000000000086686 8.72600000000000086686) (kd 0.348999999999999976907 0.348999999999999976907 0.348999999999999976907 0.348999999999999976907 0.348999999999999976907 0.348999999999999976907) (ki 0.175999999999999989786 0.175999999999999989786 0.175999999999999989786 0.175999999999999989786 0.175999999999999989786 0.175999999999999989786) (maxInt 9999 9999 9999 9999 9999 9999) (maxOutput 9999 9999 9999 9999 9999 9999) (shift 0.0 0.0 0.0 0.0 0.0 0.0) (ko 0.0 0.0 0.0 0.0 0.0 0.0) (stictionUp 0.0 0.0 0.0 0.0 0.0 0.0) (stictionDwn 0.0 0.0 0.0 0.0 0.0 0.0)) (device gazebo_controlboard) (disableImplicitNetworkWrapper) (gazeboYarpPluginsRobotName iCub) (jointNames l_hip_pitch l_hip_roll l_hip_yaw l_knee l_ankle_pitch l_ankle_roll) (max_damping 100.0 100.0 100.0 100.0 100.0 100.0) (max_stiffness 1000.0 1000.0 1000.0 1000.0 1000.0 1000.0) (min_damping 0.0 0.0 0.0 0.0 0.0 0.0) (min_stiffness 0.0 0.0 0.0 0.0 0.0 0.0) (name "iCub::left_leg_hardware_device") (robotScopedName iCub) (yarpDeviceName left_leg_hardware_device)
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| found:  l_hip_pitch l_hip_pitch
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| found:  l_hip_roll l_hip_roll
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| found:  l_hip_yaw l_hip_yaw
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| found:  l_knee l_knee
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| found:  l_ankle_pitch l_ankle_pitch
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| found:  l_ankle_roll l_ankle_roll
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| trajectory_type: minimum_jerk
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| Trajectory successfully generated.
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| refSpeed: [   10.000000     10.000000   10.000000   10.000000   10.000000   10.000000  ] 
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| refAcceleration: [   10.000000  10.000000   10.000000   10.000000   10.000000   10.000000  ] 
[WARNING] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| No positionToleranceLinear value found in ini file, default one will be used!
[WARNING] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| No positionToleranceRevolute value found in ini file, default one will be used!
[WARNING] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| No max torques value found in ini file, default one will be used!
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| max_torques: [   2000.000000    2000.000000     2000.000000     2000.000000     2000.000000     2000.000000  ]
[INFO] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| min_stiffness param found!
[INFO] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| max_stiffness param found!
[INFO] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| min_damping param found!
[INFO] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| max_damping param found!
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| min_stiffness: [   0.000000     0.000000    0.000000    0.000000    0.000000    0.000000  ]
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| max_stiffness: [   1000.000000  1000.000000     1000.000000     1000.000000     1000.000000     1000.000000  ]
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| min_damping: [   0.000000   0.000000    0.000000    0.000000    0.000000    0.000000  ]
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| max_damping: [   100.000000     100.000000  100.000000  100.000000  100.000000  100.000000  ]
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| Initializing Trajectory Generator with current values
[INFO] |yarp.dev.PolyDriver|gazebo_controlboard| Created device <gazebo_controlboard>. See C++ class GazeboYarpControlBoardDriver for documentation.
[INFO] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| Registered YARP device with instance name: iCub::left_leg_hardware_device
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| m_parameters: (IMPEDANCE_CONTROL (controlUnits metric_units) (controlLaw joint_pid_gazebo_v1) (stiffness 0.0 0.0 0.0 0.0 0.0 0.0) (damping 0.0 0.0 0.0 0.0 0.0 0.0)) (LIMITS (jntPosMax 115.0 112.0 80.0 5.0 45.0 25.0) (jntPosMin -45.0 -10.0 -80.0 -70.0 -45.0 -25.0) (jntVelMax 100.0 100.0 100.0 100.0 100.0 100.0)) (POSITION_CONTROL (controlUnits metric_units) (controlLaw joint_pid_gazebo_v1) (kp 70.0 70.0 40.0 100.0 100.0 100.0) (kd 0.149999999999999994449 0.149999999999999994449 0.349999999999999977796 0.149999999999999994449 0.149999999999999994449 0.149999999999999994449) (ki 0.170000000000000012212 0.170000000000000012212 0.349999999999999977796 0.349999999999999977796 0.349999999999999977796 0.349999999999999977796) (maxInt 9999 9999 9999 9999 9999 9999) (maxOutput 9999 9999 9999 9999 9999 9999) (shift 0.0 0.0 0.0 0.0 0.0 0.0) (ko 0.0 0.0 0.0 0.0 0.0 0.0) (stictionUp 0.0 0.0 0.0 0.0 0.0 0.0) (stictionDwn 0.0 0.0 0.0 0.0 0.0 0.0)) (TRAJECTORY_GENERATION (trajectory_type minimum_jerk)) (VELOCITY_CONTROL (velocityControlImplementationType integrator_and_position_pid) (controlUnits metric_units) (controlLaw joint_pid_gazebo_v1) (kp 8.72600000000000086686 8.72600000000000086686 8.72600000000000086686 8.72600000000000086686 8.72600000000000086686 8.72600000000000086686) (kd 0.348999999999999976907 0.348999999999999976907 0.348999999999999976907 0.348999999999999976907 0.348999999999999976907 0.348999999999999976907) (ki 0.175999999999999989786 0.175999999999999989786 0.175999999999999989786 0.175999999999999989786 0.175999999999999989786 0.175999999999999989786) (maxInt 9999 9999 9999 9999 9999 9999) (maxOutput 9999 9999 9999 9999 9999 9999) (shift 0.0 0.0 0.0 0.0 0.0 0.0) (ko 0.0 0.0 0.0 0.0 0.0 0.0) (stictionUp 0.0 0.0 0.0 0.0 0.0 0.0) (stictionDwn 0.0 0.0 0.0 0.0 0.0 0.0)) (device gazebo_controlboard) (disableImplicitNetworkWrapper) (gazeboYarpPluginsRobotName iCub) (jointNames r_hip_pitch r_hip_roll r_hip_yaw r_knee r_ankle_pitch r_ankle_roll) (max_damping 100.0 100.0 100.0 100.0 100.0 100.0) (max_stiffness 1000.0 1000.0 1000.0 1000.0 1000.0 1000.0) (min_damping 0.0 0.0 0.0 0.0 0.0 0.0) (min_stiffness 0.0 0.0 0.0 0.0 0.0 0.0) (name "iCub::right_leg_hardware_device") (robotScopedName iCub) (yarpDeviceName right_leg_hardware_device)
[DEBUG] |yarp.dev.PolyDriver|gazebo_controlboard| Parameters are (IMPEDANCE_CONTROL (controlUnits metric_units) (controlLaw joint_pid_gazebo_v1) (stiffness 0.0 0.0 0.0 0.0 0.0 0.0) (damping 0.0 0.0 0.0 0.0 0.0 0.0)) (LIMITS (jntPosMax 115.0 112.0 80.0 5.0 45.0 25.0) (jntPosMin -45.0 -10.0 -80.0 -70.0 -45.0 -25.0) (jntVelMax 100.0 100.0 100.0 100.0 100.0 100.0)) (POSITION_CONTROL (controlUnits metric_units) (controlLaw joint_pid_gazebo_v1) (kp 70.0 70.0 40.0 100.0 100.0 100.0) (kd 0.149999999999999994449 0.149999999999999994449 0.349999999999999977796 0.149999999999999994449 0.149999999999999994449 0.149999999999999994449) (ki 0.170000000000000012212 0.170000000000000012212 0.349999999999999977796 0.349999999999999977796 0.349999999999999977796 0.349999999999999977796) (maxInt 9999 9999 9999 9999 9999 9999) (maxOutput 9999 9999 9999 9999 9999 9999) (shift 0.0 0.0 0.0 0.0 0.0 0.0) (ko 0.0 0.0 0.0 0.0 0.0 0.0) (stictionUp 0.0 0.0 0.0 0.0 0.0 0.0) (stictionDwn 0.0 0.0 0.0 0.0 0.0 0.0)) (TRAJECTORY_GENERATION (trajectory_type minimum_jerk)) (VELOCITY_CONTROL (velocityControlImplementationType integrator_and_position_pid) (controlUnits metric_units) (controlLaw joint_pid_gazebo_v1) (kp 8.72600000000000086686 8.72600000000000086686 8.72600000000000086686 8.72600000000000086686 8.72600000000000086686 8.72600000000000086686) (kd 0.348999999999999976907 0.348999999999999976907 0.348999999999999976907 0.348999999999999976907 0.348999999999999976907 0.348999999999999976907) (ki 0.175999999999999989786 0.175999999999999989786 0.175999999999999989786 0.175999999999999989786 0.175999999999999989786 0.175999999999999989786) (maxInt 9999 9999 9999 9999 9999 9999) (maxOutput 9999 9999 9999 9999 9999 9999) (shift 0.0 0.0 0.0 0.0 0.0 0.0) (ko 0.0 0.0 0.0 0.0 0.0 0.0) (stictionUp 0.0 0.0 0.0 0.0 0.0 0.0) (stictionDwn 0.0 0.0 0.0 0.0 0.0 0.0)) (device gazebo_controlboard) (disableImplicitNetworkWrapper) (gazeboYarpPluginsRobotName iCub) (jointNames r_hip_pitch r_hip_roll r_hip_yaw r_knee r_ankle_pitch r_ankle_roll) (max_damping 100.0 100.0 100.0 100.0 100.0 100.0) (max_stiffness 1000.0 1000.0 1000.0 1000.0 1000.0 1000.0) (min_damping 0.0 0.0 0.0 0.0 0.0 0.0) (min_stiffness 0.0 0.0 0.0 0.0 0.0 0.0) (name "iCub::right_leg_hardware_device") (robotScopedName iCub) (yarpDeviceName right_leg_hardware_device)
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| found:  r_hip_pitch r_hip_pitch
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| found:  r_hip_roll r_hip_roll
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| found:  r_hip_yaw r_hip_yaw
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| found:  r_knee r_knee
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| found:  r_ankle_pitch r_ankle_pitch
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| found:  r_ankle_roll r_ankle_roll
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| trajectory_type: minimum_jerk
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| Trajectory successfully generated.
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| refSpeed: [   10.000000     10.000000   10.000000   10.000000   10.000000   10.000000  ] 
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| refAcceleration: [   10.000000  10.000000   10.000000   10.000000   10.000000   10.000000  ] 
[WARNING] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| No positionToleranceLinear value found in ini file, default one will be used!
[WARNING] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| No positionToleranceRevolute value found in ini file, default one will be used!
[WARNING] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| No max torques value found in ini file, default one will be used!
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| max_torques: [   2000.000000    2000.000000     2000.000000     2000.000000     2000.000000     2000.000000  ]
[INFO] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| min_stiffness param found!
[INFO] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| max_stiffness param found!
[INFO] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| min_damping param found!
[INFO] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| max_damping param found!
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| min_stiffness: [   0.000000     0.000000    0.000000    0.000000    0.000000    0.000000  ]
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| max_stiffness: [   1000.000000  1000.000000     1000.000000     1000.000000     1000.000000     1000.000000  ]
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| min_damping: [   0.000000   0.000000    0.000000    0.000000    0.000000    0.000000  ]
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| max_damping: [   100.000000     100.000000  100.000000  100.000000  100.000000  100.000000  ]
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| Initializing Trajectory Generator with current values
[INFO] |yarp.dev.PolyDriver|gazebo_controlboard| Created device <gazebo_controlboard>. See C++ class GazeboYarpControlBoardDriver for documentation.
[INFO] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| Registered YARP device with instance name: iCub::right_leg_hardware_device
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| m_parameters: (IMPEDANCE_CONTROL (controlUnits metric_units) (controlLaw joint_pid_gazebo_v1) (stiffness 0.0 0.0 0.0) (damping 0.0 0.0 0.0)) (LIMITS (jntPosMax 17.0 20.0 45.0) (jntPosMin -30.0 -20.0 -45.0) (jntVelMax 100.0 100.0 100.0)) (POSITION_CONTROL (controlUnits metric_units) (controlLaw joint_pid_gazebo_v1) (kp 1.74500000000000010658 1.74500000000000010658 1.74500000000000010658) (kd 0.121999999999999997335 0.121999999999999997335 0.121999999999999997335) (ki 0.00300000000000000006245 0.00300000000000000006245 0.00300000000000000006245) (maxInt 9999 9999 9999) (maxOutput 9999 9999 9999) (shift 0.0 0.0 0.0) (ko 0.0 0.0 0.0) (stictionUp 0.0 0.0 0.0) (stictionDwn 0.0 0.0 0.0)) (VELOCITY_CONTROL (velocityControlImplementationType integrator_and_position_pid) (controlUnits metric_units) (controlLaw joint_pid_gazebo_v1) (kp 8.72600000000000086686 8.72600000000000086686 8.72600000000000086686) (kd 0.0350000000000000033307 0.0350000000000000033307 0.0350000000000000033307) (ki 0.00300000000000000006245 0.00300000000000000006245 0.00300000000000000006245) (maxInt 9999 9999 9999) (maxOutput 9999 9999 9999) (shift 0.0 0.0 0.0) (ko 0.0 0.0 0.0) (stictionUp 0.0 0.0 0.0) (stictionDwn 0.0 0.0 0.0)) (device gazebo_controlboard) (disableImplicitNetworkWrapper) (gazeboYarpPluginsRobotName iCub) (jointNames neck_pitch neck_roll neck_yaw) (max_damping 100.0 100.0 100.0) (max_stiffness 1000.0 1000.0 1000.0) (min_damping 0.0 0.0 0.0) (min_stiffness 0.0 0.0 0.0) (name "iCub::head_hardware_device") (robotScopedName iCub) (yarpDeviceName head_hardware_device)
[DEBUG] |yarp.dev.PolyDriver|gazebo_controlboard| Parameters are (IMPEDANCE_CONTROL (controlUnits metric_units) (controlLaw joint_pid_gazebo_v1) (stiffness 0.0 0.0 0.0) (damping 0.0 0.0 0.0)) (LIMITS (jntPosMax 17.0 20.0 45.0) (jntPosMin -30.0 -20.0 -45.0) (jntVelMax 100.0 100.0 100.0)) (POSITION_CONTROL (controlUnits metric_units) (controlLaw joint_pid_gazebo_v1) (kp 1.74500000000000010658 1.74500000000000010658 1.74500000000000010658) (kd 0.121999999999999997335 0.121999999999999997335 0.121999999999999997335) (ki 0.00300000000000000006245 0.00300000000000000006245 0.00300000000000000006245) (maxInt 9999 9999 9999) (maxOutput 9999 9999 9999) (shift 0.0 0.0 0.0) (ko 0.0 0.0 0.0) (stictionUp 0.0 0.0 0.0) (stictionDwn 0.0 0.0 0.0)) (VELOCITY_CONTROL (velocityControlImplementationType integrator_and_position_pid) (controlUnits metric_units) (controlLaw joint_pid_gazebo_v1) (kp 8.72600000000000086686 8.72600000000000086686 8.72600000000000086686) (kd 0.0350000000000000033307 0.0350000000000000033307 0.0350000000000000033307) (ki 0.00300000000000000006245 0.00300000000000000006245 0.00300000000000000006245) (maxInt 9999 9999 9999) (maxOutput 9999 9999 9999) (shift 0.0 0.0 0.0) (ko 0.0 0.0 0.0) (stictionUp 0.0 0.0 0.0) (stictionDwn 0.0 0.0 0.0)) (device gazebo_controlboard) (disableImplicitNetworkWrapper) (gazeboYarpPluginsRobotName iCub) (jointNames neck_pitch neck_roll neck_yaw) (max_damping 100.0 100.0 100.0) (max_stiffness 1000.0 1000.0 1000.0) (min_damping 0.0 0.0 0.0) (min_stiffness 0.0 0.0 0.0) (name "iCub::head_hardware_device") (robotScopedName iCub) (yarpDeviceName head_hardware_device)
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| found:  neck_pitch neck_pitch
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| found:  neck_roll neck_roll
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| found:  neck_yaw neck_yaw
[WARNING] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| Missing trajectory_type param. Assuming minimum_jerk
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| Trajectory successfully generated.
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| refSpeed: [   10.000000     10.000000   10.000000  ] 
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| refAcceleration: [   10.000000  10.000000   10.000000  ] 
[WARNING] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| No positionToleranceLinear value found in ini file, default one will be used!
[WARNING] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| No positionToleranceRevolute value found in ini file, default one will be used!
[WARNING] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| No max torques value found in ini file, default one will be used!
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| max_torques: [   2000.000000    2000.000000     2000.000000  ]
[INFO] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| min_stiffness param found!
[INFO] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| max_stiffness param found!
[INFO] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| min_damping param found!
[INFO] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| max_damping param found!
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| min_stiffness: [   0.000000     0.000000    0.000000  ]
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| max_stiffness: [   1000.000000  1000.000000     1000.000000  ]
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| min_damping: [   0.000000   0.000000    0.000000  ]
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| max_damping: [   100.000000     100.000000  100.000000  ]
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| Initializing Trajectory Generator with current values
[INFO] |yarp.dev.PolyDriver|gazebo_controlboard| Created device <gazebo_controlboard>. See C++ class GazeboYarpControlBoardDriver for documentation.
[INFO] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| Registered YARP device with instance name: iCub::head_hardware_device
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| m_parameters: (IMPEDANCE_CONTROL (controlUnits metric_units) (controlLaw joint_pid_gazebo_v1) (stiffness 0.0 0.0 0.0 0.0 0.0 0.0 0.0) (damping 0.0 0.0 0.0 0.0 0.0 0.0 0.0)) (LIMITS (jntPosMax 20.0 160.0 80.0 75.0 88.0 25.0 35.0) (jntPosMin -175.0 12.0 -50.0 -3.0 -88.0 -70.0 -15.0) (jntVelMax 100.0 100.0 100.0 100.0 100.0 100.0 100.0)) (POSITION_CONTROL (controlUnits metric_units) (controlLaw joint_pid_gazebo_v1) (kp 5.74500000000000010658 5.74500000000000010658 5.74500000000000010658 1.74500000000000010658 1.74500000000000010658 1.74500000000000010658 1.74500000000000010658) (kd 0.17399999999999998801 0.17399999999999998801 0.17399999999999998801 0.17399999999999998801 0.17399999999999998801 0.17399999999999998801 0.0) (ki 0.17399999999999998801 0.17399999999999998801 0.17399999999999998801 0.17399999999999998801 0.17399999999999998801 0.17399999999999998801 0.0) (maxInt 9999 9999 9999 9999 9999 9999 9999) (maxOutput 9999 9999 9999 9999 9999 9999 9999) (shift 0.0 0.0 0.0 0.0 0.0 0.0 0.0) (ko 0.0 0.0 0.0 0.0 0.0 0.0 0.0) (stictionUp 0.0 0.0 0.0 0.0 0.0 0.0 0.0) (stictionDwn 0.0 0.0 0.0 0.0 0.0 0.0 0.0)) (TRAJECTORY_GENERATION (trajectory_type minimum_jerk)) (VELOCITY_CONTROL (velocityControlImplementationType integrator_and_position_pid) (controlUnits metric_units) (controlLaw joint_pid_gazebo_v1) (kp 8.72600000000000086686 8.72600000000000086686 8.72600000000000086686 5.23599999999999976552 5.23599999999999976552 5.23599999999999976552 5.23599999999999976552) (kd 0.0350000000000000033307 0.0350000000000000033307 0.0350000000000000033307 0.00200000000000000004163 0.00200000000000000004163 0.00200000000000000004163 0.0) (ki 0.00200000000000000004163 0.00200000000000000004163 0.00200000000000000004163 0.0 0.0 0.0 0.0) (maxInt 9999 9999 9999 9999 9999 9999 9999) (maxOutput 9999 9999 9999 9999 9999 9999 9999) (shift 0.0 0.0 0.0 0.0 0.0 0.0 0.0) (ko 0.0 0.0 0.0 0.0 0.0 0.0 0.0) (stictionUp 0.0 0.0 0.0 0.0 0.0 0.0 0.0) (stictionDwn 0.0 0.0 0.0 0.0 0.0 0.0 0.0)) (device gazebo_controlboard) (disableImplicitNetworkWrapper) (gazeboYarpPluginsRobotName iCub) (jointNames l_shoulder_pitch l_shoulder_roll l_shoulder_yaw l_elbow l_wrist_prosup l_wrist_pitch l_wrist_yaw) (max_damping 100.0 100.0 100.0 100.0 100.0 100.0 100.0) (max_stiffness 1000.0 1000.0 1000.0 1000.0 1000.0 1000.0 1000.0) (min_damping 0.0 0.0 0.0 0.0 0.0 0.0 0.0) (min_stiffness 0.0 0.0 0.0 0.0 0.0 0.0 0.0) (name "iCub::left_arm_hardware_device") (robotScopedName iCub) (yarpDeviceName left_arm_hardware_device)
[DEBUG] |yarp.dev.PolyDriver|gazebo_controlboard| Parameters are (IMPEDANCE_CONTROL (controlUnits metric_units) (controlLaw joint_pid_gazebo_v1) (stiffness 0.0 0.0 0.0 0.0 0.0 0.0 0.0) (damping 0.0 0.0 0.0 0.0 0.0 0.0 0.0)) (LIMITS (jntPosMax 20.0 160.0 80.0 75.0 88.0 25.0 35.0) (jntPosMin -175.0 12.0 -50.0 -3.0 -88.0 -70.0 -15.0) (jntVelMax 100.0 100.0 100.0 100.0 100.0 100.0 100.0)) (POSITION_CONTROL (controlUnits metric_units) (controlLaw joint_pid_gazebo_v1) (kp 5.74500000000000010658 5.74500000000000010658 5.74500000000000010658 1.74500000000000010658 1.74500000000000010658 1.74500000000000010658 1.74500000000000010658) (kd 0.17399999999999998801 0.17399999999999998801 0.17399999999999998801 0.17399999999999998801 0.17399999999999998801 0.17399999999999998801 0.0) (ki 0.17399999999999998801 0.17399999999999998801 0.17399999999999998801 0.17399999999999998801 0.17399999999999998801 0.17399999999999998801 0.0) (maxInt 9999 9999 9999 9999 9999 9999 9999) (maxOutput 9999 9999 9999 9999 9999 9999 9999) (shift 0.0 0.0 0.0 0.0 0.0 0.0 0.0) (ko 0.0 0.0 0.0 0.0 0.0 0.0 0.0) (stictionUp 0.0 0.0 0.0 0.0 0.0 0.0 0.0) (stictionDwn 0.0 0.0 0.0 0.0 0.0 0.0 0.0)) (TRAJECTORY_GENERATION (trajectory_type minimum_jerk)) (VELOCITY_CONTROL (velocityControlImplementationType integrator_and_position_pid) (controlUnits metric_units) (controlLaw joint_pid_gazebo_v1) (kp 8.72600000000000086686 8.72600000000000086686 8.72600000000000086686 5.23599999999999976552 5.23599999999999976552 5.23599999999999976552 5.23599999999999976552) (kd 0.0350000000000000033307 0.0350000000000000033307 0.0350000000000000033307 0.00200000000000000004163 0.00200000000000000004163 0.00200000000000000004163 0.0) (ki 0.00200000000000000004163 0.00200000000000000004163 0.00200000000000000004163 0.0 0.0 0.0 0.0) (maxInt 9999 9999 9999 9999 9999 9999 9999) (maxOutput 9999 9999 9999 9999 9999 9999 9999) (shift 0.0 0.0 0.0 0.0 0.0 0.0 0.0) (ko 0.0 0.0 0.0 0.0 0.0 0.0 0.0) (stictionUp 0.0 0.0 0.0 0.0 0.0 0.0 0.0) (stictionDwn 0.0 0.0 0.0 0.0 0.0 0.0 0.0)) (device gazebo_controlboard) (disableImplicitNetworkWrapper) (gazeboYarpPluginsRobotName iCub) (initialConfiguration "-0.52 0.52 0 0.785 0 0 0.0") (jointNames l_shoulder_pitch l_shoulder_roll l_shoulder_yaw l_elbow l_wrist_prosup l_wrist_pitch l_wrist_yaw) (max_damping 100.0 100.0 100.0 100.0 100.0 100.0 100.0) (max_stiffness 1000.0 1000.0 1000.0 1000.0 1000.0 1000.0 1000.0) (min_damping 0.0 0.0 0.0 0.0 0.0 0.0 0.0) (min_stiffness 0.0 0.0 0.0 0.0 0.0 0.0 0.0) (name "iCub::left_arm_hardware_device") (robotScopedName iCub) (yarpDeviceName left_arm_hardware_device)
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| found:  l_shoulder_pitch l_shoulder_pitch
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| found:  l_shoulder_roll l_shoulder_roll
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| found:  l_shoulder_yaw l_shoulder_yaw
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| found:  l_elbow l_elbow
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| found:  l_wrist_prosup l_wrist_prosup
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| found:  l_wrist_pitch l_wrist_pitch
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| found:  l_wrist_yaw l_wrist_yaw
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| trajectory_type: minimum_jerk
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| Trajectory successfully generated.
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| refSpeed: [   10.000000     10.000000   10.000000   10.000000   10.000000   10.000000   10.000000  ] 
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| refAcceleration: [   10.000000  10.000000   10.000000   10.000000   10.000000   10.000000   10.000000  ] 
[WARNING] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| No positionToleranceLinear value found in ini file, default one will be used!
[WARNING] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| No positionToleranceRevolute value found in ini file, default one will be used!
[WARNING] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| No max torques value found in ini file, default one will be used!
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| max_torques: [   2000.000000    2000.000000     2000.000000     2000.000000     2000.000000     2000.000000     2000.000000  ]
[INFO] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| min_stiffness param found!
[INFO] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| max_stiffness param found!
[INFO] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| min_damping param found!
[INFO] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| max_damping param found!
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| min_stiffness: [   0.000000     0.000000    0.000000    0.000000    0.000000    0.000000    0.000000  ]
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| max_stiffness: [   1000.000000  1000.000000     1000.000000     1000.000000     1000.000000     1000.000000     1000.000000  ]
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| min_damping: [   0.000000   0.000000    0.000000    0.000000    0.000000    0.000000    0.000000  ]
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| max_damping: [   100.000000     100.000000  100.000000  100.000000  100.000000  100.000000  100.000000  ]
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| INITIAL CONFIGURATION IS:  -0.520000    0.520000    0.000000    0.785000    0.000000    0.000000    0.000000
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| Initializing Trajectory Generator with default values
[INFO] |yarp.dev.PolyDriver|gazebo_controlboard| Created device <gazebo_controlboard>. See C++ class GazeboYarpControlBoardDriver for documentation.
[INFO] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| Registered YARP device with instance name: iCub::left_arm_hardware_device
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| m_parameters: (IMPEDANCE_CONTROL (controlUnits metric_units) (controlLaw joint_pid_gazebo_v1) (stiffness 0.0 0.0 0.0 0.0 0.0 0.0 0.0) (damping 0.0 0.0 0.0 0.0 0.0 0.0 0.0)) (LIMITS (jntPosMax 20.0 160.0 80.0 75.0 88.0 25.0 35.0) (jntPosMin -175.0 12.0 -50.0 -3.0 -88.0 -70.0 -15.0) (jntVelMax 100.0 100.0 100.0 100.0 100.0 100.0 100.0)) (POSITION_CONTROL (controlUnits metric_units) (controlLaw joint_pid_gazebo_v1) (kp 5.74500000000000010658 5.74500000000000010658 5.74500000000000010658 1.74500000000000010658 1.74500000000000010658 1.74500000000000010658 1.74500000000000010658) (kd 0.17399999999999998801 0.17399999999999998801 0.17399999999999998801 0.17399999999999998801 0.17399999999999998801 0.17399999999999998801 0.0) (ki 0.17399999999999998801 0.17399999999999998801 0.17399999999999998801 0.17399999999999998801 0.17399999999999998801 0.17399999999999998801 0.0) (maxInt 9999 9999 9999 9999 9999 9999 9999) (maxOutput 9999 9999 9999 9999 9999 9999 9999) (shift 0.0 0.0 0.0 0.0 0.0 0.0 0.0) (ko 0.0 0.0 0.0 0.0 0.0 0.0 0.0) (stictionUp 0.0 0.0 0.0 0.0 0.0 0.0 0.0) (stictionDwn 0.0 0.0 0.0 0.0 0.0 0.0 0.0)) (TRAJECTORY_GENERATION (trajectory_type minimum_jerk)) (VELOCITY_CONTROL (velocityControlImplementationType integrator_and_position_pid) (controlUnits metric_units) (controlLaw joint_pid_gazebo_v1) (kp 8.72600000000000086686 8.72600000000000086686 8.72600000000000086686 5.23599999999999976552 5.23599999999999976552 5.23599999999999976552 5.23599999999999976552) (kd 0.0350000000000000033307 0.0350000000000000033307 0.0350000000000000033307 0.00200000000000000004163 0.00200000000000000004163 0.00200000000000000004163 0.0) (ki 0.00200000000000000004163 0.00200000000000000004163 0.00200000000000000004163 0.0 0.0 0.0 0.0) (maxInt 9999 9999 9999 9999 9999 9999 9999) (maxOutput 9999 9999 9999 9999 9999 9999 9999) (shift 0.0 0.0 0.0 0.0 0.0 0.0 0.0) (ko 0.0 0.0 0.0 0.0 0.0 0.0 0.0) (stictionUp 0.0 0.0 0.0 0.0 0.0 0.0 0.0) (stictionDwn 0.0 0.0 0.0 0.0 0.0 0.0 0.0)) (device gazebo_controlboard) (disableImplicitNetworkWrapper) (gazeboYarpPluginsRobotName iCub) (jointNames r_shoulder_pitch r_shoulder_roll r_shoulder_yaw r_elbow r_wrist_prosup r_wrist_pitch r_wrist_yaw) (max_damping 100.0 100.0 100.0 100.0 100.0 100.0 100.0) (max_stiffness 1000.0 1000.0 1000.0 1000.0 1000.0 1000.0 1000.0) (min_damping 0.0 0.0 0.0 0.0 0.0 0.0 0.0) (min_stiffness 0.0 0.0 0.0 0.0 0.0 0.0 0.0) (name "iCub::right_arm_hardware_device") (robotScopedName iCub) (yarpDeviceName right_arm_hardware_device)
[DEBUG] |yarp.dev.PolyDriver|gazebo_controlboard| Parameters are (IMPEDANCE_CONTROL (controlUnits metric_units) (controlLaw joint_pid_gazebo_v1) (stiffness 0.0 0.0 0.0 0.0 0.0 0.0 0.0) (damping 0.0 0.0 0.0 0.0 0.0 0.0 0.0)) (LIMITS (jntPosMax 20.0 160.0 80.0 75.0 88.0 25.0 35.0) (jntPosMin -175.0 12.0 -50.0 -3.0 -88.0 -70.0 -15.0) (jntVelMax 100.0 100.0 100.0 100.0 100.0 100.0 100.0)) (POSITION_CONTROL (controlUnits metric_units) (controlLaw joint_pid_gazebo_v1) (kp 5.74500000000000010658 5.74500000000000010658 5.74500000000000010658 1.74500000000000010658 1.74500000000000010658 1.74500000000000010658 1.74500000000000010658) (kd 0.17399999999999998801 0.17399999999999998801 0.17399999999999998801 0.17399999999999998801 0.17399999999999998801 0.17399999999999998801 0.0) (ki 0.17399999999999998801 0.17399999999999998801 0.17399999999999998801 0.17399999999999998801 0.17399999999999998801 0.17399999999999998801 0.0) (maxInt 9999 9999 9999 9999 9999 9999 9999) (maxOutput 9999 9999 9999 9999 9999 9999 9999) (shift 0.0 0.0 0.0 0.0 0.0 0.0 0.0) (ko 0.0 0.0 0.0 0.0 0.0 0.0 0.0) (stictionUp 0.0 0.0 0.0 0.0 0.0 0.0 0.0) (stictionDwn 0.0 0.0 0.0 0.0 0.0 0.0 0.0)) (TRAJECTORY_GENERATION (trajectory_type minimum_jerk)) (VELOCITY_CONTROL (velocityControlImplementationType integrator_and_position_pid) (controlUnits metric_units) (controlLaw joint_pid_gazebo_v1) (kp 8.72600000000000086686 8.72600000000000086686 8.72600000000000086686 5.23599999999999976552 5.23599999999999976552 5.23599999999999976552 5.23599999999999976552) (kd 0.0350000000000000033307 0.0350000000000000033307 0.0350000000000000033307 0.00200000000000000004163 0.00200000000000000004163 0.00200000000000000004163 0.0) (ki 0.00200000000000000004163 0.00200000000000000004163 0.00200000000000000004163 0.0 0.0 0.0 0.0) (maxInt 9999 9999 9999 9999 9999 9999 9999) (maxOutput 9999 9999 9999 9999 9999 9999 9999) (shift 0.0 0.0 0.0 0.0 0.0 0.0 0.0) (ko 0.0 0.0 0.0 0.0 0.0 0.0 0.0) (stictionUp 0.0 0.0 0.0 0.0 0.0 0.0 0.0) (stictionDwn 0.0 0.0 0.0 0.0 0.0 0.0 0.0)) (device gazebo_controlboard) (disableImplicitNetworkWrapper) (gazeboYarpPluginsRobotName iCub) (initialConfiguration "-0.52 0.52 0 0.785 0 0 0.0") (jointNames r_shoulder_pitch r_shoulder_roll r_shoulder_yaw r_elbow r_wrist_prosup r_wrist_pitch r_wrist_yaw) (max_damping 100.0 100.0 100.0 100.0 100.0 100.0 100.0) (max_stiffness 1000.0 1000.0 1000.0 1000.0 1000.0 1000.0 1000.0) (min_damping 0.0 0.0 0.0 0.0 0.0 0.0 0.0) (min_stiffness 0.0 0.0 0.0 0.0 0.0 0.0 0.0) (name "iCub::right_arm_hardware_device") (robotScopedName iCub) (yarpDeviceName right_arm_hardware_device)
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| found:  r_shoulder_pitch r_shoulder_pitch
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| found:  r_shoulder_roll r_shoulder_roll
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| found:  r_shoulder_yaw r_shoulder_yaw
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| found:  r_elbow r_elbow
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| found:  r_wrist_prosup r_wrist_prosup
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| found:  r_wrist_pitch r_wrist_pitch
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| found:  r_wrist_yaw r_wrist_yaw
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| trajectory_type: minimum_jerk
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| Trajectory successfully generated.
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| refSpeed: [   10.000000     10.000000   10.000000   10.000000   10.000000   10.000000   10.000000  ] 
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| refAcceleration: [   10.000000  10.000000   10.000000   10.000000   10.000000   10.000000   10.000000  ] 
[WARNING] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| No positionToleranceLinear value found in ini file, default one will be used!
[WARNING] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| No positionToleranceRevolute value found in ini file, default one will be used!
[WARNING] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| No max torques value found in ini file, default one will be used!
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| max_torques: [   2000.000000    2000.000000     2000.000000     2000.000000     2000.000000     2000.000000     2000.000000  ]
[INFO] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| min_stiffness param found!
[INFO] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| max_stiffness param found!
[INFO] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| min_damping param found!
[INFO] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| max_damping param found!
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| min_stiffness: [   0.000000     0.000000    0.000000    0.000000    0.000000    0.000000    0.000000  ]
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| max_stiffness: [   1000.000000  1000.000000     1000.000000     1000.000000     1000.000000     1000.000000     1000.000000  ]
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| min_damping: [   0.000000   0.000000    0.000000    0.000000    0.000000    0.000000    0.000000  ]
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| max_damping: [   100.000000     100.000000  100.000000  100.000000  100.000000  100.000000  100.000000  ]
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| INITIAL CONFIGURATION IS:  -0.520000    0.520000    0.000000    0.785000    0.000000    0.000000    0.000000
[DEBUG] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| Initializing Trajectory Generator with default values
[INFO] |yarp.dev.PolyDriver|gazebo_controlboard| Created device <gazebo_controlboard>. See C++ class GazeboYarpControlBoardDriver for documentation.
[INFO] |gazebo-yarp-plugins.plugins.GazeboYarpControlBoard| Registered YARP device with instance name: iCub::right_arm_hardware_device
[DEBUG] Reading file /home/mebbaid/robot-code/robotology-superbuild/build/install/share/iCub/conf_icub3/icub.xml
[DEBUG] yarprobotinterface: using xml parser for DTD v3.x
[DEBUG] Reading file /home/mebbaid/robot-code/robotology-superbuild/build/install/share/iCub/conf_icub3/icub.xml
[DEBUG] Preprocessor complete in:  0 s
[INFO] startup phase starting...
[INFO] Opening device left_arm-mc_nws_yarp with parameters [("robotName" = "/icubSim"), ("name" = "/icubSim/left_arm")]
[DEBUG] |yarp.dev.PolyDriver|left_arm-mc_nws_yarp| Parameters are (device controlBoard_nws_yarp) (id left_arm-mc_nws_yarp) (name "/icubSim/left_arm") (robotName "/icubSim")
[DEBUG] |yarp.devices.controlBoard_nws_yarp| 'period' parameter missing, using default thread period = 0.02s
[INFO] |yarp.os.Port|/icubSim/left_arm/rpc:i| Port /icubSim/left_arm/rpc:i active at tcp://10.0.0.123:10227/
[INFO] |yarp.os.Port|/icubSim/left_arm/command:i| Port /icubSim/left_arm/command:i active at tcp://10.0.0.123:10228/
[INFO] |yarp.os.Port|/icubSim/left_arm/state:o| Port /icubSim/left_arm/state:o active at tcp://10.0.0.123:10229/
[INFO] |yarp.os.Port|/icubSim/left_arm/stateExt:o| Port /icubSim/left_arm/stateExt:o active at tcp://10.0.0.123:10230/
[INFO] |yarp.dev.PolyDriver|left_arm-mc_nws_yarp| Created wrapper <controlBoard_nws_yarp>. See C++ class ControlBoard_nws_yarp for documentation.
[INFO] Opening device right_arm-mc_nws_yarp with parameters [("robotName" = "/icubSim"), ("name" = "/icubSim/right_arm")]
[DEBUG] |yarp.dev.PolyDriver|right_arm-mc_nws_yarp| Parameters are (device controlBoard_nws_yarp) (id right_arm-mc_nws_yarp) (name "/icubSim/right_arm") (robotName "/icubSim")
[DEBUG] |yarp.devices.controlBoard_nws_yarp| 'period' parameter missing, using default thread period = 0.02s
[INFO] |yarp.os.Port|/icubSim/right_arm/rpc:i| Port /icubSim/right_arm/rpc:i active at tcp://10.0.0.123:10231/
[INFO] |yarp.os.Port|/icubSim/right_arm/command:i| Port /icubSim/right_arm/command:i active at tcp://10.0.0.123:10232/
[INFO] |yarp.os.Port|/icubSim/right_arm/state:o| Port /icubSim/right_arm/state:o active at tcp://10.0.0.123:10233/
[INFO] |yarp.os.Port|/icubSim/right_arm/stateExt:o| Port /icubSim/right_arm/stateExt:o active at tcp://10.0.0.123:10234/
[INFO] |yarp.dev.PolyDriver|right_arm-mc_nws_yarp| Created wrapper <controlBoard_nws_yarp>. See C++ class ControlBoard_nws_yarp for documentation.
[INFO] Opening device left_leg-mc_nws_yarp with parameters [("robotName" = "/icubSim"), ("name" = "/icubSim/left_leg")]
[DEBUG] |yarp.dev.PolyDriver|left_leg-mc_nws_yarp| Parameters are (device controlBoard_nws_yarp) (id left_leg-mc_nws_yarp) (name "/icubSim/left_leg") (robotName "/icubSim")
[DEBUG] |yarp.devices.controlBoard_nws_yarp| 'period' parameter missing, using default thread period = 0.02s
[INFO] |yarp.os.Port|/icubSim/left_leg/rpc:i| Port /icubSim/left_leg/rpc:i active at tcp://10.0.0.123:10239/
[INFO] |yarp.os.Port|/icubSim/left_leg/command:i| Port /icubSim/left_leg/command:i active at tcp://10.0.0.123:10240/
[INFO] |yarp.os.Port|/icubSim/left_leg/state:o| Port /icubSim/left_leg/state:o active at tcp://10.0.0.123:10241/
[INFO] |yarp.os.Port|/icubSim/left_leg/stateExt:o| Port /icubSim/left_leg/stateExt:o active at tcp://10.0.0.123:10242/
[INFO] |yarp.dev.PolyDriver|left_leg-mc_nws_yarp| Created wrapper <controlBoard_nws_yarp>. See C++ class ControlBoard_nws_yarp for documentation.
[INFO] Opening device right_leg-mc_nws_yarp with parameters [("robotName" = "/icubSim"), ("name" = "/icubSim/right_leg")]
[DEBUG] |yarp.dev.PolyDriver|right_leg-mc_nws_yarp| Parameters are (device controlBoard_nws_yarp) (id right_leg-mc_nws_yarp) (name "/icubSim/right_leg") (robotName "/icubSim")
[DEBUG] |yarp.devices.controlBoard_nws_yarp| 'period' parameter missing, using default thread period = 0.02s
[INFO] |yarp.os.Port|/icubSim/right_leg/rpc:i| Port /icubSim/right_leg/rpc:i active at tcp://10.0.0.123:10235/
[INFO] |yarp.os.Port|/icubSim/right_leg/command:i| Port /icubSim/right_leg/command:i active at tcp://10.0.0.123:10236/
[INFO] |yarp.os.Port|/icubSim/right_leg/state:o| Port /icubSim/right_leg/state:o active at tcp://10.0.0.123:10237/
[INFO] |yarp.os.Port|/icubSim/right_leg/stateExt:o| Port /icubSim/right_leg/stateExt:o active at tcp://10.0.0.123:10238/
[INFO] |yarp.dev.PolyDriver|right_leg-mc_nws_yarp| Created wrapper <controlBoard_nws_yarp>. See C++ class ControlBoard_nws_yarp for documentation.
[INFO] Opening device head-mc_nws_yarp with parameters [("robotName" = "/icubSim"), ("name" = "/icubSim/head")]
[DEBUG] |yarp.dev.PolyDriver|head-mc_nws_yarp| Parameters are (device controlBoard_nws_yarp) (id head-mc_nws_yarp) (name "/icubSim/head") (robotName "/icubSim")
[DEBUG] |yarp.devices.controlBoard_nws_yarp| 'period' parameter missing, using default thread period = 0.02s
[INFO] |yarp.os.Port|/icubSim/head/rpc:i| Port /icubSim/head/rpc:i active at tcp://10.0.0.123:10223/
[INFO] |yarp.os.Port|/icubSim/head/command:i| Port /icubSim/head/command:i active at tcp://10.0.0.123:10224/
[INFO] |yarp.os.Port|/icubSim/head/state:o| Port /icubSim/head/state:o active at tcp://10.0.0.123:10225/
[INFO] |yarp.os.Port|/icubSim/head/stateExt:o| Port /icubSim/head/stateExt:o active at tcp://10.0.0.123:10226/
[INFO] |yarp.dev.PolyDriver|head-mc_nws_yarp| Created wrapper <controlBoard_nws_yarp>. See C++ class ControlBoard_nws_yarp for documentation.
[INFO] Opening device torso-mc_nws_yarp with parameters [("robotName" = "/icubSim"), ("name" = "/icubSim/torso")]
[DEBUG] |yarp.dev.PolyDriver|torso-mc_nws_yarp| Parameters are (device controlBoard_nws_yarp) (id torso-mc_nws_yarp) (name "/icubSim/torso") (robotName "/icubSim")
[DEBUG] |yarp.devices.controlBoard_nws_yarp| 'period' parameter missing, using default thread period = 0.02s
[INFO] |yarp.os.Port|/icubSim/torso/rpc:i| Port /icubSim/torso/rpc:i active at tcp://10.0.0.123:10219/
[INFO] |yarp.os.Port|/icubSim/torso/command:i| Port /icubSim/torso/command:i active at tcp://10.0.0.123:10220/
[INFO] |yarp.os.Port|/icubSim/torso/state:o| Port /icubSim/torso/state:o active at tcp://10.0.0.123:10221/
[INFO] |yarp.os.Port|/icubSim/torso/stateExt:o| Port /icubSim/torso/stateExt:o active at tcp://10.0.0.123:10222/
[INFO] |yarp.dev.PolyDriver|torso-mc_nws_yarp| Created wrapper <controlBoard_nws_yarp>. See C++ class ControlBoard_nws_yarp for documentation.
[INFO] Opening device left_arm-mc_remapper with parameters [("robotName" = "/icubSim"), ("networks" = "(left_arm_joints1)"), ("left_arm_joints1" = "( 0 6 0 6 )"), ("joints" = "7")]
[DEBUG] |yarp.dev.PolyDriver|left_arm-mc_remapper| Parameters are (device controlboardremapper) (id left_arm-mc_remapper) (joints 7) (left_arm_joints1 (0 6 0 6)) (networks (left_arm_joints1)) (robotName "/icubSim")
[INFO] |yarp.dev.PolyDriver|left_arm-mc_remapper| Created device <controlboardremapper>. See C++ class ControlBoardRemapper for documentation.
[INFO] Opening device right_arm-mc_remapper with parameters [("robotName" = "/icubSim"), ("networks" = "(right_arm_joints1)"), ("right_arm_joints1" = "( 0 6 0 6 )"), ("joints" = "7")]
[DEBUG] |yarp.dev.PolyDriver|right_arm-mc_remapper| Parameters are (device controlboardremapper) (id right_arm-mc_remapper) (joints 7) (networks (right_arm_joints1)) (right_arm_joints1 (0 6 0 6)) (robotName "/icubSim")
[INFO] |yarp.dev.PolyDriver|right_arm-mc_remapper| Created device <controlboardremapper>. See C++ class ControlBoardRemapper for documentation.
[INFO] Opening device left_leg-mc_remapper with parameters [("robotName" = "/icubSim"), ("networks" = "(left_leg_joints1)"), ("left_leg_joints1" = "( 0 5 0 5 )"), ("joints" = "6")]
[DEBUG] |yarp.dev.PolyDriver|left_leg-mc_remapper| Parameters are (device controlboardremapper) (id left_leg-mc_remapper) (joints 6) (left_leg_joints1 (0 5 0 5)) (networks (left_leg_joints1)) (robotName "/icubSim")
[INFO] |yarp.dev.PolyDriver|left_leg-mc_remapper| Created device <controlboardremapper>. See C++ class ControlBoardRemapper for documentation.
[INFO] Opening device right_leg-mc_remapper with parameters [("robotName" = "/icubSim"), ("networks" = "(right_leg_joints1)"), ("right_leg_joints1" = "( 0 5 0 5 )"), ("joints" = "6")]
[DEBUG] |yarp.dev.PolyDriver|right_leg-mc_remapper| Parameters are (device controlboardremapper) (id right_leg-mc_remapper) (joints 6) (networks (right_leg_joints1)) (right_leg_joints1 (0 5 0 5)) (robotName "/icubSim")
[INFO] |yarp.dev.PolyDriver|right_leg-mc_remapper| Created device <controlboardremapper>. See C++ class ControlBoardRemapper for documentation.
[INFO] Opening device head-mc_remapper with parameters [("robotName" = "/icubSim"), ("networks" = "(head_joints1)"), ("head_joints1" = "( 0 2 0 2 )"), ("joints" = "3")]
[DEBUG] |yarp.dev.PolyDriver|head-mc_remapper| Parameters are (device controlboardremapper) (head_joints1 (0 2 0 2)) (id head-mc_remapper) (joints 3) (networks (head_joints1)) (robotName "/icubSim")
[INFO] |yarp.dev.PolyDriver|head-mc_remapper| Created device <controlboardremapper>. See C++ class ControlBoardRemapper for documentation.
[INFO] Opening device torso-mc_remapper with parameters [("robotName" = "/icubSim"), ("networks" = "(torso_joints)"), ("torso_joints" = "( 0 2 0 2 )"), ("joints" = "3")]
[DEBUG] |yarp.dev.PolyDriver|torso-mc_remapper| Parameters are (device controlboardremapper) (id torso-mc_remapper) (joints 3) (networks (torso_joints)) (robotName "/icubSim") (torso_joints (0 2 0 2))
[INFO] |yarp.dev.PolyDriver|torso-mc_remapper| Created device <controlboardremapper>. See C++ class ControlBoardRemapper for documentation.
[INFO] Opening device left_foot-FT_wrapper with parameters [("robotName" = "/icubSim"), ("period" = "10"), ("name" = "/icubSim/left_foot_heel_tiptoe")]
[DEBUG] |yarp.dev.PolyDriver|left_foot-FT_wrapper| Parameters are (device multipleanalogsensorsserver) (id left_foot-FT_wrapper) (name "/icubSim/left_foot_heel_tiptoe") (period 10) (robotName "/icubSim")
[INFO] |yarp.dev.PolyDriver|left_foot-FT_wrapper| Created wrapper <multipleanalogsensorsserver>. See C++ class MultipleAnalogSensorsServer for documentation.
[INFO] Opening device right_foot-FT_wrapper with parameters [("robotName" = "/icubSim"), ("period" = "10"), ("name" = "/icubSim/right_foot_heel_tiptoe")]
[DEBUG] |yarp.dev.PolyDriver|right_foot-FT_wrapper| Parameters are (device multipleanalogsensorsserver) (id right_foot-FT_wrapper) (name "/icubSim/right_foot_heel_tiptoe") (period 10) (robotName "/icubSim")
[INFO] |yarp.dev.PolyDriver|right_foot-FT_wrapper| Created wrapper <multipleanalogsensorsserver>. See C++ class MultipleAnalogSensorsServer for documentation.
[INFO] Opening device left_foot-FT_remapper with parameters [("robotName" = "/icubSim"), ("SixAxisForceTorqueSensorsNames" = "(l_foot_rear_ft_sensor l_foot_front_ft_sensor)")]
[DEBUG] |yarp.dev.PolyDriver|left_foot-FT_remapper| Parameters are (SixAxisForceTorqueSensorsNames (l_foot_rear_ft_sensor l_foot_front_ft_sensor)) (device multipleanalogsensorsremapper) (id left_foot-FT_remapper) (robotName "/icubSim")
[INFO] |yarp.dev.PolyDriver|left_foot-FT_remapper| Created device <multipleanalogsensorsremapper>. See C++ class MultipleAnalogSensorsRemapper for documentation.
[INFO] Opening device right_foot-FT_remapper with parameters [("robotName" = "/icubSim"), ("SixAxisForceTorqueSensorsNames" = "(r_foot_rear_ft_sensor r_foot_front_ft_sensor)")]
[DEBUG] |yarp.dev.PolyDriver|right_foot-FT_remapper| Parameters are (SixAxisForceTorqueSensorsNames (r_foot_rear_ft_sensor r_foot_front_ft_sensor)) (device multipleanalogsensorsremapper) (id right_foot-FT_remapper) (robotName "/icubSim")
[INFO] |yarp.dev.PolyDriver|right_foot-FT_remapper| Created device <multipleanalogsensorsremapper>. See C++ class MultipleAnalogSensorsRemapper for documentation.
[INFO] Opening device left_arm-FT_wrapper with parameters [("robotName" = "/icubSim"), ("period" = "10"), ("name" = "/icubSim/left_arm")]
[DEBUG] |yarp.dev.PolyDriver|left_arm-FT_wrapper| Parameters are (device multipleanalogsensorsserver) (id left_arm-FT_wrapper) (name "/icubSim/left_arm") (period 10) (robotName "/icubSim")
[INFO] |yarp.dev.PolyDriver|left_arm-FT_wrapper| Created wrapper <multipleanalogsensorsserver>. See C++ class MultipleAnalogSensorsServer for documentation.
[INFO] Opening device right_arm-FT_wrapper with parameters [("robotName" = "/icubSim"), ("period" = "10"), ("name" = "/icubSim/right_arm")]
[DEBUG] |yarp.dev.PolyDriver|right_arm-FT_wrapper| Parameters are (device multipleanalogsensorsserver) (id right_arm-FT_wrapper) (name "/icubSim/right_arm") (period 10) (robotName "/icubSim")
[INFO] |yarp.dev.PolyDriver|right_arm-FT_wrapper| Created wrapper <multipleanalogsensorsserver>. See C++ class MultipleAnalogSensorsServer for documentation.
[INFO] Opening device right_leg_hip-FT_wrapper with parameters [("robotName" = "/icubSim"), ("period" = "10"), ("name" = "/icubSim/right_leg_hip")]
[DEBUG] |yarp.dev.PolyDriver|right_leg_hip-FT_wrapper| Parameters are (device multipleanalogsensorsserver) (id right_leg_hip-FT_wrapper) (name "/icubSim/right_leg_hip") (period 10) (robotName "/icubSim")
[INFO] |yarp.dev.PolyDriver|right_leg_hip-FT_wrapper| Created wrapper <multipleanalogsensorsserver>. See C++ class MultipleAnalogSensorsServer for documentation.
[INFO] Opening device left_leg_hip-FT_wrapper with parameters [("robotName" = "/icubSim"), ("period" = "10"), ("name" = "/icubSim/left_leg_hip")]
[DEBUG] |yarp.dev.PolyDriver|left_leg_hip-FT_wrapper| Parameters are (device multipleanalogsensorsserver) (id left_leg_hip-FT_wrapper) (name "/icubSim/left_leg_hip") (period 10) (robotName "/icubSim")
[INFO] |yarp.dev.PolyDriver|left_leg_hip-FT_wrapper| Created wrapper <multipleanalogsensorsserver>. See C++ class MultipleAnalogSensorsServer for documentation.
[INFO] Opening device head-inertials_wrapper with parameters [("robotName" = "/icubSim"), ("period" = "10"), ("name" = "/icubSim/head/inertials")]
[DEBUG] |yarp.dev.PolyDriver|head-inertials_wrapper| Parameters are (device multipleanalogsensorsserver) (id head-inertials_wrapper) (name "/icubSim/head/inertials") (period 10) (robotName "/icubSim")
[INFO] |yarp.dev.PolyDriver|head-inertials_wrapper| Created wrapper <multipleanalogsensorsserver>. See C++ class MultipleAnalogSensorsServer for documentation.
[INFO] Entering action level 5 of phase startup
[INFO] Executing attach action, level 5 on device left_arm-mc_remapper with parameters [("networks" = "(left_arm_joints1)"), ("left_arm_joints1" = "left_arm_hardware_device")]
[INFO] left_arm-mc_remapper is not an IWrapper. Trying IMultipleWrapper
[INFO] Executing attach action, level 5 on device right_arm-mc_remapper with parameters [("networks" = "(right_arm_joints1)"), ("right_arm_joints1" = "right_arm_hardware_device")]
[INFO] right_arm-mc_remapper is not an IWrapper. Trying IMultipleWrapper
[INFO] Executing attach action, level 5 on device left_leg-mc_remapper with parameters [("networks" = "(left_leg_joints1)"), ("left_leg_joints1" = "left_leg_hardware_device")]
[INFO] left_leg-mc_remapper is not an IWrapper. Trying IMultipleWrapper
[INFO] Executing attach action, level 5 on device right_leg-mc_remapper with parameters [("networks" = "(right_leg_joints1)"), ("right_leg_joints1" = "right_leg_hardware_device")]
[INFO] right_leg-mc_remapper is not an IWrapper. Trying IMultipleWrapper
[INFO] Executing attach action, level 5 on device head-mc_remapper with parameters [("networks" = "(head_joints1)"), ("head_joints1" = "head_hardware_device")]
[INFO] head-mc_remapper is not an IWrapper. Trying IMultipleWrapper
[INFO] Executing attach action, level 5 on device torso-mc_remapper with parameters [("networks" = "(torso_joints)"), ("torso_joints" = "torso_hardware_device")]
[INFO] torso-mc_remapper is not an IWrapper. Trying IMultipleWrapper
[INFO] Executing attach action, level 5 on device left_foot-FT_wrapper with parameters [("networks" = "(FirstStrain)"), ("FirstStrain" = "left_foot-FT_remapper")]
[INFO] left_foot-FT_wrapper is not an IWrapper. Trying IMultipleWrapper
traversaro commented 2 years ago

I found some issues:

This issue have been fixed in https://github.com/robotology/icub-models-generator/pull/222/commits/d3c27c1f2398edb1be75d440468920eae53472ad .

There is also another fix that created the crash when using the iCubGazeboV3 (fixed), that was a bug in gazebo-yarp-plugins, see https://github.com/robotology/gazebo-yarp-plugins/pull/633 .

I also did some cosmetic changes to YARP to increase the debuggability, but for those I will do PR later.

traversaro commented 2 years ago

I started a CI job, so the new models should be in the debug222 branch of icub-models in ~10 minutes.

mebbaid commented 2 years ago

I started a CI job, so the new models should be in the debug222 branch of icub-models in ~10 minutes.

I tried the branch again, and perhaps as expected, gazebo crashes with the error

tStrain" = "icub_left_foot_rear_ft"), ("SecondStrain" = "icub_left_foot_front_ft")]
[ERROR] |yarp.device.multipleanalogsensorsremapper| Impossible to find sensor name l_foot_rear_ft_sensor , exiting.
[ERROR] Device left_foot-FT_remapper cannot execute attach
[ERROR] Cannot run attach action on device left_foot-FT_remapper
[INFO] Executing attach action, level 5 on device right_foot-FT_remapper with parameters [("networks" = "(FirstStrain SecondStrain)"), ("FirstStrain" = "icub_right_foot_rear_ft"), ("SecondStrain" = "icub_right_foot_front_ft")]
[ERROR] |yarp.device.multipleanalogsensorsremapper| Impossible to find sensor name r_foot_rear_ft_sensor , exiting.
[ERROR] Device right_foot-FT_remapper cannot execute attach
[ERROR] Cannot run attach action on device right_foot-FT_remapper
[INFO] All actions for action level 5 of startup phase started. Waiting for unfinished actions.

I think mergin the fix in https://github.com/robotology/gazebo-yarp-plugins/pull/633#event-7268631975 (if I understood correctly) should fix this.

traversaro commented 2 years ago

Yes, that problem should solved by https://github.com/robotology/gazebo-yarp-plugins/pull/633#event-7268631975 .

traversaro commented 2 years ago

Yes, that problem should solved by robotology/gazebo-yarp-plugins#633 (comment) .

The change was merged and released in gazebo-yarp-plugins v4.5.1 (https://github.com/robotology/gazebo-yarp-plugins/releases/tag/v4.5.1). According to @GiulioRomualdi, the PR is working fine, @GiulioRomualdi @mebbaid feel free to approve if you like.