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