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)