def _update_Jc(self): """ Extract the Jacobian and dJacobian matrix of contact points. """ self.Jc[:] = 0. self.dJc[:] = 0. i = 0 for c in self.constraints: if c.is_enabled() and c.is_active() and self.is_enabled[c]: frame0, frame1 = c._frames[0], c._frames[1] if isinstance(c, PointContact): self.dJc[3*i:(3*(i+1)), :] = frame1.djacobian[3:6] - \ frame0.djacobian[3:6] if frame1.body in self.bodies: self.Jc[3 * i:(3 * (i + 1)), :] += frame1.jacobian[3:6] if frame0.body in self.bodies: self.Jc[3 * i:(3 * (i + 1)), :] -= frame0.jacobian[3:6] elif isinstance(c, BallAndSocketConstraint): H1_0 = dot(inv(frame1.pose), frame0.pose) Ad1_0 = adjoint(H1_0) Ad0_1 = iadjoint(H1_0) T0_g_0 = frame0.twist T1_g_1 = frame1.twist T1_g_0 = dot(Ad0_1, T1_g_1) T0_1_0 = T0_g_0 - T1_g_0 J0 = dot(Ad1_0, frame0.jacobian) J1 = frame1.jacobian dJ0 = dot(Ad1_0, frame0.djacobian) + \ dot(dAdjoint(Ad1_0, T0_1_0), frame0.jacobian) dJ1 = frame1.djacobian self.Jc[3 * i:(3 * (i + 1)), :] = (J1[3:6] - J0[3:6]) self.dJc[3 * i:(3 * (i + 1)), :] = (dJ1[3:6] - dJ0[3:6]) i += 1
def _update_Jc(self): """ Extract the Jacobian and dJacobian matrix of contact points. """ self.Jc[:] = 0. self.dJc[:] = 0. i = 0 for c in self.constraints: if c.is_enabled() and c.is_active() and self.is_enabled[c]: frame0, frame1 = c._frames[0], c._frames[1] if isinstance(c, PointContact): self.dJc[3*i:(3*(i+1)), :] = frame1.djacobian[3:6] - \ frame0.djacobian[3:6] if frame1.body in self.bodies: self.Jc [3*i:(3*(i+1)), :] += frame1.jacobian[3:6] if frame0.body in self.bodies: self.Jc [3*i:(3*(i+1)), :] -= frame0.jacobian[3:6] elif isinstance(c, BallAndSocketConstraint): H1_0 = dot(inv(frame1.pose), frame0.pose) Ad1_0 = adjoint(H1_0) Ad0_1 = iadjoint(H1_0) T0_g_0 = frame0.twist T1_g_1 = frame1.twist T1_g_0 = dot(Ad0_1, T1_g_1) T0_1_0 = T0_g_0 - T1_g_0 J0 = dot(Ad1_0, frame0.jacobian) J1 = frame1.jacobian dJ0 = dot(Ad1_0, frame0.djacobian) + \ dot(dAdjoint(Ad1_0, T0_1_0), frame0.jacobian) dJ1 = frame1.djacobian self.Jc[3*i:(3*(i+1)), :] = (J1[3:6] - J0[3:6]) self.dJc[3*i:(3*(i+1)), :] = (dJ1[3:6] - dJ0[3:6]) i += 1
def update(self, dt): self._CoMPosition[:] = 0. self._CoMJacobian[:] = 0. self._CoMdJacobian[:] = 0. for i in arange(len(self.bodies)): H_0_com_i = dot(self.bodies[i].pose, self.H_body_com[i]) self._CoMPosition += self.mass[i] * H_0_com_i[0:3, 3] if self.compute_Jacobians is True: ##### For jacobian H_com_i_com2 = zeros((4, 4)) #com2 is aligned with ground H_com_i_com2[0:3, 0:3] = H_0_com_i[0:3, 0:3].T H_com_i_com2[3, 3] = 1. Ad_com2_body = iadjoint(dot(self.H_body_com[i], H_com_i_com2)) self._CoMJacobian[:] += self.mass[i] * dot( Ad_com2_body, self.bodies[i].jacobian)[3:6, :] ##### For dJacobian T_com2_body = self.bodies[i].twist.copy() T_com2_body[3:6] = 0. dAd_com2_body = dAdjoint(Ad_com2_body, T_com2_body) dJ_com2 = dot(Ad_com2_body, self.bodies[i].djacobian) + dot( dAd_com2_body, self.bodies[i].jacobian) self._CoMdJacobian[:] += self.mass[i] * dJ_com2[3:6, :] self._CoMPosition /= self.total_mass self._CoMJacobian /= self.total_mass self._CoMdJacobian /= self.total_mass
def body_com_properties(body, compute_J=True): """ Compute the Center of Mass properties of a body. """ H_body_com = principalframe(body.mass) H_0_com = dot(body.pose, H_body_com) P_0_com = H_0_com[0:3, 3] if compute_J: H_com_com2 = inv(H_0_com) H_com_com2[0:3, 3] = 0. Ad_com2_body = iadjoint(dot(H_body_com, H_com_com2)) J_com2 = dot(Ad_com2_body, body.jacobian) T_com2_body = body.twist.copy() T_com2_body[3:6] = 0. dAd_com2_body = dAdjoint(Ad_com2_body, T_com2_body) dJ_com2 = dot(Ad_com2_body, body.djacobian) + \ dot(dAd_com2_body, body.jacobian) return P_0_com, J_com2, dJ_com2 else: return P_0_com
def _update_matrices(self, rstate, dt): """ """ H_0_f = self._frame.pose HR_0_f = H_0_f.copy() HR_0_f[0:3, 3] = 0 AdR_0_f = adjoint(HR_0_f) T_f_0_f = self._frame.twist #WARNING: twist relative to its frame T_f_0_0 = dot(AdR_0_f, T_f_0_f) T_f_0_f_only_rot = T_f_0_f.copy() T_f_0_f_only_rot[3:6] = 0. dAdR_0_f = dAdjoint(AdR_0_f, T_f_0_f_only_rot) J = dot(AdR_0_f, self._frame.jacobian) dJ = dot(AdR_0_f, self._frame.djacobian) + \ dot(dAdR_0_f, self._frame.jacobian) gpos = self._frame.pose gvel = T_f_0_0 cmd = self._ctrl.update(gpos, gvel, rstate, dt) self._J[:] = J[self._cdof, :] self._dJ[:] = dJ[self._cdof, :] self._dVdes[:] = cmd[self._cdof]