Beispiel #1
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]
Beispiel #2
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)
Beispiel #10
0
 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