sdf = SDF() model = Model("obstacles",static=True) 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) #adjust it up to ground plane model.set_position(Vector3(0, 0, 0.0)) sdf.add_element(model) gen_boxes(dimensions=4,spacing=0.15, size=0.03) print(str(sdf))
from tol.manage import World from sdfbuilder import SDF, Link, Model from sdfbuilder.sensor import Sensor conf = Config(visualize_sensors=True) sdf = SDF() model = Model("crash") link = Link("my_link") link.make_box(1.0, 1, 1, 1) sensor = Sensor("sense", "contact") link.add_element(sensor) model.add_element(link) sdf.add_element(model) model.set_position(Vector3(0, 0, 0.5)) @trollius.coroutine def run_server(): world = yield From(World.create(conf)) counter = 0 while True: model.name = "test_bot_%d" % counter print("Inserting %s..." % model.name) fut = yield From(world.insert_model(sdf)) yield From(fut) print("Done. Waiting...") yield From(trollius.sleep(1.0)) print("Done. Removing...")
# 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) # Create a model and a wrapping SDF element, and output # Move the model up so it won't intersect with the ground when inserted. model = Model("my_robot", elements=[link, wheel_link, joint]) model.set_position(Vector3(0, 0, math.sqrt(0.5))) sdf = SDF(elements=[model]) print(str(sdf))
# 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) # Create a model and a wrapping SDF element, and output # Move the model up so it won't intersect with the ground when inserted. model = Model("my_robot", elements=[link, wheel_link, joint]) model.set_position(Vector3(0, 0, math.sqrt(0.5))) sdf = SDF(elements=[model]) print(str(sdf))