ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
1 | initial version |
You moved the link collision and visual relative to the joint axis, you probably shoud fix your front wheels so they aren't fixed. This should be less froglike:
<?xml version="1.0" ?>
<robot name="two_wheel_robot" xmlns:xacro="http://www.ros.org/wiki/xacro">
<material name="black">
<color rgba="0.0 0.0 0.0 1.0"/>
</material>
<material name="blue">
<color rgba="0.203125 0.23828125 0.28515625 1.0"/>
</material>
<material name="green">
<color rgba="0.0 0.8 0.0 1.0"/>
</material>
<material name="grey">
<color rgba="0.2 0.2 0.2 1.0"/>
</material>
<material name="orange">
<color rgba="1.0 0.423529411765 0.0392156862745 1.0"/>
</material>
<material name="brown">
<color rgba="0.870588235294 0.811764705882 0.764705882353 1.0"/>
</material>
<material name="red">
<color rgba="0.80078125 0.12890625 0.1328125 1.0"/>
</material>
<material name="white">
<color rgba="1.0 1.0 1.0 1.0"/>
</material>
<gazebo reference="link_chassis">
<material>Gazebo/Orange</material>
</gazebo>
<gazebo reference="link_left_wheel">
<material>Gazebo/Blue</material>
</gazebo>
<gazebo reference="link_right_wheel">
<material>Gazebo/Blue</material>
</gazebo>
<gazebo>
<plugin filename="libgazebo_ros_diff_drive.so" name="differential_drive_controller">
<legacyMode>false</legacyMode>
<alwaysOn>true</alwaysOn>
<updateRate>20</updateRate>
<leftJoint>joint_left_wheel</leftJoint>
<rightJoint>joint_right_wheel</rightJoint>
<rightJoint>front_joint_right_wheel</rightJoint>
<leftJoint>front_joint_left_wheel</leftJoint>
<wheelSeparation>0.2</wheelSeparation>
<wheelDiameter>0.2</wheelDiameter>
<torque>0.5</torque>
<commandTopic>cmd_vel</commandTopic>
<odometryTopic>odom</odometryTopic>
<odometryFrame>odom</odometryFrame>
<robotBaseFrame>link_chassis</robotBaseFrame>
</plugin>
</gazebo>
<link name="link_chassis">
<!-- pose and inertial -->
<pose>0 0 0.1 0 0 0</pose>
<inertial>
<mass value="5"/>
<origin rpy="0 0 0" xyz="0 0 0.1"/>
<inertia ixx="0.0395416666667" ixy="0" ixz="0" iyy="0.106208333333" iyz="0" izz="0.106208333333"/>
</inertial>
<!-- body -->
<collision name="collision_chassis">
<geometry>
<box size="1.0 0.6 0.07"/>
</geometry>
</collision>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="1.0 0.6 0.07"/>
</geometry>
<material name="blue"/>
</visual>
</link>
<link name="link_right_wheel">
<inertial>
<mass value="0.2"/>
<origin rpy="0 1.5707 1.5707" xyz="0 0 0"/>
<inertia ixx="0.000526666666667" ixy="0" ixz="0" iyy="0.000526666666667" iyz="0" izz="0.001"/>
</inertial>
<collision name="link_right_wheel_collision">
<origin rpy="0 1.5707 1.5707" xyz="0 0 0"/>
<geometry>
<cylinder length="0.04" radius="0.1"/>
</geometry>
</collision>
<visual name="link_right_wheel_visual">
<origin rpy="0 1.5707 1.5707" xyz="0 0 0"/>
<geometry>
<cylinder length="0.06" radius="0.1"/>
</geometry>
</visual>
</link>
<joint name="joint_right_wheel" type="continuous">
<!-- <joint name="joint_right_wheel" type="fixed"> -->
<origin rpy="0 0 0" xyz="-0.30 0.3 0"/>
<child link="link_right_wheel"/>
<parent link="link_chassis"/>
<axis rpy="0 0 0" xyz="0 1 0"/>
<limit effort="10000" velocity="1000"/>
<joint_properties damping="1.0" friction="1.0"/>
</joint>
<link name="link_left_wheel">
<inertial>
<mass value="0.2"/>
<origin rpy="0 1.5707 1.5707" xyz="0 0 0"/>
<inertia ixx="0.000526666666667" ixy="0" ixz="0" iyy="0.000526666666667" iyz="0" izz="0.001"/>
</inertial>
<collision name="link_left_wheel_collision">
<origin rpy="0 1.5707 1.5707" xyz="0 0 0"/>
<geometry>
<cylinder length="0.04" radius="0.1"/>
</geometry>
</collision>
<visual name="link_left_wheel_visual">
<origin rpy="0 1.5707 1.5707" xyz="0 0 0"/>
<geometry>
<cylinder length="0.06" radius="0.1"/>
</geometry>
</visual>
</link>
<joint name="joint_left_wheel" type="continuous">
<!-- <joint name="joint_left_wheel" type="fixed"> -->
<origin rpy="0 0 0" xyz="-0.3 -0.3 0"/>
<child link="link_left_wheel"/>
<parent link="link_chassis"/>
<axis rpy="0 0 0" xyz="0 1 0"/>
<limit effort="10000" velocity="1000"/>
<joint_properties damping="1.0" friction="1.0"/>
</joint>
<link name="front_link_right_wheel">
<inertial>
<mass value="0.2"/>
<origin rpy="0 1.5707 1.5707" xyz="0.15 0.15 0"/>
<inertia ixx="0.000526666666667" ixy="0" ixz="0" iyy="0.000526666666667" iyz="0" izz="0.001"/>
</inertial>
<collision name="front_link_right_wheel_collision">
<origin rpy="0 1.5707 1.5707" xyz="0.15 0.15 0"/>
<geometry>
<cylinder length="0.04" radius="0.1"/>
</geometry>
</collision>
<visual name="front_link_right_wheel_visual">
<origin rpy="0 1.5707 1.5707" xyz="0.15 0.15 0"/>
<geometry>
<cylinder length="0.06" radius="0.1"/>
</geometry>
</visual>
</link>
<!-- <joint name="front_joint_right_wheel" type="continuous"> -->
<joint name="front_joint_right_wheel" type="fixed">
<origin rpy="0 0 0" xyz="0.15 0.15 0"/>
<child link="front_link_right_wheel"/>
<parent link="link_chassis"/>
<axis rpy="0 0 0" xyz="0 1 0"/>
<limit effort="10000" velocity="1000"/>
<joint_properties damping="1.0" friction="1.0"/>
</joint>
<link name="front_link_left_wheel">
<inertial>
<mass value="0.2"/>
<origin rpy="0 1.5707 1.5707" xyz="0.15 -0.15 0"/>
<inertia ixx="0.000526666666667" ixy="0" ixz="0" iyy="0.000526666666667" iyz="0" izz="0.001"/>
</inertial>
<collision name="front_link_left_wheel_collision">
<origin rpy="0 1.5707 1.5707" xyz="0.15 -0.15 0"/>
<geometry>
<cylinder length="0.04" radius="0.1"/>
</geometry>
</collision>
<visual name="front_link_left_wheel_visual">
<origin rpy="0 1.5707 1.5707" xyz="0.15 -0.15 0"/>
<geometry>
<cylinder length="0.06" radius="0.1"/>
</geometry>
</visual>
</link>
<!-- <joint name="front_joint_left_wheel" type="continuous"> -->
<joint name="front_joint_left_wheel" type="fixed">
<origin rpy="0 0 0" xyz="0.15 -0.15 0"/>
<child link="front_link_left_wheel"/>
<parent link="link_chassis"/>
<axis rpy="0 0 0" xyz="0 1 0"/>
<limit effort="10000" velocity="1000"/>
<joint_properties damping="1.0" friction="1.0"/>
</joint>
</robot>