def exercise_basic():
assert approx_equal(sum(spatial_lib.xrot((1, 2, 3, 4, 5, 6, 7, 8, 9))), 90)
assert approx_equal(sum(spatial_lib.xtrans((1, 2, 3))), 6)
assert approx_equal(
sum(
spatial_lib.cb_as_spatial_transform(cb=matrix.rt(((1, 2, 3, 4, 5,
6, 7, 8, 9),
(1, 2, 3))))),
90)
assert approx_equal(sum(spatial_lib.crm((1, 2, 3, 4, 5, 6))), 0)
assert approx_equal(sum(spatial_lib.crf((1, 2, 3, 4, 5, 6))), 0)
i_spatial = spatial_lib.mci(m=1.234,
c=matrix.col((1, 2, 3)),
i=matrix.sym(sym_mat3=(2, 3, 4, 0.1, 0.2,
0.3)))
assert approx_equal(sum(i_spatial), 21.306)
assert approx_equal(
spatial_lib.kinetic_energy(i_spatial=i_spatial,
v_spatial=matrix.col((1, 2, 3, 4, 5, 6))),
75.109)
#
mass_points = body_lib.mass_points(sites=matrix.col_list([
(0.949, 2.815, 5.189), (0.405, 3.954, 5.917), (0.779, 5.262, 5.227)
]),
masses=[2.34, 3.56, 1.58])
assert approx_equal(mass_points.sum_of_masses(), 7.48)
assert approx_equal(mass_points.center_of_mass(),
[0.654181818182, 3.87397058824, 5.54350802139])
assert approx_equal(mass_points._sum_of_masses, 7.48)
assert approx_equal(
mass_points.inertia(pivot=matrix.col((0.9, -1.3, 0.4))), [
404.7677928, 10.04129606, 10.09577652, 10.04129606, 199.7384559,
-199.3511949, 10.09577652, -199.3511949, 206.8314171
])
def exercise_basic():
assert approx_equal(sum(spatial_lib.xrot((1,2,3,4,5,6,7,8,9))), 90)
assert approx_equal(sum(spatial_lib.xtrans((1,2,3))), 6)
assert approx_equal(
sum(spatial_lib.cb_as_spatial_transform(
cb=matrix.rt(((1,2,3,4,5,6,7,8,9), (1,2,3))))), 90)
assert approx_equal(sum(spatial_lib.crm((1,2,3,4,5,6))), 0)
assert approx_equal(sum(spatial_lib.crf((1,2,3,4,5,6))), 0)
i_spatial = spatial_lib.mci(
m=1.234,
c=matrix.col((1,2,3)),
i=matrix.sym(sym_mat3=(2,3,4,0.1,0.2,0.3)))
assert approx_equal(sum(i_spatial), 21.306)
assert approx_equal(spatial_lib.kinetic_energy(
i_spatial=i_spatial, v_spatial=matrix.col((1,2,3,4,5,6))), 75.109)
#
mass_points = body_lib.mass_points(
sites=matrix.col_list([
(0.949, 2.815, 5.189),
(0.405, 3.954, 5.917),
(0.779, 5.262, 5.227)]),
masses=[2.34, 3.56, 1.58])
assert approx_equal(mass_points.sum_of_masses(), 7.48)
assert approx_equal(mass_points.center_of_mass(),
[0.654181818182, 3.87397058824, 5.54350802139])
assert approx_equal(mass_points._sum_of_masses, 7.48)
assert approx_equal(
mass_points.inertia(pivot=matrix.col((0.9,-1.3,0.4))),
[404.7677928, 10.04129606, 10.09577652,
10.04129606, 199.7384559, -199.3511949,
10.09577652, -199.3511949, 206.8314171])
def inverse_dynamics(O, qdd_array=None, f_ext_array=None, grav_accn=None):
"""RBDA Tab. 5.1, p. 96:
Inverse Dynamics of a kinematic tree via Recursive Newton-Euler Algorithm.
qdd_array is a vector of joint acceleration variables.
The return value (tau) is a vector of joint force 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[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 qdd_array is None or len(qdd_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()
a = [None] * nb
f = [None] * nb
for ib in xrange(nb):
body = O.bodies[ib]
s = body.joint.motion_subspace
if (s is None):
vj = body.qd
if (qdd_array is None or qdd_array[ib] is None):
aj = matrix.zeros(n=6)
else:
aj = qdd_array[ib]
else:
vj = s * body.qd
if (qdd_array is None or qdd_array[ib] is None):
aj = matrix.zeros(n=6)
else:
aj = s * qdd_array[ib]
if (body.parent == -1):
a[ib] = aj
if (grav_accn is not None):
a[ib] += xup_array[ib] * (-grav_accn)
else:
a[ib] = xup_array[ib] * a[body.parent] + aj + crm(v[ib]) * vj
f[ib] = body.i_spatial * a[ib] + crf(
v[ib]) * (body.i_spatial * v[ib])
if (f_ext_array is not None and f_ext_array[ib] is not None):
f[ib] -= f_ext_array[ib]
#
tau_array = [None] * nb
for ib in xrange(nb - 1, -1, -1):
body = O.bodies[ib]
s = body.joint.motion_subspace
if (s is None):
tau_array[ib] = f[ib]
else:
tau_array[ib] = s.transpose() * f[ib]
if (body.parent != -1):
f[body.parent] += xup_array[ib].transpose() * f[ib]
#
return tau_array
def inverse_dynamics(O, qdd_array=None, f_ext_array=None, grav_accn=None):
"""RBDA Tab. 5.1, p. 96:
Inverse Dynamics of a kinematic tree via Recursive Newton-Euler Algorithm.
qdd_array is a vector of joint acceleration variables.
The return value (tau) is a vector of joint force 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[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 qdd_array is None or len(qdd_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()
a = [None] * nb
f = [None] * nb
for ib in xrange(nb):
body = O.bodies[ib]
s = body.joint.motion_subspace
if (s is None):
vj = body.qd
if (qdd_array is None or qdd_array[ib] is None):
aj = matrix.zeros(n=6)
else:
aj = qdd_array[ib]
else:
vj = s * body.qd
if (qdd_array is None or qdd_array[ib] is None):
aj = matrix.zeros(n=6)
else:
aj = s * qdd_array[ib]
if (body.parent == -1):
a[ib] = aj
if (grav_accn is not None):
a[ib] += xup_array[ib] * (-grav_accn)
else:
a[ib] = xup_array[ib] * a[body.parent] + aj + crm(v[ib]) * vj
f[ib] = body.i_spatial * a[ib] + crf(v[ib]) * (body.i_spatial * v[ib])
if (f_ext_array is not None and f_ext_array[ib] is not None):
f[ib] -= f_ext_array[ib]
#
tau_array = [None] * nb
for ib in xrange(nb-1,-1,-1):
body = O.bodies[ib]
s = body.joint.motion_subspace
if (s is None):
tau_array[ib] = f[ib]
else:
tau_array[ib] = s.transpose() * f[ib]
if (body.parent != -1):
f[body.parent] += xup_array[ib].transpose() * f[ib]
#
return tau_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
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
