Exemplos de MoveGroupInterface em Python

Exemplo n.º 1

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))

Exemplo n.º 2

Arquivo: robot_state_update.py Projeto: ros-planning/moveit

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))

Exemplo n.º 3

Arquivo: test_cleanup.py Projeto: huangguoqing1111/MAEG5755-Project

#!/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())

Exemplo n.º 4

Arquivo: python_move_group.py Projeto: devin91/moveit_0.9.15

 def setUpClass(self):
     self.group = MoveGroupInterface(self.PLANNING_GROUP,
                                     "robot_description",
                                     rospy.get_namespace())

Exemplo n.º 5

Arquivo: python_move_group.py Projeto: devin91/moveit_0.9.15

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))

Exemplo n.º 6

Arquivo: python_move_group.py Projeto: VSilvaa/catkin_src

 def setUpClass(self):
     self.group = MoveGroupInterface(self.PLANNING_GROUP,
                                     "robot_description")

Exemplo n.º 7

Arquivo: python_move_group.py Projeto: ros-planning/moveit

 def setUpClass(self):
     self.group = MoveGroupInterface(self.PLANNING_GROUP, "robot_description", rospy.get_namespace())

Exemplo n.º 8

Arquivo: python_move_group.py Projeto: ros-planning/moveit

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))

Exemplo n.º 9

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)

Exemplo n.º 10

Arquivo: python_move_group_ns.py Projeto: ros-planning/moveit

 def setUpClass(self):
     self.group = MoveGroupInterface(self.PLANNING_GROUP, "%srobot_description" % self.PLANNING_NS, self.PLANNING_NS)

Exemplo n.º 11

#!/usr/bin/env python

from moveit_ros_planning_interface._moveit_move_group_interface import MoveGroupInterface
group = MoveGroupInterface("manipulator", "robot_description")