new ik data

stack/main/scripts/control_inputs_sampling.py

@@ -29,8 +29,12 @@ def sinusoidal_sampling(control_variables):
 def uniform_sampling(control_variables):
     control_inputs_df = pd.DataFrame(columns=['ID'] + control_variables)
     tip_range = 0.15
-    mid_range = 0.15
-    base_range = 0.2
+    mid_range = 0.25
+    base_range = 0.35
+    # reasonable for src demo:
+    # tip_range = 0.20
+    # mid_range = 0.25
+    # base_range = 0.25
     ranges = [np.linspace(-val, val, 3) for val in [tip_range, mid_range, base_range]*2]
     combinations = list(product(*ranges))
 
@@ -42,7 +46,7 @@ def uniform_sampling(control_variables):
 
 def main(data_type='dynamic', sampling_type='uniform'):
     control_variables = ['u1', 'u2', 'u3', 'u4', 'u5', 'u6']
-    data_dir = os.getenv('TRUNK_DATA', '/home/asl/Documents/asl_trunk_ws/data')
+    data_dir = os.getenv('TRUNK_DATA', '/home/trunk/Documents/trunk-stack/stack/main/data')
     control_inputs_file = os.path.join(data_dir, f'trajectories/{data_type}/control_inputs_{sampling_type}.csv')
 
     if sampling_type=='sinusoidal':
@@ -56,6 +60,6 @@ def main(data_type='dynamic', sampling_type='uniform'):
 
 
 if __name__ == '__main__':
-    data_type = 'dynamic'       # 'steady_state' or 'dynamic'
+    data_type = 'steady_state'       # 'steady_state' or 'dynamic'
     sampling_type = 'uniform'   # 'uniform' or 'sinusoidal'
     main(data_type, sampling_type)

stack/main/src/controller/controller/ik_solver_node.py

@@ -28,7 +28,7 @@ def __init__(self):
         self.u_opt_previous = np.array([0, 0, 0, 0, 0, 0]) # initially no control input
 
         # Get mappings
-        self.data_dir = os.getenv('TRUNK_DATA', '/home/asl/Documents/asl_trunk_ws/data')
+        self.data_dir = os.getenv('TRUNK_DATA', '/home/trunk/Documents/trunk-stack/stack/main/data')
         self.u2y = np.load(os.path.join(self.data_dir, f'models/ik/{self.u2y_file}'))
         self.y2u = np.load(os.path.join(self.data_dir, f'models/ik/{self.y2u_file}'))
 

stack/main/src/streamer/streamer/avp_streamer_node.py

@@ -7,7 +7,7 @@
 from interfaces.msg import TrunkRigidBodies
 from geometry_msgs.msg import Point
 from .avp_subscriber import AVPSubscriber
-from interfaces.srv import TriggerImageSaving
+from interfaces.srv import TriggerImageSaving, GripperAction
 
 
 class AVPStreamerNode(Node):
@@ -34,7 +34,7 @@ def __init__(self):
         # Initialize stored positions and gripper states
         self.stored_positions = []
         self.stored_gripper_states = []
-
+        
         # Keep track of trajectory ID
         self.recording_id = -1
 
@@ -43,7 +43,16 @@ def __init__(self):
             '/avp_des_positions',
             QoSProfile(depth=10)
         )
-
+
+        # Create a gripper service client
+        self.gripper_client = self.create_client(
+            GripperAction,
+            'move_gripper'
+        )
+        # Wait for service to become available
+        while not self.gripper_client.wait_for_service(timeout_sec=1.0):
+            self.get_logger().info('Gripper service not available, waiting...')
+
         # subscribe to mocap positions
         self.subscription_rigid_bodies = self.create_subscription(
                 TrunkRigidBodies,
@@ -54,13 +63,32 @@ def __init__(self):
 
         self.streamer = AVPSubscriber(ip='10.93.181.122')
         self._timer = self.create_timer(1.0 / 10.0, self.streamer_data_sampling_callback) # runs at 10Hz
+        self.last_isGripperOpen = self.streamer.isGripperOpen #should start as closed
+
         self.get_logger().info('AVP streaming node has been started.')
 
+    def process_gripper_state(self):
+        request = GripperAction.Request()
+
+        # process state switching
+        if self.last_isGripperOpen == 1 and self.streamer.isGripperOpen == 0:
+            request.action = "close"
+        elif self.last_isGripperOpen == 0 and self.streamer.isGripperOpen == 1:
+            request.action = "open"
+
+        # Update last gripper state
+        self.last_isGripperOpen = self.streamer.isGripperOpen
+
+        # Call the service
+        self.async_response = self.gripper_client.call_async(request)
+
+
     def mocap_listener_callback(self, msg):
         self.latest_mocap_positions = msg
 
-
     def streamer_data_sampling_callback(self):
+        self.process_gripper_state()
+
         # Determine trajectory ID - when recording starts
         if self.streamer.isRecording and not self.streamer.previousRecordingState:
             self.get_logger().info('New traj.')