def gradmode(args):
"""Run pendulum swing-up problem"""
sys = CartPole()
cost = QuadCost()
N = 21
t0 = 0.0
tf = 4.0
prob = TrajOptProblem(sys, N, t0, tf, gradmode=True)
prob.xbd = [np.array([-1e20, -1e20, -1e20, -1e20]), np.array([1e20, 1e20, 1e20, 1e20])]
prob.ubd = [np.array([-100.0]), np.array([100.0])]
prob.x0bd = [np.array([0, np.pi/8, 0, 0]), np.array([0, np.pi/8, 0, 0])]
prob.xfbd = [np.array([-1e20, -1e20, -1e20, -1e20]), np.array([1e20, 1e20, 1e20, 1e20])]
prob.xfbd = [np.array([0, 0, 0, 0]), np.array([0, 0, 0, 0])]
if not args.lqr:
prob.add_obj(cost, True) # add a path cost
else:
lqr = LqrObj(R=np.ones(1), F=10*np.ones(4), Q=np.ones(4))
prob.add_lqr_obj(lqr)
snopt_mode = args.backend == 'snopt'
prob.preProcess(snopt_mode=snopt_mode) # construct the problem
# construct a solver for the problem
cfg = OptConfig(args.backend, deriv_check = 0) #, print_level=1)
slv = OptSolver(prob, cfg)
rst = slv.solve_rand()
print(rst.flag)
if rst.flag == 1:
# parse the solution
sol = prob.parse_sol(rst.sol.copy())
print(sol)
show_sol(sol)
def gradmode(args):
"""Solve the simple problem with gradient."""
sys = OneDcase()
cost = QuadCost()
N = 20
t0 = 0.0
tf = 2.0
prob = TrajOptProblem(sys, N, t0, tf, gradmode=True)
prob.xbd = [np.array([-1e20, -1e20]), np.array([1e20, 1e20])]
prob.ubd = [np.array([-1e20]), np.array([1e20])]
prob.x0bd = [np.array([0, 0]), np.array([0, 0])]
prob.xfbd = [np.array([1, 0]), np.array([1, 0])]
if not args.lqr:
prob.add_obj(cost, True) # add a path cost
else:
lqr = LqrObj(R=np.ones(1))
prob.add_lqr_obj(lqr)
snopt_mode = args.backend == 'snopt'
prob.preProcess(snopt_mode=snopt_mode) # construct the problem
# construct a solver for the problem
cfg = OptConfig(args.backend) #, print_level=3)
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 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 main():
args = get_onoff_args('backend ipopt')
sys = QuadRotor()
N = 40
dimx, dimu = sys.nx, sys.nu
cost = QuadCost(N, sys.nx, sys.nu)
t0 = 0.0
tf = 5.0
prob = TrajOptProblem(sys, N, t0, tf, gradmode=True)
prob.xbd = [-1e20 * np.ones(sys.nx), 1e20 * np.ones(sys.nx)]
prob.ubd = [0 * np.ones(sys.nu), 4 * np.ones(sys.nu)]
prob.x0bd = [np.zeros(sys.nx), np.zeros(sys.nx)]
prob.xfbd = [np.zeros(sys.nx), np.zeros(sys.nx)]
prob.xfbd[0][:3] = 5
prob.xfbd[1][:3] = 5
if False:
prob.add_obj(cost)
else:
lqr = LqrObj(R=np.ones(4))
prob.add_lqr_obj(lqr)
prob.pre_process()
# construct a solver for the problem
cfg = OptConfig(args.backend, print_level=5)
slv = OptSolver(prob, cfg)
guessx = np.zeros(prob.nx)
straightx = np.reshape(guessx[:N * dimx], (N, dimx))
for i in range(3):
straightx[:, i] = np.linspace(0, prob.xfbd[0][i], N)
guessx[N * dimx:-1] = np.random.random(N * dimu)
rst = slv.solve_guess(guessx)
print(rst.flag)
if rst.flag == 1:
# parse the solution
sol = prob.parse_sol(rst.sol.copy())
show_sol(sol)
def test_linear(ad):
print("Test on linear system")
if ad:
lin_sys = AdLinearSystem()
else:
lin_sys = LinearSystem()
N = 30
prob = TrajOptDisProblem(lin_sys, N)
lqr = LqrObj(R=np.ones(2))
prob.add_lqr_obj(lqr)
prob.xbd = [-inf_[4], inf_[4]]
prob.ubd = [-one_[2], one_[2]]
prob.x0bd = [0.5 * one_[4], 0.5 * one_[4]]
prob.xfbd = [-0. * one_[4], 0. * one_[4]]
prob.pre_process() # construct the problem
# construct a solver for the problem
cfg = OptConfig('snopt',
print_level=1,
major_iter=100,
deriv_check=0,
print_file='tmp.out')
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 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 testAnalytic():
"""Test car problem with analytic solution. See what's missing."""
sys = SecondOrderOmniCar() # simple car has no p
N = 10
t0 = 0
tf = 1
man_constr = CircleConstr()
prob = TrajOptManifoldCollocProblem(sys, N, t0, tf, man_constr)
setBound(prob)
lqr = LqrObj(R=np.ones(2), P=np.ones(1))
prob.add_lqr_obj(lqr)
prob.pre_process(defect_u=True, defect_p=False)
# construct solver
cfg = OptConfig()
slv = OptSolver(prob, cfg)
# generate an guess
x0 = np.zeros(prob.nx)
# generate the random initial guess
t = np.linspace(0, 1, 2 * N - 1)
theta = -np.pi * t**3 + 1.5 * np.pi * t**2
dtheta = -3 * np.pi * t**2 + 3 * np.pi * t
ddtheta = -6 * np.pi * t + 3 * np.pi
# assign them to it
useX, useU, useP = prob.__parseX__(x0)
radius = 1
stheta = np.sin(theta)
ctheta = np.cos(theta)
useX[:, 0] = radius * ctheta
useX[:, 1] = radius * stheta
useX[:, 2] = -radius * stheta * dtheta
useX[:, 3] = radius * ctheta * dtheta
useX[:, 4] = -radius * ctheta * dtheta**2 - radius * stheta * ddtheta
useX[:, 5] = -radius * stheta * dtheta**2 + radius * ctheta * ddtheta
useU[:, 0] = useX[:, 4]
useU[:, 1] = useX[:, 5]
# gamma is assumed zero
psf0 = prob.parse_f(x0)
rst = slv.solve_guess(x0)
psf = prob.parse_f(rst.sol)
# solve the problem
rst = slv.solve_guess(x0)
print(rst.flag)
# parse the solution
sol = prob.parse_sol(rst.sol)
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)
def test_pendulum(ad, fd):
print("Test on pendulum problem")
if ad:
pen_sys = AdPendulum()
elif fd:
pen_sys = FdPendulum()
else:
pen_sys = Pendulum()
N = 40
prob = TrajOptDisProblem(pen_sys, N)
prob.xbd = [np.array([-1e20, -1e20]), np.array([1e20, 1e20])]
prob.ubd = [np.array([-1.0]), np.array([1.0])]
prob.x0bd = [np.array([0, 0]), np.array([0, 0])]
delta = 0.2
prob.xfbd = [
np.array([np.pi - delta, -delta]),
np.array([np.pi + delta, delta])
]
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('snopt',
print_level=1,
deriv_check=0,
print_file='tmp.out',
major_iter=200)
slv = OptSolver(prob, cfg)
rst = slv.solve_rand()
print('Solve flag is ', 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()