diff --git a/stack/main/src/controller/controller/ik_solver_node.py b/stack/main/src/controller/controller/ik_solver_node.py index d078b69..cb321ca 100644 --- a/stack/main/src/controller/controller/ik_solver_node.py +++ b/stack/main/src/controller/controller/ik_solver_node.py @@ -77,7 +77,11 @@ def __init__(self): self.limit_delta = self.get_parameter('limit_delta').value self.du_max = self.get_parameter('du_max').value self.tip_only = self.get_parameter('tip_only').value - self.u_opt_previous = np.array([0., 0., 0., 0., 0., 0.]) # initially no control input + + # Initializations + self.u_opt_previous = np.array([0., 0., 0., 0., 0., 0.]) # initially no control input + self.smooth_stat = self.u_opt_previous # expontential smoothing + self.alpha = 0.5 # Get inverse kinematics mappings data_dir = os.getenv('TRUNK_DATA', '/home/trunk/Documents/trunk-stack/stack/main/data') @@ -92,7 +96,7 @@ def __init__(self): # load position the data self.ys_ik = pd.read_csv(data_dir + '/trajectories/steady_state/observations_steady_state_beta_seed0.csv') max_seed = 8 - max_targeted_seed = 3 + max_targeted_seed = 4 for seed in range(1, max_seed+1): self.ys_ik = pd.concat([self.ys_ik, pd.read_csv(data_dir +f'/trajectories/steady_state/observations_steady_state_beta_seed{seed}.csv')]) for seed in range(0, max_targeted_seed+1): @@ -115,9 +119,7 @@ def __init__(self): self.n_neighbors = 3 self.knn = NearestNeighbors(n_neighbors=self.n_neighbors, algorithm='auto') # Fit the k-nearest neighbors model - self.knn.fit(self.ys_ik[:, [6, 8]]) # Using 7th and 9th values for ys_ik (x and z of tip) - - + self.knn.fit(self.ys_ik[:, -3:]) else: raise ValueError(f"{self.ik_type} is not a valid option, choose from 'lq' or 'nn'") @@ -141,7 +143,7 @@ def interp_ik_callback(self, request, response): zf_des = np.array(request.zf) # Extract the relevant indices (x and z position of tip) - zf_des_relevant = zf_des[[6, 8]].reshape(1, -1) + zf_des_relevant = zf_des[-3:].reshape(1, -1) distances, indices = self.knn.kneighbors(zf_des_relevant) # Get the corresponding u values from us_ik @@ -157,13 +159,13 @@ def interp_ik_callback(self, request, response): # check control inputs are within the workspace u_opt = self.check_control_inputs(u_opt) - if self.limit_delta: - du = u_opt - self.u_opt_previous # delta u between timesteps - du_clipped = np.clip(du, -self.du_max, self.du_max) # clip delta u - u_opt = self.u_opt_previous + du_clipped # update u with clipped du - u_opt = self.check_control_inputs(u_opt) + # Do exponential smoothing + self.smooth_stat = self.alpha * u_opt + (1 - self.alpha) * self.smooth_stat + u_opt = self.smooth_stat + + # Update previous u_opt + self.u_opt_previous = u_opt - self.u_opt_previous = u_opt # update previous u response.uopt = u_opt.tolist() return response diff --git a/stack/main/src/streamer/streamer/avp_streamer_node.py b/stack/main/src/streamer/streamer/avp_streamer_node.py index 66c91e9..03413c0 100644 --- a/stack/main/src/streamer/streamer/avp_streamer_node.py +++ b/stack/main/src/streamer/streamer/avp_streamer_node.py @@ -62,9 +62,9 @@ def __init__(self): QoSProfile(depth=10) ) - self.streamer = AVPSubscriber(ip='10.93.181.166') # SET IP ADDRESS HERE - 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.streamer = AVPSubscriber(ip='10.93.181.166') # SET IP ADDRESS HERE + self._timer = self.create_timer(1.0 / 100.0, self.streamer_data_sampling_callback) # this defines the Hz we operate the teleop at + self.last_isGripperOpen = self.streamer.isGripperOpen # should start as closed self.get_logger().info('AVP streaming node has been started.') diff --git a/stack/main/src/streamer/streamer/avp_subscriber.py b/stack/main/src/streamer/streamer/avp_subscriber.py index ae1c8e2..f727f1c 100644 --- a/stack/main/src/streamer/streamer/avp_subscriber.py +++ b/stack/main/src/streamer/streamer/avp_subscriber.py @@ -48,8 +48,8 @@ def stream(self): self.isGripperOpen = response.isGripperOpen self.latest = positions - # Force an operating frequency of 10 Hz - time.sleep(0.1) + # Force the operating frequency + time.sleep(1 / 100.0) except Exception as e: if e.details() != self.last_error_message: # only print error message if its new