| @@ -12,10 +12,16 @@ class JTCPublisher(Node): | |||||
| trajectory = JointTrajectory( | trajectory = JointTrajectory( | ||||
| joint_names = [ "c1_joint", "f1_joint", "t1_joint" ], | joint_names = [ "c1_joint", "f1_joint", "t1_joint" ], | ||||
| points = [ | points = [ | ||||
| JointTrajectoryPoint (time_from_start = Duration(nanosec = 500000000), positions = [0, -1, 0]), | |||||
| JointTrajectoryPoint (time_from_start = Duration(nanosec = 1000000000), positions = [-0.3, -1.3, 0]), | |||||
| JointTrajectoryPoint (time_from_start = Duration(nanosec = 1500000000), positions = [0, -1, 0.5]), | |||||
| JointTrajectoryPoint (time_from_start = Duration(nanosec = 2000000000), positions = [-0.3, -1.3, 0]), | |||||
| JointTrajectoryPoint (time_from_start = Duration(sec = 1), positions = [0, -1.4, 0]), | |||||
| JointTrajectoryPoint (time_from_start = Duration(sec = 2), positions = [-0.3, -1.3, 0]), | |||||
| JointTrajectoryPoint (time_from_start = Duration(sec = 3), positions = [0, -1.4, 0.5]), | |||||
| JointTrajectoryPoint (time_from_start = Duration(sec = 4), positions = [-0.3, -1.3, 0]), | |||||
| JointTrajectoryPoint (time_from_start = Duration(sec = 5), positions = [-0.5, -1.3, 1]), | |||||
| JointTrajectoryPoint (time_from_start = Duration(sec = 6), positions = [-0.3, -1.3, 0]), | |||||
| JointTrajectoryPoint (time_from_start = Duration(sec = 7), positions = [-0.5, -1.3, 1]), | |||||
| JointTrajectoryPoint (time_from_start = Duration(sec = 8), positions = [-0.5, -1.3, 0]), | |||||
| JointTrajectoryPoint (time_from_start = Duration(sec = 9), positions = [-0.3, -1.3, 0]), | |||||
| JointTrajectoryPoint (time_from_start = Duration(sec = 10), positions =[ 0, -1.4, 0]), | |||||
| ] | ] | ||||
| ) | ) | ||||
| self.publisher_.publish(trajectory) | self.publisher_.publish(trajectory) | ||||