您好,我自己画了一个铲车的简化模型,是中间铰接转向的。在gazebo里面运行之后车的轮子沉到地下,车自己滑行偏转,怎么解决
下面是我的xacro文件:
<?xml version="1.0" encoding="utf-8"?>
<robot name="xul307a" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find velodyne_description)/urdf/HDL-32E.urdf.xacro"/>
<xacro:include filename="$(find velodyne_description)/urdf/VLP-16.urdf.xacro"/>
<link name="base_footprint">
<visual>
<geometry>
<sphere radius="0.001" />
</geometry>
</visual>
</link>
<link name="base_link">
<inertial>
<origin xyz="0 0 0" rpy="0 0 0" />
<mass value="4049.8" />
<!-- <inertia
ixx="7926.94"
ixy="0"
ixz="0"
iyy="7723.8"
iyz="0"
izz="613.68" /> -->
<inertia
ixx="613.68"
ixy="0"
ixz="0"
iyy="7723.8"
iyz="0"
izz="7926.94" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size="4.72 0.78 1.1" />
</geometry>
<material name="">
<color rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size="4.72 0.78 1.1" />
</geometry>
</collision>
</link>
<joint name="base_link2base_footprint" type="fixed">
<parent link="base_footprint" />
<child link="base_link"/>
<origin xyz="0 0 1.067" />
</joint>
<link name="back_right_wheel">
<inertial>
<origin xyz="0 0 0" rpy="0 0 0" />
<mass value="639.613" />
<inertia
ixx="87.55"
ixy="0.00"
ixz="0.00"
iyy="87.55"
iyz="0.00"
izz="156.705" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<cylinder radius="0.7" length="0.415" />
</geometry>
<material name="">
<color rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<cylinder radius="0.7" length="0.415" />
</geometry>
</collision>
</link>
<joint
name="back_right_wheel2base_link"
type="continuous">
<origin
xyz="0.59665 -0.7504 -0.3671"
rpy="1.5708 0 0" />
<parent
link="base_link" />
<child
link="back_right_wheel" />
<axis
xyz="0 0 -1" />
</joint>
<link
name="back_left_wheel">
<inertial>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<mass
value="639.613" />
<inertia
ixx="87.55"
ixy="0.00"
ixz="0.00"
iyy="87.55"
iyz="0.00"
izz="156.705" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<cylinder radius="0.7" length="0.415" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<cylinder radius="0.7" length="0.415" />
</geometry>
</collision>
</link>
<joint
name="back_left_wheel2base_link"
type="continuous">
<origin
xyz="0.59665 0.7504 -0.3671"
rpy="1.5707 0 -3.1416" />
<parent
link="base_link" />
<child
link="back_left_wheel" />
<axis
xyz="0 0 -1" />
</joint>
<link
name="steering_hinge_link">
<inertial>
<origin
xyz="2.127 0 0"
rpy="0 0 0" />
<mass
value="2985.8" />
<!-- <inertia
ixx="3314.3"
ixy="0"
ixz="0"
iyy="3164.6"
iyz="0"
izz="452.45" /> -->
<inertia
ixx="452.45"
ixy="0"
ixz="0"
iyy="3314.3"
iyz="0"
izz="3164.6" />
</inertial>
<visual>
<origin
xyz="2.127 0 0"
rpy="0 0 0" />
<geometry>
<box size="3.48 0.78 1.1" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="2.127 0 0"
rpy="0 0 0" />
<geometry>
<box size="3.48 0.78 1.1" />
</geometry>
</collision>
</link>
<joint
name="steering_hinge2base_link"
type="revolute">
<origin
xyz="2.36 0 0"
rpy="0 0 0" />
<parent
link="base_link" />
<child
link="steering_hinge_link" />
<axis
xyz="0 0 -1" />
<limit
lower="-0.74"
upper="0.74"
effort="0"
velocity="0" />
</joint>
<link
name="front_right_wheel">
<inertial>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<mass
value="639.613" />
<inertia
ixx="87.55"
ixy="0.00"
ixz="0.00"
iyy="87.55"
iyz="0.00"
izz="156.705" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<cylinder radius="0.7" length="0.415" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<cylinder radius="0.7" length="0.415" />
</geometry>
</collision>
</link>
<joint
name="front_right_wheel2steering_hinge"
type="continuous">
<origin
xyz="1.42 -0.75 -0.366"
rpy="1.57 0 0" />
<parent
link="steering_hinge_link" />
<child
link="front_right_wheel" />
<axis
xyz="0 0 -1" />
</joint>
<link
name="front_left_wheel">
<inertial>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<mass
value="639.613" />
<inertia
ixx="87.55"
ixy="0.00"
ixz="0.00"
iyy="87.55"
iyz="0.00"
izz="156.705" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<cylinder radius="0.7" length="0.415" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<cylinder radius="0.7" length="0.415" />
</geometry>
</collision>
</link>
<joint
name="front_left_wheel2steering_hinge"
type="continuous">
<origin
xyz="1.42 0.75 -0.366"
rpy="1.57 0 -3.14" />
<parent
link="steering_hinge_link" />
<child
link="front_left_wheel" />
<axis
xyz="0 0 -1" />
</joint>
<VLP-16 parent="base_link" name="velodyne_front" topic="/velodyne_points_front" hz="10" samples="1801" min_range="0.8" max_range="130.0" min_angle="-${M_PI}" max_angle="${M_PI}" >
<origin xyz="2.505 0 1.26" rpy="0 0 0" />
</VLP-16>
<VLP-16 parent="base_link" name="velodyne_back" topic="/velodyne_points_back" hz="10" samples="1801" min_range="0.8" max_range="130.0" min_angle="-${M_PI}" max_angle="${M_PI}" >
<origin xyz="-2.2056 0 0.7538" rpy="0 0 3.14" />
</VLP-16>
<!-- motors and transmissions for the two rear wheels -->
<transmission name="tran1">
<type>transmission_interface/SimpleTransmission</type>
<joint name="back_right_wheel2base_link">
<hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
</joint>
<actuator name="motor1">
<hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="tran2">
<type>transmission_interface/SimpleTransmission</type>
<joint name="back_left_wheel2base_link">
<hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
</joint>
<actuator name="motor2">
<hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<!-- EPS and transmissions for the front steering -->
<transmission name="tran3">
<type>transmission_interface/SimpleTransmission</type>
<joint name="steering_hinge2base_link">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="eps">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
<motorTorqueConstant>1000000</motorTorqueConstant>
</actuator>
</transmission>
<!-- Friction Parametres -->
<gazebo reference="back_right_wheel">
<mu1>10000000</mu1>
<mu2>10000000</mu2>
<kp>10000000</kp>
<kd>1</kd>
</gazebo>
<gazebo reference="back_left_wheel">
<mu1>10000000</mu1>
<mu2>10000000</mu2>
<kp>10000000</kp>
<kd>1</kd>
</gazebo>
<gazebo reference="front_right_wheel">
<mu1>10000000</mu1>
<mu2>10000000</mu2>
<kp>10000000</kp>
<kd>1</kd>
</gazebo>
<gazebo reference="front_left_wheel">
<mu1>10000000</mu1>
<mu2>10000000</mu2>
<kp>10000000</kp>
<kd>1</kd>
</gazebo>
<!-- Gazebo Plugins -->
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<robotNamespace>/xul307a</robotNamespace>
<robotParam>robot_description</robotParam>
<robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
<!-- <legacyModeNS>true</legacyModeNS> -->
</plugin>
</gazebo>
<gazebo>
<plugin name="joint_state_publisher" filename="libgazebo_ros_joint_state_publisher.so">
<jointName>back_right_wheel2base_link, back_left_wheel2base_link, steering_hinge2base_link,front_right_wheel2steering_hinge,front_left_wheel2steering_hinge</jointName>
<updateRate>50.0</updateRate>
<robotNamespace>/xul307a</robotNamespace>
<alwaysOn>true</alwaysOn>
</plugin>
</gazebo>
</robot>