diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_data/frankaemika_panda/testdata_sequence.xml b/moveit_planners/pilz_industrial_motion_planner/test/test_data/frankaemika_panda/testdata_sequence.xml deleted file mode 100644 index 9699c5caad3..00000000000 --- a/moveit_planners/pilz_industrial_motion_planner/test/test_data/frankaemika_panda/testdata_sequence.xml +++ /dev/null @@ -1,173 +0,0 @@ - - - - - - - - 0.0 -0.785 0.0 -2.356 0.0 1.571 0.785 - - - - -0.022096 -0.753529 0.007612 -1.716165 0.0063464 0.962660 0.771123 - 0.2 0 0.8 0.924138 -0.382057 0.0 0.0 - -0.022096 -0.753529 0.007612 -1.716165 0.0063464 0.962660 0.771123 - - - - - -0.061035 -0.301221 0.410282 -1.665777 0.120602 1.389535 1.138547 - 0.4 0.2 0.7 0.924138 -0.382057 0.0 0.0 - -0.061035 -0.301221 0.410282 -1.665777 0.120602 1.389535 1.138547 - - - - - 0.0875228 -0.540765 0.770139 -1.990062 0.366596 1.582282 1.560837 - 0.2 0.4 0.6 0.924138 -0.382057 0.0 0.0 - 0.0875228 -0.540765 0.770139 -1.990062 0.366596 1.582282 1.560837 - - - - - 0.164988 -0.805223 0.304894 -2.020563 0.230321 1.247100 1.238191 - 0.2 0.2 0.7 0.924138 -0.382057 0.0 0.0 - 0.164988 -0.805223 0.304894 -2.020563 0.230321 1.247100 1.238191 - - - - - - - - panda_arm - panda_link8 - ZeroPose - P1 - 1.0 - 0.2 - - - - panda_arm - panda_link8 - P3 - P2 - 1.0 - 0.2 - - - - panda_arm - panda_link8 - P2 - P3 - 1.0 - 0.2 - - - - - - panda_arm - panda_link8 - P2 - P3 - 0.1 - 0.05 - - - - panda_arm - panda_link8 - P1 - P2 - 0.1 - 0.05 - - - - panda_arm - panda_link8 - P2 - P4 - 0.1 - 0.05 - - - - - - panda_arm - panda_link8 - P1 - P2 - P3 - 0.1 - 0.1 - - - - panda_arm - panda_link8 - P3 - P2 - P1 - 0.1 - 0.1 - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/CMakeLists.txt b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/CMakeLists.txt index 066754c0e89..0a43a9dbfc1 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/CMakeLists.txt +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/CMakeLists.txt @@ -172,5 +172,4 @@ install(TARGETS ${UNIT_TEST_SOURCEFILES} LIBRARY DESTINATION lib RUNTIME DESTINATION lib/${PROJECT_NAME}) -install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_cartesian_limits_aggregator.yaml b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_cartesian_limits_aggregator.yaml deleted file mode 100644 index c51e42269c5..00000000000 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_cartesian_limits_aggregator.yaml +++ /dev/null @@ -1,10 +0,0 @@ -only_vel: - cartesian_limits: - max_trans_vel: 10.0 - -all_values: - cartesian_limits: - max_trans_vel: 1.0 - max_trans_acc: 2.0 - max_trans_dec: -3.0 - max_rot_vel: 4.0 diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_joint_limit_config.yaml b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_joint_limit_config.yaml deleted file mode 100644 index f9d715b9614..00000000000 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_joint_limit_config.yaml +++ /dev/null @@ -1,55 +0,0 @@ -joint_limits: - joint_1: - has_position_limits: false - has_velocity_limits: false - has_acceleration_limits: true - max_acceleration: 1.0 - has_deceleration_limits: true - max_deceleration: -1.0 - has_jerk_limits: false - has_soft_limits: false - joint_2: - has_position_limits: false - has_velocity_limits: false - has_acceleration_limits: true - max_acceleration: 2.0 - has_deceleration_limits: true - max_deceleration: -2.0 - has_jerk_limits: false - has_soft_limits: false - joint_3: - has_position_limits: false - has_velocity_limits: false - has_acceleration_limits: true - max_acceleration: 3.0 - has_deceleration_limits: true - max_deceleration: -3.0 - has_jerk_limits: false - has_soft_limits: false - joint_4: - has_position_limits: false - has_velocity_limits: false - has_acceleration_limits: true - max_acceleration: 4.0 - has_deceleration_limits: true - max_deceleration: -4.0 - has_jerk_limits: false - has_soft_limits: false - joint_5: - has_position_limits: false - has_velocity_limits: false - has_acceleration_limits: true - max_acceleration: 5.0 - has_deceleration_limits: true - max_deceleration: -5.0 - has_jerk_limits: false - has_soft_limits: false - joint_6: - has_position_limits: false - has_velocity_limits: false - has_acceleration_limits: true - max_acceleration: 6.0 - has_deceleration_limits: true - max_deceleration: -6.0 - has_jerk_limits: false - has_soft_limits: false diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_joint_limits_aggregator.yaml b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_joint_limits_aggregator.yaml deleted file mode 100644 index 5051be43a6a..00000000000 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_joint_limits_aggregator.yaml +++ /dev/null @@ -1,41 +0,0 @@ -all_valid: - joint_limits: - prbt_joint_1: - has_position_limits: true - min_position: -2.0 - max_position: 2.0 - prbt_joint_2: - has_position_limits: false - min_position: -2.0 - max_position: 2.0 - prbt_joint_3: - has_velocity_limits: true - max_velocity: 1.1 - has_acceleration_limits: false - max_acceleration: 5.5 - prbt_joint_4: - has_acceleration_limits: true - max_acceleration: 5.5 - prbt_joint_5: - has_deceleration_limits: true - max_deceleration: -6.6 - -violate_position_max: - joint_limits: - prbt_joint_3: - has_position_limits: true - min_position: -1.0 - max_position: 4.0 - -violate_position_min: - joint_limits: - prbt_joint_2: - has_position_limits: true - min_position: -4.0 - max_position: 2.0 - -violate_velocity: - joint_limits: - prbt_joint_3: - has_velocity_limits: true - max_velocity: -90.0 diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_trajectory_blender_transition_window.yaml b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_trajectory_blender_transition_window.yaml deleted file mode 100644 index cc19c49bb16..00000000000 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_trajectory_blender_transition_window.yaml +++ /dev/null @@ -1,7 +0,0 @@ -planning_group : "manipulator" -target_link : "prbt_tcp" -cartesian_velocity_tolerance : 1.0e-6 -cartesian_angular_velocity_tolerance : 1.0e-6 -joint_velocity_tolerance : 1.0e-6 -joint_acceleration_tolerance : 1.0e-2 -sampling_time : 0.01 diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_trajectory_functions.yaml b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_trajectory_functions.yaml deleted file mode 100644 index a642ce09ac2..00000000000 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_trajectory_functions.yaml +++ /dev/null @@ -1,5 +0,0 @@ -planning_group : "manipulator" -group_tip_link : "prbt_flange" -tcp_link : "prbt_tcp" -ik_fast_link : "prbt_flange" -random_test_number : 1000 diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_trajectory_generator_circ.yaml b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_trajectory_generator_circ.yaml deleted file mode 100644 index 25142d4ba49..00000000000 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_trajectory_generator_circ.yaml +++ /dev/null @@ -1,7 +0,0 @@ -planning_group : "manipulator" -target_link : "prbt_tcp" -cartesian_position_tolerance : 1.0e-3 -angular_acc_tolerance : 1.0e-3 -rot_axis_norm_tolerance : 1.0e-6 -acceleration_tolerance : 1.0e-3 -other_tolerance : 1.0e-6 diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_trajectory_generator_common.yaml b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_trajectory_generator_common.yaml deleted file mode 100644 index 4658d75d5a9..00000000000 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_trajectory_generator_common.yaml +++ /dev/null @@ -1,9 +0,0 @@ -planning_group : "manipulator" -target_link : "prbt_flange" -random_trial_number : 100 -joint_position_tolerance : 1.0e-3 -joint_velocity_tolerance : 0.3 -pose_norm_tolerance : 1.0e-6 -rot_axis_norm_tolerance : 1.0e-6 -other_tolerance : 1.0e-6 -velocity_scaling_factor : 0.1 diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_trajectory_generator_lin.yaml b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_trajectory_generator_lin.yaml deleted file mode 100644 index b36b328fedd..00000000000 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_trajectory_generator_lin.yaml +++ /dev/null @@ -1,9 +0,0 @@ -planning_group : "manipulator" -target_link_hand_computed_data : "prbt_flange" -random_trial_number : 100 -joint_position_tolerance : 1.0e-3 -joint_velocity_tolerance : 0.3 -pose_norm_tolerance : 1.0e-6 -rot_axis_norm_tolerance : 1.0e-6 -other_tolerance : 1.0e-5 -velocity_scaling_factor : 0.1 diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_trajectory_generator_ptp.yaml b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_trajectory_generator_ptp.yaml deleted file mode 100644 index 8d4938d6e60..00000000000 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/config/unittest_trajectory_generator_ptp.yaml +++ /dev/null @@ -1,6 +0,0 @@ -planning_group : "manipulator" -target_link : "prbt_tcp" -joint_position_tolerance : 1.0e-8 -joint_velocity_tolerance : 1.0e-8 -joint_acceleration_tolerance : 1.0e-8 -pose_norm_tolerance : 1.0e-6 diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/common_parameters.py b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/common_parameters.py index e983c856f1a..0fe3f6dccf3 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/common_parameters.py +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/common_parameters.py @@ -36,6 +36,13 @@ def load_yaml(package_name, file_path): return None +def load_test_data(robot_setup, test_data_file): + return load_yaml( + "pilz_industrial_motion_planner", + "test/test_data/" + robot_setup + "/config/" + test_data_file, + ) + + class MoveItConfigs: robot_description = {} robot_description_semantic = {} @@ -45,10 +52,10 @@ class MoveItConfigs: def load_moveit_config(): - moveit_config_package_name = "moveit_resources_prbt_moveit_config" - description_package_name = "moveit_resources_prbt_support" - description_xacro_file = "urdf/prbt.xacro" - robot_description_semantic_file = "config/prbt.srdf.xacro" + moveit_config_package_name = "moveit_resources_panda_moveit_config" + description_package_name = "moveit_resources_panda_description" + description_xacro_file = "urdf/panda.urdf" + robot_description_semantic_file = "config/panda.srdf" robot_description_kinematics_file = "config/kinematics.yaml" joint_limits_file = "config/joint_limits.yaml" cartesian_limits_file = "config/cartesian_limits.yaml" diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_cartesian_limits_aggregator.test.py b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_cartesian_limits_aggregator.test.py index e2947edada4..e49fd864343 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_cartesian_limits_aggregator.test.py +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_cartesian_limits_aggregator.test.py @@ -8,6 +8,7 @@ from launch import LaunchDescription from launch_ros.actions import Node from launch_testing.util import KeepAliveProc +from common_parameters import load_test_data def load_file(package_name, file_path): @@ -33,9 +34,8 @@ def load_yaml(package_name, file_path): @pytest.mark.rostest def generate_test_description(): - cartesian_limits_yaml = load_yaml( - "pilz_industrial_motion_planner", - "config/unittest_cartesian_limits_aggregator.yaml", + cartesian_limits_yaml = load_test_data( + "panda", "unittest_cartesian_limits_aggregator.yaml" ) # run test diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_joint_limit.test.py b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_joint_limit.test.py index 0cd89871243..4201e3940cb 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_joint_limit.test.py +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_joint_limit.test.py @@ -8,6 +8,7 @@ from launch import LaunchDescription from launch_ros.actions import Node from launch_testing.util import KeepAliveProc +from common_parameters import load_test_data def load_file(package_name, file_path): @@ -33,9 +34,7 @@ def load_yaml(package_name, file_path): @pytest.mark.rostest def generate_test_description(): - joint_limits_yaml = load_yaml( - "pilz_industrial_motion_planner", "config/unittest_joint_limit_config.yaml" - ) + joint_limits_yaml = load_test_data("panda", "unittest_joint_limit_config.yaml") # run test unittest_joint_limit = Node( diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_joint_limits_aggregator.test.py b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_joint_limits_aggregator.test.py index 8caf1a875d6..212bda27cff 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_joint_limits_aggregator.test.py +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_joint_limits_aggregator.test.py @@ -10,7 +10,7 @@ import os sys.path.append(os.path.dirname(__file__)) -from common_parameters import load_moveit_config, load_yaml +from common_parameters import load_moveit_config, load_yaml, load_test_data @pytest.mark.rostest diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_trajectory_blender_transition_window.test.py b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_trajectory_blender_transition_window.test.py index 20a83da0cf1..7e7faf667da 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_trajectory_blender_transition_window.test.py +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_trajectory_blender_transition_window.test.py @@ -10,7 +10,7 @@ import os sys.path.append(os.path.dirname(__file__)) -from common_parameters import load_moveit_config, load_yaml +from common_parameters import load_moveit_config, load_yaml, load_test_data @pytest.mark.rostest @@ -19,16 +19,16 @@ def generate_test_description(): # Load the context test_config = load_moveit_config() - test_param = load_yaml( - "pilz_industrial_motion_planner", - "config/unittest_trajectory_blender_transition_window.yaml", + test_param = load_test_data( + "panda", + "unittest_trajectory_blender_transition_window.yaml", ) testdata_file_name = { "testdata_file_name": get_package_share_directory( "pilz_industrial_motion_planner" ) - + "/test_data/prbt/testdata_sequence.xml" + + "/test_data/panda/testdata_sequence.xml" } # run test diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_trajectory_functions.test.py b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_trajectory_functions.test.py index 066600f68cd..791fcc9f061 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_trajectory_functions.test.py +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_trajectory_functions.test.py @@ -10,7 +10,7 @@ import os sys.path.append(os.path.dirname(__file__)) -from common_parameters import load_moveit_config, load_yaml +from common_parameters import load_moveit_config, load_yaml, load_test_data @pytest.mark.rostest @@ -19,9 +19,7 @@ def generate_test_description(): # Load the context test_config = load_moveit_config() - test_param = load_yaml( - "pilz_industrial_motion_planner", "config/unittest_trajectory_functions.yaml" - ) + test_param = load_test_data("panda", "unittest_trajectory_functions.yaml") # run test unittest_trajectory_functions = Node( diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_trajectory_generator_circ.test.py b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_trajectory_generator_circ.test.py index ad01a4d1cf3..db93d67725b 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_trajectory_generator_circ.test.py +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_trajectory_generator_circ.test.py @@ -10,7 +10,7 @@ import os sys.path.append(os.path.dirname(__file__)) -from common_parameters import load_moveit_config, load_yaml +from common_parameters import load_moveit_config, load_yaml, load_test_data @pytest.mark.rostest @@ -19,16 +19,13 @@ def generate_test_description(): # Load the context test_config = load_moveit_config() - test_param = load_yaml( - "pilz_industrial_motion_planner", - "config/unittest_trajectory_generator_circ.yaml", - ) + test_param = load_test_data("panda", "unittest_trajectory_generator_circ.yaml") testdata_file_name = { "testdata_file_name": get_package_share_directory( "pilz_industrial_motion_planner" ) - + "/test_data/prbt/testdata_sequence.xml" + + "/test_data/panda/testdata_sequence.xml" } # run test diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_trajectory_generator_common.test.py b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_trajectory_generator_common.test.py index 4b2a7b1ec92..69e7b6ba27b 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_trajectory_generator_common.test.py +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_trajectory_generator_common.test.py @@ -10,7 +10,7 @@ import os sys.path.append(os.path.dirname(__file__)) -from common_parameters import load_moveit_config, load_yaml +from common_parameters import load_moveit_config, load_yaml, load_test_data @pytest.mark.rostest @@ -19,10 +19,7 @@ def generate_test_description(): # Load the context test_config = load_moveit_config() - test_param = load_yaml( - "pilz_industrial_motion_planner", - "config/unittest_trajectory_generator_common.yaml", - ) + test_param = load_test_data("panda", "unittest_trajectory_generator_common.yaml") # run test unittest_trajectory_generator_common = Node( diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_trajectory_generator_lin.test.py b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_trajectory_generator_lin.test.py index 5cd903f8cb1..38ed105055d 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_trajectory_generator_lin.test.py +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_trajectory_generator_lin.test.py @@ -10,7 +10,7 @@ import os sys.path.append(os.path.dirname(__file__)) -from common_parameters import load_moveit_config, load_yaml +from common_parameters import load_moveit_config, load_yaml, load_test_data @pytest.mark.rostest @@ -19,16 +19,13 @@ def generate_test_description(): # Load the context test_config = load_moveit_config() - test_param = load_yaml( - "pilz_industrial_motion_planner", - "config/unittest_trajectory_generator_lin.yaml", - ) + test_param = load_test_data("panda", "unittest_trajectory_generator_lin.yaml") testdata_file_name = { "testdata_file_name": get_package_share_directory( "pilz_industrial_motion_planner" ) - + "/test_data/prbt/testdata_sequence.xml" + + "/test_data/panda/testdata_sequence.xml" } # run test diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_trajectory_generator_ptp.test.py b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_trajectory_generator_ptp.test.py index eff917f49e6..d56f004d9b5 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_trajectory_generator_ptp.test.py +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_trajectory_generator_ptp.test.py @@ -10,7 +10,7 @@ import os sys.path.append(os.path.dirname(__file__)) -from common_parameters import load_moveit_config, load_yaml +from common_parameters import load_moveit_config, load_yaml, load_test_data @pytest.mark.rostest @@ -19,10 +19,7 @@ def generate_test_description(): # Load the context test_config = load_moveit_config() - test_param = load_yaml( - "pilz_industrial_motion_planner", - "config/unittest_trajectory_generator_ptp.yaml", - ) + test_param = load_test_data("panda", "unittest_trajectory_generator_ptp.yaml") # run test unittest_trajectory_generator_ptp = Node(