diff --git a/examples/__init__.py b/examples/__init__.py
deleted file mode 100644
index b54fde3838b96b76f79c151b225069ff4b9bf380..0000000000000000000000000000000000000000
--- a/examples/__init__.py
+++ /dev/null
@@ -1 +0,0 @@
-from examples import *
diff --git a/examples/bipedal_walking_from_foot_traj.py b/examples/bipedal_walking_from_foot_traj.py
index fcffad36204428d449c8b8bac0c874e844ef6e9b..d5934d081d16b9831650e4e706823f20119fee4b 100644
--- a/examples/bipedal_walking_from_foot_traj.py
+++ b/examples/bipedal_walking_from_foot_traj.py
@@ -3,7 +3,10 @@ import numpy as np
from numpy.linalg import norm
import pinocchio
from pinocchio.utils import *
+import sys
+WITHDISPLAY = 'disp' in sys.argv
+WITHPLOT = 'plot' in sys.argv
class TaskSE3:
def __init__(self, oXf, frameId):
@@ -209,19 +212,22 @@ walkProblem = walk.createProblem(x0, stepLength, timeStep, stepKnots, supportKno
# Solving the 3d walking problem using DDP
ddp = SolverDDP(walkProblem)
cameraTF = [3., 3.68, 0.84, 0.2, 0.62, 0.72, 0.22]
-ddp.callback = [CallbackDDPLogger(), CallbackDDPVerbose(),
- CallbackSolverDisplay(talos_legs,4,1,cameraTF)]
+ddp.callback = [ CallbackDDPVerbose() ]
+if WITHPLOT: ddp.callback.append(CallbackDDPLogger())
+if WITHDISPLAY: ddp.callback.append(CallbackSolverDisplay(talos_legs,4,1,cameraTF))
ddp.th_stop = 1e-9
ddp.solve(maxiter=1000,regInit=.1,init_xs=[talos_legs.model.defaultState]*len(ddp.models()))
# Plotting the solution and the DDP convergence
-log = ddp.callback[0]
-plotOCSolution(log.xs, log.us)
-plotDDPConvergence(log.costs,log.control_regs,
- log.state_regs,log.gm_stops,
- log.th_stops,log.steps)
+if WITHPLOT:
+ log = ddp.callback[0]
+ plotOCSolution(log.xs, log.us)
+ plotDDPConvergence(log.costs,log.control_regs,
+ log.state_regs,log.gm_stops,
+ log.th_stops,log.steps)
# Visualization of the DDP solution in gepetto-viewer
-ddp.callback[2](ddp)
-CallbackSolverDisplay(talos_legs)(ddp)
\ No newline at end of file
+if WITHDISPLAY:
+ ddp.callback[2](ddp)
+ CallbackSolverDisplay(talos_legs)(ddp)
diff --git a/examples/jump.py b/examples/jump.py
index da3f5d8d054c665c73be4264eed7b8299587475d..0a56b782dee74d881ff30151ee4b5c460711ecc1 100644
--- a/examples/jump.py
+++ b/examples/jump.py
@@ -34,29 +34,32 @@ from crocoddyl.impact import CostModelImpactCoM, CostModelImpactWholeBody
import pinocchio
from pinocchio.utils import *
from numpy.linalg import norm,inv,pinv,eig,svd
+import sys
# 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 }
+ "initial": 200,
+ "landing": 200,
+ "frontal": 200,
+ "lateral": 200,
+ "twist" : 200
+}
PHASE_BACKUP = { \
- "initial": True,
- "landing": True,
- "frontal": True,
- "lateral": True,
- "twist" : True
- }
+ "initial": False,
+ "landing": False,
+ "frontal": False,
+ "lateral": False,
+ "twist" : False
+}
BACKUP_PATH = "npydata/jump."
-DISPLAY = True
+
+if 'load' in sys.argv: PHASE_ITERATIONS = { k: 0 for k in PHASE_ITERATIONS }
+if 'save' in sys.argv: PHASE_BACKUP = { k: True for k in PHASE_ITERATIONS }
+WITHDISPLAY = 'disp' in sys.argv
robot = loadTalosLegs()
robot.model.armature[6:] = .3
-robot.initDisplay(loadModel=True)
+if WITHDISPLAY: robot.initDisplay(loadModel=True)
rmodel = robot.model
rdata = rmodel.createData()
@@ -80,7 +83,8 @@ swingDuration = 0.75
stanceDurantion = 0.1
from crocoddyl.diagnostic import displayTrajectory
-disp = lambda xs,dt: displayTrajectory(robot,xs,dt)
+dodisp = lambda xs,dt: displayTrajectory(robot,xs,dt)
+disp = dodisp if WITHDISPLAY else lambda xs,dt: 0
disp.__defaults__ = ( .1, )
def runningModel(contactIds, effectors, com=None, integrationStep = 1e-2):
@@ -126,20 +130,6 @@ def runningModel(contactIds, effectors, com=None, integrationStep = 1e-2):
model.timeStep = integrationStep
return model
-def pseudoImpactModel(contactIds,effectors):
- #assert(len(effectors)==1)
- model = runningModel(contactIds,effectors,integrationStep=0)
-
- costModel = model.differential.costs
- for fid,ref in effectors.items():
- costModel.addCost('impactVel%d' % fid,weight = 100.,
- cost=CostModelFrameVelocity(rmodel,fid))
- costModel.costs['track%d'%fid ].weight = 100
- costModel.costs['xreg'].weight = 1
- costModel.costs['ureg'].weight = 0.01
-
- return model
-
def impactModel(contactIds,effectors):
State = StatePinocchio(rmodel)
@@ -210,7 +200,6 @@ for m in models[imp+1:]:
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).
@@ -225,6 +214,7 @@ 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] ]
+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])
@@ -250,13 +240,11 @@ ddp.xs = xsddp + [rmodel.defaultState]*(len(models)-len(xsddp))
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,maxiter=)
-
ddp.th_stop = 5e-4
impact.costs['track30'].weight = 1e6
impact.costs['track16'].weight = 1e6
+
+print("*** SOLVE %s ***" % PHASE_NAME)
ddp.solve(init_xs=ddp.xs,init_us=ddp.us,maxiter=PHASE_ITERATIONS[PHASE_NAME],isFeasible=True)
if PHASE_ITERATIONS[PHASE_NAME] == 0:
@@ -266,7 +254,7 @@ 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)
+disp(ddp.xs)
# Save for future use in initialization of advanced jumps
xjump0 = [ x.copy() for x in ddp.xs ]
@@ -283,11 +271,11 @@ x[15] = -1.
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=PHASE_ITERATIONS[PHASE_NAME],isFeasible=True)
+print("*** SOLVE %s ***" % PHASE_NAME)
+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)]
@@ -296,7 +284,7 @@ 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)
+disp(ddp.xs)
# ---------------------------------------------------------------------------------------------
### Jump with lateral scissors.
@@ -315,6 +303,8 @@ models[fig].differential.costs.costs['xreg'].cost.activation.weights[rmodel.nv:]
impact.costs['track30'].weight = 10**6
impact.costs['track16'].weight = 10**6
models[fig].differential.costs.costs['xreg'].weight = 10**6
+
+print("*** SOLVE %s ***" % PHASE_NAME)
ddp.solve(init_xs=ddp.xs,init_us=ddp.us,maxiter=PHASE_ITERATIONS[PHASE_NAME],isFeasible=True)
if PHASE_ITERATIONS[PHASE_NAME] == 0:
@@ -323,7 +313,7 @@ if PHASE_ITERATIONS[PHASE_NAME] == 0:
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)
+disp(ddp.xs)
models[fig].differential.costs.costs['xreg'].weight = 0.1
models[fig].differential.costs.costs['xreg'].cost.ref=x0.copy()
@@ -340,10 +330,10 @@ impact.costs['track16'].cost.ref = SE3(rotate('z',1.5),zero(3))*impact.costs['tr
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(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=PHASE_ITERATIONS[PHASE_NAME],isFeasible=True)
+impact.costs['track30'].weight = 10**6
+impact.costs['track16'].weight = 10**6
+print("*** SOLVE %s ***" % PHASE_NAME)
+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)]
@@ -351,5 +341,5 @@ if PHASE_ITERATIONS[PHASE_NAME] == 0:
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)
+disp(ddp.xs)
diff --git a/examples/salto.py b/examples/salto.py
index a7a546533cb3137251e6e9802fc314ea1e5a1cc4..15133d4ea0e3c2c8c0df36234abf1bfbc3ec293d 100644
--- a/examples/salto.py
+++ b/examples/salto.py
@@ -30,22 +30,26 @@ from crocoddyl.impact import CostModelImpactCoM, CostModelImpactWholeBody
import pinocchio
from pinocchio.utils import *
from numpy.linalg import norm,inv,pinv,eig,svd
+import sys
# Number of iterations in each phase. If 0, try to load.
PHASE_ITERATIONS = { \
- "initial": 0,
- "angle": 0,
- "landing": 0
+ "initial": 200,
+ "angle": 200,
+ "landing": 200
}
-PHASE_BACKUP = { "initial": True,
- "angle": True,
- "landing": True }
+PHASE_BACKUP = { "initial": False,
+ "angle": False,
+ "landing": False }
BACKUP_PATH = "npydata/salto."
+if 'load' in sys.argv: PHASE_ITERATIONS = { k: 0 for k in PHASE_ITERATIONS }
+if 'save' in sys.argv: PHASE_BACKUP = { k: True for k in PHASE_ITERATIONS }
+WITHDISPLAY = 'disp' in sys.argv
robot = loadTalosLegs()
robot.model.armature[6:] = .3
-robot.initDisplay(loadModel=True)
+if WITHDISPLAY: robot.initDisplay(loadModel=True)
rmodel = robot.model
rdata = rmodel.createData()
@@ -69,7 +73,8 @@ swingDuration = 0.75
stanceDurantion = 0.1
from crocoddyl.diagnostic import displayTrajectory
-disp = lambda xs,dt: displayTrajectory(robot,xs,dt)
+dodisp = lambda xs,dt: displayTrajectory(robot,xs,dt)
+disp = dodisp if WITHDISPLAY else lambda xs,dt: 0
disp.__defaults__ = ( .1, )
def runningModel(contactIds, effectors, com=None, integrationStep = 1e-2):
@@ -207,17 +212,17 @@ models[-1].differential.costs['xreg'].cost.activation.weights[:] = 1
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.callback = [ CallbackDDPVerbose() ]
ddp.th_stop = 1e-4
us0 = [ m.differential.quasiStatic(d.differential,rmodel.defaultState) \
for m,d in zip(ddp.models(),ddp.datas())[:imp] ] \
+[np.zeros(0)]+[ m.differential.quasiStatic(d.differential,rmodel.defaultState) \
for m,d in 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])
-#disp(ddp.xs)
if PHASE_ITERATIONS[PHASE_NAME] == 0:
ddp.xs = [ x for x in np.load(BACKUP_PATH+'%s.xs.npy' % PHASE_NAME)]
@@ -230,33 +235,14 @@ elif PHASE_BACKUP[PHASE_NAME]:
# Second phase of the search: optimize for increasing terminal angle of the waist.
PHASE_NAME = "angle"
-#for i,m in enumerate(models[9:imp]):
-# m.differential.costs['xreg'].weight = 10
-# m.differential.costs['xreg'].cost.ref = refs[i]
-# m.differential.costs['xreg'].cost.activation.weights[3:6] = 10
models[imp].costs['xreg'].cost.activation.weights[3:6] = 100
+impact.costs['xreg'].cost.activation.weights[3:rmodel.nv] = 10**6
+ddp.th_stop = 5e-3
-ANG = .5
-cos,sin = np.cos,np.sin
-models[imp].costs['xreg'].cost.ref[3:7] = [ 0,sin(ANG),0,cos(ANG) ]
-
-ddp.callback.append(CallbackSolverDisplay(robot,rate=5,freq=10))
-for i in range(1,7):
- #for k in range(9,imp):
- impact.costs['xreg'].cost.activation.weights[3:rmodel.nv] = 10**i
- ddp.solve(maxiter=PHASE_ITERATIONS[PHASE_NAME],regInit=.1, #!2000
- init_xs=ddp.xs,
- init_us=ddp.us, isFeasible=False)
-
-# for i,m in enumerate(models[9:imp]):
-# m.differential.costs['xreg'].cost.activation.weights[7] = 10
-# m.differential.costs['xreg'].cost.activation.weights[13] = 10
-# models[high].differential.costs['xreg'].cost.activation.weights[7] = 100
-# models[high].differential.costs['xreg'].cost.activation.weights[13] = 100
-
for ANG in np.arange(.5,3.2,.3):
- models[imp].costs['xreg'].cost.ref[3:7] = [ 0,sin(ANG),0,cos(ANG) ]
- ddp.solve(maxiter=PHASE_ITERATIONS[PHASE_NAME],regInit=.1, #!2000
+ models[imp].costs['xreg'].cost.ref[3:7] = [ 0,np.sin(ANG),0,np.cos(ANG) ]
+ if PHASE_ITERATIONS[PHASE_NAME]>0: print("*** SOLVE %s ang=%.1f ***" % (PHASE_NAME,ANG))
+ ddp.solve(maxiter=PHASE_ITERATIONS[PHASE_NAME],regInit=.1,
init_xs=ddp.xs,
init_us=ddp.us, isFeasible=True)
if PHASE_ITERATIONS[PHASE_NAME] > 0 and PHASE_BACKUP[PHASE_NAME]:
@@ -279,16 +265,17 @@ usddp = ddp.us
problem = ShootingProblem(initialState=x0,runningModels=models[:-1],terminalModel=models[-1])
ddp = SolverDDP(problem)
-ddp.callback = [ CallbackDDPLogger(), CallbackDDPVerbose() ]#, CallbackSolverDisplay(robot,rate=5,freq=10) ]
+ddp.callback = [ CallbackDDPVerbose() ]
ddp.th_stop = 1e-4
ddp.xs = xsddp + [rmodel.defaultState]*(len(models)-len(xsddp))
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] ]
-
impact.costs['track30'].weight = 1e6
impact.costs['track16'].weight = 1e6
+
+if PHASE_ITERATIONS[PHASE_NAME]>0: print("*** SOLVE %s ***" % PHASE_NAME)
ddp.solve(init_xs=ddp.xs,init_us=ddp.us,maxiter=PHASE_ITERATIONS[PHASE_NAME])
if PHASE_ITERATIONS[PHASE_NAME] == 0:
diff --git a/examples/talos_arm.py b/examples/talos_arm.py
index 07b6dd47a5f74057c56c9bc64b1fa66249b4f798..2a70aad888498a64e723493677a6db90d8561f97 100644
--- a/examples/talos_arm.py
+++ b/examples/talos_arm.py
@@ -1,8 +1,10 @@
from crocoddyl import *
import pinocchio
import numpy as np
+import sys
-
+WITHDISPLAY = 'disp' in sys.argv
+WITHPLOT = 'plot' in sys.argv
# In this example test, we will solve the reaching-goal task with the Talos arm.
# For that, we use the forward dynamics (with its analytical derivatives)
@@ -64,27 +66,27 @@ problem = ShootingProblem(x0, [ runningModel ]*T, terminalModel)
# Creating the DDP solver for this OC problem, defining a logger
ddp = SolverDDP(problem)
cameraTF = [2., 2.68, 0.54, 0.2, 0.62, 0.72, 0.22]
-ddp.callback = [CallbackDDPLogger(), CallbackDDPVerbose(1), CallbackSolverDisplay(robot,4,1,cameraTF)]
+ddp.callback = [ CallbackDDPVerbose() ]
+if WITHPLOT: ddp.callback.append(CallbackDDPLogger())
+if WITHDISPLAY: ddp.callback.append(CallbackSolverDisplay(talos_legs,4,1,cameraTF))
# Solving it with the DDP algorithm
ddp.solve()
-
# Plotting the solution and the DDP convergence
-log = ddp.callback[0]
-plotOCSolution(log.xs, log.us)
-plotDDPConvergence(log.costs,log.control_regs,
- log.state_regs,log.gm_stops,
- log.th_stops,log.steps)
-
+if WITHPLOT:
+ log = ddp.callback[0]
+ plotOCSolution(log.xs, log.us)
+ plotDDPConvergence(log.costs,log.control_regs,
+ log.state_regs,log.gm_stops,
+ log.th_stops,log.steps)
# Visualizing the solution in gepetto-viewer
-CallbackSolverDisplay(robot)(ddp)
+if WITHDISPLAY: CallbackSolverDisplay(robot)(ddp)
# Printing the reached position
frame_idx = robot.model.getFrameId(frameName)
-xT = log.xs[-1]
-qT = np.asmatrix(xT[:robot.model.nq]).T
print
print "The reached pose by the wrist is"
-print robot.framePlacement(qT, frame_idx)
\ No newline at end of file
+print ddp.datas()[-1].differential.pinocchio.oMf[frame_idx]
+