def calcDiff(self, data, x, recalc=True): if recalc: self.calc(data, x) dv_dq, dv_dv = pinocchio.getJointVelocityDerivatives( self.pinocchio, data.pinocchio, data.joint, pinocchio.ReferenceFrame.LOCAL) data.Vq[:, :] = data.fXj * dv_dq
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, u): v_partial_dq, v_partial_dv = pinocchio.getJointVelocityDerivatives(self.state.pinocchio, data.shared.pinocchio, self.joint, pinocchio.ReferenceFrame.LOCAL) self.activation.calcDiff(data.activation, data.r) data.Rx = np.hstack([self.fXj * v_partial_dq, self.fXj * v_partial_dv]) data.Lx = data.Rx.T * data.activation.Ar data.Lxx = data.Rx.T * data.activation.Arr * data.Rx
def calcDiff(self, data, x, u, recalc=True): if recalc: self.calc(data, x, u) dv_dq, dv_dvq = pinocchio.getJointVelocityDerivatives( self.pinocchio, data.pinocchio, data.joint, pinocchio.ReferenceFrame.LOCAL) Ax, Axx = self.activation.calcDiff(data.activation, data.residuals, recalc=recalc) data.Rq[:, :] = (data.fXj * dv_dq)[:3, :] data.Rv[:, :] = (data.fXj * dv_dvq)[:3, :] data.Lx[:] = np.dot(data.Rx.T, Ax) data.Lxx[:, :] = np.dot(data.Rx.T, Axx * data.Rx) return data.cost
def calcDiff(self, q): Jp = pinv( pin.computeJointJacobian(robot.model, robot.data, q, self.jointIndex)) res = np.zeros(robot.model.nv) v0 = np.zeros(robot.model.nv) for k in range(6): pin.computeForwardKinematicsDerivatives(robot.model, robot.data, q, Jp[:, k], v0) JqJpk = pin.getJointVelocityDerivatives(robot.model, robot.data, self.jointIndex, pin.LOCAL)[0] res += JqJpk[k, :] res *= self.calc(q) return res
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): v_partial_dq, v_partial_dv = pinocchio.getJointVelocityDerivatives( self.state.pinocchio, data.pinocchio, self.joint, pinocchio.ReferenceFrame.LOCAL) data.dv0_dq = self.fXj * v_partial_dq
q = pin.randomConfiguration(model) # joint configuration 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)
def calcDiff(self, data, x, recalc=True): if recalc: self.calc(data, x) v_partial_dq, v_partial_dv = pinocchio.getJointVelocityDerivatives(self.state.pinocchio, data.pinocchio, self.joint, pinocchio.ReferenceFrame.LOCAL) data.dv0_dq = self.fXj[:3, :] * v_partial_dq