def get_slot_position(self, slot_id):
self.check_slot(slot_id)
if slot_id == 0:
return Vector3(-0.5 * SLOT_THICKNESS, 0, 0)
else:
return self.connection.to_sibling_frame(
Vector3(0.5 * JOINT_CONNECTION_THICKNESS, 0, 0), self)
def get_slot_normal(self, slot_id):
self.check_slot(slot_id)
if slot_id == 0:
return Vector3(-1, 0, 0)
else:
return self.tail.to_sibling_direction(Vector3(1, 0, 0), self)
def get_slot_position(self, slot_id):
self.check_slot(slot_id)
if slot_id == 0:
return Vector3(-0.5 * SLOT_THICKNESS, 0, 0)
else:
return self.tail.to_sibling_frame(
Vector3(0.5 * SLOT_THICKNESS, 0, 0), self)
def get_slot_position(self, slot_id):
self.check_slot(slot_id)
offset = 0.5 * self.SLOT_THICKNESS
if slot_id == 0:
# Root has no relative pose so this is easy
return Vector3(-offset, 0, 0)
else:
# This is a posable group, so we must use
# sibling conversion.
return self.tail.to_sibling_frame(Vector3(offset, 0, 0), self)
def get_slot_position(self, slot_id):
self.check_slot(slot_id)
offset = 0.5 * SLOT_THICKNESS
if slot_id == 0:
# The root slot is positioned half the slot
# thickness to the left
return Vector3(-offset, 0, 0)
else:
# A `BodyPart` is a posable group, so item positions are
# in the parent frame. If we convert to the local frame we can
# simply add the offset in the x-direction
tail_pos = self.to_local_frame(self.hinge_tail.get_position())
return tail_pos + Vector3(offset, 0, 0)
def get_slot_position(self, slot):
"""
Returns the attachment position of each of the slots.
"""
# Again, prevent errors
self.check_slot(slot)
# The constructor of `BodyPart` stores the initialization's kwargs
# parameters in `self.part_params`.
length = self.part_params["length"]
# The center of the base box lies at (0, 0), move 1/2 x to the
# left to get that slot, or move 1/2 x to the right, plus the
# variable length minus the offset to get the other.
return Vector3(-0.5 * self.x, 0, 0) if slot == 0 \
else Vector3(0.5 * self.x + length - self.JOINT_OFFSET, 0, 0)
def _build_analyzer_body(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 _initialize(self, **kwargs):
"""
:param kwargs:
:return:
"""
self.mass = MASS
self.x = SENSOR_BASE_THICKNESS
self.y = self.z = SENSOR_BASE_WIDTH
super(LightSensor, self)._initialize(**kwargs)
# Add the SDF camera sensor
camera = SdfSensor(
name="{}_light_sensor".format(self.id),
sensor_type="camera",
update_rate=self.conf.sensor_update_rate)
# TODO: Set field of view
cam_details = "<camera>" \
"<image>" \
"<width>1</width><height>1</height>" \
"</image>" \
"<clip><near>{}</near><far>{}</far></clip>" \
"</camera>".format(nf(in_mm(1)), nf(in_mm(50000)))
camera.add_element(cam_details)
camera.set_position(Vector3(0.5 * self.x, 0, 0))
self.component.add_sensor(camera, "light")
if self.conf.visualize_sensors:
camera.add_element("<visualize>1</visualize>")
self.apply_color()
async def analysis_func():
analyzer = await (BodyAnalyzer.create(address=("127.0.0.1", 11346)))
# Try a maximum of 100 times
for _ in range(100):
# Generate a new robot
robot = generate_robot()
sdf = get_analysis_robot(robot, builder)
# Find out its intersections and bounding box
intersections, bbox = await (analyzer.analyze_robot(robot,
builder=builder))
if intersections:
print("Invalid model - intersections detected.", file=sys.stderr)
else:
print("No model intersections detected!", file=sys.stderr)
if bbox:
# Translate the model in z direction so it stands directly on
# the ground
print("Model bounding box: ({}, {}, {}), ({}, {}, {})".format(
bbox.min.x, bbox.min.y, bbox.min.z, bbox.max.x, bbox.max.y,
bbox.max.z),
file=sys.stderr)
model = sdf.elements[0]
model.translate(Vector3(0, 0, -bbox.min.z))
print(str(robot_to_sdf(robot, "test_bot", "controllerplugin.so")))
break
def get_slot_position(self, slot_id):
self.check_slot(slot_id)
offset = 0.5 * SLOT_THICKNESS
if slot_id == 0:
# The root slot is positioned half the slot
# thickness to the left
return self.hinge_root.to_sibling_frame(Vector3(-offset, 0, 0),
self)
else:
# A `BodyPart` is a PosableGroup, so child positions are
# similar to sibling positions. We can thus take the position
# in the tail, and use sibling conversion to get the position
# in the body part.
return self.hinge_tail.to_sibling_frame(Vector3(offset, 0, 0),
self)
def get_slot_tangent(self, slot_id):
"""
We use the same tangent vector for every slot, pointing
horizontally.
:param slot_id:
:return:
"""
return Vector3(1, 0, 0)
def _initialize(self, **kwargs):
"""
Initialize method to generate the hinge. Inheriting from
"box" makes sure there is a box of the given size in
`self.link`.
"""
super(PassiveHinge, self)._initialize(**kwargs)
# Create the variable size block. `create_link` is the recommended
# way of doing this, because it sets some non-default link properties
# (such as self_collide) which you generally need.
length = kwargs["length"]
self.var_block = self.create_component(
BoxGeom(length, self.y, self.z, 0.1), "var-block")
# We move the block in the x-direction so that it
# just about overlaps with the other block (to
# make it seem like a somewhat realistic joint)
self.var_block.translate(
Vector3(0.5 * (length + self.x) - self.JOINT_OFFSET, 0, 0))
# Now create a revolute joint at this same position. The
# joint axis is in the y-direction.
axis = Vector3(0, 1, 0)
passive_joint = Joint(
joint_type="revolute",
parent=self.component,
child=self.var_block,
axis=axis)
# Set some movement limits on the joint
passive_joint.axis.limit = Limit(
lower=math.radians(-45),
upper=math.radians(45),
effort=1.0)
# Set the joint position - in the child frame!
passive_joint.set_position(
Vector3(-0.5 * length + self.JOINT_OFFSET, 0, 0))
# Don't forget to add the joint and the link
self.add_joint(passive_joint)
# Apply the generated color
self.apply_color()
def _initialize(self, **kwargs):
"""
:param kwargs:
:return:
"""
# Call super to initialize the cylinder part of the wheel
super(Wheel, self)._initialize(**kwargs)
# Create the small box that serves as the motor
box_size = self.MOTOR_SIZE
self.attachment = self.create_component(
BoxGeom(box_size, box_size, box_size, 0.01), "cylinder-attach")
# Get attachment position and axis of the motor joint
anchor = Vector3(0, 0, 0.5 * self.LENGTH)
axis = Vector3(0, 0, 1)
# Size and position the box
self.attachment.set_position(anchor + Vector3(0, 0, 0.5 * box_size))
# Create revolute joint. Remember: joint position is in child frame
motor_joint = Joint(
joint_type="revolute",
parent=self.component,
child=self.attachment,
axis=axis)
motor_joint.set_position(Vector3(0, 0, -0.5 * box_size))
# Set a force limit on the joint
motor_joint.axis.limit = Limit(effort=0.01)
self.add_joint(motor_joint)
# Register a joint motor with a maximum velocity of
# 50 rounds per minute (the speed is in radians per second)
# We also give it a simple PID controller
pid = PID(proportional_gain=1.0, integral_gain=0.1)
max_speed = 2 * math.pi * 50.0 / 60
self.motors.append(VelocityMotor(
part_id=self.id,
motor_id="rotate",
joint=motor_joint,
pid=pid,
min_velocity=-max_speed,
max_velocity=max_speed))
self.apply_color()
def get_slot_position(self, slot):
"""
Return slot position
"""
self.check_slot(slot)
vmax = WIDTH / 2.0
if slot == 0:
# Front face
return Vector3(0, -vmax, 0)
elif slot == 1:
# Back face
return Vector3(0, vmax, 0)
elif slot == 2:
# Right face
return Vector3(vmax, 0, 0)
# Left face
return Vector3(-vmax, 0, 0)
def get_slot_position(self, slot_id):
"""
Returns the slot position for the given slot. Slot 0 is
defined as the top, slot 1 is the bottom.
:param slot_id:
:return:
"""
z = self.length / 2.0
return Vector3(0, 0, z if slot_id == 0 else -z)
def get_slot_position(self, slot_id):
"""
Modify `get_slot_position` to return the attachment of the
motor instead.
:param slot_id:
:return:
"""
v = super(Wheel, self).get_slot_position(slot_id)
return v + Vector3(0, 0, self.MOTOR_SIZE)
def _initialize(self, **kwargs):
# Initialize the root
self.root = self.create_component(
Box(SLOT_THICKNESS, SLOT_WIDTH, SLOT_WIDTH, MASS_SLOT), "root")
# Initialize the servo
servo = self.create_component(
Box(SERVO_LENGTH, SERVO_WIDTH, SERVO_HEIGHT, MASS_SERVO))
x_servo = 0.5 * (SLOT_THICKNESS + SERVO_LENGTH) + SEPARATION
# z_servo = 0.5 * (SERVO_HEIGHT - SLOT_WIDTH) + SERVO_Z_OFFSET
z_servo = 0
servo.set_position(Vector3(x_servo, 0, z_servo))
# Initialize the connection
self.connection = self.create_component(
Box(JOINT_CONNECTION_THICKNESS, JOINT_CONNECTION_WIDTH,
JOINT_CONNECTION_WIDTH, MASS_CONNECTION_SLOT), "connection")
x_conn = x_servo + 0.5 * (SERVO_LENGTH -
JOINT_CONNECTION_THICKNESS) + SEPARATION
self.connection.set_position(Vector3(x_conn, 0, 0))
# Fix the links
# root <-> servo
self.fix(self.root, servo)
# servo <revolute> connection
self.joint = Joint("revolute",
servo,
self.connection,
axis=Vector3(1, 0, 0))
self.joint.axis.limit = Limit(
effort=constants.MAX_SERVO_TORQUE_ROTATIONAL,
velocity=constants.MAX_SERVO_VELOCITY)
self.add_joint(self.joint)
# Now we add a motor that powers the joint. This particular servo
# targets a velocity. Use a simple PID controller initially.
pid = constants.SERVO_POSITION_PID
self.motors.append(PositionMotor(self.id, "rotate", self.joint, pid))
# Call color mixin
self.apply_color()
def get_slot_tangent(self, slot):
"""
Return slot tangent
"""
self.check_slot(slot)
# The top face is tangent to all supported
# slots in the plane, front, back, right, left
# Since there's no particular reason for choosing any
# other we just return the top face for all.
return Vector3(0, 0, 1)
def get_slot_tangent(self, slot):
"""
The tangent vectors determine the "zero orientation", meaning
if a body part has an orientation of 0 it will have its tangent
vector aligned with its parent. The tangent vector has to be
orthogonal to the slot normal.
We have no specific orientation requirements, so we simply
always return one tangent vector which is orthogonal to both
slot normals, i.e. the vector (0, 0, 1).
"""
self.check_slot(slot)
return Vector3(0, 0, 1)
def _initialize(self, **kwargs):
self.radius = in_mm(kwargs['radius'])
wheel = self.create_component(
Cylinder(self.radius, WHEEL_THICKNESS, MASS_WHEEL), "wheel")
self.root = self.create_component(
Box(SLOT_WIDTH, SLOT_WIDTH, SLOT_THICKNESS, MASS_SLOT))
# Create the wheel
z_wheel = 0.5 * SLOT_THICKNESS - (
SLOT_THICKNESS + SLOT_CONNECTION_THICKNESS - SLOT_WHEEL_OFFSET)
wheel.set_position(Vector3(0, 0, z_wheel))
# Attach the wheel and the root with a revolute joint
joint = Joint("revolute", self.root, wheel, axis=Vector3(0, 0, -1))
# TODO Adequate force limit for passive wheel
joint.axis.limit = Limit(effort=constants.MAX_SERVO_TORQUE_ROTATIONAL)
self.add_joint(joint)
# Call color mixin
self.apply_color()
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(arg1=min(height, scaled_height),
arg2=(SLICE_FRACTION * scaled_height))
mesh = Mesh("model://tol_arena/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 _initialize(self, **kwargs):
"""
:param kwargs:
:return:
"""
# Initialize the root, already at 0, 0, 0
self.root = self.create_component(
Box(SLOT_THICKNESS, SLOT_WIDTH, SLOT_WIDTH, MASS_SLOT), "root")
# Initialize the first connection part
x_part_a = 0.5 * (SLOT_THICKNESS + CONNECTION_PART_LENGTH) + SEPARATION
conn_a = self.create_component(
Box(CONNECTION_PART_LENGTH, SLOT_WIDTH, CONNECTION_PART_HEIGHT,
MASS_FRAME), "conn_a")
conn_a.set_position(Vector3(x_part_a, 0, 0))
# Initialize the second connection part
x_part_b = x_part_a - CONNECTION_PART_LENGTH + 2 * CONNECTION_PART_OFFSET
conn_b = self.create_component(
Box(CONNECTION_PART_LENGTH, CONNECTION_PART_HEIGHT, SLOT_WIDTH,
MASS_FRAME), "conn_b")
conn_b.set_position(Vector3(x_part_b, 0, 0))
# Initialize the tail
x_tail = x_part_b + 0.5 * (CONNECTION_PART_LENGTH +
SLOT_THICKNESS) + SEPARATION
self.tail = self.create_component(
Box(SLOT_THICKNESS, SLOT_WIDTH, SLOT_WIDTH, MASS_SLOT), "tail")
self.tail.set_position(Vector3(x_tail, 0, 0))
# Fix the parts using joints
# root <-> connection a
self.fix(self.root, conn_a)
# connection a <-> connection b
cardan = Joint("universal",
conn_a,
conn_b,
axis=Vector3(0, 0, 1),
axis2=Vector3(0, 1, 0))
limit = Limit(lower=-constants.CARDAN_LIMIT,
upper=constants.CARDAN_LIMIT)
cardan.axis.limit = cardan.axis2.limit = limit
cardan.set_position(
Vector3(0.5 * CONNECTION_PART_LENGTH - CONNECTION_PART_OFFSET))
self.add_joint(cardan)
# connection b <-> tail
self.fix(conn_b, self.tail)
# Apply color mixin
self.apply_color()
def _sensor_helper(self, label, x_sensors, y_sensor):
"""
:return:
"""
# Visual is provided by the mesh in the root
sensor_component = self.create_component(BoxGeom(
SENSOR_THICKNESS, SENSOR_WIDTH, SENSOR_HEIGHT, MASS),
label,
visual=False)
sensor_component.set_position(Vector3(x_sensors, y_sensor, 0))
# Anchor and axis are in child frame
self.fix(self.component, sensor_component)
# Create and add the SDF sensor
contact = SdfSensor(label + "_sensor",
"contact",
update_rate=self.conf.sensor_update_rate)
contact.add_element('<contact><collision>' +
sensor_component.collision.name +
'</collision></contact>')
sensor_component.add_sensor(contact, "touch")
async def run():
world = await World.create()
if world:
print("Connected to the simulator world.")
model = Model(
name='sdf_model',
static=True,
)
model.set_position(position=Vector3(0, 0, 1))
link = Link('sdf_link')
link.make_sphere(
mass=10e10,
radius=0.5,
)
link.make_color(0.7, 0.2, 0.0, 1.0)
model.add_element(link)
sdf_model = SDF(elements=[model])
await world.insert_model(sdf_model)
await world.pause(True)
while True:
await asyncio.sleep(10.0)
def _initialize(self, **kwargs):
"""
:param kwargs:
:return:
"""
self.mass = MASS
self.x = SENSOR_BASE_THICKNESS
self.y = self.z = SENSOR_BASE_WIDTH
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
visual = Visual("touch_visual",
Mesh("model://rg_robot/meshes/TouchSensor.dae"))
visual.translate(Vector3(0.5 * SENSOR_THICKNESS, 0, 0))
self.component = self.create_component(BoxGeom(self.x, self.y, self.z,
self.mass),
"box",
visual=visual)
self._sensor_helper("left", x_sensors, y_left_sensor)
self._sensor_helper("right", x_sensors, y_right_sensor)
self.apply_color()
def get_slot_tangent(self, slot):
"""
Return slot tangent
"""
self.check_slot(slot)
if slot == 0:
# Front face tangent: top face
return Vector3(0, 0, 1)
elif slot == 1:
# Back face tangent: top face
return Vector3(0, 0, 1)
elif slot == 2:
# Top face tangent: right face
return Vector3(1, 0, 0)
elif slot == 3:
# Bottom face tangent: right face
return Vector3(1, 0, 0)
elif slot == 4:
# Right face tangent: back face
return Vector3(0, 1, 0)
# Left face tangent: back face
return Vector3(0, 1, 0)
def get_slot_position(self, slot):
"""
Return slot position
"""
self.check_slot(slot)
xmax, ymax, zmax = self.x / 2.0, self.y / 2.0, self.z / 2.0
if slot == 0:
# Front face
return Vector3(0, -ymax, 0)
elif slot == 1:
# Back face
return Vector3(0, ymax, 0)
elif slot == 2:
# Top face
return Vector3(0, 0, zmax)
elif slot == 3:
# Bottom face
return Vector3(0, 0, -zmax)
elif slot == 4:
# Right face
return Vector3(xmax, 0, 0)
# Left face
return Vector3(-xmax, 0, 0)
def _initialize(self, **kwargs):
"""
:param kwargs:
:return:
"""
# Create components. Visuals are provided by the conn_a/conn_b meshes
self.hinge_root = self.create_component(Box(SLOT_THICKNESS, SLOT_WIDTH,
SLOT_WIDTH, MASS_SLOT),
"slot_a",
visual=False)
self.hinge_tail = self.create_component(Box(SLOT_THICKNESS, SLOT_WIDTH,
SLOT_WIDTH, MASS_SLOT),
"slot_b",
visual=False)
mesh = Mesh("model://rg_robot/meshes/PassiveHinge.dae")
visual_a = Visual("conn_a_visual", mesh)
visual_a.translate(Vector3(-0.5 * SLOT_THICKNESS))
conn_a = self.create_component(Box(CONNECTION_PART_LENGTH,
CONNECTION_PART_THICKNESS,
CONNECTION_PART_HEIGHT, MASS_FRAME),
"conn_a",
visual=visual_a)
# Flip visual along the x-axis by rotating PI degrees over z
# This will put it upside down, so we also flip it PI degrees over x
visual_b = Visual("conn_b_visual", mesh.copy())
visual_b.rotate_around(Vector3(1, 0, 0),
math.pi,
relative_to_child=False)
visual_b.rotate_around(Vector3(0, 0, 1),
math.pi,
relative_to_child=False)
visual_b.translate(Vector3(0.5 * SLOT_THICKNESS))
conn_b = self.create_component(Box(CONNECTION_PART_LENGTH,
CONNECTION_PART_THICKNESS,
CONNECTION_PART_HEIGHT, MASS_FRAME),
"conn_b",
visual=visual_b)
# Shorthand for variables
# Position connection part a
x_part_a = SLOT_THICKNESS / 2.0 + SEPARATION + CONNECTION_PART_LENGTH / 2.0
conn_a.set_position(Vector3(x_part_a, 0, 0))
# Position connection part b
x_part_b = x_part_a + (
CONNECTION_PART_LENGTH / 2.0 -
(CONNECTION_PART_LENGTH - CONNECTION_ROTATION_OFFSET)) * 2
conn_b.set_position(Vector3(x_part_b, 0, 0))
# Make the tail
x_tail = x_part_b + CONNECTION_PART_LENGTH / 2.0 + SEPARATION + SLOT_THICKNESS / 2.0
self.hinge_tail.set_position(Vector3(x_tail, 0, 0))
# Fix components
self.fix(self.hinge_root, conn_a)
self.fix(self.hinge_tail, conn_b)
# Connection part a <(hinge)> connection part b
# Hinge joint axis should point straight up, and anchor
# the points in the center. Note that the position of a joint
# is expressed in the child link frame, so we need to take the
# position from the original code and subtract conn_b's position
self.joint = Joint("revolute", conn_a, conn_b, axis=Vector3(0, 0, 1))
self.joint.set_position(
Vector3(CONNECTION_PART_LENGTH / 2.0 - CONNECTION_ROTATION_OFFSET,
0, 0))
self.joint.axis.limit = Limit(upper=constants.HINGE_LIMIT,
lower=-constants.HINGE_LIMIT)
self.add_joint(self.joint)
# Apply color mixin
self.apply_color()
def get_slot_tangent(self, slot_id):
self.check_slot(slot_id)
return Vector3(0, 0, 1)
def get_slot_tangent(self, slot):
self.check_slot(slot)
return Vector3(0, 1, 0)
