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])