def e_pot_and_f_ext_update(O):
O.AJA_accu = []
O.e_pot = 0
O.f_ext_bf = []
for B in O.bodies:
AJA = B.A.Tb0 * B.J.Tsp * B.A.T0b
if (B.parent == -1):
AJA_tree = None
else:
AJA_tree = O.AJA_accu[B.parent]
AJA = AJA_tree * AJA
O.AJA_accu.append(AJA)
e_pot_bf = test_utils.potential_energy_bf(sites=B.sites,
wells=B.wells,
A=B.A,
J=B.J,
AJA_tree=AJA_tree)
f_ext_using_bf = test_utils.potential_f_ext_bf(sites=B.sites,
wells=B.wells,
A=B.A,
J=B.J,
AJA_tree=AJA_tree)
O.f_ext_bf.append(f_ext_using_bf)
O.e_pot += e_pot_bf
O.e_tot = O.e_kin + O.e_pot
def e_pot_and_f_ext_update(O):
O.AJA_accu = []
O.e_pot = 0
O.f_ext_ff = []
O.f_ext_bf = []
for B in O.bodies:
AJA = B.A.Tb0 * B.J.Tsp * B.A.T0b
if (B.parent == -1):
AJA_tree = None
else:
AJA_tree = O.AJA_accu[B.parent]
AJA = AJA_tree * AJA
O.AJA_accu.append(AJA)
e_pot_ff = test_utils.potential_energy(
sites=B.sites, wells=B.wells, A=B.A, J=B.J, AJA_tree=AJA_tree)
e_pot_bf = test_utils.potential_energy_bf(
sites=B.sites, wells=B.wells, A=B.A, J=B.J, AJA_tree=AJA_tree)
assert approx_equal(e_pot_bf, e_pot_ff)
f_ext_using_ff = test_utils.potential_f_ext_ff(
sites=B.sites, wells=B.wells, A=B.A, J=B.J, AJA_tree=AJA_tree)
f_ext_using_bf = test_utils.potential_f_ext_bf(
sites=B.sites, wells=B.wells, A=B.A, J=B.J, AJA_tree=AJA_tree)
O.f_ext_ff.append(f_ext_using_ff)
O.f_ext_bf.append(f_ext_using_bf)
O.e_pot += e_pot_ff
O.e_tot = O.e_kin + O.e_pot
def energies_and_accelerations_update(O):
if (O.J.S is None):
v_spatial = O.qd
else:
v_spatial = O.J.S * O.qd
O.e_kin = kinetic_energy(I_spatial=O.I_spatial, v_spatial=v_spatial)
O.e_pot = test_utils.potential_energy(
sites=O.sites_F0, wells=O.wells, A=O.A, J=O.J)
f_ext_ff = test_utils.potential_f_ext_ff(
sites=O.sites_F0, wells=O.wells, A=O.A, J=O.J)
O.e_tot = O.e_kin + O.e_pot
#
e_pot_bf = test_utils.potential_energy_bf(
sites=O.sites_F0, wells=O.wells, A=O.A, J=O.J)
assert approx_equal(e_pot_bf, O.e_pot)
f_ext_bf = test_utils.potential_f_ext_bf(
sites=O.sites_F0, wells=O.wells, A=O.A, J=O.J)
#
model = featherstone_system_model(I=O.I_spatial, A=O.A, J=O.J)
#
q = [None] # already stored in joint as qE and qr
qd = [O.qd]
tau = None
grav_accn = [0,0,0]
qdd_using_f_ext_ff = featherstone.FDab(
model, q, qd, tau, [f_ext_ff], grav_accn, f_ext_in_ff=True)
qdd_using_f_ext_bf = featherstone.FDab(
model, q, qd, tau, [f_ext_bf], grav_accn, f_ext_in_ff=False)
assert approx_equal(qdd_using_f_ext_bf, qdd_using_f_ext_ff)
O.f_ext_bf = f_ext_bf
O.qdd = qdd_using_f_ext_bf[0]
def energies_and_accelerations_update(O):
if (O.J.S is None):
v_spatial = O.qd
else:
v_spatial = O.J.S * O.qd
O.e_kin = kinetic_energy(I_spatial=O.I_spatial, v_spatial=v_spatial)
O.e_pot = test_utils.potential_energy(
sites=O.sites_F0, wells=O.wells, A=O.A, J=O.J)
f_ext_ff = test_utils.potential_f_ext_ff(
sites=O.sites_F0, wells=O.wells, A=O.A, J=O.J)
O.e_tot = O.e_kin + O.e_pot
#
e_pot_bf = test_utils.potential_energy_bf(
sites=O.sites_F0, wells=O.wells, A=O.A, J=O.J)
assert approx_equal(e_pot_bf, O.e_pot)
f_ext_bf = test_utils.potential_f_ext_bf(
sites=O.sites_F0, wells=O.wells, A=O.A, J=O.J)
#
model = featherstone_system_model(I=O.I_spatial, A=O.A, J=O.J)
#
q = [None] # already stored in joint as qE and qr
qd = [O.qd]
tau = None
grav_accn = [0,0,0]
qdd_using_f_ext_ff = featherstone.FDab(
model, q, qd, tau, [f_ext_ff], grav_accn, f_ext_in_ff=True)
qdd_using_f_ext_bf = featherstone.FDab(
model, q, qd, tau, [f_ext_bf], grav_accn, f_ext_in_ff=False)
assert approx_equal(qdd_using_f_ext_bf, qdd_using_f_ext_ff)
O.f_ext_bf = f_ext_bf
O.qdd = qdd_using_f_ext_bf[0]
