예제 #1
0
파일: main.py 프로젝트: bobbyluig/Eclipse
    def __init__(self, robot):
        # Set up robot.
        self.robot = robot

        # Set error.
        self.epsilon = 1e-6

        # Set up Usc.
        try:
            self.usc = Usc()
            logger.info("Successfully attached to Maestro's low-level interface.")
        except ConnectionError:
            self.usc = Dummy()
            logger.warn("Failed to attached to Maestro's low-level interface. "
                        "If not debugging, consider this a fatal error.")

        # Set up virtual COM and TTL ports.
        try:
            self.maestro = Maestro()
            logger.info("Successfully attached to Maestro's command port.")
        except ConnectionError:
            self.maestro = Dummy()
            logger.warn("Failed to attached to Maestro's command port. "
                        "If not debugging, consider this a fatal error.")

        # Emergency stop.
        self.emergency = Event()

        # Zero.
        self.zero()
예제 #2
0
파일: main.py 프로젝트: bobbyluig/Eclipse
class Agility:
    def __init__(self, robot):
        # Set up robot.
        self.robot = robot

        # Set error.
        self.epsilon = 1e-6

        # Set up Usc.
        try:
            self.usc = Usc()
            logger.info("Successfully attached to Maestro's low-level interface.")
        except ConnectionError:
            self.usc = Dummy()
            logger.warn("Failed to attached to Maestro's low-level interface. "
                        "If not debugging, consider this a fatal error.")

        # Set up virtual COM and TTL ports.
        try:
            self.maestro = Maestro()
            logger.info("Successfully attached to Maestro's command port.")
        except ConnectionError:
            self.maestro = Dummy()
            logger.warn("Failed to attached to Maestro's command port. "
                        "If not debugging, consider this a fatal error.")

        # Emergency stop.
        self.emergency = Event()

        # Zero.
        self.zero()

    def stop(self):
        """
        Emergency stop. Stop all wait functions.
        """

        self.emergency.set()

    def clear(self):
        """
        Clear emergency flag.
        """

        self.emergency.clear()

    def head_rotation(self):
        """
        Provides head rotation.
        :return: Head rotation in degrees.
        """

        servo = self.robot.head[0]
        self.maestro.get_position(servo)

        return servo.get_position()

    def set_head(self, target, t=0):
        """
        Move the head to a given position.
        Blocks until completion.
        :param target: (LR, UD).
        :param t: Time in ms. 0 for max speed.
        """

        head = self.robot.head
        servos = self.robot.head_servos

        head[0].set_target(target[0])
        head[1].set_target(target[1])

        self.maestro.end_together(servos, t, True)
        self.wait(servos)

    def look_at(self, x, y):
        """
        Move the head to look at a given target.
        Note that this is an approximation. Best used in a PID loop.
        :param x: x-coordinate of target.
        :param y: y-coordinate of target.
        """

        head = self.robot.head
        camera = head.camera

        # Define velocity constant.
        k = 1.5

        # Compute deltas.
        dx = (x - 0.5 * camera.width) * -1
        dy = (y - 0.5 * camera.height) * -1

        dt = dx / camera.width * (camera.fx / 2)
        dp = dy / camera.height * (camera.fy / 2)

        # Compute suggested velocity. Balance between blur and speed.
        vt = int(round(abs(dt * k)))
        vp = int(round(abs(dt * k)))

        # Construct array.
        data = [dt, vt, dp, vp]

        # Perform motion.
        self.move_head(data)

        # Update target.
        head.target = [x, y]

        return data

    def scan(self, t, direction=None, block=False):
        """
        Scans head in a direction. If no direction is given, scans toward bound of last known location.
        If at minimum of maximum bounds, automatically selects opposite direction.
        Blocks until completely scanned towards one direction.
        :param t: Time in milliseconds.
        :param direction: A direction, either None, 1, or -1.
        :param block: Whether to wait until completion.
        """

        # Obtain definitions.
        head = self.robot.head
        camera = head.camera
        servo = head.servos[0]

        # Get bounds.
        low, high = servo.get_range()

        # Update servo.
        self.maestro.get_position(servo)

        # Check bound.
        bound = head.at_bound()

        # Create direction.
        if bound != 0:
            direction = bound * -1

        if direction is None:
            if head.target[0] < 0.5 * camera.width:
                direction = 1
            else:
                direction = -1

        # Execute.
        if direction == 1:
            servo.set_target(high)
        else:
            servo.set_target(low)

        self.maestro.end_in(servo, t)

        if block:
            self.wait(servo)

    def center_head(self, t=0):
        """
        Returns head to original position.
        :param t: The time in ms.
        """

        # Obtain definitions.
        head = self.robot.head
        servos = head.servos

        # Target zero.
        for servo in servos:
            servo.set_target(0)

        # Reset to zero.
        head.angles = [0, 0]

        # Execute.
        self.maestro.end_together(servos, t, True)
        self.wait(servos)

    def move_head(self, data):
        """
        Move head based on data parameters. Does not wait for completion.
        :param data: An array given by look_at.
        """

        # Obtain definitions.
        head = self.robot.head
        servos = head.servos

        # Update positions.
        self.maestro.get_multiple_positions(servos)

        for i in range(2):
            servo = head[i]
            current = servo.get_position()

            # Get data.
            delta = data[i * 2]
            velocity = data[i * 2 + 1]

            if velocity == 0:
                # Already at target. Do nothing.
                servo.target = servo.pwm
                target = current
            else:
                # Ensure that head is within bounds.
                low, high = servo.get_range()
                target = current + delta

                if target < low:
                    target = low
                elif target > high:
                    target = high

                servo.set_target(target)

            # Update.
            head.angles[i] = target

            # Set speed.
            self.maestro.set_speed(servo, velocity)

            # Execute.
            self.maestro.set_target(servo)

    @staticmethod
    def plot_gait(frames):
        """
        Plot a gait given some frames. Used for debugging.
        :param frames: Frames generated by execute.
        """

        fig = plt.figure()
        ax = fig.add_subplot(111, projection='3d')

        ax.set_xlabel('X Axis')
        ax.set_ylabel('Y Axis')
        ax.set_zlabel('Z Axis')

        x = frames[:, 0, 0]
        y = frames[:, 0, 1]
        z = frames[:, 0, 2]

        ax.plot(x, y, z, marker='o')
        plt.show()

    def execute_forever(self, frames, dt):
        """
        Like execute_frames(), except it runs forever.
        :param frames: An array of frames.
        :param dt: Delta t.
        :return:
        """

        # Get all legs and servos for quick access.
        legs = self.robot.legs
        servos = self.robot.leg_servos

        # Update initial leg locations.
        self.maestro.get_multiple_positions(servos)

        for leg in legs:
            leg.get_position()

        while True:
            for frame in frames:
                for i in range(4):
                    legs[i].target_point(frame[i])

                self.maestro.end_together(servos, dt)
                self.wait(servos)

    def execute_frames(self, frames, dt):
        """
        Execute some frames with a constant dt.
        :param frames: An array of frames.
        :param dt: Delta t.
        """

        # Get all legs and servos for quick access.
        legs = self.robot.legs
        servos = self.robot.leg_servos

        # Update initial leg locations.
        self.maestro.get_multiple_positions(servos)

        for frame in frames:
            for i in range(4):
                legs[i].target_point(frame[i])

            self.maestro.end_together(servos, dt)
            self.wait(servos)

    def execute_long(self, prev_frame, frames, dt):
        """
        Execute frames with constant but possibly long dt.
        Automatically computes distance, and, if necessary, interpolates to get more accurate synchronization.
        :param prev_frame: The previous frame.
        :param frames: An array of frames.
        :param dt: Delta t.
        """

        # Get all legs and servos for quick access.
        legs = self.robot.legs
        servos = self.robot.leg_servos

        # Define break constant (ms / cm).
        k = 100

        # Update initial leg locations.
        self.maestro.get_multiple_positions(servos)

        for frame in frames:
            # Compute max distance.
            d = max(np.linalg.norm(frame - prev_frame, axis=1))

            # Less than break. Too long. Linearly interpolate.
            if dt / d > k:
                n = int(round(dt / d / k)) + 1
                l_frames = self.smooth(prev_frame, frame, n)
                l_frames = l_frames[1:]

                # Compute time.
                t = dt / n

                # Execute intermediate frames.
                for l_frame in l_frames:
                    for i in range(4):
                        legs[i].target_point(l_frame[i])

                    self.maestro.end_together(servos, t)
                    self.wait(servos)
            else:
                t = dt

            for i in range(4):
                legs[i].target_point(frame[i])

            self.maestro.end_together(servos, t)
            self.wait(servos)

            prev_frame = frame

    def execute_variable(self, frames, dts):
        """
        Execute some frames with different dt.
        :param frames: An array of frames.
        :param dts: An array of dt.
        """

        # Get all legs and servos for quick access.
        legs = self.robot.legs
        servos = self.robot.leg_servos

        # Update initial leg locations.
        self.maestro.get_multiple_positions(servos)

        # Assertion check.
        assert len(frames) == len(dts)

        for t in range(len(frames)):
            for i in range(4):
                legs[i].target_point(frames[t][i])

            self.maestro.end_together(servos, dts[t])
            self.wait(servos)

    def execute_angles(self, angles, dt):
        """
        Like execute_frames(), but uses angles instead.
        :param angles: An array of angles.
        :param dt: Delta t.
        """

        # Get all legs and servos for quick access.
        legs = self.robot.legs
        servos = self.robot.leg_servos

        # Update initial leg locations.
        self.maestro.get_multiple_positions(servos)

        for angle in angles:
            for i in range(4):
                legs[i].target_angle(angle)

            self.maestro.end_together(servos, dt)
            self.wait(servos)

    def anglify(self, frames):
        """
        Converts frames generated by self.prepare to angles.
        :param frames: The input frames.
        :return: The output angles ready for execution.
        """

        # Get all legs and servos for quick access.
        legs = self.robot.legs

        # Allocate memory.
        angles = np.empty(frames.shape)

        for i in range(len(frames)):
            for l in range(4):
                a = legs[l].get_angles(frames[i][l])
                angles[i][l] = a

        return angles

    @staticmethod
    def smooth(a, b, n):
        """
        Create a smooth transition from a to b in n steps.
        :param a: The first array.
        :param b: The second array.
        :param n: The number of steps.
        :return: An array from [a, b).
        """

        assert(a.shape == b.shape)
        assert(n > 1)

        # Compute delta.
        delta = (b - a) / n

        # Allocate n-1 with dimension d+1.
        shape = (n, *a.shape)
        inter = np.empty(shape)

        for i in range(n):
            inter[i] = a + i * delta

        return inter

    def get_pose(self):
        """
        Get the relative pose of the robot.
        :return: A (4 x 3) matrix representing the current state of the robot.
        """

        # Get all legs for quick access.
        legs = self.robot.legs

        # Iterate through all legs.
        pose = []
        for leg in legs:
            position = leg.get_position()
            pose.append(position)

        return np.array(pose, dtype=float)

    def target_point(self, leg, point, t):
        """
        Move a leg to a given point in t time.
        Blocks until completion.
        :param leg: Leg index.
        :param point: (x, y, z).
        :param t: Time in milliseconds.
        """

        # Assertion check.
        assert(0 <= leg <= 3)

        # Get legs for quick access.
        legs = self.robot.legs

        # Target.
        leg = legs[leg]
        leg.target_point(point)

        # Execute.
        servos = leg.servos
        self.maestro.end_together(servos, t, True)

        # Block until completion.
        self.wait(servos)

    def lift_leg(self, leg, lift, t):
        """
        Lift a leg (change pose) in t time.
        Blcoks until completion.
        :param leg: The leg index.
        :param lift: How high to lift leg.
        :param t: Time to execute pose change.
        """

        # Assertion check.
        assert (0 <= leg <= 3)

        # Get legs for quick access.
        legs = self.robot.legs

        # Define ground.
        ground = -max([leg.length for leg in legs]) + 1

        # Empty pose.
        pose = np.zeros((4, 3))

        # Leg lift.
        pose[:, 2] = ground
        pose[leg][2] = ground + lift

        # Execute.
        self.target_pose(pose, t)

    def target_pose(self, target, t, lift=2):
        """
        Get the robot from its current pose to a new pose. Block until completion.
        The robot will lift legs appropriately to eliminate dragging.
        Automatically adjusts the center of mass during transition and target if necessary.
        :param target: The target pose.
        :param t: The total time for the adjustment.
        :param lift: How much to lift each leg.
        :return: (frames, dt) ready for execution.
        """

        # Get body for quick access.
        body = self.robot.body

        # Create data array.
        frames = []

        # Get pose. Assume updated.
        pose = self.get_pose()

        # Early exit.
        if np.array_equal(pose, target):
            return

        # Get ground, which is the lowest point.
        curr_g = np.min(pose[:, 2])
        next_g = np.min(target[:, 2])

        # Generate leg state arrays.
        pose_state = np.greater(pose[:, 2], (curr_g + self.epsilon))  # Defines which legs are in the air.
        target_state = np.greater(target[:, 2], (next_g + self.epsilon))      # Defines which legs are in the air.

        # Get all legs to (0, 0, curr_g) if they are in the air.
        if any(pose_state):
            f1 = pose.copy()

            for i in range(4):
                if pose_state[i]:
                    f1[i] = (0, 0, curr_g)

            frames.append(f1)

        # Define optimization procedure.
        def up_down(ground):
            # For every leg that is not at the right (x, y) and is on the ground in target, lift and down.
            for i in range(4):
                if not np.array_equal(pose[i][:2], target[i][:2]) and not target_state[i]:
                    # Get previous frame.
                    prev = frames[-1]
                    f4, f5 = prev.copy(), prev.copy()

                    # Move leg to target (x, y) in air.
                    x, y = target[i][:2]
                    f4[i] = (x, y, ground + lift)

                    # Compute bias and adjust.
                    s = [False, False, False, False]
                    s[i] = True
                    bias = body.adjust(s, f4, 1)
                    f3 = prev - bias
                    f4 -= bias

                    # Move leg down to target. Keep bias.
                    f5[i] = target[i]
                    f5 -= bias

                    # Append data.
                    frames.extend((f3, f4, f5))

        def to_next():
            f2 = pose.copy()
            f2[:, 2] = next_g
            frames.append(f2)

        # Different optimization order.
        if next_g > curr_g:
            # For body high -> low, get legs to next height first.
            to_next()
            up_down(next_g)
        elif curr_g > next_g:
            # For body low -> high, get legs to next height last.
            up_down(curr_g)
            to_next()

        # Move to final target if necessary.
        if not np.array_equal(frames[-1], target):
            if any(target_state):
                prev = frames[-1]
                bias = body.adjust(target_state, target)
                frames.extend((prev - bias, target - bias))
            else:
                frames.append(target)

        # Compute times. Assume equal dt.
        dt = t / len(frames)

        self.execute_long(pose, frames, dt)

    def prepare_frames(self, frames, dt, ground):
        """
        Prepare some frames which are non-circular (last frame not linked to first frame).
        :param frames: The input frames.
        :param dt: dt.
        :param ground: Ground.
        :param loop: Whether the gait loops.
        :return: (frames, dt) ready for execution.
        """

        # Define body for quick access.
        body = self.robot.body

        # Create array for biases.
        biases = np.empty(frames.shape)

        # Generate leg state arrays.
        state1 = np.greater(frames[:, :, 2], (ground + self.epsilon))       # Defines which legs are in the air.
        state2 = state1.sum(1)                                              # The number of legs in the air.

        # Define.
        steps = len(frames)

        for t in range(steps - 1):
            # Look ahead and pass data to center of mass adjustment algorithms.
            next_frame = frames[t]

            # Determine which legs are off.
            off = state1[t]
            count = state2[t]

            # Perform center of mass adjustments accordingly.
            biases[t] = body.adjust(off, next_frame, count)

        # Adjust frames.
        frames -= biases

        return frames, dt

    def prepare_gait(self, gait, debug=False):
        """
        Prepare a given gait class.
        :param gait: The gait class.
        :param debug: Show gait in a graph.
        :return: (frames, dt) ready for execution.
        """

        # Define body for quick access.
        body = self.robot.body

        # Get gait properties.
        steps = gait.steps
        ground = gait.ground
        dt = gait.time / steps
        ts = np.linspace(0, 1000, num=steps, endpoint=False)

        # Get all legs for quick access.
        legs = self.robot.legs

        # Compute shape.
        shape = (steps, 4, 3)

        # Evaluate gait.
        f = [gait.evaluate(leg, ts) for leg in legs]
        frames = np.concatenate(f).reshape(shape, order='F')

        # Debugging.
        if debug:
            self.plot_gait(frames)

        # Create array for biases.
        biases = np.empty(shape)

        # Generate leg state arrays.
        state1 = np.greater(biases[:, :, 2], (ground + 1e-6))     # Defines which legs are in the air.
        state2 = state1.sum(1)                                      # The number of legs in the air.

        # Iterate and perform static analysis.
        for t in range(steps):
            # Look ahead and pass data to center of mass adjustment algorithms.
            next_frame = frames[(t + 1) % steps]

            # Determine which legs are off.
            off = state1[t]
            count = state2[t]

            # Perform center of mass adjustments accordingly.
            biases[t] = body.adjust(off, next_frame, count)

        # Adjust frames.
        frames -= biases

        return frames, dt

    def prepare_smoothly(self, gait):
        """
        Prepare a gait by intelligently applying smoothing. Only works for planar COM adjustments.
        Plus, who doesn't like smooth things? (I'm really tired right now.)
        :param gait: The gait object.
        :return: (frames, dt) ready for execution.
        """

        # Define body for quick access.
        body = self.robot.body

        # Get gait properties.
        steps = gait.steps
        ground = gait.ground
        dt = gait.time / steps
        ts = np.linspace(0, 1000, num=steps, endpoint=False)

        # Get all legs for quick access.
        legs = self.robot.legs

        # Compute shape.
        shape = (steps, 4, 3)

        # Evaluate gait.
        f = [gait.evaluate(leg, ts) for leg in legs]
        frames = np.concatenate(f).reshape(shape, order='F')

        # Generate leg state arrays.
        state1 = np.greater(frames[:, :, 2], (ground + 1e-6))  # Defines which legs are in the air.
        state2 = state1.sum(1)  # The number of legs in the air.

        # Get indices of legs in air.
        air = np.where(state2 != 0)[0]
        air = air.tolist()

        # Create array for biases.
        biases = np.empty(shape)

        # Keep track of last air -> ground.
        t = air[-1]
        if state2[(t + 1) % steps] == 0:
            # Last air frame is an air -> ground transition.
            last_ag = t
        else:
            # There will
            last_ag = None

        # Compute biases for each frame that is not on the ground.
        for i in range(len(air)):
            # Get the index relative to all frames.
            t = air[i]

            # Compute bias as usual.
            next_frame = frames[(t + 1) % steps]
            off = state1[t]
            count = state2[t]

            biases[t] = body.adjust(off, next_frame, count)

            # Checks if the current frame represents a ground -> air transition.
            if state2[t - 1] == 0:
                curr_bias = biases[t]
                prev_bias = biases[last_ag]

                # Smooth from [t, last_ag).
                if t > last_ag:
                    n = t - last_ag
                    inter = self.smooth(prev_bias, curr_bias, n)
                    biases[last_ag:t] = inter
                else:
                    n = steps - last_ag + t
                    inter = self.smooth(prev_bias, curr_bias, n)
                    biases[last_ag:] = inter[:(steps - last_ag)]
                    biases[:t] = inter[(steps - last_ag):]

            # Check if the current frame represents an air -> ground transition.
            if state2[(t + 1) % steps] == 0:
                last_ag = t

        # Adjust frames.
        frames -= biases

        return frames, dt

    def move_body(self, x, y, z, t=0):
        """
        Move the body some x, y, and z.
        :param x: Move x.
        :param y: Move y.
        :param z: Move z.
        :param t: The time in ms.
        """

        legs = self.robot.legs
        servos = self.robot.leg_servos

        self.maestro.get_multiple_positions(servos)

        for leg in legs:
            a, b, c = leg.get_position()
            a -= x
            b -= y
            c -= z
            leg.target_point((-x, -y, -leg.length - z))

        self.maestro.end_together(servos, t)
        self.wait(servos)

    def configure(self):
        """
        Configure the Maestro by writing home positions and other configuration data to the device.
        """

        settings = self.usc.getUscSettings()
        settings.serialMode = uscSerialMode.SERIAL_MODE_USB_DUAL_PORT

        for leg in self.robot.legs:
            for servo in leg:
                servo.zero()
                channel = settings.channelSettings[servo.channel]
                channel.mode = ChannelMode.Servo
                channel.homeMode = HomeMode.Goto
                channel.home = servo.target
                channel.minimum = (servo.min_pwm // 64) * 64
                channel.maximum = -(-servo.max_pwm // 64) * 64

        for servo in self.robot.head:
            servo.zero()
            channel = settings.channelSettings[servo.channel]
            channel.mode = ChannelMode.Servo
            channel.homeMode = HomeMode.Goto
            channel.home = servo.target
            channel.minimum = (servo.min_pwm // 64) * 64
            channel.maximum = -(-servo.max_pwm // 64) * 64

        self.usc.setUscSettings(settings, False)
        self.usc.reinitialize(500)

    def go_home(self):
        """
        Let the Maestro return all servos to home.
        """

        self.maestro.go_home()

    def ready(self, z, t=2000):
        """
        Ready a gait by lower robot to plane.
        :param z: Height of gait.
        :param t: Time in milliseconds
        """

        # Compute desired pose.
        pose = np.zeros((4, 3))
        pose[:, 2] = z

        # Execute position.
        self.target_pose(pose, t)

    def zero(self):
        """
        Manual return home by resetting all leg servo targets.
        """

        # Get all legs and servos for quick access.
        legs = self.robot.legs
        s1 = self.robot.leg_servos

        for leg in legs:
            z = -leg.length
            leg.target_point((0, 0, z))

        # Execute.
        self.set_head((0, 0), 1000)
        self.maestro.end_together(s1, 1000, True)

        # Wait until completion.
        self.wait()

    def wait(self, servos=None):
        """
        Block until all servos have reached their targets.
        :param servos: An array of servos. If None, checks if all servos have reached their targets (more efficient).
        """

        while not self.is_at_target(servos=servos) and not self.emergency.is_set():
            time.sleep(0.001)

    def is_at_target(self, servos=None):
        """
        Check if servos are at their target.
        :param servos: One or more servo objects. If None, checks if all servos have reached their targets (more efficient).
        :return: True if all servos are at their targets, False otherwise.
        """

        if servos is None:
            return not self.maestro.get_moving_state()
        elif isinstance(servos, Servo):
            self.maestro.get_position(servos)

            if servos.at_target():
                return True

            return False
        else:
            self.maestro.get_multiple_positions(servos)

            if all(servo.at_target() for servo in servos):
                return True

            return False