Joints and bodies¶
In this tutorial we use the other joints of a SerialChain, the rigid-body model of
Build your own robot: joints with several coordinates, and the rail. We toss a
free brick, spin a floating one, give a body its inertia, and slide a bead along a rail.
import numpy as np
import matplotlib.pyplot as plt
import virtualmodelcontrol as vmc
from virtualmodelcontrol.models import SerialChain
Joints with more coordinates¶
A joint of a SerialChain is one of seven kinds. Its coordinates take the next entries of \(q\):
joint |
coordinates |
motion |
|---|---|---|
|
1 |
a turn about its axis |
|
1 |
a slide along its axis |
|
1 |
a turn that also slides by |
|
3 |
a rotation vector, about the joint’s point |
|
6 |
a translation, then a rotation vector: a floating base |
|
7 |
a translation, then a unit quaternion: a floating base that turns any number of times |
|
1 |
a slide along the spline through the waypoints |
A spherical, a free, a floating and a rail joint have no axis: give None. We toss a brick on a free joint. It
is four point masses at its corners, and it spins as it flies:
corners = {"a": [0.2, 0.0, 0.0], "b": [0.0, 0.1, 0.0],
"c": [0.0, 0.0, 0.3], "d": [-0.1, -0.1, -0.1]}
mass = {"a": 1.0, "b": 2.0, "c": 3.0, "d": 1.5} # [kg]
brick = vmc.Mechanism("brick", model=SerialChain(
["free"], axes=[None], points=[[0, 0, 0]],
sites={name: (1, c) for name, c in corners.items()}))
# the Param that Gravity reads
brick.add_param(vmc.Param("gravity", [0.0, 0.0, -9.81], unit="m/s^2"))
for name, m in mass.items():
brick.add(f"m_{name}", vmc.PointMass(brick.point(name), m))
brick.add("gravity", vmc.Gravity(brick))
v0 = [0.5, 0.0, 2.0, 3.0, -2.0, 1.0] # the translation rate, the spin
plant = vmc.sim.ModelPlant(brick, v0=v0, max_step=1e-4)
kin = vmc.Kinematics(brick)
times, path, tip = [], [], []
for _ in range(60):
plant.advance(0.01)
times.append(plant.t)
path.append(sum(m * kin.position(plant.q, n)
for n, m in mass.items()) / sum(mass.values()))
tip.append(kin.position(plant.q, "c"))
path, tip, times = np.array(path), np.array(tip), np.array(times)
The center of mass follows the parabola of a stone to 0.26 mm while the corner
winds around it: nothing but gravity acts, so the spin does not change it. The orientation of a
free joint is a rotation vector, which is smooth until the body has turned once. A body that
keeps spinning takes a "floating" joint: its orientation is a unit quaternion, so \(q\) has
seven entries and the velocity six, the translation’s and the body’s own angular velocity.
The same brick, thrown with a spin of 15 rad/s, turns more than twice in a second:
spinner = vmc.Mechanism("spinner", model=SerialChain(
["floating"], axes=[None], points=[[0, 0, 0]],
sites={name: (1, c) for name, c in corners.items()}))
for name, m in mass.items():
spinner.add(f"m_{name}", vmc.PointMass(spinner.point(name), m))
plant = vmc.sim.ModelPlant(spinner, v0=[0, 0, 0, 0, 15.0, 0], max_step=1e-4)
rate = []
for _ in range(50):
plant.advance(0.02)
rate.append(np.linalg.norm(plant.v[3:]))
plant.q.size, plant.v.size
(7, 6)
It turned 2.4 times, and its quaternion is still a unit.
A body does not need its point masses. A PointMass at its center gives its mass, and a
RotationalInertia on the FrameRotation of its frame gives the inertia about that point:
a 3 by 3 matrix in the frame’s axes, or its three principal moments. We replace the brick’s four
corners by their total mass and the inertia about their center, and give both the same spin:
names = list(corners)
weights = np.array([mass[n] for n in names]) # [kg]
r = np.array([corners[n] for n in names])
center = weights @ r / weights.sum() # [m]
inertia = sum(mk * (x @ x * np.eye(3) - np.outer(x, x))
for mk, x in zip(weights, r - center)) # [kg·m²]
def rigid_brick():
"""A floating brick that turns about its center of mass."""
sites = {n: (1, c) for n, c in corners.items()}
sites["center"] = (1, center)
chain = SerialChain(["floating"], axes=[None], points=[center],
sites=sites)
return vmc.Mechanism("brick", model=chain)
corner_masses, rigid = rigid_brick(), rigid_brick()
for n in names:
point = corner_masses.point(n)
corner_masses.add(f"m_{n}", vmc.PointMass(point, mass[n]))
rigid.add("mass", vmc.PointMass(rigid.point("center"), weights.sum()))
rigid.add("spin", vmc.RotationalInertia(
vmc.FrameRotation(rigid.model, "center"), inertia))
spins = {}
for name, robot in (("four corners", corner_masses), ("one body", rigid)):
plant = vmc.sim.ModelPlant(robot, v0=[0, 0, 0, 3.0, 12.0, -2.0],
max_step=1e-4)
spin = []
for _ in range(40):
plant.advance(0.025)
spin.append(plant.v[3:].copy())
spins[name] = np.array(spin)
The brick of four corners (lines) and the one body (dots) turn alike, to 9e-15 rad/s: they have the same mass and the same inertia.
A rail along a path¶
A rail carries a body along a curve. Its coordinate \(s\) is the parameter of the natural cubic
spline through the waypoints: 0 at the first waypoint and 1 at the last, with the same step
of \(s\) between neighbors, so give waypoints about equally far apart. The waypoints are
design Params (Parameters) in the frame of the joint before the rail; the body starts where its site is
at \(q = 0\), so put the site at the first waypoint to draw the path as given. A bead on a
circular wire swings as a pendulum. The wire runs well past the swing, because a spline
ends flat:
radius = 0.5 # [m]
phi = np.linspace(-1.75, 1.75, 17) # the angle along the wire [rad]
wire = np.column_stack([radius * np.sin(phi), 0 * phi,
radius * (1 - np.cos(phi))])
bead = vmc.Mechanism("bead", model=SerialChain(
[("rail", wire)], axes=[None], points=[[0, 0, 0]],
sites={"bead": (1, wire[0])})) # at the first waypoint when s = 0
bead.add_param(vmc.Param("gravity", [0.0, 0.0, -9.81], unit="m/s^2"))
bead.add("mass", vmc.PointMass(bead.point("bead"), 0.5))
bead.add("gravity", vmc.Gravity(bead))
start = 0.8 # [rad] from the bottom of the wire
plant = vmc.sim.ModelPlant(
bead, q0=[(start + phi[-1]) / (2 * phi[-1])], max_step=1e-4)
kin = vmc.Kinematics(bead)
times, angle = np.linspace(0, 2, 81), []
for t in times:
plant.advance(t - plant.t)
x, _, z = kin.position(plant.q, "bead")
angle.append(np.arctan2(x, radius - z))
Its angle should be the pendulum’s, \(\ddot\varphi = -(g/R) \sin\varphi\), which we integrate on the side:
from scipy.integrate import solve_ivp
exact = solve_ivp(lambda t, y: [y[1], -9.81 / radius * np.sin(y[0])],
(0, 2), [start, 0.0], t_eval=times, rtol=1e-10)
The bead follows the pendulum to 1.3 mrad over two seconds. A joint placed after the rail rides on it: a revolute joint gives a pendulum hung from a cart.