Merge pull request AtsushiSakai#1 from AtsushiSakai/master

Update

Author: daniel-s-ingram

.gitignore

@@ -4,6 +4,8 @@
 __pycache__/
 *.py[cod]
 *$py.class
+_build/
+.idea/
 
 # C extensions
 *.so

.gitmodules

@@ -0,0 +1,4 @@
+[submodule "doc/PythonRoboticsPaper"]
+	path = doc/PythonRoboticsPaper
+	url = https://github.com/AtsushiSakai/PythonRoboticsPaper
+

.travis.yml

@@ -3,6 +3,7 @@ python:
   - 3.6
 
 before_install:
+    - sudo apt-get update
     - wget https://repo.continuum.io/miniconda/Miniconda3-latest-Linux-x86_64.sh -O miniconda.sh
     - chmod +x miniconda.sh
     - bash miniconda.sh -b -p $HOME/miniconda
@@ -14,13 +15,15 @@ before_install:
     - conda info -a
 
 install:
-  - pip install scipy
-  - pip install numpy
-  - pip install matplotlib
-  - pip install pandas
-  - pip install cvxpy
-  - conda install -y -c cvxgrp cvxpy
-  - conda install -y -c anaconda cvxopt
+  - conda install numpy
+  - conda install scipy
+  - conda install matplotlib
+  - conda install pandas
+  - conda install -c conda-forge lapack
+  - conda install -c cvxgrp cvxpy
+  #- pip install cvxpy
+  #- conda install -y -c cvxgrp cvxpy
+  #- conda install -y -c anaconda cvxopt
 
 script:
   - python --version

AerialNavigation/Quadrotor.py …one_3d_trajectory_following/Quadrotor.pyAerialNavigation/Quadrotor.py renamed to AerialNavigation/drone_3d_trajectory_following/Quadrotor.py AerialNavigation/Quadrotor.py renamed to AerialNavigation/drone_3d_trajectory_following/Quadrotor.py

@@ -9,24 +9,26 @@
 import matplotlib.pyplot as plt
 from mpl_toolkits.mplot3d import Axes3D
 
+
 class Quadrotor():
-    def __init__(self, x=0, y=0, z=0, roll=0, pitch=0, yaw=0, size=0.25):
-        self.p1 = np.array([size/2, 0, 0, 1]).T
-        self.p2 = np.array([-size/2, 0, 0, 1]).T
-        self.p3 = np.array([0, size/2, 0, 1]).T
-        self.p4 = np.array([0, -size/2, 0, 1]).T
+    def __init__(self, x=0, y=0, z=0, roll=0, pitch=0, yaw=0, size=0.25, show_animation=True):
+        self.p1 = np.array([size / 2, 0, 0, 1]).T
+        self.p2 = np.array([-size / 2, 0, 0, 1]).T
+        self.p3 = np.array([0, size / 2, 0, 1]).T
+        self.p4 = np.array([0, -size / 2, 0, 1]).T
 
         self.x_data = []
         self.y_data = []
         self.z_data = []
-
-        plt.ion()
-
-        fig = plt.figure()
-        self.ax = fig.add_subplot(111, projection='3d')
+        self.show_animation = show_animation
 
         self.update_pose(x, y, z, roll, pitch, yaw)
 
+        if self.show_animation:
+            plt.ion()
+            fig = plt.figure()
+            self.ax = fig.add_subplot(111, projection='3d')
+
     def update_pose(self, x, y, z, roll, pitch, yaw):
         self.x = x
         self.y = y
@@ -37,7 +39,9 @@ def update_pose(self, x, y, z, roll, pitch, yaw):
         self.x_data.append(x)
         self.y_data.append(y)
         self.z_data.append(z)
-        self.plot()
+
+        if self.show_animation:
+            self.plot()
 
     def transformation_matrix(self):
         x = self.x
@@ -47,10 +51,11 @@ def transformation_matrix(self):
         pitch = self.pitch
         yaw = self.yaw
         return np.array(
-            [[cos(yaw)*cos(pitch), -sin(yaw)*cos(roll)+cos(yaw)*sin(pitch)*sin(roll), sin(yaw)*sin(roll)+cos(yaw)*sin(pitch)*cos(roll), x],
-             [sin(yaw)*cos(pitch), cos(yaw)*cos(roll)+sin(yaw)*sin(pitch)*sin(roll), -cos(yaw)*sin(roll)+sin(yaw)*sin(pitch)*cos(roll), y],
-             [-sin(pitch), cos(pitch)*sin(roll), cos(pitch)*cos(yaw), z]
-            ])
+            [[cos(yaw) * cos(pitch), -sin(yaw) * cos(roll) + cos(yaw) * sin(pitch) * sin(roll), sin(yaw) * sin(roll) + cos(yaw) * sin(pitch) * cos(roll), x],
+             [sin(yaw) * cos(pitch), cos(yaw) * cos(roll) + sin(yaw) * sin(pitch) *
+              sin(roll), -cos(yaw) * sin(roll) + sin(yaw) * sin(pitch) * cos(roll), y],
+             [-sin(pitch), cos(pitch) * sin(roll), cos(pitch) * cos(yaw), z]
+             ])
 
     def plot(self):
         T = self.transformation_matrix()
@@ -66,14 +71,15 @@ def plot(self):
                      [p1_t[1], p2_t[1], p3_t[1], p4_t[1]],
                      [p1_t[2], p2_t[2], p3_t[2], p4_t[2]], 'k.')
 
-        self.ax.plot([p1_t[0], p2_t[0]], [p1_t[1], p2_t[1]], [p1_t[2], p2_t[2]], 'r-')
-        self.ax.plot([p3_t[0], p4_t[0]], [p3_t[1], p4_t[1]], [p3_t[2], p4_t[2]], 'r-')
+        self.ax.plot([p1_t[0], p2_t[0]], [p1_t[1], p2_t[1]],
+                     [p1_t[2], p2_t[2]], 'r-')
+        self.ax.plot([p3_t[0], p4_t[0]], [p3_t[1], p4_t[1]],
+                     [p3_t[2], p4_t[2]], 'r-')
 
         self.ax.plot(self.x_data, self.y_data, self.z_data, 'b:')
 
         plt.xlim(-5, 5)
         plt.ylim(-5, 5)
         self.ax.set_zlim(0, 10)
 
-        plt.show()
         plt.pause(0.001)

AerialNavigation/TrajectoryGenerator.py …jectory_following/TrajectoryGenerator.pyAerialNavigation/TrajectoryGenerator.py renamed to AerialNavigation/drone_3d_trajectory_following/TrajectoryGenerator.py AerialNavigation/TrajectoryGenerator.py renamed to AerialNavigation/drone_3d_trajectory_following/TrajectoryGenerator.py

@@ -9,27 +9,30 @@
 from Quadrotor import Quadrotor
 from TrajectoryGenerator import TrajectoryGenerator
 
-#Simulation parameters
+show_animation = True
+
+# Simulation parameters
 g = 9.81
 m = 0.2
 Ixx = 1
 Iyy = 1
 Izz = 1
 T = 5
 
-#Proportional coefficients
+# Proportional coefficients
 Kp_x = 1
 Kp_y = 1
 Kp_z = 1
 Kp_roll = 25
 Kp_pitch = 25
 Kp_yaw = 25
 
-#Derivative coefficients
+# Derivative coefficients
 Kd_x = 10
 Kd_y = 10
 Kd_z = 1
 
+
 def quad_sim(x_c, y_c, z_c):
     """
     Calculates the necessary thrust and torques for the quadrotor to
@@ -57,9 +60,12 @@ def quad_sim(x_c, y_c, z_c):
     dt = 0.1
     t = 0
 
-    q = Quadrotor(x=x_pos, y=y_pos, z=z_pos, roll=roll, pitch=pitch, yaw=yaw, size=1)
+    q = Quadrotor(x=x_pos, y=y_pos, z=z_pos, roll=roll,
+                  pitch=pitch, yaw=yaw, size=1, show_animation=show_animation)
 
     i = 0
+    n_run = 8
+    irun = 0
 
     while True:
         while t <= T:
@@ -73,38 +79,48 @@ def quad_sim(x_c, y_c, z_c):
             des_y_acc = calculate_acceleration(y_c[i], t)
             des_z_acc = calculate_acceleration(z_c[i], t)
 
-            thrust = m*(g + des_z_acc + Kp_z*(des_z_pos - z_pos) + Kd_z*(des_z_vel - z_vel))
+            thrust = m * (g + des_z_acc + Kp_z * (des_z_pos -
+                                                  z_pos) + Kd_z * (des_z_vel - z_vel))
 
-            roll_torque = Kp_roll*(((des_x_acc*sin(des_yaw) - des_y_acc*cos(des_yaw))/g) - roll)
-            pitch_torque = Kp_pitch*(((des_x_acc*cos(des_yaw) - des_y_acc*sin(des_yaw))/g) - pitch)
-            yaw_torque = Kp_yaw*(des_yaw - yaw)
+            roll_torque = Kp_roll * \
+                (((des_x_acc * sin(des_yaw) - des_y_acc * cos(des_yaw)) / g) - roll)
+            pitch_torque = Kp_pitch * \
+                (((des_x_acc * cos(des_yaw) - des_y_acc * sin(des_yaw)) / g) - pitch)
+            yaw_torque = Kp_yaw * (des_yaw - yaw)
 
-            roll_vel += roll_torque*dt/Ixx
-            pitch_vel += pitch_torque*dt/Iyy
-            yaw_vel += yaw_torque*dt/Izz
+            roll_vel += roll_torque * dt / Ixx
+            pitch_vel += pitch_torque * dt / Iyy
+            yaw_vel += yaw_torque * dt / Izz
 
-            roll += roll_vel*dt
-            pitch += pitch_vel*dt
-            yaw += yaw_vel*dt
+            roll += roll_vel * dt
+            pitch += pitch_vel * dt
+            yaw += yaw_vel * dt
 
             R = rotation_matrix(roll, pitch, yaw)
-            acc = (np.matmul(R, np.array([0, 0, thrust]).T) - np.array([0, 0, m*g]).T)/m
+            acc = (np.matmul(R, np.array(
+                [0, 0, thrust]).T) - np.array([0, 0, m * g]).T) / m
             x_acc = acc[0]
             y_acc = acc[1]
             z_acc = acc[2]
-            x_vel += x_acc*dt
-            y_vel += y_acc*dt
-            z_vel += z_acc*dt
-            x_pos += x_vel*dt
-            y_pos += y_vel*dt
-            z_pos += z_vel*dt
+            x_vel += x_acc * dt
+            y_vel += y_acc * dt
+            z_vel += z_acc * dt
+            x_pos += x_vel * dt
+            y_pos += y_vel * dt
+            z_pos += z_vel * dt
 
             q.update_pose(x_pos, y_pos, z_pos, roll, pitch, yaw)
 
             t += dt
 
         t = 0
-        i = (i+1)%4
+        i = (i + 1) % 4
+        irun += 1
+        if irun >= n_run:
+            break
+
+    print("Done")
+
 
 def calculate_position(c, t):
     """
@@ -118,7 +134,8 @@ def calculate_position(c, t):
     Returns
         Position
     """
-    return c[0]*t**5 + c[1]*t**4 + c[2]*t**3 + c[3]*t**2 + c[4]*t + c[5]
+    return c[0] * t**5 + c[1] * t**4 + c[2] * t**3 + c[3] * t**2 + c[4] * t + c[5]
+
 
 def calculate_velocity(c, t):
     """
@@ -132,7 +149,8 @@ def calculate_velocity(c, t):
     Returns
         Velocity
     """
-    return 5*c[0]*t**4 + 4*c[1]*t**3 + 3*c[2]*t**2 + 2*c[3]*t + c[4]
+    return 5 * c[0] * t**4 + 4 * c[1] * t**3 + 3 * c[2] * t**2 + 2 * c[3] * t + c[4]
+
 
 def calculate_acceleration(c, t):
     """
@@ -146,7 +164,8 @@ def calculate_acceleration(c, t):
     Returns
         Acceleration
     """
-    return 20*c[0]*t**3 + 12*c[1]*t**2 + 6*c[2]*t + 2*c[3]
+    return 20 * c[0] * t**3 + 12 * c[1] * t**2 + 6 * c[2] * t + 2 * c[3]
+
 
 def rotation_matrix(roll, pitch, yaw):
     """
@@ -161,10 +180,12 @@ def rotation_matrix(roll, pitch, yaw):
         3x3 rotation matrix as NumPy array
     """
     return np.array(
-        [[cos(yaw)*cos(pitch), -sin(yaw)*cos(roll)+cos(yaw)*sin(pitch)*sin(roll), sin(yaw)*sin(roll)+cos(yaw)*sin(pitch)*cos(roll)],
-         [sin(yaw)*cos(pitch), cos(yaw)*cos(roll)+sin(yaw)*sin(pitch)*sin(roll), -cos(yaw)*sin(roll)+sin(yaw)*sin(pitch)*cos(roll)],
-         [-sin(pitch), cos(pitch)*sin(roll), cos(pitch)*cos(yaw)]
-        ])
+        [[cos(yaw) * cos(pitch), -sin(yaw) * cos(roll) + cos(yaw) * sin(pitch) * sin(roll), sin(yaw) * sin(roll) + cos(yaw) * sin(pitch) * cos(roll)],
+         [sin(yaw) * cos(pitch), cos(yaw) * cos(roll) + sin(yaw) * sin(pitch) *
+          sin(roll), -cos(yaw) * sin(roll) + sin(yaw) * sin(pitch) * cos(roll)],
+         [-sin(pitch), cos(pitch) * sin(roll), cos(pitch) * cos(yaw)]
+         ])
+
 
 def main():
     """
@@ -177,13 +198,14 @@ def main():
     waypoints = [[-5, -5, 5], [5, -5, 5], [5, 5, 5], [-5, 5, 5]]
 
     for i in range(4):
-        traj = TrajectoryGenerator(waypoints[i], waypoints[(i+1)%4], T)
+        traj = TrajectoryGenerator(waypoints[i], waypoints[(i + 1) % 4], T)
         traj.solve()
         x_coeffs[i] = traj.x_c
         y_coeffs[i] = traj.y_c
         z_coeffs[i] = traj.z_c
 
     quad_sim(x_coeffs, y_coeffs, z_coeffs)
 
+
 if __name__ == "__main__":
     main()

AerialNavigation/drone_3d_trajectory_following/animation.gif

@@ -7,50 +7,61 @@
 import numpy as np
 import matplotlib.pyplot as plt
 
+
 class NLinkArm(object):
-    def __init__(self, link_lengths, joint_angles, goal):
+    def __init__(self, link_lengths, joint_angles, goal, show_animation):
+        self.show_animation = show_animation
         self.n_links = len(link_lengths)
         if self.n_links is not len(joint_angles):
             raise ValueError()
 
         self.link_lengths = np.array(link_lengths)
         self.joint_angles = np.array(joint_angles)
-        self.points = [[0, 0] for _ in range(self.n_links+1)]
+        self.points = [[0, 0] for _ in range(self.n_links + 1)]
 
         self.lim = sum(link_lengths)
         self.goal = np.array(goal).T
 
-        self.fig = plt.figure()
-        self.fig.canvas.mpl_connect('button_press_event', self.click)
+        if show_animation:
+            self.fig = plt.figure()
+            self.fig.canvas.mpl_connect('button_press_event', self.click)
 
-        plt.ion()
-        plt.show()
+            plt.ion()
+            plt.show()
 
         self.update_points()
 
     def update_joints(self, joint_angles):
         self.joint_angles = joint_angles
+
         self.update_points()
 
     def update_points(self):
-        for i in range(1, self.n_links+1):
-            self.points[i][0] = self.points[i-1][0] + self.link_lengths[i-1]*np.cos(np.sum(self.joint_angles[:i]))
-            self.points[i][1] = self.points[i-1][1] + self.link_lengths[i-1]*np.sin(np.sum(self.joint_angles[:i]))
+        for i in range(1, self.n_links + 1):
+            self.points[i][0] = self.points[i - 1][0] + \
+                self.link_lengths[i - 1] * \
+                np.cos(np.sum(self.joint_angles[:i]))
+            self.points[i][1] = self.points[i - 1][1] + \
+                self.link_lengths[i - 1] * \
+                np.sin(np.sum(self.joint_angles[:i]))
 
         self.end_effector = np.array(self.points[self.n_links]).T
-        self.plot()
+        if self.show_animation:
+            self.plot()
 
     def plot(self):
         plt.cla()
 
-        for i in range(self.n_links+1):
+        for i in range(self.n_links + 1):
             if i is not self.n_links:
-                plt.plot([self.points[i][0], self.points[i+1][0]], [self.points[i][1], self.points[i+1][1]], 'r-')
+                plt.plot([self.points[i][0], self.points[i + 1][0]],
+                         [self.points[i][1], self.points[i + 1][1]], 'r-')
             plt.plot(self.points[i][0], self.points[i][1], 'ko')
 
         plt.plot(self.goal[0], self.goal[1], 'gx')
 
-        plt.plot([self.end_effector[0], self.goal[0]], [self.end_effector[1], self.goal[1]], 'g--')
+        plt.plot([self.end_effector[0], self.goal[0]], [
+                 self.end_effector[1], self.goal[1]], 'g--')
 
         plt.xlim([-self.lim, self.lim])
         plt.ylim([-self.lim, self.lim])