From 1c507c05cec6d99d142b4c3ef518e46dd1d2db5e Mon Sep 17 00:00:00 2001 From: rubenvanparys Date: Tue, 11 Apr 2017 18:16:50 +0200 Subject: [PATCH] #57 attempt to refactor complete toolbox --- examples/{ => benchmark_examples}/p2p_agv.py | 3 +- examples/{ => benchmark_examples}/p2p_agv2.py | 3 +- .../p2p_holonomic_orient.py | 2 + ...are_distributed_optimization_quadrotors.py | 2 +- examples/deployer_example.py | 3 - .../{ => notworking}/compare_buildoptions.py | 0 .../compare_buildoptions_distributed.py | 0 .../formation_holonomic_export.py | 3 +- .../rendezvous_holonomic_export.py | 3 +- examples/p2p_trailer.py | 4 +- examples/revolving_door_diffdrive.py | 2 +- omgtools/basics/spline.py | 128 +----------------- omgtools/export/export.py | 8 +- omgtools/export/export_admm.py | 22 +-- omgtools/problems/admm.py | 1 - omgtools/problems/dualdecomposition.py | 9 +- omgtools/problems/formation_central.py | 3 +- omgtools/problems/formation_dualdec.py | 2 +- omgtools/problems/problem.py | 12 +- omgtools/vehicles/agv.py | 31 ++--- omgtools/vehicles/bicycle.py | 2 +- omgtools/vehicles/dubins.py | 3 +- omgtools/vehicles/holonomic.py | 2 +- omgtools/vehicles/holonomic1d.py | 6 +- omgtools/vehicles/holonomic3d.py | 6 +- omgtools/vehicles/holonomicorient.py | 16 +-- omgtools/vehicles/quadrotor.py | 3 +- omgtools/vehicles/quadrotor3d.py | 2 +- omgtools/vehicles/trailer.py | 6 +- omgtools/vehicles/vehicle.py | 2 +- 30 files changed, 84 insertions(+), 205 deletions(-) rename examples/{ => benchmark_examples}/p2p_agv.py (97%) rename examples/{ => benchmark_examples}/p2p_agv2.py (97%) rename examples/{ => benchmark_examples}/p2p_holonomic_orient.py (97%) rename examples/{ => notworking}/compare_buildoptions.py (100%) rename examples/{ => notworking}/compare_buildoptions_distributed.py (100%) rename examples/{ => notworking}/formation_holonomic_export.py (98%) rename examples/{ => notworking}/rendezvous_holonomic_export.py (98%) diff --git a/examples/p2p_agv.py b/examples/benchmark_examples/p2p_agv.py similarity index 97% rename from examples/p2p_agv.py rename to examples/benchmark_examples/p2p_agv.py index a904f21..1afd00a 100644 --- a/examples/p2p_agv.py +++ b/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 diff --git a/examples/p2p_agv2.py b/examples/benchmark_examples/p2p_agv2.py similarity index 97% rename from examples/p2p_agv2.py rename to examples/benchmark_examples/p2p_agv2.py index 558c4c7..8285b2c 100644 --- a/examples/p2p_agv2.py +++ b/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 diff --git a/examples/p2p_holonomic_orient.py b/examples/benchmark_examples/p2p_holonomic_orient.py similarity index 97% rename from examples/p2p_holonomic_orient.py rename to examples/benchmark_examples/p2p_holonomic_orient.py index 2cc1952..a459d19 100644 --- a/examples/p2p_holonomic_orient.py +++ b/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 diff --git a/examples/compare_distributed_optimization_quadrotors.py b/examples/compare_distributed_optimization_quadrotors.py index 94d7c2e..c82a6f7 100644 --- a/examples/compare_distributed_optimization_quadrotors.py +++ b/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 diff --git a/examples/deployer_example.py b/examples/deployer_example.py index 8b021bd..46cf9ac 100644 --- a/examples/deployer_example.py +++ b/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 diff --git a/examples/compare_buildoptions.py b/examples/notworking/compare_buildoptions.py similarity index 100% rename from examples/compare_buildoptions.py rename to examples/notworking/compare_buildoptions.py diff --git a/examples/compare_buildoptions_distributed.py b/examples/notworking/compare_buildoptions_distributed.py similarity index 100% rename from examples/compare_buildoptions_distributed.py rename to examples/notworking/compare_buildoptions_distributed.py diff --git a/examples/formation_holonomic_export.py b/examples/notworking/formation_holonomic_export.py similarity index 98% rename from examples/formation_holonomic_export.py rename to examples/notworking/formation_holonomic_export.py index 053e61c..0cb81e9 100644 --- a/examples/formation_holonomic_export.py +++ b/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 diff --git a/examples/rendezvous_holonomic_export.py b/examples/notworking/rendezvous_holonomic_export.py similarity index 98% rename from examples/rendezvous_holonomic_export.py rename to examples/notworking/rendezvous_holonomic_export.py index fae3904..8c2f4c7 100644 --- a/examples/rendezvous_holonomic_export.py +++ b/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 diff --git a/examples/p2p_trailer.py b/examples/p2p_trailer.py index e48bac6..f15552d 100644 --- a/examples/p2p_trailer.py +++ b/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) diff --git a/examples/revolving_door_diffdrive.py b/examples/revolving_door_diffdrive.py index 44a1c1a..cb9913e 100644 --- a/examples/revolving_door_diffdrive.py +++ b/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() diff --git a/omgtools/basics/spline.py b/omgtools/basics/spline.py index b959e9f..2067e85 100644 --- a/omgtools/basics/spline.py +++ b/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]]) diff --git a/omgtools/export/export.py b/omgtools/export/export.py index 9b405f9..d3f421c 100644 --- a/omgtools/export/export.py +++ b/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 spline_tf(' + str(len(problem.vehicles[0].basis)) + ');\n') + code += ('\t\tvector 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<' + diff --git a/omgtools/export/export_admm.py b/omgtools/export/export_admm.py index 9ebaafd..8759d9c 100644 --- a/omgtools/export/export_admm.py +++ b/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 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 diff --git a/omgtools/problems/formation_central.py b/omgtools/problems/formation_central.py index 7e3990f..867d965 100644 --- a/omgtools/problems/formation_central.py +++ b/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.) diff --git a/omgtools/problems/formation_dualdec.py b/omgtools/problems/formation_dualdec.py index 5a9b8e9..82838ca 100644 --- a/omgtools/problems/formation_dualdec.py +++ b/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') diff --git a/omgtools/problems/problem.py b/omgtools/problems/problem.py index 62432ee..a4af8aa 100644 --- a/omgtools/problems/problem.py +++ b/omgtools/problems/problem.py @@ -18,6 +18,7 @@ # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA from ..basics.optilayer import OptiFather, OptiChild +from ..basics.spline import * from ..vehicles.fleet import get_fleet_vehicles from ..execution.plotlayer import PlotLayer from itertools import groupby @@ -180,10 +181,15 @@ def sleep(self, current_time, sleep_time, sample_time): spline_values = vehicle.signals['splines'][:, -1] spline_values = [self.father.get_variables( vehicle, 'splines'+str(k), spline=False)[-1, :] for k in range(vehicle.n_seg)] + sleep_segments = [] for segment, values in zip(spline_segments, spline_values): - for spl, value in zip(segment, values): - spl.coeffs = value*np.ones(len(spl.basis)) - vehicle.store(current_time, sample_time, spline_segments, sleep_time) + seg = [] + for s, value in zip(segment, values): + # spl.coeffs = value*np.ones(len(spl.basis)) + basis = s.basis() + seg.append(spl.Function(basis, value*np.ones(basis.dimension()))) + sleep_segments.append(seg) + vehicle.store(current_time, sample_time, sleep_segments, sleep_time) # no correction for update time! Problem.simulate(self, current_time, sleep_time, sample_time) diff --git a/omgtools/vehicles/agv.py b/omgtools/vehicles/agv.py index 998f365..7116c92 100644 --- a/omgtools/vehicles/agv.py +++ b/omgtools/vehicles/agv.py @@ -19,8 +19,7 @@ from vehicle import Vehicle from ..basics.shape import Rectangle -from ..basics.spline_extra import sample_splines, evalspline -from ..basics.spline_extra import running_integral +from ..basics.spline import * from casadi import inf import numpy as np @@ -47,6 +46,7 @@ # Spline variables of the problem: v_til and tg_ha # delta follows from v_til, tg_ha, dtg_ha + class AGV(Vehicle): def __init__(self, length=0.4, options=None, bounds=None): @@ -62,7 +62,6 @@ def __init__(self, length=0.4, options=None, bounds=None): self.ddmax = bounds['ddmax'] if 'ddmax' in bounds else np.pi/4. self.length = length - def set_default_options(self): Vehicle.set_default_options(self) self.options.update({'plot_type': 'agv'}) # by default plot a bicycle-shape @@ -119,8 +118,8 @@ def get_initial_constraints(self, splines): hop0 = self.define_parameter('hop0', 1) tdelta0 = self.define_parameter('tdelta0', 1) # tan(delta) - self.define_constraint(hop0*(-2.*evalspline(ddtg_ha, self.t/self.T)*self.length - -tdelta0*(evalspline(dv_til, self.t/self.T)*(1.+tg_ha0**2)**2)*self.T), 0., 0.) + self.define_constraint(hop0*(-2.*ddtg_ha(self.t/self.T)*self.length + - tdelta0*(dv_til(self.t/self.T)*(1.+tg_ha0**2)**2)*self.T), 0., 0.) # This constraint is obtained by using l'Hopital's rule on the expression of # tan(delta) = 2*dtg_ha*self.length / (v_til*(1+tg_ha**2)**2) # this constraint is used to impose a specific steering angle when v_til = 0, e.g. when @@ -154,9 +153,9 @@ def get_terminal_constraints(self, splines): ddtg_ha = tg_ha.derivative(2) dx = v_til*(1-tg_ha**2) dy = v_til*(2*tg_ha) - x_int, y_int = self.T*running_integral(dx), self.T*running_integral(dy) - x = x_int-evalspline(x_int, self.t/self.T) + self.pos0[0] # self.pos0 was already defined in init - y = y_int-evalspline(y_int, self.t/self.T) + self.pos0[1] + x_int, y_int = self.T*dx.antiderivative(), self.T*dy.antiderivative() + x = x_int-x_int(self.t/self.T) + self.pos0[0] # self.pos0 was already defined in init + y = y_int-y_int(self.t/self.T) + self.pos0[1] term_con = [(x, posT[0]), (y, posT[1]), (tg_ha, tg_haT)] term_con_der = [(v_til, v_tilT), (dtg_ha, self.T*dtg_haT), (dv_til, dv_tilT), (ddtg_ha, self.T**2*ddtg_haT)] @@ -175,12 +174,12 @@ def set_terminal_conditions(self, pose): def get_init_spline_value(self): # generate initial guess for spline variables - init_value = np.zeros((len(self.basis), 2)) - v_til0 = np.zeros(len(self.basis)) + init_value = np.zeros((self.basis.dimension(), 2)) + v_til0 = np.zeros(self.basis.dimension()) tg_ha0 = np.tan(self.prediction['state'][2]/2.) tg_haT = np.tan(self.poseT[2]/2.) init_value[:, 0] = v_til0 - init_value[:, 1] = np.linspace(tg_ha0, tg_haT, len(self.basis)) + init_value[:, 1] = np.linspace(tg_ha0, tg_haT, self.basis.dimension()) return init_value def check_terminal_conditions(self): @@ -221,9 +220,9 @@ def define_collision_constraints(self, hyperplanes, environment, splines): v_til, tg_ha = splines[0], splines[1] dx = v_til*(1-tg_ha**2) dy = v_til*(2*tg_ha) - x_int, y_int = self.T*running_integral(dx), self.T*running_integral(dy) - x = x_int-evalspline(x_int, self.t/self.T) + self.pos0[0] - y = y_int-evalspline(y_int, self.t/self.T) + self.pos0[1] + x_int, y_int = self.T*dx.antiderivative(), self.T*dy.antiderivative() + x = x_int-x_int(self.t/self.T) + self.pos0[0] + y = y_int-y_int(self.t/self.T) + self.pos0[1] self.define_collision_constraints_2d(hyperplanes, environment, [x, y], tg_ha) def splines2signals(self, splines, time): @@ -236,11 +235,11 @@ def splines2signals(self, splines, time): dx = v_til*(1-tg_ha**2) dy = v_til*(2*tg_ha) if not hasattr(self, 'signals'): # first iteration - dx_int, dy_int = running_integral(dx), running_integral(dy) + dx_int, dy_int = dx.antiderivative(), dy.antiderivative() x = dx_int - dx_int(time[0]) + self.pose0[0] y = dy_int - dy_int(time[0]) + self.pose0[1] else: - dx_int, dy_int = running_integral(dx), running_integral(dy) # current state + dx_int, dy_int = dx.antiderivative(), dy.antiderivative() # current state x = dx_int - dx_int(time[0]) + self.signals['state'][0, -1] y = dy_int - dy_int(time[0]) + self.signals['state'][1, -1] # sample splines diff --git a/omgtools/vehicles/bicycle.py b/omgtools/vehicles/bicycle.py index bd82028..6d2b87f 100644 --- a/omgtools/vehicles/bicycle.py +++ b/omgtools/vehicles/bicycle.py @@ -21,7 +21,7 @@ from ..problems.point2point import FreeTPoint2point, FixedTPoint2point from ..basics.shape import Rectangle, Circle from ..basics.spline import * -from casadi import inf, SX, MX +from casadi import inf import numpy as np # Elaboration of the vehicle model: diff --git a/omgtools/vehicles/dubins.py b/omgtools/vehicles/dubins.py index 5ab603d..69bf389 100644 --- a/omgtools/vehicles/dubins.py +++ b/omgtools/vehicles/dubins.py @@ -21,7 +21,7 @@ from ..problems.point2point import FreeTPoint2point, FixedTPoint2point from ..basics.shape import Square, Circle from ..basics.spline import * -from casadi import inf, SX, MX +from casadi import inf import numpy as np # Elaboration of the vehicle model: @@ -120,7 +120,6 @@ def define_trajectory_constraints(self, splines): self.define_constraint(2*dtg_ha - (1+tg_ha**2)*self.T*self.wmax, -inf, 0.) self.define_constraint(-2*dtg_ha + (1+tg_ha**2)*self.T*self.wmin, -inf, 0.) - def get_fleet_center(self, splines, rel_pos, substitute=True): T = self.define_symbol('T') t = self.define_symbol('t') diff --git a/omgtools/vehicles/holonomic.py b/omgtools/vehicles/holonomic.py index d789210..1ad1a55 100644 --- a/omgtools/vehicles/holonomic.py +++ b/omgtools/vehicles/holonomic.py @@ -19,7 +19,7 @@ from vehicle import Vehicle from ..basics.shape import Circle -from ..basics.spline import * +from ..basics.spline import sample_splines from casadi import inf import numpy as np diff --git a/omgtools/vehicles/holonomic1d.py b/omgtools/vehicles/holonomic1d.py index 0699acf..014600c 100644 --- a/omgtools/vehicles/holonomic1d.py +++ b/omgtools/vehicles/holonomic1d.py @@ -19,7 +19,7 @@ from vehicle import Vehicle from ..basics.shape import Rectangle -from ..basics.spline import * +from ..basics.spline import sample_splines from casadi import inf import numpy as np @@ -75,7 +75,9 @@ def get_init_spline_value(self): len_basis = self.basis.dimension() pos0 = self.prediction['state'][0] posT = self.positionT[0] - # init_value = np.r_[pos0[0]*np.ones(self.degree), np.linspace(pos0[0], posT[0], len(self.basis) - 2*self.degree), posT[0]*np.ones(self.degree)] + # init_value = np.r_[pos0[0]*np.ones(self.degree), np.linspace( + # pos0[0], posT[0], len(self.basis) - 2*self.degree), + # posT[0]*np.ones(self.degree)] init_value = np.linspace(pos0, posT, len_basis) return init_value diff --git a/omgtools/vehicles/holonomic3d.py b/omgtools/vehicles/holonomic3d.py index 7e9392c..2c99df2 100644 --- a/omgtools/vehicles/holonomic3d.py +++ b/omgtools/vehicles/holonomic3d.py @@ -18,7 +18,7 @@ # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA from vehicle import Vehicle -from ..basics.spline_extra import sample_splines +from ..basics.spline import sample_splines from casadi import inf import numpy as np @@ -99,14 +99,14 @@ def set_terminal_conditions(self, position): self.positionT = position def get_init_spline_value(self): - init_value = np.zeros((len(self.basis), 3)) + init_value = np.zeros((self.basis.dimension(), 3)) pos0 = self.prediction['state'] posT = self.positionT for k in range(3): # init_value[:, k] = np.r_[pos0[k]*np.ones(self.degree), np.linspace( # pos0[k], posT[k], len(self.basis) - 2*self.degree), # posT[k]*np.ones(self.degree)] - init_value[:, k] = np.linspace(pos0[k], posT[k], len(self.basis)) + init_value[:, k] = np.linspace(pos0[k], posT[k], self.basis.dimension()) return init_value def check_terminal_conditions(self): diff --git a/omgtools/vehicles/holonomicorient.py b/omgtools/vehicles/holonomicorient.py index 5644a2c..f16368b 100644 --- a/omgtools/vehicles/holonomicorient.py +++ b/omgtools/vehicles/holonomicorient.py @@ -19,7 +19,7 @@ from vehicle import Vehicle from ..basics.shape import Rectangle -from ..basics.spline_extra import sample_splines, definite_integral +from ..basics.spline import sample_splines from casadi import inf import numpy as np @@ -76,14 +76,14 @@ def define_trajectory_constraints(self, splines): if (self.options['reg_type'] == 'norm_1' and self.options['reg_weight'] != 0.0): dtg_ha = tg_ha.derivative() g_reg = self.define_spline_variable( - 'g_reg', 1, basis=dtg_ha.basis)[0] - objective = definite_integral(g_reg, self.t/self.T, 1.) + 'g_reg', 1, basis=dtg_ha.basis())[0] + objective = g_reg.integral([self.t/self.T, 1.]) self.define_constraint(dtg_ha - g_reg, -inf, 0.) self.define_constraint(-dtg_ha - g_reg, -inf, 0.) self.define_objective(self.options['reg_weight']*objective) if (self.options['reg_type'] == 'norm_2'and self.options['reg_weight'] != 0.0): dtg_ha = tg_ha.derivative() - objective = definite_integral(dtg_ha**2, self.t/self.T, 1.) + (dtg_ha**2).integral([self.t/self.T, 1.]) self.define_objective(self.options['reg_weight']*objective) def get_initial_constraints(self, splines): @@ -118,7 +118,7 @@ def set_terminal_conditions(self, pose): def get_init_spline_value(self): # for the optimization problem so use tg_ha - init_value = np.zeros((len(self.basis), 3)) + init_value = np.zeros((self.basis.dimension(), 3)) pose0 = np.zeros(3) pose0[:2] = self.prediction['state'][:2] # x,y,theta[rad] pose0[2] = np.tan(self.prediction['state'][2]/2) # tg_ha @@ -128,7 +128,7 @@ def get_init_spline_value(self): for k in range(2): # init_value[:, k] = np.r_[pos0[k]*np.ones(self.degree), np.linspace( # pos0[k], posT[k], len(self.basis) - 2*self.degree), posT[k]*np.ones(self.degree)] - init_value[:, k] = np.linspace(pose0[k], poseT[k], len(self.basis)) + init_value[:, k] = np.linspace(pose0[k], poseT[k], self.basis.dimension()) return init_value def check_terminal_conditions(self): @@ -160,8 +160,8 @@ def splines2signals(self, splines, time): signals = {} x, y, tg_ha = splines[0], splines[1], splines[2] dx, dy, dtg_ha = x.derivative(), y.derivative(), tg_ha.derivative() - theta = 2*np.arctan2(sample_splines([tg_ha], time),1) - dtheta = 2*np.array(sample_splines([dtg_ha],time))/(1+np.array(sample_splines([tg_ha], time))**2) + theta = 2*np.arctan2(sample_splines([tg_ha], time), 1) + dtheta = 2*np.array(sample_splines([dtg_ha], time))/(1+np.array(sample_splines([tg_ha], time))**2) ddx, ddy = x.derivative(2), y.derivative(2) input = np.c_[sample_splines([dx, dy], time)] input = np.r_[input,dtheta] diff --git a/omgtools/vehicles/quadrotor.py b/omgtools/vehicles/quadrotor.py index 4435b57..c16f375 100644 --- a/omgtools/vehicles/quadrotor.py +++ b/omgtools/vehicles/quadrotor.py @@ -19,7 +19,7 @@ from vehicle import Vehicle from ..basics.shape import Circle -from ..basics.spline import * +from ..basics.spline import sample_splines from casadi import inf import numpy as np @@ -41,7 +41,6 @@ def set_default_options(self): Vehicle.set_default_options(self) self.options['stop_tol'] = 1.e-2 - def init(self): # time horizon self.T = self.define_symbol('T') diff --git a/omgtools/vehicles/quadrotor3d.py b/omgtools/vehicles/quadrotor3d.py index a31c1ec..11e59e4 100755 --- a/omgtools/vehicles/quadrotor3d.py +++ b/omgtools/vehicles/quadrotor3d.py @@ -20,7 +20,7 @@ from vehicle import Vehicle from ..basics.shape import Sphere from ..basics.spline import * -from casadi import inf, SX, MX +from casadi import inf import numpy as np import time as tt diff --git a/omgtools/vehicles/trailer.py b/omgtools/vehicles/trailer.py index 6c68275..29b693f 100644 --- a/omgtools/vehicles/trailer.py +++ b/omgtools/vehicles/trailer.py @@ -20,7 +20,7 @@ from vehicle import Vehicle from dubins import Dubins from ..basics.shape import Circle, Rectangle, Square -from ..basics.spline_extra import sample_splines +from ..basics.spline import sample_splines from casadi import inf import numpy as np @@ -113,13 +113,13 @@ def set_terminal_conditions(self, theta): self.theta_trT = theta def get_init_spline_value(self): - init_value_tr = np.zeros((len(self.basis), 1)) + init_value_tr = np.zeros((self.basis.dimension(), 1)) tg_ha_tr0 = np.tan(self.prediction['state'][2]/2.) if hasattr(self, 'theta_trT'): tg_ha_trT = np.tan(self.theta_trT/2.) else: tg_ha_trT = tg_ha_tr0 - init_value_tr[:, 0] = np.linspace(tg_ha_tr0, tg_ha_trT, len(self.basis)) + init_value_tr[:, 0] = np.linspace(tg_ha_tr0, tg_ha_trT, self.basis.dimension()) init_value_veh = self.lead_veh.get_init_spline_value() init_value = np.c_[init_value_tr, init_value_veh] return init_value diff --git a/omgtools/vehicles/vehicle.py b/omgtools/vehicles/vehicle.py index d12eef0..bf0b45f 100644 --- a/omgtools/vehicles/vehicle.py +++ b/omgtools/vehicles/vehicle.py @@ -295,7 +295,7 @@ def predict(self, current_time, predict_time, sample_time, state0=None, delay=0, for key in self.trajectories: if key not in ['state', 'input', 'pose']: self.prediction[key] = self.trajectories[key][:, n_samp+delay] - input = self.trajectories['input'][delay:] + input = self.trajectories['input'][:, delay:] if state0 is None: state0 = self.signals['state'][:, -n_samp-1] # current state state = self.integrate_ode(