3
\$\begingroup\$

While reworking my teaching materials for an exercise of an introductory course on mobile robotics, I recently created a animation/simulation of an incremental rotary encoder, e.g. often used for odometry purposes.

I'm quite happy how it turned out and it was indeed helpful when talking about signal patterns induced by clockwise, respectively counter-clockwise rotation. Rotation speed, number of sectors, and frame rate have to be set up before running the script while the rotation direction can be changed on the fly by pressing x on the keyboard.

The code and an example image can be found below. It was written to work with Python 2.7 but can be adapted to run with Python 3 with a few minor modifications. My only real concern about the code would be that it might be a bit overengineered for what I was trying to accomplish.

from __future__ import print_function, division

import time
from enum import Enum

import numpy as np

import matplotlib.pyplot as plt
import matplotlib as mpl
from matplotlib.patches import Circle, Wedge
from matplotlib.collections import PatchCollection
from matplotlib.animation import FuncAnimation


class SensorState(Enum):
    LOW = 0
    HIGH = 1


class RotaryEncoder(object):

    def __init__(self, n_sectors, position_s1=0.0, position_s2=None):
        """Create a incremental rotary encoder with two sensors

        Parameters
        ----------
        n_sectors : int
            positive even number of sectors on the rotary encoder
        position_s1 : float, optional
            angular position of the 1st rotary encoder sensor (in deg).
            The default is 0.
        position_s2 : float, optional
            angular position of the 2nd rotary encoder sensor (in deg).
            The default is None, which means the encoder class chooses the
            sensor position to have necessary phase offset automatically.
        """
        self.n_sectors = int(n_sectors)
        assert self.n_sectors % 2 == 0 and self.n_sectors > 0
        self.sector_angle = 360.0 / self.n_sectors

        self.position_s1 = RotaryEncoder._clamp_angle(position_s1)
        self.position_s2 = RotaryEncoder._clamp_angle(
            -self.sector_angle / 2.0 if position_s2 is None else position_s2
        )

        self.pos = 0.0

    def _get_sensor_reading(self, pos):
        """Return the sensor reading at a given position"""
        # lets suppose the wheel stands still and the sensor moves
        measuring_pos = RotaryEncoder._clamp_angle(self.pos - pos)
        # now look at which sector the sensor currently is in
        sector_idx = int(np.floor_divide(measuring_pos, self.sector_angle)) % self.n_sectors
        # print("{:.3f} in sector {:.0f}".format(measuring_pos, sector_idx))
        # the sectors are colored black and white in an alternating pattern
        # sectors with even index are supposed to be black, otherwise white
        return SensorState.HIGH if sector_idx % 2 == 0 else SensorState.LOW

    def read_sensor1(self):
        """Get the simulated sensor reading of the 1st sensor"""
        return self._get_sensor_reading(self.position_s1)

    def read_sensor2(self):
        """Get the simulated sensor reading of the 2nd sensor"""
        return self._get_sensor_reading(self.position_s2)

    def turn(self, increment):
        """Incrementally turn the rotary encoder disc

        Parameters
        ----------
        increment : float
            the incremental turn (in deg) to be applied
        """
        self.pos = RotaryEncoder._clamp_angle(self.pos+increment)

    @staticmethod
    def _clamp_angle(angle):
        """Clamp angles to [0, 360)"""
        return np.mod(angle, 360.0)


class RotaryEncoderView(object):
    """Visualize a rotary encoder as collection of patches from matplotlib"""

    def __init__(self, axs, encoder, radius_inner, radius_outer, radius_sensor,
                 time_horizon=10):
        self.encoder = encoder

        self.radius_inner = radius_inner
        self.radius_outer = radius_outer
        self.radius_sensor = radius_sensor

        color_s1 = "green"
        color_s2 = (1.0, 0.753, 0.0)
        self.sensor_patch1 = self._draw_sensor_patch(self.encoder.position_s1, color_s1)
        self.sensor_patch2 = self._draw_sensor_patch(self.encoder.position_s2, color_s2)
        self.sector_patch_collection = self._draw_sector_patches()

        self.encoder_ax, self.sensor_ax = axs
        # prepare encoder display
        self.encoder_ax.add_collection(self.sector_patch_collection)
        self.encoder_ax.add_patch(self.sensor_patch1)
        self.encoder_ax.add_patch(self.sensor_patch2)

        self.encoder_ax.set_aspect("equal", adjustable="box")
        self.encoder_ax.set_axis_off()
        scale = 1.01
        self.encoder_ax.set_xlim(-self.radius_outer*scale, self.radius_outer*scale)
        self.encoder_ax.set_ylim(-self.radius_outer*scale, self.radius_outer*scale)

        # prepare sensor value display over time
        self.time_horizon = time_horizon

        self.sensor_ax.plot([], [], color=color_s1, ls="-")
        self.sensor_ax.plot([], [], color=color_s2, ls="-")
        self.sensor_ax.get_xaxis().set_visible(False)

        self.sensor_ax.set_ylim(-0.1, 1.05)
        self.sensor_ax.set_yticks([-0.05, 0.45, 0.5, 1.0])
        self.sensor_ax.set_yticklabels([0, 1, 0, 1])
        self.sensor_ax.grid(True, which="major", linestyle="--")

    def _draw_sector_patches(self):
        """Create the sectors in alternating colors"""
        width = self.radius_outer - self.radius_inner
        patches = []
        for i in xrange(self.encoder.n_sectors):
            color = "black" if i % 2 == 0 else "white"
            patch = Wedge((0, 0), self.radius_outer,
                          i*self.encoder.sector_angle,
                          (i+1)*self.encoder.sector_angle,
                          width=width, facecolor=color)
            patches.append(patch)

        # inner and outer line around the wedges
        patches.append(Circle((0, 0), self.radius_outer, edgecolor="black", facecolor="none"))
        patches.append(Circle((0, 0), self.radius_inner, edgecolor="black", facecolor="none"))

        return PatchCollection(patches, match_original=True)

    def _draw_sensor_patch(self, position, color):
        # draw sensors
        alpha = np.radians(position)
        x, y = self.radius_outer-self.radius_sensor, 0
        x, y = x*np.cos(alpha) - y*np.sin(alpha), x*np.sin(alpha) + y*np.cos(alpha)
        sensor_patch = Circle((x, y), self.radius_sensor,
                              facecolor=color, alpha=0.8, zorder=1000)
        return sensor_patch

    def _update_sensor_line(self, line, y_value):
        """Include new value in sensor data, trim of old data"""
        t_now = time.clock()
        line.set_xdata(np.append(line.get_xdata(), t_now))
        line.set_ydata(np.append(line.get_ydata(), y_value))

        self.sensor_ax.set_xlim(t_now-self.time_horizon, t_now)
        mask_visible = line.get_xdata() > (t_now - self.time_horizon)
        line.set_xdata(line.get_xdata()[mask_visible])
        line.set_ydata(line.get_ydata()[mask_visible])

    def turn_and_draw(self, increment):
        """Update the view (and the internal encoder)"""
        self.encoder.turn(increment)
        trans = mpl.transforms.Affine2D().rotate_deg(self.encoder.pos)\
            + self.encoder_ax.transData
        self.sector_patch_collection.set_transform(trans)

        # draw sensor graphs
        line_s1 = self.sensor_ax.lines[0]
        line_s2 = self.sensor_ax.lines[1]
        if increment != 0:
            self._update_sensor_line(line_s1, 0.5*self.encoder.read_sensor1().value+0.5)
            self._update_sensor_line(line_s2, 0.5*self.encoder.read_sensor2().value-0.05)

        return self.sector_patch_collection, self.sensor_patch1, self.sensor_patch2, line_s1, line_s2


def run_animation(n_sectors, rev_per_min, fps=60):
    """Run the actual animation"""

    settings = {"direction": 1, "run": 1}
    def press(settings, event):
        # global direction, run
        if event.key == 'x':
            settings["direction"] *= -1
            settings["run"] = 1
        elif event.key == ' ':
            settings["run"] = 0 if settings["run"] else 1

    fig, (encoder_ax, sensor_ax) = plt.subplots(ncols=2, gridspec_kw={'width_ratios': [1, 2]})
    fig.canvas.mpl_connect("key_press_event", lambda ev: press(settings, ev))

    encoder = RotaryEncoder(n_sectors=n_sectors)
    view = RotaryEncoderView((encoder_ax, sensor_ax), encoder,
                             radius_inner=1.0,
                             radius_outer=2.0,
                             radius_sensor=0.1)

    rev_per_frame = rev_per_min / 60 / fps * 360
    interval = 1000 / fps

    def animate(frame):
        """Simulate wheel rotation"""
        return view.turn_and_draw(settings["run"]*settings["direction"]*rev_per_frame)

    animation = FuncAnimation(fig, animate, frames=360, interval=interval, blit=True)

    plt.tight_layout()
    plt.show()

if __name__ == "__main__":
    run_animation(n_sectors=12, rev_per_min=1, fps=60)

example image of the rotary encoder visualization

\$\endgroup\$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.