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 GaitSelection(Node):
"""Base class for the gait selection module."""
def __init__(
self,
gait_package=None,
directory=None,
robot=None,
balance=None,
dynamic_gait=None,
):
super().__init__(NODE_NAME,
automatically_declare_parameters_from_overrides=True)
self.logger = Logger(self, __class__.__name__)
self._balance_used = False
self._dynamic_gait = False
try:
# Initialize all parameters once, and set up a callback for dynamically
# reconfiguring
if gait_package is None:
gait_package = (self.get_parameter(
"gait_package").get_parameter_value().string_value)
if directory is None:
directory = (self.get_parameter(
"gait_directory").get_parameter_value().string_value)
if balance is None:
self._balance_used = (self.get_parameter(
"balance").get_parameter_value().bool_value)
if dynamic_gait is None:
self._dynamic_gait = (self.get_parameter(
"dynamic_gait").get_parameter_value().bool_value)
self._early_schedule_duration = self._parse_duration_parameter(
"early_schedule_duration")
self._first_subgait_delay = self._parse_duration_parameter(
"first_subgait_delay")
# Setting dynamic gait parameters
self.dynamic_subgait_duration = (self.get_parameter(
"dynamic_subgait_duration").get_parameter_value().double_value)
self.middle_point_fraction = (self.get_parameter(
"middle_point_fraction").get_parameter_value().double_value)
self.middle_point_height = (self.get_parameter(
"middle_point_height").get_parameter_value().double_value)
self.minimum_stair_height = (self.get_parameter(
"minimum_stair_height").get_parameter_value().double_value)
self.push_off_fraction = (self.get_parameter(
"push_off_fraction").get_parameter_value().double_value)
self.push_off_position = (self.get_parameter(
"push_off_position").get_parameter_value().double_value)
except ParameterNotDeclaredException:
self.logger.error(
"Gait selection node started without required parameters "
"gait_package, gait_directory and balance")
self._directory_name = directory
self._gait_package = gait_package
self._gait_directory, self._default_yaml = self._initialize_gaits()
if not os.path.isdir(self._gait_directory):
self.logger.error(f"Gait directory does not exist: {directory}")
raise FileNotFoundError(directory)
if not os.path.isfile(self._default_yaml):
self.logger.error(
f"Gait default yaml file does not exist: {directory}/default.yaml"
)
self._robot = get_robot_urdf_from_service(
self) if robot is None else robot
self._joint_names = sorted(get_joint_names_from_robot(self._robot))
self._realsense_yaml = os.path.join(self._gait_directory,
"realsense_gaits.yaml")
self._realsense_gait_version_map = self._load_realsense_configuration()
(
self._gait_version_map,
self._positions,
self._dynamic_edge_version_map,
) = self._load_configuration()
self._robot_description_sub = self.create_subscription(
msg_type=String,
topic="/march/robot_description",
callback=self._update_robot_description_cb,
qos_profile=DEFAULT_HISTORY_DEPTH,
)
self._create_services()
self._gaits = self._load_gaits()
self._early_schedule_duration = self._parse_duration_parameter(
"early_schedule_duration")
self._first_subgait_delay = self._parse_duration_parameter(
"first_subgait_delay")
if not self._validate_inverse_kinematics_is_possible():
self.logger.warning(
"The currently available joints are unsuitable for "
"using inverse kinematics.\n"
"Any interpolation on foot_location will return "
"the base subgait instead. Realsense gaits will "
"not be loaded.")
self.logger.info("Successfully initialized gait selection node.")
@property
def joint_names(self):
return self._joint_names
@property
def gaits(self):
return self._gaits
def _validate_inverse_kinematics_is_possible(self):
return (validate_and_get_joint_names_for_inverse_kinematics(
self.logger) is not None)
def _initialize_gaits(self):
package_path = get_package_share_directory(self._gait_package)
gait_directory = os.path.join(package_path, self._directory_name)
default_yaml = os.path.join(gait_directory, "default.yaml")
if not os.path.isdir(gait_directory):
self.logger.error(f"Gait directory does not exist: "
f"{gait_directory}")
if not os.path.isfile(default_yaml):
self.logger.error(f"Gait default yaml file does not exist: "
f"{gait_directory}/default.yaml")
return gait_directory, default_yaml
def update_gaits(self):
"""
Update the gaits after one of the gait attributes has been changed.
"""
self._gait_directory, self._default_yaml = self._initialize_gaits()
self._realsense_yaml = os.path.join(self._gait_directory,
"realsense_gaits.yaml")
self._realsense_gait_version_map = self._load_realsense_configuration()
(
self._gait_version_map,
self._positions,
self._semi_dynamic_gait_version_map,
) = self._load_configuration()
self._loaded_gaits = self._load_gaits()
def _create_services(self) -> None:
self.create_service(
srv_type=Trigger,
srv_name="/march/gait_selection/get_version_map",
callback=lambda req, res: Trigger.Response(
success=True, message=str(self.gait_version_map)),
)
self.create_service(
srv_type=Trigger,
srv_name="/march/gait_selection/get_gait_directory",
callback=lambda req, res: Trigger.Response(
success=True, message=self._directory_name),
)
self.create_service(
srv_type=Trigger,
srv_name="/march/gait_selection/get_default_dict",
callback=self.get_default_dict_cb,
)
self.create_service(
srv_type=SetGaitVersion,
srv_name="/march/gait_selection/set_gait_version",
callback=self.set_gait_versions_cb,
)
self.create_service(
srv_type=Trigger,
srv_name="/march/gait_selection/get_directory_structure",
callback=lambda req, res: Trigger.Response(
success=True, message=str(self.scan_directory())),
)
self.create_service(
srv_type=ContainsGait,
srv_name="/march/gait_selection/contains_gait",
callback=self.contains_gait_cb,
)
def _parse_duration_parameter(self, name: str) -> Duration:
"""Get a duration parameter from the parameter server.
Returns 0 if the parameter does not exist.
Clamps the duration to 0 if it is negative.
"""
if self.has_parameter(name):
value = self.get_parameter(name).value
if value < 0:
value = 0
return Duration(seconds=value)
else:
return Duration(0)
def shortest_subgait(self) -> Subgait:
"""Get the subgait with the smallest duration of all subgaits in the loaded gaits."""
shortest_subgait = None
for gait in self._gaits.values():
for subgait in gait.subgaits.values():
if (shortest_subgait is None
or subgait.duration < shortest_subgait.duration):
shortest_subgait = subgait
return shortest_subgait
@property
def robot(self):
"""Return the robot obtained from the robot state publisher."""
return self._robot
@property
def gait_version_map(self):
"""Returns the mapping from gaits and subgaits to versions."""
return self._gait_version_map
@property
def positions(self):
"""Returns the named idle positions."""
return self._positions
def _update_robot_description_cb(self, msg):
"""
Callback that is used to update the robot description when
robot_state_publisher sends out an update.
"""
self._robot = urdf.Robot.from_xml_string(msg.data)
def set_gait_versions(self, gait_name, version_map):
"""Sets the subgait versions of given gait.
:param str gait_name: Name of the gait to change versions
:param dict version_map: Mapping subgait names to versions
"""
if gait_name not in self._gaits:
raise GaitNameNotFoundError(gait_name)
# Only update versions that are different
version_map = {
name: version
for name, version in version_map.items()
if version != self._gait_version_map[gait_name][name]
}
self._gaits[gait_name].set_subgait_versions(self._robot,
self._gait_directory,
version_map)
self._gait_version_map[gait_name].update(version_map)
self.logger.info(
f"Setting gait versions successful: {self._gaits[gait_name]}")
def set_gait_versions_cb(self, request, response):
"""Sets a new gait version to the gait selection instance.
:type msg: march_shared_resources.srv.SetGaitVersionRequest
:rtype march_shared_resources.srv.SetGaitVersionResponse
"""
if len(request.subgaits) != len(request.versions):
return [
False,
"`subgaits` and `versions` array are not of equal length"
]
version_map = dict(zip(request.subgaits, request.versions))
try:
self.logger.info(f"Setting gait versions from {request}")
self.set_gait_versions(request.gait, version_map)
response.success = True
response.message = ""
return response
except Exception as e: # noqa: PIE786
response.success = False
response.message = str(e)
return response
def contains_gait_cb(self, request, response):
"""
Checks whether a gait and subgait are loaded.
:type request: ContainsGaitRequest
:param request: service request
:return: True when the gait and subgait are loaded
"""
gait = self._gaits.get(request.gait)
if gait is None:
response.contains = False
return response
response.contains = True
for subgait in request.subgaits:
if gait[subgait] is None:
response.contains = False
return response
def scan_directory(self):
"""Scans the gait_directory recursively and create a dictionary of all
subgait files.
:returns:
dictionary of the maps and files within the directory
"""
gaits = {}
for gait in os.listdir(self._gait_directory):
gait_path = os.path.join(self._gait_directory, gait)
if os.path.isdir(gait_path):
subgaits = {}
for subgait in os.listdir(gait_path):
subgait_path = os.path.join(gait_path, subgait)
if os.path.isdir(subgait_path):
versions = sorted([
v.replace(".subgait", "")
for v in os.listdir(os.path.join(subgait_path))
if v.endswith(".subgait")
])
subgaits[subgait] = versions
gaits[gait] = subgaits
return gaits
def get_default_dict_cb(self, req, res):
defaults = {
"gaits": self._gait_version_map,
"positions": self._positions
}
return Trigger.Response(success=True, message=str(defaults))
def add_gait(self, gait):
"""Adds a gait to the loaded gaits if it does not already exist.
The to be added gait should implement `GaitInterface`.
"""
if gait.name in self._gaits:
self.logger.warning(
"Gait `{gait}` already exists in gait selection".format(
gait=gait.name))
else:
self._gaits[gait.name] = gait
def _load_gaits(self):
"""Loads the gaits in the specified gait directory.
:returns dict: A dictionary mapping gait name to gait instance
"""
gaits = {}
for gait in self._gait_version_map:
gaits[gait] = SetpointsGait.from_file(gait, self._gait_directory,
self._robot,
self._gait_version_map)
for gait in self._dynamic_edge_version_map:
self.logger.debug(f"Adding dynamic gait {gait}")
start_is_dynamic = self._dynamic_edge_version_map[gait].pop(
"start_is_dynamic", True)
final_is_dynamic = self._dynamic_edge_version_map[gait].pop(
"final_is_dynamic", True)
gaits[gait] = DynamicEdgeSetpointsGait.dynamic_from_file(
gait,
self._gait_directory,
self._robot,
self._dynamic_edge_version_map,
start_is_dynamic,
final_is_dynamic,
)
self._gait_version_map[gait] = self._dynamic_edge_version_map[gait]
self._load_realsense_gaits(gaits)
if self._balance_used and "balance_walk" in gaits:
balance_gait = BalanceGait(node=self,
default_walk=gaits["balance_walk"])
if balance_gait is not None:
self.logger.info("Successfully created a balance gait")
gaits["balanced_walk"] = balance_gait
if self._dynamic_gait:
# We pass along the gait_selection_node to be able to listen
# to the CoViD topic wihtin the DynamicSetpointGait class.
self.dynamic_setpoint_gait = DynamicSetpointGait(
gait_selection_node=self)
gaits["dynamic_walk"] = self.dynamic_setpoint_gait
self.logger.info("Added dynamic_walk to gaits")
return gaits
def _load_realsense_gaits(self, gaits):
"""
Load all gaits from the realsense gait version map.
Also create a service with a separate callback group that can be used by the
realsense gaits to get parameters from the realsense_reader. A new callback
group is necessary to prevent a deadlock.
:param gaits: The dictionary where the loaded gaits will be added to.
"""
if not self._validate_inverse_kinematics_is_possible():
return
get_gait_parameters_service = self.create_client(
srv_type=GetGaitParameters,
srv_name="/camera/process_pointcloud",
callback_group=MutuallyExclusiveCallbackGroup(),
)
for gait_name in self._realsense_gait_version_map:
gait_folder = gait_name
gait_path = os.path.join(self._gait_directory, gait_folder,
gait_name + ".gait")
with open(gait_path, "r") as gait_file:
gait_graph = yaml.load(gait_file,
Loader=yaml.SafeLoader)["subgaits"]
gait = RealsenseGait.from_yaml(
gait_selection=self,
robot=self._robot,
gait_name=gait_name,
gait_config=self._realsense_gait_version_map[gait_name],
gait_graph=gait_graph,
gait_directory=self._gait_directory,
process_service=get_gait_parameters_service,
)
gaits[gait_name] = gait
def _load_realsense_configuration(self):
if not os.path.isfile(self._realsense_yaml):
self.logger.info(
"No realsense_yaml present, no realsense gaits will be created."
)
return {}
with open(self._realsense_yaml, "r") as realsense_config_file:
return yaml.load(realsense_config_file, Loader=yaml.SafeLoader)
def _load_configuration(self):
"""Loads and verifies the gaits configuration."""
with open(self._default_yaml, "r") as default_yaml_file:
default_config = yaml.load(default_yaml_file,
Loader=yaml.SafeLoader)
version_map = default_config["gaits"]
dynamic_edge_version_map = {}
if "dynamic_edge_gaits" in default_config:
dynamic_edge_version_map = default_config["dynamic_edge_gaits"]
if not isinstance(version_map, dict):
raise TypeError("Gait version map should be of type; dictionary")
if not self._validate_version_map(version_map):
raise GaitError(msg="Gait version map: {gm}, is not valid".format(
gm=version_map))
positions = {}
for position_name, position_values in default_config[
"positions"].items():
positions[position_name] = {
"gait_type": position_values["gait_type"],
"joints": {},
}
for joint, joint_value in position_values["joints"].items():
if joint in self._joint_names:
positions[position_name]["joints"][joint] = joint_value
if set(positions[position_name]["joints"].keys()) != set(
self._joint_names):
raise NonValidGaitContentError(
f"The position {position_name} does not "
f"have a position for all required joints: it "
f"has {positions[position_name]['joints'].keys()}, "
f"required: {self._joint_names}")
return version_map, positions, dynamic_edge_version_map
def _validate_version_map(self, version_map):
"""Validates if the current versions exist.
:param dict version_map: Version map to verify
:returns bool: True when all versions exist, False otherwise
"""
for gait_name in version_map:
gait_path = os.path.join(self._gait_directory, gait_name)
if not os.path.isfile(os.path.join(gait_path,
gait_name + ".gait")):
self.logger.warning(
"gait {gn} does not exist".format(gn=gait_name))
return False
for subgait_name in version_map[gait_name]:
version = version_map[gait_name][subgait_name]
if not Subgait.validate_version(gait_path, subgait_name,
version):
self.logger.warning("{0}, {1} does not exist".format(
subgait_name, version))
return False
return True
def __getitem__(self, name):
"""Returns a gait from the loaded gaits."""
return self._gaits.get(name)
def __iter__(self):
"""Returns an iterator over all loaded gaits."""
return iter(self._gaits.values())
class DynamicSetpointGait(GaitInterface):
"""Gait built up from dynamic setpoints
:param gait_selection_node: the gait selection node
:type gait_selection_node: Node
"""
def __init__(self, gait_selection_node: Node):
super(DynamicSetpointGait, self).__init__()
self.gait_selection = gait_selection_node
self._reset()
self.joint_names = get_joint_names_from_urdf()
self._get_soft_limits()
self.start_position = self._joint_dict_to_setpoint_dict(
get_position_from_yaml("stand"))
self.end_position = self.start_position
self.gait_name = "dynamic_walk"
# Create subscribers and publishers for CoViD
self.gait_selection.create_subscription(
PointStamped,
"/foot_position/right",
self._callback_right,
DEFAULT_HISTORY_DEPTH,
)
self.gait_selection.create_subscription(
PointStamped,
"/foot_position/left",
self._callback_left,
DEFAULT_HISTORY_DEPTH,
)
self.publisher_position_right = self.gait_selection.create_publisher(
PointStamped,
"/chosen_foot_position/right",
DEFAULT_HISTORY_DEPTH,
)
self.publisher_position_left = self.gait_selection.create_publisher(
PointStamped,
"/chosen_foot_position/left",
DEFAULT_HISTORY_DEPTH,
)
# Assign reconfigurable parameters
self.update_parameters()
self.logger = Logger(self.gait_selection, __class__.__name__)
@property
def name(self) -> str:
return self.gait_name
@property
def subgait_name(self) -> str:
# Should return left_swing/right_swing for simulation to work
return self.subgait_id
@property
def version(self) -> str:
return "v0"
@property
def duration(self) -> Duration:
if self._next_command is not None:
return self._next_command.duration
else:
return None
@property
def gait_type(self) -> str:
# Return gait type based on height of desired foot location
if self._next_command is not None:
if (self.foot_location.point.y > self.minimum_stair_height
or self.foot_location.point.y < self.minimum_stair_height):
return "stairs_like"
else:
return "walk_like"
else:
return None
@property
def starting_position(self) -> EdgePosition:
return StaticEdgePosition(
self._setpoint_dict_to_joint_dict(self.start_position))
@property
def final_position(self) -> EdgePosition:
# Beunmethod to fix transitions, should be fixed
if self._next_command is not None:
return StaticEdgePosition(
self._setpoint_dict_to_joint_dict(
self.dynamic_subgait.get_final_position()))
else:
return StaticEdgePosition(
self._setpoint_dict_to_joint_dict(self.end_position))
@property
def subsequent_subgaits_can_be_scheduled_early(self) -> bool:
return True
@property
def first_subgait_can_be_scheduled_early(self) -> bool:
return True
def _reset(self) -> None:
"""Reset all attributes of the gait"""
self._should_stop = False
self._end = False
self._start_time = None
self._end_time = None
self._current_time = None
self.subgait_id = "right_swing"
self._next_command = None
self._start_is_delayed = True
self._scheduled_early = False
DEFAULT_FIRST_SUBGAIT_START_DELAY = Duration(0)
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)
DEFAULT_EARLY_SCHEDULE_UPDATE_DURATION = Duration(0)
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 stop(self) -> bool:
"""Called when the current gait should be stopped"""
self._should_stop = True
return True
def end(self) -> None:
"""Called when the gait is finished"""
self._next_command = None
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_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_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 _get_next_command(self) -> TrajectoryCommand:
"""Create the next command, based on what the current subgait is.
Also checks if the gait has to be stopped. If true, it returns
a close gait.
:returns: A TrajectoryCommand for the next subgait
:rtype: TrajectoryCommand
"""
if self.subgait_id == "right_swing":
self.subgait_id = "left_swing"
elif self.subgait_id == "left_swing":
self.subgait_id = "right_swing"
if self._end:
# If the gait has ended, the next command should be None
return None
elif self._should_stop:
return self._get_trajectory_command(stop=True)
else:
return self._get_trajectory_command()
def _update_start_pos(self) -> None:
"""Update the start position of the next subgait to be
the last position of the previous subgait."""
self.start_position = self.dynamic_subgait.get_final_position()
def _callback_right(self, foot_location: PointStamped) -> None:
"""Update the right foot position with the latest point published
on the CoViD-topic.
:param foot_location: a Point containing the x, y and z location
:type foot_location: PointStamped
"""
self.foot_location_right = foot_location
def _callback_left(self, foot_location: PointStamped) -> None:
"""Update the left foot position with the latest point published
on the CoViD-topic.
:param foot_location: a Point containing the x, y and z location
:type foot_location: PointStamped
"""
self.foot_location_left = foot_location
def _get_foot_location(self, subgait_id: str) -> PointStamped:
"""Returns the right or left foot position based upon the subgait_id
:param subgait_id: either right_swing or left_swing
:type subgait_id: str
:return: either the left or right foot position or none
:rtype: PointStamped
"""
if subgait_id == "left_swing":
return self.foot_location_left
elif subgait_id == "right_swing":
return self.foot_location_right
else:
return None
def _get_trajectory_command(self, stop=False) -> TrajectoryCommand:
"""Return a TrajectoryCommand based on current subgait_id
:return: TrajectoryCommand with the current subgait and start time.
:rtype: TrajectoryCommand
"""
if self._start_is_delayed:
self._end_time = self._start_time
if stop:
self._end = True
self.logger.info("Stopping dynamic gait.")
else:
self.foot_location = self._get_foot_location(self.subgait_id)
stop = self._check_msg_time(self.foot_location)
self._publish_foot_location(self.subgait_id, self.foot_location)
self.logger.info(
f"Stepping to location ({self.foot_location.point.x}, {self.foot_location.point.y})"
)
self.dynamic_subgait = DynamicSubgait(
self.gait_selection,
self.start_position,
self.subgait_id,
self.joint_names,
self.foot_location.point,
self.joint_soft_limits,
stop,
)
trajectory = self.dynamic_subgait.get_joint_trajectory_msg()
return TrajectoryCommand(
trajectory,
Duration(self.dynamic_subgait_duration),
self.subgait_id,
self._end_time,
)
def _update_time_stamps(self, next_command_duration: Duration) -> None:
"""Update the starting and end time
:param next_command_duration: Duration of the next command to be scheduled.
:type next_command_duration: Duration
"""
self._start_time = self._end_time
self._end_time = self._start_time + next_command_duration
def update_parameters(self) -> None:
"""Callback for gait_selection_node when the parameters have been updated."""
self.dynamic_subgait_duration = self.gait_selection.dynamic_subgait_duration
self.minimum_stair_height = self.gait_selection.minimum_stair_height
def _publish_foot_location(self, subgait_id: str,
foot_location: PointStamped) -> None:
"""Publish the foot location to which we are stepping for confirmation towards CoViD"""
if subgait_id == "left_swing":
self.publisher_position_left.publish(foot_location)
elif subgait_id == "right_swing":
self.publisher_position_right.publish(foot_location)
# UTILITY FUNCTIONS
@staticmethod
def _setpoint_dict_to_joint_dict(setpoint_dict: dict) -> dict:
"""Creates a joint_dict from a setpoint_dict.
:param setpoint_dict: A dictionary containing joint names and setpoints.
:type: dict
:returns: A dictionary containing joint names and positions.
:rtype: dict
"""
joint_dict = {}
for name, setpoint in setpoint_dict.items():
joint_dict[name] = setpoint.position
return joint_dict
@staticmethod
def _joint_dict_to_setpoint_dict(joint_dict: dict) -> dict:
"""Creates a setpoint_dict from a joint_dict.
:param joint_dict: A dictionary containing joint names and positions.
:type: dict
:returns: A dictionary containing joint names and setpoints.
:rtype: dict
"""
setpoint_dict = {}
for name, position in joint_dict.items():
setpoint_dict[name] = Setpoint(Duration(0), position, 0)
return setpoint_dict
def _get_soft_limits(self):
"""Get the limits of all joints in the urdf"""
self.joint_soft_limits = []
for joint_name in self.joint_names:
self.joint_soft_limits.append(
get_limits_robot_from_urdf_for_inverse_kinematics(joint_name))
# SAFETY
def _check_msg_time(self, foot_location: PointStamped) -> bool:
"""Checks if the foot_location given by CoViD is not older than
FOOT_LOCATION_TIME_OUT."""
msg_time = Time(
seconds=foot_location.header.stamp.sec,
nanoseconds=foot_location.header.stamp.nanosec,
)
current_time = Time(
seconds=self.gait_selection.get_clock().now().seconds_nanoseconds(
)[0],
nanoseconds=self.gait_selection.get_clock().now().
seconds_nanoseconds()[1],
)
time_difference = current_time - msg_time
self.logger.debug(
"Time difference between CoViD foot location and current time: "
f"{time_difference}.", )
if time_difference > FOOT_LOCATION_TIME_OUT:
self.logger.info(
"Foot location is more than 0.5 seconds old, time difference is "
f"{time_difference}. Stopping gait.", )
self._end = True
return True
return False
class StateMachineInput:
def __init__(self, node):
self._stopped = False
self._paused = False
self._unknown = False
self._transition_index = 0
self._gait = None
self._node = node
self.logger = Logger(self._node, __class__.__name__)
self._instruction_subscriber = node.create_subscription(
msg_type=GaitInstruction,
topic="/march/input_device/instruction",
callback=self._callback_input_device_instruction,
qos_profile=10,
)
self._instruction_response_publisher = node.create_publisher(
msg_type=GaitInstructionResponse,
topic="/march/input_device/instruction_response",
qos_profile=20,
)
def get_transition_request(self):
"""Used to return the transition request as an enum.
:returns TransitionRequest
"""
if self._transition_index == GaitInstruction.INCREMENT_STEP_SIZE:
return TransitionRequest.INCREASE_SIZE
if self._transition_index == GaitInstruction.DECREMENT_STEP_SIZE:
return TransitionRequest.DECREASE_SIZE
return TransitionRequest.NONE
def stop_requested(self):
"""Returns True when the current gait should stop, otherwise False."""
return self._stopped
def pause_requested(self):
"""Returns True when the input requests to pause the current gait, otherwise False."""
return self._paused
def unknown_requested(self):
"""Returns True when the input requests to transition to the UNKNOWN state, otherwise False."""
return self._unknown
def transition_requested(self):
"""Returns True when the input requests a gait transition, otherwise False."""
return self._transition_index != 0
def gait_requested(self):
"""Returns True when the input has a gait selected, otherwise False."""
return self._gait is not None
def gait_name(self):
"""Returns a name of the gait that has been selected, if one was selected, otherwise None."""
return self._gait
def reset(self):
"""Resets the input state to its original state on init."""
self._stopped = False
self._paused = False
self._unknown = False
self._transition_index = 0
self._gait = None
def stop_accepted(self):
self._stopped = False
def stop_rejected(self):
self._stopped = False
def gait_accepted(self):
"""Callback called when the state machine accepts the requested gait."""
response = GaitInstructionResponse(
result=GaitInstructionResponse.GAIT_ACCEPTED)
self._instruction_response_publisher.publish(response)
self.reset()
def gait_rejected(self):
"""Callback called when the state machine rejects the requested gait."""
response = GaitInstructionResponse(
result=GaitInstructionResponse.GAIT_REJECTED)
self._instruction_response_publisher.publish(response)
self.reset()
def gait_finished(self):
"""Callback called when the state machine finishes a gait."""
response = GaitInstructionResponse(
result=GaitInstructionResponse.GAIT_FINISHED)
self._instruction_response_publisher.publish(response)
self.reset()
def _callback_input_device_instruction(self, msg):
self.logger.debug(f"Callback input device instruction {msg}")
if msg.type == GaitInstruction.STOP:
self._stopped = True
elif msg.type == GaitInstruction.GAIT:
self._gait = msg.gait_name
elif msg.type == GaitInstruction.PAUSE:
self._paused = True
elif msg.type == GaitInstruction.CONTINUE:
self._paused = False
elif msg.type == GaitInstruction.INCREMENT_STEP_SIZE:
self._transition_index = GaitInstruction.INCREMENT_STEP_SIZE
elif msg.type == GaitInstruction.DECREMENT_STEP_SIZE:
self._transition_index = GaitInstruction.DECREMENT_STEP_SIZE
elif msg.type == GaitInstruction.UNKNOWN:
self._unknown = True
class TrajectoryScheduler:
"""Scheduler that sends sends the wanted trajectories to the topic listened
to by the exoskeleton/simulation."""
def __init__(self, node):
self._failed = False
self._node = node
self._goals: List[TrajectoryCommand] = []
self.logger = Logger(self._node, __class__.__name__)
# Temporary solution to communicate with ros1 action server, should
# be updated to use ros2 action implementation when simulation is
# migrated to ros2
self._trajectory_goal_pub = self._node.create_publisher(
msg_type=FollowJointTrajectoryActionGoal,
topic="/march/controller/trajectory/follow_joint_trajectory/goal",
qos_profile=TRAJECTORY_SCHEDULER_HISTORY_DEPTH,
)
self._cancel_pub = self._node.create_publisher(
msg_type=GoalID,
topic="/march/controller/trajectory/follow_joint_trajectory/cancel",
qos_profile=TRAJECTORY_SCHEDULER_HISTORY_DEPTH,
)
self._trajectory_goal_result_sub = self._node.create_subscription(
msg_type=FollowJointTrajectoryActionResult,
topic="/march/controller/trajectory/follow_joint_trajectory/result",
callback=self._done_cb,
qos_profile=TRAJECTORY_SCHEDULER_HISTORY_DEPTH,
)
# Publisher for sending hold position mode
self._trajectory_command_pub = self._node.create_publisher(
msg_type=JointTrajectory,
topic="/march/controller/trajectory/command",
qos_profile=TRAJECTORY_SCHEDULER_HISTORY_DEPTH,
)
def schedule(self, command: TrajectoryCommand):
"""Schedules a new trajectory.
:param TrajectoryCommand command: The trajectory command to schedule
"""
self._failed = False
stamp = command.start_time.to_msg()
command.trajectory.header.stamp = stamp
goal = FollowJointTrajectoryGoal(trajectory=command.trajectory)
self._trajectory_goal_pub.publish(
FollowJointTrajectoryActionGoal(
header=Header(stamp=stamp),
goal_id=GoalID(stamp=stamp, id=str(command)),
goal=goal,
)
)
info_log_message = f"Scheduling {command.name}"
debug_log_message = f"Subgait {command.name} starts "
if self._node.get_clock().now() < command.start_time:
time_difference = Duration.from_ros_duration(
command.start_time - self._node.get_clock().now()
)
debug_log_message += f"in {round(time_difference.seconds, 3)}s"
else:
debug_log_message += "now"
self._goals.append(command)
self.logger.info(info_log_message)
self.logger.debug(debug_log_message)
def cancel_active_goals(self):
now = self._node.get_clock().now()
for goal in self._goals:
if goal.start_time + goal.duration > now:
self._cancel_pub.publish(
GoalID(stamp=goal.start_time.to_msg(), id=str(goal))
)
def send_position_hold(self):
self._trajectory_command_pub.publish(JointTrajectory())
def failed(self) -> bool:
return self._failed
def reset(self):
self._failed = False
self._goals = []
def _done_cb(self, result):
if result.result.error_code != FollowJointTrajectoryResult.SUCCESSFUL:
self.logger.error(
f"Failed to execute trajectory. {result.result.error_string} ({result.result.error_code})"
)
self._failed = True