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()
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()
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))
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 _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()
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: """ # 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()