def test_subgait_transitions_to_end(self):
graph = SubgaitGraph({
"start": {
"to": "1"
},
"1": {
"to": "end",
"stop": "2"
},
"2": {
"to": "end"
}
})
self.assertListEqual(graph.end_subgaits(), ["1", "2"])
def test_stoppable_graph(self):
graph = SubgaitGraph({
"start": {
"to": "1"
},
"1": {
"to": "2"
},
"2": {
"to": "1",
"stop": "end"
}
})
self.assertTrue(graph.is_stoppable())
def dynamic_from_dict(
cls,
robot: urdf.Robot,
gait_dictionary: dict,
gait_directory: str,
gait_version_map: dict,
start_is_dynamic: bool,
final_is_dynamic: bool,
):
"""Create a new gait object using the .gait and .subgait files.
:param robot:
the robot corresponding to the given .gait file
:param gait_dictionary:
the information of the .gait file as a dictionary
:param gait_directory:
path of the directory where the .gait file is located
:param gait_version_map:
The parsed yaml file which states the version of the subgaits
:return:
If the data in the files is validated a gait object is returned
"""
gait_name = gait_dictionary["name"]
subgaits = gait_dictionary["subgaits"]
graph = SubgaitGraph(subgaits)
subgaits = {
name: cls.load_subgait(robot, gait_directory, gait_name, name,
gait_version_map)
for name in subgaits if name not in ("start", "end")
}
return cls(gait_name, subgaits, graph, start_is_dynamic,
final_is_dynamic)
def test_get_correct_to_transition(self):
subgait = "test"
graph = SubgaitGraph({
"start": {
"to": subgait
},
subgait: {
"to": "end"
}
})
self.assertEqual(graph[(subgait, "to")], "end")
def test_contained_subgait(self):
subgait = "test"
graph = SubgaitGraph({
"start": {
"to": subgait
},
subgait: {
"to": "end"
}
})
self.assertIn(subgait, graph)
def test_iter_transitions(self):
graph = SubgaitGraph({
"start": {
"to": "1"
},
"1": {
"to": "2"
},
"2": {
"to": "1",
"stop": "end"
}
})
self.assertSetEqual(set(iter(graph)), {("start", "1"), ("1", "2"),
("2", "1"), ("2", "end")})
def test_get_correct_stop_transition(self):
subgait = "test"
stop_subgait = "stopping"
graph = SubgaitGraph({
"start": {
"to": subgait
},
subgait: {
"to": "end",
"stop": stop_subgait
},
stop_subgait: {
"to": "end"
},
})
self.assertEqual(graph[(subgait, "stop")], stop_subgait)
def test_iter_transitions_with_increase_decrease(self):
graph = SubgaitGraph({
"start": {
"to": "1"
},
"1": {
"to": "end",
"increase_size": "2",
"decrease_size": "3"
},
"2": {
"to": "end"
},
"3": {
"to": "end"
},
})
self.assertSetEqual(set(iter(graph)), {("start", "1"), ("1", "end"),
("2", "end"), ("3", "end")})
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,
)
def test_valid_graph(self, name, graph):
self.assertIsInstance(SubgaitGraph(graph), SubgaitGraph)
def test_not_equal_graphs(self):
graph1 = SubgaitGraph({"start": {"to": "1"}, "1": {"to": "end"}})
graph2 = SubgaitGraph({"start": {"to": "end"}})
self.assertNotEqual(graph1, graph2)
def test_not_stoppable_graph(self):
graph = SubgaitGraph({"start": {"to": "1"}, "1": {"to": "end"}})
self.assertFalse(graph.is_stoppable())
def test_iter_transitions_without_subgaits(self):
graph = SubgaitGraph({"start": {"to": "end"}})
self.assertSetEqual(set(iter(graph)), {("start", "end")})
def test_get_invalid_transition(self):
graph = SubgaitGraph({"start": {"to": "1"}, "1": {"to": "end"}})
self.assertIsNone(graph[("1", "stop")])
def test_get_no_subgait(self):
graph = SubgaitGraph({"start": {"to": "1"}, "1": {"to": "end"}})
with self.assertRaises(KeyError):
graph.__getitem__(("2", "to"))
def test_invalid_graph(self, name, graph):
with self.assertRaises(SubgaitGraphError):
SubgaitGraph(graph)
def test_subgaits_from_start(self):
graph = SubgaitGraph({"start": {"to": "1"}, "1": {"to": "end"}})
self.assertListEqual(graph.start_subgaits(), ["1"])
def test_not_contained_subgait(self):
graph = SubgaitGraph({"start": {"to": "1"}, "1": {"to": "end"}})
self.assertNotIn("test", graph)