diff --git a/PathPlanning/__pycache__/__init__.cpython-310.pyc b/PathPlanning/__pycache__/__init__.cpython-310.pyc new file mode 100644 index 0000000..644db30 Binary files /dev/null and b/PathPlanning/__pycache__/__init__.cpython-310.pyc differ diff --git a/PathPlanning/__pycache__/__init__.cpython-39.pyc b/PathPlanning/__pycache__/__init__.cpython-39.pyc new file mode 100644 index 0000000..db20dc4 Binary files /dev/null and b/PathPlanning/__pycache__/__init__.cpython-39.pyc differ diff --git a/PathPlanning/__pycache__/maputils.cpython-310.pyc b/PathPlanning/__pycache__/maputils.cpython-310.pyc new file mode 100644 index 0000000..ab1c239 Binary files /dev/null and b/PathPlanning/__pycache__/maputils.cpython-310.pyc differ diff --git a/PathPlanning/__pycache__/rrt.cpython-310.pyc b/PathPlanning/__pycache__/rrt.cpython-310.pyc new file mode 100644 index 0000000..f16e241 Binary files /dev/null and b/PathPlanning/__pycache__/rrt.cpython-310.pyc differ diff --git a/PathPlanning/__pycache__/rrt.cpython-39.pyc b/PathPlanning/__pycache__/rrt.cpython-39.pyc new file mode 100644 index 0000000..3bc107a Binary files /dev/null and b/PathPlanning/__pycache__/rrt.cpython-39.pyc differ diff --git a/PathPlanning/__pycache__/rrt_star.cpython-310.pyc b/PathPlanning/__pycache__/rrt_star.cpython-310.pyc new file mode 100644 index 0000000..e62fdbf Binary files /dev/null and b/PathPlanning/__pycache__/rrt_star.cpython-310.pyc differ diff --git a/PathPlanning/__pycache__/rrtutils.cpython-310.pyc b/PathPlanning/__pycache__/rrtutils.cpython-310.pyc new file mode 100644 index 0000000..cad3ffc Binary files /dev/null and b/PathPlanning/__pycache__/rrtutils.cpython-310.pyc differ diff --git a/PathPlanning/__pycache__/rrtutils.cpython-39.pyc b/PathPlanning/__pycache__/rrtutils.cpython-39.pyc new file mode 100644 index 0000000..026d994 Binary files /dev/null and b/PathPlanning/__pycache__/rrtutils.cpython-39.pyc differ diff --git a/PathPlanning/__pycache__/sampleutils.cpython-310.pyc b/PathPlanning/__pycache__/sampleutils.cpython-310.pyc new file mode 100644 index 0000000..4e95318 Binary files /dev/null and b/PathPlanning/__pycache__/sampleutils.cpython-310.pyc differ diff --git a/Quadrotor/__pycache__/__init__.cpython-310.pyc b/Quadrotor/__pycache__/__init__.cpython-310.pyc new file mode 100644 index 0000000..00c03ff Binary files /dev/null and b/Quadrotor/__pycache__/__init__.cpython-310.pyc differ diff --git a/Quadrotor/__pycache__/params.cpython-310.pyc b/Quadrotor/__pycache__/params.cpython-310.pyc new file mode 100644 index 0000000..645e7b1 Binary files /dev/null and b/Quadrotor/__pycache__/params.cpython-310.pyc differ diff --git a/Quadrotor/__pycache__/quadrotor.cpython-310.pyc b/Quadrotor/__pycache__/quadrotor.cpython-310.pyc new file mode 100644 index 0000000..e838623 Binary files /dev/null and b/Quadrotor/__pycache__/quadrotor.cpython-310.pyc differ diff --git a/Quadrotor/__pycache__/quadsim.cpython-310.pyc b/Quadrotor/__pycache__/quadsim.cpython-310.pyc new file mode 100644 index 0000000..ad423d4 Binary files /dev/null and b/Quadrotor/__pycache__/quadsim.cpython-310.pyc differ diff --git a/Quadrotor/__pycache__/quaternion.cpython-310.pyc b/Quadrotor/__pycache__/quaternion.cpython-310.pyc new file mode 100644 index 0000000..3c20e70 Binary files /dev/null and b/Quadrotor/__pycache__/quaternion.cpython-310.pyc differ diff --git a/Quadrotor/__pycache__/utils.cpython-310.pyc b/Quadrotor/__pycache__/utils.cpython-310.pyc new file mode 100644 index 0000000..fd805da Binary files /dev/null and b/Quadrotor/__pycache__/utils.cpython-310.pyc differ diff --git a/TrajGen/__pycache__/__init__.cpython-310.pyc b/TrajGen/__pycache__/__init__.cpython-310.pyc new file mode 100644 index 0000000..ce34b40 Binary files /dev/null and b/TrajGen/__pycache__/__init__.cpython-310.pyc differ diff --git a/TrajGen/__pycache__/trajGen.cpython-310.pyc b/TrajGen/__pycache__/trajGen.cpython-310.pyc new file mode 100644 index 0000000..aeb5c41 Binary files /dev/null and b/TrajGen/__pycache__/trajGen.cpython-310.pyc differ diff --git a/TrajGen/__pycache__/trajutils.cpython-310.pyc b/TrajGen/__pycache__/trajutils.cpython-310.pyc new file mode 100644 index 0000000..bad974f Binary files /dev/null and b/TrajGen/__pycache__/trajutils.cpython-310.pyc differ diff --git a/__pycache__/controller.cpython-310.pyc b/__pycache__/controller.cpython-310.pyc new file mode 100644 index 0000000..e37e32b Binary files /dev/null and b/__pycache__/controller.cpython-310.pyc differ diff --git a/runsim.py b/runsim.py index a22fba9..4f163cd 100644 --- a/runsim.py +++ b/runsim.py @@ -1,4 +1,6 @@ import numpy as np +import matplotlib +import platform import matplotlib.pyplot as plt import mpl_toolkits.mplot3d.axes3d as Axes3D @@ -6,6 +8,7 @@ from TrajGen import trajGenerator, Helix_waypoints, Circle_waypoints from Quadrotor import QuadSim import controller + np.random.seed(8) # 3D boxes lx, ly, lz, hx, hy, hz @@ -17,42 +20,50 @@ [70, 50, 0, 80, 80, 100]] # limits on map dimensions -bounds = np.array([0,100]) +bounds = np.array([0, 100]) # create map with obstacles -mapobs = Map(obstacles, bounds, dim = 3) +mapobs = Map(obstacles, bounds, dim=3) -#plan a path from start to goal -start = np.array([80,20,10]) -goal = np.array([30,80,80]) +# plan a path from start to goal +start = np.array([80, 20, 10]) +goal = np.array([30, 80, 80]) -rrt = RRTStar(start = start, goal = goal, - Map = mapobs, max_iter = 500, - goal_sample_rate = 0.1) +rrt = RRTStar(start=start, goal=goal, + Map=mapobs, max_iter=500, + goal_sample_rate=0.1) waypoints, min_cost = rrt.plan() +# scale the waypoints to real dimensions +#waypoints = 0.02 * waypoints + +# Generate trajectory through waypoints +#traj = trajGenerator(waypoints, max_vel=10, gamma=1e6) -#scale the waypoints to real dimensions -waypoints = 0.02*waypoints +# initialise simulation with given controller and trajectory +#Tmax = traj.TS[-1] +#des_state = traj.get_des_state +#sim = QuadSim(controller, des_state, Tmax) +waypoints = Helix_waypoints(5) #Generate trajectory through waypoints -traj = trajGenerator(waypoints, max_vel = 10, gamma = 1e6) +traj = trajGenerator(waypoints,max_vel = 10,gamma = 1e6) #initialise simulation with given controller and trajectory Tmax = traj.TS[-1] des_state = traj.get_des_state sim = QuadSim(controller,des_state,Tmax) - -#create a figure +# create a figure fig = plt.figure() -ax = Axes3D.Axes3D(fig) -ax.set_xlim((0,2)) -ax.set_ylim((0,2)) -ax.set_zlim((0,2)) +ax=fig.add_subplot(111,projection='3d') +#ax = Axes3D.Axes3D(fig) +ax.set_xlim((0, 2)) +ax.set_ylim((0, 2)) +ax.set_zlim((0, 2)) -#plot the waypoints and obstacles +# plot the waypoints and obstacles rrt.draw_path(ax, waypoints) -mapobs.plotobs(ax, scale = 0.02) +mapobs.plotobs(ax, scale=0.02) -#run simulation +# run simulation sim.run(ax) diff --git a/test.py b/test.py new file mode 100644 index 0000000..0813905 --- /dev/null +++ b/test.py @@ -0,0 +1,13 @@ +from mpl_toolkits.mplot3d import Axes3D +import matplotlib.pyplot as plt +import numpy as np +import pandas as pd + +df_test=pd.DataFrame(data=np.random.normal(0,1,(20,3)),columns=['a','b','c']) +fig=plt.figure() +ax=fig.add_subplot(111,projection='3d') +ax.scatter=(df_test['a'],df_test['b'],df_test['c']) +ax.set_xlabel('a') +ax.set_ylabel('b') +ax.set_zlabel('c') +plt.show() \ No newline at end of file