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 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 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 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_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 _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) -> GaitUpdate:
self._current_time = current_time
self._current_subgait = self._default_walk.graph.start_subgaits()[0]
return GaitUpdate.should_schedule(self._new_trajectory_command())