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_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_simulator_error_disconnected_components_2():
with pks.Chain() as chain:
m1 = pks.Mass()
m2 = pks.Mass()
j = pks.Joint()
pks.Link(m1, j)
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_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_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_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])
#!/usr/bin/env python3
import pykinsim as pks
with pks.Chain() as chain:
f1 = pks.Fixture()
j1 = pks.Joint()
m1 = pks.Mass()
j2 = pks.Joint()
m2 = pks.Mass()
pks.Link(f1, j1, l=0.0)
pks.Link(j1, m1, l=0.33)
pks.Link(m1, j2, l=0.33)
pks.Link(j2, m2, 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)
tau = E + A @ self.w
# Store the torque in the state <-- SET THE EXTERNAL TORQUE HERE
state.torques[j1] = tau
#
# Actual model
#
with pks.Chain() as chain:
f1 = pks.Fixture()
m1 = pks.Mass()
# Create a joint with an external torque
j1 = pks.Joint(torque=pks.External)
pks.Link(f1, j1, l=0.0) # Place the joint directly at the fixture
pks.Link(j1, m1)
with pks.Simulator(chain, root=f1) as sim:
dt = 1e-2
pks.visualization.animate(sim, dt=dt, callback=AdaptiveController(dt))
# # In case you're running your own simulation loop, the code would be
# state = sim.initial_state()
# controller = AdaptiveController(dt)
# while state.t < 20.0:
# controller(state, dt)
# state = sim.step(state, dt)
# if np.abs(state.t % 1.0 - dt) < dt:
#!/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)
#!/usr/bin/env python3
import pykinsim as pks
with pks.Chain() as chain:
f1 = pks.Fixture()
m1 = pks.Mass()
# Create a joint with a constant external torque of -9.81kg m²/s². This is
# countering the torque produced by gravity
j1 = pks.Joint(torque=-9.81)
pks.Link(f1, j1, l=0.0) # Place the joint directly at the fixture
pks.Link(j1, m1)
with pks.Simulator(chain, root=f1) as sim:
pks.visualization.animate(sim)