class ActiveWheel(BodyPart, ColorMixin):
"""
Active wheel
"""
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 get_slot(self, slot_id):
self.check_slot(slot_id)
return self.root
def get_slot_position(self, slot_id):
return Vector3(0, 0, -0.5 * SLOT_THICKNESS)
def get_slot_normal(self, slot_id):
return Vector3(0, 0, -1)
def get_slot_tangent(self, slot_id):
return Vector3(0, 1, 0)
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()
class Hinge(BodyPart, ColorMixin):
"""
A passive hinge
"""
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()
def get_slot_normal(self, slot_id):
return self.get_slot_position(slot_id).normalized()
def get_slot_tangent(self, slot_id):
self.check_slot(slot_id)
return Vector3(0, 0, 1)
def get_slot(self, slot_id):
self.check_slot(slot_id)
return self.hinge_root if slot_id == 0 else self.hinge_tail
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 _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))
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))
class ActiveHinge(BodyPart, ColorMixin):
"""
A passive hinge
"""
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://tol_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://tol_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 get_slot_normal(self, slot_id):
return self.get_slot_position(slot_id).normalized()
def get_slot_tangent(self, slot_id):
self.check_slot(slot_id)
return Vector3(0, 0, 1)
def get_slot(self, slot_id):
self.check_slot(slot_id)
return self.hinge_root if slot_id == 0 else self.hinge_tail
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)
class Hinge(BodyPart, ColorMixin):
"""
A passive hinge
"""
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()
def get_slot_normal(self, slot_id):
return self.get_slot_position(slot_id).normalized()
def get_slot_tangent(self, slot_id):
self.check_slot(slot_id)
return Vector3(0, 0, 1)
def get_slot(self, slot_id):
self.check_slot(slot_id)
return self.hinge_root if slot_id == 0 else self.hinge_tail
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)
class ActiveHinge(BodyPart, ColorMixin):
"""
A passive hinge
"""
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://tol_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://tol_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 get_slot_normal(self, slot_id):
return self.get_slot_position(slot_id).normalized()
def get_slot_tangent(self, slot_id):
self.check_slot(slot_id)
return Vector3(0, 0, 1)
def get_slot(self, slot_id):
self.check_slot(slot_id)
return self.hinge_root if slot_id == 0 else self.hinge_tail
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)