From ac0b3741f2d71b3c83ad00bf689cd92aa09f9608 Mon Sep 17 00:00:00 2001 From: Marcus Grieger Date: Mon, 5 Feb 2024 20:49:33 +0100 Subject: [PATCH] Made trajectory longer --- hexapod_python/jtc_test.py | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/hexapod_python/jtc_test.py b/hexapod_python/jtc_test.py index de74af3..cf411e6 100644 --- a/hexapod_python/jtc_test.py +++ b/hexapod_python/jtc_test.py @@ -12,10 +12,16 @@ class JTCPublisher(Node): trajectory = JointTrajectory( joint_names = [ "c1_joint", "f1_joint", "t1_joint" ], 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)