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 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)