URDF Links and Joints Orientation

edit

I have created a URDF file for my six axis robot manipulator. I can launch it to visualize it in rviz via a launch file.

Here is how it looks in rviz :

When I try to move the manipulator using the Joint State Publisher, only the first joint moves at the origin along it's axis. The other links deviate from the origin. It looks like this :

My URDF file is as follows :

<?xml version="1.0"?> 
<robot name="Mitsubishi_rv-6sd">

<!-- ...................Material Definitions............................................ -->

<material name="blue">
    <color rgba="0 0 1 1"/>
</material>

<material name="black">
    <color rgba="0 0 0 1"/>
</material>

<material name="white">
    <color rgba="1 1 1 1"/>
</material>

<material name="red">
    <color rgba="0.8 0 0 1"/>
</material>

<material name="green">
    <color rgba="0 1 0 1"/>
</material>

<material name="yellow">
    <color rgba="1 1 0 1"/>
</material>


<!---............................. Link Definitions ................................-->

<link name="base_link">
    <visual>
        <geometry>
            <mesh filename="package://demo/meshes/Final/body0.stl"/>
        </geometry>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <material name ="red"/>
    </visual>
</link>


<link name="body1">
    <visual>
        <geometry>
            <mesh filename="package://demo/meshes/Final/body1.stl"/>
        </geometry>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <material name ="yellow"/>
    </visual>
</link>


<link name="body2">
    <visual>
        <geometry>
            <mesh filename="package://demo/meshes/Final/body2.stl"/>
        </geometry>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <material name ="green"/>
    </visual>
</link>


<link name="body3">
    <visual>
        <geometry>
            <mesh filename="package://demo/meshes/Final/body3.stl"/>
        </geometry>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <material name ="blue"/>
    </visual>
</link>


<link name="body4">
    <visual>
        <geometry>
            <mesh filename="package://demo/meshes/Final/body4.stl"/>
        </geometry>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <material name ="red"/>
    </visual>
</link>


<link name="body5">
    <visual>
        <geometry>
            <mesh filename="package://demo/meshes/Final/body5.stl"/>
        </geometry>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <material name ="green"/>
    </visual>
</link>


<!--...........................Joint Definitions.....................................-->

<joint name="j1" type="revolute">
    <parent link="base_link"/>
    <child link="body1"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <limit effort="1000.0" lower="-2.96" upper="2.96" velocity="0.5"/>
</joint>


<joint name="j2" type="revolute">
    <parent link="body1"/>
    <child link="body2"/>
    <axis xyz="0 1 0"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <limit effort="1000.0" lower="-2.96" upper="2.96" velocity="0.5"/>
</joint>


<joint name="j3" type="revolute">
    <parent link="body2"/>
    <child link="body3"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <axis xyz="0 1 0"/>
    <limit effort="1000.0" lower="-2.96" upper="2.96" velocity="0.5"/>
</joint>


<joint name="j4" type="revolute">
    <parent link="body3"/>
    <child link="body4"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <axis xyz="1 0 0"/>
    <limit effort="1000.0" lower="-2.96" upper="2.96" velocity="0.5"/>
</joint>


<joint name="j5" type="revolute">
    <parent link="body4"/>
    <child link="body5"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <axis xyz="0 1 0"/>
    <limit effort="1000.0 ...