def testOrderOne(args):
"""Test order one pendulum case, this is seen everywhere."""
if args.pen:
sys = OrderOnePendulum()
else:
sys = OrderOneOneD()
N = 20
t0 = 0.0
tf = 20.0
prob = TrajOptCollocProblem(sys, N, t0, tf)
prob.xbd = [
np.array([-1e20, -1e20, -1e20, -1e20]),
np.array([1e20, 1e20, 1e20, 1e20])
]
prob.ubd = [np.array([-1.5]), np.array([1.5])]
prob.x0bd = [np.array([0, 0, -1e20, -1e20]), np.array([0, 0, 1e20, 1e20])]
prob.xfbd = [
np.array([np.pi, 0, -1e20, -1e20]),
np.array([np.pi, 0, 1e20, 1e20])
]
lqr = LqrObj(R=np.ones(1))
prob.add_lqr_obj(lqr)
prob.pre_process() # construct the problem
# construct a solver for the problem
cfg = OptConfig(backend=args.backend, print_level=5)
solver = OptSolver(prob, cfg)
rst = solver.solve_rand()
print(rst.flag)
if rst.flag == 1:
print(rst.sol)
# parse the solution
sol = prob.parse_sol(rst.sol.copy())
show_sol(sol)
def constructOrderTwo():
"""Test the wrapper class for yet another naive problem."""
sys = DaeSystemWrapper(sysFunOrder2, 3, 1, 0, 1)
N = 20
t0 = 0.0
tf = 10.0
prob = TrajOptCollocProblem(sys, N, t0, tf)
prob.xbd = [np.array([-1e20, -1e20, -1e20]), np.array([1e20, 1e20, 1e20])]
prob.ubd = [np.array([-1.5]), np.array([1.5])]
prob.x0bd = [np.array([0, 0, -1e20]), np.array([0, 0, 1e20])]
prob.xfbd = [np.array([np.pi, 0, -1e20]), np.array([np.pi, 0, 1e20])]
lqr = LqrObj(R=np.ones(1))
prob.add_lqr_obj(lqr)
prob.pre_process() # construct the problem
return prob
def testLinear(args):
"""Test 1d problem with linear constraints and linear objective"""
sys = OneDcase()
N = 10
t0 = 0.0
tf = 2.0
prob = TrajOptCollocProblem(sys, N, t0, tf)
prob.xbd = [np.array([-1e20, -1e20, -1e20]), np.array([1e20, 1e20, 1e20])]
prob.ubd = [np.array([-1e20]), np.array([1e20])]
prob.x0bd = [np.array([0, 0, -1e20]), np.array([0, 0, 1e20])]
prob.xfbd = [np.array([1, 0, -1e20]), np.array([1, 0, 1e20])]
lqr = LqrObj(R=np.ones(1))
prob.add_lqr_obj(lqr)
A = np.zeros(5)
A[1] = 1
A[2] = 1 # so it basically does nothing
linPntObj = LinearPointObj(0, A, 3, 1, 0)
prob.add_obj(linPntObj)
# add linear constraint that x is increasing
A = np.zeros(5)
A[1] = 1
lb = np.zeros(1)
ub = np.ones(1)
linPntCon = LinearPointConstr(-1, A, lb, ub)
prob.add_constr(linPntCon, True)
# we want mid point to be close to 0.8
wantState = np.array([0.8, 0])
pntObj = PointObj(N, wantState)
prob.addObj(pntObj)
prob.pre_process() # construct the problem
# construct a solver for the problem
cfg = OptConfig(args.backend, print_level=5)
slv = OptSolver(prob, cfg)
rst = slv.solve_rand()
print(rst.flag, rst.sol)
if rst.flag == 1:
# parse the solution
sol = prob.parse_sol(rst.sol.copy())
show_sol(sol)
def testOneD(args):
"""Test solving one-dim problem using collocation approach"""
sys = OneDcase()
N = 10
t0 = [-1.0, 0]
tf = [2.0, 3.0]
prob = TrajOptCollocProblem(sys, N, t0, tf)
prob.xbd = [np.array([-1e20, -1e20, -1e20]), np.array([1e20, 1e20, 1e20])]
prob.ubd = [np.array([-1e20]), np.array([1e20])]
prob.x0bd = [np.array([0, 0, -1e20]), np.array([0, 0, 1e20])]
prob.xfbd = [np.array([1, 0, -1e20]), np.array([1, 0, 1e20])]
lqr = LqrObj(R=np.ones(1))
prob.add_lqr_obj(lqr)
prob.pre_process() # construct the problem
# construct a solver for the problem
cfg = OptConfig(args.backend, print_level=5)
slv = OptSolver(prob, cfg)
rst = slv.solve_rand()
print(rst.flag, rst.sol)
if rst.flag == 1:
# parse the solution
sol = prob.parse_sol(rst.sol.copy())
show_sol(sol)
def testOneLeg():
"""Test order one, leg one"""
sys = OrderOneModel()
N = 20
t0 = 0.0
tf = 10.0
prob1 = TrajOptCollocProblem(sys, N, t0, tf)
xlb = -1e20 * np.ones(8)
xub = 1e20 * np.ones(8)
ulb = -1.5 * np.ones(2)
uub = 1.5 * np.ones(2)
x0lb = np.concatenate((np.zeros(4), -1e20 * np.ones(4)))
x0ub = np.concatenate((np.zeros(4), 1e20 * np.ones(4)))
xflb = np.concatenate((np.ones(2), np.zeros(2), -1e20 * np.ones(4)))
xfub = np.concatenate((np.ones(2), np.zeros(2), 1e20 * np.ones(4)))
prob1.xbd = [xlb, xub]
prob1.ubd = [ulb, uub]
prob1.x0bd = [x0lb, x0ub]
prob1.xfbd = [xflb, xfub]
# define objective function
lqr = LqrObj(R=np.ones(2))
prob1.add_lqr_obj(lqr)
# add several constraints
obj_avoid = ObjAvoidConstr(N)
prob1.add_constr(obj_avoid)
# ready to construct this problem
prob1.pre_process() # construct the problem
# construct a solver for the problem
cfg = OptConfig(print_level=5)
slv = OptSolver(prob1, cfg)
rst = slv.solve_rand()
print(rst.flag)
if rst.flag == 1:
print(rst.sol)
# parse the solution
sol = prob1.parse_sol(rst.sol.copy())
show_sol(sol)
def testPen(args):
"""Test solving pendulum swing up problem using collocation approach"""
sys = Pendulum()
N = 20
t0 = 0.0
tf = 20.0
prob = TrajOptCollocProblem(sys, N, t0, tf)
prob.xbd = [np.array([-1e20, -1e20, -1e20]), np.array([1e20, 1e20, 1e20])]
prob.ubd = [np.array([-1.5]), np.array([1.5])]
prob.x0bd = [np.array([0, 0, -1e20]), np.array([0, 0, 1e20])]
prob.xfbd = [np.array([np.pi, 0, -1e20]), np.array([np.pi, 0, 1e20])]
lqr = LqrObj(R=np.ones(1))
prob.add_lqr_obj(lqr)
prob.pre_process() # construct the problem
# construct a solver for the problem
cfg = OptConfig(args.backend, print_level=5)
slv = OptSolver(prob, cfg)
rst = slv.solve_rand()
print(rst.flag)
if rst.flag == 1:
print(rst.sol)
# parse the solution
sol = prob.parse_sol(rst.sol.copy())
show_sol(sol)
def main():
sys = OrderTwoModel()
N = 10
t0 = 0.0
tf = 3.0
prob1 = TrajOptCollocProblem(sys, N, t0, [0.1, tf]) # maybe I need to give tips on choosing times
prob2 = TrajOptCollocProblem(sys, N, [0.1, tf], [0.11, tf])
prob3 = TrajOptCollocProblem(sys, N, [0.2, tf], [0.21, tf])
xlb = -1e20 * np.ones(6)
xub = 1e20 * np.ones(6)
ulb = -np.ones(2)
uub = -ulb
x0, xf = np.array([[0., 0.], [1.0, 0.5]])
x0lb = np.concatenate((x0, -1e20 * np.ones(4)))
x0ub = np.concatenate((x0, 1e20 * np.ones(4)))
xflb = np.concatenate((xf, -1e20 * np.ones(4)))
xfub = np.concatenate((xf, 1e20 * np.ones(4)))
prob1.xbd = [xlb, xub]
prob1.ubd = [ulb, uub]
prob1.x0bd = [x0lb, x0ub]
prob1.xfbd = [xlb, xub]
prob2.xbd = [xlb, xub]
prob2.ubd = [ulb, uub]
prob2.x0bd = [xlb, xub]
prob2.xfbd = [xlb, xub]
prob3.xbd = [xlb, xub]
prob3.ubd = [ulb, uub]
prob3.x0bd = [xlb, xub]
prob3.xfbd = [xflb, xfub]
# set bounds constraints for prob1 and prob2 at final
a = np.zeros((2, 9)) # 1 + 6 + 2
np.fill_diagonal(a[:, 1:3], 1.0)
prob1.add_constr(LinearPointConstr(-1, a, np.array([0.2, 0.2]), np.array([0.2, 1e20])))
prob2.add_constr(LinearPointConstr(-1, a, np.array([0.8, -1e20]), np.array([0.8, 0.3])))
# define objective function, #TODO: change to time optimal
obj = LqrObj(R=0.01 * np.ones(2))
prob1.add_obj(obj, path=True)
prob2.add_obj(obj, path=True)
prob3.add_obj(obj, path=True)
# optimize time
obj_a = np.zeros(prob3.tfind + 1)
obj_a[-1] = 1.0
prob3.add_obj(LinearObj(obj_a))
# add constraints to some phases
prob = TrajOptMultiPhaseCollocProblem([prob1, prob2, prob3], addx=None)
constr1 = ConnectConstr(0, 6)
constr2 = ConnectConstr(1, 6)
prob.add_connect_constr(constr1)
prob.add_connect_constr(constr2)
# ready to solve
prob.pre_process()
# cfg = OptConfig(backend='snopt', deriv_check=1, print_file='tmp.out')
cfg = OptConfig(print_level=5)
slv = OptSolver(prob, cfg)
rst = slv.solve_rand()
print(rst.flag)
if rst.flag == 1:
sol = prob.parse_sol(rst)
show_sol(sol)
def testOne():
"""Test order one pendulum case, this is seen everywhere."""
sys = OrderOneModel()
N = 20
t0 = 0.0
tf = 10.0
tmid = 5.0
prob1 = TrajOptCollocProblem(sys, N, t0, [t0 + 1, tmid])
prob2 = TrajOptCollocProblem(sys, N, tmid, tf) # [t0, tf], tf)
xlb = -1e20 * np.ones(8)
xub = 1e20 * np.ones(8)
ulb = -1.5 * np.ones(2)
uub = 1.5 * np.ones(2)
x0lb = np.concatenate((np.zeros(4), -1e20 * np.ones(4)))
x0ub = np.concatenate((np.zeros(4), 1e20 * np.ones(4)))
xflb = np.concatenate((np.ones(2), np.zeros(2), -1e20 * np.ones(4)))
xfub = np.concatenate((np.ones(2), np.zeros(2), 1e20 * np.ones(4)))
prob1.xbd = [xlb, xub]
prob1.ubd = [ulb, uub]
prob1.x0bd = [x0lb, x0ub]
prob1.xfbd = [xlb, xub]
prob2.xbd = [xlb, xub]
prob2.ubd = [ulb, uub]
prob2.x0bd = [xlb, xub]
prob2.xfbd = [xflb, xfub]
# define objective function
lqr = LqrObj(R=np.ones(2))
prob1.add_lqr_obj(lqr)
prob2.add_lqr_obj(lqr)
# add several constraints
obj_avoid = ObjAvoidConstr(2 * N - 2)
prob1.add_constr(obj_avoid)
# ready to construct this problem
prob = TrajOptMultiPhaseCollocProblem([prob1, prob2], addx=None)
# add connect constraints
constr1 = ConnectStateConstr(N, 4) # since dimq = 4
constr2 = ConnectVelocityConstr()
prob.add_constr(constr1)
prob.add_constr(constr2)
# ready to solve this problem. It is quite complicated to construct this problem, how come?
# TODO: add support for easy-to-construct constraints.
# For example, linear constraints can be constructed by giving matrix
# nonlinear constraints can be constructed by functions. These functions can be auto-diffed
prob.pre_process() # construct the problem
# construct a solver for the problem
cfg = OptConfig(print_level=5)
slv = OptSolver(prob, cfg)
rst = slv.solve_rand()
print(rst.flag)
if rst.flag == 1:
# print(rst.sol)
# parse the solution
sol = prob.parse_sol(rst)
show_sol(sol)
fig, ax = plt.subplots()
phase1 = sol['phases'][0]
phase2 = sol['phases'][1]
ax.plot(phase1['x'][:, 0], phase1['x'][:, 1])
ax.plot(phase1['x'][-1, 0],
phase1['x'][-1, 1],
marker='*',
markersize=5)
ax.plot(phase2['x'][:, 0], phase2['x'][:, 1])
ax.plot(phase2['x'][-1, 0],
phase2['x'][-1, 1],
marker='*',
markersize=5)
plt.show()