def impactModel(contactIds, effectors):
State = StatePinocchio(rmodel)
# Creating a 6D multi-contact model, and then including the supporting foot
impulseModel = ImpulseModelMultiple(rmodel, {"impulse%d" % cid: ImpulseModel6D(rmodel, cid) for cid in contactIds})
# Creating the cost model for a contact phase
costModel = CostModelSum(rmodel, nu=0)
wx = np.array([0] * 6 + [.1] * (rmodel.nv - 6) + [10] * rmodel.nv)
costModel.addCost('xreg',
weight=.1,
cost=CostModelState(rmodel,
State,
ref=rmodel.defaultState,
nu=0,
activation=ActivationModelWeightedQuad(wx)))
costModel.addCost('com',
weight=1.,
cost=CostModelImpactCoM(rmodel, activation=ActivationModelWeightedQuad(m2a([.1, .1, 3.]))))
for fid, ref in effectors.items():
costModel.addCost("track%d" % fid, weight=100., cost=CostModelFramePlacement(rmodel, fid, ref, nu=0))
# costModel.addCost("vel%d"%fid, weight=0.,
# cost = CostModelFrameVelocity(rmodel,fid,nu=0))
# Creating the action model for the KKT dynamics with simpletic Euler
# integration scheme
model = ActionModelImpact(rmodel, impulseModel, costModel)
return model
class StatePinocchioFFTest(StateTestCase):
# NUMDIFF_MODIFIER: threshold was 2.11e-4, is now 2.11e-4 (see assertNumDiff.__doc__)
# Loading Talos legs
from crocoddyl import loadTalosLegs
rmodel = loadTalosLegs().model
StateTestCase.NX = rmodel.nq + rmodel.nv
StateTestCase.STATE = StatePinocchio(rmodel)
def __init__(self, pinocchioModel, actuationModel):
self.pinocchio = pinocchioModel
self.actuation = actuationModel
self.State = StatePinocchio(self.pinocchio)
self.costs = CostModelSum(self.pinocchio)
self.nq, self.nv = self.pinocchio.nq, self.pinocchio.nv
self.nx = self.State.nx
self.ndx = self.State.ndx
self.nout = self.nv
self.nu = self.actuation.nu
self.unone = np.zeros(self.nu)
def __init__(self):
robot = self.robot = loadTalosArm(freeFloating=True)
rmodel = self.rmodel = robot.model
qmin = rmodel.lowerPositionLimit
qmin[:7] = -1
rmodel.lowerPositionLimit = qmin
qmax = rmodel.upperPositionLimit
qmax[:7] = 1
rmodel.upperPositionLimit = qmax
State = self.State = StatePinocchio(rmodel)
actModel = self.actModel = ActuationModelFreeFloating(rmodel)
contactModel = self.contactModel = ContactModelMultiple(rmodel)
contact6 = ContactModel6D(rmodel,
rmodel.getFrameId(contactName),
ref=pinocchio.SE3.Identity(),
gains=[0., 0.])
contactModel.addContact(name='contact', contact=contact6)
costModel = self.costModel = CostModelSum(rmodel, nu=actModel.nu)
self.cost1 = CostModelFrameTranslation(
rmodel,
nu=actModel.nu,
frame=rmodel.getFrameId(opPointName),
ref=np.array([.5, .4, .3]))
stateWeights = np.array([0] * 6 + [0.01] * (rmodel.nv - 6) +
[10] * rmodel.nv)
self.cost2 = CostModelState(rmodel,
State,
ref=State.zero(),
nu=actModel.nu,
activation=ActivationModelWeightedQuad(
stateWeights**2))
self.cost3 = CostModelControl(rmodel, nu=actModel.nu)
costModel.addCost(name="pos", weight=10, cost=self.cost1)
costModel.addCost(name="regx", weight=0.1, cost=self.cost2)
costModel.addCost(name="regu", weight=0.01, cost=self.cost3)
self.dmodel = DifferentialActionModelFloatingInContact(
rmodel, actModel, contactModel, costModel)
self.model = IntegratedActionModelEuler(self.dmodel)
self.data = self.model.createData()
self.cd1 = self.data.differential.costs.costs['pos']
self.cd2 = self.data.differential.costs.costs['regx']
self.cd3 = self.data.differential.costs.costs['regu']
self.ddata = self.data.differential
self.rdata = self.data.differential.pinocchio
self.x = self.State.rand()
self.q = a2m(self.x[:rmodel.nq])
self.v = a2m(self.x[rmodel.nq:])
self.u = np.random.rand(self.model.nu)
def __init__(self, rmodel, lfFoot, rfFoot, lhFoot, rhFoot):
self.rmodel = rmodel
self.rdata = rmodel.createData()
self.state = StatePinocchio(self.rmodel)
# Getting the frame id for all the legs
self.lfFootId = self.rmodel.getFrameId(lfFoot)
self.rfFootId = self.rmodel.getFrameId(rfFoot)
self.lhFootId = self.rmodel.getFrameId(lhFoot)
self.rhFootId = self.rmodel.getFrameId(rhFoot)
# Defining default state
self.rmodel.defaultState = np.concatenate([
m2a(self.rmodel.referenceConfigurations["half_sitting"]),
np.zeros(self.rmodel.nv)
])
def runningModel(contactIds, effectors, com=None, integrationStep=1e-2):
'''
Creating the action model for floating-base systems. A walker system
is by default a floating-base system.
contactIds is a list of frame Ids of points that should be in contact.
effectors is a dict of key frame ids and SE3 values of effector references.
'''
actModel = ActuationModelFreeFloating(rmodel)
State = StatePinocchio(rmodel)
# Creating a 6D multi-contact model, and then including the supporting foot
contactModel = ContactModelMultiple(rmodel)
for cid in contactIds:
contactModel.addContact('contact%d' % cid,
ContactModel6D(rmodel, cid, ref=None))
# Creating the cost model for a contact phase
costModel = CostModelSum(rmodel, actModel.nu)
wx = np.array([0] * 6 + [.1] * (rmodel.nv - 6) + [10] * rmodel.nv)
costModel.addCost('xreg',
weight=1e-1,
cost=CostModelState(
rmodel,
State,
ref=rmodel.defaultState,
nu=actModel.nu,
activation=ActivationModelWeightedQuad(wx)))
costModel.addCost('ureg',
weight=1e-4,
cost=CostModelControl(rmodel, nu=actModel.nu))
for fid, ref in effectors.items():
if not isinstance(ref, SE3):
ref = SE3(eye(3), a2m(ref))
costModel.addCost("track%d" % fid,
weight=100.,
cost=CostModelFramePlacement(rmodel, fid, ref,
actModel.nu))
if com is not None:
costModel.addCost("com",
weight=100.,
cost=CostModelCoM(rmodel, ref=com, nu=actModel.nu))
# Creating the action model for the KKT dynamics with simpletic Euler
# integration scheme
dmodel = DifferentialActionModelFloatingInContact(rmodel, actModel,
contactModel, costModel)
model = IntegratedActionModelEuler(dmodel)
model.timeStep = integrationStep
return model
def impactModel(contactIds, effectors):
'''
Creating the action model for floating-base systems during the impact phase.
:params contactIds: list of frame Ids of points that should be in contact.
:params effectors: dict of key frame ids and SE3 values of effector references. This
value should typically be provided for effector landing.
'''
State = StatePinocchio(rmodel)
# Creating a 6D multi-contact model, and then including the supporting foot
impulseModel = ImpulseModelMultiple(
rmodel,
{"impulse%d" % cid: ImpulseModel6D(rmodel, cid)
for cid in contactIds})
# Creating the cost model for a contact phase
costModel = CostModelSum(rmodel, nu=0)
wx = np.array([0] * 6 + [.1] * (rmodel.nv - 6) + [10] * rmodel.nv)
costModel.addCost('xreg',
weight=1e-1,
cost=CostModelState(
rmodel,
State,
ref=rmodel.defaultState,
nu=0,
activation=ActivationModelWeightedQuad(wx)))
# costModel.addCost('com',weight=1.,
# cost=CostModelImpactCoM(rmodel,
# activation=ActivationModelWeightedQuad(m2a([.1,.1,3.]))))
for fid, ref in effectors.items():
wp = np.array([1.] * 6)
costModel.addCost("track%d" % fid,
weight=1e5,
cost=CostModelFramePlacement(
rmodel,
fid,
ref,
nu=0,
activation=ActivationModelWeightedQuad(wp)))
# costModel.addCost("vel%d"%fid, weight=0.,
# cost = CostModelFrameVelocity(rmodel,fid,nu=0))
# Creating the action model for the KKT dynamics with simpletic Euler
# integration scheme
model = ActionModelImpact(rmodel, impulseModel, costModel)
return model
def __init__(self):
robot = self.robot = loadTalosArm()
rmodel = self.rmodel = robot.model
rmodel.armature = np.matrix([0] * rmodel.nv).T
for j in rmodel.joints[1:]:
if j.shortname() != 'JointModelFreeFlyer':
rmodel.armature[j.idx_v:j.idx_v + j.nv] = 1
State = self.State = StatePinocchio(rmodel)
self.cost1 = CostModelFrameTranslation(
rmodel,
nu=rmodel.nv,
frame=rmodel.getFrameId(opPointName),
ref=np.array([.5, .4, .3]))
self.cost2 = CostModelState(rmodel,
State,
ref=State.zero(),
nu=rmodel.nv)
self.cost3 = CostModelControl(rmodel, nu=rmodel.nv)
costModel = CostModelSum(rmodel)
costModel.addCost(name="pos", weight=10, cost=self.cost1)
costModel.addCost(name="regx", weight=0.1, cost=self.cost2)
costModel.addCost(name="regu", weight=0.01, cost=self.cost3)
self.dmodel = DifferentialActionModelFullyActuated(rmodel, costModel)
self.model = IntegratedActionModelEuler(self.dmodel)
self.data = self.model.createData()
self.cd1 = self.data.differential.costs.costs['pos']
self.cd2 = self.data.differential.costs.costs['regx']
self.cd3 = self.data.differential.costs.costs['regu']
self.ddata = self.data.differential
self.rdata = self.data.differential.pinocchio
self.x = self.State.rand()
self.q = a2m(self.x[:rmodel.nq])
self.v = a2m(self.x[rmodel.nq:])
self.u = np.random.rand(self.model.nu)
qmax = robot.model.upperPositionLimit
qmax[:7] = 1
robot.model.upperPositionLimit = qmax
rmodel = robot.model
rdata = rmodel.createData()
# -----------------------------------------
q = pinocchio.randomConfiguration(rmodel)
v = rand(rmodel.nv)
x = m2a(np.concatenate([q, v]))
u = m2a(rand(rmodel.nv - 6))
# -------------------------------------------------
np.set_printoptions(linewidth=400, suppress=True)
State = StatePinocchio(rmodel)
actModel = ActuationModelFreeFloating(State)
gains = pinocchio.utils.rand(2)
Mref_lf = FramePlacement(rmodel.getFrameId('LF_FOOT'), pinocchio.SE3.Random())
contactModel6 = ContactModel6D(State, Mref_lf, actModel.nu, gains)
rmodel.frames[Mref_lf.frame].placement = pinocchio.SE3.Random()
contactModel = ContactModelMultiple(State, actModel.nu)
contactModel.addContact("LF_FOOT_contact", contactModel6)
contactData = contactModel.createData(rdata)
model = DifferentialActionModelFloatingInContact(
State, actModel, contactModel, CostModelSum(State, actModel.nu, False), 0.,
True)
# -----------Create inital trajectory---------------
class Init:
X = []
U = []
init = Init()
x_tsid = np.matrix(conf.X_init).T
conf.ddq_init.append(np.zeros(rmodel.nv))
dx_tsid = np.vstack([x_tsid[rmodel.nv:, :], np.matrix(conf.ddq_init).T])
t_tsid = np.linspace(0., cs.contact_phases[-1].time_trajectory[-1],
len(conf.ddq_init) + 1)
x_spl = CubicHermiteSpline(a2m(t_tsid), x_tsid, dx_tsid)
state = StatePinocchio(rmodel)
dt = conf.DT
def x_eval(t):
return m2a(x_spl.eval(t)[0])
for i, (m, d) in enumerate(zip(problem.runningModels, problem.runningDatas)):
# Impact models have zero timestep, thus they are copying the same state as the beginning of the next action model
# State and control are defined at the beginning of the time step.
if isinstance(m, ActionModelImpact):
init.X.append(x_eval((i + 1) * dt))
init.U.append(np.zeros(m.nu))
# print "impact at ",i
else:
def createMultiphaseShootingProblem(rmodel, rdata, csw, timeStep):
"""
Create a Multiphase Shooting problem from the output of the centroidal solver.
:params rmodel: robot model of type pinocchio::model
:params rdata: robot data of type pinocchio::data
:params csw: contact sequence wrapper of type ContactSequenceWrapper
:params timeStep: Scalar timestep between nodes.
:returns list of IntegratedActionModels
"""
# -----------------------
# Define Cost weights
class Weights:
com = 1e1
regx = 1e-3
regu = 0.
swing_patch = 1e6
forces = 0.
contactv = 1e3
# Define state cost vector for WeightedActivation
ff_orientation = 1e1
xweight = np.array([0] * 3 + [ff_orientation] * 3 + [1.] * (rmodel.nv - 6) + [1.] * rmodel.nv)
xweight[range(18, 20)] = ff_orientation
# for patch in swing_patch: w.swing_patch.append(100.);
# Define weights for the impact costs.
imp_state = 1e2
imp_com = 1e2
imp_contact_patch = 1e6
imp_act_com = m2a([0.1, 0.1, 3.0])
# Define weights for the terminal costs
term_com = 1e8
term_regx = 1e4
w = Weights()
# ------------------------
problem_models = []
actuationff = ActuationModelFreeFloating(rmodel)
State = StatePinocchio(rmodel)
active_contact_patch = set()
active_contact_patch_prev = set()
for nphase, phase in enumerate(csw.cs.contact_phases):
t0 = phase.time_trajectory[0]
t1 = phase.time_trajectory[-1]
N = int(round((t1 - t0) / timeStep)) + 1
contact_model = ContactModelMultiple(rmodel)
# Add contact constraints for the active contact patches.
# Add SE3 cost for the non-active contact patches.
swing_patch = []
active_contact_patch_prev = active_contact_patch.copy()
active_contact_patch.clear()
for patch in csw.ee_map.keys():
if getattr(phase, patch).active:
active_contact_patch.add(patch)
active_contact = ContactModel6D(rmodel,
frame=rmodel.getFrameId(csw.ee_map[patch]),
ref=getattr(phase, patch).placement)
contact_model.addContact(patch, active_contact)
# print nphase, "Contact ",patch," added at ", getattr(phase,patch).placement.translation.T
else:
swing_patch.append(patch)
# Check if contact has been added in this phase. If this phase is not zero,
# add an impulse model to deal with this contact.
new_contacts = active_contact_patch.difference(active_contact_patch_prev)
if nphase != 0 and len(new_contacts) != 0:
# print nphase, "Impact ",[p for p in new_contacts]," added"
imp_model = ImpulseModelMultiple(
rmodel, {
"Impulse_" + patch: ImpulseModel6D(rmodel, frame=rmodel.getFrameId(csw.ee_map[patch]))
for patch in new_contacts
})
# Costs for the impulse of a new contact
cost_model = CostModelSum(rmodel, nu=0)
# State
cost_regx = CostModelState(rmodel,
State,
ref=rmodel.defaultState,
nu=actuationff.nu,
activation=ActivationModelWeightedQuad(w.xweight))
cost_model.addCost("imp_regx", cost_regx, w.imp_state)
# CoM
cost_com = CostModelImpactCoM(rmodel, activation=ActivationModelWeightedQuad(w.imp_act_com))
cost_model.addCost("imp_CoM", cost_com, w.imp_com)
# Contact Frameplacement
for patch in new_contacts:
cost_contact = CostModelFramePlacement(rmodel,
frame=rmodel.getFrameId(csw.ee_map[patch]),
ref=SE3(np.identity(3), csw.ee_splines[patch].eval(t0)[0]),
nu=actuationff.nu)
cost_model.addCost("imp_contact_" + patch, cost_contact, w.imp_contact_patch)
imp_action_model = ActionModelImpact(rmodel, imp_model, cost_model)
problem_models.append(imp_action_model)
# Define the cost and action models for each timestep in the contact phase.
# untill [:-1] because in contact sequence timetrajectory, the end-time is
# also included. e.g., instead of being [0.,0.5], time trajectory is [0,0.5,1.]
for t in np.linspace(t0, t1, N)[:-1]:
cost_model = CostModelSum(rmodel, actuationff.nu)
# For the first node of the phase, add cost v=0 for the contacting foot.
if t == 0:
for patch in active_contact_patch:
cost_vcontact = CostModelFrameVelocity(rmodel,
frame=rmodel.getFrameId(csw.ee_map[patch]),
ref=m2a(Motion.Zero().vector),
nu=actuationff.nu)
cost_model.addCost("contactv_" + patch, cost_vcontact, w.contactv)
# CoM Cost
cost_com = CostModelCoM(rmodel, ref=m2a(csw.phi_c.com_vcom.eval(t)[0][:3, :]), nu=actuationff.nu)
cost_model.addCost("CoM", cost_com, w.com)
# Forces Cost
for patch in contact_model.contacts.keys():
cost_force = CostModelForce(rmodel,
contactModel=contact_model.contacts[patch],
ref=m2a(csw.phi_c.forces[patch].eval(t)[0]),
nu=actuationff.nu)
cost_model.addCost("forces_" + patch, cost_force, w.forces)
# Swing patch cost
for patch in swing_patch:
cost_swing = CostModelFramePlacement(rmodel,
frame=rmodel.getFrameId(csw.ee_map[patch]),
ref=SE3(np.identity(3), csw.ee_splines[patch].eval(t)[0]),
nu=actuationff.nu)
cost_model.addCost("swing_" + patch, cost_swing, w.swing_patch)
# print t, "Swing cost ",patch," added at ", csw.ee_splines[patch].eval(t)[0][:3].T
# State Regularization
cost_regx = CostModelState(rmodel,
State,
ref=rmodel.defaultState,
nu=actuationff.nu,
activation=ActivationModelWeightedQuad(w.xweight))
cost_model.addCost("regx", cost_regx, w.regx)
# Control Regularization
cost_regu = CostModelControl(rmodel, nu=actuationff.nu)
cost_model.addCost("regu", cost_regu, w.regu)
dmodel = DifferentialActionModelFloatingInContact(rmodel, actuationff, contact_model, cost_model)
imodel = IntegratedActionModelEuler(dmodel, timeStep=timeStep)
problem_models.append(imodel)
# Create Terminal Model.
contact_model = ContactModelMultiple(rmodel)
# Add contact constraints for the active contact patches.
swing_patch = []
t = t1
for patch in csw.ee_map.keys():
if getattr(phase, patch).active:
active_contact = ContactModel6D(rmodel,
frame=rmodel.getFrameId(csw.ee_map[patch]),
ref=getattr(phase, patch).placement)
contact_model.addContact(patch, active_contact)
cost_model = CostModelSum(rmodel, actuationff.nu)
# CoM Cost
cost_com = CostModelCoM(rmodel, ref=m2a(csw.phi_c.com_vcom.eval(t)[0][:3, :]), nu=actuationff.nu)
cost_model.addCost("CoM", cost_com, w.term_com)
# State Regularization
cost_regx = CostModelState(rmodel,
State,
ref=rmodel.defaultState,
nu=actuationff.nu,
activation=ActivationModelWeightedQuad(w.xweight))
cost_model.addCost("regx", cost_regx, w.term_regx)
dmodel = DifferentialActionModelFloatingInContact(rmodel, actuationff, contact_model, cost_model)
imodel = IntegratedActionModelEuler(dmodel)
problem_models.append(imodel)
problem_models.append
return problem_models
u = m2a(rand(rmodel.nv))
costModel = CostModelFrameTranslation(rmodel, rmodel.getFrameId('gripper_left_fingertip_2_link'),
np.array([.5, .4, .3]))
costData = costModel.createData(rdata)
pinocchio.forwardKinematics(rmodel, rdata, q, v)
pinocchio.computeJointJacobians(rmodel, rdata, q)
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.computeJointJacobians(m, d, a2m(x[:rmodel.nq])),
lambda m, d, x, u: pinocchio.updateFramePlacements(m, d)
])
costDataND = costModelND.createData(rdata)
costModelND.calcDiff(costDataND, x, u)
assertNumDiff(costData.Lx, costDataND.Lx, NUMDIFF_MODIFIER *
costModelND.disturbance) # threshold was 1e-4, is now 2.11e-4 (see assertNumDiff.__doc__)
assertNumDiff(costData.Lu, costDataND.Lu, NUMDIFF_MODIFIER *
costModelND.disturbance) # threshold was 1e-4, is now 2.11e-4 (see assertNumDiff.__doc__)
assertNumDiff(costData.Lxx, costDataND.Lxx, NUMDIFF_MODIFIER *
data.Fx, dnum.Fx, 1e4 * mnum.disturbance
) # threshold was 2.7e-2, is now 2.11e-4 (see assertNumDiff.__doc__)
assertNumDiff(
data.Fu, dnum.Fu, 1e4 * mnum.disturbance
) # threshold was 7e-3, is now 2.11e-4 (see assertNumDiff.__doc__)
assertNumDiff(
data.Lx, dnum.Lx, 1e6 * mnum.disturbance
) # threshold was 2.7e-2, is now 2.11e-2 (see assertNumDiff.__doc__)
assertNumDiff(
data.Lu, dnum.Lu, 1e6 * mnum.disturbance
) # threshold was 7e-3, is now 2.11e-2 (see assertNumDiff.__doc__)
assert (absmax(data.Lx - dnum.Lx) / model.nx < 1e-3)
assert (absmax(data.Lu - dnum.Lu) / model.nu < 1e-3)
State = StatePinocchio(rmodel)
actModel = ActuationModelFreeFloating(rmodel)
contactModel = ContactModelMultiple(rmodel)
contactModel.addContact(name='root_joint', contact=contactModel6)
costModel = CostModelSum(rmodel, nu=actModel.nu)
costModel.addCost(
name="pos",
weight=10,
cost=CostModelFrameTranslation(
rmodel,
nu=actModel.nu,
frame=rmodel.getFrameId('gripper_left_inner_single_link'),
ref=np.array([.5, .4, .3])))
costModel.addCost(name="regx",
dnum.data0.costs['force_cone'].contact = dnum.data0.contact[model.costs['force_cone'].cost.contact]
mnum.calcDiff(dnum, x, u)
assertNumDiff(data.Fx, dnum.Fx,
1e4 * mnum.disturbance) # threshold was 2.7e-2, is now 2.11e-4 (see assertNumDiff.__doc__)
assertNumDiff(data.Fu, dnum.Fu,
1e4 * mnum.disturbance) # threshold was 7e-3, is now 2.11e-4 (see assertNumDiff.__doc__)
assertNumDiff(data.Lx, dnum.Lx,
1e6 * mnum.disturbance) # threshold was 2.7e-2, is now 2.11e-2 (see assertNumDiff.__doc__)
assertNumDiff(data.Lu, dnum.Lu,
1e6 * mnum.disturbance) # threshold was 7e-3, is now 2.11e-2 (see assertNumDiff.__doc__)
assert (absmax(data.Lx - dnum.Lx) / model.nx < 1e-3)
assert (absmax(data.Lu - dnum.Lu) / model.nu < 1e-3)
State = StatePinocchio(rmodel)
actModel = ActuationModelFreeFloating(rmodel)
contactModel = ContactModelMultiple(rmodel)
contactModel.addContact(name='root_joint', contact=contactModel6)
costModel = CostModelSum(rmodel, nu=actModel.nu)
costModel.addCost(name="pos",
weight=10,
cost=CostModelFrameTranslation(rmodel,
nu=actModel.nu,
frame=rmodel.getFrameId('gripper_left_inner_single_link'),
ref=np.array([.5, .4, .3])))
costModel.addCost(name="regx", weight=0.1, cost=CostModelState(rmodel, State, ref=State.zero(), nu=actModel.nu))
costModel.addCost(name="regu", weight=0.01, cost=CostModelControl(rmodel, nu=actModel.nu))
costModel = CostModelFrameTranslation(
rmodel, rmodel.getFrameId('gripper_left_fingertip_2_link'),
np.array([.5, .4, .3]))
costData = costModel.createData(rdata)
pinocchio.forwardKinematics(rmodel, rdata, q, v)
pinocchio.computeJointJacobians(rmodel, rdata, q)
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.computeJointJacobians(
m, d, a2m(x[:rmodel.nq])),
lambda m, d, x, u: pinocchio.updateFramePlacements(m, d)
])
costDataND = costModelND.createData(rdata)
costModelND.calcDiff(costDataND, x, u)
assertNumDiff(
costData.Lx, costDataND.Lx, NUMDIFF_MODIFIER * costModelND.disturbance
) # threshold was 1e-4, is now 2.11e-4 (see assertNumDiff.__doc__)