def __init__(self, diameter=3.0, height=3.0, name="birth_clinic"): """ :param diameter: Intended diameter of the birth clinic :param name: :return: """ super(BirthClinic, self).__init__(name=name, static=True) self.diameter = diameter scale = diameter / MESH_DIAMETER # Cannot go higher than mesh height, or lower than the bottom # of the slice. scaled_height = scale * MESH_HEIGHT self.height = max(min(height, scaled_height), SLICE_FRACTION * scaled_height) mesh = Mesh("model://tol_robot/meshes/BirthClinic.dae", scale=scale) col = Collision("bc_col", mesh) surf = Element(tag_name="surface") friction = Friction( friction=0.01, friction2=0.01, slip1=1.0, slip2=1.0 ) contact = "<contact>" \ "<ode>" \ "<kd>%s</kd>" \ "<kp>%s</kp>" \ "</ode>" \ "</contact>" % ( nf(constants.SURFACE_KD), nf(constants.SURFACE_KP) ) surf.add_element(friction) surf.add_element(contact) col.add_element(surf) vis = Visual("bc_vis", mesh.copy()) self.link = Link("bc_link", elements=[col, vis]) # By default the model has 0.5 * scale * MESH_HEIGHT below # and above the surface. Translate such that we have exactly # the desired height instead. self.link.translate(Vector3(0, 0, self.height - (0.5 * scaled_height))) self.add_element(self.link)
def _initialize(self, **kwargs): """ :param kwargs: :return: """ # Create components. Visuals are provided by the conn_a/conn_b meshes self.hinge_root = self.create_component( Box(SLOT_THICKNESS, SLOT_WIDTH, SLOT_WIDTH, MASS_SLOT), "slot_a", visual=False) self.hinge_tail = self.create_component( Box(SLOT_THICKNESS, SLOT_WIDTH, SLOT_WIDTH, MASS_SLOT), "slot_b", visual=False) mesh = Mesh("model://tol_robot/meshes/PassiveHinge.dae") visual_a = Visual("conn_a_visual", mesh) visual_a.translate(Vector3(-0.5 * SLOT_THICKNESS)) conn_a = self.create_component( Box(CONNECTION_PART_LENGTH, CONNECTION_PART_THICKNESS, CONNECTION_PART_HEIGHT, MASS_FRAME), "conn_a", visual=visual_a) # Flip visual along the x-axis by rotating PI degrees over z # This will put it upside down, so we also flip it PI degrees over x visual_b = Visual("conn_b_visual", mesh.copy()) visual_b.rotate_around(Vector3(1, 0, 0), math.pi, relative_to_child=False) visual_b.rotate_around(Vector3(0, 0, 1), math.pi, relative_to_child=False) visual_b.translate(Vector3(0.5 * SLOT_THICKNESS)) conn_b = self.create_component( Box(CONNECTION_PART_LENGTH, CONNECTION_PART_THICKNESS, CONNECTION_PART_HEIGHT, MASS_FRAME), "conn_b", visual=visual_b) # Shorthand for variables # Position connection part a x_part_a = SLOT_THICKNESS / 2.0 + SEPARATION + CONNECTION_PART_LENGTH / 2.0 conn_a.set_position(Vector3(x_part_a, 0, 0)) # Position connection part b x_part_b = x_part_a + (CONNECTION_PART_LENGTH / 2.0 - (CONNECTION_PART_LENGTH - CONNECTION_ROTATION_OFFSET)) * 2 conn_b.set_position(Vector3(x_part_b, 0, 0)) # Make the tail x_tail = x_part_b + CONNECTION_PART_LENGTH / 2.0 + SEPARATION + SLOT_THICKNESS / 2.0 self.hinge_tail.set_position(Vector3(x_tail, 0, 0)) # Fix components self.fix(self.hinge_root, conn_a) self.fix(self.hinge_tail, conn_b) # Connection part a <(hinge)> connection part b # Hinge joint axis should point straight up, and anchor # the points in the center. Note that the position of a joint # is expressed in the child link frame, so we need to take the # position from the original code and subtract conn_b's position self.joint = Joint("revolute", conn_a, conn_b, axis=Vector3(0, 0, 1)) self.joint.set_position(Vector3(CONNECTION_PART_LENGTH / 2.0 - CONNECTION_ROTATION_OFFSET, 0, 0)) self.joint.axis.limit = Limit( upper=constants.HINGE_LIMIT, lower=-constants.HINGE_LIMIT ) self.add_joint(self.joint) # Apply color mixin self.apply_color()
def _initialize(self, **kwargs): """ :param kwargs: :return: """ # Create components. Visuals are provided by the conn_a/conn_b meshes self.hinge_root = self.create_component(Box(SLOT_THICKNESS, SLOT_WIDTH, SLOT_WIDTH, MASS_SLOT), "slot_a", visual=False) self.hinge_tail = self.create_component(Box(SLOT_THICKNESS, SLOT_WIDTH, SLOT_WIDTH, MASS_SLOT), "slot_b", visual=False) mesh = Mesh("model://tol_robot/meshes/PassiveHinge.dae") visual_a = Visual("conn_a_visual", mesh) visual_a.translate(Vector3(-0.5 * SLOT_THICKNESS)) conn_a = self.create_component(Box(CONNECTION_PART_LENGTH, CONNECTION_PART_THICKNESS, CONNECTION_PART_HEIGHT, MASS_FRAME), "conn_a", visual=visual_a) # Flip visual along the x-axis by rotating PI degrees over z # This will put it upside down, so we also flip it PI degrees over x visual_b = Visual("conn_b_visual", mesh.copy()) visual_b.rotate_around(Vector3(1, 0, 0), math.pi, relative_to_child=False) visual_b.rotate_around(Vector3(0, 0, 1), math.pi, relative_to_child=False) visual_b.translate(Vector3(0.5 * SLOT_THICKNESS)) conn_b = self.create_component(Box(CONNECTION_PART_LENGTH, CONNECTION_PART_THICKNESS, CONNECTION_PART_HEIGHT, MASS_FRAME), "conn_b", visual=visual_b) # Shorthand for variables # Position connection part a x_part_a = SLOT_THICKNESS / 2.0 + SEPARATION + CONNECTION_PART_LENGTH / 2.0 conn_a.set_position(Vector3(x_part_a, 0, 0)) # Position connection part b x_part_b = x_part_a + ( CONNECTION_PART_LENGTH / 2.0 - (CONNECTION_PART_LENGTH - CONNECTION_ROTATION_OFFSET)) * 2 conn_b.set_position(Vector3(x_part_b, 0, 0)) # Make the tail x_tail = x_part_b + CONNECTION_PART_LENGTH / 2.0 + SEPARATION + SLOT_THICKNESS / 2.0 self.hinge_tail.set_position(Vector3(x_tail, 0, 0)) # Fix components self.fix(self.hinge_root, conn_a) self.fix(self.hinge_tail, conn_b) # Connection part a <(hinge)> connection part b # Hinge joint axis should point straight up, and anchor # the points in the center. Note that the position of a joint # is expressed in the child link frame, so we need to take the # position from the original code and subtract conn_b's position self.joint = Joint("revolute", conn_a, conn_b, axis=Vector3(0, 0, 1)) self.joint.set_position( Vector3(CONNECTION_PART_LENGTH / 2.0 - CONNECTION_ROTATION_OFFSET, 0, 0)) self.joint.axis.limit = Limit(upper=constants.HINGE_LIMIT, lower=-constants.HINGE_LIMIT) self.add_joint(self.joint) # Apply color mixin self.apply_color()