From 883a900fbf870580aeeb5563ce6886f89666b3b6 Mon Sep 17 00:00:00 2001
From: Nicogene <nicolo.genesio@iit.it>
Date: Wed, 18 Dec 2024 12:20:08 +0100
Subject: [PATCH] iCubGazeboV2_5_visuomanip: add support for gz-sim

---
 .../gazebo_icub_eyes.ini                      | 62 ++++++++++---------
 .../gazebo_icub_left_camera_rgbd.ini          |  4 +-
 .../gazebo_icub_left_hand.ini                 | 56 +++++++++++++++++
 .../gazebo_icub_right_camera_rgb.ini          |  4 +-
 .../gazebo_icub_right_hand.ini                | 56 +++++++++++++++++
 .../iCubGazeboV2_5_visuomanip/icub_gz-sim.xml | 47 ++++++++++++++
 .../camera/left_camera-rgb_wrapper_gz-sim.xml | 13 ++++
 .../left_arm-mc_remapper_gz-sim.xml           | 17 +++++
 .../right_arm-mc_remapper_gz-sim.xml          | 17 +++++
 .../iCubGazeboV2_5_visuomanip/model.urdf      | 49 +++++++++++++++
 10 files changed, 295 insertions(+), 30 deletions(-)
 create mode 100644 iCub_manual/conf_manual/iCubGazeboV2_5_visuomanip/gazebo_icub_left_hand.ini
 create mode 100644 iCub_manual/conf_manual/iCubGazeboV2_5_visuomanip/gazebo_icub_right_hand.ini
 create mode 100644 iCub_manual/conf_manual/iCubGazeboV2_5_visuomanip/icub_gz-sim.xml
 create mode 100644 iCub_manual/conf_manual/iCubGazeboV2_5_visuomanip/wrappers/camera/left_camera-rgb_wrapper_gz-sim.xml
 create mode 100644 iCub_manual/conf_manual/iCubGazeboV2_5_visuomanip/wrappers/motorControl/left_arm-mc_remapper_gz-sim.xml
 create mode 100644 iCub_manual/conf_manual/iCubGazeboV2_5_visuomanip/wrappers/motorControl/right_arm-mc_remapper_gz-sim.xml

diff --git a/iCub_manual/conf_manual/iCubGazeboV2_5_visuomanip/gazebo_icub_eyes.ini b/iCub_manual/conf_manual/iCubGazeboV2_5_visuomanip/gazebo_icub_eyes.ini
index 5c61499..912a298 100644
--- a/iCub_manual/conf_manual/iCubGazeboV2_5_visuomanip/gazebo_icub_eyes.ini
+++ b/iCub_manual/conf_manual/iCubGazeboV2_5_visuomanip/gazebo_icub_eyes.ini
@@ -1,55 +1,61 @@
 disableImplicitNetworkWrapper
 yarpDeviceName eyes_hardware_device
 
-jointNames eyes_tilt l_eye_pan_joint r_eye_pan_joint
+jointNames (eyes_tilt l_eye_pan_joint r_eye_pan_joint)
 
-min_stiffness 0.0    0.0    0.0
-max_stiffness 1000.0 1000.0 1000.0
-min_damping   0.0    0.0    0.0
-max_damping   100.0  100.0  100.0
+min_stiffness (0.0    0.0    0.0)
+max_stiffness (1000.0 1000.0 1000.0)
+min_damping   (0.0    0.0    0.0)
+max_damping   (100.0  100.0  100.0)
 
 [TRAJECTORY_GENERATION]
 trajectory_type minimum_jerk
 
 [COUPLING]
+# This is used by gazebo classic control board
 eyes_vergence_control (1 2) (eyes_version eyes_vergence) (-30.0 0.0) (30.0 50.0)
+# This is used by gz-sim control board
+device              couplingICubEye 
+actuatedAxesNames   (eyes_tilt eyes_version eyes_vergence)
+actuatedAxesPosMin  (-30.0  -30.0  0.0)
+actuatedAxesPosMax  (30.0   30.0   50.0)
 
 #PIDs:
 # this information is used to set the PID values in simulation for GAZEBO, we need only the first three values
 [POSITION_CONTROL]
 controlUnits  metric_units
 controlLaw    joint_pid_gazebo_v1
-kp            0.1   0.1   0.1
-kd            0.001 0.001 0.001
-ki            0.0   0.0   0.0
-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
+kp            (0.1   0.1   0.1)
+kd            (0.001 0.001 0.001)
+ki            (0.0   0.0   0.0)
+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            0.01  0.01  0.01
-kd            0.0   0.0   0.0
-ki            0.0   0.0   0.0
-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
+kp            (0.01  0.01  0.01)
+kd            (0.0   0.0   0.0)
+ki            (0.0   0.0   0.0)
+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)
 
 [IMPEDANCE_CONTROL]
 controlUnits  metric_units
 controlLaw    joint_pid_gazebo_v1
-stiffness     0.0   0.0   0.0
-damping       0.0   0.0   0.0
+stiffness     (0.0   0.0   0.0)
+damping       (0.0   0.0   0.0)
 
 [LIMITS]
-jntPosMax 30.0 55.0 30.0
-jntPosMin -30.0 -30.0 -55.0
-jntVelMax 100.0 100.0 100.0
+jntPosMax (30.0 55.0 30.0)
+jntPosMin (-30.0 -30.0 -55.0)
+jntVelMax (100.0 100.0 100.0)
diff --git a/iCub_manual/conf_manual/iCubGazeboV2_5_visuomanip/gazebo_icub_left_camera_rgbd.ini b/iCub_manual/conf_manual/iCubGazeboV2_5_visuomanip/gazebo_icub_left_camera_rgbd.ini
index f7016ff..44afd0a 100644
--- a/iCub_manual/conf_manual/iCubGazeboV2_5_visuomanip/gazebo_icub_left_camera_rgbd.ini
+++ b/iCub_manual/conf_manual/iCubGazeboV2_5_visuomanip/gazebo_icub_left_camera_rgbd.ini
@@ -1,8 +1,10 @@
 disableImplicitNetworkWrapper
 yarpDeviceName icub_left_camera_rgbd
+sensorName left_camera_rgb
+parentLinkName l_eye
 
 [CAMERA_PARAM]
 focalLengthX 343.12110728152936
 focalLengthY 343.12110728152936
 tangentialPointX 0.0
-tangentialPointY 0.0
+tangentialPointY 0.0
\ No newline at end of file
diff --git a/iCub_manual/conf_manual/iCubGazeboV2_5_visuomanip/gazebo_icub_left_hand.ini b/iCub_manual/conf_manual/iCubGazeboV2_5_visuomanip/gazebo_icub_left_hand.ini
new file mode 100644
index 0000000..353a9ca
--- /dev/null
+++ b/iCub_manual/conf_manual/iCubGazeboV2_5_visuomanip/gazebo_icub_left_hand.ini
@@ -0,0 +1,56 @@
+disableImplicitNetworkWrapper
+yarpDeviceName left_hand_hardware_device
+
+jointNames (l_hand_thumb_0_joint l_hand_thumb_1_joint l_hand_thumb_2_joint l_hand_thumb_3_joint l_hand_index_0_joint l_hand_index_1_joint l_hand_index_2_joint l_hand_index_3_joint l_hand_middle_0_joint l_hand_middle_1_joint l_hand_middle_2_joint l_hand_middle_3_joint l_hand_ring_0_joint l_hand_ring_1_joint l_hand_ring_2_joint l_hand_ring_3_joint l_hand_little_0_joint l_hand_little_1_joint l_hand_little_2_joint l_hand_little_3_joint)
+
+min_stiffness (0.0    0.0    0.0    0.0    0.0    0.0    0.0    0.0    0.0    0.0    0.0    0.0    0.0    0.0    0.0    0.0    0.0    0.0    0.0    0.0)
+max_stiffness (1000.0 1000.0 1000.0 1000.0 1000.0 1000.0 1000.0 1000.0 1000.0 1000.0 1000.0 1000.0 1000.0 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    0.0    0.0    0.0    0.0    0.0    0.0    0.0    0.0    0.0    0.0    0.0    0.0    0.0)
+max_damping   (100.0  100.0  100.0  100.0  100.0  100.0  100.0  100.0  100.0  100.0  100.0  100.0  100.0  100.0  100.0  100.0  100.0  100.0  100.0  100.0)
+
+[TRAJECTORY_GENERATION]
+trajectory_type minimum_jerk
+
+[COUPLING]
+device                   couplingICubHandMk2
+actuatedAxesNames        (l_hand_finger l_thumb_oppose l_thumb_proximal l_thumb_distal l_index_proximal l_index_distal l_middle_proximal l_middle_distal l_pinky)
+actuatedAxesPosMin       (0.0  0.0  0.0  0.0   0.0  0.0   0.0  0.0   0.0)
+actuatedAxesPosMax       (60.0 90.0 90.0 180.0 90.0 180.0 90.0 180.0 270.0)
+
+[POSITION_CONTROL]
+controlUnits  metric_units
+controlLaw    joint_pid_gazebo_v1
+kp            (0.1   0.1   0.1   0.1  0.1   0.1   0.1   0.1  0.1   0.1   0.1   0.1  0.1   0.1   0.1   0.1  0.1   0.1   0.1   0.1)
+kd            (0.01  0.01  0.01  0.01 0.01  0.01  0.01  0.01 0.01  0.01  0.01  0.01 0.01  0.01  0.01  0.01 0.01  0.01  0.01  0.01)
+ki            (0.0   0.0   0.0   0.0  0.0   0.0   0.0   0.0  0.0   0.0   0.0   0.0  0.0   0.0   0.0   0.0  0.0   0.0   0.0   0.0)
+maxInt        (9999  9999  9999  9999 9999  9999  9999  9999 9999  9999  9999  9999 9999  9999  9999  9999 9999  9999  9999  9999)
+maxOutput     (9999  9999  9999  9999 9999  9999  9999  9999 9999  9999  9999  9999 9999  9999  9999  9999 9999  9999  9999  9999)
+shift         (0.0   0.0   0.0   0.0  0.0   0.0   0.0   0.0  0.0   0.0   0.0   0.0  0.0   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   0.0  0.0   0.0   0.0   0.0  0.0   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   0.0  0.0   0.0   0.0   0.0  0.0   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   0.0  0.0   0.0   0.0   0.0  0.0   0.0   0.0   0.0  0.0   0.0   0.0   0.0)
+
+[VELOCITY_CONTROL]
+velocityControlImplementationType integrator_and_position_pid
+controlUnits  metric_units
+controlLaw    joint_pid_gazebo_v1
+kp            (8.726 8.726 8.726 5.235 8.726 8.726 8.726 5.235 8.726 8.726 8.726 8.726 8.726 8.726 8.726 8.726 8.726 8.726 8.726 8.726)
+kd            (0.035 0.035 0.035 0.002 0.035 0.035 0.035 0.002 0.035 0.035 0.035 0.035 0.035 0.035 0.035 0.035 0.035 0.035 0.035 0.035)
+ki            (0.002 0.002 0.002 0.0   0.002 0.002 0.002 0.0   0.002 0.002 0.002 0.002 0.002 0.002 0.002 0.002 0.002 0.002 0.002 0.002)
+maxInt        (9999  9999  9999  9999  9999 9999  9999  9999  9999 9999  9999  9999  9999  9999  9999  9999  9999  9999  9999  9999)
+maxOutput     (9999  9999  9999  9999  9999 9999  9999  9999  9999 9999  9999  9999  9999  9999  9999  9999  9999  9999  9999  9999)
+shift         (0.0   0.0   0.0   0.0   0.0  0.0   0.0   0.0   0.0  0.0   0.0   0.0   0.0   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   0.0   0.0  0.0   0.0   0.0   0.0   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   0.0   0.0  0.0   0.0   0.0   0.0   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   0.0   0.0  0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0)
+
+[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   0.0 0.0   0.0   0.0   0.0 0.0   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   0.0 0.0   0.0   0.0   0.0 0.0   0.0   0.0   0.0 0.0   0.0   0.0   0.0)
+
+[LIMITS]
+jntPosMax (0.0    0.0 20.0 20.0 90.0 90.0 90.0 90.0 90.0 90.0 90.0 90.0 90.0 90.0 90.0 90.0 90.0 90.0 90.0 90.0)
+jntPosMin (-20.0  0.0  0.0 0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  90.0 90.0)
+jntVelMax (100.0  100.0 100.0 100.0 100.0 100.0 100.0 100.0 100.0 100.0 100.0 100.0 100.0 100.0 100.0 100.0 100.0 100.0 100.0 100.0)
diff --git a/iCub_manual/conf_manual/iCubGazeboV2_5_visuomanip/gazebo_icub_right_camera_rgb.ini b/iCub_manual/conf_manual/iCubGazeboV2_5_visuomanip/gazebo_icub_right_camera_rgb.ini
index 747805f..6f6be22 100644
--- a/iCub_manual/conf_manual/iCubGazeboV2_5_visuomanip/gazebo_icub_right_camera_rgb.ini
+++ b/iCub_manual/conf_manual/iCubGazeboV2_5_visuomanip/gazebo_icub_right_camera_rgb.ini
@@ -1,4 +1,6 @@
 disableImplicitNetworkWrapper
 yarpDeviceName icub_right_camera_rgb
 framerate 30
-stamp 1
\ No newline at end of file
+stamp 1
+sensorName right_camera_rgb
+parentLinkName r_eye
\ No newline at end of file
diff --git a/iCub_manual/conf_manual/iCubGazeboV2_5_visuomanip/gazebo_icub_right_hand.ini b/iCub_manual/conf_manual/iCubGazeboV2_5_visuomanip/gazebo_icub_right_hand.ini
new file mode 100644
index 0000000..da10dde
--- /dev/null
+++ b/iCub_manual/conf_manual/iCubGazeboV2_5_visuomanip/gazebo_icub_right_hand.ini
@@ -0,0 +1,56 @@
+disableImplicitNetworkWrapper
+yarpDeviceName right_hand_hardware_device
+
+jointNames (r_hand_thumb_0_joint r_hand_thumb_1_joint r_hand_thumb_2_joint r_hand_thumb_3_joint r_hand_index_0_joint r_hand_index_1_joint r_hand_index_2_joint r_hand_index_3_joint r_hand_middle_0_joint r_hand_middle_1_joint r_hand_middle_2_joint r_hand_middle_3_joint r_hand_ring_0_joint r_hand_ring_1_joint r_hand_ring_2_joint r_hand_ring_3_joint r_hand_little_0_joint r_hand_little_1_joint r_hand_little_2_joint r_hand_little_3_joint)
+
+min_stiffness (0.0    0.0    0.0    0.0    0.0    0.0    0.0    0.0    0.0    0.0    0.0    0.0    0.0    0.0    0.0    0.0    0.0    0.0    0.0    0.0)
+max_stiffness (1000.0 1000.0 1000.0 1000.0 1000.0 1000.0 1000.0 1000.0 1000.0 1000.0 1000.0 1000.0 1000.0 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    0.0    0.0    0.0    0.0    0.0    0.0    0.0    0.0    0.0    0.0    0.0    0.0    0.0)
+max_damping   (100.0  100.0  100.0  100.0  100.0  100.0  100.0  100.0  100.0  100.0  100.0  100.0  100.0  100.0  100.0  100.0  100.0  100.0  100.0  100.0)
+
+[TRAJECTORY_GENERATION]
+trajectory_type minimum_jerk
+
+[COUPLING]
+device                   couplingICubHandMk2
+actuatedAxesNames        (r_hand_finger r_thumb_oppose r_thumb_proximal r_thumb_distal r_index_proximal r_index_distal r_middle_proximal r_middle_distal r_pinky)
+actuatedAxesPosMin       (0.0  0.0  0.0  0.0   0.0  0.0   0.0  0.0   0.0)
+actuatedAxesPosMax       (60.0 90.0 90.0 180.0 90.0 180.0 90.0 180.0 270.0)
+
+[POSITION_CONTROL]
+controlUnits  metric_units
+controlLaw    joint_pid_gazebo_v1
+kp            (0.1   0.1   0.1   0.1  0.1   0.1   0.1   0.1  0.1   0.1   0.1   0.1  0.1   0.1   0.1   0.1  0.1   0.1   0.1   0.1)
+kd            (0.01  0.01  0.01  0.01 0.01  0.01  0.01  0.01 0.01  0.01  0.01  0.01 0.01  0.01  0.01  0.01 0.01  0.01  0.01  0.01)
+ki            (0.0   0.0   0.0   0.0  0.0   0.0   0.0   0.0  0.0   0.0   0.0   0.0  0.0   0.0   0.0   0.0  0.0   0.0   0.0   0.0)
+maxInt        (9999  9999  9999  9999 9999  9999  9999  9999 9999  9999  9999  9999 9999  9999  9999  9999 9999  9999  9999  9999)
+maxOutput     (9999  9999  9999  9999 9999  9999  9999  9999 9999  9999  9999  9999 9999  9999  9999  9999 9999  9999  9999  9999)
+shift         (0.0   0.0   0.0   0.0  0.0   0.0   0.0   0.0  0.0   0.0   0.0   0.0  0.0   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   0.0  0.0   0.0   0.0   0.0  0.0   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   0.0  0.0   0.0   0.0   0.0  0.0   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   0.0  0.0   0.0   0.0   0.0  0.0   0.0   0.0   0.0  0.0   0.0   0.0   0.0)
+
+[VELOCITY_CONTROL]
+velocityControlImplementationType integrator_and_position_pid
+controlUnits  metric_units
+controlLaw    joint_pid_gazebo_v1
+kp            (8.726 8.726 8.726 5.235 8.726 8.726 8.726 5.235 8.726 8.726 8.726 8.726 8.726 8.726 8.726 8.726 8.726 8.726 8.726 8.726)
+kd            (0.035 0.035 0.035 0.002 0.035 0.035 0.035 0.002 0.035 0.035 0.035 0.035 0.035 0.035 0.035 0.035 0.035 0.035 0.035 0.035)
+ki            (0.002 0.002 0.002 0.0   0.002 0.002 0.002 0.0   0.002 0.002 0.002 0.002 0.002 0.002 0.002 0.002 0.002 0.002 0.002 0.002)
+maxInt        (9999  9999  9999  9999  9999 9999  9999  9999  9999 9999  9999  9999  9999  9999  9999  9999  9999  9999  9999  9999)
+maxOutput     (9999  9999  9999  9999  9999 9999  9999  9999  9999 9999  9999  9999  9999  9999  9999  9999  9999  9999  9999  9999)
+shift         (0.0   0.0   0.0   0.0   0.0  0.0   0.0   0.0   0.0  0.0   0.0   0.0   0.0   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   0.0   0.0  0.0   0.0   0.0   0.0   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   0.0   0.0  0.0   0.0   0.0   0.0   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   0.0   0.0  0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0)
+
+[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   0.0 0.0   0.0   0.0   0.0 0.0   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   0.0 0.0   0.0   0.0   0.0 0.0   0.0   0.0   0.0 0.0   0.0   0.0   0.0)
+
+[LIMITS]
+jntPosMax (0.0    0.0 20.0 20.0 90.0 90.0 90.0 90.0 90.0 90.0 90.0 90.0 90.0 90.0 90.0 90.0 90.0 90.0 90.0 90.0)
+jntPosMin (-20.0  0.0  0.0 0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  90.0 90.0)
+jntVelMax (100.0  100.0 100.0 100.0 100.0 100.0 100.0 100.0 100.0 100.0 100.0 100.0 100.0 100.0 100.0 100.0 100.0 100.0 100.0 100.0)
diff --git a/iCub_manual/conf_manual/iCubGazeboV2_5_visuomanip/icub_gz-sim.xml b/iCub_manual/conf_manual/iCubGazeboV2_5_visuomanip/icub_gz-sim.xml
new file mode 100644
index 0000000..890d7d8
--- /dev/null
+++ b/iCub_manual/conf_manual/iCubGazeboV2_5_visuomanip/icub_gz-sim.xml
@@ -0,0 +1,47 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<!DOCTYPE robot PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
+
+<robot name="iCubGazeboV2_5" build="1" portprefix="/icubSim" xmlns:xi="http://www.w3.org/2001/XInclude">
+    <devices>
+
+    <!-- MOTOR CONTROLLERS -->
+
+        <!-- wrapper are all inherited from automatically generated models -->
+        <xi:include href="../../../iCub/conf/wrappers/motorControl/torso-mc_wrapper.xml" />
+        <xi:include href="../../../iCub/conf/wrappers/motorControl/head-mc_wrapper.xml" />
+        <xi:include href="../../../iCub/conf/wrappers/motorControl/left_arm-mc_wrapper.xml" />
+        <xi:include href="../../../iCub/conf/wrappers/motorControl/right_arm-mc_wrapper.xml" />
+
+        <!-- only the torso remapper is inherited from automatically generated models -->
+        <xi:include href="../../../iCub/conf/wrappers/motorControl/torso-mc_remapper.xml" />
+
+        <!-- all the other remappers are specific to this model due to eyes and fingers -->
+        <xi:include href="wrappers/motorControl/head-mc_remapper.xml" />
+        <xi:include href="wrappers/motorControl/left_arm-mc_remapper_gz-sim.xml" />
+        <xi:include href="wrappers/motorControl/right_arm-mc_remapper_gz-sim.xml" />
+    <!-- -->
+
+    <!-- HEAD SENSORS -->
+
+        <xi:include href="wrappers/camera/left_camera-rgb_wrapper_gz-sim.xml" />
+        <xi:include href="wrappers/camera/right_camera-rgb_wrapper.xml" />
+
+    <!-- -->
+
+    <!-- INERTIAL SENSORS -->
+
+        <!-- this wrapper is inherited from automatically generated models -->
+        <xi:include href="../../../iCub/conf/wrappers/inertials/head-inertials_wrapper.xml" />
+
+    <!-- -->
+
+    <!-- MAIS (i.e. ENCODER ARRAY) SENSORS -->
+        <!-- FOR NOW THIS PLUGIN IS MISSING IN GZ-SIM -->
+        <!-- <xi:include href="wrappers/MAIS/left_arm-mais_wrapper.xml" />
+        <xi:include href="wrappers/MAIS/right_arm-mais_wrapper.xml" /> -->
+
+    <!-- -->
+
+
+    </devices>
+</robot>
diff --git a/iCub_manual/conf_manual/iCubGazeboV2_5_visuomanip/wrappers/camera/left_camera-rgb_wrapper_gz-sim.xml b/iCub_manual/conf_manual/iCubGazeboV2_5_visuomanip/wrappers/camera/left_camera-rgb_wrapper_gz-sim.xml
new file mode 100644
index 0000000..b8c590f
--- /dev/null
+++ b/iCub_manual/conf_manual/iCubGazeboV2_5_visuomanip/wrappers/camera/left_camera-rgb_wrapper_gz-sim.xml
@@ -0,0 +1,13 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<!DOCTYPE robot PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
+
+<device xmlns:xi="http://www.w3.org/2001/XInclude" name="left_camera_rgb_nws_yarp" type="frameGrabber_nws_yarp">
+    <param name="period">0.033</param>
+    <param name="name"> ${portprefix}/cam/left/rgbImage:o </param>
+    <action phase="startup" level="5" type="attach">
+        <paramlist name="networks">
+            <elem name="the_device"> icub_left_camera_rgbd </elem>
+        </paramlist>
+    </action>
+    <action phase="shutdown" level="5" type="detach" />
+</device>
diff --git a/iCub_manual/conf_manual/iCubGazeboV2_5_visuomanip/wrappers/motorControl/left_arm-mc_remapper_gz-sim.xml b/iCub_manual/conf_manual/iCubGazeboV2_5_visuomanip/wrappers/motorControl/left_arm-mc_remapper_gz-sim.xml
new file mode 100644
index 0000000..5093b6f
--- /dev/null
+++ b/iCub_manual/conf_manual/iCubGazeboV2_5_visuomanip/wrappers/motorControl/left_arm-mc_remapper_gz-sim.xml
@@ -0,0 +1,17 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE devices PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
+
+<device xmlns:xi="http://www.w3.org/2001/XInclude" name="left_arm-mc_remapper" type="controlboardremapper">
+    <paramlist name="networks">
+        <elem name="left_arm_joints1">( 0  6   0 6 )</elem>
+        <elem name="left_arm_joints2">( 7  15  0 8 )</elem>
+    </paramlist>
+    <param name="joints"> 16 </param>
+    <action phase="startup" level="5" type="attach">
+        <paramlist name="networks">
+            <elem name="left_arm_joints1"> left_arm_hardware_device </elem>
+            <elem name="left_arm_joints2"> left_hand_hardware_device </elem>
+        </paramlist>
+    </action>
+    <action phase="shutdown" level="20" type="detach" />
+</device>
diff --git a/iCub_manual/conf_manual/iCubGazeboV2_5_visuomanip/wrappers/motorControl/right_arm-mc_remapper_gz-sim.xml b/iCub_manual/conf_manual/iCubGazeboV2_5_visuomanip/wrappers/motorControl/right_arm-mc_remapper_gz-sim.xml
new file mode 100644
index 0000000..9971493
--- /dev/null
+++ b/iCub_manual/conf_manual/iCubGazeboV2_5_visuomanip/wrappers/motorControl/right_arm-mc_remapper_gz-sim.xml
@@ -0,0 +1,17 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE devices PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
+
+<device xmlns:xi="http://www.w3.org/2001/XInclude" name="right_arm-mc_remapper" type="controlboardremapper">
+    <paramlist name="networks">
+        <elem name="right_arm_joints1">( 0  6   0 6 )</elem>
+        <elem name="right_arm_joints2">( 7  15  0 8 )</elem>
+    </paramlist>
+    <param name="joints"> 16 </param>
+    <action phase="startup" level="5" type="attach">
+        <paramlist name="networks">
+            <elem name="right_arm_joints1"> right_arm_hardware_device </elem>
+            <elem name="right_arm_joints2"> right_hand_hardware_device </elem>
+        </paramlist>
+    </action>
+    <action phase="shutdown" level="20" type="detach" />
+</device>
diff --git a/iCub_manual/robots/iCubGazeboV2_5_visuomanip/model.urdf b/iCub_manual/robots/iCubGazeboV2_5_visuomanip/model.urdf
index 3f3a6af..f6df93d 100644
--- a/iCub_manual/robots/iCubGazeboV2_5_visuomanip/model.urdf
+++ b/iCub_manual/robots/iCubGazeboV2_5_visuomanip/model.urdf
@@ -2753,6 +2753,55 @@
     <yarpRobotInterfaceConfigurationFile>model://iCub/conf_manual/iCubGazeboV2_5_visuomanip/icub.xml</yarpRobotInterfaceConfigurationFile>
   </plugin>
 </gazebo>
+<gazebo>
+  <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+    <yarpConfigurationFile>model://iCub/conf/gazebo_icub_torso.ini</yarpConfigurationFile>
+  </plugin>
+  <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+    <yarpConfigurationFile>model://iCub/conf/gazebo_icub_head_without_eyes.ini</yarpConfigurationFile>
+  </plugin>
+  <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+    <yarpConfigurationFile>model://iCub/conf_manual/iCubGazeboV2_5_visuomanip/gazebo_icub_eyes.ini</yarpConfigurationFile>
+  </plugin>
+  <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+    <yarpConfigurationFile>model://iCub/conf/gazebo_icub_left_arm_no_hand_for_no_hand_model.ini</yarpConfigurationFile>
+    <initialConfiguration>-0.52 0.52 0 0.785 0 0 0.0</initialConfiguration>
+  </plugin>
+  <plugin name='gzyarp::ControlBoard' filename='gz-sim-yarp-controlboard-system'>
+    <yarpConfigurationFile>model://iCub/conf_manual/iCubGazeboV2_5_visuomanip/gazebo_icub_left_hand.ini</yarpConfigurationFile>
+  </plugin>
+  <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+    <yarpConfigurationFile>model://iCub/conf/gazebo_icub_right_arm_no_hand_for_no_hand_model.ini</yarpConfigurationFile>
+    <initialConfiguration>-0.52 0.52 0 0.785 0 0 0.0</initialConfiguration>
+  </plugin>
+  <plugin name='gzyarp::ControlBoard' filename='gz-sim-yarp-controlboard-system'>
+    <yarpConfigurationFile>model://iCub/conf_manual/iCubGazeboV2_5_visuomanip/gazebo_icub_right_hand.ini</yarpConfigurationFile>
+  </plugin>
+  <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+    <yarpConfigurationFile>model://iCub/conf/gazebo_icub_right_leg.ini</yarpConfigurationFile>
+  </plugin>
+  <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+    <yarpConfigurationFile>model://iCub/conf/gazebo_icub_left_leg.ini</yarpConfigurationFile>
+  </plugin>
+</gazebo>
+<gazebo>
+  <plugin name="gzyarp::Camera" filename="gz-sim-yarp-camera-system">
+    <yarpConfigurationFile>model://iCub/conf_manual/iCubGazeboV2_5_visuomanip/gazebo_icub_left_camera_rgbd.ini</yarpConfigurationFile>
+  </plugin>
+  <plugin name="gzyarp::Camera" filename="gz-sim-yarp-camera-system">
+    <yarpConfigurationFile>model://iCub/conf_manual/iCubGazeboV2_5_visuomanip/gazebo_icub_right_camera_rgb.ini</yarpConfigurationFile>
+  </plugin>
+</gazebo>
+<gazebo>
+  <plugin name="gzyarp::Imu" filename="gz-sim-yarp-imu-system">
+    <yarpConfigurationFile>model://iCub/conf/gazebo_icub_inertial.ini</yarpConfigurationFile>
+  </plugin>
+</gazebo>
+<gazebo>
+  <plugin name="gzyarp::RobotInterface" filename="gz-sim-yarp-robotinterface-system">
+    <yarpRobotInterfaceConfigurationFile>model://iCub/conf_manual/iCubGazeboV2_5_visuomanip/icub_gz-sim.xml</yarpRobotInterfaceConfigurationFile>
+  </plugin>
+</gazebo>
 <gazebo reference="r_hip_pitch">
    <implicitSpringDamper>1</implicitSpringDamper>
 </gazebo>