Estimate a contact force¶
In this tutorial the fingertip presses a table with a chosen force and nothing measures it: we estimate it from the controller’s command and a model of the robot, follow it while the finger moves, and read the compliance of an object from it.
import numpy as np
import matplotlib.pyplot as plt
import virtualmodelcontrol as vmc
from virtualmodelcontrol.adaptation import ForceTracking
from virtualmodelcontrol.robots import adapt
The finger and the table¶
They are those of Track a contact force: a plane 6 cm below the finger’s base, a hard spring of \(10^4\) N/m, and a spring of 100 N/m pulling the tip to a goal that the law moves until the force is 2 N. In the code below the law is told an estimate, never the table’s force:
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))
ctrl.add("gravity", vmc.GravityCompensation(finger))
compiled = vmc.compile(vmc.VirtualMechanismSystem(finger, ctrl))
wanted = np.array([0.0, 0.0, 2.0]) # [N]
kin = vmc.Kinematics(finger)
Without a force sensor¶
A load cell is not always there. The controller knows what it commands, and a model knows what
the arm’s own stiffness and weight hold. The rest of the balance is the contact. ContactForce
computes it, for the controller’s Params as they are at its last step. The plant has the table,
so the model it uses is a second finger without one. The law is told the estimate:
from virtualmodelcontrol.estimation import ContactForce
def blind(model, steps=3000):
"""One run: the table's force and what the law was told [N]."""
controller = vmc.VMCController(compiled)
estimate = ContactForce(controller, "tip", [0, 0, 1], robot=model)
law = ForceTracking(controller, "tip", "ctrl.press.goal",
normal=[0, 0, 1])
plant = vmc.sim.ModelPlant(finger, q0=[0.8, 0.8], max_step=1e-4)
t, table, told = [], [], []
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]
guess = estimate(controller)
if step > 100: # let the finger settle on the table first
law.step(controller, guess, wanted)
t.append(plant.t)
table.append(k * max(0.0, below))
told.append(guess[2])
plant.advance(1 / 500)
return {"t": np.array(t), "force": np.array(table),
"told": np.array(told)}
exact = blind(adapt.add_dynamics(adapt.finger()))
An estimate is as good as its model. We run it again with the finger’s last phalanx 15 g in the model and 25 g in the plant:
light = adapt.add_dynamics(adapt.finger())
light.params["m_dip.mass"].value = 0.015 # [kg]
wrong = blind(light)
With the exact model the table’s force settles at 2.00 N. With the wrong one the law believes it holds 2.00 N, and the table feels 2.09 N: the model’s error goes straight into the force. A load cell closes this loop, because what it tells the law is the true force. It is the same call with the cell’s reading in place of the estimate.
While it moves¶
ContactForce assumes the robot is at rest, and so it is wrong while the finger lands on the
table. A MomentumObserver has no such assumption. It works from the robot’s momentum
\(M(q)\dot q\) and what the model says would change it, so it needs no acceleration. What
the model does not explain it takes for a force from outside. Its estimate follows the true force
like a low-pass filter of bandwidth gain [1/s]: the higher, the faster, and the more noise it
lets through. Like ContactForce, it takes the model of the finger without the table. Its
arguments are the system, the step between calls [s], the gain, and that model:
from virtualmodelcontrol.estimation import ContactForce, MomentumObserver
world = adapt.add_dynamics(adapt.finger())
tip = world.point("tip")
gap = vmc.PlaneDistance(tip, normal=[0, 0, -1], origin=[0, 0, 0.06])
world.add("table", vmc.ContactSpring(gap, 1e4))
world.add("cushion", vmc.ContactDamper(gap, 5.0))
ctrl = vmc.Mechanism("ctrl")
ctrl.add("press", vmc.LinearSpring(tip - [0.0, 0.05, 0.075], 100.0))
ctrl.add("damp", vmc.LinearDamper(tip, 1.0))
ctrl.add("limits", adapt.joint_limit_spring(world))
ctrl.add("gravity", vmc.GravityCompensation(world))
controller = vmc.VMCController(
vmc.compile(vmc.VirtualMechanismSystem(world, ctrl)))
model = adapt.add_dynamics(adapt.finger())
at_rest = ContactForce(controller, "tip", [0, 0, 1], robot=model)
moving = MomentumObserver(controller.compiled.system, 1 / 500, 300.0,
robot=model) # system, step [s], gain [1/s]
plant = vmc.sim.ModelPlant(world, q0=[0.8, 0.8], max_step=1e-4)
t, true, rest, flow = [], [], [], []
for _ in range(500): # 500 Hz
u = controller.step(plant.t, plant.read())["motor_torque"]
plant.write(vmc.Signals(plant.t, motor_torque=u))
moving(plant.q, plant.v, u, plant.t) # q, v and the torques just sent
pushes = plant.elements() # what each plant component pushes with
t.append(plant.t)
true.append(pushes["table"]["force"][0] + pushes["cushion"]["force"][0])
rest.append(at_rest(controller)[2])
flow.append(moving.force("tip", [0, 0, 1])[2])
plant.advance(1 / 500)
t, true, rest, flow = (np.array(x) for x in (t, true, rest, flow))
Before the finger arrives, the estimate at rest already reads 3.5 N: it sees the controller pulling and takes the finger to be held. The observer starts from no force and follows the true one a little late, by about \(1/\text{gain}\) = 3 ms. In the first 0.2 s its error is on average 0.36 N, against 0.71 N at rest. Once the finger rests, all three agree.
An object’s compliance¶
How soft is the object the finger touches? Press it at a gentle setting of the controller’s
stiffness, then at stiffer ones. The tip sinks a little further and pushes a little harder, and
the ratio of the two changes, the distance over the force, is the object’s compliance in m/N.
Both are known without a sensor on the object: the tip’s position from the joint angles, and
the force from ContactForce. object_compliance takes the median of the samples of each
setting and gives that ratio.
Here three objects of stiffness 4000, 1000 and 250 N/m are pressed 1.5 cm in by the spring of the controller, at 50 N/m first and then at 100 and 200 N/m:
from virtualmodelcontrol.estimation import ContactForce, object_compliance
def probe(stiffness, press, model=None, steps=750):
"""Samples of the tip's position and of the estimated force [N]."""
world = adapt.add_dynamics(adapt.finger())
tip = world.point("tip")
surface = vmc.PlaneDistance(tip, normal=[0, 0, -1], origin=[0, 0, 0.06])
world.add("object", vmc.ContactSpring(surface, stiffness))
world.add("cushion", vmc.ContactDamper(surface, 5.0))
ctrl = vmc.Mechanism("ctrl")
ctrl.add("press", vmc.LinearSpring(tip - [0.0, 0.05, 0.075], press))
ctrl.add("damp", vmc.LinearDamper(tip, 1.0))
ctrl.add("limits", adapt.joint_limit_spring(world))
ctrl.add("gravity", vmc.GravityCompensation(world))
system = vmc.VirtualMechanismSystem(world, ctrl)
controller = vmc.VMCController(vmc.compile(system))
model = model or adapt.add_dynamics(adapt.finger())
estimate = ContactForce(controller, "tip", [0, 0, 1], robot=model)
plant = vmc.sim.ModelPlant(world, q0=[0.8, 0.8], max_step=1e-4)
kin, position, force = vmc.Kinematics(world), [], []
for _ in range(steps): # 500 Hz
plant.write(controller.step(plant.t, plant.read()))
position.append(kin.position(plant.q, "tip"))
force.append(estimate(controller))
plant.advance(1 / 500)
return np.array(position[-150:]), np.array(force[-150:]) # last 0.3 s
found = {}
for stiffness in (4000.0, 1000.0, 250.0): # [N/m]
gentle = probe(stiffness, 50.0)
stiff = [probe(stiffness, press) for press in (100.0, 200.0)]
found[stiffness] = [1e3 * object_compliance(*s, *gentle) for s in stiff]
print(f"{1e3 / stiffness:5.2f} mm/N: {found[stiffness][0]:5.2f} and "
f"{found[stiffness][1]:5.2f} mm/N")
0.25 mm/N: 0.25 and 0.25 mm/N
1.00 mm/N: 1.00 and 1.00 mm/N
4.00 mm/N: 4.00 and 4.00 mm/N
Each line gives the object’s true compliance, \(1/k\), then the estimates from the two stiffer settings. With the exact model of the finger they agree to rounding error (the largest difference is 5e-11 %).
lighter = adapt.add_dynamics(adapt.finger())
lighter.params["m_dip.mass"].value = 0.015 # [kg], 10 g under the plant's
baseline = probe(1000.0, 50.0, lighter)
off = 1e3 * object_compliance(*probe(1000.0, 200.0, lighter), *baseline)
A model that is wrong by a constant, here a last phalanx 10 g too light, moves the forces of both settings by the same amount, and the difference does not see it: the same object of 1.00 mm/N gives 1.00 mm/N.
What the calls need¶
ContactForce(controller, site, normal=None, robot=None)estimates the force from the controller’s command and the model of the robot. Call it with the controller.robotis the model of the arm alone, without the surroundings: it gives the arm’s own stiffness and weight, which the command balances first. The transmission, with its efficiency, is the system’s. The estimate uses the torques the controller’s law asks for, before any output stage, and leaves velocities out. A Param that acts only on a virtual state does not change the force at once, soForceTrackingfinds no direction for it and leaves it where it is.MomentumObserver(system, dt, gain, robot=None)takes the system, the time between calls [s], the bandwidthgain[1/s] and, asContactForcedoes, the model without the surroundings. Callobserver(q, v, u, t)once per step with the configuration, the velocity and the torques just sent, thenobserver.force(site, normal)for the force at a site.object_compliance(position, force, baseline_position, baseline_force)takes samples (rows of three) of the tip’s position [m] and force [N] at a stiff setting and, after them, the same at a gentle one. It returns the compliance [m/N]: the distance between the medians of the positions over the difference between the medians of the forces.