Skip to content

Pictures: screws.viz

Pictures and animations of screw axes. Everything here needs matplotlib (screws[plot]), imported only when a function is called.

Static: draw_robot, draw_screw, explore (static.py). Moving: animate_twist, animate_screw, compare_frames (animate.py), with the scenes door, drawer and screwdriver (scenes.py) and the Mesh, box and prism to build your own body. Jacobians and ellipsoids: draw_jacobian, draw_ellipsoid, animate_robot, compare_paths (jacobian.py).

Animation dataclass

A finished animation: the figure, the FuncAnimation, and the numbers drawn in each frame (theta, T_sb, and per entry point V_s, V_b, Ad, ...) for checking by hand.

Source code in screws/viz/animate.py 
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
@dataclass
class Animation:
    """A finished animation: the figure, the FuncAnimation, and the numbers drawn in each
    frame (theta, T_sb, and per entry point V_s, V_b, Ad, ...) for checking by hand."""

    fig: object
    anim: object
    fps: int
    data: dict = field(default_factory=dict)

    @property
    def thetas(self) -> np.ndarray:
        return self.data["theta"]

    @property
    def transforms(self) -> np.ndarray:
        return self.data["T_sb"]

    def save(self, path, fps: int | None = None, dpi: int | None = None) -> Path:
        """Write a .gif (Pillow, no extra needed) or .mp4 (ffmpeg from ``screws[video]``)."""
        from matplotlib import animation, rcParams

        path = Path(path)
        fps = fps or self.fps
        if path.suffix.lower() == ".gif":
            writer = animation.PillowWriter(fps=fps)
        elif path.suffix.lower() == ".mp4":
            import imageio_ffmpeg

            rcParams["animation.ffmpeg_path"] = imageio_ffmpeg.get_ffmpeg_exe()
            writer = animation.FFMpegWriter(fps=fps, bitrate=2400)
        else:
            raise ValueError(f"save to .gif or .mp4, not {path.suffix!r}")
        self.anim.save(str(path), writer=writer, dpi=dpi)
        return path

    def _repr_html_(self):
        return self.anim.to_jshtml(fps=self.fps)

save 

save(path, fps=None, dpi=None)

Write a .gif (Pillow, no extra needed) or .mp4 (ffmpeg from screws[video]).

Source code in screws/viz/animate.py 
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
def save(self, path, fps: int | None = None, dpi: int | None = None) -> Path:
    """Write a .gif (Pillow, no extra needed) or .mp4 (ffmpeg from ``screws[video]``)."""
    from matplotlib import animation, rcParams

    path = Path(path)
    fps = fps or self.fps
    if path.suffix.lower() == ".gif":
        writer = animation.PillowWriter(fps=fps)
    elif path.suffix.lower() == ".mp4":
        import imageio_ffmpeg

        rcParams["animation.ffmpeg_path"] = imageio_ffmpeg.get_ffmpeg_exe()
        writer = animation.FFMpegWriter(fps=fps, bitrate=2400)
    else:
        raise ValueError(f"save to .gif or .mp4, not {path.suffix!r}")
    self.anim.save(str(path), writer=writer, dpi=dpi)
    return path

ColumnDrawing dataclass

One column of the Jacobian as drawn, in the drawing's frame: the column S (of J_s, or J_b in the body frame), its axis line (q a point on it, the joint's own origin; s_hat; pitch h, inf for a prismatic joint), and the velocity it gives the tool point at unit rate, R J_bv[:, i] (the column itself, in 2D, of the notes' J(theta)).

Source code in screws/viz/jacobian.py 
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
@dataclass
class ColumnDrawing:
    """One column of the Jacobian as drawn, in the drawing's frame: the column S (of J_s,
    or J_b in the body frame), its axis line (q a point on it, the joint's own origin;
    s_hat; pitch h, inf for a prismatic joint), and the velocity it gives the tool point at
    unit rate, R J_bv[:, i] (the column itself, in 2D, of the notes' J(theta))."""

    index: int
    kind: str
    S: np.ndarray
    q: np.ndarray
    s_hat: np.ndarray
    h: float
    tool_velocity: np.ndarray
    axis_line: object = None
    arrow: object = None

EllipsoidDrawing dataclass

What draw_ellipsoid drew: the manipulability ellipsoid, the force ellipsoid if asked for, and the scales they were drawn at.

Source code in screws/viz/jacobian.py 
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
@dataclass
class EllipsoidDrawing:
    """What draw_ellipsoid drew: the manipulability ellipsoid, the force ellipsoid if asked
    for, and the scales they were drawn at."""

    fig: object
    ax: object
    theta: np.ndarray
    T_sb: np.ndarray
    kind: str
    planar: bool
    manipulability: Ellipsoid  # in the drawing's coordinates: {s}, or x-hat_1, x-hat_2 when planar
    force: Ellipsoid | None
    scale: float  # drawing metres per unit of semi-axis (m/s, rad/s)
    force_scale: float
    clipped: bool  # some semi-axis reaches past the frame, an unbounded one included, and is cut there

JacobianDrawing dataclass

What draw_jacobian drew: the figure, the matrix printed with its rank, and one ColumnDrawing per joint (all in the drawing's frame).

Source code in screws/viz/jacobian.py 
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
@dataclass
class JacobianDrawing:
    """What draw_jacobian drew: the figure, the matrix printed with its rank, and one
    ColumnDrawing per joint (all in the drawing's frame)."""

    fig: object
    ax: object
    frame: str
    theta: np.ndarray
    T_sb: np.ndarray
    J: np.ndarray  # the matrix printed: J_s, J_b, or the 2 x n tip Jacobian of a planar arm
    rank: int
    singular: bool
    planar: bool
    columns: list[ColumnDrawing] = field(default_factory=list)
    velocity: np.ndarray | None = None  # the tool point's velocity for dtheta, drawn head to tail
    arrow_scale: float = 1.0

Mesh dataclass

Polygons sharing a vertex array: vertices (V, 3), faces as lists of vertex indices, one colour per face (or one for all).

Source code in screws/viz/scenes.py 
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
@dataclass
class Mesh:
    """Polygons sharing a vertex array: vertices (V, 3), faces as lists of vertex indices,
    one colour per face (or one for all)."""

    vertices: np.ndarray
    faces: list
    colours: list | str = WOOD
    alpha: float = 1.0

    def face_colours(self) -> list:
        """One colour per face."""
        if isinstance(self.colours, str):
            return [self.colours] * len(self.faces)
        return list(self.colours)

    def transformed(self, T) -> Mesh:
        """The same mesh moved by the transform T."""
        T = np.asarray(T, float)
        return Mesh(self.vertices @ T[:3, :3].T + T[:3, 3], self.faces, self.colours, self.alpha)

face_colours 

face_colours()

One colour per face.

Source code in screws/viz/scenes.py 
37
38
39
40
41
def face_colours(self) -> list:
    """One colour per face."""
    if isinstance(self.colours, str):
        return [self.colours] * len(self.faces)
    return list(self.colours)

transformed 

transformed(T)

The same mesh moved by the transform T.

Source code in screws/viz/scenes.py 
43
44
45
46
def transformed(self, T) -> Mesh:
    """The same mesh moved by the transform T."""
    T = np.asarray(T, float)
    return Mesh(self.vertices @ T[:3, :3].T + T[:3, 3], self.faces, self.colours, self.alpha)

Scene dataclass

One screw motion, ready to animate. S is written in {s}; body meshes and points are in {b} coordinates; theta is (start, end) in radians (metres when S is a translation).

Source code in screws/viz/scenes.py 
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
@dataclass
class Scene:
    """One screw motion, ready to animate. S is written in {s}; body meshes and points are
    in {b} coordinates; theta is (start, end) in radians (metres when S is a translation)."""

    name: str
    S: np.ndarray
    T0: np.ndarray
    body: list
    theta: tuple
    points: np.ndarray
    fixed: list = field(default_factory=list)
    trace: int = 0
    arrow_scale: float = 0.5
    view: tuple = (24, 32)  # the notes' house view: x toward the lower left, y right, z up
    note: str = ""
    goal: np.ndarray | None = None  # where the motion ends, drawn faded with the start

    @property
    def S_b(self) -> np.ndarray:
        """The same screw axis in {b} at home: [Ad_{T_sb(0)^{-1}}] S_s."""
        return adjoint(transform_inv(self.T0)) @ self.S

    @property
    def is_translation(self) -> bool:
        return bool(np.allclose(self.S[:3], 0))

S_b property 

S_b

The same screw axis in {b} at home: [Ad_{T_sb(0)^{-1}}] S_s.

JointDrawing dataclass

What was drawn for one joint, and the numbers behind it (all in the drawing's frame).

Source code in screws/viz/static.py 
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
@dataclass
class JointDrawing:
    """What was drawn for one joint, and the numbers behind it (all in the drawing's frame)."""

    index: int
    kind: str
    omega: np.ndarray
    v: np.ndarray
    q: np.ndarray
    pitch: float
    cross: np.ndarray  # -omega x q
    pitch_term: np.ndarray  # h omega
    axis_line: object = None
    omega_arrow: object = None
    q_line: object = None
    cross_arrow: object = None
    pitch_arrow: object = None
    v_arrow: object = None

RobotDrawing dataclass

What draw_robot drew: the skeleton, and one JointDrawing per joint drawn (all in the drawing's frame).

Source code in screws/viz/static.py 
70
71
72
73
74
75
76
77
78
79
80
@dataclass
class RobotDrawing:
    """What draw_robot drew: the skeleton, and one JointDrawing per joint drawn (all in the
    drawing's frame)."""

    fig: object
    ax: object
    frame: str
    skeleton: np.ndarray  # (n + 2, 3): base, joint origins, tool, in the drawing's frame
    joints: list[JointDrawing] = field(default_factory=list)
    axes: str = "home"  # "home": the S_i (B_i); "current": the columns of J_s (J_b) at theta

ScrewDrawing dataclass

What draw_screw drew: the axis (omega, v), a point q on it, the pitch, and the two terms of v = -omega x q + h omega.

Source code in screws/viz/static.py 
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
@dataclass
class ScrewDrawing:
    """What draw_screw drew: the axis (omega, v), a point q on it, the pitch, and the two
    terms of v = -omega x q + h omega."""

    fig: object
    ax: object
    omega: np.ndarray
    v: np.ndarray
    q: np.ndarray
    pitch: float
    cross: np.ndarray
    pitch_term: np.ndarray
    axis_line: object = None
    omega_arrow: object = None
    q_line: object = None
    cross_arrow: object = None
    pitch_arrow: object = None
    v_arrow: object = None

animate_screw 

animate_screw(
    S,
    T0=None,
    *,
    body=None,
    points=None,
    theta=None,
    frames=80,
    fps=20,
    show_matrix=True,
    arrow_scale=None,
    figsize=None,
)

The screw axis S = (omega, v), written in {s}, swept over theta: the body at T(theta) = e^{[S] theta} T(0). Beside it the line read off S (q = omega x v, s-hat and h = omega . v for a unit omega) and, with show_matrix, T(theta) itself as theta grows. S may be a Scene. MR 3.3.3.2, eq. 3.85; notes 3.10.

Source code in screws/viz/animate.py 
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
def animate_screw(
    S,
    T0=None,
    *,
    body=None,
    points=None,
    theta=None,
    frames: int = 80,
    fps: int = 20,
    show_matrix: bool = True,
    arrow_scale: float | None = None,
    figsize=None,
) -> Animation:
    """The screw axis S = (omega, v), written in {s}, swept over theta: the body at
    T(theta) = e^{[S] theta} T(0). Beside it the line read off S (q = omega x v, s-hat and
    h = omega . v for a unit omega) and, with show_matrix, T(theta) itself as theta grows.
    S may be a Scene. MR 3.3.3.2, eq. 3.85; notes 3.10."""
    import matplotlib.pyplot as plt

    sc = _scene(S, T0, body, theta, points)
    th = np.linspace(sc.theta[0], sc.theta[1], frames)
    k = sc.arrow_scale if arrow_scale is None else arrow_scale
    painter = _Painter(sc, th, k)
    figsize = figsize or ((10.5, 5.6) if show_matrix else (6.4, 5.6))
    fig = plt.figure(figsize=figsize)
    if show_matrix:
        ax = fig.add_axes([0.0, 0.02, 0.58, 0.94], projection="3d")
        panel = fig.text(0.57, 0.5, "", family="monospace", fontsize=11.5, va="center")
    else:
        ax = fig.add_subplot(projection="3d")
        panel = None
    q, s_hat, h = painter.q, painter.s_hat, painter.h
    head = f"S = {_vec(sc.S)}\n"
    if q is None:
        head += f"ω = 0: no axis, a translation\nalong v̂ = ({_fmt(s_hat[0], 0)}, {_fmt(s_hat[1], 0)}, {_fmt(s_hat[2], 0)})\npitch h = ∞\n"
    else:
        head += (
            f"axis through q = ω × v = ({_fmt(q[0], 0)}, {_fmt(q[1], 0)}, {_fmt(q[2], 0)})\n"
            f"direction ŝ = ω = ({_fmt(s_hat[0], 0)}, {_fmt(s_hat[1], 0)}, {_fmt(s_hat[2], 0)})\n"
            f"pitch h = ωᵀv = {h:.3g}\n"
        )

    def update(i):
        painter.draw(ax, i, title=f"{sc.name}: {sc.note}" if sc.note else "T(θ) = e^[S]θ T(0)")
        if panel is not None:
            ang = f"{th[i]:5.2f} m" if sc.is_translation else f"{np.rad2deg(th[i]):6.1f}°"
            panel.set_text(
                head + f"\nθ = {ang}\n\nT(θ) = e^[S]θ T(0) =\n" + _matrix(painter.T[i], 7, 3)
            )

    return _finish(fig, update, frames, fps, {"theta": th, "T_sb": painter.T, "q": q, "s_hat": s_hat, "h": h})

animate_twist 

animate_twist(
    V,
    T0=None,
    *,
    body=None,
    points=None,
    duration=4.0,
    fps=20,
    arrow_scale=None,
    figsize=(6.4, 5.6),
)

A body moving under the constant twist V = (omega, v), written in {s}, for duration seconds: e^{[V] t} T0. Pass a Scene (door(), drawer(), screwdriver()) to play its motion in duration seconds, V = S thetadot. The screw axis is dashed with s-hat and the pitch, one point's path is traced in grey (a helix; a circle at zero pitch), and the purple arrows are the velocities omega x p + v of the marked points, drawn arrow_scale seconds long. Colours as everywhere in screws.viz: purple a velocity, red, green, blue a frame. MR 3.3.2; notes 3.10.

Source code in screws/viz/animate.py 
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
def animate_twist(
    V,
    T0=None,
    *,
    body=None,
    points=None,
    duration: float = 4.0,
    fps: int = 20,
    arrow_scale: float | None = None,
    figsize=(6.4, 5.6),
) -> Animation:
    """A body moving under the constant twist V = (omega, v), written in {s}, for duration
    seconds: e^{[V] t} T0. Pass a Scene (door(), drawer(), screwdriver()) to play its motion
    in duration seconds, V = S thetadot. The screw axis is dashed with s-hat and the pitch,
    one point's path is traced in grey (a helix; a circle at zero pitch), and the purple
    arrows are the velocities omega x p + v of the marked points, drawn arrow_scale seconds
    long. Colours as everywhere in screws.viz: purple a velocity, red, green, blue a frame.
    MR 3.3.2; notes 3.10."""
    import matplotlib.pyplot as plt

    if isinstance(V, Scene):
        sc = V
        rate = (sc.theta[1] - sc.theta[0]) / duration
        V = sc.S * rate
    else:
        sc = _scene(V, T0, body, (0.0, duration), points)
        rate = 1.0
        V = sc.S
    n = max(2, round(duration * fps))
    ts = np.linspace(0, duration, n)
    k = (sc.arrow_scale if arrow_scale is None else arrow_scale) / rate
    painter = _Painter(Scene(**{**sc.__dict__, "S": V, "theta": (0.0, duration)}), ts, k)
    fig = plt.figure(figsize=figsize)
    ax = fig.add_subplot(projection="3d")
    caption = fig.text(0.5, 0.03, "", ha="center", family="monospace", fontsize=9)
    name = sc.name if sc.name != "screw" else "a constant twist"

    def update(i):
        painter.draw(ax, i, title=f"{name}: V_s = {_vec(V)}")
        caption.set_text(f"t = {ts[i]:4.2f} s    arrows: ω × p + v, drawn {k:.2g} s long")

    return _finish(fig, update, n, fps, {"theta": ts, "T_sb": painter.T, "V_s": V})

compare_frames 

compare_frames(
    scene, *, frames=80, fps=20, view=None, figsize=(12, 8)
)

One motion drawn twice, per unit rate (thetadot = 1). Left, the space twist V_s: its v_s is the velocity of the body point at the {s} origin, the body imagined to fill all of space; the grey marker is the point that was there at theta = 0, carried off by the motion while the point now at the origin keeps the same velocity. Right, the body twist V_b: v_b is the velocity of the point at {b}'s origin, in {b}'s axes, the same at every theta. Below: V_s = [Ad_{T_sb(theta)}] V_b, Ad changing, V_s not. view is (elev, azim); the default looks down more steeply than the scene's own view, so the {s} origin and the axis separate. MR 3.3.2, eq. 3.83; notes 3.10.

Source code in screws/viz/animate.py 
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
def compare_frames(scene, *, frames: int = 80, fps: int = 20, view=None, figsize=(12, 8)) -> Animation:
    """One motion drawn twice, per unit rate (thetadot = 1). Left, the space twist V_s:
    its v_s is the velocity of the body point at the {s} origin, the body imagined to fill
    all of space; the grey marker is the point that was there at theta = 0, carried off
    by the motion while the point now at the origin keeps the same velocity. Right, the
    body twist V_b: v_b is the velocity of the point at {b}'s origin, in {b}'s axes, the
    same at every theta. Below: V_s = [Ad_{T_sb(theta)}] V_b, Ad changing, V_s not.
    view is (elev, azim); the default looks down more steeply than the scene's own view, so
    the {s} origin and the axis separate. MR 3.3.2, eq. 3.83; notes 3.10."""
    import matplotlib.pyplot as plt

    sc = scene if isinstance(scene, Scene) else _scene(scene, None, None, None, None)
    sc = Scene(**{**sc.__dict__, "view": view or (max(sc.view[0], 55), sc.view[1])})
    th = np.linspace(sc.theta[0], sc.theta[1], frames)
    painter = _Painter(sc, th, sc.arrow_scale)
    k = sc.arrow_scale
    S_b = sc.S_b
    Ads = np.array([adjoint(T) for T in painter.T])
    V_s = Ads @ S_b
    fig = plt.figure(figsize=figsize)
    axl = fig.add_axes([0.0, 0.3, 0.5, 0.65], projection="3d")
    axr = fig.add_axes([0.5, 0.3, 0.5, 0.65], projection="3d")
    eq = fig.text(0.5, 0.02, "", family="monospace", fontsize=11.5, ha="center", va="bottom")
    marker0 = np.zeros(3)  # the body point at the {s} origin at theta = 0, in {b} coordinates
    marker_b = (np.linalg.inv(sc.T0) @ np.append(marker0, 1))[:3]
    fwd = "rad" if not sc.is_translation else "m"

    def update(i):
        T = painter.T[i]
        painter.draw(axl, i, title="space twist V_s:\nv_s is the velocity of the body point at the {s} origin", points=False)
        m = T[:3, :3] @ marker_b + T[:3, 3]
        if painter.q is not None:  # the phantom arm: the axis to the marker, rigid with the body
            foot = painter.q + ((m - painter.q) @ painter.s_hat) * painter.s_hat
            axl.plot(*np.array([foot, m]).T, "--", color="grey", lw=1.2)
            foot0 = painter.q + ((0 - painter.q) @ painter.s_hat) * painter.s_hat
            axl.plot(*np.array([foot0, marker0]).T, ":", color=COLOURS["q"], lw=1.0)
        axl.scatter(*m, color="grey", s=25, depthshade=False)
        painter._arrow(axl, m, k * point_velocity(sc.S, m), "grey", 1.6)
        axl.scatter(0, 0, 0, color=COLOURS["v"], s=25, depthshade=False)
        painter._arrow(axl, marker0, k * V_s[i][3:], COLOURS["v"], 3.0)
        axl.text(*(k * V_s[i][3:] * 1.1 + [0, 0, 0.08 * painter.size]), "v_s", color=COLOURS["v"], fontsize=11, fontweight="bold")

        painter.draw(axr, i, title="body twist V_b:\nv_b is the velocity of the body point at the {b} origin", points=False, b_triad=1.6)
        vb_world = T[:3, :3] @ S_b[3:]
        painter._arrow(axr, T[:3, 3], k * vb_world, COLOURS["v"], 3.0)
        axr.text(*(T[:3, 3] + k * vb_world * 1.1 + [0, 0, 0.08 * painter.size]), "v_b", color=COLOURS["v"], fontsize=11, fontweight="bold")

        ang = f"{np.rad2deg(th[i]):5.1f}°" if fwd == "rad" else f"{th[i]:4.2f} m"
        left = ["V_s"] + [_fmt(x) for x in V_s[i]]
        mid = [f"[Ad_Tsb(θ)],  θ = {ang}"] + [" ".join(_fmt(x, 6, 2) for x in row) for row in Ads[i]]
        right = ["V_b"] + [_fmt(x) for x in S_b]
        rows = []
        for r in range(7):
            sep = " = " if r == 3 else "   "
            rows.append(f"{left[r]:>6}{sep}{mid[r]:^43}   {right[r]:>6}")
        eq.set_text("\n".join(rows) + f"\n(per unit rate θ̇ = 1; V_s and V_b stay fixed while the {sc.name} moves)")

    return _finish(
        fig,
        update,
        frames,
        fps,
        {"theta": th, "T_sb": painter.T, "Ad": Ads, "V_s": V_s, "V_b": np.tile(S_b, (frames, 1))},
    )

animate_robot 

animate_robot(
    robot,
    theta_start,
    theta_end=None,
    *,
    path="joint",
    frames=60,
    fps=20,
    show=SHOW_ANIMATION,
    kind="linear",
    force=False,
    chart=True,
    matrix=False,
    planar=False,
    scale=None,
    view=None,
    figsize=None,
)

The robot moving from theta_start to theta_end at a constant rate, its Jacobian and ellipsoid following, the ellipsoid's semi-axes charted below.

path="joint" (default) moves every joint at a constant rate, a straight line in joint space; path="screw" moves the tool at a constant twist, the screw motion from T_sb(theta_start) to T_sb(theta_end), the joints following by inverse kinematics seeded from the frame before. Or pass one (N, n) array of configurations as theta_start, such as a joint_trajectory or an IKResult.history, and it is played as given. show takes the items of draw_jacobian and draw_ellipsoid plus "path" (the tool's path) and "velocity" (the tool point's velocity, d/ds of its position). The chart plots the semi-axes against s from 0 to 1, so a singularity shows as one going to zero. matrix prints J_s(theta) beside the robot (J with planar=True, which draws a planar arm flat). .data holds s, theta, dtheta (per unit s), T_sb, J_s, J_b, V_s, V_b, tool, radii and measures for every frame. MR 5.4, 9.2; notes 5.5.

Source code in screws/viz/jacobian.py 
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
def animate_robot(
    robot: Robot,
    theta_start,
    theta_end=None,
    *,
    path: str = "joint",
    frames: int = 60,
    fps: int = 20,
    show=SHOW_ANIMATION,
    kind: str = "linear",
    force: bool = False,
    chart: bool = True,
    matrix: bool = False,
    planar: bool = False,
    scale=None,
    view=None,
    figsize=None,
) -> Animation:
    """The robot moving from theta_start to theta_end at a constant rate, its Jacobian and
    ellipsoid following, the ellipsoid's semi-axes charted below.

    path="joint" (default) moves every joint at a constant rate, a straight line in joint
    space; path="screw" moves the tool at a constant twist, the screw motion from
    T_sb(theta_start) to T_sb(theta_end), the joints following by inverse kinematics seeded
    from the frame before. Or pass one (N, n) array of configurations as theta_start, such
    as a joint_trajectory or an IKResult.history, and it is played as given. show takes the
    items of draw_jacobian and draw_ellipsoid plus "path" (the tool's path) and "velocity"
    (the tool point's velocity, d/ds of its position). The chart plots the semi-axes against
    s from 0 to 1, so a singularity shows as one going to zero. matrix prints J_s(theta)
    beside the robot (J with planar=True, which draws a planar arm flat). .data holds s,
    theta, dtheta (per unit s), T_sb, J_s, J_b, V_s, V_b, tool, radii and measures for every
    frame. MR 5.4, 9.2; notes 5.5.
    """
    import matplotlib.pyplot as plt

    if force and "force" not in show:
        show = tuple(show) + ("force",)
    s, thetas, dthetas = _path(robot, theta_start, theta_end, path, frames)
    painter = _RobotPainter(robot, thetas, planar=planar, kind=kind, show=show, dthetas=dthetas, scale=scale, view=view)
    data = _record(painter, s, dthetas)
    trace = np.array([p.tool for p in painter.poses])
    if figsize is None:
        figsize = (7.2 + (_matrix_inches(painter.poses[0].J) if matrix else 0), 7.6 if chart else 6.2)
    mw = min(7.2 / figsize[0], 0.7) if matrix else 1.0
    fig = plt.figure(figsize=figsize)
    top = (0.30 if painter.planar else 0.34) if chart else 0.08
    ax = painter.axes(fig, [0.0, top, mw, 0.96 - top])
    caption = fig.text(0.5 * mw, top - 0.03, "", ha="center", va="top", family="monospace", fontsize=9.5)
    cursor = None
    if chart:
        cax = fig.add_axes([0.11, 0.06, mw - 0.17, 0.13])
        units = _units(robot, kind, False)
        for k in range(data["radii"].shape[1]):
            cax.plot(s, data["radii"][:, k], color=MANIPULABILITY, lw=1.5, alpha=0.45 + 0.55 * k / max(1, data["radii"].shape[1] - 1))
        cax.set_xlim(0, 1)
        cax.set_ylim(0, 1.08 * max(float(data["radii"].max()), 1e-9))
        cax.set_xlabel("s, the fraction of the move", fontsize=9)
        cax.set_ylabel(f"semi-axes{units}", fontsize=9)
        cax.tick_params(labelsize=8)
        cursor = cax.axvline(0, color="black", lw=1.0)
    panel = None
    if matrix:
        name = r"$J(\theta)$" if painter.planar else r"$J_s(\theta)$"
        panel = _MatrixPanel(fig, mw, 0.62, painter.poses[0].J, name)
    how = {"joint": "constant joint rates", "screw": "constant twist of the tool"}
    title = f"{robot.name}: " + (how[path] if theta_end is not None else "a given path")

    def update(k):
        p = painter.poses[k]
        painter.draw(ax, k, title=title, trace=trace)
        rank, singular = _rank(p, painter.planar)
        caption.set_text(
            f"{_theta_text(robot, p.theta)}\n{_measures_text(p.manip)}"
            + (f"\nrank {'J' if painter.planar else 'J_s'} = {rank}, singular" if singular else "")
        )
        if cursor is not None:
            cursor.set_xdata([s[k], s[k]])
        if panel is not None:
            panel.update(p.J)

    return _finish(fig, update, len(s), fps, data)

compare_paths 

compare_paths(
    robot,
    theta_start,
    theta_end,
    *,
    frames=60,
    fps=20,
    show=("skeleton", "frames", "path", "velocity"),
    planar=False,
    view=None,
    figsize=(12.5, 7.6),
)

The same change of configuration made two ways, side by side. Left, constant joint rates: theta-dot is fixed and the tool's twist V_b = J_b(theta) theta-dot changes, so the tool does not follow a screw. Right, a constant twist: the tool follows the one screw motion from start to end (a helix about its axis) and the joint rates change, found by inverse kinematics. Each panel charts the quantity that changes; the caption gives the one held fixed. .data["joint"] and .data["screw"] hold each side's numbers, as animate_robot's. MR 9.2, eqs. 9.7 and 9.14; notes 3.10, 9.2.

Source code in screws/viz/jacobian.py 
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
def compare_paths(
    robot: Robot,
    theta_start,
    theta_end,
    *,
    frames: int = 60,
    fps: int = 20,
    show=("skeleton", "frames", "path", "velocity"),
    planar: bool = False,
    view=None,
    figsize=(12.5, 7.6),
) -> Animation:
    """The same change of configuration made two ways, side by side. Left, constant joint
    rates: theta-dot is fixed and the tool's twist V_b = J_b(theta) theta-dot changes, so
    the tool does not follow a screw. Right, a constant twist: the tool follows the one screw
    motion from start to end (a helix about its axis) and the joint rates change, found by
    inverse kinematics. Each panel charts the quantity that changes; the caption gives the
    one held fixed. .data["joint"] and .data["screw"] hold each side's numbers, as
    animate_robot's. MR 9.2, eqs. 9.7 and 9.14; notes 3.10, 9.2.
    """
    import matplotlib.pyplot as plt

    sj, thj, dj = _path(robot, theta_start, theta_end, "joint", frames)
    ss, ths, ds = _path(robot, theta_start, theta_end, "screw", frames)
    left = _RobotPainter(robot, thj, planar=planar, show=show, dthetas=dj, view=view)
    right = _RobotPainter(robot, ths, planar=planar, show=show, dthetas=ds, view=view)
    right.velocity_scale = left.velocity_scale = min(left.velocity_scale, right.velocity_scale)
    left.limits = right.limits = np.vstack([left._limits(), right._limits()])
    dl, dr = _record(left, sj, dj), _record(right, ss, ds)
    traces = [np.array([p.tool for p in pt.poses]) for pt in (left, right)]
    fig = plt.figure(figsize=figsize)
    axl = left.axes(fig, [0.0, 0.38, 0.5, 0.53])
    axr = right.axes(fig, [0.5, 0.38, 0.5, 0.53])
    cl = fig.add_axes([0.07, 0.11, 0.38, 0.14])
    cr = fig.add_axes([0.57, 0.11, 0.38, 0.14])
    names = [r"$\omega_1$", r"$\omega_2$", r"$\omega_3$", "$v_1$", "$v_2$", "$v_3$"]
    for j in range(6):  # a component along x-hat_b, y-hat_b or z-hat_b in that axis's colour; v dashed
        cl.plot(sj, dl["V_b"][:, j], "-" if j < 3 else "--", lw=1.4, label=names[j], color=COLOURS["xyz"[j % 3]])
    for i, colour in enumerate(_joint_shades(robot.n)):
        cr.plot(ss, ds[:, i], lw=1.4, label=f"$\\dot\\theta_{i + 1}$", color=colour)
    for c, ttl in ((cl, r"$\mathcal{V}_b(s)$, changing"), (cr, r"$\dot\theta(s)$, changing")):
        c.set_xlim(0, 1)
        c.set_title(ttl, fontsize=9.5)
        c.set_xlabel("s", fontsize=9)
        c.tick_params(labelsize=8)
        c.legend(fontsize=8, ncol=6, loc="upper center", bbox_to_anchor=(0.5, -0.3), frameon=False, handlelength=1.2)
    curs = [cl.axvline(0, color="black", lw=1.0), cr.axvline(0, color="black", lw=1.0)]
    vb = dr["V_b"][0]
    fixed_l = "dθ/ds = (" + ", ".join(_fmt(x, 0, 2) for x in dj[0]) + "), fixed"
    fixed_r = "V_b = (" + ", ".join(_fmt(x, 0, 2) for x in vb) + ") per unit s, fixed"
    fig.text(0.25, 0.335, fixed_l, ha="center", va="top", family="monospace", fontsize=9)
    fig.text(0.75, 0.335, fixed_r, ha="center", va="top", family="monospace", fontsize=9)

    def update(k):
        left.draw(axl, k, title="constant joint rates\nthe tool's twist changes", trace=traces[0])
        right.draw(axr, k, title="constant twist of the tool\nthe joint rates change", trace=traces[1])
        for c in curs:
            c.set_xdata([sj[k], sj[k]])

    data = {"s": sj, "theta": thj, "T_sb": dl["T_sb"], "joint": dl, "screw": dr}
    return _finish(fig, update, frames, fps, data)

draw_ellipsoid 

draw_ellipsoid(
    robot,
    theta,
    *,
    kind="linear",
    force=False,
    show=None,
    scale=None,
    force_scale=None,
    planar=False,
    ax=None,
    figsize=None,
    labels=True,
    view=None,
)

The robot at theta with its manipulability ellipsoid at the tool (in VELOCITY, its principal semi-axes drawn) and, with force=True, the force ellipsoid (grey, dashed).

kind="linear" is the velocity of the tool point, from J_b's linear rows as MR advises, turned into {s} so it sits where the tool is; kind="angular" is the angular velocity, from J_s's angular rows. scale is drawing metres per unit of semi-axis (m/s, or rad/s); the default makes the longest semi-axis 0.3 of the robot's reach. force_scale has its own default, the same rule for the force ellipsoid; pass force_scale=scale to draw both at one scale, as the notes do. Ellipsoids are drawn at their true size, and the frame does not grow to hold a long one: it runs out of the frame. A force ellipsoid at a singularity is unbounded along the lost direction, so it runs out on both sides, two lines in the plane or a cylinder in space; that axis is dotted and marked ∞. The title gives the semi-axes and mu1, mu2, mu3. For a chain mixing revolute and prismatic joints the semi-axes mix units and no unit is printed (notes 5.5). Every robot is drawn in 3D, where a planar arm's ellipsoid is flat and its mu are infinite; planar=True draws it flat, the ellipse of the notes' 2 x n J(theta). MR 5.4; notes 5.1, 5.5.

Source code in screws/viz/jacobian.py 
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
def draw_ellipsoid(
    robot: Robot,
    theta,
    *,
    kind: str = "linear",
    force: bool = False,
    show=None,
    scale=None,
    force_scale=None,
    planar: bool = False,
    ax=None,
    figsize=None,
    labels: bool = True,
    view=None,
) -> EllipsoidDrawing:
    """The robot at theta with its manipulability ellipsoid at the tool (in VELOCITY, its
    principal semi-axes drawn) and, with force=True, the force ellipsoid (grey, dashed).

    kind="linear" is the velocity of the tool point, from J_b's linear rows as MR advises,
    turned into {s} so it sits where the tool is; kind="angular" is the angular velocity,
    from J_s's angular rows. scale is drawing metres per unit of semi-axis (m/s, or rad/s);
    the default makes the longest semi-axis 0.3 of the robot's reach. force_scale has its own
    default, the same rule for the force ellipsoid; pass force_scale=scale to draw both at one
    scale, as the notes do. Ellipsoids are drawn at their true size, and the frame does not
    grow to hold a long one: it runs out of the frame. A force ellipsoid at a singularity is
    unbounded along the lost direction, so it runs out on both sides, two lines in the plane
    or a cylinder in space; that axis is dotted and marked ∞. The title gives the semi-axes
    and mu1, mu2, mu3. For a chain mixing revolute and prismatic joints the semi-axes mix
    units and no unit is printed (notes 5.5). Every robot is drawn in 3D, where a planar
    arm's ellipsoid is flat and its mu are infinite; planar=True draws it flat, the ellipse of
    the notes' 2 x n J(theta). MR 5.4; notes 5.1, 5.5.
    """
    import matplotlib.pyplot as plt

    theta = np.asarray(theta, float)
    if show is None:
        show = SHOW_ELLIPSOID + (("force",) if force else ())
    elif force and "force" not in show:
        show = tuple(show) + ("force",)
    painter = _RobotPainter(
        robot, [theta], planar=planar, kind=kind, show=show, scale=scale, force_scale=force_scale, labels=labels, view=view
    )
    p = painter.poses[0]
    if ax is None:
        fig = plt.figure(figsize=figsize or (7, 6.4))
        ax = painter.axes(fig)
    else:
        fig = ax.figure
    units = _units(robot, kind, False)
    title = f"{_theta_text(robot, theta)}\nsemi-axes {_radii_text(p.manip, units)}; {_measures_text(p.manip)}"
    if force:
        title += f"\nforce semi-axes {_radii_text(p.force, _units(robot, kind, True))}"
    painter.draw(ax, 0, title=title)
    clipped = painter._runs_out(p.manip, painter.scale)
    if force:
        clipped |= painter._runs_out(p.force, painter.force_scale)
    return EllipsoidDrawing(
        fig, ax, theta, p.T, kind, painter.planar, p.manip, p.force if force else None, painter.scale, painter.force_scale, clipped
    )

draw_jacobian 

draw_jacobian(
    robot,
    theta,
    *,
    frame="space",
    dtheta=None,
    show=SHOW_JACOBIAN,
    matrix=True,
    planar=False,
    ax=None,
    figsize=None,
    labels=True,
    view=None,
)

The robot at theta with each column of its Jacobian drawn as what it is.

Column i of J_s(theta) is joint i's screw axis now, S_i carried by the joints inboard of it; it is drawn as draw_robot(axes="current") draws it, a dashed line through the joint in the joints' blue (with its omega-hat for a revolute joint), labelled J_si. At the tool, an arrow per joint, numbered, gives the velocity of the tool point when that joint alone turns at unit rate, R_sb J_bv[:, i]. With dtheta the arrows are scaled by dtheta_i and laid head to tail, summing to the tool point's velocity p-dot. frame="body" draws in {b} and prints J_b; the lines are the same lines, seen from the tool. planar=True draws a planar arm flat: the arrows are then the columns J_i(theta) of the notes' 2 x n J(theta), which is the matrix printed, and the joint axes, all out of the page, are marked in the joint circles. The matrix is printed beside the robot (only when this function makes the figure). The title gives the rank, and says singular when it is below min(6, n) (min(2, n) in 2D). MR 5.1.1-5.1.2, 5.3; notes 5.1, 5.2, 5.4.

Source code in screws/viz/jacobian.py 
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
def draw_jacobian(
    robot: Robot,
    theta,
    *,
    frame: str = "space",
    dtheta=None,
    show=SHOW_JACOBIAN,
    matrix: bool = True,
    planar: bool = False,
    ax=None,
    figsize=None,
    labels: bool = True,
    view=None,
) -> JacobianDrawing:
    """The robot at theta with each column of its Jacobian drawn as what it is.

    Column i of J_s(theta) is joint i's screw axis now, S_i carried by the joints inboard of
    it; it is drawn as draw_robot(axes="current") draws it, a dashed line through the joint
    in the joints' blue (with its omega-hat for a revolute joint), labelled J_si. At the
    tool, an arrow per joint, numbered,
    gives the velocity of the tool point when that joint alone turns at unit rate,
    R_sb J_bv[:, i]. With dtheta the arrows are scaled by dtheta_i and laid head to tail,
    summing to the tool point's velocity p-dot. frame="body" draws in {b} and prints J_b;
    the lines are the same lines, seen from the tool. planar=True draws a planar arm flat:
    the arrows are then the columns J_i(theta) of the notes' 2 x n J(theta), which is the
    matrix printed, and the joint axes, all out of the page, are marked in the joint circles.
    The matrix is printed beside the robot (only when this function makes the figure). The
    title gives the rank, and says singular when it is below min(6, n) (min(2, n) in 2D).
    MR 5.1.1-5.1.2, 5.3; notes 5.1, 5.2, 5.4.
    """
    import matplotlib.pyplot as plt

    theta = np.asarray(theta, float)
    painter = _RobotPainter(
        robot, [theta], frame=frame, planar=planar, show=show, labels=labels, view=view,
        dthetas=None if dtheta is None else [np.asarray(dtheta, float)],
    )
    p = painter.poses[0]
    with_matrix = matrix and ax is None
    if ax is None:
        figsize = figsize or (7 + (_matrix_inches(p.J) if with_matrix else 0), 6.4)
        fig = plt.figure(figsize=figsize)
        split = min(7 / figsize[0], 0.7) if with_matrix else 1.0
        ax = painter.axes(fig, [0.0, 0.02, split, 0.94] if with_matrix else None)
    else:
        fig = ax.figure
    rank, singular = _rank(p, painter.planar)
    name = "J" if painter.planar else ("J_s" if frame == "space" else "J_b")
    title = f"{robot.name}: ${name}(\\theta)$, rank {rank}" + (", singular" if singular else "")
    painter.draw(ax, 0, title=title)
    if with_matrix:
        _MatrixPanel(fig, split, 0.5, p.J, f"${name}(\\theta)$")
        fig.text(split, 0.08, _theta_text(robot, theta), family="monospace", fontsize=10)
    columns = []
    for i, ((S, s_hat, h), q) in enumerate(zip(p.lines, p.joints)):
        cd = ColumnDrawing(i, robot.joint_types[i], np.asarray(S), q, s_hat, h, p.C[:, i])
        columns.append(cd)
    scale = painter.column_scale if dtheta is None else painter.velocity_scale
    return JacobianDrawing(fig, ax, frame, theta, p.T, p.J, rank, singular, painter.planar, columns, p.velocity, scale)

is_planar 

is_planar(robot)

True when the robot can be drawn flat with planar=True: every joint axis along z and every v in the xy plane (Robot.is_planar).

Source code in screws/viz/jacobian.py 
49
50
51
52
def is_planar(robot: Robot) -> bool:
    """True when the robot can be drawn flat with planar=True: every joint axis along z and
    every v in the xy plane (Robot.is_planar)."""
    return robot.is_planar()

box 

box(lo, hi, colours=WOOD, alpha=1.0)

An axis-aligned box from corner lo to corner hi.

Source code in screws/viz/scenes.py 
49
50
51
52
53
54
def box(lo, hi, colours=WOOD, alpha=1.0) -> Mesh:
    """An axis-aligned box from corner lo to corner hi."""
    (x0, y0, z0), (x1, y1, z1) = lo, hi
    v = np.array([[x, y, z] for z in (z0, z1) for y in (y0, y1) for x in (x0, x1)], float)
    faces = [[0, 1, 3, 2], [4, 5, 7, 6], [0, 1, 5, 4], [2, 3, 7, 6], [0, 2, 6, 4], [1, 3, 7, 5]]
    return Mesh(v, faces, colours, alpha)

door 

door()

The door of notes 3.10: a vertical hinge through q = (2, 0, 0), {b} on the handle at (2.9, 0, 1), S_s = (0, 0, 1, 0, -2, 0), S_b = (0, 0, 1, 0, 0.9, 0), opened 0 to 100 deg.

Source code in screws/viz/scenes.py 
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
def door() -> Scene:
    """The door of notes 3.10: a vertical hinge through q = (2, 0, 0), {b} on the handle at
    (2.9, 0, 1), S_s = (0, 0, 1, 0, -2, 0), S_b = (0, 0, 1, 0, 0.9, 0), opened 0 to 100 deg."""
    T0 = rp_to_transform(np.eye(3), [2.9, 0, 1])
    slab = box([2.0, -0.025, 0.0], [3.1, 0.025, 2.0], WOOD)
    knob = box([2.86, 0.025, 0.97], [2.94, 0.09, 1.03], HANDLE)
    wall = [
        box([1.3, -0.1, 0.0], [1.98, -0.03, 2.3], WALL, 0.35),
        box([3.12, -0.1, 0.0], [3.6, -0.03, 2.3], WALL, 0.35),
    ]
    pts_s = np.array([[2.9, 0, 1], [3.1, 0, 2.0], [2.45, 0, 0.3]])
    return Scene(
        "door",
        np.array([0, 0, 1, 0, -2, 0.0]),
        T0,
        _to_body(T0, [slab, knob]),
        (0.0, np.deg2rad(100)),
        pts_s - T0[:3, 3],
        fixed=wall,
        arrow_scale=0.5,
        note="hinge through q = (2, 0, 0), pitch 0",
    )

drawer 

drawer()

A drawer sliding 0.45 m out of a cabinet along +y: omega = 0, infinite pitch, every point with the same velocity.

Source code in screws/viz/scenes.py 
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
def drawer() -> Scene:
    """A drawer sliding 0.45 m out of a cabinet along +y: omega = 0, infinite pitch, every
    point with the same velocity."""
    T0 = rp_to_transform(np.eye(3), [0.5, 0.07, 1.125])
    tray = box([0.08, -1.0, 0.95], [0.92, 0.03, 1.3], WOOD)
    pull = box([0.4, 0.03, 1.1], [0.6, 0.07, 1.15], HANDLE)
    cabinet = box([0.0, -1.1, 0.0], [1.0, 0.0, 1.4], WALL, 0.25)
    pts_s = np.array([[0.5, 0.07, 1.125], [0.08, 0.03, 0.95], [0.92, -1.0, 1.3]])
    return Scene(
        "drawer",
        np.array([0, 0, 0, 0, 1, 0.0]),
        T0,
        _to_body(T0, [tray, pull]),
        (0.0, 0.45),
        pts_s - T0[:3, 3],
        fixed=[cabinet],
        arrow_scale=0.5,
        note="a translation, pitch infinite",
    )

prism 

prism(
    base,
    axis,
    radius,
    length,
    sides=6,
    colours=STEEL,
    alpha=1.0,
)

A right prism with a regular polygon section: from base along the unit axis.

Source code in screws/viz/scenes.py 
57
58
59
60
61
62
63
64
65
66
67
68
69
def prism(base, axis, radius, length, sides=6, colours=STEEL, alpha=1.0) -> Mesh:
    """A right prism with a regular polygon section: from base along the unit axis."""
    base, axis = np.asarray(base, float), np.asarray(axis, float)
    axis = axis / np.linalg.norm(axis)
    e1 = np.cross(axis, [1.0, 0, 0] if abs(axis[0]) < 0.9 else [0, 1.0, 0])
    e1 /= np.linalg.norm(e1)
    e2 = np.cross(axis, e1)
    ang = 2 * np.pi * np.arange(sides) / sides
    ring = radius * (np.outer(np.cos(ang), e1) + np.outer(np.sin(ang), e2))
    v = np.vstack([base + ring, base + length * axis + ring])
    faces = [[k, (k + 1) % sides, sides + (k + 1) % sides, sides + k] for k in range(sides)]
    faces += [list(range(sides)), list(range(sides, 2 * sides))]
    return Mesh(v, faces, colours, alpha)

screw_between 

screw_between(
    T_start, T_end, *, body=None, name="screw motion"
)

The one screw motion that carries the frame T_start to T_end (both in {s}): from [S] theta = log(T_end T_start^{-1}), with S a unit screw axis (|omega| = 1, or |v| = 1 for a translation) and theta the angle turned (the distance slid). Every rigid-body displacement is a screw motion (Chasles); this shows which one. The frame's origin traces the helix, and the start and end frames are drawn faded. MR 3.3.3.2; notes 3.7.

Source code in screws/viz/scenes.py 
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
def screw_between(T_start, T_end, *, body=None, name: str = "screw motion") -> Scene:
    """The one screw motion that carries the frame T_start to T_end (both in {s}): from
    [S] theta = log(T_end T_start^{-1}), with S a unit screw axis (|omega| = 1, or |v| = 1
    for a translation) and theta the angle turned (the distance slid). Every rigid-body
    displacement is a screw motion (Chasles); this shows which one. The frame's origin
    traces the helix, and the start and end frames are drawn faded. MR 3.3.3.2; notes 3.7.
    """
    T_start, T_end = np.asarray(T_start, float), np.asarray(T_end, float)
    S, theta = axis_angle6(se3_to_vec(log6(T_end @ transform_inv(T_start))))
    reach = float(np.linalg.norm(T_end[:3, 3] - T_start[:3, 3])) + 0.3
    a = 0.12 * reach  # a phone-shaped slab, sized to the motion
    if body is None:
        body = [box([-0.5 * a, -a, -0.08 * a], [0.5 * a, a, 0.08 * a], "#3B83CD")]
    points = np.array([[0.0, 0, 0], [0.5 * a, a, 0.08 * a], [-0.5 * a, -a, 0.08 * a]])
    _, _, h = screw_line(S)
    turned = f"{theta:.3g} m slid" if np.allclose(S[:3], 0) else f"{np.rad2deg(theta):.0f}° turned"
    return Scene(
        name,
        S,
        T_start,
        list(body),
        (0.0, theta),
        points,
        arrow_scale=0.15 * reach / max(theta, 1e-9),
        note=f"{turned}, pitch h = {h:.3g}",
        goal=T_end,
    )

screwdriver 

screwdriver(pitch=0.01)

A screwdriver driving down: s_hat = -z through the origin, two turns. The pitch, 0.01 m/rad by default (6.3 cm per turn), is exaggerated so the advance shows.

Source code in screws/viz/scenes.py 
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
def screwdriver(pitch: float = 0.01) -> Scene:
    """A screwdriver driving down: s_hat = -z through the origin, two turns. The pitch,
    0.01 m/rad by default (6.3 cm per turn), is exaggerated so the advance shows."""
    T0 = np.eye(4)
    T0[:3, 3] = [0, 0, 0.13]  # {b} at the tip
    shaft = prism([0, 0, 0.13], [0, 0, 1], 0.006, 0.15, 8, STEEL)
    grip = prism([0, 0, 0.28], [0, 0, 1], 0.024, 0.12, 6, [GRIP] + [HANDLE] * 5 + [HANDLE, HANDLE])
    board = box([-0.12, -0.12, -0.03], [0.12, 0.12, 0.0], WALL, 0.5)
    pts_s = np.array([[0, 0.024, 0.395], [0, 0, 0.13], [-0.0207, 0.012, 0.3]])  # on the grip, the tip
    return Scene(
        "screwdriver",
        screw_axis([0, 0, 0], [0, 0, -1], pitch),
        T0,
        _to_body(T0, [shaft, grip]),
        (0.0, 4 * np.pi),
        pts_s - T0[:3, 3],
        fixed=[board],
        arrow_scale=2.5,
        view=(30, 32),
        note=f"pitch {pitch:g} m/rad, exaggerated",
    )

draw_robot 

draw_robot(
    robot,
    theta=None,
    *,
    frame="space",
    axes="home",
    joints=None,
    show=SHOW_DEFAULT,
    ax=None,
    axis_length=None,
    figsize=(8, 7),
    labels=True,
)

Draw a robot's skeleton at theta (default: home) with one screw axis per joint.

axes="home" (default) draws the axes at home whatever theta is: the S_i in {s} (frame="space"), or the B_i in {b} (frame="body"), where {b} at home sits at the origin and the base is wherever M^{-1} puts it. axes="current" draws each axis where the joints inboard of it have carried it at theta: the columns of J_s(theta) in {s}, or of J_b(theta) in {b} at theta. At home the two are the same picture. draw_jacobian is the current one with the velocities the joints give the tool and the matrix beside it.

The skeleton joins the joints' positions when the robot knows them (a URDF, a scene, or joint_frames_home); otherwise only the base and the tool are marked. joints selects which joints get axes (indices from 0; default all). show is any of "skeleton", "frames", "axes" (the axis lines), "omega" (the omega arrows at q_i), "construction" (q_i, -omega_i x q_i, h_i omega_i and v_i at the origin; with axes="current", J_s built column by column, J_si = (omega_i, -omega_i x q_i) at zero pitch, MR 5.1.1). Returns a RobotDrawing whose joints carry the vectors drawn, for checking by hand. MR 4.1.1, 4.1.3, 5.1; notes 4.1, 4.2, 5.1.

Source code in screws/viz/static.py 
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
def draw_robot(
    robot: Robot,
    theta=None,
    *,
    frame: str = "space",
    axes: str = "home",
    joints=None,
    show=SHOW_DEFAULT,
    ax=None,
    axis_length: float | None = None,
    figsize=(8, 7),
    labels: bool = True,
) -> RobotDrawing:
    """Draw a robot's skeleton at theta (default: home) with one screw axis per joint.

    axes="home" (default) draws the axes at home whatever theta is: the S_i in {s}
    (frame="space"), or the B_i in {b} (frame="body"), where {b} at home sits at the origin
    and the base is wherever M^{-1} puts it. axes="current" draws each axis where the joints
    inboard of it have carried it at theta: the columns of J_s(theta) in {s}, or of
    J_b(theta) in {b} at theta. At home the two are the same picture. draw_jacobian is the
    current one with the velocities the joints give the tool and the matrix beside it.

    The skeleton joins the joints' positions when the robot knows them (a URDF, a scene, or
    joint_frames_home); otherwise only the base and the tool are marked. joints selects
    which joints get axes (indices from 0; default all). show is any of "skeleton",
    "frames", "axes" (the axis lines), "omega" (the omega arrows at q_i), "construction"
    (q_i, -omega_i x q_i, h_i omega_i and v_i at the origin; with axes="current", J_s built
    column by column, J_si = (omega_i, -omega_i x q_i) at zero pitch, MR 5.1.1). Returns a
    RobotDrawing whose joints carry the vectors drawn, for checking by hand.
    MR 4.1.1, 4.1.3, 5.1; notes 4.1, 4.2, 5.1.
    """
    theta = np.zeros(robot.n) if theta is None else np.asarray(theta, float)
    T_ref, axes_mat, origins = _screw_axes(robot, theta, frame, axes)
    fig, ax = _axes(ax, figsize)
    known = robot.joint_frames_home is not None
    posed = [T_ref @ F for F in robot.frames(theta)]  # joint frames then the tool, at theta
    if known:
        skeleton = np.array([T_ref[:3, 3]] + [F[:3, 3] for F in posed])
    else:  # joint positions unknown: only the base and the tool, no made-up links
        skeleton = np.array([T_ref[:3, 3], posed[-1][:3, 3]])
    joints_sel = list(range(robot.n)) if joints is None else list(joints)
    reach = max(float(np.linalg.norm(p)) for p in skeleton) or 1.0
    L = axis_length if axis_length is not None else 0.35 * reach
    sub = "s" if frame == "space" else "b"

    if "skeleton" in show:
        if known:
            ax.plot(skeleton[:, 0], skeleton[:, 1], skeleton[:, 2], "-", color=COLOURS["skeleton"], lw=3)
            ax.plot(skeleton[1:-1, 0], skeleton[1:-1, 1], skeleton[1:-1, 2], "o", color=COLOURS["joint"], ms=7)
        ax.plot(skeleton[-1:, 0], skeleton[-1:, 1], skeleton[-1:, 2], "s", color="black", ms=6)
    if "frames" in show:
        _triad(ax, np.eye(4), 0.25 * reach, f"{{{sub}}}")
        if frame == "body":
            _triad(ax, T_ref, 0.18 * reach, "{s}")
        else:
            _triad(ax, robot.M if axes == "home" else posed[-1], 0.18 * reach, "{b} at home" if axes == "home" else "{b}")

    drawing = RobotDrawing(fig, ax, frame, skeleton, axes=axes)
    extents = [skeleton]
    for i in joints_sel:
        omega, v = axes_mat[:3, i].copy(), axes_mat[3:, i].copy()
        kind = robot.joint_types[i]
        q_near, h, cross, pitch_term = _decompose(omega, v)
        q = origins[i].copy() if kind == "revolute" else q_near
        if kind == "revolute":  # the joint's own origin is on the axis; rebuild the pieces from it
            cross = -np.cross(omega, q)
            pitch_term = h * omega
        jd = JointDrawing(i, kind, omega, v, q, h, cross, pitch_term)
        direction = omega if kind == "revolute" else v / np.linalg.norm(v)
        anchor = q if kind == "revolute" else origins[i]
        k = i + 1
        if axes == "home":
            name = (r"$\mathcal{S}" if frame == "space" else r"$\mathcal{B}") + f"_{{{k}}}$"
        else:
            name = f"$J_{{{sub}{k}}}$"
        jd.axis_line, jd.omega_arrow, ends = _axis(
            ax,
            anchor,
            direction,
            L,
            line="axes" in show,
            arrow="omega" in show and kind == "revolute",
            label=name if labels and "axes" in show else None,
        )
        if "axes" in show:
            extents.append(ends)
        if "construction" in show:
            o = np.zeros(3)
            if kind == "revolute":
                (jd.q_line,) = ax.plot([0, q[0]], [0, q[1]], [0, q[2]], ":", color=COLOURS["q"], lw=1.5)
                ax.text(*(q / 2), f"$q_{{{k}}}$", color=COLOURS["q"], fontsize=10)
                if near_zero(h):  # v is the cross product itself: one arrow, labelled as both
                    jd.v_arrow = _arrow(ax, o, v, COLOURS["v"], lw=2.5)
                    if labels:
                        ax.text(*(v + 0.02 * reach), rf"$v_{{{k}}} = -\omega_{{{k}}} \times q_{{{k}}}$", color=COLOURS["v"], fontsize=10)
                else:
                    jd.cross_arrow = _arrow(ax, o, cross, COLOURS["cross"], lw=2.0)
                    jd.pitch_arrow = _arrow(ax, o + cross, pitch_term, COLOURS["pitch"], lw=2.0)
                    jd.v_arrow = _arrow(ax, o, v, COLOURS["v"], lw=2.5)
                    if labels:
                        ax.text(*(cross * 0.5 + 0.02 * reach), rf"$-\omega_{{{k}}} \times q_{{{k}}}$", color=COLOURS["cross"], fontsize=10)
                        ax.text(*(cross + 0.5 * pitch_term), rf"$h_{{{k}}}\omega_{{{k}}}$", color=COLOURS["pitch"], fontsize=10)
                        ax.text(*(v + 0.02 * reach), f"$v_{{{k}}}$", color=COLOURS["v"], fontsize=10)
                extents.append(np.array([cross, v]))
            else:
                jd.v_arrow = _arrow(ax, anchor, 0.6 * L * v, COLOURS["v"], lw=2.5)
                if labels:
                    ax.text(*(anchor + 0.6 * L * v), f"$v_{{{k}}}$", color=COLOURS["v"], fontsize=10)
        drawing.joints.append(jd)
    _set_equal(ax, np.vstack(extents))
    ax.view_init(elev=22, azim=-58)
    if axes == "home":
        ax.set_title(f"screw axes in {{{sub}}} at home" + ("" if not np.any(theta) else ", arm at θ"))
    else:
        ax.set_title(f"screw axes in {{{sub}}} at θ: the columns of $J_{sub}(\\theta)$")
    return drawing

draw_screw 

draw_screw(S, *, ax=None, figsize=(6, 6), labels=True)

One screw axis S = (omega, v) on its own: the axis, omega at q, and how v is built from q (the point on the axis nearest the origin), -omega x q and the pitch term h omega. MR 3.3.2.2, eq. 3.77; notes 3.5.

Source code in screws/viz/static.py 
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
def draw_screw(S, *, ax=None, figsize=(6, 6), labels: bool = True) -> ScrewDrawing:
    """One screw axis S = (omega, v) on its own: the axis, omega at q, and how v is built
    from q (the point on the axis nearest the origin), -omega x q and the pitch term h omega.
    MR 3.3.2.2, eq. 3.77; notes 3.5.
    """
    S = np.asarray(S, float)
    omega, v = S[:3].copy(), S[3:].copy()
    q, h, cross, pitch_term = _decompose(omega, v)
    fig, ax = _axes(ax, figsize)
    d = ScrewDrawing(fig, ax, omega, v, q, h, cross, pitch_term)
    o = np.zeros(3)
    _triad(ax, np.eye(4), 0.25 * max(0.3, float(np.linalg.norm(q)), float(np.linalg.norm(v))), "{s}")
    if near_zero(np.linalg.norm(omega)):
        d.axis_line = ax.plot([0, v[0]], [0, v[1]], [0, v[2]], "--", color=COLOURS["axis"], lw=1.4)[0]
        d.v_arrow = _arrow(ax, o, v, COLOURS["v"], lw=2.5)
        pts = np.array([o, v])
    else:
        size = max(0.3, float(np.linalg.norm(q)), float(np.linalg.norm(v)))
        d.axis_line, d.omega_arrow, ends = _axis(ax, q, omega, 0.8 * size, label=r"$\hat\omega$" if labels else None)
        (d.q_line,) = ax.plot([0, q[0]], [0, q[1]], [0, q[2]], ":", color=COLOURS["q"], lw=1.5)
        if near_zero(h):
            d.v_arrow = _arrow(ax, o, v, COLOURS["v"], lw=2.5)
        else:
            d.cross_arrow = _arrow(ax, o, cross, COLOURS["cross"], lw=2.0)
            d.pitch_arrow = _arrow(ax, o + cross, pitch_term, COLOURS["pitch"], lw=2.0)
            d.v_arrow = _arrow(ax, o, v, COLOURS["v"], lw=2.5)
        pts = np.vstack([ends, [o, q, cross, v]])
        if labels:
            ax.text(*(q / 2), "$q$", color=COLOURS["q"])
            if near_zero(h):
                ax.text(*v, r"$v = -\hat\omega \times q$", color=COLOURS["v"])
            else:
                ax.text(*(cross * 0.5), r"$-\hat\omega \times q$", color=COLOURS["cross"])
                ax.text(*(cross + 0.5 * pitch_term), r"$h\hat\omega$", color=COLOURS["pitch"])
                ax.text(*v, "$v$", color=COLOURS["v"])
    _set_equal(ax, pts, pad=0.05)
    ax.set_title(f"screw axis (omega, v), pitch h = {h:.3g}" if np.isfinite(h) else "screw axis, infinite pitch")
    return d

explore 

explore(robot, *, draw=None, **kwargs)

Joint sliders in a notebook: redraws a drawing at the slider values, by default draw_robot; pass draw=viz.draw_jacobian or viz.draw_ellipsoid to watch the Jacobian or the ellipsoids change instead. kwargs go to the drawing (frame, show, force, ...). Ranges come from joint_limits when the robot has them. Needs ipywidgets.

Source code in screws/viz/static.py 
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
def explore(robot: Robot, *, draw=None, **kwargs):
    """Joint sliders in a notebook: redraws a drawing at the slider values, by default
    draw_robot; pass draw=viz.draw_jacobian or viz.draw_ellipsoid to watch the Jacobian or
    the ellipsoids change instead. kwargs go to the drawing (frame, show, force, ...).
    Ranges come from joint_limits when the robot has them. Needs ipywidgets."""
    import ipywidgets as widgets
    import matplotlib.pyplot as plt
    from IPython.display import display

    draw = draw_robot if draw is None else draw
    sliders = [
        widgets.FloatSlider(value=min(max(0.0, lo), hi), min=lo, max=hi, step=0.01, description=f"θ{i + 1}", readout_format=".2f")
        for i, (lo, hi) in enumerate(_slider_ranges(robot))
    ]
    out = widgets.Output()

    def redraw(*_):
        with out:
            out.clear_output(wait=True)
            d = draw(robot, [s.value for s in sliders], **kwargs)
            display(d.fig)  # into the Output widget; plt.show takes no figure
            plt.close(d.fig)

    for s in sliders:
        s.observe(redraw, names="value")
    display(widgets.VBox(sliders), out)
    redraw()
    return sliders