x0 = m2a(np.concatenate([q0, v0]))
# Setting up the 3d walking problem
lfFoot = 'lf_foot'
rfFoot = 'rf_foot'
lhFoot = 'lh_foot'
rhFoot = 'rh_foot'
walk = SimpleQuadrupedProblem(rmodel, lfFoot, rfFoot, lhFoot, rhFoot)
# Setting up the walking variables
comGoTo = 0.1 # meters
timeStep = 5e-2 # seconds
supportKnots = 2
# Creating the CoM problem and solving it
ddp = SolverDDP(walk.createProblem(x0, comGoTo, timeStep, supportKnots))
# ddp.callback = [ CallbackDDPVerbose() ]
ddp.th_stop = 1e-9
ddp.solve(maxiter=1000,
regInit=.1,
init_xs=[rmodel.defaultState] * len(ddp.models()))
# Checking the CoM displacement
x0 = ddp.xs[0]
xT = ddp.xs[-1]
q0 = a2m(x0[:rmodel.nq])
qT = a2m(xT[:rmodel.nq])
data0 = rmodel.createData()
dataT = rmodel.createData()
comT = pinocchio.centerOfMass(rmodel, dataT, qT)
com0 = pinocchio.centerOfMass(rmodel, data0, q0)
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)
# ----------------------
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)
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])
# m.calc(d,ddp.xs[0])
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]
u0s = [u0.copy()]
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)
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])
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()
xddp, uddp, costddp = ddp.forwardPass(stepLength=1)
# The argmin value is barely within the limits of the control (one dimension is on limit)
ddpbox.ul = np.array([
min(np.minimum(uddp[0], min(us[0]))),
] * nu)
ddpbox.uu = np.array([
max(np.maximum(uddp[0], max(us[0]))),
] * nu)
# ddpbox.ul = np.array([-3.,]*nu)
# ddpbox.uu = np.array([3.,]*nu)
if WITH_PLOT:
import matplotlib.pyplot as plt
for x in xs:
disp(x)
ax = max(np.concatenate([(abs(x[0]), abs(x[1])) for x in xs])) * 1.2
plt.axis([-ax, ax, -ax, ax])
plt.show()
model = ActionModelLQR(1, 1, driftFree=False)
data = model.createData()
# model = ActionModelUnicycle()
nx, nu = model.nx, model.nu
problem = ShootingProblem(model.State.zero() + 1, [model], model)
ddp = SolverDDP(problem)
xs = [m.State.zero() for m in problem.runningModels + [problem.terminalModel]]
us = [np.zeros(m.nu) for m in problem.runningModels]
# xs[0][:] = problem.initialState
xs[0] = np.random.rand(nx)
us[0] = np.random.rand(nu)
xs[1] = np.random.rand(nx)
ddp.setCandidate(xs, us)
ddp.computeDirection()
xnew, unew, cnew = ddp.forwardPass(stepLength=1)
# Check versus simple (1-step) DDP
ddp.problem.calcDiff(xs, us)
l0x = problem.runningDatas[0].Lx
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)
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 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,