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) vv_skew = pinocchio.utils.skew(self.vv) vw_skew = pinocchio.utils.skew(self.vw) fXjdv_dq = self.fXj * v_partial_dq da0_dq = ( self.fXj * a_partial_dq )[:3, :] + vw_skew * fXjdv_dq[:3, :] - vv_skew * fXjdv_dq[3:, :] da0_dv = (self.fXj * a_partial_dv)[:3, :] + vw_skew * data.Jc - vv_skew * self.Jw if np.asscalar(self.gains[0]) != 0.: R = data.pinocchio.oMf[self.xref.frame].rotation da0_dq += np.asscalar( self.gains[0]) * R * pinocchio.getFrameJacobian( self.state.pinocchio, data.pinocchio, self.xref.frame, pinocchio.ReferenceFrame.LOCAL)[:3, :] if np.asscalar(self.gains[1]) != 0.: da0_dq += np.asscalar( self.gains[1]) * (self.fXj[:3, :] * v_partial_dq) da0_dv += np.asscalar( self.gains[1]) * (self.fXj[:3, :] * a_partial_da) data.da0_dx[:, :] = np.hstack([da0_dq, da0_dv])
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) vw = data.v.angular vv = data.v.linear data.Aq[:, :] = (data.fXj * da_dq)[:3, :] + skew(vw) * ( data.fXj * dv_dq)[:3, :] - skew(vv) * (data.fXj * dv_dq)[3:, :] data.Av[:, :] = (data.fXj * da_dv)[:3, :] + skew(vw) * data.J - skew(vv) * data.Jw R = data.pinocchio.oMf[self.frame].rotation if self.gains[0] != 0.: data.Aq[:, :] += self.gains[0] * R * pinocchio.getFrameJacobian( self.pinocchio, data.pinocchio, self.frame, pinocchio.ReferenceFrame.LOCAL)[:3, :] if self.gains[1] != 0.: data.Aq[:, :] += self.gains[1] * (data.fXj[:3, :] * dv_dq) data.Av[:, :] += self.gains[1] * (data.fXj[:3, :] * dv_dvq)
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 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, 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 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) vv_skew = pinocchio.utils.skew(self.vv) vw_skew = pinocchio.utils.skew(self.vw) fXjdv_dq = self.fXj * v_partial_dq Aq = (self.fXj * a_partial_dq )[:3, :] + vw_skew * fXjdv_dq[:3, :] - vv_skew * fXjdv_dq[3:, :] Av = (self.fXj * a_partial_dv)[:3, :] + vw_skew * data.Jc - vv_skew * self.Jw if np.asscalar(self.gains[0]) != 0.: R = data.pinocchio.oMf[self.xref.frame].rotation Aq += np.asscalar(self.gains[0]) * R * pinocchio.getFrameJacobian( self.state.pinocchio, data.pinocchio, self.xref.frame, pinocchio.ReferenceFrame.LOCAL)[:3, :] if np.asscalar(self.gains[1]) != 0.: Aq += np.asscalar(self.gains[1]) * (self.fXj[:3, :] * v_partial_dq) Av += np.asscalar(self.gains[1]) * (self.fXj[:3, :] * a_partial_da) data.Ax = np.hstack([Aq, Av])
v = np.matrix(np.random.rand(model.nv, 1)) # joint velocity a = np.matrix(np.random.rand(model.nv, 1)) # joint acceleration # Evaluate all the terms required by the kinematics derivatives pin.computeForwardKinematicsDerivatives(model, data, q, v, a) # Evaluate the derivatives for a precise joint (e.g. rleg6_joint) joint_name = "rleg6_joint" joint_id = model.getJointId(joint_name) # Derivatives of the spatial velocity with respect to the joint configuration and velocity vectors (dv_dq, dv_dv) = pin.getJointVelocityDerivatives(model, data, joint_id, pin.ReferenceFrame.WORLD) # or to get them in the LOCAL frame of the joint (dv_dq_local, dv_dv_local) = pin.getJointVelocityDerivatives(model, data, joint_id, pin.ReferenceFrame.LOCAL) # Derivatives of the spatial acceleration of the joint with respect to the joint configuration, velocity and acceleration vectors (dv_dq, da_dq, da_dv, da_da) = pin.getJointAccelerationDerivatives(model, data, joint_id, pin.ReferenceFrame.WORLD) # or to get them in the LOCAL frame of the joint (dv_dq_local, da_dq_local, da_dv_local, da_da_local) = pin.getJointAccelerationDerivatives(model, data, joint_id, pin.ReferenceFrame.LOCAL)
return cross(data.v[-1].angular, data.v[-1].linear) daa_dqn = df_dq(model, partial(calcaa, vq=vq, aq=aq), q) da_dqn = df_dq(model, partial(calca, vq=vq, aq=aq), q) dwxv_dqn = df_dq(model, partial(calcwxv, vq=vq, aq=aq), q) pinocchio.computeJointJacobians(model, data, q) J = pinocchio.getJointJacobian(model, data, model.joints[-1].id, pinocchio.ReferenceFrame.LOCAL) Jv = J[:3, :] Jw = J[3:, :] # a + wxv pinocchio.computeForwardKinematicsDerivatives(model, data, q, vq, aq) # da_dq = data.ddq_dq dv_dq, da_dq, da_dv, da_da = pinocchio.getJointAccelerationDerivatives( model, data, model.joints[-1].id, pinocchio.ReferenceFrame.LOCAL) assertNumDiff( da_dq, da_dqn, NUMDIFF_MODIFIER * h) # threshold was 1e-3, is now 2.11e-4 (see assertNumDiff.__doc__) def calcv(q, vq, aq): pinocchio.forwardKinematics(model, data, q, vq, aq) return data.v[-1].linear def calcw(q, vq, aq): pinocchio.forwardKinematics(model, data, q, vq, aq) return data.v[-1].angular