def gen_boxes(dimensions=4, spacing=0.5, size=0.05): for x in range(-dimensions,dimensions+1): for y in range(-dimensions,dimensions+1): l = Link("box") #box_geom = Box(1.0, 1.0, 1.0, mass=0.5) #b = StructureCombination("box", box_geom) #l.add_element(b) if (x!=0 or y!=0): l.make_box(1, size, size, 0.01*max(abs(x),abs(y))) pos = Vector3(spacing*x,spacing*y,0) l.set_position(pos) l.rotate_around(Vector3(0, 0, 1), math.radians(x*y), relative_to_child=False) model.add_element(l)
def test_direction_conversion(self): """ Tests converting vectors between direction frames. :return: """ link = Link("my_link") point = Vector3(0, 0, 1) # At this point, it should be the same in the # parent direction x, y, z = link.to_parent_direction(point) self.assertAlmostEqual(x, point.x) self.assertAlmostEqual(y, point.y) self.assertAlmostEqual(z, point.z) # Now rotate the link 90 degrees over (1, 1, 0), # this should cause the local vector (0, 1, 1) # to land at 0.5 * [sqrt(2), -sqrt(2), 0] link.rotate_around(Vector3(1, 1, 0), 0.5 * pi, relative_to_child=False) hs2 = 0.5 * sqrt(2) x, y, z = link.to_parent_direction(point) self.assertAlmostEqual(x, hs2) self.assertAlmostEqual(y, -hs2) self.assertAlmostEqual(z, 0)
def test_direction_conversion(self): """ Tests converting vectors between direction frames. :return: """ link = Link("my_link") point = Vector3(0, 0, 1) # At this point, it should be the same in the # parent direction x, y, z = link.to_parent_direction(point) self.assertAlmostEqual(x, point.x) self.assertAlmostEqual(y, point.y) self.assertAlmostEqual(z, point.z) # Now rotate the link 90 degrees over (1, 1, 0), # this should cause the local vector (0, 1, 1) # to land at 0.5 * [sqrt(2), -sqrt(2), 0] link.rotate_around(Vector3(1, 1, 0), 0.5 * pi, relative_to_child=False) hs2 = 0.5 * sqrt(2) x, y, z = link.to_parent_direction(point) self.assertAlmostEqual(x, hs2) self.assertAlmostEqual(y, -hs2) self.assertAlmostEqual(z, 0)
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 Vector3(0, 0, 1.5),
# .. of the box. box) # Now, create a link and add both items to it link = Link("my_link", elements=[box, cylinder]) # Calculate the correct inertial properties given # the collision elements. link.align_center_of_mass() link.calculate_inertial() # Rotate the link 45 degrees around the x-axis, specified in the parent frame # just to demonstrate how that works (and to demonstrate align is still # going to work after the rotation). link.rotate_around(Vector3(1, 0, 0), math.radians(45), relative_to_child=False) # Okay, not sure what this is supposed to be, but let's another wheel-like cylinder in # a new link, and connect them with joints wheel_geom = Cylinder(0.75, 0.1, mass=0.1) wheel = StructureCombination("wheel", wheel_geom) wheel_link = Link("my_wheel", elements=[wheel]) attachment_point = Vector3(0, 0, 0.5 * wheel_geom.length) wheel_link.align(attachment_point, Vector3(0, 0, 1), Vector3(0, 1, 0), Vector3(0, 0, 0.5 * box_geom.size[0] + cyl_geom.length), Vector3(0, 0, 1), Vector3(1, 0, 0), link) # Create a joint link, and set its position (which is in the child frame) joint = Joint("revolute", link, wheel_link, axis=Vector3(0, 0, 1)) joint.set_position(attachment_point)
# .. of the box. box ) # Now, create a link and add both items to it link = Link("my_link", elements=[box, cylinder]) # Calculate the correct inertial properties given # the collision elements. link.align_center_of_mass() link.calculate_inertial() # Rotate the link 45 degrees around the x-axis, specified in the parent frame # just to demonstrate how that works (and to demonstrate align is still # going to work after the rotation). link.rotate_around(Vector3(1, 0, 0), math.radians(45), relative_to_child=False) # Okay, not sure what this is supposed to be, but let's another wheel-like cylinder in # a new link, and connect them with joints wheel_geom = Cylinder(0.75, 0.1, mass=0.1) wheel = StructureCombination("wheel", wheel_geom) wheel_link = Link("my_wheel", elements=[wheel]) attachment_point = Vector3(0, 0, 0.5 * wheel_geom.length) wheel_link.align(attachment_point, Vector3(0, 0, 1), Vector3(0, 1, 0), Vector3(0, 0, 0.5 * box_geom.size[0] + cyl_geom.length), Vector3(0, 0, 1), Vector3(1, 0, 0), link) # Create a joint link, and set its position (which is in the child frame) joint = Joint("revolute", link, wheel_link, axis=Vector3(0, 0, 1)) joint.set_position(attachment_point)
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 Vector3(0, 0, 1.5),