def jacobian(dMocap, dOdom, bMm):
if xReg > 0:
J = (xReg * pinocchio.Jlog6(bMm))[2:5, :].tolist()
else:
J = []
mMb = bMm.inverse()
for dM, dO in zip(dMocap, dOdom):
P = bMm * dM * mMb * dO
JP = (dM * mMb *
dO).toActionMatrixInverse() - (mMb * dO).toActionMatrixInverse()
JlogP = np.dot(pinocchio.Jlog6(P), JP)
J.extend(JlogP)
return np.array(J)
def calcDiff(self, data, x, u, recalc=True):
if recalc:
self.calc(data, x, u)
pinocchio.updateFramePlacements(self.state.pinocchio, data.pinocchio)
data.rJf = pinocchio.Jlog6(data.rMf)
data.fJf = pinocchio.getFrameJacobian(self.state.pinocchio,
data.pinocchio, self.Mref.frame,
pinocchio.ReferenceFrame.LOCAL)
data.J = data.rJf * data.fJf
self.activation.calcDiff(data.activation, data.r, recalc)
data.Rx = np.hstack([
data.J,
pinocchio.utils.zero((self.activation.nr, self.state.nv))
])
data.Lx = np.vstack([
data.J.T * data.activation.Ar,
pinocchio.utils.zero((self.state.nv, 1))
])
data.Lxx = np.vstack([
np.hstack([
data.J.T * data.activation.Arr * data.J,
pinocchio.utils.zero((self.state.nv, self.state.nv))
]),
pinocchio.utils.zero((self.state.nv, self.state.ndx))
])
def dresidualWorld(q):
pinocchio.forwardKinematics(rmodel, rdata, q)
pinocchio.computeJointJacobians(rmodel, rdata, q)
rMi = Mref.inverse() * rdata.oMi[jid]
return np.dot(
pinocchio.Jlog6(rMi),
pinocchio.getJointJacobian(rmodel, rdata, jid,
pinocchio.ReferenceFrame.WORLD))
def calcDiff(self, q, recalc=False):
if recalc:
self.calc(q)
J = np.dot(
pin.Jlog6(self.rMf),
pin.getFrameJacobian(self.rmodel, self.rdata, self.ee_frame_id,
pin.ReferenceFrame.LOCAL))
self.J = self.weight_matrix.dot(J)
return self.J
def err_jac(maMmo):
err = []
jac = []
for maMb, moMb in measurements:
M = maMb.inverse() * maMmo * moMb
err.extend(pinocchio.log6(M).vector.tolist())
jac.extend(
np.dot(pinocchio.Jlog6(M),
moMb.toActionMatrixInverse()).tolist())
return np.array(err), np.array(jac)
def dConstraint_dx(self, x):
q = a2m(x)
pinocchio.forwardKinematics(self.rmodel, self.rdata, q)
pinocchio.computeJointJacobians(self.rmodel, self.rdata, q)
pinocchio.updateFramePlacements(self.rmodel, self.rdata)
refMeff = self.refEff.inverse() * self.rdata.oMf[self.idEff]
log_M = pinocchio.Jlog6(refMeff)
M_q = pinocchio.getFrameJacobian(self.rmodel, self.rdata, self.idEff,
LOCAL)
self.Jeq = log_M * M_q
return self.Jeq
def calcDiff(self, data, x):
v_partial_dq, a_partial_dq, a_partial_dv, a_partial_da = pinocchio.getJointAccelerationDerivatives(
self.state.pinocchio, data.pinocchio, self.joint, pinocchio.ReferenceFrame.LOCAL)
da0_dq = (self.fXj * a_partial_dq)
da0_dv = (self.fXj * a_partial_dv)
if np.asscalar(self.gains[0]) != 0.:
da0_dq += np.asscalar(self.gains[0]) * pinocchio.Jlog6(self.rMf) * data.Jc
if np.asscalar(self.gains[1]) != 0.:
da0_dq += np.asscalar(self.gains[1]) * (self.fXj * v_partial_dq)
da0_dv += np.asscalar(self.gains[1]) * (self.fXj * a_partial_da)
data.da0_dx = np.hstack([da0_dq, da0_dv])
def dConstraint_dx_rightfoot(self, x, nc=0):
q = self.vq2q(a2m(x))
pinocchio.forwardKinematics(self.rmodel, self.rdata, q)
pinocchio.updateFramePlacements(self.rmodel, self.rdata)
pinocchio.computeJointJacobians(self.rmodel, self.rdata, q)
pinocchio.updateFramePlacements(self.rmodel, self.rdata)
refMr = self.refR.inverse() * self.rdata.oMf[self.idR]
log_M = pinocchio.Jlog6(refMr)
M_q = pinocchio.getFrameJacobian(self.rmodel, self.rdata, self.idR,
LOCAL)
Q_v = self.dq_dv(a2m(x))
self.Jeq[nc:nc + 6, :] = log_M * M_q * Q_v
return self.Jeq[nc:nc + 6, :]
def calcDiff(self, data, x, recalc=True):
if recalc:
self.calc(data, x)
v_partial_dq, a_partial_dq, a_partial_dv, a_partial_da = pinocchio.getJointAccelerationDerivatives(
self.state.pinocchio, data.pinocchio, self.joint,
pinocchio.ReferenceFrame.LOCAL)
Aq = (self.fXj * a_partial_dq)
Av = (self.fXj * a_partial_dv)
if np.asscalar(self.gains[0]) != 0.:
Aq += np.asscalar(self.gains[0]) * pinocchio.Jlog6(
self.rMf) * data.Jc
if np.asscalar(self.gains[1]) != 0.:
Aq += np.asscalar(self.gains[1]) * (self.fXj * v_partial_dq)
Av += np.asscalar(self.gains[1]) * (self.fXj * a_partial_da)
data.Ax = np.hstack([Aq, Av])
def calcDiff(self, data, x, u, recalc=True):
if recalc:
self.calc(data, x, u)
nq = self.nq
pinocchio.updateFramePlacements(self.pinocchio, data.pinocchio)
J = np.dot(
pinocchio.Jlog6(data.rMf),
pinocchio.getFrameJacobian(self.pinocchio, data.pinocchio,
self.frame,
pinocchio.ReferenceFrame.LOCAL))
Ax, Axx = self.activation.calcDiff(data.activation,
data.residuals,
recalc=recalc)
data.Rq[:, :nq] = J
data.Lq[:] = np.dot(J.T, Ax)
data.Lqq[:, :] = np.dot(data.Rq.T, Axx *
data.Rq) # J is a matrix, use Rq instead.
return data.cost
def calcDiff(self, data, x, recalc=True):
if recalc:
self.calc(data, x)
dv_dq, da_dq, da_dv, da_da = pinocchio.getJointAccelerationDerivatives(
self.pinocchio, data.pinocchio, data.joint,
pinocchio.ReferenceFrame.LOCAL)
dv_dq, dv_dvq = pinocchio.getJointVelocityDerivatives(
self.pinocchio, data.pinocchio, data.joint,
pinocchio.ReferenceFrame.LOCAL)
data.Aq[:, :] = data.fXj * da_dq
data.Av[:, :] = data.fXj * da_dv
if self.gains[0] != 0.:
data.Aq[:, :] += self.gains[0] * np.dot(
pinocchio.Jlog6(data.rMf),
pinocchio.getFrameJacobian(self.pinocchio, data.pinocchio,
self.frame,
pinocchio.ReferenceFrame.LOCAL))
if self.gains[1] != 0.:
data.Aq[:, :] += self.gains[1] * data.fXj * dv_dq
data.Av[:, :] += self.gains[1] * data.fXj * dv_dvq
def test_Jlog6(self):
m = pin.SE3.Identity()
J = pin.Jlog6(m)
self.assertApprox(J, eye(6))
def calcDiff(self, q):
J = pin.computeFrameJacobian(self.rmodel, self.rdata, q,
self.frameIndex)
r = self.residual(q)
Jlog = pin.Jlog6(self.deltaM)
return 2 * J.T @ Jlog.T @ r
def f_fx(self, X, f, fx):
wMt = self.variables.tag(X, self.itag)
tdMt = self.tdMw * wMt
f[:] = pinocchio.log6(tdMt).vector
fx[:] = np.zeros_like(fx)
self.variables.Jtag(fx, self.itag)[:] = pinocchio.Jlog6(tdMt)
JOINT_ID = 6
oMdes = pinocchio.SE3(pinocchio.utils.eye(3), np.matrix([1., 0., 1.]).T)
q = pinocchio.neutral(model)
eps = 1e-4
IT_MAX = 1000
DT = 1e-1
i = 0
while True:
pinocchio.forwardKinematics(model, data, q)
dMi = oMdes.actInv(data.oMi[JOINT_ID])
err = pinocchio.log(dMi).vector
if np.linalg.norm(err) < eps:
print("Convergence achieved!")
break
if i >= IT_MAX:
print(
"\nWarning: the iterative algorithm has not reached convergence to the desired precision"
)
break
J = pinocchio.Jlog6(dMi) * pinocchio.computeJointJacobian(
model, data, q, JOINT_ID)
v = -np.linalg.pinv(J) * err
q = pinocchio.integrate(model, q, v * DT)
if not i % 10: print('error = %s' % err.T)
i += 1
print('\nresult: %s' % q.flatten().tolist())
print('\nfinal error: %s' % err.T)
def calcDiff6d(self, q):
J = pin.computeFrameJacobian(robot.model, robot.data, q,
self.frameIndex)
r = self.residual6d(q)
Jlog = pin.Jlog6(self.deltaM)
return 2 * J.T @ Jlog.T @ r
