diff --git a/crocoddyl/actuation.py b/crocoddyl/actuation.py
index d0bbc43a34d1e2daf575dfad158a75db4f847ee5..a3f2ff57875b98a42ce65c785f62d710fccc2a10 100644
--- a/crocoddyl/actuation.py
+++ b/crocoddyl/actuation.py
@@ -46,15 +46,20 @@ class ActuationDataUAM:
self.A = np.zeros([nv, ndx + nu]) # result of calcDiff
self.Ax = self.A[:, :ndx]
self.Au = self.A[:, ndx:]
- self.Au[2:6,:4] = np.array([[1,1,1,1],[-d,d,-d,d],[-d,-d,d,d],[-cm/cf,-cm/cf,cm/cf,cm/cf]])
+ self.Au[2:6,:4] = np.array([[1,1,1,1],[-d,d,d,-d],[-d,d,-d,d],[-cm/cf,-cm/cf,cm/cf,cm/cf]])
np.fill_diagonal(self.Au[6:, 4:], 1)
-# This is the matrix that, given a force vector representing the four motors, outputs the thrust and moment
-# [ 0, 0, 0, 0]
-# [ 0, 0, 0, 0]
-# [ 1, 1, 1, 1]
-# [ -d, d, -d, d]
-# [ -d, -d, d, d]
-# [ -cm/cf, -cm/cf, cm/cf, cm/cf]
+ # Actuation considering motor forces instead of thrust and moments
+ # This is the matrix that, given a force vector representing the four motors, outputs the thrust and moment
+ # [ 0, 0, 0, 0]
+ # [ 0, 0, 0, 0]
+ # [ 1, 1, 1, 1]
+ # [ -d, d, -d, d]
+ # [ -d, -d, d, d]
+ # [ -cm/cf, -cm/cf, cm/cf, cm/cf]
+
+
+
+
class ActuationModelFreeFloating:
'''
diff --git a/examples/kinton_flying.py b/examples/kinton_flying.py
index 6a8623b0a7f3dc9e740849a0c4b2b1b3b7ffce57..8edd2546aa6d38366f2a37d512e6869374307ffa 100644
--- a/examples/kinton_flying.py
+++ b/examples/kinton_flying.py
@@ -1,18 +1,34 @@
-ºfrom crocoddyl import *
+#!/usr/bin/env python
+# coding: utf-8
+
+# In[1]:
+
+
+from crocoddyl import *
import pinocchio as pin
import numpy as np
from crocoddyl.diagnostic import displayTrajectory
+
+# In[2]:
+
+
# LOAD ROBOT
robot = loadKinton()
-robot.initDisplay(loadModel=True)
+robot.initViewer(loadModel=True)
robot.display(robot.q0)
robot.framesForwardKinematics(robot.q0)
+rmodel = robot.model
+
+
+# In[3]:
+
+
# DEFINE TARGET POSITION
-target_pos = np.array([0,0,3])
-target_quat = pin.Quaternion(0, 0, 0, 1)
+target_pos = np.array([0,0,1])
+target_quat = pin.Quaternion(1, 0, 0, 0)
target_quat.normalize()
# Plot goal frame
@@ -20,8 +36,15 @@ robot.viewer.gui.addXYZaxis('world/framegoal', [1., 0., 0., 1.], .015, 4)
robot.viewer.gui.applyConfiguration('world/framegoal', target_pos.tolist() + [target_quat[0], target_quat[1], target_quat[2], target_quat[3]])
robot.viewer.gui.refresh()
+
+# In[4]:
+
+
# ACTUATION MODEL
-actModel = ActuationModelUAM(robot.model)
+distanceRotorCOG = 0.1525
+cf = 6.6e-5
+cm = 1e-6
+actModel = ActuationModelUAM(robot.model, distanceRotorCOG, cf, cm)
# COST MODEL
# Create a cost model per the running and terminal action model.
@@ -35,24 +58,50 @@ SE3ref.translation = target_pos.reshape(3,1)
SE3ref.rotation = target_quat.matrix()
-stateWeights = np.array([0] * 3 + [500.] * 3 + [0.01] * (robot.model.nv - 2) + [10] * robot.model.nv)
-goalTrackingCost = CostModelFramePlacement(robot.model,
- frame=robot.model.getFrameId(frameName),
+wBasePos = [1]
+wBaseOri = [500]
+wArmPos = [1]
+wBaseVel = [10]
+wBaseRate = [10]
+wArmVel = [10]
+stateWeights = np.array(wBasePos * 3 + wBaseOri * 3 + wArmPos * (robot.model.nv - 6) + wBaseVel * robot.model.nv)
+controlWeights = np.array([0.1]*4 + [100]*6)
+
+goalTrackingCost = CostModelFramePlacement(rmodel,
+ frame=rmodel.getFrameId(frameName),
ref=SE3ref,
nu =actModel.nu)
-xRegCost = CostModelState(robot.model, state, ref=state.zero(), nu=actModel.nu)
-uRegCost = CostModelControl(robot.model, nu=robot.model.nv-2)
+
+xRegCost = CostModelState(rmodel,
+ state,
+ ref=state.zero(),
+ nu=actModel.nu,
+ activation=ActivationModelWeightedQuad(stateWeights))
+uRegCost = CostModelControl(rmodel,
+ nu=robot.
+ model.nv-2,
+ activation = ActivationModelWeightedQuad(controlWeights))
+uLimCost = CostModelControl(rmodel,
+ nu=robot.
+ model.nv-2,
+ activation = ActivationModelInequality(np.array([0.1, 0.1, 0.1, 0.1, -1, -1, -1, -1, -1, -1]),
+ np.array([5, 5, 5, 5, 1, 1, 1, 1, 1, 1])))
# Then let's add the running and terminal cost functions
-runningCostModel.addCost(name="pos", weight=1e-3, cost=goalTrackingCost)
-runningCostModel.addCost(name="regx", weight=1e-7, cost=xRegCost)
-runningCostModel.addCost(name="regu", weight=1e-7, cost=uRegCost)
-terminalCostModel.addCost(name="pos", weight=50, cost=goalTrackingCost)
+runningCostModel.addCost(name="pos", weight=0.1, cost=goalTrackingCost)
+runningCostModel.addCost(name="regx", weight=1e-4, cost=xRegCost)
+runningCostModel.addCost(name="regu", weight=1e-6, cost=uRegCost)
+# runningCostModel.addCost(name="limu", weight=1e-3, cost=uLimCost)
+terminalCostModel.addCost(name="pos", weight=0, cost=goalTrackingCost)
# DIFFERENTIAL ACTION MODEL
runningModel = IntegratedActionModelEuler(DifferentialActionModelUAM(robot.model, actModel, runningCostModel))
terminalModel = IntegratedActionModelEuler(DifferentialActionModelUAM(robot.model, actModel, terminalCostModel))
+
+# In[5]:
+
+
# DEFINING THE SHOOTING PROBLEM & SOLVING
# Defining the time duration for running action models and the terminal one
@@ -61,19 +110,97 @@ runningModel.timeStep = dt
# For this optimal control problem, we define 250 knots (or running action
# models) plus a terminal knot
-T = 250
-q0 = [0., 0., 0., 0., 0., 0.]
-q0 = robot.model.referenceConfigurations["initial_pose"]
+T = 1000
+q0 = rmodel.referenceConfigurations["initial_pose"].copy()
+v0 = pin.utils.zero(rmodel.nv)
+x0 = m2a(np.concatenate([q0, v0]))
+rmodel.defaultState = x0.copy()
-x0 = np.hstack([m2a(q0), np.zeros(robot.model.nv)])
problem = ShootingProblem(x0, [runningModel] * T, terminalModel)
# Creating the DDP solver for this OC problem, defining a logger
-ddp = SolverDDP(problem)
-ddp.callback = [CallbackDDPVerbose()]
-ddp.callback.append(CallbackDDPLogger())
+fddp = SolverFDDP(problem)
+fddp.callback = [CallbackDDPVerbose()]
+fddp.callback.append(CallbackDDPLogger())
+
+us0 = [
+ m.differential.quasiStatic(d.differential, rmodel.defaultState)
+ if isinstance(m, IntegratedActionModelEuler) else np.zeros(0)
+ for m, d in zip(fddp.problem.runningModels, fddp.problem.runningDatas)]
+xs0 = [problem.initialState]*len(fddp.models())
# Solving it with the DDP algorithm
-ddp.solve()
+#fddp.solve(init_xs=xs0, init_us=us0)
+fddp.solve()
+
+
+# In[67]:
+
+
+displayTrajectory(robot, fddp.xs, runningModel.timeStep)
+
+
+# In[68]:
+
+
+# Control trajectory
+f1 = []
+f2 = [];
+f3 = [];
+f4 = [];
+
+for u in fddp.us:
+ f1.append(u[0])
+ f2.append(u[1])
+ f3.append(u[2])
+ f4.append(u[3])
+
+# State trajectory
+Xx = [];
+Xy = [];
+Xz = [];
+Vx = [];
+Vy = [];
+Vz = [];
+
+
+for x in fddp.xs:
+ Xx.append(x[0])
+ Xy.append(x[1])
+ Xz.append(x[2])
+ Vx.append(x[13])
+ Vy.append(x[14])
+ Vz.append(x[15])
+
+
+# In[69]:
+
+
+import matplotlib.pyplot as plt
+t = np.arange(0., 1, dt)
+
+fig, axs = plt.subplots(2,2, figsize=(15,10))
+fig.suptitle('Motor forces')
+axs[0, 0].plot(t,f1)
+axs[0, 0].set_title('Motor 1')
+axs[0, 1].plot(t,f2)
+axs[0, 1].set_title('Motor 2')
+axs[1, 0].plot(t,f3)
+axs[1, 0].set_title('Motor 3')
+axs[1, 1].plot(t,f4)
+axs[1, 1].set_title('Motor 4')
+
+plt.figure()
+t = np.append(t, 1)
+plt.plot(t,Xx,t,Xy,t,Xz)
+plt.legend(['x','y','z'])
+plt.title('State - Position')
+plt.ylabel('Position, [m]')
+plt.xlabel('[s]')
-displayTrajectory(robot, ddp.xs, runningModel.timeStep)
+plt.figure()
+plt.plot(t,Vx,t,Vy,t,Vz)
+plt.legend(['x','y','z'])
+plt.title('State - Velocity')
+plt.ylabel('Velocity, [m/s]')
+plt.xlabel('[s]')
diff --git a/examples/notebooks/kinton_fixed_base.ipynb b/examples/notebooks/kinton_fixed_base.ipynb
index 098a1a024c28e8234686ddf98d6fa358ee9110b8..c47122f4878dcf4e48104cca531c156cf708a445 100644
--- a/examples/notebooks/kinton_fixed_base.ipynb
+++ b/examples/notebooks/kinton_fixed_base.ipynb
@@ -2,7 +2,7 @@
"cells": [
{
"cell_type": "code",
- "execution_count": 3,
+ "execution_count": 5,
"metadata": {},
"outputs": [],
"source": [
@@ -11,8 +11,8 @@
"import numpy as np\n",
"\n",
"robot = loadKintonArm()\n",
- "robot.initDisplay(loadModel=True)\n",
- "robot.display(robot.q0)\n",
+ "#robot.initDisplay(loadModel=True)\n",
+ "#robot.display(robot.q0)\n",
"\n",
"robot.framesForwardKinematics(robot.q0)\n",
"\n",
@@ -23,22 +23,20 @@
},
{
"cell_type": "code",
- "execution_count": 4,
+ "execution_count": 6,
"metadata": {},
"outputs": [
{
- "data": {
- "text/plain": [
- " R =\n",
- "1 0 0\n",
- "0 1 0\n",
- "0 0 1\n",
- " p = 0 0 0"
- ]
- },
- "execution_count": 4,
- "metadata": {},
- "output_type": "execute_result"
+ "ename": "AttributeError",
+ "evalue": "'NoneType' object has no attribute 'viewer'",
+ "output_type": "error",
+ "traceback": [
+ "\u001b[0;31m------------------------------------------------------\u001b[0m",
+ "\u001b[0;31mAttributeError\u001b[0m Traceback (most recent call last)",
+ "\u001b[0;32m<ipython-input-6-cd53aa81cd48>\u001b[0m in \u001b[0;36m<module>\u001b[0;34m()\u001b[0m\n\u001b[1;32m 4\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m 5\u001b[0m \u001b[0;31m# Plot goal frame\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m----> 6\u001b[0;31m \u001b[0mrobot\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mviewer\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mgui\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0maddXYZaxis\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m'world/framegoal'\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;34m[\u001b[0m\u001b[0;36m1.\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;36m0.\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;36m0.\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;36m1.\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;36m.015\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;36m4\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m 7\u001b[0m \u001b[0mrobot\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mviewer\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mgui\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mapplyConfiguration\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m'world/framegoal'\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mtarget_pos\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mtolist\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m)\u001b[0m \u001b[0;34m+\u001b[0m \u001b[0;34m[\u001b[0m\u001b[0mtarget_quat\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;36m0\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mtarget_quat\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;36m1\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mtarget_quat\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;36m2\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mtarget_quat\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;36m3\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m 8\u001b[0m \u001b[0mrobot\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mviewer\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mgui\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mrefresh\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
+ "\u001b[0;32m/opt/openrobots/lib/python2.7/site-packages/pinocchio/robot_wrapper.pyc\u001b[0m in \u001b[0;36mviewer\u001b[0;34m(self)\u001b[0m\n\u001b[1;32m 213\u001b[0m \u001b[0;34m@\u001b[0m\u001b[0mproperty\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m 214\u001b[0m \u001b[0;32mdef\u001b[0m \u001b[0mviewer\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mself\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m--> 215\u001b[0;31m \u001b[0;32mreturn\u001b[0m \u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mviz\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mviewer\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m 216\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m 217\u001b[0m \u001b[0;32mdef\u001b[0m \u001b[0msetVisualizer\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mself\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mvisualizer\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0minit\u001b[0m\u001b[0;34m=\u001b[0m\u001b[0mTrue\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mcopy_models\u001b[0m\u001b[0;34m=\u001b[0m\u001b[0mFalse\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
+ "\u001b[0;31mAttributeError\u001b[0m: 'NoneType' object has no attribute 'viewer'"
+ ]
}
],
"source": [
@@ -61,7 +59,7 @@
},
{
"cell_type": "code",
- "execution_count": 5,
+ "execution_count": 7,
"metadata": {},
"outputs": [],
"source": [
@@ -89,7 +87,7 @@
},
{
"cell_type": "code",
- "execution_count": 4,
+ "execution_count": 8,
"metadata": {},
"outputs": [
{
@@ -1349,7 +1347,7 @@
" True)"
]
},
- "execution_count": 4,
+ "execution_count": 8,
"metadata": {},
"output_type": "execute_result"
}
@@ -1383,9 +1381,18 @@
},
{
"cell_type": "code",
- "execution_count": 5,
+ "execution_count": 9,
"metadata": {},
- "outputs": [],
+ "outputs": [
+ {
+ "name": "stderr",
+ "output_type": "stream",
+ "text": [
+ "/home/jmarti/robotics/toolboxes/crocoddyl/crocoddyl/diagnostic.py:75: DeprecatedWarning: Call to deprecated function initDisplay. Use initViewer\n",
+ " robot.initDisplay(loadModel=True)\n"
+ ]
+ }
+ ],
"source": [
"from crocoddyl.diagnostic import displayTrajectory\n",
"\n",
@@ -1423,7 +1430,7 @@
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython2",
- "version": "2.7.15+"
+ "version": "2.7.12"
}
},
"nbformat": 4,
diff --git a/examples/notebooks/kinton_flying_base.ipynb b/examples/notebooks/kinton_flying_base.ipynb
index 4fb3c6d7e245a1182f3aa0996a0d9e6b6186a47c..0ab3183e4a3955bc193bea078bff43ff29b6fc45 100644
--- a/examples/notebooks/kinton_flying_base.ipynb
+++ b/examples/notebooks/kinton_flying_base.ipynb
@@ -2,7 +2,7 @@
"cells": [
{
"cell_type": "code",
- "execution_count": 12,
+ "execution_count": 3,
"metadata": {},
"outputs": [],
"source": [
@@ -14,13 +14,13 @@
},
{
"cell_type": "code",
- "execution_count": 13,
+ "execution_count": 4,
"metadata": {},
"outputs": [],
"source": [
"# LOAD ROBOT\n",
"robot = loadKinton()\n",
- "robot.initDisplay(loadModel=True)\n",
+ "robot.initViewer(loadModel=True)\n",
"robot.display(robot.q0)\n",
"\n",
"robot.framesForwardKinematics(robot.q0)\n",
@@ -30,7 +30,7 @@
},
{
"cell_type": "code",
- "execution_count": 14,
+ "execution_count": 5,
"metadata": {},
"outputs": [],
"source": [
@@ -47,7 +47,7 @@
},
{
"cell_type": "code",
- "execution_count": 70,
+ "execution_count": 6,
"metadata": {},
"outputs": [],
"source": [
@@ -102,7 +102,7 @@
"runningCostModel.addCost(name=\"pos\", weight=0.1, cost=goalTrackingCost)\n",
"runningCostModel.addCost(name=\"regx\", weight=1e-4, cost=xRegCost)\n",
"runningCostModel.addCost(name=\"regu\", weight=1e-6, cost=uRegCost)\n",
- "runningCostModel.addCost(name=\"limu\", weight=1e-3, cost=uLimCost)\n",
+ "# runningCostModel.addCost(name=\"limu\", weight=1e-3, cost=uLimCost)\n",
"terminalCostModel.addCost(name=\"pos\", weight=0, cost=goalTrackingCost)\n",
"\n",
"# DIFFERENTIAL ACTION MODEL\n",
@@ -112,60 +112,11 @@
},
{
"cell_type": "code",
- "execution_count": 72,
+ "execution_count": null,
"metadata": {
"scrolled": true
},
- "outputs": [
- {
- "name": "stdout",
- "output_type": "stream",
- "text": [
- "iter \t cost \t stop \t grad \t xreg \t ureg \t step \t feas\n",
- " 0 1.71904e+01 1.92210e-01 9.72121e+01 1.00000e-09 1.00000e-09 1.0000 1\n",
- " 1 1.71813e+01 1.16782e-03 7.44667e+00 1.00000e-09 1.00000e-09 0.0078 1\n",
- " 2 1.71666e+01 1.01968e-03 6.70858e+00 1.00000e-09 1.00000e-09 0.0078 1\n",
- " 3 1.71467e+01 9.16008e-04 6.16712e+00 1.00000e-09 1.00000e-09 0.0078 1\n",
- " 4 1.71243e+01 8.36192e-04 5.74361e+00 1.00000e-09 1.00000e-09 0.0078 1\n",
- " 5 1.71037e+01 7.80623e-04 5.42246e+00 1.00000e-09 1.00000e-09 0.0078 1\n",
- " 6 1.70795e+01 7.30754e-04 5.12354e+00 1.00000e-09 1.00000e-09 0.0078 1\n",
- " 7 1.70553e+01 6.87075e-04 4.86947e+00 1.00000e-09 1.00000e-09 0.0078 1\n",
- " 8 1.70330e+01 6.54222e-04 4.65985e+00 1.00000e-09 1.00000e-09 0.0078 1\n",
- " 9 1.70097e+01 6.25821e-04 4.46337e+00 1.00000e-09 1.00000e-09 0.0078 1\n",
- "iter \t cost \t stop \t grad \t xreg \t ureg \t step \t feas\n",
- " 10 1.69963e+01 5.96136e-04 4.27391e+00 1.00000e-09 1.00000e-09 0.0156 1\n",
- " 11 1.69882e+01 6.35090e-04 4.08751e+00 1.00000e-09 1.00000e-09 0.0156 1\n",
- " 12 1.69717e+01 6.81772e-04 3.92440e+00 1.00000e-09 1.00000e-09 0.0156 1\n",
- " 13 1.69443e+01 7.09163e-04 3.75651e+00 1.00000e-09 1.00000e-09 0.0156 1\n",
- " 14 1.69174e+01 7.18390e-04 3.59820e+00 1.00000e-09 1.00000e-09 0.0156 1\n",
- " 15 1.68860e+01 7.26212e-04 3.44888e+00 1.00000e-09 1.00000e-09 0.0156 1\n",
- " 16 1.68592e+01 7.27712e-04 3.31537e+00 1.00000e-09 1.00000e-09 0.0156 1\n",
- " 17 1.68314e+01 7.35580e-04 3.19566e+00 1.00000e-09 1.00000e-09 0.0156 1\n",
- " 18 1.68004e+01 7.38995e-04 3.07849e+00 1.00000e-09 1.00000e-09 0.0156 1\n",
- " 19 1.67759e+01 7.37410e-04 2.97316e+00 1.00000e-09 1.00000e-09 0.0156 1\n",
- "iter \t cost \t stop \t grad \t xreg \t ureg \t step \t feas\n",
- " 20 1.67428e+01 7.43080e-04 2.87042e+00 1.00000e-09 1.00000e-09 0.0156 1\n",
- " 21 1.67125e+01 7.32659e-04 2.76652e+00 1.00000e-09 1.00000e-09 0.0156 1\n",
- " 22 1.66826e+01 7.25649e-04 2.67049e+00 1.00000e-09 1.00000e-09 0.0156 1\n"
- ]
- },
- {
- "ename": "KeyboardInterrupt",
- "evalue": "",
- "output_type": "error",
- "traceback": [
- "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
- "\u001b[0;31mKeyboardInterrupt\u001b[0m Traceback (most recent call last)",
- "\u001b[0;32m<ipython-input-72-b175ebf9a54b>\u001b[0m in \u001b[0;36m<module>\u001b[0;34m()\u001b[0m\n\u001b[1;32m 28\u001b[0m \u001b[0;31m# Solving it with the DDP algorithm\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m 29\u001b[0m \u001b[0;31m#fddp.solve(init_xs=xs0, init_us=us0)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m---> 30\u001b[0;31m \u001b[0mfddp\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0msolve\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m",
- "\u001b[0;32m/home/jmarti/robotics/toolboxes/crocoddyl/crocoddyl/fddp.pyc\u001b[0m in \u001b[0;36msolve\u001b[0;34m(self, maxiter, init_xs, init_us, isFeasible, regInit)\u001b[0m\n\u001b[1;32m 141\u001b[0m \u001b[0;32mwhile\u001b[0m \u001b[0mTrue\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m 142\u001b[0m \u001b[0;32mtry\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m--> 143\u001b[0;31m \u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mcomputeDirection\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mrecalc\u001b[0m\u001b[0;34m=\u001b[0m\u001b[0mrecalc\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m 144\u001b[0m \u001b[0;32mexcept\u001b[0m \u001b[0mArithmeticError\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m 145\u001b[0m \u001b[0mrecalc\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mFalse\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
- "\u001b[0;32m/home/jmarti/robotics/toolboxes/crocoddyl/crocoddyl/fddp.pyc\u001b[0m in \u001b[0;36mcomputeDirection\u001b[0;34m(self, recalc)\u001b[0m\n\u001b[1;32m 71\u001b[0m \u001b[0;32mif\u001b[0m \u001b[0mrecalc\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m 72\u001b[0m \u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mcalc\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m---> 73\u001b[0;31m \u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mbackwardPass\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m 74\u001b[0m \u001b[0;32mreturn\u001b[0m \u001b[0;34m[\u001b[0m\u001b[0mnp\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mnan\u001b[0m\u001b[0;34m]\u001b[0m \u001b[0;34m*\u001b[0m \u001b[0;34m(\u001b[0m\u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mproblem\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mT\u001b[0m \u001b[0;34m+\u001b[0m \u001b[0;36m1\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mk\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mVx\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m 75\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n",
- "\u001b[0;32m/home/jmarti/robotics/toolboxes/crocoddyl/crocoddyl/fddp.pyc\u001b[0m in \u001b[0;36mbackwardPass\u001b[0;34m(self)\u001b[0m\n\u001b[1;32m 265\u001b[0m \u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mQuu\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0mt\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0mrange\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mmodel\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mnu\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mrange\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mmodel\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mnu\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m]\u001b[0m \u001b[0;34m+=\u001b[0m \u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mu_reg\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m 266\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m--> 267\u001b[0;31m \u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mcomputeGains\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mt\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m 268\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m 269\u001b[0m \u001b[0;31m# Vx = Qx - Qu K + .5(- Qxu k - k Qux + k Quu K + K Quu k)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
- "\u001b[0;32m/home/jmarti/robotics/toolboxes/crocoddyl/crocoddyl/fddp.pyc\u001b[0m in \u001b[0;36mcomputeGains\u001b[0;34m(self, t)\u001b[0m\n\u001b[1;32m 293\u001b[0m \u001b[0;32mif\u001b[0m \u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mQuu\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0mt\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mshape\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;36m0\u001b[0m\u001b[0;34m]\u001b[0m \u001b[0;34m>\u001b[0m \u001b[0;36m0\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m 294\u001b[0m \u001b[0mLb\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mscl\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mcho_factor\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mQuu\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0mt\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m--> 295\u001b[0;31m \u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mK\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0mt\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;34m:\u001b[0m\u001b[0;34m]\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mscl\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mcho_solve\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mLb\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mQux\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0mt\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m 296\u001b[0m \u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mk\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0mt\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m]\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mscl\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mcho_solve\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mLb\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mQu\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0mt\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m 297\u001b[0m \u001b[0;32melse\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
- "\u001b[0;32m/home/jmarti/.local/lib/python2.7/site-packages/scipy/linalg/decomp_cholesky.pyc\u001b[0m in \u001b[0;36mcho_solve\u001b[0;34m(c_and_lower, b, overwrite_b, check_finite)\u001b[0m\n\u001b[1;32m 207\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m 208\u001b[0m \u001b[0mpotrs\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mget_lapack_funcs\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m'potrs'\u001b[0m\u001b[0;34m,\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;34m(\u001b[0m\u001b[0mc\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mb1\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m--> 209\u001b[0;31m \u001b[0mx\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0minfo\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mpotrs\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mc\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mb1\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mlower\u001b[0m\u001b[0;34m=\u001b[0m\u001b[0mlower\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0moverwrite_b\u001b[0m\u001b[0;34m=\u001b[0m\u001b[0moverwrite_b\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m 210\u001b[0m \u001b[0;32mif\u001b[0m \u001b[0minfo\u001b[0m \u001b[0;34m!=\u001b[0m \u001b[0;36m0\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m 211\u001b[0m raise ValueError('illegal value in %d-th argument of internal potrs'\n",
- "\u001b[0;31mKeyboardInterrupt\u001b[0m: "
- ]
- }
- ],
+ "outputs": [],
"source": [
"# DEFINING THE SHOOTING PROBLEM & SOLVING\n",
"\n",