Python MoveGroupCommander.compute_cartesian_path方法代码示例

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import compute_cartesian_path [as 别名]
    def __init__(self):

        moveit_commander.roscpp_initialize(sys.argv)
        rospy.init_node('cobra_test_cartesian', anonymous=True)

        arm = MoveGroupCommander(ARM_GROUP)

        arm.allow_replanning(True)

        rospy.sleep(2)

        pose = PoseStamped().pose

        # same orientation for all
        q = quaternion_from_euler(0.0, 0.0, 0.0)  # roll, pitch, yaw
        pose.orientation = Quaternion(*q)

        i = 1
        while i <= 10:
            waypoints = list()
            j = 1
            while j <= 5:
                # random pose
                rand_pose = arm.get_random_pose(arm.get_end_effector_link())
                pose.position.x = rand_pose.pose.position.x
                pose.position.y = rand_pose.pose.position.y
                pose.position.z = rand_pose.pose.position.z
                waypoints.append(copy.deepcopy(pose))
                j += 1

            (plan, fraction) = arm.compute_cartesian_path(
                                      waypoints,   # waypoints to follow
                                      0.01,        # eef_step
                                      0.0)         # jump_threshold

            print "====== waypoints created ======="
            # print waypoints

            # ======= show cartesian path ====== #
            arm.go()
            rospy.sleep(10)
            i += 1

        print "========= end ========="

        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import compute_cartesian_path [as 别名]
    right_arm.execute(plan1)
    print "============ Waiting while RVIZ executes plan1..."
    rospy.sleep(5)
    waypoints = []
    waypoints.append(right_arm.get_current_pose().pose)
    print right_arm.get_current_pose().pose

    points = 20
    for i in xrange(points):
        wpose = geometry_msgs.msg.Pose()
        wpose.orientation.w = waypoints[i-1].orientation.w + 0.5285
        wpose.orientation.x = waypoints[i-1].orientation.x - 0.6736
        wpose.orientation.y = waypoints[i-1].orientation.y - 1.638
        wpose.orientation.z = waypoints[i-1].orientation.z - 0.308
        wpose.position.y = waypoints[i-1].position.y - 0.1
        wpose.position.z = waypoints[i-1].position.z 
        wpose.position.x = waypoints[i-1].position.x + 0.5
        
        waypoints.append(copy.deepcopy(wpose))

    
    (right, fraction) = right_arm.compute_cartesian_path(
                                                         waypoints,   # waypoints to follow
                                                         0.01,        # eef_step
                                                         0.0)         # jump_threshold
    
    print "============ Waiting while RVIZ displays Right..."
    rospy.sleep(5)
    right_arm.execute(right)

    roscpp_shutdown()

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import compute_cartesian_path [as 别名]
	while rospy.get_time() == 0.0: pass
	
	### Create a handle for the Planning Scene Interface
	psi = PlanningSceneInterface()
	rospy.sleep(1.0)
	
	### Create a handle for the Move Group Commander
	mgc = MoveGroupCommander("arm")
	rospy.sleep(1.0)
	
	
	### Add virtual obstacle
	pose = gen_pose(pos=[-0.2, -0.1, 1.2])
	psi.add_box("box", pose, size=(0.15, 0.15, 0.6))
	rospy.sleep(1.0)
	
	### Move to stored joint position
	mgc.set_named_target("left")
	mgc.go()
	
	### Move to Cartesian position
	goal_pose = gen_pose(pos=[0.123, -0.417, 1.361], euler=[3.1415, 0.0, 1.5707])
	mgc.go(goal_pose.pose)
	
	### Move Cartesian linear
	goal_pose.pose.position.z -= 0.1
	(traj,frac) = mgc.compute_cartesian_path([goal_pose.pose], 0.01, 4, True)
	mgc.execute(traj)
	
	print "Done"

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import compute_cartesian_path [as 别名]
    rospy.sleep(5)
    waypoints = []
    waypoints.append(right_arm.get_current_pose().pose)
    print right_arm.get_current_pose().pose

    gain = 0.1
    points = 5
    for i in xrange(points):
        start_pose = geometry_msgs.msg.Pose()
        start_pose.orientation.w = waypoints[i - 1].orientation.w
        start_pose.orientation.x = waypoints[i - 1].orientation.x
        start_pose.orientation.y = waypoints[i - 1].orientation.y
        start_pose.orientation.z = waypoints[i - 1].orientation.z
        start_pose.position.y = waypoints[i - 1].position.y - 0.096416
        start_pose.position.z = waypoints[i - 1].position.z + 0.2
        start_pose.position.x = waypoints[i - 1].position.x - 0.01

        waypoints.append(copy.deepcopy(start_pose))

    (plan_waypoints, fraction) = right_arm.compute_cartesian_path(
        waypoints, 0.01, 0.0  # waypoints to follow  # eef_step
    )  # jump_threshold
    print fraction * 100, "% planned"

    print "============ Waiting while RVIZ displays come..."
    rospy.sleep(5)
    right_arm.execute(plan_waypoints)
    print "============ Waiting while RVIZ execute..."

    roscpp_shutdown()

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import compute_cartesian_path [as 别名]

#.........这里部分代码省略.........
        for _ in range(5):
            self.move_tip(y=0.01)
            time.sleep(0.1)
            
        self.check_fingers_collisions(True)
        
    def swap_object(self, new_model_name):
        """
        Replaces the current object with a new one.Replaces
        
        @new_model_name the name of the folder in which the object is (e.g. beer)
        """
        try:
            self.__delete_model(self.__current_model_name)
        except:
            rospy.logwarn("Failed to delete: " + self.__current_model_name)
        
        try:
            sdf = None
            initial_pose = Pose()
            initial_pose.position.x = 0.15
            initial_pose.position.z = 0.82
            
            with open(self.__path_to_models + new_model_name + "/model.sdf", "r") as model:
                sdf = model.read()
            res = self.__spawn_model(new_model_name, sdf, "", initial_pose, "world")
            rospy.logerr( "RES: " + str(res) )
            self.__current_model_name = new_model_name
        except:
            rospy.logwarn("Failed to delete: " + self.__current_model_name)
   
   

    def __compute_arm_target_for_ball(self):
        ball_pose = self.get_object_pose()

        # come at it from the top
        arm_target = ball_pose
        arm_target.position.z += 0.5

        quaternion = quaternion_from_euler(-pi/2., 0.0, 0.0)
        arm_target.orientation.x = quaternion[0]
        arm_target.orientation.y = quaternion[1]
        arm_target.orientation.z = quaternion[2]
        arm_target.orientation.w = quaternion[3]

        return arm_target

    def __pre_grasp(self, arm_target):
        self.hand_commander.set_named_target("open")
        plan = self.hand_commander.plan()
        self.hand_commander.execute(plan, wait=True)

        for _ in range(10):
            self.arm_commander.set_start_state_to_current_state()
            self.arm_commander.set_pose_targets([arm_target])
            plan = self.arm_commander.plan()
            if self.arm_commander.execute(plan):
                return True

    def __grasp(self, arm_target):
        waypoints = []
        waypoints.append(self.arm_commander.get_current_pose(self.arm_commander.get_end_effector_link()).pose)
        arm_above_ball = deepcopy(arm_target)
        arm_above_ball.position.z -= 0.12
        waypoints.append(arm_above_ball)

        self.arm_commander.set_start_state_to_current_state()
        (plan, fraction) = self.arm_commander.compute_cartesian_path(waypoints, 0.01, 0.0)
        print fraction
        if not self.arm_commander.execute(plan):
            return False

        self.hand_commander.set_named_target("close")
        plan = self.hand_commander.plan()
        if not self.hand_commander.execute(plan, wait=True):
            return False

        self.hand_commander.attach_object("cricket_ball__link")

    def __lift(self, arm_target):
        waypoints = []
        waypoints.append(self.arm_commander.get_current_pose(self.arm_commander.get_end_effector_link()).pose)
        arm_above_ball = deepcopy(arm_target)
        arm_above_ball.position.z += 0.1
        waypoints.append(arm_above_ball)

        self.arm_commander.set_start_state_to_current_state()
        (plan, fraction) = self.arm_commander.compute_cartesian_path(waypoints, 0.01, 0.0)
        print fraction
        if not self.arm_commander.execute(plan):
            return False

    def __start_ctrl(self):
        rospy.loginfo("STARTING CONTROLLERS")
        self.__switch_ctrl.call(start_controllers=["hand_controller", "arm_controller", "joint_state_controller"], 
                                stop_controllers=[], strictness=1)
                                
    def __joint_state_cb(self, msg):
        self.__last_joint_state = msg

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import compute_cartesian_path [as 别名]
    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)

        # Initialize the ROS node
        rospy.init_node('moveit_demo', anonymous=True)
        
        cartesian = rospy.get_param('~cartesian', True)
                        
        # Connect to the right_arm move group
        right_arm = MoveGroupCommander('right_arm')
        
        # Allow replanning to increase the odds of a solution
        right_arm.allow_replanning(True)
        
        # Set the right arm reference frame
        right_arm.set_pose_reference_frame('base_footprint')
                
        # Allow some leeway in position(meters) and orientation (radians)
        right_arm.set_goal_position_tolerance(0.01)
        right_arm.set_goal_orientation_tolerance(0.1)
        
        # Get the name of the end-effector link
        end_effector_link = right_arm.get_end_effector_link()
                                        
        # Start in the "straight_forward" configuration stored in the SRDF file
        right_arm.set_named_target('straight_forward')
        
        # Plan and execute a trajectory to the goal configuration
        right_arm.go()
        
        # Get the current pose so we can add it as a waypoint
        start_pose = right_arm.get_current_pose(end_effector_link).pose
                
        # Initialize the waypoints list
        waypoints = []
                
        # Set the first waypoint to be the starting pose
        if cartesian:
            # Append the pose to the waypoints list
            waypoints.append(start_pose)
            
        wpose = deepcopy(start_pose)
                
        # Set the next waypoint back 0.2 meters and right 0.2 meters
        wpose.position.x -= 0.2
        wpose.position.y -= 0.2

        if cartesian:
            # Append the pose to the waypoints list
            waypoints.append(deepcopy(wpose))
        else:
            right_arm.set_pose_target(wpose)
            right_arm.go()
            rospy.sleep(1)
         
        # Set the next waypoint to the right 0.15 meters
        wpose.position.x += 0.05
        wpose.position.y += 0.15
        wpose.position.z -= 0.15
          
        if cartesian:
            # Append the pose to the waypoints list
            waypoints.append(deepcopy(wpose))
        else:
            right_arm.set_pose_target(wpose)
            right_arm.go()
            rospy.sleep(1)
            
        if cartesian:
            # Append the pose to the waypoints list
            waypoints.append(deepcopy(start_pose))
        else:
            right_arm.set_pose_target(start_pose)
            right_arm.go()
            rospy.sleep(1)
            
        if cartesian:
            fraction = 0.0
            maxtries = 100
            attempts = 0
            
            # Set the internal state to the current state
            right_arm.set_start_state_to_current_state()
     
            # Plan the Cartesian path connecting the waypoints
            while fraction < 1.0 and attempts < maxtries:
                (plan, fraction) = right_arm.compute_cartesian_path (
                                        waypoints,   # waypoint poses
                                        0.01,        # eef_step
                                        0.0,         # jump_threshold
                                        True)        # avoid_collisions
                
                # Increment the number of attempts 
                attempts += 1
                
                # Print out a progress message
                if attempts % 10 == 0:
                    rospy.loginfo("Still trying after " + str(attempts) + " attempts...")
                         
#.........这里部分代码省略.........

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import compute_cartesian_path [as 别名]
    goal_point = Point(0.45, -0.2, 1.4)
    goal_ori = Quaternion(0.0,0.0,0.0,1.0)
    goal_pose = Pose(goal_point, goal_ori)
    right_arm_mgc.set_pose_reference_frame('base_link')
    rospy.loginfo("Planning on frame:" + str(right_arm_mgc.get_planning_frame()))
    waypoints = []
    waypoints.append(init_pose)#.pose)
    waypoints.append(goal_pose)
    
    eef_step_max = 0.05
    jump_thresh_max = 10000
    while True:
        curr_eef_step = eef_step_max * random()
        curr_jump_thresh = jump_thresh_max * random()
        #path, fraction = right_arm_mgc.compute_cartesian_path(waypoints, curr_eef_step, curr_jump_thresh, False)
        path, fraction = right_arm_mgc.compute_cartesian_path(waypoints, 0.01, 1.2, False)

        rospy.loginfo("Fraction and path:")
        rospy.loginfo(str(fraction))
        if fraction <= 0:
            rospy.logerr("Couldnt compute path with eef_step:"+ str(curr_eef_step) + " and curr_jump_thresh: " + str(curr_jump_thresh))
        else:
            rospy.logwarn("!!!!! Found a solution with eef_step: " + str(curr_eef_step) + " and curr_jump_thresh: " + str(curr_jump_thresh))
            break
    rospy.loginfo(str(path))

    #waypoints_pa = PoseArray()
    #path = RobotTrajectory()

    for point in path.joint_trajectory.points: # THIS ONLY SENDS THE MESSAGE FOR THE FIRST JOINT
        joint_stt = JointState()

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import compute_cartesian_path [as 别名]
    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)

        # Initialize the ROS node
        rospy.init_node('moveit_demo', anonymous=True)
        
        cartesian = rospy.get_param('~cartesian', True)
                        
        # Connect to the arm move group
        arm = MoveGroupCommander('arm')
        
        # Allow replanning to increase the odds of a solution
        arm.allow_replanning(True)
        
        # Set the right arm reference frame
        arm.set_pose_reference_frame('base_link')
                
        # Allow some leeway in position(meters) and orientation (radians)
        arm.set_goal_position_tolerance(0.01)
        arm.set_goal_orientation_tolerance(0.05)
        
        # Get the name of the end-effector link
        end_effector_link = arm.get_end_effector_link()
                                        
        # Set an initial position for the arm
        start_position = [0.0, 0.5, -0.0074579719079, -1.67822729461, -3.1415174069, -1.1, 3.1415174069]

        # Set the goal pose of the end effector to the stored pose
        arm.set_joint_value_target(start_position)
        
        # Plan and execute a trajectory to the goal configuration
        arm.go()
        
        # Get the current pose so we can add it as a waypoint
        start_pose = arm.get_current_pose(end_effector_link).pose
                
        # Initialize the waypoints list
        waypoints = []
                
        # Set the first waypoint to be the starting pose
        if cartesian:
            # Append the pose to the waypoints list
            waypoints.append(start_pose)
            
        wpose = deepcopy(start_pose)
                
        # Move end effector to the right 0.3 meters
        wpose.position.y -= 0.3

        if cartesian:
            # Append the pose to the waypoints list
            waypoints.append(deepcopy(wpose))
        else:
            arm.set_pose_target(wpose)
            arm.go()
            rospy.sleep(1)
            
        # Move end effector up and back
        wpose.position.x -= 0.2
        wpose.position.z += 0.3

        if cartesian:
            # Append the pose to the waypoints list
            waypoints.append(deepcopy(wpose))
        else:
            arm.set_pose_target(wpose)
            arm.go()
            rospy.sleep(1)
            
        if cartesian:
            # Append the pose to the waypoints list
            waypoints.append(deepcopy(start_pose))
        else:
            arm.set_pose_target(start_pose)
            arm.go()
            rospy.sleep(1)
            
        if cartesian:
            fraction = 0.0
            maxtries = 100
            attempts = 0
            
            # Set the internal state to the current state
            arm.set_start_state_to_current_state()
     
            # Plan the Cartesian path connecting the waypoints
            while fraction < 1.0 and attempts < maxtries:
                (plan, fraction) = arm.compute_cartesian_path (
                                        waypoints,   # waypoint poses
                                        0.025,       # eef_step
                                        0.0,         # jump_threshold
                                        True)        # avoid_collisions
                
                # Increment the number of attempts 
                attempts += 1
                
                # Print out a progress message
                if attempts % 10 == 0:
                    rospy.loginfo("Still trying after " + str(attempts) + " attempts...")
#.........这里部分代码省略.........

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import compute_cartesian_path [as 别名]
    #wpose.orientation.w = 1.0
    #wpose.position.x = waypoints[0].position.x + 0.1
    #wpose.position.y = waypoints[0].position.y
    #wpose.position.z = waypoints[0].position.z
    waypoints.append(copy.deepcopy(wpose))
    print waypoints
    # second move down
    #wpose.position.z -= 0.10
    #waypoints.append(copy.deepcopy(wpose))

    # third move to the side
    #wpose.position.y += 0.05
    #waypoints.append(copy.deepcopy(wpose))
    
    (plan3, fraction) = group.compute_cartesian_path(
                             waypoints,   # waypoints to follow
                             0.01,        # eef_step
                             0.0, False)         # jump_threshold

    print "============ Waiting while RVIZ displays plan3..."
    print fraction
    print plan3
    rospy.sleep(5)
    #print "Sending execution command"
    #group.execute(plan3)
    #group.go()
    #rospy.wait_for_service('compute_ik')
    #getJointPosition =  rospy.ServiceProxy('compute_ik', GetPositionIK )
    #service_request = GetPositionIKRequest()
    #service_request.ik_request.group_name = "arm";
    
    #service_request.ik_request.pose_stamped = currentPoseHandLink

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import compute_cartesian_path [as 别名]

#.........这里部分代码省略.........
        ### Create a handle for the Planning Scene Interface
        psi = PlanningSceneInterface()
        
        
        rc.get_current_state()
        rospy.logwarn(rc.get_current_state())
        sss.wait_for_input()
        #rate = rospy.Rate(0.1) # 10hz
        rate = rospy.Rate(1) # 10hz
        rospy.sleep(1)
        
        
        theSub = rospy.Subscriber('/attached_collision_object', AttachedCollisionObject, attCollObjCb, queue_size = 1)
        
        while not rospy.is_shutdown():
            approach_pose = PoseStamped()
            approach_pose.header.frame_id = "table"
            approach_pose.header.stamp = rospy.Time(0)
            approach_pose.pose.position.x = 0.146
            approach_pose.pose.position.y = 0.622
            approach_pose.pose.position.z = 0.01
            quat = tf.transformations.quaternion_from_euler(0, 0, 1.57/2)
            approach_pose.pose.orientation.x = quat[0]
            approach_pose.pose.orientation.y = quat[1]
            approach_pose.pose.orientation.z = quat[2]
            approach_pose.pose.orientation.w = quat[3]
#             mgc.set_start_state_to_current_state()
#             (traj_approach,frac_approach) = mgc.compute_cartesian_path([approach_pose.pose], eef_step, jump_threshold, True)
#             if(frac_approach != 1):
#                 rospy.logwarn("Planning did not succeed before adding collision objects")
#             else:
#                 rospy.logwarn("Planning succeeded before adding collision objects")
# 
#             rospy.logwarn("waiting for input to add dummy box")
#             sss.wait_for_input()
#             
            box_dummy_pose = PoseStamped()
            box_dummy_pose.header.frame_id =  "table"
            box_dummy_pose.pose.position.x = 0.147
            box_dummy_pose.pose.position.y = 0.636
            box_dummy_pose.pose.position.z = 0
            psi.add_box("dummy_box", box_dummy_pose, (0.18,0.09,0.26))# #size x,y,z x is always to the left viewing the robot from the PC  
#             rospy.logwarn("I have added the box")
#             rospy.sleep(1)
#             rospy.logwarn("waiting for input to try planning with dummy box")
#             sss.wait_for_input()
#             
#             (traj_approach,frac_approach) = mgc.compute_cartesian_path([approach_pose.pose], eef_step, jump_threshold, True)
#             if(frac_approach != 1):
#                 rospy.logwarn("Planning did not succeed after adding collision objects (dummy box)")
#             else:
#                 rospy.logwarn("Planning succeeded after adding collision objects (dummy box)")
#                 
            rospy.logwarn("waiting for input to add end effector box box")
            sss.wait_for_input()
            # end effector collision object
            link_attached_to_ef = "ee_link"
            mb_ef_collisonobj = "metal_bracket"
            mb_ef_pose = PoseStamped()
            mb_ef_pose.header.frame_id =  link_attached_to_ef
            mb_ef_pose.pose.position.x = 0.065/2.0
            mb_ef_pose.pose.position.y = 0.0
            mb_ef_pose.pose.position.z = 0.0
            mb_ef_size = (0.065,0.06,0.06)



            psi.attach_box(link_attached_to_ef, mb_ef_collisonobj, mb_ef_pose, mb_ef_size, [link_attached_to_ef, "wrist_3_link"])
            
            
            raw_input("0 hi")
            
            mgc.attach_object("dummy_box", link_attached_to_ef, [link_attached_to_ef, "wrist_3_link"])
            

            
            
            rospy.logwarn("I have added the attached box to the end effector")
            
            
            rospy.sleep(1)
            raw_input("1 hi")           
            
            rospy.logwarn(rc.get_current_state())
            # mgc.attach_object(object_name, link_name, touch_links)
            mgc.set_start_state_to_current_state()
            rospy.logwarn(rc.get_current_state())
            raw_input("2 hi")
            rospy.logwarn("waiting for input to try planning with end effector box")
            sss.wait_for_input()
            
            (traj_approach,frac_approach) = mgc.compute_cartesian_path([approach_pose.pose], eef_step, jump_threshold, True)
            if(frac_approach != 1):
                rospy.logwarn("Planning did not succeed after adding collision objects (dummy box)")
            else:
                rospy.logwarn("Planning succeeded after adding collision objects (dummy box)")
            
            rospy.logwarn("waiting for input to try planning next loop")
            sss.wait_for_input()
            rate.sleep()

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import compute_cartesian_path [as 别名]
    def __init__(self):

        moveit_commander.roscpp_initialize(sys.argv)
        rospy.init_node('cobra_move_cartesian', anonymous=True)

        arm = MoveGroupCommander(ARM_GROUP)

        arm.allow_replanning(True)
        # arm.set_goal_position_tolerance(0.01)

        arm.set_named_target(START_POSITION)
        arm.go()
        rospy.sleep(2)

        waypoints = list()

        pose = PoseStamped().pose

        # start with the current pose
        waypoints.append(copy.deepcopy(arm.get_current_pose(arm.get_end_effector_link()).pose))

        # same orientation for all
        q = quaternion_from_euler(0.0, 0.0, 0.0)  # roll, pitch, yaw
        pose.orientation = Quaternion(*q)

        # first pose
        pose.position.x = 0.35
        pose.position.y = 0.25
        pose.position.z = 0.3
        waypoints.append(copy.deepcopy(pose))

        # second pose
        pose.position.x = 0.25
        pose.position.y = -0.3
        pose.position.z = 0.3
        waypoints.append(copy.deepcopy(pose))

        # third pose
        pose.position.x += 0.15
        waypoints.append(copy.deepcopy(pose))

        # fourth pose
        pose.position.y += 0.15
        waypoints.append(copy.deepcopy(pose))

        (plan, fraction) = arm.compute_cartesian_path(
                                  waypoints,   # waypoints to follow
                                  0.01,        # eef_step
                                  0.0)         # jump_threshold

        print "====== waypoints created ======="
        # print waypoints

        # ======= show cartesian path ====== #
        arm.go()
        rospy.sleep(10)

        print "========= end ========="

        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)