virtualmodelcontrol 1.0.0__tar.gz → 1.1.0__tar.gz

{virtualmodelcontrol-1.0.0 → virtualmodelcontrol-1.1.0}/CHANGELOG.md

@@ -4,6 +4,41 @@ All notable changes to this project are documented here. The format follows
 [Keep a Changelog](https://keepachangelog.com/en/1.1.0/), and versions follow
 [Semantic Versioning](https://semver.org/) (0.x: the API may change between minor versions).
 
+## [1.1.0] - 2026-10-09
+
+Static friction of the motors: a model of it, a compensation for it, its identification and a
+tutorial, all off unless asked for. Nothing that worked before changes.
+
+### Added
+
+- Static friction of the motors, off by default. `StaticFriction(breakaway, kinetic, width)`
+  [N·m] is the torque that friction takes from a motor as a smooth function of the torque applied
+  to it: all of it below the breakaway torque F (the motor is stuck), the kinetic torque Fc with
+  the sign of the command past it (Fc = 0: static friction gone once it moves; Fc = F: it stays),
+  smooth (C^∞) over `width`, so solvers differentiate it. It depends on the torque only, not on
+  the speed. `Efficiency(c1, ..., friction=StaticFriction(...))` takes it before the polynomial, so
+  every transmission carries it (`Direct`, `Underactuated`, `TendonTransmission`, stacked ones, and
+  configuration files), and the simulator, `compile_dynamics`, `Collocation`, `Shooting`, `MPC`, the
+  Kalman filter and the force law all have it. Its Params are `friction.breakaway`,
+  `friction.kinetic` and `friction.width`; with F = Fc = 0, the default, nothing changes. Not passive
+  by construction: when a command brakes the motion it can create up to F |θ̇| of power, which the
+  tutorial states and measures.
+- `control.StaticFrictionCompensation(kinetic, width, fraction)` (and `.of(friction, fraction)`), an
+  output stage that adds the kinetic friction torque in the direction of the command, smooth in the
+  command. Against a `StaticFriction(F, Fc)` it delivers the command once the motor moves and
+  narrows the dead band from F to F − Fc. Nothing turns it on but you: no template or controller
+  has it by default. It has a `symbolic` form, so `Problem(system, output=[...])` and
+  `sim.rollout(..., output=[...])` apply it in a plan as `VMCController(output=)` does in a run
+  (`Problem` now raises for an output stage that has no `symbolic` form).
+- Identification of the friction: its Params are the robot's, so `identification.fit_params` fits
+  them, with the stiffness and damping, to a run in which the command rises slowly from rest;
+  `fit_efficiency(commanded, delivered, friction=True, width=)` fits the efficiency and the friction
+  to static torques (per motor or `shared`), by a grid and a refinement over the breakaway and the
+  kinetic torque.
+- `robots.adapt.finger_friction()` and `hand_friction()`: the lab's friction values of the finger
+  (0.20 N·m) and the hand (0.10 N·m) as a `StaticFriction`, to ask for.
+- Docs: the tutorial "Static friction of the motors".
+
 ## [1.0.0] - 2026-10-07
 
 The first stable release. It also carries the work meant for 0.6.0 (planning with virtual states,

{virtualmodelcontrol-1.0.0 → virtualmodelcontrol-1.1.0}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.5
 Name: virtualmodelcontrol
-Version: 1.0.0
+Version: 1.1.0
 Summary: Virtual Model Control: robots and controllers as mechanisms of springs, dampers and inertances, compiled with CasADi.
 Project-URL: Repository, https://github.com/vigno0405/VirtualModelControl
 Project-URL: Documentation, https://vigno0405.github.io/VirtualModelControl/

{virtualmodelcontrol-1.0.0 → virtualmodelcontrol-1.1.0}/docs/index.md

@@ -108,6 +108,7 @@ tutorials/parameters
 tutorials/configurations
 tutorials/kinematics
 tutorials/build-a-robot
+tutorials/friction
 tutorials/extend
 tutorials/outside
 ```

{virtualmodelcontrol-1.0.0 → virtualmodelcontrol-1.1.0}/docs/installation.md

@@ -120,10 +120,10 @@ pip install -e .
 An experiment should always run the same code. Pin the version, and move the pin on purpose:
 
 ```bash
-pip install "virtualmodelcontrol==1.0.0"
+pip install "virtualmodelcontrol==1.1.0"
 ```
 
-In a `requirements.txt` file, write `virtualmodelcontrol==1.0.0`. In a conda `environment.yml`,
+In a `requirements.txt` file, write `virtualmodelcontrol==1.1.0`. In a conda `environment.yml`,
 list it under `pip:`. `pip show virtualmodelcontrol` tells you which version you have, and the
 [changelog](development/changelog.md) what changed between versions.
 

{virtualmodelcontrol-1.0.0 → virtualmodelcontrol-1.1.0}/docs/tutorials/build-a-robot.md

@@ -54,7 +54,9 @@ np.abs(a - b).max()  # [m], the two descriptions agree
 
 A robot is a mechanism around its model. Its physical components are point masses at sites,
 gravity, and here a little viscous friction in the joints. The simulator needs all of them; the
-controller needs only the point masses, for `GravityCompensation`.
+controller needs only the point masses, for `GravityCompensation`. The static friction of the
+motors, which holds a joint until the command passes a breakaway torque, is a part of the
+transmission: see [static friction of the motors](friction.md).
 
 ```{code-cell} python
 arm = vmc.Mechanism("arm", model=chain)

virtualmodelcontrol-1.1.0/docs/tutorials/friction.md

@@ -0,0 +1,285 @@
+---
+file_format: mystnb
+kernelspec:
+  name: python3
+---
+
+# Static friction of the motors
+
+A motor does not deliver a small torque: friction holds it until the command passes a
+breakaway value. A virtual spring that pulls with less than that does not move the joint, and
+the robot stops short of its goal. In this tutorial we model that friction as a function of
+the torque we apply, see what it does to a controller, compensate it, identify it from data,
+and plan with it. Everything here is off unless we ask for it.
+
+```{code-cell} python
+:tags: [remove-cell]
+import docs_setup
+from myst_nb import glue
+```
+
+## The model
+
+`StaticFriction(breakaway, kinetic, width)` is the torque $\tau_f$ that friction takes from a
+motor, as a function of the torque $u$ we apply to it, in N·m. Below the breakaway torque $F$
+the motor is stuck: friction takes all of $u$. Past it the motor moves, and friction takes the
+kinetic torque $F_c$, with the sign of $u$. With $F_c = 0$ the static friction is gone once the
+motor moves, and with $F_c = F$ it stays. Around $|u| = F$ the change is smooth over `width`,
+and the whole map is smooth, so a solver can differentiate it. It depends on the torque only,
+not on the speed.
+
+```{code-cell} python
+import numpy as np
+import matplotlib.pyplot as plt
+import virtualmodelcontrol as vmc
+
+F = 0.2  # breakaway torque [N·m]
+u = np.linspace(-0.6, 0.6, 601)  # commanded torque [N·m]
+
+fig, (top, bottom) = plt.subplots(2, 1, sharex=True)
+for Fc, style in ((0.0, "-"), (F, "--")):
+    friction = vmc.StaticFriction(F, Fc, 0.01)
+    top.plot(u, friction(u), style, label=f"$F_c$ = {Fc}")
+    bottom.plot(u, u - friction(u), style)
+top.set_ylabel(r"friction $\tau_f$")
+top.set_ylim(-0.3, 0.6)  # room for the legend
+bottom.set_ylabel(r"delivered $u - \tau_f$")
+bottom.set_xlabel(r"commanded torque $u$ [N$\cdot$m]")
+top.legend(loc="upper left", fontsize=18);
+```
+
+```{code-cell} python
+:tags: [remove-cell]
+gone, stays = vmc.StaticFriction(F, 0.0, 0.01), vmc.StaticFriction(F, F, 0.01)
+delivered = u - gone(u)
+assert abs(delivered[abs(u) < 0.15]).max() < 2e-3  # a dead band
+assert abs(delivered[abs(u) > 0.4] - u[abs(u) > 0.4]).max() < 2e-3
+assert abs((u - stays(u))[u > 0.4] - (u[u > 0.4] - F)).max() < 2e-3
+assert not vmc.StaticFriction()(u).any()  # no friction by default
+```
+
+The motor delivers nothing in the dead band, and then the command less $F_c$. The default is
+$F = F_c = 0$, which is no friction at all. The three numbers are `design` Params, to tune or to
+identify.
+
+## In a robot
+
+Friction belongs to the motor, so it goes with the robot's transmission: its `Efficiency` takes
+a `friction`. Here is a joint with an inertia and a damper, and a virtual spring of
+1 N·m/rad that pulls it to 1 rad:
+
+```{code-cell} python
+from virtualmodelcontrol.models import Direct, Efficiency, JointSpace
+
+def system_of(friction=None, stiffness=1.0):
+    efficiency = Efficiency(1.0, friction=friction)
+    robot = vmc.Mechanism("robot", model=JointSpace(1, unit="rad"),
+                          actuation=Direct(efficiency))
+    q = robot.joint(0)
+    robot.add("inertia", vmc.Inertance(q, 1.0))  # [kg·m²]
+    robot.add("damper", vmc.LinearDamper(q, 1.5))  # [N·m·s/rad]
+    ctrl = vmc.Mechanism("ctrl")
+    goal = vmc.Ref("goal", 1, [1.0])  # [rad]
+    ctrl.add("spring", vmc.LinearSpring(q - goal, stiffness))
+    ctrl.add("damper", vmc.LinearDamper(q, 1.0))
+    return vmc.VirtualMechanismSystem(robot, ctrl)
+
+def run(system, output=(), T=30.0):
+    plant = vmc.sim.ModelPlant(system.robot)
+    controller = vmc.VMCController(vmc.compile(system), output=list(output))
+    clock = vmc.sim.SimClock(dt=1 / 100)
+    return vmc.sim.run(plant, controller, clock, T=T).arrays()
+```
+
+We run it without friction, and with friction that stays at 0.3 N·m once the motor moves:
+
+```{code-cell} python
+kept = vmc.StaticFriction(0.3, 0.3, 0.005)
+free, held = run(system_of()), run(system_of(kept))
+
+fig, ax = plt.subplots()
+ax.plot(free["t"], free["q"][:, 0], label="no friction")
+ax.plot(held["t"], held["q"][:, 0], "--", label=r"friction 0.3 N$\cdot$m")
+ax.set_xlabel("time [s]")
+ax.set_ylabel("angle [rad]")
+ax.legend(loc="lower right", fontsize=18);
+```
+
+```{code-cell} python
+:tags: [remove-cell]
+short = 1.0 - held["q"][-1, 0]
+assert abs(1.0 - free["q"][-1, 0]) < 1e-3
+assert abs(short - 0.3) < 0.015
+glue("short", float(short), display=False)
+```
+
+The joint without friction arrives. With it, the joint stops {glue:text}`short:.2f` rad short:
+the spring pulls with $K e$, and the motor is stuck while that is below the breakaway torque, so
+it stops at $e = F / K$. A stiffer spring narrows the gap, and a spring that is too soft for the
+friction does not move the joint at all.
+
+## Compensation
+
+The compensation adds the friction to the command, in the direction of the command:
+$u + F_c u / \sqrt{u^2 + w^2}$, smooth in $u$. `StaticFrictionCompensation.of(friction)` takes
+$F_c$ and the width from a `StaticFriction`, and an optional `fraction` of it. It is an output
+stage, applied after the controller's law like the other ones, and nothing turns it on but us:
+
+```{code-cell} python
+from virtualmodelcontrol.control import StaticFrictionCompensation
+
+full = StaticFrictionCompensation.of(kept)
+half = StaticFrictionCompensation.of(kept, fraction=0.5)
+stages = {"none": [], "half": [half], "full": [full]}
+errors = {name: 1.0 - run(system_of(kept), stage)["q"][-1, 0]
+          for name, stage in stages.items()}
+errors
+```
+
+```{code-cell} python
+:tags: [remove-cell]
+assert abs(errors["none"] - 0.3) < 0.015
+assert abs(errors["half"] - 0.15) < 0.02
+assert abs(errors["full"]) < 0.02
+glue("full", float(errors["full"]), display=False)
+glue("half", float(errors["half"]), display=False)
+```
+
+Compensating all of the friction leaves {glue:text}`full:.3f` rad, and half of it leaves
+{glue:text}`half:.2f` rad. The compensation is exact only for the kinetic torque: if the
+breakaway torque $F$ is larger than $F_c$, a band of $F - F_c$ is left, because a command that
+is not enough to break away is not enough with the compensation either.
+
+The compensation adds energy: it pushes with $F_c$ in the direction of the command, which is
+up to $F_c |\dot\theta|$ of power. The friction map has its own limit. It depends on the torque
+alone, so it does not know which way the motor moves. When a command brakes the motion, the map
+takes from the braking torque and can create power, up to $F |\dot\theta|$:
+
+```{code-cell} python
+friction = vmc.StaticFriction(F, 0.0, 0.01)
+rate = np.linspace(-3, 3, 61)  # [rad/s]
+power = friction(u)[:, None] * rate[None, :]  # friction torque times rate
+created = -power.min()  # [W]
+created, F * abs(rate).max()
+```
+
+```{code-cell} python
+:tags: [remove-cell]
+assert 0 < created <= F * abs(rate).max()
+agree = (u[:, None] * rate[None, :]) >= 0
+assert (power[agree] >= -1e-15).all()  # and it only dissipates when they agree
+glue("created", float(created), display=False)
+glue("bound", float(F * abs(rate).max()), display=False)
+```
+
+So the friction is dissipative when the command and the motion agree, and otherwise creates at
+most {glue:text}`created:.2f` W here, at 3 rad/s, under the bound $F |\dot\theta|$ of
+{glue:text}`bound:.1f` W. A passivity argument that counts on the motors' friction can count
+on no more than that.
+
+## Identification
+
+The three numbers are Params of the robot, so `fit_params` finds them from a run, with the
+stiffness and damping. The command has to rise slowly from rest, a ramp, or the breakaway torque
+does not show: after a step the motor has broken away at the first sample. We simulate a robot
+with friction under a ramp of torque, up and down, and fit from a rough guess:
+
+```{code-cell} python
+from virtualmodelcontrol.identification import fit_params
+
+dt = 1 / 330
+rise = np.arange(0.0, 0.8, 0.5 * dt)  # [N·m], 0.5 N·m/s
+command = np.concatenate([np.zeros(150), rise, rise[::-1], np.zeros(150)])
+
+truth = vmc.StaticFriction(0.3, 0.1, 0.002)
+robot = system_of(truth).robot
+robot.add("spring", vmc.LinearSpring(robot.joint(0), 40.0))
+none = vmc.VirtualMechanismSystem(robot, vmc.Mechanism("none"))
+log = vmc.sim.rollout(none, [0.0], len(command) * dt, dt, max_step=dt,
+                      u=command[:, None])
+
+guess = system_of(vmc.StaticFriction(0.2, 0.05, 0.002)).robot
+guess.add("spring", vmc.LinearSpring(guess.joint(0), 30.0))
+names = ["efficiency.friction.breakaway", "efficiency.friction.kinetic",
+         "spring.stiffness"]
+fit = fit_params(guess, names, [log], smoothing=11)
+for name in names:
+    print(name, fit.values[name].ravel().round(3))
+```
+
+```{code-cell} python
+:tags: [remove-cell]
+found = {name: float(fit.values[name].ravel()[0]) for name in names}
+assert abs(found[names[0]] - 0.3) < 0.01 and abs(found[names[1]] - 0.1) < 0.01
+assert abs(found[names[2]] - 40.0) < 0.5
+glue("breakaway", float(found[names[0]]), display=False)
+glue("kinetic", float(found[names[1]]), display=False)
+```
+
+The fit gives {glue:text}`breakaway:.2f` N·m for the breakaway and {glue:text}`kinetic:.2f` N·m
+for the kinetic torque, against 0.30 and 0.10 in the simulated robot. The fit is local: it
+starts from the robot's values, and a start far from the truth can end in a wrong one, so the
+first command at which the motor moved is a good guess for the breakaway torque. If we have the delivered torque of each motor at static commands, from a load cell,
+`fit_efficiency(commanded, delivered, friction=True)` fits the efficiency and the friction
+together; the points must cover the dead band, on both sides.
+
+```{code-cell} python
+from virtualmodelcontrol.identification import fit_efficiency
+
+commanded = np.linspace(-1.0, 1.0, 81)[:, None] * np.ones((1, 2))  # [N·m]
+dead = vmc.StaticFriction([0.25, 0.15], 0.05)  # breakaway of each motor
+motors = Efficiency((0.8, 0.6), friction=dead)
+efficiency = fit_efficiency(commanded, motors(commanded), friction=True,
+                            width=0.01)
+efficiency.friction.params["breakaway"].value.round(2)
+```
+
+```{code-cell} python
+:tags: [remove-cell]
+assert abs(efficiency.friction.params["breakaway"].value - [0.25, 0.15]).max() < 0.01
+assert abs(efficiency.params["c1"].value - [0.8, 0.6]).max() < 0.01
+```
+
+## Planning with it
+
+The friction is in the robot's dynamics, so a plan, an optimization and an MPC see it. A
+compensation joins them through `Problem(output=...)`, which applies the stage to the
+controller's command as `VMCController(output=...)` does in a simulation:
+
+```{code-cell} python
+from virtualmodelcontrol import optimization as opt
+
+kept = vmc.StaticFriction(0.3, 0.3, 0.02)
+stage = StaticFrictionCompensation.of(kept, fraction=0.7)
+system = system_of(kept)
+
+problem = opt.Problem(system, output=[stage])
+problem.add(opt.Shooting([0.0], 3.0, 11, v0=[0.0], substeps=5))
+plan = problem.solve()
+sim = vmc.sim.rollout(system, [0.0], 3.0, 0.06, v0=[0.0], max_step=0.06,
+                      output=[stage])
+gap = abs(plan.q[:-1] - sim["q"][::5]).max()
+```
+
+```{code-cell} python
+:tags: [remove-cell]
+assert plan.converged and gap < 1e-8
+glue("gap", float(gap), display=False)
+```
+
+The plan follows the simulation within {glue:text}`gap:.0e` rad, as without friction: it is the
+same closed loop, with the motor's friction and the compensation in it. `MPC(problem)` takes the
+same problem, so it plans with friction too.
+
+## Good to know
+
+- **Off by default.** `Efficiency(friction=None)` and `StaticFriction()` with no torques are the
+  robots as they were, and no template turns the compensation on. The finger's and the hand's
+  lab values, `adapt.finger_friction()` and `adapt.hand_friction()`, are there to ask for.
+- **The width** is the smoothing of the corner at the breakaway torque, in N·m. A smaller one
+  is closer to a real motor and harder for a solver: a plan from rest may need a warm start.
+- **No memory.** The map depends on the torque we apply and not on how the motor got there, so
+  it has no hysteresis: a real motor that has stopped holds up to $F$ against the net torque,
+  load included.
+- **Where it acts.** The friction takes its torque from the command before the efficiency
+  polynomial, in the units of the command.

{virtualmodelcontrol-1.0.0 → virtualmodelcontrol-1.1.0}/src/virtualmodelcontrol/__init__.py

@@ -55,7 +55,7 @@ from .mechanisms import (
     TanhSpring,
     Time,
 )
-from .models import Efficiency, Kinematics
+from .models import Efficiency, Kinematics, StaticFriction
 from .system import VirtualMechanismSystem
 
 try:
@@ -128,6 +128,7 @@ __all__ = [
     "SpeedRegulator",
     "SphereDistance",
     "Stack",
+    "StaticFriction",
     "Sum",
     "TanhDamper",
     "TanhSpring",

{virtualmodelcontrol-1.0.0 → virtualmodelcontrol-1.1.0}/src/virtualmodelcontrol/_version.py

@@ -18,7 +18,7 @@ version_tuple: tuple[int | str, ...]
 commit_id: str | None
 __commit_id__: str | None
 
-__version__ = version = '1.0.0'
-__version_tuple__ = version_tuple = (1, 0, 0)
+__version__ = version = '1.1.0'
+__version_tuple__ = version_tuple = (1, 1, 0)
 
 __commit_id__ = commit_id = None

{virtualmodelcontrol-1.0.0 → virtualmodelcontrol-1.1.0}/src/virtualmodelcontrol/control/__init__.py

@@ -2,7 +2,13 @@
 
 from .blending import SwapController, blend_weight
 from .controller import VMCController
-from .output import FrictionCompensation, Pretension, TorqueLimit, TorqueOffset
+from .output import (
+    FrictionCompensation,
+    Pretension,
+    StaticFrictionCompensation,
+    TorqueLimit,
+    TorqueOffset,
+)
 from .projection import project_psd
 from .schedule import Schedule, ScheduledController
 from .state import StateController
@@ -14,6 +20,7 @@ __all__ = [
     "Schedule",
     "ScheduledController",
     "StateController",
+    "StaticFrictionCompensation",
     "SwapController",
     "Tank",
     "TorqueLimit",

{virtualmodelcontrol-1.0.0 → virtualmodelcontrol-1.1.0}/src/virtualmodelcontrol/control/output.py

@@ -9,6 +9,7 @@ from __future__ import annotations
 from dataclasses import dataclass
 from typing import Any
 
+import casadi as ca
 import numpy as np
 from numpy.typing import ArrayLike
 
@@ -33,6 +34,40 @@ class FrictionCompensation:
         return u + np.asarray(self.max_torque) * np.exp(-((v / self.velocity) ** 2)) * np.sign(u)
 
 
+@register("output", "static_friction_compensation")
+@dataclass
+class StaticFrictionCompensation:
+    """The kinetic friction torque added in the direction of the command, smooth in the command:
+    u + fraction · Fc · u / √(u² + w²).
+
+    ``kinetic`` Fc [N·m] (scalar or per motor), ``width`` w [N·m], ``fraction`` of Fc that is
+    compensated. Against an actuator with ``StaticFriction(F, Fc)`` it delivers the command once
+    the actuator moves and narrows the dead band from F to F − Fc. It adds energy (up to Fc |θ̇|),
+    so nothing turns it on but you. ``symbolic`` is the same map for a plan (``Problem(output=)``).
+    """
+
+    kinetic: ArrayLike
+    width: float = 0.01
+    fraction: float = 1.0
+
+    @classmethod
+    def of(cls, friction: Any, fraction: float = 1.0) -> StaticFrictionCompensation:
+        """The compensation of a ``StaticFriction``, at its current kinetic torque and width."""
+        return cls(
+            friction.params["kinetic"].value, float(friction.params["width"].value), fraction
+        )
+
+    def __call__(self, u: np.ndarray, meas: Signals) -> np.ndarray:
+        """Add the compensation."""
+        a = np.sqrt(u * u + self.width**2)
+        return u + self.fraction * np.asarray(self.kinetic) * u / a
+
+    def symbolic(self, u: Any) -> Any:
+        """The same map for CasADi expressions."""
+        a = ca.sqrt(u * u + self.width**2)
+        return u + self.fraction * ca.DM(np.asarray(self.kinetic, dtype=float)) * u / a
+
+
 @register("output", "pretension")
 @dataclass
 class Pretension:

virtualmodelcontrol-1.1.0/src/virtualmodelcontrol/identification/efficiency.py

@@ -0,0 +1,95 @@
+"""A transmission efficiency fitted to static measurements, by least squares through the origin."""
+
+from __future__ import annotations
+
+import numpy as np
+from numpy.typing import ArrayLike
+
+from ..models.efficiency import Efficiency
+from ..models.friction import StaticFriction, friction_torque
+
+
+def fit_efficiency(
+    commanded: ArrayLike,
+    measured: ArrayLike,
+    degree: int = 1,
+    *,
+    weights: ArrayLike | None = None,
+    shared: bool = False,
+    friction: bool = False,
+    width: float = 0.01,
+) -> Efficiency:
+    """The polynomial efficiency, of ``degree``, that best explains static measurements.
+
+    ``commanded`` [N·m] has one row per static point and one column per motor. Without
+    ``weights``, ``measured`` holds each motor's delivered torque, laid out the same way. With
+    ``weights`` (same layout), it holds one value per point, Σⱼ wⱼ τⱼ: a force along a direction,
+    say, with wⱼ from the Jacobian. Each motor gets its own coefficients, or one set for all with
+    ``shared``. No constant term: no command, no torque.
+
+    With ``friction``, the actuator's static friction is fitted too, the breakaway and the kinetic
+    torque of a ``StaticFriction`` (``width`` [N·m] is its smoothing, which static points do not
+    determine): the points must then span the dead band, commands from well below to well above
+    the breakaway torque, in both directions. It needs each motor's delivered torque: no
+    ``weights``.
+    """
+    u = np.asarray(commanded, dtype=float)
+    u = u.reshape(len(u), -1)
+    if friction:
+        if weights is not None:
+            raise ValueError("friction needs the delivered torque of each motor: leave out weights")
+        return _with_friction(
+            u, np.asarray(measured, dtype=float).reshape(u.shape), degree, shared, width
+        )
+    powers = np.stack([u**k for k in range(1, degree + 1)], axis=-1)  # point, motor, power
+    if weights is None:
+        tau = np.asarray(measured, dtype=float).reshape(u.shape)
+        if shared:
+            return Efficiency(*_least_squares(powers.reshape(-1, degree), tau.ravel()))
+        c = np.array([_least_squares(powers[:, j], tau[:, j]) for j in range(u.shape[1])])
+        return Efficiency(*c.T)
+    w = np.asarray(weights, dtype=float).reshape(u.shape)
+    y = np.asarray(measured, dtype=float).ravel()
+    phi = w[:, :, None] * powers
+    if shared:
+        return Efficiency(*_least_squares(phi.sum(axis=1), y))
+    return Efficiency(*_least_squares(phi.reshape(len(u), -1), y).reshape(u.shape[1], degree).T)
+
+
+def _least_squares(a: np.ndarray, b: np.ndarray) -> np.ndarray:
+    return np.linalg.lstsq(a, b, rcond=None)[0]
+
+
+def _with_friction(
+    u: np.ndarray, tau: np.ndarray, degree: int, shared: bool, width: float
+) -> Efficiency:
+    """The polynomial and the friction that best explain delivered torques, motor by motor or one
+    set for all. For given friction the polynomial is linear, so only the breakaway and the
+    kinetic torque are searched: on a grid, then refined."""
+    from scipy.optimize import minimize
+
+    def fit(x: np.ndarray, y: np.ndarray) -> tuple[np.ndarray, float, float]:
+        def residual(f: np.ndarray) -> tuple[float, np.ndarray]:
+            net = x - friction_torque(x, abs(f[0]), abs(f[1]), width)
+            powers = np.stack([net**k for k in range(1, degree + 1)], axis=1)
+            c = _least_squares(powers, y)
+            return float(((powers @ c - y) ** 2).sum()), c
+
+        top = np.abs(x).max()
+        grid = [(F, Fc) for F in np.linspace(0.0, 0.95 * top, 40) for Fc in np.linspace(0.0, F, 6)]
+        best = min(grid, key=lambda f: residual(np.array(f))[0])
+        f = minimize(
+            lambda f: residual(f)[0],
+            np.array(best),
+            method="Nelder-Mead",
+            options={"xatol": 1e-7, "fatol": 1e-14},
+        ).x
+        return residual(f)[1], abs(float(f[0])), abs(float(f[1]))
+
+    if shared:
+        c, F, Fc = fit(u.T.ravel(), tau.T.ravel())  # the points of every motor, one after another
+        return Efficiency(*c, friction=StaticFriction(F, Fc, width))
+    fits = [fit(u[:, j], tau[:, j]) for j in range(u.shape[1])]
+    c = np.array([f[0] for f in fits])
+    friction = StaticFriction([f[1] for f in fits], [f[2] for f in fits], width)
+    return Efficiency(*c.T, friction=friction)

{virtualmodelcontrol-1.0.0 → virtualmodelcontrol-1.1.0}/src/virtualmodelcontrol/models/__init__.py

@@ -5,6 +5,7 @@ from .assembly import Assembly, StackedActuation
 from .continuum import PCC, segment_frame
 from .efficiency import Efficiency
 from .equations import Equations
+from .friction import StaticFriction
 from .function import FunctionModel
 from .joint_space import JointSpace
 from .kinematic import KinematicModel, evaluate_frame, from_dict
@@ -26,6 +27,7 @@ __all__ = [
     "Passive",
     "SerialChain",
     "StackedActuation",
+    "StaticFriction",
     "TendonTransmission",
     "Underactuated",
     "evaluate_frame",

{virtualmodelcontrol-1.0.0 → virtualmodelcontrol-1.1.0}/src/virtualmodelcontrol/models/efficiency.py

@@ -1,4 +1,5 @@
-"""Transmission efficiency: the motor torque a robot receives, a polynomial of the commanded one."""
+"""Transmission efficiency: the motor torque a robot receives, a polynomial of the commanded one,
+after the friction of the actuator."""
 
 from __future__ import annotations
 
@@ -10,6 +11,7 @@ from numpy.typing import ArrayLike
 from ..core.params import ParamSet, as_param
 from ..core.registry import register
 from ..core.units import NM
+from .friction import StaticFriction, as_friction
 
 
 @register("efficiency", "polynomial")
@@ -20,39 +22,57 @@ class Efficiency:
     ``Efficiency(c1, c2, ...)``: each coefficient is one value for every motor or one per motor.
     ``Efficiency(1.0)``, the default of every transmission, delivers the commanded torque. The
     coefficients are ``design`` Params ``c1`` … ``cn``, to tune or to identify.
+
+    ``friction`` (a ``StaticFriction``, none by default) is the actuator's static friction: the
+    polynomial then takes the torque that friction leaves, with u − τ_f(u) in place of u. Its
+    Params are ``friction.breakaway``, ``friction.kinetic`` and ``friction.width``.
     """
 
-    def __init__(self, *coefficients: Any) -> None:
+    def __init__(self, *coefficients: Any, friction: Any = None) -> None:
         if not coefficients:
             raise ValueError("an efficiency needs at least its linear coefficient c1")
         self.params = ParamSet()
         for k, c in enumerate(coefficients, start=1):
             unit, bounds = ("", (0.0, 1.0)) if k == 1 else (f"({NM})^{1 - k}", (-np.inf, np.inf))
             self.params.add(as_param(c, f"c{k}", unit=unit, bounds=bounds, scope="design"), f"c{k}")
+        self._degree = len(coefficients)
+        self.friction: StaticFriction | None = as_friction(friction)
+        if self.friction is not None:
+            self.params.merge(self.friction.params, "friction")
 
     @property
     def degree(self) -> int:
         """The highest power of the commanded torque."""
-        return len(self.params)
+        return self._degree
 
     def delivered(self, u: Any, p: dict[str, Any]) -> Any:
         """Delivered torques for commanded ones ``u``, with the coefficients ``p`` by name."""
+        if self.friction is not None:
+            own = {k[len("friction.") :]: v for k, v in p.items() if k.startswith("friction.")}
+            u = u - self.friction.torque(u, own)
         return sum(p[f"c{k}"] * u**k for k in range(1, self.degree + 1))
 
     def __call__(self, u: ArrayLike) -> np.ndarray:
         """Delivered torques [N·m] at the current coefficients; motors along the last axis."""
-        u = np.asarray(u, dtype=float)
-        return np.asarray(sum(self.params[f"c{k}"].value * u**k for k in range(1, self.degree + 1)))
+        net = np.asarray(u, dtype=float)
+        if self.friction is not None:
+            net = net - self.friction(net)
+        return np.asarray(
+            sum(self.params[f"c{k}"].value * net**k for k in range(1, self.degree + 1))
+        )
 
     def to_dict(self) -> dict[str, Any]:
-        """The coefficients at their current values."""
-        values = [p.value.tolist() for p in self.params.values()]
-        return {"type": "polynomial", "coefficients": values}
+        """The coefficients (and the friction) at their current values."""
+        values = [self.params[f"c{k}"].value.tolist() for k in range(1, self.degree + 1)]
+        out: dict[str, Any] = {"type": "polynomial", "coefficients": values}
+        if self.friction is not None:
+            out["friction"] = self.friction.to_dict()
+        return out
 
     @classmethod
     def from_dict(cls, data: dict[str, Any]) -> Efficiency:
         """Inverse of ``to_dict``."""
-        return cls(*data["coefficients"])
+        return cls(*data["coefficients"], friction=data.get("friction"))
 
 
 def as_efficiency(value: Any) -> Efficiency: