def calculate_closest_approaches(path): """Calculate Closest Approaches Given a standard SurgicalSim path, the closest approaches of that path to each marker will be calculated. Arguments: path: A SurgicalSim formatted path. Returns: A list of distances of size N, where N is the number of markers. """ segments = pathutils._detect_segments(path) distances = [] for gate_idx in np.arange(constants.G_NUM_GATES): x_gate = pathutils.get_path_gate_pos(path, segments[gate_idx], gate_idx) x_tooltip = pathutils.get_path_tooltip_pos(path, segments[gate_idx]) dist = np.sqrt((x_gate[0] - x_tooltip[0]) ** 2 + (x_gate[1] - x_tooltip[1]) ** 2 + (x_gate[2] - x_tooltip[2]) ** 2) distances.append(dist) return distances
def start(self, fps, fast_step=False): """Start Begin the continuous event loop for the simulation. This event loop can be exited using the ctrl+c keyboard interrupt. Real-time constraints are enforced. [Hz] Arguments: fps: The value of frames per second of the simulation. fast_step: If True, the ODE fast step algorithm will be used. This is faster and requires less memory but is less accurate. (Default: False) """ paused = False stopped = False # Define the total time for the tooltip traversal t_total = 20.0 # Define the simulation frame rate t = 0.0 # [s] dt = 1.0 / float(fps) # [s] # Keep track of time overshoot in the case that simulation time must be # increased in order to maintain real-time constraints t_overshoot = 0.0 # Get the initial path position (center of gate7) pos_start = self.env.get_body_pos('gate7') # [m] # Get the first position of the PA10 at rest pos_init = self.env.get_body_pos('tooltip') # [m] # Calculate the new required joint angles of the PA10 #pa10_joint_angles = self.kinematics.calc_inverse_kinematics(pos_init, pos_start) # TODO: Move the PA10 end-effector to the starting position along the path # TODO: TEMP - Move the temporary end-effector pointer to the starting position self.env.set_group_pos('pointer', pos_start) # Generate long-term path from initial position t_input = np.linspace(start=0.0, stop=1.0, num=t_total / dt) t_input = np.reshape(t_input, (len(t_input), 1)) rnn_path = self.rnn.extrapolate(t_input, [pos_start], len(t_input) - 1) # Add the initial condition point back onto the data rnn_path = np.vstack((pos_start, rnn_path)) # Retrieve one set of standard gate position/orientation data file_path = pathutils.list_data_files(constants.G_TRAINING_DATA_DIR)[0] gate_data = datastore.retrieve(file_path) gate_start_idx = constants.G_GATE_IDX gate_end_idx = gate_start_idx + constants.G_NUM_GATE_INPUTS # Reshape the gate positions data gate_data = gate_data[0:1, gate_start_idx:gate_end_idx] gate_data = np.tile(gate_data, (len(rnn_path), 1)) # Complete the rnn path data rnn_path = np.hstack((t_input, gate_data, rnn_path)) # Save generated path for later examination datastore.store(rnn_path, constants.G_RNN_STATIC_PATH_OUT) # Define a variable to hold the final path (with real-time correction) final_path = rnn_path[:-1].copy() path_saved = False # Detect all path segments between gates in the generated path segments = pathutils._detect_segments(rnn_path) path_idx = 0 x_path_offset = np.array([0.0, 0.0, 0.0]) # [m] v_curr = np.array([0.0, 0.0, 0.0]) # [m/s] a_max = constants.G_MAX_ACCEL # [m/s^2] # Get the static table position x_table = self.env.get_body_pos('table') while not stopped: t_start = time.time() # If the last calculation took too long, catch up dt_warped = dt + t_overshoot self.env.set_dt(dt_warped) # Determine if the viewer is stopped. Then we can quit if self.viewer.is_dead: break # Pause the simulation if we are at the end if path_idx == len(rnn_path) - 1 or paused: self.env.step(paused=True, fast=fast_step) # If we have really hit the end of the simulation, save/plot the path if not paused and not path_saved: # Save the final data to a file datastore.store(final_path, constants.G_RNN_DYNAMIC_PATH_OUT) path_saved = True continue # Not a very elegant solution to pausing at the start, but it works if t <= 1000.0: self.env.step(paused=True, fast=fast_step) t += dt_warped continue # Determine the current path segment curr_segment_idx = 0 for segment_idx, segment_end in enumerate(segments): if path_idx <= segment_end: curr_segment_idx = segment_idx break x_curr = pathutils.get_path_tooltip_pos(rnn_path, path_idx) + x_path_offset x_next = pathutils.get_path_tooltip_pos( rnn_path, path_idx + 1) + x_path_offset # Get the expected gate position x_gate_expected = pathutils.get_path_gate_pos( rnn_path, segments[curr_segment_idx], curr_segment_idx) # Get the actual gate position x_gate_actual = self.env.get_body_pos('gate%d' % curr_segment_idx) # Calculate the new position from change to new gate position dx_gate = x_gate_actual - (x_gate_expected + x_path_offset) x_new = x_next + dx_gate # Calculate the new velocity v_new = (x_new - x_curr) / dt_warped # Calculate the new acceleration a_new = (v_new - v_curr) / dt_warped # Calculate the acceleration vector norm a_new_norm = np.linalg.norm(a_new) # Limit the norm vector a_new_norm_clipped = np.clip(a_new_norm, -a_max, a_max) # Determine the ratio of the clipped norm, protect against divide by zero if a_new_norm != 0.0: ratio_unclipped = a_new_norm_clipped / a_new_norm else: ratio_unclipped = 0.0 # Scale the acceleration vector by this ratio a_new = a_new * ratio_unclipped # Calculate the new change in velocity dv_new = a_new * dt_warped v_new = v_curr + dv_new # Calculate the new change in position dx_new = v_new * dt_warped x_new = x_curr + dx_new # Modify final path data with current tooltip and gate positions pathutils.set_path_time(final_path, path_idx, t) pathutils.set_path_tooltip_pos(final_path, path_idx, x_curr) for gate_idx in range(constants.G_NUM_GATES): gate_name = 'gate%d' % gate_idx x_gate = self.env.get_body_pos(gate_name) pathutils.set_path_gate_pos(final_path, path_idx, gate_idx, x_gate) # Store this velocity for the next time step v_curr = v_new # Recalculate the current offset x_path_offset += x_new - x_next # Perform inverse kinematics to get joint angles pa10_joint_angles = self.kinematics.calc_inverse_kinematics( x_curr, x_new) # TODO: TEMP - MOVE ONLY POINTER, NO PA10 self.env.set_group_pos('pointer', x_new) if constants.G_TABLE_IS_OSCILLATING: # Move the table with y-axis oscillation x_table_next = shaker_table(t, x_table) else: x_table_next = x_table self.env.set_body_pos('table', x_table_next) # Step through the world by 1 time frame and actuate pa10 joints self.env.performAction(pa10_joint_angles, fast=fast_step) # Update current time after this step t += dt_warped path_idx += 1 # Determine the difference in virtual vs actual time t_warped = dt - (time.time() - t_start) # Attempt to enforce real-time constraints if t_warped >= 0.0: # The calculation took less time than the virtual time. Sleep # the rest off time.sleep(t_warped) t_overshoot = 0.0 else: # The calculation took more time than the virtual time. We need # to catch up with the virtual time on the next time step t_overshoot = -t_warped return
def start(self, fps, fast_step=False): """Start Begin the continuous event loop for the simulation. This event loop can be exited using the ctrl+c keyboard interrupt. Real-time constraints are enforced. [Hz] Arguments: fps: The value of frames per second of the simulation. fast_step: If True, the ODE fast step algorithm will be used. This is faster and requires less memory but is less accurate. (Default: False) """ paused = False stopped = False # Define the total time for the tooltip traversal t_total = 20.0 # Define the simulation frame rate t = 0.0 # [s] dt = 1.0 / float(fps) # [s] # Keep track of time overshoot in the case that simulation time must be # increased in order to maintain real-time constraints t_overshoot = 0.0 # Get the initial path position (center of gate7) pos_start = self.env.get_body_pos('gate7') # [m] # Get the first position of the PA10 at rest pos_init = self.env.get_body_pos('tooltip') # [m] # Calculate the new required joint angles of the PA10 #pa10_joint_angles = self.kinematics.calc_inverse_kinematics(pos_init, pos_start) # TODO: Move the PA10 end-effector to the starting position along the path # TODO: TEMP - Move the temporary end-effector pointer to the starting position self.env.set_group_pos('pointer', pos_start) # Generate long-term path from initial position t_input = np.linspace(start=0.0, stop=1.0, num=t_total/dt) t_input = np.reshape(t_input, (len(t_input), 1)) rnn_path = self.rnn.extrapolate(t_input, [pos_start], len(t_input)-1) # Add the initial condition point back onto the data rnn_path = np.vstack((pos_start, rnn_path)) # Retrieve one set of standard gate position/orientation data file_path = pathutils.list_data_files(constants.G_TRAINING_DATA_DIR)[0] gate_data = datastore.retrieve(file_path) gate_start_idx = constants.G_GATE_IDX gate_end_idx = gate_start_idx + constants.G_NUM_GATE_INPUTS # Reshape the gate positions data gate_data = gate_data[0:1,gate_start_idx:gate_end_idx] gate_data = np.tile(gate_data, (len(rnn_path), 1)) # Complete the rnn path data rnn_path = np.hstack((t_input, gate_data, rnn_path)) # Save generated path for later examination datastore.store(rnn_path, constants.G_RNN_STATIC_PATH_OUT) # Define a variable to hold the final path (with real-time correction) final_path = rnn_path[:-1].copy() path_saved = False # Detect all path segments between gates in the generated path segments = pathutils._detect_segments(rnn_path) path_idx = 0 x_path_offset = np.array([0.0, 0.0, 0.0]) # [m] v_curr = np.array([0.0, 0.0, 0.0]) # [m/s] a_max = constants.G_MAX_ACCEL # [m/s^2] # Get the static table position x_table = self.env.get_body_pos('table') while not stopped: t_start = time.time() # If the last calculation took too long, catch up dt_warped = dt + t_overshoot self.env.set_dt(dt_warped) # Determine if the viewer is stopped. Then we can quit if self.viewer.is_dead: break # Pause the simulation if we are at the end if path_idx == len(rnn_path) - 1 or paused: self.env.step(paused=True, fast=fast_step) # If we have really hit the end of the simulation, save/plot the path if not paused and not path_saved: # Save the final data to a file datastore.store(final_path, constants.G_RNN_DYNAMIC_PATH_OUT) path_saved = True continue # Not a very elegant solution to pausing at the start, but it works if t <= 1000.0: self.env.step(paused=True, fast=fast_step) t += dt_warped continue # Determine the current path segment curr_segment_idx = 0 for segment_idx, segment_end in enumerate(segments): if path_idx <= segment_end: curr_segment_idx = segment_idx break x_curr = pathutils.get_path_tooltip_pos(rnn_path, path_idx) + x_path_offset x_next = pathutils.get_path_tooltip_pos(rnn_path, path_idx+1) + x_path_offset # Get the expected gate position x_gate_expected = pathutils.get_path_gate_pos( rnn_path, segments[curr_segment_idx], curr_segment_idx ) # Get the actual gate position x_gate_actual = self.env.get_body_pos('gate%d'%curr_segment_idx) # Calculate the new position from change to new gate position dx_gate = x_gate_actual - (x_gate_expected + x_path_offset) x_new = x_next + dx_gate # Calculate the new velocity v_new = (x_new - x_curr) / dt_warped # Calculate the new acceleration a_new = (v_new - v_curr) / dt_warped # Calculate the acceleration vector norm a_new_norm = np.linalg.norm(a_new) # Limit the norm vector a_new_norm_clipped = np.clip(a_new_norm, -a_max, a_max) # Determine the ratio of the clipped norm, protect against divide by zero if a_new_norm != 0.0: ratio_unclipped = a_new_norm_clipped / a_new_norm else: ratio_unclipped = 0.0 # Scale the acceleration vector by this ratio a_new = a_new * ratio_unclipped # Calculate the new change in velocity dv_new = a_new * dt_warped v_new = v_curr + dv_new # Calculate the new change in position dx_new = v_new * dt_warped x_new = x_curr + dx_new # Modify final path data with current tooltip and gate positions pathutils.set_path_time(final_path, path_idx, t) pathutils.set_path_tooltip_pos(final_path, path_idx, x_curr) for gate_idx in range(constants.G_NUM_GATES): gate_name = 'gate%d' % gate_idx x_gate = self.env.get_body_pos(gate_name) pathutils.set_path_gate_pos(final_path, path_idx, gate_idx, x_gate) # Store this velocity for the next time step v_curr = v_new # Recalculate the current offset x_path_offset += x_new - x_next # Perform inverse kinematics to get joint angles pa10_joint_angles = self.kinematics.calc_inverse_kinematics(x_curr, x_new) # TODO: TEMP - MOVE ONLY POINTER, NO PA10 self.env.set_group_pos('pointer', x_new) if constants.G_TABLE_IS_OSCILLATING: # Move the table with y-axis oscillation x_table_next = shaker_table(t, x_table) else: x_table_next = x_table self.env.set_body_pos('table', x_table_next) # Step through the world by 1 time frame and actuate pa10 joints self.env.performAction(pa10_joint_angles, fast=fast_step) # Update current time after this step t += dt_warped path_idx += 1 # Determine the difference in virtual vs actual time t_warped = dt - (time.time() - t_start) # Attempt to enforce real-time constraints if t_warped >= 0.0: # The calculation took less time than the virtual time. Sleep # the rest off time.sleep(t_warped) t_overshoot = 0.0 else: # The calculation took more time than the virtual time. We need # to catch up with the virtual time on the next time step t_overshoot = -t_warped return
def main(): path_file = '../../neuralsim/generated.dat' #'../../results/sample5.dat' path = datastore.retrieve(path_file) # A list of the the segments of the optimized path segments = pathutils._detect_segments(path) # The new path generated by original path and corrective algorithm new_path = None x_path_offset = np.array([0.0, 0.0, 0.0]) # [m] v_curr = np.array([0.0, 0.0, 0.0]) # [m/s] a_max = constants.G_MAX_ACCEL # [m/s^2] for i, _ in enumerate(path): if i == len(path) - 1: continue # Detect current segment seg_idx = 0 for j in range(len(segments)): if i <= segments[j]: seg_idx = j break # Get current time and position t_curr = pathutils.get_path_time(path, i) * t_total t_next = pathutils.get_path_time(path, i + 1) * t_total dt = (t_next - t_curr) x_curr = pathutils.get_path_tooltip_pos(path, i) + x_path_offset x_next = pathutils.get_path_tooltip_pos(path, i + 1) + x_path_offset # Get the expected gate position at this timestep x_gate_expected = pathutils.get_path_gate_pos(path, segments[seg_idx], seg_idx) # Get current gate position x_gate_actual = generate_gate_pos(t_curr, path, seg_idx) dx_gate = x_gate_actual - (x_gate_expected + x_path_offset) # Calculate the new position with positional change from target to gate x_new = x_next + dx_gate # Calculate the new velocity v_new = (x_new - x_curr) / dt # Calculate the new acceleration a_new = (v_new - v_curr) / dt # Calculate the acceleration vector norm a_new_norm = np.linalg.norm(a_new) # Limit the norm vector a_new_norm_clipped = np.clip(a_new_norm, -a_max, a_max) # Determine the ratio of the clipped norm if a_new_norm != 0: ratio_unclipped = a_new_norm_clipped / a_new_norm else: ratio_unclipped = 0.0 # Scale the acceleration vector by this ratio a_new = a_new * ratio_unclipped # Calculate the new change in velocity dv_new = a_new * dt v_new = v_curr + dv_new # Calculate the new change in position dx_new = v_new * dt x_new = x_curr + dx_new # Store the next movement for later if new_path is None: new_path = x_new else: new_path = np.vstack((new_path, x_new)) # Store this velocity for the next time step v_curr = v_new # Recalculate the current offset x_path_offset += x_new - x_next # Plot the inputted path fig = plt.figure(facecolor='white') axis = fig.gca(projection='3d') pos_start_idx = constants.G_POS_IDX pos_end_idx = pos_start_idx + constants.G_NUM_POS_DIMS full_path = path[:-1].copy() full_path[:, pos_start_idx:pos_end_idx] = new_path pathutils.display_path(axis, full_path, title='Path') plt.show() return
def main(): path_file = '../../neuralsim/generated.dat'#'../../results/sample5.dat' path = datastore.retrieve(path_file) # A list of the the segments of the optimized path segments = pathutils._detect_segments(path) # The new path generated by original path and corrective algorithm new_path = None x_path_offset = np.array([0.0, 0.0, 0.0]) # [m] v_curr = np.array([0.0, 0.0, 0.0]) # [m/s] a_max = constants.G_MAX_ACCEL # [m/s^2] for i, _ in enumerate(path): if i == len(path) - 1: continue # Detect current segment seg_idx = 0 for j in range(len(segments)): if i <= segments[j]: seg_idx = j break # Get current time and position t_curr = pathutils.get_path_time(path, i) * t_total t_next = pathutils.get_path_time(path, i+1) * t_total dt = (t_next - t_curr) x_curr = pathutils.get_path_tooltip_pos(path, i) + x_path_offset x_next = pathutils.get_path_tooltip_pos(path, i+1) + x_path_offset # Get the expected gate position at this timestep x_gate_expected = pathutils.get_path_gate_pos(path, segments[seg_idx], seg_idx) # Get current gate position x_gate_actual = generate_gate_pos(t_curr, path, seg_idx) dx_gate = x_gate_actual - (x_gate_expected + x_path_offset) # Calculate the new position with positional change from target to gate x_new = x_next + dx_gate # Calculate the new velocity v_new = (x_new - x_curr) / dt # Calculate the new acceleration a_new = (v_new - v_curr) / dt # Calculate the acceleration vector norm a_new_norm = np.linalg.norm(a_new) # Limit the norm vector a_new_norm_clipped = np.clip(a_new_norm, -a_max, a_max) # Determine the ratio of the clipped norm if a_new_norm != 0: ratio_unclipped = a_new_norm_clipped / a_new_norm else: ratio_unclipped = 0.0 # Scale the acceleration vector by this ratio a_new = a_new * ratio_unclipped # Calculate the new change in velocity dv_new = a_new * dt v_new = v_curr + dv_new # Calculate the new change in position dx_new = v_new * dt x_new = x_curr + dx_new # Store the next movement for later if new_path is None: new_path = x_new else: new_path = np.vstack((new_path, x_new)) # Store this velocity for the next time step v_curr = v_new # Recalculate the current offset x_path_offset += x_new - x_next # Plot the inputted path fig = plt.figure(facecolor='white') axis = fig.gca(projection='3d') pos_start_idx = constants.G_POS_IDX pos_end_idx = pos_start_idx + constants.G_NUM_POS_DIMS full_path = path[:-1].copy() full_path[:,pos_start_idx:pos_end_idx] = new_path pathutils.display_path(axis, full_path, title='Path') plt.show() return