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 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):
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 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 check():
model = featherstone_system_model(
m=sim.m,
I=sim.I_F1,
J=six_dof_joint_euler_params_featherstone(qE=sim.J1.qE, qr=sim.J1.qr))
q = [None] # already stored in J1 as qE and qr
qd = [sim.qd]
tau = None
f_ext = [matrix.col((sim.nc_F1, sim.f_F1)).resolve_partitions()]
grav_accn = [0,0,0]
qdd = featherstone.FDab(model, q, qd, tau, f_ext, grav_accn)
if (i_step % 10 == 0):
print >> out, "ang acc 3D:", sim.wd_F1.elems
print >> out, " 6D:", qdd[0].elems[:3]
print >> out
print >> out, "lin acc 3D:", sim.as_F1.elems
print >> out, " 6D:", qdd[0].elems[3:]
print >> out
assert approx_equal(qdd[0].elems[:3], sim.wd_F1)
assert approx_equal(qdd[0].elems[3:], sim.as_F1)
def exercise_ID_FDab():
write_matlab_script(label="ID_FDab",
precision=12,
code="""\
% modified version of test_FD.m
% test the correctness of FDab, and ID by checking that FDab
% is the inverse of ID.
% step 1: create a complicated kinematic tree, and adjust some of the
% pitches so that it contains helical and prismatic as well as revolute
% joints.
tree = autoTree( 12, 1.5, 1, 0.95 );
tree.pitch(3) = 0.1;
tree.pitch(5) = inf;
tree.pitch(7) = -0.1;
tree.pitch(9) = inf;
% step 2: choose random initial conditions
rand("state", [0]);
q = pi * (2*rand(12,1) - 1);
qd = 2*rand(12,1) - 1;
qdd = 2*rand(12,1) - 1;
q
qd
qdd
for i = 1:tree.NB
f_ext{i} = 2*rand(6,1) - 1;
end
f_ext
grav_accn = 2*rand(3,1) - 1;
grav_accn
% step 3: use ID to calculate the force required to produce qdd; then use
% FDab to calculate the acceleration that this force
% produces.
tau = ID( tree, q, qd, qdd );
qdd_ab = FDab( tree, q, qd, tau );
% step 4: compare results. In theory, we should have qdd_ab==qdd and
% qdd_crb==qdd. However, rounding errors will make them slightly
% different. Expect rounding errors in the vicinity of 1e-14 on this
% test.
tau
qdd_ab
tau = ID( tree, q, qd, qdd, f_ext );
qdd_ab = FDab( tree, q, qd, tau, f_ext );
tau
qdd_ab
tau = ID( tree, q, qd, qdd, f_ext, grav_accn );
qdd_ab = FDab( tree, q, qd, tau, f_ext, grav_accn );
tau
qdd_ab
""")
tree = fs.autoTree(nb=12, bf=1.5, skew=1, taper=0.95)
tree.pitch[2] = 0.1
tree.pitch[4] = fs.Inf
tree.pitch[6] = -0.1
tree.pitch[8] = fs.Inf
q = [
2.16406631697e+00, 1.62077531448e+00, -4.99063476296e-01,
-1.51477073237e+00, 7.08411636458e-02, -5.97316430786e-01,
1.78315912482e+00, -1.23582258961e+00, -1.47045673813e-01,
5.23904805187e-01, 2.56424888393e+00, 2.94483836087e-02
]
qd = [
-4.36324311201e-01, 5.11608408314e-01, 2.36737993351e-01,
-4.98987317275e-01, 8.19492511936e-01, 9.65570952075e-01,
6.20434471993e-01, 8.04331900879e-01, -3.79704861361e-01,
4.59663496520e-01, 7.97676575936e-01, 3.67967863831e-01
]
qdd = [
-5.57145690946e-02, -7.98597583863e-01, -1.31656329092e-01,
2.21773946888e-01, 8.26022106476e-01, 9.33212735542e-01,
-4.59804468946e-02, 7.30619855543e-01, -4.79015379216e-01,
6.10055654026e-01, 9.73986076712e-02, -9.71916599672e-01
]
f_ext_given = [
matrix.col(f) for f in
[[
4.39409372808e-01, -2.02352915551e-01, 6.49689954296e-01,
3.36306402464e-01, -9.97714361371e-01, -1.28442670694e-02
],
[
7.35205550986e-01, -5.12178246226e-01, -3.49591274505e-01,
7.40942464217e-01, -6.17865816995e-01, 1.35021481241e-01
],
[
-5.22768142770e-01, 9.35080500580e-01, 6.06358938560e-01,
-1.04060857129e-01, -8.39108362895e-01, -3.59890790655e-01
],
[
1.58812850411e-02, 8.65667648454e-01, -7.81884308138e-01,
1.02534492181e-01, 4.13122819734e-01, 9.48818226568e-02
],
[
6.28933726583e-01, 8.05672139406e-02, 9.27677091948e-01,
2.06371255923e-01, 1.75234128351e-01, -1.10021947449e-01
],
[
1.92573723166e-01, -2.30197708055e-01, 1.51302028330e-01,
-4.19340995194e-01, -6.21217342891e-01, -6.26540943489e-01
],
[
2.25546359737e-01, 3.13318777979e-01, -4.69380159812e-02,
-8.20351277609e-01, 5.15207843933e-01, 7.53540741646e-01
],
[
8.46762031893e-01, 6.84920446280e-01, 7.96346242716e-01,
8.46164879640e-01, 8.11998498961e-02, -2.17407899531e-01
],
[
4.10566799709e-01, -4.48731757376e-01, 6.23257417016e-01,
6.98971930373e-01, 7.90077934853e-01, 1.79602367062e-01
],
[
8.99529746464e-01, 1.59390021491e-01, -9.88737867377e-02,
3.20490757245e-01, 9.92515678707e-01, 8.33882435895e-01
],
[
5.86650168260e-01, -8.35254023607e-01, 2.25566210081e-01,
-2.71115960617e-02, 2.60294680823e-01, 6.90155151343e-01
],
[
-5.13928755876e-01, 4.62978441582e-01, -7.65731413583e-01,
-5.59078926264e-01, 5.89165943421e-01, -3.34927701561e-01
]]
]
grav_accn_given = [
6.31826193067e-01, -7.98784959568e-01, -7.07283022175e-01
]
expected_taus = [
[
-1.07802735301e+01, -2.49508384454e+00, 1.26947454308e+01,
1.91173545407e+01, -2.15095801697e+00, 4.51666158929e+00,
4.74401858265e-01, 2.18443796607e+00, -2.16077117905e+00,
2.83314247697e-01, -1.05175849728e-01, -1.50102709018e-01
],
[
-9.58228539302e+00, 1.26086473525e+00, 1.17897084957e+01,
2.39352891888e+01, -2.05738994237e+00, 3.95457944509e+00,
1.53558904623e-01, 1.01233113762e+00, -2.34037354611e+00,
3.82188034435e-01, -3.30742059809e-01, 6.15628704565e-01
],
[
-1.50057630860e+01, -1.28108933617e+01, -3.95574970014e+00,
6.23340162074e+00, 3.55385144046e+00, 1.48006620138e+00,
-7.33767304124e-01, -1.54512467666e-01, -8.05660270955e-01,
3.27795875394e-01, -2.60889565195e-01, 8.55164362101e-01
]
]
for f_ext, grav_accn, expected_tau in zip([None, f_ext_given, f_ext_given],
[None, None, grav_accn_given],
expected_taus):
tau = fs.ID(model=tree,
q=q,
qd=qd,
qdd=qdd,
f_ext=f_ext,
grav_accn=grav_accn)
assert len(tau) == len(expected_tau)
assert approx_equal([m.elems[0] for m in tau], expected_tau)
qdd_ab = fs.FDab(model=tree,
q=q,
qd=qd,
tau=tau,
f_ext=f_ext,
grav_accn=grav_accn)
assert len(qdd_ab) == len(qdd)
assert approx_equal([m.elems[0] for m in qdd_ab], qdd)