def __init__(self, name, color, **kwargs): super(Highlight, self).__init__(name, static=True, **kwargs) self.highlight = Link("hl_link") self.highlight.make_cylinder(10e10, 0.4, 0.001, collision=False) r, g, b, a = color self.highlight.make_color(r, g, b, a) self.add_element(self.highlight)
class Wall(Model): """ Simple wall model to wall off the """ def __init__(self, name, start, end, thickness, height, **kwargs): """ Construct a wall of the given thickness and height from `start` to `end`. This simply results in a box. :param start: Starting point of the wall. :type start: Vector3 :param end: Ending point of the wall. :type end: Vector3 :return: """ super(Wall, self).__init__(name, static=True, **kwargs) assert start.z == end.z, "Walls with different start / end z are undefined." center = 0.5 * (end + start) diff = end - start size = abs(diff) self.wall = Link("wall_link") self.wall.make_box(10e10, size, thickness, height) # Rotate the wall so it aligns with the vector from # x to y self.align( Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 0, 1), center, diff, Vector3(0, 0, 1), Posable("mock") ) self.add_element(self.wall)
def __init__(self, name, start, end, thickness, height, **kwargs): """ Construct a wall of the given thickness and height from `start` to `end`. This simply results in a box. :param start: Starting point of the wall. :type start: Vector3 :param end: Ending point of the wall. :type end: Vector3 :return: """ super(Wall, self).__init__(name, static=True, **kwargs) assert start.z == end.z, "Walls with different start / end z are undefined." center = 0.5 * (end + start) diff = end - start size = abs(diff) self.wall = Link("wall_link") self.wall.make_box(10e10, size, thickness, height) # Rotate the wall so it aligns with the vector from # x to y self.align( Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 0, 1), center, diff, Vector3(0, 0, 1), Posable("mock") ) self.add_element(self.wall)
class Highlight(Model): """ Model to highlight newly inserted robots / selected parents """ def __init__(self, name, color, **kwargs): super(Highlight, self).__init__(name, static=True, **kwargs) self.highlight = Link("hl_link") self.highlight.make_cylinder(10e10, 0.4, 0.001, collision=False) r, g, b, a = color self.highlight.make_color(r, g, b, a) self.add_element(self.highlight)
class Highlight(Model): """ Model to highlight newly inserted robots / selected parents """ def __init__(self, name, color, **kwargs): super(Highlight, self).__init__(name, static=True, **kwargs) self.highlight = Link("hl_link") self.highlight.make_cylinder(10e10, 0.4, 0.001, collision=False) r, g, b, a = color self.highlight.make_color(r, g, b, a) self.add_element(self.highlight)
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_analyzer_body(self, model, body_parts, connections): """ Builds a model from body parts in analyzer mode - i.e. one link per body part to allow collision detection. :param model: :param body_parts: :type body_parts: list[BodyPart] :return: """ part_map = {} for body_part in body_parts: link = Link("link_"+body_part.id, self_collide=True) link.set_position(body_part.get_position()) link.set_rotation(body_part.get_rotation()) body_part.set_position(Vector3()) body_part.set_rotation(Quaternion()) link.add_element(body_part) model.add_element(link) part_map[body_part.id] = link # Create fake joints for all connections so they will # not trigger collisions. for a, b in connections: joint = FixedJoint(part_map[a], part_map[b]) model.add_element(joint)
def __init__(self, name, color, **kwargs): super(Highlight, self).__init__(name, static=True, **kwargs) self.highlight = Link("hl_link") self.highlight.make_cylinder(10e10, 0.4, 0.001, collision=False) r, g, b, a = color self.highlight.make_color(r, g, b, a) self.add_element(self.highlight)
def _build_analyzer_body(self, model, body_parts, connections): """ Builds a model from body parts in analyzer mode - i.e. one link per body part to allow collision detection. :param model: :param body_parts: :type body_parts: list[BodyPart] :return: """ part_map = {} for body_part in body_parts: link = Link("link_"+body_part.id, self_collide=True) link.set_position(body_part.get_position()) link.set_rotation(body_part.get_rotation()) body_part.set_position(Vector3()) body_part.set_rotation(Quaternion()) link.add_element(body_part) model.add_element(link) part_map[body_part.id] = link # Create fake joints for all connections so they will # not trigger collisions. for a, b in connections: joint = FixedJoint(part_map[a], part_map[b]) model.add_element(joint)
def test_link_inertia(self): """ A similar test to what is done in structure.py to test compound inertia, only now we do it in a link. :return: """ # First, the comparison inertial total_mass = 100 simple_box = Box(4, 8, 12, mass=total_mass) i1 = simple_box.get_inertial() # Same split as in structure.py test, only now # we're making sure it is distributed around the # origin since box inertial above is relative to # its center of mass link = Link("test_link") y_axis = Vector3(0, 1, 0) deg90 = 0.5 * math.pi # Split 5 / 7 in z-direction frac = total_mass / 12.0 total_up = 5 * frac total_down = 7 * frac # We split the upper part 6 / 2 in the y-direction # and align in the center. frac_up = total_up / 8.0 sub1 = Collision("sub1", geometry=Box(5, 6, 4, mass=6 * frac_up)) sub1.rotate_around(y_axis, deg90) sub1.translate(Vector3(0, 1, 3.5)) link.add_element(sub1) sub2 = Collision("sub2", geometry=Box(5, 2, 4, mass=2 * frac_up)) sub2.rotate_around(y_axis, deg90) sub2.translate(Vector3(0, -3, 3.5)) link.add_element(sub2) sub3 = Collision("sub3", geometry=Box(4, 8, 7, mass=total_down)) sub3.translate(Vector3(0, 0, -2.5)) link.add_element(sub3) link.calculate_inertial() self.assertEqualTensors(i1, link.inertial)
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 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 gen_boxes(model_name, dimensions=4, spacing=0.5, size=0.05, height=0.015, center_sq=1): model = Model(model_name, static=True) for x in range(-dimensions, dimensions + 1): for y in range(-dimensions, dimensions + 1): l = Link("box") if (abs(x) >= center_sq or abs(y) >= center_sq): l.make_box(1, size, size, height) pos = Vector3(spacing * x, spacing * y, height / 2) l.set_position(pos) #l.rotate_around(Vector3(0, 0, 1), math.radians(x*y), relative_to_child=False) model.add_element(l) return model
def gen_steps(model_name, num_steps=6, offset=0.4, height=0.02, width=3.0, depth=0.2, incline=0): model = Model(model_name, static=True) steps = PosableGroup() for x in range(0, num_steps): l = Link("box") l.make_box(1, depth, width, height) pos = Vector3(offset + depth * x, 0, height / 2 + x * height) l.set_position(pos) steps.add_element(l) for x in range(0, 4): steps.set_rotation( Quaternion.from_rpy(0, math.radians(-incline), math.radians(90 * x))) model.add_element(steps.copy()) return model
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)
from sdfbuilder import Link, PosableGroup, SDF, Model from sdfbuilder.math import Vector3 from math import pi link = Link("my_link") minibox = Link("my_minibox") # Link one is a vertical box link.make_box(1.0, 2, 2, 4) # Minibox is... well, a mini box minibox.make_box(0.1, 0.2, 0.2, 0.2) minibox.align( # Bottom left corner of minibox Vector3(-0.1, -0.1, -0.1), # Normal vector Vector3(-0.1, -0.1, -0.1), # Tangent vector Vector3(-0.1, -0.1, 0.2), # Top left of link Vector3(-1, -1, 2), # Normal vector Vector3(0, 0, 1), # Tangent vector
# .. align with the top .. Vector3(0, 0, 0.5 * box_geom.size[2]), # .. and normal vector (other direction) .. Vector3(0, 0, 1), # .. rotation of the tangent vector .. Vector3(0, 1, 0), # .. 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)
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
Vector3(0, 1, 0), # .. align with the top .. Vector3(0, 0, 0.5 * box_geom.size[2]), # .. and normal vector (other direction) .. Vector3(0, 0, 1), # .. rotation of the tangent vector .. Vector3(0, 1, 0), # .. 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)
import sys sys.path.append(os.path.dirname(os.path.abspath(__file__))+'/../') import trollius from trollius import From from tol.config import Config 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
m = Model(name="mybot") sdf = SDF(elements=[m]) geom = Box(SENSOR_BASE_THICKNESS, SENSOR_BASE_WIDTH, SENSOR_BASE_WIDTH, MASS) base = StructureCombination("base", geom) x_sensors = 0.5 * (SENSOR_BASE_THICKNESS + SENSOR_THICKNESS) y_left_sensor = -0.5 * SENSOR_WIDTH - SEPARATION y_right_sensor = 0.5 * SENSOR_WIDTH + SEPARATION left = StructureCombination("left", Box(SENSOR_THICKNESS, SENSOR_WIDTH, SENSOR_HEIGHT, MASS)) left.set_position(Vector3(x_sensors, y_left_sensor, 0)) right = StructureCombination("right", Box(SENSOR_THICKNESS, SENSOR_WIDTH, SENSOR_HEIGHT, MASS)) right.set_position(Vector3(x_sensors, y_right_sensor, 0)) link = Link("my_link") link.add_elements([base, left, right]) link.align_center_of_mass() link.calculate_inertial() m.add_element(link) apply_surface_parameters(m) m.translate(Vector3(0, 0, in_mm(30))) with open("/home/elte/.gazebo/models/test_bot/model.sdf", "w") as f: f.write(str(sdf))
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
from sdfbuilder import Link, PosableGroup, SDF, Model from sdfbuilder.math import Vector3 from math import pi link = Link("my_link") minibox = Link("my_minibox") # Link one is a vertical box link.make_box(1.0, 2, 2, 4) # Minibox is... well, a mini box minibox.make_box(0.1, 0.2, 0.2, 0.2) minibox.align( # Bottom left corner of minibox Vector3(-0.1, -0.1, -0.1), # Normal vector Vector3(-0.1, -0.1, -0.1), # Tangent vector Vector3(-0.1, -0.1, 0.2), # Top left of link Vector3(-1, -1, 2), # Normal vector Vector3(0, 0, 1), # Tangent vector
from __future__ import print_function import sys from sdfbuilder import Link, Model, SDF from sdfbuilder.math import Vector3 # Create two similar boxes link1 = Link("box1") link1.make_box(1.0, 0.1, 0.3, 0.5) link2 = Link("box2") link2.make_box(1.0, 0.1, 0.3, 0.5) # Align the top of box2 with the front of box1 link2.align(Vector3(0, 0, 0.25), Vector3(0, 0, -1), Vector3(1, 0, 0), Vector3(0, -0.15, 0), Vector3(0, 1, 0), Vector3(0, 0, 1), link1) if __name__ == '__main__': sdf = SDF() model = Model("my_model") model.add_element(link1) model.add_element(link2) sdf.add_element(model) print(str(sdf))
from __future__ import print_function import sys from sdfbuilder import Link, Model, SDF from sdfbuilder.math import Vector3 # Create two similar boxes link1 = Link("box1") link1.make_box(1.0, 0.1, 0.3, 0.5) link2 = Link("box2") link2.make_box(1.0, 0.1, 0.3, 0.5) # Align the top of box2 with the front of box1 link2.align( Vector3(0, 0, 0.25), Vector3(0, 0, -1), Vector3(1, 0, 0), Vector3(0, -0.15, 0), Vector3(0, 1, 0), Vector3(0, 0, 1), link1 ) if __name__ == '__main__': sdf = SDF() model = Model("my_model") model.add_element(link1) model.add_element(link2) sdf.add_element(model)