clean up seven joint arm script

Author: AtsushiSakai

ArmNavigation/seven_joint_arm_to_point_control/rrt_star_seven_joint_arm.py renamed to ArmNavigation/rrt_star_seven_joint_arm_control/rrt_star_seven_joint_arm_control.py

@@ -224,7 +224,7 @@ def generate_final_course(self, goal_ind):
             path.append(node.x)
             node = node.parent
         path.append(node.x)
-
+        reversed(path)
         return path
 
     def calc_dist_to_goal(self, x):
@@ -385,17 +385,22 @@ def main():
         # Draw final path
         if show_animation:
             ax = rrt_star.draw_graph()
-            for i, q in enumerate(reversed(path)):
+
+            # Plot final configuration
+            x_points, y_points, z_points = seven_joint_arm.get_points(path[-1])
+            ax.plot([x for x in x_points],
+                    [y for y in y_points],
+                    [z for z in z_points],
+                    "o-", color="red", ms=5, mew=0.5)
+
+            for i, q in enumerate(path):
                 x_points, y_points, z_points = seven_joint_arm.get_points(q)
-                if i == 0 or i == len(path) - 1:
-                    color = None
-                else:
-                    color = "grey"
                 ax.plot([x for x in x_points],
                         [y for y in y_points],
                         [z for z in z_points],
-                        "o-", color=color,  ms=4, mew=0.5)
+                        "o-", color="grey",  ms=4, mew=0.5)
                 plt.pause(0.01)
+
             plt.show()
 
 

tests/test_rrt_star_seven_joint_arm.py

@@ -3,10 +3,10 @@
 from unittest import TestCase
 
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
-                "/../ArmNavigation/seven_joint_arm_to_point_control/")
+                "/../ArmNavigation/rrt_star_seven_joint_arm_control/")
 
 try:
-    import rrt_star_seven_joint_arm as m
+    import rrt_star_seven_joint_arm_control as m
 except ImportError:
     raise