Track a contact force¶

In this tutorial the fingertip presses a table with a chosen force. We do not work out how deep the goal must be. The controller moves its own goal until the measured force is the one we want.

The law¶

A spring pulls the fingertip to a goal, and the table pushes back. The force on the table is a function of the goal, so we can ask which way to move the goal to bring the force closer to the wanted one. ForceTracking answers it. It moves live Params of the controller, such as the spring’s goal or its stiffness, by gradient descent of \(\tfrac12 \lVert f - f_\mathrm{des} \rVert^2\). A Param is live when it can change while the controller runs (Parameters). At every control step the law moves them by the largest step that changes the force by less than max_force_step [N], 0.01 N by default.

The force \(f\) is the one the motors push through the contact point, a vector of three entries. The law needs the measured force too, in the same form. On a real robot it comes from a load cell, or from an estimate. Here it comes from the simulated table first.

Press with a chosen force¶

The table is the one of the contact tutorial, a plane 6 cm below the finger’s base. The finger’s \(z\) axis points down, so the table’s outward normal is \(-z\), and the finger pushes on it along \(+z\).

import numpy as np
import matplotlib.pyplot as plt
import virtualmodelcontrol as vmc
from virtualmodelcontrol.adaptation import ForceTracking
from virtualmodelcontrol.robots import adapt

k = 1e4  # [N/m], a hard table
finger = adapt.add_dynamics(adapt.finger())
tip = finger.point("tip")
gap = vmc.PlaneDistance(tip, normal=[0, 0, -1], origin=[0, 0, 0.06])
finger.add("table", vmc.ContactSpring(gap, k))
finger.add("cushion", vmc.ContactDamper(gap, 5.0))  # [N·s/m]

goal = vmc.Ref("goal", 3, value=[0.0, 0.05, 0.06])  # starts on the table
ctrl = vmc.Mechanism("ctrl")
ctrl.add("press", vmc.LinearSpring(tip - goal, 100.0))  # [N/m]
ctrl.add("damp", vmc.LinearDamper(tip, 1.0))
ctrl.add("limits", adapt.joint_limit_spring(finger))  # joints in range
ctrl.add("gravity", vmc.GravityCompensation(finger))
system = vmc.VirtualMechanismSystem(finger, ctrl)
compiled = vmc.compile(system)

The loop is the usual one. After the controller’s step it reads the table’s force, as a load cell would, and gives it to the law, which changes the Param. The wanted force is 2 N along \(+z\), into the table, and normal=[0, 0, 1] tells the law that the force it tracks is the one along \(z\). adapted names the Param and act replaces the law, in the sections below. The law can also be told an estimate in place of the table’s force: see Estimate a contact force.

wanted = np.array([0.0, 0.0, 2.0])  # [N]
kin = vmc.Kinematics(finger)

def press(controller, adapted="ctrl.press.goal", act=None, steps=3000):
    if act is None:
        law = ForceTracking(controller, "tip", adapted, normal=[0, 0, 1])
        act = law.step
    plant = vmc.sim.ModelPlant(finger, q0=[0.8, 0.8], max_step=1e-4)
    names = ["t", "force", "depth", "stiffness", "level"]
    log = {name: [] for name in names}
    for step in range(steps):  # 500 Hz
        plant.write(controller.step(plant.t, plant.read()))
        below = kin.position(plant.q, "tip")[2] - 0.06  # [m]
        table = np.array([0.0, 0.0, k * max(0.0, below)])  # a load cell
        if step > 100:  # let the finger settle on the table first
            act(controller, table, wanted)
        live = controller.live_params()
        level = getattr(controller, "level", 0.0)  # a tank's budget [J]
        depth = live["ctrl.press.goal"][2] - 0.06  # [m]
        stiffness = float(live["ctrl.press.stiffness"])  # [N/m]
        row = (plant.t, table[2], depth, stiffness, level)
        for name, value in zip(names, row):
            log[name].append(value)
        plant.advance(1 / 500)
    return {name: np.array(values) for name, values in log.items()}

free = press(vmc.VMCController(compiled))

The spike at the start is the finger landing on the table. From 1.0 s on, the force stays within 0.1 N of the wanted one. At the end it is 2.00 N on average, and it ripples by 0.042 N from peak to peak: every step changes the force by about max_force_step, so near the target it steps over it and back. The goal ends 20.3 mm below the table, a depth we never had to compute. A smaller max_force_step, 0.002 N for instance, gives a smaller ripple and a slower approach. The law’s max_step also caps how far a Param moves in one step, in the Param’s own unit (meters for a goal). It has nothing to do with the max_step of ModelPlant, the integration step.

Through a tank¶

Moving a goal deeper stores energy in the spring. The goal moves only as fast as a tank pays for it. Pass the tank in place of the controller, to the loop and to the law. An empty tank fills from what the controller’s dampers take, which happens while the finger moves. Once it rests, nothing refills it:

def with_tank(level):  # [J]
    return vmc.control.Tank(vmc.VMCController(compiled), level=level)

stalled = press(with_tank(0.0))
funded = press(with_tank(0.05))

The tank with 0.05 J gets there and ends with 0.038 J. The empty one stalls at 1.32 N: its level is 0.000 J at the end, because the finger has come to rest and its dampers take nothing. A tank lets the controller’s energy rise only by what it holds, so a law running through one cannot pump energy into the controller, whatever force it is asked for.

By stiffness¶

The law moves any live Param, so the stiffness of the spring can do the work of its goal: the same call with "ctrl.press.stiffness" in place of the goal’s name. We fix the goal 20 mm below the table and start with a spring that is too soft:

soft = vmc.VMCController(compiled)
soft.set({"ctrl.press.stiffness": 40.0,  # [N/m]
          "ctrl.press.goal": [0.0, 0.05, 0.08]})  # [m]
stiff = press(soft, "ctrl.press.stiffness")

The stiffness settles at 101 N/m. The spring and the table push in series, so the force is \(K k d / (K + k)\) for a goal at depth \(d\), and 2 N at 20 mm needs 101 N/m. A new value stays within the Param’s bounds, so a stiffness never goes below zero. A stiffness given as a matrix stays symmetric and positive semidefinite, so the spring cannot store negative energy.

Laws without a model¶

Two more laws need no model of the robot, only a force. ForceRatio scales a stiffness up when the force is too small and down when it is too large, by max_change (5 % here) times the relative error at each step: quickly far from the target and gently near it. Stiffening sets the stiffness from the measured force,

soft until the finger touches something, stiffer as it presses. In the call, low and high are \(K_\mathrm{low}\) and \(K_\mathrm{high}\) and alpha is \(\alpha\). With the goal fixed 20 mm below the table and the spring starting soft:

from virtualmodelcontrol.adaptation import ForceRatio, Stiffening

def fresh():
    controller = vmc.VMCController(compiled)
    controller.set({"ctrl.press.stiffness": 40.0,  # [N/m]
                    "ctrl.press.goal": [0.0, 0.05, 0.08]})  # [m]
    return controller

ratio_run = fresh()
ratio_law = ForceRatio(ratio_run, "ctrl.press.stiffness", max_change=0.05)
scaled = press(ratio_run, act=ratio_law.step)

stiff_run = fresh()
stiffen = Stiffening(stiff_run, "ctrl.press.stiffness",
                     low=40.0, high=200.0, alpha=0.2)  # [N/m], [N/m], [1/N]
stiffened = press(stiff_run,
                  act=lambda c, force, wanted: stiffen.step(c, force[2]))

The ratio law ends at 2.00 N with a stiffness of 101 N/m, as the gradient law did. The stiffening has no target. It settles where the force it produces and the stiffness it asks for agree, which need not be the wanted force: 1.71 N at 86 N/m. A stiffness that depends on the deflection, in place of the force, is a nonlinear spring: vmc.SigmoidSpring and vmc.PolynomialSpring.

What the calls need¶

• ForceTracking(controller, site, params, normal=None, *, max_force_step=0.01, max_step=None, rate=None): a step is the largest one that changes the force by max_force_step, or moves a Param by max_step, or by a fixed rate if one is given. site is where the contact is, a name such as "tip", an arc parameter s, or (part, s) for a part of an assembly, as in the two arms. params are glob patterns of the controller’s live Params; compile with runtime=[...] for the Params that are not live by default.

• normal is the direction of the force the robot exerts on its surroundings, opposite to the surface’s outward normal. Without it the law tracks the full 3D force. The measured and the wanted force are vectors of three entries.

• step(controller, f_meas, f_des) makes one step and returns the jump of the controller’s energy [J] that was applied (a tank applies only the part it pays for). law.alpha is the step size of the last step. direction(...) gives the descent direction of each Param without applying it.

• ForceRatio(controller, params, max_change=0.05) and Stiffening(controller, params, low, high, alpha) take the controller and glob patterns like ForceTracking. ForceRatio.step takes the measured and the wanted force, Stiffening.step the measured force along the contact normal, a number. Both return the jump too; law.ratio and law.k are what they asked for.

Take it to the robot¶

The loops above run a simulated plant. On the robot the same law runs inside the loop that talks to the motors, with nothing from vmc.sim: the controller steps with the reading, and the law changes the controller with the force that a load cell or an estimate gives. Here the finger’s controller of “Press with a chosen force” runs through a tank that starts empty (level) and holds at most 10 mJ (capacity), as one control period of your control loop would, with a made-up reading:

from virtualmodelcontrol.control import Tank

budget = Tank(vmc.VMCController(compiled), level=0.0, capacity=0.01)  # [J]
law = ForceTracking(budget, "tip", "ctrl.press.goal", normal=[0, 0, 1])
wanted = np.array([0.0, 0.0, 2.0])  # [N]

def control_step(t, reading, force):
    """What your loop does each period: the torques, then the law."""
    command = budget.step(t, reading)
    law.step(budget, force, wanted)
    return command["motor_torque"]

reading = vmc.Signals(0.0, motor_position=[0.8, 0.8],
                      motor_velocity=[0.0, 0.0])
budget.reset(0.0, reading)
control_step(0.0, reading, force=np.array([0.0, 0.0, 0.5]))  # [N·m]

array([0.03696953, 0.04673963])