From c79b9b0b8ca8120c20d5f3da071d7306a9dde6c7 Mon Sep 17 00:00:00 2001
From: Rohan Budhiraja <proyan@users.noreply.github.com>
Date: Tue, 19 Feb 2019 00:13:40 +0100
Subject: [PATCH] [locomotion] talos warm start add flag for display and add
callbacktime
---
locomotion/talos_warm_start.py | 15 ++++++++++++---
1 file changed, 12 insertions(+), 3 deletions(-)
diff --git a/locomotion/talos_warm_start.py b/locomotion/talos_warm_start.py
index 5a32ed41..646ac226 100644
--- a/locomotion/talos_warm_start.py
+++ b/locomotion/talos_warm_start.py
@@ -6,7 +6,7 @@ from centroidal_utils import createPhiFromContactSequence, createMultiphaseShoot
from crocoddyl import m2a, a2m
from crocoddyl import ShootingProblem, SolverDDP
from crocoddyl import ActionModelImpact
-from crocoddyl import CallbackDDPVerbose, CallbackSolverDisplay, CallbackDDPLogger
+from crocoddyl import CallbackDDPVerbose, CallbackSolverDisplay, CallbackDDPLogger, CallbackSolverTimer
import numpy as np
np.set_printoptions(linewidth=400, suppress=True)
robot = conf.robot
@@ -61,14 +61,23 @@ assert(i==len(conf.U_init))
assert(i==len(conf.X_init)-1)
init.X.append(conf.X_init[-1])
#---------------Display Initial Trajectory--------------
-if conf.DISPLAY and False:
+if conf.DISPLAY:
robot.initDisplay(loadModel=True)
+if conf.DISPLAY and False:
for x in init.X:
robot.display(a2m(x[:robot.nq]))
#sleep(0.005)
#----------------------
ddp = SolverDDP(problem)
-ddp.callback = [CallbackDDPVerbose(), CallbackSolverDisplay(robot,4)]
+ddp.callback = [CallbackDDPVerbose(), CallbackSolverTimer()]
+if conf.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)
+#----------------------
--
GitLab