class GaitStateMachine:
"""The state machine used to make sure that only valid transitions will
be made."""
UNKNOWN = "unknown"
def __init__(self, gait_selection: GaitSelection,
trajectory_scheduler: TrajectoryScheduler):
"""Generates a state machine from given gaits and resets it to
UNKNOWN state.
In order to start the state machine see `run`.
:param GaitSelection gait_selection: Loaded gaits to build graph from
:param TrajectoryScheduler trajectory_scheduler: Scheduler interface for
scheduling trajectories
"""
self._gait_selection = gait_selection
self._trajectory_scheduler = trajectory_scheduler
self.logger = Logger(self._gait_selection, __class__.__name__)
self._input = StateMachineInput(gait_selection)
self._transition_callbacks = []
self._gait_callbacks = []
self._stop_accepted_callbacks = []
self._gait_graph = GaitGraph(self._gait_selection)
self._gait_graph.generate_graph()
# Current state is eiter an EdgePositions or a string representing the active gait
self._current_state: State = UnknownEdgePosition()
self._current_gait = None
self._shutdown_requested = False
# Boolean flag that indicates that the gait should stop
self._should_stop = False
# Boolean flag to not execute a stop when the gait is already stopping
self._is_stopping = False
self.update_timer = None
self.timer_period = (self._gait_selection.get_parameter(
"timer_period").get_parameter_value().double_value)
self.current_state_pub = self._gait_selection.create_publisher(
msg_type=CurrentState,
topic="/march/gait_selection/current_state",
qos_profile=DEFAULT_HISTORY_DEPTH,
)
self.current_gait_pub = self._gait_selection.create_publisher(
msg_type=CurrentGait,
topic="/march/gait_selection/current_gait",
qos_profile=DEFAULT_HISTORY_DEPTH,
)
self.error_sub = self._gait_selection.create_subscription(
msg_type=Error,
topic="/march/error",
callback=self._error_cb,
qos_profile=DEFAULT_HISTORY_DEPTH,
)
self._right_foot_on_ground = True
self._left_foot_on_ground = True
self._force_right_foot = 0
self._force_left_foot = 0
self._right_pressure_sub = self._gait_selection.create_subscription(
msg_type=ContactsState,
topic="/march/sensor/right_pressure_sole",
callback=lambda msg: self._update_foot_on_ground_cb(
Side.right, msg),
callback_group=ReentrantCallbackGroup(),
qos_profile=DEFAULT_HISTORY_DEPTH,
)
self._left_pressure_sub = self._gait_selection.create_subscription(
msg_type=ContactsState,
topic="/march/sensor/left_pressure_sole",
callback=lambda msg: self._update_foot_on_ground_cb(
Side.left, msg),
callback_group=ReentrantCallbackGroup(),
qos_profile=DEFAULT_HISTORY_DEPTH,
)
self._get_possible_gaits_client = self._gait_selection.create_service(
srv_type=PossibleGaits,
srv_name="/march/gait_selection/get_possible_gaits",
callback=self._possible_gaits_cb,
callback_group=ReentrantCallbackGroup(),
)
self.add_transition_callback(self._current_state_cb)
self.add_gait_callback(self._current_gait_cb)
self.logger.debug("Initialized state machine")
def _is_idle(self):
return isinstance(self._current_state, EdgePosition)
def _update_foot_on_ground_cb(self, side, msg):
"""Update the status of the feet on ground based on pressure sole data,
this is currently decided based on the force in simulation, but the numbers
for this will be updated when range of real pressure soles is known."""
if len(msg.states) > 0:
force = sum(state.total_wrench.force.z for state in msg.states)
# Assign force to specific foot
if side is Side.right:
self._force_right_foot = force
else:
self._force_left_foot = force
# If there are no contacts, change foot on ground to False
elif len(msg.states) == 0:
if side is Side.right:
self._force_right_foot = 0
else:
self._force_left_foot = 0
def _possible_gaits_cb(self, request, response):
"""Standard callback for the get possible gaits service"""
response.gaits = self.get_possible_gaits()
return response
def _current_state_cb(self, state: State):
"""Standard transition callback for when current state changes,
publishes the current state. More callbacks can be added using
add_transition_callback."""
if self._is_idle():
state_type = CurrentState.IDLE
state_name = self._gait_graph.get_name_of_position(
self._current_state)
else:
state_type = CurrentState.GAIT
state_name = self._current_state
self.current_state_pub.publish(
CurrentState(
header=Header(
stamp=self._gait_selection.get_clock().now().to_msg()),
state=state_name,
state_type=state_type,
))
def _current_gait_cb(self, gait_name, subgait_name, version,
duration: Duration, gait_type):
"""Standard callback when gait changes, publishes the current gait
More callbacke can be added using add_gait_callback"""
self.logger.debug(f"Current subgait updated to {subgait_name}")
self.current_gait_pub.publish(
CurrentGait(
header=Header(
stamp=self._gait_selection.get_clock().now().to_msg()),
gait=gait_name,
subgait=subgait_name,
version=version,
duration=duration.to_msg(),
gait_type=gait_type,
))
def _error_cb(self, msg):
"""Standard callback for state machine errors, completely stops
updating if the error is fatal. If non fatal, it stops the current
gait."""
if msg.type == Error.NON_FATAL:
self.stop_gait()
elif msg.type == Error.FATAL:
self.request_shutdown("A fatal error was posted to /march/error")
def get_possible_gaits(self):
"""Returns possible names of gaits that can be executed.
:returns List of names, or empty list when a gait is executing.
"""
if self._is_idle():
return self._gait_graph.possible_gaits_from_idle(
self._current_state)
else:
return []
def add_transition_callback(self, cb):
"""Adds a callback function that will be called when a transition to
a state happens.
The given method should be running as shortly as possible, since they
will be called from within the main loop.
:param cb: method that accepts a name of the state and a boolean if it
is an idle state.
"""
self._add_callback(self._transition_callbacks, cb)
def add_gait_callback(self, cb):
"""Adds a callback function that will be called when a trajectory of a gait is being scheduled.
The given method should be running as shortly as possible, since they
will be called from within the main loop.
:param cb: Callable method that accepts 5 args: gait name, subgait name,
version, duration and gait type.
"""
self._add_callback(self._gait_callbacks, cb)
def add_stop_accepted_callback(self, cb):
"""Adds a callback function that will be called when a gait accepts the stop command.
The given method should be running as shortly as possible, since they
will be called from within the main loop.
:param cb: Callable method that accepts no arguments and returns None.
"""
self._add_callback(self._stop_accepted_callbacks, cb)
def run(self):
"""Runs the state machine until shutdown is requested."""
self.update_timer = self._gait_selection.create_timer(
timer_period_sec=self.timer_period,
callback=self.update,
)
def update(self):
"""
Updates the current state based on the elapsed time, after the state
machine is started, this function is called every timer period.
"""
if not self._shutdown_requested:
if self._input.unknown_requested():
self._input.gait_accepted()
self._transition_to_unknown()
self._input.gait_finished()
self._call_transition_callbacks()
self._current_gait = None
self._trajectory_scheduler.reset()
if self._is_idle():
self._process_idle_state()
else:
self._process_gait_state()
else:
self.update_timer.cancel()
def request_shutdown(self, msg: Optional[str] = None):
"""Requests shutdown, which will terminate the state machine as soon as
possible."""
base_msg = "Shutdown requested"
if msg is not None:
base_msg += ": " + msg
self.logger.fatal(base_msg)
self._shutdown_requested = True
shutdown_system()
def stop_gait(self):
"""Requests a stop from the current executing gait, but keeps the state
machine running."""
if not self._is_idle() and not self._is_stopping:
self._should_stop = True
def check_correct_foot_pressure(self) -> bool:
"""
Check if the pressure is placed on the foot opposite to the subgait starting foot.
If not, issue a warning. This will only be checked when transitioning from idle to gait state
"""
if self._current_gait is not None:
if ("right" in self._current_gait.subgait_name
and self._force_right_foot > self._force_left_foot):
self.logger.warn(
"Incorrect pressure placement, place pressure on left foot"
)
return False
if ("left" in self._current_gait.subgait_name
and self._force_left_foot > self._force_right_foot):
self.logger.warn(
"Incorrect pressure placement, place pressure on right foot"
)
return False
return True
def _process_idle_state(self):
"""If the current state is idle, this function processes input for
what to do next."""
if self._input.gait_requested():
gait_name = self._input.gait_name()
self.logger.info(f"Requested gait `{gait_name}`")
gait = self._gait_selection._gaits.get(gait_name)
if (gait is not None
and gait_name in self._gait_graph.possible_gaits_from_idle(
self._current_state) or gait_name == "dynamic_walk"):
if (isinstance(gait.starting_position, DynamicEdgePosition)
and gait.starting_position != self._current_state):
self.logger.warn(
f"The gait {gait_name} does not have the correct dynamic "
f"starting position, should be {self._current_state}, but was "
f"{gait.starting_position}")
self._input.gait_rejected()
return
self._current_state = gait_name
self._should_stop = False
self._input.gait_accepted()
self._call_transition_callbacks()
self.logger.info(f"Accepted gait `{gait_name}`")
else:
self._input.gait_rejected()
self.logger.info(
f"Cannot execute gait `{gait_name}` from idle state `"
f"{self._current_state}`")
def _process_gait_state(self):
"""Processes the current state when there is a gait happening.
Schedules the next subgait if there is no trajectory happening or
finishes the gait if it is done."""
now = self._gait_selection.get_clock().now()
if self._current_gait is None:
self._current_gait = self._gait_selection._gaits[
self._current_state]
self.logger.info(f"Executing gait `{self._current_gait.name}`")
if self._current_gait.first_subgait_can_be_scheduled_early:
gait_update = self._current_gait.start(
now, self._gait_selection._first_subgait_delay)
else:
gait_update = self._current_gait.start(now)
if gait_update == GaitUpdate.empty():
self._input.gait_finished()
# Find the start position of the current gait, to go back to idle.
self._current_state = self._current_gait.starting_position
self._current_gait = None
self.logger.info(
f"Starting the gait returned "
f"no trajectory, going back to idle state "
f"{self._gait_graph.get_name_of_position(self._current_state)}"
)
return
if not self.check_correct_foot_pressure():
self.logger.debug(
f"Foot forces when incorrect pressure warning was issued: "
f"left={self._force_left_foot}, right={self._force_right_foot}"
)
self._process_gait_update(gait_update)
if self._trajectory_scheduler.failed():
self._trajectory_scheduler.reset()
self._current_gait.end()
self._current_gait = None
self._transition_to_unknown()
self._input.gait_finished()
return
self._handle_input()
if self._current_gait.subsequent_subgaits_can_be_scheduled_early:
gait_update = self._current_gait.update(
now, self._gait_selection._early_schedule_duration)
else:
gait_update = self._current_gait.update(now)
self._process_gait_update(gait_update)
def _process_gait_update(self, gait_update: GaitUpdate):
# Call gait callback if there is a new subgait
if gait_update.is_new_subgait:
self._call_gait_callbacks()
# Schedule a new trajectory if any
if gait_update.new_trajectory_command is not None:
self._trajectory_scheduler.schedule(
gait_update.new_trajectory_command)
# Process finishing of the gait
if gait_update.is_finished:
self._current_state = self._current_gait.final_position
self._current_gait.end()
self._input.gait_finished()
self._call_transition_callbacks()
self._current_gait = None
self._is_stopping = False
self._trajectory_scheduler.reset()
self.logger.info(f"Finished gait `{self._current_gait.name}`")
def _handle_input(self):
"""Handles stop and transition input from the input device. This input
is passed on to the current gait to execute the request"""
if self._is_stop_requested() and not self._is_stopping:
self._should_stop = False
self._is_stopping = True
if self._current_gait.stop():
self.logger.info(
f"Gait {self._current_gait.name} responded to stop")
self._input.stop_accepted()
self._call_callbacks(self._stop_accepted_callbacks)
else:
self.logger.info(
f"Gait {self._current_gait.name} does not respond to stop")
self._input.stop_rejected()
if self._input.transition_requested():
request = self._input.get_transition_request()
self._input.reset()
if self._current_gait.transition(request):
self.logger.info(
f"Gait {self._current_gait.name} responded to transition request "
f"{request.name}")
else:
self.logger.info(
f"Gait {self._current_gait.name} does not respond to transition "
f"request {request.name}")
def _call_transition_callbacks(self):
"""Calls all transition callbacks when the current state changes."""
self._call_callbacks(self._transition_callbacks, self._current_state)
def _is_stop_requested(self):
"""Returns true if either the input device requested a stop or some other
external source requested a stop."""
return self._input.stop_requested() or self._should_stop
def _call_gait_callbacks(self):
"""Calls all added gait callbacks when the current gait changes."""
if self._current_gait is not None:
self._call_callbacks(
self._gait_callbacks,
self._current_gait.name,
self._current_gait.subgait_name,
self._current_gait.version,
self._current_gait.duration,
self._current_gait.gait_type,
)
def _transition_to_unknown(self):
"""When the unknown button is pressed, this function resets the
state machine to unknown state."""
if self._current_gait is not None:
self._trajectory_scheduler.send_position_hold()
self._trajectory_scheduler.cancel_active_goals()
self._current_state = UnknownEdgePosition()
self.logger.info("Transitioned to unknown")
@staticmethod
def _add_callback(callbacks, cb):
"""Adds a method to a list if it is callable."""
if callable(cb):
callbacks.append(cb)
@staticmethod
def _call_callbacks(callbacks, *args):
"""Calls multiple methods with same set of arguments."""
for cb in callbacks:
cb(*args)
class GaitGraph:
"""
The gait graph is made to keep track of all named positions and gait transitions
that are available. It is generated at startup and is used by the gait state
machine to get the possible gaits and to verify a requested gait.
"""
NamedPositions = Dict[EdgePosition, str]
IdleTransitions = Dict[EdgePosition, Set[str]]
GaitTransitions = Dict[str, EdgePosition]
UNNAMED = "unnamed"
UNKNOWN = "unknown"
def __init__(self, gait_selection: GaitSelection):
self._gait_selection = gait_selection
self.logger = Logger(self._gait_selection, __class__.__name__)
self._named_positions: GaitGraph.NamedPositions = {}
self._idle_transitions: GaitGraph.IdleTransitions = {}
self._dynamic_transitions: Set[str] = set()
self._gait_transitions: GaitGraph.GaitTransitions = {}
self._unnamed_count = 0
def possible_gaits_from_idle(self, current_state: EdgePosition) -> Set:
"""
Get the list of gait names that are (possibly) available from this edge
position.
:param current_state: The current position of the exo.
:return: A list of gait names
"""
if isinstance(current_state, DynamicEdgePosition):
return self._dynamic_transitions
else:
return self._idle_transitions[current_state]
def generate_graph(self):
"""Generate the gait graph."""
self._make_named_positions()
self._make_transitions()
self._make_home_gaits()
self._validate_from_transitions()
self._validate_to_transitions()
def _make_named_positions(self):
"""Create all named positions from the default.yaml.
Create a StaticEdgePosition for every named position and a default UnknownEdgePosition.
"""
self._named_positions = {
StaticEdgePosition(position["joints"]): name
for name, position in self._gait_selection.positions.items()
}
self._named_positions[UnknownEdgePosition()] = GaitGraph.UNKNOWN
def get_name_of_position(self, position: EdgePosition) -> str:
"""
Get the name of a given position. Static positions will have a name from the
default.yaml or a name generated at startup. Dynamic gaits have no name,
but return a string with the values.
:param position: The position to get the name from.
:return: The name.
"""
if position in self._named_positions:
return self._named_positions[position]
else:
return f"unnamed: {position}"
def _make_transitions(self):
"""Make all gait transitions.
For every gait, first checks the starting position and adds an idle transition
from the gait's starting position to the gait name.
Then the final position is checked, and a transition from the gait name
to the final position is added.
"""
for gait in self._gait_selection._gaits.values():
if (gait.starting_position
not in self._named_positions) and isinstance(
gait.starting_position, StaticEdgePosition):
position_name = self._new_unnamed()
self.logger.warn(
f"No named position given for starting position of gait `"
f"{gait.name}, creating {position_name}. The starting position "
f"is {gait.starting_position}")
self._named_positions[gait.starting_position] = position_name
self._add_idle_transition(gait.starting_position, gait.gait_name)
if (gait.final_position
not in self._named_positions) and isinstance(
gait.final_position, StaticEdgePosition):
position_name = self._new_unnamed()
self.logger.warn(
f"No named position given for final position of gait `"
f"{gait.name}, creating {position_name}. The final position is "
f"{gait.final_position}")
self._named_positions[gait.final_position] = position_name
self._gait_transitions[gait.gait_name] = gait.final_position
def _make_home_gaits(self):
"""Make all home gaits.
For every named position, create a home gait and add the home gait
to the GaitSelection and to the idle transitions.
"""
for position, name in self._named_positions.items():
if isinstance(position, UnknownEdgePosition):
continue
position_dict = dict(
zip(self._gait_selection.joint_names, position.values))
home_gait = HomeGait(name, position_dict, "")
home_gait_name = home_gait.name
if home_gait_name in self._gait_transitions:
raise GaitStateMachineError(
f"Gaits cannot have the same name as home gait `{home_gait_name}`"
)
self._gait_selection._gaits[home_gait_name] = home_gait
self._add_idle_transition(home_gait.starting_position,
home_gait_name)
def _add_idle_transition(self, start_position: EdgePosition,
gait_name: str):
"""Add an idle transition.
:param start_position Position the gait is started from.
:param gait_name Name of the gait that is started.
"""
if isinstance(start_position, DynamicEdgePosition):
self._dynamic_transitions.add(gait_name)
elif start_position in self._idle_transitions:
self._idle_transitions[start_position].add(gait_name)
else:
self._idle_transitions[start_position] = {gait_name}
def _validate_from_transitions(self):
"""Check that from every position there is a transition
:return Returns true if from every position there is a transition.
"""
no_from_transitions = []
for position, name in self._named_positions.items():
if position not in self._idle_transitions:
no_from_transitions.append(name)
if len(no_from_transitions) > 0:
self.logger.warn(
f'There are no transitions from named positions: [{", ".join(no_from_transitions)}]'
)
return False
return True
def _validate_to_transitions(self):
"""Check that all positions can be reached.
:return Returns true if all positions can be reached.
"""
no_to_transitions = []
for position, name in self._named_positions.items():
if (not isinstance(position, UnknownEdgePosition)
and position not in self._gait_transitions.values()):
no_to_transitions.append(name)
if len(no_to_transitions) > 0:
self.logger.warn(
f'There are no transitions to named positions: [{", ".join(no_to_transitions)}]'
)
return False
return True
def _new_unnamed(self) -> str:
"""Generate a new unnamed position.
The first time this will give the position unnamed_0.
The unnamed count is increased each call.
"""
count = self._unnamed_count
self._unnamed_count += 1
return f"{GaitGraph.UNNAMED}_{count}"
def __str__(self) -> str:
"""Convert the gait graph to a human-friendly string."""
s = "Positions:\n"
for position, name in self._named_positions.items():
s += f"\t{name}: {position}\n"
s += "Idle transitions:\n"
for position, gaits in self._idle_transitions.items():
for gait in gaits:
s += f"\t{self._named_positions[position]} - {gait}\n"
s += "Gait transitions:\n"
for gait, position in self._gait_transitions.items():
s += f"\t{gait} - {self._named_positions[position]}\n"
return s
class BalanceGait(GaitInterface):
"""Base class to create a gait using the moveit motion planning."""
CAPTURE_POINT_SERVICE_TIMEOUT = 1.0
MOVEIT_INTERFACE_SERVICE_TIMEOUT = 1.0
def __init__(self,
node: Node,
gait_name: str = "balanced_walk",
default_walk: Gait = None):
self.gait_name = gait_name
self._node = node
self._default_walk = default_walk
self._constructing = False
self.logger = Logger(self._node, __class__.__name__)
self._current_subgait = None
self._current_subgait_duration = Duration(0)
self._start_time = None
self._end_time = None
self._current_time = None
self.capture_point_event = Event()
self.capture_point_result = None
self._capture_point_service = {
"left_leg":
node.create_client(srv_name="/march/capture_point/foot_left",
srv_type=CapturePointPose),
"right_leg":
node.create_client(srv_name="/march/capture_point/foot_right",
srv_type=CapturePointPose),
}
self.moveit_event = Event()
self.moveit_trajectory_result = None
self._moveit_trajectory_service = node.create_client(
srv_name="/march/moveit/get_trajectory",
srv_type=GetMoveItTrajectory)
@property
def default_walk(self) -> Gait:
"""Return the default walk subgait."""
return self._default_walk
@default_walk.setter
def default_walk(self, new_default_walk: Gait):
"""Set a new default walk subgait to the balance subgait.
:param new_default_walk:
A new subgait which is the default balance walking pattern
"""
self._default_walk = new_default_walk
def compute_swing_leg_target(self, leg_name: str,
subgait_name: str) -> bool:
"""Set the swing leg target to capture point.
:param leg_name: The name of the used move group
:param subgait_name: the normal subgait name
"""
subgait_duration = self.default_walk[subgait_name].duration
if not self._capture_point_service[leg_name].wait_for_service(
timeout_sec=3):
self.logger.warn(f"Capture point service not found: "
f"{self._capture_point_service[leg_name]}")
self.capture_point_event.clear()
future = self._capture_point_service[leg_name].call_async(
CapturePointPose.Request(duration=subgait_duration.seconds))
future.add_done_callback(self.capture_point_cb)
return self.capture_point_event.wait(
timeout=self.CAPTURE_POINT_SERVICE_TIMEOUT)
def capture_point_cb(self, future: Future):
"""Set capture point result when the capture point service returns."""
self.capture_point_result = future.result()
self.capture_point_event.set()
def compute_stance_leg_target(self, leg_name: str,
subgait_name: str) -> JointState:
"""Set the target of the stance leg to the end of the gait file.
:param leg_name: The name of the move group which does not use capture point
:param subgait_name: the normal subgait name
:return the duration of the default subgait
"""
side_prefix = "right" if "right" in leg_name else "left"
default_subgait = deepcopy(self.default_walk[subgait_name])
non_capture_point_joints = []
for joint in default_subgait.joints:
if side_prefix in joint.name:
non_capture_point_joints.append(joint)
joint_state = JointState()
joint_state.header = Header()
joint_state.header.stamp = self._node.get_clock().now().to_msg()
joint_state.name = [joint.name for joint in non_capture_point_joints]
joint_state.position = [
joint.setpoints[-1].position for joint in non_capture_point_joints
]
joint_state.velocity = [
joint.setpoints[-1].velocity for joint in non_capture_point_joints
]
return joint_state
def construct_trajectory(self, swing_leg: str, subgait_name: str):
"""Constructs a balance trajectory for all joints.
:param swing_leg: The name of the swing leg ('left_leg' or 'right_leg')
:param subgait_name: the normal subgait name
:return: the balance trajectory
"""
if swing_leg not in ["right_leg", "left_leg"]:
self.logger.warn(f"Swing leg was not one of the possible legs "
f"(left_leg or right_leg), but {swing_leg}, "
f"using default walk instead")
return self.default_walk[subgait_name].to_joint_trajectory_msg()
stance_leg = "right_leg" if swing_leg == "left_leg" else "left_leg"
capture_point_success = self.compute_swing_leg_target(
swing_leg, subgait_name)
if not capture_point_success:
self.logger.warn(
"Capture point call took too long, using default gait.")
return self.default_walk[subgait_name].to_joint_trajectory_msg()
stance_leg_target = self.compute_stance_leg_target(
stance_leg, subgait_name)
self.moveit_event.clear()
request = GetMoveItTrajectory.Request(
swing_leg=swing_leg,
swing_leg_target_pose=self.capture_point_result.capture_point,
stance_leg_target=stance_leg_target,
)
trajectory_future = self._moveit_trajectory_service.call_async(request)
trajectory_future.add_done_callback(self.moveit_event_cb)
if self.moveit_event.wait(self.MOVEIT_INTERFACE_SERVICE_TIMEOUT):
return self.moveit_trajectory_result.trajectory
else:
self.logger.warn(
"Moveit interface call took too long, using default gait.")
return self.default_walk[subgait_name].to_joint_trajectory_msg()
def moveit_event_cb(self, future: Future):
"""Set moveit trajectory result when the capture point service returns."""
self.moveit_trajectory_result = future.result()
self.moveit_event.set()
def get_joint_trajectory_msg(self, name: str) -> JointTrajectory:
"""Returns the trajectory of a subgait name that could use moveit.
:param name: the name of the subgait
"""
if name in ["right_open_2", "right_swing_2"]: # noqa: SIM116
return self.construct_trajectory("right_leg", name)
elif name == "left_swing_2":
return self.construct_trajectory("left_leg", name)
else:
return self.default_walk[name].to_joint_trajectory_msg()
# GaitInterface
@property
def name(self):
return self.gait_name
@property
def subgait_name(self):
return self._current_subgait
@property
def duration(self):
return self._current_subgait_duration
@property
def gait_type(self):
return "walk_like"
@property
def starting_position(self):
return self._default_walk.starting_position
@property
def final_position(self):
return self._default_walk.final_position
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())
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 _new_trajectory_command(self) -> TrajectoryCommand:
"""Update the trajectory values and generate a new trajectory command.
:return Return a TrajectoryCommand for the next subgait.
"""
trajectory = self.get_joint_trajectory_msg(self._current_subgait)
time_from_start = trajectory.points[-1].time_from_start
self._current_subgait_duration = Duration.from_msg(time_from_start)
self._start_time = self._current_time
self._end_time = self._start_time + self._current_subgait_duration
return TrajectoryCommand(
trajectory,
self._current_subgait_duration,
self.subgait_name,
self._start_time,
)
def end(self):
self._current_subgait = None
self._current_subgait_duration = Duration(0)
class RealsenseGait(SetpointsGait):
"""
The RealsenseGait class is used for creating gaits based on the parameters given
by the realsense reader. From these parameters the subgaits to interpolate are
interpolated after a realsense call during the start of the gait. It is based on the
setpoints gait, and it uses the interpolation over 1 or 2 dimensions with 2 or 4
subgaits respectively.
"""
SERVICE_TIMEOUT = Duration(seconds=2.0)
INITIAL_START_DELAY_TIME = Duration(seconds=10.0)
CAMERA_NAME_MAP = {
"front": GetGaitParameters.Request.CAMERA_FRONT,
"back": GetGaitParameters.Request.CAMERA_BACK,
}
REALSENSE_CATEGORY_MAP = {
"stairs_up": GetGaitParameters.Request.STAIRS_UP,
"stairs_down": GetGaitParameters.Request.STAIRS_DOWN,
"ramp_up": GetGaitParameters.Request.RAMP_UP,
"ramp_down": GetGaitParameters.Request.RAMP_DOWN,
"sit": GetGaitParameters.Request.SIT,
"curb_up": GetGaitParameters.Request.CURB_UP,
"curb_down": GetGaitParameters.Request.CURB_DOWN,
}
def __init__(
self,
gait_name: str,
subgaits: dict,
graph: SubgaitGraph,
gait_selection: GaitSelection,
realsense_category: str,
camera_to_use: str,
subgaits_to_interpolate: dict,
dimensions: InterpolationDimensions,
process_service: Client,
starting_position: EdgePosition,
final_position: EdgePosition,
parameters: List[float],
dependent_on: List[str],
responsible_for: List[str],
):
super(RealsenseGait, self).__init__(gait_name, subgaits, graph)
self._gait_selection = gait_selection
self.logger = Logger(self._gait_selection, __class__.__name__)
self.parameters = parameters
self.dimensions = dimensions
self.dimensions = dimensions
self.realsense_category = self.realsense_category_from_string(
realsense_category)
self.camera_to_use = self.camera_msg_from_string(camera_to_use)
self.subgaits_to_interpolate = subgaits_to_interpolate
# Set up service and event for asynchronous
self._get_gait_parameters_service = process_service
self.realsense_service_event = Event()
self.realsense_service_result = None
self._starting_position = starting_position
self._final_position = final_position
self._dependent_on = dependent_on
self._responsible_for = responsible_for
@property
def dependent_on(self):
return self._dependent_on
@property
def responsible_for(self):
return self._responsible_for
@property
def subsequent_subgaits_can_be_scheduled_early(self) -> bool:
"""
Whether a subgait can be scheduled early, this is not possible for the realsense
gait, since this will later have a service call to determine the next subgait.
"""
return True
@property
def first_subgait_can_be_scheduled_early(self) -> bool:
"""
Whether the first subgait can be started with a delay, this is possible for
the realsense gait.
"""
return True
@property
def starting_position(self) -> EdgePosition:
return self._starting_position
@property
def final_position(self) -> EdgePosition:
return self._final_position
@classmethod
def from_yaml(
cls,
gait_selection: GaitSelection,
robot: urdf.Robot,
gait_name: str,
gait_config: dict,
gait_graph: dict,
gait_directory: str,
process_service: Client,
):
"""
Construct a realsense gait from the gait_config from the realsense_gaits.yaml.
:param gait_selection: The GaitSelection node that will be used for making the
service calls to the
realsense reader.
:param robot: The urdf robot that can be used to verify the limits of the
subgaits.
:param gait_name: The name of the gait.
:param gait_config: The yaml node with the needed configurations.
:param gait_graph: The graph from the .gait file with the subgait transitions.
:param gait_directory: The gait_directory that is being used.
:param process_service: The service from which to get the gait parameters
:return: The constructed RealsenseGait
"""
graph = SubgaitGraph(gait_graph)
subgaits_to_interpolate = {}
try:
dimensions = InterpolationDimensions.from_integer(
gait_config["dimensions"])
dependent_on = gait_config.get("dependent_on", [])
responsible_for = gait_config.get("responsible_for", [])
parameters = [0.0 for _ in range(amount_of_parameters(dimensions))]
realsense_category = gait_config["realsense_category"]
camera_to_use = gait_config["camera_to_use"]
subgait_version_map = gait_config["subgaits"]
# Create subgaits to interpolate with
for subgait_name in subgait_version_map:
subgaits_to_interpolate[subgait_name] = [
Subgait.from_name_and_version(robot, gait_directory,
gait_name, subgait_name,
version)
for version in subgait_version_map[subgait_name]
]
if len(subgaits_to_interpolate[subgait_name]
) != amount_of_subgaits(dimensions):
raise WrongRealSenseConfigurationError(
f"The amount of subgaits in the realsense version map "
f"({len(subgaits_to_interpolate[subgait_name])}) doesn't match "
f"the amount of dimensions for subgait {subgait_name}")
subgaits = {}
for subgait_name in subgait_version_map:
if subgait_name not in ("start", "end"):
subgaits[subgait_name] = Subgait.interpolate_n_subgaits(
dimensions=dimensions,
subgaits=subgaits_to_interpolate[subgait_name],
parameters=parameters,
use_foot_position=True,
)
starting_position = cls.parse_edge_position(
gait_config["starting_position"],
subgaits[graph.start_subgaits()[0]].starting_position,
)
final_position = cls.parse_edge_position(
gait_config["final_position"],
subgaits[graph.end_subgaits()[0]].final_position,
)
except KeyError as e:
raise WrongRealSenseConfigurationError(
f"There was a missing key to create realsense gait in gait {gait_name}:"
f" {e}")
except ValueError as e:
raise WrongRealSenseConfigurationError(
f"There was a wrong value in the config for the realsense gait"
f" {gait_name}: {e}")
return cls(
gait_name=gait_name,
subgaits=subgaits,
graph=graph,
gait_selection=gait_selection,
realsense_category=realsense_category,
camera_to_use=camera_to_use,
subgaits_to_interpolate=subgaits_to_interpolate,
dimensions=dimensions,
process_service=process_service,
starting_position=starting_position,
final_position=final_position,
parameters=parameters,
dependent_on=dependent_on,
responsible_for=responsible_for,
)
@classmethod
def parse_edge_position(cls, config_value: str,
position_values: Dict[str, float]):
"""
Parse the edge position based on the string in the realsense_gaits.yaml.
:param config_value: The value in the yaml file.
:param position_values: The actual joint positions at the edge of the gait.
:return: The edge position to use.
"""
if config_value == "static":
return StaticEdgePosition(position_values)
elif config_value == "dynamic":
return DynamicEdgePosition(position_values)
else:
raise WrongRealSenseConfigurationError(
"The edge position did not have a "
"valid value, should be static or "
f"dynamic, but was `{config_value}`")
@classmethod
def realsense_category_from_string(cls, gait_name: str) -> int:
"""
Construct the realsense gait from the string in the realsense_gaits.yaml.
:param gait_name: The string from the config.
:return: The integer to send to the realsense reader to define the category.
"""
if gait_name not in cls.REALSENSE_CATEGORY_MAP:
raise WrongRealSenseConfigurationError(
f"Gait name {gait_name} from the config is not known as a possible "
f"realsense reader gait configuration")
return cls.REALSENSE_CATEGORY_MAP[gait_name]
@classmethod
def camera_msg_from_string(cls, camera_name: str) -> int:
"""
Construct the camera name msg from the string in the realsense_gaits.yaml.
:param camera_name: The string from the config.
:return: The integer to send to the realsense reader to define the camera.
"""
if camera_name not in cls.CAMERA_NAME_MAP:
raise WrongRealSenseConfigurationError(
f"The camera configuration {camera_name} from the realsense_gaits.yaml"
f"is not one of the known camera names: {cls.CAMERA_NAME_MAP.keys()}"
)
return cls.CAMERA_NAME_MAP[camera_name]
DEFAULT_FIRST_SUBGAIT_DELAY_START_RS_DURATION = Duration(0)
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 get_realsense_update(self):
"""
Makes a realsense service call and handles the result
:return: Whether the call was successful
"""
service_call_succesful = self.make_realsense_service_call()
if not service_call_succesful:
self.logger.warn("No service response received within timeout")
return False
gait_parameters_response = self.realsense_service_result
if gait_parameters_response is None or not gait_parameters_response.success:
self.logger.warn(
"No gait parameters were found, gait will not be started, "
f"{gait_parameters_response}")
return False
return self.update_gaits_from_realsense_call(
gait_parameters_response.gait_parameters)
def update_gaits_from_realsense_call(
self, gait_parameters: GaitParameters) -> bool:
"""
Update the gait parameters based on the message of the current gaits and its
responsibilities.
:param gait_parameters: The parameters to update to.
"""
success = True
self.set_parameters(gait_parameters)
success &= self.interpolate_subgaits_from_parameters()
if self._responsible_for and success:
for gait_name in self._responsible_for:
gait = self._gait_selection.gaits[gait_name]
# Make a recursive call to also handle the dependencies of the
# dependent gait
if isinstance(gait, RealsenseGait):
gait.update_gaits_from_realsense_call(gait_parameters)
return success
def make_realsense_service_call(self) -> bool:
"""
Make a call to the realsense service, if it is available
and returns the response.
:return: Whether the call was successful
"""
if self._current_subgait is not None:
subgait_name = self._current_subgait.subgait_name
else:
# Assume that the gait is starting and use the first subgait name
subgait_name = self.graph.start_subgaits()[0]
request = GetGaitParameters.Request(
realsense_category=self.realsense_category,
camera_to_use=self.camera_to_use,
subgait_name=subgait_name,
)
self.realsense_service_event.clear()
if self._get_gait_parameters_service.wait_for_service(
timeout_sec=self.SERVICE_TIMEOUT.seconds):
gait_parameters_response_future = (
self._get_gait_parameters_service.call_async(request))
gait_parameters_response_future.add_done_callback(
self._realsense_response_cb)
else:
self.logger.error(
f"The service took longer than {self.SERVICE_TIMEOUT} to become "
f"available, is the realsense reader running?")
return False
return self.realsense_service_event.wait(
timeout=self.SERVICE_TIMEOUT.seconds)
def _realsense_response_cb(self, future: Future):
"""Set capture point result when the capture point service returns."""
self.realsense_service_result = future.result()
self.realsense_service_event.set()
def interpolate_subgaits_from_parameters(self) -> bool:
"""Change all subgaits to one interpolated from the current parameters."""
self.logger.info(
f"Interpolating gait {self.gait_name} with parameters:"
f" {self.parameters}")
new_subgaits = {}
for subgait_name in self.subgaits.keys():
new_subgaits[subgait_name] = Subgait.interpolate_n_subgaits(
dimensions=self.dimensions,
subgaits=self.subgaits_to_interpolate[subgait_name],
parameters=self.parameters,
use_foot_position=True,
)
try:
self.set_subgaits(new_subgaits, self._gait_selection)
except NonValidGaitContentError:
return False
return True
def set_parameters(self, gait_parameters: GaitParameters) -> None:
"""
Set the gait parameters based on the message.
:param gait_parameters: The parameters to set.
"""
if self.dimensions == InterpolationDimensions.ONE_DIM:
self.parameters = [gait_parameters.first_parameter]
elif self.dimensions == InterpolationDimensions.TWO_DIM:
self.parameters = [
gait_parameters.first_parameter,
gait_parameters.second_parameter,
]
else:
raise UnknownDimensionsError(self.dimensions)
def set_edge_positions(self, starting_position: EdgePosition,
final_position: EdgePosition):
"""
Set the new edge positions. Overrides from the setpoints gait, which does not
store the starting or final position
:param starting_position: The new starting position
:param final_position: The new final position
"""
self._starting_position = starting_position
self._final_position = final_position