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
