def start( self, current_time: Time, first_subgait_delay: Optional[Duration] = ZERO_DURATION, ) -> GaitUpdate: """Start the gait. Creates a trajectory command to go towards the idle position in the given duration. :returns Returns a GaitUpdate that usually contains a TrajectoryCommand. """ if first_subgait_delay is not None and first_subgait_delay > Duration( 0): self._start_time = current_time + first_subgait_delay self._end_time = self._start_time + self._duration self._scheduled_early = True return GaitUpdate.should_schedule_early( TrajectoryCommand( self._get_trajectory_msg(), self._duration, self._name, self._start_time, )) else: self._start_time = current_time self._end_time = self._start_time + self._duration return GaitUpdate.should_schedule( TrajectoryCommand( self._get_trajectory_msg(), self._duration, self._name, self._start_time, ))
def update(self, current_time: Time, *_) -> GaitUpdate: """Give an update on the progress of the gait. If the current subgait is still running, this does nothing. If the gait should be stopped, this will be done If the current subgait is done, it will start the next subgait :param current_time: Current time :returns: Returns a GaitUpdate that may contain a TrajectoryCommand, and any of the flags set to true, depending on the state of the Gait. """ self._current_time = current_time if self._should_freeze: return self._execute_freeze() # If the current subgait is not finished, no new trajectory is necessary if current_time < self._end_time: return GaitUpdate.empty() # If the subgait is finished and it was frozen, execute the subgait after freeze if self._is_frozen: self._current_subgait = self._subgait_after_freeze self._is_frozen = False self._update_time_stamps(self._current_subgait) return GaitUpdate.should_schedule( self._command_from_current_subgait()) return self._update_next_subgait()
def _update_next_subgait_early(self) -> GaitUpdate: """Update the next subgait. First the next subgait is determined, which is based on the status of transitioning and whether the subgait has to be stopped. If the next subgait is not None a TrajectoryComamnd containing that subgait is returned. When updating the next subgait early, we don't set the current_subgait or a new start time. Instead we only send a schedule command and let self._update_next_subgait() deal with clean up after the previous subgait has actually finished. :return: optional trajectory_command """ self._scheduled_early = True if self._transition_to_subgait is not None and not self._is_transitioning: # We should schedule a transition subgait next_subgait = self._make_transition_subgait() elif self._transition_to_subgait is not None and self._is_transitioning: next_subgait = self.subgaits.get( self._transition_to_subgait.subgait_name) else: next_subgait = self._next_graph_subgait() self._next_subgait = next_subgait # If there is no subgait, return None and False for is_finished if next_subgait is None: return GaitUpdate.empty() return GaitUpdate.should_schedule_early( TrajectoryCommand.from_subgait(next_subgait, self._end_time))
def start( self, current_time: Time, first_subgait_delay: Optional[ Duration] = DEFAULT_FIRST_SUBGAIT_START_DELAY, ) -> GaitUpdate: """Start the gait. Sets current subgait to the first subgait, resets the time and generates the first trajectory command. May optionally delay the first subgait. :param first_subgait_delay Optional duration to delay the first subgait by. :return: A TrajectoryCommand message with the trajectory of the first subgait. """ self._reset() self._current_time = current_time self._current_subgait = self.subgaits[self.graph.start_subgaits()[0]] self._next_subgait = self._current_subgait # Delay first subgait if duration is greater than zero if first_subgait_delay > Duration(0): self._start_is_delayed = True self._update_time_stamps(self._current_subgait, first_subgait_delay) return GaitUpdate.should_schedule_early( self._command_from_current_subgait()) else: self._start_is_delayed = False self._update_time_stamps(self._current_subgait) return GaitUpdate.should_schedule( self._command_from_current_subgait())
def _update_next_subgait_early(self) -> GaitUpdate: """Already schedule the next subgait with the end time of the current subgait as the start time. :returns: a GaitUpdate that is empty or contains a trajectory command :rtype: GaitUpdate """ self._scheduled_early = True self._next_command = self._get_next_command() if self._next_command is None: return GaitUpdate.empty() return GaitUpdate.should_schedule_early(self._next_command)
def _update_next_subgait(self) -> GaitUpdate: """Update the next subgait. Behaves differently based on whether a new subgait has been scheduled early. First the next subgait is determined, which is based on the status of transitioning and whether the subgait has to be stopped. Then if the next subgait is not None the current subgait is replaced by the next subgait, and all appriopriate values are updated. If a new subgait hasn't been scheduled early, this function also returns a TrajectoryCommand. :return: optional trajectory_command, is_finished """ if self._transition_to_subgait is not None and not self._is_transitioning: # We should schedule a transition subgait self._is_transitioning = True next_subgait = self._make_transition_subgait() elif self._transition_to_subgait is not None and self._is_transitioning: # A transition subgait has ended next_subgait = self.subgaits.get( self._transition_to_subgait.subgait_name) self._transition_to_subgait = None self._is_transitioning = False elif self._scheduled_early: # We scheduled early and already determined the next subgait next_subgait = self._next_subgait else: # We determine the next subgait with the subgait graph next_subgait = self._next_graph_subgait() # If there is no next subgait, then we are finished if next_subgait is None: return GaitUpdate.finished() # Update subgait and timestamps self._update_time_stamps(next_subgait) self._current_subgait = next_subgait # Schedule the next subgait if we haven't already scheduled early if not self._scheduled_early: return GaitUpdate.should_schedule( self._command_from_current_subgait()) else: # Reset early schedule attributes self._scheduled_early = False self._next_subgait = None return GaitUpdate.subgait_updated()
def update(self, current_time: Time) -> GaitUpdate: self._current_time = current_time if self._current_time < self._end_time or self._constructing: return GaitUpdate.empty() else: next_subgait = self._default_walk.graph[( self._current_subgait, self._default_walk.graph.TO)] if next_subgait == self._default_walk.graph.END: return GaitUpdate.finished() self._constructing = True self._current_subgait = next_subgait command = self._new_trajectory_command() self._constructing = False return GaitUpdate.should_schedule(command)
def update( self, current_time: Time, early_schedule_duration: Optional[Duration] = ZERO_DURATION, ) -> GaitUpdate: """Give an update on the progress of the gait. :param current_time: Current time. :param early_schedule_duration: The duration to schedule the gait early. :returns Returns a GaitUpdate with only the is_finished set to either true or false. """ if current_time >= self._end_time: return GaitUpdate.finished() elif self._scheduled_early and current_time > self._start_time: return GaitUpdate.subgait_updated() else: return GaitUpdate.empty()
def _update_state_machine(self) -> GaitUpdate: """Update the state machine that the new subgait has begun. Also updates the starting position and time stamps for the next subgait. :returns: a GaitUpdate for the state machine :rtype: GaitUpdate """ if self._next_command is None: return GaitUpdate.finished() self._update_start_pos() self._update_time_stamps(self._next_command.duration) self._scheduled_early = False return GaitUpdate.subgait_updated()
def test_update_subgait_schedule_early(self): self.gait.start(Time(seconds=0)) gait_update = self.gait.update(Time(seconds=1), Duration(seconds=0.8)) self.assertEqual(self.gait.subgait_name, "right_open") self.assertEqual(self.gait._start_time, Time(seconds=0)) self.assertEqual(self.gait._end_time, Time(seconds=1.5)) self.assertTrue(self.gait._scheduled_early) self.assertEqual( gait_update, GaitUpdate.should_schedule_early( TrajectoryCommand.from_subgait( self.gait.subgaits["left_swing"], Time(seconds=1.5))), ) gait_update = self.gait.update(Time(seconds=1.6), Duration(seconds=0.8)) self.assertEqual(gait_update, GaitUpdate.subgait_updated())
def test_update_subgait_start_delayed_did_start(self): self.gait.start(Time(seconds=0), Duration(seconds=3)) gait_update = self.gait.update(Time(seconds=3.5)) self.assertEqual(self.gait.subgait_name, "right_open") self.assertEqual(self.gait._start_time, Time(seconds=3)) self.assertEqual(self.gait._end_time, Time(seconds=4.5)) self.assertFalse(self.gait._start_is_delayed) self.assertEqual(gait_update, GaitUpdate.subgait_updated())
def test_update_subgait_not_done(self): self.gait.start(Time(seconds=0)) gait_update = self.gait.update(Time(seconds=0.6)) self.assertEqual(self.gait.subgait_name, "right_open") self.assertEqual(self.gait._start_time, Time(seconds=0)) self.assertEqual(self.gait._end_time, Time(seconds=1.5)) self.assertFalse(self.gait._scheduled_early) self.assertEqual(gait_update, GaitUpdate.empty())
def start( self, current_time: Time, first_subgait_delay: Optional[ Duration] = DEFAULT_FIRST_SUBGAIT_DELAY_START_RS_DURATION, ) -> GaitUpdate: """ This function is called to start the realsense gait, it does the following. 1) Make a service call to march_realsense_reader. 2) Update all subgaits to interpolated subgaits with the given parameters (this will later become only some of the subgaits when the update function is also used). 3) Update the gait parameters to prepare for start 4) Return the first subgait, if correct parameters were found. :return: A gait update that tells the state machine what to do. Empty means that that state machine should not start a gait. """ self._reset() # Delay start until parameterization is done self._start_is_delayed = True # Start time will be set later, but to prevent updates during the service # calls to think the gait start time has passed, set start time in the future. self._start_time = current_time + self.INITIAL_START_DELAY_TIME self._current_time = current_time # If a gait is dependent on some other gait its subgaits are already # interpolated from parameters so we can skip the realsense call if not self._dependent_on: realsense_update_successful = self.get_realsense_update() if not realsense_update_successful: return GaitUpdate.empty() self._current_subgait = self.subgaits[self.graph.start_subgaits()[0]] self._next_subgait = self._current_subgait if first_subgait_delay is None: first_subgait_delay = self.DEFAULT_FIRST_SUBGAIT_DELAY_START_RS_DURATION self._start_time = self._gait_selection.get_clock().now( ) + first_subgait_delay self._end_time = self._start_time + self._current_subgait.duration return GaitUpdate.should_schedule_early( self._command_from_current_subgait())
def _update_start_subgait(self) -> GaitUpdate: """Update the state machine that the start gait has begun. Also updates the start position and the time stamps for the next subgait. :returns: a GaitUpdate for the state machine :rtype: GaitUpdate""" self._start_is_delayed = False self._update_start_pos() self._update_time_stamps(self._next_command.duration) return GaitUpdate.subgait_updated()
def test_start_delayed(self): gait_update = self.gait.start(Time(seconds=0), Duration(seconds=1)) self.assertEqual(self.gait.subgait_name, "right_open") self.assertEqual(self.gait._start_time, Time(seconds=1)) self.assertEqual(self.gait._end_time, Time(seconds=2.5)) self.assertTrue(self.gait._start_is_delayed) self.assertEqual( gait_update, GaitUpdate.should_schedule_early( self.gait._command_from_current_subgait()), )
def test_start(self): gait_update = self.gait.start(Time(seconds=0)) self.assertEqual(self.gait.subgait_name, "right_open") self.assertEqual(self.gait._start_time, Time(seconds=0)) self.assertEqual(self.gait._end_time, Time(seconds=1.5)) self.assertFalse(self.gait._start_is_delayed) self.assertEqual( gait_update, GaitUpdate.should_schedule( self.gait._command_from_current_subgait()), )
def test_update_subgait_done(self): self.gait.start(Time(seconds=0)) gait_update = self.gait.update(Time(seconds=1.8)) self.assertEqual(self.gait.subgait_name, "left_swing") self.assertEqual(self.gait._start_time, Time(seconds=1.8)) self.assertEqual(self.gait._end_time, Time(seconds=2.9)) self.assertFalse(self.gait._scheduled_early) self.assertEqual( gait_update, GaitUpdate.should_schedule( self.gait._command_from_current_subgait()), )
def update( self, current_time: Time, early_schedule_duration: Optional[ Duration] = DEFAULT_EARLY_SCHEDULE_UPDATE_DURATION, ) -> GaitUpdate: """Give an update on the progress of the gait. - If the start was delayed and we have passed the start time, we are now actually starting the gait. Hence the is_new_subgait flag shuold be set to true. - If the previous subgait ended, schedule a new one. - If we haven't scheduled early yet, and we are within early_schedule_duration of the end time, then schedule a new subgait early. - Else return nothing. :param current_time: Current time :param early_schedule_duration: Optional duration to schedule early :returns: Returns a GaitUpdate that may contain a TrajectoryCommand, and any of the flags set to true, depending on the state of the Gait. """ self._current_time = current_time if self._start_is_delayed: if self._current_time >= self._start_time: # Reset start delayed flag and update first subgait self._start_is_delayed = False return GaitUpdate.subgait_updated() else: return GaitUpdate.empty() if self._current_time >= self._end_time: return self._update_next_subgait() if (early_schedule_duration > Duration(0) and not self._scheduled_early and self._current_time >= self._end_time - early_schedule_duration): return self._update_next_subgait_early() return GaitUpdate.empty()
def update( self, current_time: Time, early_schedule_duration: Duration = DEFAULT_EARLY_SCHEDULE_UPDATE_DURATION, ) -> GaitUpdate: """Give an update on the progress of the gait. This function is called every cycle of the gait_state_machine. Schedules the first subgait when the delay has passed. Starts scheduling subsequent subgaits when the previous subgait is within early scheduling duration and updates the state machine when the next subgait is started. :param current_time: Current time. :type current_time: Time :param early_schedule_duration: Duration that determines how long ahead the next subgait is planned :type early_schedule_duration: Duration :return: GaitUpdate containing TrajectoryCommand when finished, else empty GaitUpdate :rtype: GaitUpdate """ self._current_time = current_time if self._start_is_delayed: if self._current_time >= self._start_time: return self._update_start_subgait() else: return GaitUpdate.empty() if (self._current_time >= self._end_time - early_schedule_duration and not self._scheduled_early): return self._update_next_subgait_early() if self._current_time >= self._end_time: return self._update_state_machine() return GaitUpdate.empty()
def _execute_freeze(self) -> GaitUpdate: """ Freezes the subgait, currently this means that there is a new subgait started of the given freeze duration which ends at the current position. If this happens in the middle of a subgait, it plans the rest of the original subgait after the freeze. :return: Returns a GaitUpdate """ self._freeze_position = self._position_after_time() self._previous_subgait = self._current_subgait.subgait_name self._subgait_after_freeze = self.subgait_after_freeze() self._current_subgait = self._freeze_subgait() self._should_freeze = False self._is_frozen = True self._update_time_stamps(self._current_subgait) return GaitUpdate.should_schedule(self._command_from_current_subgait())
def start( self, current_time: Time, first_subgait_delay: Duration = DEFAULT_FIRST_SUBGAIT_START_DELAY, ) -> GaitUpdate: """Starts the gait. The subgait will be scheduled with the delay given by first_subgait_delay. :param current_time: Time at which the subgait will start :type current_time: Time :param first_subgait_delay: Delay of first subgait schedule :type first_subgait_delay: Duration :return: A GaitUpdate containing a TrajectoryCommand :rtype: GaitUpdate """ self._reset() self.update_parameters() self._current_time = current_time self._start_time = self._current_time + first_subgait_delay self._next_command = self._get_trajectory_command() return GaitUpdate.should_schedule_early(self._next_command)
def start(self, current_time: Time) -> GaitUpdate: self._current_time = current_time self._current_subgait = self._default_walk.graph.start_subgaits()[0] return GaitUpdate.should_schedule(self._new_trajectory_command())