def robot_to_sdf(robot, name="test_bot", plugin_controller=None):
model = builder.get_sdf_model(robot,
plugin_controller,
update_rate=UPDATE_RATE,
name=name)
sdf = SDF()
sdf.add_element(model)
return sdf
def get_analysis_robot(robot, builder):
"""
Creates an SDF model suitable for analysis from a robot object
and a builder.
:param robot:
:type robot: Robot
:param builder:
:type builder: BodyBuilder
:return:
"""
model = builder.get_sdf_model(robot, analyzer_mode=True, controller_plugin=None, name="analyze_bot")
model.remove_elements_of_type(Sensor, recursive=True)
sdf = SDF()
sdf.add_element(model)
return sdf
def get_simulation_robot(robot, name, builder, conf):
"""
:param robot:
:param name:
:param builder:
:param conf: Config
:return:
"""
model = builder.get_sdf_model(robot, controller_plugin="libtolrobotcontrol.so",
update_rate=conf.controller_update_rate, name=name)
apply_surface_parameters(model)
sdf = SDF()
sdf.add_element(model)
return sdf
def get_simulation_robot(robot, name, builder, conf):
"""
:param robot:
:param name:
:param builder:
:param conf: Config
:return:
"""
model = builder.get_sdf_model(robot, controller_plugin="libtolrobotcontrol.so",
update_rate=conf.controller_update_rate, name=name)
apply_surface_parameters(model)
sdf = SDF()
sdf.add_element(model)
return sdf
def place_birth_clinic(self, diameter, height, angle=None):
"""
CURRENTLY NOT USED.
Places the birth clinic. Since we're lazy and rotating appears to cause errors,
we're just deleting the old birth clinic and inserting a new one every time.
Inserts the birth clinic
:param height:
:param diameter:
:param angle:
:return:
"""
if self.birth_clinic_model:
# Delete active birth clinic and place new
yield From(
wait_for(self.delete_model(self.birth_clinic_model.name)))
self.birth_clinic_model = None
if angle is None:
angle = random.random() * 2 * math.pi
name = "birth_clinic_" + str(self.get_robot_id())
self.birth_clinic_model = bc = BirthClinic(name=name,
diameter=diameter,
height=height)
bc.rotate_around(Vector3(0, 0, 1), angle)
future = yield From(self.insert_model(SDF(elements=[bc])))
raise Return(future)
def get_analysis_robot(robot, builder):
"""
Creates an SDF model suitable for analysis from a robot object
and a builder.
:param robot:
:type robot: Robot
:param builder:
:type builder: BodyBuilder
:return:
"""
model = builder.get_sdf_model(robot,
analyzer_mode=True,
controller_plugin=None,
name="analyze_bot")
model.remove_elements_of_type(Sensor, recursive=True)
sdf = SDF()
sdf.add_element(model)
return sdf
def add_highlight(self, position, color):
"""
Adds a circular highlight at the given position.
:param position:
:param color:
:return:
"""
hl = Highlight("highlight_" + str(self.get_robot_id()), color)
position = position.copy()
position.z = 0
hl.set_position(position)
sdf = SDF(elements=[hl])
fut = yield From(self.insert_model(sdf))
raise Return(fut, hl)
def get_simulation_robot(robot, name, builder, conf, battery_charge=None):
"""
:param robot:
:param name:
:param builder:
:param conf: Config
:param battery_charge:
:return:
"""
battery = None if battery_charge is None else BasicBattery(battery_charge)
brain_conf = None if not hasattr(conf, 'brain_conf') else conf.brain_conf
model = builder.get_sdf_model(robot=robot,
controller_plugin="libtolrobotcontrol.so",
update_rate=conf.controller_update_rate,
name=name,
battery=battery,
brain_conf=brain_conf)
apply_surface_parameters(model, conf.world_step_size)
sdf = SDF()
sdf.add_element(model)
return sdf
def build_walls(self, points):
"""
Builds a wall defined by the given points, used to shield the
arena.
:param points:
:return: Future that resolves when all walls have been inserted.
"""
futures = []
l = len(points)
for i in range(l):
start = points[i]
end = points[(i + 1) % l]
wall = Wall("wall_%d" % i, start, end, constants.WALL_THICKNESS, constants.WALL_HEIGHT)
future = yield From(self.insert_model(SDF(elements=[wall])))
futures.append(future)
raise Return(multi_future(futures))
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
#generate a grid of boxes and write to file
sdf = SDF()
model_name = "boxes_grid"
model = gen_boxes(model_name,
dimensions=6,
spacing=0.3,
size=0.04,
height=0.02)
sdf.add_element(model)
write_model(model_name, sdf)
#generate steps
sdf = SDF()
model_name = "exp2_steps"
model = gen_steps(model_name, incline=4, offset=0.8)
#model.set_position(Vector3(0,0,-0.041)) #inaccurate height compensation due to the incline - does not seem to affect the model?
sdf.add_element(model)
id: Wheel
type: Wheel
params:
red: 0.0
green: 1.0
blue: 0.0
5:
id: Wheel2
type: Wheel
params:
red: 0.0
green: 1.0
blue: 0.0
brain:
params:
Wheel-out-0:
type: Oscillator
period: 3
Wheel2-out-0:
type: Oscillator
period: 3
'''
bot = yaml_to_robot(body_spec, brain_spec, bot_yaml)
builder = RobotBuilder(BodyBuilder(body_spec), NeuralNetBuilder(brain_spec))
model = builder.get_sdf_model(bot, "libtolrobotcontrol.so")
model.translate(Vector3(0, 0, 0.5))
sdf = SDF()
sdf.add_element(model)
print(str(sdf))
from sdfbuilder.structure import Collision, Visual, Box, StructureCombination
from sdfbuilder.math import Vector3
import math
from tol.build.builder import apply_surface_parameters
MASS = in_grams(3)
SENSOR_BASE_WIDTH = in_mm(34)
SENSOR_BASE_THICKNESS = in_mm(1.5)
SENSOR_THICKNESS = in_mm(9)
SENSOR_WIDTH = in_mm(18.5)
SENSOR_HEIGHT = in_mm(16)
SEPARATION = in_mm(1)
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")
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)
if __name__ == '__main__':
sdf = SDF(elements=[BirthClinic()])
print(str(sdf))
def robot_to_sdf(robot, name="test_bot", plugin_controller=None):
model = builder.get_sdf_model(robot, plugin_controller, update_rate=UPDATE_RATE, name=name)
sdf = SDF()
sdf.add_element(model)
return 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))
