cap urdf match real hardware dimensions (fix #57)

models/urdf/ur5e_cap.urdf

@@ -298,7 +298,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0.074 0 0"/>
       <geometry>
-        <box size="0.148 0.1 0.1"/>
+        <box size="0.148 0.23 0.13"/>
       </geometry>
       <material name="pt_material">
         <color rgba="0.3 0.8 0.3 0.95"/>
@@ -307,12 +307,12 @@
     <collision>
       <origin rpy="0 0 0" xyz="0.074 0 0"/>
       <geometry>
-        <box size="0.148 0.1 0.1"/>
+        <box size="0.148 0.23 0.13"/>
       </geometry>
     </collision>
   </link>
   <joint name="pt_to_robot" type="fixed">
-    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <origin rpy="0.7854 0 0" xyz="0 0.0106 0.0106"/>
     <parent link="ee_link"/>
     <child link="print_tip"/>
   </joint>

src/gen_2cal_test.py

@@ -27,7 +27,7 @@ def create_plan(points, env, gui=False):
         for p in points[arm]:
 
             # Go to the point
-            jpoint = tu.get_closest(env.ik_pos(p,0),home, max_diff=100)
+            jpoint = tu.get_closest(env.ik_pos(p,arm),home, max_diff=100)
             go2point = plan.plan_to(jpoint, arm)
             plan.appendTrajectory([go2point], arm)
 
@@ -48,7 +48,7 @@ if __name__ == "__main__":
     env = tu.SimEnv(env_desc=env_desc2, gui=True, home=home)
 
     points1 = [
-            [ 0.215,-0.1, 0.05],
+            [-0.215,-0.1, 0.05],
             ]
     points2 = [
             [ 0.215,-0.1, 0.05],