class BirthClinic(Model): """ Birth clinic model, consists of two cylinders, one of which is rotated. """ 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 _build_body(self, traversed, model, plugin, component, link=None, child_joints=None): """ :param traversed: Set of components that were already traversed, prevents loops (since all connections are two-sided). :type traversed: set :param model: :param plugin: :param component: :type component: Component :param link: :type link: Link :return: The primary link used in this traversal, i.e. the link passed to it or the first link created. """ if component in traversed: return link create_link = link is None traversed.add(component) if create_link: # New tree, create a link. The component name # should contain the body part ID so this name should # be unique. # The position of the link is determined later when we decide # its center of mass. link = Link("link_%s" % component.name, self_collide=True) child_joints = [] model.add_element(link) # Add this component to the link. position = link.to_local_frame(component.get_position()) rotation = link.to_local_direction(component.get_rotation()) component.set_position(position) component.set_rotation(rotation) link.add_element(component) # Add sensors registered on this component plugin.add_elements(component.get_plugin_sensors(link)) for conn in component.connections: if conn.joint: # Create the subtree after the joint other_link = self._build_body(traversed, model, plugin, conn.other) if other_link is None: # This connection was already created # from the other side. continue # Check whether this component is the parent or the child # of the joint connection (it is the parent by default) # This is relevant for another reason, because we will # be moving the internals of the current joint around # to center the inertia later on. This means that for # joints for which the current link is the child we need # to move the joint as well since the joint position is # expressed in the child frame. If the current link is # the parent there is no need to move it with this one, # and since `create_joint` is called after everything # within the child link has been fully neither does # the other link code. parent_link = link child_link = other_link is_child = conn.joint.parent is not component if is_child: parent_link, child_link = other_link, link joint = conn.joint.create_joint(parent_link, child_link, conn.joint.parent, conn.joint.child) model.add_element(joint) if is_child: child_joints.append(joint) else: # Just add this element to the current link self._build_body(traversed, model, plugin, conn.other, link, child_joints) if create_link: # Give the link the inertial properties of the combined collision, # only calculated by the item which created the link. # Note that we need to align the center of mass with the link's origin, # which will move the internal components around. In order to compensate # for this in the internal position we need to reposition the link according # to this change. translation = link.align_center_of_mass() link.translate(-translation) link.calculate_inertial() # Translate all the joints expressed in this link's frame that # were created before repositioning # Note that the joints need the same translation as the child elements # rather than the counter translation of the link. for joint in child_joints: joint.translate(translation) return link
# them around together. group = PosableGroup() group.add_element(link) group.add_element(minibox) # Move and rotate the group to confuse the mechanism # (this should just be undone at the align later) group.rotate_around(Vector3(1, 1, 0), 0.1 * pi) group.translate(Vector3(0.6, 0.7, 0.8)) # Create a new, larger box called link 2 link2 = Link("my_link_2") link2.make_box(2.0, 4, 3, 3) # Translate and rotate just to add some extra complexity link2.translate(Vector3(0.5, 0.5, 2)) link2.rotate_around(Vector3(1, 1, 1), 0.5 * pi) # Now align the group so its right center lands at # the top center of link 2 group.align( # Center of the right face of box 1 Vector3(1, 0, 0), # Vector normal to box 2 right face Vector3(1, 0, 0), # Vector normal to box 2 top face should align with...(*) Vector3(0, 0, 1), # Center of the top face of box 2