Use quintic splines in example move

Author: urfeex

ur_robot_driver/scripts/example_move.py

@@ -33,7 +33,7 @@
 # This is an example of how to interface the robot without any additional ROS components. For
 # real-life applications, we do recommend to use something like MoveIt!
 
-import time
+from math import pi
 
 import rclpy
 from rclpy.action import ActionClient
@@ -49,27 +49,39 @@
         {
             "positions": [0.043128, -1.28824, 1.37179, -1.82208, -1.63632, -0.18],
             "velocities": [0, 0, 0, 0, 0, 0],
+            "accelerations": [0, 0, 0, 0, 0, 0],
             "time_from_start": Duration(sec=4, nanosec=0),
         },
         {
             "positions": [-0.195016, -1.70093, 0.902027, -0.944217, -1.52982, -0.195171],
             "velocities": [0, 0, 0, 0, 0, 0],
+            "accelerations": [0, 0, 0, 0, 0, 0],
             "time_from_start": Duration(sec=8, nanosec=0),
         },
     ],
     "traj1": [
         {
             "positions": [-0.195016, -1.70094, 0.902027, -0.944217, -1.52982, -0.195171],
             "velocities": [0, 0, 0, 0, 0, 0],
+            "accelerations": [0, 0, 0, 0, 0, 0],
             "time_from_start": Duration(sec=0, nanosec=0),
         },
         {
             "positions": [0.30493, -0.982258, 0.955637, -1.48215, -1.72737, 0.204445],
             "velocities": [0, 0, 0, 0, 0, 0],
+            "accelerations": [0, 0, 0, 0, 0, 0],
             "time_from_start": Duration(sec=8, nanosec=0),
         },
     ],
 }
+HOME = {
+    "elbow_joint": 0.0,
+    "shoulder_lift_joint": -1.5708,
+    "shoulder_pan_joint": 0.0,
+    "wrist_1_joint": -1.5708,
+    "wrist_2_joint": 0.0,
+    "wrist_3_joint": 0.0,
+}
 
 
 class JTCClient(rclpy.node.Node):
@@ -96,6 +108,23 @@ def __init__(self):
         if self.joints is None or len(self.joints) == 0:
             raise Exception('"joints" parameter is required')
 
+        waypts = [[HOME[joint] + i * pi / 4 for joint in self.joints] for i in [0, -1, 1]]
+        time_vec = [
+            Duration(sec=4, nanosec=0),
+            Duration(sec=8, nanosec=0),
+            Duration(sec=12, nanosec=0),
+        ]
+        TRAJECTORIES["test_traj"] = [
+            {
+                "positions": waypts[i],
+                "velocities": [0] * 6,
+                "accelerations": [0] * 6,
+                "time_from_start": time_vec[i],
+            }
+            for i in range(len(waypts))
+        ]
+        # (time_vec[i], waypts[i]) for i in range(len(waypts))]
+
         self._action_client = ActionClient(self, FollowJointTrajectory, controller_name)
         self.get_logger().info(f"Waiting for action server on {controller_name}")
         self._action_client.wait_for_server()
@@ -116,15 +145,17 @@ def parse_trajectories(self):
                 point = JointTrajectoryPoint()
                 point.positions = pt["positions"]
                 point.velocities = pt["velocities"]
+                point.accelerations = pt["accelerations"]
                 point.time_from_start = pt["time_from_start"]
                 goal.points.append(point)
 
             self.goals[traj_name] = goal
 
     def execute_next_trajectory(self):
         if self.i >= len(self.goals):
-            self.get_logger().info("Done with all trajectories")
-            raise SystemExit
+            self.i = 0
+            # self.get_logger().info("Done with all trajectories")
+            # raise SystemExit
         traj_name = list(self.goals)[self.i]
         self.i = self.i + 1
         if traj_name:
@@ -159,7 +190,6 @@ def get_result_callback(self, future):
         status = future.result().status
         self.get_logger().info(f"Done with result: {self.status_to_str(status)}")
         if status == GoalStatus.STATUS_SUCCEEDED:
-            time.sleep(2)
             self.execute_next_trajectory()
         else:
             if result.error_code != FollowJointTrajectory.Result.SUCCESSFUL: