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