def setUp(self):
self.x = self.ROBOT_STATE.rand()
self.robot_data = self.ROBOT_MODEL.createData()
self.data = self.IMPULSE.createData(self.robot_data)
self.data_der = self.IMPULSE_DER.createData(self.robot_data)
nq, nv = self.ROBOT_MODEL.nq, self.ROBOT_MODEL.nv
pinocchio.forwardKinematics(self.ROBOT_MODEL, self.robot_data, self.x[:nq], self.x[nq:],
pinocchio.utils.zero(nv))
pinocchio.computeJointJacobians(self.ROBOT_MODEL, self.robot_data)
pinocchio.updateFramePlacements(self.ROBOT_MODEL, self.robot_data)
pinocchio.computeForwardKinematicsDerivatives(self.ROBOT_MODEL, self.robot_data, self.x[:nq], self.x[nq:],
pinocchio.utils.zero(nv))
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 setUp(self):
self.robot_data = self.ROBOT_MODEL.createData()
self.x = self.ROBOT_STATE.rand()
self.u = pinocchio.utils.rand(self.ROBOT_MODEL.nv)
self.multibody_data = crocoddyl.DataCollectorMultibody(self.robot_data)
self.data = self.COST.createData(self.multibody_data)
self.data_der = self.COST_DER.createData(self.multibody_data)
nq, nv = self.ROBOT_MODEL.nq, self.ROBOT_MODEL.nv
pinocchio.forwardKinematics(self.ROBOT_MODEL, self.robot_data, self.x[:nq], self.x[nq:])
pinocchio.computeForwardKinematicsDerivatives(self.ROBOT_MODEL, self.robot_data, self.x[:nq], self.x[nq:],
pinocchio.utils.zero(nv))
pinocchio.computeJointJacobians(self.ROBOT_MODEL, self.robot_data, self.x[:nq])
pinocchio.updateFramePlacements(self.ROBOT_MODEL, self.robot_data)
pinocchio.jacobianCenterOfMass(self.ROBOT_MODEL, self.robot_data, self.x[:nq], False)
def test_numdiff(self):
rmodel,rdata = self.rmodel,self.rdata
q,vq,aq,dq= self.q,self.vq,self.aq,self.dq
#### Compute d/dq VCOM with the algo.
pin.computeAllTerms(rmodel,rdata,q,vq)
pin.computeForwardKinematicsDerivatives(rmodel,rdata,q,vq,aq)
dvc_dq = pin.getCenterOfMassVelocityDerivatives(rmodel,rdata)
#### Approximate d/dq VCOM by finite diff.
def calc_vc(q,vq):
""" Compute COM velocity """
pin.centerOfMass(rmodel,rdata,q,vq)
return rdata.vcom[0]
dvc_dqn = df_dq(rmodel,lambda _q: calc_vc(_q,vq),q)
self.assertTrue(np.allclose(dvc_dq,dvc_dqn,atol=np.sqrt(self.precision)))
def setUp(self):
self.x = self.ROBOT_STATE.rand()
self.robot_data = self.ROBOT_MODEL.createData()
self.contacts = crocoddyl.ContactModelMultiple(self.ROBOT_STATE)
self.contacts.addContact("myContact", self.CONTACT)
self.data = self.CONTACT.createData(self.robot_data)
self.data_multiple = self.contacts.createData(self.robot_data)
nq, nv = self.ROBOT_MODEL.nq, self.ROBOT_MODEL.nv
pinocchio.forwardKinematics(self.ROBOT_MODEL, self.robot_data, self.x[:nq], self.x[nq:],
pinocchio.utils.zero(nv))
pinocchio.computeJointJacobians(self.ROBOT_MODEL, self.robot_data)
pinocchio.updateFramePlacements(self.ROBOT_MODEL, self.robot_data)
pinocchio.computeForwardKinematicsDerivatives(self.ROBOT_MODEL, self.robot_data, self.x[:nq], self.x[nq:],
pinocchio.utils.zero(nv))
def setUp(self):
self.x = self.ROBOT_STATE.rand()
self.robot_data = self.ROBOT_MODEL.createData()
self.impulseSum = crocoddyl.ImpulseModelMultiple(self.ROBOT_STATE)
self.datas = collections.OrderedDict([[name, impulse.createData(self.robot_data)]
for name, impulse in self.IMPULSES.items()])
for name, impulse in self.IMPULSES.items():
self.impulseSum.addImpulse(name, impulse)
self.dataSum = self.impulseSum.createData(self.robot_data)
nq, nv = self.ROBOT_MODEL.nq, self.ROBOT_MODEL.nv
pinocchio.forwardKinematics(self.ROBOT_MODEL, self.robot_data, self.x[:nq], self.x[nq:],
pinocchio.utils.zero(nv))
pinocchio.computeJointJacobians(self.ROBOT_MODEL, self.robot_data)
pinocchio.updateFramePlacements(self.ROBOT_MODEL, self.robot_data)
pinocchio.computeForwardKinematicsDerivatives(self.ROBOT_MODEL, self.robot_data, self.x[:nq], self.x[nq:],
pinocchio.utils.zero(nv))
def test_numdiff(self):
rmodel, rdata = self.rmodel, self.rdata
q, vq, aq, dq = self.q, self.vq, self.aq, self.dq
#### Compute d/dq VCOM with the algo.
pin.computeAllTerms(rmodel, rdata, q, vq)
pin.computeForwardKinematicsDerivatives(rmodel, rdata, q, vq, aq)
dvc_dq = pin.getCenterOfMassVelocityDerivatives(rmodel, rdata)
#### Approximate d/dq VCOM by finite diff.
def calc_vc(q, vq):
""" Compute COM velocity """
pin.centerOfMass(rmodel, rdata, q, vq)
return rdata.vcom[0].copy()
dvc_dqn = df_dq(rmodel, lambda _q: calc_vc(_q, vq), q)
self.assertTrue(
np.allclose(dvc_dq, dvc_dqn, atol=np.sqrt(self.precision)))
def test_derivatives(self):
model = self.model
data = self.data
q = self.q
v = self.v
a = self.a
pin.computeForwardKinematicsDerivatives(model, data, q, v, a)
pin.getFrameVelocityDerivatives(model, data, self.frame_idx, pin.WORLD)
pin.getFrameVelocityDerivatives(model, data, self.frame_idx, pin.LOCAL)
pin.getFrameVelocityDerivatives(model, data, self.frame_idx,
pin.LOCAL_WORLD_ALIGNED)
pin.getFrameAccelerationDerivatives(model, data, self.frame_idx,
pin.WORLD)
pin.getFrameAccelerationDerivatives(model, data, self.frame_idx,
pin.LOCAL)
pin.getFrameAccelerationDerivatives(model, data, self.frame_idx,
pin.LOCAL_WORLD_ALIGNED)
def setUp(self):
self.robot_data = self.ROBOT_MODEL.createData()
self.x = self.ROBOT_STATE.rand()
self.u = pinocchio.utils.rand(self.ROBOT_MODEL.nv)
self.cost_sum = crocoddyl.CostModelSum(self.ROBOT_STATE)
self.cost_sum.addCost('myCost', self.COST, 1.)
self.data = self.COST.createData(self.robot_data)
self.data_sum = self.cost_sum.createData(self.robot_data)
nq, nv = self.ROBOT_MODEL.nq, self.ROBOT_MODEL.nv
pinocchio.forwardKinematics(self.ROBOT_MODEL, self.robot_data,
self.x[:nq], self.x[nq:])
pinocchio.computeForwardKinematicsDerivatives(self.ROBOT_MODEL,
self.robot_data,
self.x[:nq], self.x[nq:],
pinocchio.utils.zero(nv))
pinocchio.computeJointJacobians(self.ROBOT_MODEL, self.robot_data,
self.x[:nq])
pinocchio.updateFramePlacements(self.ROBOT_MODEL, self.robot_data)
pinocchio.jacobianCenterOfMass(self.ROBOT_MODEL, self.robot_data,
self.x[:nq], False)
model = pin.buildSampleModelHumanoidRandom()
data = model.createData()
# Set bounds (by default they are undefinded)
model.lowerPositionLimit = -np.matrix(np.ones((model.nq, 1)))
model.upperPositionLimit = np.matrix(np.ones((model.nq, 1)))
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)
def calcwxv(q, vq, aq):
pinocchio.forwardKinematics(model, data, q, vq, aq)
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)
costModelND.disturbance) # threshold was 1e-4, is now 2.11e-4 (see assertNumDiff.__doc__)
assertNumDiff(costData.Lxu, costDataND.Lxu, NUMDIFF_MODIFIER *
costModelND.disturbance) # threshold was 1e-4, is now 2.11e-4 (see assertNumDiff.__doc__)
assertNumDiff(costData.Luu, costDataND.Luu, NUMDIFF_MODIFIER *
costModelND.disturbance) # threshold was 1e-4, is now 2.11e-4 (see assertNumDiff.__doc__)
q = pinocchio.randomConfiguration(rmodel)
v = rand(rmodel.nv)
x = m2a(np.concatenate([q, v]))
u = m2a(rand(rmodel.nv))
costModel = CostModelFrameVelocity(rmodel, rmodel.getFrameId('gripper_left_fingertip_2_link'))
costData = costModel.createData(rdata)
pinocchio.forwardKinematics(rmodel, rdata, q, v)
pinocchio.computeForwardKinematicsDerivatives(rmodel, rdata, q, v, zero(rmodel.nv))
pinocchio.updateFramePlacements(rmodel, rdata)
costModel.calcDiff(costData, x, u)
costModelND = CostModelNumDiff(
costModel,
StatePinocchio(rmodel),
withGaussApprox=True,
reevals=[
lambda m, d, x, u: pinocchio.forwardKinematics(m, d, a2m(x[:rmodel.nq]), a2m(x[rmodel.nq:])),
lambda m, d, x, u: pinocchio.computeForwardKinematicsDerivatives(m, d, a2m(x[:rmodel.nq]), a2m(x[
rmodel.nq:]), zero(rmodel.nv)), lambda m, d, x, u: pinocchio.updateFramePlacements(m, d)
])
costDataND = costModelND.createData(rdata)
contactModel = ContactModel6D(
rmodel,
rmodel.getFrameId('gripper_left_fingertip_2_link'),
ref=pinocchio.SE3.Random(),
gains=[4., 4.])
contactData = contactModel.createData(rdata)
q = pinocchio.randomConfiguration(rmodel)
v = rand(rmodel.nv)
x = m2a(np.concatenate([q, v]))
u = m2a(rand(rmodel.nv - 6))
pinocchio.forwardKinematics(rmodel, rdata, q, v, zero(rmodel.nv))
pinocchio.computeJointJacobians(rmodel, rdata)
pinocchio.updateFramePlacements(rmodel, rdata)
pinocchio.computeForwardKinematicsDerivatives(rmodel, rdata, q, v,
zero(rmodel.nv))
contactModel.calc(contactData, x)
contactModel.calcDiff(contactData, x)
rdata2 = rmodel.createData()
pinocchio.computeAllTerms(rmodel, rdata2, q, v)
pinocchio.updateFramePlacements(rmodel, rdata2)
contactData2 = contactModel.createData(rdata2)
contactModel.calc(contactData2, x)
assert (norm(contactData.a0 - contactData2.a0) < 1e-9)
assert (norm(contactData.J - contactData2.J) < 1e-9)
def returna_at0(q, v):
x = np.vstack([q, v]).flat
pinocchio.computeAllTerms(rmodel, rdata2, q, v)
def calcDiff(self, data, x, u=None, recalc=True):
'''
k = [Mv;0]; K = [MJ^T;J0]
r = [vnext;f] = K^-1 k
dr/dv = K^-1 [M;0]
dr/dq = -K^-1 K'K^-1 k + K^-1 k' = -K^-1 (K'r-k')
= -K^-1 [ M'vnext + J'^T f- M'v ]
[ J'vnext ]
= -K^-1 [ M'(vnext-v) + J'^T f ]
[ J' vnext ]
'''
if recalc:
xout, cost = self.calc(data, x, u)
nq, nv = self.nq, self.nv
q = a2m(x[:nq])
v = a2m(x[-nv:])
vnext = a2m(data.vnext)
fs = data.impulse.forces
# Derivative M' dv + J'f + b'
g6bak = self.pinocchio.gravity.copy()
self.pinocchio.gravity = pinocchio.Motion.Zero()
pinocchio.computeRNEADerivatives(self.pinocchio, data.pinocchio, q,
zero(nv), vnext - v, fs)
self.pinocchio.gravity = g6bak
data.did_dq[:, :] = data.pinocchio.dtau_dq
# Derivative of the impulse constraint
pinocchio.computeForwardKinematicsDerivatives(self.pinocchio,
data.pinocchio, q, vnext,
zero(nv))
# pinocchio.updateFramePlacements(self.pinocchio,data.pinocchio)
self.impulse.calcDiff(data.impulse, x, recalc=False)
data.dv_dq = data.impulse.Vq
data.Fq[:nv, :] = 0
np.fill_diagonal(data.Fq[:nv, :], 1) # dq/dq
data.Fv[:nv, :] = 0 # dq/dv
data.Fx[nv:, :nv] = -np.dot(data.Kinv[:nv, :],
np.vstack([data.did_dq, data.dv_dq
])) # dvnext/dq
data.Fx[nv:, nv:] = np.dot(data.Kinv[:nv, :nv],
data.K[:nv, :nv]) # dvnext/dv
# data.Rx[:,:] = 0
# np.fill_diagonal(data.Rv,-1)
# data.Rx[:,:] += data.Fx[nv:,:]
# data.Rx *= self.impulseWeight
# data.Lx [:] = np.dot(data.Rx.T,data.costResiduals)
# data.Lxx[:,:] = np.dot(data.Rx.T,data.Rx)
if isinstance(self.costs, CostModelImpactBase):
self.costs.setImpactDiffData(data.costs, data.Fx[nv:, :])
if isinstance(self.costs, CostModelSum):
for cmodel, cdata in zip(self.costs.costs.values(),
data.costs.costs.values()):
if isinstance(cmodel.cost, CostModelImpactBase):
cmodel.cost.setImpactDiffData(cdata, data.Fx[nv:, :])
self.costs.calcDiff(data.costs, x, u=None, recalc=recalc)
return data.xnext, data.cost
