Exemple #1
0
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
Exemple #3
0
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)
Exemple #4
0
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)
Exemple #6
0
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()