Skip to content
New issue

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

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

Already on GitHub? Sign in to your account

Migrate iCub3 models to use gazebo_yarp_robotinterface plugin #222

Merged
merged 3 commits into from
Sep 28, 2022

Conversation

traversaro
Copy link
Member

@traversaro traversaro commented Jun 19, 2022

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
Copy link
Member Author

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 robotology/icub-models#164 .

Copy link
Member

@Nicogene Nicogene left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

@traversaro
Copy link
Member Author

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
Copy link
Member Author

Requires robotology/gazebo-yarp-plugins#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
Copy link
Member Author

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
Copy link
Contributor

[ ] 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
Copy link
Contributor

  • 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
Copy link
Member Author

[ ] 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
Copy link
Member Author

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 traversaro changed the title [WIP] Migrate iCub3 models to use gazebo_yarp_robotinterface plugin Migrate iCub3 models to use gazebo_yarp_robotinterface plugin Aug 23, 2022
@traversaro traversaro requested review from mebbaid and removed request for prashanthr05 August 24, 2022 10:59
@traversaro
Copy link
Member Author

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
Copy link
Contributor

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
Copy link
Contributor

mebbaid commented Aug 25, 2022

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
Copy link
Contributor

mebbaid commented Aug 25, 2022

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
Copy link
Member Author

I found some issues:

  • Wrong action level for most remapper/wrappers
  • Missing period in controlboard nws, resulting in a period of 0.02 instead of the usual 0.01

This issue have been fixed in d3c27c1 .

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

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

@traversaro
Copy link
Member Author

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

@mebbaid
Copy link
Contributor

mebbaid commented Aug 29, 2022

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 robotology/gazebo-yarp-plugins#633 (comment) (if I understood correctly) should fix this.

@traversaro
Copy link
Member Author

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

@traversaro
Copy link
Member Author

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.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

4 participants