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
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()
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()
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
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
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
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
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
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
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