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()