def compute(self):
frame_id = self.robot.model.getFrameId('lh_foot')
print pin.frameJacobian(self.robot.model, self.robot.data, frame_id, self.robot.q0)
print '---'
print pin.computeJacobians(self.robot.model, self.robot.data, self.robot.q0)
print '---'
print pin.se3.jacobian(self.robot.model, self.robot.data, self.robot.q0, frame_id, False, True) #False == world
def finiteDifferencesdA_dq(model, data, q, v, h):
'''
dAi/dq
'''
q0 = q.copy()
v0 = v.copy()
tensor_dAi_dq = np.zeros((6, model.nv, model.nv))
se3.forwardKinematics(model, data, q0, v0)
se3.computeJacobians(model, data, q0)
pcom_ref = se3.centerOfMass(model, data, q0).copy()
se3.ccrba(model, data, q0, v0)
A0i = data.Ag.copy()
oMc_ref = se3.SE3.Identity() #data.oMi[1].copy()
oMc_ref.translation = pcom_ref #oMc_ref.translation - pcom_ref
for j in range(model.nv):
#vary q
vh = np.matrix(np.zeros((model.nv, 1)))
vh[j] = h
q_integrated = se3.integrate(model, q0.copy(), vh)
se3.forwardKinematics(model, data, q_integrated)
se3.computeJacobians(model, data, q_integrated)
pcom_int = se3.centerOfMass(model, data, q_integrated).copy()
se3.ccrba(model, data, q_integrated, v0)
A0_int = data.Ag.copy()
oMc_int = se3.SE3.Identity() #data.oMi[1].copy()
oMc_int.translation = pcom_int #oMc_int.translation - pcom_int
cMc_int = oMc_ref.inverse() * oMc_int
A0_int = cMc_int.dualAction * A0_int
tensor_dAi_dq[:, :, j] = (A0_int - A0i) / h
return tensor_dAi_dq
def update(self, q):
se3.computeAllTerms(self.model, self.data, self.q, self.v)
#se3.forwardKinematics(self.model, self.data, q, self.v, self.a)
#- se3::forwardKinematics
#- se3::crba
#- se3::nonLinearEffects
#- se3::computeJacobians
#- se3::centerOfMass
#- se3::jacobianCenterOfMass
#- se3::kineticEnergy
#- se3::potentialEnergy
se3.framesKinematics(self.model, self.data, q)
se3.computeJacobians(self.model, self.data, q)
se3.rnea(self.model, self.data, q, self.v, self.a)
self.biais(self.q, self.v)
self.q = q.copy()
def hessian(robot, q, crossterms=False):
'''
Compute the Hessian tensor of all the robot joints.
If crossterms, then also compute the Si x Si terms,
that are not part of the true Hessian but enters in some computations like VRNEA.
'''
lambdas.setRobotArgs(robot)
H = np.zeros([6, robot.model.nv, robot.model.nv])
se3.computeJacobians(robot.model, robot.data, q)
J = robot.data.J
skiplast = -1 if not crossterms else None
for joint_i in range(1, robot.model.njoints):
for joint_j in ancestors(joint_i)[:skiplast]: # j is a child of i
for i in iv(joint_i):
for j in iv(joint_j):
Si = J[:, i]
Sj = J[:, j]
H[:, i, j] = np.asarray(Mcross(Sj, Si))[:, 0]
return H
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 finiteDifferencesddA_dq(model, data, q, v, h):
'''
d(A_dot)/dq
'''
q0 = q.copy()
v0 = v.copy()
tensor_ddA_dq = np.zeros((6, model.nv, model.nv))
se3.forwardKinematics(model, data, q0, v0)
se3.computeJacobians(model, data, q0)
#se3.centerOfMass(model, data, q0)
pcom_ref = se3.centerOfMass(model, data, q0).copy()
vcom_ref = data.vcom[0].copy()
#se3.ccrba(model, data, q0, v0.copy())
se3.dccrba(model, data, q0, v0.copy())
dA0 = np.nan_to_num(data.dAg).copy()
oMc_ref = se3.SE3.Identity()
#oMc_ref.translation = pcom_ref
oMc_ref.translation = vcom_ref
for j in range(model.nv):
#vary q
vh = np.matrix(np.zeros((model.nv, 1)))
vh[j] = h
q_integrated = se3.integrate(model, q0.copy(), vh)
se3.forwardKinematics(model, data, q_integrated) #, v0.copy())
#se3.computeJacobians(model, data, q_integrated)
#se3.centerOfMass(model, data, q_integrated)
pcom_int = se3.centerOfMass(model, data, q_integrated).copy()
vcom_int = data.vcom[0].copy()
#se3.ccrba(model, data, q_integrated, v0.copy())
se3.dccrba(model, data, q_integrated, v0.copy())
dA0_int = np.nan_to_num(data.dAg).copy()
oMc_int = se3.SE3.Identity()
#oMc_int.translation = pcom_int
oMc_int.translation = vcom_int
cMc_int = oMc_ref.inverse() * oMc_int
dA0_int = cMc_int.dualAction * dA0_int
tensor_ddA_dq[:, :, j] = (dA0_int - dA0) / h
return tensor_ddA_dq
def computeJacobians(self, q):
return se3.computeJacobians(self.model, self.data, q)
def updateRobotConfig(self, q):
se3.computeAllTerms(self.robot.model, self.robot.data, q, self.robot.v)
se3.framesKinematics(self.robot.model, self.robot.data, q)
se3.computeJacobians(self.robot.model, self.robot.data, q)