Hand: grasp, and the hand on a UR5¶

In this example we simulate the ADAPT hand grasping a ball, then mount the hand on a UR5 arm.

The robot¶

The ADAPT hand has a thumb and four fingers, driven through tendons by 13 motors: four for the thumb’s joints, one that spreads the fingers, and two per finger, for its first joint (MCP) and for its last two joints together (PIP and DIP). \(q\) holds the motor angles [rad], in this order:

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

adapt.HAND_MOTORS

('thumb_CMC1',
 'thumb_CMC2',
 'thumb_MCP',
 'thumb_IP',
 'spread',
 'index_MCP',
 'index_PIP',
 'middle_MCP',
 'middle_PIP',
 'ring_MCP',
 'ring_PIP',
 'pinky_MCP',
 'pinky_PIP')

The figure shows the hand from the front at \(q = 0\), with \(z\) up. Beside each joint is the index of the motor that turns it. The spread motor turns the index, ring and pinky fingers (dashed). The middle finger stays in place. Every number of the template is a Param, for example:

adapt.hand takes the module’s tables as keyword arguments and replaces only the entries given, here the thumb’s fingertip in its last joint frame:

custom = adapt.hand(tip_offsets={"thumb": (-0.025, 0.0, 0.0)})  # [m]
for robot in (adapt.hand(), custom):
    print(vmc.Kinematics(robot).position(np.zeros(13), "thumb/tip"))

[-0.117424  0.025127  0.014851]
[-0.142424  0.025127 -0.002649]

On the real hand, adapt.hand_output_stage() adds a friction feed-forward of 0.1 N·m that fades out above about 0.03 rad/s, then clips each command to ±0.8 N·m. The simulation below leaves it out. The real hand’s tendons only pull. The model lets every motor turn both ways.

adapt.HAND_MOTOR_EFFICIENCY holds the measured motor efficiencies: the finger’s two values on the MCP and PIP motors of each finger and on the thumb’s MCP and IP motors, and 1 elsewhere. The simulation leaves them out, for the reason given in the finger example. adapt.hand(efficiency=adapt.HAND_MOTOR_EFFICIENCY) includes them.

Grasp a ball¶

A ball sits in front of the palm, in the robot mechanism. The simulator feels it, while the controller, compiled from its own components only, does not know it is there (see contact). Springs pull three fingertips to the ball’s center.

The template’s thumb tip, (0, 0, 0.0175) in its last joint frame like the fingers’, is the point the hand’s controllers attach to. It lies beside the thumb’s last phalanx, which runs along \(-x\). For a fingertip grasp we take the end of that phalanx, as in the custom hand above. The motor damping and the ball’s stiffness are illustrative.

center, radius = np.array([-0.02, 0.08, 0.035]), 0.03  # [m]
k_ball = 5000.0  # [N/m], stiffness of the ball's surface
digits = ("thumb", "index", "middle")
thumb_end = {"thumb": (-0.025, 0.0, 0.0)}  # [m], end of the last phalanx

hand = adapt.hand(tip_offsets=thumb_end)
hand = adapt.add_dynamics(hand, damping=0.01)  # [N·m·s/rad]
ctrl = vmc.Mechanism("ctrl")
for d in digits:
    tip = hand.point(f"{d}/tip")
    ball = vmc.SphereDistance(tip, center=center, radius=radius)
    hand.add(f"ball_{d}", vmc.ContactSpring(ball, k_ball))
    ctrl.add(f"grasp_{d}", vmc.LinearSpring(tip - center, 40.0))  # [N/m]
    ctrl.add(f"damp_{d}", vmc.LinearDamper(tip, 0.2))  # [N·s/m]
ctrl.add("limits", adapt.hand_joint_limit_spring(hand))
ctrl.add("gravity", vmc.GravityCompensation(hand))
system = vmc.VirtualMechanismSystem(hand, ctrl)
controller = vmc.VMCController(vmc.compile(system))
plant = vmc.sim.ModelPlant(hand)
log = vmc.sim.run(plant, controller, vmc.sim.SimClock(dt=1 / 330), T=2.5)

kin = vmc.Kinematics(hand)
rows = log.arrays()
fig, ax = plt.subplots()
for d in digits:
    tips = np.array([kin.position(q, f"{d}/tip") for q in rows["q"]])
    gap = np.linalg.norm(tips - center, axis=1) - radius  # [m]
    ax.plot(rows["t"], k_ball * np.maximum(-gap, 0.0), label=d)
ax.set_xlabel("time [s]")
ax.set_ylabel("contact force [N]")
ax.legend();

The ball pushes a fingertip back once the tip enters it. The thumb reaches it first, after 173 ms, and the index and middle fingertips within 458 ms. Each then presses with 1.19 N: the pull of its spring at the surface, stiffness times radius (1.2 N), a little less as the ball gives.

def draw(ax, row):
    disc = plt.Circle(center[1:], radius, color=viz.PALETTE[1], alpha=0.3)
    ax.add_patch(disc)

viz.animate(hand, log, "hand-grasp.mp4", plane="yz", draw=draw,
            springs=[(f"{d}/tip", center) for d in digits])

The hand on a UR5¶

The hand sits on the flange of a UR5. The UR5’s six revolute joints come from its Denavit-Hartenberg table (ur5.arm(d=..., a=..., alpha=...) takes another one):

Joint	Name	d [m]	a [m]	α [rad]
1	shoulder_pan	0.089159	0	π/2
2	shoulder_lift	0	-0.425	0
3	elbow	0	-0.39225	0
4	wrist_1	0.10915	0	π/2
5	wrist_2	0.09465	0	−π/2
6	wrist_3	0.0823	0	0

ur5.with_hand builds both as one robot: \(q\) holds the six arm joints, then the hand’s motors. The hand sits at mounting_position [m] on the flange, turned by mounting_angle [rad] about its \(z\) axis. Gravity stays in the arm’s base frame, so the hand’s weight follows the arm’s pose. The UR5 is position-controlled and has no masses in the template, so there is nothing to simulate. Its joint angles are only measured, and only the hand’s torques are sent.

from virtualmodelcontrol.robots import ur5

robot = ur5.with_hand()  # the hand centered on the flange, not turned
ctrl = vmc.Mechanism("ctrl")
ctrl.add("gravity", vmc.GravityCompensation(robot))
system = vmc.VirtualMechanismSystem(robot, ctrl)
controller = vmc.VMCController(vmc.compile(system))

arm = [np.pi, -1.2, 1.4, -np.pi / 2 - 0.2, -np.pi / 2, -np.pi / 2]  # [rad]
x = np.concatenate([arm, np.zeros(13)])
meas = vmc.Signals(0.0, motor_position=x, motor_velocity=np.zeros(19))
u = controller.step(0.0, meas)["motor_torque"]
u[6:].round(4)  # the hand's 13 motor torques [N·m], sent to the hand

array([ 0.0006, -0.0039,  0.0004,  0.0001,  0.0002,  0.001 , -0.0003,
        0.0013, -0.0003,  0.0011, -0.0003,  0.0004, -0.0002])

They are a few mN·m each: the torques that hold up the weight of the fingers, the only thing this controller does.

The figure draws this pose:

fig, ax = plt.subplots(figsize=(6.4, 5.0))
viz.draw_robot(ax, robot, x)
lines = viz.skeleton(robot, x)  # the arm, then the five digits
for digit in lines[1:]:  # the palm: from the flange to each digit
    ax.plot(*viz.project([lines[0][-1], digit[0]]), color="0.6", lw=3)
viz.label_axes(ax);

The flange faces down, so the arm holds the hand upside down.

The hand is mounted on the flange as a part of an Assembly: Soft arms and assemblies shows how parts are mounted on each other.