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