roboticstoolbox-python 1.3.0__cp312-cp312-win_amd64.whl

roboticstoolbox/tools/xacro/tests/robots/pr2/sensors/kinect_prosilica_camera.urdf.xacro

@@ -0,0 +1,181 @@
+<?xml version="1.0"?>
+<root xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
+      xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
+      xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
+      xmlns:xacro="http://ros.org/wiki/xacro">
+  
+  <xacro:include filename="kinect_prosilica_camera.gazebo.xacro" />
+
+  <xacro:property name="ir_depth_rgb_offset_y" value="0.01"/> <!-- FIXME: what is this offset? -->
+
+  <!-- kinect and prosilica combo -->
+  <xacro:macro name="kinect_prosilica_camera_swept_back_v0" params="name parent *origin">
+    <!-- stl contains structure + kinect + prosilica combo -->
+    <joint name="${name}_joint" type="fixed">
+      <!-- <limit lower="0.1" upper="0.1" effort="10000" velocity="100.0"/> -->
+      <!-- <insert_block name="origin" /> -->
+      <origin xyz="-0.138 0 0.091" rpy="0 0 0"/>
+      <parent link="${parent}"/>
+      <child link="${name}_link"/>
+    </joint>
+    <link name="${name}_link">
+      <inertial>
+        <mass value="0.1" />
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <inertia ixx="0.01"  ixy="0.0"  ixz="0.0"
+                 iyy="0.01"  iyz="0.0"
+                 izz="0.01" />
+      </inertial>
+      <visual>
+        <origin xyz="0.0 0.0 0" rpy="0 0 0"/>
+        <geometry>
+          <mesh filename="package://pr2_description/meshes/sensors/kinect_prosilica_v0/115x100_swept_back--coarse.STL" scale="0.001 0.001 0.001" />
+        </geometry>
+        <material name="gray">
+          <color rgba="0.5 0.5 0.5 1"/>
+        </material>
+      </visual>
+      <collision>
+        <origin xyz="0.0 0.0 0" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="0.0005"/>
+        </geometry>
+      </collision>
+    </link>
+
+    <!-- plugin for updating structure pose \
+    <xacro:kinect_prosilica_gazebo joint_name="${name}_kinect_prosilica_joint"
+                                   link_name="${name}_kinect_prosilica_link" />
+    -->
+
+    <!-- kinect ir sensor physical attachment -->
+    <joint name="${name}_kinect_ir_joint" type="fixed">
+      <origin xyz="-0.032267 0.0125 0.136453" rpy="0 0 0"/>
+      <parent link="${name}_link"/>
+      <child link="${name}_kinect_ir_link"/>
+    </joint>
+    <link name="${name}_kinect_ir_link">
+      <inertial>
+        <mass value="0.1" />
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <inertia ixx="0.01"  ixy="0.0"  ixz="0.0"
+                 iyy="0.01"  iyz="0.0"
+                 izz="0.01" />
+      </inertial>
+      <visual>
+        <origin xyz="0.0 0.0 0" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="0.0005"/>
+        </geometry>
+      </visual>
+      <collision>
+        <origin xyz="0.0 0.0 0" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="0.0005"/>
+        </geometry>
+      </collision>
+    </link>
+    <!-- kinect ir sensor frame -->
+    <joint name="${name}_kinect_ir_optical_frame_joint" type="fixed">
+      <origin xyz="${cal_kinect_x}            ${cal_kinect_y + ir_depth_rgb_offset_y}         ${cal_kinect_z}"
+                rpy="${-M_PI/2+cal_kinect_roll} ${0.0+cal_kinect_pitch} ${-M_PI/2+cal_kinect_yaw}" />
+      <parent link="${name}_kinect_ir_link"/>
+      <child link="${name}_kinect_ir_optical_frame"/>
+    </joint>
+
+    <link name="${name}_kinect_ir_optical_frame"/>
+    <!-- gazebo extensions -->
+    <xacro:kinect_ir_gazebo_v0 link_name="${name}_kinect_ir_link" frame_name="${name}_kinect_ir_optical_frame" camera_name="head_mount_kinect" />
+
+
+
+
+    <!-- kinect rgb sensor physical attachment -->
+    <joint name="${name}_kinect_rgb_joint" type="fixed">
+      <origin xyz="0 -0.03 0" rpy="0 0 0"/>
+      <parent link="${name}_kinect_ir_link"/>
+      <child link="${name}_kinect_rgb_link"/>
+    </joint>
+
+    <link name="${name}_kinect_rgb_link" >
+      <inertial>
+        <mass value="0.1" />
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <inertia ixx="0.01"  ixy="0.0"  ixz="0.0"
+                 iyy="0.01"  iyz="0.0"
+                 izz="0.01" />
+      </inertial>
+      <visual>
+        <origin xyz="0.0 0.0 0" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="0.0005"/>
+        </geometry>
+      </visual>
+      <collision>
+        <origin xyz="0.0 0.0 0" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="0.0005"/>
+        </geometry>
+      </collision>
+    </link>
+
+    <!-- kinect rgb sensor frame -->
+    <joint name="${name}_kinect_rgb_optical_frame_joint" type="fixed">
+      <origin xyz="${cal_kinect_x}            ${cal_kinect_y}         ${cal_kinect_z}"
+                rpy="${-M_PI/2+cal_kinect_roll} ${0.0+cal_kinect_pitch} ${-M_PI/2+cal_kinect_yaw}" />
+      <parent link="${name}_kinect_rgb_link"/>
+      <child link="${name}_kinect_rgb_optical_frame"/>
+    </joint>
+
+    <link name="${name}_kinect_rgb_optical_frame"/>
+    <!-- gazebo extensions -->
+    <xacro:kinect_rgb_gazebo_v0 link_name="${name}_kinect_rgb_link" frame_name="${name}_kinect_rgb_optical_frame" camera_name="head_mount_kinect" />
+
+
+
+
+
+    <!-- prosilica sensor physical attachment -->
+    <joint name="${name}_prosilica_joint" type="fixed">
+      <origin xyz="-0.046457 0.0125 0.088921" rpy="0 0 0"/>
+      <parent link="${name}_link"/>
+      <child link="${name}_prosilica_link"/>
+    </joint>
+    <link name="${name}_prosilica_link">
+      <inertial>
+        <mass value="0.1" />
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <inertia ixx="0.01"  ixy="0.0"  ixz="0.0"
+                 iyy="0.01"  iyz="0.0"
+                 izz="0.01" />
+      </inertial>
+      <visual>
+        <origin xyz="0.0 0.0 0" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="0.0005"/>
+        </geometry>
+      </visual>
+      <collision>
+        <origin xyz="0.0 0.0 0" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="0.0005"/>
+        </geometry>
+      </collision>
+    </link>
+    <!-- prosilica sensor frame -->
+    <joint name="${name}_prosilica_optical_frame_joint" type="fixed">
+      <origin xyz="${cal_high_def_x}            ${cal_high_def_y}         ${cal_high_def_z}"
+                rpy="${-M_PI/2+cal_high_def_roll} ${0.0+cal_high_def_pitch} ${-M_PI/2+cal_high_def_yaw}" />
+      <parent link="${name}_prosilica_link"/>
+      <child link="${name}_prosilica_optical_frame"/>
+    </joint>
+
+    <link name="${name}_prosilica_optical_frame"/>
+    <!-- gazebo extensions -->
+    <xacro:prosilica_cam_gazebo_v1 link_name="${name}_prosilica_link" camera_name="prosilica_camera" frame_name="${name}_prosilica_optical_frame" />
+
+  </xacro:macro>
+
+
+</root>
+

roboticstoolbox/tools/xacro/tests/robots/pr2/sensors/microstrain_3dmgx2_imu.gazebo.xacro

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <xacro:macro name="microstrain_3dmgx2_imu_gazebo_v0" params="name imu_topic update_rate stdev">
+    <gazebo>
+      <plugin name="imu_controller" filename="libgazebo_ros_imu.so">
+        <alwaysOn>true</alwaysOn>
+        <updateRate>${update_rate}</updateRate>
+        <bodyName>${name}_link</bodyName>
+        <topicName>${imu_topic}</topicName>
+        <gaussianNoise>${stdev*stdev}</gaussianNoise>
+        <xyzOffset>0 0 0</xyzOffset> 
+        <rpyOffset>0 -180.0 0</rpyOffset>
+        <serviceName>/default_imu</serviceName>
+      </plugin>
+    </gazebo>
+  </xacro:macro>
+
+</robot>

roboticstoolbox/tools/xacro/tests/robots/pr2/sensors/microstrain_3dmgx2_imu.urdf.xacro

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <xacro:include filename="microstrain_3dmgx2_imu.gazebo.xacro" />
+
+  <xacro:macro name="microstrain_3dmgx2_imu_v0" params="name parent *origin imu_topic update_rate stdev">
+    <joint name="${name}_joint" type="fixed">
+      <axis xyz="0 1 0" />
+      <xacro:insert_block name="origin" />
+      <parent link="${parent}_link"/>
+      <child link="${name}_link"/>
+    </joint>
+    <link name="${name}_link">
+      <inertial>
+        <mass value="0.001" />
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.000001" iyz="0" izz="0.0001" />
+      </inertial>
+    </link>
+    <!-- gazebo extensions -->
+    <xacro:microstrain_3dmgx2_imu_gazebo_v0 name="${name}" imu_topic="${imu_topic}" update_rate="${update_rate}" stdev="${stdev}" />
+  </xacro:macro>
+</robot>
+

roboticstoolbox/tools/xacro/tests/robots/pr2/sensors/projector_wg6802418.gazebo.xacro

@@ -0,0 +1,31 @@
+<?xml version="1.0"?>
+<root xmlns:xacro="http://ros.org/wiki/xacro">
+  
+<!-- this controller is pushed into a body scope (previously model scope)
+     this will only work with added r36415 patch in simulator_gazebo stack
+     otherwise projector will not work without removing the reference tag -->
+<xacro:macro name="projector_wg6802418_gazebo_v0" params="name">
+  <gazebo reference="${name}_child_frame">
+    <projector name="${name}_projector_wg6802418">
+      <pose>0 0 0 0 -1.5707 0</pose>
+      <texture>stereo_projection_pattern_high_res_red.png</texture>
+      <fov>${55.0/180.0*M_PI}</fov>
+      <near_clip>0.1</near_clip>
+      <far_clip>10</far_clip>
+    </projector>
+  </gazebo>
+  <gazebo>
+    <plugin name="${name}_controller" filename="libgazebo_ros_projector.so">
+      <projector>${name}_child_frame/${name}_projector_wg6802418</projector>
+      <alwaysOn>true</alwaysOn>
+      <updateRate>15.0</updateRate>
+      <textureName>stereo_projection_pattern_high_res_red.png</textureName>
+      <filterTextureName>stereo_projection_pattern_filter.png</filterTextureName>        
+      <textureTopicName>${name}_controller/image</textureTopicName>
+      <projectorTopicName>${name}_controller/projector</projectorTopicName>
+    </plugin>
+  </gazebo>
+
+</xacro:macro>
+
+</root>

roboticstoolbox/tools/xacro/tests/robots/pr2/sensors/projector_wg6802418.urdf.xacro

@@ -0,0 +1,42 @@
+<?xml version="1.0"?>
+<root xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
+      xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
+      xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
+      xmlns:xacro="http://ros.org/wiki/xacro">
+  
+  <xacro:include filename="projector_wg6802418.gazebo.xacro" />
+
+  <xacro:macro name="projector_wg6802418_v0" params="name parent *origin">
+    <joint name="${name}_frame_joint" type="fixed">
+      <xacro:insert_block name="origin" />
+      <parent link="${parent}"/>
+      <child link="${name}_frame"/>
+    </joint>
+    <link name="${name}_frame">
+      <inertial>
+        <mass value="0.01" />
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <inertia ixx="0.001"  ixy="0.0"  ixz="0.0"
+                 iyy="0.001"  iyz="0.0"
+                 izz="0.001" />
+      </inertial>
+    </link>
+    <joint name="${name}_child_frame_joint" type="fixed">
+      <origin xyz="0 0 0" rpy="0 ${-M_PI/2} 0" />
+      <parent link="${name}_frame"/>
+      <child link="${name}_child_frame"/>
+    </joint>
+    <link name="${name}_child_frame">
+      <inertial>
+        <mass value="0.01" />
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <inertia ixx="0.001"  ixy="0.0"  ixz="0.0"
+                 iyy="0.001"  iyz="0.0"
+                 izz="0.001" />
+      </inertial>
+    </link>
+    <!-- extensions -->
+    <xacro:projector_wg6802418_gazebo_v0 name="${name}"/>
+  </xacro:macro>
+
+</root>

roboticstoolbox/tools/xacro/tests/robots/pr2/sensors/prosilica_gc2450_camera.gazebo.xacro

@@ -0,0 +1,43 @@
+<?xml version="1.0"?>
+<root xmlns:xacro="http://ros.org/wiki/xacro">
+  
+<xacro:macro name="prosilica_cam_gazebo_v0" params="camera_name frame_name">
+  <gazebo reference="${frame_name}_frame">
+    <sensor type="camera" name="${frame_name}_sensor">
+      <always_on>true</always_on>
+      <update_rate>20.0</update_rate>
+      <camera>
+        <horizontal_fov>${M_PI/4}</horizontal_fov>
+        <image>
+          <format>R8G8B8</format>
+          <width>2448</width>
+          <height>2050</height>
+        </image>
+        <clip>
+          <near>0.1</near>
+          <far>100</far>
+        </clip>
+      </camera>
+      <plugin name="${frame_name}_controller" filename="libgazebo_ros_prosilica.so">
+        <updateRate>20.0</updateRate>
+        <cameraName>${camera_name}</cameraName>
+        <imageTopicName>image_raw</imageTopicName>
+        <cameraInfoTopicName>camera_info</cameraInfoTopicName>
+        <pollServiceName>request_image</pollServiceName>
+        <frameName>${frame_name}_optical_frame</frameName>
+        <CxPrime>1224.5</CxPrime>
+        <Cx>1224.5</Cx>
+        <Cy>1025.5</Cy>
+        <focalLength>2954.998083</focalLength>
+        <distortionK1>0.0</distortionK1>
+        <distortionK2>0.0</distortionK2>
+        <distortionK3>0.0</distortionK3>
+        <distortionT1>0.0</distortionT1>
+        <distortionT2>0.0</distortionT2>
+      </plugin>
+    </sensor>
+    <material value="Gazebo/Black" />
+  </gazebo>
+</xacro:macro>
+
+</root>

roboticstoolbox/tools/xacro/tests/robots/pr2/sensors/prosilica_gc2450_camera.urdf.xacro

@@ -0,0 +1,49 @@
+<?xml version="1.0"?>
+<root xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
+      xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
+      xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
+      xmlns:xacro="http://ros.org/wiki/xacro">
+  
+  <xacro:include filename="prosilica_gc2450_camera.gazebo.xacro" />
+
+  <xacro:macro name="prosilica_cam_v0" params="frame_name parent *origin camera_name">
+    <joint name="${frame_name}_frame_joint" type="fixed">
+      <xacro:insert_block name="origin" />
+      <parent link="${parent}"/>
+      <child link="${frame_name}_frame"/>
+    </joint>
+    <link name="${frame_name}_frame">
+
+      <inertial>
+        <mass value="0.01" />
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <inertia ixx="0.001"  ixy="0.0"  ixz="0.0"
+                 iyy="0.001"  iyz="0.0"
+                 izz="0.001" />
+      </inertial>
+
+    </link>
+
+    <joint name="${frame_name}_optical_frame_joint" type="fixed">
+      <origin xyz="${cal_high_def_x}            ${cal_high_def_y}         ${cal_high_def_z}"
+                rpy="${-M_PI/2+cal_high_def_roll} ${0.0+cal_high_def_pitch} ${-M_PI/2+cal_high_def_yaw}" />
+      <parent link="${frame_name}_frame"/>
+      <child link="${frame_name}_optical_frame"/>
+    </joint>
+    <link name="${frame_name}_optical_frame">
+
+      <inertial>
+        <mass value="0.01" />
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <inertia ixx="0.001"  ixy="0.0"  ixz="0.0"
+                 iyy="0.001"  iyz="0.0"
+                 izz="0.001" />
+      </inertial>
+
+    </link>
+
+    <!-- extensions -->
+    <xacro:prosilica_cam_gazebo_v0 camera_name="${camera_name}" frame_name="${frame_name}" />
+  </xacro:macro>
+
+</root>

roboticstoolbox/tools/xacro/tests/robots/pr2/sensors/stereo_camera.gazebo.xacro

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+
+<robot xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
+       xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
+       xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
+       xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <!-- this macro is used for creating wide and narrow double stereo camera in simulation -->
+  <xacro:macro name="stereo_camera_gazebo_v0" params="name focal_length hfov image_width image_height">
+    
+    <gazebo reference="${name}_link">
+      <material value="PR2/Blue" />
+      <turnGravityOff value="true" />
+    </gazebo>
+
+    <gazebo reference="${name}_optical_frame">
+      <material value="Gazebo/White" />
+      <turnGravityOff value="true" />
+    </gazebo>
+
+  </xacro:macro>
+
+</robot>

roboticstoolbox/tools/xacro/tests/robots/pr2/sensors/stereo_camera.urdf.xacro

@@ -0,0 +1,71 @@
+<?xml version="1.0"?>
+
+<robot xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
+       xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
+       xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
+       xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <!-- stereo camera macro uses wge100_camera macros -->
+  <xacro:include filename="wge100_camera.urdf.xacro" />
+
+  <xacro:include filename="stereo_camera.gazebo.xacro" />
+
+  <xacro:property name="stereo_dx" value="0.00" />
+  <xacro:property name="stereo_dy" value="-0.09" /> <!-- +y to the left -->
+  <xacro:property name="stereo_dz" value="0.00" />
+  <xacro:property name="stereo_rx" value="0.00" />
+  <xacro:property name="stereo_ry" value="0.00" />
+  <xacro:property name="stereo_rz" value="0.00" />
+
+  <!-- this macro is used for creating wide and narrow double stereo camera links -->
+  <xacro:macro name="stereo_camera_v0" params="name parent focal_length hfov image_format image_width image_height *origin">
+
+    <joint name="${name}_frame_joint" type="fixed">
+      <xacro:insert_block name="origin" />
+      <parent link="${parent}"/>
+      <child link="${name}_link"/>
+    </joint>
+    <!-- camera link is at center of the optical frame, but in x-forward notation -->
+    <link name="${name}_link">
+      
+      <inertial>
+        <mass value="0.1" />
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <inertia ixx="0.01"  ixy="0"  ixz="0" iyy="0.01"  iyz="0"  izz="0.01" /> <!-- this inertia is made up for now. -->
+      </inertial>
+
+    </link>
+
+    <!-- attach optical frame to the camera link -->
+    <joint name="${name}_optical_frame_joint" type="fixed">
+      <origin xyz="0 0 0" rpy="${-M_PI/2} 0.0 ${-M_PI/2}" /> <!-- rotate frame from x-forward to z-forward camera coords -->
+      <parent link="${name}_link"/>
+      <child link="${name}_optical_frame"/>
+    </joint>
+    <!-- optical frame for the stereo camera, with z-forward notation, this is the frame stereo camera images users should refer to -->
+    <link name="${name}_optical_frame" type="camera"/>
+
+    <!-- stereo left camera -->
+    <xacro:wge100_camera_v0 name="${name}_l_stereo_camera" camera_name="${name}/left" image_format="${image_format}" image_topic_name="image_raw"
+                            camera_info_topic_name="camera_info" parent="${name}_link"
+                            hfov="${hfov}" focal_length="${focal_length}"
+                            frame_id="${name}_optical_frame" hack_baseline="0"
+                            image_width="${image_width}" image_height="${image_height}">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+    </xacro:wge100_camera_v0>
+
+    <!-- stereo right camera -->
+    <xacro:wge100_camera_v0 name="${name}_r_stereo_camera" camera_name="${name}/right" image_format="${image_format}" image_topic_name="image_raw"
+                            camera_info_topic_name="camera_info" parent="${name}_l_stereo_camera_frame"
+                            hfov="${hfov}" focal_length="${focal_length}"
+                            frame_id="${name}_optical_frame" hack_baseline="${-stereo_dy}"
+                            image_width="${image_width}" image_height="${image_height}">
+      <origin xyz="${stereo_dx} ${stereo_dy} ${stereo_dz}" rpy="${stereo_rx} ${stereo_ry} ${stereo_rz}" />
+    </xacro:wge100_camera_v0>
+
+    <!-- extensions -->
+    <xacro:stereo_camera_gazebo_v0 name="${name}" focal_length="${focal_length}" hfov="${hfov}"
+                                   image_width="${image_width}" image_height="${image_height}"/>
+
+  </xacro:macro>
+</robot>

roboticstoolbox/tools/xacro/tests/robots/pr2/sensors/wge100_camera.gazebo.xacro

@@ -0,0 +1,46 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+  <xacro:macro name="wge100_camera_gazebo_v0" params="name camera_name image_format image_topic_name camera_info_topic_name hfov focal_length frame_id hack_baseline image_width image_height">
+    <gazebo reference="${name}_frame">
+      <sensor type="camera" name="${name}_sensor">
+        <always_on>true</always_on>
+        <update_rate>25.0</update_rate>
+        <camera>
+          <horizontal_fov>${hfov*M_PI/180.0}</horizontal_fov>
+          <image>
+            <format>${image_format}</format>
+            <width>${image_width}</width>
+            <height>${image_height}</height>
+          </image>
+          <clip>
+            <near>0.1</near>
+            <far>100</far>
+          </clip>
+        </camera>
+        <plugin name="${name}_controller" filename="libgazebo_ros_camera.so">
+          <alwaysOn>true</alwaysOn>
+          <updateRate>25.0</updateRate>
+          <cameraName>${camera_name}</cameraName>
+          <imageTopicName>${image_topic_name}</imageTopicName>
+          <cameraInfoTopicName>${camera_info_topic_name}</cameraInfoTopicName>
+          <frameName>${frame_id}</frameName>
+          <hackBaseline>${hack_baseline}</hackBaseline>
+          <CxPrime>${(image_width+1)/2}</CxPrime>
+          <Cx>${(image_width+1)/2}</Cx>
+          <Cy>${(image_height+1)/2}</Cy>
+          <!-- image_width / (2*tan(hfov_radian /2)) -->
+          <!-- 320 for wide and 772.55 for narrow stereo camera -->
+          <focalLength>${focal_length}</focalLength>
+          <distortionK1>0.0</distortionK1>
+          <distortionK2>0.0</distortionK2>
+          <distortionK3>0.0</distortionK3>
+          <distortionT1>0.0</distortionT1>
+          <distortionT2>0.0</distortionT2>
+        </plugin>
+      </sensor>
+      <turnGravityOff>true</turnGravityOff>
+      <material>PR2/Blue</material>
+    </gazebo>
+  </xacro:macro>
+
+</robot>

roboticstoolbox/tools/xacro/tests/robots/pr2/sensors/wge100_camera.urdf.xacro

@@ -0,0 +1,47 @@
+<?xml version="1.0"?>
+<!-- XML namespaces -->
+<robot xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
+       xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
+       xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
+       xmlns:xacro="http://ros.org/wiki/xacro">
+  
+  <xacro:include filename="wge100_camera.gazebo.xacro" />
+
+  <!-- hack_baseline is used to simulate right stereo camera projection matrix translation to left stereo camera frame, currently on the hardware,
+       custom left stereo camera frame_id is passed to right stereo wge100 camera node at launch time, baseline is stored on the camera. -->
+  <xacro:macro name="wge100_camera_v0" params="name camera_name image_format image_topic_name camera_info_topic_name parent hfov focal_length frame_id hack_baseline image_width image_height *origin">
+
+    <joint name="${name}_frame_joint" type="fixed">
+      <xacro:insert_block name="origin" />
+      <parent link="${parent}"/>
+      <child link="${name}_frame"/>
+    </joint>
+    <link name="${name}_frame">
+
+      <inertial>
+        <mass value="0.01" />
+        <origin xyz="0 0 0" />
+        <inertia ixx="0.001"  ixy="0.0"  ixz="0.0"
+                 iyy="0.001"  iyz="0.0"
+                 izz="0.001" />
+      </inertial>
+
+    </link>
+
+    <joint name="${name}_optical_frame_joint" type="fixed">
+      <origin xyz="0 0 0" rpy="${-M_PI/2} 0.0 ${-M_PI/2}" />
+      <parent link="${name}_frame" />
+      <child link="${name}_optical_frame"/>
+    </joint>
+    <link name="${name}_optical_frame"/>
+
+    <!-- extensions -->
+    <xacro:wge100_camera_gazebo_v0 name="${name}" camera_name="${camera_name}" image_format="${image_format}" image_topic_name="${image_topic_name}"
+                                   camera_info_topic_name="${camera_info_topic_name}"
+                                   hfov="${hfov}" focal_length="${focal_length}"
+                                   frame_id="${frame_id}" hack_baseline="${hack_baseline}"
+                                   image_width="${image_width}" image_height="${image_height}"/>
+
+  </xacro:macro>
+
+</robot>

roboticstoolbox/tools/xacro/tests/robots/pr2/shoulder_v0/shoulder.gazebo.xacro

@@ -0,0 +1,40 @@
+<?xml version="1.0"?>
+<robot xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
+       xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
+       xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
+       xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <xacro:macro name="pr2_shoulder_gazebo_v0" params="side">
+
+    <!-- Shoulder pan -->
+    <gazebo reference="${side}_shoulder_pan_link">
+      <turnGravityOff>true</turnGravityOff>
+    </gazebo>
+    <gazebo reference="${side}_shoulder_pan_joint">
+      <stopKd value="1.0" />
+      <stopKp value="1000000.0" />
+    </gazebo>
+
+    <!-- Shoulder lift -->
+    <gazebo reference="${side}_shoulder_lift_link">
+      <turnGravityOff>true</turnGravityOff>
+    </gazebo>
+    <gazebo reference="${side}_shoulder_lift_joint">
+      <stopKd value="1.0" />
+      <stopKp value="1000000.0" />
+    </gazebo>
+  </xacro:macro>
+
+  <!-- Upperarm roll: internal fixed attchment point for upper arm -->
+  <xacro:macro name="pr2_upper_arm_roll_gazebo_v0" params="side" >
+    <gazebo reference="${side}_upper_arm_roll_link">
+      <material value="PR2/RollLinks" />
+      <turnGravityOff>true</turnGravityOff>
+    </gazebo>
+    <gazebo reference="${side}_upper_arm_roll_joint">
+      <stopKd value="1.0" />
+      <stopKp value="1000000.0" />
+      <fudgeFactor value="0.5" />
+    </gazebo>
+  </xacro:macro>
+</robot>