diff --git a/docs/agents/acu_agent.rst b/docs/agents/acu_agent.rst index babdd6387..8d1791186 100644 --- a/docs/agents/acu_agent.rst +++ b/docs/agents/acu_agent.rst @@ -154,8 +154,9 @@ ignorance: of "az", "el", "third", and "none". See further explanation in :class:`ACUAgent `. - ``scan_params``: Default scan parameters; currently ``az_speed`` - (float, deg/s) and ``az_accel`` (float, deg/s/s). If not specfied, - these are given default values depending on the platform type. + (float, deg/s) ``az_accel`` (float, deg/s/s), ``el_freq`` (float, Hz), + and ``turnaround_method`` (str). If not specfied, these are given + default values depending on the platform type. Other agent functions diff --git a/socs/agents/acu/agent.py b/socs/agents/acu/agent.py index 1e1f31e61..654c4c9d3 100644 --- a/socs/agents/acu/agent.py +++ b/socs/agents/acu/agent.py @@ -32,11 +32,13 @@ 'az_speed': 2, 'az_accel': 1, 'el_freq': .15, + 'turnaround_method': 'standard', }, 'satp': { 'az_speed': 1, 'az_accel': 1, 'el_freq': 0, + 'turnaround_method': 'standard', }, } @@ -1794,6 +1796,8 @@ def set_speed_mode(self, session, params): @ocs_agent.param('az_speed', type=float, default=None) @ocs_agent.param('az_accel', type=float, default=None) @ocs_agent.param('el_freq', type=float, default=None) + @ocs_agent.param('turnaround_method', type=str, default=None, + choices=[None, 'standard', 'three_leg']) @ocs_agent.param('reset', default=False, type=bool) @inlineCallbacks def set_scan_params(self, session, params): @@ -1814,7 +1818,7 @@ def set_scan_params(self, session, params): """ if params['reset']: self.scan_params.update(self.default_scan_params) - for k in ['az_speed', 'az_accel', 'el_freq']: + for k in ['az_speed', 'az_accel', 'el_freq', 'turnaround_method']: if params[k] is not None: self.scan_params[k] = params[k] self.log.info('Updated default scan params to {sp}', sp=self.scan_params) @@ -2034,18 +2038,23 @@ def line_batcher(ff_scan, t_shift=0., n=10): 'mid_inc', 'mid_dec']) @ocs_agent.param('az_drift', type=float, default=None) @ocs_agent.param('az_only', type=bool, default=True) - @ocs_agent.param('type', default=1, choices=[1, 2, 3]) + @ocs_agent.param('scan_type', default=1, choices=[1, 2, 3]) @ocs_agent.param('az_vel_ref', type=float, default=None) + @ocs_agent.param('turnaround_method', default=None, + choices=[None, 'standard', 'three_leg']) @ocs_agent.param('scan_upload_length', type=float, default=None) + @ocs_agent.param('type', default=None, choices=[1, 2, 3]) @inlineCallbacks def generate_scan(self, session, params): """generate_scan(az_endpoint1, az_endpoint2, \ az_speed=None, az_accel=None, \ el_endpoint1=None, el_endpoint2=None, \ - el_speed=None, \ + el_speed=None, el_freq=None, \ num_scans=None, start_time=None, \ wait_to_start=None, step_time=None, \ az_start='end', az_drift=None, az_only=True, \ + scan_type=1, az_vel_ref=None, \ + turnaround_method=None, \ scan_upload_length=None) **Process** - Scan generator, currently only works for @@ -2062,6 +2071,7 @@ def generate_scan(self, session, params): track. el_endpoint2 (float): this is ignored. el_speed (float): this is ignored. + el_freq(float): frequency of the elevation nods for scan_type=3. num_scans (int or None): if not None, limits the scan to the specified number of constant velocity legs. The process will exit without error once that has @@ -2092,16 +2102,22 @@ def generate_scan(self, session, params): az_only (bool): if True (the default), then only the Azimuth axis is put in ProgramTrack mode, and the El axis is put in Stop mode. - type (int): What type of scan to use. Only 1, 2, 3 are valid. + scan_type (int): What type of scan to use. Only 1, 2, 3 are valid. Type 1 is a constant elevation scan. Type 2 includes a variation in az speed that scales as sin(az). Type 3 is a Type 2 with an sinusoidal el nod. az_vel_ref (float or None): azimuth to center the velocity profile at. If None then the average of the endpoints is used. + turnaround_method (str): The method used for generating turnaround. + Default (None) generates the baseline minimal jerk trajectory. + 'three_leg' generates a three-leg turnaround which attempts to + minimize the acceleration at the midpoint of the turnaround. scan_upload_length (float): number of seconds for each set of uploaded points. If this is not specified, the track manager will try to use as short a time as is reasonable. + type (int): Temporary alias for scan_type. Do not + use. Will be removed. Notes: Note that all parameters are optional except for @@ -2116,6 +2132,11 @@ def generate_scan(self, session, params): if self._get_sun_policy('motion_blocked'): return False, "Motion blocked; Sun avoidance in progress." + if params['type'] is not None: + self.log.warn('Caller passed "type" instead of "scan_type" arg; moving.') + params['scan_type'] = params['type'] + del params['type'] + self.log.info('User scan params: {params}', params=params) az_endpoint1 = params['az_endpoint1'] @@ -2128,12 +2149,23 @@ def generate_scan(self, session, params): az_speed = params['az_speed'] az_accel = params['az_accel'] el_freq = params['el_freq'] + turnaround_method = params['turnaround_method'] if az_speed is None: az_speed = self.scan_params['az_speed'] if az_accel is None: az_accel = self.scan_params['az_accel'] if el_freq is None: el_freq = self.scan_params['el_freq'] + if turnaround_method is None: + if params['scan_type'] in [2, 3]: + turnaround_method = 'three_leg' + self.log.info('Setting turnaround_method="three_leg" for type2/3 scan.') + else: + turnaround_method = self.scan_params['turnaround_method'] + + # Check if the turnaround method is usable for the called scan type. + if turnaround_method == "standard" and params['scan_type'] != 1: + raise ValueError("Cannot use standard turnaround method with type 2 or 3 scans!") # Do we need to limit the az_accel? This limit comes from a # maximum jerk parameter; the equation below (without the @@ -2175,7 +2207,8 @@ def generate_scan(self, session, params): el_speed = params.get('el_speed', 0.0) plan = sh.plan_scan(az_endpoint1, az_endpoint2, el=el_endpoint1, v_az=az_speed, a_az=az_accel, - az_start=scan_params.get('az_start')) + az_start=scan_params.get('az_start'), + scan_type=params['scan_type']) # Use the plan to set scan upload parameters. if scan_params.get('step_time') is None: @@ -2237,30 +2270,36 @@ def generate_scan(self, session, params): # Prepare the point generator. free_form = False - if params["type"] == 1: + if params['scan_type'] == 1: + if turnaround_method == 'three_leg': + free_form = True + g = sh.generate_constant_velocity_scan(az_endpoint1=az_endpoint1, az_endpoint2=az_endpoint2, az_speed=az_speed, acc=az_accel, + turnaround_method=turnaround_method, el_endpoint1=el_endpoint1, el_endpoint2=el_endpoint2, el_speed=el_speed, az_first_pos=plan['init_az'], **scan_params) - elif params["type"] == 2: + elif params['scan_type'] == 2: free_form = True g = sh.generate_type2_scan(az_endpoint1=az_endpoint1, az_endpoint2=az_endpoint2, az_speed=az_speed, acc=az_accel, + turnaround_method=turnaround_method, el_endpoint1=el_endpoint1, az_vel_ref=az_vel_ref, az_first_pos=plan['init_az'], **scan_params) - elif params["type"] == 3: + elif params['scan_type'] == 3: free_form = True azonly = False g = sh.generate_type3_scan(az_endpoint1=az_endpoint1, az_endpoint2=az_endpoint2, az_speed=az_speed, acc=az_accel, + turnaround_method=turnaround_method, el_endpoint1=el_endpoint1, el_endpoint2=el_endpoint2, el_freq=el_freq, @@ -2283,8 +2322,8 @@ def generate_scan(self, session, params): 'el1': el_endpoint1, 'el2': el_endpoint2, 'el_freq': el_freq, - 'type': params['type'], - 'turnaround_type': sh.TURNAROUNDS_ENUM['standard'], + 'type': params['scan_type'], + 'turnaround_type': sh.TURNAROUNDS_ENUM[turnaround_method], }) self.agent.publish_to_feed('scan_params', @@ -2416,6 +2455,9 @@ def _run_track(self, session, point_gen, step_time, azonly=False, first_upload_time = None wait_stop_timeout = None + # eesh + unabort_failure = False + while True: now = time.time() current_modes = {'Az': self.data['status']['summary']['Azimuth_mode'], @@ -2447,6 +2489,8 @@ def _run_track(self, session, point_gen, step_time, azonly=False, else: if got_progtrack: self.log.warn('Unexpected exit from ProgramTrack mode!') + if mode == 'stop': + unabort_failure = True # close enough! mode = 'abort' elif now - start_time > MAX_PROGTRACK_SET_TIME: self.log.warn('Failed to set ProgramTrack mode in a timely fashion.') @@ -2458,7 +2502,7 @@ def _run_track(self, session, point_gen, step_time, azonly=False, if current_modes['Remote'] == 0: self.log.warn('ACU no longer in remote mode!') mode = 'abort' - if session.status == 'stopping': + if session.status == 'stopping' and mode not in ['stop', 'abort']: mode = 'stop' stop_message = 'User-requested stop.' lines = [] @@ -2477,7 +2521,7 @@ def _run_track(self, session, point_gen, step_time, azonly=False, # check is used to decide we're done. while mode == 'go' and (len(lines) <= new_line_target or lines[-1].group_flag != 0): try: - lines.extend(next(point_gen)) + lines.append(next(point_gen)) except StopIteration: mode = 'stop' stop_message = 'Stop due to end of the planned track.' @@ -2491,10 +2535,6 @@ def _run_track(self, session, point_gen, step_time, azonly=False, # Grab the minimum batch upload_lines, lines = lines[:new_line_target], lines[new_line_target:] - # If the last line has a "group" flag, keep transferring lines. - while len(lines) and len(upload_lines) and upload_lines[-1].group_flag != 0: - upload_lines.append(lines.pop(0)) - if len(upload_lines): # Discard the group flag and upload all. text = sh.get_track_points_text( @@ -2545,6 +2585,8 @@ def _run_track(self, session, point_gen, step_time, azonly=False, 'Clear Stack') if mode == 'abort': + if unabort_failure: + return True, 'Problems on shutdown but close enough.' return False, 'Problems during scan' return True, f'Scan ended. {stop_message}' diff --git a/socs/agents/acu/drivers.py b/socs/agents/acu/drivers.py index 9e3ae90ea..e1dfbf7f1 100644 --- a/socs/agents/acu/drivers.py +++ b/socs/agents/acu/drivers.py @@ -7,6 +7,8 @@ import numpy as np +import socs.agents.acu.three_leg_turnaround as three_leg_tr + #: The number of seconds in a day. DAY = 86400 @@ -17,6 +19,8 @@ #: Registry for turn-around profile types. TURNAROUNDS_ENUM = { 'standard': 0, + # 1: reserved for "standard but explicitly commanded" + 'three_leg': 2, } @@ -289,18 +293,34 @@ def _get_target_az(current_az, current_t, increasing, az_endpoint1, az_endpoint2 return target +def get_const_vel_subscan(t0, az0, az1, speed, step_time, + group=0, stop_at_end=False): + t_scan = abs(az1 - az0) / speed + n = max(1, int(round(t_scan / step_time))) + 1 + az = np.linspace(az0, az1, n) + t = t0 + np.linspace(0, t_scan, n) + v = np.zeros(n) + np.sign(az1 - az0) * speed + f = np.zeros(n, int) + 1 + f[-1] = 2 + g = np.zeros(n, int) + if group: + g[:group] = 1 + if stop_at_end: + v[-1] = 0. + return (t, az, v, f, g) + + def generate_constant_velocity_scan(az_endpoint1, az_endpoint2, az_speed, acc, el_endpoint1, el_endpoint2, el_speed=0, - num_batches=None, num_scans=None, start_time=None, wait_to_start=10., step_time=1., - batch_size=500, az_start='mid_inc', az_first_pos=None, - az_drift=None): + az_drift=None, + turnaround_method='standard'): """Python generator to produce times, azimuth and elevation positions, azimuth and elevation velocities, azimuth and elevation flags for arbitrarily long constant-velocity azimuth scans. @@ -316,9 +336,6 @@ def generate_constant_velocity_scan(az_endpoint1, az_endpoint2, az_speed, constant az scans, this must be equal to el_endpoint1. el_speed (float): speed of the elevation motion. For constant az scans, set to 0.0 - num_batches (int or None): sets the number of batches for the - generator to create. Default value is None (interpreted as infinite - batches). num_scans (int or None): if not None, limits the points returned to the specified number of constant velocity legs. start_time (float or None): a ctime at which to start the scan. @@ -329,9 +346,6 @@ def generate_constant_velocity_scan(az_endpoint1, az_endpoint2, az_speed, step_time (float): time between points on the constant-velocity parts of the motion. Default value is 1.0 seconds. Minimum value is 0.05 seconds. - batch_size (int): number of values to produce in each iteration. - Default is 500. Batch size is reset to the length of one leg of the - motion if num_batches is not None. az_start (str): part of the scan to start at. To start at one of the extremes, use 'az_endpoint1', 'az_endpoint2', or 'end' (same as 'az_endpoint1'). To start in the midpoint @@ -344,43 +358,42 @@ def generate_constant_velocity_scan(az_endpoint1, az_endpoint2, az_speed, az_drift (float): The rate (deg / s) at which to shift the scan endpoints in time. This can be used to better track celestial sources in targeted scans. + turnaround_method (str): The method used for generating turnaround. + Default ('standard') generates the baseline minimal jerk trajectory. + 'three_leg' generates a three-leg turnaround which attempts to + minimize the acceleration at the midpoint of the turnaround. Yields: - points (list): a list of TrackPoint objects. Raises - StopIteration once exit condition, if defined, is met. + TrackPoint """ if az_endpoint1 == az_endpoint2: raise ValueError('Generator requires two different az endpoints!') - # Force the el_speed to 0. It matters because an el_speed in - # ProgramTrack data that exceeds the ACU limits will cause the - # point to be rejected, even if there's no motion in el planned - # (which, at the time of this writing, there is not). - el_speed = 0. - # Note that starting scan direction gets modified, below, # depending on az_start. - increasing = az_endpoint2 > az_endpoint1 + section = 2 + if az_endpoint2 > az_endpoint1: + section = 0 if az_start in ['az_endpoint1', 'az_endpoint2', 'end']: if az_start in ['az_endpoint1', 'end']: az = az_endpoint1 else: az = az_endpoint2 - increasing = not increasing + section = (section + 2) % 4 elif az_start in ['mid_inc', 'mid_dec', 'mid']: az = (az_endpoint1 + az_endpoint2) / 2 if az_start == 'mid': pass elif az_start == 'mid_inc': - increasing = True + section = 0 else: - increasing = False + section = 2 else: raise ValueError(f'az_start value "{az_start}" not supported. Choose from ' 'az_endpoint1, az_endpoint2, mid_inc, mid_dec') - az_vel = az_speed if increasing else -az_speed + az_vel = az_speed if (section == 0) else -az_speed # Bias the starting point for the first leg? if az_first_pos is not None: @@ -396,15 +409,6 @@ def generate_constant_velocity_scan(az_endpoint1, az_endpoint2, az_speed, if step_time < 0.05: raise ValueError('Time step size too small, must be at least ' '0.05 seconds') - daz = step_time * az_speed - el_vel = el_speed - az_flag = 0 - el_flag = 0 - if num_batches is None: - stop_iter = float('inf') - else: - stop_iter = num_batches - batch_size = int(np.ceil(abs(az_endpoint2 - az_endpoint1) / daz)) def dec_num_scans(): nonlocal num_scans @@ -414,87 +418,37 @@ def dec_num_scans(): def check_num_scans(): return num_scans is None or num_scans > 0 - target_az = _get_target_az(az, t, increasing, az_endpoint1, az_endpoint2, az_speed, az_drift) - point_group_batch = 0 - - i = 0 - while i < stop_iter and check_num_scans(): - i += 1 - point_block = [] - for j in range(batch_size): - point_block.append(TrackPoint( - timestamp=t + t0, - az=az, el=el, az_vel=az_vel, el_vel=el_vel, - az_flag=az_flag, el_flag=el_flag, - group_flag=int(point_group_batch > 0))) - - if point_group_batch > 0: - point_group_batch -= 1 - - if increasing: - if az <= (target_az - 2 * daz): - t += step_time - az += daz - az_vel = az_speed - el_vel = el_speed - az_flag = 1 - el_flag = 0 - elif az == target_az: - # Turn around. - t += turntime - az_vel = -1 * az_speed - el_vel = el_speed - az_flag = 1 - el_flag = 0 - increasing = False - target_az = _get_target_az(az, t, increasing, az_endpoint1, az_endpoint2, az_speed, az_drift) - dec_num_scans() - point_group_batch = MIN_GROUP_NEW_LEG - 1 - else: - time_remaining = (target_az - az) / az_speed - az = target_az - t += time_remaining - az_vel = az_speed - el_vel = el_speed - az_flag = 2 - el_flag = 0 - else: - if az >= (target_az + 2 * daz): - t += step_time - az -= daz - az_vel = -1 * az_speed - el_vel = el_speed - az_flag = 1 - el_flag = 0 - elif az == target_az: - # Turn around. - t += turntime - az_vel = az_speed - el_vel = el_speed - az_flag = 1 - el_flag = 0 - increasing = True - target_az = _get_target_az(az, t, increasing, az_endpoint1, az_endpoint2, az_speed, az_drift) - dec_num_scans() - point_group_batch = MIN_GROUP_NEW_LEG - 1 - else: - time_remaining = (az - target_az) / az_speed - az = target_az - t += time_remaining - az_vel = -1 * az_speed - el_vel = el_speed - az_flag = 2 - el_flag = 0 - - if not check_num_scans(): - # Kill the velocity on the last point and exit -- this - # was recommended at LAT FAT for smoothly stopping the - # motion at end of program. - point_block[-1].az_vel = 0 - point_block[-1].el_vel = 0 - break - - yield point_block + while check_num_scans(): + + if section in [0, 2]: + # Edge-to-edge + dec_num_scans() + increasing = (section == 0) + target_az = _get_target_az(az, t, increasing, az_endpoint1, az_endpoint2, az_speed, az_drift) + vects = get_const_vel_subscan(t, az, target_az, az_speed, step_time, + group=MIN_GROUP_NEW_LEG - 1, + stop_at_end=not check_num_scans()) + for _t, _az, _vel, _flag, _gflag in zip(*vects): + yield TrackPoint( + timestamp=_t + t0, az=_az, el=el, az_vel=_vel, el_vel=0, + az_flag=_flag, el_flag=0, + group_flag=_gflag) + t, az = [v[-1] for v in vects[:2]] + + elif section in [1, 3]: + # Turn-around + if turnaround_method == "three_leg": + az_vel = az_speed if section == 1 else -az_speed + turnaround_track = three_leg_tr.gen_three_leg_turnaround( + t0=t + t0, az0=az, el0=el, v0=az_vel, + turntime=turntime, + az_flag=2, el_flag=0, + point_group_batch=0) + for tp in turnaround_track[:-1]: + yield tp + t += turntime + + section = (section + 1) % 4 def generate_type3_scan(az_endpoint1, az_endpoint2, az_speed, @@ -509,7 +463,8 @@ def generate_type3_scan(az_endpoint1, az_endpoint2, az_speed, batch_size=500, az_start='mid_inc', az_first_pos=None, - az_drift=None): + az_drift=None, + turnaround_method='three_leg'): """Python generator to produce times, azimuth and elevation positions, azimuth and elevation velocities, azimuth and elevation flags for arbitrarily long type 3 scan. @@ -627,7 +582,6 @@ def get_scan_time(az0, az1, az_speed, az_cent): if step_time < 0.05: raise ValueError('Time step size too small, must be at least ' '0.05 seconds') - el_vel = el_throw * el_freq * 2 * np.pi * np.cos(t * el_freq * 2 * np.pi) az_flag = 0 el_flag = 0 if num_batches is None: @@ -647,14 +601,23 @@ def check_num_scans(): target_az = _get_target_az(az, t, increasing, az_endpoint1, az_endpoint2, az_speed / np.sin(np.deg2rad(az - az_cent)), az_drift) point_group_batch = 0 + def get_el(_t): + return (el_cent - el_throw * np.cos(_t * el_freq * 2 * np.pi), + el_throw * el_freq * 2 * np.pi * np.sin(_t * el_freq * 2 * np.pi)) + i = 0 + point_queue = [] while i < stop_iter and check_num_scans(): i += 1 point_block = [] for j in range(batch_size): + if len(point_queue): # Pull from points in the queue first + point_block.append(point_queue.pop(0)) + continue + point_block.append(TrackPoint( timestamp=t + t0, - az=az, el=el, az_vel=az_vel / np.sin(np.deg2rad(az - az_cent)), el_vel=el_vel, + az=az, el=0, az_vel=az_vel / np.sin(np.deg2rad(az - az_cent)), el_vel=1, az_flag=az_flag, el_flag=el_flag, group_flag=int(point_group_batch > 0))) @@ -665,57 +628,73 @@ def check_num_scans(): if get_scan_time(az, target_az, az_speed, az_cent) > 2 * step_time: t += step_time az += step_time * az_speed / np.sin(np.deg2rad(az - az_cent)) - el = el_cent + el_throw * np.sin(t * el_freq * 2 * np.pi) az_vel = az_speed - el_vel = el_throw * el_freq * 2 * np.pi * np.cos(t * el_freq * 2 * np.pi) - az_flag = 1 + az_flag = 0 # 1 el_flag = 0 elif az == target_az: + point_group_batch = MIN_GROUP_NEW_LEG - 1 + if turnaround_method == "three_leg": + _v = az_vel / np.sin(np.deg2rad(az - az_cent)) + turnaround_track = three_leg_tr.gen_three_leg_turnaround( + t0=t + t0, az0=az, el0=el, v0=_v, + turntime=tt[1], + az_flag=az_flag, el_flag=el_flag, + step_time=step_time, + second_leg_time=0., + point_group_batch=point_group_batch) + for track_point in turnaround_track: + point_queue.append(track_point) # Add the TrackPoints from the turnaround into the queue. + # Turn around. t += tt[1] az_vel = -1 * az_speed - el_vel = 0 - az_flag = 1 + az_flag = 1 # 1 el_flag = 0 increasing = False target_az = _get_target_az(az, t, increasing, az_endpoint1, az_endpoint2, az_speed / np.sin(np.deg2rad(az - az_cent)), az_drift) dec_num_scans() - point_group_batch = MIN_GROUP_NEW_LEG - 1 else: time_remaining = get_scan_time(az, target_az, az_speed, az_cent) az = target_az t += time_remaining az_vel = az_speed - el_vel = 0 - az_flag = 2 + az_flag = 0 # 2 el_flag = 0 else: if get_scan_time(az, target_az, az_speed, az_cent) > 2 * step_time: t += step_time az -= step_time * az_speed / np.sin(np.deg2rad(az - az_cent)) - el = el_cent + el_throw * np.sin(t * el_freq * 2 * np.pi) az_vel = -1 * az_speed - el_vel = el_throw * el_freq * 2 * np.pi * np.cos(t * el_freq * 2 * np.pi) - az_flag = 1 + az_flag = 0 # 1 el_flag = 0 elif az == target_az: + point_group_batch = MIN_GROUP_NEW_LEG - 1 + if turnaround_method == "three_leg": + _v = az_vel / np.sin(np.deg2rad(az - az_cent)) + turnaround_track = three_leg_tr.gen_three_leg_turnaround( + t0=t + t0, az0=az, el0=el, v0=_v, + turntime=tt[-1], + step_time=step_time, + second_leg_time=0., + az_flag=az_flag, el_flag=el_flag, + point_group_batch=point_group_batch) + for track_point in turnaround_track: + point_queue.append(track_point) # Add the TrackPoints from the turnaround into the queue. + # Turn around. t += tt[-1] az_vel = az_speed - el_vel = 0 - az_flag = 1 + az_flag = 1 # 1 el_flag = 0 increasing = True target_az = _get_target_az(az, t, increasing, az_endpoint1, az_endpoint2, az_speed / np.sin(np.deg2rad(az - az_cent)), az_drift) dec_num_scans() - point_group_batch = MIN_GROUP_NEW_LEG - 1 else: time_remaining = get_scan_time(az, target_az, az_speed, az_cent) az = target_az t += time_remaining az_vel = -1 * az_speed - el_vel = 0 - az_flag = 2 + az_flag = 0 # 2 el_flag = 0 if not check_num_scans(): @@ -723,9 +702,15 @@ def check_num_scans(): # was recommended at LAT FAT for smoothly stopping the # motion at end of program. point_block[-1].az_vel = 0 - point_block[-1].el_vel = 0 + point_block[-1].el_vel = 1000 break + for p in point_block: + if p.el_vel == 1000: + p.el_vel = 0. + else: + p.el, p.el_vel = get_el(p.timestamp - t0) + yield point_block @@ -740,7 +725,8 @@ def generate_type2_scan(az_endpoint1, az_endpoint2, az_speed, batch_size=500, az_start='mid_inc', az_first_pos=None, - az_drift=None): + az_drift=None, + turnaround_method='three_leg'): """Python generator to produce times, azimuth and elevation positions, azimuth and elevation velocities, azimuth and elevation flags for arbitrarily long type 2 scan. @@ -800,10 +786,12 @@ def generate_type2_scan(az_endpoint1, az_endpoint2, az_speed, batch_size=batch_size, az_start=az_start, az_first_pos=az_first_pos, - az_drift=az_drift) + az_drift=az_drift, + turnaround_method=turnaround_method) -def plan_scan(az_end1, az_end2, el, v_az=1, a_az=1, az_start=None): +def plan_scan(az_end1, az_end2, el, v_az=1, a_az=1, az_start=None, + scan_type=1): """Determine some important parameters for running a ProgramTrack scan with the desired end points, velocity, and mean turn-around acceleration. @@ -873,6 +861,10 @@ def plan_scan(az_end1, az_end2, el, v_az=1, a_az=1, az_start=None): assert (2 * abs(throw / v_az) / dt >= 5) plan['step_time'] = dt + # In the case of type 2/3 scans, force step_time to be at most 0.1 seconds. + if scan_type in [2, 3]: + plan['step_time'] = min(0.1, plan['step_time']) + # Turn around prep distance (deg)? 5 point periods, times the vel. turnprep_buffer = 5 * dt * v_az diff --git a/socs/agents/acu/three_leg_turnaround.py b/socs/agents/acu/three_leg_turnaround.py new file mode 100644 index 000000000..6f0b5854b --- /dev/null +++ b/socs/agents/acu/three_leg_turnaround.py @@ -0,0 +1,211 @@ +import numpy as np + + +def gen_three_leg_turnaround(t0, az0, el0, v0, turntime, az_flag, el_flag, point_group_batch, + second_leg_time=None, second_leg_velocity=0, step_time=0.05): + from .drivers import TrackPoint + """ + Generates the trajectory of a 3part turnaround given the initial position and velocity of the platform. + This function generates a turnaround in three "legs": + + 1. The initial deceleration. + 2. The middle leg with a low velocity/acceleration to gently turn the gears of the motors around so + they contact the other face of the bearing with minimal force. The default velocity and acceleration + is 0 so the platform comes to a full stop in this leg by a default. + 3. The final acceleration to the scan velocity in the opposite direction. + + The turnaround time of this function adheres to the same equation as the "baseline" turnaround function: + + turntime = (2.0 * scan_velocity) / scan_acceleration + + Thus, for the same scan velocity and scan acceleration this turnaround will take the same time as the baseline. + + Args: + t0 (float): The initial time of the turnaround. + az0 (float): The iniital azimuth position of the turnaround. Should be equal to the final azimuth position of the turnaround. + el0 (float): The initial elevation of the turnaround. El velocity is forced to 0 here so this is only used for creating TrackPoints. + v0 (float): The initial azimuth velocity of the turnaround. + turntime (float): The turnaround time given by the above equation. + az_flag (int): The az flag used by the ACU. Inherited from the scan generation function and not changed. Used for TrackPoints. + el_flag (int): The el flag used by the ACU. Inherited from the scan generation function and not changed. Used for TrackPoints. + point_group_batch (int): the point group batch used by the ACU. Inherited from the scan generation function and not changed. Used for TrackPoints. + second_leg_time (float): The time used by the second leg of the turnaround. Defaults to 1 second. + This limits the minimum turnaround time to ~2.0 seconds! + second_leg_velocity (float): The velocity targeted by the beginning/end of the second leg of the turnaround. Defaults to 0 deg/s. + second_leg acceleration = 2.0 * second_leg_velocity / second_leg_time. + step_time (float): The step time between points in the turnaround. Defaults to 0.1 seconds (10Hz). + """ + + # Enforce 0 el velocity. Can be changed later if these want to be used with type2 or type3 LAT scans, + # but more work is necessary to get to that point. We shouldn't mix the two yet! + el_vel = 0. + + if second_leg_time is None: + second_leg_time = turntime / 3.0 # Cut the turnaround into equal thirds unless otherwise specified. + if second_leg_time == 0: + assert (second_leg_velocity == 0) + second_leg_acceleration = 0. + else: + second_leg_acceleration = 2.0 * second_leg_velocity / second_leg_time + + # Assert we have at least 0.5 seconds for the first and second legs of the turnaround! + # This limits the turntime to >= 1.5 seconds + assert (turntime - second_leg_time) >= 1.0, \ + "Time for the second leg of the turnaround is too long! The time remaining for the first and third legs is < 1.0 seconds!" + + # Solve for the first leg of the turnaround + t_start_1 = 0 # We have to solve the trajectory around 0 or the linear equations become very large. Add t back on later + t_target_1 = t_start_1 + (turntime - second_leg_time) / 2 # The first and third legs share the same portion of the turnaround time. + az_start_1 = az0 + v_start_1 = v0 + v_target_1 = second_leg_velocity * np.sign(v_start_1) + a_start_1 = 0 + a_target_1 = second_leg_acceleration * -1 * np.sign(v0) + j_start_1 = 0 # We always target a jerk of 0 at the beginning and end of turnaround legs. + j_target_1 = 0 + ts_1, azs_1, vs_1 = _gen_trajectory(t_start_1, t_target_1, az_start_1, + v_start_1, v_target_1, a_start_1, + a_target_1, j_start_1, j_target_1, + step_time) + + # Solve for the second leg of the turnaround + t_start_2 = t_target_1 + t_target_2 = t_start_2 + second_leg_time + az_start_2 = azs_1[-1] # The acceleration of the beggining of the next turnaround leg should always match the end of the last leg. + v_start_2 = v_target_1 + v_target_2 = v_start_2 * -1 + a_start_2 = a_target_1 + a_target_2 = a_start_2 + j_start_2 = 0 + j_target_2 = 0 + if second_leg_time == 0: + ts_2, azs_2, vs_2 = [t_start_2, t_target_2], [az_start_2, az_start_2], [0., 0.] + else: + ts_2, azs_2, vs_2 = _gen_trajectory(t_start_2, t_target_2, az_start_2, + v_start_2, v_target_2, a_start_2, + a_target_2, j_start_2, j_target_2, + step_time) + + # Solve for the third leg of the turnaround + t_start_3 = t_target_2 + t_target_3 = t_start_3 + (turntime - second_leg_time) / 2.0 + az_start_3 = azs_2[-1] + v_start_3 = v_target_2 + v_target_3 = v0 * -1 + a_start_3 = a_target_2 + a_target_3 = 0 + j_start_3 = 0 + j_target_3 = 0 + ts_3, azs_3, vs_3 = _gen_trajectory(t_start_3, t_target_3, az_start_3, + v_start_3, v_target_3, a_start_3, + a_target_3, j_start_3, j_target_3, + step_time) + + # Concatenate the times, azimuth positions, and azimuth velocities together. + # The first point of each leg is a duplicate of the last so we drop those points. + ts = np.concatenate([ts_1[1:], ts_2[1:-1], ts_3[1:]]) + t0 + azs = np.concatenate([azs_1[1:], azs_2[1:-1], azs_3[1:]]) + vs = np.concatenate([vs_1[1:], vs_2[1:-1], vs_3[1:]]) + + # Turn our turnaround solution into TrackPoint's for the ACU. + turnaround_track = [] + for t, az, v in zip(ts, azs, vs): + turnaround_track.append(TrackPoint(timestamp=t, + az=az, el=el0, az_vel=v, el_vel=el_vel, + az_flag=az_flag, el_flag=el_flag, + group_flag=int(point_group_batch > 0))) + + return turnaround_track + + +def _gen_trajectory(t_i, t_f, xn1_i, x0_i, x0_f, x1_i, x1_f, x2_i, x2_f, step_time): + """ + Generally, generates the trajectory that minimizes the third derivative of the parameter defined by x0. + + In the context of this module, this function is used to generate the legs of the 3part turnaround in a way that + minimizes the snap of the motion. Because we don't know the final positions of each of the legs we must + generate the trajectories using the initial and final velocity, acceleration, and jerk, which minimizes the snap. + + In this context, x0 is the function of velocity, x1 is acceleration, x2 is jerk, and xn1 is position. + + Args: + t_i (float): The initial time of the trajectory. + t_f (float): The final time of the trajectory. + xn1_i (float): The initial position. + x0_i (float): The initial velocity. + x0_f (float): The final velocity. + x1_i (float): The initial acceleration. + x1_f (float): The final acceleration. + x2_i (float): The initial jerk. + x2_f (float): The final jerk. + + Returns: + ts (float array): A numpy array of timestamps. + xs (float array): A numpy array of azimuth positions. + vs (float array): A numpy array of azimuth velocities. + """ + + # Solve for the polynomial components that fits our initial and final conditions + A = solve_fifth_polynomial_lin_eqs(t_i, t_f, x0_i, x0_f, x1_i, x1_f, x2_i, x2_f) + + ts = np.arange(t_i, t_f + step_time, step_time) # Divide our times into points with step_time spacing + vs = np.polyval(A[::-1], ts) + + xs = np.polyval(np.polyint(A[::-1]), ts) + xs = xs - xs[0] + xn1_i # Solve for the positions of each point + + return ts, xs, vs + + +# Linear Algebra Below +def solve_fifth_polynomial_lin_eqs(t_i, t_f, x0_i, x0_f, x1_i, x1_f, x2_i, x2_f): + """ + Solves for the components of a polynomial equation of order five the form: + + x0 = A0 + A1*x + A2*x^2 + A3*x^3 + A4*x^4 + A5*a^5, + + given the initial/final conditions of the 0th, 1st, and 2nd derivatives. + + This solution minimizes the third derivative over the trajectory between t_i and t_f. + + Args: + t_i (float): starting time + t_f (float): stop time + x0_i (float): initial 0th derivative condition + x0_f (float): final 0th derivative condition + x1_i (float): initial 1st derivative condition + x1_f (float): final 1st derivative condition + x2_i (float): initial 2nd derivative condition + x2_f (float): initial 2nd derivative condition + + Returns: + A0 (float): The solved 0th parameter of the order five polynomial. + A1 (float): The solved 1st parameter of the order five polynomial. + A2 (float): The solved 2nd parameter of the order five polynomial. + A3 (float): The solved 3rd parameter of the order five polynomial. + A4 (float): The solved 4th parameter of the order five polynomial. + A5 (float): The solved 5th parameter of the order five polynomial. + """ + + x0 = [1, 1, 1, 1, 1, 1] + x1 = [0, 1, 2, 3, 4, 5] + x2 = [0, 0, 2, 6, 12, 20] + + A = np.zeros((6, 6)) + for i, x in enumerate([x0, x1, x2]): + for j, y in enumerate(x): + if j < i: + continue + + A[2 * i, j] = y * t_i**(j - i) + A[2 * i + 1, j] = y * t_f**(j - i) + + B = np.zeros(6) + B[0] = x0_i + B[1] = x0_f + B[2] = x1_i + B[3] = x1_f + B[4] = x2_i + B[5] = x2_f + + return np.linalg.solve(A, B)