diff --git a/examples/jump.py b/examples/jump.py
index b34d2f9000b4aed23570213c4ccc30ab4e066791..da3f5d8d054c665c73be4264eed7b8299587475d 100644
--- a/examples/jump.py
+++ b/examples/jump.py
@@ -35,6 +35,25 @@ import pinocchio
from pinocchio.utils import *
from numpy.linalg import norm,inv,pinv,eig,svd
+# Number of iterations in each phase. If 0, try to load.
+PHASE_ITERATIONS = { \
+ "initial": 100,
+ "landing": 100,
+ "frontal": 100,
+ "lateral": 100,
+ "twist" : 100
+ }
+PHASE_ITERATIONS = { k: 0 for k in PHASE_ITERATIONS }
+PHASE_BACKUP = { \
+ "initial": True,
+ "landing": True,
+ "frontal": True,
+ "lateral": True,
+ "twist" : True
+ }
+BACKUP_PATH = "npydata/jump."
+DISPLAY = True
+
robot = loadTalosLegs()
robot.model.armature[6:] = .3
robot.initDisplay(loadModel=True)
@@ -195,6 +214,8 @@ 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) ]
@@ -204,13 +225,20 @@ us0 = [ m.differential.quasiStatic(d.differential,rmodel.defaultState) \
+[np.zeros(0)]+[ m.differential.quasiStatic(d.differential,rmodel.defaultState) \
for m,d in zip(ddp.models(),ddp.datas())[imp+1:-1] ]
-ddp.solve(maxiter=20,regInit=.1,
+ddp.solve(maxiter=PHASE_ITERATIONS[PHASE_NAME],regInit=.1,
init_xs=[rmodel.defaultState]*len(ddp.models()),
init_us=us0[:imp])
-disp(ddp.xs)
+
+if PHASE_ITERATIONS[PHASE_NAME] == 0:
+ ddp.xs = [ x for x in np.load(BACKUP_PATH+'%s.xs.npy' % PHASE_NAME)]
+ ddp.us = [ u for u in np.load(BACKUP_PATH+'%s.us.npy' % PHASE_NAME)]
+elif PHASE_BACKUP[PHASE_NAME]:
+ np.save(BACKUP_PATH+'%s.xs.npy' % PHASE_NAME,ddp.xs)
+ np.save(BACKUP_PATH+'%s.us.npy' % PHASE_NAME,ddp.us)
# ---------------------------------------------------------------------------------------------
# Solve both take-off and landing.
+PHASE_NAME = "landing"
xsddp = ddp.xs
usddp = ddp.us
@@ -223,22 +251,31 @@ ddp.us = usddp + [ np.zeros(0) if isinstance(m,ActionModelImpact) else \
m.differential.quasiStatic(d.differential,rmodel.defaultState) \
for m,d in zip(ddp.models(),ddp.datas())[len(usddp):-1] ]
-ddp.th_stop = 1e-1
-ddp.solve(init_xs=ddp.xs,init_us=ddp.us)
-
+#ddp.th_stop = 1e-1
+#ddp.solve(init_xs=ddp.xs,init_us=ddp.us,maxiter=)
ddp.th_stop = 5e-4
impact.costs['track30'].weight = 1e6
impact.costs['track16'].weight = 1e6
-ddp.solve(init_xs=ddp.xs,init_us=ddp.us,maxiter=100,isFeasible=True)
-disp(ddp.xs)
+ddp.solve(init_xs=ddp.xs,init_us=ddp.us,maxiter=PHASE_ITERATIONS[PHASE_NAME],isFeasible=True)
+
+if PHASE_ITERATIONS[PHASE_NAME] == 0:
+ ddp.xs = [ x for x in np.load(BACKUP_PATH+'%s.xs.npy' % PHASE_NAME)]
+ ddp.us = [ u for u in np.load(BACKUP_PATH+'%s.us.npy' % PHASE_NAME)]
+elif PHASE_BACKUP[PHASE_NAME]:
+ np.save(BACKUP_PATH+'%s.xs.npy' % PHASE_NAME,ddp.xs)
+ np.save(BACKUP_PATH+'%s.us.npy' % PHASE_NAME,ddp.us)
+if DISPLAY: disp(ddp.xs)
+# Save for future use in initialization of advanced jumps
xjump0 = [ x.copy() for x in ddp.xs ]
ujump0 = [ u.copy() for u in ddp.us ]
-'''
+# ---------------------------------------------------------------------------------------------
### Jump with frontal scissors.
+PHASE_NAME = "frontal"
+
fig = high+2
x = x0.copy()
x[9 ] = 1.
@@ -250,15 +287,24 @@ for i in range(6,9):
impact.costs['track30'].weight = 10**i
impact.costs['track16'].weight = 10**i
models[fig].differential.costs.costs['xreg'].weight = 10**i
- ddp.solve(init_xs=ddp.xs,init_us=ddp.us,maxiter=100,isFeasible=True)
- disp(ddp.xs)
+ ddp.solve(init_xs=ddp.xs,init_us=ddp.us,maxiter=PHASE_ITERATIONS[PHASE_NAME],isFeasible=True)
+
+if PHASE_ITERATIONS[PHASE_NAME] == 0:
+ ddp.xs = [ x for x in np.load(BACKUP_PATH+'%s.xs.npy' % PHASE_NAME)]
+ ddp.us = [ u for u in np.load(BACKUP_PATH+'%s.us.npy' % PHASE_NAME)]
+elif PHASE_BACKUP[PHASE_NAME]:
+ np.save(BACKUP_PATH+'%s.xs.npy' % PHASE_NAME,ddp.xs)
+ np.save(BACKUP_PATH+'%s.us.npy' % PHASE_NAME,ddp.us)
+
+if DISPLAY: disp(ddp.xs)
+
+# ---------------------------------------------------------------------------------------------
+### Jump with lateral scissors.
+PHASE_NAME = "lateral"
-xjump1 = [ x.copy() for x in ddp.xs ]
-ujump1 = [ u.copy() for u in ddp.us ]
ddp.xs = xjump0
ddp.us = ujump0
-### Jump with lateral scissors.
fig = high+2
x = x0.copy()
x[8 ] = .8
@@ -266,100 +312,44 @@ x[14] = -.8
models[fig].differential.costs.costs['xreg'].cost.ref=x.copy()
models[fig].differential.costs.costs['xreg'].cost.activation.weights[rmodel.nv:] = 0
-for i in range(6,9):
- impact.costs['track30'].weight = 10**i
- impact.costs['track16'].weight = 10**i
- models[fig].differential.costs.costs['xreg'].weight = 10**i
- ddp.solve(init_xs=ddp.xs,init_us=ddp.us,maxiter=100,isFeasible=True)
- disp(ddp.xs)
-
-xjump2 = [ x.copy() for x in ddp.xs ]
-ujump2 = [ u.copy() for u in ddp.us ]
-ddp.xs = xjump0
-ddp.us = ujump0
+impact.costs['track30'].weight = 10**6
+impact.costs['track16'].weight = 10**6
+models[fig].differential.costs.costs['xreg'].weight = 10**6
+ddp.solve(init_xs=ddp.xs,init_us=ddp.us,maxiter=PHASE_ITERATIONS[PHASE_NAME],isFeasible=True)
+
+if PHASE_ITERATIONS[PHASE_NAME] == 0:
+ ddp.xs = [ x for x in np.load(BACKUP_PATH+'%s.xs.npy' % PHASE_NAME)]
+ ddp.us = [ u for u in np.load(BACKUP_PATH+'%s.us.npy' % PHASE_NAME)]
+elif PHASE_BACKUP[PHASE_NAME]:
+ np.save(BACKUP_PATH+'%s.xs.npy' % PHASE_NAME,ddp.xs)
+ np.save(BACKUP_PATH+'%s.us.npy' % PHASE_NAME,ddp.us)
+if DISPLAY: disp(ddp.xs)
-models[fig].differential.costs.costs['xreg'].weight = 100
+models[fig].differential.costs.costs['xreg'].weight = 0.1
models[fig].differential.costs.costs['xreg'].cost.ref=x0.copy()
models[fig].differential.costs.costs['xreg'].cost.activation.weights[rmodel.nv:] = 10
-'''
-'''
+# ---------------------------------------------------------------------------------------------
### Jump with twist PI/2
+PHASE_NAME = "twist"
+
+ddp.xs = xjump0
+ddp.us = ujump0
+
impact.costs['track16'].cost.ref = SE3(rotate('z',1.5),zero(3))*impact.costs['track16'].cost.ref
impact.costs['track30'].cost.ref = SE3(rotate('z',1.5),zero(3))*impact.costs['track30'].cost.ref
models[-1].differential.costs.costs['xreg'].cost.activation.weights[5] = 0
-for i in range(4,9):
+for i in range(6,7):
impact.costs['track30'].weight = 10**i
impact.costs['track16'].weight = 10**i
- ddp.solve(init_xs=ddp.xs,init_us=ddp.us,maxiter=100,isFeasible=True)
- disp(ddp.xs)
-
-xjump3 = [ x.copy() for x in ddp.xs ]
-ujump3 = [ u.copy() for u in ddp.us ]
-#ddp.xs = xjump0
-#ddp.us = ujump0
-
-### Jump with twist PI
-impact.costs['track16'].cost.ref = SE3(rotate('z',3.15),zero(3))*impact.costs['track16'].cost.ref
-impact.costs['track30'].cost.ref = SE3(rotate('z',3.15),zero(3))*impact.costs['track30'].cost.ref
-models[-1].differential.costs.costs['xreg'].cost.activation.weights[5] = 0
+ ddp.solve(init_xs=ddp.xs,init_us=ddp.us,maxiter=PHASE_ITERATIONS[PHASE_NAME],isFeasible=True)
-for i in range(4,9):
- impact.costs['track30'].weight = 10**i
- impact.costs['track16'].weight = 10**i
- ddp.solve(init_xs=ddp.xs,init_us=ddp.us,maxiter=100,isFeasible=True)
- disp(ddp.xs)
-
-xjump4 = [ x.copy() for x in ddp.xs ]
-ujump4 = [ u.copy() for u in ddp.us ]
-ddp.xs = xjump0
-ddp.us = ujump0
+if PHASE_ITERATIONS[PHASE_NAME] == 0:
+ ddp.xs = [ x for x in np.load(BACKUP_PATH+'%s.xs.npy' % PHASE_NAME)]
+ ddp.us = [ u for u in np.load(BACKUP_PATH+'%s.us.npy' % PHASE_NAME)]
+elif PHASE_BACKUP[PHASE_NAME]:
+ np.save(BACKUP_PATH+'%s.xs.npy' % PHASE_NAME,ddp.xs)
+ np.save(BACKUP_PATH+'%s.us.npy' % PHASE_NAME,ddp.us)
+if DISPLAY: disp(ddp.xs)
-'''
-
-### Salto!
-impact.costs['track16'].cost.ref = SE3(eye(3),left0)
-impact.costs['track30'].cost.ref = SE3(eye(3),right0)
-models[-1].differential.costs.costs['xreg'].cost.activation.weights[5] = 1
-
-xsalto = models[high].differential.costs.costs['xreg'].cost.ref.copy()
-xsalto[3]=1
-xsalto[6]=0
-models[high].differential.costs.costs['xreg'].cost.ref = xsalto
-models[high].differential.costs.costs['xreg'].cost.activation.weights[3:5] = 100
-
-impact.costs['track30'].weight = 0.1
-impact.costs['track16'].weight = 0.1
-ddp.xs = [x.copy() for x in xjump0]
-ddp.us = [u.copy() for u in ujump0]
-
-# from pinocchio.utils import se3ToXYZQUAT
-# D = 2*np.pi/(imp-9)
-# for i,x in enumerate(ddp.xs[9:imp]):
-# '''
-# oM1 1_p
-# p st [ R p ] 0^c = c = R c + p => p = c-Rc
-# '''
-# q = a2m(x)[:rmodel.nq]
-# pinocchio.centerOfMass(rmodel,rdata,q)
-# R = rotate('y',D*i)
-# c = rdata.com[0].copy()
-# p = c-R*c
-# M = SE3(R,p)
-# x[:7] = se3ToXYZQUAT(M*rdata.oMi[1])
-
-
-
-
-for i in range(1,9):
- # impact.costs['track30'].weight = 10**i
- # impact.costs['track16'].weight = 10**i
- models[high].differential.costs.costs['xreg'].weight = 10**i
- ddp.solve(init_xs=ddp.xs,init_us=ddp.us,maxiter=100,isFeasible=False)
- disp(ddp.xs)
-
-xjump5 = [ x.copy() for x in ddp.xs ]
-ujump5 = [ u.copy() for u in ddp.us ]
-ddp.xs = xjump0
-ddp.us = ujump0
diff --git a/examples/npydata/jump.frontal.us.npy b/examples/npydata/jump.frontal.us.npy
new file mode 100644
index 0000000000000000000000000000000000000000..07b4e75a465f286f3b8afa198d1acec4a0abe130
Binary files /dev/null and b/examples/npydata/jump.frontal.us.npy differ
diff --git a/examples/npydata/jump.frontal.xs.npy b/examples/npydata/jump.frontal.xs.npy
new file mode 100644
index 0000000000000000000000000000000000000000..fb237217df0b653b7070655ab3848bb5d3884173
Binary files /dev/null and b/examples/npydata/jump.frontal.xs.npy differ
diff --git a/examples/npydata/jump.initial.us.npy b/examples/npydata/jump.initial.us.npy
new file mode 100644
index 0000000000000000000000000000000000000000..471a214638ed4b7e459f26819831c3e988d295aa
Binary files /dev/null and b/examples/npydata/jump.initial.us.npy differ
diff --git a/examples/npydata/jump.initial.xs.npy b/examples/npydata/jump.initial.xs.npy
new file mode 100644
index 0000000000000000000000000000000000000000..d0b987f2bf3fc78fc6622c1272e3291124398cab
Binary files /dev/null and b/examples/npydata/jump.initial.xs.npy differ
diff --git a/examples/npydata/jump.landing.us.npy b/examples/npydata/jump.landing.us.npy
new file mode 100644
index 0000000000000000000000000000000000000000..baedbe399b5c8e8d2c1a20da5f0bd78bf61272ce
Binary files /dev/null and b/examples/npydata/jump.landing.us.npy differ
diff --git a/examples/npydata/jump.landing.xs.npy b/examples/npydata/jump.landing.xs.npy
new file mode 100644
index 0000000000000000000000000000000000000000..8eaaf9a3a9d5bd41f479b1601541a45e176cc96e
Binary files /dev/null and b/examples/npydata/jump.landing.xs.npy differ
diff --git a/examples/npydata/jump.lateral.us.npy b/examples/npydata/jump.lateral.us.npy
new file mode 100644
index 0000000000000000000000000000000000000000..45a861a605868713ce4c62e0cd3dcd5d698177b7
Binary files /dev/null and b/examples/npydata/jump.lateral.us.npy differ
diff --git a/examples/npydata/jump.lateral.xs.npy b/examples/npydata/jump.lateral.xs.npy
new file mode 100644
index 0000000000000000000000000000000000000000..bb11d4b5a9b419e11f0c7d0d371883c651d34b1b
Binary files /dev/null and b/examples/npydata/jump.lateral.xs.npy differ
diff --git a/examples/npydata/jump.twist.us.npy b/examples/npydata/jump.twist.us.npy
new file mode 100644
index 0000000000000000000000000000000000000000..abf49a23a2205a43d25fe58edce2dcffdbe46b0f
Binary files /dev/null and b/examples/npydata/jump.twist.us.npy differ
diff --git a/examples/npydata/jump.twist.xs.npy b/examples/npydata/jump.twist.xs.npy
new file mode 100644
index 0000000000000000000000000000000000000000..2834bae5e9cca61dc175d3efa0df7af0981021f2
Binary files /dev/null and b/examples/npydata/jump.twist.xs.npy differ
diff --git a/examples/salto.py b/examples/salto.py
index 936a31c5f8aff9be79439bbe3d0b68e12968c314..a7a546533cb3137251e6e9802fc314ea1e5a1cc4 100644
--- a/examples/salto.py
+++ b/examples/salto.py
@@ -35,7 +35,7 @@ from numpy.linalg import norm,inv,pinv,eig,svd
PHASE_ITERATIONS = { \
"initial": 0,
"angle": 0,
- "landing": 100
+ "landing": 0
}
PHASE_BACKUP = { "initial": True,
"angle": True,
@@ -222,7 +222,7 @@ ddp.solve(maxiter=PHASE_ITERATIONS[PHASE_NAME],regInit=.1,
if PHASE_ITERATIONS[PHASE_NAME] == 0:
ddp.xs = [ x for x in np.load(BACKUP_PATH+'%s.xs.npy' % PHASE_NAME)]
ddp.us = [ u for u in np.load(BACKUP_PATH+'%s.us.npy' % PHASE_NAME)]
-elif PHASE_BACKUP["initial"]:
+elif PHASE_BACKUP[PHASE_NAME]:
np.save(BACKUP_PATH+'%s.xs.npy' % PHASE_NAME,ddp.xs)
np.save(BACKUP_PATH+'%s.us.npy' % PHASE_NAME,ddp.us)