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 d_pot_d_q_via_finite_differences(O, eps=1.e-6):
result = []
for q in [O.J.qE, O.J.qr]:
for i in xrange(len(q)):
fs = []
for signed_eps in [eps, -eps]:
q_eps = list(q)
q_eps[i] += signed_eps
q_eps = matrix.col(q_eps)
if (q is O.J.qE):
qE = q_eps
qr = O.J.qr
else:
qE = O.J.qE
qr = q_eps
J = joint_lib.six_dof(type=O.J.type,
qE=qE,
qr=qr,
r_is_qr=O.J.r_is_qr)
e_pot = test_utils.potential_energy(sites=O.sites_F0,
wells=O.wells,
A=O.A,
J=J)
fs.append(e_pot)
result.append((fs[0] - fs[1]) / (2 * eps))
return matrix.col(result)
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 compute_functional_and_gradients(O, qE, use_analytical_gradients):
J = O.spherical_type(qE=qE)
f = test_utils.potential_energy(
sites=O.sites, wells=O.wells, A=O.A, J=J)
fin = O.d_pot_d_q_via_finite_differences(J=J)
if (use_analytical_gradients):
ana = O.d_pot_d_q(J=J)
assert approx_equal(ana, fin, eps=1.e-4)
g = ana
else:
g = fin
return f, g
def compute_functional_and_gradients(O, qE, use_analytical_gradients):
J = O.spherical_type(qE=qE)
f = test_utils.potential_energy(
sites=O.sites, wells=O.wells, A=O.A, J=J)
fin = O.d_pot_d_q_via_finite_differences(J=J)
if (use_analytical_gradients):
ana = O.d_pot_d_q(J=J)
assert approx_equal(ana, fin, eps=1.e-4)
g = ana
else:
g = fin
return f, g
def d_pot_d_q_via_finite_differences(O, J=None, eps=1.e-6):
if (J is None): J = O.J
result = []
q = J.qE
for i in xrange(len(q)):
fs = []
for signed_eps in [eps, -eps]:
q_eps = list(q)
q_eps[i] += signed_eps
q_eps = matrix.col(q_eps)
J = O.spherical_type(qE=q_eps)
e_pot = test_utils.potential_energy(
sites=O.sites, wells=O.wells, A=O.A, J=J)
fs.append(e_pot)
result.append((fs[0]-fs[1])/(2*eps))
return matrix.col(result)
def d_pot_d_q_via_finite_differences(O, J=None, eps=1.e-6):
if (J is None): J = O.J
result = []
q = J.qE
for i in range(len(q)):
fs = []
for signed_eps in [eps, -eps]:
q_eps = list(q)
q_eps[i] += signed_eps
q_eps = matrix.col(q_eps)
J = O.spherical_type(qE=q_eps)
e_pot = test_utils.potential_energy(
sites=O.sites, wells=O.wells, A=O.A, J=J)
fs.append(e_pot)
result.append((fs[0]-fs[1])/(2*eps))
return matrix.col(result)
def d_pot_d_q_via_finite_differences(O, eps=1.e-6):
result = []
for q in [O.J.qE, O.J.qr]:
for i in xrange(len(q)):
fs = []
for signed_eps in [eps, -eps]:
q_eps = list(q)
q_eps[i] += signed_eps
q_eps = matrix.col(q_eps)
if (q is O.J.qE): qE = q_eps; qr=O.J.qr
else: qE = O.J.qE; qr=q_eps
J = joint_lib.six_dof(
type=O.J.type, qE=qE, qr=qr, r_is_qr=O.J.r_is_qr)
e_pot = test_utils.potential_energy(
sites=O.sites_F0, wells=O.wells, A=O.A, J=J)
fs.append(e_pot)
result.append((fs[0]-fs[1])/(2*eps))
return matrix.col(result)
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
