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 _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 _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 _initialize(self, **kwargs):
self.radius = in_mm(kwargs['radius'])
# Create the root
self.root = self.create_component(
Box(SLOT_WIDTH, SLOT_WIDTH, SLOT_THICKNESS, MASS_SLOT), "root")
# Create the servo
servo = self.create_component(Box(SERVO_HEIGHT, SERVO_WIDTH, SERVO_LENGTH, MASS_SERVO), "servo")
servo.set_position(Vector3(0, 0, X_SERVO))
# Create the wheel
wheel = self.create_component(Cylinder(self.radius, WHEEL_THICKNESS, MASS_WHEEL), "wheel")
wheel.set_position(Vector3(0, 0, X_WHEEL))
# Fix the root to the servo
self.fix(self.root, servo)
# Attach the wheel and the root with a revolute joint
self.joint = Joint("revolute", servo, wheel, axis=Vector3(0, 0, -1))
self.joint.set_position(Vector3(0, 0, -WHEEL_THICKNESS))
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_VELOCITY_PID
self.motors.append(VelocityMotor(
self.id, "rotate", self.joint, pid=pid,
max_velocity=constants.MAX_SERVO_VELOCITY,
min_velocity=-constants.MAX_SERVO_VELOCITY
))
# Call color mixin
self.apply_color()
def _initialize(self, **kwargs):
"""
:param kwargs:
:return:
"""
self.link = self.create_component(
Box(WIDTH, WIDTH, HEIGHT, MASS),
"box",
visual=Mesh("model://tol_robot/meshes/CoreComponent.dae"))
# Now we will add the IMU sensor. First, we must
# create a sensor in SDF. Be careful to give the
# sensor a name which is unique for the entire
# robot, adding the ID will help us do that.
imu = SdfSensor("imu_sensor",
"imu",
update_rate=self.conf.sensor_update_rate)
self.link.add_sensor(imu)
self.apply_color()
def _initialize(self, **kwargs):
"""
:param kwargs:
:return:
"""
self.link = self.create_component(
Box(WIDTH, WIDTH, HEIGHT, MASS),
"box",
visual=Mesh("model://tol_robot/meshes/CoreComponent.dae"))
if not self.conf.disable_sensors:
# Now we will add the IMU sensor. First, we must
# create a sensor in SDF. The sensor must have a name which
# is unique for the robot - `add_sensor` ensures this for us
# assuming the `prefix` argument is left to its default.
imu = SdfSensor("imu_sensor",
"imu",
update_rate=self.conf.sensor_update_rate)
self.link.add_sensor(imu)
self.apply_color()
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 _initialize(self, **kwargs):
"""
:param kwargs:
:return:
"""
# Shorthand for variables
# Initialize root
self.hinge_root = self.create_component(
Box(SLOT_THICKNESS, SLOT_WIDTH, SLOT_WIDTH, MASS_SLOT), "root")
# Make frame
visual1 = Visual("frame_visual",
Mesh("model://rg_robot/meshes/ActiveHinge_Frame.dae"))
frame = self.create_component(Box(FRAME_LENGTH, SLOT_WIDTH,
FRAME_HEIGHT, MASS_FRAME),
"frame",
visual=visual1)
x_frame = SLOT_THICKNESS / 2.0 + SEPARATION + FRAME_LENGTH / 2.0
frame.set_position(Vector3(x_frame, 0, 0))
# Make servo
visual2 = Visual(
"servo_visual",
Mesh("model://rg_robot/meshes/ActiveCardanHinge_Servo_Holder.dae"))
servo = self.create_component(Box(SERVO_LENGTH, SLOT_WIDTH,
SERVO_HEIGHT, MASS_SERVO),
"servo",
visual=visual2)
x_servo = x_frame + (FRAME_ROTATION_OFFSET - 0.5 * FRAME_LENGTH) + \
(-0.5 * SERVO_LENGTH + SERVO_ROTATION_OFFSET)
servo.set_position(Vector3(x_servo, 0, 0))
# TODO Color servo?
# Make the tail. Visual is provided by Servo_Holder above.
self.hinge_tail = self.create_component(Box(SLOT_THICKNESS, SLOT_WIDTH,
SLOT_WIDTH, MASS_SLOT),
"tail",
visual=False)
x_tail = x_servo + SERVO_LENGTH / 2.0 + SEPARATION + SLOT_THICKNESS / 2.0
self.hinge_tail.set_position(Vector3(x_tail, 0, 0))
# Create joints to hold the pieces in position
# root <-> frame
self.fix(self.hinge_root, frame)
# 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", servo, frame, axis=Vector3(0, 1, 0))
self.joint.set_position(
Vector3(-0.5 * FRAME_LENGTH + FRAME_ROTATION_OFFSET, 0, 0))
self.joint.axis.limit = Limit(lower=-constants.SERVO_LIMIT,
upper=constants.SERVO_LIMIT,
effort=constants.MAX_SERVO_TORQUE,
velocity=constants.MAX_SERVO_VELOCITY)
self.add_joint(self.joint)
# connection part b <-> tail
self.fix(servo, self.hinge_tail)
# Now we add a motor that powers the joint. This particular servo
# targets a position. Use a simple PID controller initially.
pid = constants.SERVO_POSITION_PID
self.motors.append(PositionMotor(self.id, "rotate", self.joint, pid))
# Apply color mixin
self.apply_color()
def _initialize(self, **kwargs):
self.radius = in_mm(kwargs['radius'])
# Because of the cylinder shapes, x axis is swapped with z axis
# as compared to the Robogen code.
# Initialize root
self.root = self.create_component(
Box(SLOT_WIDTH, SLOT_WIDTH, SLOT_THICKNESS, MASS_SLOT), "root")
# Initialize servo
z_servo = 0
servo = self.create_component(
Box(SERVO_HEIGHT, SERVO_WIDTH, SERVO_LENGTH, MASS_SERVO), "servo")
servo.set_position(Vector3(z_servo, 0, X_SERVO))
# Initialize the base
spoke_mass = MASS_WHEG / 4.0
wheg_base_radius = WHEG_BASE_RADIUS
wheg_base = self.create_component(
Cylinder(wheg_base_radius, WHEG_THICKNESS, spoke_mass),
"wheg_base")
wheg_base.set_position(Vector3(z_servo, 0, X_WHEG_BASE))
# Initialize the spokes
spoke1 = self.create_component(
Box(self.radius, WHEG_WIDTH, WHEG_THICKNESS, spoke_mass), "spoke1")
spoke2 = self.create_component(
Box(self.radius, WHEG_WIDTH, WHEG_THICKNESS, spoke_mass), "spoke2")
spoke3 = self.create_component(
Box(self.radius, WHEG_WIDTH, WHEG_THICKNESS, spoke_mass), "spoke3")
# Rotate the spokes
spokes = [spoke1, spoke2, spoke3]
""" :type : list[Posable] """
rotations = [60, 180, 300]
axis = Vector3(0, 0, 1)
r = wheg_base_radius + 0.5 * self.radius
for spoke, rotation in itertools.izip(spokes, rotations):
spoke.rotate_around(axis, math.radians(rotation))
rotate_radians = math.radians(rotation)
x = r * math.cos(rotate_radians)
y = r * math.sin(rotate_radians)
spoke.set_position(Vector3(z_servo + x, y, X_WHEG_BASE))
# Create the connecting joints
self.fix(self.root, servo)
for spoke in spokes:
self.fix(wheg_base, spoke)
# Revolute joint of the servo
self.joint = Joint("revolute", servo, wheg_base, axis=Vector3(0, 0, 1))
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_VELOCITY_PID
self.motors.append(
VelocityMotor(self.id,
"rotate",
self.joint,
pid=pid,
max_velocity=constants.MAX_SERVO_VELOCITY,
min_velocity=-constants.MAX_SERVO_VELOCITY))
# Call color mixin
self.apply_color()
def _initialize(self, **kwargs):
"""
:param kwargs:
:return:
"""
self.root = self.create_component(
Box(SLOT_THICKNESS, SLOT_WIDTH, SLOT_WIDTH, MASS_SLOT), "root")
conn_a = self.create_component(
Box(CONNECTION_PART_LENGTH, CONNECTION_PART_WIDTH,
CONNECTION_PART_HEIGHT, MASS_SERVO), "conn_a")
cross_a = self.create_component(
Box(CROSS_THICKNESS, CROSS_WIDTH, CROSS_HEIGHT, MASS_CROSS / 3.0),
"cross_a")
cross_edge_mass = MASS_CROSS / 12.0
cross_a_edge_1 = self.create_component(
Box(CROSS_CENTER_OFFSET, CROSS_WIDTH, CROSS_THICKNESS,
cross_edge_mass), "cross_a_edge_1")
cross_a_edge_2 = self.create_component(
Box(CROSS_CENTER_OFFSET, CROSS_WIDTH, CROSS_THICKNESS,
cross_edge_mass), "cross_a_edge_2")
cross_b_edge_1 = self.create_component(
Box(CROSS_CENTER_OFFSET, CROSS_THICKNESS, CROSS_WIDTH,
cross_edge_mass), "cross_b_edge_1")
cross_b_edge_2 = self.create_component(
Box(CROSS_CENTER_OFFSET, CROSS_THICKNESS, CROSS_WIDTH,
cross_edge_mass), "cross_b_edge_2")
cross_b = self.create_component(
Box(CROSS_THICKNESS, CROSS_HEIGHT, CROSS_WIDTH, MASS_CROSS / 3.0),
"cross_b")
conn_b = self.create_component(
Box(CONNECTION_PART_LENGTH, CONNECTION_PART_HEIGHT,
CONNECTION_PART_WIDTH, MASS_SERVO), "conn_b")
self.tail = self.create_component(
Box(SLOT_THICKNESS, SLOT_WIDTH, SLOT_WIDTH, MASS_SLOT), "tail")
# Initialize the first connection part
x_part_a = 0.5 * (SLOT_THICKNESS + CONNECTION_PART_LENGTH) + SEPARATION
conn_a.set_position(Vector3(x_part_a, 0, 0))
# Initialize the cross
x_cross = x_part_a + CONNECTION_PART_OFFSET
cross_pos = Vector3(x_cross, 0, 0)
cross_a.set_position(cross_pos)
cross_b.set_position(cross_pos)
# Cross connection edges
x_cross_a_edge = x_cross - 0.5 * (CROSS_WIDTH + CROSS_THICKNESS)
z_cross_a_edge = 0.5 * (CROSS_HEIGHT + CROSS_THICKNESS)
cross_a_edge_1.set_position(Vector3(x_cross_a_edge, 0, z_cross_a_edge))
cross_a_edge_2.set_position(Vector3(x_cross_a_edge, 0,
-z_cross_a_edge))
x_cross_b_edge = x_cross + 0.5 * (CROSS_WIDTH + CROSS_THICKNESS)
y_cross_b_edge = 0.5 * (CROSS_HEIGHT - CROSS_THICKNESS)
cross_b_edge_1.set_position(Vector3(x_cross_b_edge, y_cross_b_edge, 0))
cross_b_edge_2.set_position(Vector3(x_cross_b_edge, -y_cross_b_edge,
0))
# Initialize the second connection part
x_part_b = x_cross + CONNECTION_PART_OFFSET
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.set_position(Vector3(x_tail, 0, 0))
# Fix the parts using joints
# root <-> connection a
self.fix(self.root, conn_a)
# connection a <hinge> cross part a
# easier to position if conn_a is the child
joint_a = Joint("revolute", cross_a, conn_a, axis=Vector3(0, 0, 1))
joint_a.set_position(
Vector3(-0.5 * CONNECTION_PART_LENGTH +
CONNECTION_PART_ROTATION_OFFSET))
joint_a.axis.limit = Limit(lower=-constants.SERVO_LIMIT,
upper=constants.SERVO_LIMIT,
effort=constants.MAX_SERVO_TORQUE,
velocity=constants.MAX_SERVO_VELOCITY)
self.add_joint(joint_a)
# cross part b <hinge> connection b
joint_b = Joint("revolute", cross_b, conn_b, axis=Vector3(0, 1, 0))
joint_b.set_position(
Vector3(0.5 * CONNECTION_PART_LENGTH -
CONNECTION_PART_ROTATION_OFFSET))
joint_b.axis.limit = Limit(lower=-constants.SERVO_LIMIT,
upper=constants.SERVO_LIMIT,
effort=constants.MAX_SERVO_TORQUE,
velocity=constants.MAX_SERVO_VELOCITY)
self.add_joint(joint_b)
# connection b <-> tail
self.fix(conn_b, self.tail)
# Fix the cross
self.fix(cross_a, cross_b)
self.fix(cross_a_edge_1, cross_a)
self.fix(cross_a_edge_2, cross_a)
self.fix(cross_b_edge_1, cross_b)
self.fix(cross_b_edge_2, cross_b)
# Apply color mixin
self.apply_color()
# Register motors
pid = constants.SERVO_POSITION_PID
self.motors.append(PositionMotor(self.id, "motor_a", joint_a, pid=pid))
self.motors.append(PositionMotor(self.id, "motor_b", joint_b, pid=pid))
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")
link.add_elements([base, left, right])
from sdfbuilder import SDF, Model, Link, PosableGroup
from sdfbuilder.structure import Box, Cylinder, Collision, Visual, StructureCombination
from sdfbuilder.math import Vector3
from sdfbuilder.joint import Joint
import math
# Create a box geometry
box_geom = Box(1.0, 1.0, 1.0, mass=0.5)
# Create a collision and a visual element using this box geometry
# A collision / visual's pose is determined by its geometry, so
# never use the same geometry object twice or you will get weird
# behavior when rotating.
box_col = Collision("box_collision", box_geom)
box_vis = Visual("box_visual", box_geom.copy())
# Add the two to a PosableGroup, so we can conveniently move them
# around together.
box = PosableGroup(elements=[box_col, box_vis])
# Now create a Cylinder visual/collision combi. You didn't think that the above would
# be the easiest way to do it now did you? The above was just to show you
# how that works.
cyl_geom = Cylinder(radius=0.25, length=0.5, mass=0.1)
cylinder = StructureCombination("cylinder", cyl_geom)
# Align the cylinder such that...
cylinder.align(
# .. its bottom ..
Vector3(0, 0, -0.5 * cyl_geom.length),
def _initialize(self, **kwargs):
"""
:param kwargs:
:return:
"""
# Variable shorthands
mass = self.MASS_SLOT
thickness = self.SLOT_THICKNESS
width = self.SLOT_WIDTH
conn_width = self.CONNECTION_PART_WIDTH
conn_thickness = self.CONNECTION_PART_THICKNESS
mass_per_cm = self.MASS_CONNECTION_PER_CM
sep = self.SEPARATION
# Connection length is in mm
self.conn_length = conn_length = in_mm(kwargs['connection_length'])
# Initialize root
self.root = self.create_component(Box(thickness, width, width, mass),
"root")
# Initialize connection
x_connection = 0.5 * (thickness + conn_length) + sep
connection = self.create_component(
Box(conn_length, conn_width, conn_thickness,
10 * mass_per_cm * conn_length), "connection")
connection.set_position(Vector3(x_connection, 0, 0))
# Tail
x_slot_b = x_connection + 0.5 * (conn_length + thickness) + sep
self.tail = self.create_component(Box(thickness, width, width, mass),
"tail")
self.tail.set_position(Vector3(x_slot_b, 0, 0))
# Create fixed joints between the parts
# root <> connection
self.fix(self.root, connection)
# connection <> tail
self.fix(connection, self.tail)
# Now perform the rotations. `BodyPart` guarantees
# that it has origin position and zero orientation
# at initialisation time, as to avoid confusion with
# `relative_to_child`.
self.tilt = tilt = kwargs['alpha']
self.rotation = rotation = kwargs['beta']
# First rotate the connection and the tail over beta
rotation_axis = Vector3(1, 0, 0)
connection.rotate_around(rotation_axis,
rotation,
relative_to_child=False)
self.tail.rotate_around(rotation_axis,
rotation,
relative_to_child=False)
# First tilt the tail over alpha
self.tail.rotate_around(Vector3(0, 1, 0), tilt, relative_to_child=True)
# Trigger color mixin
self.apply_color()
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://tol_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()
from sdfbuilder import SDF, Model, Link, PosableGroup
from sdfbuilder.structure import Box, Cylinder, Collision, Visual, StructureCombination
from sdfbuilder.math import Vector3
from sdfbuilder.joint import Joint
import math
# Create a box geometry
box_geom = Box(1.0, 1.0, 1.0, mass=0.5)
# Create a collision and a visual element using this box geometry
# A collision / visual's pose is determined by its geometry, so
# never use the same geometry object twice or you will get weird
# behavior when rotating.
box_col = Collision("box_collision", box_geom)
box_vis = Visual("box_visual", box_geom.copy())
# Add the two to a PosableGroup, so we can conveniently move them
# around together.
box = PosableGroup(elements=[box_col, box_vis])
# Now create a Cylinder visual/collision combi. You didn't think that the above would
# be the easiest way to do it now did you? The above was just to show you
# how that works.
cyl_geom = Cylinder(radius=0.25, length=0.5, mass=0.1)
cylinder = StructureCombination("cylinder", cyl_geom)
# Align the cylinder such that...
cylinder.align(
# .. its bottom ..
Vector3(0, 0, -0.5 * cyl_geom.length),