model fixed

Author: jhavl

examples/test.py

@@ -21,22 +21,15 @@
 
 # Add the puma to the simulator
 env.add(r)
+time.sleep(1)
 
-env.hold()
+Tep = r.fkine(r.q) * sm.SE3.Tx(1.0) * sm.SE3.Ty(1.0)
 
-# Tep = puma.fkine() * sm.SE3.Tz(0.1)
+arrived = False
 
-# arrived = False
+dt = 0.05
 
-# dt = 0.05
-
-# while not arrived:
-
-#     start = time.time()
-#     v, arrived = rtb.p_servo(puma.fkine(), Tep, 0.1)
-#     puma.qd = np.linalg.pinv(puma.jacobe()) @ v
-#     env.step(50)
-#     stop = time.time()
-
-#     if stop - start < dt:
-#         time.sleep(dt - (stop - start))
+while not arrived:
+    v, arrived = rtb.p_servo(r.fkine(r.q), Tep, 0.1)
+    r.qd = np.linalg.pinv(r.jacobe(r.q)) @ v
+    env.step(dt)

roboticstoolbox/models/URDF/Frankie.py

@@ -40,8 +40,6 @@ def __init__(self):
             gripper_links=elinks[11]
         )
 
-        print(self)
-
         self.qdlim = np.array([
             4.0, 4.0,
             2.1750, 2.1750, 2.1750, 2.1750, 2.6100, 2.6100, 2.6100,

roboticstoolbox/models/URDF/xacro/franka_description/robots/frankie_arm.xacro

@@ -12,17 +12,6 @@
     </xacro:unless>
 
     <link name="${arm_id}_base0">
-      <visual>
-        <origin xyz="0 0 0.15" rpy="0 0 0"/>
-        <geometry>
-          <box size="0.6 0.4 0.3" />
-        </geometry>
-      </visual>
-      <collision>
-        <geometry>
-          <box size="0.4 0.6 0.5" />
-        </geometry>
-      </collision>
     </link>
     <link name="${arm_id}_base1">
     </link>
@@ -41,6 +30,18 @@
       <limit effort="87" lower="-999999" upper="999999" velocity="4.0"/>
     </joint>
     <link name="${arm_id}_link0">
+      <visual>
+        <origin xyz="0 0 -0.15" rpy="0 0 0"/>
+        <geometry>
+          <box size="0.6 0.4 0.3" />
+        </geometry>
+      </visual>
+      <collision>
+        <origin xyz="0 0 -0.15" rpy="0 0 0"/>
+        <geometry>
+          <box size="0.6 0.4 0.3" />
+        </geometry>
+      </collision>
       <visual>
         <geometry>
           <mesh filename="package://${description_pkg}/meshes/visual/link0.dae"/>