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