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 _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 _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,
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 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()