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 
309
310
311
312
313
314
315
316
317
318
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 
312
313
314
315
316
317
318
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 
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
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 
251
252
253
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 
240
241
242
@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 
247
248
249
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 
237
238
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 
244
245
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 
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
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 
292
293
294
295
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 
297
298
299
300
301
302
303
304
305
306
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 
278
279
280
281
282
283
284
285
286
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 
281
282
283
284
285
286
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 
43
44
45
46
47
48
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 
46
47
48
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 
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
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 
77
78
79
80
81
82
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 
84
85
86
87
88
89
90
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 
 93
 94
 95
 96
 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
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 
108
109
110
111
112
113
114
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 
116
117
118
119
120
121
122
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 
51
52
53
54
55
56
57
58
59
60
61
62
63
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 
61
62
63
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 
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
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 
259
260
261
262
263
264
265
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 
271
272
273
274
275
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).
Source code in src/skelarm/control.py 
125
126
127
128
129
130
131
132
133
134
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
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)``.
    """
    model = skeleton.clone()
    duration = task_trajectory.duration
    times = np.linspace(0.0, duration, round(duration / dt) + 1)
    seed = model.q.copy()
    q_samples = []
    for t in times:
        target = task_trajectory.sample(float(t))[0]
        compute_inverse_kinematics(model, target, method=method, q0=seed)
        seed = model.q.copy()  # solution is written back to the skeleton; reuse as the next seed
        q_samples.append(seed.copy())
    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 
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
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
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)

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 applied tau, and any channels from controller.log_channels().

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

Returns:

Type Description
StateLog

The recorded run (duration / dt rounded, plus the initial frame).
Source code in src/skelarm/control.py 
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
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,
) -> 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 applied ``tau``, and any
    channels from ``controller.log_channels()``.

    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).

    Returns
    -------
    StateLog
        The recorded run (``duration / dt`` rounded, plus the initial frame).
    """
    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},
    }
    log = StateLog(model, producer=type(controller).__name__, channel_meta=channel_meta, extra=extra)
    controller.reset(model)

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

        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, **controller.log_channels())
    return log