virtualmodelcontrol 0.1.0__py3-none-any.whl

virtualmodelcontrol/mechanisms/coordinates/ops.py

@@ -0,0 +1,146 @@
+"""Coordinates built from other coordinates: differences, slices, stacks, projections, norms."""
+
+from __future__ import annotations
+
+from collections.abc import Callable, Sequence
+from typing import Any
+
+import casadi as ca
+
+from ...core.params import Param, Scope, as_param
+from ...core.symbolic import smooth_norm
+from .base import Context, Coordinate
+
+
+class Difference(Coordinate):
+    """a − b, e.g. a point minus its goal (the deflection of a spring)."""
+
+    def __init__(self, a: Coordinate, b: Coordinate) -> None:
+        if a.dim != b.dim:
+            raise ValueError(f"cannot subtract a {b.dim}-entry coordinate from a {a.dim}-entry one")
+        super().__init__(a.dim, a.unit)
+        self.a, self.b = a, b
+
+    def children(self) -> tuple[Coordinate, ...]:
+        """The two operands."""
+        return (self.a, self.b)
+
+    def value(self, ctx: Context) -> Any:
+        """a − b."""
+        return ctx.value(self.a) - ctx.value(self.b)
+
+
+class Slice(Coordinate):
+    """Some entries of a coordinate."""
+
+    def __init__(self, coord: Coordinate, index: int | slice | Sequence[int]) -> None:
+        all_indices = list(range(coord.dim))
+        if isinstance(index, slice):
+            indices = all_indices[index]
+        elif isinstance(index, int):
+            indices = [all_indices[index]]
+        else:
+            indices = [all_indices[i] for i in index]
+        super().__init__(len(indices), coord.unit)
+        self.coord, self.indices = coord, indices
+
+    def children(self) -> tuple[Coordinate, ...]:
+        """The sliced coordinate."""
+        return (self.coord,)
+
+    def value(self, ctx: Context) -> Any:
+        """Selected entries."""
+        v = ctx.value(self.coord)
+        return ca.vertcat(*[v[i] for i in self.indices])
+
+
+class Stack(Coordinate):
+    """Several coordinates stacked into one."""
+
+    def __init__(self, *coords: Coordinate) -> None:
+        units = {c.unit for c in coords}
+        super().__init__(sum(c.dim for c in coords), units.pop() if len(units) == 1 else "")
+        self.coords = coords
+
+    def children(self) -> tuple[Coordinate, ...]:
+        """The stacked coordinates."""
+        return self.coords
+
+    def value(self, ctx: Context) -> Any:
+        """Values stacked in order."""
+        return ca.vertcat(*[ctx.value(c) for c in self.coords])
+
+
+class Projection(Coordinate):
+    """Signed length of a coordinate along a direction: y = n̂ᵀ c with n̂ = n / ‖n‖.
+
+    A spring on a projection acts along n only (a cart). The direction is an ``episode`` Param.
+    """
+
+    def __init__(self, coord: Coordinate, direction: Any, *, scope: Scope = "episode") -> None:
+        super().__init__(1, coord.unit)
+        self.coord = coord
+        self.direction = as_param(direction, "direction", scope=scope)
+        if self.direction.size != coord.dim:
+            raise ValueError(f"direction needs {coord.dim} entries, got {self.direction.size}")
+
+    def params(self) -> dict[str, Param]:
+        """The direction."""
+        return {"direction": self.direction}
+
+    def children(self) -> tuple[Coordinate, ...]:
+        """The projected coordinate."""
+        return (self.coord,)
+
+    def value(self, ctx: Context) -> Any:
+        """n̂ᵀ c."""
+        n = ca.reshape(ctx.param(self.direction), self.coord.dim, 1)
+        return ca.dot(n, ctx.value(self.coord)) / ca.norm_2(n)
+
+
+class Norm(Coordinate):
+    """Smooth Euclidean length of a coordinate, √(cᵀc + ε)."""
+
+    def __init__(self, coord: Coordinate, eps: float = 1e-18) -> None:
+        super().__init__(1, coord.unit)
+        self.coord, self.eps = coord, eps
+
+    def children(self) -> tuple[Coordinate, ...]:
+        """The measured coordinate."""
+        return (self.coord,)
+
+    def value(self, ctx: Context) -> Any:
+        """√(cᵀc + ε)."""
+        return smooth_norm(ctx.value(self.coord), self.eps)
+
+
+class Custom(Coordinate):
+    """y = fn(child values, Param expressions by name), with ``fn`` written in CasADi operations."""
+
+    def __init__(
+        self,
+        fn: Callable[..., Any],
+        children: Sequence[Coordinate] = (),
+        *,
+        dim: int,
+        unit: str = "",
+        params: dict[str, Any] | None = None,
+    ) -> None:
+        super().__init__(dim, unit)
+        self.fn = fn
+        self._children = tuple(children)
+        self._params = {name: as_param(v, name) for name, v in (params or {}).items()}
+
+    def params(self) -> dict[str, Param]:
+        """Params passed to ``fn`` by keyword."""
+        return dict(self._params)
+
+    def children(self) -> tuple[Coordinate, ...]:
+        """Coordinates passed to ``fn`` in order."""
+        return self._children
+
+    def value(self, ctx: Context) -> Any:
+        """``fn`` applied to the children's values and the Params."""
+        args = [ctx.value(c) for c in self._children]
+        kwargs = {name: ctx.param(p) for name, p in self._params.items()}
+        return ca.reshape(self.fn(*args, **kwargs), self.dim, 1)

virtualmodelcontrol/mechanisms/coordinates/references.py

@@ -0,0 +1,43 @@
+"""References: coordinates held in Params, such as goals and obstacle positions."""
+
+from __future__ import annotations
+
+from typing import Any
+
+import casadi as ca
+import numpy as np
+
+from ...core.params import Param, Scope
+from .base import Context, Coordinate
+
+
+class Ref(Coordinate):
+    """A reference value held in a Param; ``stage`` scope, so live at every step by default."""
+
+    def __init__(
+        self,
+        name: str,
+        dim: int | None = None,
+        value: Any = None,
+        *,
+        unit: str = "m",
+        scope: Scope = "stage",
+    ) -> None:
+        if isinstance(value, Param):
+            param = value
+        else:
+            if value is None:
+                value = np.zeros(1 if dim is None else dim)
+            param = Param(name, np.ravel(value), unit=unit, scope=scope)
+        if dim is not None and dim != param.size:
+            raise ValueError(f"Ref {name!r}: dim {dim} but the value has {param.size} entries")
+        super().__init__(param.size, param.unit)
+        self.param = param
+
+    def params(self) -> dict[str, Param]:
+        """The reference Param."""
+        return {self.param.name: self.param}
+
+    def value(self, ctx: Context) -> Any:
+        """The Param as a column."""
+        return ca.reshape(ctx.param(self.param), self.dim, 1)

virtualmodelcontrol/mechanisms/mechanism.py

@@ -0,0 +1,88 @@
+"""Mechanism: coordinates plus components, for a physical robot and a virtual one alike."""
+
+from __future__ import annotations
+
+from typing import Any
+
+from numpy.typing import ArrayLike
+
+from ..core.params import Param, ParamSet
+from .components.base import Component
+from .coordinates.base import walk
+from .coordinates.frames import FramePoint
+from .coordinates.joints import Joint, State
+
+
+class Mechanism:
+    """Named components acting on coordinates.
+
+    A robot mechanism has a kinematic ``model`` (and optionally an ``actuation``); its components
+    are physical (masses, identified stiffness). A controller is a mechanism without a model:
+    its components act on the robot's coordinates and on its own virtual states.
+    """
+
+    def __init__(self, name: str, model: Any = None, actuation: Any = None) -> None:
+        self.name = name
+        self.model = model
+        self.actuation = actuation
+        self.components: dict[str, Component] = {}
+        self.states: dict[str, State] = {}
+        self._own_params: dict[str, Param] = {}
+
+    def add(self, name: str, component: Component) -> Component:
+        """Add ``component`` under ``name`` and return it."""
+        if name in self.components:
+            raise ValueError(f"{self.name!r} already has a component named {name!r}")
+        self.components[name] = component
+        return component
+
+    def add_state(self, name: str, dim: int = 1, unit: str = "", initial: ArrayLike = 0.0) -> State:
+        """Add a virtual degree of freedom (it needs an inertance) and return its coordinate."""
+        if name in self.states:
+            raise ValueError(f"{self.name!r} already has a state named {name!r}")
+        state = State(name, dim, unit, initial)
+        self.states[name] = state
+        return state
+
+    def add_param(self, param: Param) -> Param:
+        """Add a Param that belongs to the mechanism itself (gravity, say) and return it."""
+        self._own_params[param.name] = param
+        return param
+
+    def point(self, at: str | None = None, *, s: Any = None, offset: Any = None) -> FramePoint:
+        """A point of the model: a named site, or arc parameter ``s`` [0, 1]."""
+        if self.model is None:
+            raise ValueError(f"{self.name!r} has no kinematic model")
+        return FramePoint(self.model, at, s=s, offset=offset)
+
+    def joint(self, index: Any) -> Joint:
+        """Entries of the robot's generalized coordinates."""
+        return Joint(index, getattr(self.model, "q_unit", ""))
+
+    @property
+    def space(self) -> Any:
+        """Configuration space of the model."""
+        if self.model is None:
+            raise ValueError(f"{self.name!r} has no kinematic model")
+        return self.model.space
+
+    @property
+    def params(self) -> ParamSet:
+        """Every Param of the mechanism, by local name (component Params as ``name.param``)."""
+        ps = ParamSet()
+        if self.model is not None:
+            ps.merge(self.model.params)
+        if self.actuation is not None:
+            ps.merge(self.actuation.params)
+        for param in self._own_params.values():
+            ps.add(param)
+        for cname, component in self.components.items():
+            for local, param in component.params().items():
+                ps.add(param, f"{cname}.{local}", rename=True)
+            for coord in walk(component.coord):
+                for local, param in coord.params().items():
+                    ps.add(param, f"{cname}.{local}", rename=True)
+        return ps
+
+    def __repr__(self) -> str:
+        return f"Mechanism({self.name!r}, components={list(self.components)})"

virtualmodelcontrol/models/__init__.py

@@ -0,0 +1,22 @@
+"""Models: kinematics and actuation of robots."""
+
+from .actuation import Actuation, Direct, TendonTransmission
+from .assembly import Assembly, StackedActuation
+from .continuum import PCC, segment_frame
+from .kinematic import KinematicModel, evaluate_frame, from_dict
+from .rigid import LinearCoupling, SerialChain
+
+__all__ = [
+    "PCC",
+    "Actuation",
+    "Assembly",
+    "Direct",
+    "KinematicModel",
+    "LinearCoupling",
+    "SerialChain",
+    "StackedActuation",
+    "TendonTransmission",
+    "evaluate_frame",
+    "from_dict",
+    "segment_frame",
+]

virtualmodelcontrol/models/actuation.py

@@ -0,0 +1,196 @@
+"""Actuation: how motor angles follow from q, and how motor torques u give τ = B(q) u."""
+
+from __future__ import annotations
+
+from collections.abc import Sequence
+from typing import Any, Protocol
+
+import casadi as ca
+import numpy as np
+
+from ..core.params import ParamSet, as_param
+from ..core.registry import register
+from ..core.units import RAD, M
+
+
+class Actuation(Protocol):
+    """How motors drive a robot. Implement these to add a transmission (``register("actuation")``).
+
+    ``config_from_motors`` / ``velocity_from_motors`` invert the map when the controller reads
+    motors directly; ``allocate`` solves B u = τ.
+    """
+
+    params: ParamSet
+
+    def motor_sizes(self, space: Any) -> tuple[int, int]:
+        """Numbers of motor angles and motor rates."""
+        ...
+
+    def motor_angles(self, q: Any, p: dict[str, Any]) -> Any:
+        """Motor angles θ(q) [rad]."""
+        ...
+
+    def motor_rates(self, q: Any, v: Any, p: dict[str, Any]) -> Any:
+        """Motor rates θ̇ [rad/s]."""
+        ...
+
+    def generalized_force(self, u: Any, q: Any, p: dict[str, Any]) -> Any:
+        """τ = B(q) u."""
+        ...
+
+    def allocate(self, tau: Any, q: Any, p: dict[str, Any]) -> Any:
+        """Motor torques u with B(q) u = τ."""
+        ...
+
+    def config_from_motors(self, theta: Any, p: dict[str, Any]) -> Any:
+        """q from motor angles."""
+        ...
+
+    def velocity_from_motors(self, q: Any, theta_dot: Any, p: dict[str, Any]) -> Any:
+        """v from motor rates."""
+        ...
+
+
+@register("actuation", "direct")
+class Direct:
+    """One actuator per generalized coordinate: motor angles θ = q, B = I, u = τ."""
+
+    def __init__(self) -> None:
+        self.params = ParamSet()
+
+    def motor_sizes(self, space: Any) -> tuple[int, int]:
+        """Numbers of motor angles and motor rates: nq and nv."""
+        return space.nq, space.nv
+
+    def motor_angles(self, q: Any, p: dict[str, Any]) -> Any:
+        """θ = q."""
+        return q
+
+    def motor_rates(self, q: Any, v: Any, p: dict[str, Any]) -> Any:
+        """θ̇ = v."""
+        return v
+
+    def generalized_force(self, u: Any, q: Any, p: dict[str, Any]) -> Any:
+        """τ = u."""
+        return u
+
+    def allocate(self, tau: Any, q: Any, p: dict[str, Any]) -> Any:
+        """u = τ."""
+        return tau
+
+    def config_from_motors(self, theta: Any, p: dict[str, Any]) -> Any:
+        """q = θ."""
+        return theta
+
+    def velocity_from_motors(self, q: Any, theta_dot: Any, p: dict[str, Any]) -> Any:
+        """v = θ̇."""
+        return theta_dot
+
+    def to_dict(self) -> dict[str, Any]:
+        """No arguments."""
+        return {"type": "direct"}
+
+    @classmethod
+    def from_dict(cls, data: dict[str, Any]) -> Direct:
+        """Inverse of ``to_dict``."""
+        return cls()
+
+
+@register("actuation", "tendons")
+class TendonTransmission:
+    """Rigid tendons on PCC segments, with motor angle θ = −ΔL / r: θ > 0 pulls a tendon.
+
+    Tendon j of a segment changes length by ΔL_j = Dl − Dx cos δ_j − Dy sin δ_j. Per segment i:
+    ``seg{i}.delta`` tendon angles around the section at the base [rad] and ``seg{i}.r`` spool
+    radius [m], ``design`` Params. With three tendons per segment the map is invertible.
+    """
+
+    def __init__(self, delta: Sequence[Any], r: Any) -> None:
+        n = len(delta)
+        r_list = list(r) if isinstance(r, (list, tuple, np.ndarray)) else [r] * n
+        if len(r_list) != n:
+            raise ValueError(f"{n} segments of tendon angles but {len(r_list)} spool radii")
+        self.n_segments = n
+        self.params = ParamSet()
+        for i in range(n):
+            key = f"seg{i + 1}.delta"
+            free = (-np.inf, np.inf)
+            self.params.add(as_param(delta[i], key, unit=RAD, scope="design", bounds=free), key)
+            key = f"seg{i + 1}.r"
+            self.params.add(
+                as_param(r_list[i], key, unit=M, scope="design", bounds=(0, np.inf)), key
+            )
+        self.n_tendons = [self.params[f"seg{i + 1}.delta"].size for i in range(n)]
+        self.n_motors = sum(self.n_tendons)
+
+    def motor_sizes(self, space: Any) -> tuple[int, int]:
+        """Numbers of motor angles and motor rates: one per tendon."""
+        return self.n_motors, self.n_motors
+
+    def _blocks(self, p: dict[str, Any]) -> list[Any]:
+        """∂θ_i/∂q_i = −A_i / r_i per segment (the map is linear in q)."""
+        out = []
+        for i in range(self.n_segments):
+            delta, r = p[f"seg{i + 1}.delta"], p[f"seg{i + 1}.r"]
+            rows = [
+                ca.horzcat(-ca.cos(delta[j]), -ca.sin(delta[j]), 1) for j in range(delta.numel())
+            ]
+            out.append(-ca.vertcat(*rows) / r)
+        return out
+
+    def tendon_lengths(self, q: Any, p: dict[str, Any]) -> Any:
+        """Length change of every tendon, ΔL [m]."""
+        out = []
+        for i, block in enumerate(self._blocks(p)):
+            r = p[f"seg{i + 1}.r"]
+            out.append(-r * ca.mtimes(block, q[3 * i : 3 * i + 3]))
+        return ca.vertcat(*out)
+
+    def motor_angles(self, q: Any, p: dict[str, Any]) -> Any:
+        """θ = −ΔL / r [rad]."""
+        blocks = self._blocks(p)
+        return ca.vertcat(*[ca.mtimes(B, q[3 * i : 3 * i + 3]) for i, B in enumerate(blocks)])
+
+    def jacobian(self, p: dict[str, Any]) -> Any:
+        """∂θ/∂q, constant; the input matrix is B = (∂θ/∂q)ᵀ."""
+        return ca.diagcat(*self._blocks(p))
+
+    def motor_rates(self, q: Any, v: Any, p: dict[str, Any]) -> Any:
+        """θ̇ = (∂θ/∂q) v."""
+        return ca.mtimes(self.jacobian(p), v)
+
+    def generalized_force(self, u: Any, q: Any, p: dict[str, Any]) -> Any:
+        """τ = B u with B = (∂θ/∂q)ᵀ."""
+        return ca.mtimes(self.jacobian(p).T, u)
+
+    def allocate(self, tau: Any, q: Any, p: dict[str, Any]) -> Any:
+        """Motor torques u with B u = τ."""
+        blocks = self._blocks(p)
+        return ca.vertcat(*[ca.solve(B.T, tau[3 * i : 3 * i + 3]) for i, B in enumerate(blocks)])
+
+    def config_from_motors(self, theta: Any, p: dict[str, Any]) -> Any:
+        """q from motor angles (the exact inverse of ``motor_angles``)."""
+        out, offset = [], 0
+        for i, block in enumerate(self._blocks(p)):
+            m = self.n_tendons[i]
+            out.append(ca.solve(block, theta[offset : offset + m]))
+            offset += m
+        return ca.vertcat(*out)
+
+    def velocity_from_motors(self, q: Any, theta_dot: Any, p: dict[str, Any]) -> Any:
+        """v from motor velocities (the map is linear, so this is the same inverse)."""
+        return self.config_from_motors(theta_dot, p)
+
+    def to_dict(self) -> dict[str, Any]:
+        """Constructor arguments at the current Param values."""
+        n = range(1, self.n_segments + 1)
+        return {
+            "type": "tendons",
+            "delta": [self.params[f"seg{i}.delta"].value.tolist() for i in n],
+            "r": [float(self.params[f"seg{i}.r"].value) for i in n],
+        }
+
+    @classmethod
+    def from_dict(cls, data: dict[str, Any]) -> TendonTransmission:
+        """Inverse of ``to_dict``."""
+        return cls(data["delta"], data["r"])

virtualmodelcontrol/models/assembly.py

@@ -0,0 +1,154 @@
+"""Assemblies: several models mounted on one base (fingers on a palm, two arms on a frame)."""
+
+from __future__ import annotations
+
+from collections.abc import Mapping, Sequence
+from typing import Any
+
+import casadi as ca
+import numpy as np
+
+from ..core.params import Param, ParamSet, as_param
+from ..core.registry import get, register
+from ..core.space import Product
+from ..core.symbolic import rotation_from_vector
+from .actuation import Direct
+
+
+@register("model", "assembly")
+class Assembly:
+    """Several models mounted at fixed poses on one base; q stacks the parts' coordinates.
+
+    ``parts`` maps a name to ``(model, position, rotation)``: the mount position [m] and rotation
+    vector [rad] in the base frame, ``design`` Params named ``{name}.mount.position`` and
+    ``{name}.mount.rotation``. Sites are ``"name/site"``; for a continuous part, give
+    ``at="name"`` and an arc parameter s.
+    """
+
+    def __init__(self, parts: Mapping[str, tuple[Any, Any, Any]]) -> None:
+        self.parts = {name: spec[0] for name, spec in parts.items()}
+        self.space = Product(*[m.space for m in self.parts.values()])
+        self.params = ParamSet()
+        self._local: dict[str, dict[str, str]] = {}
+        self._mount: dict[str, tuple[Param, Param]] = {}
+        free = (-np.inf, np.inf)
+        for name, (model, position, rotation) in parts.items():
+            self.params.merge(model.params, name)
+            self._local[name] = {loc: self.params.name_of(par) for loc, par in model.params.items()}
+            pos = as_param(
+                position, f"{name}.mount.position", unit="m", scope="design", bounds=free
+            )
+            rot = as_param(
+                rotation, f"{name}.mount.rotation", unit="rad", scope="design", bounds=free
+            )
+            self.params.add(pos, f"{name}.mount.position")
+            self.params.add(rot, f"{name}.mount.rotation")
+            self._mount[name] = (pos, rot)
+        self._slices: dict[str, slice] = {}
+        offset = 0
+        for name, model in self.parts.items():
+            self._slices[name] = slice(offset, offset + model.space.nq)
+            offset += model.space.nq
+        self.sites = tuple(f"{n}/{s}" for n, m in self.parts.items() for s in m.sites)
+        units = {getattr(m, "q_unit", "") for m in self.parts.values()}
+        self.q_unit = units.pop() if len(units) == 1 else ""
+
+    def part_view(self, name: str, p: dict[str, Any]) -> dict[str, Any]:
+        """A part's Params under its own names."""
+        return {loc: p[full] for loc, full in self._local[name].items()}
+
+    def frame(self, q: Any, at: Any, p: dict[str, Any]) -> tuple[Any, Any]:
+        """(R, position) of ``"part/site"``, or of ``(part, s)`` for a continuous part."""
+        if isinstance(at, tuple):
+            name, where = at
+        else:
+            name, where = at.split("/", 1)
+        if name not in self.parts:
+            raise KeyError(f"unknown part {name!r}; parts: {list(self.parts)}")
+        R, pos = self.parts[name].frame(q[self._slices[name]], where, self.part_view(name, p))
+        R_m = rotation_from_vector(ca.reshape(p[f"{name}.mount.rotation"], 3, 1))
+        p_m = ca.reshape(p[f"{name}.mount.position"], 3, 1)
+        return ca.mtimes(R_m, R), p_m + ca.mtimes(R_m, pos)
+
+    def stacked_actuation(self, actuations: Mapping[str, Any] | None = None) -> StackedActuation:
+        """One actuation for the whole assembly: each part's own (default ``Direct``)."""
+        actuations = actuations or {}
+        return StackedActuation(
+            [(self.parts[n].space, actuations.get(n, Direct()), n) for n in self.parts]
+        )
+
+    def to_dict(self) -> dict[str, Any]:
+        """Each part's model and mount pose."""
+        return {
+            "type": "assembly",
+            "parts": {
+                n: [m.to_dict(), self._mount[n][0].value.tolist(), self._mount[n][1].value.tolist()]
+                for n, m in self.parts.items()
+            },
+        }
+
+    @classmethod
+    def from_dict(cls, data: dict[str, Any]) -> Assembly:
+        """Inverse of ``to_dict``."""
+        parts = {
+            n: (get("model", spec[0]["type"]).from_dict(spec[0]), spec[1], spec[2])
+            for n, spec in data["parts"].items()
+        }
+        return cls(parts)
+
+
+class StackedActuation:
+    """Each part of an assembly driven by its own actuation; motors are stacked in part order."""
+
+    def __init__(self, parts: Sequence[tuple[Any, Any, str]]) -> None:
+        self.parts = list(parts)
+        self.params = ParamSet()
+        for _space, actuation, name in self.parts:
+            self.params.merge(actuation.params, name)
+        self._sizes = [(s.nq, s.nv, *a.motor_sizes(s)) for s, a, _ in self.parts]
+
+    def _view(self, i: int, p: dict[str, Any]) -> dict[str, Any]:
+        name, actuation = self.parts[i][2], self.parts[i][1]
+        return {loc: p[f"{name}.{loc}"] for loc in actuation.params}
+
+    def _split(self, x: Any, which: int) -> list[Any]:
+        out, offset = [], 0
+        for sizes in self._sizes:
+            out.append(x[offset : offset + sizes[which]])
+            offset += sizes[which]
+        return out
+
+    def motor_sizes(self, space: Any) -> tuple[int, int]:
+        """Total motor angles and rates."""
+        return sum(s[2] for s in self._sizes), sum(s[3] for s in self._sizes)
+
+    def _map(self, method: str, xs: list[Any], p: dict[str, Any], *extra: list[Any]) -> Any:
+        out = []
+        for i, (_space, actuation, _name) in enumerate(self.parts):
+            args = [x[i] for x in (xs, *extra)]
+            out.append(getattr(actuation, method)(*args, self._view(i, p)))
+        return ca.vertcat(*out)
+
+    def motor_angles(self, q: Any, p: dict[str, Any]) -> Any:
+        """Each part's motor angles, stacked."""
+        return self._map("motor_angles", self._split(q, 0), p)
+
+    def motor_rates(self, q: Any, v: Any, p: dict[str, Any]) -> Any:
+        """Each part's motor rates, stacked."""
+        return self._map("motor_rates", self._split(q, 0), p, self._split(v, 1))
+
+    def generalized_force(self, u: Any, q: Any, p: dict[str, Any]) -> Any:
+        """Each part's generalized force, stacked."""
+        return self._map("generalized_force", self._split(u, 3), p, self._split(q, 0))
+
+    def allocate(self, tau: Any, q: Any, p: dict[str, Any]) -> Any:
+        """Each part's motor torques, stacked."""
+        return self._map("allocate", self._split(tau, 1), p, self._split(q, 0))
+
+    def config_from_motors(self, theta: Any, p: dict[str, Any]) -> Any:
+        """Each part's configuration, stacked."""
+        return self._map("config_from_motors", self._split(theta, 2), p)
+
+    def velocity_from_motors(self, q: Any, theta_dot: Any, p: dict[str, Any]) -> Any:
+        """Each part's velocity, stacked."""
+        return self._map("velocity_from_motors", self._split(q, 0), p, self._split(theta_dot, 3))

virtualmodelcontrol/models/continuum/__init__.py

@@ -0,0 +1,5 @@
+"""Continuum models."""
+
+from .pcc import PCC, segment_frame
+
+__all__ = ["PCC", "segment_frame"]