def jacobian(self, c2, robot, q):
Ja = se3.jacobian(robot.model, robot.data, q, self.jointParent, False, True)
Jb = se3.jacobian(robot.model, robot.data, q, c2.jointParent, False, True)
Xa = se3.SE3(self.R, self.w).action
Xb = se3.SE3(c2.R, c2.w).action
J = (Xa * Ja)[2, :] - (Xb * Jb)[2, :]
return J
def jacobian(self, c2, robot, q):
Ja = pin.jacobian(robot.model, robot.data, q, self.jointParent, False, True)
Jb = pin.jacobian(robot.model, robot.data, q, c2.jointParent, False, True)
Xa = pin.SE3(self.R, self.w).action
Xb = pin.SE3(c2.R, c2.w).action
J = (Xa * Ja)[2, :] - (Xb * Jb)[2, :]
return J
def record(self, motion, variable, idx=None):
Jtask = []
task = []
if variable is 'joint':
for i in range(0, len(motion)):
se3.forwardKinematics(self.model, self.data, motion[i])
#task['rotation'].append(se3.utils.matrixToRpy(self.data.oMi[idx].rotation))
#task['translation'].append(self.data.oMi[idx].translation)
#task.append([self.data.oMi[idx].translation,
# se3.utils.matrixToRpy(self.data.oMi[idx].rotation)])
#task.append(np.row_stack((self.data.oMi[idx].translation,
# se3.utils.matrixToRpy(self.data.oMi[idx].rotation))))
task.append(
np.row_stack(
(self.data.oMi[idx].translation,
np.matrix(
euler_from_matrix(self.data.oMi[idx].rotation,
'syxz')).T)))
#M = (self.data.oMi[idx-1].rotation).T*self.data.oMi[idx].rotation
#task.append(np.row_stack((self.data.oMi[idx].translation,
# np.matrix(euler_from_matrix(M,'sxyz')).T)))
#M = self.oMp*self.data.oMi[idx].rotation
Jtask.append(
se3.jacobian(self.model, self.data, motion[i], idx, True,
True))
elif variable is 'com':
for i in range(0, len(motion)):
se3.forwardKinematics(self.model, self.data, motion[i])
task.append(self.com(motion[i]).getA()[:, idx])
Jtask.append(self.Jcom(motion[i]))
return task, Jtask
def finiteDifferencesdJi_dq(model, data, q, h, joint_id, local):
#dJi/dq
q0 = q.copy()
se3.forwardKinematics(model, data, q0)
se3.computeJacobians(model, data, q0)
tensor_dJi_dq = np.zeros((6, model.nv, model.nv))
J0i = se3.jacobian(model, data, q0, joint_id, local, True).copy()
oMi = data.oMi[joint_id].copy()
for j in range(model.nv):
vh = np.matrix(np.zeros((model.nv, 1)))
vh[j] = h #eps
q_integrate = se3.integrate(model, q0.copy(), vh) # dJk/dqi = q0 + v*h
se3.forwardKinematics(model, data, q_integrate)
se3.computeJacobians(model, data, q_integrate)
oMint = data.oMi[joint_id].copy()
iMint = oMi.inverse() * oMint
J0_int = se3.jacobian(model, data, q_integrate, joint_id, local,
True).copy()
J0_int_i = J0_int.copy()
J0_int_i = iMint.action * J0_int_i
tensor_dJi_dq[:, :, j] = (J0_int_i - J0i) / h
return tensor_dJi_dq
def jacobian(self, q, index, update_geometry=True, local_frame=True):
return se3.jacobian(self.model, self.data, q, index, local_frame,
update_geometry)
def wJrh(self,q):
return se3.jacobian(self.model,self.data,self.rh,q,False)
def wJrh(self, q):
return se3.jacobian(self.model, self.data, self.rh, q, False)