def compute_am_jacobian(self, q, p, dof_indices=None):
"""Compute a matrix J(p) such that the angular momentum with respect to
the point p is
L_p(q, qd) = dot(J(q), qd).
q -- joint angle values
qd -- joint-angle velocities
p -- application point, either a fixed point or the instantaneous COM,
in world coordinates
"""
J = zeros((3, self.nb_dofs))
with self.rave:
self.set_dof_values(q, dof_indices)
for link in self.rave.GetLinks():
m = link.GetMass()
i = link.GetIndex()
c = link.GetGlobalCOM()
R = link.GetTransform()[0:3, 0:3]
I = dot(R, dot(link.GetLocalInertia(), R.T))
J_trans = self.rave.ComputeJacobianTranslation(i, c)
J_rot = self.rave.ComputeJacobianAxisAngle(i)
J += dot(crossmat(c - p), m * J_trans) + dot(I, J_rot)
if dof_indices is not None:
return J[:, dof_indices]
elif self.active_dofs and len(q) == self.nb_active_dofs:
return J[:, self.active_dofs]
return J
def compute_am_hessian(self, q, p, dof_indices=None):
"""Returns a matrix H(q) such that the rate of change of the angular
momentum with respect to point p is
Ld_p(q, qd) = dot(J(q), qdd) + dot(qd.T, dot(H(q), qd)),
where J(q) is the result of self.compute_am_jacobian(q, p).
q -- joint angle values
qd -- joint-angle velocities
p -- application point, either a fixed point or the instantaneous COM,
in world coordinates
"""
def crosstens(M):
assert M.shape[0] == 3
Z = zeros(M.shape[1])
T = array([[Z, -M[2, :], M[1, :]],
[M[2, :], Z, -M[0, :]],
[-M[1, :], M[0, :], Z]])
return T.transpose([2, 0, 1]) # T.shape == (M.shape[1], 3, 3)
def middot(M, T):
"""Dot product of a matrix with the mid-coordinate of a 3D tensor.
M -- matrix with shape (n, m)
T -- tensor with shape (a, m, b)
Outputs a tensor of shape (a, n, b).
"""
return tensordot(M, T, axes=(1, 1)).transpose([1, 0, 2])
H = zeros((self.nb_dofs, 3, self.nb_dofs))
with self.rave:
self.set_dof_values(q)
for link in self.rave.GetLinks():
m = link.GetMass()
i = link.GetIndex()
c = link.GetGlobalCOM()
R = link.GetTransform()[0:3, 0:3]
# J_trans = self.rave.ComputeJacobianTranslation(i, c)
J_rot = self.rave.ComputeJacobianAxisAngle(i)
H_trans = self.rave.ComputeHessianTranslation(i, c)
H_rot = self.rave.ComputeHessianAxisAngle(i)
I = dot(R, dot(link.GetLocalInertia(), R.T))
H += middot(crossmat(c - p), m * H_trans) \
+ middot(I, H_rot) \
- dot(crosstens(dot(I, J_rot)), J_rot)
if dof_indices:
return ((H[dof_indices, :, :])[:, :, dof_indices])
elif self.active_dofs and len(q) == self.nb_active_dofs:
return ((H[self.active_dofs, :, :])[:, :, self.active_dofs])
return H
