Beispiel #1
0
 def __init__(self):
     print("init ManipulationNode")
     rospy.init_node("Manipulation_Control")
     self.__manipulation = Manipulation()
     rospy.Service(MOVE_SERVICE, Move, self.__handle_move)
     rospy.Service(PLAN_SERVICE, Plan, self.__handle_plan)
     rospy.Service(MOVE_WITH_PLAN_SERVICE, MoveWithPlan,
                   self.__handle_move_with_plan)
     rospy.Service(ADD_COLLISION_OBJECTS_SERVICE, AddCollisionObjects,
                   self.__handle_add_objects)
     rospy.Service(GET_COLLSISION_OBJECT_SERVICE, GetCollisionObject,
                   self.__handle_get_collision_object)
     rospy.Service(MOVE_MASTCAM_SERVICE, MoveMastCam,
                   self.__handle_mast_cam)
     rospy.Service(OPEN_GRIPPER_SERVICE, OpenGripper,
                   self.__handle_open_gripper)
     rospy.Service(CLOSE_GRIPPER_SERVICE, CloseGripper,
                   self.__handle_close_gripper)
     self.__base_publisher = rospy.Publisher(BASE_ORIGIN_TOPIC,
                                             PointStamped)
     self.__eef_position_publisher = rospy.Publisher(
         GET_EEF_POSITION_TOPIC, PointStamped)
     print(
         "ASDFASDFASFLKJKDJSFKJLASDFKJLDKFDKJLFKDSFHDSKFK############################################################################"
     )
 def __init__(self):
     print("init ManipulationNode")
     rospy.init_node("Manipulation_Control")
     self.__manipulation = Manipulation()
     rospy.Service(MOVE_SERVICE, Move, self.__handle_move)
     rospy.Service(PLAN_SERVICE, Plan, self.__handle_plan)
     rospy.Service(MOVE_WITH_PLAN_SERVICE, MoveWithPlan, self.__handle_move_with_plan)
     rospy.Service(ADD_COLLISION_OBJECTS_SERVICE, AddCollisionObjects, self.__handle_add_objects)
     rospy.Service(GET_COLLSISION_OBJECT_SERVICE, GetCollisionObject, self.__handle_get_collision_object)
     rospy.Service(MOVE_MASTCAM_SERVICE, MoveMastCam, self.__handle_mast_cam)
     rospy.Service(OPEN_GRIPPER_SERVICE, OpenGripper, self.__handle_open_gripper)
     rospy.Service(CLOSE_GRIPPER_SERVICE, CloseGripper, self.__handle_close_gripper)
     self.__base_publisher = rospy.Publisher(BASE_ORIGIN_TOPIC, PointStamped)
     self.__eef_position_publisher = rospy.Publisher(GET_EEF_POSITION_TOPIC, PointStamped)
     print("ASDFASDFASFLKJKDJSFKJLASDFKJLDKFDKJLFKDSFHDSKFK############################################################################")
Beispiel #3
0
    def execute(self, userdata):
        rospy.logdebug('CamToDropzone: Executing state CamToDropzone')

        listener = TransformListener()

        if utils.manipulation is None:
            utils.manipulation = Manipulation()
            utils.manipulation.set_turbo_mode()
            utils.manipulation.set_movement_time(9)

        # TODO: Exception schmeissen wenn abort_after vergangen ist
        abort_after = 15
        then = int(time.time())
        now = int(time.time())
        while not listener.frameExists("drop_point"):
            if not (now - then < abort_after):
                rospy.logdebug('drop_point frame not found!')
                return 'fail'
            rospy.loginfo("wait for drop_point frame")
            rospy.sleep(2.)
            now = int(time.time())

        then = int(time.time())
        now = int(time.time())
        while not listener.frameExists("mdl_middle"):
            if not (now - then < abort_after):
                rospy.logdebug('CamToDropzone: mdl_middle frame not found!')
                return 'fail'
            rospy.sleep(2.)
            now = int(time.time())

        if len(userdata.scan_conveyor_pose) == 0:
            rospy.logdebug('CamToDropzone: No scanPose, calculate now...')
            p = utils.manipulation.scan_conveyor_pose()
            userdata.scan_conveyor_pose.append(p)
        else:
            rospy.logdebug('CamToDropzone: scanPose found!')
            p = userdata.scan_conveyor_pose[0]

        rospy.logdebug('CamToDropzone: Move arm to scan_conveyor_pose')

        for i in range(0, 3):
            if utils.manipulation.move_to(p):
                return 'scanPoseReached'
            else:
                if i == 2:
                    rospy.logdebug('CamToDropzone: Cant move arm to scan_conveyor_pose')
                    return 'fail'
Beispiel #4
0
    def execute(self, userdata):
        rospy.loginfo('Executing state CamToDropzone')

        if utils.manipulation is None:
            utils.manipulation = Manipulation()
            rospy.sleep(2)

        # 1. Foerderband in PS haun, wenns noch nicht drin ist.
        subscriber = rospy.Subscriber("suturo/yaml_pars0r", Task,
                                      self.addConveyorBelt)
        # 1: Dropzone Punkt ueber Vektor dp finden
        # 2: Breite des Bandes ueber 2*drop_deviation[1] bestimmen
        # 3: Hoehe des Bandes ueber drop_center_point[2] (z wert) + Banddicke (0.01m) holen
        # 4: Lange des Bandes mit mdl Vektor bestimmen + 0.10m fuer Laenge hinter dem dp Vektor
        # TODO 5: Orientierung de Bandes mit mdl Vektor bestimmen

        return 'success'
Beispiel #5
0
    def execute(self, userdata):
        rospy.loginfo('Executing state SearchObject')

        if self._missing_objects is None:
            self._missing_objects = []
            for obj in userdata.yaml.objects:
                print("adding missing object" + obj.name)
                self._missing_objects.append(obj.name)

        if utils.manipulation is None:
            utils.manipulation = Manipulation()
            rospy.sleep(2)

        # Add the found objects
        for obj in userdata.fitted_objects:
            # Check if the object was already found
            if [
                    x for x in self._found_objects
                    if x.mpe_object.id == obj.mpe_object.id
            ]:
                rospy.loginfo('Object %s was already found.' %
                              obj.mpe_object.id)
            else:
                self._found_objects.append(obj)
                if obj.mpe_object.id in self._missing_objects:
                    self._missing_objects.remove(obj.mpe_object.id)
        userdata.objects_found = self._found_objects

        rospy.loginfo('##########################################')
        rospy.loginfo('Objects found:')
        for obj in self._found_objects:
            rospy.loginfo(obj.mpe_object.id)
        rospy.loginfo('##########################################')

        # Check if all objects were found
        if not self._missing_objects:
            return 'noObjectsLeft'

        return 'searchObject'
Beispiel #6
0
    def execute(self, userdata):
        rospy.logdebug('PlaceTask6: Executing state PlaceTask6')

        listener = TransformListener()

        if utils.manipulation is None:
            utils.manipulation = Manipulation()

        # TODO: Exception schmeissen wenn abort_after vergangen ist
        abort_after = 15
        then = int(time.time())
        now = int(time.time())
        while not listener.frameExists("target_zone"):
            if not now - then < abort_after:
                rospy.logdebug("PlaceTask6: target_zone frame not found")
                return 'fail'
            rospy.loginfo("wait for target_zone frame")
            rospy.sleep(2.)
            now = int(time.time())

        self.__place_pose = self.get_place_point(userdata)

        rospy.logdebug('PlaceTask6: Move Arm to Place Pose')
        rospy.logdebug(self.__place_pose)
        for i in range(0, 7):
            if utils.manipulation.move_to(self.__place_pose):
                rospy.logdebug('PlaceTask6: OpenGripper')
                utils.manipulation.open_gripper()
                return 'success'
            else:
                rospy.logdebug('PlaceTask6: Calculate new place position...')
                if i <= 2:
                    self.__place_pose = self.get_place_point(userdata)
                if i > 2:
                    self.__place_pose = self.get_place_point(userdata, 3)
                if i == 6:
                    rospy.logdebug('PlaceTask6: Cant find place position!')
                    utils.manipulation.open_gripper()
                    return 'fail'
Beispiel #7
0
 def create_manipulation(self):
     utils.manipulation = Manipulation()
Beispiel #8
0
#!/usr/bin/env python
import rospy

from suturo_planning_manipulation.manipulation import Manipulation


if __name__ == '__main__':
    # print (lambda x: a(2, x))(3)
    rospy.init_node('head_mover', anonymous=True)
    #
    mani = Manipulation()

    print mani.get_arm_base_move_group().get_current_joint_values()
Beispiel #9
0
class ManipulationNode(object):
    def __init__(self):
        print("init ManipulationNode")
        rospy.init_node("Manipulation_Control")
        self.__manipulation = Manipulation()
        rospy.Service(MOVE_SERVICE, Move, self.__handle_move)
        rospy.Service(PLAN_SERVICE, Plan, self.__handle_plan)
        rospy.Service(MOVE_WITH_PLAN_SERVICE, MoveWithPlan,
                      self.__handle_move_with_plan)
        rospy.Service(ADD_COLLISION_OBJECTS_SERVICE, AddCollisionObjects,
                      self.__handle_add_objects)
        rospy.Service(GET_COLLSISION_OBJECT_SERVICE, GetCollisionObject,
                      self.__handle_get_collision_object)
        rospy.Service(MOVE_MASTCAM_SERVICE, MoveMastCam,
                      self.__handle_mast_cam)
        rospy.Service(OPEN_GRIPPER_SERVICE, OpenGripper,
                      self.__handle_open_gripper)
        rospy.Service(CLOSE_GRIPPER_SERVICE, CloseGripper,
                      self.__handle_close_gripper)
        self.__base_publisher = rospy.Publisher(BASE_ORIGIN_TOPIC,
                                                PointStamped)
        self.__eef_position_publisher = rospy.Publisher(
            GET_EEF_POSITION_TOPIC, PointStamped)
        print(
            "ASDFASDFASFLKJKDJSFKJLASDFKJLDKFDKJLFKDSFHDSKFK############################################################################"
        )

    def __handle_move(self, msg):
        goal_pose = self.__get_goal_pose(msg)

        if msg.type == MoveRequest.ACTION_MOVE_ARM_TO:
            result = self.__manipulation.move_to(goal_pose,
                                                 msg.do_not_blow_up_list)
        elif msg.type == MoveRequest.ACTION_MOVE_ARM_AND_BASE_TO:
            result = self.__manipulation.move_arm_and_base_to(
                goal_pose, msg.do_not_blow_up_list)
        elif msg.type == MoveRequest.ACTION_MOVE_BASE:
            result = self.__manipulation.move_base(goal_pose)
        else:
            result = False
        return MoveResponse(result)

    def __handle_plan(self, msg):
        func = None
        if msg.move_group == PlanRequest.MOVE_GROUP_ARM:
            func = self.__manipulation.plan_arm_to
        elif msg.move_group == PlanRequest.MOVE_GROUP_ARM_BASE:
            func = self.__manipulation.plan_arm_and_base_to
        goal = self.__get_goal_pose(msg)
        start_state = None
        if msg.has_start_state:
            start_state = msg.start_state
        plan = func(goal, start_state)
        return PlanResponse(plan)

    def __handle_move_with_plan(self, msg):
        result = self.__manipulation.move_with_plan_to(msg.plan)
        return MoveWithPlanResponse(result=result)

    def __handle_add_objects(self, msg):
        self.__manipulation.get_planning_scene().add_objects(msg.objects)
        return AddCollisionObjectsResponse()

    def __handle_mast_cam(self, msg):
        message = self.__manipulation.pan_tilt(msg.pan, msg.tilt)
        result = False
        if message.find("path finished") != -1:
            result = True
        return MoveMastCamResponse(result)

    def __handle_get_collision_object(self, msg):
        return self.__manipulation.get_planning_scene().get_collision_object(
            msg.name)

    def __handle_open_gripper(self, msg):
        position = manipulation_constants.gripper_max_pose
        if msg.position is not None:
            position = msg.position
        return self.__manipulation.open_gripper(position)

    def __handle_close_gripper(self, msg):
        obj = msg.object
        if obj == CollisionObject():
            obj = None
        grasp_point = msg.grasp_point
        if grasp_point == PointStamped():
            grasp_point = None
        return self.__manipulation.close_gripper(obj, grasp_point)

    def __publish(self):
        rate = rospy.Rate(10)
        while not rospy.is_shutdown():
            self.__base_publisher.publish(
                self.__manipulation.get_base_origin())
            self.__eef_position_publisher.publish(
                self.__manipulation.get_eef_position())
            rate.sleep()

    def start(self):
        publisher_thread = Thread(target=self.__publish)
        publisher_thread.start()
        while not rospy.is_shutdown():
            try:
                rospy.spin()
            except KeyboardInterrupt:
                break

    def __get_goal_pose(self, msg):
        if msg.goal_pose_name != '':
            return msg.goal_pose_name
        else:
            return msg.goal_pose
Beispiel #10
0
#     dest.point = Point( -0.85, 0.85, 0.00)
#     mani.place_and_move(dest)
#
#     mani.grasp_and_move("green_cylinder")
#
#     dest = PointStamped()
#     dest.header.frame_id = "/odom_combined"
#     dest.point = Point(-0.85, -0.85, 0)
#     mani.place_and_move(dest)



if __name__ == '__main__':
    rospy.init_node('head_mover', anonymous=True)

    mani = Manipulation()
    # test_task3(mani)

    num_of_scans = 12
    max_rad = 5.9
    rad_per_step = max_rad / num_of_scans

    #x is zwischen 0 und num_of_scans
    x = 5
    rad = x * rad_per_step - 2.945
    rospy.loginfo('Turning arm %s' % str(rad))
    mani.turn_arm(rad)

    # co = mani.get_planning_scene().get_collision_object("red_cube")
    # print mani.calc_object_weight(co, 2710)
    # print mani.get_arm_move_group().get_current_joint_values()
Beispiel #11
0
    # dest.header.frame_id = "/odom_combined"
    # dest.point = Point( -0.85, 0.85, 0.00)
    # mani.place_and_move(dest)

    # mani.grasp_and_move("green_cylinder")
    #
    # dest = PointStamped()
    # dest.header.frame_id = "/odom_combined"
    # dest.point = Point(-0.85, -0.85, 0)
    # mani.place_and_move(dest)


if __name__ == '__main__':
    rospy.init_node('head_mover', log_level=rospy.DEBUG)

    m = Manipulation()
    # print m.get_arm_base_move_group().get_goal_orientation_tolerance()
    # print m.get_arm_base_move_group().get_goal_position_tolerance()
    # print m.get_arm_base_move_group().get_goal_joint_tolerance()
    # print m.get_arm_base_move_group().get_goal_tolerance()
    # print m.get_arm_move_group().get
    # m.grasp("cyan_cylinder")
    # m.open_gripper()
    # m.close_gripper()
    # m.get_arm_move_group().set_named_target("scan_pose1")
    # print m.get_arm_move_group().get_goal_tolerance()
    # print m.get_arm_move_group().get_goal_position_tolerance()
    # print m.get_arm_move_group().get_goal_orientation_tolerance()
    # rospy.sleep(2)
    # test_task1(m)
Beispiel #12
0
    def execute(self, userdata):
        rospy.loginfo('Executing state ScanMapMastCam')
        if utils.manipulation is None:
            utils.manipulation = Manipulation(userdata.yaml)
            rospy.sleep(2)

        utils.map = Map(2)
        # arm_base = utils.manipulation.get_base_origin()
        # rospy.sleep(4)

        utils.manipulation.pan_tilt(0.2, 0.5)
        rospy.sleep(utils.waiting_time_before_scan)
        utils.map.add_point_cloud(scene_cam=True)

        utils.manipulation.pan_tilt(0.2825, 0.775)
        rospy.sleep(utils.waiting_time_before_scan)
        utils.map.add_point_cloud(scene_cam=True)

        utils.manipulation.pan_tilt(0, 1.1)
        rospy.sleep(utils.waiting_time_before_scan)
        utils.map.add_point_cloud(scene_cam=True)

        utils.manipulation.pan_tilt(-0.2825, 0.775)
        rospy.sleep(utils.waiting_time_before_scan)
        utils.map.add_point_cloud(scene_cam=True)

        utils.manipulation.pan_tilt(-0.2, 0.5)
        rospy.sleep(utils.waiting_time_before_scan)
        utils.map.add_point_cloud(scene_cam=True)
        utils.manipulation.pan_tilt(0, 0.45)

        if not userdata.enable_movement:
            return 'mapScanned'

        co = utils.map.to_collision_object()
        utils.manipulation.get_planning_scene().add_object(co)
        # utils.manipulation.get_planning_scene().add_object(co)
        rospy.sleep(2)
        n = 8
        for i in range(0, n):
            a = 2 * pi * ((i + 0.0) / (n + 0.0))

            goal_base_pose = PoseStamped()
            goal_base_pose.header.frame_id = "/odom_combined"

            goal_base_pose.pose.position = Point(cos(a), sin(a), 0)
            if goal_base_pose.pose.position.x < 0 and goal_base_pose.pose.position.y < 0 or \
                                    goal_base_pose.pose.position.x > 0 and goal_base_pose.pose.position.y > 0:
                continue

            goal_base_pose.pose.position = set_vector_length(
                0.25, goal_base_pose.pose.position)

            goal_base_pose.pose.orientation.w = 1

            if utils.manipulation.move_base(goal_base_pose):
                rospy.sleep(utils.waiting_time_before_scan)
                utils.map.add_point_cloud(
                    arm_origin=utils.manipulation.get_base_origin().point,
                    scene_cam=True)
                break

        # utils.map.add_point_cloud(arm_origin=arm_base, scene_cam=True)
        # utils.map.add_point_cloud(arm_origin=arm_base, scene_cam=True)

        # utils.map.publish_as_marker()

        if userdata.yaml.task_type == Task.TASK_5:
            utils.map.all_unknowns_to_obstacle()
            #utils.map.remove_puzzle_fixture(userdata.yaml)

        co = utils.map.to_collision_object()
        utils.manipulation.get_planning_scene().add_object(co)
        return 'mapScanned'
Beispiel #13
0
#     dest.header.frame_id = "/odom_combined"
#     # dest.point = Point(-0.3, -0.4, 0.03)
#     dest.point = Point( -0.85, 0.85, 0.00)
#     mani.place_and_move(dest)
#
#     mani.grasp_and_move("green_cylinder")
#
#     dest = PointStamped()
#     dest.header.frame_id = "/odom_combined"
#     dest.point = Point(-0.85, -0.85, 0)
#     mani.place_and_move(dest)

if __name__ == '__main__':
    rospy.init_node('head_mover', anonymous=True)

    mani = Manipulation()
    # test_task3(mani)

    num_of_scans = 12
    max_rad = 5.9
    rad_per_step = max_rad / num_of_scans

    #x is zwischen 0 und num_of_scans
    x = 5
    rad = x * rad_per_step - 2.945
    rospy.loginfo('Turning arm %s' % str(rad))
    mani.turn_arm(rad)

    # co = mani.get_planning_scene().get_collision_object("red_cube")
    # print mani.calc_object_weight(co, 2710)
    # print mani.get_arm_move_group().get_current_joint_values()
Beispiel #14
0
from math import pi
from geometry_msgs.msg import Quaternion
import rospy
from suturo_planning_manipulation.force_test import plan_to
from suturo_planning_manipulation.manipulation import Manipulation, geometry_msgs
from suturo_planning_manipulation.mathemagie import euler_to_quaternion

__author__ = 'benny'

if __name__ == '__main__':
    rospy.init_node('force_test', anonymous=True)
    m = Manipulation()
    t_point = geometry_msgs.msg.Pose()
    t_point.position.x = -0.4
    t_point.position.y = 0
    t_point.position.z = 0.3
    t_point.orientation = euler_to_quaternion(0, pi, 0)
    m.direct_move(plan_to(t_point, m))
    m.open_gripper()
class ManipulationNode(object):

    def __init__(self):
        print("init ManipulationNode")
        rospy.init_node("Manipulation_Control")
        self.__manipulation = Manipulation()
        rospy.Service(MOVE_SERVICE, Move, self.__handle_move)
        rospy.Service(PLAN_SERVICE, Plan, self.__handle_plan)
        rospy.Service(MOVE_WITH_PLAN_SERVICE, MoveWithPlan, self.__handle_move_with_plan)
        rospy.Service(ADD_COLLISION_OBJECTS_SERVICE, AddCollisionObjects, self.__handle_add_objects)
        rospy.Service(GET_COLLSISION_OBJECT_SERVICE, GetCollisionObject, self.__handle_get_collision_object)
        rospy.Service(MOVE_MASTCAM_SERVICE, MoveMastCam, self.__handle_mast_cam)
        rospy.Service(OPEN_GRIPPER_SERVICE, OpenGripper, self.__handle_open_gripper)
        rospy.Service(CLOSE_GRIPPER_SERVICE, CloseGripper, self.__handle_close_gripper)
        self.__base_publisher = rospy.Publisher(BASE_ORIGIN_TOPIC, PointStamped)
        self.__eef_position_publisher = rospy.Publisher(GET_EEF_POSITION_TOPIC, PointStamped)
        print("ASDFASDFASFLKJKDJSFKJLASDFKJLDKFDKJLFKDSFHDSKFK############################################################################")
    
    def __handle_move(self, msg):
        goal_pose = self.__get_goal_pose(msg)

        if msg.type == MoveRequest.ACTION_MOVE_ARM_TO:
            result = self.__manipulation.move_to(goal_pose, msg.do_not_blow_up_list)
        elif msg.type == MoveRequest.ACTION_MOVE_ARM_AND_BASE_TO:
            result = self.__manipulation.move_arm_and_base_to(goal_pose, msg.do_not_blow_up_list)
        elif msg.type == MoveRequest.ACTION_MOVE_BASE:
            result = self.__manipulation.move_base(goal_pose)
        else:
            result = False
        return MoveResponse(result)

    def __handle_plan(self, msg):
        func = None
        if msg.move_group == PlanRequest.MOVE_GROUP_ARM:
            func = self.__manipulation.plan_arm_to
        elif msg.move_group == PlanRequest.MOVE_GROUP_ARM_BASE:
            func = self.__manipulation.plan_arm_and_base_to
        goal = self.__get_goal_pose(msg)
        start_state = None
        if msg.has_start_state:
            start_state = msg.start_state
        plan = func(goal, start_state)
        return PlanResponse(plan)

    def __handle_move_with_plan(self, msg):
        result = self.__manipulation.move_with_plan_to(msg.plan)
        return MoveWithPlanResponse(result=result)

    def __handle_add_objects(self, msg):
        self.__manipulation.get_planning_scene().add_objects(msg.objects)
        return AddCollisionObjectsResponse()

    def __handle_mast_cam(self, msg):
        message = self.__manipulation.pan_tilt(msg.pan, msg.tilt)
        result = False
        if message.find("path finished") != -1:
            result = True
        return MoveMastCamResponse(result)

    def __handle_get_collision_object(self, msg):
        return self.__manipulation.get_planning_scene().get_collision_object(msg.name)

    def __handle_open_gripper(self, msg):
        position = manipulation_constants.gripper_max_pose
        if msg.position is not None:
            position = msg.position
        return self.__manipulation.open_gripper(position)

    def __handle_close_gripper(self, msg):
        obj = msg.object
        if obj == CollisionObject():
            obj = None
        grasp_point = msg.grasp_point
        if grasp_point == PointStamped():
            grasp_point = None
        return self.__manipulation.close_gripper(obj, grasp_point)

    def __publish(self):
        rate = rospy.Rate(10)
        while not rospy.is_shutdown():
            self.__base_publisher.publish(self.__manipulation.get_base_origin())
            self.__eef_position_publisher.publish(self.__manipulation.get_eef_position())
            rate.sleep()

    def start(self):
        publisher_thread = Thread(target=self.__publish)
        publisher_thread.start()
        while not rospy.is_shutdown():
            try:
                rospy.spin()
            except KeyboardInterrupt:
                break

    def __get_goal_pose(self, msg):
        if msg.goal_pose_name != '':
            return msg.goal_pose_name
        else:
            return msg.goal_pose
Beispiel #16
0
    # dest.header.frame_id = "/odom_combined"
    # dest.point = Point( -0.85, 0.85, 0.00)
    # mani.place_and_move(dest)

    # mani.grasp_and_move("green_cylinder")
    #
    # dest = PointStamped()
    # dest.header.frame_id = "/odom_combined"
    # dest.point = Point(-0.85, -0.85, 0)
    # mani.place_and_move(dest)


if __name__ == "__main__":
    rospy.init_node("head_mover", log_level=rospy.DEBUG)

    m = Manipulation()
    # print m.get_arm_base_move_group().get_goal_orientation_tolerance()
    # print m.get_arm_base_move_group().get_goal_position_tolerance()
    # print m.get_arm_base_move_group().get_goal_joint_tolerance()
    # print m.get_arm_base_move_group().get_goal_tolerance()
    # print m.get_arm_move_group().get
    # m.grasp("cyan_cylinder")
    # m.open_gripper()
    # m.close_gripper()
    # m.get_arm_move_group().set_named_target("scan_pose1")
    # print m.get_arm_move_group().get_goal_tolerance()
    # print m.get_arm_move_group().get_goal_position_tolerance()
    # print m.get_arm_move_group().get_goal_orientation_tolerance()
    # rospy.sleep(2)
    # test_task1(m)