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)