AS: updates for trajectroy control

workspace/src/joint_angles.py

@@ -11,11 +11,11 @@ def main():
 
     
     # Example joint positions to send
-    joint_positions1 = [-0.5,-0.6, 0.2,0.0, 0.0, 0.0]
-    joint_positions2 = [-0.5,-0.6, 0.6,0.0,0.0, 0.0]
-    joint_positions3 = [-0.5,0.6, 0.6,0.0, 0.0, 0.0]
-    joint_positions4 = [-0.5,0.6, 0.2,0.0, 0.0, 0.0]
-    joint_positions5 = [-0.5,-0.6, 0.2,0.0, 0.0, 0.0]
+    joint_positions1 = [0.3,0.3, 0.2, 0.0, 0.0, 0.0]
+    joint_positions2 = [0.3,0.1, 0.6, 0.0,0.0, 0.0]
+    joint_positions3 = [0.3,0.4, 0.6, 0.0, 0.0, 0.0]
+    joint_positions4 = [0.4,0.3, 0.2, 0.0, 0.0, 0.0]
+    joint_positions5 = [0.3,0.3, 0.2, 0.0, 0.0, 0.0]
 
     msg = oscbuildparse.OSCMessage("/joint_trajectory", None, [joint_positions1, joint_positions2, joint_positions3, joint_positions4, joint_positions5])
     osc_send(msg, "osc_client")

workspace/src/joint_control/joint_control/trajectory_server_cart.py

@@ -11,9 +11,10 @@ import time
 
 class ScaledJointTrajectoryPublisher(Node):
     """Node to publish joint trajectories based on OSC messages."""
-    def __init__(self, joint_names, robot):
+    def __init__(self, joint_names, robot, cost_mask):
         super().__init__('scaled_joint_trajectory_publisher')
         
+        self.cost_mask = cost_mask
         self.robot = robot
         self.trajectroy_topic_name = input("Enter the topic name to which the joint trajectory should be sent to: ")
 
@@ -65,7 +66,7 @@ class ScaledJointTrajectoryPublisher(Node):
                 for j in range(steps):
                     print(f'j = {j}')
                     print(6)
-                    sol = self.robot.ik_LM(cart_traj[j], q0=joint_positions)
+                    sol = self.robot.ik_LM(cart_traj[j], q0=joint_positions, mask = self.cost_mask, joint_limits = True)
                     print(7)
                     if sol[1] == 1:
                         print(8)
@@ -118,8 +119,19 @@ def main():
     robot = rtb.ERobot.URDF(path_to_urdf)
     print(robot)
     rclpy.init()
-
-    node = ScaledJointTrajectoryPublisher(joint_names, robot)
+    while True:
+        try:
+            print("The cost mask determines which coordinates are used for the IK. Each element of the cost mask corresponds to a catesian coordinate [x, y, z, Rx, Ry, Rz].")
+            print("The cost mask [1, 1, 1, 0, 0, 0] means that the IK will only consider translation and no rotaion.")
+            cost_mask = [int(i) for i in input(f"Enter the cost mask (6 integers (1 or 0) separated by spaces, of which <= {robot.n} are 1): ").split()]
+            if sum(cost_mask) <= robot.n and len(cost_mask) == 6:
+                break
+            else:
+                print(f"Invalid input. Expected 6 integers of which {robot.n if robot.n < 6 else 6} or less are 1.")
+        except ValueError:
+            print("Invalid input. Please enter integers only.")
+    print(f"Cost mask: {cost_mask}")
+    node = ScaledJointTrajectoryPublisher(joint_names, robot, cost_mask)
 
     # Run both ROS 2 and OSC Server together
     try:

workspace/src/joint_control/setup.py

@@ -25,10 +25,10 @@ setup(
             'cart_coords = joint_control.cart_tcp_server:main',
             'trajectory_server = joint_control.trajectory_server:main',
             'trajectory_server_cart = joint_control.trajectory_server_cart:main',
+            'trajectory_server_cart_fast = joint_control.trajectory_server_cart_fast:main',
             'plugdata = joint_control.plugdata:main',
-            'plugdata2 = joint_control.plugdata2:main',
+            'plugdata_cart = joint_control.plugdata_cart:main',
             'test=joint_control.test:main',
-            'plugdata3 = joint_control.plugdata3:main',
         ],
     },
 )