def test_simulator_error_cyclic_2():
with pks.Chain() as chain:
m1 = pks.Mass()
m2 = pks.Mass()
l = pks.Link(m1, m2)
l = pks.Link(m2, m1)
with pytest.raises(pks.CyclicChainError):
with pks.Simulator(chain, m1) as sim:
pass
def test_simulator_error_disconnected_components_1():
with pks.Chain() as chain:
m1 = pks.Mass()
m2 = pks.Mass()
with pytest.raises(pks.DisconnectedChainError):
with pks.Simulator(chain, m1) as sim:
pass
with pytest.raises(pks.DisconnectedChainError):
with pks.Simulator(chain, m2) as sim:
pass
def test_tree_forward():
with pks.Chain() as chain:
m1 = pks.Mass()
m2 = pks.Mass()
j = pks.Joint("y")
l1 = pks.Link(m1, j)
l2 = pks.Link(j, m2)
tree, root = chain.tree()
assert root is m1
assert len(tree) == 2
assert m1 in tree
assert len(tree[m1]) == 1
assert tree[m1][0] is l1
def test_variable_name_deduction():
with pks.Chain() as chain:
m1 = pks.Mass()
m2 = pks.Mass()
j = pks.Joint("y")
l1 = pks.Link(m1, j)
l2 = pks.Link(j, m2)
assert chain.label == "chain"
assert m1.label == "m1"
assert m2.label == "m2"
assert j.label == "j"
assert l1.label == "l1"
assert l2.label == "l2"
def test_simulator_forward_kinematics_2():
with pks.Chain() as chain:
m1 = pks.Mass()
m2 = pks.Mass()
pks.Link(m1, m2)
with pks.Simulator(chain, m1) as sim:
trafos = sim.kinematics(sim.initial_state())
assert len(trafos) == 2
assert m1 in trafos
assert np.all(np.array(trafos[m1]) == np.eye(4))
assert m2 in trafos
assert np.all(np.array(trafos[m2]) == (np.eye(4) + np.eye(4, 4, 3)))
def test_simulator_falling_point_mass():
with pks.Chain() as chain:
m1 = pks.Mass(m=1.0)
with pks.Simulator(chain) as sim:
state = sim.run(1.0)
assert np.abs(state.loc[2] + (9.81 / 2)) < 1e-12
def test_simulator_forward_kinematics_1():
with pks.Chain() as chain:
m1 = pks.Mass()
with pks.Simulator(chain, m1) as sim:
trafos = sim.kinematics(sim.initial_state())
assert len(trafos) == 1
assert m1 in trafos
assert np.all(np.array(trafos[m1]) == np.eye(4))
def test_simulator_error_multiple_sources():
with pks.Chain() as chain:
m1 = pks.Mass()
m2 = pks.Mass()
m3 = pks.Mass()
pks.Link(m1, m3)
pks.Link(m2, m3)
with pytest.raises(pks.MultipleSourcesError):
with pks.Simulator(chain, m1) as sim:
pass
with pytest.raises(pks.MultipleSourcesError):
with pks.Simulator(chain, m2) as sim:
pass
with pytest.raises(pks.MultipleSourcesError):
with pks.Simulator(chain, m3) as sim:
pass
def test_simulator_forward_kinematics_symbolic():
l1, l2 = sp.symbols("l1 l2")
with pks.Chain() as chain:
f1 = pks.Fixture()
j1 = pks.Joint()
m1 = pks.Mass()
m2 = pks.Mass()
pks.Link(f1, j1, l=0.0)
pks.Link(j1, m1, l=l1)
pks.Link(m1, m2, l=l2)
with pks.Simulator(chain, f1) as sim:
trafos = sim.kinematics()
theta = sim.symbols.theta[0]
x, z = sim.symbols.x, sim.symbols.z
assert sp.simplify(x + l1 * sp.cos(theta) - trafos[m1][0, 3]) == 0
assert sp.simplify(z - l1 * sp.sin(theta) - trafos[m1][2, 3]) == 0
assert sp.simplify(x + (l1 + l2) * sp.cos(theta) - trafos[m2][0, 3]) == 0
assert sp.simplify(z - (l1 + l2) * sp.sin(theta) - trafos[m2][2, 3]) == 0
def test_joint_index_order_preservation_2():
with pks.Chain() as chain:
m1 = pks.Mass()
j3 = pks.Joint("z", torque=pks.External)
j2 = pks.Joint("y", torque=pks.External)
j1 = pks.Joint("x")
l1 = pks.Link(m1, j1)
l2 = pks.Link(m1, j2)
l3 = pks.Link(m1, j3)
with pks.Simulator(chain, m1) as sim:
assert sim._joint_idx_map[j1] == 2
assert sim._joint_idx_map[j2] == 1
assert sim._joint_idx_map[j3] == 0
assert sim._joint_torque_idx_map[j2] == 1
assert sim._joint_torque_idx_map[j3] == 0
def test_simulator_dynamics_symbolic():
with pks.Chain() as chain:
f1 = pks.Fixture()
j1 = pks.Joint()
m1 = pks.Mass()
pks.Link(f1, j1, l=0.0)
pks.Link(j1, m1, l=1.0)
with pks.Simulator(chain, f1) as sim:
dynamics = sim.dynamics(g=(0.0, 0.0, 9.81))
print(dynamics)
theta = sim.symbols.var_theta[0]
assert sim.symbols.var_ddtheta[0] in dynamics
ddtheta = dynamics[sim.symbols.var_ddtheta[0]]
assert sp.simplify(9.81 * sp.cos(theta) - ddtheta) == 0
#!/usr/bin/env python3
import sympy as sp
import pykinsim as pks
theta, l1, l2 = sp.symbols("theta l1 l2")
with pks.Chain() as chain:
f1 = pks.Fixture()
j1 = pks.Joint(axis="y", theta=theta)
m1 = pks.Mass()
m2 = pks.Mass()
pks.Link(f1, j1, l=0.0)
pks.Link(j1, m1, l=l1)
pks.Link(m1, m2, l=l2)
with pks.Simulator(chain, f1) as sim:
state = sim.symbolic_state()
trafos = sim.kinematics(state)
print("m1.x(t) =", trafos[m1][0, 3])
print("m1.z(t) =", trafos[m1][2, 3])
print("m2.x(t) =", trafos[m2][0, 3])
print("m2.z(t) =", trafos[m2][2, 3])
#!/usr/bin/env python3
import math
import pykinsim as pks
with pks.Chain() as chain:
m1 = pks.Mass(m=2.0)
f1 = pks.Fixture()
j1 = pks.Joint()
m2 = pks.Mass()
j2 = pks.Joint()
m3 = pks.Mass()
pks.Link(j1, f1, l=0.0)
pks.Link(j1, m1, l=0.5, ry=math.pi)
pks.Link(j1, m2, l=0.33)
pks.Link(m2, j2, l=0.33)
pks.Link(j2, m3, l=0.33)
with pks.Simulator(chain, root=f1) as sim:
pks.visualization.animate(sim)
#!/usr/bin/env python3
import pykinsim as pks
with pks.Chain() as chain:
f1 = pks.Fixture(axes="yz")
j1 = pks.Joint()
m1 = pks.Mass()
f2 = pks.Fixture(axes="xy")
pks.Link(f1, j1, l=0.0) # Place the joint directly at the fixture
pks.Link(j1, m1)
pks.Link(m1, f2)
with pks.Simulator(chain, root=f1) as sim:
pks.visualization.animate(sim)
#!/usr/bin/env python3
import pykinsim as pks
# Construct the kinematic chain
with pks.Chain() as chain:
f1 = pks.Fixture() # A fixture is an object that does not move
j1 = pks.Joint(axis="y") # A rotational joint along the y-axis
m1 = pks.Mass(m=1.0) # A mass of 1kg
pks.Link(f1, j1, l=0.0) # Place the joint directly at the fixture
pks.Link(j1, m1, l=1.0) # A link with length 1m
# Build the underlying simulator and run the simulation
with pks.Simulator(chain, root=f1) as sim:
state = sim.initial_state()
for i in range(3):
# Advance the simulation for T=0.25s
state = sim.run(0.25, state)
# Draw the pendulum to a figure using matplotlib
vis = sim.visualize(state, "matplotlib")
vis.fig.savefig("pendulum_{}.svg".format(i),
bbox_inches="tight",
transparent=True)
#!/usr/bin/env python3
import math
import pykinsim as pks
with pks.Chain() as chain:
f1 = pks.Fixture(axes="xyz")
j1 = pks.Joint(axis="x")
j2 = pks.Joint(axis="y", theta=math.pi) # Fold the arm in on itself as an initial state
m1 = pks.Mass(m=1.0)
pks.Link(f1, j1, l=0.0, ry=-math.pi/2)
pks.Link(j1, j2)
pks.Link(j2, m1)
with pks.Simulator(chain, root=f1) as sim:
pks.visualization.animate(sim)