def test_simulator_error_cyclic_1():
with pks.Chain() as chain:
m1 = pks.Mass()
l = pks.Link(m1, m1)
with pytest.raises(pks.CyclicChainError):
with pks.Simulator(chain, m1) as sim:
pass
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_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_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_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_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
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
#!/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])