Skip to content

Commit 266a092

Browse files
URJalaURJala
committed
finish splitting passthrough controller tests
1 parent 1727bd1 commit 266a092

File tree

1 file changed

+110
-90
lines changed

1 file changed

+110
-90
lines changed

‎ur_robot_driver/test/integration_test_passthrough_controller.py‎

Lines changed: 110 additions & 90 deletions
Original file line numberDiff line numberDiff line change
@@ -77,7 +77,7 @@ def generate_test_description(tf_prefix):
7777
Duration(sec=8, nanosec=0),
7878
Duration(sec=12, nanosec=0),
7979
]
80-
TEST_TRAJECTORY = zip(time_vec, waypts)
80+
TEST_TRAJECTORY = [(time_vec[i], waypts[i]) for i in range(len(waypts))]
8181

8282

8383
class RobotDriverTest(unittest.TestCase):
@@ -165,96 +165,116 @@ def test_passthrough_trajectory(self, tf_prefix):
165165
)
166166
self.assertEqual(result.error_code, FollowJointTrajectory.Result.SUCCESSFUL)
167167

168-
# def test_quintic_trajectory(self, tf_prefix):
169-
# # Full quintic trajectory
170-
171-
# trajectory = JointTrajectory(
172-
# points=[
173-
# JointTrajectoryPoint(
174-
# positions=pos,
175-
# time_from_start=times,
176-
# velocities=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
177-
# accelerations=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
178-
# )
179-
# for (times, pos) in TEST_TRAJECTORY
180-
# ],
181-
# joint_names=[tf_prefix + joint for joint in ROBOT_JOINTS],
182-
# )
183-
# goal_time_tolerance = Duration(sec=1, nanosec=0)
184-
# goal_tolerance = [
185-
# JointTolerance(position=0.01, name=tf_prefix + joint) for joint in ROBOT_JOINTS
186-
# ]
187-
# goal_handle = self._passthrough_forward_joint_trajectory.send_goal(
188-
# trajectory=trajectory,
189-
# goal_time_tolerance=goal_time_tolerance,
190-
# goal_tolerance=goal_tolerance,
191-
# )
168+
def test_quintic_trajectory(self, tf_prefix):
169+
# Full quintic trajectory
170+
self.assertTrue(
171+
self._controller_manager_interface.switch_controller(
172+
strictness=SwitchController.Request.BEST_EFFORT,
173+
activate_controllers=["passthrough_trajectory_controller"],
174+
deactivate_controllers=["scaled_joint_trajectory_controller"],
175+
).ok
176+
)
177+
trajectory = JointTrajectory(
178+
points=[
179+
JointTrajectoryPoint(
180+
positions=pos,
181+
time_from_start=times,
182+
velocities=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
183+
accelerations=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
184+
)
185+
for (times, pos) in TEST_TRAJECTORY
186+
],
187+
joint_names=[tf_prefix + joint for joint in ROBOT_JOINTS],
188+
)
189+
goal_time_tolerance = Duration(sec=1, nanosec=0)
190+
goal_tolerance = [
191+
JointTolerance(position=0.01, name=tf_prefix + joint) for joint in ROBOT_JOINTS
192+
]
193+
goal_handle = self._passthrough_forward_joint_trajectory.send_goal(
194+
trajectory=trajectory,
195+
goal_time_tolerance=goal_time_tolerance,
196+
goal_tolerance=goal_tolerance,
197+
)
192198

193-
# self.assertTrue(goal_handle.accepted)
194-
# if goal_handle.accepted:
195-
# result = self._passthrough_forward_joint_trajectory.get_result(
196-
# goal_handle, TIMEOUT_EXECUTE_TRAJECTORY
197-
# )
198-
# self.assertEqual(result.error_code, FollowJointTrajectory.Result.SUCCESSFUL)
199+
self.assertTrue(goal_handle.accepted)
200+
if goal_handle.accepted:
201+
result = self._passthrough_forward_joint_trajectory.get_result(
202+
goal_handle, TIMEOUT_EXECUTE_TRAJECTORY
203+
)
204+
self.assertEqual(result.error_code, FollowJointTrajectory.Result.SUCCESSFUL)
199205

200-
# def test_impossible_goal_tolerance_fails(self, tf_prefix):
201-
# # Test impossible goal tolerance, should fail.
206+
def test_impossible_goal_tolerance_fails(self, tf_prefix):
207+
# Test impossible goal tolerance, should fail.
208+
self.assertTrue(
209+
self._controller_manager_interface.switch_controller(
210+
strictness=SwitchController.Request.BEST_EFFORT,
211+
activate_controllers=["passthrough_trajectory_controller"],
212+
deactivate_controllers=["scaled_joint_trajectory_controller"],
213+
).ok
214+
)
215+
trajectory = JointTrajectory(
216+
points=[
217+
JointTrajectoryPoint(positions=pos, time_from_start=times)
218+
for (times, pos) in TEST_TRAJECTORY
219+
],
220+
joint_names=[tf_prefix + joint for joint in ROBOT_JOINTS],
221+
)
222+
goal_tolerance = [
223+
JointTolerance(position=0.000000001, name=tf_prefix + joint) for joint in ROBOT_JOINTS
224+
]
225+
goal_time_tolerance = Duration(sec=1, nanosec=0)
226+
goal_handle = self._passthrough_forward_joint_trajectory.send_goal(
227+
trajectory=trajectory,
228+
goal_time_tolerance=goal_time_tolerance,
229+
goal_tolerance=goal_tolerance,
230+
)
231+
self.assertTrue(goal_handle.accepted)
232+
if goal_handle.accepted:
233+
result = self._passthrough_forward_joint_trajectory.get_result(
234+
goal_handle, TIMEOUT_EXECUTE_TRAJECTORY
235+
)
236+
self.assertEqual(
237+
result.error_code, FollowJointTrajectory.Result.GOAL_TOLERANCE_VIOLATED
238+
)
202239

203-
# trajectory = JointTrajectory(
204-
# points=[
205-
# JointTrajectoryPoint(positions=pos, time_from_start=times)
206-
# for (times, pos) in TEST_TRAJECTORY
207-
# ],
208-
# joint_names=[tf_prefix + joint for joint in ROBOT_JOINTS],
209-
# )
210-
# goal_tolerance = [
211-
# JointTolerance(position=0.000000001, name=tf_prefix + joint) for joint in ROBOT_JOINTS
212-
# ]
213-
# goal_time_tolerance = Duration(sec=1, nanosec=0)
214-
# goal_handle = self._passthrough_forward_joint_trajectory.send_goal(
215-
# trajectory=trajectory,
216-
# goal_time_tolerance=goal_time_tolerance,
217-
# goal_tolerance=goal_tolerance,
218-
# )
219-
# self.assertTrue(goal_handle.accepted)
220-
# if goal_handle.accepted:
221-
# result = self._passthrough_forward_joint_trajectory.get_result(
222-
# goal_handle, TIMEOUT_EXECUTE_TRAJECTORY
223-
# )
224-
# self.assertEqual(
225-
# result.error_code, FollowJointTrajectory.Result.GOAL_TOLERANCE_VIOLATED
226-
# )
240+
def test_impossible_goal_time_tolerance_fails(self, tf_prefix):
241+
# Test impossible goal time
242+
self.assertTrue(
243+
self._controller_manager_interface.switch_controller(
244+
strictness=SwitchController.Request.BEST_EFFORT,
245+
activate_controllers=["passthrough_trajectory_controller"],
246+
deactivate_controllers=["scaled_joint_trajectory_controller"],
247+
).ok
248+
)
227249

228-
# def test_impossible_goal_time_tolerance_fails(self, tf_prefix):
229-
# # Test impossible goal time
230-
# goal_tolerance = [
231-
# JointTolerance(position=0.01, name=tf_prefix + joint) for joint in ROBOT_JOINTS
232-
# ]
233-
# goal_time_tolerance = Duration(sec=0, nanosec=10)
234-
# trajectory = JointTrajectory(
235-
# points=[
236-
# JointTrajectoryPoint(positions=pos, time_from_start=times)
237-
# for (times, pos) in TEST_TRAJECTORY
238-
# ],
239-
# joint_names=[tf_prefix + joint for joint in ROBOT_JOINTS],
240-
# )
241-
# goal_handle = self._passthrough_forward_joint_trajectory.send_goal(
242-
# trajectory=trajectory,
243-
# goal_time_tolerance=goal_time_tolerance,
244-
# goal_tolerance=goal_tolerance,
245-
# )
246-
# self.assertTrue(goal_handle.accepted)
247-
# if goal_handle.accepted:
248-
# result = self._passthrough_forward_joint_trajectory.get_result(
249-
# goal_handle, TIMEOUT_EXECUTE_TRAJECTORY
250-
# )
251-
# self.assertEqual(
252-
# result.error_code, FollowJointTrajectory.Result.GOAL_TOLERANCE_VIOLATED
253-
# )
254-
# self.assertTrue(
255-
# self._controller_manager_interface.switch_controller(
256-
# strictness=SwitchController.Request.BEST_EFFORT,
257-
# deactivate_controllers=["passthrough_trajectory_controller"],
258-
# activate_controllers=["scaled_joint_trajectory_controller"],
259-
# ).ok
260-
# )
250+
goal_tolerance = [
251+
JointTolerance(position=0.01, name=tf_prefix + joint) for joint in ROBOT_JOINTS
252+
]
253+
goal_time_tolerance = Duration(sec=0, nanosec=10)
254+
trajectory = JointTrajectory(
255+
points=[
256+
JointTrajectoryPoint(positions=pos, time_from_start=times)
257+
for (times, pos) in TEST_TRAJECTORY
258+
],
259+
joint_names=[tf_prefix + joint for joint in ROBOT_JOINTS],
260+
)
261+
goal_handle = self._passthrough_forward_joint_trajectory.send_goal(
262+
trajectory=trajectory,
263+
goal_time_tolerance=goal_time_tolerance,
264+
goal_tolerance=goal_tolerance,
265+
)
266+
self.assertTrue(goal_handle.accepted)
267+
if goal_handle.accepted:
268+
result = self._passthrough_forward_joint_trajectory.get_result(
269+
goal_handle, TIMEOUT_EXECUTE_TRAJECTORY
270+
)
271+
self.assertEqual(
272+
result.error_code, FollowJointTrajectory.Result.GOAL_TOLERANCE_VIOLATED
273+
)
274+
self.assertTrue(
275+
self._controller_manager_interface.switch_controller(
276+
strictness=SwitchController.Request.BEST_EFFORT,
277+
deactivate_controllers=["passthrough_trajectory_controller"],
278+
activate_controllers=["scaled_joint_trajectory_controller"],
279+
).ok
280+
)

0 commit comments

Comments
 (0)