def test_interpolate_subgaits_wrong_parameter(self):
# should be 0 <= parameter <= 1
base_subgait, other_subgait = self.load_interpolatable_subgaits_ik()
with self.assertRaises(ValueError):
Subgait.interpolate_subgaits(base_subgait,
other_subgait,
2,
use_foot_position=True)
def __init__(self, view, robot):
self.view = view
# Default values
self.gait_directory = None
self.playback_speed = 100
self.time_slider_thread = None
self.current_time = 0
self.robot = robot
empty_subgait_file = os.path.join(
rospkg.RosPack().get_path('march_rqt_gait_generator'),
'resource/empty_gait/empty_subgait/empty_subgait.subgait')
self.subgait = ModifiableSubgait.from_file(robot, empty_subgait_file,
self)
self.settings_changed_history = RingBuffer(capacity=100, dtype=list)
self.settings_changed_redo_list = RingBuffer(capacity=100, dtype=list)
standing = Subgait.from_file(robot, empty_subgait_file)
previous_subgait_controller = SideSubgaitController(
view=self.view.side_subgait_view['previous'], default=standing)
next_subgait_controller = SideSubgaitController(
view=self.view.side_subgait_view['next'], default=standing)
self.side_subgait_controller = {
'previous': previous_subgait_controller,
'next': next_subgait_controller
}
self.connect_buttons()
self.view.load_gait_into_ui(self.subgait)
for joint in self.subgait.joints:
self.connect_plot(joint)
def test_interpolate_subgaits_interpolated(self):
# test whether each setpoint is correctly interpolated
parameter = 0.4
base_subgait, other_subgait = self.load_interpolatable_subgaits()
new_subgait = Subgait.interpolate_subgaits(base_subgait,
other_subgait,
parameter,
use_foot_position=False)
for i, joint in enumerate(new_subgait.joints):
for j, setpoint in enumerate(joint.setpoints):
base_setpoint = base_subgait.joints[i].setpoints[j]
other_setpoint = other_subgait.joints[i].setpoints[j]
self.assertAlmostEqual(
base_setpoint.time * (1 - parameter) +
parameter * other_setpoint.time,
setpoint.time,
places=4,
)
self.assertAlmostEqual(
base_setpoint.position * (1 - parameter) +
parameter * other_setpoint.position,
setpoint.position,
places=4,
)
self.assertAlmostEqual(
base_setpoint.velocity * (1 - parameter) +
parameter * other_setpoint.velocity,
setpoint.velocity,
places=4,
)
def test_interpolate_subgaits_parameter_one(self):
base_subgait, other_subgait = self.load_interpolatable_subgaits_ik()
new_subgait = Subgait.interpolate_subgaits(base_subgait,
other_subgait,
1,
use_foot_position=True)
self.assertEqual(other_subgait, new_subgait)
def test_interpolate_subgaits_duration(self):
base_subgait, other_subgait = self.load_interpolatable_subgaits()
parameter = 0.2
new_subgait = Subgait.interpolate_subgaits(base_subgait, other_subgait,
parameter)
new_duration = parameter * base_subgait.duration + (
1 - parameter) * other_subgait.duration
self.assertEqual(new_duration, new_subgait.duration)
def load_interpolatable_subgaits(self,
subgait_name='left_close',
base_version='MV_walk_leftclose_v1',
other_version='MV_walk_leftclose_v2'):
base_subgait_path = '{rsc}/{gait}/{subgait}/{version}.subgait'.format(
rsc=self.resources_folder,
gait=self.gait_name,
subgait=subgait_name,
version=base_version)
base_subgait = Subgait.from_file(self.robot, base_subgait_path)
other_subgait_path = '{rsc}/{gait}/{subgait}/{version}.subgait'.format(
rsc=self.resources_folder,
gait=self.gait_name,
subgait=subgait_name,
version=other_version)
other_subgait = Subgait.from_file(self.robot, other_subgait_path)
return base_subgait, other_subgait
def test_interpolate_subgaits_duration_ik(self):
base_subgait, other_subgait = self.load_interpolatable_subgaits_ik()
parameter = 0.2
new_subgait = Subgait.interpolate_subgaits(base_subgait,
other_subgait,
parameter,
use_foot_position=True)
new_duration = (parameter * base_subgait.duration +
(1 - parameter) * other_subgait.duration)
self.assertEqual(new_duration, new_subgait.duration)
def test_invalid_subgait_transition(self):
other_subgait_name = 'right_close'
other_version = 'MV_walk_rightclose_v2'
other_subgait_path = '{rsc}/{gait}/{subgait}/{version}.subgait'.format(
rsc=self.resources_folder,
gait=self.gait_name,
subgait=other_subgait_name,
version=other_version)
other_subgait = Subgait.from_file(self.robot, other_subgait_path)
self.assertFalse(
other_subgait.validate_subgait_transition(self.subgait))
def test_invalid_subgait_transition_unequal_joints(self):
other_subgait_name = 'right_close'
other_version = 'MV_walk_rightclose_v2_seven_joints'
other_subgait_path = '{rsc}/{gait}/{subgait}/{version}.subgait'.format(
rsc=self.resources_folder,
gait=self.gait_name,
subgait=other_subgait_name,
version=other_version)
other_subgait = Subgait.from_file(self.robot, other_subgait_path)
with self.assertRaises(NonValidGaitContent):
other_subgait.validate_subgait_transition(self.subgait)
def to_subgait(
joints,
duration,
gait_name='balance_gait',
gait_type='walk_like',
version='moveit',
subgait_name='balance_subgait',
description='Subgait created using the moveit motion planning.'):
"""Create a subgait using the standard format in the march shared classes."""
return Subgait(joints, duration, gait_type, gait_name, subgait_name,
version, description)
def test_valid_subgait_transition(self):
other_subgait_name = "right_close"
other_version = "MV_walk_rightclose_v2"
other_subgait_path = "{rsc}/{gait}/{subgait}/{version}.subgait".format(
rsc=self.resources_folder,
gait=self.gait_name,
subgait=other_subgait_name,
version=other_version,
)
other_subgait = Subgait.from_file(self.robot, other_subgait_path)
self.assertTrue(
self.subgait.validate_subgait_transition(other_subgait))
def load_interpolatable_subgaits_ik(
self,
subgait_name="swing",
base_version="forward_swing",
other_version="backward_swing",
):
base_subgait_path = "{rsc}/{gait}/{subgait}/{version}.subgait".format(
rsc=self.resources_folder,
gait=self.gait_name_ik,
subgait=subgait_name,
version=base_version,
)
base_subgait = Subgait.from_file(self.robot, base_subgait_path)
other_subgait_path = "{rsc}/{gait}/{subgait}/{version}.subgait".format(
rsc=self.resources_folder,
gait=self.gait_name_ik,
subgait=subgait_name,
version=other_version,
)
other_subgait = Subgait.from_file(self.robot, other_subgait_path)
return base_subgait, other_subgait
def load_interpolatable_subgaits(
self,
subgait_name="left_close",
base_version="MV_walk_leftclose_v1",
other_version="MV_walk_leftclose_v2",
):
base_subgait_path = "{rsc}/{gait}/{subgait}/{version}.subgait".format(
rsc=self.resources_folder,
gait=self.gait_name,
subgait=subgait_name,
version=base_version,
)
base_subgait = Subgait.from_file(self.robot, base_subgait_path)
other_subgait_path = "{rsc}/{gait}/{subgait}/{version}.subgait".format(
rsc=self.resources_folder,
gait=self.gait_name,
subgait=subgait_name,
version=other_version,
)
other_subgait = Subgait.from_file(self.robot, other_subgait_path)
return base_subgait, other_subgait
def test_from_files_interpolated_correct(self):
base_subgait_path = '{rsc}/{gait}/{subgait}/{version}.subgait'.format(
rsc=self.resources_folder,
gait=self.gait_name,
subgait='left_close',
version='MV_walk_leftclose_v1')
other_subgait_path = '{rsc}/{gait}/{subgait}/{version}.subgait'.format(
rsc=self.resources_folder,
gait=self.gait_name,
subgait='left_close',
version='MV_walk_leftclose_v2')
subgait = Subgait.from_files_interpolated(self.robot,
base_subgait_path,
other_subgait_path, 0.5)
self.assertIsInstance(subgait, Subgait)
def setUp(self):
self.gait_name = 'walk'
self.subgait_name = 'left_swing'
self.version = 'MV_walk_leftswing_v2'
self.resources_folder = rospkg.RosPack().get_path(
'march_shared_classes') + '/test/resources'
self.robot = urdf.Robot.from_xml_file(
rospkg.RosPack().get_path('march_description') +
'/urdf/march4.urdf')
self.subgait_path = '{rsc}/{gait}/{subgait}/{version}.subgait'.format(
rsc=self.resources_folder,
gait=self.gait_name,
subgait=self.subgait_name,
version=self.version)
self.subgait = Subgait.from_file(self.robot, self.subgait_path)
def test_final_position(self):
position = 1.0
subgait = Subgait(
[
JointTrajectory(
"test",
Limits(0.0, 0.0, 0.0),
[
Setpoint(0.0, 0.0, 0.0),
Setpoint(0.5, position, 0.0),
],
1.0,
)
],
1.0,
)
self.assertDictEqual(subgait.final_position, {"test": position})
def load_subgait(robot, gait_directory, gait_name, subgait_name,
gait_version_map):
"""Read the .subgait file and extract the data.
:returns
if gait and subgait names are valid return populated Gait object
"""
if not gait_version_map.get(gait_name):
raise GaitNameNotFound(gait_name)
if not gait_version_map[gait_name].get(subgait_name):
raise SubgaitNameNotFound(subgait_name)
version = gait_version_map[gait_name][subgait_name]
subgait_path = os.path.join(gait_directory, gait_name, subgait_name,
version + '.subgait')
if not os.path.isfile(subgait_path):
raise FileNotFoundError(file_path=subgait_path)
return Subgait.from_file(robot, subgait_path)
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')):
rospy.logwarn('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):
rospy.logwarn('{0}, {1} does not exist'.format(
subgait_name, version))
return False
return True
def test_from_files_interpolated_correct(self):
base_subgait_path = "{rsc}/{gait}/{subgait}/{version}.subgait".format(
rsc=self.resources_folder,
gait=self.gait_name,
subgait="left_close",
version="MV_walk_leftclose_v1",
)
other_subgait_path = "{rsc}/{gait}/{subgait}/{version}.subgait".format(
rsc=self.resources_folder,
gait=self.gait_name,
subgait="left_close",
version="MV_walk_leftclose_v2",
)
subgait = Subgait.from_files_interpolated(
self.robot,
base_subgait_path,
other_subgait_path,
0.5,
use_foot_position=False,
)
self.assertIsInstance(subgait, Subgait)
def test_from_files_interpolated_correct_ik(self):
base_subgait_path = "{rsc}/{gait}/{subgait}/{version}.subgait".format(
rsc=self.resources_folder,
gait=self.gait_name_ik,
subgait=self.base_subgait_name,
version=self.base_version,
)
other_subgait_path = "{rsc}/{gait}/{subgait}/{version}.subgait".format(
rsc=self.resources_folder,
gait=self.gait_name_ik,
subgait=self.other_subgait_name,
version=self.other_version,
)
subgait = Subgait.from_files_interpolated(
self.robot,
base_subgait_path,
other_subgait_path,
0.5,
use_foot_position=True,
)
self.assertIsInstance(subgait, Subgait)
def setUp(self):
self.gait_name = "walk"
self.gait_name_ik = "ik_test"
self.subgait_name = "left_swing"
self.base_subgait_name = "swing"
self.other_subgait_name = "swing"
self.version = "MV_walk_leftswing_v2"
self.base_version = "forward_swing"
self.other_version = "backward_swing"
self.resources_folder = (
rospkg.RosPack().get_path("march_shared_classes") +
"/test/resources")
self.robot = urdf.Robot.from_xml_file(
rospkg.RosPack().get_path("march_description") +
"/urdf/march4.urdf")
self.subgait_path = "{rsc}/{gait}/{subgait}/{version}.subgait".format(
rsc=self.resources_folder,
gait=self.gait_name,
subgait=self.subgait_name,
version=self.version,
)
self.subgait = Subgait.from_file(self.robot, self.subgait_path)
def test_interpolate_subgaits_parameter_one(self):
base_subgait, other_subgait = self.load_interpolatable_subgaits()
new_subgait = Subgait.interpolate_subgaits(base_subgait, other_subgait,
1)
self.assertEqual(other_subgait, new_subgait)
def test_interpolate_subgaits_wrong_amount_of_joints(self):
base_subgait, other_subgait = self.load_interpolatable_subgaits(
'right_close', 'MV_walk_rightclose_v2',
'MV_walk_rightclose_v2_seven_joints')
with self.assertRaises(SubgaitInterpolationError):
Subgait.interpolate_subgaits(base_subgait, other_subgait, 0.5)
def test_from_file_no_robot(self):
self.assertIsNone(Subgait.from_file(None, self.subgait_path))
def test_from_file_none_path(self):
with self.assertRaises(FileNotFoundError):
Subgait.from_file(self.robot, None)
def test_from_file_invalid_path(self):
with self.assertRaises(FileNotFoundError):
Subgait.from_file(
self.robot,
self.resources_folder + '/MV_walk_leftswing_v2.subgait')
def test_interpolate_subgaits_wrong_joint_names(self):
base_subgait, other_subgait = self.load_interpolatable_subgaits(
other_version='MV_walk_leftclose_v2_wrong_joint_name')
with self.assertRaises(SubgaitInterpolationError):
Subgait.interpolate_subgaits(base_subgait, other_subgait, 0.5)