def setUp(self):
# Creating the model data
self.model = self.MODEL
self.data = self.model.createData()
# Creating shooting problem
self.problem = ShootingProblem(self.model.State.zero(), [self.model, self.model], self.model)
def createProblem(self, x0, comGoTo, timeStep, numKnots):
# Compute the current foot positions
q0 = a2m(x0[:self.rmodel.nq])
pinocchio.forwardKinematics(self.rmodel, self.rdata, q0)
pinocchio.updateFramePlacements(self.rmodel, self.rdata)
com0 = m2a(pinocchio.centerOfMass(self.rmodel, self.rdata, q0))
# Defining the action models along the time instances
comModels = []
# Creating the action model for the CoM task
comForwardModels = [
self.createModels(
timeStep,
[self.lfFootId, self.rfFootId, self.lhFootId, self.rhFootId],
) for k in range(numKnots)
]
comForwardTermModel = self.createModels(
timeStep,
[self.lfFootId, self.rfFootId, self.lhFootId, self.rhFootId],
com0 + [comGoTo, 0., 0.])
comForwardTermModel.differential.costs['comTrack'].weight = 1e6
# Adding the CoM tasks
comModels += comForwardModels + [comForwardTermModel]
# Defining the shooting problem
problem = ShootingProblem(x0, comModels, comModels[-1])
return problem
def setUp(self):
# Creating the model
self.model = self.MODEL
self.data = self.model.createData()
# Defining the shooting problem
self.problem = ShootingProblem(self.model.State.zero(), [self.model, self.model], self.model)
# Creating the KKT solver
self.kkt = SolverKKT(self.problem)
# Setting up a warm-point
xs = [m.State.zero() for m in self.problem.runningModels + [self.problem.terminalModel]]
us = [np.zeros(m.nu) for m in self.problem.runningModels]
self.kkt.setCandidate(xs, us)
rdata = robot.data
rmodel.defaultState = np.concatenate([m2a(robot.q0), np.zeros(rmodel.nv)])
# ----------------Load Contact Phases and PostProcess-----------------------
cs = ContactSequenceHumanoid(0)
cs.loadFromXML(conf.MUSCOD_CS_OUTPUT_FILENAME, conf.CONTACT_SEQUENCE_XML_TAG)
csw = ContactSequenceWrapper(cs, conf.contact_patches)
csw.createCentroidalPhi(rmodel, rdata)
csw.createEESplines(rmodel, rdata, conf.X_init, 0.005)
# ----------------Define Problem----------------------------
models = createMultiphaseShootingProblem(rmodel, rdata, csw, conf.DT)
# disp = lambda xs: disptraj(robot, xs)
problem = ShootingProblem(m2a(conf.X_init[0]), models[:-1], models[-1])
# Set contacts in the data elements. Ugly.
# This is defined for IAMEuler. If using IAMRK4, differential is a list. so we need to change.
for d in problem.runningDatas:
if hasattr(
d, "differential"
): # Because we also have the impact models without differntial.
for (patchname,
contactData) in d.differential.contact.contacts.iteritems():
if "forces_" + patchname in d.differential.costs.costs:
d.differential.costs["forces_" +
patchname].contact = contactData
# ---------Ugliness over------------------------
models += [
runningModel([rightId], {leftId: left0 + [0, 0, 0.1]},
com=com0 + [-0.1, 0, 0],
integrationStep=5e-2)
]
pass1 = models[10]
pass2 = models[21]
pass1.differential.costs['com'].weight = 100000
pass2.differential.costs['com'].weight = 100000
pass2.differential.costs['track16'].weight = 100000
# --- DDP
problem = ShootingProblem(initialState=x0,
runningModels=models[:-1],
terminalModel=models[-1])
ddp = SolverDDP(problem)
# ddp.callback = [ CallbackDDPLogger(), CallbackDDPVerbose() ]
ddp.th_stop = 1e-6
ddp.setCandidate()
m = models[0]
d = m.createData()
m = m.differential
d = d.differential
# m=m.differential.contact['contact30']
# d=d.differential.contact['contact30']
m.calcDiff(d, ddp.xs[0], ddp.us[0])
xref = fix.State.rand()
xref[fix.rmodel.nq:] = 0
fix.calc(xref)
f = ff
f.u[:] = (
0 *
pinocchio.rnea(fix.rmodel, fix.rdata, fix.q, fix.v * 0, fix.v * 0)).flat
f.v[:] = 0
f.x[f.rmodel.nq:] = f.v.flat
# f.u[:] = np.zeros(f.model.nu)
f.model.differential.costs['pos'].weight = 1
f.model.differential.costs['regx'].weight = 0.01
f.model.differential.costs['regu'].weight = 0.0001
fterm = f.__class__()
fterm.model.differential.costs['pos'].weight = 1000
fterm.model.differential.costs['regx'].weight = 1
fterm.model.differential.costs['regu'].weight = 0.01
problem = ShootingProblem(f.x, [f.model] * T, fterm.model)
u0s = [f.u] * T
x0s = problem.rollout(u0s)
# disp = lambda xs: disptraj(f.robot, xs)
ddp = SolverDDP(problem)
# ddp.callback = [ CallbackDDPLogger(), CallbackDDPVerbose() ]
ddp.th_stop = 1e-18
ddp.solve(maxiter=1000)
assertNumDiff(data.Fu, dnum.Fu,
NUMDIFF_MODIFIER * mnum.disturbance) # threshold was 3.16e-2, is now 2.11e-4 (see assertNumDiff.__doc__)
assertNumDiff(data.Lx, dnum.Lx,
NUMDIFF_MODIFIER * mnum.disturbance) # threshold was 3.16e-2, is now 2.11e-4 (see assertNumDiff.__doc__)
assertNumDiff(data.Lu, dnum.Lu,
NUMDIFF_MODIFIER * mnum.disturbance) # threshold was 3.16e-2, is now 2.11e-4 (see assertNumDiff.__doc__)
assertNumDiff(dnum.Lxx, data.Lxx,
NUMDIFF_MODIFIER * mnum.disturbance) # threshold was 3.16e-2, is now 2.11e-4 (see assertNumDiff.__doc__)
assertNumDiff(dnum.Lxu, data.Lxu,
NUMDIFF_MODIFIER * mnum.disturbance) # threshold was 3.16e-2, is now 2.11e-4 (see assertNumDiff.__doc__)
assertNumDiff(dnum.Luu, data.Luu,
NUMDIFF_MODIFIER * mnum.disturbance) # threshold was 3.16e-2, is now 2.11e-4 (see assertNumDiff.__doc__)
# -------------------------------------------------------------------------------
# -------------------------------------------------------------------------------
# -------------------------------------------------------------------------------
# --- DDP FOR THE ARM ---
dmodel = DifferentialActionModelPositioning(rmodel)
model = IntegratedActionModelEuler(dmodel)
problem = ShootingProblem(model.State.zero() + 1, [model], model)
kkt = SolverKKT(problem)
kkt.th_stop = 1e-18
xkkt, ukkt, dkkt = kkt.solve()
ddp = SolverDDP(problem)
ddp.th_stop = 1e-18
xddp, uddp, dddp = ddp.solve()
assert (norm(uddp[0] - ukkt[0]) < 1e-6)
model.timeStep = 1e-1
dmodel.costs['pos'].weight = 1
dmodel.costs['regx'].weight = 0
dmodel.costs['regu'].weight = 0
# Choose a cost that is reachable.
x0 = model.State.rand()
xref = model.State.rand()
xref[:7] = x0[:7]
xref[3:7] = [0, 0, 0,
1] # TODO: remove this after adding assertion to include any case
pinocchio.forwardKinematics(rmodel, rdata, a2m(xref[:rmodel.nq]))
pinocchio.updateFramePlacements(rmodel, rdata)
c1.ref[:] = m2a(rdata.oMf[c1.frame].translation.copy())
problem = ShootingProblem(x0, [model], model)
ddp = SolverDDP(problem)
ddp.callback = [CallbackDDPLogger()]
ddp.th_stop = 1e-18
xddp, uddp, doneddp = ddp.solve(maxiter=400)
assert (doneddp)
assert (norm(ddp.datas()[-1].differential.costs['pos'].residuals) < 1e-3)
assert (norm(
m2a(ddp.datas()[-1].differential.costs['pos'].pinocchio.oMf[
c1.frame].translation) - c1.ref) < 1e-3)
u0 = np.zeros(model.nu)
x1 = model.calc(data, problem.initialState, u0)[0]
x0s = [problem.initialState.copy(), x1]
# --------------------------------------------------- # --------------------------------------------------- np.set_printoptions(linewidth=400, suppress=True) np.random.seed(220) nq = 4 nu = 2 nv = nq dmodel = DifferentialActionModelLQR(nq, nu) model = IntegratedActionModelEuler(dmodel, withCostResiduals=False) nx = model.nx nu = model.nu T = 1 problem = ShootingProblem(model.State.zero() + 2, [model] * T, model) xs = [m.State.zero() for m in problem.runningModels + [problem.terminalModel]] us = [np.zeros(m.nu) for m in problem.runningModels] x0ref = problem.initialState xs[0] = np.random.rand(nx) xs[1] = np.random.rand(nx) us[0] = np.random.rand(nu) # xs[1] = model.calc(data,xs[0],us[0])[0].copy() ddpbox = SolverBoxDDP(problem) ddpbox.qpsolver = quadprogWrapper ddp = SolverDDP(problem) ddp.setCandidate(xs, us) ddp.computeDirection()
# --- TEST KKT --- # --------------------------------------------------- # --------------------------------------------------- # --------------------------------------------------- NX = 1 NU = 1 model = ActionModelUnicycle() data = model.createData() LQR = isinstance(model, ActionModelLQR) x = model.State.rand() u = np.zeros([model.nu]) problem = ShootingProblem(model.State.zero(), [model, model], model) kkt = SolverKKT(problem) xs = [m.State.zero() for m in problem.runningModels + [problem.terminalModel]] us = [np.zeros(m.nu) for m in problem.runningModels] # Test dimensions of KKT calc. kkt.setCandidate(xs, us) # Test that the solution respect the dynamics (or linear approx of it). dxs, dus, ls = kkt.computeDirection() x0, x1, x2 = dxs u0, u1 = dus l0, l1, l2 = ls # If LQR. test that a random solution respect the dynamics xs = [m.State.rand() for m in problem.runningModels + [problem.terminalModel]]
