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