Merge pull request #745 from cmastalli/topic/fix-notebook

Fix few backup scripts inside notebook folder

examples/notebooks/arm_example.py

@@ -7,10 +7,11 @@
 robot_model = robot.model
 
 DT = 1e-3
-T = 25
+T = 250
 target = np.array([0.4, 0., .4])
 
-robot.initViewer(loadModel=True)
+cameraTF = [2., 2.68, 0.54, 0.2, 0.62, 0.72, 0.22]
+display = crocoddyl.GepettoDisplay(robot, cameraTF=cameraTF, floor=False)
 robot.viewer.gui.addSphere('world/point', .05, [1., 0., 0., 1.])  # radius = .1, RGBA=1001
 robot.viewer.gui.applyConfiguration('world/point', target.tolist() + [0., 0., 0., 1.])  # xyz+quaternion
 robot.viewer.gui.refresh()
@@ -28,11 +29,11 @@
 
 # Then let's added the running and terminal cost functions
 runningCostModel.addCost("gripperPose", goalTrackingCost, 1.)
-runningCostModel.addCost("stateReg", xRegCost, 1e-4)
-runningCostModel.addCost("ctrlReg", uRegCost, 1e-7)
-terminalCostModel.addCost("gripperPose", goalTrackingCost, 1000.)
-terminalCostModel.addCost("stateReg", xRegCost, 1e-4)
-terminalCostModel.addCost("ctrlReg", uRegCost, 1e-7)
+runningCostModel.addCost("stateReg", xRegCost, 5e-2)
+runningCostModel.addCost("ctrlReg", uRegCost, 1e-5)
+terminalCostModel.addCost("gripperPose", goalTrackingCost, 10000.)
+terminalCostModel.addCost("stateReg", xRegCost, 5e-2)
+terminalCostModel.addCost("ctrlReg", uRegCost, 1e-5)
 
 # Create the action model
 actuation = crocoddyl.ActuationModelFull(state)
@@ -44,7 +45,7 @@
 #terminalModel.differential.armature = 0.2 * np.matrix(np.ones(state.nv)).T
 
 # Create the problem
-q0 = np.matrix([2., 1.5, -2., 0., 0., 0., 0.]).T
+q0 = np.array([2., 1.5, -2., 0., 0., 0., 0.]).T
 x0 = np.concatenate([q0, pinocchio.utils.zero(state.nv)])
 problem = crocoddyl.ShootingProblem(x0, [runningModel] * T, terminalModel)
 
@@ -56,7 +57,7 @@
 ddp.solve()
 
 # Visualizing the solution in gepetto-viewer
-crocoddyl.displayTrajectory(robot, ddp.xs, runningModel.dt)
+display.displayFromSolver(ddp)
 
 robot_data = robot_model.createData()
 xT = ddp.xs[-1]

examples/notebooks/cartpole_swing_up.py

@@ -3,77 +3,46 @@
 from IPython.display import HTML
 
 from cartpole_utils import animateCartpole
-from crocoddyl import *
+import crocoddyl
 
 
-class DifferentialActionModelCartpole:
+class DifferentialActionModelCartpole(crocoddyl.DifferentialActionModelAbstract):
     def __init__(self):
-        self.State = StateVector(4)
-        self.nq, self.nv = 2, 2
-        self.nx = 4
-        self.ndx = 4
-        self.nout = 2
-        self.nu = 1
-        self.ncost = 6
+        crocoddyl.DifferentialActionModelAbstract.__init__(self, crocoddyl.StateVector(4), 1, 6)  # nu = 1; nr = 6
         self.unone = np.zeros(self.nu)
 
         self.m1 = 1.
         self.m2 = .1
         self.l = .5
         self.g = 9.81
-        self.costWeights = [1., 1., 0.1, 0.001, 0.001, 1.]  # sin,1-cos,x,xdot,thdot,f
+        self.costWeights = [1., 1., 0.1, 0.001, 0.001, 1.]  # sin, 1-cos, x, xdot, thdot, f
 
-    def createData(self):
-        return DifferentialActionDataCartpole(self)
-
-    def calc(model, data, x, u=None):
+    def calc(self, data, x, u=None):
         if u is None: u = model.unone
         # Getting the state and control variables
-        x, th, xdot, thdot = x
-        f, = u
+        y, th, ydot, thdot = np.asscalar(x[0]), np.asscalar(x[1]), np.asscalar(x[2]), np.asscalar(x[3])
+        f = np.asscalar(u[0])
 
         # Shortname for system parameters
-        m1, m2, l, g = model.m1, model.m2, model.l, model.g
+        m1, m2, l, g = self.m1, self.m2, self.l, self.g
         s, c = np.sin(th), np.cos(th)
 
         # Defining the equation of motions
         m = m1 + m2
         mu = m1 + m2 * s**2
         xddot = (f + m2 * c * s * g - m2 * l * s * thdot**2) / mu
         thddot = (c * f / l + m * g * s / l - m2 * c * s * thdot**2) / mu
-        data.xout[:] = [xddot, thddot]
+        data.xout = np.matrix([xddot, thddot]).T
 
         # Computing the cost residual and value
-        data.costResiduals[:] = [s, 1 - c, x, xdot, thdot, f]
-        data.costResiduals[:] *= model.costWeights
-        data.cost = .5 * sum(data.costResiduals**2)
-        return data.xout, data.cost
+        data.r = np.matrix(self.costWeights * np.array([s, 1 - c, y, ydot, thdot, f])).T
+        data.cost = .5 * np.asscalar(sum(np.asarray(data.r)**2))
 
-    def calcDiff(model, data, x, u=None):
+    def calcDiff(self, data, x, u=None):
         # Advance user might implement the derivatives
         pass
 
 
-class DifferentialActionDataCartpole:
-    def __init__(self, model):
-        self.cost = np.nan
-        self.xout = np.zeros(model.nout)
-
-        nx, nu, ndx, nout = model.nx, model.nu, model.ndx, model.nout
-        self.costResiduals = np.zeros([model.ncost])
-        self.g = np.zeros([ndx + nu])
-        self.L = np.zeros([ndx + nu, ndx + nu])
-        self.F = np.zeros([nout, ndx + nu])
-
-        self.Lx = self.g[:ndx]
-        self.Lu = self.g[ndx:]
-        self.Lxx = self.L[:ndx, :ndx]
-        self.Lxu = self.L[:ndx, ndx:]
-        self.Luu = self.L[ndx:, ndx:]
-        self.Fx = self.F[:, :ndx]
-        self.Fu = self.F[:, ndx:]
-
-
 # Creating the DAM for the cartpole
 cartpoleDAM = DifferentialActionModelCartpole()
 cartpoleData = cartpoleDAM.createData()
@@ -82,31 +51,31 @@ def __init__(self, model):
 # Using NumDiff for computing the derivatives. We specify the
 # withGaussApprox=True to have approximation of the Hessian based on the
 # Jacobian of the cost residuals.
-cartpoleND = DifferentialActionModelNumDiff(cartpoleDAM, withGaussApprox=True)
+cartpoleND = crocoddyl.DifferentialActionModelNumDiff(cartpoleDAM, True)
 
 # Getting the IAM using the simpletic Euler rule
 timeStep = 5e-2
-cartpoleIAM = IntegratedActionModelEuler(cartpoleND, timeStep)
+cartpoleIAM = crocoddyl.IntegratedActionModelEuler(cartpoleND, timeStep)
 
 # Creating the shooting problem
 x0 = np.array([0., 3.14, 0., 0.])
 T = 50
 
 terminalCartpole = DifferentialActionModelCartpole()
-terminalCartpoleDAM = DifferentialActionModelNumDiff(terminalCartpole, withGaussApprox=True)
-terminalCartpoleIAM = IntegratedActionModelEuler(terminalCartpoleDAM)
+terminalCartpoleDAM = crocoddyl.DifferentialActionModelNumDiff(terminalCartpole, True)
+terminalCartpoleIAM = crocoddyl.IntegratedActionModelEuler(terminalCartpoleDAM)
 
 terminalCartpole.costWeights[0] = 100
 terminalCartpole.costWeights[1] = 100
 terminalCartpole.costWeights[2] = 1.
 terminalCartpole.costWeights[3] = 0.1
 terminalCartpole.costWeights[4] = 0.01
 terminalCartpole.costWeights[5] = 0.0001
-problem = ShootingProblem(x0, [cartpoleIAM] * T, terminalCartpoleIAM)
+problem = crocoddyl.ShootingProblem(x0, [cartpoleIAM] * T, terminalCartpoleIAM)
 
 # Solving it using DDP
-ddp = SolverDDP(problem)
-ddp.callback = [CallbackDDPVerbose()]
-xs, us, done = ddp.solve(maxiter=300)
+ddp = crocoddyl.SolverDDP(problem)
+ddp.setCallbacks([crocoddyl.CallbackVerbose()])
+ddp.solve([], [], 300)
 
-HTML(animateCartpole(xs).to_html5_video())
+HTML(animateCartpole(ddp.xs).to_html5_video())

examples/notebooks/p2p.py

@@ -7,7 +7,7 @@
 robot_model = robot.model
 
 DT = 1e-3
-T = 15
+T = 150
 
 state = crocoddyl.StateMultibody(robot.model)
 
@@ -25,8 +25,9 @@
     [1., 0., 1., 1.],
 ]
 
-robot.initViewer(loadModel=True)
-gv = robot.viewer.gui
+cameraTF = [2., 2.68, 0.54, 0.2, 0.62, 0.72, 0.22]
+display = crocoddyl.GepettoDisplay(robot, cameraTF=cameraTF, floor=False)
+gv = display.robot.viewer.gui
 for i, p in enumerate(ps):
     gv.addSphere('world/point%d' % i, .05, colors[i])
     gv.applyConfiguration('world/point%d' % i, p.tolist() + [0., 0., 0., 1.])
@@ -49,19 +50,19 @@
 
     # Create the running action model
     runningCostModel = crocoddyl.CostModelSum(state)
-    runningCostModel.addCost("gripperPose", goalTrackingCost, 1.)
+    runningCostModel.addCost("gripperPose", goalTrackingCost, 1e-3)
     runningCostModel.addCost("xReg", xRegCost, 1e-4)
     runningCostModel.addCost("uReg", uRegCost, 1e-7)
     runningModels += [crocoddyl.DifferentialActionModelFreeFwdDynamics(state, actuation, runningCostModel)]
 
     # Create the terminal action model
     terminalCostModel = crocoddyl.CostModelSum(state)
-    terminalCostModel.addCost("gripperPose", goalTrackingCost, 1000.)
+    terminalCostModel.addCost("gripperPose", goalTrackingCost, 1.)
     terminalCostModel.addCost("xreg", xRegCost, 1e-4)
     terminalCostModel.addCost("uReg", uRegCost, 1e-7)
     terminalModels += [crocoddyl.DifferentialActionModelFreeFwdDynamics(state, actuation, terminalCostModel)]
 
-q0 = np.matrix([2., 1.5, -2., 0., 0., 0., 0.]).T
+q0 = np.array([2., 1.5, -2., 0., 0., 0., 0.]).T
 x0 = np.concatenate([q0, pinocchio.utils.zero(state.nv)])
 seqs = [[crocoddyl.IntegratedActionModelEuler(runningModel, DT)] * T +
         [crocoddyl.IntegratedActionModelEuler(terminalModel)]
@@ -70,7 +71,6 @@
 
 # Creating the DDP solver for this OC problem, defining a logger
 ddp = crocoddyl.SolverDDP(problem)
-cameraTF = [2., 2.68, 0.54, 0.2, 0.62, 0.72, 0.22]
 ddp.setCallbacks([crocoddyl.CallbackVerbose()])
 
 # Solving it with the DDP algorithm
@@ -83,7 +83,7 @@
     ddp.solve(ddp.xs, ddp.us, 10)
 
 # Visualizing the solution in gepetto-viewer
-crocoddyl.displayTrajectory(robot, ddp.xs, DT)
+display.displayFromSolver(ddp)
 
 robot_data = robot_model.createData()
 for i, s in enumerate(seqs + [1]):