More accurate description of the AL5D kinematics

roboticstoolbox/models/DH/AL5D.py

@@ -51,11 +51,11 @@ class AL5D(DHRobot):
             # zero = 0.0
 
         # robot length values (metres)
-        a = [0, 0.002, 0.14679, 0.17751]
-        d = [-0.06858, 0, 0, 0]
+        a = [0, 0.002, 0.14533, 0.18232]
+        d = [-0.06858, 0, -0.00077, 0.0011]
 
         alpha = [pi, pi / 2, pi, pi]
-        offset = [pi / 2, pi, -0.0427, -0.0427 - pi / 2]
+        offset = [pi, -pi/2 - 0.0059, pi/2 - 0.0378, pi / 2 - 0.0319]
 
         # mass data as measured
         # mass = [0.187, 0.044, 0.207, 0.081]
@@ -85,7 +85,7 @@ class AL5D(DHRobot):
 
         links = []
 
-        for j in range(3):
+        for j in range(4):
             link = RevoluteMDH(
                 d=d[j],
                 a=a[j],
@@ -100,7 +100,7 @@ class AL5D(DHRobot):
             )
             links.append(link)
 
-        tool = SE3(0.07719, 0, 0)
+        tool = SE3(0.08573, 0, 0)*SE3.Rx(pi)
 
         super().__init__(
             links,

rtb-data/rtbdata/xacro/al5d_description/urdf/al5d_robot.urdf

@@ -21,10 +21,10 @@
     </inertial>
   </link>
 
-  <link name="link1">
+  <link name="waist">
     <visual>
       <geometry>
-        <mesh filename="package://al5d_description/meshes/link1.stl" scale="0.001 0.001 0.001"/>
+        <mesh filename="package://al5d_description/meshes/waist.stl" scale="0.001 0.001 0.001"/>
       </geometry>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <material name="black"/>
@@ -35,12 +35,12 @@
     </inertial>
   </link>
 
-  <link name="link2">
+  <link name="shoulder">
     <visual>
       <geometry>
-        <mesh filename="package://al5d_description/meshes/link2.stl" scale="0.001 0.001 0.001"/>
+        <mesh filename="package://al5d_description/meshes/shoulder.stl" scale="0.001 0.001 0.001"/>
       </geometry>
-      <origin xyz="0 0 0"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
       <material name="gray"/>
     </visual>
     <inertial>
@@ -49,12 +49,12 @@
     </inertial>
   </link>
 
-  <link name="link3">
+  <link name="elbow">
     <visual>
       <geometry>
-        <mesh filename="package://al5d_description/meshes/link3.stl" scale="0.001 0.001 0.001"/>
+        <mesh filename="package://al5d_description/meshes/elbow.stl" scale="0.001 0.001 0.001"/>
       </geometry>
-      <origin rpy="0 0 -0.0427" xyz="0 0 0"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
       <material name="gray"/>
     </visual>
     <inertial>
@@ -63,12 +63,12 @@
     </inertial>
   </link>
   
-  <link name="link4">
+  <link name="gripper">
     <visual>
       <geometry>
-        <mesh filename="package://al5d_description/meshes/link4.stl" scale="0.001 0.001 0.001"/>
+        <mesh filename="package://al5d_description/meshes/gripper.stl" scale="0.001 0.001 0.001"/>
       </geometry>
-      <origin rpy="0 0 1.528096327" xyz="0 0 0"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
       <material name="black"/>
     </visual>
     <inertial>
@@ -77,9 +77,11 @@
     </inertial>
   </link>
 
+  <link name="tool"/>
+
   <joint name="j1" type="revolute">
     <parent link="base"/>
-    <child link="link1"/>
+    <child link="waist"/>
     <origin rpy="0 3.141592653 0" xyz="0 0 0.06858"/>
     <axis xyz="0 0 1"/>
     <!-- This is descibed in child frame -->
@@ -87,8 +89,8 @@
   </joint>
 
   <joint name="j2" type="revolute">
-    <parent link="link1"/>
-    <child link="link2"/>
+    <parent link="waist"/>
+    <child link="shoulder"/>
     <origin rpy="0 1.570796325 -1.570796325" xyz="0.002 0 0"/>
     <axis xyz="0 0 1"/>
     <!-- This is descibed in child frame -->
@@ -96,20 +98,28 @@
   </joint>
 
   <joint name="j3" type="revolute">
-    <parent link="link2"/>
-    <child link="link3"/>
-    <origin rpy="0 3.141592653 1.570796325" xyz="0.14679 0 0"/>
+    <parent link="shoulder"/>
+    <child link="elbow"/>
+    <origin rpy="0 3.141592653 1.570796325" xyz="0.14679 0 0.00077"/>
     <axis xyz="0 0 1"/>
     <!-- This is descibed in child frame -->
     <limit effort="1000.0" lower="-1.570796325" upper="1.570796325" velocity="0"/>
   </joint>
 
   <joint name="j4" type="revolute">
-    <parent link="link3"/>
-    <child link="link4"/>
-    <origin rpy="3.141592653 0 1.570796325" xyz="0.17751 0 0"/>
+    <parent link="elbow"/>
+    <child link="gripper"/>
+    <origin rpy="3.141592653 0 -1.570796325" xyz="0.18232 0 0.0011"/>
     <axis xyz="0 0 1"/>
     <limit effort="1000.0" lower="-1.570796325" upper="1.570796325" velocity="0"/>
   </joint>
+
+  <joint name="fixed_joint" type="fixed">
+    <parent link="gripper"/>
+    <child link="tool"/>
+    <origin rpy="-1.570796325 0 0" xyz="0 -0.08573 0"/>
+    <axis xyz="0 0 1"/>
+  </joint>
+	
 </robot>