moveit2
The MoveIt Motion Planning Framework for ROS 2.
python_move_group.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 import unittest
4 import numpy as np
5 import rospy
6 import rostest
7 import os
8 
9 from moveit_ros_planning_interface._moveit_move_group_interface import (
10  MoveGroupInterface,
11 )
12 from moveit_msgs.msg import MoveItErrorCodes
13 
14 
15 class PythonMoveGroupTest(unittest.TestCase):
16  PLANNING_GROUP = "manipulator"
17 
18  @classmethod
19  def setUpClass(self):
20  self.groupgroup = MoveGroupInterface(
21  self.PLANNING_GROUPPLANNING_GROUP, "robot_description", rospy.get_namespace()
22  )
23 
24  @classmethod
25  def tearDown(self):
26  pass
27 
28  def check_target_setting(self, expect, *args):
29  if len(args) == 0:
30  args = [expect]
31  self.group.set_joint_value_target(*args)
32  res = self.group.get_joint_value_target()
33  self.assertTrue(
34  np.all(np.asarray(res) == np.asarray(expect)),
35  "Setting failed for %s, values: %s" % (type(args[0]), res),
36  )
37 
39  n = self.groupgroup.get_variable_count()
40  self.check_target_settingcheck_target_setting([0.1] * n)
41  self.check_target_settingcheck_target_setting((0.2,) * n)
42  self.check_target_settingcheck_target_setting(np.zeros(n))
43  self.check_target_settingcheck_target_setting(
44  [0.3] * n, {name: 0.3 for name in self.groupgroup.get_active_joints()}
45  )
46  self.check_target_settingcheck_target_setting([0.5] + [0.3] * (n - 1), "joint_1", 0.5)
47 
48  def plan(self, target):
49  self.groupgroup.set_joint_value_target(target)
50  return self.groupgroup.plan()
51 
52  def test_validation(self):
53  current = np.asarray(self.groupgroup.get_current_joint_values())
54 
55  error_code1, plan1, time = self.planplan(current + 0.2)
56  error_code2, plan2, time = self.planplan(current + 0.2)
57 
58  # both plans should have succeeded:
59  error_code = MoveItErrorCodes()
60  error_code.deserialize(error_code1)
61  self.assertEqual(error_code.val, MoveItErrorCodes.SUCCESS)
62  error_code = MoveItErrorCodes()
63  error_code.deserialize(error_code2)
64  self.assertEqual(error_code.val, MoveItErrorCodes.SUCCESS)
65 
66  # first plan should execute
67  self.assertTrue(self.groupgroup.execute(plan1))
68 
69  # second plan should be invalid now (due to modified start point) and rejected
70  self.assertFalse(self.groupgroup.execute(plan2))
71 
72  # newly planned trajectory should execute again
73  error_code3, plan3, time = self.planplan(current)
74  error_code = MoveItErrorCodes()
75  error_code.deserialize(error_code3)
76  self.assertEqual(error_code.val, MoveItErrorCodes.SUCCESS)
77  self.assertTrue(self.groupgroup.execute(plan3))
78 
80  current = self.groupgroup.get_current_joint_values()
81  result = self.groupgroup.get_jacobian_matrix(current)
82  # Value check by known value at the initial pose
83  expected = np.array(
84  [
85  [0.0, 0.8, -0.2, 0.0, 0.0, 0.0],
86  [0.89, 0.0, 0.0, 0.0, 0.0, 0.0],
87  [0.0, -0.74, 0.74, 0.0, 0.1, 0.0],
88  [0.0, 0.0, 0.0, -1.0, 0.0, -1.0],
89  [0.0, 1.0, -1.0, 0.0, -1.0, 0.0],
90  [1.0, 0.0, 0.0, 0.0, 0.0, 0.0],
91  ]
92  )
93  self.assertTrue(np.allclose(result, expected))
94 
95  result = self.groupgroup.get_jacobian_matrix(current, [1.0, 1.0, 1.0])
96  expected = np.array(
97  [
98  [1.0, 1.8, -1.2, 0.0, -1.0, 0.0],
99  [1.89, 0.0, 0.0, 1.0, 0.0, 1.0],
100  [0.0, -1.74, 1.74, 1.0, 1.1, 1.0],
101  [0.0, 0.0, 0.0, -1.0, 0.0, -1.0],
102  [0.0, 1.0, -1.0, 0.0, -1.0, 0.0],
103  [1.0, 0.0, 0.0, 0.0, 0.0, 0.0],
104  ]
105  )
106 
107 
108 if __name__ == "__main__":
109  PKGNAME = "moveit_ros_planning_interface"
110  NODENAME = "moveit_test_python_move_group"
111  rospy.init_node(NODENAME)
112  rostest.rosrun(PKGNAME, NODENAME, PythonMoveGroupTest)
def check_target_setting(self, expect, *args)
Definition: plan.py:1