Exemplo n.º 1
0
 def assign_random_velocities(O,
       e_kin_target=None,
       e_kin_epsilon=1e-12,
       random_gauss=None):
   if (e_kin_target is None):
     work_e_kin_target = 1
   elif (e_kin_target == 0):
     O.assign_zero_velocities()
     return
   else:
     assert e_kin_target >= 0
     work_e_kin_target = e_kin_target
   from scitbx.array_family import flex
   qd_e_kin_scales = flex.double(
     O.qd_e_kin_scales(e_kin_epsilon=e_kin_epsilon))
   if (O.degrees_of_freedom != 0):
     qd_e_kin_scales *= (work_e_kin_target / O.degrees_of_freedom)**0.5
   if (random_gauss is None):
     import random
     random_gauss = random.gauss
   i_qd = 0
   for body in O.bodies:
     qd_new = []
     for qd in body.joint.qd_zero:
       qd_new.append(qd + random_gauss(mu=0, sigma=qd_e_kin_scales[i_qd]))
       i_qd += 1
     body.qd = matrix.col(qd_new)
   assert i_qd == O.degrees_of_freedom
   O.flag_velocities_as_changed()
   if (e_kin_target is not None):
     O.reset_e_kin(e_kin_target=e_kin_target, e_kin_epsilon=e_kin_epsilon)
   return qd_e_kin_scales
Exemplo n.º 2
0
 def assign_random_velocities(O,
                              e_kin_target=None,
                              e_kin_epsilon=1e-12,
                              random_gauss=None):
     if (e_kin_target is None):
         work_e_kin_target = 1
     elif (e_kin_target == 0):
         O.assign_zero_velocities()
         return
     else:
         assert e_kin_target >= 0
         work_e_kin_target = e_kin_target
     from scitbx.array_family import flex
     qd_e_kin_scales = flex.double(
         O.qd_e_kin_scales(e_kin_epsilon=e_kin_epsilon))
     if (O.degrees_of_freedom != 0):
         qd_e_kin_scales *= (work_e_kin_target / O.degrees_of_freedom)**0.5
     if (random_gauss is None):
         import random
         random_gauss = random.gauss
     i_qd = 0
     for body in O.bodies:
         qd_new = []
         for qd in body.joint.qd_zero:
             qd_new.append(qd +
                           random_gauss(mu=0, sigma=qd_e_kin_scales[i_qd]))
             i_qd += 1
         body.qd = matrix.col(qd_new)
     assert i_qd == O.degrees_of_freedom
     O.flag_velocities_as_changed()
     if (e_kin_target is not None):
         O.reset_e_kin(e_kin_target=e_kin_target,
                       e_kin_epsilon=e_kin_epsilon)
     return qd_e_kin_scales
Exemplo n.º 3
0
 def unpack_qd(O, qd_packed):
   assert qd_packed.size() == O.degrees_of_freedom
   i = 0
   for body in O.bodies:
     n = body.joint.degrees_of_freedom
     body.qd = matrix.col(qd_packed[i:i+n])
     i += n
   assert i == O.degrees_of_freedom
   O.flag_velocities_as_changed()
Exemplo n.º 4
0
 def unpack_qd(O, qd_packed):
     assert qd_packed.size() == O.degrees_of_freedom
     i = 0
     for body in O.bodies:
         n = body.joint.degrees_of_freedom
         body.qd = matrix.col(qd_packed[i:i + n])
         i += n
     assert i == O.degrees_of_freedom
     O.flag_velocities_as_changed()
Exemplo n.º 5
0
 def spatial_velocities(O):
   "RBDA Example 4.4, p. 80"
   if (O.__spatial_velocities is None):
     O.__spatial_velocities = []
     cb_up_array = O.cb_up_array()
     for ib in xrange(len(O.bodies)):
       body = O.bodies[ib]
       s = body.joint.motion_subspace
       if (s is None): vj = body.qd
       else:           vj = s * body.qd
       if (body.parent == -1):
         O.__spatial_velocities.append(vj)
       else:
         cb_up = cb_up_array[ib]
         vp = O.__spatial_velocities[body.parent].elems
         r_va = cb_up.r * matrix.col(vp[:3])
         vp = matrix.col((
           r_va,
           cb_up.r * matrix.col(vp[3:]) + cb_up.t.cross(r_va))) \
             .resolve_partitions()
         O.__spatial_velocities.append(vp + vj)
   return O.__spatial_velocities
Exemplo n.º 6
0
 def spatial_velocities(O):
     "RBDA Example 4.4, p. 80"
     if (O.__spatial_velocities is None):
         O.__spatial_velocities = []
         cb_up_array = O.cb_up_array()
         for ib in xrange(len(O.bodies)):
             body = O.bodies[ib]
             s = body.joint.motion_subspace
             if (s is None): vj = body.qd
             else: vj = s * body.qd
             if (body.parent == -1):
                 O.__spatial_velocities.append(vj)
             else:
                 cb_up = cb_up_array[ib]
                 vp = O.__spatial_velocities[body.parent].elems
                 r_va = cb_up.r * matrix.col(vp[:3])
                 vp = matrix.col((
                   r_va,
                   cb_up.r * matrix.col(vp[3:]) + cb_up.t.cross(r_va))) \
                     .resolve_partitions()
                 O.__spatial_velocities.append(vp + vj)
     return O.__spatial_velocities
Exemplo n.º 7
0
 def mean_linear_velocity(O, number_of_sites_in_each_tree):
   if (number_of_sites_in_each_tree is None):
     number_of_sites_in_each_tree = O.number_of_sites_in_each_tree()
   sum_v = matrix.col((0,0,0))
   sum_n = 0
   for ib,n in number_of_sites_in_each_tree:
     body = O.bodies[ib]
     v = body.joint.get_linear_velocity(qd=body.qd)
     if (v is None): continue
     sum_v += v * n
     sum_n += n
   if (sum_n == 0):
     return None
   return sum_v / sum_n
Exemplo n.º 8
0
 def mean_linear_velocity(O, number_of_sites_in_each_tree):
     if (number_of_sites_in_each_tree is None):
         number_of_sites_in_each_tree = O.number_of_sites_in_each_tree()
     sum_v = matrix.col((0, 0, 0))
     sum_n = 0
     for ib, n in number_of_sites_in_each_tree:
         body = O.bodies[ib]
         v = body.joint.get_linear_velocity(qd=body.qd)
         if (v is None): continue
         sum_v += v * n
         sum_n += n
     if (sum_n == 0):
         return None
     return sum_v / sum_n
Exemplo n.º 9
0
 def qd_e_kin_scales(O, e_kin_epsilon=1.e-12):
   result = []
   rap = result.append
   for body,asi in zip(O.bodies, O.accumulated_spatial_inertia()):
     s = body.joint.motion_subspace
     j_dof = body.joint.degrees_of_freedom
     qd = [0] * j_dof
     for i in xrange(j_dof):
       qd[i] = 1
       vj = matrix.col(qd)
       qd[i] = 0
       if (s is not None): vj = s * vj
       e_kin = kinetic_energy(i_spatial=asi, v_spatial=vj)
       if (e_kin < e_kin_epsilon):
         rap(1)
       else:
         rap(1 / e_kin**0.5)
   return result
Exemplo n.º 10
0
 def qd_e_kin_scales(O, e_kin_epsilon=1.e-12):
     result = []
     rap = result.append
     for body, asi in zip(O.bodies, O.accumulated_spatial_inertia()):
         s = body.joint.motion_subspace
         j_dof = body.joint.degrees_of_freedom
         qd = [0] * j_dof
         for i in xrange(j_dof):
             qd[i] = 1
             vj = matrix.col(qd)
             qd[i] = 0
             if (s is not None): vj = s * vj
             e_kin = kinetic_energy(i_spatial=asi, v_spatial=vj)
             if (e_kin < e_kin_epsilon):
                 rap(1)
             else:
                 rap(1 / e_kin**0.5)
     return result
Exemplo n.º 11
0
  def forward_dynamics_ab(O, tau_array=None, f_ext_array=None, grav_accn=None):
    """RBDA Tab. 7.1, p. 132:
Forward Dynamics of a kinematic tree via the Articulated-Body Algorithm.
tau_array is a vector of force variables.
The return value (qdd_array) is a vector of joint acceleration variables.
f_ext_array specifies external forces acting on the bodies. If f_ext_array
is None then there are no external forces; otherwise, f_ext_array[i] is a
spatial force vector giving the force acting on body i, expressed in body i
coordinates.
grav_accn is a 6D vector expressing the linear acceleration due to gravity.
    """
    assert tau_array is None or len(tau_array) == len(O.bodies)
    assert f_ext_array is None or len(f_ext_array) == len(O.bodies)
    nb = len(O.bodies)
    xup_array = O.xup_array()
    v = O.spatial_velocities()
    c = [None] * nb
    ia = [None] * nb
    pa = [None] * nb
    for ib in xrange(nb):
      body = O.bodies[ib]
      s = body.joint.motion_subspace
      if (s is None): vj = body.qd
      else:           vj = s * body.qd
      if (body.parent == -1): c[ib] = matrix.col([0,0,0,0,0,0])
      else:                   c[ib] = crm(v[ib]) * vj
      ia[ib] = body.i_spatial
      pa[ib] = crf(v[ib]) * (body.i_spatial * v[ib])
      if (f_ext_array is not None and f_ext_array[ib] is not None):
        pa[ib] -= f_ext_array[ib]
    #
    u = [None] * nb
    d_inv = [None] * nb
    u_ = [None] * nb
    for ib in xrange(nb-1,-1,-1):
      body = O.bodies[ib]
      s = body.joint.motion_subspace
      if (s is None):
        u[ib] = ia[ib]
        d = u[ib]
        u_[ib] = -pa[ib]
      else:
        u[ib] = ia[ib] * s
        d = s.transpose() * u[ib]
        u_[ib] = -s.transpose() * pa[ib]
      if (tau_array is not None and tau_array[ib] is not None):
        u_[ib] += tau_array[ib]
      d_inv[ib] = generalized_inverse(d)
      if (body.parent != -1):
        u_d_inv = u[ib] * d_inv[ib];
        ia_ = ia[ib] - u_d_inv * u[ib].transpose()
        pa_ = pa[ib] + ia_*c[ib] + u_d_inv * u_[ib]
        ia[body.parent] += xup_array[ib].transpose() * ia_ * xup_array[ib]
        pa[body.parent] += xup_array[ib].transpose() * pa_
    #
    a = [None] * nb
    qdd_array = [None] * nb
    for ib in xrange(nb):
      body = O.bodies[ib]
      s = body.joint.motion_subspace
      if (body.parent == -1):
        a[ib] = c[ib]
        if (grav_accn is not None):
          a[ib] += xup_array[ib] * (-grav_accn)
      else:
        a[ib] = xup_array[ib] * a[body.parent] + c[ib]
      qdd_array[ib] = d_inv[ib] * (u_[ib] - u[ib].transpose()*a[ib])
      if (s is None):
        a[ib] += qdd_array[ib]
      else:
        a[ib] += s * qdd_array[ib]
    #
    return qdd_array
Exemplo n.º 12
0
    def forward_dynamics_ab(O,
                            tau_array=None,
                            f_ext_array=None,
                            grav_accn=None):
        """RBDA Tab. 7.1, p. 132:
Forward Dynamics of a kinematic tree via the Articulated-Body Algorithm.
tau_array is a vector of force variables.
The return value (qdd_array) is a vector of joint acceleration variables.
f_ext_array specifies external forces acting on the bodies. If f_ext_array
is None then there are no external forces; otherwise, f_ext_array[i] is a
spatial force vector giving the force acting on body i, expressed in body i
coordinates.
grav_accn is a 6D vector expressing the linear acceleration due to gravity.
    """
        assert tau_array is None or len(tau_array) == len(O.bodies)
        assert f_ext_array is None or len(f_ext_array) == len(O.bodies)
        nb = len(O.bodies)
        xup_array = O.xup_array()
        v = O.spatial_velocities()
        c = [None] * nb
        ia = [None] * nb
        pa = [None] * nb
        for ib in xrange(nb):
            body = O.bodies[ib]
            s = body.joint.motion_subspace
            if (s is None): vj = body.qd
            else: vj = s * body.qd
            if (body.parent == -1): c[ib] = matrix.col([0, 0, 0, 0, 0, 0])
            else: c[ib] = crm(v[ib]) * vj
            ia[ib] = body.i_spatial
            pa[ib] = crf(v[ib]) * (body.i_spatial * v[ib])
            if (f_ext_array is not None and f_ext_array[ib] is not None):
                pa[ib] -= f_ext_array[ib]
        #
        u = [None] * nb
        d_inv = [None] * nb
        u_ = [None] * nb
        for ib in xrange(nb - 1, -1, -1):
            body = O.bodies[ib]
            s = body.joint.motion_subspace
            if (s is None):
                u[ib] = ia[ib]
                d = u[ib]
                u_[ib] = -pa[ib]
            else:
                u[ib] = ia[ib] * s
                d = s.transpose() * u[ib]
                u_[ib] = -s.transpose() * pa[ib]
            if (tau_array is not None and tau_array[ib] is not None):
                u_[ib] += tau_array[ib]
            d_inv[ib] = generalized_inverse(d)
            if (body.parent != -1):
                u_d_inv = u[ib] * d_inv[ib]
                ia_ = ia[ib] - u_d_inv * u[ib].transpose()
                pa_ = pa[ib] + ia_ * c[ib] + u_d_inv * u_[ib]
                ia[body.
                   parent] += xup_array[ib].transpose() * ia_ * xup_array[ib]
                pa[body.parent] += xup_array[ib].transpose() * pa_
        #
        a = [None] * nb
        qdd_array = [None] * nb
        for ib in xrange(nb):
            body = O.bodies[ib]
            s = body.joint.motion_subspace
            if (body.parent == -1):
                a[ib] = c[ib]
                if (grav_accn is not None):
                    a[ib] += xup_array[ib] * (-grav_accn)
            else:
                a[ib] = xup_array[ib] * a[body.parent] + c[ib]
            qdd_array[ib] = d_inv[ib] * (u_[ib] - u[ib].transpose() * a[ib])
            if (s is None):
                a[ib] += qdd_array[ib]
            else:
                a[ib] += s * qdd_array[ib]
        #
        return qdd_array