Skip to content

Control API

skelarm.control

Trajectory-tracking control: references, torque controllers, and a fixed-step loop.

This module turns a planned reference into joint torques. A task-space trajectory is first converted to a joint-space reference (samplewise inverse kinematics or resolved motion rate), then a torque controller (joint PD, inverse-dynamics feedforward plus PD, or computed torque) tracks it. :func:simulate_controlled runs a controller against the dynamics with a fixed time step and returns a :class:~skelarm.StateLog for replay and analysis.

See docs/reference/07_control.md for the theory.

ComputedTorque

Bases: TrackingController

Computed torque control: τ = H(q)(q̈_r + Kd ė + Kp e) + b(q,q̇).

Source code in src/skelarm/control.py 
339
340
341
342
343
344
345
346
347
348
class ComputedTorque(TrackingController):
    """Computed torque control: ``τ = H(q)(q̈_r + Kd ė + Kp e) + b(q,q̇)``."""

    def control(self, t: float, skeleton: Skeleton) -> NDArray[np.float64]:
        """Compute the model-cancelling torque for the current state."""
        q_r, dq_r, ddq_r = self.reference.sample(t)
        error = q_r - skeleton.q
        commanded = ddq_r + self.kd * (dq_r - skeleton.dq) + self.kp * error
        self._store(q_r, error)
        return compute_mass_matrix(skeleton) @ commanded + compute_coriolis_gravity_vector(skeleton)

control(t, skeleton)

Compute the model-cancelling torque for the current state.

Source code in src/skelarm/control.py 
342
343
344
345
346
347
348
def control(self, t: float, skeleton: Skeleton) -> NDArray[np.float64]:
    """Compute the model-cancelling torque for the current state."""
    q_r, dq_r, ddq_r = self.reference.sample(t)
    error = q_r - skeleton.q
    commanded = ddq_r + self.kd * (dq_r - skeleton.dq) + self.kp * error
    self._store(q_r, error)
    return compute_mass_matrix(skeleton) @ commanded + compute_coriolis_gravity_vector(skeleton)

Controller

Bases: ABC

Base class for joint-torque controllers.

A controller is callable as f(t, skeleton) -> tau so stateless controllers work directly as a :func:~skelarm.simulate_robot torque callback. Stateful controllers use :meth:reset and :meth:update together with :func:simulate_controlled, and may expose internal signals via :meth:log_channels.

Source code in src/skelarm/control.py 
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
class Controller(ABC):
    """Base class for joint-torque controllers.

    A controller is callable as ``f(t, skeleton) -> tau`` so stateless controllers
    work directly as a :func:`~skelarm.simulate_robot` torque callback. Stateful
    controllers use :meth:`reset` and :meth:`update` together with
    :func:`simulate_controlled`, and may expose internal signals via
    :meth:`log_channels`.
    """

    def reset(self, skeleton: Skeleton) -> None:  # noqa: B027
        """Initialize controller state at movement onset (default: no-op)."""

    @abstractmethod
    def control(self, t: float, skeleton: Skeleton) -> NDArray[np.float64]:
        """Return the joint torque for the current time and skeleton state."""

    def update(self, t: float, skeleton: Skeleton, dt: float) -> None:  # noqa: B027
        """Advance internal state once per fixed control step (default: no-op)."""

    def log_channels(self) -> dict[str, ArrayLike]:
        """Return controller-internal signals to record each step (default: none)."""
        return {}

    def __call__(self, t: float, skeleton: Skeleton) -> NDArray[np.float64]:
        """Evaluate the control law (so the controller is a torque callback)."""
        return self.control(t, skeleton)

__call__(t, skeleton)

Evaluate the control law (so the controller is a torque callback).

Source code in src/skelarm/control.py 
281
282
283
def __call__(self, t: float, skeleton: Skeleton) -> NDArray[np.float64]:
    """Evaluate the control law (so the controller is a torque callback)."""
    return self.control(t, skeleton)

control(t, skeleton) abstractmethod

Return the joint torque for the current time and skeleton state.

Source code in src/skelarm/control.py 
270
271
272
@abstractmethod
def control(self, t: float, skeleton: Skeleton) -> NDArray[np.float64]:
    """Return the joint torque for the current time and skeleton state."""

log_channels()

Return controller-internal signals to record each step (default: none).

Source code in src/skelarm/control.py 
277
278
279
def log_channels(self) -> dict[str, ArrayLike]:
    """Return controller-internal signals to record each step (default: none)."""
    return {}

reset(skeleton)

Initialize controller state at movement onset (default: no-op).

Source code in src/skelarm/control.py 
267
268
def reset(self, skeleton: Skeleton) -> None:  # noqa: B027
    """Initialize controller state at movement onset (default: no-op)."""

update(t, skeleton, dt)

Advance internal state once per fixed control step (default: no-op).

Source code in src/skelarm/control.py 
274
275
def update(self, t: float, skeleton: Skeleton, dt: float) -> None:  # noqa: B027
    """Advance internal state once per fixed control step (default: no-op)."""

InverseDynamicsFeedforwardPD

Bases: TrackingController

Inverse-dynamics feedforward plus PD: τ = ID(q_r,q̇_r,q̈_r) + Kp e + Kd ė.

Source code in src/skelarm/control.py 
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
class InverseDynamicsFeedforwardPD(TrackingController):
    """Inverse-dynamics feedforward plus PD: ``τ = ID(q_r,q̇_r,q̈_r) + Kp e + Kd ė``."""

    def __init__(self, reference: JointReference, kp: ArrayLike, kd: ArrayLike) -> None:
        """Store the reference and gains; the feedforward model is built lazily."""
        super().__init__(reference, kp, kd)
        self._model: Skeleton | None = None

    def control(self, t: float, skeleton: Skeleton) -> NDArray[np.float64]:
        """Compute the feedforward-plus-PD torque for the current state."""
        q_r, dq_r, ddq_r = self.reference.sample(t)
        if self._model is None:
            self._model = skeleton.clone()  # separate model so the reference state does not disturb the plant
        self._model.set_state(q=q_r, dq=dq_r, ddq=ddq_r)
        compute_inverse_dynamics(self._model)
        error = q_r - skeleton.q
        self._store(q_r, error)
        return self._model.tau + self.kp * error + self.kd * (dq_r - skeleton.dq)

__init__(reference, kp, kd)

Store the reference and gains; the feedforward model is built lazily.

Source code in src/skelarm/control.py 
322
323
324
325
def __init__(self, reference: JointReference, kp: ArrayLike, kd: ArrayLike) -> None:
    """Store the reference and gains; the feedforward model is built lazily."""
    super().__init__(reference, kp, kd)
    self._model: Skeleton | None = None

control(t, skeleton)

Compute the feedforward-plus-PD torque for the current state.

Source code in src/skelarm/control.py 
327
328
329
330
331
332
333
334
335
336
def control(self, t: float, skeleton: Skeleton) -> NDArray[np.float64]:
    """Compute the feedforward-plus-PD torque for the current state."""
    q_r, dq_r, ddq_r = self.reference.sample(t)
    if self._model is None:
        self._model = skeleton.clone()  # separate model so the reference state does not disturb the plant
    self._model.set_state(q=q_r, dq=dq_r, ddq=ddq_r)
    compute_inverse_dynamics(self._model)
    error = q_r - skeleton.q
    self._store(q_r, error)
    return self._model.tau + self.kp * error + self.kd * (dq_r - skeleton.dq)

JointPD

Bases: TrackingController

Direct joint-space PD control: τ = Kp(q_r-q) + Kd(q̇_r-q̇).

Source code in src/skelarm/control.py 
308
309
310
311
312
313
314
315
316
class JointPD(TrackingController):
    """Direct joint-space PD control: ``τ = Kp(q_r-q) + Kd(q̇_r-q̇)``."""

    def control(self, t: float, skeleton: Skeleton) -> NDArray[np.float64]:
        """Compute the PD torque for the current state."""
        q_r, dq_r, _ = self.reference.sample(t)
        error = q_r - skeleton.q
        self._store(q_r, error)
        return self.kp * error + self.kd * (dq_r - skeleton.dq)

control(t, skeleton)

Compute the PD torque for the current state.

Source code in src/skelarm/control.py 
311
312
313
314
315
316
def control(self, t: float, skeleton: Skeleton) -> NDArray[np.float64]:
    """Compute the PD torque for the current state."""
    q_r, dq_r, _ = self.reference.sample(t)
    error = q_r - skeleton.q
    self._store(q_r, error)
    return self.kp * error + self.kd * (dq_r - skeleton.dq)

JointReference

Bases: Protocol

A joint-space reference that can be sampled for (q_r, dq_r, ddq_r).

Source code in src/skelarm/control.py 
47
48
49
50
51
52
class JointReference(Protocol):
    """A joint-space reference that can be sampled for ``(q_r, dq_r, ddq_r)``."""

    def sample(self, t: float) -> tuple[NDArray[np.float64], NDArray[np.float64], NDArray[np.float64]]:
        """Return the reference angles, velocities, and accelerations at time ``t``."""
        ...

sample(t)

Return the reference angles, velocities, and accelerations at time t.

Source code in src/skelarm/control.py 
50
51
52
def sample(self, t: float) -> tuple[NDArray[np.float64], NDArray[np.float64], NDArray[np.float64]]:
    """Return the reference angles, velocities, and accelerations at time ``t``."""
    ...

SampledJointReference

A joint reference stored as time-sampled arrays, read by linear interpolation.

Parameters:

Name Type Description Default
times ArrayLike

Sample times, shape (N,), increasing.

 required
q ArrayLike

Joint angles, velocities, and accelerations, each shape (N, num_joints).

 required
dq ArrayLike

Joint angles, velocities, and accelerations, each shape (N, num_joints).

 required
ddq ArrayLike

Joint angles, velocities, and accelerations, each shape (N, num_joints).

 required
Source code in src/skelarm/control.py 
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
class SampledJointReference:
    """A joint reference stored as time-sampled arrays, read by linear interpolation.

    Parameters
    ----------
    times : ArrayLike
        Sample times, shape ``(N,)``, increasing.
    q, dq, ddq : ArrayLike
        Joint angles, velocities, and accelerations, each shape ``(N, num_joints)``.
    """

    def __init__(self, times: ArrayLike, q: ArrayLike, dq: ArrayLike, ddq: ArrayLike) -> None:
        """Store the sampled reference arrays."""
        self.times = np.asarray(times, dtype=np.float64)
        self.q = np.asarray(q, dtype=np.float64)
        self.dq = np.asarray(dq, dtype=np.float64)
        self.ddq = np.asarray(ddq, dtype=np.float64)

    def sample(self, t: float) -> tuple[NDArray[np.float64], NDArray[np.float64], NDArray[np.float64]]:
        """Linearly interpolate the reference at time ``t`` (held at the endpoints)."""
        return (
            _interp_columns(t, self.times, self.q),
            _interp_columns(t, self.times, self.dq),
            _interp_columns(t, self.times, self.ddq),
        )

__init__(times, q, dq, ddq)

Store the sampled reference arrays.

Source code in src/skelarm/control.py 
81
82
83
84
85
86
def __init__(self, times: ArrayLike, q: ArrayLike, dq: ArrayLike, ddq: ArrayLike) -> None:
    """Store the sampled reference arrays."""
    self.times = np.asarray(times, dtype=np.float64)
    self.q = np.asarray(q, dtype=np.float64)
    self.dq = np.asarray(dq, dtype=np.float64)
    self.ddq = np.asarray(ddq, dtype=np.float64)

sample(t)

Linearly interpolate the reference at time t (held at the endpoints).

Source code in src/skelarm/control.py 
88
89
90
91
92
93
94
def sample(self, t: float) -> tuple[NDArray[np.float64], NDArray[np.float64], NDArray[np.float64]]:
    """Linearly interpolate the reference at time ``t`` (held at the endpoints)."""
    return (
        _interp_columns(t, self.times, self.q),
        _interp_columns(t, self.times, self.dq),
        _interp_columns(t, self.times, self.ddq),
    )

SampledTaskReference

A task-space reference stored as time-sampled arrays, read by linear interpolation.

Use this when a (typically pre-smoothed) endpoint path p_r(t) is supplied as samples; it satisfies :class:TaskReference, so :func:ik_joint_reference can turn it into a joint reference for the tracking controllers.

Parameters:

Name Type Description Default
times ArrayLike

Sample times, shape (N,), increasing and starting at 0.

 required
p ArrayLike

Endpoint position, velocity, and acceleration, each shape (N, dim).

 required
dp ArrayLike

Endpoint position, velocity, and acceleration, each shape (N, dim).

 required
ddp ArrayLike

Endpoint position, velocity, and acceleration, each shape (N, dim).

 required
Source code in src/skelarm/control.py 
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
class SampledTaskReference:
    """A task-space reference stored as time-sampled arrays, read by linear interpolation.

    Use this when a (typically pre-smoothed) endpoint path ``p_r(t)`` is supplied as
    samples; it satisfies :class:`TaskReference`, so :func:`ik_joint_reference` can turn
    it into a joint reference for the tracking controllers.

    Parameters
    ----------
    times : ArrayLike
        Sample times, shape ``(N,)``, increasing and starting at ``0``.
    p, dp, ddp : ArrayLike
        Endpoint position, velocity, and acceleration, each shape ``(N, dim)``.
    """

    def __init__(self, times: ArrayLike, p: ArrayLike, dp: ArrayLike, ddp: ArrayLike) -> None:
        """Store the sampled task-space arrays and derive the duration."""
        self.times = np.asarray(times, dtype=np.float64)
        self.p = np.asarray(p, dtype=np.float64)
        self.dp = np.asarray(dp, dtype=np.float64)
        self.ddp = np.asarray(ddp, dtype=np.float64)
        self.duration = float(self.times[-1]) if self.times.size else 0.0

    def sample(self, t: float) -> tuple[NDArray[np.float64], NDArray[np.float64], NDArray[np.float64]]:
        """Linearly interpolate the endpoint reference at time ``t`` (held at the endpoints)."""
        return (
            _interp_columns(t, self.times, self.p),
            _interp_columns(t, self.times, self.dp),
            _interp_columns(t, self.times, self.ddp),
        )

__init__(times, p, dp, ddp)

Store the sampled task-space arrays and derive the duration.

Source code in src/skelarm/control.py 
112
113
114
115
116
117
118
def __init__(self, times: ArrayLike, p: ArrayLike, dp: ArrayLike, ddp: ArrayLike) -> None:
    """Store the sampled task-space arrays and derive the duration."""
    self.times = np.asarray(times, dtype=np.float64)
    self.p = np.asarray(p, dtype=np.float64)
    self.dp = np.asarray(dp, dtype=np.float64)
    self.ddp = np.asarray(ddp, dtype=np.float64)
    self.duration = float(self.times[-1]) if self.times.size else 0.0

sample(t)

Linearly interpolate the endpoint reference at time t (held at the endpoints).

Source code in src/skelarm/control.py 
120
121
122
123
124
125
126
def sample(self, t: float) -> tuple[NDArray[np.float64], NDArray[np.float64], NDArray[np.float64]]:
    """Linearly interpolate the endpoint reference at time ``t`` (held at the endpoints)."""
    return (
        _interp_columns(t, self.times, self.p),
        _interp_columns(t, self.times, self.dp),
        _interp_columns(t, self.times, self.ddp),
    )

TaskReference

Bases: Protocol

A task-space reference p_r(t) sampled for (p, dp, ddp).

Anything exposing .sample(t) and a .duration is a task reference, so :class:~skelarm.Trajectory, :class:SampledTaskReference, and the periodic curves all qualify and can feed :func:ik_joint_reference.

Source code in src/skelarm/control.py 
55
56
57
58
59
60
61
62
63
64
65
66
67
class TaskReference(Protocol):
    """A task-space reference ``p_r(t)`` sampled for ``(p, dp, ddp)``.

    Anything exposing ``.sample(t)`` and a ``.duration`` is a task reference, so
    :class:`~skelarm.Trajectory`, :class:`SampledTaskReference`, and the periodic
    curves all qualify and can feed :func:`ik_joint_reference`.
    """

    duration: float

    def sample(self, t: float) -> tuple[NDArray[np.float64], NDArray[np.float64], NDArray[np.float64]]:
        """Return the reference position, velocity, and acceleration at time ``t``."""
        ...

sample(t)

Return the reference position, velocity, and acceleration at time t.

Source code in src/skelarm/control.py 
65
66
67
def sample(self, t: float) -> tuple[NDArray[np.float64], NDArray[np.float64], NDArray[np.float64]]:
    """Return the reference position, velocity, and acceleration at time ``t``."""
    ...

TrackingController

Bases: Controller

A controller that tracks a joint reference, logging q_ref and error.

Source code in src/skelarm/control.py 
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
class TrackingController(Controller):
    """A controller that tracks a joint reference, logging ``q_ref`` and ``error``."""

    def __init__(self, reference: JointReference, kp: ArrayLike, kd: ArrayLike) -> None:
        """Store the reference and (scalar or per-joint) PD gains."""
        self.reference = reference
        self.kp = np.asarray(kp, dtype=np.float64)
        self.kd = np.asarray(kd, dtype=np.float64)
        self._q_ref: NDArray[np.float64] | None = None
        self._error: NDArray[np.float64] | None = None

    def _store(self, q_ref: NDArray[np.float64], error: NDArray[np.float64]) -> None:
        self._q_ref = q_ref
        self._error = error

    def log_channels(self) -> dict[str, ArrayLike]:
        """Record the joint reference and tracking error once tracking has begun."""
        if self._q_ref is None or self._error is None:
            return {}
        return {"q_ref": self._q_ref, "error": self._error}

__init__(reference, kp, kd)

Store the reference and (scalar or per-joint) PD gains.

Source code in src/skelarm/control.py 
289
290
291
292
293
294
295
def __init__(self, reference: JointReference, kp: ArrayLike, kd: ArrayLike) -> None:
    """Store the reference and (scalar or per-joint) PD gains."""
    self.reference = reference
    self.kp = np.asarray(kp, dtype=np.float64)
    self.kd = np.asarray(kd, dtype=np.float64)
    self._q_ref: NDArray[np.float64] | None = None
    self._error: NDArray[np.float64] | None = None

log_channels()

Record the joint reference and tracking error once tracking has begun.

Source code in src/skelarm/control.py 
301
302
303
304
305
def log_channels(self) -> dict[str, ArrayLike]:
    """Record the joint reference and tracking error once tracking has begun."""
    if self._q_ref is None or self._error is None:
        return {}
    return {"q_ref": self._q_ref, "error": self._error}

ik_joint_reference(skeleton, task_trajectory, *, dt, method='lm_sugihara')

Convert a task-space trajectory to a joint reference with samplewise IK.

Each sample is solved with :func:compute_inverse_kinematics, warm-started from the previous solution for joint continuity. Velocities and accelerations are estimated by finite differences. The input skeleton is not mutated.

Parameters:

Name Type Description Default
skeleton Skeleton

The arm to solve for; its current pose seeds the first IK solve.

 required
task_trajectory TaskReference

The desired endpoint trajectory p_r(t) (any :class:TaskReference: a planned :class:~skelarm.Trajectory, a :class:SampledTaskReference, or a periodic curve).

 required
dt float

Sampling interval (seconds).

 required
method str

IK method passed to :func:compute_inverse_kinematics.

 'lm_sugihara'

Returns:

Type Description
SampledJointReference

The joint reference (q_r, dq_r, ddq_r).

Warns:

Type Description
UserWarning

When one or more samples end more than 1e-4 m away from the task path (e.g. it leaves the workspace or a joint limit blocks it). One aggregated warning reports the failure count, the first failing sample time, and the worst residual; the best-effort joint angles are kept, so the reference deviates from the task path at those samples.
Source code in src/skelarm/control.py 
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
def ik_joint_reference(
    skeleton: Skeleton,
    task_trajectory: TaskReference,
    *,
    dt: float,
    method: str = "lm_sugihara",
) -> SampledJointReference:
    """Convert a task-space trajectory to a joint reference with samplewise IK.

    Each sample is solved with :func:`compute_inverse_kinematics`, warm-started
    from the previous solution for joint continuity. Velocities and accelerations
    are estimated by finite differences. The input ``skeleton`` is not mutated.

    Parameters
    ----------
    skeleton : Skeleton
        The arm to solve for; its current pose seeds the first IK solve.
    task_trajectory : TaskReference
        The desired endpoint trajectory ``p_r(t)`` (any :class:`TaskReference`:
        a planned :class:`~skelarm.Trajectory`, a :class:`SampledTaskReference`, or a
        periodic curve).
    dt : float
        Sampling interval (seconds).
    method : str, optional
        IK method passed to :func:`compute_inverse_kinematics`.

    Returns
    -------
    SampledJointReference
        The joint reference ``(q_r, dq_r, ddq_r)``.

    Warns
    -----
    UserWarning
        When one or more samples end more than ``1e-4`` m away from the task
        path (e.g. it leaves the workspace or a joint limit blocks it). One
        aggregated warning reports the failure count, the first failing sample
        time, and the worst residual; the best-effort joint angles are kept, so
        the reference deviates from the task path at those samples.
    """
    model = skeleton.clone()
    duration = task_trajectory.duration
    times = np.linspace(0.0, duration, round(duration / dt) + 1)
    seed = model.q.copy()
    q_samples = []
    failures: list[tuple[float, float]] = []  # (sample time, residual norm)
    for t in times:
        target = task_trajectory.sample(float(t))[0]
        result = compute_inverse_kinematics(model, target, method=method, q0=seed)
        if result.residual_norm > _REFERENCE_WARN_RESIDUAL:
            failures.append((float(t), result.residual_norm))
        seed = model.q.copy()  # solution is written back to the skeleton; reuse as the next seed
        q_samples.append(seed.copy())
    if failures:
        worst_t, worst_residual = max(failures, key=lambda item: item[1])
        warnings.warn(
            f"inverse kinematics did not converge on {len(failures)} of {times.size} reference samples "
            f"(first at t={failures[0][0]:.3g} s; worst residual {worst_residual:.3g} m at t={worst_t:.3g} s); "
            "the joint reference deviates from the task path there",
            stacklevel=2,
        )
    q = np.array(q_samples, dtype=np.float64)
    dq = np.gradient(q, times, axis=0)
    ddq = np.gradient(dq, times, axis=0)
    return SampledJointReference(times, q, dq, ddq)

resolved_rate_joint_reference(skeleton, task_trajectory, *, dt, damping=0.001, k_task=0.0)

Convert a task-space trajectory to a joint reference by resolved motion rate.

Integrates q̇_r = Jᵀ(JJᵀ+µI)⁻¹(ṗ_r + K_task(p_r - p)) forward in time. The task-feedback term K_task corrects drift from the desired path. The input skeleton is not mutated.

Parameters:

Name Type Description Default
skeleton Skeleton

The arm to convert for; its current pose is the integration start.

 required
task_trajectory TaskReference

The desired endpoint trajectory.

 required
dt float

Integration step (seconds).

 required
damping float

Damping µ for the damped pseudoinverse.

 0.001
k_task float

Task-space position-feedback gain.

 0.0

Returns:

Type Description
SampledJointReference

The joint reference (q_r, dq_r, ddq_r).
Source code in src/skelarm/control.py 
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
def resolved_rate_joint_reference(
    skeleton: Skeleton,
    task_trajectory: TaskReference,
    *,
    dt: float,
    damping: float = 1e-3,
    k_task: float = 0.0,
) -> SampledJointReference:
    """Convert a task-space trajectory to a joint reference by resolved motion rate.

    Integrates ``q̇_r = Jᵀ(JJᵀ+µI)⁻¹(ṗ_r + K_task(p_r - p))`` forward in time. The
    task-feedback term ``K_task`` corrects drift from the desired path. The input
    ``skeleton`` is not mutated.

    Parameters
    ----------
    skeleton : Skeleton
        The arm to convert for; its current pose is the integration start.
    task_trajectory : TaskReference
        The desired endpoint trajectory.
    dt : float
        Integration step (seconds).
    damping : float, optional
        Damping ``µ`` for the damped pseudoinverse.
    k_task : float, optional
        Task-space position-feedback gain.

    Returns
    -------
    SampledJointReference
        The joint reference ``(q_r, dq_r, ddq_r)``.
    """
    model = skeleton.clone()
    duration = task_trajectory.duration
    times = np.linspace(0.0, duration, round(duration / dt) + 1)
    q = model.q.copy()
    q_samples = []
    dq_samples = []
    for t in times:
        model.q = q  # eager setter refreshes forward kinematics
        tip = model.links[-1]
        position = np.array([tip.xe, tip.ye], dtype=np.float64)
        p_r, dp_r, _ = task_trajectory.sample(float(t))
        jacobian = compute_jacobian(model)
        rhs = dp_r + k_task * (p_r - position)
        qdot = jacobian.T @ np.linalg.solve(jacobian @ jacobian.T + damping * np.eye(2), rhs)
        q_samples.append(q.copy())
        dq_samples.append(qdot.copy())
        q = q + qdot * dt
    q_arr = np.array(q_samples, dtype=np.float64)
    dq_arr = np.array(dq_samples, dtype=np.float64)
    ddq_arr = np.gradient(dq_arr, times, axis=0)
    return SampledJointReference(times, q_arr, dq_arr, ddq_arr)

simulate_controlled(skeleton, controller, *, duration, dt=0.001, grav_vec=None, enforce_limits=True, extra=None, external_force=None)

Run a controller against the dynamics with a fixed time step.

Uses semi-implicit (symplectic) Euler, the same integration as the GUI simulator. A clone of skeleton is simulated, so the caller's skeleton is not mutated. Each frame records q, dq, the controller's tau, and any channels from controller.log_channels().

An external_force scripts a disturbance at the arm's tip, such as a push or a hand that holds the arm. It is a tip force acting in addition to the controller's torque, mapped to the joints as J^T F like the mouse force of the GUI simulator, and recorded as the ext_force channel, which tools/player.py draws as an arrow.

Parameters:

Name Type Description Default
skeleton Skeleton

Initial state (q / dq); not mutated.

 required
controller Controller

The torque controller to run.

 required
duration float

Total simulated time (seconds).

 required
dt float

Fixed control/integration step (seconds).

 0.001
grav_vec NDArray[float64] | None

Gravity vector; defaults to zero (planar motion).

 None
enforce_limits bool

Apply the joint limits as hard stops in the dynamics (default). When False, the limits no longer constrain the simulation, so they apply only to the kinematics (posing and inverse kinematics).

 True
extra Mapping[str, Any] | None

Free-form metadata to embed in the returned log's [extra] table (e.g. the scenario config that produced the run, for later reproduction).

 None
external_force Callable[[float, Skeleton], ArrayLike] | None

f(t, skeleton) -> (fx, fy): the tip force (N, base frame) at time t and the current state, called once per step after the controller. It must not modify the skeleton. None (the default) applies none and records no ext_force channel.

 None

Returns:

Type Description
StateLog

The recorded run (duration / dt rounded, plus the initial frame).

Raises:

Type Description
ValueError

If external_force returns anything but a force (fx, fy).
Source code in src/skelarm/control.py 
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
def simulate_controlled(
    skeleton: Skeleton,
    controller: Controller,
    *,
    duration: float,
    dt: float = 1e-3,
    grav_vec: NDArray[np.float64] | None = None,
    enforce_limits: bool = True,
    extra: Mapping[str, Any] | None = None,
    external_force: Callable[[float, Skeleton], ArrayLike] | None = None,
) -> StateLog:
    """Run a controller against the dynamics with a fixed time step.

    Uses semi-implicit (symplectic) Euler, the same integration as the GUI
    simulator. A clone of ``skeleton`` is simulated, so the caller's skeleton is
    not mutated. Each frame records ``q``, ``dq``, the controller's ``tau``, and any
    channels from ``controller.log_channels()``.

    An ``external_force`` scripts a disturbance at the arm's tip, such as a push or
    a hand that holds the arm. It is a tip force acting in addition to the
    controller's torque, mapped to the joints as ``J^T F`` like the mouse force of
    the GUI simulator, and recorded as the ``ext_force`` channel, which
    ``tools/player.py`` draws as an arrow.

    Parameters
    ----------
    skeleton : Skeleton
        Initial state (``q`` / ``dq``); not mutated.
    controller : Controller
        The torque controller to run.
    duration : float
        Total simulated time (seconds).
    dt : float, optional
        Fixed control/integration step (seconds).
    grav_vec : NDArray[np.float64] | None, optional
        Gravity vector; defaults to zero (planar motion).
    enforce_limits : bool, optional
        Apply the joint limits as hard stops in the dynamics (default). When
        ``False``, the limits no longer constrain the simulation, so they apply
        only to the kinematics (posing and inverse kinematics).
    extra : Mapping[str, Any] | None, optional
        Free-form metadata to embed in the returned log's ``[extra]`` table (e.g.
        the scenario config that produced the run, for later reproduction).
    external_force : Callable[[float, Skeleton], ArrayLike] | None, optional
        ``f(t, skeleton) -> (fx, fy)``: the tip force (N, base frame) at time ``t``
        and the current state, called once per step after the controller. It must
        not modify the skeleton. ``None`` (the default) applies none and records no
        ``ext_force`` channel.

    Returns
    -------
    StateLog
        The recorded run (``duration / dt`` rounded, plus the initial frame).

    Raises
    ------
    ValueError
        If ``external_force`` returns anything but a force ``(fx, fy)``.
    """
    model = skeleton.clone()
    lower = upper = None
    if enforce_limits:
        lower = np.array([link.prop.qmin for link in model.links[1:]], dtype=np.float64)
        upper = np.array([link.prop.qmax for link in model.links[1:]], dtype=np.float64)
    joints = [f"j{i + 1}" for i in range(model.num_joints)]
    channel_meta = {
        "q": {"unit": "rad", "label": "joint angle", "columns": joints},
        "dq": {"unit": "rad/s", "label": "joint velocity", "columns": joints},
        "tau": {"unit": "N*m", "label": "applied joint torque", "columns": joints},
    }
    if external_force is not None:
        channel_meta["ext_force"] = {"unit": "N", "label": "external tip force", "columns": ["fx", "fy"]}
    log = StateLog(model, producer=type(controller).__name__, channel_meta=channel_meta, extra=extra)
    controller.reset(model)

    def tip_force(t: float) -> dict[str, NDArray[np.float64]]:
        """The scripted tip force at time ``t`` as a log channel (none without a script)."""
        if external_force is None:
            return {}
        # A copy, since a callback may return the same buffer every step and the log keeps each frame's array.
        force = np.array(external_force(t, model), dtype=np.float64)
        if force.shape != (2,):
            msg = f"external_force must return a tip force (fx, fy), got shape {force.shape}"
            raise ValueError(msg)
        return {"ext_force": force}

    t = 0.0
    for _ in range(round(duration / dt)):
        controller.update(t, model, dt)
        tau = controller.control(t, model)
        force = tip_force(t)
        log.record(t, q=model.q, dq=model.dq, tau=tau, **force, **controller.log_channels())

        if force:
            tau = tau + compute_jacobian(model).T @ force["ext_force"]
        integrate_with_limits(model, tau, dt, lower, upper, grav_vec)
        t += dt

    # Final frame: record the settled state with the torque that would be applied next.
    tau = controller.control(t, model)
    log.record(t, q=model.q, dq=model.dq, tau=tau, **tip_force(t), **controller.log_channels())
    return log