acc = dx[rmodel.nv:] / dt
u = pinocchio.rnea(rmodel, rdata, a2m(xp[:rmodel.nq]),
a2m(xp[rmodel.nq:]), a2m(acc))
m.differential.calc(d.differential, init.X[-1])
contactJ = d.differential.contact.J
f = np.dot(np.linalg.pinv(contactJ.T[:6, :]), u[:6])
u -= (np.dot(contactJ.transpose(), f))
init.U.append(np.array(u[6:]).squeeze().copy())
init.X.append(conf.X_init[-1])
# ---------------Display Initial Trajectory--------------
if conf.DISPLAY:
robot.initDisplay(loadModel=True)
if conf.DISPLAY:
for x in init.X:
robot.display(a2m(x[:robot.nq]))
# sleep(0.005)
# ----------------------
ddp = SolverDDP(problem)
ddp.callback = [CallbackDDPVerbose()] # CallbackSolverTimer()]
if conf.RUNTIME_DISPLAY:
ddp.callback.append(CallbackSolverDisplay(robot, 4))
ddp.th_stop = 1e-9
ddp.solve(maxiter=1000, regInit=0.1, init_xs=init.X, init_us=init.U)
# ---------------Display Final Trajectory--------------
if conf.DISPLAY:
for x in init.X:
robot.display(a2m(x[:robot.nq]))
# sleep(0.005)
# ----------------------
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]
u0s = [u0.copy()]
dmodel.costs['regu'].weight = 1e-3
v1 = pinv(np.vstack([Jr, Jl, Jcom])) * np.vstack([zero(12), vcom])
x0[rmodel.nq:] = v1.flat
mimp1.costs['track16'].cost.activation.weights[:2] = 0
mimp2.costs['track30'].cost.activation.weights[:2] = 0
for m in models:
m.differential.contact['contact16'].gains[1] = 10
m.differential.contact['contact30'].gains[1] = 10
# ---------------------------------------------------------------------------------------------
problem = ShootingProblem(initialState=x0,
runningModels=models[:-1],
terminalModel=models[-1])
ddp = SolverDDP(problem)
ddp.callback = [CallbackDDPLogger(), CallbackDDPVerbose()]
if 'cb' in sys.argv and WITHDISPLAY:
ddp.callback.append(CallbackSolverDisplay(robot, rate=-1))
ddp.th_stop = 1e-6
fddp = SolverFDDP(problem)
fddp.callback = [CallbackDDPLogger(), CallbackDDPVerbose()]
if 'cb' in sys.argv and WITHDISPLAY:
fddp.callback.append(CallbackSolverDisplay(robot, rate=-1))
fddp.th_stop = 1e-6
us0 = [
m.differential.quasiStatic(d.differential, rmodel.defaultState)
if isinstance(m, IntegratedActionModelEuler) else np.zeros(0)
for m, d in zip(ddp.problem.runningModels, ddp.problem.runningDatas)
]
for m in models[imp + 1:]:
m.differential.costs['xreg'].weight = 0.0
m.differential.contact['contact16'].gains[1] = 30
m.differential.contact['contact30'].gains[1] = 30
models[-1].differential.costs['xreg'].weight = 1000
models[-1].differential.costs['xreg'].cost.activation.weights[:] = 1
# ---------------------------------------------------------------------------------------------
# Solve both take-off and landing.
# Solve the initial phase (take-off).
PHASE_NAME = "initial"
problem = ShootingProblem(initialState=x0, runningModels=models[:imp], terminalModel=models[imp])
ddp = SolverDDP(problem)
ddp.callback = [CallbackDDPLogger(), CallbackDDPVerbose()] # CallbackSolverDisplay(robot,rate=5) ]
ddp.th_stop = 1e-4
us0 = [
m.differential.quasiStatic(d.differential, rmodel.defaultState)
for m, d in list(zip(ddp.models(), ddp.datas()))[:imp]
] + [np.zeros(0)] + [
m.differential.quasiStatic(d.differential, rmodel.defaultState)
for m, d in list(zip(ddp.models(), ddp.datas()))[imp + 1:-1]
]
print("*** SOLVE %s ***" % PHASE_NAME)
ddp.solve(maxiter=PHASE_ITERATIONS[PHASE_NAME],
regInit=.1,
init_xs=[rmodel.defaultState] * len(ddp.models()),
init_us=us0[:imp])
models[-1].differential.costs['xreg'].weight = 1000
models[-1].differential.costs['xreg'].cost.activation.weights[:] = 1
# ---------------------------------------------------------------------------------------------
# ---------------------------------------------------------------------------------------------
# ---------------------------------------------------------------------------------------------
# Solve only the take-off for an initial vanilla-flavor jump
# Solve the initial phase (take-off).
PHASE_NAME = "initial"
problem = ShootingProblem(initialState=x0,
runningModels=models[:imp],
terminalModel=models[imp])
ddp = SolverDDP(problem)
ddp.alphas = [4**(-n) for n in range(10)]
ddp.callback = [CallbackDDPVerbose()]
ddp.th_stop = 1e-4
us0 = [
m.differential.quasiStatic(d.differential, rmodel.defaultState)
for m, d in list(zip(ddp.models(), ddp.datas()))[:imp]
] + [np.zeros(0)] + [
m.differential.quasiStatic(d.differential, rmodel.defaultState)
for m, d in list(zip(ddp.models(), ddp.datas()))[imp + 1:-1]
]
if PHASE_ITERATIONS[PHASE_NAME] > 0:
print("*** SOLVE %s ***" % PHASE_NAME)
ddp.solve(maxiter=PHASE_ITERATIONS[PHASE_NAME],
regInit=.1,
init_xs=[rmodel.defaultState] * len(ddp.models()),
init_us=us0[:imp])