"""Dynamic Motion Primitive (DMP) generation and reproduction utilities.
DMPs represent demonstrations as stable dynamical systems with learned Gaussian-basis forcing terms.
This module supports standard signals, quaternion orientations, and Cartesian pose trajectories.
DMP parameter dictionaries use the following entries:
.. list-table:: DMP parameters
:header-rows: 1
:widths: 20 80
* - Parameter
- Description
* - ``N``
- Number of Gaussian basis functions (default: ``25``).
* - ``a_z``
- Transformation-system spring gain (default: ``48.0``).
* - ``b_z``
- Transformation-system damping gain (default: ``a_z / 4``).
* - ``a_x``
- Canonical-system phase decay rate (default: ``2.0``).
* - ``dt``
- Integration time step; inferred from the demonstration if omitted.
* - ``tau``
- Demonstration duration used for temporal scaling.
* - ``c``
- Centers of the Gaussian basis functions in phase space.
* - ``sigma``
- Variances of the Gaussian basis functions.
* - ``w``
- Learned basis-function weights for each trajectory dimension.
* - ``ps``
- Scaling factor for canonical phase decoding DMP.
* - ``y0``
- Initial signal value or, for a Cartesian DMP, initial pose.
* - ``dy0``
- Initial signal velocity or Cartesian twist.
* - ``goal``
- Goal signal value or, for a Cartesian DMP, goal pose.
* - ``q0``
- Initial quaternion for a quaternion DMP.
* - ``o0``
- Initial scaled angular velocity for a quaternion DMP.
* - ``gq``
- Goal quaternion for a quaternion DMP.
* - ``diag``
- Per-axis forcing-term scale for a quaternion DMP.
Copyright (c) 2026 Jozef Stefan Institute
Authors: Leon Zlajpah.
"""
from __future__ import annotations
from typing import Any, Dict, Mapping, MutableMapping, Optional, Tuple
import numpy as np
from robotblockset.tools import gradientCartesianPath, gradientPath, gradientQuaternionPath, vector
from robotblockset.transformations import q2r, qexp, qinv, qmtimes, qnormalize, r2q, rot_v
from robotblockset.rbs_typing import Accelerations3DType, ArrayLike, Poses3DType, QuaternionsType, Velocities3DType
Array = np.ndarray
DMPType = Dict[str, Any]
DMPState = MutableMapping[str, Any]
DMPParameters = DMPType
def _parameters(dmp: Optional[Mapping[str, Any]] = None) -> DMPParameters:
result: DMPParameters = dict(dmp or {})
result.setdefault("N", 25)
result.setdefault("a_z", 48.0)
result.setdefault("b_z", result["a_z"] / 4.0)
result.setdefault("a_x", 2.0)
return result
def _time_vector(time: Any, samples: int) -> Array:
values = np.asarray(time, dtype=float).reshape(-1)
if values.size == 1:
values = np.linspace(0.0, (samples - 1) * values[0], samples)
if values.size != samples:
raise ValueError("Time vector length must match the trajectory")
values = values - values[0]
if values.size < 2 or values[-1] <= 0:
raise ValueError("Incorrect trajectory time values")
return values
def _integration_step_count(tf: float, dt: float) -> int:
"""Return the number of complete integration intervals up to ``tf``."""
final_time = float(tf)
time_step = float(dt)
if not np.isfinite(final_time) or final_time < 0:
raise ValueError("Final time must be finite and non-negative")
if not np.isfinite(time_step) or time_step <= 0:
raise ValueError("DMP integration time step must be finite and positive")
intervals = final_time / time_step
nearest_integer = round(intervals)
tolerance = 100.0 * np.finfo(float).eps * max(1.0, abs(intervals))
if abs(intervals - nearest_integer) <= tolerance:
return int(nearest_integer)
return int(np.floor(intervals))
def _kernels(dmp: DMPParameters) -> None:
centers = np.exp(-dmp["a_x"] * np.linspace(0.0, 1.0, int(dmp["N"])))
sigma = (np.diff(centers) * 0.75) ** 2
dmp["c"] = centers
dmp["sigma"] = np.r_[sigma, sigma[-1]]
def _forcing_matrix(dmp: Mapping[str, Any], phase: Array) -> Array:
psi = np.exp(-0.5 * (phase[:, None] - dmp["c"][None, :]) ** 2 / dmp["sigma"][None, :])
return phase[:, None] * psi / np.maximum(psi.sum(axis=1, keepdims=True), np.finfo(float).eps)
def _quaternion_log_error(goal: Array, current: Array) -> Array:
"""Return the shortest axis-angle error."""
relative = np.asarray(qmtimes(goal, qinv(current)), dtype=float)
vector_part = relative[1:]
vector_norm = np.linalg.norm(vector_part)
if vector_norm <= 1e-12:
return np.zeros(3)
half_angle = np.arccos(np.clip(relative[0], -1.0, 1.0))
error = half_angle * vector_part / vector_norm
if np.linalg.norm(error) > np.pi:
error = (2.0 * np.pi - 2.0 * half_angle) * (-vector_part / vector_norm)
return 2.0 * error
[docs]
def encodeDMP(inputTime: ArrayLike, inputTrajectory: ArrayLike, DMP: Optional[Mapping[str, Any]] = None) -> DMPParameters:
"""Encode sampled signals into a non-recursive locally weighted DMP.
Parameters
----------
inputTime : ArrayLike
Sample times ``(n,)`` or a scalar sample interval.
inputTrajectory : ArrayLike
Input signals with shape ``(n, m)`` or ``(n,)``.
DMP : Mapping[str, Any], optional
Initial parameters. Supported values include ``N``, ``a_z``, ``b_z``,
``a_x``, ``ps``, and ``dt``. Missing values use DMP defaults.
Returns
-------
DMPType
DMP parameters including ``w``, ``c``, ``sigma``, ``tau``, ``y0``,
``dy0``, and ``goal``.
Raises
------
ValueError
If the trajectory or time vector has an invalid shape or duration.
"""
y = np.asarray(inputTrajectory, dtype=float)
if y.ndim == 1:
y = y[:, None]
if y.ndim != 2:
raise ValueError("Trajectory must be a two-dimensional array")
dmp = _parameters(DMP)
time = _time_vector(inputTime, y.shape[0])
dmp.setdefault("dt", float(np.mean(np.diff(time))))
dy = gradientPath(y, time)
ddy = gradientPath(dy, time)
dmp["tau"] = float(time[-1])
dmp["goal"] = y[-1].copy()
dmp["y0"] = y[0].copy()
dmp["dy0"] = dy[0].copy()
_kernels(dmp)
phase = np.exp(-dmp["a_x"] * time / dmp["tau"])
target = dmp["tau"] ** 2 * ddy - dmp["a_z"] * (dmp["b_z"] * (dmp["goal"] - y) - dmp["tau"] * dy)
dmp["w"] = np.linalg.lstsq(_forcing_matrix(dmp, phase), target, rcond=None)[0]
dmp["ps"] = 1
return dmp
[docs]
def integrateStepDMP(DMP: Mapping[str, Any], S: DMPState, phase_scaling: float = 1.0) -> DMPState:
"""Advance a standard DMP state by one Euler step.
``S`` is updated in place and contains at least ``x``, ``y``, and ``z``.
The calculated ``dy``, ``ddy``, and normalized basis activation are added
to the state.
Parameters
----------
DMP : Mapping[str, Any]
Standard DMP parameter dictionary.
S : DMPState
Mutable integration state.
phase_scaling : float, optional
Positive canonical-phase time-scaling factor. Values greater than
``1`` slow phase progression and values below ``1`` accelerate it.
Default is ``1``.
Returns
-------
DMPState
The updated state dictionary.
Raises
------
ValueError
If ``phase_scaling`` is not greater than zero.
"""
if not phase_scaling > 0:
raise ValueError("phase_scaling must be greater than zero")
w = np.asarray(DMP["w"], dtype=float)
if w.shape[0] != len(DMP["c"]):
w = w.T
phase_rate = -DMP["a_x"] * S["x"] / phase_scaling / DMP["tau"]
S["x"] += phase_rate * DMP["dt"]
psi = np.exp(-((S["x"] - DMP["c"]) ** 2) / (2.0 * DMP["sigma"]))
force = (w * S["x"] * psi[:, None]).sum(axis=0) / max(psi.sum(), np.finfo(float).eps)
dz = (DMP["a_z"] * (DMP["b_z"] * (np.asarray(DMP["goal"]) - S["y"]) - S["z"]) + force) / DMP["tau"]
dy = S["z"] / DMP["tau"]
S["z"] = S["z"] + dz * DMP["dt"]
S["y"] = S["y"] + dy * DMP["dt"]
S["dy"] = dy
S["ddy"] = dz / DMP["tau"]
S["basis"] = psi * S["x"]
return S
[docs]
def decodeDMP(tf: float, DMP: Mapping[str, Any]) -> Tuple[Array, Array, Array, Array]:
"""Generate a trajectory from a standard DMP up to ``tf``.
Parameters
----------
tf : float
Requested final time in seconds.
DMP : Mapping[str, Any]
Standard DMP parameter dictionary.
Returns
-------
tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]
Position, velocity, acceleration, and canonical phase arrays. The
first three have shape ``(n, m)`` and phase has shape ``(n,)``. The
initial state at ``t = 0`` is included, so decoding a demonstration
duration of ``(n - 1) * dt`` returns ``n`` samples.
"""
state: DMPState = {"y": np.asarray(DMP["y0"], dtype=float).copy(), "z": np.asarray(DMP.get("dy0", np.zeros_like(DMP["y0"])), dtype=float).copy(), "x": 1.0}
initial_derivatives: DMPState = {"y": state["y"].copy(), "z": state["z"].copy(), "x": state["x"]}
integrateStepDMP(DMP, initial_derivatives)
values = [state["y"].copy()]
velocities = [initial_derivatives["dy"].copy()]
accelerations = [initial_derivatives["ddy"].copy()]
phases = [state["x"]]
for _ in range(_integration_step_count(tf, DMP["dt"])):
integrateStepDMP(DMP, state)
values.append(state["y"].copy())
velocities.append(state["dy"].copy())
accelerations.append(state["ddy"].copy())
phases.append(state["x"])
return np.asarray(values), np.asarray(velocities), np.asarray(accelerations), np.asarray(phases)
[docs]
def encodeQuaternionDMP(inputTime: ArrayLike, inputQuaternionTrajectory: QuaternionsType, DMP: Optional[Mapping[str, Any]] = None) -> DMPParameters:
"""Encode a quaternion trajectory into a quaternion DMP.
Quaternions use the scalar-first convention ``[w, x, y, z]``. Angular
velocity and acceleration are represented as three-dimensional vectors.
Parameters
----------
inputTime : ArrayLike
Sample times ``(n,)`` or a scalar sample interval.
inputQuaternionTrajectory : QuaternionsType
Unit quaternion trajectory with shape ``(n, 4)``.
DMP : Mapping[str, Any], optional
Initial DMP parameters. Missing values use DMP defaults.
Returns
-------
DMPType
Quaternion DMP parameters, including ``q0``, ``gq``, ``w``, ``c``,
``sigma``, ``tau``, and ``diag``.
Raises
------
ValueError
If the quaternion trajectory or time vector is invalid.
"""
q = np.asarray(inputQuaternionTrajectory, dtype=float)
if q.ndim != 2 or q.shape[1] != 4:
raise ValueError("Quaternion trajectory must have shape (n, 4)")
q = qnormalize(q)
dmp = _parameters(DMP)
time = _time_vector(inputTime, q.shape[0])
dmp.setdefault("dt", float(np.mean(np.diff(time))))
omega = gradientQuaternionPath(q, time)
domega = gradientPath(omega, time)
omega[[0, -1]] = 0.0
domega[[0, -1]] = 0.0
dmp["tau"], dmp["q0"], dmp["gq"] = float(time[-1]), q[0].copy(), q[-1].copy()
dmp["diag"] = np.ones(3)
_kernels(dmp)
phase = np.exp(-dmp["a_x"] * time / dmp["tau"])
target = dmp["tau"] ** 2 * domega + dmp["a_z"] * dmp["tau"] * omega - dmp["a_z"] * dmp["b_z"] * np.asarray([_quaternion_log_error(dmp["gq"], sample) for sample in q])
dmp["w"] = np.linalg.lstsq(_forcing_matrix(dmp, phase), target, rcond=None)[0]
dmp["o0"] = np.zeros(3)
dmp["ps"] = 1
return dmp
[docs]
def integrateStepQuaternionDMP(DMP: Mapping[str, Any], S: DMPState, phase_scaling: float = 1.0) -> DMPState:
"""Advance a quaternion DMP state by one Euler step.
The quaternion in ``S['q']`` is integrated on the unit sphere and remains
normalized after the update.
Parameters
----------
DMP : Mapping[str, Any]
Quaternion DMP parameter dictionary.
S : DMPState
Mutable state containing ``q``, ``o``, and ``x``.
phase_scaling : float, optional
Positive canonical-phase time-scaling factor. Values greater than
``1`` slow phase progression and values below ``1`` accelerate it.
Default is ``1``.
Returns
-------
DMPState
Updated state containing quaternion, angular velocity, and angular
acceleration values.
Raises
------
ValueError
If ``phase_scaling`` is not greater than zero.
"""
if not phase_scaling > 0:
raise ValueError("phase_scaling must be greater than zero")
phase_rate = -DMP["a_x"] * S["x"] / phase_scaling / DMP["tau"]
S["x"] += phase_rate * DMP["dt"]
psi = np.exp(-((S["x"] - DMP["c"]) ** 2) / (2.0 * DMP["sigma"]))
force = (np.asarray(DMP["w"]) * S["x"] * psi[:, None]).sum(axis=0) / max(psi.sum(), np.finfo(float).eps)
if S["x"] < np.exp(-DMP["a_x"]):
force[:] = 0.0
angular_acceleration = (DMP["a_z"] * (DMP["b_z"] * _quaternion_log_error(DMP["gq"], S["q"]) - S["o"]) + force * DMP["diag"]) / DMP["tau"]
S["o"] += angular_acceleration * DMP["dt"]
S["q"] = qnormalize(qmtimes(qexp(np.r_[0.0, S["o"] / DMP["tau"]] * DMP["dt"] / 2.0), S["q"]))
S["w"] = S["o"] / DMP["tau"]
S["dw"] = angular_acceleration / DMP["tau"]
return S
[docs]
def decodeQuaternionDMP(tf: float, DMP: Mapping[str, Any]) -> Tuple[QuaternionsType, Array, Array, Array]:
"""Generate a quaternion trajectory from a quaternion DMP.
Parameters
----------
tf : float
Requested final time in seconds.
DMP : Mapping[str, Any]
Quaternion DMP parameter dictionary.
Returns
-------
tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]
Unit quaternions ``(n, 4)``, angular velocities ``(n, 3)``, angular
accelerations ``(n, 3)``, and canonical phase values ``(n,)``. The
initial state at ``t = 0`` is included.
"""
state: DMPState = {"q": np.asarray(DMP["q0"], dtype=float).copy(), "o": np.asarray(DMP.get("o0", np.zeros(3)), dtype=float).copy(), "x": 1.0}
initial_derivatives: DMPState = {"q": state["q"].copy(), "o": state["o"].copy(), "x": state["x"]}
integrateStepQuaternionDMP(DMP, initial_derivatives)
values = [state["q"].copy()]
velocities = [initial_derivatives["w"].copy()]
accelerations = [initial_derivatives["dw"].copy()]
phases = [state["x"]]
for _ in range(_integration_step_count(tf, DMP["dt"])):
integrateStepQuaternionDMP(DMP, state)
values.append(state["q"].copy())
velocities.append(state["w"].copy())
accelerations.append(state["dw"].copy())
phases.append(state["x"])
return np.asarray(values), np.asarray(velocities), np.asarray(accelerations), np.asarray(phases)
[docs]
def encodeCartesianDMP(inputTime: ArrayLike, inputTrajectory: Poses3DType, DMP: Optional[Mapping[str, Any]] = None) -> DMPParameters:
"""Encode an SE(3) trajectory as a combined Cartesian DMP.
The input pose is ``[x, y, z, qw, qx, qy, qz]``. The returned dictionary
combines the three position DMP outputs and the three-dimensional
quaternion DMP forcing term, matching the MATLAB Cartesian DMP format.
Parameters
----------
inputTime : ArrayLike
Sample times ``(n,)`` or a scalar sample interval.
inputTrajectory : Poses3DType
Cartesian pose trajectory with shape ``(n, 7)``.
DMP : Mapping[str, Any], optional
Initial DMP parameters. Missing values use DMP defaults.
Returns
-------
DMPType
Combined Cartesian DMP parameter dictionary.
Raises
------
ValueError
If the input trajectory is not an ``(n, 7)`` pose path.
"""
trajectory = np.asarray(inputTrajectory, dtype=float)
if trajectory.ndim != 2 or trajectory.shape[1] != 7:
raise ValueError("Cartesian trajectory must have shape (n, 7)")
parameters = _parameters(DMP)
position = encodeDMP(inputTime, trajectory[:, :3], parameters)
orientation = encodeQuaternionDMP(inputTime, trajectory[:, 3:], parameters)
combined = dict(position)
combined["y0"] = np.r_[position["y0"], orientation["q0"]]
combined["goal"] = np.r_[position["goal"], orientation["gq"]]
combined["dy0"] = np.r_[position["dy0"], np.zeros(3)]
combined["w"] = np.c_[position["w"], orientation["w"]]
return combined
[docs]
def decodeCartesianDMP(tf: float, DMP: Mapping[str, Any]) -> Tuple[Poses3DType, Velocities3DType, Accelerations3DType, Array]:
"""Generate an SE(3) trajectory from a combined Cartesian DMP.
Parameters
----------
tf : float
Requested final time in seconds.
DMP : Mapping[str, Any]
Combined Cartesian DMP parameter dictionary.
Returns
-------
tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]
Cartesian poses ``(n, 7)``, twists ``(n, 6)``, accelerations
``(n, 6)``, and canonical phase values ``(n,)``. The initial state at
``t = 0`` is included.
"""
state: DMPState = {
"y": np.asarray(DMP["y0"], dtype=float).copy(),
"z": np.asarray(DMP.get("dy0", np.zeros(6)), dtype=float).copy(),
"x": 1.0,
}
initial_derivatives: DMPState = {"y": state["y"].copy(), "z": state["z"].copy(), "x": state["x"]}
integrateStepCartesianDMP(DMP, initial_derivatives)
states = [
{
"y": state["y"].copy(),
"dy": initial_derivatives["dy"].copy(),
"ddy": initial_derivatives["ddy"].copy(),
"x": state["x"],
}
]
for _ in range(_integration_step_count(tf, DMP["dt"])):
integrateStepCartesianDMP(DMP, state)
states.append(
{
"y": state["y"].copy(),
"dy": state["dy"].copy(),
"ddy": state["ddy"].copy(),
"x": state["x"],
}
)
return (
np.asarray([state["y"] for state in states]),
np.asarray([state["dy"] for state in states]),
np.asarray([state["ddy"] for state in states]),
np.asarray([state["x"] for state in states]),
)
[docs]
def Path2DMP(p: ArrayLike, N: int = 25) -> DMPParameters:
"""Encode a time-parameterized path into a standard DMP.
Parameters
----------
p : ArrayLike
Path array ``(n, m + 1)``. The first column contains time and the
remaining columns contain the signals.
N : int, optional
Number of Gaussian basis functions. Default is ``25``.
Returns
-------
DMPType
Encoded DMP parameter dictionary.
Raises
------
ValueError
If the path does not contain a time column and at least one signal.
"""
path = np.asarray(p, dtype=float)
if path.ndim != 2 or path.shape[1] < 2:
raise ValueError("Path must have time in its first column")
return encodeDMP(path[:, 0], path[:, 1:], {"N": N, "dt": path[1, 0] - path[0, 0], "a_z": 48.0, "b_z": 12.0, "a_x": 2.0})
[docs]
def DMP2Path(DMP: Mapping[str, Any], x_f: Optional[float] = None) -> Array:
"""Decode a standard DMP into a time, phase, and state path.
Parameters
----------
DMP : Mapping[str, Any]
Standard DMP parameter dictionary.
x_f : float, optional
Stopping phase. If omitted, it is selected from ``tau`` and ``dt``.
Returns
-------
np.ndarray
Array with columns ``[time, phase, y, dy, ddy]``.
"""
if x_f is None:
x_f = np.exp(-DMP["a_x"] * (DMP["tau"] + 2 * DMP["dt"]) / DMP["tau"])
state: DMPState = {"y": np.asarray(DMP["y0"]).copy(), "z": np.asarray(DMP.get("dy0", np.zeros_like(DMP["y0"]))).copy(), "x": 1.0, "dy": np.zeros_like(DMP["y0"]), "ddy": np.zeros_like(DMP["y0"])}
rows = []
time = 0.0
while state["x"] > x_f:
integrateStepDMP(DMP, state)
time += DMP["dt"]
rows.append(np.r_[time, state["x"], state["y"], state["dy"], state["ddy"]])
return np.asarray(rows)
[docs]
def x_encodeDMP(y: ArrayLike, DMP: Mapping[str, Any]) -> DMPParameters:
"""Encode uniformly sampled signals using recursive regression.
Parameters
----------
y : ArrayLike
Uniformly sampled signals with shape ``(n, m)`` or ``(n,)``.
DMP : Mapping[str, Any]
Initial parameters. It must contain ``dt``; other missing parameters
use DMP defaults.
Returns
-------
DMPType
DMP parameter dictionary containing recursively fitted weights.
"""
result = _parameters(DMP)
values = np.asarray(y, dtype=float)
if values.ndim == 1:
values = values[:, None]
result["y0"], result["goal"] = values[0].copy(), values[-1].copy()
result["tau"] = (values.shape[0] - 1) * result["dt"]
dy, ddy = gradientPath(values, result["dt"]), gradientPath(gradientPath(values, result["dt"]), result["dt"])
result["dy0"] = dy[0].copy()
_kernels(result)
phase = np.exp(-result["a_x"] * np.arange(values.shape[0]) * result["dt"] / result["tau"])
target = result["tau"] ** 2 * ddy - result["a_z"] * (result["b_z"] * (result["goal"] - values) - result["tau"] * dy)
result["w"] = np.linalg.lstsq(_forcing_matrix(result, phase), target, rcond=None)[0]
return result
[docs]
def x_decodeDMP(DMP: Mapping[str, Any], S: DMPState) -> DMPState:
"""Advance a recursive-regression DMP state by one step.
Parameters
----------
DMP : Mapping[str, Any]
DMP parameter dictionary.
S : DMPState
Mutable state containing phase, position, and scaled velocity.
Returns
-------
DMPState
Updated state dictionary.
"""
return integrateStepDMP(DMP, S)
[docs]
def integrateStepCartesianDMP(DMP: Mapping[str, Any], S: DMPState, phase_scaling: float = 1.0) -> DMPState:
"""Advance a combined Cartesian DMP state by one Euler step.
Parameters
----------
DMP : Mapping[str, Any]
Combined Cartesian DMP parameter dictionary.
S : DMPState
Mutable state containing a seven-dimensional pose ``y``, six-
dimensional scaled velocity ``z``, and phase ``x``.
phase_scaling : float, optional
Positive canonical-phase time-scaling factor. Values greater than
``1`` slow phase progression and values below ``1`` accelerate it.
Default is ``1``.
Returns
-------
DMPState
Updated Cartesian state with pose, velocity, acceleration, and phase.
Raises
------
ValueError
If ``phase_scaling`` is not greater than zero.
"""
if not phase_scaling > 0:
raise ValueError("phase_scaling must be greater than zero")
position = dict(DMP)
position["w"] = np.asarray(DMP["w"])[:, :3]
position["y0"] = np.asarray(DMP["y0"])[..., :3]
position["goal"] = np.asarray(DMP["goal"])[..., :3]
orientation = dict(DMP)
orientation["w"] = np.asarray(DMP["w"])[:, 3:]
orientation["q0"] = np.asarray(DMP["y0"])[..., 3:]
orientation["gq"] = np.asarray(DMP["goal"])[..., 3:]
orientation["diag"] = np.ones(3)
ps = {"y": S["y"][:3], "z": S["z"][:3], "x": S["x"]}
qs = {"q": S["y"][3:], "o": S["z"][3:], "x": S["x"]}
integrateStepDMP(position, ps, phase_scaling=phase_scaling)
integrateStepQuaternionDMP(orientation, qs, phase_scaling=phase_scaling)
S.update(y=np.r_[ps["y"], qs["q"]], dy=np.r_[ps["dy"], qs["w"]], ddy=np.r_[ps["ddy"], qs["dw"]], z=np.r_[ps["z"], qs["o"]], x=(ps["x"] + qs["x"]) / 2)
return S