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

iCubGazeboV2_5_visuomanip: add support for gz-sim #249

Merged
merged 1 commit into from
Dec 19, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
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
Loading