|
@@ -0,0 +1,779 @@
|
|
|
+<sdf version='1.7'>
|
|
|
+ <world name='default'>
|
|
|
+ <actor name="walking_actor">
|
|
|
+ <skin>
|
|
|
+ <filename>moonwalk.dae</filename>
|
|
|
+ <scale>1.0</scale>
|
|
|
+ </skin>
|
|
|
+ <pose>0 0 0 0 0 0</pose>
|
|
|
+ <animation name="walking">
|
|
|
+ <filename>walk.dae</filename>
|
|
|
+ </animation>
|
|
|
+ <script>
|
|
|
+ <trajectory id="0" type="walking">
|
|
|
+ <!-- 定义行人的轨迹点 -->
|
|
|
+ <waypoint>
|
|
|
+ <time>0.0</time>
|
|
|
+ <pose>0 0 0 0 0 0</pose>
|
|
|
+ </waypoint>
|
|
|
+ <waypoint>
|
|
|
+ <time>1.0</time>
|
|
|
+ <pose>1 0 0 0 0 0</pose>
|
|
|
+ </waypoint>
|
|
|
+ <waypoint>
|
|
|
+ <time>2.0</time>
|
|
|
+ <pose>2 0 0 0 0 0</pose>
|
|
|
+ </waypoint>
|
|
|
+ <waypoint>
|
|
|
+ <time>3.0</time>
|
|
|
+ <pose>3 0 0 0 0 0</pose>
|
|
|
+ </waypoint>
|
|
|
+ <waypoint>
|
|
|
+ <time>4.0</time>
|
|
|
+ <pose>5 0 0 0 0 0</pose>
|
|
|
+ </waypoint>
|
|
|
+ <waypoint>
|
|
|
+ <time>7.0</time>
|
|
|
+ <pose>7 0 0 0 0 0</pose>
|
|
|
+ </waypoint>
|
|
|
+ <!-- 你可以根据需要继续添加更多的 waypoints -->
|
|
|
+ </trajectory>
|
|
|
+ </script>
|
|
|
+ </actor>
|
|
|
+ <model name='ground_plane'>
|
|
|
+ <static>1</static>
|
|
|
+ <link name='link'>
|
|
|
+ <collision name='collision'>
|
|
|
+ <geometry>
|
|
|
+ <plane>
|
|
|
+ <normal>0 0 1</normal>
|
|
|
+ <size>100 100</size>
|
|
|
+ </plane>
|
|
|
+ </geometry>
|
|
|
+ <surface>
|
|
|
+ <contact>
|
|
|
+ <collide_bitmask>65535</collide_bitmask>
|
|
|
+ <ode/>
|
|
|
+ </contact>
|
|
|
+ <friction>
|
|
|
+ <ode>
|
|
|
+ <mu>100</mu>
|
|
|
+ <mu2>50</mu2>
|
|
|
+ </ode>
|
|
|
+ <torsional>
|
|
|
+ <ode/>
|
|
|
+ </torsional>
|
|
|
+ </friction>
|
|
|
+ <bounce/>
|
|
|
+ </surface>
|
|
|
+ <max_contacts>10</max_contacts>
|
|
|
+ </collision>
|
|
|
+ <visual name='visual'>
|
|
|
+ <cast_shadows>0</cast_shadows>
|
|
|
+ <geometry>
|
|
|
+ <plane>
|
|
|
+ <normal>0 0 1</normal>
|
|
|
+ <size>100 100</size>
|
|
|
+ </plane>
|
|
|
+ </geometry>
|
|
|
+ <material>
|
|
|
+ <script>
|
|
|
+ <uri>file://media/materials/scripts/gazebo.material</uri>
|
|
|
+ <name>Gazebo/Grey</name>
|
|
|
+ </script>
|
|
|
+ </material>
|
|
|
+ </visual>
|
|
|
+ <self_collide>0</self_collide>
|
|
|
+ <enable_wind>0</enable_wind>
|
|
|
+ <kinematic>0</kinematic>
|
|
|
+ </link>
|
|
|
+ </model>
|
|
|
+ <light name='sun' type='directional'>
|
|
|
+ <cast_shadows>1</cast_shadows>
|
|
|
+ <pose>0 0 10 0 -0 0</pose>
|
|
|
+ <diffuse>0.8 0.8 0.8 1</diffuse>
|
|
|
+ <specular>0.2 0.2 0.2 1</specular>
|
|
|
+ <attenuation>
|
|
|
+ <range>1000</range>
|
|
|
+ <constant>0.9</constant>
|
|
|
+ <linear>0.01</linear>
|
|
|
+ <quadratic>0.001</quadratic>
|
|
|
+ </attenuation>
|
|
|
+ <direction>-0.5 0.1 -0.9</direction>
|
|
|
+ <spot>
|
|
|
+ <inner_angle>0</inner_angle>
|
|
|
+ <outer_angle>0</outer_angle>
|
|
|
+ <falloff>0</falloff>
|
|
|
+ </spot>
|
|
|
+ </light>
|
|
|
+ <model name='map'>
|
|
|
+ <link name='link'>
|
|
|
+ <inertial>
|
|
|
+ <mass>15</mass>
|
|
|
+ <inertia>
|
|
|
+ <ixx>0</ixx>
|
|
|
+ <ixy>0</ixy>
|
|
|
+ <ixz>0</ixz>
|
|
|
+ <iyy>0</iyy>
|
|
|
+ <iyz>0</iyz>
|
|
|
+ <izz>0</izz>
|
|
|
+ </inertia>
|
|
|
+ </inertial>
|
|
|
+ <collision name='collision'>
|
|
|
+ <pose>0 0 0 0 -0 0</pose>
|
|
|
+ <geometry>
|
|
|
+ <mesh>
|
|
|
+ <uri>model://map/meshes/map.stl</uri>
|
|
|
+ </mesh>
|
|
|
+ </geometry>
|
|
|
+ <max_contacts>10</max_contacts>
|
|
|
+ <surface>
|
|
|
+ <contact>
|
|
|
+ <ode/>
|
|
|
+ </contact>
|
|
|
+ <bounce/>
|
|
|
+ <friction>
|
|
|
+ <torsional>
|
|
|
+ <ode/>
|
|
|
+ </torsional>
|
|
|
+ <ode/>
|
|
|
+ </friction>
|
|
|
+ </surface>
|
|
|
+ </collision>
|
|
|
+ <visual name='visual'>
|
|
|
+ <pose>0 0 0 0 -0 0</pose>
|
|
|
+ <geometry>
|
|
|
+ <mesh>
|
|
|
+ <uri>model://map/meshes/map.stl</uri>
|
|
|
+ </mesh>
|
|
|
+ </geometry>
|
|
|
+ </visual>
|
|
|
+ <self_collide>0</self_collide>
|
|
|
+ <enable_wind>0</enable_wind>
|
|
|
+ <kinematic>0</kinematic>
|
|
|
+ </link>
|
|
|
+ <static>1</static>
|
|
|
+ </model>
|
|
|
+ <gravity>0 0 -9.8</gravity>
|
|
|
+ <magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
|
|
|
+ <atmosphere type='adiabatic'/>
|
|
|
+ <physics type='ode'>
|
|
|
+ <max_step_size>0.001</max_step_size>
|
|
|
+ <real_time_factor>1</real_time_factor>
|
|
|
+ <real_time_update_rate>1000</real_time_update_rate>
|
|
|
+ </physics>
|
|
|
+ <scene>
|
|
|
+ <ambient>0.4 0.4 0.4 1</ambient>
|
|
|
+ <background>0.7 0.7 0.7 1</background>
|
|
|
+ <shadows>1</shadows>
|
|
|
+ </scene>
|
|
|
+ <wind/>
|
|
|
+ <spherical_coordinates>
|
|
|
+ <surface_model>EARTH_WGS84</surface_model>
|
|
|
+ <latitude_deg>0</latitude_deg>
|
|
|
+ <longitude_deg>0</longitude_deg>
|
|
|
+ <elevation>0</elevation>
|
|
|
+ <heading_deg>0</heading_deg>
|
|
|
+ </spherical_coordinates>
|
|
|
+ <state world_name='default'>
|
|
|
+ <sim_time>292 65000000</sim_time>
|
|
|
+ <real_time>268 582679075</real_time>
|
|
|
+ <wall_time>1724209719 851567727</wall_time>
|
|
|
+ <iterations>265335</iterations>
|
|
|
+ <model name='ground_plane'>
|
|
|
+ <pose>0 0 0 0 -0 0</pose>
|
|
|
+ <scale>1 1 1</scale>
|
|
|
+ <link name='link'>
|
|
|
+ <pose>0 0 0 0 -0 0</pose>
|
|
|
+ <velocity>0 0 0 0 -0 0</velocity>
|
|
|
+ <acceleration>0 0 0 0 -0 0</acceleration>
|
|
|
+ <wrench>0 0 0 0 -0 0</wrench>
|
|
|
+ </link>
|
|
|
+ </model>
|
|
|
+ <model name='map'>
|
|
|
+ <pose>0 0 0 0 -0 0</pose>
|
|
|
+ <scale>1 1 1</scale>
|
|
|
+ <link name='link'>
|
|
|
+ <pose>0 0 0 0 -0 0</pose>
|
|
|
+ <velocity>0 0 0 0 -0 0</velocity>
|
|
|
+ <acceleration>0 0 0 0 -0 0</acceleration>
|
|
|
+ <wrench>0 0 0 0 -0 0</wrench>
|
|
|
+ </link>
|
|
|
+ </model>
|
|
|
+ <model name='mycar'>
|
|
|
+ <pose>2.2279 3.2462 -1.2e-05 3e-06 0 -2.93302</pose>
|
|
|
+ <scale>1 1 1</scale>
|
|
|
+ <link name='back_wheel'>
|
|
|
+ <pose>2.46515 3.29642 0.007501 -0.416959 -0.62943 2.93266</pose>
|
|
|
+ <velocity>-3.9e-05 0.0002 0.006698 -0.025614 -0.005962 0.006021</velocity>
|
|
|
+ <acceleration>0.334584 -2.56698 13.4784 -2.50005 -0.793274 2.6325</acceleration>
|
|
|
+ <wrench>0.003346 -0.02567 0.134784 0 -0 0</wrench>
|
|
|
+ </link>
|
|
|
+ <link name='base_footprint'>
|
|
|
+ <pose>2.2279 3.2462 -1.2e-05 3e-06 0 -2.93302</pose>
|
|
|
+ <velocity>1.4e-05 5e-05 0.003758 -0.018203 -0.003864 9.6e-05</velocity>
|
|
|
+ <acceleration>-0.004455 -0.066883 7.77548 -1.39246 -0.468595 3.07025</acceleration>
|
|
|
+ <wrench>-0.023435 -0.351806 40.899 0 -0 0</wrench>
|
|
|
+ </link>
|
|
|
+ <link name='front_wheel'>
|
|
|
+ <pose>1.99066 3.19599 0.007501 -0.367357 -0.529104 2.98238</pose>
|
|
|
+ <velocity>-2.8e-05 0.000167 0.00675 -0.021281 -0.004563 0.006565</velocity>
|
|
|
+ <acceleration>0.341849 -2.50879 13.5786 1.00522 1.20571 2.92131</acceleration>
|
|
|
+ <wrench>0.003418 -0.025088 0.135786 0 -0 0</wrench>
|
|
|
+ </link>
|
|
|
+ <link name='left_wheel'>
|
|
|
+ <pose>2.27967 3.00162 0.032502 0.000188 0.252131 -2.93301</pose>
|
|
|
+ <velocity>-0.000166 0.001187 0.010191 -0.038584 -0.011802 -0.021289</velocity>
|
|
|
+ <acceleration>0.103113 -0.505907 23.0083 0.142703 -1.0837 1.86287</acceleration>
|
|
|
+ <wrench>0.005156 -0.025295 1.15041 0 -0 0</wrench>
|
|
|
+ </link>
|
|
|
+ <link name='right_wheel'>
|
|
|
+ <pose>2.17614 3.49078 0.032502 0.000528 0.182448 -2.93297</pose>
|
|
|
+ <velocity>4.8e-05 -0.000178 0.001181 0.009217 0.001243 0.004341</velocity>
|
|
|
+ <acceleration>0.325626 -1.44326 1.84527 -0.847185 1.3061 -1.75118</acceleration>
|
|
|
+ <wrench>0.016281 -0.072163 0.092264 0 -0 0</wrench>
|
|
|
+ </link>
|
|
|
+ </model>
|
|
|
+ <light name='sun'>
|
|
|
+ <pose>0 0 10 0 -0 0</pose>
|
|
|
+ </light>
|
|
|
+ </state>
|
|
|
+ <gui fullscreen='0'>
|
|
|
+ <camera name='user_camera'>
|
|
|
+ <pose>5 -5 2 0 0.275643 2.35619</pose>
|
|
|
+ <view_controller>orbit</view_controller>
|
|
|
+ <projection_type>perspective</projection_type>
|
|
|
+ </camera>
|
|
|
+ </gui>
|
|
|
+ <model name='mycar'>
|
|
|
+ <link name='base_footprint'>
|
|
|
+ <inertial>
|
|
|
+ <pose>0 0 0.061587 0 -0 0</pose>
|
|
|
+ <mass>5.26</mass>
|
|
|
+ <inertia>
|
|
|
+ <ixx>0.0856892</ixx>
|
|
|
+ <ixy>0</ixy>
|
|
|
+ <ixz>0</ixz>
|
|
|
+ <iyy>0.0856892</iyy>
|
|
|
+ <iyz>0</iyz>
|
|
|
+ <izz>0.156323</izz>
|
|
|
+ </inertia>
|
|
|
+ </inertial>
|
|
|
+ <collision name='base_footprint_fixed_joint_lump__base_link_collision'>
|
|
|
+ <pose>0 0 0.055 0 -0 0</pose>
|
|
|
+ <geometry>
|
|
|
+ <cylinder>
|
|
|
+ <length>0.08</length>
|
|
|
+ <radius>0.25</radius>
|
|
|
+ </cylinder>
|
|
|
+ </geometry>
|
|
|
+ <surface>
|
|
|
+ <contact>
|
|
|
+ <ode/>
|
|
|
+ </contact>
|
|
|
+ <friction>
|
|
|
+ <ode/>
|
|
|
+ <torsional>
|
|
|
+ <ode/>
|
|
|
+ </torsional>
|
|
|
+ </friction>
|
|
|
+ <bounce/>
|
|
|
+ </surface>
|
|
|
+ <max_contacts>10</max_contacts>
|
|
|
+ </collision>
|
|
|
+ <collision name='base_footprint_fixed_joint_lump__imu_collision_1'>
|
|
|
+ <pose>0 0 0.145 0 -0 0</pose>
|
|
|
+ <geometry>
|
|
|
+ <box>
|
|
|
+ <size>0.01 0.01 0.01</size>
|
|
|
+ </box>
|
|
|
+ </geometry>
|
|
|
+ <surface>
|
|
|
+ <contact>
|
|
|
+ <ode/>
|
|
|
+ </contact>
|
|
|
+ <friction>
|
|
|
+ <ode/>
|
|
|
+ <torsional>
|
|
|
+ <ode/>
|
|
|
+ </torsional>
|
|
|
+ </friction>
|
|
|
+ <bounce/>
|
|
|
+ </surface>
|
|
|
+ <max_contacts>10</max_contacts>
|
|
|
+ </collision>
|
|
|
+ <collision name='base_footprint_fixed_joint_lump__support_collision_2'>
|
|
|
+ <pose>0 0 0.145 0 -0 0</pose>
|
|
|
+ <geometry>
|
|
|
+ <cylinder>
|
|
|
+ <length>0.1</length>
|
|
|
+ <radius>0.01</radius>
|
|
|
+ </cylinder>
|
|
|
+ </geometry>
|
|
|
+ <surface>
|
|
|
+ <contact>
|
|
|
+ <ode/>
|
|
|
+ </contact>
|
|
|
+ <friction>
|
|
|
+ <ode/>
|
|
|
+ <torsional>
|
|
|
+ <ode/>
|
|
|
+ </torsional>
|
|
|
+ </friction>
|
|
|
+ <bounce/>
|
|
|
+ </surface>
|
|
|
+ <max_contacts>10</max_contacts>
|
|
|
+ </collision>
|
|
|
+ <collision name='base_footprint_fixed_joint_lump__laser_collision_3'>
|
|
|
+ <pose>0 0 0.22 0 -0 0</pose>
|
|
|
+ <geometry>
|
|
|
+ <cylinder>
|
|
|
+ <length>0.05</length>
|
|
|
+ <radius>0.03</radius>
|
|
|
+ </cylinder>
|
|
|
+ </geometry>
|
|
|
+ <surface>
|
|
|
+ <contact>
|
|
|
+ <ode/>
|
|
|
+ </contact>
|
|
|
+ <friction>
|
|
|
+ <ode/>
|
|
|
+ <torsional>
|
|
|
+ <ode/>
|
|
|
+ </torsional>
|
|
|
+ </friction>
|
|
|
+ <bounce/>
|
|
|
+ </surface>
|
|
|
+ <max_contacts>10</max_contacts>
|
|
|
+ </collision>
|
|
|
+ <visual name='base_footprint_visual'>
|
|
|
+ <pose>0 0 0 0 -0 0</pose>
|
|
|
+ <geometry>
|
|
|
+ <sphere>
|
|
|
+ <radius>0.001</radius>
|
|
|
+ </sphere>
|
|
|
+ </geometry>
|
|
|
+ </visual>
|
|
|
+ <visual name='base_footprint_fixed_joint_lump__base_link_visual_1'>
|
|
|
+ <pose>0 0 0.055 0 -0 0</pose>
|
|
|
+ <geometry>
|
|
|
+ <cylinder>
|
|
|
+ <length>0.08</length>
|
|
|
+ <radius>0.25</radius>
|
|
|
+ </cylinder>
|
|
|
+ </geometry>
|
|
|
+ <material>
|
|
|
+ <script>
|
|
|
+ <name>Gazebo/Yellow</name>
|
|
|
+ <uri>file://media/materials/scripts/gazebo.material</uri>
|
|
|
+ </script>
|
|
|
+ </material>
|
|
|
+ </visual>
|
|
|
+ <visual name='base_footprint_fixed_joint_lump__imu_visual_2'>
|
|
|
+ <pose>0 0 0.145 0 -0 0</pose>
|
|
|
+ <geometry>
|
|
|
+ <box>
|
|
|
+ <size>0.01 0.01 0.01</size>
|
|
|
+ </box>
|
|
|
+ </geometry>
|
|
|
+ <material>
|
|
|
+ <script>
|
|
|
+ <name>Gazebo/Red</name>
|
|
|
+ <uri>file://media/materials/scripts/gazebo.material</uri>
|
|
|
+ </script>
|
|
|
+ </material>
|
|
|
+ </visual>
|
|
|
+ <visual name='base_footprint_fixed_joint_lump__support_visual_3'>
|
|
|
+ <pose>0 0 0.145 0 -0 0</pose>
|
|
|
+ <geometry>
|
|
|
+ <cylinder>
|
|
|
+ <length>0.1</length>
|
|
|
+ <radius>0.01</radius>
|
|
|
+ </cylinder>
|
|
|
+ </geometry>
|
|
|
+ <material>
|
|
|
+ <script>
|
|
|
+ <name>Gazebo/Gray</name>
|
|
|
+ <uri>file://media/materials/scripts/gazebo.material</uri>
|
|
|
+ </script>
|
|
|
+ </material>
|
|
|
+ </visual>
|
|
|
+ <visual name='base_footprint_fixed_joint_lump__laser_visual_4'>
|
|
|
+ <pose>0 0 0.22 0 -0 0</pose>
|
|
|
+ <geometry>
|
|
|
+ <cylinder>
|
|
|
+ <length>0.05</length>
|
|
|
+ <radius>0.03</radius>
|
|
|
+ </cylinder>
|
|
|
+ </geometry>
|
|
|
+ <material>
|
|
|
+ <script>
|
|
|
+ <name>Gazebo/Bluek</name>
|
|
|
+ <uri>file://media/materials/scripts/gazebo.material</uri>
|
|
|
+ </script>
|
|
|
+ </material>
|
|
|
+ </visual>
|
|
|
+ <gravity>1</gravity>
|
|
|
+ <sensor name='imu_sensor' type='imu'>
|
|
|
+ <always_on>1</always_on>
|
|
|
+ <update_rate>100</update_rate>
|
|
|
+ <visualize>1</visualize>
|
|
|
+ <topic>/imu</topic>
|
|
|
+ <plugin name='imu_plugin' filename='libgazebo_ros_imu_sensor.so'>
|
|
|
+ <topicName>/imu</topicName>
|
|
|
+ <bodyName>imu</bodyName>
|
|
|
+ <updateRateHZ>100.0</updateRateHZ>
|
|
|
+ <gaussianNoise>0.0</gaussianNoise>
|
|
|
+ <xyzOffset>0 0 0</xyzOffset>
|
|
|
+ <rpyOffset>0 0 0</rpyOffset>
|
|
|
+ <frameName>imu</frameName>
|
|
|
+ <robotNamespace>/</robotNamespace>
|
|
|
+ </plugin>
|
|
|
+ <pose>0 0 0.145 0 -0 0</pose>
|
|
|
+ <imu/>
|
|
|
+ </sensor>
|
|
|
+ <sensor name='rplidar' type='ray'>
|
|
|
+ <visualize>1</visualize>
|
|
|
+ <update_rate>5.5</update_rate>
|
|
|
+ <ray>
|
|
|
+ <scan>
|
|
|
+ <horizontal>
|
|
|
+ <samples>360</samples>
|
|
|
+ <resolution>1</resolution>
|
|
|
+ <min_angle>-3</min_angle>
|
|
|
+ <max_angle>3</max_angle>
|
|
|
+ </horizontal>
|
|
|
+ <vertical>
|
|
|
+ <samples>1</samples>
|
|
|
+ <min_angle>0</min_angle>
|
|
|
+ <max_angle>0</max_angle>
|
|
|
+ </vertical>
|
|
|
+ </scan>
|
|
|
+ <range>
|
|
|
+ <min>0.1</min>
|
|
|
+ <max>30</max>
|
|
|
+ <resolution>0.01</resolution>
|
|
|
+ </range>
|
|
|
+ <noise>
|
|
|
+ <type>gaussian</type>
|
|
|
+ <mean>0</mean>
|
|
|
+ <stddev>0.01</stddev>
|
|
|
+ </noise>
|
|
|
+ </ray>
|
|
|
+ <plugin name='gazebo_rplidar' filename='libgazebo_ros_laser.so'>
|
|
|
+ <topicName>/scan</topicName>
|
|
|
+ <frameName>laser</frameName>
|
|
|
+ <robotNamespace>/</robotNamespace>
|
|
|
+ </plugin>
|
|
|
+ <pose>0 0 0.22 0 -0 0</pose>
|
|
|
+ </sensor>
|
|
|
+ <self_collide>0</self_collide>
|
|
|
+ <enable_wind>0</enable_wind>
|
|
|
+ <kinematic>0</kinematic>
|
|
|
+ </link>
|
|
|
+ <joint name='back_wheel2base_link' type='revolute'>
|
|
|
+ <pose relative_to='base_footprint'>-0.2425 0 0.0075 0 -0 0</pose>
|
|
|
+ <parent>base_footprint</parent>
|
|
|
+ <child>back_wheel</child>
|
|
|
+ <axis>
|
|
|
+ <xyz>1 1 1</xyz>
|
|
|
+ <limit>
|
|
|
+ <lower>-1e+16</lower>
|
|
|
+ <upper>1e+16</upper>
|
|
|
+ </limit>
|
|
|
+ <dynamics>
|
|
|
+ <spring_reference>0</spring_reference>
|
|
|
+ <spring_stiffness>0</spring_stiffness>
|
|
|
+ </dynamics>
|
|
|
+ </axis>
|
|
|
+ </joint>
|
|
|
+ <link name='back_wheel'>
|
|
|
+ <pose relative_to='back_wheel2base_link'>0 0 0 0 -0 0</pose>
|
|
|
+ <inertial>
|
|
|
+ <pose>0 0 0 0 -0 0</pose>
|
|
|
+ <mass>0.01</mass>
|
|
|
+ <inertia>
|
|
|
+ <ixx>2.25e-07</ixx>
|
|
|
+ <ixy>0</ixy>
|
|
|
+ <ixz>0</ixz>
|
|
|
+ <iyy>2.25e-07</iyy>
|
|
|
+ <iyz>0</iyz>
|
|
|
+ <izz>2.25e-07</izz>
|
|
|
+ </inertia>
|
|
|
+ </inertial>
|
|
|
+ <collision name='back_wheel_collision'>
|
|
|
+ <pose>0 0 0 0 -0 0</pose>
|
|
|
+ <geometry>
|
|
|
+ <sphere>
|
|
|
+ <radius>0.0075</radius>
|
|
|
+ </sphere>
|
|
|
+ </geometry>
|
|
|
+ <surface>
|
|
|
+ <contact>
|
|
|
+ <ode/>
|
|
|
+ </contact>
|
|
|
+ <friction>
|
|
|
+ <ode/>
|
|
|
+ <torsional>
|
|
|
+ <ode/>
|
|
|
+ </torsional>
|
|
|
+ </friction>
|
|
|
+ <bounce/>
|
|
|
+ </surface>
|
|
|
+ <max_contacts>10</max_contacts>
|
|
|
+ </collision>
|
|
|
+ <visual name='back_wheel_visual'>
|
|
|
+ <pose>0 0 0 0 -0 0</pose>
|
|
|
+ <geometry>
|
|
|
+ <sphere>
|
|
|
+ <radius>0.0075</radius>
|
|
|
+ </sphere>
|
|
|
+ </geometry>
|
|
|
+ <material>
|
|
|
+ <script>
|
|
|
+ <name>Gazebo/Red</name>
|
|
|
+ <uri>file://media/materials/scripts/gazebo.material</uri>
|
|
|
+ </script>
|
|
|
+ </material>
|
|
|
+ </visual>
|
|
|
+ <self_collide>0</self_collide>
|
|
|
+ <enable_wind>0</enable_wind>
|
|
|
+ <kinematic>0</kinematic>
|
|
|
+ </link>
|
|
|
+ <joint name='front_wheel2base_link' type='revolute'>
|
|
|
+ <pose relative_to='base_footprint'>0.2425 0 0.0075 0 -0 0</pose>
|
|
|
+ <parent>base_footprint</parent>
|
|
|
+ <child>front_wheel</child>
|
|
|
+ <axis>
|
|
|
+ <xyz>1 1 1</xyz>
|
|
|
+ <limit>
|
|
|
+ <lower>-1e+16</lower>
|
|
|
+ <upper>1e+16</upper>
|
|
|
+ </limit>
|
|
|
+ <dynamics>
|
|
|
+ <spring_reference>0</spring_reference>
|
|
|
+ <spring_stiffness>0</spring_stiffness>
|
|
|
+ </dynamics>
|
|
|
+ </axis>
|
|
|
+ </joint>
|
|
|
+ <link name='front_wheel'>
|
|
|
+ <pose relative_to='front_wheel2base_link'>0 0 0 0 -0 0</pose>
|
|
|
+ <inertial>
|
|
|
+ <pose>0 0 0 0 -0 0</pose>
|
|
|
+ <mass>0.01</mass>
|
|
|
+ <inertia>
|
|
|
+ <ixx>2.25e-07</ixx>
|
|
|
+ <ixy>0</ixy>
|
|
|
+ <ixz>0</ixz>
|
|
|
+ <iyy>2.25e-07</iyy>
|
|
|
+ <iyz>0</iyz>
|
|
|
+ <izz>2.25e-07</izz>
|
|
|
+ </inertia>
|
|
|
+ </inertial>
|
|
|
+ <collision name='front_wheel_collision'>
|
|
|
+ <pose>0 0 0 0 -0 0</pose>
|
|
|
+ <geometry>
|
|
|
+ <sphere>
|
|
|
+ <radius>0.0075</radius>
|
|
|
+ </sphere>
|
|
|
+ </geometry>
|
|
|
+ <surface>
|
|
|
+ <contact>
|
|
|
+ <ode/>
|
|
|
+ </contact>
|
|
|
+ <friction>
|
|
|
+ <ode/>
|
|
|
+ <torsional>
|
|
|
+ <ode/>
|
|
|
+ </torsional>
|
|
|
+ </friction>
|
|
|
+ <bounce/>
|
|
|
+ </surface>
|
|
|
+ <max_contacts>10</max_contacts>
|
|
|
+ </collision>
|
|
|
+ <visual name='front_wheel_visual'>
|
|
|
+ <pose>0 0 0 0 -0 0</pose>
|
|
|
+ <geometry>
|
|
|
+ <sphere>
|
|
|
+ <radius>0.0075</radius>
|
|
|
+ </sphere>
|
|
|
+ </geometry>
|
|
|
+ <material>
|
|
|
+ <script>
|
|
|
+ <name>Gazebo/Red</name>
|
|
|
+ <uri>file://media/materials/scripts/gazebo.material</uri>
|
|
|
+ </script>
|
|
|
+ </material>
|
|
|
+ </visual>
|
|
|
+ <self_collide>0</self_collide>
|
|
|
+ <enable_wind>0</enable_wind>
|
|
|
+ <kinematic>0</kinematic>
|
|
|
+ </link>
|
|
|
+ <joint name='left_wheel2base_link' type='revolute'>
|
|
|
+ <pose relative_to='base_footprint'>0 0.25 0.0325 0 -0 0</pose>
|
|
|
+ <parent>base_footprint</parent>
|
|
|
+ <child>left_wheel</child>
|
|
|
+ <axis>
|
|
|
+ <xyz>0 1 0</xyz>
|
|
|
+ <limit>
|
|
|
+ <lower>-1e+16</lower>
|
|
|
+ <upper>1e+16</upper>
|
|
|
+ </limit>
|
|
|
+ <dynamics>
|
|
|
+ <spring_reference>0</spring_reference>
|
|
|
+ <spring_stiffness>0</spring_stiffness>
|
|
|
+ </dynamics>
|
|
|
+ </axis>
|
|
|
+ </joint>
|
|
|
+ <link name='left_wheel'>
|
|
|
+ <pose relative_to='left_wheel2base_link'>0 0 0 0 -0 0</pose>
|
|
|
+ <inertial>
|
|
|
+ <pose>0 0 0 0 -0 0</pose>
|
|
|
+ <mass>0.05</mass>
|
|
|
+ <inertia>
|
|
|
+ <ixx>1.41406e-05</ixx>
|
|
|
+ <ixy>0</ixy>
|
|
|
+ <ixz>0</ixz>
|
|
|
+ <iyy>1.41406e-05</iyy>
|
|
|
+ <iyz>0</iyz>
|
|
|
+ <izz>2.64063e-05</izz>
|
|
|
+ </inertia>
|
|
|
+ </inertial>
|
|
|
+ <collision name='left_wheel_collision'>
|
|
|
+ <pose>0 0 0 1.5705 -0 0</pose>
|
|
|
+ <geometry>
|
|
|
+ <cylinder>
|
|
|
+ <length>0.015</length>
|
|
|
+ <radius>0.0325</radius>
|
|
|
+ </cylinder>
|
|
|
+ </geometry>
|
|
|
+ <surface>
|
|
|
+ <contact>
|
|
|
+ <ode/>
|
|
|
+ </contact>
|
|
|
+ <friction>
|
|
|
+ <ode/>
|
|
|
+ <torsional>
|
|
|
+ <ode/>
|
|
|
+ </torsional>
|
|
|
+ </friction>
|
|
|
+ <bounce/>
|
|
|
+ </surface>
|
|
|
+ <max_contacts>10</max_contacts>
|
|
|
+ </collision>
|
|
|
+ <visual name='left_wheel_visual'>
|
|
|
+ <pose>0 0 0 1.5705 -0 0</pose>
|
|
|
+ <geometry>
|
|
|
+ <cylinder>
|
|
|
+ <length>0.015</length>
|
|
|
+ <radius>0.0325</radius>
|
|
|
+ </cylinder>
|
|
|
+ </geometry>
|
|
|
+ <material>
|
|
|
+ <script>
|
|
|
+ <name>Gazebo/Red</name>
|
|
|
+ <uri>file://media/materials/scripts/gazebo.material</uri>
|
|
|
+ </script>
|
|
|
+ </material>
|
|
|
+ </visual>
|
|
|
+ <self_collide>0</self_collide>
|
|
|
+ <enable_wind>0</enable_wind>
|
|
|
+ <kinematic>0</kinematic>
|
|
|
+ </link>
|
|
|
+ <joint name='right_wheel2base_link' type='revolute'>
|
|
|
+ <pose relative_to='base_footprint'>0 -0.25 0.0325 0 -0 0</pose>
|
|
|
+ <parent>base_footprint</parent>
|
|
|
+ <child>right_wheel</child>
|
|
|
+ <axis>
|
|
|
+ <xyz>0 1 0</xyz>
|
|
|
+ <limit>
|
|
|
+ <lower>-1e+16</lower>
|
|
|
+ <upper>1e+16</upper>
|
|
|
+ </limit>
|
|
|
+ <dynamics>
|
|
|
+ <spring_reference>0</spring_reference>
|
|
|
+ <spring_stiffness>0</spring_stiffness>
|
|
|
+ </dynamics>
|
|
|
+ </axis>
|
|
|
+ </joint>
|
|
|
+ <link name='right_wheel'>
|
|
|
+ <pose relative_to='right_wheel2base_link'>0 0 0 0 -0 0</pose>
|
|
|
+ <inertial>
|
|
|
+ <pose>0 0 0 0 -0 0</pose>
|
|
|
+ <mass>0.05</mass>
|
|
|
+ <inertia>
|
|
|
+ <ixx>1.41406e-05</ixx>
|
|
|
+ <ixy>0</ixy>
|
|
|
+ <ixz>0</ixz>
|
|
|
+ <iyy>1.41406e-05</iyy>
|
|
|
+ <iyz>0</iyz>
|
|
|
+ <izz>2.64063e-05</izz>
|
|
|
+ </inertia>
|
|
|
+ </inertial>
|
|
|
+ <collision name='right_wheel_collision'>
|
|
|
+ <pose>0 0 0 1.5705 -0 0</pose>
|
|
|
+ <geometry>
|
|
|
+ <cylinder>
|
|
|
+ <length>0.015</length>
|
|
|
+ <radius>0.0325</radius>
|
|
|
+ </cylinder>
|
|
|
+ </geometry>
|
|
|
+ <surface>
|
|
|
+ <contact>
|
|
|
+ <ode/>
|
|
|
+ </contact>
|
|
|
+ <friction>
|
|
|
+ <ode/>
|
|
|
+ <torsional>
|
|
|
+ <ode/>
|
|
|
+ </torsional>
|
|
|
+ </friction>
|
|
|
+ <bounce/>
|
|
|
+ </surface>
|
|
|
+ <max_contacts>10</max_contacts>
|
|
|
+ </collision>
|
|
|
+ <visual name='right_wheel_visual'>
|
|
|
+ <pose>0 0 0 1.5705 -0 0</pose>
|
|
|
+ <geometry>
|
|
|
+ <cylinder>
|
|
|
+ <length>0.015</length>
|
|
|
+ <radius>0.0325</radius>
|
|
|
+ </cylinder>
|
|
|
+ </geometry>
|
|
|
+ <material>
|
|
|
+ <script>
|
|
|
+ <name>Gazebo/Red</name>
|
|
|
+ <uri>file://media/materials/scripts/gazebo.material</uri>
|
|
|
+ </script>
|
|
|
+ </material>
|
|
|
+ </visual>
|
|
|
+ <self_collide>0</self_collide>
|
|
|
+ <enable_wind>0</enable_wind>
|
|
|
+ <kinematic>0</kinematic>
|
|
|
+ </link>
|
|
|
+ <static>0</static>
|
|
|
+ <plugin name='differential_drive_controller' filename='libgazebo_ros_diff_drive.so'>
|
|
|
+ <rosDebugLevel>Debug</rosDebugLevel>
|
|
|
+ <publishWheelTF>1</publishWheelTF>
|
|
|
+ <robotNamespace>/</robotNamespace>
|
|
|
+ <publishTf>1</publishTf>
|
|
|
+ <publishWheelJointState>1</publishWheelJointState>
|
|
|
+ <alwaysOn>1</alwaysOn>
|
|
|
+ <updateRate>100.0</updateRate>
|
|
|
+ <legacyMode>1</legacyMode>
|
|
|
+ <leftJoint>left_wheel2base_link</leftJoint>
|
|
|
+ <rightJoint>right_wheel2base_link</rightJoint>
|
|
|
+ <wheelSeparation>0.5</wheelSeparation>
|
|
|
+ <wheelDiameter>0.065</wheelDiameter>
|
|
|
+ <broadcastTF>1</broadcastTF>
|
|
|
+ <wheelTorque>30</wheelTorque>
|
|
|
+ <wheelAcceleration>1.8</wheelAcceleration>
|
|
|
+ <commandTopic>/cmd_vel</commandTopic>
|
|
|
+ <odometryFrame>odom</odometryFrame>
|
|
|
+ <odometryTopic>/odom</odometryTopic>
|
|
|
+ <robotBaseFrame>base_footprint</robotBaseFrame>
|
|
|
+ </plugin>
|
|
|
+ <pose>2.2875 3.00658 0 0 0 -2.92677</pose>
|
|
|
+ </model>
|
|
|
+ </world>
|
|
|
+</sdf>
|