Skip to content

Fix python tests #1979

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
sjahr merged 2 commits into from
Mar 20, 2023
Merged

Fix python tests #1979

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
6ac7019
ensure joint models in robot_model submodule
peterdavidfagan Feb 28, 2023
a71b3e5
add build tests
peterdavidfagan Feb 28, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
28 changes: 27 additions & 1 deletion moveit_py/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand Down Expand Up @@ -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()
4 changes: 4 additions & 0 deletions moveit_py/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,10 @@
<depend>moveit_ros_planning_interface</depend>
<depend>moveit_ros_planning</depend>
<depend>moveit_core</depend>

<test_depend>ament_cmake_pytest</test_depend>
<test_depend>python3-pytest</test_depend>

<export>
<build_type>ament_cmake</build_type>
</export>
Expand Down
4 changes: 3 additions & 1 deletion moveit_py/src/moveit/moveit_core/robot_model/joint_model.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -44,7 +44,9 @@ namespace bind_robot_model

void init_joint_model(py::module& m)
{
py::class_<moveit::core::VariableBounds, std::shared_ptr<moveit::core::VariableBounds>>(m, "VariableBounds")
py::module robot_model = m.def_submodule("robot_model");

py::class_<moveit::core::VariableBounds, std::shared_ptr<moveit::core::VariableBounds>>(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_)
Expand Down
4 changes: 3 additions & 1 deletion moveit_py/src/moveit/moveit_core/robot_model/joint_model_group.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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_<moveit::core::JointModelGroup>(m, "JointModelGroup",
py::module robot_model = m.def_submodule("robot_model");

py::class_<moveit::core::JointModelGroup>(robot_model, "JointModelGroup",
R"(
Representation of a group of joints that are part of a robot model.
)")
Expand Down
48 changes: 22 additions & 26 deletions moveit_py/test/unit/test_robot_model.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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"]
)
Expand All @@ -56,28 +58,22 @@ 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."))

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")
Expand Down
115 changes: 58 additions & 57 deletions moveit_py/test/unit/test_robot_state.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,17 +3,31 @@

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):
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)
Expand All @@ -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)
Expand All @@ -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)
Expand All @@ -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)
Expand All @@ -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]),
Expand All @@ -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",
Expand All @@ -93,49 +100,43 @@ 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
)

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
)

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",
Expand All @@ -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__":
Expand Down