Commit 172a4a2a authored by Carlos Mastalli's avatar Carlos Mastalli
Browse files

Merge branch 'devel' into 'master'

First version of the DDP structure

See merge request loco-3d/cddp!9
parents 01739e27 97ec63db
*.pyc
*.pyd
*.pyo
*~
\ No newline at end of file
[submodule "models/hyq_description"]
path = models/hyq_description
url = git@github.com:robot-locomotion/hyq-description.git
[submodule "models/talos_data"]
path = models/talos_data
url = git@gepgitlab.laas.fr:pyrene-dev/talos_data.git
......@@ -170,7 +170,8 @@ class RK4Discretizer(Discretizer):
:param dt: sampling period
:returns: discrete time state and control derivatives
"""
# Computing four stages of RK4
# Computing four stages of RK4. It assumes that an unique linear model for
# all the RK nodes (TODO: check if this is a good assumption)
system.f(data, x, u)
system.fx(data, x, u)
system.fu(data, x, u)
......
......@@ -87,7 +87,7 @@ class EulerIntegrator(Integrator):
:param nq: dimension of the configuration manifold
:param nv: dimension of the tangent space of the configuration manifold
"""
self.x_next = np.matrix(np.zeros((nq, 1)))
# No extra data needs to be created
def __call__(self, system, data, x, u, dt):
""" Integrate the system dynamics using the forward Euler scheme.
......@@ -99,9 +99,9 @@ class EulerIntegrator(Integrator):
:param dt: sampling period
:returns: next state vector
"""
np.copyto(self.x_next,
system.advanceConfiguration(x.copy(), dt * system.f(data, x, u)))
return self.x_next
np.copyto(data.f,
system.advanceConfiguration(x, dt * system.f(data, x, u)))
return data.f
class GeometricEulerIntegrator(GeometricIntegrator):
......@@ -112,8 +112,7 @@ class GeometricEulerIntegrator(GeometricIntegrator):
:param nq: dimension of the configuration manifold
:param nv: dimension of the tangent space of the configuration manifold
"""
# Data for integration
self.x_next = np.matrix(np.zeros((nq + nv, 1)))
# No extra data needs to be created
def __call__(self, system, data, q, v, tau, dt):
""" Integrate the system dynamics using the forward Euler scheme.
......@@ -126,10 +125,10 @@ class GeometricEulerIntegrator(GeometricIntegrator):
:param dt: sampling period
:returns: next state vector
"""
self.x_next[data.nq:] = v + system.g(data, q, v, tau) * dt
self.x_next[:data.nq] = \
system.advanceConfiguration(q.copy(), self.x_next[data.nq:] * dt)
return self.x_next
data.f[data.nq:] = v + system.g(data, q, v, tau) * dt
data.f[:data.nq] = \
system.advanceConfiguration(q, data.f[data.nq:] * dt)
return data.f
class RK4Integrator(Integrator):
......@@ -140,7 +139,6 @@ class RK4Integrator(Integrator):
:param nq: dimension of the configuration manifold
:param nv: dimension of the tangent space of the configuration manifold
"""
self.x_next = np.matrix(np.zeros((nq, 1)))
self.k1 = np.matrix(np.zeros((nv, 1)))
self.k2 = np.matrix(np.zeros((nv, 1)))
self.k3 = np.matrix(np.zeros((nv, 1)))
......@@ -158,12 +156,12 @@ class RK4Integrator(Integrator):
:returns: next state vector
"""
np.copyto(self.k1, dt * system.f(data, x, u))
np.copyto(self.k2, dt * system.f(data, system.advanceConfiguration(x.copy(), 0.5 * self.k1), u))
np.copyto(self.k3, dt * system.f(data, system.advanceConfiguration(x.copy(), 0.5 * self.k2), u))
np.copyto(self.k4, dt * system.f(data, system.advanceConfiguration(x.copy(), self.k3), u))
np.copyto(self.k2, dt * system.f(data, system.advanceConfiguration(x, 0.5 * self.k1), u))
np.copyto(self.k3, dt * system.f(data, system.advanceConfiguration(x, 0.5 * self.k2), u))
np.copyto(self.k4, dt * system.f(data, system.advanceConfiguration(x, self.k3), u))
np.copyto(self.sum_k, 1. / 6 * (self.k1 + 2. * self.k2 + 2. * self.k3 + self.k4))
np.copyto(self.x_next, system.advanceConfiguration(x.copy(), self.sum_k))
return self.x_next
np.copyto(data.f, system.advanceConfiguration(x, self.sum_k))
return data.f
def computeFlow(integrator, system, data, timeline, x, controls):
......@@ -174,6 +172,6 @@ def computeFlow(integrator, system, data, timeline, x, controls):
dt = timeline[k+1] - timeline[k]
t = timeline[k]
control = controls[k]
x_flow[k+1] = integrator.integrate(model, data, t, x_flow[k], control, dt)
x_flow[k+1] = integrator.integrate(system, data, t, x_flow[k], control, dt)
return x_flow
......@@ -56,6 +56,7 @@ class DDP(object):
# Global variables for the DDP algorithm
self.z = 0.
self.z_new = 0.
self.V_exp = np.matrix(np.zeros(1)) # Backward-pass Value function at t0
self.V = np.matrix(np.zeros(1)) # Forward-pass Value function at t0
self.dV_exp = np.matrix(np.zeros(1)) # Expected total cost reduction given alpha
......@@ -82,7 +83,7 @@ class DDP(object):
self.muV = 0.
self.muLM = 0.
self.mu0V = 0.
self.mu0LM = 1e-8
self.mu0LM = 0.
self.muV_inc = 10.
self.muV_dec = 0.5
self.muLM_inc = 10.
......@@ -94,7 +95,7 @@ class DDP(object):
self.alpha_inc = 2.
self.alpha_dec = 0.5
self.alpha_min = 1e-3
self.change_lb = 0.
self.armijo_condition = 1e-3
self.change_ub = 100.
# Global variables for analysing solver performance
......@@ -102,6 +103,8 @@ class DDP(object):
self.gamma_itr = [0.] * self.max_iter
self.theta_itr = [0.] * self.max_iter
self.alpha_itr = [0.] * self.max_iter
self.muLM_itr = [0.] * self.max_iter
self.muV_itr = [0.] * self.max_iter
def setFromConfigFile(self, config_file):
""" Sets the properties of the DDP solver from a YAML file.
......@@ -121,6 +124,8 @@ class DDP(object):
self.gamma_itr = [0.] * self.max_iter
self.theta_itr = [0.] * self.max_iter
self.alpha_itr = [0.] * self.max_iter
self.muLM_itr = [0.] * self.max_iter
self.muV_itr = [0.] * self.max_iter
# Setting up regularization
self.mu0LM = float(data['ddp']['regularization']['levenberg_marquard']['mu0'])
......@@ -134,6 +139,7 @@ class DDP(object):
self.alpha_min = float(data['ddp']['line_search']['min_stepsize'])
self.alpha_inc = float(data['ddp']['line_search']['inc_rate'])
self.alpha_dec = float(data['ddp']['line_search']['dec_rate'])
self.armijo_condition = float(data['ddp']['line_search']['armijo_condition'])
def compute(self, x0, U=None):
""" Computes the DDP algorithm.
......@@ -158,10 +164,12 @@ class DDP(object):
self.n_iter = 0
for i in range(self.max_iter):
# Recording the number of iterations
# Recording the number of iterations and mu values
self.n_iter = i
print ("Iteration", self.n_iter, "muV", self.muV,
"muLM", self.muLM, "alpha", self.alpha)
self.muLM_itr[i] = self.muLM
self.muV_itr[i] = self.muV
# Running the backward sweep
while not self.backwardPass(self.muLM, self.muV, self.alpha):
......@@ -199,6 +207,12 @@ class DDP(object):
if self.muLM < self.eps: # this is full Newton direction
self.muLM = self.eps
# This regularization smooth the policy updates. Experimentally it helps
# to reduce the number of iteration whenever the problem isn't well posed
self.muV *= self.muV_dec
if self.muV < self.eps:
self.muV = self.eps
# Increasing the stepsize for the next iteration
self.alpha *= self.alpha_inc
if self.alpha > 1.:
......@@ -367,10 +381,10 @@ class DDP(object):
# Checking the changes
self.dV[0] = self.V - self.V_exp
# print "--------------------------------------cost reduction", self.V[0,0], self.dV[0,0]
self.z = np.asscalar(self.dV) / np.asscalar(self.dV_exp)
# print "--------------------------------------------------------z", self.z
if self.z > self.change_lb and self.z < self.change_ub:
self.z_new = np.asscalar(self.dV) / np.asscalar(self.dV_exp)
if self.z_new > self.armijo_condition and self.z_new < self.change_ub:
self.z = self.z_new
# Accepting the new trajectory and control, defining them as nominal ones
for k in range(self.N):
it = self.intervals[k]
......@@ -462,7 +476,9 @@ class DDP(object):
def getConvergenceSequence(self):
return np.asarray(self.gamma_itr[:self.n_iter+1]), \
np.asarray(self.theta_itr[:self.n_iter+1]), \
np.asarray(self.alpha_itr[:self.n_iter+1])
np.asarray(self.alpha_itr[:self.n_iter+1]), \
np.asarray(self.muLM_itr[:self.n_iter+1]), \
np.asarray(self.muV_itr[:self.n_iter+1])
def saveToFile(self, filename):
import pickle
......
......@@ -41,6 +41,9 @@ class NumDiffSparseConstrainedForwardDynamics(NumDiffGeometricDynamicalSystem):
self.J = np.matrix(np.zeros((self.nc * len(self.contact_set), self._model.nv)))
self.gamma = np.matrix(np.zeros((self.nc * len(self.contact_set), 1)))
# Internal data for operators
self._q = np.matrix(np.zeros((self.nq,1)))
self._v = np.matrix(np.zeros((2*self.nv,1)))
def g(self, data, q, v, tau):
""" Compute the forward dynamics given a constrained contact set and store
......@@ -93,7 +96,8 @@ class NumDiffSparseConstrainedForwardDynamics(NumDiffGeometricDynamicalSystem):
:param dq: configuration point displacement
:returns: the next configuration point
"""
return se3.integrate(self._model, q, dq)
np.copyto(self._q, se3.integrate(self._model, q, dq))
return self._q
def differenceConfiguration(self, x_next, x_curr):
""" Operator that differentiates the configuration state.
......@@ -101,8 +105,8 @@ class NumDiffSparseConstrainedForwardDynamics(NumDiffGeometricDynamicalSystem):
:param x_next: next joint configuration and velocity [q_next, v_next]
:param x_curr: current joint configuration and velocity [q_curr, v_curr]
"""
q_next = x_next[:self._model.nq]
q_curr = x_curr[:self._model.nq]
dq = se3.difference(self._model, q_curr, q_next)
dv = x_next[self._model.nq:] - x_curr[self._model.nq:]
return np.vstack([dq, dv])
q_next = x_next[:self.nq]
q_curr = x_curr[:self.nq]
self._v[:self.nv] = se3.difference(self._model, q_curr, q_next)
self._v[self.nv:] = x_next[self.nq:] - x_curr[self.nq:]
return self._v
......@@ -32,6 +32,10 @@ class NumDiffForwardDynamics(NumDiffDynamicalSystem):
discretizer = EulerDiscretizer()
NumDiffDynamicalSystem.__init__(self, nq, nv, m, integrator, discretizer)
# Internal data for operators
self._x = np.matrix(np.zeros((nq,1)))
self._v = np.matrix(np.zeros((nv,1)))
def f(self, data, x, u):
""" Compute the forward dynamics through ABA and store it the result in
data.
......@@ -56,9 +60,9 @@ class NumDiffForwardDynamics(NumDiffDynamicalSystem):
"""
q = x[:self._model.nq]
dq = dx[:self._model.nv]
x[:self._model.nq] = se3.integrate(self._model, q, dq)
x[self._model.nq:] += dx[self._model.nv:]
return x
self._x[:self._model.nq] = se3.integrate(self._model, q, dq)
self._x[self._model.nq:] = x[self._model.nq:] + dx[self._model.nv:]
return self._x
def differenceConfiguration(self, x_next, x_curr):
""" Operator that differentiates the configuration state.
......@@ -68,9 +72,9 @@ class NumDiffForwardDynamics(NumDiffDynamicalSystem):
"""
q_next = x_next[:self._model.nq]
q_curr = x_curr[:self._model.nq]
dq = se3.difference(self._model, q_curr, q_next)
dv = x_next[self._model.nq:] - x_curr[self._model.nq:]
return np.vstack([dq, dv])
self._v[:self._model.nv] = se3.difference(self._model, q_curr, q_next)
self._v[self._model.nv:] = x_next[self._model.nq:] - x_curr[self._model.nq:]
return self._v
class NumDiffSparseForwardDynamics(NumDiffGeometricDynamicalSystem):
......@@ -102,6 +106,10 @@ class NumDiffSparseForwardDynamics(NumDiffGeometricDynamicalSystem):
discretizer = GeometricEulerDiscretizer()
NumDiffGeometricDynamicalSystem.__init__(self, nq, nv, m, integrator, discretizer)
# Internal data for operators
self._q = np.matrix(np.zeros((self.nq,1)))
self._v = np.matrix(np.zeros((2*self.nv,1)))
def g(self, data, q, v, tau):
""" Compute the forward dynamics through ABA and store it the result in
data.
......@@ -122,7 +130,8 @@ class NumDiffSparseForwardDynamics(NumDiffGeometricDynamicalSystem):
:param dq: joint configuration displacement
:returns: the next configuration point
"""
return se3.integrate(self._model, q, dq)
np.copyto(self._q, se3.integrate(self._model, q, dq))
return self._q
def differenceConfiguration(self, x_next, x_curr):
""" Operator that differentiates the configuration state.
......@@ -132,9 +141,9 @@ class NumDiffSparseForwardDynamics(NumDiffGeometricDynamicalSystem):
"""
q_next = x_next[:self.nq]
q_curr = x_curr[:self.nq]
dq = se3.difference(self._model, q_curr, q_next)
dv = x_next[self.nq:] - x_curr[self.nq:]
return np.vstack([dq, dv])
self._v[:self.nv] = se3.difference(self._model, q_curr, q_next)
self._v[self.nv:] = x_next[self.nq:] - x_curr[self.nq:]
return self._v
class SparseForwardDynamics(GeometricDynamicalSystem):
......@@ -165,6 +174,10 @@ class SparseForwardDynamics(GeometricDynamicalSystem):
discretizer = GeometricEulerDiscretizer()
GeometricDynamicalSystem.__init__(self, nq, nv, m, integrator, discretizer)
# Internal data for operators
self._q = np.matrix(np.zeros((self.nq,1)))
self._v = np.matrix(np.zeros((2*self.nv,1)))
def g(self, data, q, v, tau):
""" Compute the ABA and its derivatives and store the ABA result in data.
......@@ -220,7 +233,8 @@ class SparseForwardDynamics(GeometricDynamicalSystem):
:param dq: joint configuration displacement
:returns: the next configuration point
"""
return se3.integrate(self._model, q, dq)
np.copyto(self._q, se3.integrate(self._model, q, dq))
return self._q
def differenceConfiguration(self, x_next, x_curr):
""" Operator that differentiates the configuration state.
......@@ -228,8 +242,8 @@ class SparseForwardDynamics(GeometricDynamicalSystem):
:param x_next: next joint configuration and velocity [q_next, v_next]
:param x_curr: current joint configuration and velocity [q_curr, v_curr]
"""
q_next = x_next[:self._model.nq]
q_curr = x_curr[:self._model.nq]
dq = se3.difference(self._model, q_curr, q_next)
dv = x_next[self._model.nq:] - x_curr[self._model.nq:]
return np.vstack([dq, dv])
\ No newline at end of file
q_next = x_next[:self.nq]
q_curr = x_curr[:self.nq]
self._v[:self.nv] = se3.difference(self._model, q_curr, q_next)
self._v[self.nv:] = x_next[self.nq:] - x_curr[self.nq:]
return self._v
\ No newline at end of file
......@@ -70,24 +70,32 @@ def visualizePlan(robot, T, x0, X, frame_idx=None, path=False):
t0 = T[k+1]
it += 1
def plotDDPConvergence(J, gamma, theta, alpha):
def plotDDPConvergence(J, muLM, muV, gamma, theta, alpha):
import matplotlib.pyplot as plt
plt.figure(1)
plt.figure(1,figsize=(6.4, 8))
# Plotting the total cost sequence
plt.subplot(311)
plt.subplot(511)
plt.ylabel('cost')
plt.plot(J)
# Plotting the gradient sequence
plt.subplot(312)
plt.plot(gamma, label='gamma')
plt.plot(theta, label='theta')
# Ploting mu sequences
plt.subplot(512)
plt.ylabel('mu')
plt.plot(muLM, label='LM')
plt.plot(muV, label='V')
plt.legend()
plt.ylabel('gradient')
# Plotting the gradient sequence (gamma and theta)
plt.subplot(513)
plt.ylabel('gamma')
plt.plot(gamma)
plt.subplot(514)
plt.ylabel('theta')
plt.plot(theta)
# Plotting the alpha sequence
plt.subplot(313)
plt.subplot(515)
plt.ylabel('alpha')
ind = np.arange(len(alpha))
plt.bar(ind, alpha)
......
......@@ -93,8 +93,9 @@ print robot.framePosition(qf, frame_idx)
if plot:
J = ddp.getTotalCostSequence()
gamma, theta, alpha = ddp.getConvergenceSequence()
cddp.plotDDPConvergence(J, gamma, 1e-3*theta, alpha)
gamma, theta, alpha, muLM, muV = ddp.getConvergenceSequence()
cddp.plotDDPConvergence(J, muLM, muV, gamma, theta, alpha)
if display:
T = timeline
......
......@@ -14,4 +14,5 @@ ddp:
line_search:
min_stepsize: 1e-3
inc_rate: 2.
dec_rate: 0.5
\ No newline at end of file
dec_rate: 0.5
armijo_condition: 1e-3
\ No newline at end of file
......@@ -12,10 +12,10 @@ constraint = False
# Creating the system model
import rospkg
filename = str(os.path.dirname(os.path.abspath(__file__)))
path = rospkg.RosPack().get_path('talos_data')
urdf = path + '/robots/talos_left_arm.urdf'
os.environ['ROS_PACKAGE_PATH'] = filename
path = filename + '/talos_data/'
urdf = path + 'robots/talos_left_arm.urdf'
robot = se3.robot_wrapper.RobotWrapper(urdf, path)
model = robot.model
# system = cddp.NumDiffForwardDynamics(model)
......@@ -56,7 +56,7 @@ timeline = np.arange(0.0, 0.25, 1e-3) # np.linspace(0., 0.5, 51)
ddp = cddp.DDP(system, cost_manager, timeline, cddp.DDPDebug(robot))
# Configuration the solver from YAML file
ddp.setFromConfigFile(filename + "/arm_config.yaml")
ddp.setFromConfigFile(filename + "/talos_arm_config.yaml")
# Solving the problem
ddp.compute(x0)
......@@ -73,8 +73,9 @@ print robot.framePosition(qf, frame_idx)
if plot:
J = ddp.getTotalCostSequence()
gamma, theta, alpha = ddp.getConvergenceSequence()
cddp.plotDDPConvergence(J, gamma, theta, alpha)
gamma, theta, alpha, muLM, muV = ddp.getConvergenceSequence()
cddp.plotDDPConvergence(J, muLM, muV, gamma, theta, alpha)
if display:
T = timeline
......@@ -93,8 +94,8 @@ if constraint:
if plot:
J = ddp.getTotalCostSequence()
gamma, theta, alpha = ddp.getConvergenceSequence()
cddp.plotDDPConvergence(J, gamma, theta, alpha)
gamma, theta, alpha, muLM, muV = ddp.getConvergenceSequence()
cddp.plotDDPConvergence(J, muLM, muV, gamma, theta, alpha)
if display:
X = ddp.getStateTrajectory()
......
......@@ -14,4 +14,5 @@ ddp:
line_search:
min_stepsize: 1e-3
inc_rate: 2.
dec_rate: 0.5
\ No newline at end of file
dec_rate: 0.5
armijo_condition: 1e-3
\ No newline at end of file
talos_data @ 4e540b87
Subproject commit 4e540b87d420b1b065057a0a36964ccefceab713
......@@ -3,7 +3,6 @@ import numpy as np
import cddp
plot_enable = False
class LinearDDPTest(unittest.TestCase):
def setUp(self):
# Creating the dynamic model of the system and its integrator
......@@ -12,7 +11,7 @@ class LinearDDPTest(unittest.TestCase):
system = cddp.SpringMass(integrator, discretizer)
# Create random initial and desired state
x0 = np.random.rand(system.getConfigurationDimension(), 1)
self.x0 = np.random.rand(system.getConfigurationDimension(), 1)
x_des = np.random.rand(system.getConfigurationDimension(), 1)
x_des[1] = 0.
......@@ -32,34 +31,114 @@ class LinearDDPTest(unittest.TestCase):
cost_manager.addRunning(xu_cost)
# Creating the DDP solver
timeline = np.arange(0.0, 3., 0.001) # np.linspace(0., 0.5, 51)
timeline = np.arange(0.0, 0.5, 0.001) # np.linspace(0., 0.5, 51)
self.ddp = cddp.DDP(system, cost_manager, timeline)
def test_against_kkt_solution(self):
# Running the DDP solver
self.ddp.compute(x0)
self.ddp.compute(self.x0)
if plot_enable:
import cddp.utils as utils
t = timeline[1:-1]
x = np.asarray([np.asscalar(k.x[0]) for k in self.ddp.intervals[0:-2]])
u = np.asarray([np.asscalar(k.u[0]) for k in self.ddp.intervals[0:-2]])
utils.plot(t, x)
utils.plot(t, 0.001*u)
utils.show_plot()
# Creating the variables of the KKT problem
n = self.ddp.intervals[0].system.ndx
m = self.ddp.intervals[0].system.m
N = self.ddp.N
G = np.matrix(np.zeros((N*n+n, 1)))
gradJ = np.matrix(np.zeros((N*(n+m)+n, 1)))
gradG = np.matrix(np.zeros((N*(n+m)+n, N*n+n)))
hessL = np.matrix(np.zeros((N*(n+m)+n, N*(n+m)+n)))
# Running a backward-pass in order to update the derivatives of the
# DDP solution (no regularization and full Newton step)
self.ddp.backwardPass(0., 0., 1.)
# Building the KKT matrix and vector given the cost and dynamics derivatives
# from the DDP backward-pass
for k in range(N):
data = self.ddp.intervals[k]
# Running cost and its derivatives
lx = data.cost.total.lx
lu = data.cost.total.lu
lxx = data.cost.total.lxx
luu = data.cost.total.luu
lux = data.cost.total.lux
# Dynamics and its derivatives
f = data.system.f
fx = data.system.fx
fu = data.system.fu
# Updating the constraint and cost functions and them gradient, and the
# Hessian of this problem
G[k*n:(k+1)*n] = f
gradG[k*(n+m):(k+1)*(n+m), k*n:(k+1)*n] = np.block([ [fx.T],[fu.T] ])
gradJ[k*(n+m):(k+1)*(n+m)] = np.block([ [lx],[lu] ])
hessL[k*(n+m):(k+1)*(n+m),k*(n+m):(k+1)*(n+m)] = \
np.block([ [lxx, lux.T],[lux, luu] ])
# Updating the terms given the terminal state
G[N*n:(N+1)*n] = f
gradG[N*(n+m):(N+1)*(n+m), N*n:(N+1)*n] = fx.T
gradJ[N*(n+m):(N+1)*(n+m)] = lx
hessL[N*(n+m):(N+1)*(n+m), N*(n+m):(N+1)*(n+m)] = \
self.ddp.terminal_interval.cost.total.lxx
# Computing the KKT matrix and vector
kkt_mat = np.block([ [hessL,gradG],[gradG.T, np.zeros((N*n+n,N*n+n))] ])
kkt_vec = np.block([ [gradJ],[G] ])
# Solving the KKT problem
sol = np.linalg.solve(kkt_mat, kkt_vec)
# Recording the KKT solution into a list
X_kkt = []
U_kkt = []
for k in range(N):
w = sol[k*(n+m):(k+1)*(n+m)]
X_kkt.append(w[:n])
U_kkt.append(w[-m])
# Getting the DDP solution
X_opt = self.ddp.getStateTrajectory()
U_opt = self.ddp.getControlSequence()
for i in range(N-1):
# Checking the DDP solution is almost equals to KKT solution
self.assertTrue(np.allclose(X_kkt[i], X_opt[i], atol=1e-3),
"State KKT solution at " + str(i) + " is not the same.")
# self.assertTrue(np.allclose(U_kkt[i], U_opt[i], atol=1e-2),
# "Control KKT solution at " + str(i) + " is not the same.")
def test_positive_expected_improvement(self):
# Running the DDP solver
self.ddp.compute(self.x0)
self.assertGreater(-self.ddp.dV_exp, 0.,
"The expected improvement is not positive.")
def test_positive_obtained_improvement(self):
# Running the DDP solver
self.ddp.compute(self.x0)
self.assertGreater(-self.ddp.dV, 0.,
"The obtained improvement is not positive.")
def test_improvement_ratio_equals_one(self):
# Running the DDP solver
self.ddp.compute(self.x0)
self.assertAlmostEqual(
self.ddp.z, 1., 3, \
self.ddp.z, 1., 2, \
"The improvement ration is not equals to 1.")
def test_regularization_in_backward_pass(self):
# Backward-pass without regularization