def test_transition_stairs_up_walk_small_right_swing(self):
# Test if the TransitionSubgait is created without an error
walk_small = Subgait.from_name_and_version(self.robot, self.resources,
"walk_small", "right_swing",
"MIV_final")
stairs_up = Subgait.from_name_and_version(self.robot, self.resources,
"stairs_up", "right_swing",
"MIV_final")
TransitionSubgait.from_subgaits(stairs_up, walk_small, "test")
def test_walk_transition_medium_to_small_right_swing(self):
# Test if the TransitionSubgait is created without an error
walk_small = Subgait.from_name_and_version(self.robot, self.resources,
"walk_small", "right_swing",
"MIV_final")
walk_medium = Subgait.from_name_and_version(
self.robot,
self.resources,
"walk_medium",
"right_swing",
"MV_walk_rightswing_v1",
)
TransitionSubgait.from_subgaits(walk_medium, walk_small, "test")
def _freeze_subgait(self):
"""
Generates a subgait of the freeze duration based on the current position.
:return: A subgait to freeze in current position
"""
new_dict = {
"description": "A subgait that stays in the same position",
"duration": self._freeze_duration.nanoseconds,
"gait_type": self._current_subgait.gait_type,
"joints": {
joint.name: [{
"position": self._freeze_position[joint.name],
"time_from_start": self._freeze_duration.nanoseconds,
"velocity": 0,
}]
for joint in self._current_subgait.joints
},
}
# freeze subgait
return Subgait.from_dict(
robot=self._current_subgait.robot,
subgait_dict=new_dict,
gait_name=self.gait_name,
subgait_name="freeze",
version="Only version, generated from code",
)
def from_subgait(subgait: Subgait, start_time: Time):
"""Create a TrajectoryCommand from a subgait."""
return TrajectoryCommand(
subgait.to_joint_trajectory_msg(),
subgait.duration,
subgait.subgait_name,
start_time,
)
def test_no_duplicate_duration_in_transition_subgait(self):
# Test if the TransitionSubgait is created without an error
walk_small = Subgait.from_name_and_version(self.robot, self.resources,
"walk_small", "right_swing",
"MIV_final")
walk_medium = Subgait.from_name_and_version(
self.robot,
self.resources,
"walk_medium",
"right_swing",
"MV_walk_rightswing_v1",
)
transition_subgait = TransitionSubgait.from_subgaits(
walk_small, walk_medium, "test")
for joint in transition_subgait.joints:
durations = [setpoint.time for setpoint in joint.setpoints]
self.assertLessEqual(
len(durations),
len(set(durations)),
msg="Duplicate timestamps found in setpoints.",
)
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 _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 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,
)