Two arms: squeeze an object¶

In this example we squeeze an object between the tips of two soft arms, with one virtual spring that couples the arms. Then we hold it with a force we choose, each arm adapting its own spring.

The robot¶

bimanual.arms builds two Helyx arms, like the soft arm of the soft-arm example but with a longer first segment (29, 14.5 and 14.5 cm), standing side by side on one frame and pointing up. As one robot, its configuration stacks the right arm’s \(\Delta\) and then the left arm’s, and its motors follow the same order.

The frame’s origin lies midway between the bases, with \(z\) up. Both arms keep its orientation, so the tendons of each sit as in the soft-arm example, and the left arm’s motors are the last nine (indices 9 to 17). These are the Params of the default pair, read from the template; the rest of the page uses it:

The stiffness and damping come from an identification on the real arms. Only the simulator uses them. The fit used the commanded motor torques, so the simulated arms take each torque as the controller sends it: their efficiency is 1, the default of every template. The efficiency of the tendons, measured against a load cell, is bimanual.EFFICIENCY (0.12). It matters for the forces the real arms exert on their surroundings. The efficiency page explains when to use it.

The arms’ encoders follow the library’s sign convention (bimanual.ENCODER_SIGN is +1), and their controller runs at 500 Hz. On the real arms, bimanual.output_stage() adds 0.03 N·m to every command, a soft stop of 0.03 N·m/rad on motors that release their tendon past 30°, and a clip at ±0.5 N·m. The simulation below leaves it out.

The template takes its geometry as arguments, one value for both arms or a dict by arm. Here the segments are 30, 15 and 15 cm long (the default is 29, 14.5 and 14.5 cm) and the bases 30 cm apart (the default is 25 cm). This pair is only a check of the geometry arguments:

import numpy as np
import virtualmodelcontrol as vmc
from virtualmodelcontrol.robots import bimanual

pair = bimanual.arms(lengths=(0.3, 0.15, 0.15),  # [m], both arms
                     base_positions={"right": (0.15, 0.0, 0.0),
                                     "left": (-0.15, 0.0, 0.0)})
kin = vmc.Kinematics(pair)
[kin.position(np.zeros(18), (arm, 1.0)) for arm in bimanual.ARMS]  # tips

[array([0.15, 0.  , 0.6 ]), array([-0.15,  0.  ,  0.6 ])]

Squeeze an object¶

An object 10 cm wide sits between the tips. In the simulation it belongs to the robot mechanism, as a contact: a one-sided spring on the distance between the tips that pushes them apart once they come closer than its width. The controller does not know about it.

arms = bimanual.add_dynamics(bimanual.arms())
right, left = arms.point("right", s=1.0), arms.point("left", s=1.0)
distance = vmc.Norm(right - left)  # between the tips [m]
width, k_object = 0.10, 200.0  # [m], [N/m]
arms.add("object", vmc.ContactSpring(distance - width, k_object))

The controller pulls the tips together with a spring of zero rest length between them, slows their closing with a damper in parallel, and cancels the weight of the segments.

K = 20.0  # [N/m]
ctrl = vmc.Mechanism("ctrl")
ctrl.add("squeeze", vmc.LinearSpring(distance, K))
ctrl.add("damp", vmc.LinearDamper(distance, 1.0))  # [N·s/m]
ctrl.add("gravity", vmc.GravityCompensation(arms))

system = vmc.VirtualMechanismSystem(arms, ctrl)
controller = vmc.VMCController(vmc.compile(system))
plant = vmc.sim.ModelPlant(arms)
clock = vmc.sim.SimClock(dt=1 / bimanual.CONTROL_RATE)
log = vmc.sim.run(plant, controller, clock, T=2.0)

We follow the distance between the tips, the force of the spring and the force on the object:

import matplotlib.pyplot as plt
from virtualmodelcontrol import viz

kin = vmc.Kinematics(arms)
rows = log.arrays()

def tips(q):
    return kin.position(q, ("right", 1.0)), kin.position(q, ("left", 1.0))

d = np.array([np.linalg.norm(a - b) for a, b in map(tips, rows["q"])])
on_object = k_object * np.maximum(width - d, 0.0)  # [N]

fig, (top, bottom) = plt.subplots(2, 1, figsize=(4.8, 6.0), sharex=True)
top.plot(rows["t"], 100 * d)
top.axhline(100 * width, color=viz.PALETTE[1], ls="--", label="object")
top.set_ylabel("tip distance [cm]")
top.legend()
bottom.plot(rows["t"], K * d, label="spring")
bottom.plot(rows["t"], on_object, label="on the object")
bottom.set_xlabel("time [s]")
bottom.set_ylabel("force [N]")
bottom.legend();

The tips meet the object after 0.16 s and stop on it. The spring’s torques then pull with 1.88 N and the object feels 1.21 N: the rest holds the soft arms bent. A stiffer spring squeezes harder.

Hold it with a chosen force¶

The spring above squeezes with whatever force its stiffness leaves. To hold the object with a chosen force, we give each arm a spring of its own, towards a goal on a face of the object, and let a force tracking law move each arm’s goal. Each arm pushes along the line between the tips, the \(x\) axis: the right arm towards \(-x\), the left one towards \(+x\). The goals start on the object’s faces, 5 cm from the middle, at the height of the tips at rest.

from virtualmodelcontrol.adaptation import ForceTracking
from virtualmodelcontrol.estimation import ContactForce

push = {"right": np.array([-1.0, 0.0, 0.0]),  # each arm pushes
        "left": np.array([1.0, 0.0, 0.0])}  # towards the other
height = kin.position(np.zeros(18), ("right", 1.0))[2]  # the tips at rest
grip = vmc.Mechanism("grip")
for arm, x in (("right", 0.05), ("left", -0.05)):  # on the object's faces
    tip = arms.point(arm, s=1.0)
    goal = vmc.Ref(f"goal_{arm}", 3, value=[x, 0.0, height])
    grip.add(arm, vmc.LinearSpring(tip - goal, 30.0))  # [N/m]
    grip.add(f"damp_{arm}", vmc.LinearDamper(tip, 2.0))
grip.add("gravity", vmc.GravityCompensation(arms))
grip_system = vmc.VirtualMechanismSystem(arms, grip)
compiled = vmc.compile(grip_system)

Each arm needs to know the force it exerts. With a sensor on the object, both read the object’s force. Without one, each arm estimates its own force from the model of the arms alone, a copy that has no object. The laws start at 1.5 s, once the arms have settled on the faces. hold returns the time, the object’s force, what the right arm believes it pushes with, and the level of the tank, if there is one:

wanted = 1.0  # [N]

def hold(sensor, level=None, step=0.01):
    controller = vmc.VMCController(compiled)
    if level is not None:  # run through a tank of this many joules
        controller = vmc.control.Tank(controller, level=level)
    model = bimanual.add_dynamics(bimanual.arms())  # no object
    law, estimate = {}, {}
    for arm in bimanual.ARMS:
        site = (arm, 1.0)
        law[arm] = ForceTracking(controller, site, f"grip.{arm}.goal_{arm}",
                                 push[arm], max_force_step=step)  # [N]
        estimate[arm] = ContactForce(controller, site, push[arm],
                                     robot=model)
    plant = vmc.sim.ModelPlant(arms)
    dt, log = 1 / bimanual.CONTROL_RATE, []
    for _ in range(int(5.0 / dt)):
        plant.write(controller.step(plant.t, plant.read()))
        a, b = (kin.position(plant.q, (arm, 1.0)) for arm in bimanual.ARMS)
        true = k_object * max(0.0, width - np.linalg.norm(a - b))  # [N]
        for arm in bimanual.ARMS:
            told = true * push[arm] if sensor else estimate[arm](controller)
            if plant.t > 1.5:
                law[arm].step(controller, told, wanted * push[arm])
        believed = estimate["right"](controller) @ push["right"]
        tank = getattr(controller, "level", 0.0)  # [J]
        log.append((plant.t, true, believed, tank))
        plant.advance(dt)
    return np.array(log)

with_sensor, without = hold(sensor=True), hold(sensor=False)

The object feels 1.00 N with the estimates alone and 1.00 N with the sensor. Peak to peak, the force ripples by 4e-06 N with the estimates alone, which is no ripple at all, and by 0.062 N with the sensor. With the sensor, the ripple is proportional to max_force_step (0.01 N by default): half that step gives 0.031 N. The right arm’s estimate, 1.00 N, is right too, because the model has no error here. A wrong mass in the model would give a wrong force, as in Estimate a contact force.

Moving a goal inward stores energy in the spring, and a tank pays for it. The laws take the tank in place of the controller. We run it with 0.2 J and with an empty tank:

funded, empty = hold(False, level=0.2), hold(False, level=0.0)

With 0.2 J the object feels 1.00 N at the end, and 0.13 J are left. The empty tank had 0.045 J when the laws started, what the arms’ dampers took on their way to the object, and the object feels only 0.21 N.

Animate¶

viz.animate draws only springs from the robot to fixed points. The draw callback adds the object and the spring between the tips. It receives the log’s row at each frame.

def draw(ax, row):
    a, b = tips(row["q"])
    center = (a + b) / 2
    ax.add_patch(plt.Circle((center[0], center[2]), width / 2,
                            color=viz.PALETTE[1], alpha=0.3))
    viz.draw_spring(ax, b, a)

viz.animate(arms, log, "two-arms.mp4", draw=draw)