move to pose test and animation are added

Author: AtsushiSakai

PathTracking/move_to_pose/animation.gif

@@ -110,17 +110,19 @@ def transformation_matrix(x, y, theta):
 
 
 def main():
-    x_start = 20 * random()
-    y_start = 20 * random()
-    theta_start = 2 * np.pi * random() - np.pi
-    x_goal = 20 * random()
-    y_goal = 20 * random()
-    theta_goal = 2 * np.pi * random() - np.pi
-    print("Initial x: %.2f m\nInitial y: %.2f m\nInitial theta: %.2f rad\n" %
-          (x_start, y_start, theta_start))
-    print("Goal x: %.2f m\nGoal y: %.2f m\nGoal theta: %.2f rad\n" %
-          (x_goal, y_goal, theta_goal))
-    move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal)
+
+    for i in range(5):
+        x_start = 20 * random()
+        y_start = 20 * random()
+        theta_start = 2 * np.pi * random() - np.pi
+        x_goal = 20 * random()
+        y_goal = 20 * random()
+        theta_goal = 2 * np.pi * random() - np.pi
+        print("Initial x: %.2f m\nInitial y: %.2f m\nInitial theta: %.2f rad\n" %
+              (x_start, y_start, theta_start))
+        print("Goal x: %.2f m\nGoal y: %.2f m\nGoal theta: %.2f rad\n" %
+              (x_goal, y_goal, theta_goal))
+        move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal)
 
 
 if __name__ == '__main__':

PathTracking/move_to_pose/move_to_pose.py

@@ -645,6 +645,13 @@ Ref:
 
 # Path tracking
 
+## move to a pose control
+
+This is a simulation of moving to a pose control
+
+![2](https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathTracking/move_to_pose/animation.gif)
+
+
 ## Pure pursuit tracking
 
 Path tracking simulation with pure pursuit steering control and PID speed control.