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
