screws

Screw-theory robotics in Python, after Lynch and Park, Modern Robotics (MR). screws is the MR code library reorganised: the same mathematics under snake_case names, MR's own names kept as aliases, a Robot class, a URDF loader, robots that ship ready to use, and a bridge to CoppeliaSim.

Install

One or the other:

uv add screws                 # the mathematics only; numpy is the sole dependency
uv add "screws[coppelia]"     # the same, plus the CoppeliaSim ZMQ remote API client

Extras for the bridge: screws[plot] (matplotlib, for Log.plot) and screws[video] (imageio and ffmpeg, for Recorder). uv add "screws[coppelia,plot,video]" takes all three.

Four lines

import screws

ur5 = screws.robots.ur5()                          # M, screw axes, inertias from the textbook's URDF
T = ur5.fk([0.3, -1.2, 0.8, -0.4, 1.1, 0.2])         # a reachable, non-singular pose
result = ur5.ik(T, theta0=[0.1, -1.4, 0.1, 0.1, 1.4, 0.1])   # result.theta, result.converged, result.history

The free functions are the reference implementation and read as the book does: screws.exp6(screws.vec_to_se3(S * theta)) is $e^{[\mathcal{S}]\theta}$, screws.fk_space(M, S, theta) is the space form of the product of exponentials, and screws.FKinSpace is the very same function under MR's name.

Screw-axis lists are 6xn, one axis per column, as MR writes them. Nothing guesses the orientation (a 6x6 array is ambiguous for a six-axis arm), so hand entry goes through a sequence of 6-vectors: screws.Robot.from_screw_axes(M, [S1, S2, ...]).

Check your own code against the library

import numpy as np
import screws

def my_exp6(se3mat): ...                          # your implementation

rng = np.random.default_rng(0)
cases = [screws.vec_to_se3(rng.normal(size=6)) for _ in range(20)]
screws.testing.check(my_exp6, screws.exp6, cases)  # raises on the first disagreement

Dynamics and trajectories

import numpy as np
import screws

ur5 = screws.robots.ur5()                          # link frames and inertias from the URDF
theta = np.zeros(6)
tau_g = ur5.gravity_forces(theta)                  # joint torques that hold the arm still
M = ur5.mass_matrix(theta)                         # 6x6, symmetric positive definite
tau = ur5.inverse_dynamics(theta, np.zeros(6), np.ones(6) * 0.1)   # + optional F_tip
ddtheta = ur5.forward_dynamics(theta, np.zeros(6), tau)
theta_ref = screws.joint_trajectory(theta, theta + 0.5, T_final=2.0, N=41)   # 41 rows, quintic

inverse_dynamics, mass_matrix, velocity_quadratic_forces, gravity_forces, end_effector_forces, forward_dynamics, euler_step and the two *_trajectory functions are MR chapter 8 as free functions too, with the robot's link_frames, link_inertias and S passed in. joint_trajectory, screw_trajectory and cartesian_trajectory take scaling="quintic" (default) or "cubic". A Robot without inertias raises MissingInertias from every dynamics method.

CoppeliaSim

Works with CoppeliaSim 4.9 or later through the ZMQ remote API (0.1 verified on 4.10.0).

from screws.coppelia import Scene

with Scene() as scene:                 # connects to localhost:23000 in stepping mode
    arm = scene.arm("/UR5")            # the joints under that tree, base to tip
    robot = arm.robot()                # a screws.Robot read off the scene at zero
    arm.mode("position")
    log = scene.run(lambda t, theta, dtheta: theta_desired(t), duration=5.0, arm=arm)
log.plot()                             # four stacked time plots: theta, dtheta, tau, command
log.to_csv("run.csv")

Record a movie. One frame per simulation step, so the movie plays at simulated time however slowly the controller ran:

with Scene() as scene:
    arm = scene.arm("/UR5")
    arm.mode("position")
    with scene.record_video("run.mp4", position=(1.5, -1.5, 1.0), look_at=(0, 0, 0.4)):
        scene.run(controller, duration=5.0, arm=arm)

record_video creates a camera (or reuses one: camera="/Camera"), grabs its image before every step (every=2 halves the frame rate), and on exit writes .mp4 or .gif.

In a notebook, release the clock between cells. In stepping mode CoppeliaSim waits for the next step(), and while it waits its window stops redrawing; after a few seconds it shows a "ZMQ remote API server ... abort execution" banner, and clicking that kills the remote API and strands the notebook. End a cell that stepped with scene.release(): the simulation then runs on its own (position targets hold the arm still) and the window stays live. The next scene.step() takes the clock back.

Read the scene's masses. arm.robot(inertias=True) reads every dynamic shape's mass and inertia and builds the MR link frames and spatial inertias, so a model-based controller can be run against the simulator's own numbers, and against the textbook's URDF numbers, and the difference explained. relative_to="base" makes {s} the model's base frame.

Scene owns the connection and the clock (start, step, release, stop, time, dt, frame, show_frame). Scene.run records every step into a Log (t, theta, dtheta, tau, command, T_sb as arrays); Log.plot() draws one time-series axis per joint quantity and shows the figure, Log.to_csv writes it out. Arm reads (theta, dtheta, tau, tip_frame) and commands in one of three modes (position, velocity, torque), or teleports without physics to animate an IK history. Arm.robot() derives M and the screw axes from the scene's joint frames (omega is the joint's z axis, v = -omega x q).

Pictures of the screw axes

import screws
from screws import viz

ur5 = screws.robots.ur5()
viz.draw_robot(ur5)                                   # skeleton, {s}, and S_1 ... S_6 at home
viz.draw_robot(ur5, frame="body")                     # the same robot seen from {b}: B_1 ... B_6
viz.draw_robot(ur5, joints=[2], show=("skeleton", "frames", "axes", "omega", "construction"))
viz.draw_screw(screws.screw_axis(q=[0.3, 0, 0], s_hat=[0, 0, 1], h=0.05))   # one axis by itself
viz.explore(ur5)                                      # joint sliders, in a notebook

draw_robot draws the robot's skeleton at theta (default: home), the reference frame, and one screw axis per joint through the joint's home position with its ω̂ arrow. With "construction" it also draws, for each chosen joint, the point q on the axis, the cross product −ω̂×q, the pitch term hω̂, and their sum v at the origin, so the formula v = −ω̂×q + hω̂ is something you can look at. The body-frame view redraws everything in {b}, which is why the B_i differ from the S_i. Every drawing returns the vectors it drew, so a figure can be checked by hand. Needs screws[plot].

Watching a screw motion

from screws import viz

viz.animate_screw(viz.door())          # the door swung about its hinge, with T(θ) = e^[S]θ T(0) beside it
viz.animate_twist(viz.screwdriver())   # the screwdriver under a constant twist: a helix about -z
viz.compare_frames(viz.door())         # V_s beside V_b, and V_s = [Ad_Tsb] V_b underneath
viz.animate_screw(viz.screw_between(T_start, T_end))   # the one screw motion from one frame to another
viz.animate_twist([0, 0, 1, 0, -0.5, 0.1], duration=4).save("mine.gif")   # any twist, any body

A twist is a motion before it is a six-vector. animate_twist plays a constant twist on a body: the screw axis drawn as a line with ŝ and the pitch h, one point's path traced (a helix, or a circle at zero pitch), and red arrows for the velocities ω × p + v of a few points. animate_screw sweeps a screw axis over θ and prints the line read off it, q = ω × v and h = ωᵀv, with T(θ) changing alongside.

Three pitches. The scenes viz.door(), viz.drawer() and viz.screwdriver() are the three motions of the notes' chapter 3 review: zero, infinite and finite pitch.

Space and body twists. compare_frames draws one motion twice. On the left, v_s is the velocity of the body point at the {s} origin: the grey marker is the point that was there at θ = 0, carried off by the motion while the point now at the origin keeps the same velocity. On the right, v_b is the velocity of the point at {b}. Under them, V_s = [Ad_Tsb(θ)] V_b, the adjoint changing and V_s fixed.

animate_twist plays the same door in time, V_s = S θ̇:

From one frame to another. Every rigid-body displacement is a screw motion (Chasles). viz.screw_between(T_start, T_end) finds the one that carries T_start to T_end, from [S]θ = log(T_end T_start⁻¹) with S a unit screw axis, and animate_screw shows the frame travelling along it, its origin tracing the helix, the start and end frames faded:

Each call returns an Animation: .save("x.gif") (or .mp4 with screws[video]), it plays inline in a notebook, and .data holds the numbers drawn. viz.Scene, viz.box and viz.prism build your own body. The geometry is screws.screw_line, screw_motion, point_velocity and helix, numpy only. examples/screw_motions.py writes every GIF here.

An example: putting

Full-resolution movie (mp4), recorded by the example itself with screws.coppelia.Recorder.

examples/putt.py builds a green in a running CoppeliaSim (turf with a real cup cut through it, a pin and flag, a ball, and a putter on the UR5's flange), drives the arm in kinematic mode, addresses the ball, swings the putter through it like a pendulum at a chosen face speed, returns to the address pose, and reports whether the ball dropped. It records the putt at 1080p and, given PNGs, lays a seal and a logo on the green and texts the flag:

uv run python examples/putt.py --video putt.mp4 --flag-text SMU --seal seal.png --logo logo.png

The pieces are in screws.coppelia.golf: build_green, attach_putter (whose .robot() moves M from the flange to the putter face), address_pose, swing_path, putt, and the image helpers text_image, seal_image and logo_image. Vary the speed, the hole position or the ball's friction and see what the physics does. Two lessons the example encodes: the stock model's demo script is removed when the bridge takes the arm (it would fight you), and a tool that must strike repeatably runs the arm in arm.mode("kinematic").

Two UR5s

screws.robots.ur5() is built from the URDF the textbook prints in section 4.2, whose lengths are the manufacturer's to a tenth of a millimetre (89.159, 135.85, 425, 119.7, 392.25, 93, 94.65 and 82.3 mm) and which carries the link inertias. screws.robots.ur5(source="textbook") is the rounded table of MR Figure 4.6 ($W_1 = 109$, $W_2 = 82$, $L_1 = 425$, $L_2 = 392$, $H_1 = 89$, $H_2 = 95$ mm) with no inertias. Use the first to match a simulator or a real arm; use the second to reproduce the book's worked examples digit for digit. The two agree in every axis direction and differ by under a millimetre in position.

Names

Where a screws function's signature matches MR's, the alias is that function (screws.FKinSpace is screws.fk_space). IKinBody and IKinSpace are thin wrappers that return MR's (thetalist, success) tuple; the primaries return an IKResult with the iteration history. No deprecation warnings, ever.

Licence

MIT. Portions derived from the Modern Robotics code library, copyright 2018 Huan Weng, Bill Hunt, Jarvis Schultz and Mikhail Todes, MIT licence; see LICENSE.