class RobotStateUpdateTest(unittest.TestCase): PLANNING_GROUP = "manipulator" @classmethod def setUpClass(self): self.group = MoveGroupInterface(self.PLANNING_GROUP, "robot_description", rospy.get_namespace()) @classmethod def tearDown(self): pass def plan(self, target): self.group.set_joint_value_target(target) return self.group.plan() def test(self): current = np.asarray(self.group.get_current_joint_values()) for i in range(30): target = current + np.random.uniform(-0.5, 0.5, size=current.shape) # if plan was successfully executed, current state should be reported at target error_code1, plan, time = self.plan(target) error_code = MoveItErrorCodes() error_code.deserialize(error_code1) if (error_code.val == MoveItErrorCodes.SUCCESS) and self.group.execute(plan): actual = np.asarray(self.group.get_current_joint_values()) self.assertTrue( np.allclose(target, actual, atol=1e-4, rtol=0.0)) # otherwise current state should be still the same else: actual = np.asarray(self.group.get_current_joint_values()) self.assertTrue( np.allclose(current, actual, atol=1e-4, rtol=0.0))
class RobotStateUpdateTest(unittest.TestCase): PLANNING_GROUP = "manipulator" @classmethod def setUpClass(self): self.group = MoveGroupInterface(self.PLANNING_GROUP, "robot_description", rospy.get_namespace()) @classmethod def tearDown(self): pass def plan(self, target): self.group.set_joint_value_target(target) return self.group.plan() def test(self): current = np.asarray(self.group.get_current_joint_values()) for i in range(30): target = current + np.random.uniform(-0.5, 0.5, size=current.shape) # if plan was successfully executed, current state should be reported at target error_code1, plan, time = self.plan(target) error_code = MoveItErrorCodes() error_code.deserialize(error_code1) if (error_code.val == MoveItErrorCodes.SUCCESS) and self.group.execute(plan): actual = np.asarray(self.group.get_current_joint_values()) self.assertTrue(np.allclose(target, actual, atol=1e-4, rtol=0.0)) # otherwise current state should be still the same else: actual = np.asarray(self.group.get_current_joint_values()) self.assertTrue(np.allclose(current, actual, atol=1e-4, rtol=0.0))
#!/usr/bin/env python import rospy from moveit_ros_planning_interface._moveit_move_group_interface import ( MoveGroupInterface, ) group = MoveGroupInterface("manipulator", "robot_description", rospy.get_namespace())
def setUpClass(self): self.group = MoveGroupInterface(self.PLANNING_GROUP, "robot_description", rospy.get_namespace())
class PythonMoveGroupTest(unittest.TestCase): PLANNING_GROUP = "manipulator" @classmethod def setUpClass(self): self.group = MoveGroupInterface(self.PLANNING_GROUP, "robot_description", rospy.get_namespace()) @classmethod def tearDown(self): pass def check_target_setting(self, expect, *args): if len(args) == 0: args = [expect] self.group.set_joint_value_target(*args) res = self.group.get_joint_value_target() self.assertTrue( np.all(np.asarray(res) == np.asarray(expect)), "Setting failed for %s, values: %s" % (type(args[0]), res)) def test_target_setting(self): n = self.group.get_variable_count() self.check_target_setting([0.1] * n) self.check_target_setting((0.2, ) * n) self.check_target_setting(np.zeros(n)) self.check_target_setting( [0.3] * n, {name: 0.3 for name in self.group.get_active_joints()}) self.check_target_setting([0.5] + [0.3] * (n - 1), "joint_1", 0.5) def plan(self, target): self.group.set_joint_value_target(target) return self.group.compute_plan() def test_validation(self): current = np.asarray(self.group.get_current_joint_values()) plan1 = self.plan(current + 0.2) plan2 = self.plan(current + 0.2) # first plan should execute self.assertTrue(self.group.execute(plan1)) # second plan should be invalid now (due to modified start point) and rejected self.assertFalse(self.group.execute(plan2)) # newly planned trajectory should execute again plan3 = self.plan(current) self.assertTrue(self.group.execute(plan3))
def setUpClass(self): self.group = MoveGroupInterface(self.PLANNING_GROUP, "robot_description")
def setUpClass(self): self.group = MoveGroupInterface(self.PLANNING_GROUP, "robot_description", rospy.get_namespace())
class PythonMoveGroupTest(unittest.TestCase): PLANNING_GROUP = "manipulator" @classmethod def setUpClass(self): self.group = MoveGroupInterface(self.PLANNING_GROUP, "robot_description", rospy.get_namespace()) @classmethod def tearDown(self): pass def check_target_setting(self, expect, *args): if len(args) == 0: args = [expect] self.group.set_joint_value_target(*args) res = self.group.get_joint_value_target() self.assertTrue(np.all(np.asarray(res) == np.asarray(expect)), "Setting failed for %s, values: %s" % (type(args[0]), res)) def test_target_setting(self): n = self.group.get_variable_count() self.check_target_setting([0.1] * n) self.check_target_setting((0.2,) * n) self.check_target_setting(np.zeros(n)) self.check_target_setting([0.3] * n, {name: 0.3 for name in self.group.get_active_joints()}) self.check_target_setting([0.5] + [0.3]*(n-1), "joint_1", 0.5) def plan(self, target): self.group.set_joint_value_target(target) return self.group.plan() def test_validation(self): current = np.asarray(self.group.get_current_joint_values()) error_code1, plan1, time = self.plan(current + 0.2) error_code2, plan2, time = self.plan(current + 0.2) # both plans should have succeeded: error_code = MoveItErrorCodes() error_code.deserialize(error_code1) self.assertEqual(error_code.val, MoveItErrorCodes.SUCCESS) error_code = MoveItErrorCodes() error_code.deserialize(error_code2) self.assertEqual(error_code.val, MoveItErrorCodes.SUCCESS) # first plan should execute self.assertTrue(self.group.execute(plan1)) # second plan should be invalid now (due to modified start point) and rejected self.assertFalse(self.group.execute(plan2)) # newly planned trajectory should execute again error_code3, plan3, time = self.plan(current) error_code = MoveItErrorCodes() error_code.deserialize(error_code3) self.assertEqual(error_code.val, MoveItErrorCodes.SUCCESS) self.assertTrue(self.group.execute(plan3))
class PythonMoveGroupNsTest(unittest.TestCase): PLANNING_GROUP = "manipulator" PLANNING_NS = "test_ns/" @classmethod def setUpClass(self): self.group = MoveGroupInterface( self.PLANNING_GROUP, "%srobot_description" % self.PLANNING_NS, self.PLANNING_NS, ) @classmethod def tearDown(self): pass def check_target_setting(self, expect, *args): if len(args) == 0: args = [expect] self.group.set_joint_value_target(*args) res = self.group.get_joint_value_target() self.assertTrue( np.all(np.asarray(res) == np.asarray(expect)), "Setting failed for %s, values: %s" % (type(args[0]), res), ) def test_target_setting(self): n = self.group.get_variable_count() self.check_target_setting([0.1] * n) self.check_target_setting((0.2, ) * n) self.check_target_setting(np.zeros(n)) self.check_target_setting( [0.3] * n, {name: 0.3 for name in self.group.get_active_joints()}) self.check_target_setting([0.5] + [0.3] * (n - 1), "joint_1", 0.5) def plan(self, target): self.group.set_joint_value_target(target) return self.group.plan() def test_validation(self): current = np.asarray(self.group.get_current_joint_values()) error_code1, plan1, time = self.plan(current + 0.2) error_code2, plan2, time = self.plan(current + 0.2) # both plans should have succeeded: error_code = MoveItErrorCodes() error_code.deserialize(error_code1) self.assertEqual(error_code.val, MoveItErrorCodes.SUCCESS) error_code = MoveItErrorCodes() error_code.deserialize(error_code2) self.assertEqual(error_code.val, MoveItErrorCodes.SUCCESS) # first plan should execute self.assertTrue(self.group.execute(plan1)) # second plan should be invalid now (due to modified start point) and rejected self.assertFalse(self.group.execute(plan2)) # newly planned trajectory should execute again error_code3, plan3, time = self.plan(current) self.assertTrue(self.group.execute(plan3)) error_code = MoveItErrorCodes() error_code.deserialize(error_code3) self.assertEqual(error_code.val, MoveItErrorCodes.SUCCESS)
def setUpClass(self): self.group = MoveGroupInterface(self.PLANNING_GROUP, "%srobot_description" % self.PLANNING_NS, self.PLANNING_NS)
#!/usr/bin/env python from moveit_ros_planning_interface._moveit_move_group_interface import MoveGroupInterface group = MoveGroupInterface("manipulator", "robot_description")