Skip to content

Conversation

@LeoGori
Copy link

@LeoGori LeoGori commented Aug 29, 2025

Changes explanation

Here, for each file changed, I leave the explanation of the differences between this branch's last commit (75b5543) and the one prior to the changes of this fork (cfec47a)

Laser sensor: adjust max scan angle (360 → 359)

The front laser config reduces max_angle from 360 to 359 degrees, likely to avoid overlap/duplication of the first and last ray when wrapping around a full circle (a common off-by-one issue in 360° scans).

diff --git a/urdf/R1Mk3/conf/gazebo_cer_laser_sensor.ini b/urdf/R1Mk3/conf/gazebo_cer_laser_sensor.ini
index 0f00fb2..1d55d61 100644
--- a/urdf/R1Mk3/conf/gazebo_cer_laser_sensor.ini
+++ b/urdf/R1Mk3/conf/gazebo_cer_laser_sensor.ini
@@ -11,7 +11,7 @@ clip_min_range     0.2
 enable_discard_range 1 
 discard_max_range  5.0
 discard_min_range  0.2 
-max_angle       360 
+max_angle       359 
 min_angle       0  
 resolution      1  
 allow_infinity  1

Added back laser sensor configuration file

Introduces a dedicated back laser INI with 360° sweep, 0.25° resolution, and skip window (57–303) to mask chassis/occlusions.

diff --git a/urdf/R1Mk3/conf/gazebo_cer_lasersensor_back.ini b/urdf/R1Mk3/conf/gazebo_cer_lasersensor_back.ini
new file mode 100644
index 0000000..4f76399
--- /dev/null
+++ b/urdf/R1Mk3/conf/gazebo_cer_lasersensor_back.ini
@@ -0,0 +1,20 @@
+yarpDeviceName laser_back
+disableImplicitNetworkWrapper
+sensorName base_laser2
+parentLinkName mobile_base_body_link
+period 10
+
+[include "./gazebo_cer_robotname.ini"]
+robotName ${gazeboYarpPluginsRobotName}
+      
+[SENSOR]
+max_distance    20.0
+min_distance    0.2 
+max_angle       360 
+min_angle       0  
+resolution      0.25
+allow_infinity  1
+
+[SKIP]
+min  57  
+max  303 

Added front laser sensor configuration file

Adds a dedicated front laser INI mirroring the rear’s ranges and resolution, with a different skip window (35–325).

diff --git a/urdf/R1Mk3/conf/gazebo_cer_lasersensor_front.ini b/urdf/R1Mk3/conf/gazebo_cer_lasersensor_front.ini
new file mode 100644
index 0000000..a9c8de7
--- /dev/null
+++ b/urdf/R1Mk3/conf/gazebo_cer_lasersensor_front.ini
@@ -0,0 +1,21 @@
+yarpDeviceName laser_front
+disableImplicitNetworkWrapper
+
+sensorName base_laser
+parentLinkName mobile_base_body_link
+period 10
+
+[include "./gazebo_cer_robotname.ini"]
+robotName ${gazeboYarpPluginsRobotName}
+
+[SENSOR]
+max_distance    20.0
+min_distance    0.2 
+max_angle       360 
+min_angle       0  
+resolution      0.25
+allow_infinity  1
+
+[SKIP]
+min  35 
+max  325

New odometry device

Creates a wheel odometry device with wheel radius and track parameters, attached to the mobile base hardware.

diff --git a/urdf/R1Mk3/conf/gazebo_cer_odometry/gazebo_cer_odometry.xml b/urdf/R1Mk3/conf/gazebo_cer_odometry/gazebo_cer_odometry.xml
new file mode 100644
index 0000000..57e466c
--- /dev/null
+++ b/urdf/R1Mk3/conf/gazebo_cer_odometry/gazebo_cer_odometry.xml
@@ -0,0 +1,12 @@
+<?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="cer_odometry" type="cerOdometry">
+    <param name="geom_r">   0.16416 </param> <!--//m wheels radius          ((320.0 / 2 / 1000.0)*1.026) -->
+    <param name="geom_l">   0.338   </param> <!--//m wheels distance                                     -->
+    
+    <action phase="startup" level="7" type="attach">
+            <param name="device">  mobile_base_hardware_device </param>
+    </action>
+    <action phase="shutdown" level="3" type="detach" />
+</device>

Odometry network wrappers (ROS 2 and YARP)

Publishes odometry via both ROS 2 (/odometry) and YARP ports; both attach to the same odometry device.

diff --git a/urdf/R1Mk3/conf/gazebo_cer_odometry/gazebo_cer_odometry_nws_ros2.xml b/urdf/R1Mk3/conf/gazebo_cer_odometry/gazebo_cer_odometry_nws_ros2.xml
new file mode 100644
index 0000000..d8f2bb6
--- /dev/null
+++ b/urdf/R1Mk3/conf/gazebo_cer_odometry/gazebo_cer_odometry_nws_ros2.xml
@@ -0,0 +1,14 @@
+<?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="cer_odometry_nws_ros2" type="odometry2D_nws_ros2">
+    <param name="topic_name">   /odometry </param>
+    <param name="node_name">    cer_odometry_nws_ros2 </param>
+    <param name="base_frame">   mobile_base_body_link </param>
+    <param name="odom_frame">   odom </param>
+    
+    <action phase="startup" level="10" type="attach">
+            <param name="device">  cer_odometry </param>
+    </action>
+    <action phase="shutdown" level="15" type="detach" />
+</device>
diff --git a/urdf/R1Mk3/conf/gazebo_cer_odometry/gazebo_cer_odometry_nws_yarp.xml b/urdf/R1Mk3/conf/gazebo_cer_odometry/gazebo_cer_odometry_nws_yarp.xml
new file mode 100644
index 0000000..20df095
--- /dev/null
+++ b/urdf/R1Mk3/conf/gazebo_cer_odometry/gazebo_cer_odometry_nws_yarp.xml
@@ -0,0 +1,13 @@
+<?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="cer_odometry_nws_yarp" type="odometry2D_nws_yarp">
+    <param name="odometry_port_name">  /r1mk3Sim/odometry </param>
+    <param name="odometer_port_name">  /r1mk3Sim/odometer </param>
+    <param name="velocity_port_name">  /r1mk3Sim/velocity </param>
+
+    <action phase="startup" level="10" type="attach">
+            <param name="device">  cer_odometry </param>
+    </action>
+    <action phase="shutdown" level="15" type="detach" />
+</device>

Main robot config updates (r1mk3.xml)

Comments out FT sensor wrappers, adds consolidated motor-control wrappers (YARP + ROS 2), integrates odometry, and switches from a single lidar wrapper to front/back lidar wrappers plus a double-lidar combiner.

diff --git a/urdf/R1Mk3/conf/r1mk3.xml b/urdf/R1Mk3/conf/r1mk3.xml
index d3bab25..3edef34 100644
--- a/urdf/R1Mk3/conf/r1mk3.xml
+++ b/urdf/R1Mk3/conf/r1mk3.xml
@@ -19,12 +19,28 @@
     <xi:include href="wrappers/motorControl/base-mc_remapper.xml" />
 
     <!-- FT SENSORS -->
-    <xi:include href="wrappers/FT/left_arm-FT_wrapper.xml" />
-    <xi:include href="wrappers/FT/right_arm-FT_wrapper.xml" />
+    <!-- <xi:include href="wrappers/FT/left_arm-FT_wrapper.xml" /> -->
+    <!-- <xi:include href="wrappers/FT/right_arm-FT_wrapper.xml" /> -->
+    <!-- ROS2  -->
+    <xi:include href="wrappers/motorControl/alljoints_remapper.xml" />
+    <xi:include href="wrappers/motorControl/alljoints_ros2_wrapper.xml" />
+    <xi:include href="wrappers/motorControl/alljoints_yarp_wrapper.xml" />
+
+    <!-- odometry -->
+    <xi:include href="gazebo_cer_odometry/gazebo_cer_odometry.xml" />
+    <xi:include href="gazebo_cer_odometry/gazebo_cer_odometry_nws_ros2.xml" />
+    <xi:include href="gazebo_cer_odometry/gazebo_cer_odometry_nws_yarp.xml" />
 
     <!-- SENSORS -->
-    <xi:include href="wrappers/sensors/lidar_wrapper.xml" />
+    <!-- <xi:include href="wrappers/sensors/lidar_wrapper.xml" /> -->
     <xi:include href="wrappers/sensors/rgbd_camera_wrapper.xml" />
 
+    <!-- lidars -->
+    <xi:include href="wrappers/sensors/gazebo_cer_lasersensor_back_nws_yarp.xml" />
+    <xi:include href="wrappers/sensors/gazebo_cer_lasersensor_front_nws_yarp.xml" />
+    <xi:include href="wrappers/gazebo_cer_doublelaser/gazebo_cer_cerDoubleLidar.xml" /> 
+    <xi:include href="wrappers/gazebo_cer_doublelaser/gazebo_cer_doublelaser_nws_yarp.xml" />
+    <xi:include href="wrappers/gazebo_cer_doublelaser/gazebo_cer_doublelaser_nws_ros2.xml" />
+
     </devices>
 </robot>

New double-lidar device (combines front & back scans)

Defines cerDoubleLidar that attaches to laser_front and laser_back, exposing a unified scan with shared sensor parameters.

diff --git a/urdf/R1Mk3/conf/wrappers/gazebo_cer_doublelaser/gazebo_cer_cerDoubleLidar.xml b/urdf/R1Mk3/conf/wrappers/gazebo_cer_doublelaser/gazebo_cer_cerDoubleLidar.xml
new file mode 100644
index 0000000..c02451a
--- /dev/null
+++ b/urdf/R1Mk3/conf/wrappers/gazebo_cer_doublelaser/gazebo_cer_cerDoubleLidar.xml
@@ -0,0 +1,35 @@
+<?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="double_laser" type="cerDoubleLidar">  
+
+    <group name="LASERFRONT-CFG">
+        <param name="sensorName">     L_Front    </param>
+        <param name="pose">        0.070 0.0 0.031 0.0          </param>
+        <!-- <param name="file">        ../../gazebo_cer_lasersensor_front.ini         </param> -->
+    </group>
+    <group name="LASERBACK-CFG">
+        <param name="sensorName">     L_Back     </param>
+        <param name="pose">        -0.070 0.0 0.031 3.14159     </param>
+        <!-- <param name="file">        ../../gazebo_cer_lasersensor_back.ini         </param> -->
+    </group>
+    <group name="SENSOR">                      
+        <param name="max_distance">    20.0          </param>
+        <param name="min_distance">    0.2          </param>
+        <param name="max_angle">       360          </param>
+        <param name="min_angle">       0            </param>
+        <param name="resolution">      0.25          </param>
+        <param name="allow_infinity">  1            </param>
+    </group>
+    
+    <action phase="startup" level="3" type="attach">
+        <paramlist name="networks">
+            <elem name="L_Front">  laser_front </elem>
+            <elem name="L_Back">   laser_back </elem>
+        </paramlist>
+    </action>
+    
+    <action phase="shutdown" level="3" type="detach"/>
+
+</device>
diff --git a/urdf/R1Mk3/conf/wrappers/gazebo_cer_doublelaser/gazebo_cer_doublelaser_nws_ros2.xml b/urdf/R1Mk3/conf/wrappers/gazebo_cer_doublelaser/gazebo_cer_doublelaser_nws_ros2.xml
new file mode 100644
index 0000000..7c15187
--- /dev/null
+++ b/urdf/R1Mk3/conf/wrappers/gazebo_cer_doublelaser/gazebo_cer_doublelaser_nws_ros2.xml
@@ -0,0 +1,15 @@
+<?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="lidarWrapR" type="rangefinder2D_nws_ros2">
+    <param name="period"> 0.01 </param>
+    <param name="node_name">    rangefinder_ros_node    </param>
+    <param name="topic_name">   /double_lidar   </param>
+    <param name="frame_id">    mobile_base_double_lidar   </param>
+    <action phase="startup" level="5" type="attach">
+        <paramlist name="networks">
+            <elem name="double_laser">  double_laser </elem> 
+        </paramlist>
+    </action>
+    <action phase="shutdown" level="5" type="detach" />
+</device>
diff --git a/urdf/R1Mk3/conf/wrappers/gazebo_cer_doublelaser/gazebo_cer_doublelaser_nws_yarp.xml b/urdf/R1Mk3/conf/wrappers/gazebo_cer_doublelaser/gazebo_cer_doublelaser_nws_yarp.xml
new file mode 100644
index 0000000..6449748
--- /dev/null
+++ b/urdf/R1Mk3/conf/wrappers/gazebo_cer_doublelaser/gazebo_cer_doublelaser_nws_yarp.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="lidarWrapY" type="rangefinder2D_nws_yarp">
+    <param name="period"> 0.01 </param>
+    <param name="name">   ${portprefix}/laser:o    </param>
+    <action phase="startup" level="5" type="attach">
+        <paramlist name="networks">
+            <elem name="double_laser">  double_laser </elem> 
+        </paramlist>
+    </action>
+    <action phase="shutdown" level="5" type="detach" />
+</device>

New all-joints YARP control wrapper

Adds a consolidated YARP control board wrapper exposing all joints on ${portprefix}/all_joints.

diff --git a/urdf/R1Mk3/conf/wrappers/motorControl/alljoints_yarp_wrapper.xml b/urdf/R1Mk3/conf/wrappers/motorControl/alljoints_yarp_wrapper.xml
new file mode 100644
index 0000000..38de765
--- /dev/null
+++ b/urdf/R1Mk3/conf/wrappers/motorControl/alljoints_yarp_wrapper.xml
@@ -0,0 +1,10 @@
+<?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="cbw_yarp" type="controlBoard_nws_yarp">
+    <param name="name"> ${portprefix}/all_joints </param>
+    <action phase="startup" level="10" type="attach">
+        <param name="device"> cer_all_joints_mc_remapper </param>
+    </action>
+    <action phase="shutdown" level="15" type="detach" />
+</device>
\ No newline at end of file

New front/back lidar YARP wrappers

Publishes separate front and back scans to ${portprefix}/laser/front:o and ${portprefix}/laser/back:o.

diff --git a/urdf/R1Mk3/conf/wrappers/sensors/gazebo_cer_lasersensor_back_nws_yarp.xml b/urdf/R1Mk3/conf/wrappers/sensors/gazebo_cer_lasersensor_back_nws_yarp.xml
new file mode 100644
index 0000000..b145434
--- /dev/null
+++ b/urdf/R1Mk3/conf/wrappers/sensors/gazebo_cer_lasersensor_back_nws_yarp.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="lidarWrapBack" type="rangefinder2D_nws_yarp">
+    <param name="period"> 0.01 </param>
+    <param name="name">   ${portprefix}/laser/back:o    </param>
+    <action phase="startup" level="5" type="attach">
+        <paramlist name="networks">
+            <elem name="Laser2">  laser_back </elem> 
+        </paramlist>
+    </action>
+    <action phase="shutdown" level="5" type="detach" />
+</device>
diff --git a/urdf/R1Mk3/conf/wrappers/sensors/gazebo_cer_lasersensor_front_nws_yarp.xml b/urdf/R1Mk3/conf/wrappers/sensors/gazebo_cer_lasersensor_front_nws_yarp.xml
new file mode 100644
index 0000000..085dd30
--- /dev/null
+++ b/urdf/R1Mk3/conf/wrappers/sensors/gazebo_cer_lasersensor_front_nws_yarp.xml
@@ -0,0 +1,14 @@
+<?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="lidarWrapFront" type="rangefinder2D_nws_yarp">
+    <param name="period"> 0.01 </param>
+    <param name="name">   ${portprefix}/laser/front:o    </param>
+    <action phase="startup" level="5" type="attach">
+        <paramlist name="networks">
+            <elem name="Laser">  laser_front </elem> 
+        </paramlist>
+    </action>
+    <action phase="shutdown" level="5" type="detach" />
+</device>

New mobile base mesh assets

Adds OBJ/MTL for a new mobile base body visual, enabling finer visuals than the previous STL.

diff --git a/urdf/R1Mk3/meshes/obj/sim_cer_mobilebase_body.mtl b/urdf/R1Mk3/meshes/obj/sim_cer_mobilebase_body.mtl
...
diff --git a/urdf/R1Mk3/meshes/obj/sim_cer_mobilebase_body.obj b/urdf/R1Mk3/meshes/obj/sim_cer_mobilebase_body.obj
...

URDF: new visuals, lidar layout, and sensor/arm tweaks

  • Switches visual/collision mesh from STL to new OBJ and lowers the mobile base body by 0.16 m (origin ... -0.16).
  • Adds front, back, and double-lidar links/joints.
  • Reworks Gazebo laser sensors: two separate ray sensors with 0–2π angles, longer range (0.04–20 m), and higher noise stddev (0.1). Each uses the new front/back INI files.
  • Adjusts initial configurations (head and arm) and sets zero friction on base collision surface (μ=μ2=0).
diff --git a/urdf/R1Mk3/robots/R1Mk3Gazebo/model.urdf b/urdf/R1Mk3/robots/R1Mk3Gazebo/model.urdf
index 8258809..7547128 100644
--- a/urdf/R1Mk3/robots/R1Mk3Gazebo/model.urdf
+++ b/urdf/R1Mk3/robots/R1Mk3Gazebo/model.urdf
@@ -13,22 +13,35 @@
       <inertia ixx="0.5475446262144261" ixy="0" ixz="-0.06348414627429329" iyy="0.5797524718439273" iyz="0" izz="0.270222711489103"/>
     </inertial>
     <visual>
-      <origin xyz="0 0 0" rpy="0 -0 0"/>
+      <origin xyz="0 0 -0.16" rpy="0 -0 0"/>
       <geometry>
-        <mesh filename="package://R1Mk3/meshes/simmechanics/sim_emorph_mob_base_body.stl" scale="0.001 0.001 0.001"/>
+        <!-- <mesh filename="package://R1Mk3/meshes/simmechanics/sim_emorph_mob_base_body.stl" scale="0.001 0.001 0.001"/> -->
+        <mesh filename="package://R1Mk3/meshes/obj/sim_cer_mobilebase_body.obj" scale="0.001 0.001 0.001"/>
       </geometry>
       <material name="">
         <color rgba="0.5 0.5 0.5 1"/>
       </material>
     </visual>
     <collision>
-      <origin xyz="0 0 0" rpy="0 -0 0"/>
+      <origin xyz="0 0 -0.16" rpy="0 -0 0"/>
       <geometry>
-        <mesh filename="package://R1Mk3/meshes/simmechanics/sim_emorph_mob_base_body.stl" scale="0.001 0.001 0.001"/>
+        <!-- <mesh filename="package://R1Mk3/meshes/simmechanics/sim_emorph_mob_base_body.stl" scale="0.001 0.001 0.001"/> -->
+        <mesh filename="package://R1Mk3/meshes/obj/sim_cer_mobilebase_body.obj" scale="0.001 0.001 0.001"/>      
       </geometry>
       <material name="">
         <color rgba="0.5 0.5 0.5 1"/>
       </material>
+      <surface>
+        <contact>
+          <ode/>
+        </contact>
+        <friction>
+          <ode>
+            <mu>0</mu>
+            <mu2>0</mu2>
+          </ode>
+        </friction>
+      </surface>
     </collision>
   </link>
   <joint name="mobile_base_l_wheel_joint" type="revolute">
@@ -2023,11 +2036,26 @@
     <parent link="l_hand_thumb_3"/>
     <child link="l_hand_thumb_skin_9"/>
   </joint>
-  <link name="mobile_base_lidar"/>
-  <joint name="mobile_base_lidar_fixed_joint" type="fixed">
-    <origin xyz="0.07 0 0.031" rpy="0 -0 0"/>
+  <link name="mobile_base_lidar_F"/>
+  <joint name="mobile_base_lidar_F_fixed_joint" type="fixed">
+    <origin xyz="0.07 0 0.031" rpy="0 0 0"/>
+    <parent link="mobile_base_body_link"/>
+    <child link="mobile_base_lidar_F"/>
+    <dynamics damping="0.1"/>
+  </joint>
+  <link name="mobile_base_lidar_B"/>
+  <joint name="mobile_base_lidar_B_fixed_joint" type="fixed">
+    <origin xyz="-0.085 0 0.031" rpy="0 0 3.14"/>
     <parent link="mobile_base_body_link"/>
-    <child link="mobile_base_lidar"/>
+    <child link="mobile_base_lidar_B"/>
+    <dynamics damping="0.1"/>
+  </joint>
+  <link name="mobile_base_double_lidar"/>
+  <joint name="mobile_base_double_lidar_fixed_joint" type="fixed">
+    <origin xyz="0.0 0 0.031" rpy="0 0 0"/>
+    <parent link="mobile_base_body_link"/>
+    <child link="mobile_base_double_lidar"/>
+    <dynamics damping="0.1"/>
   </joint>
   <link name="r_hand_index_skin_0"/>
   <joint name="r_hand_index_skin_0_fixed_joint" type="fixed">
@@ -2408,7 +2436,8 @@
   <gazebo>
     <plugin name="controlboard_head" filename="libgazebo_yarp_controlboard.so">
       <yarpConfigurationFile>model://R1Mk3/conf/gazebo_cer_head.ini</yarpConfigurationFile>
-    </plugin>
+      <initialConfiguration>0.4 0.0</initialConfiguration>
+  </plugin>
   </gazebo>
   <gazebo>
     <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
@@ -2418,7 +2447,7 @@
   <gazebo>
     <plugin name="controlboard_left_arm_no_hand" filename="libgazebo_yarp_controlboard.so">
       <yarpConfigurationFile>model://R1Mk3/conf/gazebo_cer_left_arm.ini</yarpConfigurationFile>
-      <initialConfiguration>-0.52 0.52 0 0.785 0 0 0.0</initialConfiguration>
+      <initialConfiguration>-1.02 0.52 0 0.785 0 0 0.0</initialConfiguration>
     </plugin>
   </gazebo>
   <gazebo>
@@ -2466,7 +2495,7 @@
   <gazebo>
     <plugin name="controlboard_right_arm" filename="libgazebo_yarp_controlboard.so">
       <yarpConfigurationFile>model://R1Mk3/conf/gazebo_cer_right_arm.ini</yarpConfigurationFile>
-      <initialConfiguration>-0.52 0.52 0 0.785 0 0 0.0</initialConfiguration>
+      <initialConfiguration>-1.02 0.52 0 0.785 0 0 0.0</initialConfiguration>
     </plugin>
   </gazebo>
   <gazebo>
@@ -2512,8 +2541,13 @@
     </plugin>
   </gazebo>
   <gazebo>
-    <plugin name="gzyarp::Laser" filename="gz-sim-yarp-laser-system">
-      <yarpConfigurationFile>model://R1Mk3/conf/gazebo_cer_laser_sensor.ini</yarpConfigurationFile>
+    <plugin name="gzyarp::Laser" filename="libgazebo_yarp_lasersensor.so">
+      <yarpConfigurationFile>model://R1Mk3/conf/gazebo_cer_lasersensor_front.ini</yarpConfigurationFile>
+    </plugin>
+  </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::Laser" filename="libgazebo_yarp_lasersensor.so">
+      <yarpConfigurationFile>model://R1Mk3/conf/gazebo_cer_lasersensor_back.ini</yarpConfigurationFile>
     </plugin>
   </gazebo>
   <gazebo>
@@ -2697,33 +2731,52 @@
     </force_torque>
     <origin rpy="-3.14159 -2.20185e-17 1.5708 " xyz="0 0 0"/>
   </sensor>
-  <gazebo reference="mobile_base_body_link">
+
+  <!-- Define laser link -->
+  <link name="base_laser_link">
+    <inertial>
+      <mass value="0.001"/>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <inertia ixx="1e-6" iyy="1e-6" izz="1e-6" ixy="0" ixz="0" iyz="0"/>
+    </inertial>
+  </link>
+
+  <!-- Joint to attach laser to the base -->
+  <joint name="base_laser_joint" type="fixed">
+    <parent link="mobile_base_body_link"/>
+    <child link="base_laser_link"/>
+    <origin xyz="0.07 0 0.031" rpy="0 0 0"/>
+  </joint>
+  
+  <gazebo reference="base_laser_link">
     <sensor name="base_laser" type="ray">
       <always_on>1</always_on>
       <update_rate>40.000000</update_rate>
-      <pose>0.07 0 0.031 0 -0 0 </pose>
+      
+      <visualize>1</visualize>
+      
       <ray>
         <scan>
           <horizontal>
             <samples>360</samples>
-            <resolution>1</resolution>
-            <min_angle>-3.14</min_angle>
-            <max_angle>3.14</max_angle>
+            <resolution>4</resolution>
+            <min_angle>0</min_angle>
+            <max_angle>6.28</max_angle>
           </horizontal>
         </scan>
         <range>
-          <min>0.10</min>
-          <max>5.0</max>
-          <resolution>0.01</resolution>
+          <min>0.04</min>
+          <max>20</max>
+          <resolution>0.1</resolution>
         </range>
         <noise>
           <type>gaussian</type>
           <mean>0.0</mean>
-          <stddev>0.005</stddev>
+          <stddev>0.1</stddev>
         </noise>
       </ray>
-      <plugin name="laser_sensor" filename="libgazebo_yarp_lasersensor.so">
-        <yarpConfigurationFile>model://R1Mk3/conf/gazebo_cer_laser_sensor.ini</yarpConfigurationFile>
+      <plugin name="lasersensor_front" filename="libgazebo_yarp_lasersensor.so">
+        <yarpConfigurationFile>model://R1Mk3/conf/gazebo_cer_lasersensor_front.ini</yarpConfigurationFile>
       </plugin>
     </sensor>
   </gazebo>
@@ -2731,6 +2784,60 @@
     <parent link="mobile_base_body_link"/>
     <origin rpy="0 -0 0 " xyz="0.07 0 0.031"/>
   </sensor>
+
+    <!-- Define laser link -->
+  <link name="base_laser2_link">
+    <inertial>
+      <mass value="0.001"/>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <inertia ixx="1e-6" iyy="1e-6" izz="1e-6" ixy="0" ixz="0" iyz="0"/>
+    </inertial>
+  </link>
+
+  <!-- Joint to attach laser to the base -->
+  <joint name="base_laser_joint2" type="fixed">
+    <parent link="mobile_base_body_link"/>
+    <child link="base_laser2_link"/>
+    <origin xyz="-0.085 0 0.031" rpy="0 -0 3.14159"/>
+  </joint>
+
+  <gazebo reference="base_laser2_link">
+    <sensor name="base_laser2" type="ray">
+      <always_on>1</always_on>
+      <update_rate>40.000000</update_rate>
+
+      <visualize>1</visualize>
+
+      <ray>
+        <scan>
+          <horizontal>
+            <samples>360</samples>
+            <resolution>4</resolution>
+            <min_angle>0</min_angle>
+            <max_angle>6.28</max_angle>
+          </horizontal>
+        </scan>
+        <range>
+          <min>0.04</min>
+          <max>20</max>
+          <resolution>0.1</resolution>
+        </range>
+        <noise>
+          <type>gaussian</type>
+          <mean>0.0</mean>
+          <stddev>0.1</stddev>
+        </noise>
+      </ray>
+      <plugin name="lasersensor_back" filename="libgazebo_yarp_lasersensor.so">
+        <yarpConfigurationFile>model://R1Mk3/conf/gazebo_cer_lasersensor_back.ini</yarpConfigurationFile>
+      </plugin>
+    </sensor>
+  </gazebo>
+  <sensor name="base_laser2" type="ray">
+    <parent link="mobile_base_body_link"/>
+    <origin rpy="0 -0 3.14159 " xyz="0.085 0 0.031"/>
+  </sensor>
+
   <gazebo reference="head_link">
     <sensor name="realsense_depth" type="depth">
       <always_on>1</always_on>

URDF Changes (Detailed Explanations)

1) Mobile base visual: mesh swap & Z offset

-      <origin xyz="0 0 0" rpy="0 -0 0"/>
+      <origin xyz="0 0 -0.16" rpy="0 -0 0"/>
       <geometry>
-        <mesh filename="package://R1Mk3/meshes/simmechanics/sim_emorph_mob_base_body.stl" scale="0.001 0.001 0.001"/>
+        <!-- <mesh filename="package://R1Mk3/meshes/simmechanics/sim_emorph_mob_base_body.stl" scale="0.001 0.001 0.001"/> -->
+        <mesh filename="package://R1Mk3/meshes/obj/sim_cer_mobilebase_body.obj" scale="0.001 0.001 0.001"/>

Explanation:

  • Z offset lowered by 0.16 m, aligning the visual better with ground plane.
  • Mesh format changed from STL to OBJ with MTL support, enabling richer visuals.

2) Base collision surface: friction disabled

+      <surface>
+        <contact>
+          <ode/>
+        </contact>
+        <friction>
+          <ode>
+            <mu>0</mu>
+            <mu2>0</mu2>
+          </ode>
+        </friction>
+      </surface>

Explanation:

  • Explicit ODE friction model added with μ=μ2=0 → no friction.
  • Robot base will slide easily; may be intentional for testing or may need realistic friction coefficients.

3) Separate lidar links (front, back, double)

-  <link name="mobile_base_lidar"/>
-  <joint name="mobile_base_lidar_fixed_joint" type="fixed">
-    <origin xyz="0.07 0 0.031" rpy="0 -0 0"/>
+  <link name="mobile_base_lidar_F"/>
+  <joint name="mobile_base_lidar_F_fixed_joint" type="fixed">
+    <origin xyz="0.07 0 0.031" rpy="0 0 0"/>
...
+  <link name="mobile_base_lidar_B"/>
+  <joint name="mobile_base_lidar_B_fixed_joint" type="fixed">
+    <origin xyz="-0.085 0 0.031" rpy="0 0 3.14"/>
...
+  <link name="mobile_base_double_lidar"/>
+  <joint name="mobile_base_double_lidar_fixed_joint" type="fixed">
+    <origin xyz="0.0 0 0.031" rpy="0 0 0"/>

Explanation:

  • Single lidar replaced with three links: front, back, and a combined “double-lidar”.
  • Front faces forward (+x), back faces rear (rotated 180°), double is centered.
  • Enables independent sensors and combined processing.

4) Initial configurations (head & arm)

-    </plugin>
+      <initialConfiguration>0.4 0.0</initialConfiguration>
+  </plugin>
...
-      <initialConfiguration>-0.52 0.52 0 0.785 0 0 0.0</initialConfiguration>
+      <initialConfiguration>-1.02 0.52 0 0.785 0 0 0.0</initialConfiguration>

Explanation:

  • Head: adds 0.4 0.0 startup pose.
  • Arm: first joint moved from -0.52 → -1.02 rad (more tucked).

5) Laser plugin and config split

-    <plugin name="gzyarp::Laser" filename="gz-sim-yarp-laser-system">
-      <yarpConfigurationFile>model://R1Mk3/conf/gazebo_cer_laser_sensor.ini</yarpConfigurationFile>
+    <plugin name="gzyarp::Laser" filename="libgazebo_yarp_lasersensor.so">
+      <yarpConfigurationFile>model://R1Mk3/conf/gazebo_cer_lasersensor_front.ini</yarpConfigurationFile>
     </plugin>
...
+    <plugin name="gzyarp::Laser" filename="libgazebo_yarp_lasersensor.so">
+      <yarpConfigurationFile>model://R1Mk3/conf/gazebo_cer_lasersensor_back.ini</yarpConfigurationFile>
+    </plugin>

Explanation:

  • Plugin library switched to libgazebo_yarp_lasersensor.so.
  • Config split into front and back INIs → more control over each lidar.

6) First ray sensor (front lidar)

-            <resolution>1</resolution>
-            <min_angle>-3.14</min_angle>
-            <max_angle>3.14</max_angle>
+            <resolution>4</resolution>
+            <min_angle>0</min_angle>
+            <max_angle>6.28</max_angle>
...
-          <min>0.10</min>
-          <max>5.0</max>
-          <resolution>0.01</resolution>
+          <min>0.04</min>
+          <max>20</max>
+          <resolution>0.1</resolution>
...
-          <stddev>0.005</stddev>
+          <stddev>0.1</stddev>

Explanation:

  • Angular sweep changed from [-π, π] → [0, 2π].
  • Resolution 1 → 4 (different beam sampling).
  • Range extended (0.1–5.0 m → 0.04–20 m).
  • Coarser range resolution (0.01 → 0.1 m).
  • Noise stddev increased (0.005 → 0.1).
  • Plugin renamed to lasersensor_front with matching INI.

7) Second ray sensor (back lidar)

+  <link name="base_laser2_link">...</link>
+  <joint name="base_laser_joint2" type="fixed">...</joint>
+  <gazebo reference="base_laser2_link">
+    <sensor name="base_laser2" type="ray">...</sensor>
+  </gazebo>

Explanation:

  • Adds a back laser sensor with its own link/joint.
  • Provides separate pose and plugin, completes front/back sensor split.

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.

1 participant