add add_drillbit() and fix 7b0c766e

src/modular/URDF_writer.py

@@ -180,8 +180,8 @@ def write_srdf(self, builder_joint_map=None):
             group_name = "chain" + self.urdf_writer.branch_switcher.get(i + 1)
             # group_name = "arm" + self.urdf_writer.branch_switcher.get(i + 1)
             groups.append(ET.SubElement(root, 'group', name=group_name))
-            base_link = self.urdf_writer.get_chain_base_link(joints_chain)
-            tip_link = self.urdf_writer.get_chain_tip_link(joints_chain)
+            base_link = self.urdf_writer.find_chain_base_link(joints_chain)
+            tip_link = self.urdf_writer.find_chain_tip_link(joints_chain)
             chains.append(ET.SubElement(groups[i], 'chain', base_link=base_link, tip_link=tip_link))
             i += 1
         i = 0
@@ -392,8 +392,8 @@ def write_srdf(self, builder_joint_map=None):
             group_name = "arm" + self.urdf_writer.branch_switcher.get(i + 1)
             #group_name = "chain_"+str(i+1)
             groups.append(ET.SubElement(root, 'group', name=group_name))
-            base_link = self.urdf_writer.get_chain_base_link(joints_chain)
-            tip_link = self.urdf_writer.get_chain_tip_link(joints_chain)
+            base_link = self.urdf_writer.find_chain_base_link(joints_chain)
+            tip_link = self.urdf_writer.find_chain_tip_link(joints_chain)
             chains.append(ET.SubElement(groups[i], 'chain', base_link=base_link, tip_link=tip_link))
             i += 1
         i = 0
@@ -2252,6 +2252,53 @@ def add_socket(self, x_offset=0.0, y_offset=0.0, z_offset=0.0, angle_offset=0.0)
         # self.print(self.parent_module)
 
         return data
+
+    def add_drillbit(self, length=0.27, radius=0.012, mass=0.1):
+        drillbit_name = 'drillbit'+ self.parent_module.tag
+        ET.SubElement(self.root,
+            "xacro:add_cylinder",
+            type="drillbit",
+            name=drillbit_name,
+            size_z=str(length),
+            mass=str(mass),
+            radius=str(radius))
+
+        trasl = tf.transformations.translation_matrix((0.0, 0.0, length))
+        rot = tf.transformations.euler_matrix(0.0, 0.0, 0.0, 'sxyz')
+        transform = ModuleNode.get_rototranslation(trasl, rot)
+        x, y, z, roll, pitch, yaw = ModuleNode.get_xyzrpy(transform)
+
+        father_name = self.parent_module.tcp_name
+
+        ET.SubElement(self.root,
+                      "xacro:add_fixed_joint",
+                      type="fixed_joint",
+                      name="fixed_" + drillbit_name,
+                      father=father_name,
+                      child=drillbit_name,
+                      x=x,
+                      y=y,
+                      z=z,
+                      roll=roll,
+                      pitch=pitch,
+                      yaw=yaw)
+
+        self.collision_elements.append((father_name, drillbit_name))
+
+        if self.speedup:
+            self.urdf_string = ""
+        else:
+            # Process the urdf string by calling the process_urdf method. Parse, convert from xacro and write to string
+            self.urdf_string = self.process_urdf()
+
+        # Create a dictionary containing the urdf string just processed and other parameters needed by the web app
+        data = {'result': self.urdf_string,
+                'lastModule_type': self.parent_module.type,
+                'lastModule_name': self.parent_module.name,
+                'flange_size': self.parent_module.flange_size,
+                'count': self.parent_module.i}
+
+        return data
 
     # Add a cylinder as a fake end-effector
     def add_simple_ee(self, x_offset=0.0, y_offset=0.0, z_offset=0.0, angle_offset=0.0, mass=1.0, radius=0.02):
@@ -2270,7 +2317,7 @@ def add_simple_ee(self, x_offset=0.0, y_offset=0.0, z_offset=0.0, angle_offset=0
                       "xacro:add_cylinder",
                       type="simple_ee",
                       name=simple_ee.name,
-                      flange_size_z=str(z_offset),
+                      size_z=str(z_offset),
                       mass=str(mass),
                       radius=str(radius))
 
@@ -3717,7 +3764,7 @@ def add_acm_to_srdf(self, srdf):
 
             # ensure the urdf string has been already generated
             if getattr(self, 'urdf_string', None) is None:
-                self.write_urdf()
+                raise RuntimeError("URDF string not generated yet, please call write_urdf() or process_urdf() first")
 
             # set verbosity level of the mcdc module
             if self.logger.level == logging.DEBUG: