9 from moveit_ros_planning_interface._moveit_move_group_interface
import MoveGroupInterface
13 PLANNING_GROUP =
"manipulator" 26 self.group.set_joint_value_target(*args)
27 res = self.group.get_joint_value_target()
28 self.assertTrue(np.all(np.asarray(res) == np.asarray(expect)),
29 "Setting failed for %s, values: %s" % (type(args[0]), res))
32 n = self.group.get_variable_count()
40 self.group.set_joint_value_target(target)
41 return self.group.compute_plan()
44 current = np.asarray(self.group.get_current_joint_values())
46 plan1 = self.
plan(current + 0.2)
47 plan2 = self.
plan(current + 0.2)
50 self.assertTrue(self.group.execute(plan1))
53 self.assertFalse(self.group.execute(plan2))
56 plan3 = self.
plan(current)
57 self.assertTrue(self.group.execute(plan3))
60 current = self.group.get_current_joint_values()
61 result = self.group.get_jacobian_matrix(current)
63 expected = np.array([[ 0. , 0.8 , -0.2 , 0. , 0. , 0. ],
64 [ 0.89, 0. , 0. , 0. , 0. , 0. ],
65 [ 0. , -0.74, 0.74, 0. , 0.1 , 0. ],
66 [ 0. , 0. , 0. , -1. , 0. , -1. ],
67 [ 0. , 1. , -1. , 0. , -1. , 0. ],
68 [ 1. , 0. , 0. , 0. , 0. , 0. ]])
69 self.assertTrue(np.allclose(result, expected))
72 if __name__ ==
'__main__':
73 PKGNAME =
'moveit_ros_planning_interface' 74 NODENAME =
'moveit_test_python_move_group' 75 rospy.init_node(NODENAME)
76 rostest.rosrun(PKGNAME, NODENAME, PythonMoveGroupTest)
def test_target_setting(self)
def test_get_jacobian_matrix(self)
def check_target_setting(self, expect, args)
def test_validation(self)