From 6fd67593289bbe4e5e5d2beb7ef81f0e671fa240 Mon Sep 17 00:00:00 2001 From: EdoardoRomiti Date: Mon, 9 Oct 2023 15:14:42 +0200 Subject: [PATCH] add `add_drillbit()` and fix `7b0c766e` --- src/modular/URDF_writer.py | 59 ++++++++++++++++++++++++++++++++++---- 1 file changed, 53 insertions(+), 6 deletions(-) diff --git a/src/modular/URDF_writer.py b/src/modular/URDF_writer.py index fed5b9f..d52534a 100644 --- a/src/modular/URDF_writer.py +++ b/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: