diff --git a/moveit_py/CMakeLists.txt b/moveit_py/CMakeLists.txt index 1ad76f6282..5a881b2347 100644 --- a/moveit_py/CMakeLists.txt +++ b/moveit_py/CMakeLists.txt @@ -12,13 +12,22 @@ find_package(Python3 REQUIRED COMPONENTS Interpreter Development) find_package(pybind11_vendor REQUIRED) find_package(pybind11 REQUIRED) +# enables using the Python extensions from the build space for testing +file(WRITE "${CMAKE_CURRENT_BINARY_DIR}/test_moveit/__init__.py" "") + add_subdirectory(src/moveit/moveit_py_utils) ament_python_install_package(moveit) # Set the build location and install location for a CPython extension function(configure_build_install_location _library_name) - install(TARGETS ${_library_name} + # Install into test_moveit folder in build space for unit tests to import + set_target_properties(${_library_name} PROPERTIES + # Use generator expression to avoid prepending a build type specific directory on Windows + LIBRARY_OUTPUT_DIRECTORY $<1:${CMAKE_CURRENT_BINARY_DIR}/test_moveit> + RUNTIME_OUTPUT_DIRECTORY $<1:${CMAKE_CURRENT_BINARY_DIR}/test_moveit>) + + install(TARGETS ${_library_name} DESTINATION "${PYTHON_INSTALL_DIR}/moveit" ) endfunction() @@ -64,4 +73,21 @@ target_link_libraries(planning PRIVATE moveit_ros_planning::moveit_cpp moveit_py_utils) configure_build_install_location(planning) + +if(BUILD_TESTING) + find_package(ament_cmake_pytest REQUIRED) + set(_pytest_tests + test/unit/test_robot_model.py + test/unit/test_robot_state.py + ) + foreach(_test_path ${_pytest_tests}) + get_filename_component(_test_name ${_test_path} NAME_WE) + ament_add_pytest_test(${_test_name} ${_test_path} + APPEND_ENV AMENT_PREFIX_INDEX=${ament_index_build_path} PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR} + TIMEOUT 60 + WORKING_DIRECTORY "${CMAKE_SOURCE_DIR}" + ) + endforeach() +endif() + ament_package() diff --git a/moveit_py/package.xml b/moveit_py/package.xml index 77d6610845..7be2d02690 100644 --- a/moveit_py/package.xml +++ b/moveit_py/package.xml @@ -24,6 +24,10 @@ moveit_ros_planning_interface moveit_ros_planning moveit_core + + ament_cmake_pytest + python3-pytest + ament_cmake diff --git a/moveit_py/src/moveit/moveit_core/robot_model/joint_model.cpp b/moveit_py/src/moveit/moveit_core/robot_model/joint_model.cpp index 49d85bd79b..8132615eaa 100644 --- a/moveit_py/src/moveit/moveit_core/robot_model/joint_model.cpp +++ b/moveit_py/src/moveit/moveit_core/robot_model/joint_model.cpp @@ -44,7 +44,9 @@ namespace bind_robot_model void init_joint_model(py::module& m) { - py::class_>(m, "VariableBounds") + py::module robot_model = m.def_submodule("robot_model"); + + py::class_>(robot_model, "VariableBounds") .def_readonly("min_position", &moveit::core::VariableBounds::min_position_) .def_readonly("max_position", &moveit::core::VariableBounds::max_position_) .def_readonly("position_bounded", &moveit::core::VariableBounds::position_bounded_) diff --git a/moveit_py/src/moveit/moveit_core/robot_model/joint_model_group.cpp b/moveit_py/src/moveit/moveit_core/robot_model/joint_model_group.cpp index 45c3c0608f..03f84b3298 100644 --- a/moveit_py/src/moveit/moveit_core/robot_model/joint_model_group.cpp +++ b/moveit_py/src/moveit/moveit_core/robot_model/joint_model_group.cpp @@ -49,7 +49,9 @@ bool satisfies_position_bounds(const moveit::core::JointModelGroup* jmg, const E void init_joint_model_group(py::module& m) { - py::class_(m, "JointModelGroup", + py::module robot_model = m.def_submodule("robot_model"); + + py::class_(robot_model, "JointModelGroup", R"( Representation of a group of joints that are part of a robot model. )") diff --git a/moveit_py/test/unit/test_robot_model.py b/moveit_py/test/unit/test_robot_model.py index 9cad84dc94..a616ba7a51 100644 --- a/moveit_py/test/unit/test_robot_model.py +++ b/moveit_py/test/unit/test_robot_model.py @@ -1,53 +1,55 @@ import unittest - -from moveit_py.core import JointModelGroup, RobotModel +from test_moveit.core.robot_model import JointModelGroup, RobotModel # TODO (peterdavidfagan): depend on moveit_resources package directly. # (https://github.com/peterdavidfagan/moveit2/blob/moveit_py/moveit_core/utils/src/robot_model_test_utils.cpp) + +import os + +dir_path = os.path.dirname(os.path.realpath(__file__)) +URDF_FILE = "{}/fixtures/panda.urdf".format(dir_path) +SRDF_FILE = "{}/fixtures/panda.srdf".format(dir_path) + + +def get_robot_model(): + """Helper function that returns a RobotModel instance.""" + return RobotModel(urdf_xml_path=URDF_FILE, srdf_xml_path=SRDF_FILE) + + class TestRobotModel(unittest.TestCase): def test_initialization(self): """ Test that the RobotModel can be initialized with xml filepaths. """ - robot = RobotModel( - urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf" - ) + robot = get_robot_model() self.assertIsInstance(robot, RobotModel) def test_name_property(self): """ Test that the RobotModel name property returns the correct name. """ - robot = RobotModel( - urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf" - ) + robot = get_robot_model() self.assertEqual(robot.name, "panda") def test_model_frame_property(self): """ Test that the RobotModel model_frame property returns the correct name. """ - robot = RobotModel( - urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf" - ) + robot = get_robot_model() self.assertEqual(robot.model_frame, "world") def test_root_joint_name_property(self): """ Test that the RobotModel root_link property returns the correct name. """ - robot = RobotModel( - urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf" - ) + robot = get_robot_model() self.assertEqual(robot.root_joint_name, "virtual_joint") def test_joint_model_group_names_property(self): """ Test that the RobotModel joint_model_group_names property returns the correct names. """ - robot = RobotModel( - urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf" - ) + robot = get_robot_model() self.assertCountEqual( robot.joint_model_group_names, ["panda_arm", "hand", "panda_arm_hand"] ) @@ -56,18 +58,14 @@ def test_joint_model_groups_property(self): """ Test that the RobotModel joint_model_groups returns a list of JointModelGroups. """ - robot = RobotModel( - urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf" - ) + robot = get_robot_model() self.assertIsInstance(robot.joint_model_groups[0], JointModelGroup) def test_has_joint_model_group(self): """ Test that the RobotModel has_joint_model_group returns True for existing groups. """ - robot = RobotModel( - urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf" - ) + robot = get_robot_model() self.assertTrue(robot.has_joint_model_group("panda_arm")) self.assertFalse(robot.has_joint_model_group("The answer is 42.")) @@ -75,9 +73,7 @@ def test_get_joint_model_group(self): """ Test that the RobotModel get_joint_model_group returns the correct group. """ - robot = RobotModel( - urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf" - ) + robot = get_robot_model() jmg = robot.get_joint_model_group("panda_arm") self.assertIsInstance(jmg, JointModelGroup) self.assertEqual(jmg.name, "panda_arm") diff --git a/moveit_py/test/unit/test_robot_state.py b/moveit_py/test/unit/test_robot_state.py index d4ccd2fe02..f5f6d8ed4a 100644 --- a/moveit_py/test/unit/test_robot_state.py +++ b/moveit_py/test/unit/test_robot_state.py @@ -3,7 +3,23 @@ from geometry_msgs.msg import Pose -from moveit_py.core import RobotState, RobotModel +from test_moveit.core.robot_state import RobotState +from test_moveit.core.robot_model import RobotModel + + +# TODO (peterdavidfagan): depend on moveit_resources package directly. +# (https://github.com/peterdavidfagan/moveit2/blob/moveit_py/moveit_core/utils/src/robot_model_test_utils.cpp) + +import os + +dir_path = os.path.dirname(os.path.realpath(__file__)) +URDF_FILE = "{}/fixtures/panda.urdf".format(dir_path) +SRDF_FILE = "{}/fixtures/panda.srdf".format(dir_path) + + +def get_robot_model(): + """Helper function that returns a RobotModel instance.""" + return RobotModel(urdf_xml_path=URDF_FILE, srdf_xml_path=SRDF_FILE) class TestRobotState(unittest.TestCase): @@ -11,9 +27,7 @@ def test_initialization(self): """ Test that RobotState can be initialized with a RobotModel """ - robot_model = RobotModel( - urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf" - ) + robot_model = get_robot_model() robot_state = RobotState(robot_model) self.assertIsInstance(robot_state, RobotState) @@ -22,9 +36,7 @@ def test_robot_model_property(self): """ Test that the robot_model property returns the correct RobotModel """ - robot_model = RobotModel( - urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf" - ) + robot_model = get_robot_model() robot_state = RobotState(robot_model) self.assertEqual(robot_state.robot_model, robot_model) @@ -33,11 +45,9 @@ def test_get_frame_transform(self): """ Test that the frame transform is correct """ - robot_model = RobotModel( - urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf" - ) + robot_model = get_robot_model() robot_state = RobotState(robot_model) - + robot_state.update() frame_transform = robot_state.get_frame_transform("panda_link0") self.assertIsInstance(frame_transform, np.ndarray) @@ -47,10 +57,9 @@ def test_get_pose(self): """ Test that the pose is correct """ - robot_model = RobotModel( - urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf" - ) + robot_model = get_robot_model() robot_state = RobotState(robot_model) + robot_state.update() pose = robot_state.get_pose(link_name="panda_link8") self.assertIsInstance(pose, Pose) @@ -60,10 +69,9 @@ def test_get_jacobian_1(self): """ Test that the jacobian is correct """ - robot_model = RobotModel( - urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf" - ) + robot_model = get_robot_model() robot_state = RobotState(robot_model) + robot_state.update() jacobian = robot_state.get_jacobian( joint_model_group_name="panda_arm", reference_point_position=np.array([0.0, 0.0, 0.0]), @@ -76,10 +84,9 @@ def test_get_jacobian_2(self): """ Test that the jacobian is correct """ - robot_model = RobotModel( - urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf" - ) + robot_model = get_robot_model() robot_state = RobotState(robot_model) + robot_state.update() jacobian = robot_state.get_jacobian( joint_model_group_name="panda_arm", link_name="panda_link6", @@ -93,11 +100,9 @@ def test_set_joint_group_positions(self): """ Test that the joint group positions can be set """ - robot_model = RobotModel( - urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf" - ) + robot_model = get_robot_model() robot_state = RobotState(robot_model) - + robot_state.update() joint_group_positions = np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]) robot_state.set_joint_group_positions( joint_model_group_name="panda_arm", position_values=joint_group_positions @@ -105,18 +110,16 @@ def test_set_joint_group_positions(self): self.assertEqual( joint_group_positions.tolist(), - robot_state.copy_joint_group_positions("panda_arm").tolist(), + robot_state.get_joint_group_positions("panda_arm").tolist(), ) def test_set_joint_group_velocities(self): """ Test that the joint group velocities can be set """ - robot_model = RobotModel( - urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf" - ) + robot_model = get_robot_model() robot_state = RobotState(robot_model) - + robot_state.update() joint_group_velocities = np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]) robot_state.set_joint_group_velocities( joint_model_group_name="panda_arm", velocity_values=joint_group_velocities @@ -124,18 +127,16 @@ def test_set_joint_group_velocities(self): self.assertEqual( joint_group_velocities.tolist(), - robot_state.copy_joint_group_velocities("panda_arm").tolist(), + robot_state.get_joint_group_velocities("panda_arm").tolist(), ) def test_set_joint_group_accelerations(self): """ Test that the joint group accelerations can be set """ - robot_model = RobotModel( - urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf" - ) + robot_model = get_robot_model() robot_state = RobotState(robot_model) - + robot_state.update() joint_group_accelerations = np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]) robot_state.set_joint_group_accelerations( joint_model_group_name="panda_arm", @@ -144,32 +145,32 @@ def test_set_joint_group_accelerations(self): self.assertEqual( joint_group_accelerations.tolist(), - robot_state.copy_joint_group_accelerations("panda_arm").tolist(), + robot_state.get_joint_group_accelerations("panda_arm").tolist(), ) # TODO (peterdavidfagan): requires kinematics solver to be loaded - def test_set_from_ik(self): - """ - Test that the robot state can be set from an IK solution - """ - robot_model = RobotModel( - urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf" - ) - robot_state = RobotState(robot_model) - - pose = Pose() - pose.position.x = 0.5 - pose.position.y = 0.5 - pose.position.z = 0.5 - pose.orientation.w = 1.0 - - robot_state.set_from_ik( - joint_model_group_name="panda_arm", - geometry_pose=pose, - tip_name="panda_link8", - ) - - self.assertEqual(robot_state.get_pose("panda_link8"), pose) + # def test_set_from_ik(self): + # """ + # Test that the robot state can be set from an IK solution + # """ + # robot_model = RobotModel( + # urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf" + # ) + # robot_state = RobotState(robot_model) + # robot_state.update() + # pose = Pose() + # pose.position.x = 0.5 + # pose.position.y = 0.5 + # pose.position.z = 0.5 + # pose.orientation.w = 1.0 + + # robot_state.set_from_ik( + # joint_model_group_name="panda_arm", + # geometry_pose=pose, + # tip_name="panda_link8", + # ) + + # self.assertEqual(robot_state.get_pose("panda_link8"), pose) if __name__ == "__main__":