CoppeliaSim: screws.coppelia

The CoppeliaSim bridge. Requires screws[coppelia]; import screws never loads it.

SimulatorNotRunning

Bases: RuntimeError

CoppeliaSim did not answer on the remote API port.

Source code in screws/coppelia/_sim.py

class SimulatorNotRunning(RuntimeError):
    """CoppeliaSim did not answer on the remote API port."""

Arm

A serial chain in the scene: its joints ordered base to tip, and its tip object.

Joints are every joint under path, ordered by depth, or the ones named in joints (aliases or paths, in order) when a gripper or other tool adds joints of its own. The model's embedded scripts are removed (disable_scripts=False keeps them); removed_scripts names what went. The tip is the first dummy under path whose alias is "tip" or "ee" (or contains "tip"), else the last joint.

Source code in screws/coppelia/arm.py

class Arm:
    """A serial chain in the scene: its joints ordered base to tip, and its tip object.

    Joints are every joint under ``path``, ordered by depth, or the ones named in ``joints``
    (aliases or paths, in order) when a gripper or other tool adds joints of its own. The
    model's embedded scripts are removed (``disable_scripts=False`` keeps them); ``removed_scripts``
    names what went. The tip is the first dummy
    under ``path`` whose alias is "tip" or "ee" (or contains "tip"), else the last joint.
    """

    def __init__(self, scene, path: str, joints=None, *, disable_scripts: bool = True):
        self.scene = scene
        self.sim = scene.sim
        self.path = path
        self.base = scene._handle(path)
        self.alias = self.sim.getObjectAlias(self.base, -1)
        self.removed_scripts: tuple[str, ...] = ()
        if disable_scripts:
            self.removed_scripts = self._remove_scripts()
        found = list(self.sim.getObjectsInTree(self.base, self.sim.sceneobject_joint, 0))
        found.sort(key=self._depth)
        if joints is None:
            joints = found
        else:
            by_alias = {self.sim.getObjectAlias(h, -1): h for h in found}
            picked = []
            for j in joints:
                if isinstance(j, str) and j in by_alias:
                    picked.append(by_alias[j])
                elif isinstance(j, str):
                    try:
                        picked.append(scene._handle(j))
                    except LookupError as exc:
                        raise LookupError(
                            f"no joint {j!r} under {path}; found {sorted(by_alias)}"
                        ) from exc
                else:
                    picked.append(int(j))
            joints = picked
        self.handles: tuple[int, ...] = tuple(joints)
        self.joint_names: tuple[str, ...] = tuple(self.sim.getObjectAlias(h, -1) for h in joints)
        self.tip, self.tip_alias = self._find_tip()
        self._mode: str | None = None
        self._saved_static: dict[int, int] | None = None

    def _remove_scripts(self) -> tuple[str, ...]:
        """Remove the model's embedded scripts. Stock models ship with demo scripts (the UR5's
        moves the joints through three preset poses every run) that fight a remote controller:
        targets drift when they stop being re-sent, and each run gets jerkier."""
        kind = getattr(self.sim, "sceneobject_script", None)
        if kind is None:
            return ()
        scripts = list(self.sim.getObjectsInTree(self.base, kind, 0))
        if not scripts:
            return ()
        names = tuple(self.sim.getObjectAlias(h, -1) for h in scripts)
        self.sim.removeObjects(scripts)
        return names

    def _depth(self, h: int) -> int:
        d = 0
        while h != self.sim.handle_world and h != self.base:
            h = self.sim.getObjectParent(h)
            d += 1
        return d

    def _find_tip(self) -> tuple[int, str]:
        dummies = list(self.sim.getObjectsInTree(self.base, self.sim.sceneobject_dummy, 0))
        named = {self.sim.getObjectAlias(h, -1): h for h in dummies}
        for want in ("tip", "ee", "connection"):
            if want in named:
                return named[want], want
        for alias, h in named.items():
            if any(key in alias.lower() for key in ("tip", "connection")):
                return h, alias
        if not self.handles:
            raise LookupError(f"{self.path} has no joints and no tip dummy")
        # Fall back to the last joint's first non-joint child (the last link), else the joint.
        last = self.handles[-1]
        for kind in (self.sim.sceneobject_shape, self.sim.sceneobject_dummy):
            for h in self.sim.getObjectsInTree(last, kind, 1 + 2):  # exclude base, first children only
                return h, self.sim.getObjectAlias(h, -1)
        return last, self.joint_names[-1]

    @property
    def n(self) -> int:
        return len(self.handles)

    # ----- reading ----------------------------------------------------------------------

    def theta(self) -> np.ndarray:
        """Joint positions (rad or m)."""
        return np.array([self.sim.getJointPosition(h) for h in self.handles], dtype=float)

    def dtheta(self) -> np.ndarray:
        """Joint velocities."""
        return np.array([self.sim.getJointVelocity(h) for h in self.handles], dtype=float)

    def tau(self) -> np.ndarray:
        """Measured joint forces or torques; NaN for a joint that reports none (not dynamic)."""
        out = []
        for h in self.handles:
            try:
                out.append(float(self.sim.getJointForce(h)))
            except Exception:  # noqa: BLE001 - the remote API raises a plain Exception
                out.append(float("nan"))
        return np.array(out, dtype=float)

    def tip_frame(self) -> np.ndarray:
        """The tip's 4x4 configuration in the world frame."""
        return self.scene.frame(self.tip)

    def joint_frames(self) -> list[np.ndarray]:
        """Each joint's 4x4 frame in the world frame; the joint axis is the local z axis."""
        return [self.scene.frame(h) for h in self.handles]

    def joint_types(self) -> tuple[str, ...]:
        """"revolute" or "prismatic" for each joint, base to tip."""
        return tuple(
            "prismatic" if self.sim.getJointType(h) == self.sim.joint_prismatic else "revolute"
            for h in self.handles
        )

    def joint_limits(self) -> np.ndarray | None:
        """nx2 limits from the joints' intervals, or None if any joint is cyclic."""
        out = []
        for h in self.handles:
            cyclic, interval = self.sim.getJointInterval(h)
            if cyclic:
                return None
            out.append([interval[0], interval[0] + interval[1]])
        return np.array(out, dtype=float)

    # ----- commanding -------------------------------------------------------------------

    def mode(self, kind: str) -> None:
        """Set how the arm is driven.

        "position", "velocity" and "torque" are the dynamic control modes: the physics engine
        moves the joints toward the commanded targets with the model's motors, so a command
        is tracked with some lag. "kinematic" switches the joints to kinematic mode and makes
        the arm's shapes static: a command is the position itself, followed exactly, and the
        arm becomes a rigid mover that pushes dynamic objects (the right setting for a tool
        that must strike something repeatably). Leaving kinematic mode restores the shapes'
        flags and the dynamic joint mode.
        """
        if kind not in _MODES:
            raise ValueError(f"mode must be one of {_MODES}; got {kind!r}")
        if kind == "kinematic":
            if self._saved_static is None:
                shapes = list(self.sim.getObjectsInTree(self.base, self.sim.sceneobject_shape, 0))
                self._saved_static = {
                    h: int(self.sim.getObjectInt32Param(h, self.sim.shapeintparam_static)) for h in shapes
                }
                for h in shapes:
                    self.sim.setObjectInt32Param(h, self.sim.shapeintparam_static, 1)
            for h in self.handles:
                self.sim.setJointMode(h, self.sim.jointmode_kinematic, 0)
            self._mode = kind
            return
        self.restore_dynamics()
        value = {
            "position": self.sim.jointdynctrl_position,
            "velocity": self.sim.jointdynctrl_velocity,
            "torque": self.sim.jointdynctrl_force,
        }[kind]
        for h in self.handles:
            self.sim.setObjectInt32Param(h, self.sim.jointintparam_dynctrlmode, value)
        self._mode = kind

    def restore_dynamics(self) -> None:
        """Leave kinematic mode: put the shapes' static flags and the joints' dynamic mode back.
        Called by mode(...) and by the Scene on exit, so a run never leaves the model frozen."""
        if self._saved_static is None:
            return
        for h, flag in self._saved_static.items():
            self.sim.setObjectInt32Param(h, self.sim.shapeintparam_static, flag)
        self._saved_static = None
        for h in self.handles:
            self.sim.setJointMode(h, self.sim.jointmode_dynamic, 0)
        if self._mode == "kinematic":
            self._mode = None

    def _require(self, kind: str) -> None:
        if self._mode != kind:
            raise RuntimeError(
                f"the arm is in {self._mode!r} mode; call arm.mode({kind!r}) before a {kind} command"
            )

    def command(self, u) -> None:
        """Dispatch on the current mode: positions, velocities or torques."""
        if self._mode is None:
            raise RuntimeError('set a mode first: arm.mode("position" | "velocity" | "torque")')
        dispatch = {
            "position": self.command_positions,
            "velocity": self.command_velocities,
            "torque": self.command_torques,
            "kinematic": self.teleport,
        }
        dispatch[self._mode](u)

    def _vector(self, u) -> np.ndarray:
        u = np.asarray(u, dtype=float).reshape(-1)
        if u.shape[0] != self.n:
            raise ValueError(f"got {u.shape[0]} values for {self.n} joints")
        return u

    def command_positions(self, theta) -> None:
        """Position mode: set each joint's target position."""
        self._require("position")
        for h, v in zip(self.handles, self._vector(theta)):
            self.sim.setJointTargetPosition(h, float(v))

    def command_velocities(self, dtheta) -> None:
        """Velocity mode: set each joint's target velocity."""
        self._require("velocity")
        for h, v in zip(self.handles, self._vector(dtheta)):
            self.sim.setJointTargetVelocity(h, float(v))

    def command_torques(self, tau) -> None:
        """Force mode: the signed joint force or torque is applied directly
        (sim.setJointTargetForce with signedValue, CoppeliaSim 4.3+)."""
        self._require("torque")
        for h, v in zip(self.handles, self._vector(tau)):
            self.sim.setJointTargetForce(h, float(v), True)

    def teleport(self, theta) -> None:
        """Set joint positions directly, without physics: for animating IK iterates."""
        for h, v in zip(self.handles, self._vector(theta)):
            self.sim.setJointPosition(h, float(v))

    # ----- the robot off the scene ------------------------------------------------------

    def _nearest_joint(self, h: int) -> int | None:
        """The closest joint ancestor of h among this arm's joints, or None."""
        handles = set(self.handles)
        p = self.sim.getObjectParent(h)
        while p != self.sim.handle_world and p != self.base:
            if p in handles:
                return p
            p = self.sim.getObjectParent(p)
        return None

    def link_shapes(self) -> list[list[int]]:
        """The dynamic (non-static) shapes of each moving link, grouped by nearest joint ancestor.

        Static shapes are visual shells in the stock models and carry meaningless masses.
        """
        groups: dict[int, list[int]] = {h: [] for h in self.handles}
        for s in self.sim.getObjectsInTree(self.base, self.sim.sceneobject_shape, 0):
            if self.sim.getObjectInt32Param(s, self.sim.shapeintparam_static):
                continue
            j = self._nearest_joint(s)
            if j is not None:
                groups[j].append(s)
        return [groups[h] for h in self.handles]

    def _link_inertias_at_zero(self, frames, M, T_ws_inv):
        """MR link_frames and link_inertias from the scene's shapes; the arm must be at zero."""
        link_frames, inertias = [], []
        prev = np.eye(4)
        for i, (shapes, F_joint) in enumerate(zip(self.link_shapes(), frames)):
            if not shapes:
                raise ValueError(
                    f"joint {self.joint_names[i]!r} has no dynamic shape under it; the scene "
                    "carries no mass for that link (is the model dynamically enabled?)"
                )
            ms, cs, Is = [], [], []
            for s in shapes:
                m = float(self.sim.getShapeMass(s))
                I9, T12 = self.sim.getShapeInertia(s)
                T_com = T_ws_inv @ self.scene.frame(s) @ matrix12_to_transform(T12)
                R = T_com[:3, :3]
                ms.append(m)
                cs.append(T_com[:3, 3])
                Is.append(R @ inertia9_to_matrix(I9) @ R.T)  # about the shape's COM, in {s} axes
            m = sum(ms)
            c = sum(mk * ck for mk, ck in zip(ms, cs)) / m
            inertia = np.zeros((3, 3))
            for mk, ck, Ik in zip(ms, cs, Is):
                d = ck - c
                inertia += Ik + mk * (float(d @ d) * np.eye(3) - np.outer(d, d))
            R_i = F_joint[:3, :3]
            F_i = rp_to_transform(R_i, c)  # frame {i}: link COM, joint i's orientation
            G = np.zeros((6, 6))
            G[:3, :3] = R_i.T @ inertia @ R_i
            G[3:, 3:] = m * np.eye(3)
            link_frames.append(transform_inv(prev) @ F_i)
            inertias.append(G)
            prev = F_i
        link_frames.append(transform_inv(prev) @ M)
        return tuple(link_frames), tuple(inertias)

    def robot(self, *, inertias: bool = False, relative_to: str = "world") -> Robot:
        """A screws.Robot read from the scene at the zero position.

        M is the tip frame; joint i's screw axis has omega = its frame's z axis and
        q = its origin (notes 4.1: v = -omega x q). relative_to="world" (default) makes {s}
        CoppeliaSim's world frame; relative_to="base" makes {s} the frame of the object at
        ``path``, so the result is independent of where the model stands in the scene.
        inertias=True also reads every dynamic shape's mass and inertia (MR 8.3 convention:
        frame {i} at the link's centre of mass with joint i's orientation). The arm is
        teleported to zero for the reading and put back afterwards, so call this before
        starting the simulation or accept a jump.
        """
        if relative_to not in ("world", "base"):
            raise ValueError(f'relative_to must be "world" or "base"; got {relative_to!r}')
        here = self.theta()
        self.teleport(np.zeros(self.n))
        try:
            T_ws_inv = transform_inv(self.scene.frame(self.base)) if relative_to == "base" else np.eye(4)
            frames = [T_ws_inv @ F for F in self.joint_frames()]
            M = T_ws_inv @ self.tip_frame()
            link_frames = link_inertias = None
            if inertias:
                link_frames, link_inertias = self._link_inertias_at_zero(frames, M, T_ws_inv)
        finally:
            self.teleport(here)
        axes = []
        for F, kind in zip(frames, self.joint_types()):
            z, q = F[:3, 2], F[:3, 3]
            axes.append(prismatic_axis(z) if kind == "prismatic" else revolute_axis(q, z))
        return Robot(
            name=self.alias,
            M=M,
            S=np.column_stack(axes),
            joint_types=self.joint_types(),
            joint_names=self.joint_names,
            joint_limits=self.joint_limits(),
            joint_frames_home=tuple(frames),
            link_frames=link_frames,
            link_inertias=link_inertias,
        )

    def __repr__(self) -> str:
        return f"Arm({self.path!r}, joints={list(self.joint_names)}, tip={self.tip_alias!r})"

theta

theta()

Joint positions (rad or m).

Source code in screws/coppelia/arm.py

def theta(self) -> np.ndarray:
    """Joint positions (rad or m)."""
    return np.array([self.sim.getJointPosition(h) for h in self.handles], dtype=float)

dtheta

dtheta()

Joint velocities.

Source code in screws/coppelia/arm.py

def dtheta(self) -> np.ndarray:
    """Joint velocities."""
    return np.array([self.sim.getJointVelocity(h) for h in self.handles], dtype=float)

tau

tau()

Measured joint forces or torques; NaN for a joint that reports none (not dynamic).

Source code in screws/coppelia/arm.py

def tau(self) -> np.ndarray:
    """Measured joint forces or torques; NaN for a joint that reports none (not dynamic)."""
    out = []
    for h in self.handles:
        try:
            out.append(float(self.sim.getJointForce(h)))
        except Exception:  # noqa: BLE001 - the remote API raises a plain Exception
            out.append(float("nan"))
    return np.array(out, dtype=float)

tip_frame

tip_frame()

The tip's 4x4 configuration in the world frame.

Source code in screws/coppelia/arm.py

def tip_frame(self) -> np.ndarray:
    """The tip's 4x4 configuration in the world frame."""
    return self.scene.frame(self.tip)

joint_frames

joint_frames()

Each joint's 4x4 frame in the world frame; the joint axis is the local z axis.

Source code in screws/coppelia/arm.py

def joint_frames(self) -> list[np.ndarray]:
    """Each joint's 4x4 frame in the world frame; the joint axis is the local z axis."""
    return [self.scene.frame(h) for h in self.handles]

joint_types

joint_types()

"revolute" or "prismatic" for each joint, base to tip.

Source code in screws/coppelia/arm.py

def joint_types(self) -> tuple[str, ...]:
    """"revolute" or "prismatic" for each joint, base to tip."""
    return tuple(
        "prismatic" if self.sim.getJointType(h) == self.sim.joint_prismatic else "revolute"
        for h in self.handles
    )

joint_limits

joint_limits()

nx2 limits from the joints' intervals, or None if any joint is cyclic.

Source code in screws/coppelia/arm.py

def joint_limits(self) -> np.ndarray | None:
    """nx2 limits from the joints' intervals, or None if any joint is cyclic."""
    out = []
    for h in self.handles:
        cyclic, interval = self.sim.getJointInterval(h)
        if cyclic:
            return None
        out.append([interval[0], interval[0] + interval[1]])
    return np.array(out, dtype=float)

mode

mode(kind)

Set how the arm is driven.

"position", "velocity" and "torque" are the dynamic control modes: the physics engine moves the joints toward the commanded targets with the model's motors, so a command is tracked with some lag. "kinematic" switches the joints to kinematic mode and makes the arm's shapes static: a command is the position itself, followed exactly, and the arm becomes a rigid mover that pushes dynamic objects (the right setting for a tool that must strike something repeatably). Leaving kinematic mode restores the shapes' flags and the dynamic joint mode.

Source code in screws/coppelia/arm.py

def mode(self, kind: str) -> None:
    """Set how the arm is driven.

    "position", "velocity" and "torque" are the dynamic control modes: the physics engine
    moves the joints toward the commanded targets with the model's motors, so a command
    is tracked with some lag. "kinematic" switches the joints to kinematic mode and makes
    the arm's shapes static: a command is the position itself, followed exactly, and the
    arm becomes a rigid mover that pushes dynamic objects (the right setting for a tool
    that must strike something repeatably). Leaving kinematic mode restores the shapes'
    flags and the dynamic joint mode.
    """
    if kind not in _MODES:
        raise ValueError(f"mode must be one of {_MODES}; got {kind!r}")
    if kind == "kinematic":
        if self._saved_static is None:
            shapes = list(self.sim.getObjectsInTree(self.base, self.sim.sceneobject_shape, 0))
            self._saved_static = {
                h: int(self.sim.getObjectInt32Param(h, self.sim.shapeintparam_static)) for h in shapes
            }
            for h in shapes:
                self.sim.setObjectInt32Param(h, self.sim.shapeintparam_static, 1)
        for h in self.handles:
            self.sim.setJointMode(h, self.sim.jointmode_kinematic, 0)
        self._mode = kind
        return
    self.restore_dynamics()
    value = {
        "position": self.sim.jointdynctrl_position,
        "velocity": self.sim.jointdynctrl_velocity,
        "torque": self.sim.jointdynctrl_force,
    }[kind]
    for h in self.handles:
        self.sim.setObjectInt32Param(h, self.sim.jointintparam_dynctrlmode, value)
    self._mode = kind

restore_dynamics

restore_dynamics()

Leave kinematic mode: put the shapes' static flags and the joints' dynamic mode back. Called by mode(...) and by the Scene on exit, so a run never leaves the model frozen.

Source code in screws/coppelia/arm.py

def restore_dynamics(self) -> None:
    """Leave kinematic mode: put the shapes' static flags and the joints' dynamic mode back.
    Called by mode(...) and by the Scene on exit, so a run never leaves the model frozen."""
    if self._saved_static is None:
        return
    for h, flag in self._saved_static.items():
        self.sim.setObjectInt32Param(h, self.sim.shapeintparam_static, flag)
    self._saved_static = None
    for h in self.handles:
        self.sim.setJointMode(h, self.sim.jointmode_dynamic, 0)
    if self._mode == "kinematic":
        self._mode = None

command

command(u)

Dispatch on the current mode: positions, velocities or torques.

Source code in screws/coppelia/arm.py

def command(self, u) -> None:
    """Dispatch on the current mode: positions, velocities or torques."""
    if self._mode is None:
        raise RuntimeError('set a mode first: arm.mode("position" | "velocity" | "torque")')
    dispatch = {
        "position": self.command_positions,
        "velocity": self.command_velocities,
        "torque": self.command_torques,
        "kinematic": self.teleport,
    }
    dispatch[self._mode](u)

command_positions

command_positions(theta)

Position mode: set each joint's target position.

Source code in screws/coppelia/arm.py

def command_positions(self, theta) -> None:
    """Position mode: set each joint's target position."""
    self._require("position")
    for h, v in zip(self.handles, self._vector(theta)):
        self.sim.setJointTargetPosition(h, float(v))

command_velocities

command_velocities(dtheta)

Velocity mode: set each joint's target velocity.

Source code in screws/coppelia/arm.py

def command_velocities(self, dtheta) -> None:
    """Velocity mode: set each joint's target velocity."""
    self._require("velocity")
    for h, v in zip(self.handles, self._vector(dtheta)):
        self.sim.setJointTargetVelocity(h, float(v))

command_torques

command_torques(tau)

Force mode: the signed joint force or torque is applied directly (sim.setJointTargetForce with signedValue, CoppeliaSim 4.3+).

Source code in screws/coppelia/arm.py

def command_torques(self, tau) -> None:
    """Force mode: the signed joint force or torque is applied directly
    (sim.setJointTargetForce with signedValue, CoppeliaSim 4.3+)."""
    self._require("torque")
    for h, v in zip(self.handles, self._vector(tau)):
        self.sim.setJointTargetForce(h, float(v), True)

teleport

teleport(theta)

Set joint positions directly, without physics: for animating IK iterates.

Source code in screws/coppelia/arm.py

def teleport(self, theta) -> None:
    """Set joint positions directly, without physics: for animating IK iterates."""
    for h, v in zip(self.handles, self._vector(theta)):
        self.sim.setJointPosition(h, float(v))

link_shapes

link_shapes()

The dynamic (non-static) shapes of each moving link, grouped by nearest joint ancestor.

Static shapes are visual shells in the stock models and carry meaningless masses.

Source code in screws/coppelia/arm.py

def link_shapes(self) -> list[list[int]]:
    """The dynamic (non-static) shapes of each moving link, grouped by nearest joint ancestor.

    Static shapes are visual shells in the stock models and carry meaningless masses.
    """
    groups: dict[int, list[int]] = {h: [] for h in self.handles}
    for s in self.sim.getObjectsInTree(self.base, self.sim.sceneobject_shape, 0):
        if self.sim.getObjectInt32Param(s, self.sim.shapeintparam_static):
            continue
        j = self._nearest_joint(s)
        if j is not None:
            groups[j].append(s)
    return [groups[h] for h in self.handles]

robot

robot(*, inertias=False, relative_to='world')

A screws.Robot read from the scene at the zero position.

M is the tip frame; joint i's screw axis has omega = its frame's z axis and q = its origin (notes 4.1: v = -omega x q). relative_to="world" (default) makes {s} CoppeliaSim's world frame; relative_to="base" makes {s} the frame of the object at path, so the result is independent of where the model stands in the scene. inertias=True also reads every dynamic shape's mass and inertia (MR 8.3 convention: frame {i} at the link's centre of mass with joint i's orientation). The arm is teleported to zero for the reading and put back afterwards, so call this before starting the simulation or accept a jump.

Source code in screws/coppelia/arm.py

def robot(self, *, inertias: bool = False, relative_to: str = "world") -> Robot:
    """A screws.Robot read from the scene at the zero position.

    M is the tip frame; joint i's screw axis has omega = its frame's z axis and
    q = its origin (notes 4.1: v = -omega x q). relative_to="world" (default) makes {s}
    CoppeliaSim's world frame; relative_to="base" makes {s} the frame of the object at
    ``path``, so the result is independent of where the model stands in the scene.
    inertias=True also reads every dynamic shape's mass and inertia (MR 8.3 convention:
    frame {i} at the link's centre of mass with joint i's orientation). The arm is
    teleported to zero for the reading and put back afterwards, so call this before
    starting the simulation or accept a jump.
    """
    if relative_to not in ("world", "base"):
        raise ValueError(f'relative_to must be "world" or "base"; got {relative_to!r}')
    here = self.theta()
    self.teleport(np.zeros(self.n))
    try:
        T_ws_inv = transform_inv(self.scene.frame(self.base)) if relative_to == "base" else np.eye(4)
        frames = [T_ws_inv @ F for F in self.joint_frames()]
        M = T_ws_inv @ self.tip_frame()
        link_frames = link_inertias = None
        if inertias:
            link_frames, link_inertias = self._link_inertias_at_zero(frames, M, T_ws_inv)
    finally:
        self.teleport(here)
    axes = []
    for F, kind in zip(frames, self.joint_types()):
        z, q = F[:3, 2], F[:3, 3]
        axes.append(prismatic_axis(z) if kind == "prismatic" else revolute_axis(q, z))
    return Robot(
        name=self.alias,
        M=M,
        S=np.column_stack(axes),
        joint_types=self.joint_types(),
        joint_names=self.joint_names,
        joint_limits=self.joint_limits(),
        joint_frames_home=tuple(frames),
        link_frames=link_frames,
        link_inertias=link_inertias,
    )

Log

Per-step records of simulated time, joint state, command and end-effector frame.

Arrays: t (N,), theta (N, n), dtheta (N, n), tau (N, n), command (N, n) and T_sb (N, 4, 4). Empty arrays before the first record.

Source code in screws/coppelia/log.py

class Log:
    """Per-step records of simulated time, joint state, command and end-effector frame.

    Arrays: t (N,), theta (N, n), dtheta (N, n), tau (N, n), command (N, n) and T_sb
    (N, 4, 4). Empty arrays before the first record.
    """

    def __init__(self):
        self._t: list[float] = []
        self._theta: list[np.ndarray] = []
        self._dtheta: list[np.ndarray] = []
        self._tau: list[np.ndarray] = []
        self._command: list[np.ndarray] = []
        self._T_sb: list[np.ndarray] = []

    def record(self, t, theta, dtheta, tau, command, T_sb) -> None:
        """Append one step; Scene.run calls this after every step."""
        self._t.append(float(t))
        self._theta.append(np.asarray(theta, dtype=float))
        self._dtheta.append(np.asarray(dtheta, dtype=float))
        self._tau.append(np.asarray(tau, dtype=float))
        n = len(self._theta[-1])
        self._command.append(
            np.full(n, np.nan) if command is None else np.asarray(command, dtype=float)
        )
        self._T_sb.append(np.asarray(T_sb, dtype=float))

    def __len__(self) -> int:
        return len(self._t)

    @property
    def t(self) -> np.ndarray:
        return np.array(self._t)

    @property
    def theta(self) -> np.ndarray:
        return np.array(self._theta)

    @property
    def dtheta(self) -> np.ndarray:
        return np.array(self._dtheta)

    @property
    def tau(self) -> np.ndarray:
        return np.array(self._tau)

    @property
    def command(self) -> np.ndarray:
        return np.array(self._command)

    @property
    def T_sb(self) -> np.ndarray:
        return np.array(self._T_sb)

    def to_csv(self, path) -> None:
        """Write t, theta_i, dtheta_i, tau_i, command_i and the 12 entries of T_sb per row."""
        n = self.theta.shape[1] if len(self) else 0
        header = (
            ["t"]
            + [f"theta_{i + 1}" for i in range(n)]
            + [f"dtheta_{i + 1}" for i in range(n)]
            + [f"tau_{i + 1}" for i in range(n)]
            + [f"command_{i + 1}" for i in range(n)]
            + [f"T_{r}{c}" for r in range(3) for c in range(4)]
        )
        with Path(path).open("w", newline="") as f:
            w = csv.writer(f)
            w.writerow(header)
            for k in range(len(self)):
                w.writerow(
                    [self._t[k], *self._theta[k], *self._dtheta[k], *self._tau[k],
                     *self._command[k], *self._T_sb[k][:3, :].reshape(-1)]
                )

    def to_mr_csv(self, path) -> None:
        """Write joint angles only, one row per step, no header: the MR wiki scenes' format."""
        np.savetxt(path, self.theta, delimiter=",")

    def plot(self, *, show: bool = True):
        """Four stacked time-series axes, one line per joint: theta, dtheta, tau and command
        against simulated time. Shows the window unless show=False; returns the figure either
        way (for fig.savefig). Needs matplotlib (``screws[plot]``)."""
        import matplotlib.pyplot as plt

        fig, axes = plt.subplots(4, 1, sharex=True, figsize=(8, 9))
        for ax, (name, data) in zip(
            axes, [("theta", self.theta), ("dtheta", self.dtheta), ("tau", self.tau),
                   ("command", self.command)]
        ):
            if len(self):
                ax.plot(self.t, data)
            ax.set_ylabel(name)
            ax.grid(True)
        axes[-1].set_xlabel("t (s)")
        fig.tight_layout()
        if show:
            plt.show()
        return fig

record

record(t, theta, dtheta, tau, command, T_sb)

Append one step; Scene.run calls this after every step.

Source code in screws/coppelia/log.py

def record(self, t, theta, dtheta, tau, command, T_sb) -> None:
    """Append one step; Scene.run calls this after every step."""
    self._t.append(float(t))
    self._theta.append(np.asarray(theta, dtype=float))
    self._dtheta.append(np.asarray(dtheta, dtype=float))
    self._tau.append(np.asarray(tau, dtype=float))
    n = len(self._theta[-1])
    self._command.append(
        np.full(n, np.nan) if command is None else np.asarray(command, dtype=float)
    )
    self._T_sb.append(np.asarray(T_sb, dtype=float))

to_csv

to_csv(path)

Write t, theta_i, dtheta_i, tau_i, command_i and the 12 entries of T_sb per row.

Source code in screws/coppelia/log.py

def to_csv(self, path) -> None:
    """Write t, theta_i, dtheta_i, tau_i, command_i and the 12 entries of T_sb per row."""
    n = self.theta.shape[1] if len(self) else 0
    header = (
        ["t"]
        + [f"theta_{i + 1}" for i in range(n)]
        + [f"dtheta_{i + 1}" for i in range(n)]
        + [f"tau_{i + 1}" for i in range(n)]
        + [f"command_{i + 1}" for i in range(n)]
        + [f"T_{r}{c}" for r in range(3) for c in range(4)]
    )
    with Path(path).open("w", newline="") as f:
        w = csv.writer(f)
        w.writerow(header)
        for k in range(len(self)):
            w.writerow(
                [self._t[k], *self._theta[k], *self._dtheta[k], *self._tau[k],
                 *self._command[k], *self._T_sb[k][:3, :].reshape(-1)]
            )

to_mr_csv

to_mr_csv(path)

Write joint angles only, one row per step, no header: the MR wiki scenes' format.

Source code in screws/coppelia/log.py

def to_mr_csv(self, path) -> None:
    """Write joint angles only, one row per step, no header: the MR wiki scenes' format."""
    np.savetxt(path, self.theta, delimiter=",")

plot

plot(*, show=True)

Four stacked time-series axes, one line per joint: theta, dtheta, tau and command against simulated time. Shows the window unless show=False; returns the figure either way (for fig.savefig). Needs matplotlib (screws[plot]).

Source code in screws/coppelia/log.py

def plot(self, *, show: bool = True):
    """Four stacked time-series axes, one line per joint: theta, dtheta, tau and command
    against simulated time. Shows the window unless show=False; returns the figure either
    way (for fig.savefig). Needs matplotlib (``screws[plot]``)."""
    import matplotlib.pyplot as plt

    fig, axes = plt.subplots(4, 1, sharex=True, figsize=(8, 9))
    for ax, (name, data) in zip(
        axes, [("theta", self.theta), ("dtheta", self.dtheta), ("tau", self.tau),
               ("command", self.command)]
    ):
        if len(self):
            ax.plot(self.t, data)
        ax.set_ylabel(name)
        ax.grid(True)
    axes[-1].set_xlabel("t (s)")
    fig.tight_layout()
    if show:
        plt.show()
    return fig

Scene

A CoppeliaSim scene driven from Python one step at a time.

with Scene() as scene: connects (or takes an injected sim), switches on stepping, and on exit stops the simulation if this object started it. Time only advances when you call step().

In a notebook, call release() when a cell has finished stepping. A stepping client that sits idle blocks CoppeliaSim's window: it stops redrawing, and after a few seconds it shows a "ZMQ remote API server ... abort execution" banner, which, clicked, kills the remote API and strands the notebook. Released, the simulation runs on its own between cells (the joints hold their targets) and the window stays live; the next step() takes the clock back.

Source code in screws/coppelia/scene.py

class Scene:
    """A CoppeliaSim scene driven from Python one step at a time.

    ``with Scene() as scene:`` connects (or takes an injected ``sim``), switches on
    stepping, and on exit stops the simulation if this object started it. Time only
    advances when you call step().

    In a notebook, call release() when a cell has finished stepping. A stepping client that
    sits idle blocks CoppeliaSim's window: it stops redrawing, and after a few seconds it
    shows a "ZMQ remote API server ... abort execution" banner, which, clicked, kills the
    remote API and strands the notebook. Released, the simulation runs on its own between
    cells (the joints hold their targets) and the window stays live; the next step() takes
    the clock back.
    """

    def __init__(self, host="localhost", port=23000, *, sim=None, log: bool = True):
        self.sim = _sim.connect(host, port) if sim is None else sim
        self.sim.setStepping(True)
        self.released = False
        self.started = False
        self.log = Log() if log else None
        self._triads: dict[str, list[int]] = {}
        self._recorders: list[Recorder] = []
        self._created_sensors: list[int] = []
        self._tracked: list[int] = []
        self._time_step_to_restore: float | None = None
        self._arms: list[Arm] = []

    def __enter__(self):
        return self

    def __exit__(self, exc_type, exc, tb) -> None:
        if self.started:
            self.stop()
        for arm in self._arms:
            try:
                arm.restore_dynamics()
            except Exception:  # noqa: BLE001, S110 - the simulator may already be gone
                pass
        if self._time_step_to_restore is not None:
            try:
                self.sim.setFloatParam(self.sim.floatparam_simulation_time_step, self._time_step_to_restore)
            except Exception:  # noqa: BLE001, S110 - the simulator may already be gone
                pass
            self._time_step_to_restore = None
        for handles in (self._created_sensors, self._tracked):
            if handles:
                try:
                    self.sim.removeObjects(list(handles))
                except Exception:  # noqa: BLE001, S110 - the simulator may already be gone
                    pass
                handles.clear()
        # De-register as a stepping client: the server advances only when every registered
        # stepping client has called step(), so a client that leaves silently freezes the clock
        # for everyone who comes after it.
        try:
            self.sim.setStepping(False)
        except Exception:  # noqa: BLE001, S110 - the simulator may already be gone; nothing to do
            pass
        client = getattr(self.sim, "_screws_client", None)
        socket = getattr(client, "socket", None)
        if socket is not None:
            socket.close()

    # ----- time -------------------------------------------------------------------------

    def start(self) -> None:
        """Start the simulation. A simulation left running by a crashed client is stopped first,
        since in stepping mode it would wait forever for that client's next step()."""
        if self.sim.getSimulationState() != self.sim.simulation_stopped:
            self.stop()
        self.sim.startSimulation()
        self.started = True

    def step(self) -> None:
        """Advance one simulation step; active recorders grab a frame first. After release(),
        the clock is taken back first."""
        if self.released:
            self.sim.setStepping(True)
            self.released = False
        for rec in list(self._recorders):
            rec.tick()
        self.sim.step()

    def release(self) -> None:
        """Hand the clock back to CoppeliaSim until the next step(): the simulation runs on
        its own, at its own pace, and CoppeliaSim's window keeps redrawing. Call it at the end
        of a notebook cell that stepped; idle in stepping mode, the window locks up."""
        if not self.released:
            self.sim.setStepping(False)
            self.released = True

    def stop(self) -> None:
        """Stop the simulation and wait until it has actually stopped.

        stopSimulation is asynchronous; objects created while the simulation is still winding
        down are deleted when the stop completes, so nothing should be built until it has."""
        self.sim.stopSimulation()
        self.started = False
        for _ in range(100):
            if self.sim.getSimulationState() == self.sim.simulation_stopped:
                return
            time.sleep(0.02)

    def set_time_step(self, dt: float) -> None:
        """Set the simulation (control) time step in seconds, while stopped; the physics engine
        keeps its own smaller step. The previous value is restored when the Scene exits."""
        if self.started or self.sim.getSimulationState() != self.sim.simulation_stopped:
            raise RuntimeError("set the time step while the simulation is stopped, not running")
        if self._time_step_to_restore is None:
            self._time_step_to_restore = float(self.sim.getFloatParam(self.sim.floatparam_simulation_time_step))
        self.sim.setFloatParam(self.sim.floatparam_simulation_time_step, float(dt))

    @property
    def time(self) -> float:
        return float(self.sim.getSimulationTime())

    @property
    def dt(self) -> float:
        return float(self.sim.getSimulationTimeStep())

    # ----- objects ----------------------------------------------------------------------

    def _handle(self, obj) -> int:
        if isinstance(obj, (int, np.integer)):
            return int(obj)
        try:
            return int(self.sim.getObject(obj))
        except Exception as exc:
            raise LookupError(f"no object at {obj!r} in the scene") from exc

    def frame(self, obj) -> np.ndarray:
        """The 4x4 configuration of an object (path or handle) in the world frame."""
        return _sim.matrix12_to_transform(
            self.sim.getObjectMatrix(self._handle(obj), self.sim.handle_world)
        )

    def set_frame(self, obj, T) -> None:
        """Place an object (path or handle) at T in the world frame."""
        self.sim.setObjectMatrix(self._handle(obj), _sim.transform_to_matrix12(T), self.sim.handle_world)

    def show_frame(self, T, name: str = "frame", size: float = 0.1) -> None:
        """Draw a triad at T: red x, green y, blue z. Drawing the same name again moves it."""
        self._remove_triad(name)
        T = np.asarray(T, dtype=float)
        o = T[:3, 3]
        handles = []
        for k, colour in enumerate(([1, 0, 0], [0, 1, 0], [0, 0, 1])):
            h = self.sim.addDrawingObject(self.sim.drawing_lines, 3, 0.0, -1, 2, colour)
            tip = o + size * T[:3, k]
            self.sim.addDrawingObjectItem(h, [*o, *tip])
            handles.append(h)
        self._triads[name] = handles

    def clear_frames(self) -> None:
        """Remove every triad drawn by show_frame."""
        for name in list(self._triads):
            self._remove_triad(name)

    def _remove_triad(self, name: str) -> None:
        for h in self._triads.pop(name, []):
            self.sim.removeDrawingObject(h)

    def track(self, *handles: int) -> None:
        """Remove these objects when the Scene exits, however the run ends."""
        self._tracked.extend(int(h) for h in handles)

    def untrack(self, *handles: int) -> None:
        """Stop tracking these objects, so the Scene leaves them when it exits."""
        for h in handles:
            if int(h) in self._tracked:
                self._tracked.remove(int(h))

    def camera(
        self,
        path: str | None = None,
        *,
        position=(1.5, -1.5, 1.0),
        look_at=(0.0, 0.0, 0.4),
        resolution=(640, 480),
        fov_deg: float = 60.0,
    ) -> int:
        """A vision sensor to record from: the one at ``path``, or a new perspective sensor
        placed at ``position`` looking at ``look_at`` with the image's up along world z. A
        created sensor is removed when the Scene exits."""
        if path is not None:
            return self._handle(path)
        w, h = int(resolution[0]), int(resolution[1])
        if w % 2 or h % 2:
            raise ValueError(f"resolution must have even width and height for video encoding; got {w}x{h}")
        handle = int(
            self.sim.createVisionSensor(
                1 + 2,  # explicit handling, perspective projection
                [w, h, 0, 0],
                [0.01, 10.0, float(np.deg2rad(fov_deg)), 0.1, 0, 0, 0, 0, 0, 0, 0],
            )
        )
        p = np.asarray(position, dtype=float)
        z = np.asarray(look_at, dtype=float) - p  # the sensor looks along its +z axis
        z = z / np.linalg.norm(z)
        up = np.array([0.0, 0.0, 1.0])
        if abs(float(up @ z)) > 0.999:  # looking straight up or down: any horizontal up
            up = np.array([0.0, 1.0, 0.0])
        y = up - float(up @ z) * z  # image up: world up made perpendicular to the view
        y = y / np.linalg.norm(y)
        x = np.cross(y, z)
        R = np.column_stack([x, y, z])
        assert so3.is_so3(R)
        self.sim.setObjectMatrix(handle, _sim.transform_to_matrix12(se3.rp_to_transform(R, p)), self.sim.handle_world)
        self._created_sensors.append(handle)
        return handle

    def record_video(self, path, *, camera=None, every: int = 1, **camera_kwargs) -> Recorder:
        """A Recorder context: ``with scene.record_video("run.mp4") as rec: scene.run(...)``.

        ``camera`` is a sensor handle or path; None creates one with ``camera_kwargs``
        (position, look_at, resolution, fov_deg). Frames are grabbed every ``every`` steps
        and the movie plays at simulated time. Needs screws[video] to save.
        """
        if camera is not None and camera_kwargs:
            raise TypeError(
                f"camera kwargs {sorted(camera_kwargs)} only apply when a camera is created; "
                "drop them or drop camera="
            )
        if importlib.util.find_spec("imageio") is None:
            raise ImportError('saving a movie needs imageio: install "screws[video]"')
        rec = Recorder(self, path, camera=-1, every=every)  # validates the extension first
        rec.camera = self.camera(**camera_kwargs) if camera is None else self._handle(camera)
        self._recorders.append(rec)  # records inside Scene.run with or without a with-block
        return rec

    def arm(self, path: str, joints=None, *, disable_scripts: bool = True) -> Arm:
        """The Arm under path; ``joints`` names a subset (aliases or paths) when a tool adds its
        own; the model's embedded demo scripts are removed unless ``disable_scripts=False``."""
        arm = Arm(self, path, joints, disable_scripts=disable_scripts)
        self._arms.append(arm)
        return arm

    # ----- running ----------------------------------------------------------------------

    def record(self, arm: Arm, command=None, *, theta=None, dtheta=None) -> None:
        """Append one row to the log; pass theta/dtheta already read this step to avoid re-reading."""
        if self.log is not None:
            theta = arm.theta() if theta is None else theta
            dtheta = arm.dtheta() if dtheta is None else dtheta
            self.log.record(self.time, theta, dtheta, arm.tau(), command, arm.tip_frame())

    def run(self, controller: Callable, *, duration: float, arm: Arm, log: bool = True) -> Log:
        """Loop controller(t, theta, dtheta) -> command over the simulation for duration seconds.

        Starts the simulation if needed, records before each step, returns the Log.
        """
        if not self.started:
            self.start()
        if log and self.log is None:
            self.log = Log()
        steps = round(duration / self.dt)
        for _ in range(steps):
            theta, dtheta = arm.theta(), arm.dtheta()
            u = controller(self.time, theta, dtheta)
            if u is not None:
                arm.command(u)
            if log:
                self.record(arm, u, theta=theta, dtheta=dtheta)
            self.step()
        return self.log

start

start()

Start the simulation. A simulation left running by a crashed client is stopped first, since in stepping mode it would wait forever for that client's next step().

Source code in screws/coppelia/scene.py

def start(self) -> None:
    """Start the simulation. A simulation left running by a crashed client is stopped first,
    since in stepping mode it would wait forever for that client's next step()."""
    if self.sim.getSimulationState() != self.sim.simulation_stopped:
        self.stop()
    self.sim.startSimulation()
    self.started = True

step

step()

Advance one simulation step; active recorders grab a frame first. After release(), the clock is taken back first.

Source code in screws/coppelia/scene.py

def step(self) -> None:
    """Advance one simulation step; active recorders grab a frame first. After release(),
    the clock is taken back first."""
    if self.released:
        self.sim.setStepping(True)
        self.released = False
    for rec in list(self._recorders):
        rec.tick()
    self.sim.step()

release

release()

Hand the clock back to CoppeliaSim until the next step(): the simulation runs on its own, at its own pace, and CoppeliaSim's window keeps redrawing. Call it at the end of a notebook cell that stepped; idle in stepping mode, the window locks up.

Source code in screws/coppelia/scene.py

def release(self) -> None:
    """Hand the clock back to CoppeliaSim until the next step(): the simulation runs on
    its own, at its own pace, and CoppeliaSim's window keeps redrawing. Call it at the end
    of a notebook cell that stepped; idle in stepping mode, the window locks up."""
    if not self.released:
        self.sim.setStepping(False)
        self.released = True

stop

stop()

Stop the simulation and wait until it has actually stopped.

stopSimulation is asynchronous; objects created while the simulation is still winding down are deleted when the stop completes, so nothing should be built until it has.

Source code in screws/coppelia/scene.py

def stop(self) -> None:
    """Stop the simulation and wait until it has actually stopped.

    stopSimulation is asynchronous; objects created while the simulation is still winding
    down are deleted when the stop completes, so nothing should be built until it has."""
    self.sim.stopSimulation()
    self.started = False
    for _ in range(100):
        if self.sim.getSimulationState() == self.sim.simulation_stopped:
            return
        time.sleep(0.02)

set_time_step

set_time_step(dt)

Set the simulation (control) time step in seconds, while stopped; the physics engine keeps its own smaller step. The previous value is restored when the Scene exits.

Source code in screws/coppelia/scene.py

def set_time_step(self, dt: float) -> None:
    """Set the simulation (control) time step in seconds, while stopped; the physics engine
    keeps its own smaller step. The previous value is restored when the Scene exits."""
    if self.started or self.sim.getSimulationState() != self.sim.simulation_stopped:
        raise RuntimeError("set the time step while the simulation is stopped, not running")
    if self._time_step_to_restore is None:
        self._time_step_to_restore = float(self.sim.getFloatParam(self.sim.floatparam_simulation_time_step))
    self.sim.setFloatParam(self.sim.floatparam_simulation_time_step, float(dt))

frame

frame(obj)

The 4x4 configuration of an object (path or handle) in the world frame.

Source code in screws/coppelia/scene.py

def frame(self, obj) -> np.ndarray:
    """The 4x4 configuration of an object (path or handle) in the world frame."""
    return _sim.matrix12_to_transform(
        self.sim.getObjectMatrix(self._handle(obj), self.sim.handle_world)
    )

set_frame

set_frame(obj, T)

Place an object (path or handle) at T in the world frame.

Source code in screws/coppelia/scene.py

def set_frame(self, obj, T) -> None:
    """Place an object (path or handle) at T in the world frame."""
    self.sim.setObjectMatrix(self._handle(obj), _sim.transform_to_matrix12(T), self.sim.handle_world)

show_frame

show_frame(T, name='frame', size=0.1)

Draw a triad at T: red x, green y, blue z. Drawing the same name again moves it.

Source code in screws/coppelia/scene.py

def show_frame(self, T, name: str = "frame", size: float = 0.1) -> None:
    """Draw a triad at T: red x, green y, blue z. Drawing the same name again moves it."""
    self._remove_triad(name)
    T = np.asarray(T, dtype=float)
    o = T[:3, 3]
    handles = []
    for k, colour in enumerate(([1, 0, 0], [0, 1, 0], [0, 0, 1])):
        h = self.sim.addDrawingObject(self.sim.drawing_lines, 3, 0.0, -1, 2, colour)
        tip = o + size * T[:3, k]
        self.sim.addDrawingObjectItem(h, [*o, *tip])
        handles.append(h)
    self._triads[name] = handles

clear_frames

clear_frames()

Remove every triad drawn by show_frame.

Source code in screws/coppelia/scene.py

def clear_frames(self) -> None:
    """Remove every triad drawn by show_frame."""
    for name in list(self._triads):
        self._remove_triad(name)

track

track(*handles)

Remove these objects when the Scene exits, however the run ends.

Source code in screws/coppelia/scene.py

def track(self, *handles: int) -> None:
    """Remove these objects when the Scene exits, however the run ends."""
    self._tracked.extend(int(h) for h in handles)

untrack

untrack(*handles)

Stop tracking these objects, so the Scene leaves them when it exits.

Source code in screws/coppelia/scene.py

def untrack(self, *handles: int) -> None:
    """Stop tracking these objects, so the Scene leaves them when it exits."""
    for h in handles:
        if int(h) in self._tracked:
            self._tracked.remove(int(h))

camera

camera(
    path=None,
    *,
    position=(1.5, -1.5, 1.0),
    look_at=(0.0, 0.0, 0.4),
    resolution=(640, 480),
    fov_deg=60.0,
)

A vision sensor to record from: the one at path, or a new perspective sensor placed at position looking at look_at with the image's up along world z. A created sensor is removed when the Scene exits.

Source code in screws/coppelia/scene.py

def camera(
    self,
    path: str | None = None,
    *,
    position=(1.5, -1.5, 1.0),
    look_at=(0.0, 0.0, 0.4),
    resolution=(640, 480),
    fov_deg: float = 60.0,
) -> int:
    """A vision sensor to record from: the one at ``path``, or a new perspective sensor
    placed at ``position`` looking at ``look_at`` with the image's up along world z. A
    created sensor is removed when the Scene exits."""
    if path is not None:
        return self._handle(path)
    w, h = int(resolution[0]), int(resolution[1])
    if w % 2 or h % 2:
        raise ValueError(f"resolution must have even width and height for video encoding; got {w}x{h}")
    handle = int(
        self.sim.createVisionSensor(
            1 + 2,  # explicit handling, perspective projection
            [w, h, 0, 0],
            [0.01, 10.0, float(np.deg2rad(fov_deg)), 0.1, 0, 0, 0, 0, 0, 0, 0],
        )
    )
    p = np.asarray(position, dtype=float)
    z = np.asarray(look_at, dtype=float) - p  # the sensor looks along its +z axis
    z = z / np.linalg.norm(z)
    up = np.array([0.0, 0.0, 1.0])
    if abs(float(up @ z)) > 0.999:  # looking straight up or down: any horizontal up
        up = np.array([0.0, 1.0, 0.0])
    y = up - float(up @ z) * z  # image up: world up made perpendicular to the view
    y = y / np.linalg.norm(y)
    x = np.cross(y, z)
    R = np.column_stack([x, y, z])
    assert so3.is_so3(R)
    self.sim.setObjectMatrix(handle, _sim.transform_to_matrix12(se3.rp_to_transform(R, p)), self.sim.handle_world)
    self._created_sensors.append(handle)
    return handle

record_video

record_video(
    path, *, camera=None, every=1, **camera_kwargs
)

A Recorder context: with scene.record_video("run.mp4") as rec: scene.run(...).

camera is a sensor handle or path; None creates one with camera_kwargs (position, look_at, resolution, fov_deg). Frames are grabbed every every steps and the movie plays at simulated time. Needs screws[video] to save.

Source code in screws/coppelia/scene.py

def record_video(self, path, *, camera=None, every: int = 1, **camera_kwargs) -> Recorder:
    """A Recorder context: ``with scene.record_video("run.mp4") as rec: scene.run(...)``.

    ``camera`` is a sensor handle or path; None creates one with ``camera_kwargs``
    (position, look_at, resolution, fov_deg). Frames are grabbed every ``every`` steps
    and the movie plays at simulated time. Needs screws[video] to save.
    """
    if camera is not None and camera_kwargs:
        raise TypeError(
            f"camera kwargs {sorted(camera_kwargs)} only apply when a camera is created; "
            "drop them or drop camera="
        )
    if importlib.util.find_spec("imageio") is None:
        raise ImportError('saving a movie needs imageio: install "screws[video]"')
    rec = Recorder(self, path, camera=-1, every=every)  # validates the extension first
    rec.camera = self.camera(**camera_kwargs) if camera is None else self._handle(camera)
    self._recorders.append(rec)  # records inside Scene.run with or without a with-block
    return rec

arm

arm(path, joints=None, *, disable_scripts=True)

The Arm under path; joints names a subset (aliases or paths) when a tool adds its own; the model's embedded demo scripts are removed unless disable_scripts=False.

Source code in screws/coppelia/scene.py

def arm(self, path: str, joints=None, *, disable_scripts: bool = True) -> Arm:
    """The Arm under path; ``joints`` names a subset (aliases or paths) when a tool adds its
    own; the model's embedded demo scripts are removed unless ``disable_scripts=False``."""
    arm = Arm(self, path, joints, disable_scripts=disable_scripts)
    self._arms.append(arm)
    return arm

record

record(arm, command=None, *, theta=None, dtheta=None)

Append one row to the log; pass theta/dtheta already read this step to avoid re-reading.

Source code in screws/coppelia/scene.py

def record(self, arm: Arm, command=None, *, theta=None, dtheta=None) -> None:
    """Append one row to the log; pass theta/dtheta already read this step to avoid re-reading."""
    if self.log is not None:
        theta = arm.theta() if theta is None else theta
        dtheta = arm.dtheta() if dtheta is None else dtheta
        self.log.record(self.time, theta, dtheta, arm.tau(), command, arm.tip_frame())

run

run(controller, *, duration, arm, log=True)

Loop controller(t, theta, dtheta) -> command over the simulation for duration seconds.

Starts the simulation if needed, records before each step, returns the Log.

Source code in screws/coppelia/scene.py

def run(self, controller: Callable, *, duration: float, arm: Arm, log: bool = True) -> Log:
    """Loop controller(t, theta, dtheta) -> command over the simulation for duration seconds.

    Starts the simulation if needed, records before each step, returns the Log.
    """
    if not self.started:
        self.start()
    if log and self.log is None:
        self.log = Log()
    steps = round(duration / self.dt)
    for _ in range(steps):
        theta, dtheta = arm.theta(), arm.dtheta()
        u = controller(self.time, theta, dtheta)
        if u is not None:
            arm.command(u)
        if log:
            self.record(arm, u, theta=theta, dtheta=dtheta)
        self.step()
    return self.log

Recorder

Grabs a vision sensor's image before every step and writes an MP4 or GIF on exit.

Use through scene.record_video(path, ...). frames is (N, H, W, 3) uint8, upright; fps is 1 / (every * dt), so the movie plays at simulated time however slowly the controller ran. capture() grabs a frame by hand; Scene.run grabs one every every steps.

Source code in screws/coppelia/video.py

class Recorder:
    """Grabs a vision sensor's image before every step and writes an MP4 or GIF on exit.

    Use through ``scene.record_video(path, ...)``. ``frames`` is (N, H, W, 3) uint8, upright;
    ``fps`` is 1 / (every * dt), so the movie plays at simulated time however slowly the
    controller ran. ``capture()`` grabs a frame by hand; ``Scene.run`` grabs one every
    ``every`` steps.
    """

    def __init__(self, scene, path, camera: int, every: int = 1):
        path = Path(path)
        if path.suffix.lower() not in _EXTENSIONS:
            raise ValueError(f"cannot write {path.suffix!r}; use one of {_EXTENSIONS}")
        if every < 1:
            raise ValueError("every must be at least 1")
        self.scene = scene
        self.sim = scene.sim
        self.path = path
        self.camera = camera
        self.every = int(every)
        self._frames: list[np.ndarray] = []
        self._steps = 0
        self.saved: Path | None = None

    # ----- frames -----------------------------------------------------------------------

    def capture(self) -> np.ndarray:
        """Grab the camera's current image (upright, RGB uint8) and keep it."""
        try:
            self.sim.handleVisionSensor(self.camera)  # refresh an explicitly handled sensor
        except Exception:  # noqa: BLE001, S110 - an implicitly handled sensor is already fresh
            pass
        raw, (w, h) = self.sim.getVisionSensorImg(self.camera)
        img = np.frombuffer(bytes(raw), dtype=np.uint8).reshape(int(h), int(w), 3)
        frame = np.flipud(img).copy()  # CoppeliaSim images are bottom-up
        self._frames.append(frame)
        return frame

    def tick(self) -> None:
        """Called by Scene.run before each step: capture every ``every``-th step."""
        if self._steps % self.every == 0:
            self.capture()
        self._steps += 1

    @property
    def frame_count(self) -> int:
        return len(self._frames)

    @property
    def frames(self) -> np.ndarray:
        return np.array(self._frames) if self._frames else np.zeros((0, 0, 0, 3), dtype=np.uint8)

    @property
    def fps(self) -> float:
        return 1.0 / (self.every * self.scene.dt)

    # ----- saving -----------------------------------------------------------------------

    def save(self, path=None) -> Path:
        """Encode the frames to ``path`` (default: the path given at creation). Needs screws[video]."""
        import imageio.v2 as imageio

        path = Path(path) if path is not None else self.path
        if path.suffix.lower() not in _EXTENSIONS:
            raise ValueError(f"cannot write {path.suffix!r}; use one of {_EXTENSIONS}")
        frames = list(self._frames)
        if not frames:
            raise ValueError("no frames were captured")
        if path.suffix.lower() == ".gif":
            imageio.mimwrite(path, frames, duration=1000.0 / self.fps, loop=0)
        else:
            imageio.mimwrite(path, frames, fps=self.fps, macro_block_size=1)
        if not path.exists() or path.stat().st_size == 0:
            raise OSError(f"the encoder wrote nothing to {path} (is ffmpeg available? are the frames even-sized?)")
        self.saved = path
        return path

    # ----- context ----------------------------------------------------------------------

    def __enter__(self):
        if self not in self.scene._recorders:
            self.scene._recorders.append(self)
        return self

    def __exit__(self, exc_type, exc, tb) -> None:
        if self in self.scene._recorders:
            self.scene._recorders.remove(self)
        if not self._frames:
            return
        try:
            self.save()
        except Exception as save_error:
            if exc_type is None:
                raise
            warnings.warn(f"the movie was not saved ({save_error}); the run's own error follows", stacklevel=2)

capture

capture()

Grab the camera's current image (upright, RGB uint8) and keep it.

Source code in screws/coppelia/video.py

def capture(self) -> np.ndarray:
    """Grab the camera's current image (upright, RGB uint8) and keep it."""
    try:
        self.sim.handleVisionSensor(self.camera)  # refresh an explicitly handled sensor
    except Exception:  # noqa: BLE001, S110 - an implicitly handled sensor is already fresh
        pass
    raw, (w, h) = self.sim.getVisionSensorImg(self.camera)
    img = np.frombuffer(bytes(raw), dtype=np.uint8).reshape(int(h), int(w), 3)
    frame = np.flipud(img).copy()  # CoppeliaSim images are bottom-up
    self._frames.append(frame)
    return frame

tick

tick()

Called by Scene.run before each step: capture every every-th step.

Source code in screws/coppelia/video.py

def tick(self) -> None:
    """Called by Scene.run before each step: capture every ``every``-th step."""
    if self._steps % self.every == 0:
        self.capture()
    self._steps += 1

save

save(path=None)

Encode the frames to path (default: the path given at creation). Needs screws[video].

Source code in screws/coppelia/video.py

def save(self, path=None) -> Path:
    """Encode the frames to ``path`` (default: the path given at creation). Needs screws[video]."""
    import imageio.v2 as imageio

    path = Path(path) if path is not None else self.path
    if path.suffix.lower() not in _EXTENSIONS:
        raise ValueError(f"cannot write {path.suffix!r}; use one of {_EXTENSIONS}")
    frames = list(self._frames)
    if not frames:
        raise ValueError("no frames were captured")
    if path.suffix.lower() == ".gif":
        imageio.mimwrite(path, frames, duration=1000.0 / self.fps, loop=0)
    else:
        imageio.mimwrite(path, frames, fps=self.fps, macro_block_size=1)
    if not path.exists() or path.stat().st_size == 0:
        raise OSError(f"the encoder wrote nothing to {path} (is ffmpeg available? are the frames even-sized?)")
    self.saved = path
    return path

connect

connect(host='localhost', port=23000, *, timeout_s=5.0)

Connect to CoppeliaSim's ZMQ remote API and return its sim object.

Raises SimulatorNotRunning, with the sentence a student needs, if nothing answers.

Source code in screws/coppelia/_sim.py

def connect(host: str = "localhost", port: int = 23000, *, timeout_s: float = 5.0):
    """Connect to CoppeliaSim's ZMQ remote API and return its ``sim`` object.

    Raises SimulatorNotRunning, with the sentence a student needs, if nothing answers.
    """
    socket = None
    try:
        client = _new_client(host, port)
        socket = getattr(client, "socket", None)
        # The client's own `timeout` is a server-side setting sent with the first request;
        # its recv() blocks forever. Put a real receive timeout on the socket for the probe.
        if socket is not None:
            socket.setsockopt(_zmq().RCVTIMEO, int(timeout_s * 1000))
            socket.setsockopt(_zmq().LINGER, 0)
        sim = client.require("sim")
        sim.getSimulationTime()
    except Exception as exc:  # zmq.Again, ConnectionRefusedError, ...: all mean "not running"
        if socket is not None:
            socket.close()
        raise SimulatorNotRunning(
            f"No CoppeliaSim answered at {host}:{port} ({type(exc).__name__}). {STUDENT_SENTENCE}"
        ) from exc
    if socket is not None:
        socket.setsockopt(_zmq().RCVTIMEO, -1)
    sim._screws_client = client  # keep the client alive as long as sim is
    return sim

The putting example: screws.coppelia.golf

A putting green for the UR5: a ball, a hole, a kinematic putter on the flange, and a putt.

The ball is the only dynamic body. The putter is a static, respondable tool rigidly attached to the arm's last link, so it moves exactly where the joint trajectory puts it and pushes the ball through contact. Everything else is kinematics from the rest of screws: the face pose that addresses the ball, a straight stroke along the target line, IK for every waypoint.

Green dataclass

The turf, the cup cut through it, the pin, and the ball.

Source code in screws/coppelia/golf.py

@dataclass
class Green:
    """The turf, the cup cut through it, the pin, and the ball."""

    scene: object
    ball: int
    turf: list[int]  # the slabs and the wedge insert, all static convex pieces (hidden)
    surface: int  # the one smooth visual surface with the round hole (no physics)
    cup_floor: int
    pin: tuple[int, ...]
    hole_position: np.ndarray  # (x, y, top) at the turf surface
    top: float  # height of the turf surface above the floor
    seal: int | None = None
    logo: int | None = None

    def ball_position(self) -> np.ndarray:
        """The ball's centre in the world frame."""
        return np.asarray(self.scene.sim.getObjectPosition(self.ball, self.scene.sim.handle_world), float)

    def ball_speed(self) -> float:
        """The ball's linear speed."""
        v, _ = self.scene.sim.getObjectVelocity(self.ball)
        return float(np.linalg.norm(v))

    def distance_to_hole(self) -> float:
        """Horizontal distance from the ball's centre to the cup's centre."""
        return float(np.linalg.norm((self.ball_position() - self.hole_position)[:2]))

    def holed(self) -> bool:
        """True once the ball has dropped into the cup: inside the rim and below the surface."""
        p = self.ball_position()
        return self.distance_to_hole() <= HOLE_RADIUS and p[2] < self.top - BALL_RADIUS / 2

    def handles(self) -> list[int]:
        """Every object the green added to the scene."""
        extras = [h for h in (self.seal, self.logo) if h is not None]
        return [self.ball, *self.turf, self.surface, self.cup_floor, *self.pin, *extras]

    def remove(self) -> None:
        """Take the green out of the scene."""
        self.scene.untrack(*self.handles())
        self.scene.sim.removeObjects(self.handles())

ball_position

ball_position()

The ball's centre in the world frame.

Source code in screws/coppelia/golf.py

def ball_position(self) -> np.ndarray:
    """The ball's centre in the world frame."""
    return np.asarray(self.scene.sim.getObjectPosition(self.ball, self.scene.sim.handle_world), float)

ball_speed

ball_speed()

The ball's linear speed.

Source code in screws/coppelia/golf.py

def ball_speed(self) -> float:
    """The ball's linear speed."""
    v, _ = self.scene.sim.getObjectVelocity(self.ball)
    return float(np.linalg.norm(v))

distance_to_hole

distance_to_hole()

Horizontal distance from the ball's centre to the cup's centre.

Source code in screws/coppelia/golf.py

def distance_to_hole(self) -> float:
    """Horizontal distance from the ball's centre to the cup's centre."""
    return float(np.linalg.norm((self.ball_position() - self.hole_position)[:2]))

holed

holed()

True once the ball has dropped into the cup: inside the rim and below the surface.

Source code in screws/coppelia/golf.py

def holed(self) -> bool:
    """True once the ball has dropped into the cup: inside the rim and below the surface."""
    p = self.ball_position()
    return self.distance_to_hole() <= HOLE_RADIUS and p[2] < self.top - BALL_RADIUS / 2

handles

handles()

Every object the green added to the scene.

Source code in screws/coppelia/golf.py

def handles(self) -> list[int]:
    """Every object the green added to the scene."""
    extras = [h for h in (self.seal, self.logo) if h is not None]
    return [self.ball, *self.turf, self.surface, self.cup_floor, *self.pin, *extras]

remove

remove()

Take the green out of the scene.

Source code in screws/coppelia/golf.py

def remove(self) -> None:
    """Take the green out of the scene."""
    self.scene.untrack(*self.handles())
    self.scene.sim.removeObjects(self.handles())

Putter dataclass

A putter on the arm: a shaft along the tool's +z and a face whose outward normal is the tool's +x, both static shapes parented to the last link (drive the arm in kinematic mode). T_tip_face is the face frame in the tip (last link) frame.

Source code in screws/coppelia/golf.py

@dataclass
class Putter:
    """A putter on the arm: a shaft along the tool's +z and a face whose outward normal is
    the tool's +x, both static shapes parented to the last link (drive the arm in kinematic
    mode). T_tip_face is the face frame in the tip (last link) frame."""

    scene: object
    shaft: int
    face: int
    T_tip_face: np.ndarray
    face_size: tuple[float, float, float]

    def robot(self, robot: Robot) -> Robot:
        """The arm's Robot with M moved from the flange to the putter face."""
        return replace(robot, name=f"{robot.name}+putter", M=robot.M @ self.T_tip_face)

    def remove(self) -> None:
        """Take the putter off the arm and out of the scene."""
        self.scene.untrack(self.shaft, self.face)
        self.scene.sim.removeObjects([self.shaft, self.face])

robot

robot(robot)

The arm's Robot with M moved from the flange to the putter face.

Source code in screws/coppelia/golf.py

def robot(self, robot: Robot) -> Robot:
    """The arm's Robot with M moved from the flange to the putter face."""
    return replace(robot, name=f"{robot.name}+putter", M=robot.M @ self.T_tip_face)

remove

remove()

Take the putter off the arm and out of the scene.

Source code in screws/coppelia/golf.py

def remove(self) -> None:
    """Take the putter off the arm and out of the scene."""
    self.scene.untrack(self.shaft, self.face)
    self.scene.sim.removeObjects([self.shaft, self.face])

PuttResult dataclass

How a putt went: whether it dropped, where the ball stopped, and the paths of the ball and the putter face.

Source code in screws/coppelia/golf.py

@dataclass
class PuttResult:
    """How a putt went: whether it dropped, where the ball stopped, and the paths of the
    ball and the putter face."""

    holed: bool
    distance: float  # final horizontal distance from the ball to the hole, m
    ball_path: np.ndarray  # (N, 3) ball positions, one per step from the stroke onward
    face_path: np.ndarray  # (N, 3) putter-face positions (from the measured joints), same steps
    theta_final: np.ndarray

camera_yaw

camera_yaw(position, look_at)

The yaw (about z) whose x direction is a camera's horizontal right: what to pass as build_green's yaw so text laid on the green reads correctly from that camera.

Source code in screws/coppelia/golf.py

def camera_yaw(position, look_at) -> float:
    """The yaw (about z) whose x direction is a camera's horizontal right: what to pass as
    build_green's ``yaw`` so text laid on the green reads correctly from that camera."""
    d = np.asarray(look_at, float)[:2] - np.asarray(position, float)[:2]
    return float(np.arctan2(d[1], d[0]) - np.pi / 2)

camera_forward

camera_forward(position, look_at)

The horizontal unit vector pointing away from a camera: "up" in its image on the green.

Source code in screws/coppelia/golf.py

def camera_forward(position, look_at) -> np.ndarray:
    """The horizontal unit vector pointing away from a camera: "up" in its image on the green."""
    d = np.asarray(look_at, float)[:2] - np.asarray(position, float)[:2]
    return d / np.linalg.norm(d)

text_image

text_image(
    path,
    text,
    *,
    fg=(255, 255, 255),
    bg=SMU_RED_8BIT,
    aspect=14 / 9,
    size=512,
)

Write a square PNG of bold text on a solid background (defaults: white on SMU red) for a flag plane of the given width:height aspect: the text is drawn at that aspect and the image stretched to a square, so it reads correctly once mapped onto the plane (a non-square texture would render dark).

Source code in screws/coppelia/golf.py

def text_image(path, text: str, *, fg=(255, 255, 255), bg=SMU_RED_8BIT, aspect: float = 14 / 9, size: int = 512) -> Path:
    """Write a square PNG of bold text on a solid background (defaults: white on SMU red) for a
    flag plane of the given width:height aspect: the text is drawn at that aspect and the
    image stretched to a square, so it reads correctly once mapped onto the plane (a
    non-square texture would render dark)."""
    from PIL import Image, ImageDraw

    canvas = (size, max(1, round(size / aspect)))
    im = Image.new("RGB", canvas, bg)
    draw = ImageDraw.Draw(im)
    font_size = int(canvas[1] * 0.6)
    font = _font(font_size)
    while font_size > 8:
        box = draw.textbbox((0, 0), text, font=font)
        if box[2] - box[0] <= canvas[0] * 0.85 and box[3] - box[1] <= canvas[1] * 0.75:
            break
        font_size = int(font_size * 0.9)
        font = _font(font_size)
    box = draw.textbbox((0, 0), text, font=font)
    x = (canvas[0] - (box[2] - box[0])) / 2 - box[0]
    y = (canvas[1] - (box[3] - box[1])) / 2 - box[1]
    draw.text((x, y), text, font=font, fill=fg)
    path = Path(path)
    im.resize((size, size), Image.LANCZOS).save(path)
    return path

seal_image

seal_image(
    path,
    artwork,
    *,
    disc=(255, 255, 255),
    background=TURF_COLOUR_8BIT,
    size=512,
)

Write a PNG of a logo (a PNG with transparency, or any image) on a white disc that blends into the turf outside the disc, for a flat plane laid on the green. The artwork's own colours are kept, as the brand guide asks. size must be a power of two.

Source code in screws/coppelia/golf.py

def seal_image(path, artwork, *, disc=(255, 255, 255), background=TURF_COLOUR_8BIT, size: int = 512) -> Path:
    """Write a PNG of a logo (a PNG with transparency, or any image) on a white disc that blends
    into the turf outside the disc, for a flat plane laid on the green. The artwork's own
    colours are kept, as the brand guide asks. size must be a power of two."""
    from PIL import Image, ImageDraw

    art = Image.open(artwork).convert("RGBA")
    im = Image.new("RGB", (size, size), background)
    draw = ImageDraw.Draw(im)
    draw.ellipse((0, 0, size - 1, size - 1), fill=disc)
    inner = int(size * 0.86)
    art = art.resize((inner, inner), Image.LANCZOS)
    offset = (size - inner) // 2
    im.paste(art, (offset, offset), art)
    path = Path(path)
    im.save(path)
    return path

logo_image

logo_image(path, artwork, *, plate=None, size=TEXTURE_SIZE)

Write a square PNG of a logo (a PNG with transparency) for a plane laid flat on the green, and return (path, aspect): the artwork is stretched to fill the square (non-square textures render dark) and reads correctly on a plane of width:height = aspect, which is what build_green's logo_aspect takes. With plate=None the transparent parts take the turf colour, so a white or coloured logo looks painted on the grass; plate=(255, 255, 255) puts it on a white sign. Colours are kept.

Source code in screws/coppelia/golf.py

def logo_image(path, artwork, *, plate=None, size: int = TEXTURE_SIZE) -> tuple[Path, float]:
    """Write a square PNG of a logo (a PNG with transparency) for a plane laid flat on the
    green, and return (path, aspect): the artwork is stretched to fill the square (non-square
    textures render dark) and reads correctly on a plane of width:height = aspect, which is
    what build_green's logo_aspect takes. With plate=None the transparent parts take the
    turf colour, so a white or coloured logo looks painted on the grass; plate=(255, 255, 255)
    puts it on a white sign. Colours are kept.
    """
    from PIL import Image

    art = Image.open(artwork).convert("RGBA")
    background = TURF_COLOUR_8BIT if plate is None else plate
    if plate is not None:  # a visible plate: pad the artwork so the plate shows around it
        w, h = art.size
        m = round(0.06 * w)
        padded = Image.new("RGBA", (w + 2 * m, h + 2 * m), (*plate, 255))
        padded.paste(art, (m, m), art)
        art = padded
    aspect = art.size[0] / art.size[1]
    canvas = Image.new("RGB", (size, size), background)
    stretched = art.resize((size, size), Image.LANCZOS)
    canvas.paste(stretched, (0, 0), stretched)
    path = Path(path)
    canvas.save(path)
    return path, aspect

surface_mesh

surface_mesh(
    *,
    size,
    thickness,
    center,
    hole,
    hole_radius=HOLE_RADIUS,
    n=96,
)

Vertices (V, 3) and triangles (F, 3) of a rectangular slab with a round hole through it: the turf as one smooth visual surface (the physics uses green_pieces underneath).

size is (sx, sy) and center (cx, cy) of the slab, whose bottom is at z = 0 and top at z = thickness; hole is (hx, hy). Outward normals, closed manifold, so it serves as a static respondable mesh.

Source code in screws/coppelia/golf.py

def surface_mesh(*, size, thickness, center, hole, hole_radius=HOLE_RADIUS, n: int = 96):
    """Vertices (V, 3) and triangles (F, 3) of a rectangular slab with a round hole through it:
    the turf as one smooth *visual* surface (the physics uses green_pieces underneath).

    size is (sx, sy) and center (cx, cy) of the slab, whose bottom is at z = 0 and top at
    z = thickness; hole is (hx, hy). Outward normals, closed manifold, so it serves as a
    static respondable mesh.
    """
    sx, sy = float(size[0]), float(size[1])
    cx, cy = float(center[0]), float(center[1])
    hx, hy = float(hole[0]), float(hole[1])
    xmin, xmax, ymin, ymax = cx - sx / 2, cx + sx / 2, cy - sy / 2, cy + sy / 2
    if not (xmin < hx - hole_radius and hx + hole_radius < xmax and ymin < hy - hole_radius and hy + hole_radius < ymax):
        raise ValueError("the hole must lie inside the slab")
    angles = 2 * np.pi * np.arange(n) / n
    inner = [(a, np.array([hx + hole_radius * np.cos(a), hy + hole_radius * np.sin(a)])) for a in angles]

    def ray_to_edge(a):
        d = np.array([np.cos(a), np.sin(a)])
        ts = []
        if d[0] > 1e-12:
            ts.append((xmax - hx) / d[0])
        if d[0] < -1e-12:
            ts.append((xmin - hx) / d[0])
        if d[1] > 1e-12:
            ts.append((ymax - hy) / d[1])
        if d[1] < -1e-12:
            ts.append((ymin - hy) / d[1])
        t = min(ts)
        return np.array([hx, hy]) + t * d

    outer = [(a, ray_to_edge(a)) for a in angles]
    for corner in ((xmax, ymax), (xmin, ymax), (xmin, ymin), (xmax, ymin)):
        a = float(np.arctan2(corner[1] - hy, corner[0] - hx)) % (2 * np.pi)
        if all(abs(a - b) > 1e-9 for b, _ in outer):
            outer.append((a, np.array(corner, float)))
    outer.sort(key=lambda ap: ap[0])
    ni, no = len(inner), len(outer)
    # vertex layout: top inner, top outer, bottom inner, bottom outer
    TI, TO, BI, BO = 0, ni, ni + no, ni + no + ni
    V = []
    for z in (thickness, 0.0):
        V += [[p[0], p[1], z] for _, p in inner]
        V += [[p[0], p[1], z] for _, p in outer]
    V = np.array(V)
    F = []
    # top: zip the two angle-sorted polygons; bottom: the same with reversed winding
    i = j = 0
    while i < ni or j < no:
        a_next = inner[(i + 1) % ni][0] + (2 * np.pi if i + 1 >= ni else 0.0) if i < ni else np.inf
        b_next = outer[(j + 1) % no][0] + (2 * np.pi if j + 1 >= no else 0.0) if j < no else np.inf
        if a_next <= b_next:
            F.append([TI + i % ni, TO + j % no, TI + (i + 1) % ni])
            i += 1
        else:
            F.append([TI + i % ni, TO + j % no, TO + (j + 1) % no])
            j += 1
    top_count = len(F)
    for tri in list(F[:top_count]):
        F.append([tri[0] + BI, tri[2] + BI, tri[1] + BI])
    for j in range(no):  # outer walls, outward
        k = (j + 1) % no
        F.append([BO + j, BO + k, TO + k])
        F.append([BO + j, TO + k, TO + j])
    for i in range(ni):  # hole walls, facing into the cup
        k = (i + 1) % ni
        F.append([TI + i, TI + k, BI + k])
        F.append([TI + i, BI + k, BI + i])
    return V, np.array(F, dtype=int)

green_pieces

green_pieces(
    *,
    size,
    thickness,
    center,
    hole,
    hole_radius=HOLE_RADIUS,
    n=32,
)

The turf as convex pieces: four cuboid slabs and, in a square insert around the cup, n wedge prisms whose inner edges form the round hole.

Returns {"slabs": [(cx, cy, sx, sy), ...], "wedges": [(V, F), ...]}. Slabs have their bottom at z = 0 and top at z = thickness; wedge tops sit 0.3 mm lower so a ball rolling off a slab never catches their edge. Convex pieces keep the physics engine happy (a non-convex mesh is flagged by CoppeliaSim and kicks a rolling ball at its internal edges).

Source code in screws/coppelia/golf.py

def green_pieces(*, size, thickness, center, hole, hole_radius=HOLE_RADIUS, n: int = 32):
    """The turf as convex pieces: four cuboid slabs and, in a square insert around the cup,
    n wedge prisms whose inner edges form the round hole.

    Returns {"slabs": [(cx, cy, sx, sy), ...], "wedges": [(V, F), ...]}. Slabs have their
    bottom at z = 0 and top at z = thickness; wedge tops sit 0.3 mm lower so a ball rolling
    off a slab never catches their edge. Convex pieces keep the physics engine happy (a
    non-convex mesh is flagged by CoppeliaSim and kicks a rolling ball at its internal edges).
    """
    sx, sy = float(size[0]), float(size[1])
    cx, cy = float(center[0]), float(center[1])
    hx, hy = float(hole[0]), float(hole[1])
    a = hole_radius + INSERT_MARGIN
    xmin, xmax, ymin, ymax = cx - sx / 2, cx + sx / 2, cy - sy / 2, cy + sy / 2
    if not (xmin <= hx - a and hx + a <= xmax and ymin <= hy - a and hy + a <= ymax):
        raise ValueError("the cup and its insert must lie inside the turf")
    slabs = [
        ((xmin + (hx - a)) / 2, cy, (hx - a) - xmin, sy),  # left of the insert, full depth
        (((hx + a) + xmax) / 2, cy, xmax - (hx + a), sy),  # right of the insert, full depth
        (hx, (ymin + (hy - a)) / 2, 2 * a, (hy - a) - ymin),  # in front of the insert
        (hx, ((hy + a) + ymax) / 2, 2 * a, ymax - (hy + a)),  # behind the insert
    ]
    slabs = [sl for sl in slabs if sl[2] > 1e-9 and sl[3] > 1e-9]
    angles = 2 * np.pi * np.arange(n) / n
    inner = [np.array([hx + hole_radius * np.cos(t), hy + hole_radius * np.sin(t)]) for t in angles]
    corners = []
    for corner in ((hx + a, hy + a), (hx - a, hy + a), (hx - a, hy - a), (hx + a, hy - a)):
        corners.append((float(np.arctan2(corner[1] - hy, corner[0] - hx)) % (2 * np.pi), np.array(corner)))
    wedges = []
    for i in range(n):
        t0, t1 = angles[i], angles[(i + 1) % n] + (2 * np.pi if i + 1 >= n else 0.0)
        outer0 = _ray_to_square(hx, hy, t0, hx - a, hx + a, hy - a, hy + a)
        outer1 = _ray_to_square(hx, hy, t1, hx - a, hx + a, hy - a, hy + a)
        between = [c for ang, c in corners if t0 < ang < t1 or t0 < ang + 2 * np.pi < t1]
        between.sort(key=lambda c: (float(np.arctan2(c[1] - hy, c[0] - hx)) - t0) % (2 * np.pi))
        # counter-clockwise around the wedge: out along ray i, along the square, back in
        poly = [inner[(i + 1) % n], inner[i], outer0, *between, outer1]
        wedges.append(_prism(poly, 0.0, thickness - 0.0003))
    return {"slabs": slabs, "wedges": wedges}

build_green

build_green(
    scene,
    *,
    ball_position,
    hole_position,
    size=(3.0, 3.0),
    thickness=TURF_THICKNESS,
    friction=0.8,
    damping=0.6,
    restitution=0.2,
    flag_image=None,
    seal_image=None,
    seal_position=None,
    seal_size=0.35,
    logo_image=None,
    logo_position=None,
    logo_width=0.6,
    logo_aspect=None,
    yaw=0.0,
)

Lay turf with a real cup at (x, y), a pin in it, and a ball at (x, y) on the surface.

The turf is a static respondable slab of the given size (centred between the ball and the hole) and thickness, with the cup cut through it and a cup floor below; see green_pieces for why it is built from convex pieces. The ball's material (friction, Bullet's linear and angular damping, restitution) defaults roll about 0.3 m from a 0.3 m/s face on a dynamic putter.

flag_image (a PNG, e.g. from text_image) textures the flag; seal_image (e.g. from seal_image) is laid flat on the turf as a seal_size square at seal_position (x, y), default: beside the line, on the camera side; logo_image (e.g. from logo_image) is laid flat logo_width wide at logo_position, default: just past the seal along the yawed direction, with height logo_width / logo_aspect (default: the file's own aspect; pass the aspect logo_image returns, since it writes a square). yaw (radians about z) turns both so their text reads along a camera's horizontal: pass the camera's right-hand direction, e.g. golf.camera_yaw(position, look_at). Everything built is removed when the Scene exits, or by Green.remove().

Source code in screws/coppelia/golf.py

def build_green(
    scene,
    *,
    ball_position,
    hole_position,
    size=(3.0, 3.0),
    thickness: float = TURF_THICKNESS,
    friction: float = 0.8,
    damping: float = 0.6,
    restitution: float = 0.2,
    flag_image=None,
    seal_image=None,
    seal_position=None,
    seal_size: float = 0.35,
    logo_image=None,
    logo_position=None,
    logo_width: float = 0.6,
    logo_aspect: float | None = None,
    yaw: float = 0.0,
) -> Green:
    """Lay turf with a real cup at (x, y), a pin in it, and a ball at (x, y) on the surface.

    The turf is a static respondable slab of the given size (centred between the ball and
    the hole) and thickness, with the cup cut through it and a cup floor below; see
    green_pieces for why it is built from convex pieces. The ball's material (friction,
    Bullet's linear and angular damping, restitution) defaults roll about 0.3 m from a
    0.3 m/s face on a dynamic putter.

    flag_image (a PNG, e.g. from text_image) textures the flag; seal_image (e.g. from
    seal_image) is laid flat on the turf as a seal_size square at seal_position (x, y),
    default: beside the line, on the camera side; logo_image (e.g. from logo_image) is laid
    flat logo_width wide at logo_position, default: just past the seal along the yawed
    direction, with height logo_width / logo_aspect (default: the file's own aspect; pass the
    aspect logo_image returns, since it writes a square). yaw (radians about z) turns both so their text reads along a camera's
    horizontal: pass the camera's right-hand direction, e.g. golf.camera_yaw(position, look_at).
    Everything built is removed when the Scene exits, or by Green.remove().
    """
    sim = scene.sim
    bx, by = float(ball_position[0]), float(ball_position[1])
    hx, hy = float(hole_position[0]), float(hole_position[1])
    center = ((bx + hx) / 2, (by + hy) / 2)
    pieces = green_pieces(size=size, thickness=thickness, center=center, hole=(hx, hy))
    turf = []
    for cx, cy, sx, sy in pieces["slabs"]:
        h = sim.createPrimitiveShape(sim.primitiveshape_cuboid, [sx, sy, thickness], 0)
        _shape(sim, h, static=True, respondable=True)
        sim.setObjectPosition(h, [cx, cy, thickness / 2], sim.handle_world)
        _colour(sim, h, TURF_COLOUR)
        turf.append(h)
    for V, F in pieces["wedges"]:
        h = sim.createMeshShape(0, 0.0, [float(x) for x in V.reshape(-1)], [int(k) for k in F.reshape(-1)])
        _shape(sim, h, static=True, respondable=True)
        _colour(sim, h, TURF_COLOUR)
        turf.append(h)
    for h in turf:  # the physics pieces are invisible; one smooth surface is what you see
        sim.setObjectInt32Param(h, sim.objintparam_visibility_layer, 0)
    V, F = surface_mesh(size=size, thickness=thickness, center=center, hole=(hx, hy), hole_radius=HOLE_RADIUS)
    surface = sim.createMeshShape(0, 0.0, [float(x) for x in V.reshape(-1)], [int(k) for k in F.reshape(-1)])
    _shape(sim, surface, static=True, respondable=False)
    _colour(sim, surface, TURF_COLOUR)
    cup_floor = sim.createPrimitiveShape(sim.primitiveshape_cylinder, [2 * HOLE_RADIUS, 2 * HOLE_RADIUS, 0.004], 0)
    _shape(sim, cup_floor, static=True, respondable=True)
    sim.setObjectPosition(cup_floor, [hx, hy, 0.002], sim.handle_world)
    _colour(sim, cup_floor, (0.08, 0.08, 0.08))
    pin_height = 0.6
    pin = sim.createPrimitiveShape(sim.primitiveshape_cylinder, [0.008, 0.008, pin_height], 0)
    _shape(sim, pin, static=True, respondable=False)
    sim.setObjectPosition(pin, [hx, hy, pin_height / 2], sim.handle_world)
    _colour(sim, pin, (0.95, 0.95, 0.9))
    flag_w, flag_h = 0.14, 0.09
    if flag_image is not None:
        flag, _, _ = sim.createTexture(str(flag_image), 0, [flag_w, flag_h])
        _shape(sim, flag, static=True, respondable=False)
        _colour(sim, flag, (1.0, 1.0, 1.0))
        # a vertical plane: image x along world x, image up along world z, facing -y
        R = np.column_stack([[1.0, 0.0, 0.0], [0.0, 0.0, 1.0], [0.0, -1.0, 0.0]])
        scene.set_frame(flag, rp_to_transform(R, [hx + 0.004 + flag_w / 2, hy, pin_height - flag_h / 2 - 0.01]))
    else:
        flag = sim.createPrimitiveShape(sim.primitiveshape_cuboid, [flag_w, 0.003, flag_h], 0)
        _shape(sim, flag, static=True, respondable=False)
        sim.setObjectPosition(flag, [hx + 0.004 + flag_w / 2, hy, pin_height - flag_h / 2 - 0.01], sim.handle_world)
        _colour(sim, flag, SMU_RED)
    R_yaw = rot([0.0, 0.0, 1.0], yaw)
    right = R_yaw[:2, 0]
    seal = None
    if seal_position is None:
        seal_position = (center[0] + 0.15, center[1] - 0.45)
    if seal_image is not None:
        seal, _, _ = sim.createTexture(str(seal_image), 0, [seal_size, seal_size])
        _shape(sim, seal, static=True, respondable=False)
        _colour(sim, seal, (1.0, 1.0, 1.0))  # textures modulate the shape colour
        scene.set_frame(seal, rp_to_transform(R_yaw, [float(seal_position[0]), float(seal_position[1]), thickness + 0.0005]))
    logo = None
    if logo_image is not None:
        if logo_aspect is None:
            from PIL import Image

            w, h = Image.open(logo_image).size
            logo_aspect = w / h
        logo_h = logo_width / logo_aspect
        if logo_position is None:
            logo_position = np.asarray(seal_position, float) + (seal_size / 2 + 0.08 + logo_width / 2) * right
        logo, _, _ = sim.createTexture(str(logo_image), 0, [logo_width, logo_h])
        _shape(sim, logo, static=True, respondable=False)
        _colour(sim, logo, (1.0, 1.0, 1.0))
        scene.set_frame(logo, rp_to_transform(R_yaw, [float(logo_position[0]), float(logo_position[1]), thickness + 0.0005]))
    ball = sim.createPrimitiveShape(sim.primitiveshape_spheroid, [2 * BALL_RADIUS] * 3, 0)
    sim.setShapeMass(ball, BALL_MASS)
    _shape(sim, ball, static=False, respondable=True)
    sim.setObjectPosition(ball, [bx, by, thickness + BALL_RADIUS], sim.handle_world)
    sim.setEngineFloatParam(sim.bullet_body_friction, ball, friction)
    sim.setEngineFloatParam(sim.bullet_body_restitution, ball, restitution)
    sim.setEngineFloatParam(sim.bullet_body_lineardamping, ball, damping)
    sim.setEngineFloatParam(sim.bullet_body_angulardamping, ball, damping)
    _colour(sim, ball, (0.95, 0.95, 0.95))
    green = Green(scene, ball, turf, surface, cup_floor, (pin, flag), np.array([hx, hy, thickness]), thickness, seal, logo)
    scene.track(*green.handles())
    return green

attach_putter

attach_putter(
    scene,
    arm,
    *,
    shaft_length=0.3,
    shaft_radius=0.006,
    face_size=(0.02, 0.1, 0.02),
)

Build a putter on the arm's tip: static shapes parented to the last link.

The face is a box of face_size (thickness along tool x, width along y, height along z) centred shaft_length along the tool's +z axis; its striking face is the +x side and it is respondable. The shaft is a visual only. Drive the arm with arm.mode("kinematic") for a putt: the tree is then static and the face is a rigid mover that strikes the ball exactly where the trajectory says (a respondable static shape on a dynamic tree is flagged by CoppeliaSim as unrealistic, and a dynamic face on a force sensor wobbles and lags).

Source code in screws/coppelia/golf.py

def attach_putter(
    scene,
    arm,
    *,
    shaft_length: float = 0.30,
    shaft_radius: float = 0.006,
    face_size=(0.02, 0.10, 0.02),
) -> Putter:
    """Build a putter on the arm's tip: static shapes parented to the last link.

    The face is a box of face_size (thickness along tool x, width along y, height along z)
    centred shaft_length along the tool's +z axis; its striking face is the +x side and it is
    respondable. The shaft is a visual only. Drive the arm with arm.mode("kinematic") for a
    putt: the tree is then static and the face is a rigid mover that strikes the ball exactly
    where the trajectory says (a respondable static shape on a *dynamic* tree is flagged by
    CoppeliaSim as unrealistic, and a dynamic face on a force sensor wobbles and lags).
    """
    sim = scene.sim
    T_tip = arm.tip_frame()
    T_tip_face = trans([0.0, 0.0, shaft_length])
    shaft = sim.createPrimitiveShape(sim.primitiveshape_cylinder, [2 * shaft_radius, 2 * shaft_radius, shaft_length], 0)
    _shape(sim, shaft, static=True, respondable=False)
    scene.set_frame(shaft, T_tip @ trans([0.0, 0.0, shaft_length / 2]))
    _colour(sim, shaft, (0.6, 0.6, 0.6))
    face = sim.createPrimitiveShape(sim.primitiveshape_cuboid, list(face_size), 0)
    _shape(sim, face, static=True, respondable=True)
    scene.set_frame(face, T_tip @ T_tip_face)
    _colour(sim, face, (0.8, 0.1, 0.1))
    for h in (shaft, face):
        sim.setObjectParent(h, arm.tip, True)
    scene.track(shaft, face)
    return Putter(scene, shaft, face, T_tip_face, tuple(float(x) for x in face_size))

address_pose

address_pose(ball, hole, *, back=0.06)

The face frame that addresses the ball: back metres behind it on the target line, at ball-centre height, face normal (tool x) along the line, shaft (tool z) pointing down.

Source code in screws/coppelia/golf.py

def address_pose(ball, hole, *, back: float = 0.06) -> np.ndarray:
    """The face frame that addresses the ball: back metres behind it on the target line, at
    ball-centre height, face normal (tool x) along the line, shaft (tool z) pointing down."""
    ball = np.asarray(ball, float)
    x = _target_line(ball, hole)
    z = np.array([0.0, 0.0, -1.0])
    y = np.cross(z, x)
    return rp_to_transform(np.column_stack([x, y, z]), ball - back * x)

stroke_path

stroke_path(
    ball,
    hole,
    *,
    back=0.06,
    through=0.04,
    speed=0.5,
    dt=0.05,
)

Face poses, one per control step, for a straight stroke at constant speed from back metres behind the ball to through metres past its centre. The address pose itself is not included; the first pose is one step into the stroke.

Source code in screws/coppelia/golf.py

def stroke_path(ball, hole, *, back: float = 0.06, through: float = 0.04, speed: float = 0.5, dt: float = 0.05):
    """Face poses, one per control step, for a straight stroke at constant speed from back
    metres behind the ball to through metres past its centre. The address pose itself is not
    included; the first pose is one step into the stroke."""
    T0 = address_pose(ball, hole, back=back)
    x = T0[:3, 0]
    length = back + through
    step = speed * dt  # exact face speed; the last pose is clamped to the stroke length
    n = max(1, int(np.ceil(length / step - 1e-9)))
    return [rp_to_transform(T0[:3, :3], T0[:3, 3] + min(k * step, length) * x) for k in range(1, n + 1)]

swing_path

swing_path(
    ball,
    hole,
    *,
    shaft_length,
    back_angle=0.25,
    through_angle=0.2,
    speed=0.3,
    dt=0.01,
    gap=0.01,
    face_thickness=0.02,
    back_time=1.0,
    return_time=1.0,
)

A putting stroke as a pendulum swing of the putter about the wrist.

The address pose puts the face gap metres behind the ball's surface. The pivot is the top of the shaft; the face swings in the vertical plane of the target line: back and up by back_angle (quintic timing over back_time), then down through the ball at a constant angular rate speed / shaft_length (so the face meets the ball at that speed), on to through_angle, and finally back to the address pose over return_time.

Returns {"backswing": [...], "downswing": [...], "back": [...]} of face poses, one per dt.

Source code in screws/coppelia/golf.py

def swing_path(
    ball,
    hole,
    *,
    shaft_length: float,
    back_angle: float = 0.25,
    through_angle: float = 0.2,
    speed: float = 0.3,
    dt: float = 0.01,
    gap: float = 0.01,
    face_thickness: float = 0.02,
    back_time: float = 1.0,
    return_time: float = 1.0,
) -> dict:
    """A putting stroke as a pendulum swing of the putter about the wrist.

    The address pose puts the face gap metres behind the ball's surface. The pivot is the top
    of the shaft; the face swings in the vertical plane of the target line: back and up by
    back_angle (quintic timing over back_time), then down through the ball at a constant
    angular rate speed / shaft_length (so the face meets the ball at that speed), on to
    through_angle, and finally back to the address pose over return_time.

    Returns {"backswing": [...], "downswing": [...], "back": [...]} of face poses, one per dt.
    """
    T0 = address_pose(ball, hole, back=BALL_RADIUS + face_thickness / 2 + gap)
    R0, o0 = T0[:3, :3], T0[:3, 3]
    y = R0[:, 1]
    pivot = o0 + np.array([0.0, 0.0, shaft_length])

    def pose(phi):  # positive phi swings the face backward (-x) and up
        R = exp3(vec_to_so3(-y * phi))
        return rp_to_transform(R @ R0, pivot + R @ (o0 - pivot))

    n_back = max(1, round(back_time / dt))
    backswing = [pose(back_angle * quintic_time_scaling(back_time, k * dt)) for k in range(1, n_back + 1)]
    omega = speed / shaft_length
    n_down = max(1, int(np.ceil((back_angle + through_angle) / (omega * dt) - 1e-9)))
    downswing = [pose(max(back_angle - k * omega * dt, -through_angle)) for k in range(1, n_down + 1)]
    n_ret = max(1, round(return_time / dt))
    back = [pose(-through_angle * (1.0 - quintic_time_scaling(return_time, k * dt))) for k in range(1, n_ret + 1)]
    return {"backswing": backswing, "downswing": downswing, "back": back}

putt

putt(
    scene,
    arm,
    robot_face,
    green,
    *,
    speed=0.3,
    back_angle=0.25,
    through_angle=0.2,
    gap=0.01,
    lift=0.12,
    approach_time=2.0,
    settle_time=5.0,
    seed=None,
    shaft_length=0.3,
    face_thickness=0.02,
)

Address the ball, swing the putter through it toward the hole, and watch it roll.

robot_face is the arm's Robot with M at the putter face (Putter.robot). Put the arm in kinematic mode (arm.mode("kinematic")) and start the simulation first. The approach goes in joint space to a pose lift metres above the address pose and descends straight onto it; the stroke is swing_path's pendulum: backswing, a pause, a downswing meeting the ball at speed, follow-through, and a return to the address pose, where the arm waits until the ball stops or settle_time has passed.

Use a 10 ms control step (scene.set_time_step(0.01)): at the default 50 ms the face jumps 15 mm per step and the strike is no longer clean. 0.3 m/s rolls the ball about 0.3 m on the default green.

Source code in screws/coppelia/golf.py

def putt(
    scene,
    arm,
    robot_face: Robot,
    green: Green,
    *,
    speed: float = 0.3,
    back_angle: float = 0.25,
    through_angle: float = 0.2,
    gap: float = 0.01,
    lift: float = 0.12,
    approach_time: float = 2.0,
    settle_time: float = 5.0,
    seed=None,
    shaft_length: float = 0.30,
    face_thickness: float = 0.02,
) -> PuttResult:
    """Address the ball, swing the putter through it toward the hole, and watch it roll.

    robot_face is the arm's Robot with M at the putter face (Putter.robot). Put the arm in
    kinematic mode (arm.mode("kinematic")) and start the simulation first. The approach goes
    in joint space to a pose lift metres above the address pose and descends straight onto
    it; the stroke is swing_path's pendulum: backswing, a pause, a downswing meeting the ball
    at speed, follow-through, and a return to the address pose, where the arm waits until the
    ball stops or settle_time has passed.

    Use a 10 ms control step (scene.set_time_step(0.01)): at the default 50 ms the face jumps
    15 mm per step and the strike is no longer clean. 0.3 m/s rolls the ball about 0.3 m on
    the default green.
    """
    dt = scene.dt
    ball = green.ball_position()
    hole = green.hole_position
    T_address = address_pose(ball, hole, back=BALL_RADIUS + face_thickness / 2 + gap)
    T_lift = trans([0.0, 0.0, lift]) @ T_address
    theta_now = arm.theta()
    seeds = [theta_now] if seed is None else [np.asarray(seed, float), theta_now]
    theta_lift = _ik_or_raise(robot_face, T_lift, seeds, "lifted address")
    theta_address = _ik_or_raise(robot_face, T_address, [theta_lift], "address")
    ball_path, face_path = [], []

    def go(thetas, log=False):
        for th in thetas:
            arm.command(th)
            scene.step()
            if log:
                ball_path.append(green.ball_position())
                face_path.append(robot_face.fk(arm.theta())[:3, 3])

    def follow(poses, theta, what):
        out = []
        for T in poses:
            theta = _ik_or_raise(robot_face, T, [theta], what)
            out.append(theta)
        return out

    n_approach = max(2, round(approach_time / dt))
    go(joint_trajectory(theta_now, theta_lift, approach_time, n_approach)[1:])
    descent = follow(cartesian_trajectory(T_lift, T_address, approach_time / 2, max(2, n_approach // 2))[1:], theta_lift, "descent")
    go(descent)
    go([theta_address] * round(0.5 / dt))  # a still moment at address

    swing = swing_path(
        ball, hole, shaft_length=shaft_length, back_angle=back_angle, through_angle=through_angle,
        speed=speed, dt=dt, gap=gap, face_thickness=face_thickness,
    )
    backswing = follow(swing["backswing"], theta_address, "backswing")
    go(backswing, log=True)
    go([backswing[-1]] * round(0.3 / dt), log=True)  # the pause at the top
    downswing = follow(swing["downswing"], backswing[-1], "downswing")
    go(downswing, log=True)
    go([downswing[-1]] * round(0.3 / dt), log=True)  # hold the follow-through
    back = follow(swing["back"], downswing[-1], "return")
    go(back, log=True)
    theta = back[-1]
    for k in range(round(settle_time / dt)):
        go([theta], log=True)
        if k > round(0.5 / dt) and green.ball_speed() < 2e-3:
            break
    return PuttResult(
        green.holed(), green.distance_to_hole(), np.array(ball_path), np.array(face_path), theta
    )