Skip to content

Reaching API

skelarm.reaching

Endpoint-feedback reaching controllers.

These controllers move the endpoint to a task-space target p* using a virtual spring-damper in task space, rather than tracking a preplanned time trajectory. They range from a constant target spring through time-varying stiffness to online reference shaping with a shaped equilibrium p_s. All are stateful endpoint controllers run via :func:skelarm.control.simulate_controlled.

Isotropic (scalar) task-space stiffness and damping are used, matching the planar reaching formulation. See docs/reference/08_reaching_control.md for the theory.

AdaptiveReferenceShaping

Bases: OnlineReferenceShaping

Online reference shaping with slow/rapid adaptation of the apparent initial endpoint.

Extends online reference shaping (Seto-Sugihara IROS 2010) so the reach stays smooth when the target changes mid-motion or a large external force displaces the endpoint. An apparent initial endpoint p_a normalizes the shaping ratio :func:adaptive_shaping_ratio; it drifts toward the current endpoint with a first-order lag (slow adaptation) and is reset to the endpoint when the endpoint leaves the current reaching region (rapid adaptation).

Parameters:

Name Type Description Default
target ArrayLike

The task-space target (may be reassigned during motion).

 required
k_task float

Task-space stiffness and damping.

 required
d_task float

Task-space stiffness and damping.

 required
c_joint float

Joint viscous damping gain.

 0.0
epsilon float

Small positive floor for the shaping ratio.

 0.01
t_adapt float

Slow-adaptation time constant T (seconds).

 0.5
t1 float

Lag-filter time constants (seconds).

 0.1
t2 float

Lag-filter time constants (seconds).

 0.1
Source code in src/skelarm/reaching.py 
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
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
class AdaptiveReferenceShaping(OnlineReferenceShaping):
    """Online reference shaping with slow/rapid adaptation of the apparent initial endpoint.

    Extends online reference shaping (Seto-Sugihara IROS 2010) so the reach stays
    smooth when the target changes mid-motion or a large external force displaces the
    endpoint. An apparent initial endpoint ``p_a`` normalizes the shaping ratio
    :func:`adaptive_shaping_ratio`; it drifts toward the current endpoint with a
    first-order lag (slow adaptation) and is reset to the endpoint when the endpoint
    leaves the current reaching region (rapid adaptation).

    Parameters
    ----------
    target : ArrayLike
        The task-space target (may be reassigned during motion).
    k_task, d_task : float
        Task-space stiffness and damping.
    c_joint : float, optional
        Joint viscous damping gain.
    epsilon : float, optional
        Small positive floor for the shaping ratio.
    t_adapt : float, optional
        Slow-adaptation time constant ``T`` (seconds).
    t1, t2 : float, optional
        Lag-filter time constants (seconds).
    """

    def __init__(
        self,
        target: ArrayLike,
        *,
        k_task: float,
        d_task: float,
        c_joint: float = 0.0,
        epsilon: float = 0.01,
        t_adapt: float = 0.5,
        t1: float = 0.1,
        t2: float = 0.1,
    ) -> None:
        """Store the shaping floor and slow-adaptation time constant."""
        super().__init__(target, k_task=k_task, d_task=d_task, c_joint=c_joint, r=epsilon, t1=t1, t2=t2)
        self.epsilon = float(epsilon)
        self.t_adapt = float(t_adapt)
        self._apparent: NDArray[np.float64] | None = None  # apparent initial endpoint p_a

    @property
    def apparent_initial(self) -> NDArray[np.float64] | None:
        """The current apparent initial endpoint ``p_a`` (``None`` before reset)."""
        return self._apparent

    def reset(self, skeleton: Skeleton) -> None:
        """Initialize the apparent initial endpoint to the current endpoint."""
        super().reset(skeleton)
        self._apparent = _endpoint_position(skeleton)

    def update(self, t: float, skeleton: Skeleton, dt: float) -> None:
        """Adapt ``p_a``, recompute the shaping ratio, then advance the lag filter."""
        if self._apparent is None:
            self.reset(skeleton)
        assert self._apparent is not None  # set by reset
        position = _endpoint_position(skeleton)
        remaining = float(np.linalg.norm(self.target - position))
        span = float(np.linalg.norm(self.target - self._apparent))
        # Rapid adaptation: if the endpoint left the reaching region, reset p_a to it.
        if span > _MIN_SPAN and remaining / span > 1.0:
            self._apparent = position.copy()
            span = remaining
        self.r = adaptive_shaping_ratio(remaining, span, self.epsilon)
        # Slow adaptation: drift p_a toward the current endpoint with a first-order lag.
        self._apparent = self._apparent + dt * (position - self._apparent) / self.t_adapt
        super().update(t, skeleton, dt)

    def log_channels(self) -> dict[str, ArrayLike]:
        """Record the endpoint, equilibrium, and apparent initial endpoint."""
        channels = super().log_channels()
        if channels and self._apparent is not None:
            channels["apparent_initial"] = self._apparent
        return channels

apparent_initial property

The current apparent initial endpoint p_a (None before reset).

__init__(target, *, k_task, d_task, c_joint=0.0, epsilon=0.01, t_adapt=0.5, t1=0.1, t2=0.1)

Store the shaping floor and slow-adaptation time constant.

Source code in src/skelarm/reaching.py 
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
def __init__(
    self,
    target: ArrayLike,
    *,
    k_task: float,
    d_task: float,
    c_joint: float = 0.0,
    epsilon: float = 0.01,
    t_adapt: float = 0.5,
    t1: float = 0.1,
    t2: float = 0.1,
) -> None:
    """Store the shaping floor and slow-adaptation time constant."""
    super().__init__(target, k_task=k_task, d_task=d_task, c_joint=c_joint, r=epsilon, t1=t1, t2=t2)
    self.epsilon = float(epsilon)
    self.t_adapt = float(t_adapt)
    self._apparent: NDArray[np.float64] | None = None  # apparent initial endpoint p_a

log_channels()

Record the endpoint, equilibrium, and apparent initial endpoint.

Source code in src/skelarm/reaching.py 
380
381
382
383
384
385
def log_channels(self) -> dict[str, ArrayLike]:
    """Record the endpoint, equilibrium, and apparent initial endpoint."""
    channels = super().log_channels()
    if channels and self._apparent is not None:
        channels["apparent_initial"] = self._apparent
    return channels

reset(skeleton)

Initialize the apparent initial endpoint to the current endpoint.

Source code in src/skelarm/reaching.py 
358
359
360
361
def reset(self, skeleton: Skeleton) -> None:
    """Initialize the apparent initial endpoint to the current endpoint."""
    super().reset(skeleton)
    self._apparent = _endpoint_position(skeleton)

update(t, skeleton, dt)

Adapt p_a, recompute the shaping ratio, then advance the lag filter.

Source code in src/skelarm/reaching.py 
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
def update(self, t: float, skeleton: Skeleton, dt: float) -> None:
    """Adapt ``p_a``, recompute the shaping ratio, then advance the lag filter."""
    if self._apparent is None:
        self.reset(skeleton)
    assert self._apparent is not None  # set by reset
    position = _endpoint_position(skeleton)
    remaining = float(np.linalg.norm(self.target - position))
    span = float(np.linalg.norm(self.target - self._apparent))
    # Rapid adaptation: if the endpoint left the reaching region, reset p_a to it.
    if span > _MIN_SPAN and remaining / span > 1.0:
        self._apparent = position.copy()
        span = remaining
    self.r = adaptive_shaping_ratio(remaining, span, self.epsilon)
    # Slow adaptation: drift p_a toward the current endpoint with a first-order lag.
    self._apparent = self._apparent + dt * (position - self._apparent) / self.t_adapt
    super().update(t, skeleton, dt)

EndpointController

Bases: Controller

Base class for task-space spring-damper controllers.

Implements the shared law tau = J^T (k (p_eq - p) - D pdot) - Cq qdot given a spring equilibrium p_eq and spring stiffness k chosen by subclasses.

Parameters:

Name Type Description Default
target ArrayLike

The task-space target p* (x, y).

 required
k_task float

Task-space spring stiffness.

 required
d_task float

Task-space damping gain.

 required
c_joint float

Joint viscous damping gain Cq.

 0.0
Source code in src/skelarm/reaching.py 
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 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
123
124
125
126
127
128
129
130
131
132
class EndpointController(Controller):
    """Base class for task-space spring-damper controllers.

    Implements the shared law ``tau = J^T (k (p_eq - p) - D pdot) - Cq qdot`` given
    a spring equilibrium ``p_eq`` and spring stiffness ``k`` chosen by subclasses.

    Parameters
    ----------
    target : ArrayLike
        The task-space target ``p*`` (x, y).
    k_task : float
        Task-space spring stiffness.
    d_task : float
        Task-space damping gain.
    c_joint : float, optional
        Joint viscous damping gain ``Cq``.
    """

    def __init__(self, target: ArrayLike, *, k_task: float, d_task: float, c_joint: float = 0.0) -> None:
        """Store the target and the stiffness / damping gains."""
        self.target = target  # via the property setter below
        self.k_task = float(k_task)
        self.d_task = float(d_task)
        self.c_joint = float(c_joint)
        self._endpoint: NDArray[np.float64] | None = None
        self._equilibrium: NDArray[np.float64] | None = None

    @property
    def target(self) -> NDArray[np.float64]:
        """The task-space target ``p*``. Reassign it (then :meth:`reset`) to switch goals live."""
        return self._target

    @target.setter
    def target(self, value: ArrayLike) -> None:
        self._target = np.asarray(value, dtype=np.float64)

    def _endpoint_torque(self, skeleton: Skeleton, equilibrium: NDArray[np.float64], k: float) -> NDArray[np.float64]:
        """Compute the spring-damper joint torque toward ``equilibrium`` with stiffness ``k``."""
        position = _endpoint_position(skeleton)
        velocity = compute_endpoint_velocity(skeleton)
        force = k * (equilibrium - position) - self.d_task * velocity
        torque = compute_jacobian(skeleton).T @ force - self.c_joint * skeleton.dq
        self._endpoint = position
        self._equilibrium = equilibrium
        return torque

    def log_channels(self) -> dict[str, ArrayLike]:
        """Record the endpoint and the spring equilibrium once control has run."""
        if self._endpoint is None or self._equilibrium is None:
            return {}
        return {"endpoint": self._endpoint, "equilibrium": self._equilibrium}

target property writable

The task-space target p*. Reassign it (then :meth:reset) to switch goals live.

__init__(target, *, k_task, d_task, c_joint=0.0)

Store the target and the stiffness / damping gains.

Source code in src/skelarm/reaching.py 
100
101
102
103
104
105
106
107
def __init__(self, target: ArrayLike, *, k_task: float, d_task: float, c_joint: float = 0.0) -> None:
    """Store the target and the stiffness / damping gains."""
    self.target = target  # via the property setter below
    self.k_task = float(k_task)
    self.d_task = float(d_task)
    self.c_joint = float(c_joint)
    self._endpoint: NDArray[np.float64] | None = None
    self._equilibrium: NDArray[np.float64] | None = None

log_channels()

Record the endpoint and the spring equilibrium once control has run.

Source code in src/skelarm/reaching.py 
128
129
130
131
132
def log_channels(self) -> dict[str, ArrayLike]:
    """Record the endpoint and the spring equilibrium once control has run."""
    if self._endpoint is None or self._equilibrium is None:
        return {}
    return {"endpoint": self._endpoint, "equilibrium": self._equilibrium}

OnlineReferenceShaping

Bases: EndpointController

Online reference shaping with a fixed ratio r (Seto-Sugihara IROS 2009).

The spring equilibrium is a shaped reference p_s produced by passing r p* + (1-r) p through a second-order lag filter (two cascaded first-order lags with time constants t1 and t2). Feeding back the current endpoint keeps p_s near the arm when the endpoint is constrained.

Parameters:

Name Type Description Default
target ArrayLike

The task-space target.

 required
k_task float

Task-space stiffness and damping.

 required
d_task float

Task-space stiffness and damping.

 required
c_joint float

Joint viscous damping gain.

 0.0
r float

Fixed shaping ratio in (0, 1].

 0.3
t1 float

Lag-filter time constants (seconds).

 0.1
t2 float

Lag-filter time constants (seconds).

 0.1
Source code in src/skelarm/reaching.py 
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
class OnlineReferenceShaping(EndpointController):
    """Online reference shaping with a fixed ratio ``r`` (Seto-Sugihara IROS 2009).

    The spring equilibrium is a shaped reference ``p_s`` produced by passing
    ``r p* + (1-r) p`` through a second-order lag filter (two cascaded first-order
    lags with time constants ``t1`` and ``t2``). Feeding back the current endpoint
    keeps ``p_s`` near the arm when the endpoint is constrained.

    Parameters
    ----------
    target : ArrayLike
        The task-space target.
    k_task, d_task : float
        Task-space stiffness and damping.
    c_joint : float, optional
        Joint viscous damping gain.
    r : float, optional
        Fixed shaping ratio in ``(0, 1]``.
    t1, t2 : float, optional
        Lag-filter time constants (seconds).
    """

    def __init__(
        self,
        target: ArrayLike,
        *,
        k_task: float,
        d_task: float,
        c_joint: float = 0.0,
        r: float = 0.3,
        t1: float = 0.1,
        t2: float = 0.1,
    ) -> None:
        """Store the shaping ratio and lag-filter time constants."""
        super().__init__(target, k_task=k_task, d_task=d_task, c_joint=c_joint)
        self.r = float(r)
        self.t1 = float(t1)
        self.t2 = float(t2)
        self._filter_state: NDArray[np.float64] | None = None  # intermediate lag output
        self._shaped: NDArray[np.float64] | None = None  # shaped equilibrium p_s

    def reset(self, skeleton: Skeleton) -> None:
        """Initialize the shaped reference to the current endpoint at movement onset."""
        position = _endpoint_position(skeleton)
        self._filter_state = position.copy()
        self._shaped = position.copy()

    def update(self, t: float, skeleton: Skeleton, dt: float) -> None:  # noqa: ARG002
        """Advance the second-order lag filter that produces the shaped equilibrium."""
        if self._filter_state is None or self._shaped is None:
            self.reset(skeleton)
            return
        position = _endpoint_position(skeleton)
        target_input = self.r * self.target + (1.0 - self.r) * position
        self._filter_state = self._filter_state + dt * (target_input - self._filter_state) / self.t1
        self._shaped = self._shaped + dt * (self._filter_state - self._shaped) / self.t2

    def control(self, t: float, skeleton: Skeleton) -> NDArray[np.float64]:  # noqa: ARG002
        """Return the spring-damper torque toward the shaped equilibrium ``p_s``."""
        if self._shaped is None:
            self.reset(skeleton)
        assert self._shaped is not None  # set by reset
        return self._endpoint_torque(skeleton, self._shaped, self.k_task)

__init__(target, *, k_task, d_task, c_joint=0.0, r=0.3, t1=0.1, t2=0.1)

Store the shaping ratio and lag-filter time constants.

Source code in src/skelarm/reaching.py 
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
def __init__(
    self,
    target: ArrayLike,
    *,
    k_task: float,
    d_task: float,
    c_joint: float = 0.0,
    r: float = 0.3,
    t1: float = 0.1,
    t2: float = 0.1,
) -> None:
    """Store the shaping ratio and lag-filter time constants."""
    super().__init__(target, k_task=k_task, d_task=d_task, c_joint=c_joint)
    self.r = float(r)
    self.t1 = float(t1)
    self.t2 = float(t2)
    self._filter_state: NDArray[np.float64] | None = None  # intermediate lag output
    self._shaped: NDArray[np.float64] | None = None  # shaped equilibrium p_s

control(t, skeleton)

Return the spring-damper torque toward the shaped equilibrium p_s.

Source code in src/skelarm/reaching.py 
247
248
249
250
251
252
def control(self, t: float, skeleton: Skeleton) -> NDArray[np.float64]:  # noqa: ARG002
    """Return the spring-damper torque toward the shaped equilibrium ``p_s``."""
    if self._shaped is None:
        self.reset(skeleton)
    assert self._shaped is not None  # set by reset
    return self._endpoint_torque(skeleton, self._shaped, self.k_task)

reset(skeleton)

Initialize the shaped reference to the current endpoint at movement onset.

Source code in src/skelarm/reaching.py 
231
232
233
234
235
def reset(self, skeleton: Skeleton) -> None:
    """Initialize the shaped reference to the current endpoint at movement onset."""
    position = _endpoint_position(skeleton)
    self._filter_state = position.copy()
    self._shaped = position.copy()

update(t, skeleton, dt)

Advance the second-order lag filter that produces the shaped equilibrium.

Source code in src/skelarm/reaching.py 
237
238
239
240
241
242
243
244
245
def update(self, t: float, skeleton: Skeleton, dt: float) -> None:  # noqa: ARG002
    """Advance the second-order lag filter that produces the shaped equilibrium."""
    if self._filter_state is None or self._shaped is None:
        self.reset(skeleton)
        return
    position = _endpoint_position(skeleton)
    target_input = self.r * self.target + (1.0 - self.r) * position
    self._filter_state = self._filter_state + dt * (target_input - self._filter_state) / self.t1
    self._shaped = self._shaped + dt * (self._filter_state - self._shaped) / self.t2

PositionDependentShaping

Bases: OnlineReferenceShaping

Online reference shaping with a position-dependent ratio r(d) (Humanoids 2009).

Extends :class:OnlineReferenceShaping by recomputing the shaping ratio each step from the normalized remaining distance, so the reach starts gently and converges faster near the target.

Parameters:

Name Type Description Default
target ArrayLike

The task-space target.

 required
k_task float

Task-space stiffness and damping.

 required
d_task float

Task-space stiffness and damping.

 required
c_joint float

Joint viscous damping gain.

 0.0
a float

Small positive shaping constant for :func:shaping_ratio.

 0.01
t1 float

Lag-filter time constants (seconds).

 0.1
t2 float

Lag-filter time constants (seconds).

 0.1
Source code in src/skelarm/reaching.py 
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
class PositionDependentShaping(OnlineReferenceShaping):
    """Online reference shaping with a position-dependent ratio ``r(d)`` (Humanoids 2009).

    Extends :class:`OnlineReferenceShaping` by recomputing the shaping ratio each
    step from the normalized remaining distance, so the reach starts gently and
    converges faster near the target.

    Parameters
    ----------
    target : ArrayLike
        The task-space target.
    k_task, d_task : float
        Task-space stiffness and damping.
    c_joint : float, optional
        Joint viscous damping gain.
    a : float, optional
        Small positive shaping constant for :func:`shaping_ratio`.
    t1, t2 : float, optional
        Lag-filter time constants (seconds).
    """

    def __init__(
        self,
        target: ArrayLike,
        *,
        k_task: float,
        d_task: float,
        c_joint: float = 0.0,
        a: float = 0.01,
        t1: float = 0.1,
        t2: float = 0.1,
    ) -> None:
        """Store the shaping constant; the ratio ``r`` is recomputed each step."""
        super().__init__(target, k_task=k_task, d_task=d_task, c_joint=c_joint, r=0.0, t1=t1, t2=t2)
        self.a = float(a)
        self._initial: NDArray[np.float64] | None = None  # endpoint at movement onset

    def reset(self, skeleton: Skeleton) -> None:
        """Record the onset endpoint used to normalize the remaining distance."""
        super().reset(skeleton)
        self._initial = _endpoint_position(skeleton)

    def update(self, t: float, skeleton: Skeleton, dt: float) -> None:
        """Recompute ``r(d)`` from the remaining distance, then advance the lag filter."""
        if self._initial is None:
            self.reset(skeleton)
        assert self._initial is not None  # set by reset
        position = _endpoint_position(skeleton)
        span = float(np.linalg.norm(self.target - self._initial))
        d = float(np.linalg.norm(self.target - position)) / span if span > _MIN_SPAN else 0.0
        self.r = shaping_ratio(d, self.a)
        super().update(t, skeleton, dt)

__init__(target, *, k_task, d_task, c_joint=0.0, a=0.01, t1=0.1, t2=0.1)

Store the shaping constant; the ratio r is recomputed each step.

Source code in src/skelarm/reaching.py 
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
def __init__(
    self,
    target: ArrayLike,
    *,
    k_task: float,
    d_task: float,
    c_joint: float = 0.0,
    a: float = 0.01,
    t1: float = 0.1,
    t2: float = 0.1,
) -> None:
    """Store the shaping constant; the ratio ``r`` is recomputed each step."""
    super().__init__(target, k_task=k_task, d_task=d_task, c_joint=c_joint, r=0.0, t1=t1, t2=t2)
    self.a = float(a)
    self._initial: NDArray[np.float64] | None = None  # endpoint at movement onset

reset(skeleton)

Record the onset endpoint used to normalize the remaining distance.

Source code in src/skelarm/reaching.py 
292
293
294
295
def reset(self, skeleton: Skeleton) -> None:
    """Record the onset endpoint used to normalize the remaining distance."""
    super().reset(skeleton)
    self._initial = _endpoint_position(skeleton)

update(t, skeleton, dt)

Recompute r(d) from the remaining distance, then advance the lag filter.

Source code in src/skelarm/reaching.py 
297
298
299
300
301
302
303
304
305
306
def update(self, t: float, skeleton: Skeleton, dt: float) -> None:
    """Recompute ``r(d)`` from the remaining distance, then advance the lag filter."""
    if self._initial is None:
        self.reset(skeleton)
    assert self._initial is not None  # set by reset
    position = _endpoint_position(skeleton)
    span = float(np.linalg.norm(self.target - self._initial))
    d = float(np.linalg.norm(self.target - position)) / span if span > _MIN_SPAN else 0.0
    self.r = shaping_ratio(d, self.a)
    super().update(t, skeleton, dt)

TimeVaryingStiffness

Bases: EndpointController

Virtual spring-damper with a gamma-shaped time-varying stiffness k(t).

The stiffness starts at zero and grows to k0 (Sekimoto-Arimoto 2006), giving a small initial torque. The damping gain is fixed at zeta1 * k0.

Parameters:

Name Type Description Default
target ArrayLike

The task-space target.

 required
k0 float

Saturated stiffness.

 required
alpha float

Rate of the gamma-shaped stiffness rise.

 required
zeta1 float

Damping ratio so the damping gain is zeta1 * k0.

 required
c_joint float

Joint viscous damping gain.

 0.0
Source code in src/skelarm/reaching.py 
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
class TimeVaryingStiffness(EndpointController):
    """Virtual spring-damper with a gamma-shaped time-varying stiffness ``k(t)``.

    The stiffness starts at zero and grows to ``k0`` (Sekimoto-Arimoto 2006), giving
    a small initial torque. The damping gain is fixed at ``zeta1 * k0``.

    Parameters
    ----------
    target : ArrayLike
        The task-space target.
    k0 : float
        Saturated stiffness.
    alpha : float
        Rate of the gamma-shaped stiffness rise.
    zeta1 : float
        Damping ratio so the damping gain is ``zeta1 * k0``.
    c_joint : float, optional
        Joint viscous damping gain.
    """

    def __init__(self, target: ArrayLike, *, k0: float, alpha: float, zeta1: float, c_joint: float = 0.0) -> None:
        """Store the saturated stiffness, rate, and damping ratio."""
        super().__init__(target, k_task=k0, d_task=zeta1 * k0, c_joint=c_joint)
        self.k0 = float(k0)
        self.alpha = float(alpha)
        self._k = 0.0

    def stiffness(self, t: float) -> float:
        """Gamma-distribution-shaped stiffness ``k(t)`` (zero at ``t<=0``, ``->k0``)."""
        if t <= 0.0:
            return 0.0
        a = self.alpha
        return self.k0 * (1.0 - (1.0 + a * t + 0.5 * a * a * t * t) * math.exp(-a * t))

    def control(self, t: float, skeleton: Skeleton) -> NDArray[np.float64]:
        """Return the spring-damper torque with the current time-varying stiffness."""
        self._k = self.stiffness(t)
        return self._endpoint_torque(skeleton, self.target, self._k)

    def log_channels(self) -> dict[str, ArrayLike]:
        """Record the endpoint, equilibrium, and the current stiffness."""
        channels = super().log_channels()
        if channels:
            channels["stiffness"] = self._k
        return channels

__init__(target, *, k0, alpha, zeta1, c_joint=0.0)

Store the saturated stiffness, rate, and damping ratio.

Source code in src/skelarm/reaching.py 
163
164
165
166
167
168
def __init__(self, target: ArrayLike, *, k0: float, alpha: float, zeta1: float, c_joint: float = 0.0) -> None:
    """Store the saturated stiffness, rate, and damping ratio."""
    super().__init__(target, k_task=k0, d_task=zeta1 * k0, c_joint=c_joint)
    self.k0 = float(k0)
    self.alpha = float(alpha)
    self._k = 0.0

control(t, skeleton)

Return the spring-damper torque with the current time-varying stiffness.

Source code in src/skelarm/reaching.py 
177
178
179
180
def control(self, t: float, skeleton: Skeleton) -> NDArray[np.float64]:
    """Return the spring-damper torque with the current time-varying stiffness."""
    self._k = self.stiffness(t)
    return self._endpoint_torque(skeleton, self.target, self._k)

log_channels()

Record the endpoint, equilibrium, and the current stiffness.

Source code in src/skelarm/reaching.py 
182
183
184
185
186
187
def log_channels(self) -> dict[str, ArrayLike]:
    """Record the endpoint, equilibrium, and the current stiffness."""
    channels = super().log_channels()
    if channels:
        channels["stiffness"] = self._k
    return channels

stiffness(t)

Gamma-distribution-shaped stiffness k(t) (zero at t<=0, ->k0).

Source code in src/skelarm/reaching.py 
170
171
172
173
174
175
def stiffness(self, t: float) -> float:
    """Gamma-distribution-shaped stiffness ``k(t)`` (zero at ``t<=0``, ``->k0``)."""
    if t <= 0.0:
        return 0.0
    a = self.alpha
    return self.k0 * (1.0 - (1.0 + a * t + 0.5 * a * a * t * t) * math.exp(-a * t))

VirtualSpringDamper

Bases: EndpointController

Constant virtual spring-damper toward a fixed target (Arimoto-Sekimoto 2006).

Source code in src/skelarm/reaching.py 
135
136
137
138
139
140
class VirtualSpringDamper(EndpointController):
    """Constant virtual spring-damper toward a fixed target (Arimoto-Sekimoto 2006)."""

    def control(self, t: float, skeleton: Skeleton) -> NDArray[np.float64]:  # noqa: ARG002
        """Return the spring-damper torque toward the fixed target."""
        return self._endpoint_torque(skeleton, self.target, self.k_task)

control(t, skeleton)

Return the spring-damper torque toward the fixed target.

Source code in src/skelarm/reaching.py 
138
139
140
def control(self, t: float, skeleton: Skeleton) -> NDArray[np.float64]:  # noqa: ARG002
    """Return the spring-damper torque toward the fixed target."""
    return self._endpoint_torque(skeleton, self.target, self.k_task)

adaptive_shaping_ratio(remaining, span, epsilon)

Adaptive shaping ratio r from an apparent initial endpoint (IROS 2010).

Parameters:

Name Type Description Default
remaining float

Remaining distance to the target, ||p*-p||.

 required
span float

Distance from the apparent initial endpoint to the target, ||p*-p_a||.

 required
epsilon float

Small positive floor (0 < epsilon << 1).

 required

Returns:

Type Description
float

r = 1 - (1-epsilon) * remaining/span clamped to [epsilon, 1]. It is epsilon when the endpoint sits at the apparent initial point and rises to one as the endpoint approaches the target.
Source code in src/skelarm/reaching.py 
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
def adaptive_shaping_ratio(remaining: float, span: float, epsilon: float) -> float:
    """Adaptive shaping ratio ``r`` from an apparent initial endpoint (IROS 2010).

    Parameters
    ----------
    remaining : float
        Remaining distance to the target, ``||p*-p||``.
    span : float
        Distance from the apparent initial endpoint to the target, ``||p*-p_a||``.
    epsilon : float
        Small positive floor (``0 < epsilon << 1``).

    Returns
    -------
    float
        ``r = 1 - (1-epsilon) * remaining/span`` clamped to ``[epsilon, 1]``. It is
        ``epsilon`` when the endpoint sits at the apparent initial point and rises to
        one as the endpoint approaches the target.
    """
    if span <= _MIN_SPAN:
        return 1.0
    ratio = remaining / span
    return float(np.clip(1.0 - (1.0 - epsilon) * ratio, epsilon, 1.0))

shaping_ratio(d, a)

Position-dependent shaping ratio r(d) (Humanoids 2009), clamped to (0, 1].

Parameters:

Name Type Description Default
d float

Normalized remaining distance ||p*-p|| / ||p*-p0||.

 required
a float

Small positive shaping constant (0 < a << 1).

 required

Returns:

Type Description
float

r = 0.5(sqrt((d-1)^2 + 4a) - (d-1)) clamped to [0, 1]. It is about sqrt(a) at the start (d=1) and approaches one near the target.
Source code in src/skelarm/reaching.py 
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
def shaping_ratio(d: float, a: float) -> float:
    """Position-dependent shaping ratio ``r(d)`` (Humanoids 2009), clamped to ``(0, 1]``.

    Parameters
    ----------
    d : float
        Normalized remaining distance ``||p*-p|| / ||p*-p0||``.
    a : float
        Small positive shaping constant (``0 < a << 1``).

    Returns
    -------
    float
        ``r = 0.5(sqrt((d-1)^2 + 4a) - (d-1))`` clamped to ``[0, 1]``. It is about
        ``sqrt(a)`` at the start (``d=1``) and approaches one near the target.
    """
    r = 0.5 * (math.sqrt((d - 1.0) ** 2 + 4.0 * a) - (d - 1.0))
    return float(np.clip(r, 0.0, 1.0))