Skip to content

Commit

Permalink
Merge pull request #249 from robotology/feat/visuomanipGzSim
Browse files Browse the repository at this point in the history
iCubGazeboV2_5_visuomanip: add support for gz-sim
  • Loading branch information
Nicogene authored Dec 19, 2024
2 parents c3be914 + 883a900 commit 2ba07f7
Show file tree
Hide file tree
Showing 10 changed files with 295 additions and 30 deletions.
Original file line number Diff line number Diff line change
@@ -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)
Original file line number Diff line number Diff line change
@@ -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
Original file line number Diff line number Diff line change
@@ -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)
Original file line number Diff line number Diff line change
@@ -1,4 +1,6 @@
disableImplicitNetworkWrapper
yarpDeviceName icub_right_camera_rgb
framerate 30
stamp 1
stamp 1
sensorName right_camera_rgb
parentLinkName r_eye
Original file line number Diff line number Diff line change
@@ -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)
47 changes: 47 additions & 0 deletions iCub_manual/conf_manual/iCubGazeboV2_5_visuomanip/icub_gz-sim.xml
Original file line number Diff line number Diff line change
@@ -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>
Original file line number Diff line number Diff line change
@@ -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>
Original file line number Diff line number Diff line change
@@ -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>
Original file line number Diff line number Diff line change
@@ -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>
Loading

0 comments on commit 2ba07f7

Please sign in to comment.