<filename>examples/LunarLander.py
"""

Lunar lander example

Authors: <NAME>, <NAME>
Date: 05/02/2021
"""

import sys
sys.path.insert(0, '..')

import numpy as np
from sympy import symbols
from sympy import sin, cos
from sympy import Matrix

from pydcol.Animator import draw_lander
from pydcol.CollocMethods import *
from pydcol.ProblemDefinition import CollocationProblem

if __name__ == "__main__":

	colloc_method = HERM

	# define variables
	x, xdot, y, ydot, th, thdot = symbols("x xdot y ydot th thdot")
	Fl, Ft = symbols("Fl Ft")
	state_vars = [x, xdot, y, ydot, th, thdot]
	control_vars = [Fl, Ft]

	# Given system equations
	m, g, J = 10e3, 1.6, 1e5 # kg, m/s^2. kg * m^2
	xddot = (Fl*cos(th) - Ft*sin(th)) / m
	yddot = (Fl*sin(th) + Ft*cos(th) - m*g) / m
	thddot = 4*Fl/J
	ode = [xdot, xddot, ydot, yddot, thdot, thddot]

	t0_ = 0
	tf_ = 100
	N_ = 100
	tspan = np.linspace(t0_, tf_, N_)

	# [x, xdot, y, ydot, th, thdot]
	X_start = np.array([0.5e3, 0, 3e3, 0, 0, 0], dtype=float) # known initial state
	X_goal = np.array([0, 0, 0, 0, 0, 0], dtype=float) # desired goal state
	x_bounds = [[None,None], [None,None], [0,None], [None,None], [-np.pi/4,np.pi/4], [None,None]]
	u_bounds = [[-5e3, 5e3],[0, 40e3]]
	bounds = x_bounds + u_bounds

	# Define problem
	print("Setup")
	problem = CollocationProblem(state_vars, control_vars, ode, tspan, X_start, X_goal, colloc_method)

	# solve problem
	print("Solve")
	sol_c = problem.solve(bounds=bounds, solver='scipy')

	# evaluate solution
	problem.evaluate(ivp_method='Radau')

	# draw lander
	draw_lander(sol_c.x, sol_c.u, save_anim=False)

<filename>examples/LunarLander.py
"""

Lunar lander example

Authors: <NAME>, <NAME>
Date: 05/02/2021
"""

import sys
sys.path.insert(0, '..')

import numpy as np
from sympy import symbols
from sympy import sin, cos
from sympy import Matrix

from pydcol.Animator import draw_lander
from pydcol.CollocMethods import *
from pydcol.ProblemDefinition import CollocationProblem

if __name__ == "__main__":

	colloc_method = HERM

	# define variables
	x, xdot, y, ydot, th, thdot = symbols("x xdot y ydot th thdot")
	Fl, Ft = symbols("Fl Ft")
	state_vars = [x, xdot, y, ydot, th, thdot]
	control_vars = [Fl, Ft]

	# Given system equations
	m, g, J = 10e3, 1.6, 1e5 # kg, m/s^2. kg * m^2
	xddot = (Fl*cos(th) - Ft*sin(th)) / m
	yddot = (Fl*sin(th) + Ft*cos(th) - m*g) / m
	thddot = 4*Fl/J
	ode = [xdot, xddot, ydot, yddot, thdot, thddot]

	t0_ = 0
	tf_ = 100
	N_ = 100
	tspan = np.linspace(t0_, tf_, N_)

	# [x, xdot, y, ydot, th, thdot]
	X_start = np.array([0.5e3, 0, 3e3, 0, 0, 0], dtype=float) # known initial state
	X_goal = np.array([0, 0, 0, 0, 0, 0], dtype=float) # desired goal state
	x_bounds = [[None,None], [None,None], [0,None], [None,None], [-np.pi/4,np.pi/4], [None,None]]
	u_bounds = [[-5e3, 5e3],[0, 40e3]]
	bounds = x_bounds + u_bounds

	# Define problem
	print("Setup")
	problem = CollocationProblem(state_vars, control_vars, ode, tspan, X_start, X_goal, colloc_method)

	# solve problem
	print("Solve")
	sol_c = problem.solve(bounds=bounds, solver='scipy')

	# evaluate solution
	problem.evaluate(ivp_method='Radau')

	# draw lander
	draw_lander(sol_c.x, sol_c.u, save_anim=False)

Lunar lander example  Authors: <NAME>, <NAME> Date: 05/02/2021 # define variables # Given system equations # kg, m/s^2. kg * m^2 # [x, xdot, y, ydot, th, thdot] # known initial state # desired goal state # Define problem # solve problem # evaluate solution # draw lander