def __init__(
self,
gait_selection_node: Node,
starting_position: dict,
subgait_id: str,
joint_names: List[str],
location: Point,
joint_soft_limits: List[Limits],
stop: bool,
):
self.logger = Logger(gait_selection_node, __class__.__name__)
self._get_parameters(gait_selection_node)
self.time = [
0,
self.push_off_fraction * self.duration,
self.middle_point_fraction * self.duration,
self.duration,
]
self.starting_position = starting_position
self.location = location
self.joint_names = joint_names
self.subgait_id = subgait_id
self.joint_soft_limits = joint_soft_limits
self.stop = stop
self.pose = Pose()
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
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 __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)
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 __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 __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__)
class DynamicSubgait:
"""Creates joint trajectories based on the desired foot location.
:param gait_selection_node: The gait_selection node
:type gait_selection: Node
:param starting_position: The first setpoint of the subgait, usually the last setpoint of the previous subgait.
:type starting_position: dict
:param subgait_id: Whether it is a left_swing or right_swing.
:type subgait_id: str
:param joint_names: Names of the joints
:type joint_names: list
:param position: Desired foot position
:type position: Point
:param joint_soft_limits: list containing soft limits in alphabetical order
:type joint_soft_limits: List[Limits]
:param stop: whether it is a close gait or not
:type stop: bool
"""
def __init__(
self,
gait_selection_node: Node,
starting_position: dict,
subgait_id: str,
joint_names: List[str],
location: Point,
joint_soft_limits: List[Limits],
stop: bool,
):
self.logger = Logger(gait_selection_node, __class__.__name__)
self._get_parameters(gait_selection_node)
self.time = [
0,
self.push_off_fraction * self.duration,
self.middle_point_fraction * self.duration,
self.duration,
]
self.starting_position = starting_position
self.location = location
self.joint_names = joint_names
self.subgait_id = subgait_id
self.joint_soft_limits = joint_soft_limits
self.stop = stop
self.pose = Pose()
def _get_extra_ankle_setpoint(self) -> Setpoint:
"""Returns an extra setpoint for the swing leg ankle
that can be used to create a push off.
:returns: An extra setpoint for the swing leg ankle
:rtype: Setpoint
"""
return Setpoint(
Duration(self.time[SetpointTime.PUSH_OFF_INDEX]),
self.push_off_position,
0.0,
)
def _solve_middle_setpoint(self) -> None:
"""Calls IK solver to compute the joint angles needed for the middle setpoint
:returns: A setpoint_dict for the middle position.
:rtype: dict
"""
middle_position = self.pose.solve_mid_position(
self.location.x,
self.location.y,
self.middle_point_fraction,
self.middle_point_height,
self.subgait_id,
)
self.middle_setpoint_dict = self._from_list_to_setpoint(
self.joint_names,
middle_position,
None,
self.time[SetpointTime.MIDDLE_POINT_INDEX],
)
def _solve_desired_setpoint(self) -> None:
"""Calls IK solver to compute the joint angles needed for the
desired x and y coordinate"""
if self.stop:
self.desired_position = self._from_joint_dict_to_list(
get_position_from_yaml("stand")
)
else:
self.desired_position = self.pose.solve_end_position(
self.location.x, self.location.y, self.subgait_id
)
self.desired_setpoint_dict = self._from_list_to_setpoint(
self.joint_names,
self.desired_position,
None,
self.time[SetpointTime.END_POINT_INDEX],
)
def _to_joint_trajectory_class(self) -> None:
"""Creates a list of DynamicJointTrajectories for each joint"""
self.joint_trajectory_list = []
for name in self.joint_names:
setpoint_list = [
self.starting_position[name],
self.middle_setpoint_dict[name],
self.desired_setpoint_dict[name],
]
# Add an extra setpoint to the ankle to create a push off:
if (name == "right_ankle" and self.subgait_id == "right_swing") or (
name == "left_ankle" and self.subgait_id == "left_swing"
):
setpoint_list.insert(
EXTRA_ANKLE_SETPOINT_INDEX, self._get_extra_ankle_setpoint()
)
self.joint_trajectory_list.append(DynamicJointTrajectory(setpoint_list))
def get_joint_trajectory_msg(self) -> trajectory_msg.JointTrajectory:
"""Return a joint_trajectory_msg containing the interpolated
trajectories for each joint
:returns: A joint_trajectory_msg
:rtype: joint_trajectory_msg
"""
# Update pose:
pose_list = [joint.position for joint in self.starting_position.values()]
self.pose = Pose(pose_list)
self._solve_middle_setpoint()
self._solve_desired_setpoint()
self._get_extra_ankle_setpoint()
# Create joint_trajectory_msg
self._to_joint_trajectory_class()
joint_trajectory_msg = trajectory_msg.JointTrajectory()
joint_trajectory_msg.joint_names = self.joint_names
timestamps = np.linspace(self.time[0], self.time[-1], INTERPOLATION_POINTS)
for timestamp in timestamps:
joint_trajecory_point = trajectory_msg.JointTrajectoryPoint()
joint_trajecory_point.time_from_start = Duration(timestamp).to_msg()
for joint_index, joint_trajectory in enumerate(self.joint_trajectory_list):
interpolated_setpoint = joint_trajectory.get_interpolated_setpoint(
timestamp
)
joint_trajecory_point.positions.append(interpolated_setpoint.position)
joint_trajecory_point.velocities.append(interpolated_setpoint.velocity)
self._check_joint_limits(joint_index, joint_trajecory_point)
joint_trajectory_msg.points.append(joint_trajecory_point)
return joint_trajectory_msg
def get_final_position(self) -> dict:
"""Get setpoint_dictionary of the final setpoint.
:return: The final setpoint of the subgait.
:rtype: dict
"""
return self._from_list_to_setpoint(
self.joint_names,
self.desired_position,
None,
self.time[SetpointTime.START_INDEX],
)
def _from_list_to_setpoint(
self,
joint_names: List[str],
position: List[float],
velocity: List[float],
time: float,
) -> dict:
"""Computes setpoint_dictionary from a list
:param joint_names: Names of the joints.
:type joint_names: list
:param position: Positions for each joint.
:type position: list
:param velocity: Optional velocities for each joint. If None, velocity will be set to zero.
:type velocity: list
:param time: Time at which the setpoint should be set.
:type time: float
:returns: A Setpoint_dict containing time, position and velocity for each joint
:rtype: dict
"""
setpoint_dict = {}
velocity = np.zeros_like(position) if (velocity is None) else velocity
for i in range(len(joint_names)):
setpoint_dict.update(
{
joint_names[i]: Setpoint(
Duration(time),
position[i],
velocity[i],
)
}
)
return setpoint_dict
def _from_joint_dict_to_list(self, joint_dict: dict) -> List[float]:
"""Return the values in a joint_dict as a list."""
return list(joint_dict.values())
def _get_parameters(self, gait_selection_node: Node) -> None:
"""Gets the dynamic gait parameters from the gait_selection_node
:param gait_selection_node: the gait selection node
:type gait_selection_node: Node
"""
self.duration = gait_selection_node.dynamic_subgait_duration
self.middle_point_height = gait_selection_node.middle_point_height
self.middle_point_fraction = gait_selection_node.middle_point_fraction
self.push_off_fraction = gait_selection_node.push_off_fraction
self.push_off_position = gait_selection_node.push_off_position
def _check_joint_limits(
self,
joint_index: int,
joint_trajectory_point: trajectory_msg.JointTrajectoryPoint,
) -> None:
"""Check if values in the joint_trajectory_point are within the soft and
velocity limits defined in the urdf
:param joint_index: Index of the joint in the alphabetical joint_names list
:type joint_index: int
:param joint_trajectory_point: point in time containing position and velocity
:type joint_trajectory_point: trajectory_msg.JointTrajectoryPoint
"""
position = joint_trajectory_point.positions[joint_index]
velocity = joint_trajectory_point.velocities[joint_index]
if (
position > self.joint_soft_limits[joint_index].upper
or position < self.joint_soft_limits[joint_index].lower
):
self.logger.info(
f"DynamicSubgait: {self.joint_names[joint_index]} will be outside of soft limits, "
f"position: {position}, soft limits: "
f"[{self.joint_soft_limits[joint_index].lower}, {self.joint_soft_limits[joint_index].upper}]."
)
raise Exception(
f"{self.joint_names[joint_index]} will be outside its soft limits."
)
if abs(velocity) > self.joint_soft_limits[joint_index].velocity:
self.logger.info(
f"DynamicSubgait: {self.joint_names[joint_index]} will be outside of velocity limits, "
f"velocity: {velocity}, velocity limit: {self.joint_soft_limits[joint_index].velocity}."
)
raise Exception(
f"{self.joint_names[joint_index]} will be outside its velocity limits."
)
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.")
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 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 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 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 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 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
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)
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")
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