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)
kkt.setCandidate(xs, us) dxkkt, dukkt, lkkt = kkt.computeDirection() xkkt, ukkt, donekkt = kkt.solve(maxiter=2) assert (donekkt) ddp.setCandidate(xs, us) ddp.computeDirection() xddp, uddp, costddp = ddp.forwardPass(stepLength=1) assert (norm(xddp[0] - xkkt[0]) < 1e-9) assert (norm(xddp[1] - xkkt[1]) < 1e-9) assert (norm(uddp[0] - ukkt[0]) < 1e-9) # Test step length us = [np.random.rand(m.nu) for m in problem.runningModels] xs = problem.rollout(us) kkt.setCandidate(xs, us, isFeasible=True) ddp.setCandidate(xs, us, isFeasible=False) dxkkt, dukkt, lkkt = kkt.computeDirection() step = .1 dvkkt = kkt.tryStep(step) xkkt, ukkt = kkt.xs_try, kkt.us_try dxddp, duddp, lddp = ddp.computeDirection() dvddp = ddp.tryStep(step) xddp, uddp = ddp.xs_try, ddp.us_try assert (norm(xddp[0] - xkkt[0]) < 1e-9) assert (norm(xddp[1] - xkkt[1]) < 1e-9) assert (norm(uddp[0] - ukkt[0]) < 1e-9) d1, d2 = kkt.expectedImprovement()