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 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_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_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 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_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
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
from pytrajectory.visualisation import Animation
def draw(xt, image):
x = xt[0]
phi = xt[2]
car_width = 0.05
car_heigth = 0.02
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
def draw(xti, image):
x, phi1, phi2 = xti[0], xti[2], xti[4]
l1 = 0.7
l2 = 0.5
car_width = 0.05
ua=0,
ub=0,
use_chains=False,
use_std_approach=False,
refsol=refsol,
ierr=None,
maxIt=3,
eps=1e-1,
sol_steps=100,
reltol=1e-3,
accIt=1,
show_ir=True)
# dt_sim=0.004
# time to run the iteration
x, u = S.solve()
if S.reached_accuracy:
print "successed!"
else:
print "Not successed!"
S.save(fname='pickles/model_trajectory' + str(Tb) + '.pcl')
import sys
import matplotlib as mpl
from pytrajectory.visualisation import Animation
from sympy import cos, sin
N = 3
# all rods have the same length
rod_lengths = [0.5] * N
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:
with open(pfname, "rb") as pfile:
cont_dict = pickle.load(pfile)
xxf, uuf = aux.get_xx_uu_funcs_from_containerdict(cont_dict)
tt = np.linspace(uuf.a, uuf.b, 1000)
if 0:
a=Ta,
b=Tb,
xa=xa,
xb=xb,
ua=uu[0, :],
ub=uu[-1, :],
use_chains=False,
refsol=refsol,
ierr=None,
maxIt=3,
eps=1e-1,
sol_steps=100,
reltol=1e-3,
accIt=1)
S2.solve(tcpport=None)
# for animation
S = S2
elif 1:
deg = pi / 180
tt = np.linspace(Ta, Tb, 100)
IPS()
sim_res = []
residum_res = []
N = 200
reslist = []
for i in range(0, N):
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)
err = dxres - ffres
constraints=None,
eps=1e-1,
su=30,
kx=2,
use_chains=False,
#first_guess={'seed': 4, 'scale': 10, 'u1': lambda t: 0},
refsol=refsol,
uref=uref_fnc,
use_std_approach=False,
sol_steps=200,
show_ir=True)
S = S2
# start BVP-solution
x, u = S2.solve()
IPS()
sys.argv.append('plot')
# 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):
x = xt[0]
car_width = 0.05