add use case

Author: AtsushiSakai

AerialNavigation/rocket_powered_landing/rocket_powered_landing.py

@@ -343,11 +343,7 @@ def omega(self, w):
 
     def initialize_trajectory(self, X, U):
         """
-        Initialize the trajectory.
-
-        :param X: Numpy array of states to be initialized
-        :param U: Numpy array of inputs to be initialized
-        :return: The initialized X and U
+        Initialize the trajectory with linear approximation.
         """
         K = X.shape[1]
 
@@ -565,6 +561,46 @@ def axis3d_equal(X, Y, Z, ax):
         ax.plot([xb], [yb], [zb], 'w')
 
 
+def plot_animation(X, U):
+
+    fig = plt.figure()
+    ax = fig.gca(projection='3d')
+
+    for k in range(K):
+        plt.cla()
+        ax.plot(X[2, :], X[3, :], X[1, :])  # trajectory
+        ax.scatter3D([0.0], [0.0], [0.0], c="r",
+                     marker="x")  # target landing point
+        axis3d_equal(X[2, :], X[3, :], X[1, :], ax)
+
+        rx, ry, rz = X[1:4, k]
+        vx, vy, vz = X[4:7, k]
+        qw, qx, qy, qz = X[7:11, k]
+
+        CBI = np.array([
+            [1 - 2 * (qy ** 2 + qz ** 2), 2 * (qx * qy + qw * qz),
+             2 * (qx * qz - qw * qy)],
+            [2 * (qx * qy - qw * qz), 1 - 2 *
+             (qx ** 2 + qz ** 2), 2 * (qy * qz + qw * qx)],
+            [2 * (qx * qz + qw * qy), 2 * (qy * qz - qw * qx),
+             1 - 2 * (qx ** 2 + qy ** 2)]
+        ])
+
+        Fx, Fy, Fz = np.dot(np.transpose(CBI), U[:, k])
+        dx, dy, dz = np.dot(np.transpose(CBI), np.array([1., 0., 0.]))
+
+        # attitude vector
+        ax.quiver(ry, rz, rx, dy, dz, dx, length=0.5, linewidth=3.0,
+                  arrow_length_ratio=0.0, color='black')
+
+        # thrust vector
+        ax.quiver(ry, rz, rx, -Fy, -Fz, -Fx, length=0.1,
+                  arrow_length_ratio=0.0, color='red')
+
+        ax.set_title("Rocket powered landing")
+        plt.pause(0.5)
+
+
 def main():
     print("start!!")
     m = Model_6DoF()
@@ -625,45 +661,5 @@ def main():
     print("done!!")
 
 
-def plot_animation(X, U):
-
-    fig = plt.figure()
-    ax = fig.gca(projection='3d')
-
-    for k in range(K):
-        plt.cla()
-        ax.plot(X[2, :], X[3, :], X[1, :])  # trajectory
-        ax.scatter3D([0.0], [0.0], [0.0], c="r",
-                     marker="x")  # target landing point
-        axis3d_equal(X[2, :], X[3, :], X[1, :], ax)
-
-        rx, ry, rz = X[1:4, k]
-        vx, vy, vz = X[4:7, k]
-        qw, qx, qy, qz = X[7:11, k]
-
-        CBI = np.array([
-            [1 - 2 * (qy ** 2 + qz ** 2), 2 * (qx * qy + qw * qz),
-             2 * (qx * qz - qw * qy)],
-            [2 * (qx * qy - qw * qz), 1 - 2 *
-             (qx ** 2 + qz ** 2), 2 * (qy * qz + qw * qx)],
-            [2 * (qx * qz + qw * qy), 2 * (qy * qz - qw * qx),
-             1 - 2 * (qx ** 2 + qy ** 2)]
-        ])
-
-        Fx, Fy, Fz = np.dot(np.transpose(CBI), U[:, k])
-        dx, dy, dz = np.dot(np.transpose(CBI), np.array([1., 0., 0.]))
-
-        # attitude vector
-        ax.quiver(ry, rz, rx, dy, dz, dx, length=0.5, linewidth=3.0,
-                  arrow_length_ratio=0.0, color='black')
-
-        # thrust vector
-        ax.quiver(ry, rz, rx, -Fy, -Fz, -Fx, length=0.1,
-                  arrow_length_ratio=0.0, color='red')
-
-        ax.set_title("Rocket powered landing")
-        plt.pause(0.5)
-
-
 if __name__ == '__main__':
     main()

README.md

@@ -11,6 +11,7 @@
 Python codes for robotics algorithm.
 
 
+
 # Table of Contents
    * [What is this?](#what-is-this)
    * [Requirements](#requirements)
@@ -59,6 +60,7 @@ Python codes for robotics algorithm.
    * [Aerial Navigation](#aerial-navigation)
       * [drone 3d trajectory following](#drone-3d-trajectory-following)
    * [License](#license)
+   * [Use-case](#use-case)
    * [Contribution](#contribution)
    * [Support](#support)
    * [Authors](#authors)
@@ -569,3 +571,4 @@ This is a list: [Users comments](https://github.com/AtsushiSakai/PythonRobotics/
 
 
 
+