#57 attempt to refactor complete toolbox

examples/p2p_agv.py → examples/benchmark_examples/p2p_agv.py

@@ -16,7 +16,8 @@
 # You should have received a copy of the GNU Lesser General Public
 # License along with this program; if not, write to the Free Software
 # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
-
+import sys, os
+sys.path.insert(0, os.getcwd()+'/../..')
 from omgtools import *
 
 # create vehicle

examples/p2p_agv2.py → examples/benchmark_examples/p2p_agv2.py

@@ -16,7 +16,8 @@
 # You should have received a copy of the GNU Lesser General Public
 # License along with this program; if not, write to the Free Software
 # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
-
+import sys, os
+sys.path.insert(0, os.getcwd()+'/../..')
 from omgtools import *
 
 # create vehicle

examples/p2p_holonomic_orient.py → examples/benchmark_examples/p2p_holonomic_orient.py

@@ -17,6 +17,8 @@
 # License along with this program; if not, write to the Free Software
 # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
 
+import sys, os
+sys.path.insert(0, os.getcwd()+'/../..')
 from omgtools import *
 
 # create vehicle

examples/compare_distributed_optimization_quadrotors.py

@@ -58,7 +58,7 @@
     splines = problem.father.get_variables(vehicle, 'splines0')
     pos_c = vehicle.get_fleet_center(
         splines, vehicle.rel_pos_c, substitute=False)
-    pos_c = np.hstack([c.coeffs for c in pos_c])
+    pos_c = np.hstack([c.data() for c in pos_c])
     var_central = np.vstack((var_central, np.c_[pos_c]))
 
 # create & solve ADMM problem

examples/deployer_example.py

@@ -60,9 +60,6 @@
 
 n_samp = int(np.round(update_time/sample_time, 6))
 t00 = time.time()
-
-t00 = time.time()
-
 for via_point, obstacle_pos in zip(via_points, obstacle_positions):
     vehicle.set_terminal_conditions(via_point)
     target_reached = False

examples/formation_holonomic_export.py → examples/notworking/formation_holonomic_export.py

@@ -16,7 +16,8 @@
 # You should have received a copy of the GNU Lesser General Public
 # License along with this program; if not, write to the Free Software
 # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
-
+import sys, os
+sys.path.insert(0, os.getcwd()+'/..')
 from omgtools import *
 import os
 import csv

examples/rendezvous_holonomic_export.py → examples/notworking/rendezvous_holonomic_export.py

@@ -16,7 +16,8 @@
 # You should have received a copy of the GNU Lesser General Public
 # License along with this program; if not, write to the Free Software
 # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
-
+import sys, os
+sys.path.insert(0, os.getcwd()+'/..')
 from omgtools import *
 import os
 import csv

examples/p2p_trailer.py

@@ -42,13 +42,13 @@
 problem.father.add(vehicle)  # add vehicle to optifather, such that it knows the trailer variables
 problem.vehicles.append(vehicle)
 # todo: isn't there are a cleaner way?
-vehicle.to_simulate = False 
+vehicle.to_simulate = False
 # extra solver settings which may improve performance
 problem.set_options({'solver_options': {'ipopt': {'ipopt.hessian_approximation': 'limited-memory', 'ipopt.linear_solver': 'ma57'}}})
 problem.init()
 
 # problem.set_options({'hard_term_con': True, 'horizon_time': 12})
-# vehicle.problem = problem  # to plot error 
+# vehicle.problem = problem  # to plot error
 
 # create simulator
 simulator = Simulator(problem)

examples/revolving_door_diffdrive.py

@@ -60,7 +60,7 @@
     problem.set_options(
         {'solver_options': {'ipopt': {'ipopt.linear_solver': 'ma57',
         							  'ipopt.hessian_approximation': 'limited-memory',
-        							  'ipopt.warm_start_mult_bound_push': 1e-6}}}) 
+        							  'ipopt.warm_start_mult_bound_push': 1e-6}}})
 problem.init()
 
 # problem.export2AMPL()

omgtools/basics/spline.py

@@ -3,80 +3,11 @@
 except ImportError:
     print "meco_binaries not found."
 import splines as spl
-# import spline_old as spl_old
-# import spline_extra as spl_ex
 import numpy as np
 from scipy.interpolate import splev
 from casadi import SX, MX, DM, Function
 
 
-""" helper functions -> should vanish """
-""" ================================= """
-
-
-# def old2new(spline_old):
-#     basis = spl.BSplineBasis(spline_old.basis.knots, spline_old.basis.degree)
-#     coeffs = spline_old.coeffs
-#     return spl.Function(basis, coeffs)
-
-
-# def new2old(spline_new):
-#     basis = spl_old.BSplineBasis(spline_new.basis().knots(), spline_new.basis().degree())
-#     coeffs = spline_new.data()
-#     return spl_old.BSpline(basis, coeffs)
-
-
-# def new2oldbasis(basis_new):
-#     return spl_old.BSplineBasis(basis_new.knots(), basis_new.degree())
-
-
-""" methods desired to implement in spline toolbox -> should vanish """
-""" =============================================================== """
-
-
-# def __add__(self, other):
-#     if isinstance(other, self.__class__):
-#         return old2new(new2old(self).__add__(new2old(other)))
-#     else:
-#         return old2new(new2old(self).__add__(other))
-
-
-# def __neg__(self):
-#     return old2new(new2old(self).__neg__())
-
-
-# def __sub__(self, other):
-#     return self + (-other)
-
-
-# def __rsub__(self, other):
-#     return other + (-self)
-
-
-# def __mul__(self, other):
-#     if isinstance(other, self.__class__):
-#         return old2new(new2old(self).__mul__(new2old(other)))
-#     else:
-#         return old2new(new2old(self).__mul__(other))
-
-
-# def __rmul__(self, other):
-#     return self.__mul__(other)
-
-
-# def __pow__(self, power):
-#     return old2new(new2old(self).__pow__(power))
-
-
-# spl.Function.__add__ = __add__
-# spl.Function.__radd__ = __add__
-# spl.Function.__neg__ = __neg__
-# spl.Function.__sub__ = __sub__
-# spl.Function.__rsub__ = __rsub__
-# spl.Function.__mul__ = __mul__
-# spl.Function.__rmul__ = __rmul__
-# spl.Function.__pow__ = __pow__
-
 """ extra BSplineBasis functions """
 """ ============================ """
 
@@ -104,64 +35,6 @@ def extrapolate_basis(basis, t_extra, m=None):
     b3, Tin = b2.insert_knots([knots[-1]]*m)
     return b3, Tin.dot(Tex)
 
-# def extrapolate_basis(basis, t_extra, m=None):
-#     # Create transformation matrix that extrapolates the spline over an extra
-#     # knot interval of t_extra long.
-#     knots = basis.knots()
-#     deg = basis.degree()
-#     N = basis.dimension()
-#     if m is None:
-#         # m is number of desired knots at interpolation border
-#         # default value is # knots at second last knot place of original spline
-#         m = 1
-#         while knots[-deg-2-m] >= knots[-deg-2]:
-#             m += 1
-#     knots2 = np.r_[knots[:-deg-1], knots[-deg-1]*np.ones(m),
-#                    (knots[-1]+t_extra)*np.ones(deg+1)]
-#     basis2 = spl.BSplineBasis(knots2, deg)
-#     A = np.zeros((deg+1, deg+1))
-#     B = np.zeros((deg+1, deg+1))
-#     # only (deg+1) last coefficients change: we need (deg+1) equations, giving
-#     # a relation between (deg+1) last coefficients before and after extrapolation
-
-#     # (deg+1-m) relations based on evaluation of basis functions on (deg+1-m)
-#     # last greville points
-#     if m < deg+1:
-#         eval_points = basis.greville()[-(deg+1-m):]
-#         a = np.c_[[basis2(e)[-(deg+1+m):-m] for e in eval_points]]
-#         b = np.c_[[basis(e)[-(deg+1):] for e in eval_points]]
-#         a1, a2 = a[:, :m], a[:, m:]
-#         b1, b2 = b[:, :m], b[:, m:]
-#         A[:(deg+1-m), -(deg+1):-m] = a2
-#         B[:(deg+1-m), :m] = b1 - a1  # this should be zeros
-#         B[:(deg+1-m), m:] = b2
-#     else:
-#         A[0, -(deg+1)] = 1.
-#         B[0, -1] = 1.
-#     # m relations based on continuity of m last derivatives
-#     A1, B1 = np.identity(deg+1), np.identity(deg+1)
-#     for i in range(1, deg+1):
-#         A1_tmp = np.zeros((deg+1-i, deg+1-i+1))
-#         B1_tmp = np.zeros((deg+1-i, deg+1-i+1))
-#         for j in range(deg+1-i):
-#             B1_tmp[j, j] = -(deg+1-i)/(knots[j+N] - knots[j+N-deg-1+i])
-#             B1_tmp[j, j+1] = (deg+1-i)/(knots[j+N] - knots[j+N-deg-1+i])
-#             A1_tmp[j, j] = -(deg+1-i)/(knots2[j+N+m] - knots2[j+N-deg-1+m+i])
-#             A1_tmp[j, j+1] = (deg+1-i)/(knots2[j+N+m] - knots2[j+N-deg-1+m+i])
-#         A1, B1 = A1_tmp.dot(A1), B1_tmp.dot(B1)
-#         if i >= deg+1-m:
-#             b1 = B1[-1, :]
-#             a1 = A1[-(deg-i+1), :]
-#             A[i, :] = a1
-#             B[i, :] = b1
-#     # put everything in transformation matrix
-#     _T = np.linalg.solve(A, B)
-#     _T[abs(_T) < 1e-10] = 0.
-#     T = np.zeros((N+m, N))
-#     T[:N, :N] = np.eye(N)
-#     T[-(deg+1):, -(deg+1):] = _T
-#     return spl.BSplineBasis(knots2, deg), T
-
 
 def shiftoverknot_basis(basis):
     knots = basis.knots()
@@ -189,6 +62,7 @@ def shiftfirstknot_basis(basis, t_shift):
     knots2 = b2.knots()
     return spl.BSplineBasis(knots2[degree+1:], degree), T[degree+1:, :]
 
+
 # def shiftfirstknot_basis(basis, t_shift):
 #     knots = basis.knots()
 #     b2, T =  basis.kick_boundary([t_shift, knots[-1]])

omgtools/export/export.py

@@ -366,17 +366,17 @@ def _create_initSplines(self, father, problem):
 
     def _create_transformSplines(self, father, problem, point2point):
         code, cnt = '', 0
-        tf_len = len(problem.vehicles[0].basis)
+        tf_len = problem.vehicles[0].basis.dimension()
         if point2point.__class__.__name__ == 'FixedTPoint2point':
             code += '\tint interval_prev = (int)(round((current_time_prev*(vehicle->getKnotIntervals())/horizon_time)*1.e6)/1.e6);\n'
             code += '\tint interval_now = (int)(round((current_time*(vehicle->getKnotIntervals())/horizon_time)*1.e6)/1.e6);\n'
             code += '\tif(interval_now > interval_prev){\n'
-            code += ('\t\tvector<double> spline_tf(' + str(len(problem.vehicles[0].basis)) + ');\n')
+            code += ('\t\tvector<double> spline_tf(' + str(problem.vehicles[0].basis.dimension()) + ');\n')
             for label, child in father.children.items():
                 for name, var in child._variables.items():
                     if name in child._splines_prim:
-                        if (len(child._splines_prim[name]['basis']) > tf_len):
-                            tf_len = len(child._splines_prim[name]['basis'])
+                        if (child._splines_prim[name]['basis'].dimension() > tf_len):
+                            tf_len = child._splines_prim[name]['basis'].dimension()
                             code += ('\t\tspline_tf.resize(' + str(tf_len)+');\n')
                         tf = 'splines_tf["'+name+'"]'
                         code += ('\t\tfor(int k=0; k<' +

omgtools/export/export_admm.py

@@ -105,9 +105,9 @@ def _create_spline_tf(self, father, problem):
             for name, ind in q_i.items():
                 if name in child._splines_prim:
                     basis = child._splines_prim[name]['basis']
-                    for l in range(len(basis)):
-                        sl_min = l*len(basis)
-                        sl_max = (l+1)*len(basis)
+                    for l in range(basis.dimension()):
+                        sl_min = l*basis.dimension()
+                        sl_max = (l+1)*basis.dimension()
                         if set(range(sl_min, sl_max)) <= set(ind):
                             spl = child._splines_prim[name]
                             if spl['init'] is not None:
@@ -150,9 +150,9 @@ def _create_initSplines(self, father, problem):
             for name, ind in q_i.items():
                 if name in child._splines_prim:
                     basis = child._splines_prim[name]['basis']
-                    for l in range(len(basis)):
-                        sl_min = l*len(basis)
-                        sl_max = (l+1)*len(basis)
+                    for l in range(basis.dimension()):
+                        sl_min = l*basis.dimension()
+                        sl_max = (l+1)*basis.dimension()
                         if set(range(sl_min, sl_max)) <= set(ind):
                             code += '\tsplines_tf["xvar_'+name+'"] = XVAR_'+name.upper()+'_TF;\n'
         return {'initSplines': code}
@@ -169,18 +169,18 @@ def _create_transformSharedSplines(self, father, problem, point2point):
             for name, ind in q_i.items():
                 if name in child._splines_prim:
                     basis = child._splines_prim[name]['basis']
-                    for l in range(len(basis)):
-                        sl_min = l*len(basis)
-                        sl_max = (l+1)*len(basis)
+                    for l in range(basis.dimension()):
+                        sl_min = l*basis.dimension()
+                        sl_max = (l+1)*basis.dimension()
                         if set(range(sl_min, sl_max)) <= set(ind):
-                            code += ('\t\tfor(int i=0; i<' + str(len(basis)) + '; i++){\n')
+                            code += ('\t\tfor(int i=0; i<' + str(basis.dimension()) + '; i++){\n')
                             for var in ['x_i', 'z_i', 'l_i']:
                                 code += ('\t\t\t' + var + '_tf['+str(cnt)+'+i] = 0;\n')
                             code += ('\t\t\tfor(int k=0; k<n_nghb; k++){\n')
                             for var in ['z_ji', 'l_ji', 'z_ij', 'l_ij']:
                                 code += ('\t\t\t\t' + var + '_tf['+str(cnt)+'+i+k*n_shared] = 0;\n')
                             code += '\t\t\t}\n'
-                            code += ('\t\t\tfor(int j=0; j<' + str(len(basis)) + '; j++){\n')
+                            code += ('\t\t\tfor(int j=0; j<' + str(basis.dimension()) + '; j++){\n')
                             for var in ['x_i', 'z_i', 'l_i']:
                                 code += ('\t\t\t\t'+var+'_tf['+str(cnt)+'+i] += splines_tf["xvar_'+name+'"][i][j]*variables_admm["'+var+'"]['+str(cnt)+'+j];\n')
                             code += ('\t\t\t\tfor(int k=0; k<n_nghb; k++){\n')

omgtools/problems/admm.py

@@ -85,7 +85,6 @@ def construct_upd_x(self, problem=None):
             # transform spline variables: only consider future piece of spline
             tf = lambda cfs, basis: spl.Function(basis, cfs).shiftfirstknot_fwd(t0).data()
             # tf = lambda cfs, basis: mtimes(basis.shiftfirstknot(t0)[1], cfs)
-            self._transform_spline([x_i], tf, self.q_i)
             self._transform_spline([x_i, z_i, l_i], tf, self.q_i)
             self._transform_spline([z_ji, l_ji], tf, self.q_ji)
             # construct objective

omgtools/problems/dualdecomposition.py

@@ -18,7 +18,7 @@
 # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
 
 from ..basics.optilayer import OptiFather, create_function
-from ..basics.spline_extra import shift_knot1_fwd, shiftoverknot
+from ..basics.spline import *
 from problem import Problem
 from dualmethod import DualUpdater, DualProblem
 from casadi import symvar, mtimes, MX, reshape, substitute
@@ -88,7 +88,7 @@ def construct_upd_xz(self, problem=None):
             T = self.define_symbol('T')
             t0 = t/T
             # transform spline variables: only consider future piece of spline
-            tf = lambda cfs, basis: shift_knot1_fwd(cfs, basis, t0)
+            tf = lambda cfs, basis: spl.Function(basis, cfs).shiftfirstknot_fwd(t0).data()
             self._transform_spline(x_i, tf, self.q_i)
             self._transform_spline([z_ij, l_ij], tf, self.q_ij)
             self._transform_spline(l_ji, tf, self.q_ji)
@@ -232,7 +232,7 @@ def init_step(self, current_time, update_time):
         # transform spline variables
         if ((current_time > 0. and
              np.round(current_time, 6) % self.problem.knot_time == 0)):
-            tf = shiftoverknot
+            tf = lambda cfs, basis: basis.shiftoverknot()[1].dot(cfs)
             for key in ['x_i']:
                 self.var_dd[key] = self._transform_spline(
                     self.var_dd[key], tf, self.q_i)
@@ -247,8 +247,7 @@ def get_residuals(self, current_time):
         t0 = time.time()
         current_time = np.round(current_time, 6) % self.problem.knot_time
         horizon_time = self.problem.options['horizon_time']
-        tf = lambda cfs, basis: shift_knot1_fwd(
-            cfs, basis, current_time/horizon_time)
+        tf = lambda cfs, basis: spl.Function(basis, cfs).shiftfirstknot_fwd(t0).data()
         x_j = self._transform_spline(self.var_dd['x_j'], tf, self.q_ij).cat
         z_ij = self._transform_spline(self.var_dd['z_ij'], tf, self.q_ij).cat
         pr = la.norm(x_j-z_ij)**2

omgtools/problems/formation_central.py

@@ -18,7 +18,6 @@
 # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
 
 from point2point import FixedTPoint2point
-from ..basics.spline_extra import definite_integral
 from casadi import inf
 
 
@@ -70,7 +69,7 @@ def construct(self):
                     if self.options['soft_formation']:
                         weight = self.options['soft_formation_weight']
                         eps = self.define_spline_variable('eps_form_'+str(ind_v)+str(ind_n), basis=veh.basis)[0]
-                        obj = weight*definite_integral(eps, t/T, 1.)
+                        obj = weight*eps.integral([t/T, 1.])
                         self.define_objective(obj)
                         self.define_constraint(pos_c_veh[ind_v] - pos_c_nghb[ind_n] - eps, -inf, 0.)
                         self.define_constraint(-pos_c_veh[ind_v] + pos_c_nghb[ind_n] - eps, -inf, 0.)

omgtools/problems/formation_dualdec.py

@@ -57,7 +57,7 @@ def construct(self):
         # terminal constraints (stability issue)
         for veh in self.vehicles:
             for spline in centra[veh]:
-                for d in range(1, spline.basis.degree+1):
+                for d in range(1, spline.basis().degree()+1):
                     # constraints imposed on distributedproblem instance will be
                     # invoked on the z-variables (because it is interpreted as
                     # 'interconnection constraint')