9 from moveit_ros_planning_interface._moveit_move_group_interface
import MoveGroupInterface
13 PLANNING_GROUP =
"manipulator" 24 self.group.set_joint_value_target(target)
25 return self.group.compute_plan()
28 current = np.asarray(self.group.get_current_joint_values())
30 target = current + np.random.uniform(-0.5, 0.5, size = current.shape)
32 if self.group.execute(self.
plan(target)):
33 actual = np.asarray(self.group.get_current_joint_values())
34 self.assertTrue(np.allclose(target, actual, atol=1e-4, rtol=0.0))
37 actual = np.asarray(self.group.get_current_joint_values())
38 self.assertTrue(np.allclose(current, actual, atol=1e-4, rtol=0.0))
41 if __name__ ==
'__main__':
42 PKGNAME =
'moveit_ros_planning_interface' 43 NODENAME =
'moveit_test_robot_state_update' 44 rospy.init_node(NODENAME)
45 rostest.rosrun(PKGNAME, NODENAME, RobotStateUpdateTest)