建的机器人模型直接剧烈碰撞并且卡退

模型xacro：

<?xml version="1.0"?>
<robot name="jackal" xmlns:xacro="http://www.ros.org/wiki/xacro">
 <xacro:property name="PI" value="3.1415926535897931" />

 <xacro:property name="wheelbase" value="0.262" />
 <xacro:property name="track" value="0.37559" />
 <xacro:property name="wheel_vertical_offset" value="0.0345" />
 <xacro:property name="footprint_vertical_offset" value="-0.0655" />

 <xacro:property name="wheel_radius" value="0.098" />
 <xacro:property name="wheel_width" value="0.040" />

 <xacro:property name="chassis_length" value="0.420" />
 <xacro:property name="chassis_width" value="0.310" />
 <xacro:property name="chassis_height" value="0.184" />

 <xacro:property name="dummy_inertia" value="1e-09"/>

 <xacro:property name="mount_spacing" value="0.120" />

 <material name="dark_grey"><color rgba="0.2 0.2 0.2 1.0" /></material>
 <material name="light_grey"><color rgba="0.4 0.4 0.4 1.0" /></material>
 <material name="yellow"><color rgba="0.8 0.8 0.0 1.0" /></material>
 <material name="black"><color rgba="0.15 0.15 0.15 1.0" /></material>

 <xacro:macro name="Box_inertial_matrix" params="m l w h">

 <mass value="${m}" />
 <inertia ixx="${m*(h*h + l*l)/12}" ixy = "0" ixz = "0"
 iyy="${m*(w*w + l*l)/12}" iyz= "0"
 izz="${m*(w*w + h*h)/12}" />
 
 </xacro:macro>
 <xacro:macro name="wheel" params="prefix *joint_pose">
 <link name="${prefix}_wheel_link">
 <visual>
 <origin xyz="0 0 0" rpy="${PI/2} 0 0"/>
 <geometry>
 <mesh filename="package://jackal_description/meshes/jackal-wheel.stl"/>
 </geometry>
 <material name="black" />
 </visual>
 <collision>
 <origin xyz="0 0 0" rpy="${PI/2} 0 0"/>
 <geometry>
 <cylinder radius="${wheel_radius}" length="${wheel_width}"/>
 </geometry>
 </collision>
 <inertial>
 <origin xyz="0 0 0" rpy="0 0 0"/>
 <mass value="1.477"/>
 <inertia
 ixx="0.0013" ixy="0" ixz="0"
 iyy="0.0024" iyz="0"
 izz="0.0013"/>
 </inertial>
 </link>

 <gazebo reference="${prefix}_wheel_link">
 <material>Gazebo/DarkGrey</material>
 </gazebo>

 <joint name="${prefix}_wheel" type="continuous">
 <parent link="chassis_link"/>
 <child link="${prefix}_wheel_link" />
 <xacro:insert_block name="joint_pose" />
 <axis xyz="0 1 0" />
 </joint>

 <!-- In reality, Jackal has only two motors, one per side. However, it's more
 straightforward for Gazebo to simulate as if there's an actuator per wheel. -->
 <transmission name="${prefix}_wheel_trans">
 <type>transmission_interface/SimpleTransmission</type>
 <joint name="${prefix}_wheel">
 <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
 </joint>
 <actuator name="${prefix}_actuator">
 <mechanicalReduction>1</mechanicalReduction>
 </actuator>
 </transmission>
 </xacro:macro>

 <xacro:wheel prefix="front_left">
 <origin xyz="${wheelbase/2} ${track/2} ${wheel_vertical_offset}" rpy="0 0 0" />
 </xacro:wheel>
 <xacro:wheel prefix="front_right">
 <origin xyz="${wheelbase/2} ${-track/2} ${wheel_vertical_offset}" rpy="0 0 0" />
 </xacro:wheel>
 <xacro:wheel prefix="rear_left">
 <origin xyz="${-wheelbase/2} ${track/2} ${wheel_vertical_offset}" rpy="0 0 0" />
 </xacro:wheel>
 <xacro:wheel prefix="rear_right">
 <origin xyz="${-wheelbase/2} ${-track/2} ${wheel_vertical_offset}" rpy="0 0 0" />
 </xacro:wheel>

 <xacro:macro name="arrow_" params="prefix *joint_pose">
 <link name="arrow">
 <visual>
 <geometry>
 <box size=".04 .04 .4"/>
 </geometry>
 <material name="yellow">
 <color rgba="0 0 1 0.2"/>
 </material>
 </visual>
 <collision>
 <origin xyz="0 0 0" rpy="${PI/2} 0 0"/>
 <geometry>
 <cylinder radius="0.001" length="0.1"/>
 </geometry>
 </collision>
 <inertial>
 <xacro:Box_inertial_matrix m="0.00001" l="0.04" w="0.04" h="0.4" />
 <origin xyz="0 0 0" rpy="0 0 0"/>
 </inertial>
 </link>

 <gazebo reference="arrow">
 <material>Gazebo/DarkGrey</material>
 </gazebo>

 <joint name="arrow_joint" type="fixed">
 <parent link="base_link"/>
 <child link="arrow" />
 <xacro:insert_block name="joint_pose" />
 <axis xyz="0 1 0" />
 </joint>
 </xacro:macro>

 <xacro:arrow_ prefix="arrow_">
 <origin xyz="0 0 0.35" rpy="0 0 0" />
 </xacro:arrow_>

 <xacro:macro name="pitch_pole" params="prefix *joint_pose">
 <link name="pitch_pole_link">
 <visual>
 <geometry>
 <box size=".04 .04 .2"/>
 </geometry>
 <material name="yellow">
 <color rgba="0 0 1 1"/>
 </material>
 </visual>
 <inertial>
 <origin xyz="0 0 -0.1" rpy="0 0 0"/>
 <xacro:Box_inertial_matrix m="0.00001" l="0.04" w="0.04" h="0.2" />
 </inertial>
 </link>

 <gazebo reference="pitch_pole_link">
 <material>Gazebo/DarkGrey</material>
 </gazebo>

 <joint name="pitch_pole" type="revolute">
 <parent link="arrow"/>
 <child link="pitch_pole_link" />
 <xacro:insert_block name="joint_pose" />
 <limit lower="-1" upper="1" effort="100" velocity="5"/>
 <axis xyz="0 1 0" />
 </joint>

 </xacro:macro>

 <xacro:pitch_pole prefix="pitch">
 <origin xyz="0 0.04 0.1" rpy="0 -${PI/4} 0" />
 </xacro:pitch_pole>

 <xacro:macro name="roll_pole" params="prefix *joint_pose">
 <link name="roll_pole_link">
 <visual>
 <geometry>
 <box size=".04 .04 .2"/>
 </geometry>
 <material name="yellow">
 <color rgba="0 0 1 1"/>
 </material>
 </visual>
 <collision>
 <origin xyz="0 0 0" rpy="${PI/2} 0 0"/>
 <geometry>
 <cylinder radius="0.001" length="0.1"/>
 </geometry>
 </collision>

 <inertial>
 <origin xyz="0 0 0" rpy="0 0 0"/>
 <xacro:Box_inertial_matrix m="0.00001" l="0.04" w="0.04" h="0.2" />
 </inertial>
 </link>

 <gazebo reference="roll_pole_link">
 <material>Gazebo/DarkGrey</material>
 </gazebo>

 <joint name="roll_pole" type="revolute">
 <parent link="pitch_pole_link"/>
 <child link="roll_pole_link" />
 <xacro:insert_block name="joint_pose" />
 <limit lower="-0.4" upper="0.4" effort="100" velocity="5"/>
 <axis xyz="0 1 0" />
 </joint>

 </xacro:macro>

 <xacro:roll_pole prefix="roll">
 <origin xyz="0 0 -0.1" rpy="${PI/2} ${PI} 0" />
 </xacro:roll_pole>

 <link name="base_footprint">
 <visual>
 <geometry>
 <cylinder length=".02" radius="0.25"/>
 </geometry>
 <material name="black">
 <color rgba="0 0 0 1"/>
 </material>
 </visual>
 </link>
 <link name="base_link" >
 </link>
 <joint name="base_footprint_joint" type="revolute">
 <limit lower="-1.57" upper="1.57" effort="100" velocity="10"/>
 <parent link="base_link"/>
 <child link="base_footprint"/>
 </joint>
 <joint name="base_link_joint" type="fixed">
 <origin xyz="0 0 0" rpy="0 0 0" />
 <parent link="base_link"/>
 <child link="chassis_link" />
 </joint>

 <link name="chassis_link">
 <visual>
 <origin xyz="0 0 ${footprint_vertical_offset}" rpy="${PI/2} 0 ${PI/2}"/>
 <geometry>
 <mesh filename="package://jackal_description/meshes/jackal-base.stl"/>
 </geometry>
 <material name="dark_grey" />
 </visual>
 <collision>
 <origin xyz="0 0 ${chassis_height/2}"/>
 <geometry>
 <box size="${chassis_length} ${chassis_width} ${chassis_height}"/>
 </geometry>
 </collision>
 <inertial>
 <!-- Center of mass -->
 <origin xyz="0.012 0.002 0.067" rpy="0 0 0"/>
 <mass value="16.523"/>
 <!-- Moments of inertia: ( chassis without wheels ) -->
 <inertia
 ixx="0.3136" ixy="-0.0008" ixz="0.0164"
 iyy="0.3922" iyz="-0.0009"
 izz="0.4485"/>
 </inertial>
 </link>

 <link name="fenders_link">
 <visual>
 <origin xyz="0 0 ${footprint_vertical_offset}" rpy="${PI/2} 0 ${PI/2}"/>
 <geometry>
 <mesh filename="package://jackal_description/meshes/jackal-fenders.stl"/>
 </geometry>
 <material name="yellow" />
 </visual>
 </link>
 <joint name="fenders_joint" type="fixed">
 <parent link="chassis_link" />
 <child link="fenders_link" />
 </joint>

 <!-- TODO: Make this internal_imu_link or something, and use a mixed-in xacro
 to supply the joint between it and imu_link. This is important so that imu_link
 always points to the "active" IMU. When an upgrade IMU is connected, the
 internal_imu_link should remain, but imu_link should point to the upgrade one. -->
 <link name="imu_link">
 <inertial>
 <mass value="0.001"/>
 <origin xyz="0 0 0" rpy="0 0 0"/>
 <inertia ixx="${dummy_inertia}" ixy="0.0" ixz="0.0" iyy="${dummy_inertia}" iyz="0.0" izz="${dummy_inertia}"/>
 </inertial>
 </link>
 <joint name="imu_joint" type="fixed">
 <parent link="chassis_link" />
 <child link="imu_link" />
 </joint>

 <!-- TODO: Same note as applies to imu_link -->
 <link name="navsat_link">
 <visual>
 <geometry>
 <cylinder radius="0.026" length="0.016" />
 </geometry>
 <origin xyz="0 0 0.008" />
 <material name="black" />
 </visual>
 </link>
 <joint name="navsat_joint" type="fixed">
 <parent link="chassis_link" />
 <child link="navsat_link" />
 <origin xyz="-0.180 0.126 0.1815" />
 </joint>

 <link name="mid_mount"></link>
 <joint name="mid_mount_joint" type="fixed">
 <parent link="chassis_link" />
 <child link="mid_mount" />
 <origin xyz="0 0 ${chassis_height}" />
 </joint>

 <link name="rear_mount"></link>
 <joint name="rear_mount_joint" type="fixed">
 <parent link="mid_mount" />
 <child link="rear_mount" />
 <origin xyz="${-mount_spacing} 0 0" />
 </joint>

 <link name="front_mount"></link>
 <joint name="front_mount_joint" type="fixed">
 <parent link="mid_mount" />
 <child link="front_mount" />
 <origin xyz="${mount_spacing} 0 0" />
 </joint>

 <!-- Bring in simulation data for Gazebo. -->
 <xacro:include filename="$(find jackal_description)/urdf/jackal.gazebo" />

 <xacro:if value="$(optenv JACKAL_URDF_EXTRAS 0)">
 <xacro:include filename="$(env JACKAL_URDF_EXTRAS)" />
 </xacro:if>

 <xacro:include filename="/home/luzhen/catkin_ws/src/velodyne_simulator/velodyne_description/urdf/VLP-16.urdf.xacro"/>
 <xacro:VLP-16 parent="roll_pole_link" name="velodyne_r" topic="/velodyne_points_r" hz="10" samples="1024" gpu="true" lasers="32" min_range="0.5">
 <origin xyz="0 0 -0.1" rpy="-${PI/2} 0 ${PI/2}" />
 </xacro:VLP-16>
 <xacro:VLP-16 parent="roll_pole_link" name="velodyne_l" topic="/velodyne_points_l" hz="10" samples="1024" gpu="true" lasers="32" min_range="0.5">
 <origin xyz="0 0 0.1" rpy="-${PI/2} 0 ${PI/2}" />
 </xacro:VLP-16>
 
</robot>

激光雷达的模型：

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="VLP-16">
 <xacro:property name="M_PI" value="3.1415926535897931" />
 <xacro:macro name="VLP-16" params="*origin parent:=base_link name:=velodyne topic:=/velodyne_points organize_cloud:=false hz:=10 lasers:=16 samples:=1875 collision_range:=0.3 min_range:=0.9 max_range:=130.0 noise:=0.008 min_angle:=-${M_PI} max_angle:=${M_PI} gpu:=false">

 <joint name="${name}_base_mount_joint" type="fixed">
 <xacro:insert_block name="origin" /> 
 <parent link="${parent}"/>
 <child link="${name}_base_link"/>
 </joint>

 <link name="${name}_base_link">
 <inertial>
 <mass value="0.01"/>
 <origin xyz="0 0 0.03585"/>
 <inertia ixx="${(0.01 * (3.0*0.0516*0.0516 + 0.0717*0.0717)) / 12.0}" ixy="0" ixz="0"
 iyy="${(0.01 * (3.0*0.0516*0.0516 + 0.0717*0.0717)) / 12.0}" iyz="0"
 izz="${0.5 * 0.01 * (0.0516*0.0516)}"/>
 </inertial>
 <visual>
 <geometry>
 <mesh filename="package://velodyne_description/meshes/VLP16_base_1.dae" />
 </geometry>
 </visual>
 <visual>
 <geometry>
 <mesh filename="package://velodyne_description/meshes/VLP16_base_2.dae" />
 </geometry>
 </visual>
 <!-- <collision>
 <origin rpy="0 0 0" xyz="0 0 0.03585"/>
 <geometry>
 <cylinder radius="0.0516" length="0.0717"/>
 </geometry>
 </collision> -->
 </link>

 <joint name="${name}_base_scan_joint" type="fixed" >
 <origin xyz="0 0 0.0377" rpy="0 0 0" />
 <parent link="${name}_base_link" />
 <child link="${name}"/>
 </joint>

 <link name="${name}">
 <inertial>
 <mass value="0.01"/>
 <origin xyz="0 0 0"/>
 <inertia ixx="1e-7" ixy="0" ixz="0" iyy="1e-7" iyz="0" izz="1e-7"/>
 </inertial>
 <visual>
 <origin xyz="0 0 -0.0377" />
 <geometry>
 <mesh filename="package://velodyne_description/meshes/VLP16_scan.dae" />
 </geometry>
 </visual>
 </link>

 <!-- Gazebo requires the velodyne_gazebo_plugins package -->
 <gazebo reference="${name}">
 <xacro:if value="${gpu}">
 <sensor type="gpu_ray" name="${name}-VLP16">
 <pose>0 0 0 0 0 0</pose>
 <visualize>false</visualize>
 <update_rate>${hz}</update_rate>
 <ray>
 <scan>
 <horizontal>
 <samples>${samples}</samples>
 <resolution>1</resolution>
 <min_angle>${min_angle}</min_angle>
 <max_angle>${max_angle}</max_angle>
 </horizontal>
 <vertical>
 <samples>${lasers}</samples>
 <resolution>1</resolution>
 <min_angle>-${15.0*M_PI/180.0}</min_angle>
 <max_angle> ${15.0*M_PI/180.0}</max_angle>
 </vertical>
 </scan>
 <range>
 <min>${collision_range}</min>
 <max>${max_range+1}</max>
 <resolution>0.001</resolution>
 </range>
 <noise>
 <type>gaussian</type>
 <mean>0.0</mean>
 <stddev>0.0</stddev>
 </noise>
 </ray>
 <plugin name="gazebo_ros_laser_controller" filename="libgazebo_ros_velodyne_gpu_laser.so">
 <topicName>${topic}</topicName>
 <frameName>${name}</frameName>
 <organize_cloud>${organize_cloud}</organize_cloud>
 <min_range>${min_range}</min_range>
 <max_range>${max_range}</max_range>
 <gaussianNoise>${noise}</gaussianNoise>
 </plugin>
 </sensor>
 </xacro:if>
 <xacro:unless value="${gpu}">
 <sensor type="ray" name="${name}-VLP16">
 <pose>0 0 0 0 0 0</pose>
 <visualize>false</visualize>
 <update_rate>${hz}</update_rate>
 <ray>
 <scan>
 <horizontal>
 <samples>${samples}</samples>
 <resolution>1</resolution>
 <min_angle>${min_angle}</min_angle>
 <max_angle>${max_angle}</max_angle>
 </horizontal>
 <vertical>
 <samples>${lasers}</samples>
 <resolution>1</resolution>
 <min_angle>-${15.0*M_PI/180.0}</min_angle>
 <max_angle> ${15.0*M_PI/180.0}</max_angle>
 </vertical>
 </scan>
 <range>
 <min>${collision_range}</min>
 <max>${max_range+1}</max>
 <resolution>0.001</resolution>
 </range>
 <noise>
 <type>gaussian</type>
 <mean>0.0</mean>
 <stddev>0.0</stddev>
 </noise>
 </ray>
 <plugin name="gazebo_ros_laser_controller" filename="libgazebo_ros_velodyne_laser.so">
 <topicName>${topic}</topicName>
 <frameName>${name}</frameName>
 <organize_cloud>${organize_cloud}</organize_cloud>
 <min_range>${min_range}</min_range>
 <max_range>${max_range}</max_range>
 <gaussianNoise>${noise}</gaussianNoise>
 </plugin>
 </sensor>
 </xacro:unless>
 </gazebo>

 </xacro:macro>
</robot>