def energies_and_accelerations_update(O):
model = featherstone_system_model(bodies=O.bodies)
q = [None] * len(O.bodies)
qd = [B.qd for B in O.bodies]
#
O.e_kin = e_kin_from_model(model, q, qd)
O.e_pot_and_f_ext_update()
#
tau = None
grav_accn = [0, 0, 0]
qdd_using_f_ext_ff = featherstone.FDab(model,
q,
qd,
tau,
O.f_ext_ff,
grav_accn,
f_ext_in_ff=True)
qdd_using_f_ext_bf = featherstone.FDab(model,
q,
qd,
tau,
O.f_ext_bf,
grav_accn,
f_ext_in_ff=False)
assert approx_equal(qdd_using_f_ext_bf, qdd_using_f_ext_ff)
O.qdd = qdd_using_f_ext_ff
#
X0s = FDab_X0(model, q, qd)
e_pot_vfy = check_transformations(O.bodies, model.Ttree, X0s)
assert approx_equal(e_pot_vfy, O.e_pot)
def d_pot_d_q(O): model = featherstone_system_model(bodies=O.bodies) q = [None]*len(O.bodies) qd = [matrix.col((0,)) for B in O.bodies] qdd = [matrix.col((0,)) for B in O.bodies] grav_accn = [0,0,0] return featherstone.ID(model, q, qd, qdd, O.f_ext_bf, grav_accn)
def d_pot_d_q(O):
model = featherstone_system_model(bodies=O.bodies)
q = [None] * len(O.bodies)
qd = [matrix.col((0, )) for B in O.bodies]
qdd = [matrix.col((0, )) for B in O.bodies]
grav_accn = [0, 0, 0]
return featherstone.ID(model, q, qd, qdd, O.f_ext_bf, grav_accn)
def energies_and_accelerations_update(O):
model = featherstone_system_model(bodies=O.bodies)
q = [None] * len(O.bodies)
qd = [B.qd for B in O.bodies]
#
O.e_kin = e_kin_from_model(model, q, qd)
O.e_pot_and_f_ext_update()
#
tau = None
grav_accn = [0, 0, 0]
O.qdd = featherstone.FDab(model, q, qd, tau, O.f_ext_bf, grav_accn)
def energies_and_accelerations_update(O): model = featherstone_system_model(bodies=O.bodies) q = [None]*len(O.bodies) qd = [B.qd for B in O.bodies] # O.e_kin = e_kin_from_model(model, q, qd) O.e_pot_and_f_ext_update() # tau = None grav_accn = [0,0,0] O.qdd = featherstone.FDab(model, q, qd, tau, O.f_ext_bf, grav_accn)
def d_pot_d_q(O):
model = featherstone_system_model(bodies=O.bodies)
q = [None] * len(O.bodies)
qd = [B.J.qd_zero for B in O.bodies]
qdd = [B.J.qdd_zero for B in O.bodies]
grav_accn = [0, 0, 0]
taus = featherstone.ID(model, q, qd, qdd, O.f_ext_bf, grav_accn)
result = []
for B, tau in zip(O.bodies, taus):
tau_as_d_pot_d_q = getattr(B.J, "tau_as_d_pot_d_q", None)
if (tau_as_d_pot_d_q is None):
result.append(tau)
else:
result.append(tau_as_d_pot_d_q(tau=tau))
return result
def d_pot_d_q(O):
model = featherstone_system_model(bodies=O.bodies)
q = [None]*len(O.bodies)
qd = [B.J.qd_zero for B in O.bodies]
qdd = [B.J.qdd_zero for B in O.bodies]
grav_accn = [0,0,0]
taus = featherstone.ID(model, q, qd, qdd, O.f_ext_bf, grav_accn)
result = []
for B,tau in zip(O.bodies, taus):
tau_as_d_pot_d_q = getattr(B.J, "tau_as_d_pot_d_q", None)
if (tau_as_d_pot_d_q is None):
result.append(tau)
else:
result.append(tau_as_d_pot_d_q(tau=tau))
return result
def sensitivity_test(O, n_significant_digits):
"RBDA section 10.2, p. 199-201"
model = featherstone_system_model(bodies=O.bodies)
q = [None]*len(O.bodies)
qd = [B.qd for B in O.bodies]
qdd = [matrix.col([1]*len(B.qd)) for B in O.bodies]
grav_accn = [0,0,0]
tau = featherstone.ID(model, q, qd, qdd, O.f_ext_bf, grav_accn)
if (n_significant_digits is not None):
assert n_significant_digits > 0
fmt = "%%.%dg" % n_significant_digits
tau_trunc = []
for v in tau:
tau_trunc.append(matrix.col([float(fmt%e) for e in v]))
tau = tau_trunc
qdd = featherstone.FDab(model, q, qd, tau, O.f_ext_bf, grav_accn)
result = []
for v in qdd: result.extend(v.elems)
return result
def sensitivity_test(O, n_significant_digits):
"RBDA section 10.2, p. 199-201"
model = featherstone_system_model(bodies=O.bodies)
q = [None] * len(O.bodies)
qd = [B.qd for B in O.bodies]
qdd = [matrix.col([1] * len(B.qd)) for B in O.bodies]
grav_accn = [0, 0, 0]
tau = featherstone.ID(model, q, qd, qdd, O.f_ext_bf, grav_accn)
if (n_significant_digits is not None):
assert n_significant_digits > 0
fmt = "%%.%dg" % n_significant_digits
tau_trunc = []
for v in tau:
tau_trunc.append(matrix.col([float(fmt % e) for e in v]))
tau = tau_trunc
qdd = featherstone.FDab(model, q, qd, tau, O.f_ext_bf, grav_accn)
result = []
for v in qdd:
result.extend(v.elems)
return result
def energies_and_accelerations_update(O):
model = featherstone_system_model(bodies=O.bodies)
q = [None]*len(O.bodies)
qd = [B.qd for B in O.bodies]
#
O.e_kin = e_kin_from_model(model, q, qd)
O.e_pot_and_f_ext_update()
#
tau = None
grav_accn = [0,0,0]
qdd_using_f_ext_ff = featherstone.FDab(
model, q, qd, tau, O.f_ext_ff, grav_accn, f_ext_in_ff=True)
qdd_using_f_ext_bf = featherstone.FDab(
model, q, qd, tau, O.f_ext_bf, grav_accn, f_ext_in_ff=False)
assert approx_equal(qdd_using_f_ext_bf, qdd_using_f_ext_ff)
O.qdd = qdd_using_f_ext_ff
#
X0s = FDab_X0(model, q, qd)
e_pot_vfy = check_transformations(O.bodies, model.Ttree, X0s)
assert approx_equal(e_pot_vfy, O.e_pot)