def test_di_integrator_pure_with_complete_guess(self): # solve Problem for the first time first_guess = {'seed': 20} S1 = TransitionProblem(rhs_di, a=0.0, b=2.0, xa=xa_di, xb=xb_di, ua=0, ub=0, show_ir=False, ierr=None, first_guess=first_guess, use_chains=False) S1.solve() assert S1.reached_accuracy first_guess2 = { 'complete_guess': S1.eqs.sol, 'n_spline_parts': aux.Container(x=S1.eqs.trajectories.n_parts_x, u=S1.eqs.trajectories.n_parts_u) } S2 = S1.create_new_TP(first_guess=first_guess2) S2.solve() assert S2.reached_accuracy # now test changed boundary conditions S3 = S2.create_new_TP(first_guess=first_guess2, xb=[1.5, 0.0]) S3.solve() assert S3.reached_accuracy
def solve(problem_spec): # system state boundary values for a = 0.0 [s] and b = 2.0 [s] xa = problem_spec.xx_start xb = problem_spec.xx_end T_end = problem_spec.T_transition # constraints dictionary con = problem_spec.constraints ua = problem_spec.u_start ub = problem_spec.u_end def f_pytrajectory(xx, uu, uuref, t, pp): """ Right hand side of the vectorfield defining the system dynamics This function wraps the rhs-function of the problem_spec to make it compatible to pytrajectory. :param xx: state :param uu: input :param uuref: reference input (not used) :param t: time (not used) :param pp: additionial free parameters (not used) :return: xdot """ return problem_spec.rhs(xx, uu) first_guess = problem_spec.first_guess # create the trajectory object S = TransitionProblem(f_pytrajectory, a=0.0, b=T_end, xa=xa, xb=xb, ua=ua, ub=ub, constraints=con, use_chains=True, first_guess=first_guess) # alter some method parameters to increase performance S.set_param('su', 10) # start x, u = S.solve() solution_data = SolutionData() solution_data.x_func = x solution_data.u_func = u save_plot(problem_spec, solution_data) return solution_data
def test_brockett_system(self): S1 = TransitionProblem(rhs_brockett_system, a=0.0, b=2.0, xa=xa_br, xb=xb_br, ua=None, ub=None, show_ir=False, ierr=None, use_chains=False) S1.solve() assert S1.reached_accuracy
def test_di_integrator_pure(self): S1 = TransitionProblem(rhs_di, a=0.0, b=2.0, xa=xa_di, xb=xb_di, ua=0, ub=0, show_ir=False, ierr=None, use_chains=False) S1.solve() assert S1.reached_accuracy
def test_di_con_u1_projective_integrator(self): con = {'u1': [-1.2, 1.2]} S1 = TransitionProblem(rhs_di, a=0.0, b=2.0, xa=xa_di, xb=xb_di, ua=0, ub=0, constraints=con, show_ir=False, ierr=None, use_chains=False) S1.solve() assert S1.reached_accuracy
def test_di_integrator_pure_with_random_guess(self): first_guess = {'seed': 20} S1 = TransitionProblem(rhs_di, a=0.0, b=2.0, xa=xa_di, xb=xb_di, ua=0, ub=0, show_ir=False, ierr=None, first_guess=first_guess, use_chains=False) S1.solve() assert S1.reached_accuracy
def test_di_constraint_x2_projective(self): con = {'x2': [-1, 10]} con = {'x2': [-0.1, 0.65]} S1 = TransitionProblem(rhs_di, a=0.0, b=2.0, xa=xa_di, xb=xb_di, ua=0, ub=0, constraints=con, show_ir=False, ierr=None, use_chains=False) S1.solve() assert S1.reached_accuracy
def test_constr_inv_pendulum(self): con = {'x1': [-0.8, 0.3], 'x2': [-2.0, 2.0], 'u1': [-7.0, 7.0]} eps = 7e-2 # increase runtime-speed (prevent additional run with 80 spline parts) S1 = TransitionProblem(rhs_inv_pend, a=0.0, b=3.0, xa=xa_inv_pend, xb=xb_inv_pend, ua=0, ub=0, constraints=con, show_ir=False, accIt=0, eps=eps, use_chains=False) S1.solve() assert S1.reached_accuracy
def test_di_timescaled(self): """The double integrator with an additional free parameter for time scaling""" con = { 'u1': [-1.3, 1.3], 'x2': [-.1, .8], } S1 = TransitionProblem(rhs_di_time_scaled, a=0.0, b=2.0, xa=xa_di, xb=xb_di, ua=0, ub=0, constraints=con, show_ir=False, accIt=0, use_chains=False) S1.solve() assert S1.reached_accuracy
def test_di_integrator_pure_seed(self): S1 = TransitionProblem(rhs_di, a=0.0, b=2.0, xa=xa_di, xb=xb_di, ua=0, ub=0, show_ir=False, ierr=None, use_chains=False, maxIt=1, seed=0) S1.solve() S2 = TransitionProblem(rhs_di, a=0.0, b=2.0, xa=xa_di, xb=xb_di, ua=0, ub=0, show_ir=False, ierr=None, use_chains=False, maxIt=1, seed=1141) S2.solve() # assert that the different seed has taken effect assert S1.eqs.solver.res_list[0] != S2.eqs.solver.res_list[0] assert S2.eqs._first_guess == {"seed": 1141} assert S1.reached_accuracy assert S2.reached_accuracy
# system state boundary values for a = 0.0 [s] and b = 2.0 [s] xa = [0.0, 0.0, 3 / 2.0 * np.pi, 0.0] xb = [0.0, 0.0, 1 / 2.0 * np.pi, 0.0] # boundary values for the inputs ua = [0.0] ub = [0.0] # create System first_guess = {'seed': 1529} # choose a seed which leads to quick convergence S = TransitionProblem(f, a=0.0, b=2.0, xa=xa, xb=xb, ua=ua, ub=ub, use_chains=True, first_guess=first_guess) # alter some method parameters to increase performance S.set_param('su', 10) # run iteration S.solve() # the following code provides an animation of the system above # for a more detailed explanation have a look at the 'Visualisation' section in the documentation import sys import matplotlib as mpl
xb = [0.0, np.pi, 0.0, 0.0] ua = [0.0] ub = [0.0] Tend = b + 0 # ensure meaningfull input if increasing pfname = "swingup_splines.pcl" if 0: first_guess = {'seed': 20} S = TransitionProblem(pytraj_f, a, b, xa, xb, ua, ub, first_guess=first_guess, kx=2, eps=5e-2, use_chains=False) # , sol_steps=1300 # time to run the iteration solC = S.solve(return_format="info_container") cont_dict = aux.containerize_splines(S.eqs.trajectories.splines) with open(pfname, "wb") as pfile: pickle.dump(cont_dict, pfile) else:
xb = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] ua = [0.0] ub = [0.0] # here we specify the constraints for the velocity of the car con = {'x1': [-1.0, 1.0], 'x2': [-2.0, 2.0]} # now we create our Trajectory object and alter some method parameters via the keyword arguments S = TransitionProblem(f, a, b, xa, xb, ua, ub, constraints=con, eps=2e-1, su=20, kx=2, use_chains=False, use_std_approach=False) # time to run the iteration x, u = S.solve() # the following code provides an animation of the system above # for a more detailed explanation have a look at the 'Visualisation' section in the documentation import sys import matplotlib as mpl from pytrajectory.visualisation import Animation
# eps=4e-1, su=30, kx=2, use_chains=False, # use_std_approach=False) # time to run the iteration # x, u = S.solve() if 1: xa = np.array([1.8 * pi, 1.5 * pi, 1.5 * pi, 0.0, 0.0, 0.0, 0.0, 0.0]) S = TransitionProblem(model_rhs, a=Ta, b=Tb, xa=xa, xb=xb, ua=ua, ub=ub, use_chains=False, first_guess=None, ierr=None, maxIt=3, eps=1e-1, sol_steps=100, reltol=1e-3, accIt=1) IPS() u_scale = 1 # u_values = np.r_[0, 10, 0, -10, 0, -10, 0, 10, 10, 0,] u_values = np.r_[0, 10, 0, -10, 0] u_values2 = np.r_[0, 10, 0, -20, 0] u_values0 = np.r_[0, 0, 0, 0, 0]
ua = [0.0] ub = [0.0] from pytrajectory import log log.console_handler.setLevel(10) # now we create our Trajectory object and alter some method parameters via the keyword arguments first_guess = {'seed': 20} S = TransitionProblem(f, a, b, xa, xb, ua, ub, first_guess=first_guess, kx=2, eps=5e-2, use_chains=False, sol_steps=1300) # time to run the iteration S.solve(tcpport=5006) # now that we (hopefully) have found a solution, # we can visualise our systems dynamic # therefore we define a function that draws an image of the system # according to the given simulation data
return ff # system state boundary values for a = 0.0 [s] and b = 2.0 [s] xa = [0.0, 0.0] xb = [1.0, 0.0] # constraints dictionary con = {'x2': [-0.1, 0.65]} # create the trajectory object S = TransitionProblem(f, a=0.0, b=2.0, xa=xa, xb=xb, constraints=con, use_chains=False) # start x, u = S.solve() # the following code provides an animation of the system above # for a more detailed explanation have a look at the 'Visualisation' section in the documentation import sys import matplotlib as mpl from pytrajectory.visualisation import Animation def draw(xt, image):
xa = [ 0.0, 0.0, 0.4*np.pi, 0.0] xb = [ 0.2*np.pi, 0.0, 0.2*np.pi, 0.0] # boundary values for the inputs ua = [0.0] ub = [0.0] # create trajectory object S = TransitionProblem(f, a=0.0, b=1.8, xa=xa, xb=xb, ua=ua, ub=ub) # also alter some method parameters to increase performance S.set_param('su', 20) S.set_param('kx', 3) # run iteration S.solve() # the following code provides an animation of the system above # for a more detailed explanation have a look at the 'Visualisation' section in the documentation import sys import matplotlib as mpl from pytrajectory.visualisation import Animation
# system state boundary values for a = 0.0 [s] and b = 3.0 [s] xa = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] xb = [10.0, 0.0, 5.0, 0.0, 0.0, 0.0] # boundary values for the inputs ua = [ 0.5 * 9.81 * 50.0 / (cos(5 / 360.0 * 2 * pi)), 0.5 * 9.81 * 50.0 / (cos(5 / 360.0 * 2 * pi)) ] ub = [ 0.5 * 9.81 * 50.0 / (cos(5 / 360.0 * 2 * pi)), 0.5 * 9.81 * 50.0 / (cos(5 / 360.0 * 2 * pi)) ] # create trajectory object S = TransitionProblem(f, a=0.0, b=3.0, xa=xa, xb=xb, ua=ua, ub=ub) # don't take advantage of the system structure (integrator chains) # (this will result in a faster solution here) S.set_param('use_chains', False) # also alter some other method parameters to increase performance S.set_param('kx', 5) # run iteration S.solve() # the following code provides an animation of the system above # for a more detailed explanation have a look at the 'Visualisation' section in the documentation import sys import matplotlib as mpl
args = aux.Container( poolsize=3, ff=model_rhs, a=Ta, xa=xa, xb=xb, ua=0, ub=0, use_chains=False, ierr=None, maxIt=5, eps=4e-1, kx=2, use_std_approach=False, seed=[1, 2, 30, 81], constraints=con, show_ir=False, b=2.4 + np.r_[.4, .5] ) if "single" in sys.argv: args.b = 3.7 args.maxIt = 7 args.dict.pop("poolsize") args.show_ir = True args.seed = 1 args.maxIt = 4 TP1 = TransitionProblem(**args.dict) results = xx, uu = TP1.solve() # ensure that the result is compatible with system dynamics sic = TP1.return_sol_info_container() # collocation points: cp1 = TP1.eqs.cpts[1] # right hand side ffres = np.array(model_rhs(xx(cp1), uu(cp1), None, None, None), dtype=np.float) # derivative of the state trajectory (left hand side) dxres = TP1.eqs.trajectories.dx(cp1)
# then we specify all boundary conditions a = 0.0 xa = [0.0, 0.0, pi, 0.0, pi, 0.0, pi, 0.0] b = 3.5 xb = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] ua = [0.0] ub = [0.0] from pytrajectory import log log.console_handler.setLevel(10) # now we create our Trajectory object and alter some method parameters via the keyword arguments S = TransitionProblem(f, a, b, xa, xb, ua, ub, constraints=None, eps=4e-1, su=30, kx=2, use_chains=False, use_std_approach=False) # time to run the iteration x, u = S.solve() # the following code provides an animation of the system above # for a more detailed explanation have a look at the 'Visualisation' section # in the documentation import sys import matplotlib as mpl from pytrajectory.visualisation import Animation # all rods have the same length rod_lengths = [0.5] * N
ua = [0.0] ub = [0.0] from pytrajectory import log log.console_handler.setLevel(20) # now we create our Trajectory object and alter some method parameters via the keyword arguments first_guess = {'seed': 20} S = TransitionProblem(f, a, b, xa, xb, ua, ub, first_guess=first_guess, kx=2, eps=5e-2, use_chains=False, sol_steps=1300) # time to run the iteration S.solve() # now that we (hopefully) have found a solution, # we can visualise our systems dynamic # therefore we define a function that draws an image of the system # according to the given simulation data
(1 / l2) * (g * sin(x5) + uu * cos(x5)) ]) return ff # system state boundary values for a = 0.0 [s] and b = 2.0 [s] xa = [0.0, 0.0, np.pi, 0.0, np.pi, 0.0] xb = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # boundary values for the input ua = [0.0] ub = [0.0] # create trajectory object S = TransitionProblem(f, a=0.0, b=2.0, xa=xa, xb=xb, ua=ua, ub=ub) # alter some method parameters to increase performance S.set_param('su', 10) S.set_param('eps', 8e-2) # run iteration S.solve() # the following code provides an animation of the system above # for a more detailed explanation have a look at the 'Visualisation' section in the documentation import sys import matplotlib as mpl from pytrajectory.visualisation import Animation
xb_0 = np.r_[pi, pi, pi, 0.2, 0.0, 0.0, 0.0, 0.0] xb_des = np.r_[0, 0, 0, 0, 0.0, 0.0, 0.0, 0.0] from pytrajectory import log log.console_handler.setLevel(20) con = {'x4': (-1, 1), 'x8': (-20, 20), 'u1': (-20, 20)} con = {} S = TransitionProblem(model_rhs, Ta, Tb, xa, xb_0, ua, ub, constraints=con, eps=1e-1, kx=2, use_chains=False, first_guess={'seed': 5}, use_std_approach=False, sol_steps=200, show_ir=False) S.solve() N = 20 i = 0 di = 1 while i <= N: S_old = S # new final value xb = xb_0 + i*1.0/N*(xb_des - xb_0) first_guess2 = {'complete_guess': S.eqs.sol,
# S = ControlSystem(model_rhs, Ta, Tb, xa, xb, ua, ub) # state, u = S.solve() from pytrajectory import log log.console_handler.setLevel(10) # now we create our Trajectory object and alter some method parameters via the keyword arguments, S = TransitionProblem(model_rhs, Ta, Tb, xa, xb, ua, ub, constraints=None, eps=1e-1, su=30, kx=2, use_chains=False, first_guess={'seed': 3}, use_std_approach=False, sol_steps=200, show_ir=True) # create a reference solution via simulation u_values = [0, 3, -4, -4, 7, -3] # refsol = aux.make_refsol_by_simulation(S, u_values=u_values, plot_u=True, plot_x_idx=4) refsol = aux.make_refsol_by_simulation(S, u_values=u_values) S = TransitionProblem(model_rhs,
c = 0 ff.append(c) return np.array(ff) if 0: # original system S = TransitionProblem(rhs1, Ta, Tb, xa1, xb1, constraints=None, eps=1e-1, su=30, kx=2, use_chains=False, first_guess={ 'seed': 4, 'scale': 10 }, use_std_approach=False, sol_steps=200, ierr=None, show_ir=True) # This factor adjusts how strong a deviation from the standard input is penalized. # Experience: 1 is much too strong input_penalty_scale = 0.1 def rhs2(
ff = np.array([ x2, m * s * (-l * x4**2 + g * c) / (M + m * s**2) + 1 / (M + m * s**2) * u1, x4, s * (-m * l * x4**2 * c + g * (M + m)) / (M * l + m * l * s**2) + c / (M * l + l * m * s**2) * u1 ]) return ff # boundary values at the start (a = 0.0 [s]) xa = [0.0, 0.0, 0.0, 0.0] # boundary values at the end (b = 2.0 [s]) xb = [1.0, 0.0, 0.0, 0.0] # create trajectory object S = TransitionProblem(f, a=0.0, b=2.0, xa=xa, xb=xb) # change method parameter to increase performance S.set_param('use_chains', False) # run iteration S.solve() # the following code provides an animation of the system above # for a more detailed explanation have a look at the 'Visualisation' section in the documentation import sys import matplotlib as mpl from pytrajectory.visualisation import Animation def draw(xti, image):
def test_di_invalid_boundary(self): xa_bad1 = [0, -4] xa_bad2 = [0, 4] xb_bad1 = [1, -4] xb_bad2 = [1, 4] ua_bad = [10] ub_bad = [10] con = {'x1': [-1, 2], 'x2': [-4, 4], 'u1': [-5, 5]} kwargs = dict(show_ir=False, ierr=None, use_chains=False, constraints=con) # no Problem S1 = TransitionProblem(rhs_di, a=0.0, b=2.0, xa=xa, xb=xb, ua=[0], ub=[0], **kwargs) with pytest.raises(ch.ConstraintError) as err: TransitionProblem(rhs_di, a=0.0, b=2.0, xa=xa_bad1, xb=xb, ua=[0], ub=[0], **kwargs) with pytest.raises(ch.ConstraintError) as err: TransitionProblem(rhs_di, a=0.0, b=2.0, xa=xa_bad2, xb=xb, ua=[0], ub=[0], **kwargs) with pytest.raises(ch.ConstraintError) as err: TransitionProblem(rhs_di, a=0.0, b=2.0, xa=xa, xb=xb_bad1, ua=[0], ub=[0], **kwargs) with pytest.raises(ch.ConstraintError) as err: TransitionProblem(rhs_di, a=0.0, b=2.0, xa=xa, xb=xb_bad2, ua=[0], ub=[0], **kwargs) with pytest.raises(ch.ConstraintError) as err: TransitionProblem(rhs_di, a=0.0, b=2.0, xa=xa, xb=xb, ua=ua_bad, ub=[0], **kwargs) with pytest.raises(ch.ConstraintError) as err: TransitionProblem(rhs_di, a=0.0, b=2.0, xa=xa, xb=xb, ua=[0], ub=ub_bad, **kwargs)
ua = [0.0] ub = [0.0] # next, this is the dictionary containing the constraints con = {'x1': [-0.8, 0.3], 'x2': [-2.0, 2.0]} first_guess = {'seed': 50} # now we create our Trajectory object and alter some method parameters via the keyword arguments S = TransitionProblem(f, a, b, xa, xb, ua, ub, constraints=con, kx=2, eps=5e-2, first_guess=first_guess, use_chains=False, sol_steps=1300, reltol=2e-5) # time to run the iteration S.solve(tcpport=5006) # S.solve() # the following code provides an animation of the system above # for a more detailed explanation have a look at the 'Visualisation' section in the documentation import sys import matplotlib as mpl
# then we specify all boundary conditions a = 0.0 xa = [0.0, 0.0, pi, 0.0, pi, 0.0, pi, 0.0] b = 3.5 xb = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] ua = [0.0] ub = [0.0] # here we specify the constraints for the velocity of the car con = {0 : [-1.0, 1.0], 1 : [-5.0, 5.0]} # now we create our Trajectory object and alter some method parameters via the keyword arguments S = TransitionProblem(f, a, b, xa, xb, ua, ub, constraints=con, eps=4e-1, su=20, kx=2, use_chains=False) # time to run the iteration x, u = S.solve() # the following code provides an animation of the system above # for a more detailed explanation have a look at the 'Visualisation' section in the documentation import sys import matplotlib as mpl from pytrajectory.visualisation import Animation def create_draw_function(N=1, car_width_height=[0.05, 0.02], rod_lengths=0.5, pendulum_sizes=0.015): # if all rods have the same length if type(rod_lengths) in {int, float}: rod_lengths = [rod_lengths] * N