Soft arms and assemblies¶
In this tutorial we build a continuum arm with tendons, spread its mass along it, and mount parts on each other as an assembly. Then we take the soft arm’s controller to the robot.
import numpy as np
import matplotlib.pyplot as plt
import virtualmodelcontrol as vmc
from virtualmodelcontrol.models import SerialChain
Other kinds of robots¶
A continuum arm is a PCC model, driven by a TendonTransmission:
from virtualmodelcontrol.models import PCC, TendonTransmission
third = 2 * np.pi / 3 # three tendons, evenly around the arm [rad]
tendon_angles = [[0, third, -third], # the angles of each segment's tendons
[np.pi / 3, np.pi, -np.pi / 3]] # the second one is turned
soft = vmc.Mechanism(
"soft", model=PCC([0.2, 0.2], 0.03), # lengths, section radius [m]
actuation=TendonTransmission(tendon_angles, 0.003)) # spool radius [m]
soft.actuation.motor_sizes(soft.space) # motor angles, motor rates
(6, 6)
The arm’s mass is not lumped by itself. soft.add_mass_along(name, mass, s0, s1, n) spreads a mass
evenly between two arc parameters as \(n\) point masses at the nodes of Gauss-Legendre quadrature,
which is exact for any polynomial of degree \(2n - 1\) in the arc length. Here the height of the
center of the mass of a rod bent through 1.2 rad in all converges with \(n\) to rounding error:
# a bend of 0.018 m over the section radius 0.03 m is 0.6 rad per segment
q_bent = np.array([0.018, 0, 0, 0.018, 0, 0])
kin_soft = vmc.Kinematics(soft)
def moment(n):
"""The mass times the height of its center [kg·m], with n masses."""
rod = vmc.Mechanism("rod", model=soft.model)
names = rod.add_mass_along("rod", 1.0, 0.0, 1.0, n)
parts = [rod.components[k] for k in names]
def height(c):
return kin_soft.position(q_bent, float(c.coord.s.value))[2]
return sum(float(c.mass.value) * height(c) for c in parts)
counts = np.arange(1, 9)
fine = moment(64)
mistake = [abs(moment(n) - fine) / fine for n in counts]
One mass is 6 % off, four masses 3e-09.
A robot known only by its joints, such as the turtle’s cranks, is a JointSpace model: its
controllers act on robot.joint(i). A LinearCoupling lets one motor drive several joints.
An Assembly mounts parts on a base or on another part’s frame, as the hand on the UR5’s
flange. Each part is mounted at a position and a rotation, in the base frame or, with a parent
site, in the frame of that site of another part, so that it moves with it. The sites of a part are
those of its own model, with its joints at 0 (as in SerialChain):
from virtualmodelcontrol.models import Assembly, LinearCoupling
gripper = LinearCoupling(SerialChain(
["revolute", "revolute"], axes=[[0, 0, 1]] * 2,
points=[[0, 0, 0], [0.05, 0, 0]], sites={"tip": (2, [0.09, 0, 0])},
), [[1.0], [0.5]]) # one motor; the second joint turns half as much
two_link = SerialChain(
["revolute", "revolute"], axes=[[0, 0, 1]] * 2,
points=[[0, 0, 0], [0.30, 0, 0]], sites={"tip": (2, [0.55, 0, 0])})
both = Assembly({ # name: (model, position [m], rotation [rad], parent)
"arm": (two_link, (0, 0, 0), (0, 0, 0)),
"gripper": (gripper, (0, 0, 0), (0, 0, 0), "arm/tip"), # on the tip
})
vmc.Kinematics(both).position([0.0, 0.0, 0.3], "gripper/tip") # [m]
array([0.63378471, 0.03217463, 0. ])
The motors of an assembly are those of its parts, stacked in order. A part that nothing drives,
such as a floating body, gets models.Passive(nv) in assembly.stacked_actuation (a dict of
the actuations by part) and adds no motors: Crawl with a flywheel builds a crawler
that way.
To write a new kind of model, see Extend the library.
Take the soft arm to the robot¶
The controller of a robot needs nothing from vmc.sim. Here a spring takes the tip of the soft arm
above to a goal; one control period of your control loop is a step with the motors’ reading, which
for this arm is the tendons’ angles and rates:
ctrl = vmc.Mechanism("ctrl")
tip = soft.point(s=1.0)
ctrl.add("reach", vmc.LinearSpring(tip - [0.05, 0.0, 0.3], 100.0))
ctrl.add("damp", vmc.LinearDamper(tip, 2.0))
system = vmc.VirtualMechanismSystem(soft, ctrl)
controller = vmc.VMCController(vmc.compile(system))
n_angles, n_rates = soft.actuation.motor_sizes(soft.space)
reading = vmc.Signals(0.0, motor_position=np.zeros(n_angles),
motor_velocity=np.zeros(n_rates))
controller.reset(0.0, reading) # start from the first reading
controller.step(0.0, reading)["motor_torque"] # [N·m], one per tendon
array([ 0.11 , -0.04 , -0.04 , 0.02666667, -0.02333334,
0.02666667])