def test_frame_algo(self):
model = self.model
data = model.createData()
q = pin.neutral(model)
v = np.random.rand((model.nv))
frame_id = self.frame_idx
J1 = pin.computeFrameJacobian(model, data, q, frame_id)
J2 = pin.computeFrameJacobian(model, data, q, frame_id, pin.LOCAL)
self.assertApprox(J1, J2)
data2 = model.createData()
pin.computeJointJacobians(model, data2, q)
J3 = pin.getFrameJacobian(model, data2, frame_id, pin.LOCAL)
self.assertApprox(J1, J3)
dJ1 = pin.frameJacobianTimeVariation(model, data, q, v, frame_id,
pin.LOCAL)
data3 = model.createData()
pin.computeJointJacobiansTimeVariation(model, data3, q, v)
dJ2 = pin.getFrameJacobianTimeVariation(model, data3, frame_id,
pin.LOCAL)
self.assertApprox(dJ1, dJ2)
def update_kinematics(self, q, dq):
# Update the pinocchio model.
self.robot.forwardKinematics(q, dq)
self.robot.computeJointJacobians(q)
self.robot.framesForwardKinematics(q)
pin.computeJointJacobiansTimeVariation(self.robot.model,
self.robot.data, q, dq)
pin.computeCentroidalMapTimeVariation(self.robot.model,
self.robot.data, q, dq)
# update the op space Jacobian
# the momentum Jacobian
self.J[:6, :] = self.robot.data.Ag
# the feet Jacobians
for i, idx in enumerate(self.endeff_ids):
self.J[6 + 3 * i:6 + 3 * (i + 1), :] = self.robot.getFrameJacobian(
idx, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)[:3]
# this is joint regularization part
self.J[(self.ne + 2) * 3:, 6:] = np.identity(self.nv - 6)
# update the dJdt dq component aka the drift
# the momentum drift
self.drift_terms[:6, ] = self.robot.data.dAg @ dq
# the feet drift
for i, idx in enumerate(self.endeff_ids):
self.drift_terms[
6 + 3 * i:6 + 3 *
(i + 1), ] = pin.getFrameJacobianTimeVariation(
self.robot.model, self.robot.data, idx,
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)[:3] @ dq
def get_link_jacobian_dot(self, link_id):
frame_id = self._model.getFrameId(link_id)
pin.computeJointJacobiansTimeVariation(self._model, self._data,
self._q, self._q_dot)
jac_dot = pin.getFrameJacobianTimeVariation(
self._model, self._data, frame_id,
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)
# Pinocchio has linear on top of angular
ret = np.zeros_like(jac_dot)
ret[0:3] = jac_dot[3:6]
ret[3:6] = jac_dot[0:3]
return np.copy(ret)
def TimeDerivativeJacobian(self, q, v):
"""
Description:
1. Computes the Time Derivative Jacobian Tensor of the robot.
Args:
q -> np.array (3,) : Joint Positions of the robot.
v -> np.array (3,) : Joint Velocities of the robot.
Returns:
np.array (3,3): Time Derivative Jacobian Tensor of the robot.
"""
frame = pin.ReferenceFrame.LOCAL_WORLD_ALIGNED
pin.computeJointJacobians(self.model, self.model_data, q)
pin.framesForwardKinematics(self.model, self.model_data, q)
pin.computeJointJacobiansTimeVariation(self.model, self.model_data, q,
v)
return pin.getJointJacobianTimeVariation(self.model, self.model_data,
self.EOAT_ID, frame)[:3]
def imp_ctrl(self, q, force_dir, joint_id, vel, pos, v, K_xy, K_z):
q = self.trafo_q(q)
v = self.trafo_q(v)
m = 1.0
d_xy = np.sqrt(4 * m * 500)
d_z = np.sqrt(4 * m * 500)
M = np.eye(3) * m
M_inv = np.linalg.pinv(M)
D = np.eye(3)
D[0, 0] = D[1, 1] = d_xy
D[2, 2] = d_z
K = np.eye(3)
K[0, 0] = K_xy
K[1, 1] = K_xy
K[2, 2] = K_z
F = 0.01 #0.05
M_joint = pin.crba(self.model, self.data, q)
pos_gain = np.matmul(K, pos)
damp_gain = np.matmul(D, vel)
# damp_gain = 0.0
force_gain = F * force_dir
acc = np.matmul(M_inv, (force_gain - pos_gain - damp_gain))
# TODO: I think it is sufficient if this is calculated only once:
full_J_var = pin.computeJointJacobiansTimeVariation(
self.model, self.data, q, v)
J_var_frame = pin.getJointJacobianTimeVariation(
self.model, self.data, joint_id,
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)[:3, :]
dyn_comp = np.matmul(J_var_frame, v)
acc = acc - dyn_comp
# TODO: I think it is sufficient if this is calculated only once:
full_JAC = pin.computeJointJacobians(self.model, self.data, q)
J_NEW = pin.getJointJacobian(
self.model, self.data, joint_id,
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)[:3, :]
J = J_NEW
J_inv = np.matmul(np.linalg.pinv(np.matmul(J.T, J)), J.T)
# print (J_inv)
torque = np.matmul(M_joint, np.matmul(J_inv, acc))
return self.inv_trafo_q(torque)