Skip to content

Commit a71b3e5

Browse files
peterdavidfaganpeterdavidfagan
committed
add build tests
1 parent 6ac7019 commit a71b3e5

4 files changed

Lines changed: 111 additions & 84 deletions

File tree

‎moveit_py/CMakeLists.txt‎

Lines changed: 27 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -12,13 +12,22 @@ find_package(Python3 REQUIRED COMPONENTS Interpreter Development)
1212
find_package(pybind11_vendor REQUIRED)
1313
find_package(pybind11 REQUIRED)
1414

15+
# enables using the Python extensions from the build space for testing
16+
file(WRITE "${CMAKE_CURRENT_BINARY_DIR}/test_moveit/__init__.py" "")
17+
1518
add_subdirectory(src/moveit/moveit_py_utils)
1619

1720
ament_python_install_package(moveit)
1821

1922
# Set the build location and install location for a CPython extension
2023
function(configure_build_install_location _library_name)
21-
install(TARGETS ${_library_name}
24+
# Install into test_moveit folder in build space for unit tests to import
25+
set_target_properties(${_library_name} PROPERTIES
26+
# Use generator expression to avoid prepending a build type specific directory on Windows
27+
LIBRARY_OUTPUT_DIRECTORY $<1:${CMAKE_CURRENT_BINARY_DIR}/test_moveit>
28+
RUNTIME_OUTPUT_DIRECTORY $<1:${CMAKE_CURRENT_BINARY_DIR}/test_moveit>)
29+
30+
install(TARGETS ${_library_name}
2231
DESTINATION "${PYTHON_INSTALL_DIR}/moveit"
2332
)
2433
endfunction()
@@ -64,4 +73,21 @@ target_link_libraries(planning PRIVATE moveit_ros_planning::moveit_cpp
6473
moveit_py_utils)
6574
configure_build_install_location(planning)
6675

76+
77+
if(BUILD_TESTING)
78+
find_package(ament_cmake_pytest REQUIRED)
79+
set(_pytest_tests
80+
test/unit/test_robot_model.py
81+
test/unit/test_robot_state.py
82+
)
83+
foreach(_test_path ${_pytest_tests})
84+
get_filename_component(_test_name ${_test_path} NAME_WE)
85+
ament_add_pytest_test(${_test_name} ${_test_path}
86+
APPEND_ENV AMENT_PREFIX_INDEX=${ament_index_build_path} PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR}
87+
TIMEOUT 60
88+
WORKING_DIRECTORY "${CMAKE_SOURCE_DIR}"
89+
)
90+
endforeach()
91+
endif()
92+
6793
ament_package()

‎moveit_py/package.xml‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -24,6 +24,10 @@
2424
<depend>moveit_ros_planning_interface</depend>
2525
<depend>moveit_ros_planning</depend>
2626
<depend>moveit_core</depend>
27+
28+
<test_depend>ament_cmake_pytest</test_depend>
29+
<test_depend>python3-pytest</test_depend>
30+
2731
<export>
2832
<build_type>ament_cmake</build_type>
2933
</export>

‎moveit_py/test/unit/test_robot_model.py‎

Lines changed: 22 additions & 26 deletions
Original file line numberDiff line numberDiff line change
@@ -1,53 +1,55 @@
11
import unittest
2-
3-
from moveit_py.core import JointModelGroup, RobotModel
2+
from test_moveit.core.robot_model import JointModelGroup, RobotModel
43

54
# TODO (peterdavidfagan): depend on moveit_resources package directly.
65
# (https://github.com/peterdavidfagan/moveit2/blob/moveit_py/moveit_core/utils/src/robot_model_test_utils.cpp)
6+
7+
import os
8+
9+
dir_path = os.path.dirname(os.path.realpath(__file__))
10+
URDF_FILE = "{}/fixtures/panda.urdf".format(dir_path)
11+
SRDF_FILE = "{}/fixtures/panda.srdf".format(dir_path)
12+
13+
14+
def get_robot_model():
15+
"""Helper function that returns a RobotModel instance."""
16+
return RobotModel(urdf_xml_path=URDF_FILE, srdf_xml_path=SRDF_FILE)
17+
18+
719
class TestRobotModel(unittest.TestCase):
820
def test_initialization(self):
921
"""
1022
Test that the RobotModel can be initialized with xml filepaths.
1123
"""
12-
robot = RobotModel(
13-
urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf"
14-
)
24+
robot = get_robot_model()
1525
self.assertIsInstance(robot, RobotModel)
1626

1727
def test_name_property(self):
1828
"""
1929
Test that the RobotModel name property returns the correct name.
2030
"""
21-
robot = RobotModel(
22-
urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf"
23-
)
31+
robot = get_robot_model()
2432
self.assertEqual(robot.name, "panda")
2533

2634
def test_model_frame_property(self):
2735
"""
2836
Test that the RobotModel model_frame property returns the correct name.
2937
"""
30-
robot = RobotModel(
31-
urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf"
32-
)
38+
robot = get_robot_model()
3339
self.assertEqual(robot.model_frame, "world")
3440

3541
def test_root_joint_name_property(self):
3642
"""
3743
Test that the RobotModel root_link property returns the correct name.
3844
"""
39-
robot = RobotModel(
40-
urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf"
41-
)
45+
robot = get_robot_model()
4246
self.assertEqual(robot.root_joint_name, "virtual_joint")
4347

4448
def test_joint_model_group_names_property(self):
4549
"""
4650
Test that the RobotModel joint_model_group_names property returns the correct names.
4751
"""
48-
robot = RobotModel(
49-
urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf"
50-
)
52+
robot = get_robot_model()
5153
self.assertCountEqual(
5254
robot.joint_model_group_names, ["panda_arm", "hand", "panda_arm_hand"]
5355
)
@@ -56,28 +58,22 @@ def test_joint_model_groups_property(self):
5658
"""
5759
Test that the RobotModel joint_model_groups returns a list of JointModelGroups.
5860
"""
59-
robot = RobotModel(
60-
urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf"
61-
)
61+
robot = get_robot_model()
6262
self.assertIsInstance(robot.joint_model_groups[0], JointModelGroup)
6363

6464
def test_has_joint_model_group(self):
6565
"""
6666
Test that the RobotModel has_joint_model_group returns True for existing groups.
6767
"""
68-
robot = RobotModel(
69-
urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf"
70-
)
68+
robot = get_robot_model()
7169
self.assertTrue(robot.has_joint_model_group("panda_arm"))
7270
self.assertFalse(robot.has_joint_model_group("The answer is 42."))
7371

7472
def test_get_joint_model_group(self):
7573
"""
7674
Test that the RobotModel get_joint_model_group returns the correct group.
7775
"""
78-
robot = RobotModel(
79-
urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf"
80-
)
76+
robot = get_robot_model()
8177
jmg = robot.get_joint_model_group("panda_arm")
8278
self.assertIsInstance(jmg, JointModelGroup)
8379
self.assertEqual(jmg.name, "panda_arm")

‎moveit_py/test/unit/test_robot_state.py‎

Lines changed: 58 additions & 57 deletions
Original file line numberDiff line numberDiff line change
@@ -3,17 +3,31 @@
33

44
from geometry_msgs.msg import Pose
55

6-
from moveit_py.core import RobotState, RobotModel
6+
from test_moveit.core.robot_state import RobotState
7+
from test_moveit.core.robot_model import RobotModel
8+
9+
10+
# TODO (peterdavidfagan): depend on moveit_resources package directly.
11+
# (https://github.com/peterdavidfagan/moveit2/blob/moveit_py/moveit_core/utils/src/robot_model_test_utils.cpp)
12+
13+
import os
14+
15+
dir_path = os.path.dirname(os.path.realpath(__file__))
16+
URDF_FILE = "{}/fixtures/panda.urdf".format(dir_path)
17+
SRDF_FILE = "{}/fixtures/panda.srdf".format(dir_path)
18+
19+
20+
def get_robot_model():
21+
"""Helper function that returns a RobotModel instance."""
22+
return RobotModel(urdf_xml_path=URDF_FILE, srdf_xml_path=SRDF_FILE)
723

824

925
class TestRobotState(unittest.TestCase):
1026
def test_initialization(self):
1127
"""
1228
Test that RobotState can be initialized with a RobotModel
1329
"""
14-
robot_model = RobotModel(
15-
urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf"
16-
)
30+
robot_model = get_robot_model()
1731
robot_state = RobotState(robot_model)
1832

1933
self.assertIsInstance(robot_state, RobotState)
@@ -22,9 +36,7 @@ def test_robot_model_property(self):
2236
"""
2337
Test that the robot_model property returns the correct RobotModel
2438
"""
25-
robot_model = RobotModel(
26-
urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf"
27-
)
39+
robot_model = get_robot_model()
2840
robot_state = RobotState(robot_model)
2941

3042
self.assertEqual(robot_state.robot_model, robot_model)
@@ -33,11 +45,9 @@ def test_get_frame_transform(self):
3345
"""
3446
Test that the frame transform is correct
3547
"""
36-
robot_model = RobotModel(
37-
urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf"
38-
)
48+
robot_model = get_robot_model()
3949
robot_state = RobotState(robot_model)
40-
50+
robot_state.update()
4151
frame_transform = robot_state.get_frame_transform("panda_link0")
4252

4353
self.assertIsInstance(frame_transform, np.ndarray)
@@ -47,10 +57,9 @@ def test_get_pose(self):
4757
"""
4858
Test that the pose is correct
4959
"""
50-
robot_model = RobotModel(
51-
urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf"
52-
)
60+
robot_model = get_robot_model()
5361
robot_state = RobotState(robot_model)
62+
robot_state.update()
5463
pose = robot_state.get_pose(link_name="panda_link8")
5564

5665
self.assertIsInstance(pose, Pose)
@@ -60,10 +69,9 @@ def test_get_jacobian_1(self):
6069
"""
6170
Test that the jacobian is correct
6271
"""
63-
robot_model = RobotModel(
64-
urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf"
65-
)
72+
robot_model = get_robot_model()
6673
robot_state = RobotState(robot_model)
74+
robot_state.update()
6775
jacobian = robot_state.get_jacobian(
6876
joint_model_group_name="panda_arm",
6977
reference_point_position=np.array([0.0, 0.0, 0.0]),
@@ -76,10 +84,9 @@ def test_get_jacobian_2(self):
7684
"""
7785
Test that the jacobian is correct
7886
"""
79-
robot_model = RobotModel(
80-
urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf"
81-
)
87+
robot_model = get_robot_model()
8288
robot_state = RobotState(robot_model)
89+
robot_state.update()
8390
jacobian = robot_state.get_jacobian(
8491
joint_model_group_name="panda_arm",
8592
link_name="panda_link6",
@@ -93,49 +100,43 @@ def test_set_joint_group_positions(self):
93100
"""
94101
Test that the joint group positions can be set
95102
"""
96-
robot_model = RobotModel(
97-
urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf"
98-
)
103+
robot_model = get_robot_model()
99104
robot_state = RobotState(robot_model)
100-
105+
robot_state.update()
101106
joint_group_positions = np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0])
102107
robot_state.set_joint_group_positions(
103108
joint_model_group_name="panda_arm", position_values=joint_group_positions
104109
)
105110

106111
self.assertEqual(
107112
joint_group_positions.tolist(),
108-
robot_state.copy_joint_group_positions("panda_arm").tolist(),
113+
robot_state.get_joint_group_positions("panda_arm").tolist(),
109114
)
110115

111116
def test_set_joint_group_velocities(self):
112117
"""
113118
Test that the joint group velocities can be set
114119
"""
115-
robot_model = RobotModel(
116-
urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf"
117-
)
120+
robot_model = get_robot_model()
118121
robot_state = RobotState(robot_model)
119-
122+
robot_state.update()
120123
joint_group_velocities = np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0])
121124
robot_state.set_joint_group_velocities(
122125
joint_model_group_name="panda_arm", velocity_values=joint_group_velocities
123126
)
124127

125128
self.assertEqual(
126129
joint_group_velocities.tolist(),
127-
robot_state.copy_joint_group_velocities("panda_arm").tolist(),
130+
robot_state.get_joint_group_velocities("panda_arm").tolist(),
128131
)
129132

130133
def test_set_joint_group_accelerations(self):
131134
"""
132135
Test that the joint group accelerations can be set
133136
"""
134-
robot_model = RobotModel(
135-
urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf"
136-
)
137+
robot_model = get_robot_model()
137138
robot_state = RobotState(robot_model)
138-
139+
robot_state.update()
139140
joint_group_accelerations = np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0])
140141
robot_state.set_joint_group_accelerations(
141142
joint_model_group_name="panda_arm",
@@ -144,32 +145,32 @@ def test_set_joint_group_accelerations(self):
144145

145146
self.assertEqual(
146147
joint_group_accelerations.tolist(),
147-
robot_state.copy_joint_group_accelerations("panda_arm").tolist(),
148+
robot_state.get_joint_group_accelerations("panda_arm").tolist(),
148149
)
149150

150151
# TODO (peterdavidfagan): requires kinematics solver to be loaded
151-
def test_set_from_ik(self):
152-
"""
153-
Test that the robot state can be set from an IK solution
154-
"""
155-
robot_model = RobotModel(
156-
urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf"
157-
)
158-
robot_state = RobotState(robot_model)
159-
160-
pose = Pose()
161-
pose.position.x = 0.5
162-
pose.position.y = 0.5
163-
pose.position.z = 0.5
164-
pose.orientation.w = 1.0
165-
166-
robot_state.set_from_ik(
167-
joint_model_group_name="panda_arm",
168-
geometry_pose=pose,
169-
tip_name="panda_link8",
170-
)
171-
172-
self.assertEqual(robot_state.get_pose("panda_link8"), pose)
152+
# def test_set_from_ik(self):
153+
# """
154+
# Test that the robot state can be set from an IK solution
155+
# """
156+
# robot_model = RobotModel(
157+
# urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf"
158+
# )
159+
# robot_state = RobotState(robot_model)
160+
# robot_state.update()
161+
# pose = Pose()
162+
# pose.position.x = 0.5
163+
# pose.position.y = 0.5
164+
# pose.position.z = 0.5
165+
# pose.orientation.w = 1.0
166+
167+
# robot_state.set_from_ik(
168+
# joint_model_group_name="panda_arm",
169+
# geometry_pose=pose,
170+
# tip_name="panda_link8",
171+
# )
172+
173+
# self.assertEqual(robot_state.get_pose("panda_link8"), pose)
173174

174175

175176
if __name__ == "__main__":

0 commit comments

Comments
 (0)