def test_subgaits_from_start(self):
graph = SubgaitGraph({"start": {"to": "1"}, "1": {"to": "end"}})
self.assertListEqual(graph.start_subgaits(), ["1"])
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,
)