collision object for the print head (implement #22)

models/urdf/ur5e.urdf

 <?xml version="1.0" ?>
-<!-- =================================================================================== -->
-<!-- |    This document was autogenerated by xacro from ../../universal_robot/ur_e_description/urdf/ur5e_robot.urdf.xacro | -->
-<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
-<!-- =================================================================================== -->
 <robot name="ur5e">
-  <gazebo>
-    <plugin filename="libgazebo_ros_control.so" name="ros_control">
-      <!--robotNamespace>/</robotNamespace-->
-      <!--robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType-->
-    </plugin>
-    <!--
-    <plugin name="gazebo_ros_power_monitor_controller" filename="libgazebo_ros_power_monitor.so">
-      <alwaysOn>true</alwaysOn>
-      <updateRate>1.0</updateRate>
-      <timeout>5</timeout>
-      <powerStateTopic>power_state</powerStateTopic>
-      <powerStateRate>10.0</powerStateRate>
-      <fullChargeCapacity>87.78</fullChargeCapacity>     
-      <dischargeRate>-474</dischargeRate>
-      <chargeRate>525</chargeRate>
-      <dischargeVoltage>15.52</dischargeVoltage>
-      <chargeVoltage>16.41</chargeVoltage>
-    </plugin>
--->
-  </gazebo>
-  <!--property name="shoulder_height" value="0.089159" /-->
-  <!--property name="shoulder_offset" value="0.13585" /-->
-  <!-- shoulder_offset - elbow_offset + wrist_1_length = 0.10915 -->
-  <!--property name="upper_arm_length" value="0.42500" /-->
-  <!--property name="elbow_offset" value="0.1197" /-->
-  <!-- CAD measured -->
-  <!--property name="forearm_length" value="0.39225" /-->
-  <!--property name="wrist_1_length" value="0.093" /-->
-  <!-- CAD measured -->
-  <!--property name="wrist_2_length" value="0.09465" /-->
-  <!-- In CAD this distance is 0.930, but in the spec it is 0.09465 -->
-  <!--property name="wrist_3_length" value="0.0823" /-->
-  <!-- manually measured -->
   <link name="base_link">
     <visual>
       <geometry>
@@ -331,6 +294,29 @@
     <parent link="wrist_3_link"/>
     <child link="tool0"/>
   </joint>
+
+  <link name="print_tip">
+    <visual>
+      <origin rpy="0 0 0" xyz="0.075 0 0"/>
+      <geometry>
+        <box size="0.15 0.1 0.1"/>
+      </geometry>
+      <material name="pt_material">
+        <color rgba="0.3 0.8 0.3 0.95"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.075 0 0"/>
+      <geometry>
+        <box size="0.15 0.1 0.1"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="pt_to_robot" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <parent link="ee_link"/>
+    <child link="print_tip"/>
+  </joint>
   <link name="world"/>
   <joint name="world_joint" type="fixed">
     <parent link="world"/>

src/tip_T_test.py

@@ -10,9 +10,7 @@ home = [0, -2.1, 2.1, -pi/2, -pi/2, 0]
 def test_pos(pos, env):
     js = env.ik_pos(pos, 0)
     env.setJoints(0, js[0])
-    p_fk= env._fk(js[0],0)[:3,3]
     p_fk_tip = env.fk(js[0],0)[:3,3]
-    print(p_fk)
     print(p_fk_tip)
 
 if __name__ == "__main__":

src/utils/simEnv.py

@@ -27,7 +27,7 @@ def defaultPlanningEnv():
     return PlanningEnv([[0,0,1,0],[0,0,0,1]],
                        [[0.5, 0, 0.001],[-0.5, 0, 0.001]],
                        [[0,0,0,1],[0,0,0,1]],
-                       [[0.1,0.01,0.05],[0.1,0.1,0.2]])
+                       [[0.15,0,0],[0.15,0,0]]) # WARNING - also edit urdf
 
 urdf_folder = Path(__file__).parent / "../../models/urdf"
 
@@ -140,14 +140,6 @@ class SimEnv:
         return np.asarray(js).reshape(n_sols,6)
 
 
-    def _fk(self, joints, robot):
-        """
-        deprecated - does not include the print tip transform
-        """
-        pmat = np.asarray(self.ur5e_kin.forward(joints)).reshape(3,4)
-        return np.linalg.inv(self.world2armT[robot]) @ np.concatenate((pmat, [[0,0,0,1]]))
-
-
     def fk(self, joints, robot):
         pmat = np.asarray(self.ur5e_kin.forward(joints)).reshape(3,4)
         return np.linalg.inv(self.world2armT[robot]) @ \