Merge branch 'release-1.3.0'

Author: cknoll

pytrajectory/__init__.py

@@ -15,7 +15,7 @@
 from log import logging
 
 # current version
-__version__ = '1.2.0'
+__version__ = '1.3.0'
 
 # Placeholder for the datetime string of latest commit
 __date__ = "2016-01-15 14:12:08"

pytrajectory/auxiliary.py

@@ -6,6 +6,10 @@
 
 from log import logging, Timer
 
+
+class NanError(ValueError):
+    pass
+
 class IntegChain(object):
     '''
     This class provides a representation of an integrator chain.

pytrajectory/collocation.py

@@ -43,7 +43,7 @@ def __init__(self, sys, **kwargs):
         self._parameters = dict()
         self._parameters['tol'] = kwargs.get('tol', 1e-5)
         self._parameters['reltol'] = kwargs.get('reltol', 2e-5)
-        self._parameters['sol_steps'] = kwargs.get('sol_steps', 100)
+        self._parameters['sol_steps'] = kwargs.get('sol_steps', 50)
         self._parameters['method'] = kwargs.get('method', 'leven')
         self._parameters['coll_type'] = kwargs.get('coll_type', 'equidistant')
 
@@ -403,7 +403,7 @@ def get_guess(self):
         self.guess = guess
 
 
-    def solve(self, G, DG):
+    def solve(self, G, DG, new_solver=True):
         '''
         This method is used to solve the collocation equation system.
         
@@ -415,17 +415,25 @@ def solve(self, G, DG):
         
         DG : callable
             Function for the jacobian.
+        
+        new_solver : bool
+                     flag to determine whether a new solver instance should
+                     be initialized (default True)
         '''
 
         logging.debug("Solving Equation System")
 
         # create our solver
-        self.solver = Solver(F=G, DF=DG, x0=self.guess,
-                             tol=self._parameters['tol'],
-                             reltol=self._parameters['reltol'],
-                             maxIt=self._parameters['sol_steps'],
-                             method=self._parameters['method'])
-        
+        if new_solver:
+            self.solver = Solver(F=G, DF=DG, x0=self.guess,
+                                tol=self._parameters['tol'],
+                                reltol=self._parameters['reltol'],
+                                maxIt=self._parameters['sol_steps'],
+                                method=self._parameters['method'])
+        else:
+            # assume self.solver exists and at we already did a solution run
+            assert self.solver.solve_count > 0
+
         # solve the equation system
         self.sol = self.solver.solve()
 

pytrajectory/solver.py

@@ -3,6 +3,8 @@
 import scipy as scp
 import time
 
+from auxiliary import NanError
+
 from log import logging
 
 
@@ -34,7 +36,8 @@ class Solver:
         The solver to use
     '''
 
-    def __init__(self, F, DF, x0, tol=1e-5, reltol=2e-5, maxIt=100, method='leven'):
+    def __init__(self, F, DF, x0, tol=1e-5, reltol=2e-5, maxIt=50,
+                                            method='leven', mu=1e-4):
         self.F = F
         self.DF = DF
         self.x0 = x0
@@ -43,6 +46,13 @@ def __init__(self, F, DF, x0, tol=1e-5, reltol=2e-5, maxIt=100, method='leven'):
         self.maxIt = maxIt
         self.method = method
 
+        self.solve_count = 0
+        
+        # this is LM specific
+        self.mu=mu
+        self.res = 1
+        self.res_old = -1
+        
         self.sol = None
 
 
@@ -52,6 +62,8 @@ def solve(self):
         collocation equation system.
         '''
 
+        self.solve_count += 1
+
         if (self.method == 'leven'):
             logging.debug("Run Levenberg-Marquardt method")
             self.leven()
@@ -72,20 +84,17 @@ def leven(self):
         '''
         i = 0
         x = self.x0
-        res = 1
-        res_alt = -1
 
         eye = scp.sparse.identity(len(self.x0))
 
         #mu = 1.0
-        mu = 1e-4
-        mu_old = mu
+        self.mu = 1e-4
 
         # borders for convergence-control
         b0 = 0.2
         b1 = 0.8
 
-        roh = 0.0
+        rho = 0.0
 
         reltol = self.reltol
 
@@ -94,7 +103,9 @@ def leven(self):
         # measure the time for the LM-Algorithm
         T_start = time.time()
 
-        while((res > self.tol) and (self.maxIt > i) and (abs(res-res_alt) > reltol)):
+        break_outer_loop = False
+        
+        while (not break_outer_loop):
             i += 1
 
             #if (i-1)%4 == 0:
@@ -103,7 +114,7 @@ def leven(self):
 
             break_inner_loop = False
             while (not break_inner_loop):                
-                A = DFx.T.dot(DFx) + mu**2*eye
+                A = DFx.T.dot(DFx) + self.mu**2*eye
 
                 b = DFx.T.dot(Fx)
 
@@ -113,6 +124,11 @@ def leven(self):
 
                 Fxs = self.F(xs)
 
+                if any(np.isnan(Fxs)):
+                    # this might be caused by too small mu
+                    msg = "Invalid start guess (leads to nan)"
+                    raise NanError(msg)
+
                 normFx = norm(Fx)
                 normFxs = norm(Fxs)
 
@@ -121,47 +137,72 @@ def leven(self):
 
                 R1 = (normFx - normFxs)
                 R2 = (normFx - (norm(Fx+DFx.dot(s))))
-                roh = R1 / R2
+                rho = R1 / R2
 
                 # note smaller bigger mu means less progress but
                 # "more regular" conditions
 
                 if R1 < 0 or R2 < 0:
                     # the step was too big -> residuum would be increasing
-                    mu*= 2
-                    roh = 0.0 # ensure another iteration
+                    self.mu *= 2
+                    rho = 0.0 # ensure another iteration
 
                     #logging.debug("increasing res. R1=%f, R2=%f, dismiss solution" % (R1, R2))
 
-                elif (roh<=b0):
-                    mu = 2*mu
-                elif (roh>=b1):
-                    
-                    mu = 0.5*mu
+                elif (rho<=b0):
+                    self.mu *= 2
+                elif (rho>=b1):
+                    self.mu *= 0.5
 
-                # -> if b0 < roh < b1 : leave mu unchanged
-                
-                logging.debug("  roh= %f    mu= %f"%(roh,mu))
+                # -> if b0 < rho < b1 : leave mu unchanged
 
-                if roh < 0:
-                    logging.warn("roh < 0 (should not happen)")
+                logging.debug("  rho= %f    mu= %f"%(rho, self.mu))
+
+                if np.isnan(rho):
+                    # this should might be caused by large values for xs
+                    # but it should have been catched above
+                    logging.warn("rho = nan (should not happen)")
+                    raise NanError()
+               
+                if rho < 0:
+                    logging.warn("rho < 0 (should not happen)")
 
                 # if the system more or less behaves linearly 
-                break_inner_loop = roh > b0
+                break_inner_loop = rho > b0
 
             Fx = Fxs
             x = xs
 
-            #roh = 0.0
-            res_alt = res
-            res = normFx
-            if i>1 and res > res_alt:
+            # store for possible future usage
+            self.x0 = xs
+            
+            #rho = 0.0
+            self.res_old = self.res
+            self.res = normFx
+            if i>1 and self.res > self.res_old:
                 logging.warn("res_old > res  (should not happen)")
 
-            logging.debug("nIt= %d    res= %f"%(i,res))
+            logging.debug("nIt= %d    res= %f"%(i,self.res))
+            
+            self.cond_abs_tol = self.res <= self.tol
+            self.cond_rel_tol = abs(self.res-self.res_old) <= reltol
+            self.cond_num_steps = i >= self.maxIt
+            
+            break_outer_loop = self.cond_abs_tol or self.cond_rel_tol \
+                                                 or self.cond_num_steps
 
         # LM Algorithm finished
         T_LM = time.time() - T_start
+        
+        if i == 0:
+            from IPython import embed as IPS
+            IPS()
+        
         self.avg_LM_time = T_LM / i
 
+        # Note: if self.cond_num_steps == True, the LM-Algorithm was stopped
+        # due to maximum number of iterations
+        # -> it might be worth to continue 
+        
+        
         self.sol = x

pytrajectory/system.py

@@ -60,6 +60,7 @@ class ControlSystem(object):
         eps           1e-2            Tolerance for the solution of the initial value problem
         ierr          1e-1            Tolerance for the error on the whole interval
         tol           1e-5            Tolerance for the solver of the equation system
+        dt_sim        1e-2            Sample time for integration (initial value problem)
         use_chains    True            Whether or not to use integrator chains
         sol_steps     100             Maximum number of iteration steps for the eqs solver
         first_guess   None            to initiate free parameters (might be useful: {'seed': value})
@@ -72,6 +73,8 @@ def __init__(self, ff, a=0., b=1., xa=[], xb=[], ua=[], ub=[], constraints=None,
         self._parameters['maxIt'] = kwargs.get('maxIt', 10)
         self._parameters['eps'] = kwargs.get('eps', 1e-2)
         self._parameters['ierr'] = kwargs.get('ierr', 1e-1)
+        self._parameters['dt_sim'] = kwargs.get('dt_sim', 0.01)
+        self._parameters['accIt'] = kwargs.get('accIt', 5)
 
         # create an object for the dynamical system
         self.dyn_sys = DynamicalSystem(f_sym=ff, a=a, b=b, xa=xa, xb=xb, ua=ua, ub=ub)
@@ -108,7 +111,7 @@ def set_param(self, param='', value=None):
             The new value
         '''
 
-        if param in {'maxIt', 'eps', 'ierr'}:
+        if param in {'maxIt', 'eps', 'ierr', 'dt_sim'}:
             self._parameters[param] = value
 
         elif param in {'n_parts_x', 'sx', 'n_parts_u', 'su', 'kx', 'use_chains', 'nodes_type', 'use_std_approach'}:
@@ -262,13 +265,18 @@ def solve(self):
 
         # do the first iteration step
         logging.info("1st Iteration: {} spline parts".format(self.eqs.trajectories.n_parts_x))
-        self._iterate()
-        
+        try:        
+            self._iterate()
+        except auxiliary.NanError:
+            logging.warn("NanError")
+            return None, None
+
         # this was the first iteration
         # now we are getting into the loop
         self.nIt = 1
 
         while not self.reached_accuracy and self.nIt < self._parameters['maxIt']:
+            
             # raise the number of spline parts
             self.eqs.trajectories._raise_spline_parts()
 
@@ -280,8 +288,12 @@ def solve(self):
                 logging.info("{}th Iteration: {} spline parts".format(self.nIt+1, self.eqs.trajectories.n_parts_x))
 
             # start next iteration step
-            self._iterate()
-
+            try:        
+                self._iterate()
+            except auxiliary.NanError:
+                logging.warn("NanError")
+                return None, None
+            
             # increment iteration number
             self.nIt += 1
 
@@ -308,6 +320,16 @@ def _iterate(self):
 
         As a last, the resulting initial value problem is simulated.
         '''
+        
+        # Note: in pytrajectory there are Three main levels of 'iteration'
+        # Level 3: perform one LM-Step (i.e. calculate a new set of parameters) 
+        # This is implemented in solver.py. Ends when tolerances are met or
+        # the maximum number of steps is reached
+        # Level 2: restarts the LM-Algorithm with the last values
+        # and stops if the desired accuracy for the initial value problem
+        # is met or if the maximum number of steps solution attempts is reached
+        # Level 1: increasing the spline number.
+        # In Each step solve a nonlinear optimization problem (with LM)
 
         # Initialise the spline function objects
         self.eqs.trajectories.init_splines()
@@ -320,16 +342,40 @@ def _iterate(self):
         G, DG = C.G, C.DG
 
         # Solve the collocation equation system
-        sol = self.eqs.solve(G, DG)
 
-        # Set the found solution
-        self.eqs.trajectories.set_coeffs(sol)
+        new_solver = True
+        while True:
+            sol = self.eqs.solve(G, DG, new_solver=new_solver)
+            
+            # in the following iterations we want to use the same solver
+            # object (we just had an intermediate look, whether the solution
+            # of the initial value problem is already sufficient accurate.)
+            
+            new_solver = False
 
-        # Solve the resulting initial value problem
-        self.simulate()
-        
-        # check if desired accuracy is reached
-        self.check_accuracy()
+            # Set the found solution
+            self.eqs.trajectories.set_coeffs(sol)
+
+            # Solve the resulting initial value problem
+            self.simulate()
+
+            # check if desired accuracy is reached
+            self.check_accuracy()
+
+            # any of the follwing  conditions ends the loop
+            slvr = self.eqs.solver
+            cond1 = self.reached_accuracy
+            
+            # following means: solver stopped not
+            # only because of maximum stepp             # number
+            cond2 = (not slvr.cond_num_steps) or slvr.cond_abs_tol \
+                                              or slvr.cond_rel_tol
+            cond3 = self.eqs.solver.solve_count >= self._parameters['accIt']
+
+            if cond1 or cond2 or cond3:
+                break
+            else:
+                logging.debug('New attempt\n\n')
 
     def simulate(self):
         '''
@@ -358,7 +404,7 @@ def simulate(self):
             start.append(start_dict[x])
 
         # create simulation object
-        S = Simulator(ff, T, start, self.eqs.trajectories.u)
+        S = Simulator(ff, T, start, self.eqs.trajectories.u, dt=self._parameters['dt_sim'])
 
         logging.debug("start: %s"%str(start))