def __init__(self):
      
        # Retrieve params:
        
        self._grasp_object_name = rospy.get_param('~grasp_object_name', 'lego_block')
        
        self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.016)

        self._arm_group     = rospy.get_param('~arm', 'arm_move_group')
        self._gripper_group = rospy.get_param('~gripper', 'gripper')

        self._approach_retreat_desired_dist = rospy.get_param('~approach_retreat_desired_dist', 0.01)
        self._approach_retreat_min_dist = rospy.get_param('~approach_retreat_min_dist', 0.4)

        # Create (debugging) publishers:
        self._grasps_pub = rospy.Publisher('grasps', PoseArray, queue_size=1, latch=True)
        
        # Create planning scene where we will add the objects etc.
        self._scene = PlanningSceneInterface()
        # Create robot commander: interface to comand the manipulator programmatically (get the planning_frame for exemple
        self._robot = RobotCommander()

        rospy.sleep(1.0)

        # Clean the scene (remove the old objects:
        self._scene.remove_world_object(self._grasp_object_name)

        # Add table and Coke can objects to the planning scene:
        # TODO get the position of the detected object
        self._pose_object_grasp = self._add_object_grasp(self._grasp_object_name)

        rospy.sleep(1.0)

        # Retrieve groups (arm and gripper):
        self._arm     = self._robot.get_group(self._arm_group)
        self._gripper = self._robot.get_group(self._gripper_group)

        # Create grasp generator 'generate' action client:
        self._grasps_ac = SimpleActionClient('/moveit_simple_grasps_server/generate', GenerateGraspsAction)
        if not self._grasps_ac.wait_for_server(rospy.Duration(5.0)):
            rospy.logerr('Grasp generator action client not available!')
            rospy.signal_shutdown('Grasp generator action client not available!')
            return

        # Create move group 'pickup' action client:
        self._pickup_ac = SimpleActionClient('/pickup', PickupAction)
        if not self._pickup_ac.wait_for_server(rospy.Duration(5.0)):
            rospy.logerr('Pick up action client not available!')
            rospy.signal_shutdown('Pick up action client not available!')
            return

        # Pick object:
        while not self._pickup(self._arm_group, self._grasp_object_name, self._grasp_object_width):
            rospy.logwarn('Pick up failed! Retrying ...')
            rospy.sleep(1.0)

        rospy.loginfo('Pick up successfully')
 def __init__(self, name):
     # stuff for grasp planning
     self.tf_listener = tf.TransformListener()
     self.tf_broadcaster = tf.TransformBroadcaster()
     self.cbbf = ClusterBoundingBoxFinder(self.tf_listener, self.tf_broadcaster, "base_link")
     self.last_objects = RecognizedObjectArray()
     #rospy.Subscriber("object_array", RecognizedObjectArray, self.objects_callback)
     self.sub = rospy.Subscriber("/recognized_object_array", RecognizedObjectArray, self.objects_callback)
     self.grasp_publisher = rospy.Publisher("generated_grasps", PoseArray)
     
     rospy.loginfo("Connecting to pickup AS")
     self.pickup_ac = SimpleActionClient('/pickup', PickupAction)
     #pickup_ac.wait_for_server() # needed?
     
     rospy.loginfo("Connecting to grasp generator AS")
     self.grasps_ac = SimpleActionClient('/grasp_generator_server/generate', GenerateBlockGraspsAction)
     #grasps_ac.wait_for_server() # needed? 
     
     #planning scene for motion planning
     self.scene = PlanningSceneInterface()
     
     # blocking action server
     self.grasp_obj_as = ActionServer(name, GraspObjectAction, self.goal_callback, self.cancel_callback, False)
     self.feedback = GraspObjectFeedback()
     self.result = GraspObjectResult()
     self.current_goal = None
     self.grasp_obj_as.start()
Beispiel #3
0
 def __init__(self):
     self._gdict = {}
     self._group_name = ""
     self._prev_group_name = ""
     self._planning_scene_interface = PlanningSceneInterface()
     self._robot = RobotCommander()
     self._last_plan = None
     self._db_host = None
     self._db_port = 33829
     self._trace = False
	def __init__(self):

		self.node_name = "PickAndPlaceServer"
		rospy.loginfo("Initalizing PickAndPlaceServer...")
		self.sg = SphericalGrasps()

		# Get the object size
		self.object_height = 0.1
		self.object_width = 0.05
		self.object_depth = 0.05
		self.pick_pose = rospy.get_param('~pickup_marker_pose')
		self.place_pose = rospy.get_param('~place_marker_pose')

		rospy.loginfo("%s: Waiting for pickup action server...", self.node_name)
		self.pickup_ac = SimpleActionClient('/pickup', PickupAction)
		connected = self.pickup_ac.wait_for_server(rospy.Duration(3000))
		if not connected:
			rospy.logerr("%s: Could not connect to pickup action server", self.node_name)
			exit()
		rospy.loginfo("%s: Connected to pickup action server", self.node_name)

		rospy.loginfo("%s: Waiting for place action server...", self.node_name)
		self.place_ac = SimpleActionClient('/place', PlaceAction)
		if not self.place_ac.wait_for_server(rospy.Duration(3000)):
			rospy.logerr("%s: Could not connect to place action server", self.node_name)
			exit()
		rospy.loginfo("%s: Connected to place action server", self.node_name)

		self.scene = PlanningSceneInterface()
		rospy.loginfo("Connecting to /get_planning_scene service")
		self.scene_srv = rospy.ServiceProxy('/get_planning_scene', GetPlanningScene)
		self.scene_srv.wait_for_service()
		rospy.loginfo("Connected.")

		rospy.loginfo("Connecting to clear octomap service...")
		self.clear_octomap_srv = rospy.ServiceProxy('/clear_octomap', Empty)
		self.clear_octomap_srv.wait_for_service()
		rospy.loginfo("Connected!")

		# Get the links of the end effector exclude from collisions
		self.links_to_allow_contact = rospy.get_param('~links_to_allow_contact', None)
		if self.links_to_allow_contact is None:
			rospy.logwarn("Didn't find any links to allow contacts... at param ~links_to_allow_contact")
		else:
			rospy.loginfo("Found links to allow contacts: " + str(self.links_to_allow_contact))

		self.pick_as = SimpleActionServer(self.pick_pose, PickUpPoseAction,
			execute_cb=self.pick_cb, auto_start=False)
		self.pick_as.start()

		self.place_as = SimpleActionServer(self.place_pose, PickUpPoseAction,
			execute_cb=self.place_cb, auto_start=False)
		self.place_as.start()
    def __init__(self, name):
        # stuff for grasp planning
        rospy.loginfo("Getting a TransformListener...")
        self.tf_listener = tf.TransformListener()
        rospy.loginfo("Getting a TransformBroadcaster...")
        self.tf_broadcaster = tf.TransformBroadcaster()
        rospy.loginfo("Initializing a ClusterBoundingBoxFinder...")
        self.cbbf = ClusterBoundingBoxFinder(self.tf_listener, self.tf_broadcaster, "base_link")
        self.last_clusters = None
        rospy.loginfo("Subscribing to '" + RECOGNIZED_OBJECT_ARRAY_TOPIC + "'...")
        self.sub = rospy.Subscriber(RECOGNIZED_OBJECT_ARRAY_TOPIC, RecognizedObjectArray, self.objects_callback)

        if DEBUG_MODE:
            self.to_grasp_object_pose_pub = rospy.Publisher(TO_BE_GRASPED_OBJECT_POSE_TOPIC, PoseStamped)

        rospy.loginfo("Connecting to pickup AS '" + PICKUP_AS + "'...")
        self.pickup_ac = SimpleActionClient(PICKUP_AS, PickupAction)
        self.pickup_ac.wait_for_server()

        rospy.loginfo("Connecting to place AS '" + PLACE_AS + "'...")
        self.place_ac = SimpleActionClient(PLACE_AS, PlaceAction)
        self.place_ac.wait_for_server()

        rospy.loginfo("Connecting to grasp generator AS '" + GRASP_GENERATOR_AS + "'...")
        self.grasps_ac = SimpleActionClient(GRASP_GENERATOR_AS, GenerateGraspsAction)
        self.grasps_ac.wait_for_server()

        rospy.loginfo("Connecting to depth throttle server '" + DEPTH_THROTLE_SRV + "'...")
        self.depth_service = rospy.ServiceProxy(DEPTH_THROTLE_SRV, Empty)
        self.depth_service.wait_for_service()

        rospy.loginfo("Getting a PlanningSceneInterface instance...")
        self.scene = PlanningSceneInterface()

        # blocking action server
        rospy.loginfo("Creating Action Server '" + name + "'...")
        self.grasp_obj_as = ActionServer(name, ObjectManipulationAction, self.goal_callback, self.cancel_callback, False)
        self.as_feedback = ObjectManipulationFeedback()
        self.as_result = ObjectManipulationActionResult()
        self.current_goal = None

        # Take care of left and right arm grasped stuff
        self.right_hand_object = None
        self.left_hand_object = None
        self.current_side = 'right'

        rospy.loginfo("Starting '" + OBJECT_MANIPULATION_AS + "' Action Server!")
        self.grasp_obj_as.start()
Beispiel #6
0
    def __init__(self):
        rospy.init_node('moveit_web',disable_signals=True)
        self.jspub = rospy.Publisher('/update_joint_states',JointState)
        self.psw_pub = rospy.Publisher('/planning_scene_world', PlanningSceneWorld)

        # Give time for subscribers to connect to the publisher
        rospy.sleep(1)
        self.goals = []

        # HACK: Synthesize a valid initial joint configuration for PR2
        initial_joint_state = JointState()
        initial_joint_state.name = ['r_elbow_flex_joint']
        initial_joint_state.position = [-0.1]
        self.jspub.publish(initial_joint_state)

        # Create group we'll use all along this demo
        # self.move_group = MoveGroupCommander('right_arm_and_torso')
        self.move_group = MoveGroupCommander(self.robot_data['group_name'])
        self._move_group = self.move_group._g
        self.ps = PlanningSceneInterface()

        self.status = {'text':'ready to plan','ready':True}
    def __init__(self):



        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)

        rospy.init_node('scene_generator')

        # Use the planning scene object to add or remove objects
        self.scene = PlanningSceneInterface()

        # Create a scene publisher to push changes to the scene
        self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10)

        # Create a dictionary to hold object colors
        self.colors = dict()


        # Prepare Gazebo Subscriber
        self.pwh = None
        self.pwh_copy = None
        self.idx_targ = None
        self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback)


        # PREPARE THE SCENE
        while self.pwh is None:
            rospy.sleep(0.05)

        ############## CLEAR THE SCENE ################



        # Run and keep in the BG the scene generator also add the ability to kill the code with ctrl^c
        timerThread = threading.Thread(target=self.scene_generator)
        timerThread.daemon = True
        timerThread.start()
Beispiel #8
0
    def __init__(self):
        smach.State.__init__(self, 
            outcomes=['succeeded','failed'],
            input_keys=[],
            output_keys=['new_box'])

        # initialize tf listener
        self.listener = tf.TransformListener()
        
        ### Create a handle for the Move Group Commander
        self.mgc = MoveGroupCommander("manipulator")
        
        ### Create a handle for the Planning Scene Interface
        self.psi = PlanningSceneInterface()
#         
        
        ### initialize service for gripper on universal arm 
        self.io_srv = rospy.ServiceProxy('set_io', SetIO)
        
        self.eef_step = 0.01
        self.jump_threshold = 2
        rospy.logwarn("Initializing Grasp")
        rospy.sleep(1)
Beispiel #9
0
def main():
    rospy.init_node('moveit_py_place', anonymous=True)
    #right_arm.set_planner_id("KPIECEkConfigDefault");
    scene = PlanningSceneInterface()
    robot = RobotCommander()
    #group = MoveGroupCommander("head")
    right_arm = MoveGroupCommander("right_arm")
    #right_arm.set_planner_id("KPIECEkConfigDefault");
    rospy.logwarn("cleaning world")
    GRIPPER_FRAME = 'gripper_bracket_f2'
    #scene.remove_world_object("table")
    scene.remove_world_object("part")
    scene.remove_attached_object(GRIPPER_FRAME, "part")
    p = PoseStamped()
    p.header.frame_id = robot.get_planning_frame()

    p.pose.position.x = 0.67
    p.pose.position.y = -0.
    p.pose.position.z = 0.75
    scene.add_box("part", p, (0.07, 0.01, 0.2))

    # move to a random target
    #group.set_named_target("ahead")
    #group.go()
    #rospy.sleep(1)

    result = False
    n_attempts = 0
       
    # repeat until will succeed
    while result == False:
        result = robot.right_arm.pick("part")      
        n_attempts += 1
        print "Attempts pickup: ", n_attempts
        rospy.sleep(0.2)
    
    #robot.right_arm.pick("part")
    #right_arm.go()
    rospy.sleep(5)
# Author: Ioan Sucan

import sys
import rospy
from moveit_commander import RobotCommander, MoveGroupCommander, PlanningSceneInterface, roscpp_initialize, roscpp_shutdown
from geometry_msgs.msg import PoseStamped
from moveit_msgs.msg import Grasp, GripperTranslation, PlaceLocation
from trajectory_msgs.msg import JointTrajectoryPoint
import tf

if __name__=='__main__':

    roscpp_initialize(sys.argv)
    rospy.init_node('moveit_py_demo', anonymous=True)
    
    scene = PlanningSceneInterface()
    robot = RobotCommander()
    right_arm = MoveGroupCommander("arm")
    print "============ Robot Groups:"
    print robot.get_group_names()
    print "============ Robot Links for arm:"
    print robot.get_link_names("arm")
    print "============ Robot Links for gripper:"
    print robot.get_link_names("gripper")
    print right_arm.get_end_effector_link()
    print "============ Printing robot state"
    #print robot.get_current_state()
    print "============"

    
    rospy.sleep(1)
    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)

        rospy.init_node('moveit_demo')

        # Use the planning scene object to add or remove objects
        scene = PlanningSceneInterface()

        # Create a scene publisher to push changes to the scene
        self.scene_pub = rospy.Publisher('planning_scene',
                                         PlanningScene,
                                         queue_size=10)

        # Create a publisher for displaying gripper poses
        self.gripper_pose_pub = rospy.Publisher('gripper_pose',
                                                PoseStamped,
                                                queue_size=10)

        # Create a dictionary to hold object colors
        self.colors = dict()

        # Initialize the move group for the right arm
        arm = MoveGroupCommander(GROUP_NAME_ARM)

        # Initialize the move group for the right gripper
        gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)

        # Get the name of the end-effector link
        end_effector_link = arm.get_end_effector_link()

        # Allow some leeway in position (meters) and orientation (radians)
        arm.set_goal_position_tolerance(0.05)
        arm.set_goal_orientation_tolerance(0.1)

        # Allow replanning to increase the odds of a solution
        arm.allow_replanning(True)

        # Set the right arm reference frame
        arm.set_pose_reference_frame(REFERENCE_FRAME)

        # Allow 5 seconds per planning attempt
        arm.set_planning_time(5)

        # Set a limit on the number of pick attempts before bailing
        max_pick_attempts = 50

        # Set a limit on the number of place attempts
        max_place_attempts = 50

        # Give the scene a chance to catch up
        rospy.sleep(2)

        # Give each of the scene objects a unique name
        table_id = 'table'
        box1_id = 'box1'
        box2_id = 'box2'

        # Remove leftover objects from a previous run
        scene.remove_world_object(table_id)
        scene.remove_world_object(box1_id)
        scene.remove_world_object(box2_id)

        # Give the scene a chance to catch up
        rospy.sleep(1)

        # Start the arm in the "resting" pose stored in the SRDF file
        arm.set_named_target('up')
        arm.go()

        # Open the gripper to the neutral position
        gripper.set_joint_value_target(GRIPPER_NEUTRAL)
        gripper.go()

        rospy.sleep(1)

        # Set the height of the table off the ground
        table_ground = 0.45

        # Set the dimensions of the scene objects [l, w, h]
        table_size = [0.2, 1.0, 0.01]
        box1_size = [0.1, 0.05, 0.05]
        box2_size = [0.05, 0.05, 0.15]

        # Add a table top and two boxes to the scene
        table_pose = PoseStamped()
        table_pose.header.frame_id = REFERENCE_FRAME
        table_pose.pose.position.x = -0.45
        table_pose.pose.position.y = 0.0
        table_pose.pose.position.z = table_ground + table_size[2] / 2.0
        table_pose.pose.orientation.w = 1.0
        scene.add_box(table_id, table_pose, table_size)

        box1_pose = PoseStamped()
        box1_pose.header.frame_id = REFERENCE_FRAME
        box1_pose.pose.position.x = -0.41
        box1_pose.pose.position.y = -0.3
        box1_pose.pose.position.z = table_ground + table_size[
            2] + box1_size[2] / 2.0
        box1_pose.pose.orientation.w = 1.0
        scene.add_box(box1_id, box1_pose, box1_size)

        box2_pose = PoseStamped()
        box2_pose.header.frame_id = REFERENCE_FRAME
        box2_pose.pose.position.x = -0.39
        box2_pose.pose.position.y = 0.3
        box2_pose.pose.position.z = table_ground + table_size[
            2] + box2_size[2] / 2.0
        box2_pose.pose.orientation.w = 1.0
        scene.add_box(box2_id, box2_pose, box2_size)

        # Make the table red and the boxes orange
        self.setColor(table_id, 0.8, 0, 0, 1.0)
        self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
        self.setColor(box2_id, 0.8, 0.4, 0, 1.0)

        # Send the colors to the planning scene
        self.sendColors()

        # Set the support surface name to the table object
        arm.set_support_surface_name(table_id)

        # Set the target pose in between the boxes and on the table
        target_pose = PoseStamped()
        target_pose.header.frame_id = REFERENCE_FRAME
        target_pose.pose.position.x = -0.42
        target_pose.pose.position.y = 0.0
        target_pose.pose.position.z = table_ground + table_size[
            2] + target_size[2] / 2.0
        target_pose.pose.orientation.w = 1.0

        # Specify a pose to place the target after being picked up
        place_pose = PoseStamped()
        place_pose.header.frame_id = REFERENCE_FRAME
        place_pose.pose.position.x = 0.5
        place_pose.pose.position.y = 0.5
        place_pose.pose.position.z = 0
        place_pose.pose.orientation.w = 1.0

        # Set the start state to the current state
        arm.set_start_state_to_current_state()

        # Set the goal pose of the end effector to the stored pose
        arm.set_pose_target(target_pose, end_effector_link)

        # Plan the trajectory to the goal
        traj = arm.plan()

        # Execute the planned trajectory
        arm.execute(traj)

        # Close the gripper to the neutral position
        gripper.set_joint_value_target(GRIPPER_CLOSED)
        gripper.go()

        # Pause for a second
        rospy.sleep(2)

        # Set the place pose of the end effector to the stored pose
        arm.set_pose_target(place_pose, end_effector_link)
        arm.go()

        # Open the gripper to the neutral position
        gripper.set_joint_value_target(GRIPPER_NEUTRAL)
        gripper.go()
        rospy.sleep(2)

        # Return the arm to the "resting" pose stored in the SRDF file
        arm.set_named_target('resting')
        arm.go()

        rospy.sleep(1)

        # Shut down MoveIt cleanly
        moveit_commander.roscpp_shutdown()

        # Exit the script
        moveit_commander.os._exit(0)
Beispiel #12
0
class Planner(object):
    move_group = None
    goals = None
    jspub = None
    namespace = None

    # These will eventually go to model objects
    robot_data = {
        'group_name': 'right_arm_and_torso',
        'eef_link': 'r_wrist_joint_link'
    }

    # Current state of the 'session' (right now, only one)
    current_scene = None
    status = None
    link_poses = None

    def __init__(self):
        rospy.init_node('moveit_web',disable_signals=True)
        self.jspub = rospy.Publisher('/update_joint_states',JointState)
        self.psw_pub = rospy.Publisher('/planning_scene_world', PlanningSceneWorld)

        # Give time for subscribers to connect to the publisher
        rospy.sleep(1)
        self.goals = []

        # HACK: Synthesize a valid initial joint configuration for PR2
        initial_joint_state = JointState()
        initial_joint_state.name = ['r_elbow_flex_joint']
        initial_joint_state.position = [-0.1]
        self.jspub.publish(initial_joint_state)

        # Create group we'll use all along this demo
        # self.move_group = MoveGroupCommander('right_arm_and_torso')
        self.move_group = MoveGroupCommander(self.robot_data['group_name'])
        self._move_group = self.move_group._g
        self.ps = PlanningSceneInterface()

        self.status = {'text':'ready to plan','ready':True}

    def get_scene(self):
        return self.current_scene

    def set_scene(self, scene):
        self.current_scene = scene
        psw = PlanningSceneWorld()
        for co_json in scene['objects']:
            # TODO: Fix orientation by using proper quaternions on the client
            pose = self._make_pose(co_json['pose'])
            # TODO: Decide what to do with STL vs. Collada. The client has a Collada
            # loader but the PlanningSceneInterface can only deal with STL.
            # TODO: Proper mapping between filenames and URLs
            # filename = '/home/julian/aaad/moveit/src/moveit_web/django%s' % co_json['meshUrl']
            filename = '/home/julian/aaad/moveit/src/moveit_web/django/static/meshes/table_4legs.stl'
            co = self.ps.make_mesh(co_json['name'], pose, filename)
            psw.collision_objects.append(co)
        self.psw_pub.publish(psw)


    def get_link_poses(self):
        if self.link_poses is None:
            self.link_poses = self._move_group.get_link_poses_compressed()
        return self.link_poses

    # Create link back to socket.io namespace to allow emitting information
    def set_socket(self, namespace):
        self.namespace = namespace

    def emit(self, event, data=None):
        if self.namespace:
            self.namespace.emit(event, data)

    def emit_new_goal(self, pose):
        self.emit('target_pose', message_converter.convert_ros_message_to_dictionary(pose)['pose'])

    def set_random_goal(self):
        goal_pose = self.move_group.get_random_pose()
        # goal_pose = self.move_group.get_random_pose('base_footprint')
        self.emit_new_goal(goal_pose)

    def _make_pose(self, json_pose):
        pose = PoseStamped()
        pose.header.frame_id = "odom_combined"
        pp = json_pose['position']
        pose.pose.position.x = pp['x']
        pose.pose.position.y = pp['y']
        pose.pose.position.z = pp['z']
        # TODO: Orientation is not working. See about
        # properly using Quaternions everywhere
        pp = json_pose['orientation']
        pose.pose.orientation.x = pp['x']
        pose.pose.orientation.y = pp['y']
        pose.pose.orientation.z = pp['z']
        pose.pose.orientation.w = pp['w']
        return pose

    def plan_to_poses(self, poses):
        goal_pose = self._make_pose(poses[0])
        self.move_group.set_pose_target(goal_pose)
        # self.move_group.set_pose_target(goal_pose,'base_footprint')
        self.emit('status',{'text':'Starting to plan'})
        trajectory = self.move_group.plan()
        if trajectory is None or len(trajectory.joint_trajectory.joint_names) == 0:
            self.status = {'reachable':False,'text':'Ready to plan','ready':True}
            self.emit('status', self.status)
        else:
            self.status = {'reachable':True,'text':'Rendering trajectory'}
            self.emit('status', self.status)
            self.publish_trajectory(trajectory)

    def publish_goal_position(self, trajectory):
        self.publish_position(self, trajectory, -1)

    def publish_position(self, trajectory, step):
        jsmsg = JointState()
        jsmsg.name = trajectory.joint_trajectory.joint_names
        jsmsg.position = trajectory.joint_trajectory.points[step].positions
        self.jspub.publish(jsmsg)

    def publish_trajectory(self, trajectory):
        cur_time = 0.0
        acceleration = 4.0
        for i in range(len(trajectory.joint_trajectory.points)):
            point = trajectory.joint_trajectory.points[i]
            gevent.sleep((point.time_from_start - cur_time)/acceleration)
            cur_time = point.time_from_start
            # self.publish_position(trajectory, i)

            # TODO: Only say "True" to update state on the last step of the trajectory
            new_poses = self._move_group.update_robot_state(trajectory.joint_trajectory.joint_names,
                    trajectory.joint_trajectory.points[i].positions, True)
            self.link_poses = new_poses
            self.emit('link_poses', new_poses)

        self.status = {'text':'Ready to plan','ready':True}
        self.emit('status', self.status)
Beispiel #13
0
class CokeCanPickAndPlace:
    def __init__(self):
        # Retrieve params:
        self._table_object_name = rospy.get_param('~table_object_name',
                                                  'Grasp_Table')
        self._grasp_object_name = rospy.get_param('~grasp_object_name',
                                                  'Grasp_Object')

        self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.01)

        self._arm_group = rospy.get_param('~arm', 'arm')
        self._gripper_group = rospy.get_param('~gripper', 'gripper')

        self._approach_retreat_desired_dist = rospy.get_param(
            '~approach_retreat_desired_dist', 0.6)
        self._approach_retreat_min_dist = rospy.get_param(
            '~approach_retreat_min_dist', 0.4)

        # Create (debugging) publishers:
        self._grasps_pub = rospy.Publisher('grasps',
                                           PoseArray,
                                           queue_size=1,
                                           latch=True)
        self._places_pub = rospy.Publisher('places',
                                           PoseArray,
                                           queue_size=1,
                                           latch=True)

        # Create planning scene and robot commander:
        self._scene = PlanningSceneInterface()
        self._robot = RobotCommander()

        rospy.sleep(1.0)

        # Clean the scene:
        self._scene.remove_world_object(self._table_object_name)
        self._scene.remove_world_object(self._grasp_object_name)

        # Add table and Coke can objects to the planning scene:
        self._pose_table = self._add_table(self._table_object_name)
        self._pose_coke_can = self._add_coke_can(self._grasp_object_name)

        rospy.sleep(1.0)

        # Define target place pose:
        self._pose_place = Pose()

        self._pose_place.position.x = self._pose_coke_can.position.x
        self._pose_place.position.y = self._pose_coke_can.position.y + 0.02
        self._pose_place.position.z = self._pose_coke_can.position.z

        self._pose_place.orientation = Quaternion(
            *quaternion_from_euler(0.0, 0.0, 0.0))

        # Retrieve groups (arm and gripper):
        self._arm = self._robot.get_group(self._arm_group)
        self._gripper = self._robot.get_group(self._gripper_group)

        # Create grasp generator 'generate' action client:
        self._grasps_ac = SimpleActionClient(
            '/moveit_simple_grasps_server/generate', GenerateGraspsAction)
        if not self._grasps_ac.wait_for_server(rospy.Duration(5.0)):
            rospy.logerr('Grasp generator action client not available!')
            rospy.signal_shutdown(
                'Grasp generator action client not available!')
            return

        # Create move group 'pickup' action client:
        self._pickup_ac = SimpleActionClient('/pickup', PickupAction)
        if not self._pickup_ac.wait_for_server(rospy.Duration(5.0)):
            rospy.logerr('Pick up action client not available!')
            rospy.signal_shutdown('Pick up action client not available!')
            return

        # Create move group 'place' action client:
        self._place_ac = SimpleActionClient('/place', PlaceAction)
        if not self._place_ac.wait_for_server(rospy.Duration(5.0)):
            rospy.logerr('Place action client not available!')
            rospy.signal_shutdown('Place action client not available!')
            return

        # Pick Coke can object:
        while not self._pickup(self._arm_group, self._grasp_object_name,
                               self._grasp_object_width):
            rospy.logwarn('Pick up failed! Retrying ...')
            rospy.sleep(1.0)

        rospy.loginfo('Pick up successfully')

        # Place Coke can object on another place on the support surface (table):
        while not self._place(self._arm_group, self._grasp_object_name,
                              self._pose_place):
            rospy.logwarn('Place failed! Retrying ...')
            rospy.sleep(1.0)

        rospy.loginfo('Place successfully')

    def __del__(self):
        # Clean the scene:
        self._scene.remove_world_object(self._grasp_object_name)
        self._scene.remove_world_object(self._table_object_name)

    def _add_table(self, name):
        p = PoseStamped()
        p.header.frame_id = self._robot.get_planning_frame()
        p.header.stamp = rospy.Time.now()

        p.pose.position.x = 0.5
        p.pose.position.y = 0.0
        p.pose.position.z = 0.22

        q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0))
        p.pose.orientation = Quaternion(*q)

        # Table size from ~/.gazebo/models/table/model.sdf, using the values
        # for the surface link.
        self._scene.add_box(name, p, (0.5, 0.4, 0.02))

        return p.pose

    def _add_coke_can(self, name):
        p = PoseStamped()
        p.header.frame_id = self._robot.get_planning_frame()
        p.header.stamp = rospy.Time.now()

        p.pose.position.x = 0.25
        p.pose.position.y = 0
        p.pose.position.z = 0.32

        q = quaternion_from_euler(0.0, 0.0, 0.0)
        p.pose.orientation = Quaternion(*q)

        # Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae,
        # using the measure tape tool from meshlab.
        # The box is the bounding box of the coke cylinder.
        # The values are taken from the cylinder base diameter and height.
        self._scene.add_box(name, p, (0.01, 0.01, 0.009))

        return p.pose

    def _generate_grasps(self, pose, width):
        """
        Generate grasps by using the grasp generator generate action; based on
        server_test.py example on moveit_simple_grasps pkg.
        """

        # Create goal:
        goal = GenerateGraspsGoal()

        goal.pose = pose
        goal.width = width

        options = GraspGeneratorOptions()
        # simple_graps.cpp doesn't implement GRASP_AXIS_Z!
        #options.grasp_axis      = GraspGeneratorOptions.GRASP_AXIS_Z
        options.grasp_direction = GraspGeneratorOptions.GRASP_DIRECTION_UP
        options.grasp_rotation = GraspGeneratorOptions.GRASP_ROTATION_FULL

        # @todo disabled because it works better with the default options
        #goal.options.append(options)

        # Send goal and wait for result:
        state = self._grasps_ac.send_goal_and_wait(goal)
        if state != GoalStatus.SUCCEEDED:
            rospy.logerr('Grasp goal failed!: %s' %
                         self._grasps_ac.get_goal_status_text())
            return None

        grasps = self._grasps_ac.get_result().grasps

        # Publish grasps (for debugging/visualization purposes):
        self._publish_grasps(grasps)

        return grasps

    def _generate_places(self, target):
        """
        Generate places (place locations), based on
        https://github.com/davetcoleman/baxter_cpp/blob/hydro-devel/
        baxter_pick_place/src/block_pick_place.cpp
        """

        # Generate places:
        places = []
        now = rospy.Time.now()
        for angle in numpy.arange(0.0, numpy.deg2rad(360.0),
                                  numpy.deg2rad(1.0)):
            # Create place location:
            place = PlaceLocation()

            place.place_pose.header.stamp = now
            place.place_pose.header.frame_id = self._robot.get_planning_frame()

            # Set target position:
            place.place_pose.pose = copy.deepcopy(target)

            # Generate orientation (wrt Z axis):
            q = quaternion_from_euler(0.0, 0.0, angle)
            place.place_pose.pose.orientation = Quaternion(*q)

            # Generate pre place approach:
            place.pre_place_approach.desired_distance = self._approach_retreat_desired_dist
            place.pre_place_approach.min_distance = self._approach_retreat_min_dist

            place.pre_place_approach.direction.header.stamp = now
            place.pre_place_approach.direction.header.frame_id = self._robot.get_planning_frame(
            )

            place.pre_place_approach.direction.vector.x = 0
            place.pre_place_approach.direction.vector.y = 0
            place.pre_place_approach.direction.vector.z = -1

            # Generate post place approach:
            place.post_place_retreat.direction.header.stamp = now
            place.post_place_retreat.direction.header.frame_id = self._robot.get_planning_frame(
            )

            place.post_place_retreat.desired_distance = self._approach_retreat_desired_dist
            place.post_place_retreat.min_distance = self._approach_retreat_min_dist

            place.post_place_retreat.direction.vector.x = 0
            place.post_place_retreat.direction.vector.y = 0
            place.post_place_retreat.direction.vector.z = 1

            # Add place:
            places.append(place)

        # Publish places (for debugging/visualization purposes):
        self._publish_places(places)

        return places

    def _create_pickup_goal(self, group, target, grasps):
        """
        Create a MoveIt! PickupGoal
        """

        # Create goal:
        goal = PickupGoal()

        goal.group_name = group
        goal.target_name = target

        goal.possible_grasps.extend(grasps)

        goal.allowed_touch_objects.append(target)

        goal.support_surface_name = self._table_object_name

        # Configure goal planning options:
        goal.allowed_planning_time = 7.0

        goal.planning_options.planning_scene_diff.is_diff = True
        goal.planning_options.planning_scene_diff.robot_state.is_diff = True
        goal.planning_options.plan_only = False
        goal.planning_options.replan = True
        goal.planning_options.replan_attempts = 20

        return goal

    def _create_place_goal(self, group, target, places):
        """
        Create a MoveIt! PlaceGoal
        """

        # Create goal:
        goal = PlaceGoal()

        goal.group_name = group
        goal.attached_object_name = target

        goal.place_locations.extend(places)

        # Configure goal planning options:
        goal.allowed_planning_time = 7.0

        goal.planning_options.planning_scene_diff.is_diff = True
        goal.planning_options.planning_scene_diff.robot_state.is_diff = True
        goal.planning_options.plan_only = False
        goal.planning_options.replan = True
        goal.planning_options.replan_attempts = 20

        return goal

    def _pickup(self, group, target, width):
        """
        Pick up a target using the planning group
        """

        # Obtain possible grasps from the grasp generator server:
        grasps = self._generate_grasps(self._pose_coke_can, width)

        # Create and send Pickup goal:
        goal = self._create_pickup_goal(group, target, grasps)

        state = self._pickup_ac.send_goal_and_wait(goal)
        if state != GoalStatus.SUCCEEDED:
            rospy.logerr('Pick up goal failed!: %s' %
                         self._pickup_ac.get_goal_status_text())
            return None

        result = self._pickup_ac.get_result()

        # Check for error:
        err = result.error_code.val
        if err != MoveItErrorCodes.SUCCESS:
            rospy.logwarn('Group %s cannot pick up target %s!: %s' %
                          (group, target, str(moveit_error_dict[err])))

            return False

        return True

    def _place(self, group, target, place):
        """
        Place a target using the planning group
        """

        # Obtain possible places:
        places = self._generate_places(place)

        # Create and send Place goal:
        goal = self._create_place_goal(group, target, places)

        state = self._place_ac.send_goal_and_wait(goal)
        if state != GoalStatus.SUCCEEDED:
            rospy.logerr('Place goal failed!: %s' %
                         self._place_ac.get_goal_status_text())
            return None

        result = self._place_ac.get_result()

        # Check for error:
        err = result.error_code.val
        if err != MoveItErrorCodes.SUCCESS:
            rospy.logwarn('Group %s cannot place target %s!: %s' %
                          (group, target, str(moveit_error_dict[err])))

            return False

        return True

    def _publish_grasps(self, grasps):
        """
        Publish grasps as poses, using a PoseArray message
        """

        if self._grasps_pub.get_num_connections() > 0:
            msg = PoseArray()
            msg.header.frame_id = self._robot.get_planning_frame()
            msg.header.stamp = rospy.Time.now()

            for grasp in grasps:
                p = grasp.grasp_pose.pose

                msg.poses.append(Pose(p.position, p.orientation))

            self._grasps_pub.publish(msg)

    def _publish_places(self, places):
        """
        Publish places as poses, using a PoseArray message
        """

        if self._places_pub.get_num_connections() > 0:
            msg = PoseArray()
            msg.header.frame_id = self._robot.get_planning_frame()
            msg.header.stamp = rospy.Time.now()

            for place in places:
                msg.poses.append(place.place_pose.pose)

            self._places_pub.publish(msg)
    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)

        rospy.init_node('moveit_obstacles_demo')

        # Construct the initial scene object
        scene = PlanningSceneInterface()

        # Create a scene publisher to push changes to the scene
        self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5)

        # Create a dictionary to hold object colors
        self.colors = dict()

        # Pause for the scene to get ready
        rospy.sleep(1)

        # Initialize the move group for the right arm
        arm = MoveGroupCommander(GROUP_NAME_ARM)

        # Get the name of the end-effector link
        end_effector_link = arm.get_end_effector_link()

        # Allow some leeway in position (meters) and orientation (radians)
        arm.set_goal_position_tolerance(0.01)
        arm.set_goal_orientation_tolerance(0.05)

        # Allow replanning to increase the odds of a solution
        arm.allow_replanning(True)

        # Set the right arm reference frame accordingly
        arm.set_pose_reference_frame(REFERENCE_FRAME)

        # Allow 5 seconds per planning attempt
        arm.set_planning_time(5)

        # Give each of the scene objects a unique name
        table_id = 'table'
        box1_id = 'box1'
        box2_id = 'box2'

        # Remove leftover objects from a previous run
        scene.remove_world_object(table_id)
        scene.remove_world_object(box1_id)
        scene.remove_world_object(box2_id)

        # Give the scene a chance to catch up
        rospy.sleep(1)

        # Start the arm in the "resting" pose stored in the SRDF file
        arm.set_named_target('l_arm_init')
        arm.go()

        rospy.sleep(2)

        # Set the height of the table off the ground
        table_ground = 0.65

        # Set the length, width and height of the table and boxes
        table_size = [0.2, 0.7, 0.01]
        box1_size = [0.1, 0.05, 0.05]
        box2_size = [0.05, 0.05, 0.15]

        # Add a table top and two boxes to the scene
        table_pose = PoseStamped()
        table_pose.header.frame_id = REFERENCE_FRAME
        table_pose.pose.position.x = 0.35
        table_pose.pose.position.y = 0.0
        table_pose.pose.position.z = table_ground + table_size[2] / 2.0
        table_pose.pose.orientation.w = 1.0
        scene.add_box(table_id, table_pose, table_size)

        box1_pose = PoseStamped()
        box1_pose.header.frame_id = REFERENCE_FRAME
        box1_pose.pose.position.x = 0.3
        box1_pose.pose.position.y = 0
        box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0
        box1_pose.pose.orientation.w = 1.0
        scene.add_box(box1_id, box1_pose, box1_size)

        box2_pose = PoseStamped()
        box2_pose.header.frame_id = REFERENCE_FRAME
        box2_pose.pose.position.x = 0.3
        box2_pose.pose.position.y = 0.25
        box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0
        box2_pose.pose.orientation.w = 1.0
        scene.add_box(box2_id, box2_pose, box2_size)

        # Make the table red and the boxes orange
        self.setColor(table_id, 0.8, 0, 0, 1.0)
        self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
        self.setColor(box2_id, 0.8, 0.4, 0, 1.0)

        # Send the colors to the planning scene
        self.sendColors()

        # Set the target pose in between the boxes and above the table
        target_pose = PoseStamped()
        target_pose.header.frame_id = REFERENCE_FRAME
        target_pose.pose.position.x = 0.22
        target_pose.pose.position.y = 0.14
        target_pose.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05

        q = quaternion_from_euler(0, 0, -1.57079633)
        target_pose.pose.orientation.x = q[0]
        target_pose.pose.orientation.y = q[1]
        target_pose.pose.orientation.z = q[2]
        target_pose.pose.orientation.w = q[3]

        # Set the target pose for the arm
        arm.set_pose_target(target_pose, end_effector_link)

        # Move the arm to the target pose (if possible)
        arm.go()

        # Pause for a moment...
        rospy.sleep(2)

        # Return the arm to the "resting" pose stored in the SRDF file
        arm.set_named_target('l_arm_init')
        arm.go()

        # Exit MoveIt cleanly
        moveit_commander.roscpp_shutdown()

        # Exit the script
        moveit_commander.os._exit(0)
class scene_generator:
    def __init__(self):



        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)

        rospy.init_node('scene_generator')

        # Use the planning scene object to add or remove objects
        self.scene = PlanningSceneInterface()

        # Create a scene publisher to push changes to the scene
        self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10)

        # Create a dictionary to hold object colors
        self.colors = dict()


        # Prepare Gazebo Subscriber
        self.pwh = None
        self.pwh_copy = None
        self.idx_targ = None
        self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback)


        # PREPARE THE SCENE
        while self.pwh is None:
            rospy.sleep(0.05)

        ############## CLEAR THE SCENE ################



        # Run and keep in the BG the scene generator also add the ability to kill the code with ctrl^c
        timerThread = threading.Thread(target=self.scene_generator)
        timerThread.daemon = True
        timerThread.start()



################################################################# FUNCTIONS #################################################################################



    def model_state_callback(self,msg):

        self.pwh = ModelStates()
        self.pwh = msg


    def scene_generator(self):
#        print obj_att
        global target_pose
        global target_id
        global target_size

        next_call = time.time()
        while True:
            next_call = next_call+1
            target_id = 'target'
            self.taid = self.pwh.name.index('custom_1')
            table_id = 'table'
            self.tid = self.pwh.name.index('table') 
            obstacle1_id = 'obstacle1'
            self.o1id = self.pwh.name.index('wood_cube_5cm')
            obstacle2_id = 'obstacle2'
            self.o2id = self.pwh.name.index('custom_2')

            # Set the sizes [l, w, h]
            table_size = [1.5, 0.8, 0.03]
            target_size = [0.05, 0.05, 0.15]
            obstacle1_size = [0.05, 0.05, 0.05]
            obstacle2_size = [0.05, 0.05, 0.10]

            ## Set the target pose on the table
            target_pose = PoseStamped()
            target_pose.header.frame_id = REFERENCE_FRAME
            target_pose.pose = self.pwh.pose[self.taid]
            if self.obj_att is None:
                # Add the target object to the scene
                self.scene.add_box(target_id, target_pose, target_size)

                table_pose = PoseStamped()
                table_pose.header.frame_id = REFERENCE_FRAME
                table_pose.pose = self.pwh.pose[self.tid]
                table_pose.pose.position.z += 1
                self.scene.add_box(table_id, table_pose, table_size)
                
                obstacle1_pose = PoseStamped()
                obstacle1_pose.header.frame_id = REFERENCE_FRAME
                obstacle1_pose.pose = self.pwh.pose[self.o1id]
                obstacle1_pose.pose.position.z += 0.025
                # Add the target object to the scene 
                self.scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size)

                obstacle2_pose = PoseStamped()
                obstacle2_pose.header.frame_id = REFERENCE_FRAME
                obstacle2_pose.pose = self.pwh.pose[self.o2id]
                # Add the target object to the scene 
                self.scene.add_box(obstacle2_id, obstacle2_pose, obstacle2_size)

                ### Make the target purple ###
                self.setColor(target_id, 0.6, 0, 1, 1.0)

                # Send the colors to the planning scene
                self.sendColors()
            else: 
                self.scene.remove_world_object('target')

            time.sleep(next_call - time.time())
        #threading.Timer(0.5, self.scene_generator).start()

    # Set the color of an object
    def setColor(self, name, r, g, b, a = 0.9):
        # Initialize a MoveIt color object
        color = ObjectColor()

        # Set the id to the name given as an argument
        color.id = name

        # Set the rgb and alpha values given as input
        color.color.r = r
        color.color.g = g
        color.color.b = b
        color.color.a = a

        # Update the global color dictionary
        self.colors[name] = color

    # Actually send the colors to MoveIt!
    def sendColors(self):
        # Initialize a planning scene object
        p = PlanningScene()

        # Need to publish a planning scene diff        
        p.is_diff = True

        # Append the colors from the global color dictionary 
        for color in self.colors.values():
            p.object_colors.append(color)

        # Publish the scene diff
        self.scene_pub.publish(p)
Beispiel #16
0
    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)

        #rospy.init_node('moveit_demo')

        # Initialize the ROS node
        rospy.init_node('moveit_demo', anonymous=True)
        cartesian = rospy.get_param('~cartesian', True)
        print "===== It is OK ===="
        rospy.sleep(3)

        # Construct the initial scene object
        scene = PlanningSceneInterface()

        # Create a scene publisher to push changes to the scene
        self.scene_pub = rospy.Publisher('planning_scene',
                                         PlanningScene,
                                         queue_size=1)

        # Create a dictionary to hold object colors
        self.colors = dict()

        # Pause for the scene to get ready
        rospy.sleep(1)

        # Initialize the move group for the left arm
        left_arm = MoveGroupCommander('left_arm')
        left_gripper = MoveGroupCommander('left_gripper')
        # Get the name of the end-effector link
        left_eef = left_arm.get_end_effector_link()

        # Allow some leeway in position (meters) and orientation (radians)
        left_arm.set_goal_position_tolerance(0.01)
        left_arm.set_goal_orientation_tolerance(0.05)

        # Allow replanning to increase the odds of a solution
        left_arm.allow_replanning(True)

        left_reference_frame = left_arm.get_planning_frame()
        # Set the left arm reference frame
        left_arm.set_pose_reference_frame('base')
        # Allow 5 seconds per planning attempt
        left_arm.set_planning_time(10)

        # Set a limit on the number of pick attempts before bailing
        max_pick_attempts = 10

        # Set a limit on the number of place attempts
        max_place_attempts = 10

        # Give the scene a chance to catch up
        rospy.sleep(2)

        object1_id = 'object1'
        table_id = 'table'
        target_id = 'target'
        tool_id = 'tool'
        #obstacle1_id = 'obstacle1'
        # Remove leftover objects from a previous run
        scene.remove_world_object(object1_id)
        scene.remove_world_object(table_id)
        scene.remove_world_object(target_id)
        scene.remove_world_object(tool_id)

        # Remove any attached objects from a previous session
        scene.remove_attached_object('base', target_id)

        # Give the scene a chance to catch up
        rospy.sleep(1)

        # Start the arm in the "resting" pose stored in the SRDF file
        left_arm.set_named_target('left_arm_zero')
        left_arm.go()
        rospy.sleep(1)
        left_gripper.set_joint_value_target(GRIPPER_OPEN)
        left_gripper.go()
        rospy.sleep(1)

        # Set the height of the table off the ground
        table_ground = 0.0

        object1_size = [0.088, 0.04, 0.02]

        # Set the target size [l, w, h]
        target_size = [0.02, 0.01, 0.12]
        # Add a table top and two boxes to the scene
        #obstacle1_size = [0.3, 0.05, 0.45]

        # Add a table top and two boxes to the scene

        #obstacle1_pose = PoseStamped()
        #obstacle1_pose.header.frame_id = left_reference_frame
        #obstacle1_pose.pose.position.x = 0.96
        #obstacle1_pose.pose.position.y = 0.24
        #obstacle1_pose.pose.position.z = 0.04
        #obstacle1_pose.pose.orientation.w = 1.0
        #scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size)

        #self.setColor(obstacle1_id, 0.8, 0.4, 0, 1.0)

        object1_pose = PoseStamped()
        object1_pose.header.frame_id = left_reference_frame
        object1_pose.pose.position.x = 0.80
        object1_pose.pose.position.y = 0.04
        object1_pose.pose.position.z = table_ground + table_size[
            2] + object1_size[2] / 2.0
        object1_pose.pose.orientation.w = 1.0
        scene.add_box(object1_id, object1_pose, object1_size)

        # Add a table top and two boxes to the scene
        table_pose = PoseStamped()
        table_pose.header.frame_id = left_reference_frame
        table_pose.pose.position.x = 1
        table_pose.pose.position.y = 0.7
        table_pose.pose.position.z = table_ground + table_size[2] / 2.0
        table_pose.pose.orientation.w = 1.0
        scene.add_box(table_id, table_pose, table_size)

        # Set the target pose in between the boxes and on the table
        target_pose = PoseStamped()
        target_pose.header.frame_id = left_reference_frame
        target_pose.pose.position.x = 0.58
        target_pose.pose.position.y = 0.1878
        target_pose.pose.position.z = .1116
        target_pose.pose.orientation.x = 0.1325
        target_pose.pose.orientation.y = 0.9908
        target_pose.pose.orientation.z = table_ground + table_size[
            2] + target_size[2] / 2.0
        target_pose.pose.orientation.w = 0.0254
        # Add the target object to the scene
        scene.add_box(target_id, target_pose, target_size)

        # Make the table red and the boxes orange
        self.setColor(object1_id, 0.8, 0, 0, 1.0)
        self.setColor(table_id, 0.8, 0, 0, 1.0)

        # Make the target yellow
        self.setColor(target_id, 0.9, 0.9, 0, 1.0)

        # Send the colors to the planning scene
        self.sendColors()

        # Set the support surface name to the table object
        left_arm.set_support_surface_name(table_id)

        # Specify a pose to place the target after being picked up
        place_pose = PoseStamped()
        place_pose.header.frame_id = left_reference_frame
        place_pose.pose.position.x = 0.18
        place_pose.pose.position.y = -0.18
        place_pose.pose.position.z = table_ground + table_size[
            2] + target_size[2] / 2.0
        place_pose.pose.orientation.w = 1.0
        0
        # Initialize the grasp pose to the target pose
        grasp_pose = target_pose

        # Shift the grasp pose by half the width of the target to center it
        #grasp_pose.pose.position.y -= target_size[1] / 2.0

        # Generate a list of grasps
        grasps = self.make_grasps(grasp_pose, [target_id])

        # Publish the grasp poses so they can be viewed in RViz
        for grasp in grasps:
            self.gripper_pose_pub.publish(grasp.grasp_pose)
            rospy.sleep(0.2)

        # Track success/failure and number of attempts for pick operation
        result = None
        n_attempts = 0

        # Repeat until we succeed or run out of attempts
        while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
            n_attempts += 1
            rospy.loginfo("Pick attempt: " + str(n_attempts))
            result = left_arm.pick(target_id, grasps)
            rospy.sleep(0.2)

        # If the pick was successful, attempt the place operation
        if result == MoveItErrorCodes.SUCCESS:
            result = None
            n_attempts = 0

            # Generate valid place poses
            places = self.make_places(place_pose)

            # Repeat until we succeed or run out of attempts
            while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts:
                n_attempts += 1
                rospy.loginfo("Place attempt: " + str(n_attempts))
                for place in places:
                    result = left_arm.place(target_id, place)
                    if result == MoveItErrorCodes.SUCCESS:
                        break
                rospy.sleep(0.2)

            if result != MoveItErrorCodes.SUCCESS:
                rospy.loginfo("Place operation failed after " +
                              str(n_attempts) + " attempts.")
        else:
            rospy.loginfo("Pick operation failed after " + str(n_attempts) +
                          " attempts.")

        # Return the arm to the "resting" pose stored in the SRDF file
        left_arm.set_named_target('left_arm_zero')
        left_arm.go()

        # Open the gripper to the neutral position
        left_gripper.set_joint_value_target(GRIPPER_OPEN)
        left_gripper.go()

        rospy.sleep(1)

        # Shut down MoveIt cleanly
        moveit_commander.roscpp_shutdown()

        # Exit the script
        moveit_commander.os._exit(0)
Beispiel #17
0
    def __init__(self):
        # 初始化move_group的API
        moveit_commander.roscpp_initialize(sys.argv)

        # 初始化ROS节点
        rospy.init_node('moveit_fk_demo', anonymous=True)

        # 初始化场景对象
        scene = PlanningSceneInterface()
        rospy.sleep(1)

        # 初始化需要使用move group控制的机械臂中的arm group
        arm = moveit_commander.MoveGroupCommander('xarm6')

        # 设置机械臂运动的允许误差值
        arm.set_goal_joint_tolerance(0.001)

        # 设置允许的最大速度和加速度
        arm.set_max_acceleration_scaling_factor(0.5)
        arm.set_max_velocity_scaling_factor(0.5)

        # 控制机械臂先回到初始化位置
        arm.set_named_target('home')
        arm.go()
        rospy.sleep(1)

        # 移除场景中之前运行残留的物体
        scene.remove_world_object('eaibot_n1')
        # 设置box的高度
        eaibot_n1_height = 0
        # 设置box的三维尺寸
        eaibot_n1_size = [0.65, 0.7, 0.01]
        # 将box加入场景当中
        eaibot_n1_pose = PoseStamped()
        eaibot_n1_pose.header.frame_id = 'link_base'
        eaibot_n1_pose.pose.position.x = 0.0
        eaibot_n1_pose.pose.position.y = 0.0
        eaibot_n1_pose.pose.position.z = eaibot_n1_height + eaibot_n1_size[
            2] / 2.0
        eaibot_n1_pose.pose.orientation.w = 1.0
        scene.add_box('eaibot_n1', eaibot_n1_pose, eaibot_n1_size)
        rospy.sleep(2)

        # 移除场景中之前运行残留的物体
        scene.remove_world_object('ground')
        # 设置box的高度
        ground_height = -0.3
        # 设置box的三维尺寸
        ground_size = [2, 2, 0.01]
        # 将box加入场景当中
        ground_pose = PoseStamped()
        ground_pose.header.frame_id = 'link_base'
        ground_pose.pose.position.x = 0.0
        ground_pose.pose.position.y = 0.0
        ground_pose.pose.position.z = ground_height + ground_size[2] / 2.0
        ground_pose.pose.orientation.w = 1.0
        scene.add_box('ground', ground_pose, ground_size)
        rospy.sleep(2)

        # 设置机械臂的目标位置,使用六轴的位置数据进行描述(单位:弧度)
        joint_positions = [
            -0.001745, 1.394518, -1.9059, -0.013963, 0.513127, 0.005236
        ]
        arm.set_joint_value_target(joint_positions)

        # 控制机械臂完成运动
        arm.go()
        rospy.sleep(1)

        # 控制机械臂先回到初始化位置
        arm.set_named_target('home')
        arm.go()
        rospy.sleep(1)

        # 关闭并退出moveit
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)
Beispiel #18
0
    def __init__(self):
        # Retrieve params:
        #if there is no param named 'table_object_name' then use the default
        self._table_object_name = rospy.get_param('~table_object_name',
                                                  'Grasp_Table')
        self._grasp_object_name = rospy.get_param('~grasp_object_name',
                                                  'Grasp_Object')

        self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.01)

        self._arm_group = rospy.get_param('~manipulator', 'manipulator')
        self._gripper_group = rospy.get_param('~gripper', 'gripper')

        self._approach_retreat_desired_dist = rospy.get_param(
            '~approach_retreat_desired_dist', 0.2)
        self._approach_retreat_min_dist = rospy.get_param(
            '~approach_retreat_min_dist', 0.1)

        # Create (debugging) publishers:
        self._grasps_pub = rospy.Publisher('grasps',
                                           PoseArray,
                                           queue_size=1,
                                           latch=True)
        self._places_pub = rospy.Publisher('places',
                                           PoseArray,
                                           queue_size=1,
                                           latch=True)

        # Create planning scene and robot commander:
        self._scene = PlanningSceneInterface()
        self._robot = RobotCommander()

        rospy.sleep(1.0)

        # Clean the scene:
        self._scene.remove_world_object(self._table_object_name)
        self._scene.remove_world_object(self._grasp_object_name)

        # Add table and Coke can objects to the planning scene:
        self._pose_table = self._add_table(self._table_object_name)
        self._pose_coke_can = self._add_grasp_block_(self._grasp_object_name)

        rospy.sleep(1.0)

        ##############################################################################################################################
        ##############################################################################################################################
        # Define target place pose:
        self._pose_place = Pose()

        self._pose_place.position.x = self._pose_coke_can.position.x
        self._pose_place.position.y = self._pose_coke_can.position.y - 0.20
        self._pose_place.position.z = self._pose_coke_can.position.z
        self._pose_place.orientation = Quaternion(
            *quaternion_from_euler(0.0, 0.0, 0.0))

        ###different reffere
        #reference frame is base_link
        self._pose_coke_can.position.z = self._pose_coke_can.position.z - 0.9  # base_link is 0.9 above
        self._pose_place.position.z = self._pose_place.position.z + 0.05  # target pose a little above

        # Retrieve groups (arm and gripper):
        self._arm = self._robot.get_group(self._arm_group)
        self._gripper = self._robot.get_group(self._gripper_group)

        self._arm.set_named_target('pickup')
        self._arm.go(wait=True)
        print("Pickup pose")

        # Create grasp generator 'generate' action client:
        self._grasps_ac = SimpleActionClient(
            '/moveit_simple_grasps_server/generate', GenerateGraspsAction)
        if not self._grasps_ac.wait_for_server(rospy.Duration(5.0)):
            rospy.logerr('Grasp generator action client not available!')
            rospy.signal_shutdown(
                'Grasp generator action client not available!')
            return

        # Create move group 'pickup' action client:
        self._pickup_ac = SimpleActionClient('/pickup', PickupAction)
        if not self._pickup_ac.wait_for_server(rospy.Duration(5.0)):
            rospy.logerr('Pick up action client not available!')
            rospy.signal_shutdown('Pick up action client not available!')
            return

        # Create move group 'place' action client:
        self._place_ac = SimpleActionClient('/place', PlaceAction)
        if not self._place_ac.wait_for_server(rospy.Duration(5.0)):
            rospy.logerr('Place action client not available!')
            rospy.signal_shutdown('Place action client not available!')
            return

        # Pick Coke can object:
        while not self._pickup(self._arm_group, self._grasp_object_name,
                               self._grasp_object_width):
            rospy.logwarn('Pick up failed! Retrying ...')
            rospy.sleep(1.0)

        rospy.loginfo('Pick up successfully')
        print("pose_place: ", self._pose_place)

        # Place Coke can object on another place on the support surface (table):
        while not self._place(self._arm_group, self._grasp_object_name,
                              self._pose_place):
            rospy.logwarn('Place failed! Retrying ...')
            rospy.sleep(1.0)

        rospy.loginfo('Place successfully')
class TestPlanners(object):
    def __init__(self, group_id, planner):

        rospy.init_node('moveit_test_planners', anonymous=True)
        self.pass_list = []
        self.fail_list = []
        self.planner = planner
        self.scene = PlanningSceneInterface()
        self.robot = RobotCommander()
        self.group = MoveGroupCommander(group_id)
        rospy.sleep(1)

        self.group.set_planner_id(self.planner)

        self.test_trajectories_empty_environment()
        self.test_waypoints()
        self.test_trajectories_with_walls_and_ground()

        for pass_test in self.pass_list:
            print(pass_test)

        for fail_test in self.fail_list:
            print(fail_test)

    def _add_walls_and_ground(self):
        # publish a scene
        p = geometry_msgs.msg.PoseStamped()
        p.header.frame_id = self.robot.get_planning_frame()

        p.pose.position.x = 0
        p.pose.position.y = 0
        # offset such that the box is below ground (to prevent collision with
        # the robot itself)
        p.pose.position.z = -0.11
        p.pose.orientation.x = 0
        p.pose.orientation.y = 0
        p.pose.orientation.z = 0
        p.pose.orientation.w = 1
        self.scene.add_box("ground", p, (3, 3, 0.1))

        p.pose.position.x = 0.4
        p.pose.position.y = 0.85
        p.pose.position.z = 0.4
        p.pose.orientation.x = 0.5
        p.pose.orientation.y = -0.5
        p.pose.orientation.z = 0.5
        p.pose.orientation.w = 0.5
        self.scene.add_box("wall_front", p, (0.8, 2, 0.01))

        p.pose.position.x = 1.33
        p.pose.position.y = 0.4
        p.pose.position.z = 0.4
        p.pose.orientation.x = 0.0
        p.pose.orientation.y = -0.707388
        p.pose.orientation.z = 0.0
        p.pose.orientation.w = 0.706825
        self.scene.add_box("wall_right", p, (0.8, 2, 0.01))

        p.pose.position.x = -0.5
        p.pose.position.y = 0.4
        p.pose.position.z = 0.4
        p.pose.orientation.x = 0.0
        p.pose.orientation.y = -0.707107
        p.pose.orientation.z = 0.0
        p.pose.orientation.w = 0.707107
        self.scene.add_box("wall_left", p, (0.8, 2, 0.01))
        # rospy.sleep(1)

    def _check_plan(self, plan):
        if len(plan.joint_trajectory.points) > 0:
            return True
        else:
            return False

    def _plan_joints(self, joints):
        self.group.clear_pose_targets()
        group_variable_values = self.group.get_current_joint_values()
        group_variable_values[0:6] = joints[0:6]
        self.group.set_joint_value_target(group_variable_values)

        plan = self.group.plan()
        return self._check_plan(plan)

    def test_trajectories_rotating_each_joint(self):
        # test_joint_values = [numpy.pi/2.0, numpy.pi-0.33, -numpy.pi/2]
        test_joint_values = [numpy.pi / 2.0]
        joints = [0.0, 0.0, 0.0, -numpy.pi / 2.0, 0.0, 0.0]
        # Joint 4th is colliding with the hand
        # for joint in range(6):
        for joint in [0, 1, 2, 3, 5]:
            for value in test_joint_values:
                joints[joint] = value
            if not self._plan_joints(joints):
                self.fail_list.append(
                    "Failed: test_trajectories_rotating_each_joint, " +
                    self.planner + ", joints:" + str(joints))
            else:
                self.pass_list.append(
                    "Passed: test_trajectories_rotating_each_joint, " +
                    self.planner + ", joints:" + str(joints))

    def test_trajectories_empty_environment(self):
        # Up - Does not work with sbpl but it does with ompl
        joints = [0.0, -1.99, 2.19, 0.58, 0.0, -0.79, 0.0, 0.0]
        if not self._plan_joints(joints):
            self.fail_list.append(
                "Failed: test_trajectories_empty_environment, " +
                self.planner + ", joints:" + str(joints))
        else:
            self.pass_list.append(
                "Passed: test_trajectories_empty_environment, " +
                self.planner + ", joints:" + str(joints))

        # All joints up
        joints = [
            -1.67232, -2.39104, 0.264862, 0.43346, 2.44148, 2.48026, 0.0, 0.0
        ]
        if not self._plan_joints(joints):
            self.fail_list.append(
                "Failed: test_trajectories_empty_environment, " +
                self.planner + ", joints:" + str(joints))
        else:
            self.pass_list.append(
                "Passed: test_trajectories_empty_environment, " +
                self.planner + ", joints:" + str(joints))

        # Down
        joints = [
            -0.000348431194526, 0.397651011661, 0.0766181197394,
            -0.600353691727, -0.000441966540076, 0.12612019707, 0.0, 0.0
        ]
        if not self._plan_joints(joints):
            self.fail_list.append(
                "Failed: test_trajectories_empty_environment, " +
                self.planner + ", joints:" + str(joints))
        else:
            self.pass_list.append(
                "Passed: test_trajectories_empty_environment, " +
                self.planner + ", joints:" + str(joints))

        # left
        joints = [
            0.146182953165, -2.6791929848, -0.602721109682, -3.00575848765,
            0.146075718452, 0.00420656698366, 0.0, 0.0
        ]
        if not self._plan_joints(joints):
            self.fail_list.append(
                "Failed: test_trajectories_empty_environment, " +
                self.planner + ", joints:" + str(joints))
        else:
            self.pass_list.append(
                "Passed: test_trajectories_empty_environment, " +
                self.planner + ", joints:" + str(joints))

        # Front
        joints = [
            1.425279839, -0.110370375874, -1.52548746261, -1.50659865247,
            -1.42700242769, 3.1415450794, 0.0, 0.0
        ]
        if not self._plan_joints(joints):
            self.fail_list.append(
                "Failed: test_trajectories_empty_environment, " +
                self.planner + ", joints:" + str(joints))
        else:
            self.pass_list.append(
                "Passed: test_trajectories_empty_environment, " +
                self.planner + ", joints:" + str(joints))

        # Behind
        joints = [
            1.57542451065, 3.01734161219, 2.01043257686, -1.14647092839,
            0.694689321451, -0.390769365032, 0.0, 0.0
        ]
        if not self._plan_joints(joints):
            self.fail_list.append(
                "Failed: test_trajectories_empty_environment, " +
                self.planner + ", joints:" + str(joints))
        else:
            self.pass_list.append(
                "Passed: test_trajectories_empty_environment, " +
                self.planner + ", joints:" + str(joints))

        # Should fail because it is in self-collision
        joints = [
            -0.289797803762, 2.37263860495, 2.69118483159, 1.65486712181,
            1.04235601797, -1.69730925867, 0.0, 0.0
        ]
        if not self._plan_joints(joints):
            self.fail_list.append(
                "Failed: test_trajectories_empty_environment, " +
                self.planner + ", joints:" + str(joints))
        else:
            self.pass_list.append(
                "Passed: test_trajectories_empty_environment, " +
                self.planner + ", joints:" + str(joints))

    def test_waypoints(self):
        # Start planning in a given joint position
        joints = [
            -0.324590029242, -1.42602359749, -1.02523472017, -0.754761892979,
            0.344227622185, -3.03250264451, 0.0, 0.0
        ]
        current = RobotState()
        current.joint_state.name = self.robot.get_current_state(
        ).joint_state.name
        current_joints = list(
            self.robot.get_current_state().joint_state.position)
        current_joints[0:8] = joints
        current.joint_state.position = current_joints

        self.group.set_start_state(current)

        waypoints = []

        initial_pose = self.group.get_current_pose().pose

        initial_pose.position.x = -0.301185959729
        initial_pose.position.y = 0.517069787724
        initial_pose.position.z = 1.20681710541
        initial_pose.orientation.x = 0.0207499700474
        initial_pose.orientation.y = -0.723943002716
        initial_pose.orientation.z = -0.689528413407
        initial_pose.orientation.w = 0.00515118111221

        # start with a specific position
        waypoints.append(initial_pose)

        # first move it down
        wpose = geometry_msgs.msg.Pose()
        wpose.orientation = waypoints[0].orientation
        wpose.position.x = waypoints[0].position.x
        wpose.position.y = waypoints[0].position.y
        wpose.position.z = waypoints[0].position.z - 0.20
        waypoints.append(copy.deepcopy(wpose))

        # second front
        wpose.position.y += 0.20
        waypoints.append(copy.deepcopy(wpose))

        # third side
        wpose.position.x -= 0.20
        waypoints.append(copy.deepcopy(wpose))

        # fourth return to initial pose
        wpose = waypoints[0]
        waypoints.append(copy.deepcopy(wpose))

        (plan3,
         fraction) = self.group.compute_cartesian_path(waypoints, 0.01, 0.0)
        if not self._check_plan(plan3) and fraction > 0.8:
            self.fail_list.append("Failed: test_waypoints, " + self.planner)
        else:
            self.pass_list.append("Passed: test_waypoints, " + self.planner)

    def test_trajectories_with_walls_and_ground(self):
        self._add_walls_and_ground()

        # Should fail to plan: Goal is in collision with the wall_front
        joints = [
            0.302173213174, 0.192487443763, -1.94298265002, 1.74920382275,
            0.302143499777, 0.00130280337897, 0.0, 0.0
        ]
        if not self._plan_joints(joints):
            self.fail_list.append(
                "Failed: test_trajectories_with_walls_and_ground, " +
                self.planner + ", joints:" + str(joints))
        else:
            self.pass_list.append(
                "Passed: test_trajectories_with_walls_and_ground, " +
                self.planner + ", joints:" + str(joints))

        # Should fail to plan: Goal is in collision with the ground
        joints = [
            3.84825722288e-05, 0.643694953509, -1.14391175311, 1.09463824437,
            0.000133883149666, -0.594498939239, 0.0, 0.0
        ]
        if not self._plan_joints(joints):
            self.fail_list.append(
                "Failed: test_trajectories_with_walls_and_ground, " +
                self.planner + ", joints:" + str(joints))
        else:
            self.pass_list.append(
                "Passed: test_trajectories_with_walls_and_ground, " +
                self.planner + ", joints:" + str(joints))

        # Goal close to right corner
        joints = [
            0.354696232081, -0.982224980654, 0.908055961723, -1.92328051116,
            -1.3516255551, 2.8225061435, 0.0, 0.0
        ]
        if not self._plan_joints(joints):
            self.fail_list.append(
                "Failed: test_trajectories_with_walls_and_ground, " +
                self.planner + ", joints:" + str(joints))
        else:
            self.pass_list.append(
                "Passed, test_trajectories_with_walls_and_ground, " +
                self.planner + ", joints:" + str(joints))

        self.scene.remove_world_object("ground")
        self.scene.remove_world_object("wall_left")
        self.scene.remove_world_object("wall_right")
        self.scene.remove_world_object("wall_front")
Beispiel #20
0
    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)

        rospy.init_node('moveit_demo')

        # Construct the initial scene object
        scene = PlanningSceneInterface()

        # Create a scene publisher to push changes to the scene
        self.scene_pub = rospy.Publisher('planning_scene',
                                         PlanningScene,
                                         queue_size=10)

        # Create a dictionary to hold object colors
        self.colors = dict()

        # Pause for the scene to get ready
        rospy.sleep(1)

        # Initialize the move group for the right arm
        left_arm = MoveGroupCommander('left_arm')
        left_gripper = MoveGroupCommander('left_gripper')
        # Get the name of the end-effector link
        left_eef = left_arm.get_end_effector_link()

        # Allow some leeway in position (meters) and orientation (radians)
        left_arm.set_goal_position_tolerance(0.01)
        left_arm.set_goal_orientation_tolerance(0.05)

        # Allow replanning to increase the odds of a solution
        left_arm.allow_replanning(True)

        left_reference_frame = left_arm.get_planning_frame()

        # Allow 5 seconds per planning attempt
        left_arm.set_planning_time(5)

        object1_id = 'object1'
        obstacle1_id = 'obstacle1'
        # Remove leftover objects from a previous run
        scene.remove_world_object(object1_id)

        # Give the scene a chance to catch up
        rospy.sleep(1)

        # Start the arm in the "resting" pose stored in the SRDF file
        left_arm.set_named_target('left_arm_zero')
        left_arm.go()
        rospy.sleep(1)
        left_gripper.set_joint_value_target(GRIPPER_OPEN)
        left_gripper.go()
        rospy.sleep(1)

        object1_size = [0.088, 0.04, 0.02]

        # Add a table top and two boxes to the scene
        obstacle1_size = [0.3, 0.05, 0.45]

        # Add a table top and two boxes to the scene

        obstacle1_pose = PoseStamped()
        obstacle1_pose.header.frame_id = left_reference_frame
        obstacle1_pose.pose.position.x = 0.96
        obstacle1_pose.pose.position.y = 0.5
        obstacle1_pose.pose.position.z = 0.04
        obstacle1_pose.pose.orientation.w = 1.0
        scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size)

        self.setColor(obstacle1_id, 0.8, 0.4, 0, 1.0)

        object1_pose = PoseStamped()
        object1_pose.header.frame_id = left_reference_frame
        object1_pose.pose.position.x = 0.80
        object1_pose.pose.position.y = 0.3
        object1_pose.pose.position.z = 0
        object1_pose.pose.orientation.w = 1.0
        scene.add_box(object1_id, object1_pose, object1_size)

        self.setColor(object1_id, 0.8, 0, 0, 1.0)

        # Send the colors to the planning scene
        self.sendColors()

        # Set the start state to the current state
        left_arm.set_start_state_to_current_state()
        # Set the target pose in between the boxes and above the table
        target_pose = PoseStamped()
        target_pose.header.frame_id = left_reference_frame
        target_pose.pose.position.x = 0.58
        target_pose.pose.position.y = 0.1878
        target_pose.pose.position.z = .1116
        target_pose.pose.orientation.x = 0.1325
        target_pose.pose.orientation.y = 0.9908
        target_pose.pose.orientation.z = 0.0095
        target_pose.pose.orientation.w = 0.0254

        # Set the target pose for the arm
        left_arm.set_pose_target(target_pose, left_eef)

        # Move the arm to the target pose (if possible)
        left_arm.go()

        # Pause for a moment...
        rospy.sleep(2)
        left_gripper.set_joint_value_target(GRIPPER_CLOSE)
        left_gripper.go()
        rospy.sleep(1)
        scene.attach_box(left_eef, object1_id, object1_pose, object1_size)
        # Cartesian Paths
        waypoints1 = []
        start_pose = left_arm.get_current_pose(left_eef).pose
        wpose = deepcopy(start_pose)
        waypoints1.append(deepcopy(wpose))

        wpose.position.x -= 0.05
        wpose.position.y += 0.105
        wpose.position.z += 0.07
        waypoints1.append(deepcopy(wpose))

        wpose.position.x -= 0.05
        wpose.position.y += 0.105
        wpose.position.z -= 0.07
        waypoints1.append(deepcopy(wpose))

        (cartesian_plan, fraction) = left_arm.compute_cartesian_path(
            waypoints1,  # waypoint poses
            0.01,  # eef_step 1cm
            0.0,  # jump_threshold
            True)  # avoid_collisions

        left_arm.execute(cartesian_plan)
        rospy.sleep(2)
        left_gripper.set_joint_value_target(GRIPPER_OPEN)
        left_gripper.go()
        rospy.sleep(1)
        scene.remove_attached_object(left_eef, object1_id)
        # Exit MoveIt cleanly
        waypoints2 = []
        start_pose = left_arm.get_current_pose(left_eef).pose
        wpose = deepcopy(start_pose)
        waypoints2.append(deepcopy(wpose))
        wpose.position.z += 0.07
        waypoints2.append(deepcopy(wpose))

        wpose.position.x += 0.1
        wpose.position.y -= 0.21
        waypoints2.append(deepcopy(wpose))

        wpose.position.z -= 0.07
        waypoints2.append(deepcopy(wpose))

        (cartesian_plan, fraction) = left_arm.compute_cartesian_path(
            waypoints2,  # waypoint poses
            0.01,  # eef_step 1cm
            0.0,  # jump_threshold
            True)  # avoid_collisions

        left_arm.execute(cartesian_plan)
        rospy.sleep(2)
        moveit_commander.roscpp_shutdown()

        # Exit the script
        moveit_commander.os._exit(0)
Beispiel #21
0
      def __init__(self):
            roscpp_initialize(sys.argv)        
            rospy.init_node('ur3_move',anonymous=True)

            rospy.loginfo("Waiting for ar_pose_marker topic...")
            rospy.wait_for_message('ar_pose_marker', AlvarMarkers)

            rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_tf_listener)
            rospy.loginfo("Maker messages detected. Starting followers...")

            display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
                                                   moveit_msgs.msg.DisplayTrajectory,
                                                   queue_size=20)

            #self.listener = tf.TransformListener()
            self.goal_x = 0
            self.goal_y = 0
            self.goal_z = 0

            self.goal_ori_x = 0
            self.goal_ori_y = 0
            self.goal_ori_z = 0
            self.goal_ori_w = 0

            
            self.marker = []
            self.position_list = []
            self.orientation_list = []

            self.m_idd = 0
            self.m_pose_x = []
            self.m_pose_y = []
            self.m_pose_z = []
            self.m_ori_w = []
            self.m_ori_x = []
            self.m_ori_y = []
            self.m_ori_z = []

            self.ar_pose = Pose()
            self.goalPoseFromAR = Pose()
            self.br = tf.TransformBroadcaster()
            #self.goalPose_from_arPose = Pose()

            self.trans = []
            self.rot = []

            self.calculed_coke_pose = Pose()
            #rospy.loginfo("Waiting for ar_pose_marker topic...")
            #rospy.wait_for_message('ar_pose_marker', AlvarMarkers)

            #rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_callback)
            #rospy.loginfo("Maker messages detected. Starting followers...")


            self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty)

            self.scene = PlanningSceneInterface()
            self.robot_cmd = RobotCommander()

            self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
            #robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
            self.robot_arm.set_goal_orientation_tolerance(0.005)
            self.robot_arm.set_planning_time(5)
            self.robot_arm.set_num_planning_attempts(5)           

            self.display_trajectory_publisher = display_trajectory_publisher

            rospy.sleep(2)
            # Allow replanning to increase the odds of a solution
            self.robot_arm.allow_replanning(True)                 
            
            self.clear_octomap()
    def __init__(self):
        # Retrieve params:
        self._table_object_name = rospy.get_param('~table_object_name',
                                                  'table')
        self._grasp_object_name = rospy.get_param('~grasp_object_name',
                                                  'coke_can')

        self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.01)

        self._arm_group = rospy.get_param('~arm', 'arm')
        #self._gripper_group = rospy.get_param('~gripper', 'gripper')

        self._approach_retreat_desired_dist = rospy.get_param(
            '~approach_retreat_desired_dist', 0.3)
        self._approach_retreat_min_dist = rospy.get_param(
            '~approach_retreat_min_dist', 0.1)

        # Create (debugging) publishers:
        self._grasps_pub = rospy.Publisher('grasps',
                                           PoseArray,
                                           queue_size=1,
                                           latch=True)
        self._places_pub = rospy.Publisher('places',
                                           PoseArray,
                                           queue_size=1,
                                           latch=True)

        # Create planning scene and robot commander:
        self._scene = PlanningSceneInterface()
        self._robot = RobotCommander()

        rospy.sleep(1.0)

        # Clean the scene:
        self._scene.remove_world_object(self._table_object_name)
        self._scene.remove_world_object(self._grasp_object_name)

        # Add table and Coke can objects to the planning scene:
        self._pose_table = self._add_table(self._table_object_name)
        self._pose_coke_can = self._add_coke_can(self._grasp_object_name)

        rospy.sleep(1.0)

        #-----------------------------------------------------------
        # Define target place pose:
        self._pose_place = Pose()

        self._pose_place.position.x = self._pose_coke_can.position.x + 0.05
        self._pose_place.position.y = self._pose_coke_can.position.y
        self._pose_place.position.z = self._pose_coke_can.position.z - 0.1

        self._pose_place.orientation = Quaternion(
            *quaternion_from_euler(0.0, 0.0, 0.0))

        rospy.loginfo('x = %f, y = %f, z = %f', self._pose_place.position.x,
                      self._pose_place.position.y, self._pose_place.position.z)

        # Retrieve groups (arm and gripper):
        self._arm = self._robot.get_group(self._arm_group)
        #self._gripper = self._robot.get_group(self._gripper_group)

        # Create grasp generator 'generate' action client:
        self._grasps_ac = SimpleActionClient(
            '/moveit_simple_grasps_server/generate', GenerateGraspsAction)
        if not self._grasps_ac.wait_for_server(rospy.Duration(5.0)):
            rospy.logerr('Grasp generator action client not available!')
            rospy.signal_shutdown(
                'Grasp generator action client not available!')
            return

        # Create move group 'pickup' action client:
        self._pickup_ac = SimpleActionClient('/pickup', PickupAction)
        if not self._pickup_ac.wait_for_server(rospy.Duration(5.0)):
            rospy.logerr('Pick up action client not available!')
            rospy.signal_shutdown('Pick up action client not available!')
            return

        # Create move group 'place' action client:
        self._place_ac = SimpleActionClient('/place', PlaceAction)
        if not self._place_ac.wait_for_server(rospy.Duration(5.0)):
            rospy.logerr('Place action client not available!')
            rospy.signal_shutdown('Place action client not available!')
            return

        # Pick Coke can object:
        while not self._pickup(self._arm_group, self._grasp_object_name,
                               self._grasp_object_width):
            rospy.logwarn('Pick up failed! Retrying ...')
            rospy.sleep(1.0)

        rospy.loginfo('Pick up successfully')

        # Place Coke can object on another place on the support surface (table):
        while not self._place(self._arm_group, self._grasp_object_name,
                              self._pose_place):
            rospy.logwarn('Place failed! Retrying ...')
            rospy.sleep(1.0)

        rospy.loginfo('Place successfully')
#    a = robot.right_arm
#    a.set_start_state(RobotState())
#    r = a.get_random_joint_values()
#    print "Planning to random joint position: "
#    print r
#    p = a.plan(r)
#    print "Solution:"
#    print p
#
#    roscpp_shutdown()
    
    
    roscpp_initialize(sys.argv)
    rospy.init_node('moveit_py_demo', anonymous=True)
    
    scene = PlanningSceneInterface()
    robot = RobotCommander()
    rospy.sleep(1)

    # clean the scene
    #scene.remove_world_object("pole")
    #scene.remove_world_object("table")
    scene.remove_world_object("part")

    # publish a demo scene
    p = PoseStamped()
    p.header.frame_id = robot.get_planning_frame()
    # p.pose.position.x = 0.7
    # p.pose.position.y = -0.4
    # p.pose.position.z = 1.15
    p.pose.orientation.w = 1.0
Beispiel #24
0
    def __init__(self):
        # 初始化move_group的API
        moveit_commander.roscpp_initialize(sys.argv)

        # 初始化ROS节点
        rospy.init_node('moveit_obstacles_demo')

        # 初始化场景对象
        scene = PlanningSceneInterface()

        # 创建一个发布场景变化信息的发布者
        self.scene_pub = rospy.Publisher('planning_scene',
                                         PlanningScene,
                                         queue_size=5)

        # 创建一个存储物体颜色的字典对象
        self.colors = dict()

        # 等待场景准备就绪
        rospy.sleep(1)

        # 初始化需要使用move group控制的机械臂中的arm group
        arm = MoveGroupCommander('arm')

        # 获取终端link的名称
        end_effector_link = arm.get_end_effector_link()

        # 设置位置(单位:米)和姿态(单位:弧度)的允许误差
        arm.set_goal_position_tolerance(0.01)
        arm.set_goal_orientation_tolerance(0.05)

        # 当运动规划失败后,允许重新规划
        arm.allow_replanning(True)

        # 设置目标位置所使用的参考坐标系
        reference_frame = 'base_link'
        arm.set_pose_reference_frame(reference_frame)

        # 设置每次运动规划的时间限制:5s
        arm.set_planning_time(5)

        # 设置场景物体的名称
        table_id = 'table'
        box1_id = 'box1'
        box2_id = 'box2'

        # 移除场景中之前运行残留的物体
        scene.remove_world_object(table_id)
        scene.remove_world_object(box1_id)
        scene.remove_world_object(box2_id)
        rospy.sleep(1)

        # 控制机械臂先回到初始化位置
        arm.set_named_target('home')
        arm.go()
        rospy.sleep(2)

        # 设置桌面的高度
        #table_ground = 0.25
        table_ground = 0.10

        # 设置table、box1和box2的三维尺寸
        table_size = [0.2, 0.7, 0.01]
        box1_size = [0.1, 0.05, 0.05]
        box2_size = [0.05, 0.05, 0.15]

        # 将三个物体加入场景当中
        table_pose = PoseStamped()
        table_pose.header.frame_id = reference_frame
        #table_pose.pose.position.x = 0.26
        table_pose.pose.position.x = 0.30
        table_pose.pose.position.y = 0.0
        table_pose.pose.position.z = table_ground + table_size[2] / 2.0
        table_pose.pose.orientation.w = 1.0
        scene.add_box(table_id, table_pose, table_size)

        box1_pose = PoseStamped()
        box1_pose.header.frame_id = reference_frame
        #box1_pose.pose.position.x = 0.21
        box1_pose.pose.position.x = 0.30
        box1_pose.pose.position.y = -0.1
        box1_pose.pose.position.z = table_ground + table_size[
            2] + box1_size[2] / 2.0
        box1_pose.pose.orientation.w = 1.0
        scene.add_box(box1_id, box1_pose, box1_size)

        box2_pose = PoseStamped()
        box2_pose.header.frame_id = reference_frame
        box2_pose.pose.position.x = 0.30
        box2_pose.pose.position.y = 0.15
        box2_pose.pose.position.z = table_ground + table_size[
            2] + box2_size[2] / 2.0
        box2_pose.pose.orientation.w = 1.0
        scene.add_box(box2_id, box2_pose, box2_size)

        # 将桌子设置成红色,两个box设置成橙色
        self.setColor(table_id, 0.8, 0, 0, 1.0)
        self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
        self.setColor(box2_id, 0.8, 0.4, 0, 1.0)

        # 将场景中的颜色设置发布
        self.sendColors()

        # 设置机械臂的运动目标位置,位于桌面之上两个盒子之间
        target_pose = PoseStamped()
        target_pose.header.frame_id = reference_frame
        target_pose.pose.position.x = 0.30
        target_pose.pose.position.y = 0.10
        target_pose.pose.position.z = table_pose.pose.position.z + table_size[
            2] + 0.05
        target_pose.pose.orientation.w = 1.0

        # 控制机械臂运动到目标位置
        arm.set_pose_target(target_pose, end_effector_link)
        arm.go()
        rospy.sleep(2)

        # 设置机械臂的运动目标位置,进行避障规划
        target_pose2 = PoseStamped()
        target_pose2.header.frame_id = reference_frame
        target_pose2.pose.position.x = 0.30
        target_pose2.pose.position.y = -0.10
        target_pose2.pose.position.z = table_pose.pose.position.z + table_size[
            2] + 0.05
        target_pose2.pose.orientation.w = 1.0

        # 控制机械臂运动到目标位置
        arm.set_pose_target(target_pose2, end_effector_link)
        arm.go()
        rospy.sleep(2)

        # 控制机械臂回到初始化位置
        arm.set_named_target('home')
        arm.go()

        # 关闭并退出moveit
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)
Beispiel #25
0
    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)

        rospy.init_node('moveit_demo')
        
        #Initialize robot
        robot = moveit_commander.RobotCommander()

        # Use the planning scene object to add or remove objects
        scene = PlanningSceneInterface()

        # Create a scene publisher to push changes to the scene
        self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10)

        # Create a publisher for displaying gripper poses
        self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=10)
        
        # Create a publisher for displaying object frames
        self.object_frames_pub = rospy.Publisher('object_frames', PoseStamped, queue_size=10)

        ## Create a publisher for visualizing direction ###
        self.p_pub = rospy.Publisher('target', PoseStamped, latch=True, queue_size = 10)

        # Create a dictionary to hold object colors
        self.colors = dict()

        # Initialize the MoveIt! commander for the arm
        right_arm = MoveGroupCommander(GROUP_NAME_ARM)

        # Initialize the MoveIt! commander for the gripper
        right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)

        # Allow 5 seconds per planning attempt
        right_arm.set_planning_time(5)

        # Prepare Gripper and open it
        self.ac = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',pr2c.Pr2GripperCommandAction)
        self.ac.wait_for_server()

        g_open = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.088, 100))
        g_close = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.0, 1))
        self.ac.send_goal(g_open)

        # Give the scene a chance to catch up
        rospy.sleep(2)
        
        # Prepare Gazebo Subscriber
        self.pwh = None
        self.pwh_copy = None
        self.idx_targ = None
        self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback)


        rospy.sleep(2)
        
        # PREPARE THE SCENE
       	
       	while self.pwh is None:
            rospy.sleep(0.05)

        target_id = 'target'
        self.taid = self.pwh.name.index('wood_cube_5cm')
        table_id = 'table'
        self.tid = self.pwh.name.index('table') 
        #obstacle1_id = 'obstacle1'
        #self.o1id = self.pwh.name.index('wood_block_10_2_1cm')

        # Remove leftover objects from a previous run
        scene.remove_world_object(target_id)
        scene.remove_world_object(table_id)
        #scene.remove_world_object(obstacle1_id)

        # Remove any attached objects from a previous session
        scene.remove_attached_object(GRIPPER_FRAME, target_id)

        # Set the target size [l, w, h]
        target_size = [0.05, 0.05, 0.05]
        table_size = [1.5, 0.8, 0.03]
        #obstacle1_size = [0.1, 0.025, 0.01]

        ## Set the target pose on the table
        target_pose = PoseStamped()
        target_pose.header.frame_id = REFERENCE_FRAME
        target_pose.pose = self.pwh.pose[self.taid]
        target_pose.pose.position.z += 0.025
        # Add the target object to the scene
        scene.add_box(target_id, target_pose, target_size)

        table_pose = PoseStamped()
        table_pose.header.frame_id = REFERENCE_FRAME
        table_pose.pose = self.pwh.pose[self.tid]
        table_pose.pose.position.z += 1
        scene.add_box(table_id, table_pose, table_size)
        
        #obstacle1_pose = PoseStamped()
        #obstacle1_pose.header.frame_id = REFERENCE_FRAME
        #obstacle1_pose.pose = self.pwh.pose[self.o1id]
        ## Add the target object to the scene
        #scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size)

        # Specify a pose to place the target after being picked up
        place_pose = PoseStamped()
        place_pose.header.frame_id = REFERENCE_FRAME
        place_pose.pose.position.x = 0.50
        place_pose.pose.position.y = -0.30
        place_pose.pose.orientation.w = 1.0

        # Add the target object to the scene
        scene.add_box(target_id, target_pose, target_size)
                
        ### Make the target purple ###
        self.setColor(target_id, 0.6, 0, 1, 1.0)

        # Send the colors to the planning scene
        self.sendColors()
        
        #print target_pose
        self.object_frames_pub.publish(target_pose)
        rospy.sleep(2)

        print "==================== Generating Transformations ==========================="

        M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])
        M1[0,3] = target_pose.pose.position.x
        M1[1,3] = target_pose.pose.position.y 
        M1[2,3] = target_pose.pose.position.z

        M2 = transformations.euler_matrix(0, 1.57, 0)
        M2[0,3] = 0.0  # offset about x
        M2[1,3] = 0.0   # about y
        M2[2,3] = 0.25  # about z

        T = np.dot(M1,  M2)
        pre_grasping = deepcopy(target_pose)
        pre_grasping.pose.position.x = T[0,3] 
        pre_grasping.pose.position.y = T[1,3]
        pre_grasping.pose.position.z = T[2,3]

        quat = transformations.quaternion_from_matrix(T)
        pre_grasping.pose.orientation.x = quat[0]
        pre_grasping.pose.orientation.y = quat[1]
        pre_grasping.pose.orientation.z = quat[2]
        pre_grasping.pose.orientation.w = quat[3]
        pre_grasping.header.frame_id = 'gazebo_world'


#        # Initialize the grasp object
#        g = Grasp()
#        grasps = []
#        # Set the first grasp pose to the input pose
#        g.grasp_pose = pre_grasping
#        g.allowed_touch_objects = [target_id]
#        grasps.append(deepcopy(g))

#        right_arm.pick(target_id, grasps)


        #Change the frame_id for the planning to take place!
        #target_pose.header.frame_id = 'gazebo_world'
        
        self.p_pub.publish(pre_grasping)
        right_arm.set_pose_target(pre_grasping, GRIPPER_FRAME)
        plan = right_arm.plan()
        rospy.sleep(2)
        right_arm.go(wait=True)



        # Shut down MoveIt cleanly
        moveit_commander.roscpp_shutdown()

        # Exit the script
        moveit_commander.os._exit(0)
class WorldManager:
    def __init__(self, server):

        self.server = server
        moveit_commander.roscpp_initialize(sys.argv)

        self.planning_scene_topic = rospy.get_param("planning_scene_topic")
        self.run_recognition_topic = rospy.get_param("run_recognition_topic")
        self.detected_model_frame_id = rospy.get_param(
            "detected_model_frame_id")

        self.scene = PlanningSceneInterface()
        self.robot = moveit_commander.RobotCommander()

        self.model_manager = ModelRecManager()

        self.planning_scene_service_proxy = rospy.ServiceProxy(
            self.planning_scene_topic, moveit_msgs.srv.GetPlanningScene)

        self._run_recognition_as = actionlib.SimpleActionServer(
            self.run_recognition_topic,
            graspit_msgs.msg.RunObjectRecognitionAction,
            execute_cb=self._run_recognition_as_cb,
            auto_start=False)
        self._run_recognition_as.start()
        rospy.loginfo("World Manager Node is Up and Running")

    def _run_recognition_as_cb(self, goal):
        print("_run_recognition_as_cb")

        print("about to remove_all_objects_from_planner()")
        self.remove_all_objects_from_planner()
        print("finished remove_all_objects_from_planner()")

        self.model_manager.refresh()

        print("about to add_all_objects_to_planner()")
        self.add_all_objects_to_planner()
        print("finished add_all_objects_to_planner()")

        _result = graspit_msgs.msg.RunObjectRecognitionResult()
        print("graspit_msgs.msg.RunObjectRecognitionResult()")

        for model in self.model_manager.model_list:
            object_info = graspit_msgs.msg.ObjectInfo(model.object_name,
                                                      model.model_name,
                                                      model.get_world_pose())
            _result.object_info.append(object_info)
            position = model.get_world_pose().position
            position = Point(position.x, position.y, position.z)
            make6DofMarker(
                fixed=False,
                frame=model.model_name,
                interaction_mode=InteractiveMarkerControl.MOVE_ROTATE_3D,
                position=position,
                show_6dof=True)

        print("finished for loop")
        server.applyChanges()

        self._run_recognition_as.set_succeeded(_result)
        return []

    def get_body_names_from_planner(self):
        rospy.wait_for_service(self.planning_scene_topic, 5)
        components = PlanningSceneComponents(
            PlanningSceneComponents.WORLD_OBJECT_NAMES +
            PlanningSceneComponents.TRANSFORMS)

        ps_request = moveit_msgs.srv.GetPlanningSceneRequest(
            components=components)
        ps_response = self.planning_scene_service_proxy.call(ps_request)

        body_names = [
            co.id for co in ps_response.scene.world.collision_objects
        ]

        return body_names

    def remove_all_objects_from_planner(self):

        body_names = self.get_body_names_from_planner()

        while len(body_names) > 0:
            print(
                "removing bodies from the planner, this can potentially take several tries"
            )
            for body_name in body_names:
                self.scene.remove_world_object(body_name)

            body_names = self.get_body_names_from_planner()

    def add_all_objects_to_planner(self):
        self.add_obstacles()
        for model in self.model_manager.model_list:
            model_name = model.model_name.strip('/')
            print "Adding " + str(model_name) + "To Moveit"
            filename = file_name_dict[model_name]
            if os.path.isfile(filename):

                stamped_model_pose = geometry_msgs.msg.PoseStamped()
                stamped_model_pose.header.frame_id = self.detected_model_frame_id
                stamped_model_pose.pose = model.get_world_pose()

                self.scene.add_mesh(model.object_name, stamped_model_pose,
                                    filename)

            else:
                rospy.logwarn('File doesn\'t exist - object %s, filename %s' %
                              (model.object_name, filename))

    def add_walls(self):
        walls = rospy.get_param('/walls')
        for wall_params in walls:
            rospy.loginfo("Adding wall " + str(wall_params))
            self.add_wall(wall_params)
        return

    def add_wall(self, wall_params):
        name = wall_params["name"]
        x_thickness = wall_params["x_thickness"]
        y_thickness = wall_params["y_thickness"]
        z_thickness = wall_params["z_thickness"]
        x = wall_params["x"]
        y = wall_params["y"]
        z = wall_params["z"]
        qx = wall_params["qx"]
        qy = wall_params["qy"]
        qz = wall_params["qz"]
        qw = wall_params["qw"]
        frame_id = wall_params["frame_id"]

        back_wall_pose = geometry_msgs.msg.PoseStamped()
        back_wall_pose.header.frame_id = '/' + frame_id
        wall_dimensions = [x_thickness, y_thickness, z_thickness]

        back_wall_pose.pose.position = geometry_msgs.msg.Point(**{
            'x': x,
            'y': y,
            'z': z
        })
        back_wall_pose.pose.orientation = geometry_msgs.msg.Quaternion(**{
            'x': qx,
            'y': qy,
            'z': qz,
            'w': qw
        })
        self.scene.add_box(name, back_wall_pose, wall_dimensions)

        return

    def add_obstacles(self):
        self.add_walls()
class GraspObjectServer:

    def __init__(self, name):
        # stuff for grasp planning
        self.tf_listener = tf.TransformListener()
        self.tf_broadcaster = tf.TransformBroadcaster()
        self.cbbf = ClusterBoundingBoxFinder(self.tf_listener, self.tf_broadcaster, "base_link")
        self.last_objects = RecognizedObjectArray()
        #rospy.Subscriber("object_array", RecognizedObjectArray, self.objects_callback)
        self.sub = rospy.Subscriber("/recognized_object_array", RecognizedObjectArray, self.objects_callback)
        self.grasp_publisher = rospy.Publisher("generated_grasps", PoseArray)
        
        rospy.loginfo("Connecting to pickup AS")
        self.pickup_ac = SimpleActionClient('/pickup', PickupAction)
        #pickup_ac.wait_for_server() # needed?
        
        rospy.loginfo("Connecting to grasp generator AS")
        self.grasps_ac = SimpleActionClient('/grasp_generator_server/generate', GenerateBlockGraspsAction)
        #grasps_ac.wait_for_server() # needed? 
        
        #planning scene for motion planning
        self.scene = PlanningSceneInterface()
        
        # blocking action server
        self.grasp_obj_as = ActionServer(name, GraspObjectAction, self.goal_callback, self.cancel_callback, False)
        self.feedback = GraspObjectFeedback()
        self.result = GraspObjectResult()
        self.current_goal = None
        self.grasp_obj_as.start()

    def objects_callback(self, data):
        rospy.loginfo(rospy.get_name() + ": This message contains %d objects." % len(data.objects))
        self.last_objects = data
        
    def goal_callback(self, goal):      
        if self.current_goal:
          goal.set_rejected() # "Server busy"
          return
        elif len(self.last_objects.objects) - 1 < goal.get_goal().target_id:
          goal.set_rejected() # "No objects to grasp were received on the objects topic."
          return
        else:
          #store and accept new goal
          self.current_goal = goal
          self.current_goal.set_accepted()
          #run grasping state machine
          self.grasping_sm()
          #finished, get rid of goal
          self.current_goal = None
        
    def cancel_callback(self, goal):
        #TODO stop motions?
        self.current_goal.set_canceled()

    def grasping_sm(self):
      if self.current_goal:
        self.update_feedback("running clustering")
        (object_points, obj_bbox_dims, 
         object_bounding_box, object_pose) = self.cbbf.find_object_frame_and_bounding_box(self.last_objects.objects[self.current_goal.get_goal().target_id].point_clouds[0])
        #TODO visualize bbox
        #TODO publish filtered pointcloud?
        print obj_bbox_dims
        ########
        self.update_feedback("check reachability")
        ########
        self.update_feedback("generate grasps")
        #transform pose to base_link
        self.tf_listener.waitForTransform("base_link", object_pose.header.frame_id, object_pose.header.stamp, rospy.Duration(5))
        trans_pose = self.tf_listener.transformPose("base_link", object_pose)
        object_pose = trans_pose
	      #HACK remove orientation -> pose is aligned with parent(base_link)
        object_pose.pose.orientation.w = 1.0
        object_pose.pose.orientation.x = 0.0
        object_pose.pose.orientation.y = 0.0
        object_pose.pose.orientation.z = 0.0
        # shift object pose up by halfway, clustering code gives obj frame on the bottom because of too much noise on the table cropping (2 1pixel lines behind the objects)
        # TODO remove this hack, fix it in table filtering
        object_pose.pose.position.z += obj_bbox_dims[2]/2.0
        grasp_list = self.generate_grasps(object_pose, obj_bbox_dims[0]) # width is the bbox size on x
        # check if there are grasps, if not, abort
        if len(grasp_list) == 0:
          self.update_aborted("no grasps received")
          return
        self.publish_grasps_as_poses(grasp_list)
        self.feedback.grasps = grasp_list
        self.current_goal.publish_feedback(self.feedback)
        self.result.grasps = grasp_list
        ########
        self.update_feedback("setup planning scene")
        #remove old objects
        self.scene.remove_world_object("object_to_grasp")
        # add object to grasp to planning scene      
        self.scene.add_box("object_to_grasp", object_pose, 
                           (obj_bbox_dims[0], obj_bbox_dims[1], obj_bbox_dims[2]))
        self.result.object_scene_name = "object_to_grasp"
        ########
        if self.current_goal.get_goal().execute_grasp:
          self.update_feedback("execute grasps")
          pug = self.createPickupGoal("object_to_grasp", grasp_list)
          rospy.loginfo("Sending goal")
          self.pickup_ac.send_goal(pug)
          rospy.loginfo("Waiting for result")
          self.pickup_ac.wait_for_result()
          result = self.pickup_ac.get_result()
          rospy.loginfo("Result is:")
          print result
          rospy.loginfo("Human readable error: " + str(moveit_error_dict[result.error_code.val]))
        ########
        self.update_feedback("finished")
        self.result.object_pose = object_pose
        #bounding box in a point message
        self.result.bounding_box = Point()
        self.result.bounding_box.x = obj_bbox_dims[0]
        self.result.bounding_box.y = obj_bbox_dims[1]
        self.result.bounding_box.z = obj_bbox_dims[2]
        self.current_goal.set_succeeded(result = self.result)
        #self.current_goal.set_aborted()
        
    def update_feedback(self, text):
        self.feedback.last_state = text
        self.current_goal.publish_feedback(self.feedback)
        
    def update_aborted(self, text = ""):
        self.update_feedback("aborted." + text)
        self.current_goal.set_aborted()
        
    def generate_grasps(self, pose, width):
          #send request to block grasp generator service
          if not self.grasps_ac.wait_for_server(rospy.Duration(3.0)):
            return []
          rospy.loginfo("Successfully connected.")
          goal = GenerateBlockGraspsGoal()
          goal.pose = pose.pose
          goal.width = width
          self.grasps_ac.send_goal(goal)
          rospy.loginfo("Sent goal, waiting:\n" + str(goal))
          t_start = rospy.Time.now()
          self.grasps_ac.wait_for_result()
          t_end = rospy.Time.now()
          t_total = t_end - t_start
          rospy.loginfo("Result received in " + str(t_total.to_sec()))
          grasp_list = self.grasps_ac.get_result().grasps
          return grasp_list

    def publish_grasps_as_poses(self, grasps):
          rospy.loginfo("Publishing PoseArray on /grasp_pose_from_block_bla for grasp_pose")
          graspmsg = Grasp()
          grasp_PA = PoseArray()
          header = Header()
          header.frame_id = "base_link"
          header.stamp = rospy.Time.now()
          grasp_PA.header = header
          for graspmsg in grasps:
              p = Pose(graspmsg.grasp_pose.pose.position, graspmsg.grasp_pose.pose.orientation)
              grasp_PA.poses.append(p)
          self.grasp_publisher.publish(grasp_PA)
          rospy.sleep(0.1)
          
    def createPickupGoal(self, target, possible_grasps, group="right_arm_torso_grasping"):
          """ Create a PickupGoal with the provided data"""
          pug = PickupGoal()
          pug.target_name = target
          pug.group_name = group
          pug.possible_grasps.extend(possible_grasps)
          pug.allowed_planning_time = 5.0
          pug.planning_options.planning_scene_diff.is_diff = True
          pug.planning_options.planning_scene_diff.robot_state.is_diff = True
          pug.planning_options.plan_only = False
          pug.planning_options.replan = True
          pug.planning_options.replan_attempts = 10
          pug.attached_object_touch_links = ['arm_right_5_link', "arm_right_grasping_frame"]
          pug.allowed_touch_objects.append(target)
          #pug.attached_object_touch_links.append('all')  
          return pug
Beispiel #28
0
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# Author: Ioan Sucan

import sys
import rospy
from moveit_commander import RobotCommander, PlanningSceneInterface, roscpp_initialize, roscpp_shutdown
from geometry_msgs.msg import PoseStamped

if __name__ == '__main__':

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

    scene = PlanningSceneInterface()
    robot = RobotCommander()
    rospy.sleep(1)

    # clean the scene
    scene.remove_world_object("pole")
    scene.remove_world_object("table")
    scene.remove_world_object("part")

    # publish a demo scene
    p = PoseStamped()
    p.header.frame_id = robot.get_planning_frame()
    p.pose.position.x = 0.7
    p.pose.position.y = -0.4
    p.pose.position.z = 0.85
    p.pose.orientation.w = 1.0
Beispiel #29
0
        arm.set_pose_target(p)
        planed = arm.plan()
        if len(planed.joint_trajectory.points) == 0:
            print "useless"
            attemp += 1
            again = True
    if again:
        return None
    return planed


if __name__ == '__main__':
    roscpp_initialize(sys.argv)
    rospy.init_node('moveit_py_demo', anonymous=True)
    robot = RobotCommander()
    scene = PlanningSceneInterface()

    # -----------------------------
    #  Add objecto to the scene
    #------------------------------
    
    rospy.sleep(2)
    scene.remove_world_object("base")
    scene.remove_attached_object("gripper_link", "box")

    p = geometry_msgs.msg.PoseStamped()
    p.header.frame_id = "base_link"
    p.pose.position.x = 0.165
    p.pose.position.y = 0.
    p.pose.position.z = 0.066 - 0.115
    p.pose.orientation.x = 0.707106
Beispiel #30
0
    def __init__(self):
        # 初始化move_group的API
        moveit_commander.roscpp_initialize(sys.argv)
        
        # 初始化ROS节点
        rospy.init_node('moveit_attached_object_demo')
        
        # 初始化场景对象
        scene = PlanningSceneInterface()
        rospy.sleep(1)
                                
        # 初始化需要使用move group控制的机械臂中的arm group
        arm = MoveGroupCommander('manipulator')
        
        # 获取终端link的名称
        end_effector_link = arm.get_end_effector_link()
        
        # 设置位置(单位:米)和姿态(单位:弧度)的允许误差
        arm.set_goal_position_tolerance(0.01)
        arm.set_goal_orientation_tolerance(0.05)
       
        # 当运动规划失败后,允许重新规划
        arm.allow_replanning(True)

        # 控制机械臂回到初始化位置
        arm.set_named_target('home')
        arm.go()

        # 设置每次运动规划的时间限制:10s
        arm.set_planning_time(10)
        
        # 移除场景中之前运行残留的物体
        scene.remove_attached_object(end_effector_link, 'tool')
        scene.remove_world_object('table') 

        # 设置桌面的高度
        table_ground = 0.6
        
        # 设置table和tool的三维尺寸
        table_size = [0.1, 0.3, 0.02]
        tool_size = [0.2, 0.02, 0.02]
        
        # 设置tool的位姿
        p = PoseStamped()
        p.header.frame_id = end_effector_link
        
        p.pose.position.x = tool_size[0] / 2.0
        p.pose.position.y = 0
        p.pose.position.z = -0.015
        p.pose.orientation.w = 1
        
        # 将tool附着到机器人的终端
        scene.attach_box(end_effector_link, 'tool', p, tool_size)

        # 将table加入场景当中
        table_pose = PoseStamped()
        table_pose.header.frame_id = 'base_link'
        table_pose.pose.position.x = 0.3
        table_pose.pose.position.y = 0.0
        table_pose.pose.position.z = table_ground + table_size[2] / 2.0
        table_pose.pose.orientation.w = 1.0
        scene.add_box('table', table_pose, table_size)
        
        rospy.sleep(2)  

        # 更新当前的位姿
        arm.set_start_state_to_current_state()

        # 设置机械臂的目标位置,使用六轴的位置数据进行描述(单位:弧度)
        joint_positions = [0.827228546495185, 0.29496592875743577, 1.1185644936946095, -0.7987583317769674, -0.18950024740190782, 0.11752152218233858]
        arm.set_joint_value_target(joint_positions)
                 
        # 控制机械臂完成运动
        arm.go()
        rospy.sleep(1)
        
        # 控制机械臂回到初始化位置
        arm.set_named_target('home')
        arm.go()

        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)
    def __init__(self):
        # 初始化move_group的API
        moveit_commander.roscpp_initialize(sys.argv)

        # 初始化ROS节点
        rospy.init_node('moveit_ik_demo')
        # 初始化需要使用move group控制的机械臂中的arm group

        rospy.Subscriber("obj_pose", Pose, obj_pose_get)

        # 初始化场景对象
        scene = PlanningSceneInterface()
        # 创建一个发布场景变化信息的发布者
        self.scene_pub = rospy.Publisher('planning_scene',
                                         PlanningScene,
                                         queue_size=5)
        # 创建一个存储物体颜色的字典对象
        self.colors = dict()
        # 等待场景准备就绪
        rospy.sleep(1)
        # 设置场景物体的名称
        table_id = 'table'
        box1_id = 'box1'
        box2_id = 'box2'
        # 移除场景中之前运行残留的物体
        scene.remove_world_object(table_id)
        scene.remove_world_object(box1_id)
        scene.remove_world_object(box2_id)
        rospy.sleep(1)

        obj_pose.position.x = 0.4
        obj_pose.position.y = 0.1
        obj_pose.position.z = 0.25
        # 0.40  0.35 # 0.4-5
        # 0     0    # 0
        # 0.26  0.26 # 0
        # 设置table、box1和box2的三维尺寸
        table_size = [0.2, 0.3, 0.01]
        box1_size = [0.03, 0.001, 0.08]
        # box2_size = [0.03, 0.03, 0.05]
        table_pose = PoseStamped()
        table_pose.header.frame_id = reference_frame
        table_pose.pose.position.x = table_size[
            0] / 2.0 + obj_pose.position.x - 0.05
        table_pose.pose.position.y = obj_pose.position.y
        table_pose.pose.position.z = obj_pose.position.z - table_size[
            2] / 2.0 - box1_size[2] / 2.0
        # table_pose.pose.orientation.w = 1.0
        scene.add_box(table_id, table_pose, table_size)
        box1_pose = PoseStamped()
        box1_pose.header.frame_id = reference_frame
        box1_pose.pose.position.x = obj_pose.position.x
        box1_pose.pose.position.y = obj_pose.position.y
        box1_pose.pose.position.z = obj_pose.position.z
        # box1_pose.pose.orientation.w = 1.0
        scene.add_box(box1_id, box1_pose, box1_size)

        # 将桌子设置成红色,两个box设置成橙色
        self.setColor(table_id, 0, 0, 0, 1)
        self.setColor(box1_id, 0.8, 0, 0, 1.0)
        self.setColor(box2_id, 0.8, 0, 0, 1.0)
        # self.setColor(table_id, 0.8, 0, 0, 1.0) red
        # self.setColor(box1_id, 0.8, 0.4, 0, 1.0) yellow
        # self.setColor(box1_id, 1, 1, 1, 1.0) white
        # self.setColor(sphere_id, 0, 0, 0, 1) black
        # 将场景中的颜色设置发布
        self.sendColors()
Beispiel #32
0
#!/usr/bin/env python

import sys
import rospy
from moveit_commander import RobotCommander, MoveGroupCommander
from moveit_commander import PlanningSceneInterface, roscpp_initialize, roscpp_shutdown
from geometry_msgs.msg import PoseStamped
from moveit_msgs.msg import Grasp, GripperTranslation, PlaceLocation
from trajectory_msgs.msg import JointTrajectoryPoint

if __name__ == '__main__':

    roscpp_initialize(sys.argv)
    rospy.init_node('moveit_py_demo', anonymous=True)
    GRIPPER_FRAME = 'right_gripper_link'
    scene = PlanningSceneInterface()
    robot = RobotCommander()
    right_arm = MoveGroupCommander("right_arm")
    right_gripper = MoveGroupCommander("right_gripper")
    right_arm.set_planner_id("KPIECEkConfigDefault")
    rospy.sleep(1)

    # clean the scene
    scene.remove_world_object("table")
    scene.remove_world_object("part")
    scene.remove_attached_object(GRIPPER_FRAME, "part")
    #rospy.logwarn("cleaning world")
    #right_arm.set_named_target("r_start")
    #right_arm.go()

    #right_gripper.set_named_target("open")
#! /usr/bin/env python
# -*- coding: utf-8 -*-
"""
Created on Dec 5 10:31:00 2013

@author: Sam Pfeiffer
"""

# from moveit_commander import MoveGroupCommander
from moveit_commander import RobotCommander, PlanningSceneInterface  # , roscpp_initialize, roscpp_shutdown
import rospy
import actionlib
from geometry_msgs.msg import PoseStamped
from trajectory_msgs.msg import JointTrajectoryPoint
from moveit_msgs.msg import Grasp, PickupAction, PickupGoal, PickupResult


if __name__ == "__main__":
    rospy.init_node("remove_world_objects")

    scene = PlanningSceneInterface()

    rospy.sleep(1)

    rospy.loginfo("Cleaning world objects")
    # clean the scene
    scene.remove_world_object("table")
    scene.remove_world_object("part")
Beispiel #34
0
class MoveGroupCommandInterpreter(object):
    """
    Interpreter for simple commands
    """

    DEFAULT_FILENAME = "move_group.cfg"
    GO_DIRS = {
        "up": (2, 1),
        "down": (2, -1),
        "z": (2, 1),
        "left": (1, 1),
        "right": (1, -1),
        "y": (1, 1),
        "forward": (0, 1),
        "backward": (0, -1),
        "back": (0, -1),
        "x": (0, 1)
    }

    def __init__(self):
        self._gdict = {}
        self._group_name = ""
        self._prev_group_name = ""
        self._planning_scene_interface = PlanningSceneInterface()
        self._robot = RobotCommander()
        self._last_plan = None
        self._db_host = None
        self._db_port = 33829
        self._trace = False

    def get_active_group(self):
        if len(self._group_name) > 0:
            return self._gdict[self._group_name]
        else:
            return None

    def get_loaded_groups(self):
        return self._gdict.keys()

    def execute(self, cmd):
        cmd = self.resolve_command_alias(cmd)
        result = self.execute_generic_command(cmd)
        if result != None:
            return result
        else:
            if len(self._group_name) > 0:
                return self.execute_group_command(
                    self._gdict[self._group_name], cmd)
            else:
                return (
                    MoveGroupInfoLevel.INFO, self.get_help() +
                    "\n\nNo groups initialized yet. You must call the 'use' or the 'load' command first.\n"
                )

    def execute_generic_command(self, cmd):
        if os.path.isfile("cmd/" + cmd):
            cmd = "load cmd/" + cmd
        cmdlow = cmd.lower()
        if cmdlow.startswith("use"):
            if cmdlow == "use":
                return (MoveGroupInfoLevel.INFO,
                        "\n".join(self.get_loaded_groups()))
            clist = cmd.split()
            clist[0] = clist[0].lower()
            if len(clist) == 2:
                if clist[0] == "use":
                    if clist[1] == "previous":
                        clist[1] = self._prev_group_name
                        if len(clist[1]) == 0:
                            return (MoveGroupInfoLevel.DEBUG, "OK")
                    if clist[1] in self._gdict:
                        self._prev_group_name = self._group_name
                        self._group_name = clist[1]
                        return (MoveGroupInfoLevel.DEBUG, "OK")
                    else:
                        try:
                            mg = MoveGroupCommander(clist[1])
                            self._gdict[clist[1]] = mg
                            self._group_name = clist[1]
                            return (MoveGroupInfoLevel.DEBUG, "OK")
                        except MoveItCommanderException as e:
                            return (MoveGroupInfoLevel.FAIL,
                                    "Initializing " + clist[1] + ": " + str(e))
                        except:
                            return (MoveGroupInfoLevel.FAIL,
                                    "Unable to initialize " + clist[1])
        elif cmdlow.startswith("trace"):
            if cmdlow == "trace":
                return (MoveGroupInfoLevel.INFO,
                        "trace is on" if self._trace else "trace is off")
            clist = cmdlow.split()
            if clist[0] == "trace" and clist[1] == "on":
                self._trace = True
                return (MoveGroupInfoLevel.DEBUG, "OK")
            if clist[0] == "trace" and clist[1] == "off":
                self._trace = False
                return (MoveGroupInfoLevel.DEBUG, "OK")
        elif cmdlow.startswith("load"):
            filename = self.DEFAULT_FILENAME
            clist = cmd.split()
            if len(clist) > 2:
                return (MoveGroupInfoLevel.WARN,
                        "Unable to parse load command")
            if len(clist) == 2:
                filename = clist[1]
                if not os.path.isfile(filename):
                    filename = "cmd/" + filename
            line_num = 0
            line_content = ""
            try:
                f = open(filename, 'r')
                for line in f:
                    line_num += 1
                    line = line.rstrip()
                    line_content = line
                    if self._trace:
                        print("%s:%d:  %s" %
                              (filename, line_num, line_content))
                    comment = line.find("#")
                    if comment != -1:
                        line = line[0:comment].rstrip()
                    if line != "":
                        self.execute(line.rstrip())
                    line_content = ""
                return (MoveGroupInfoLevel.DEBUG, "OK")
            except MoveItCommanderException as e:
                if line_num > 0:
                    return (MoveGroupInfoLevel.WARN,
                            "Error at %s:%d:  %s\n%s" %
                            (filename, line_num, line_content, str(e)))
                else:
                    return (MoveGroupInfoLevel.WARN,
                            "Processing " + filename + ": " + str(e))
            except:
                if line_num > 0:
                    return (MoveGroupInfoLevel.WARN, "Error at %s:%d:  %s" %
                            (filename, line_num, line_content))
                else:
                    return (MoveGroupInfoLevel.WARN,
                            "Unable to load " + filename)
        elif cmdlow.startswith("save"):
            filename = self.DEFAULT_FILENAME
            clist = cmd.split()
            if len(clist) > 2:
                return (MoveGroupInfoLevel.WARN,
                        "Unable to parse save command")
            if len(clist) == 2:
                filename = clist[1]
            try:
                f = open(filename, 'w')
                for gr in self._gdict.keys():
                    f.write("use " + gr + "\n")
                    known = self._gdict[gr].get_remembered_joint_values()
                    for v in known.keys():
                        f.write(v + " = [" +
                                " ".join([str(x) for x in known[v]]) + "]\n")
                if self._db_host != None:
                    f.write("database " + self._db_host + " " +
                            str(self._db_port) + "\n")
                return (MoveGroupInfoLevel.DEBUG, "OK")
            except:
                return (MoveGroupInfoLevel.WARN, "Unable to save " + filename)
        else:
            return None

    def execute_group_command(self, g, cmdorig):
        cmd = cmdorig.lower()

        if cmd == "stop":
            g.stop()
            return (MoveGroupInfoLevel.DEBUG, "OK")

        if cmd == "id":
            return (MoveGroupInfoLevel.INFO, g.get_name())

        if cmd == "help":
            return (MoveGroupInfoLevel.INFO, self.get_help())

        if cmd == "vars":
            known = g.get_remembered_joint_values()
            return (MoveGroupInfoLevel.INFO,
                    "[" + " ".join(known.keys()) + "]")

        if cmd == "joints":
            joints = g.get_joints()
            return (MoveGroupInfoLevel.INFO, "\n" + "\n".join(
                [str(i) + " = " + joints[i]
                 for i in range(len(joints))]) + "\n")

        if cmd == "show":
            return self.command_show(g)

        if cmd == "current":
            return self.command_current(g)

        if cmd == "ground":
            pose = PoseStamped()
            pose.pose.position.x = 0
            pose.pose.position.y = 0
            # offset such that the box is 0.1 mm below ground (to prevent collision with the robot itself)
            pose.pose.position.z = -0.0501
            pose.pose.orientation.x = 0
            pose.pose.orientation.y = 0
            pose.pose.orientation.z = 0
            pose.pose.orientation.w = 1
            pose.header.stamp = rospy.get_rostime()
            pose.header.frame_id = self._robot.get_root_link()
            self._planning_scene_interface.attach_box(
                self._robot.get_root_link(), "ground", pose, (3, 3, 0.1))
            return (MoveGroupInfoLevel.INFO, "Added ground")

        if cmd == "eef":
            if len(g.get_end_effector_link()) > 0:
                return (MoveGroupInfoLevel.INFO, g.get_end_effector_link())
            else:
                return (MoveGroupInfoLevel.INFO,
                        "There is no end effector defined")

        if cmd == "database":
            if self._db_host == None:
                return (MoveGroupInfoLevel.INFO, "Not connected to a database")
            else:
                return (MoveGroupInfoLevel.INFO, "Connected to " +
                        self._db_host + ":" + str(self._db_port))
        if cmd == "plan":
            if self._last_plan != None:
                return (MoveGroupInfoLevel.INFO, str(self._last_plan))
            else:
                return (MoveGroupInfoLevel.INFO, "No previous plan")

        if cmd == "constrain":
            g.clear_path_constraints()
            return (MoveGroupInfoLevel.SUCCESS, "Cleared path constraints")

        if cmd == "tol" or cmd == "tolerance":
            return (MoveGroupInfoLevel.INFO,
                    "Joint = %0.6g, Position = %0.6g, Orientation = %0.6g" %
                    g.get_goal_tolerance())

        if cmd == "time":
            return (MoveGroupInfoLevel.INFO, str(g.get_planning_time()))

        if cmd == "execute":
            if self._last_plan != None:
                if g.execute(self._last_plan):
                    return (MoveGroupInfoLevel.SUCCESS,
                            "Plan submitted for execution")
                else:
                    return (MoveGroupInfoLevel.WARN,
                            "Failed to submit plan for execution")
            else:
                return (MoveGroupInfoLevel.WARN, "No motion plan computed")

        # see if we have assignment between variables
        assign_match = re.match(r"^(\w+)\s*=\s*(\w+)$", cmd)
        if assign_match:
            known = g.get_remembered_joint_values()
            if assign_match.group(2) in known:
                g.remember_joint_values(assign_match.group(1),
                                        known[assign_match.group(2)])
                return (MoveGroupInfoLevel.SUCCESS, assign_match.group(1) +
                        " is now the same as " + assign_match.group(2))
            else:
                return (MoveGroupInfoLevel.WARN,
                        "Unknown command: '" + cmd + "'")

        # see if we have assignment of matlab-like vector syntax
        set_match = re.match(r"^(\w+)\s*=\s*\[([\d\.e\-\+\s]+)\]$", cmd)
        if set_match:
            try:
                g.remember_joint_values(
                    set_match.group(1),
                    [float(x) for x in set_match.group(2).split()])
                return (MoveGroupInfoLevel.SUCCESS,
                        "Remembered joint values [" + set_match.group(2) +
                        "] under the name " + set_match.group(1))
            except:
                return (MoveGroupInfoLevel.WARN,
                        "Unable to parse joint value [" + set_match.group(2) +
                        "]")

        # see if we have assignment of matlab-like element update
        component_match = re.match(
            r"^(\w+)\s*\[\s*(\d+)\s*\]\s*=\s*([\d\.e\-\+]+)$", cmd)
        if component_match:
            known = g.get_remembered_joint_values()
            if component_match.group(1) in known:
                try:
                    val = known[component_match.group(1)]
                    val[int(component_match.group(2))] = float(
                        component_match.group(3))
                    g.remember_joint_values(component_match.group(1), val)
                    return (MoveGroupInfoLevel.SUCCESS,
                            "Updated " + component_match.group(1) + "[" +
                            component_match.group(2) + "]")
                except:
                    return (MoveGroupInfoLevel.WARN,
                            "Unable to parse index or value in '" + cmd + "'")
            else:
                return (MoveGroupInfoLevel.WARN,
                        "Unknown command: '" + cmd + "'")

        clist = cmdorig.split()
        clist[0] = clist[0].lower()

        # if this is an unknown one-word command, it is probably a variable
        if len(clist) == 1:
            known = g.get_remembered_joint_values()
            if cmd in known:
                return (MoveGroupInfoLevel.INFO,
                        "[" + " ".join([str(x) for x in known[cmd]]) + "]")
            else:
                return (MoveGroupInfoLevel.WARN,
                        "Unknown command: '" + cmd + "'")

        # command with one argument
        if len(clist) == 2:
            if clist[0] == "go":
                self._last_plan = None
                if clist[1] == "rand" or clist[1] == "random":
                    vals = g.get_random_joint_values()
                    g.set_joint_value_target(vals)
                    if g.go():
                        return (MoveGroupInfoLevel.SUCCESS,
                                "Moved to random target [" +
                                " ".join([str(x) for x in vals]) + "]")
                    else:
                        return (MoveGroupInfoLevel.FAIL,
                                "Failed while moving to random target [" +
                                " ".join([str(x) for x in vals]) + "]")
                else:
                    try:
                        g.set_named_target(clist[1])
                        if g.go():
                            return (MoveGroupInfoLevel.SUCCESS,
                                    "Moved to " + clist[1])
                        else:
                            return (MoveGroupInfoLevel.FAIL,
                                    "Failed while moving to " + clist[1])
                    except MoveItCommanderException as e:
                        return (MoveGroupInfoLevel.WARN,
                                "Going to " + clist[1] + ": " + str(e))
                    except:
                        return (MoveGroupInfoLevel.WARN,
                                clist[1] + " is unknown")
            if clist[0] == "plan":
                self._last_plan = None
                vals = None
                if clist[1] == "rand" or clist[1] == "random":
                    vals = g.get_random_joint_values()
                    g.set_joint_value_target(vals)
                    self._last_plan = g.plan()[1]  # The trajectory msg
                else:
                    try:
                        g.set_named_target(clist[1])
                        self._last_plan = g.plan()[1]  # The trajectory msg
                    except MoveItCommanderException as e:
                        return (MoveGroupInfoLevel.WARN,
                                "Planning to " + clist[1] + ": " + str(e))
                    except:
                        return (MoveGroupInfoLevel.WARN,
                                clist[1] + " is unknown")
                if self._last_plan != None:
                    if len(self._last_plan.joint_trajectory.points
                           ) == 0 and len(
                               self._last_plan.multi_dof_joint_trajectory.
                               points) == 0:
                        self._last_plan = None
                dest_str = clist[1]
                if vals != None:
                    dest_str = "[" + " ".join([str(x) for x in vals]) + "]"
                if self._last_plan != None:
                    return (MoveGroupInfoLevel.SUCCESS,
                            "Planned to " + dest_str)
                else:
                    return (MoveGroupInfoLevel.FAIL,
                            "Failed while planning to " + dest_str)
            elif clist[0] == "pick":
                self._last_plan = None
                if g.pick(clist[1]):
                    return (MoveGroupInfoLevel.SUCCESS,
                            "Picked object " + clist[1])
                else:
                    return (MoveGroupInfoLevel.FAIL,
                            "Failed while trying to pick object " + clist[1])
            elif clist[0] == "place":
                self._last_plan = None
                if g.place(clist[1]):
                    return (MoveGroupInfoLevel.SUCCESS,
                            "Placed object " + clist[1])
                else:
                    return (MoveGroupInfoLevel.FAIL,
                            "Failed while trying to place object " + clist[1])
            elif clist[0] == "planner":
                g.set_planner_id(clist[1])
                return (MoveGroupInfoLevel.SUCCESS,
                        "Planner is now " + clist[1])
            elif clist[0] == "record" or clist[0] == "rec":
                g.remember_joint_values(clist[1])
                return (MoveGroupInfoLevel.SUCCESS,
                        "Remembered current joint values under the name " +
                        clist[1])
            elif clist[0] == "rand" or clist[0] == "random":
                g.remember_joint_values(clist[1], g.get_random_joint_values())
                return (MoveGroupInfoLevel.SUCCESS,
                        "Remembered random joint values under the name " +
                        clist[1])
            elif clist[0] == "del" or clist[0] == "delete":
                g.forget_joint_values(clist[1])
                return (MoveGroupInfoLevel.SUCCESS,
                        "Forgot joint values under the name " + clist[1])
            elif clist[0] == "show":
                known = g.get_remembered_joint_values()
                if clist[1] in known:
                    return (MoveGroupInfoLevel.INFO,
                            "[" + " ".join([str(x)
                                            for x in known[clist[1]]]) + "]")
                else:
                    return (MoveGroupInfoLevel.WARN,
                            "Joint values for " + clist[1] + " are not known")
            elif clist[0] == "tol" or clist[0] == "tolerance":
                try:
                    g.set_goal_tolerance(float(clist[1]))
                    return (MoveGroupInfoLevel.SUCCESS, "OK")
                except:
                    return (MoveGroupInfoLevel.WARN,
                            "Unable to parse tolerance value '" + clist[1] +
                            "'")
            elif clist[0] == "time":
                try:
                    g.set_planning_time(float(clist[1]))
                    return (MoveGroupInfoLevel.SUCCESS, "OK")
                except:
                    return (MoveGroupInfoLevel.WARN,
                            "Unable to parse planning duration value '" +
                            clist[1] + "'")
            elif clist[0] == "constrain":
                try:
                    g.set_path_constraints(clist[1])
                    return (MoveGroupInfoLevel.SUCCESS, "OK")
                except:
                    if self._db_host != None:
                        return (MoveGroupInfoLevel.WARN,
                                "Constraint " + clist[1] + " is not known.")
                    else:
                        return (MoveGroupInfoLevel.WARN,
                                "Not connected to a database.")
            elif clist[0] == "wait":
                try:
                    time.sleep(float(clist[1]))
                    return (MoveGroupInfoLevel.SUCCESS,
                            clist[1] + " seconds passed")
                except:
                    return (MoveGroupInfoLevel.WARN,
                            "Unable to wait '" + clist[1] + "' seconds")
            elif clist[0] == "database":
                try:
                    g.set_constraints_database(clist[1], self._db_port)
                    self._db_host = clist[1]
                    return (MoveGroupInfoLevel.SUCCESS, "Connected to " +
                            self._db_host + ":" + str(self._db_port))
                except:
                    return (MoveGroupInfoLevel.WARN, "Unable to connect to '" +
                            clist[1] + ":" + str(self._db_port) + "'")
            else:
                return (MoveGroupInfoLevel.WARN,
                        "Unknown command: '" + cmd + "'")

        if len(clist) == 3:
            if clist[0] == "go" and clist[1] in self.GO_DIRS:
                self._last_plan = None
                try:
                    offset = float(clist[2])
                    (axis, factor) = self.GO_DIRS[clist[1]]
                    return self.command_go_offset(g, offset, factor, axis,
                                                  clist[1])
                except MoveItCommanderException as e:
                    return (MoveGroupInfoLevel.WARN,
                            "Going " + clist[1] + ": " + str(e))
                except:
                    return (MoveGroupInfoLevel.WARN,
                            "Unable to parse distance '" + clist[2] + "'")
            elif clist[0] == "allow" and (clist[1] == "look"
                                          or clist[1] == "looking"):
                if clist[2] == "1" or clist[2] == "true" or clist[
                        2] == "T" or clist[2] == "True":
                    g.allow_looking(True)
                else:
                    g.allow_looking(False)
                return (MoveGroupInfoLevel.DEBUG, "OK")
            elif clist[0] == "allow" and (clist[1] == "replan"
                                          or clist[1] == "replanning"):
                if clist[2] == "1" or clist[2] == "true" or clist[
                        2] == "T" or clist[2] == "True":
                    g.allow_replanning(True)
                else:
                    g.allow_replanning(False)
                return (MoveGroupInfoLevel.DEBUG, "OK")
            elif clist[0] == "database":
                try:
                    g.set_constraints_database(clist[1], int(clist[2]))
                    self._db_host = clist[1]
                    self._db_port = int(clist[2])
                    return (MoveGroupInfoLevel.SUCCESS, "Connected to " +
                            self._db_host + ":" + str(self._db_port))
                except:
                    self._db_host = None
                    return (MoveGroupInfoLevel.WARN, "Unable to connect to '" +
                            clist[1] + ":" + clist[2] + "'")
        if len(clist) == 4:
            if clist[0] == "rotate":
                try:
                    g.set_rpy_target([float(x) for x in clist[1:]])
                    if g.go():
                        return (MoveGroupInfoLevel.SUCCESS,
                                "Rotation complete")
                    else:
                        return (MoveGroupInfoLevel.FAIL,
                                "Failed while rotating to " +
                                " ".join(clist[1:]))
                except MoveItCommanderException as e:
                    return (MoveGroupInfoLevel.WARN, str(e))
                except:
                    return (MoveGroupInfoLevel.WARN,
                            "Unable to parse X-Y-Z rotation  values '" +
                            " ".join(clist[1:]) + "'")
        if len(clist) >= 7:
            if clist[0] == "go":
                self._last_plan = None
                approx = False
                if (len(clist) > 7):
                    if (clist[7] == "approx" or clist[7] == "approximate"):
                        approx = True
                try:
                    p = Pose()
                    p.position.x = float(clist[1])
                    p.position.y = float(clist[2])
                    p.position.z = float(clist[3])
                    q = tf.transformations.quaternion_from_euler(
                        float(clist[4]), float(clist[5]), float(clist[6]))
                    p.orientation.x = q[0]
                    p.orientation.y = q[1]
                    p.orientation.z = q[2]
                    p.orientation.w = q[3]
                    if approx:
                        g.set_joint_value_target(p, True)
                    else:
                        g.set_pose_target(p)
                    if g.go():
                        return (MoveGroupInfoLevel.SUCCESS,
                                "Moved to pose target\n%s\n" % (str(p)))
                    else:
                        return (MoveGroupInfoLevel.FAIL,
                                "Failed while moving to pose \n%s\n" %
                                (str(p)))
                except MoveItCommanderException as e:
                    return (MoveGroupInfoLevel.WARN,
                            "Going to pose target: %s" % (str(e)))
                except:
                    return (MoveGroupInfoLevel.WARN, "Unable to parse pose '" +
                            " ".join(clist[1:]) + "'")

        return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'")

    def command_show(self, g):
        known = g.get_remembered_joint_values()
        res = []
        for k in known.keys():
            res.append(k + " = [" + " ".join([str(x) for x in known[k]]) + "]")
        return (MoveGroupInfoLevel.INFO, "\n".join(res))

    def command_current(self, g):
        res = "joints = [" + " ".join(
            [str(x) for x in g.get_current_joint_values()]) + "]"
        if len(g.get_end_effector_link()) > 0:
            res = res + "\n" + g.get_end_effector_link() + " pose = [\n" + str(
                g.get_current_pose()) + " ]"
            res = res + "\n" + g.get_end_effector_link() + " RPY = " + str(
                g.get_current_rpy())
        return (MoveGroupInfoLevel.INFO, res)

    def command_go_offset(self, g, offset, factor, dimension_index,
                          direction_name):
        if g.has_end_effector_link():
            try:
                g.shift_pose_target(dimension_index, factor * offset)
                if g.go():
                    return (MoveGroupInfoLevel.SUCCESS, "Moved " +
                            direction_name + " by " + str(offset) + " m")
                else:
                    return (MoveGroupInfoLevel.FAIL,
                            "Failed while moving " + direction_name)
            except MoveItCommanderException as e:
                return (MoveGroupInfoLevel.WARN, str(e))
            except:
                return (MoveGroupInfoLevel.WARN,
                        "Unable to process pose update")
        else:
            return (MoveGroupInfoLevel.WARN,
                    "No known end effector. Cannot move " + direction_name)

    def resolve_command_alias(self, cmdorig):
        cmd = cmdorig.lower()
        if cmd == "which":
            return "id"
        if cmd == "groups":
            return "use"
        return cmdorig

    def get_help(self):
        res = []
        res.append("Known commands:")
        res.append("  help                show this screen")
        res.append(
            "  allow looking <true|false>       enable/disable looking around")
        res.append(
            "  allow replanning <true|false>    enable/disable replanning")
        res.append("  constrain           clear path constraints")
        res.append(
            "  constrain <name>    use the constraint <name> as a path constraint"
        )
        res.append(
            "  current             show the current state of the active group")
        res.append(
            "  database            display the current database connection (if any)"
        )
        res.append(
            "  delete <name>       forget the joint values under the name <name>"
        )
        res.append(
            "  eef                 print the name of the end effector attached to the current group"
        )
        res.append(
            "  execute             execute a previously computed motion plan")
        res.append(
            "  go <name>           plan and execute a motion to the state <name>"
        )
        res.append(
            "  go rand             plan and execute a motion to a random state"
        )
        res.append(
            "  go <dir> <dx>|      plan and execute a motion in direction up|down|left|right|forward|backward for distance <dx>"
        )
        res.append(
            "  ground              add a ground plane to the planning scene")
        res.append(
            "  id|which            display the name of the group that is operated on"
        )
        res.append(
            "  joints              display names of the joints in the active group"
        )
        res.append(
            "  load [<file>]       load a set of interpreted commands from a file"
        )
        res.append("  pick <name>         pick up object <name>")
        res.append("  place <name>        place object <name>")
        res.append("  plan <name>         plan a motion to the state <name>")
        res.append("  plan rand           plan a motion to a random state")
        res.append(
            "  planner <name>      use planner <name> to plan next motion")
        res.append(
            "  record <name>       record the current joint values under the name <name>"
        )
        res.append(
            "  rotate <x> <y> <z>  plan and execute a motion to a specified orientation (about the X,Y,Z axes)"
        )
        res.append(
            "  save [<file>]       save the currently known variables as a set of commands"
        )
        res.append(
            "  show                display the names and values of the known states"
        )
        res.append("  show <name>         display the value of a state")
        res.append("  stop                stop the active group")
        res.append(
            "  time                show the configured allowed planning time")
        res.append("  time <val>          set the allowed planning time")
        res.append(
            "  tolerance           show the tolerance for reaching the goal region"
        )
        res.append(
            "  tolerance <val>     set the tolerance for reaching the goal region"
        )
        res.append(
            "  trace <on|off>      enable/disable replanning or looking around"
        )
        res.append(
            "  use <name>          switch to using the group named <name> (and load it if necessary)"
        )
        res.append(
            "  use|groups          show the group names that are already loaded"
        )
        res.append(
            "  vars                display the names of the known states")
        res.append("  wait <dt>           sleep for <dt> seconds")
        res.append("  x = y               assign the value of y to x")
        res.append("  x = [v1 v2...]      assign a vector of values to x")
        res.append(
            "  x[idx] = val        assign a value to dimension idx of x")
        return "\n".join(res)

    def get_keywords(self):
        known_vars = []
        known_constr = []
        if self.get_active_group() != None:
            known_vars = self.get_active_group().get_remembered_joint_values(
            ).keys()
            known_constr = self.get_active_group().get_known_constraints()
        groups = self._robot.get_group_names()
        return {
            'go':
            ['up', 'down', 'left', 'right', 'backward', 'forward', 'random'] +
            known_vars,
            'help': [],
            'record':
            known_vars,
            'show':
            known_vars,
            'wait': [],
            'delete':
            known_vars,
            'database': [],
            'current': [],
            'use':
            groups,
            'load': [],
            'save': [],
            'pick': [],
            'place': [],
            'plan':
            known_vars,
            'allow': ['replanning', 'looking'],
            'constrain':
            known_constr,
            'vars': [],
            'joints': [],
            'tolerance': [],
            'time': [],
            'eef': [],
            'execute': [],
            'ground': [],
            'id': []
        }
Beispiel #35
0
class MoveGroupCommandInterpreter(object):
    """
    Interpreter for simple commands
    """

    DEFAULT_FILENAME = "move_group.cfg"
    GO_DIRS = {"up" : (2,1), "down" : (2, -1), "z" : (2, 1),
               "left" : (1, 1), "right" : (1, -1), "y" : (1, 1),
               "forward" : (0, 1), "backward" : (0, -1), "back" : (0, -1), "x" : (0, 1) }

    def __init__(self):
        self._gdict = {}
        self._group_name = ""
        self._prev_group_name = ""
        self._planning_scene_interface = PlanningSceneInterface()
        self._robot = RobotCommander()
        self._last_plan = None
        self._db_host = None
        self._db_port = 33829
        self._trace = False

    def get_active_group(self):
        if len(self._group_name) > 0:
            return self._gdict[self._group_name]
        else:
            return None

    def get_loaded_groups(self):
        return self._gdict.keys()

    def execute(self, cmd):
        cmd = self.resolve_command_alias(cmd)
        result = self.execute_generic_command(cmd)
        if result != None:
            return result
        else:
            if len(self._group_name) > 0:
                return self.execute_group_command(self._gdict[self._group_name], cmd)
            else:
                return (MoveGroupInfoLevel.INFO, self.get_help() + "\n\nNo groups initialized yet. You must call the 'use' or the 'load' command first.\n")

    def execute_generic_command(self, cmd):
        if os.path.isfile("cmd/" + cmd):
            cmd = "load cmd/" + cmd
        cmdlow = cmd.lower()
        if cmdlow.startswith("use"):
            if cmdlow == "use":
                return (MoveGroupInfoLevel.INFO, "\n".join(self.get_loaded_groups()))
            clist = cmd.split()
            clist[0] = clist[0].lower()
            if len(clist) == 2:
                if clist[0] == "use":
                    if clist[1] == "previous":
                        clist[1] = self._prev_group_name
                        if len(clist[1]) == 0:
                            return (MoveGroupInfoLevel.DEBUG, "OK")
                    if self._gdict.has_key(clist[1]):
                        self._prev_group_name = self._group_name
                        self._group_name = clist[1]
                        return (MoveGroupInfoLevel.DEBUG, "OK")
                    else:
                        try:
                            mg = MoveGroupCommander(clist[1])
                            self._gdict[clist[1]] = mg
                            self._group_name = clist[1]
                            return (MoveGroupInfoLevel.DEBUG, "OK")
                        except MoveItCommanderException as e:
                            return (MoveGroupInfoLevel.FAIL, "Initializing " + clist[1] + ": " + str(e))
                        except:
                            return (MoveGroupInfoLevel.FAIL, "Unable to initialize " + clist[1])
        elif cmdlow.startswith("trace"):
            if cmdlow == "trace":
                return (MoveGroupInfoLevel.INFO, "trace is on" if self._trace else "trace is off")
            clist = cmdlow.split()
            if clist[0] == "trace" and clist[1] == "on":
                self._trace = True
                return (MoveGroupInfoLevel.DEBUG, "OK")
            if clist[0] == "trace" and clist[1] == "off":
                self._trace = False
                return (MoveGroupInfoLevel.DEBUG, "OK")
        elif cmdlow.startswith("load"):
            filename = self.DEFAULT_FILENAME
            clist = cmd.split()
            if len(clist) > 2:
                return (MoveGroupInfoLevel.WARN, "Unable to parse load command")
            if len(clist) == 2:
                filename = clist[1]
                if not os.path.isfile(filename):
                    filename = "cmd/" + filename
            line_num = 0
            line_content = ""
            try:
                f = open(filename, 'r')
                for line in f:
                    line_num += 1
                    line = line.rstrip()
                    line_content = line
                    if self._trace:
                        print ("%s:%d:  %s" % (filename, line_num, line_content))
                    comment = line.find("#")
                    if comment != -1:
                      line = line[0:comment].rstrip()
                    if line != "":
                      self.execute(line.rstrip())
                    line_content = ""
                return (MoveGroupInfoLevel.DEBUG, "OK")
            except MoveItCommanderException as e:  
                if line_num > 0:
                    return (MoveGroupInfoLevel.WARN, "Error at %s:%d:  %s\n%s" % (filename, line_num, line_content, str(e)))
                else:
                    return (MoveGroupInfoLevel.WARN, "Processing " + filename + ": " + str(e))
            except:
                if line_num > 0:
                    return (MoveGroupInfoLevel.WARN, "Error at %s:%d:  %s" % (filename, line_num, line_content))
                else:
                    return (MoveGroupInfoLevel.WARN, "Unable to load " + filename)
        elif cmdlow.startswith("save"):
            filename = self.DEFAULT_FILENAME
            clist = cmd.split()
            if len(clist) > 2:
                return (MoveGroupInfoLevel.WARN, "Unable to parse save command")
            if len(clist) == 2:
                filename = clist[1]
            try:
                f = open(filename, 'w')
                for gr in self._gdict.keys():
                    f.write("use " + gr + "\n")
                    known = self._gdict[gr].get_remembered_joint_values()
                    for v in known.keys():
                        f.write(v + " = [" + " ".join([str(x) for x in known[v]]) + "]\n")
                if self._db_host != None:
                    f.write("database " + self._db_host + " " + str(self._db_port) + "\n")
                return (MoveGroupInfoLevel.DEBUG, "OK")
            except:
                return (MoveGroupInfoLevel.WARN, "Unable to save " + filename)
        else:
            return None

    def execute_group_command(self, g, cmdorig):
        cmd = cmdorig.lower()

        if cmd == "stop":
            g.stop()
            return (MoveGroupInfoLevel.DEBUG, "OK")

        if cmd == "id":
            return (MoveGroupInfoLevel.INFO, g.get_name())

        if cmd == "help":
            return (MoveGroupInfoLevel.INFO, self.get_help())

        if cmd == "vars":
            known = g.get_remembered_joint_values()
            return (MoveGroupInfoLevel.INFO, "[" + " ".join(known.keys()) + "]")

        if cmd == "joints":
            joints = g.get_joints()
            return (MoveGroupInfoLevel.INFO, "\n" + "\n".join([str(i) + " = " + joints[i] for i in range(len(joints))]) + "\n")

        if cmd == "show":
            return self.command_show(g)

        if cmd == "current":
            return self.command_current(g)

        if cmd == "ground":
            pose = PoseStamped()
            pose.pose.position.x = 0
            pose.pose.position.y = 0
            # offset such that the box is 0.1 mm below ground (to prevent collision with the robot itself)
            pose.pose.position.z = -0.0501
            pose.pose.orientation.x = 0
            pose.pose.orientation.y = 0
            pose.pose.orientation.z = 0
            pose.pose.orientation.w = 1
            pose.header.stamp = rospy.get_rostime()
            pose.header.frame_id = self._robot.get_root_link()
            self._planning_scene_interface.attach_box(self._robot.get_root_link(), "ground", pose, (3, 3, 0.1))
            return (MoveGroupInfoLevel.INFO, "Added ground")

        if cmd == "eef":
            if len(g.get_end_effector_link()) > 0:
                return (MoveGroupInfoLevel.INFO, g.get_end_effector_link())
            else:
                return (MoveGroupInfoLevel.INFO, "There is no end effector defined")

        if cmd == "database":
            if self._db_host == None:
                return (MoveGroupInfoLevel.INFO, "Not connected to a database")
            else:
                return (MoveGroupInfoLevel.INFO, "Connected to " + self._db_host + ":" + str(self._db_port))
        if cmd == "plan":
            if self._last_plan != None:
                return (MoveGroupInfoLevel.INFO, str(self._last_plan))
            else:
                return (MoveGroupInfoLevel.INFO, "No previous plan")

        if cmd == "constrain":
            g.clear_path_constraints()
            return (MoveGroupInfoLevel.SUCCESS, "Cleared path constraints")

        if cmd == "tol" or cmd == "tolerance":
            return (MoveGroupInfoLevel.INFO, "Joint = %0.6g, Position = %0.6g, Orientation = %0.6g" % g.get_goal_tolerance())

        if cmd == "time":
            return (MoveGroupInfoLevel.INFO, str(g.get_planning_time()))

        if cmd == "execute":
            if self._last_plan != None:
                if g.execute(self._last_plan):
                    return (MoveGroupInfoLevel.SUCCESS, "Plan submitted for execution")
                else:
                    return (MoveGroupInfoLevel.WARN, "Failed to submit plan for execution")
            else:
                return (MoveGroupInfoLevel.WARN, "No motion plan computed")

        # see if we have assignment between variables
        assign_match = re.match(r"^(\w+)\s*=\s*(\w+)$", cmd)
        if assign_match:
            known = g.get_remembered_joint_values()
            if known.has_key(assign_match.group(2)):
                g.remember_joint_values(assign_match.group(1), known[assign_match.group(2)])
                return (MoveGroupInfoLevel.SUCCESS, assign_match.group(1) + " is now the same as " + assign_match.group(2))
            else:
                return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'")
        
        # see if we have assignment of matlab-like vector syntax
        set_match = re.match(r"^(\w+)\s*=\s*\[([\d\.e\-\+\s]+)\]$", cmd)
        if set_match:
            try:
                g.remember_joint_values(set_match.group(1), [float(x) for x in set_match.group(2).split()])
                return (MoveGroupInfoLevel.SUCCESS, "Remembered joint values [" + set_match.group(2) + "] under the name " + set_match.group(1))
            except:
                return (MoveGroupInfoLevel.WARN, "Unable to parse joint value [" + set_match.group(2) + "]")

        # see if we have assignment of matlab-like element update
        component_match = re.match(r"^(\w+)\s*\[\s*(\d+)\s*\]\s*=\s*([\d\.e\-\+]+)$", cmd)
        if component_match:
            known = g.get_remembered_joint_values()
            if known.has_key(component_match.group(1)):
                try:
                    val = known[component_match.group(1)]
                    val[int(component_match.group(2))] = float(component_match.group(3))
                    g.remember_joint_values(component_match.group(1), val)
                    return (MoveGroupInfoLevel.SUCCESS, "Updated " + component_match.group(1) + "[" + component_match.group(2) + "]")
                except:
                    return (MoveGroupInfoLevel.WARN, "Unable to parse index or value in '" + cmd +"'")
            else:
                return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'")

        clist = cmdorig.split()
        clist[0] = clist[0].lower()

        # if this is an unknown one-word command, it is probably a variable
        if len(clist) == 1:
            known = g.get_remembered_joint_values()
            if known.has_key(cmd):
                return (MoveGroupInfoLevel.INFO, "[" + " ".join([str(x) for x in known[cmd]]) + "]")
            else:
                return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'")

        # command with one argument
        if len(clist) == 2:
            if clist[0] == "go":
                self._last_plan = None
                if clist[1] == "rand" or clist[1] == "random":
                    vals = g.get_random_joint_values()
                    g.set_joint_value_target(vals)
                    if g.go():
                        return (MoveGroupInfoLevel.SUCCESS, "Moved to random target [" + " ".join([str(x) for x in vals]) + "]")
                    else:
                        return (MoveGroupInfoLevel.FAIL, "Failed while moving to random target [" + " ".join([str(x) for x in vals]) + "]")
                else:
                    try:
                        g.set_named_target(clist[1])
                        if g.go():
                            return (MoveGroupInfoLevel.SUCCESS, "Moved to " + clist[1])
                        else:
                            return (MoveGroupInfoLevel.FAIL, "Failed while moving to " + clist[1])
                    except MoveItCommanderException as e:
                        return (MoveGroupInfoLevel.WARN, "Going to " + clist[1] + ": " + str(e))
                    except:
                        return (MoveGroupInfoLevel.WARN, clist[1] + " is unknown")
            if clist[0] == "plan":
                self._last_plan = None
                vals = None
                if clist[1] == "rand" or clist[1] == "random":
                    vals = g.get_random_joint_values()
                    g.set_joint_value_target(vals)
                    self._last_plan = g.plan()
                else:
                    try:
                        g.set_named_target(clist[1])
                        self._last_plan = g.plan()
                    except MoveItCommanderException as e:
                        return (MoveGroupInfoLevel.WARN, "Planning to " + clist[1] + ": " + str(e))
                    except:
                        return (MoveGroupInfoLevel.WARN, clist[1] + " is unknown")
                if self._last_plan != None:
                    if len(self._last_plan.joint_trajectory.points) == 0 and len(self._last_plan.multi_dof_joint_trajectory.points) == 0:
                        self._last_plan = None
                dest_str = clist[1]
                if vals != None:
                    dest_str = "[" + " ".join([str(x) for x in vals]) + "]"
                if self._last_plan != None:
                    return (MoveGroupInfoLevel.SUCCESS, "Planned to " + dest_str)
                else:
                    return (MoveGroupInfoLevel.FAIL, "Failed while planning to " + dest_str)
            elif clist[0] == "pick":
                self._last_plan = None
                if g.pick(clist[1]):
                    return (MoveGroupInfoLevel.SUCCESS, "Picked object " + clist[1])
                else:
                    return (MoveGroupInfoLevel.FAIL, "Failed while trying to pick object " + clist[1])
            elif clist[0] == "place":
                self._last_plan = None
                if g.place(clist[1]):
                    return (MoveGroupInfoLevel.SUCCESS, "Placed object " + clist[1])
                else:
                    return (MoveGroupInfoLevel.FAIL, "Failed while trying to place object " + clist[1])
            elif clist[0] == "planner":
                g.set_planner_id(clist[1])
                return (MoveGroupInfoLevel.SUCCESS, "Planner is now " + clist[1])
            elif clist[0] == "record" or clist[0] == "rec":
                g.remember_joint_values(clist[1])
                return (MoveGroupInfoLevel.SUCCESS, "Remembered current joint values under the name " + clist[1])
            elif clist[0] == "rand" or clist[0] == "random":
                g.remember_joint_values(clist[1], g.get_random_joint_values())
                return (MoveGroupInfoLevel.SUCCESS, "Remembered random joint values under the name " + clist[1])
            elif clist[0] == "del" or clist[0] == "delete":
                g.forget_joint_values(clist[1])    
                return (MoveGroupInfoLevel.SUCCESS, "Forgot joint values under the name " + clist[1])
            elif clist[0] == "show":
                known = g.get_remembered_joint_values()
                if known.has_key(clist[1]):
                    return (MoveGroupInfoLevel.INFO, "[" + " ".join([str(x) for x in known[clist[1]]]) + "]")
                else:
                    return (MoveGroupInfoLevel.WARN, "Joint values for " + clist[1] + " are not known")
            elif clist[0] == "tol" or clist[0] == "tolerance":
                try:
                    g.set_goal_tolerance(float(clist[1]))
                    return (MoveGroupInfoLevel.SUCCESS, "OK")
                except:
                    return (MoveGroupInfoLevel.WARN, "Unable to parse tolerance value '" + clist[1] + "'")
            elif clist[0] == "time":
                try:
                    g.set_planning_time(float(clist[1]))
                    return (MoveGroupInfoLevel.SUCCESS, "OK")
                except:
                    return (MoveGroupInfoLevel.WARN, "Unable to parse planning duration value '" + clist[1] + "'")
            elif clist[0] == "constrain":
                try:
                    g.set_path_constraints(clist[1])
                    return (MoveGroupInfoLevel.SUCCESS, "OK")
                except:
                    if self._db_host != None:
                        return (MoveGroupInfoLevel.WARN, "Constraint " + clist[1] + " is not known.")
                    else:
                        return (MoveGroupInfoLevel.WARN, "Not connected to a database.")
            elif clist[0] == "wait":
                try:
                    time.sleep(float(clist[1]))
                    return (MoveGroupInfoLevel.SUCCESS, clist[1] + " seconds passed")
                except:
                    return (MoveGroupInfoLevel.WARN, "Unable to wait '" + clist[1] + "' seconds")
            elif clist[0] == "database":
                try:
                    g.set_constraints_database(clist[1], self._db_port)
                    self._db_host = clist[1]
                    return (MoveGroupInfoLevel.SUCCESS, "Connected to " + self._db_host + ":" + str(self._db_port))
                except:
                    return (MoveGroupInfoLevel.WARN, "Unable to connect to '" + clist[1] + ":" + str(self._db_port) + "'")
            else:
                return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'")

        if len(clist) == 3:
            if clist[0] == "go" and self.GO_DIRS.has_key(clist[1]):
                self._last_plan = None
                try:
                    offset = float(clist[2])
                    (axis, factor) = self.GO_DIRS[clist[1]]
                    return self.command_go_offset(g, offset, factor, axis, clist[1])
                except MoveItCommanderException as e:
                    return (MoveGroupInfoLevel.WARN, "Going " + clist[1] + ": " + str(e))
                except:
                    return (MoveGroupInfoLevel.WARN, "Unable to parse distance '" + clist[2] + "'")
            elif clist[0] == "allow" and (clist[1] == "look" or clist[1] == "looking"):
                if clist[2] == "1" or clist[2] == "true" or clist[2] == "T" or clist[2] == "True":
                    g.allow_looking(True)
                else:
                    g.allow_looking(False)
                return (MoveGroupInfoLevel.DEBUG, "OK")
            elif clist[0] == "allow" and (clist[1] == "replan" or clist[1] == "replanning"):
                if clist[2] == "1" or clist[2] == "true" or clist[2] == "T" or clist[2] == "True":
                    g.allow_replanning(True)
                else:
                    g.allow_replanning(False)
                return (MoveGroupInfoLevel.DEBUG, "OK")
            elif clist[0] == "database":
                try:
                    g.set_constraints_database(clist[1], int(clist[2]))
                    self._db_host = clist[1]
                    self._db_port = int(clist[2])
                    return (MoveGroupInfoLevel.SUCCESS, "Connected to " + self._db_host + ":" + str(self._db_port))
                except:
                    self._db_host = None
                    return (MoveGroupInfoLevel.WARN, "Unable to connect to '" + clist[1] + ":" + clist[2] + "'")
        if len(clist) == 4:
            if clist[0] == "rotate":
                try:
                    g.set_rpy_target([float(x) for x in clist[1:]])
                    if g.go():
                        return (MoveGroupInfoLevel.SUCCESS, "Rotation complete")
                    else:
                        return (MoveGroupInfoLevel.FAIL, "Failed while rotating to " + " ".join(clist[1:]))
                except MoveItCommanderException as e:
                    return (MoveGroupInfoLevel.WARN, str(e))
                except:
                    return (MoveGroupInfoLevel.WARN, "Unable to parse X-Y-Z rotation  values '" + " ".join(clist[1:]) + "'")
        if len(clist) >= 7:
            if clist[0] == "go":
                self._last_plan = None
                approx = False
                if (len(clist) > 7):
                    if (clist[7] == "approx" or clist[7] == "approximate"):
                        approx = True
                try:
                    p = Pose()
                    p.position.x = float(clist[1])
                    p.position.y = float(clist[2])
                    p.position.z = float(clist[3])
                    q = tf.transformations.quaternion_from_euler(float(clist[4]), float(clist[5]), float(clist[6]))
                    p.orientation.x = q[0]
                    p.orientation.y = q[1]
                    p.orientation.z = q[2]
                    p.orientation.w = q[3]
                    if approx:
                        g.set_joint_value_target(p, True)
                    else:
                        g.set_pose_target(p)
                    if g.go():
                        return (MoveGroupInfoLevel.SUCCESS, "Moved to pose target\n%s\n" % (str(p)))
                    else:
                        return (MoveGroupInfoLevel.FAIL, "Failed while moving to pose \n%s\n" % (str(p)))
                except MoveItCommanderException as e:
                    return (MoveGroupInfoLevel.WARN, "Going to pose target: %s" % (str(e)))
                except:
                    return (MoveGroupInfoLevel.WARN, "Unable to parse pose '" + " ".join(clist[1:]) + "'")
 
        return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'")

    def command_show(self, g):
        known = g.get_remembered_joint_values()
        res = []
        for k in known.keys():
            res.append(k + " = [" + " ".join([str(x) for x in known[k]]) + "]")
        return (MoveGroupInfoLevel.INFO, "\n".join(res))
        
    def command_current(self, g):
        res = "joints = [" + " ".join([str(x) for x in g.get_current_joint_values()]) + "]"
        if len(g.get_end_effector_link()) > 0:
            res = res + "\n" + g.get_end_effector_link() + " pose = [\n" + str(g.get_current_pose()) + " ]"
            res = res + "\n" + g.get_end_effector_link() + " RPY = " + str(g.get_current_rpy())
        return (MoveGroupInfoLevel.INFO, res)

    def command_go_offset(self, g, offset, factor, dimension_index, direction_name):
        if g.has_end_effector_link():
            try:
                g.shift_pose_target(dimension_index, factor * offset)
                if g.go():
                    return (MoveGroupInfoLevel.SUCCESS, "Moved " + direction_name + " by " + str(offset) + " m")
                else:
                    return (MoveGroupInfoLevel.FAIL, "Failed while moving " + direction_name)
            except MoveItCommanderException as e:
                return (MoveGroupInfoLevel.WARN, str(e))
            except:
                return (MoveGroupInfoLevel.WARN, "Unable to process pose update")
        else:
            return (MoveGroupInfoLevel.WARN, "No known end effector. Cannot move " + direction_name)

    def resolve_command_alias(self, cmdorig):
        cmd = cmdorig.lower()
        if cmd == "which":
            return "id"
        if cmd == "groups":
            return "use"
        return cmdorig

    def get_help(self):
        res = []
        res.append("Known commands:")
        res.append("  help                show this screen")
        res.append("  id|which            display the name of the group that is operated on")
        res.append("  load [<file>]       load a set of interpreted commands from a file")
        res.append("  save [<file>]       save the currently known variables as a set of commands")
        res.append("  use <name>          switch to using the group named <name> (and load it if necessary)")
        res.append("  use|groups          show the group names that are already loaded")
        res.append("  vars                display the names of the known states")
        res.append("  show                display the names and values of the known states")
        res.append("  show <name>         display the value of a state")
        res.append("  record <name>       record the current joint values under the name <name>")
        res.append("  delete <name>       forget the joint values under the name <name>")
        res.append("  current             show the current state of the active group")
        res.append("  constrain <name>    use the constraint <name> as a path constraint")
        res.append("  constrain           clear path constraints")
        res.append("  eef                 print the name of the end effector attached to the current group")
        res.append("  go <name>           plan and execute a motion to the state <name>")
        res.append("  go <dir> <dx>|      plan and execute a motion in direction up|down|left|right|forward|backward for distance <dx>")
        res.append("  go rand             plan and execute a motion to a random state")
        res.append("  plan <name>         plan a motion to the state <name>")
        res.append("  execute             execute a previously computed motion plan")
        res.append("  rotate <x> <y> <z>  plan and execute a motion to a specified orientation (about the X,Y,Z axes)")
        res.append("  tolerance           show the tolerance for reaching the goal region")
        res.append("  tolerance <val>     set the tolerance for reaching the goal region")
        res.append("  wait <dt>           sleep for <dt> seconds")
        res.append("  x = y               assign the value of y to x")
        res.append("  x[idx] = val        assign a value to dimension idx of x")
        res.append("  x = [v1 v2...]      assign a vector of values to x")
        res.append("  trace <on|off>      enable/disable replanning or looking around")
        res.append("  ground              add a ground plane to the planning scene")
        res.append("  allow replanning <true|false>    enable/disable replanning")
        res.append("  allow looking <true|false>       enable/disable looking around")
        return "\n".join(res)

    def get_keywords(self):
        known_vars = []
        known_constr = []
        if self.get_active_group() != None:
            known_vars = self.get_active_group().get_remembered_joint_values().keys()
            known_constr = self.get_active_group().get_known_constraints()
        groups = self._robot.get_group_names()
        return {'go':['up', 'down', 'left', 'right', 'backward', 'forward', 'random'] + known_vars,
                'help':[],
                'record':known_vars,
                'show':known_vars,
                'wait':[],
                'delete':known_vars,
                'database': [],
                'current':[],
                'use':groups,
                'load':[],
                'save':[],
                'pick':[],
                'place':[],
                'plan':known_vars,
                'allow':['replanning', 'looking'],
                'constrain':known_constr,
                'vars':[],
                'joints':[],
                'tolerance':[],
                'time':[],
                'eef':[],
                'execute':[],
                'ground':[],
                'id':[]}
    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)

        rospy.init_node('moveit_demo')

        # Construct the initial scene object
        scene = PlanningSceneInterface()

        # Create a scene publisher to push changes to the scene
        self.scene_pub = rospy.Publisher('planning_scene',
                                         PlanningScene,
                                         queue_size=5)

        # Create a dictionary to hold object colors
        self.colors = dict()

        # Pause for the scene to get ready
        rospy.sleep(1)

        # Initialize the move group for the right arm
        right_arm = MoveGroupCommander('right_arm')

        # DAVES Initialize the move group for the right gripper
        right_gripper = MoveGroupCommander('left_gripper')

        # Open the gripper to the neutral position
        right_gripper.set_joint_value_target([-0.5])  #radians
        #    rospy.loginfo("=============> DEBUG EXCEPT: Ignoring set_joint_value_target error")
        right_gripper.go()

        # Get the name of the end-effector link
        end_effector_link = right_arm.get_end_effector_link()

        # Allow some leeway in position (meters) and orientation (radians)
        right_arm.set_goal_position_tolerance(0.01)  # 0.01
        right_arm.set_goal_orientation_tolerance(0.1)  # 0.05

        # Allow replanning to increase the odds of a solution
        right_arm.allow_replanning(True)

        # Set the reference frame for pose targets
        reference_frame = 'base_footprint'

        # Set the right arm reference frame accordingly
        right_arm.set_pose_reference_frame(reference_frame)

        # Allow 5 seconds per planning attempt
        right_arm.set_planning_time(5)

        # Give each of the scene objects a unique name
        table_id = 'table'
        box1_id = 'box1'
        box2_id = 'box2'

        # Remove leftover objects from a previous run
        scene.remove_world_object(table_id)
        scene.remove_world_object(box1_id)
        scene.remove_world_object(box2_id)

        # Give the scene a chance to catch up
        rospy.sleep(1)

        # Start the arm in the "resting" pose stored in the SRDF file
        right_arm.set_named_target('right_arm_home')
        right_arm.go()

        rospy.sleep(2)

        # Set the height of the table off the ground
        table_ground = 0.45  # 0.75

        # DAVES ADDED
        table_offset_x = 0.30  # meters (centemeters / 100)
        table_offset_y = -0.30  # meters (positive = left)

        # Set the length, width and height of the table and boxes
        table_size = [0.2, 0.7, 0.01]
        box1_size = [0.1, 0.05, 0.05]
        box2_size = [0.05, 0.05, 0.15]

        # Add a table top and two boxes to the scene
        table_pose = PoseStamped()
        table_pose.header.frame_id = reference_frame
        table_pose.pose.position.x = 0.26 + table_offset_x
        table_pose.pose.position.y = table_offset_y
        table_pose.pose.position.z = table_ground + table_size[2] / 2.0
        table_pose.pose.orientation.w = 1.0
        scene.add_box(table_id, table_pose, table_size)

        box1_pose = PoseStamped()
        box1_pose.header.frame_id = reference_frame
        box1_pose.pose.position.x = 0.21 + table_offset_x
        box1_pose.pose.position.y = -0.1 + table_offset_y
        box1_pose.pose.position.z = table_ground + table_size[
            2] + box1_size[2] / 2.0
        box1_pose.pose.orientation.w = 1.0
        scene.add_box(box1_id, box1_pose, box1_size)

        box2_pose = PoseStamped()
        box2_pose.header.frame_id = reference_frame
        box2_pose.pose.position.x = 0.19 + table_offset_x
        box2_pose.pose.position.y = 0.15 + table_offset_y
        box2_pose.pose.position.z = table_ground + table_size[
            2] + box2_size[2] / 2.0
        box2_pose.pose.orientation.w = 1.0
        scene.add_box(box2_id, box2_pose, box2_size)

        # Make the table red and the boxes orange
        self.setColor(table_id, 0.8, 0, 0, 1.0)
        self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
        self.setColor(box2_id, 0.8, 0.4, 0, 1.0)

        # Send the colors to the planning scene
        self.sendColors()

        # Set the target pose in between the boxes and above the table
        target_pose = PoseStamped()
        target_pose.header.frame_id = reference_frame
        target_pose.pose.position.x = 0.2 + table_offset_x
        target_pose.pose.position.y = table_offset_y
        target_pose.pose.position.z = table_pose.pose.position.z + table_size[
            2] + 0.05
        target_pose.pose.orientation.w = 1.0

        # Set the target pose for the arm
        right_arm.set_pose_target(target_pose, end_effector_link)

        # Move the arm to the target pose (if possible)
        right_arm.go()

        # Pause for a moment...
        rospy.sleep(2)

        # Return the arm to the "resting" pose stored in the SRDF file
        right_arm.set_named_target('right_arm_home')
        right_arm.go()

        # Exit MoveIt cleanly
        moveit_commander.roscpp_shutdown()

        # Exit the script
        moveit_commander.os._exit(0)
    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)
        
        rospy.init_node('SceneSetup')
        
        # Construct the initial scene object
        scene = PlanningSceneInterface()
        
        # Create a scene publisher to push changes to the scene
        self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
     
        # Create a dictionary to hold object colors
        self.colors = dict()
        
        # Pause for the scene to get ready
        rospy.sleep(1)

        # Set the reference frame for pose targets
        reference_frame = 'rack1'
        table_id = 'table'
        bowl_id = 'bowl'
        pitcher_id = 'pitcher'
  
        # Remove leftover objects from a previous run
        scene.remove_world_object(table_id)
        scene.remove_world_object(bowl_id)
        scene.remove_world_object(pitcher_id)

        # Set the height of the table off the ground
        table_ground = 0.5762625 
        
        # Set the length, width and height of the table and boxes
        table_size = [1.0128, 0.481, 0.0238125]

        table_pose = PoseStamped()
        table_pose.header.frame_id = reference_frame
        table_pose.pose.position.x = 0
        table_pose.pose.position.y = 0.847725
        table_pose.pose.position.z = table_ground

        # Set the height of the bowl
        bowl_ground = 0.57816875 
        bowl_pose = PoseStamped()
        bowl_pose.header.frame_id = reference_frame
        bowl_pose.pose.position.x = 0
        bowl_pose.pose.position.y = 0.847725
        bowl_pose.pose.position.z = bowl_ground

        # Set the height of the pitcher
        pitcher_ground = 0.57816875 
        pitcher_pose = PoseStamped()
        pitcher_pose.header.frame_id = reference_frame
        pitcher_pose.pose.position.x = 0.4
        pitcher_pose.pose.position.y = 0.847725
        pitcher_pose.pose.position.z = pitcher_ground
        pitcher_pose.pose.orientation.w = -0.5
        pitcher_pose.pose.orientation.z = 0.707

        # Make the table red and the boxes orange
        self.setColor(table_id, 0.8, 0.4, 0, 1.0)
        self.setColor(bowl_id, 0, 0.4, 0.8, 1.0)
        self.setColor(pitcher_id, 0, 0.4, 0.8, 1.0)
        self.sendColors()   


        
        scene.add_box(table_id, table_pose, table_size)
        scene.add_mesh(bowl_id, bowl_pose, '/home/yzheng/catkin_ws/src/manipulation_scenarios/ycb_object_models/models/stl/bowl.stl')
               #scene.add_mesh(pitcher_id, pitcher_pose, '/home/yzheng/catkin_ws/src/manipulation_scenarios/ycb_object_models/models/stl/pitcher.stl')

        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)
class PickAndPlaceServer(object):
    def __init__(self):
        rospy.loginfo("Initalizing PickAndPlaceServer...")
        self.sg = SphericalGrasps()
        rospy.loginfo("Connecting to pickup AS")
        self.pickup_ac = SimpleActionClient('/pickup', PickupAction)
        self.pickup_ac.wait_for_server()
        rospy.loginfo("Succesfully connected.")
        '''rospy.loginfo("Connecting to place AS")
        self.place_ac = SimpleActionClient('/place', PlaceAction)
        self.place_ac.wait_for_server()
        rospy.loginfo("Succesfully connected.")'''
        self.scene = PlanningSceneInterface()
        rospy.loginfo("Connecting to /get_planning_scene service")
        self.scene_srv = rospy.ServiceProxy(
            '/get_planning_scene', GetPlanningScene)
        self.scene_srv.wait_for_service()
        rospy.loginfo("Connected.")

        rospy.loginfo("Connecting to clear octomap service...")
        self.clear_octomap_srv = rospy.ServiceProxy(
            '/clear_octomap', Empty)
        self.clear_octomap_srv.wait_for_service()
        rospy.loginfo("Connected!")

        def set_height(height):
            self.object_height = height.data
            rospy.loginfo("Object height set to " + str(self.object_height))

        rospy.Subscriber("height", Float64, set_height)

        # Get the object size
        self.object_height = rospy.get_param('~object_height')
        self.object_width = rospy.get_param('~object_width')
        self.object_depth = rospy.get_param('~object_depth')

        # Get the links of the end effector exclude from collisions
        self.links_to_allow_contact = rospy.get_param('~links_to_allow_contact', None)
        if self.links_to_allow_contact is None:
            rospy.logwarn("Didn't find any links to allow contacts... at param ~links_to_allow_contact")
        else:
            rospy.loginfo("Found links to allow contacts: " + str(self.links_to_allow_contact))

        self.pick_as = SimpleActionServer(
            '/pickup_pose', PickUpPoseAction,
            execute_cb=self.pick_cb, auto_start=False)
        self.pick_as.start()

        '''self.place_as = SimpleActionServer(
            '/place_pose', PickUpPoseAction,
            execute_cb=self.place_cb, auto_start=False)
        self.place_as.start()'''



    def pick_cb(self, goal):
        """
        :type goal: PickUpPoseGoal
        """
        error_code = self.grasp_object(goal.object_pose)
        p_res = PickUpPoseResult()
        p_res.error_code = error_code
        if error_code != 1:
            self.pick_as.set_aborted(p_res)
        else:
            self.pick_as.set_succeeded(p_res)

    '''def place_cb(self, goal):
        """
        :type goal: PickUpPoseGoal
        """
        error_code = self.place_object(goal.object_pose)
        p_res = PickUpPoseResult()
        p_res.error_code = error_code
        if error_code != 1:
            self.place_as.set_aborted(p_res)
        else:
            self.place_as.set_succeeded(p_res)'''

    def wait_for_planning_scene_object(self, object_name='part'):
        rospy.loginfo(
            "Waiting for object '" + object_name + "'' to appear in planning scene...")
        gps_req = GetPlanningSceneRequest()
        gps_req.components.components = gps_req.components.WORLD_OBJECT_NAMES

        part_in_scene = False
        while not rospy.is_shutdown() and not part_in_scene:
            # This call takes a while when rgbd sensor is set
            gps_resp = self.scene_srv.call(gps_req)
            # check if 'part' is in the answer
            for collision_obj in gps_resp.scene.world.collision_objects:
                if collision_obj.id == object_name:
                    part_in_scene = True
                    break
            else:
                rospy.sleep(1.0)

        rospy.loginfo("'" + object_name + "'' is in scene!")

    #HEARTS tried inputting multiple cubes to approximate cylinder
    #TODO re-investigate if time allows
    #def rotate_quat(self,quat,angle_deg,axis_euler):
        '''
        Rotate about axis_euler by angle_deg,the quaternion quat,
        returned as a quaternion
        '''
        #euler = tf.transformations.euler_from_quaternion(quat)
        #euler(2) = euler(0.5*np.pi)


    def grasp_object(self, object_pose):
        rospy.loginfo("Removing any previous 'part' object")
        self.scene.remove_attached_object("arm_tool_link")
        self.scene.remove_world_object("part")
        self.scene.remove_world_object("table")
        rospy.loginfo("Clearing octomap")
        self.clear_octomap_srv.call(EmptyRequest())
        rospy.sleep(2.0)  # Removing is fast
        rospy.loginfo("Adding new 'part' object")

        rospy.loginfo("Object pose: %s", object_pose.pose)
        part_pose = copy.deepcopy(object_pose)
        part_pose.pose.position.z -= self.object_height/4
        #Add object description in scene
        self.scene.add_box("part", part_pose, (self.object_depth, self.object_width,
                                               self.object_height))

        # Add rotated boxes (for collision avoidance)
        #box1_pose = copy.deepcopy(object_pose)

        rospy.loginfo("Second%s", object_pose.pose)
        table_pose = copy.deepcopy(object_pose)

        #define a virtual table below the object
        table_height = object_pose.pose.position.z - self.object_height
        table_width  = 1.8
        table_depth  = 0.5
        #TODO update so it works when detecting centre of object
        table_pose.pose.position.z += -(self.object_height)/2 -table_height/2
        table_height -= 0.008 #remove few milimeters to prevent contact between the object and the table

        self.scene.add_box("table", table_pose, (table_depth, table_width, table_height))

        # # We need to wait for the object part to appear
        self.wait_for_planning_scene_object("part")
        self.wait_for_planning_scene_object("table")

        # compute grasps
        possible_grasps = self.sg.create_grasps_from_object_pose(object_pose)
        self.pickup_ac
        goal = createPickupGoal(
            "arm_torso", "part", object_pose, possible_grasps, self.links_to_allow_contact)

        rospy.loginfo("Waiting for final pick instruction")
        rospy.wait_for_message('/pick_it',String)

        rospy.loginfo("Sending goal")
        self.pickup_ac.send_goal(goal)
        rospy.loginfo("Waiting for result")
        self.pickup_ac.wait_for_result()
        result = self.pickup_ac.get_result()
        rospy.logdebug("Using torso result: " + str(result))
        rospy.loginfo(
            "Pick result: " +
        str(moveit_error_dict[result.error_code.val]))

        return result.error_code.val

    '''def place_object(self, object_pose):
Beispiel #39
0
    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)
        
        rospy.init_node('moveit_demo')
        
        # Use the planning scene object to add or remove objects
        scene = PlanningSceneInterface()
        
        # Create a scene publisher to push changes to the scene
        self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
        
        # Create a publisher for displaying gripper poses
        self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped)
        
        # Create a dictionary to hold object colors
        self.colors = dict()
                        
        # Initialize the move group for the right arm
        right_arm = MoveGroupCommander(GROUP_NAME_ARM)
        
        # Initialize the move group for the right gripper
        right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
        
        # Get the name of the end-effector link
        end_effector_link = right_arm.get_end_effector_link()
 
        # Allow some leeway in position (meters) and orientation (radians)
        right_arm.set_goal_position_tolerance(0.05)
        right_arm.set_goal_orientation_tolerance(0.1)

        # 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(REFERENCE_FRAME)
        
        # Allow 10 seconds per planning attempt
        right_arm.set_planning_time(10)
        
        # Set a limit on the number of pick attempts before bailing
        max_pick_attempts = 10
        
        # Set a limit on the number of place attempts
        max_place_attempts = 5
                
        # Give the scene a chance to catch up
        rospy.sleep(2)

        # Give each of the scene objects a unique name        
        table_id = 'table'
        box1_id = 'box1'
        box2_id = 'box2'
        target_id = 'target'
        tool_id = 'tool'
                
        # Remove leftover objects from a previous run
        scene.remove_world_object(table_id)
        scene.remove_world_object(box1_id)
        scene.remove_world_object(box2_id)
        scene.remove_world_object(target_id)
        scene.remove_world_object(tool_id)
        
        # Remove any attached objects from a previous session
        scene.remove_attached_object(GRIPPER_FRAME, target_id)
        
        # Give the scene a chance to catch up    
        rospy.sleep(1)
        
        # Start the arm in the "resting" pose stored in the SRDF file
        right_arm.set_named_target('right_start')
        right_arm.go()
        
        # Open the gripper to the neutral position
        right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
        right_gripper.go()
       
        rospy.sleep(1)

        # Set the height of the table off the ground
        table_ground = 0.65
        
        # Set the dimensions of the scene objects [l, w, h]
        table_size = [0.2, 0.7, 0.01]
        box1_size = [0.1, 0.05, 0.05]
        box2_size = [0.05, 0.05, 0.15]
        
        # Set the target size [l, w, h]
        target_size = [0.02, 0.01, 0.12]
        
        # Add a table top and two boxes to the scene
        table_pose = PoseStamped()
        table_pose.header.frame_id = REFERENCE_FRAME
        table_pose.pose.position.x = 0.55
        table_pose.pose.position.y = 0.0
        table_pose.pose.position.z = table_ground + table_size[2] / 2.0
        table_pose.pose.orientation.w = 1.0
        scene.add_box(table_id, table_pose, table_size)
        
        box1_pose = PoseStamped()
        box1_pose.header.frame_id = REFERENCE_FRAME
        box1_pose.pose.position.x = 0.55
        box1_pose.pose.position.y = -0.1
        box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0
        box1_pose.pose.orientation.w = 1.0   
        scene.add_box(box1_id, box1_pose, box1_size)
        
        box2_pose = PoseStamped()
        box2_pose.header.frame_id = REFERENCE_FRAME
        box2_pose.pose.position.x = 0.54
        box2_pose.pose.position.y = 0.13
        box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0
        box2_pose.pose.orientation.w = 1.0   
        scene.add_box(box2_id, box2_pose, box2_size)       
        
        # Set the target pose in between the boxes and on the table
        target_pose = PoseStamped()
        target_pose.header.frame_id = REFERENCE_FRAME
        target_pose.pose.position.x = 0.60
        target_pose.pose.position.y = 0.0
        target_pose.pose.position.z = table_ground + table_size[2] + target_size[2] / 2.0
        target_pose.pose.orientation.w = 1.0
        
        # Add the target object to the scene
        scene.add_box(target_id, target_pose, target_size)
        
        # Make the table red and the boxes orange
        self.setColor(table_id, 0.8, 0, 0, 1.0)
        self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
        self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
        
        # Make the target yellow
        self.setColor(target_id, 0.9, 0.9, 0, 1.0)
        
        # Send the colors to the planning scene
        self.sendColors()
        
        # Set the support surface name to the table object
        right_arm.set_support_surface_name(table_id)
        
        # Specify a pose to place the target after being picked up
        place_pose = PoseStamped()
        place_pose.header.frame_id = REFERENCE_FRAME
        place_pose.pose.position.x = 0.50
        place_pose.pose.position.y = -0.25
        place_pose.pose.position.z = table_ground + table_size[2] + target_size[2] / 2.0
        place_pose.pose.orientation.w = 1.0

        # Initialize the grasp pose to the target pose
        grasp_pose = target_pose
        
        # Shift the grasp pose by half the width of the target to center it
        grasp_pose.pose.position.y -= target_size[1] / 2.0
                
        # Generate a list of grasps
        grasps = self.make_grasps(grasp_pose, [target_id])

        # Publish the grasp poses so they can be viewed in RViz
        for grasp in grasps:
            self.gripper_pose_pub.publish(grasp.grasp_pose)
            rospy.sleep(0.2)
    
        # Track success/failure and number of attempts for pick operation
        result = None
        n_attempts = 0
        
        # Repeat until we succeed or run out of attempts
        while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
            n_attempts += 1
            rospy.loginfo("Pick attempt: " +  str(n_attempts))
            result = right_arm.pick(target_id, grasps)
            rospy.sleep(0.2)
        
        # If the pick was successful, attempt the place operation   
        if result == MoveItErrorCodes.SUCCESS:
            result = None
            n_attempts = 0
            #_------------------------now we move to the other table__________-------------------------------------------
            #_------------------------now we move to the other table__________-------------------------------------------
            # Generate valid place poses
            places = self.make_places(place_pose)
            
            # Repeat until we succeed or run out of attempts
            while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts:
                n_attempts += 1
                rospy.loginfo("Place attempt: " +  str(n_attempts))
                for place in places:
                    result = right_arm.place(target_id, place)
                    if result == MoveItErrorCodes.SUCCESS:
                        break
                rospy.sleep(0.2)
                
            if result != MoveItErrorCodes.SUCCESS:
                rospy.loginfo("Place operation failed after " + str(n_attempts) + " attempts.")
        else:
            rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.")
                
        # Return the arm to the "resting" pose stored in the SRDF file
        right_arm.set_named_target('right_start')
        right_arm.go()
        
        # Open the gripper to the neutral position
        right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
        right_gripper.go()
       
        rospy.sleep(1)

        # Shut down MoveIt cleanly
        moveit_commander.roscpp_shutdown()
        
        # Exit the script
        moveit_commander.os._exit(0)
    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)

        rospy.init_node('moveit_demo')

        # Use the planning scene object to add or remove objects
        scene = PlanningSceneInterface()

        # Create a scene publisher to push changes to the scene
        self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)

        # Create a publisher for displaying gripper poses
        self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped)

        # Initialize the move group for the right arm
        Arm = MoveGroupCommander(GROUP_NAME_ARM)

        Arm.set_planner_id("RRTstarkConfigDefault")
        # Initialize the move group for the right gripper
        Hand = MoveGroupCommander(GROUP_NAME_GRIPPER)

        # Get the name of the end-effector link
        end_effector_link = Arm.get_end_effector_link()

        # Allow some leeway in position (meters) and orientation (radians)
        Arm.set_goal_position_tolerance(0.05)
        Arm.set_goal_orientation_tolerance(0.1)

        # Allow replanning to increase the odds of a solution
        Arm.allow_replanning(True)

        # Set the right arm reference frame
        Arm.set_pose_reference_frame(REFERENCE_FRAME)

        # Allow 5 seconds per planning attempt
        Arm.set_planning_time(10)

        # Set a limit on the number of pick attempts before bailing
        max_pick_attempts = 3

        # Set a limit on the number of place attempts
        max_place_attempts = 3

        # Give the scene a chance to catch up
        rospy.sleep(2)

        # Give each of the scene objects a unique name

        target_id = 'target'

        # Remove leftover objects from a previous run

        scene.remove_world_object(target_id)

        # Remove any attached objects from a previous session
        scene.remove_attached_object(GRIPPER_FRAME, target_id)

        # Give the scene a chance to catch up
        rospy.sleep(1)

        # Open the gripper to the neutral position
        #Hand.set_joint_value_target(GRIPPER_NEUTRAL)
        #Hand.go()

        #rospy.sleep(1)

        target_size = [0.01, 0.01, 0.01]

        # Set the target pose in between the boxes and on the table
        target_pose = PoseStamped()
        target_pose.header.frame_id = REFERENCE_FRAME
        target_pose.pose.position.x = 0.3
        target_pose.pose.position.y = 0.03
        target_pose.pose.position.z = -0.3
        target_pose.pose.orientation.w = 1.0

        # Add the target object to the scene
        scene.add_box(target_id, target_pose, target_size)

        # Initialize the grasp pose to the target pose
        grasp_pose = PoseStamped()
        grasp_pose.header.frame_id = REFERENCE_FRAME
        grasp_pose.pose = target_pose.pose
        grasp_pose.pose.position.y = 0.2

        # Generate a list of grasps
        grasps = self.make_grasps(grasp_pose, [target_id])

        # Publish the grasp poses so they can be viewed in RViz
        for grasp in grasps:
            self.gripper_pose_pub.publish(grasp.grasp_pose)
            rospy.sleep(0.2)

        # Track success/failure and number of attempts for pick operation
        result = None
        n_attempts = 0

        # Repeat until we succeed or run out of attempts
        while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
            n_attempts += 1
            rospy.loginfo("Pick attempt: " + str(n_attempts))
            result = Arm.pick(target_id, grasps)
            rospy.sleep(0.2)

        rospy.sleep(1)

        # Shut down MoveIt cleanly
        moveit_commander.roscpp_shutdown()

        # Exit the script
        moveit_commander.os._exit(0)
    arm = MoveGroupCommander(PLANNING_GROUP)

    # # tolerance
    # arm.set_goal_position_tolerance(0.01)
    # arm.set_goal_orientation_tolerance(0.01)
    # arm.set_goal_joint_tolerance(0.01)
    # arm.set_goal_tolerance(0.01)

    # Action Clients for Place
    rospy.loginfo("Connecting to place AS")
    place_ac = actionlib.SimpleActionClient('/place', PlaceAction)
    place_ac.wait_for_server()
    rospy.loginfo("Successfully connected")

    # create Planning Scene
    scene = PlanningSceneInterface()
    rospy.sleep(1)

    # publish a demo scene
    p = PoseStamped()
    p.header.frame_id = REFERENCE_LINK
    p.header.stamp = rospy.Time.now()

    quat = quaternion_from_euler(0.0, 0.0, 0.0)  # roll, pitch, yaw
    p.pose.orientation = Quaternion(*quat)

    p.pose.position.x = 0.6
    p.pose.position.y = 0.0
    p.pose.position.z = 0.0
    # add table
    scene.add_box(TABLE_OBJECT, p, (0.5, 0.5, 0.2))
    def __init__(self):
        # 初始化move_group的API
        moveit_commander.roscpp_initialize(sys.argv)
        
        # 初始化ROS节点
        rospy.init_node('moveit_pick_and_place_demo')
        
        # 初始化场景对象
        scene = PlanningSceneInterface()
        
        # 创建一个发布场景变化信息的发布者
        self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10)
        
        # 创建一个发布抓取姿态的发布者
        self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=10)
        
        # 创建一个存储物体颜色的字典对象
        self.colors = dict()
                        
        # 初始化需要使用move group控制的机械臂中的arm group
        arm = MoveGroupCommander(GROUP_NAME_ARM)
        
        # 初始化需要使用move group控制的机械臂中的gripper group
        gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
        
        # 获取终端link的名称
        end_effector_link = arm.get_end_effector_link()
 
        # 设置位置(单位:米)和姿态(单位:弧度)的允许误差
        arm.set_goal_position_tolerance(0.05)
        arm.set_goal_orientation_tolerance(0.1)

        # 当运动规划失败后,允许重新规划
        arm.allow_replanning(True)
        
        # 设置目标位置所使用的参考坐标系
        arm.set_pose_reference_frame(REFERENCE_FRAME)
        
        # 设置每次运动规划的时间限制:5s
        arm.set_planning_time(5)
        
        # 设置pick和place阶段的最大尝试次数
        max_pick_attempts = 5
        max_place_attempts = 5
        rospy.sleep(2)

        # 设置场景物体的名称 
        table_id = 'table'
        box1_id = 'box1'
        box2_id = 'box2'
        target_id = 'target'
                
        # 移除场景中之前运行残留的物体
        scene.remove_world_object(table_id)
        scene.remove_world_object(box1_id)
        scene.remove_world_object(box2_id)
        scene.remove_world_object(target_id)
        
        # 移除场景中之前与机器臂绑定的物体
        scene.remove_attached_object(GRIPPER_FRAME, target_id)  
        rospy.sleep(1)
        
        # 控制机械臂先回到初始化位置
        arm.set_named_target('home')
        arm.go()
        
        # 控制夹爪张开
        gripper.set_joint_value_target(GRIPPER_OPEN)
        gripper.go()
        rospy.sleep(1)

        # 设置桌面的高度
        table_ground = 0.2
        
        # 设置table、box1和box2的三维尺寸[长, 宽, 高]
        table_size = [0.2, 0.7, 0.01]
        box1_size = [0.1, 0.05, 0.05]
        box2_size = [0.05, 0.05, 0.15]
        
        # 将三个物体加入场景当中
        table_pose = PoseStamped()
        table_pose.header.frame_id = REFERENCE_FRAME
        table_pose.pose.position.x = 0.45
        table_pose.pose.position.y = 0.0
        table_pose.pose.position.z = table_ground + table_size[2] / 2.0
        table_pose.pose.orientation.w = 1.0
        scene.add_box(table_id, table_pose, table_size)
        
        box1_pose = PoseStamped()
        box1_pose.header.frame_id = REFERENCE_FRAME
        box1_pose.pose.position.x = 0.41
        box1_pose.pose.position.y = -0.1
        box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0
        box1_pose.pose.orientation.w = 1.0   
        scene.add_box(box1_id, box1_pose, box1_size)
        
        box2_pose = PoseStamped()
        box2_pose.header.frame_id = REFERENCE_FRAME
        box2_pose.pose.position.x = 0.39
        box2_pose.pose.position.y = 0.13
        box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0
        box2_pose.pose.orientation.w = 1.0   
        scene.add_box(box2_id, box2_pose, box2_size)       
                
        # 将桌子设置成蓝色,两个box设置成橙色
        self.setColor(table_id, 0, 0, 0.8, 1.0)
        self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
        self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
        
        # 设置目标物体的尺寸
        target_size = [0.02, 0.01, 0.12]
        
        # 设置目标物体的位置,位于桌面之上两个盒子之间
        target_pose = PoseStamped()
        target_pose.header.frame_id = REFERENCE_FRAME
        target_pose.pose.position.x = 0.42
        target_pose.pose.position.y = 0.0
        target_pose.pose.position.z = table_ground + table_size[2] + target_size[2] / 2.0
        target_pose.pose.orientation.w = 1.0
        
        # 将抓取的目标物体加入场景中
        scene.add_box(target_id, target_pose, target_size)
        
        # 将目标物体设置为黄色
        self.setColor(target_id, 0.9, 0.9, 0, 1.0)
        
        # 将场景中的颜色设置发布
        self.sendColors()

        # 设置支持的外观
        arm.set_support_surface_name(table_id)
        
        # 设置一个place阶段需要放置物体的目标位置
        place_pose = PoseStamped()
        place_pose.header.frame_id = REFERENCE_FRAME
        place_pose.pose.position.x = 0.42
        place_pose.pose.position.y = -0.2
        place_pose.pose.position.z = table_ground + table_size[2] + target_size[2] / 2.0
        place_pose.pose.orientation.w = 1.0

        # 将目标位置设置为机器人的抓取目标位置
        grasp_pose = target_pose
                
        # 生成抓取姿态
        grasps = self.make_grasps(grasp_pose, [target_id])

        # 将抓取姿态发布,可以在rviz中显示
        for grasp in grasps:
            self.gripper_pose_pub.publish(grasp.grasp_pose)
            rospy.sleep(0.2)
    
        # 追踪抓取成功与否,以及抓取的尝试次数
        result = None
        n_attempts = 0
        
        # 重复尝试抓取,直道成功或者超多最大尝试次数
        while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
            n_attempts += 1
            rospy.loginfo("Pick attempt: " +  str(n_attempts))
            result = arm.pick(target_id, grasps)
            rospy.sleep(0.2)
        
        # 如果pick成功,则进入place阶段 
        if result == MoveItErrorCodes.SUCCESS:
            result = None
            n_attempts = 0
            
            # 生成放置姿态
            places = self.make_places(place_pose)
            
            # 重复尝试放置,直道成功或者超多最大尝试次数
            while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts:
                n_attempts += 1
                rospy.loginfo("Place attempt: " +  str(n_attempts))
                for place in places:
                    result = arm.place(target_id, place)
                    if result == MoveItErrorCodes.SUCCESS:
                        break
                rospy.sleep(0.2)
                
            if result != MoveItErrorCodes.SUCCESS:
                rospy.loginfo("Place operation failed after " + str(n_attempts) + " attempts.")
        else:
            rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.")
                
        # 控制机械臂回到初始化位置
        arm.set_named_target('home')
        arm.go()
        
        # 控制夹爪回到张开的状态
        gripper.set_joint_value_target(GRIPPER_OPEN)
        gripper.go()
        rospy.sleep(1)

        # 关闭并退出moveit
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)
 
### Helper function 
def gen_pose(frame_id="/base_link", pos=[0,0,0], euler=[0,0,0]):
	pose = PoseStamped()
	pose.header.frame_id = frame_id
	pose.header.stamp = rospy.Time.now()
	pose.pose.position.x, pose.pose.position.y, pose.pose.position.z = pos
	pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w = quaternion_from_euler(*euler)
	return pose

if __name__ == '__main__':
	rospy.init_node('scripting_example')
	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()
Beispiel #44
0
    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)

        rospy.init_node('moveit_demo')

        # Construct the initial scene object
        scene = PlanningSceneInterface()

        # Create a scene publisher to push changes to the scene
        self.scene_pub = rospy.Publisher('planning_scene',
                                         PlanningScene,
                                         queue_size=5)

        # Create a dictionary to hold object colors
        self.colors = dict()

        # Pause for the scene to get ready
        rospy.sleep(1)

        # Initialize the move group for the lwr_kuka4plus
        lwr_kuka4plus = MoveGroupCommander('lwr_kuka4plus')

        # Get the name of the end-effector link
        end_effector_link = lwr_kuka4plus.get_end_effector_link()

        # Allow some leeway in position (meters) and orientation (radians)
        lwr_kuka4plus.set_goal_position_tolerance(0.01)
        lwr_kuka4plus.set_goal_orientation_tolerance(0.05)

        # Allow replanning to increase the odds of a solution
        lwr_kuka4plus.allow_replanning(True)

        # Set the reference frame for pose targets
        reference_frame = 'lwr_base_link'

        # Set the lwr_kuka4plus reference frame accordingly
        lwr_kuka4plus.set_pose_reference_frame(reference_frame)

        # Allow 5 seconds per planning attempt
        lwr_kuka4plus.set_planning_time(5)

        # Give each of the scene objects a unique name
        table_id = 'table'
        box1_id = 'box1'
        box2_id = 'box2'

        # Remove leftover objects from a previous run
        scene.remove_world_object(table_id)
        scene.remove_world_object(box1_id)
        scene.remove_world_object(box2_id)

        # Give the scene a chance to catch up
        rospy.sleep(1)

        # Start the arm in the "home" pose stored in the SRDF file
        lwr_kuka4plus.set_named_target('home')
        lwr_kuka4plus.go()

        rospy.sleep(2)

        # Set the height of the table off the ground
        table_ground = 0.75

        # Set the length, width and height of the table and boxes
        table_size = [0.2, 0.7, 0.01]
        box1_size = [0.1, 0.05, 0.05]
        box2_size = [0.05, 0.05, 0.15]

        # Add a table top and two boxes to the scene
        table_pose = PoseStamped()
        table_pose.header.frame_id = reference_frame
        table_pose.pose.position.x = 0.46
        table_pose.pose.position.y = 0.0
        table_pose.pose.position.z = table_ground + table_size[2] / 2.0
        table_pose.pose.orientation.w = 1.0
        scene.add_box(table_id, table_pose, table_size)

        box1_pose = PoseStamped()
        box1_pose.header.frame_id = reference_frame
        box1_pose.pose.position.x = 0.41
        box1_pose.pose.position.y = -0.1
        box1_pose.pose.position.z = table_ground + table_size[
            2] + box1_size[2] / 2.0
        box1_pose.pose.orientation.w = 1.0
        scene.add_box(box1_id, box1_pose, box1_size)

        box2_pose = PoseStamped()
        box2_pose.header.frame_id = reference_frame
        box2_pose.pose.position.x = 0.39
        box2_pose.pose.position.y = 0.15
        box2_pose.pose.position.z = table_ground + table_size[
            2] + box2_size[2] / 2.0
        box2_pose.pose.orientation.w = 1.0
        scene.add_box(box2_id, box2_pose, box2_size)

        # Make the table red and the boxes orange
        self.setColor(table_id, 0.8, 0, 0, 1.0)
        self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
        self.setColor(box2_id, 0.8, 0.4, 0, 1.0)

        # Send the colors to the planning scene
        self.sendColors()

        # Set the target pose in between the boxes and above the table
        target_pose = PoseStamped()
        target_pose.header.frame_id = reference_frame
        target_pose.pose.position.x = 0.2
        target_pose.pose.position.y = 0.0
        target_pose.pose.position.z = table_pose.pose.position.z + table_size[
            2] + 0.05
        target_pose.pose.orientation.w = 1.0

        # Set the target pose for the arm
        lwr_kuka4plus.set_pose_target(target_pose, end_effector_link)

        # Move the arm to the target pose (if possible)
        lwr_kuka4plus.go()

        # Pause for a moment...
        rospy.sleep(2)

        # Return the arm to the "home" pose stored in the SRDF file
        lwr_kuka4plus.set_named_target('home')
        lwr_kuka4plus.go()

        # Exit MoveIt cleanly
        moveit_commander.roscpp_shutdown()

        # Exit the script
        moveit_commander.os._exit(0)
Beispiel #45
0
			eef_pose.pose.orientation.x = heh[0]
			eef_pose.pose.orientation.y = heh[1]
			eef_pose.pose.orientation.z = heh[2]
			eef_pose.pose.orientation.w = heh[3]
			eef_pose = listener.transformPose('world',eef_pose)
			break
		count = count+1
		
		rate.sleep()

	return eef_pose
	
if __name__ == '__main__':

	rospy.init_node('spawnobject')
	scene = PlanningSceneInterface()

	br = tf.TransformBroadcaster()
	listener = tf.TransformListener()

	robot = MoveGroupCommander("sia5d");
	gripper = MoveGroupCommander("gripper");
	rospy.sleep(1)

	p = PoseStamped()
	p1 = PoseStamped()
	p2 = PoseStamped()
	bowl2eef = PoseStamped()	
	p_goal = PoseStamped()
	p.header.frame_id = "world"
	p1.header.frame_id = "world"
Beispiel #46
0
#!/usr/bin/env python

import sys
import rospy
from moveit_commander import RobotCommander, PlanningSceneInterface, roscpp_initialize, roscpp_shutdown, MoveGroupCommander
from geometry_msgs.msg import PoseStamped
import moveit_msgs.msg
import geometry_msgs.msg
import moveit_commander

if __name__=='__main__':

    roscpp_initialize(sys.argv)
    rospy.init_node('move_demo', anonymous=True)
    
    scene = PlanningSceneInterface()
    robot = RobotCommander()
    group = MoveGroupCommander("manipulator")

    # specify the planner
    group.set_planner_id("RRTkConfigDefault")

    rospy.sleep(3)

    ## We create this DisplayTrajectory publisher which is used below to publish
    ## trajectories for RVIZ to visualize.

    # display_trajectory_publisher = rospy.Publisher(
    #                                   '/move_group/display_planned_path',
    #                                   moveit_msgs.msg.DisplayTrajectory)
class PickAruco(object):
    def __init__(self):
        rospy.loginfo("Initalizing...")
        self.bridge = CvBridge()
        self.tfBuffer = tf2_ros.Buffer()
        self.tf_l = tf2_ros.TransformListener(self.tfBuffer)

        rospy.loginfo("Waiting for /pickup_pose AS...")
        self.pick_as = SimpleActionClient('/pickup_pose', PickUpPoseAction)
        time.sleep(1.0)
        if not self.pick_as.wait_for_server(rospy.Duration(20)):
            rospy.logerr("Could not connect to /pickup_pose AS")
            exit()
        rospy.loginfo("Waiting for /place_pose AS...")
        self.place_as = SimpleActionClient('/place_pose', PickUpPoseAction)

        self.place_as.wait_for_server()

        rospy.loginfo("Setting publishers to torso and head controller...")
        self.torso_cmd = rospy.Publisher('/torso_controller/command',
                                         JointTrajectory,
                                         queue_size=1)
        self.head_cmd = rospy.Publisher('/head_controller/command',
                                        JointTrajectory,
                                        queue_size=1)
        self.detected_pose_pub = rospy.Publisher('/detected_aruco_pose',
                                                 PoseStamped,
                                                 queue_size=1,
                                                 latch=True)

        rospy.loginfo("Waiting for '/play_motion' AS...")
        self.play_m_as = SimpleActionClient('/play_motion', PlayMotionAction)
        if not self.play_m_as.wait_for_server(rospy.Duration(20)):
            rospy.logerr("Could not connect to /play_motion AS")
            exit()
        rospy.loginfo("Connected!")
        rospy.sleep(1.0)
        rospy.loginfo("Done initializing PickAruco.")
        self.pick_g = None
        self.move_pub = rospy.Publisher('/mobile_base_controller/cmd_vel',
                                        Twist,
                                        queue_size=1)
        self.scene = PlanningSceneInterface()

    def strip_leading_slash(self, s):
        return s[1:] if s.startswith("/") else s

    def pick_aruco(self, string_operation):

        #rospy.sleep(2.0)
        if string_operation == "pick":
            self.prepare_robot()
            # Detect object
            rospy.loginfo(
                "spherical_grasp_gui: Waiting for an aruco detection")

            aruco_pose = rospy.wait_for_message('/aruco_single/pose',
                                                PoseStamped)
            aruco_pose.header.frame_id = self.strip_leading_slash(
                aruco_pose.header.frame_id)
            rospy.loginfo("Got: " + str(aruco_pose))

            rospy.loginfo("spherical_grasp_gui: Transforming from frame: " +
                          aruco_pose.header.frame_id + " to 'base_footprint'")
            ps = PoseStamped()
            ps.pose.position = aruco_pose.pose.position
            ps.pose.orientation = aruco_pose.pose.orientation
            ps.header.stamp = self.tfBuffer.get_latest_common_time(
                "base_footprint", aruco_pose.header.frame_id)
            ps.header.frame_id = aruco_pose.header.frame_id
            transform_ok = False
            while not transform_ok and not rospy.is_shutdown():
                try:
                    transform = self.tfBuffer.lookup_transform(
                        "base_footprint", ps.header.frame_id, rospy.Time(0))
                    aruco_ps = do_transform_pose(ps, transform)
                    transform_ok = True
                except tf2_ros.ExtrapolationException as e:
                    rospy.logwarn(
                        "Exception on transforming point... trying again \n(" +
                        str(e) + ")")
                    rospy.sleep(0.01)
                    ps.header.stamp = self.tfBuffer.get_latest_common_time(
                        "base_footprint", aruco_pose.header.frame_id)
                pick_g = PickUpPoseGoal()

            rospy.loginfo("Setting cube pose based on ArUco detection")
            pick_g.object_pose.pose.position = aruco_ps.pose.position
            pick_g.object_pose.pose.position.z -= 0.1 * (1.0 / 2.0)

            rospy.loginfo("aruco pose in base_footprint:" + str(pick_g))

            pick_g.object_pose.header.frame_id = 'base_footprint'
            pick_g.object_pose.pose.orientation.w = 1.0
            self.detected_pose_pub.publish(pick_g.object_pose)
            rospy.loginfo("Gonna pick:" + str(pick_g))
            self.pick_as.send_goal_and_wait(pick_g)
            rospy.loginfo("Done!")

            # rospy.loginfo("Moving arm to a safe pose")
            # pmg = PlayMotionGoal()
            # pmg.motion_name = 'pick_final_pose'
            # pmg.skip_planning = False
            # self.play_m_as.send_goal_and_wait(pmg)
            # rospy.loginfo("Raise object done.")

            # Move to safe
            home = False
            i = 0
            #self.move_back(5)
            while not home and i < 3:
                home = self.move_arm_home()
                i += 1
                rospy.sleep(0.5)

            result = self.pick_as.get_result()
            if str(moveit_error_dict[result.error_code]) != "SUCCESS":
                rospy.logerr("Failed to pick, not trying further")
                return TriggerResponse(False, "Failed to pick")
            self.pick_g = pick_g
            return TriggerResponse(True, "Succeeded")

        elif string_operation == "place":
            if not self.pick_g:
                rospy.logerr("Failed to place, no object position known")
                return TriggerResponse(
                    False, "Failed to place, no object position known")
            pick_g = self.pick_g
            # Move torso to its maximum height
            self.lift_torso()

            # Raise arm
            rospy.loginfo("Moving arm to a safe pose")
            pmg = PlayMotionGoal()
            pmg.motion_name = 'prepare_grasp'  #'pick_final_pose'
            pmg.skip_planning = False
            self.play_m_as.send_goal_and_wait(pmg)
            rospy.loginfo("Raise object done.")

            # Place the object back to its position
            rospy.loginfo("Gonna place near where it was")
            pick_g.object_pose.pose.position.z += 0.05
            pick_g.object_pose.pose.position.y -= 0.35
            self.place_as.send_goal_and_wait(pick_g)
            rospy.loginfo("Done!")
            # Move to safe
            home = False
            i = 0
            #self.move_back(5)
            while not home and i < 3:
                home = self.move_arm_home()
                i += 1
                rospy.sleep(0.5)

            result = self.place_as.get_result()
            if str(moveit_error_dict[result.error_code]) != "SUCCESS":
                rospy.logerr("Failed to place")
                return TriggerResponse(False, "Failed to place")
            self.pick_g = None
            return TriggerResponse(True, "Succeeded")

    def lift_torso(self):
        rospy.loginfo("Moving torso up")
        jt = JointTrajectory()
        jt.joint_names = ['torso_lift_joint']
        jtp = JointTrajectoryPoint()
        jtp.positions = [0.34]
        jtp.time_from_start = rospy.Duration(2.5)
        jt.points.append(jtp)
        self.torso_cmd.publish(jt)

    def lower_head(self):
        rospy.loginfo("Moving head down")
        jt = JointTrajectory()
        jt.joint_names = ['head_1_joint', 'head_2_joint']
        jtp = JointTrajectoryPoint()
        jtp.positions = [0.0, -0.75]
        jtp.time_from_start = rospy.Duration(2.0)
        jt.points.append(jtp)
        self.head_cmd.publish(jt)
        rospy.loginfo("Done.")

    def prepare_robot(self):
        rospy.loginfo("Unfold arm safely")
        pmg = PlayMotionGoal()
        pmg.motion_name = 'pregrasp'
        pmg.skip_planning = False
        self.play_m_as.send_goal_and_wait(pmg)
        rospy.loginfo("Done.")

        self.lower_head()

        rospy.loginfo("Robot prepared.")

    def move_back(self, n):
        for i in range(n):
            self.move_pub.publish(Twist(linear=Vector3(-0.15, 0, 0)))
            rospy.sleep(0.05)

    def move_arm_home(self):
        rospy.loginfo("Fold arm safely")
        pmg = PlayMotionGoal()
        pmg.motion_name = 'home'
        pmg.skip_planning = False
        res = self.play_m_as.send_goal_and_wait(pmg)
        if res != GoalStatus.SUCCEEDED:
            return False
        self.scene.remove_world_object("table")
        rospy.loginfo("Done.")
        return True
class ObjectManipulationAS:

    def __init__(self, name):
        # stuff for grasp planning
        rospy.loginfo("Getting a TransformListener...")
        self.tf_listener = tf.TransformListener()
        rospy.loginfo("Getting a TransformBroadcaster...")
        self.tf_broadcaster = tf.TransformBroadcaster()
        rospy.loginfo("Initializing a ClusterBoundingBoxFinder...")
        self.cbbf = ClusterBoundingBoxFinder(self.tf_listener, self.tf_broadcaster, "base_link")
        self.last_clusters = None
        rospy.loginfo("Subscribing to '" + RECOGNIZED_OBJECT_ARRAY_TOPIC + "'...")
        self.sub = rospy.Subscriber(RECOGNIZED_OBJECT_ARRAY_TOPIC, RecognizedObjectArray, self.objects_callback)

        if DEBUG_MODE:
            self.to_grasp_object_pose_pub = rospy.Publisher(TO_BE_GRASPED_OBJECT_POSE_TOPIC, PoseStamped)

        rospy.loginfo("Connecting to pickup AS '" + PICKUP_AS + "'...")
        self.pickup_ac = SimpleActionClient(PICKUP_AS, PickupAction)
        self.pickup_ac.wait_for_server()

        rospy.loginfo("Connecting to place AS '" + PLACE_AS + "'...")
        self.place_ac = SimpleActionClient(PLACE_AS, PlaceAction)
        self.place_ac.wait_for_server()

        rospy.loginfo("Connecting to grasp generator AS '" + GRASP_GENERATOR_AS + "'...")
        self.grasps_ac = SimpleActionClient(GRASP_GENERATOR_AS, GenerateGraspsAction)
        self.grasps_ac.wait_for_server()

        rospy.loginfo("Connecting to depth throttle server '" + DEPTH_THROTLE_SRV + "'...")
        self.depth_service = rospy.ServiceProxy(DEPTH_THROTLE_SRV, Empty)
        self.depth_service.wait_for_service()

        rospy.loginfo("Getting a PlanningSceneInterface instance...")
        self.scene = PlanningSceneInterface()

        # blocking action server
        rospy.loginfo("Creating Action Server '" + name + "'...")
        self.grasp_obj_as = ActionServer(name, ObjectManipulationAction, self.goal_callback, self.cancel_callback, False)
        self.as_feedback = ObjectManipulationFeedback()
        self.as_result = ObjectManipulationActionResult()
        self.current_goal = None

        # Take care of left and right arm grasped stuff
        self.right_hand_object = None
        self.left_hand_object = None
        self.current_side = 'right'

        rospy.loginfo("Starting '" + OBJECT_MANIPULATION_AS + "' Action Server!")
        self.grasp_obj_as.start()

    def objects_callback(self, data):
        rospy.loginfo(rospy.get_name() + ": This message contains %d objects." % len(data.objects))
        self.last_clusters = data

    def goal_callback(self, goal):
        if self.current_goal:
            goal.set_rejected()  # "Server busy"
            return
        # TODO: Check if pose is not empty, if it is, reject
#         elif len(self.last_clusters.objects) - 1 < goal.get_goal().target_id:
#             goal.set_rejected() # "No objects to grasp were received on the objects topic."
#             return
        else:
            #store and accept new goal
            self.current_goal = goal
            self.current_goal.set_accepted()
            #run the corresponding operation
            if self.current_goal.get_goal().operation == ObjectManipulationGoal.PICK:
                rospy.loginfo("ObjectManipulation: PICK!")
                self.pick_operation()
            elif self.current_goal.get_goal().operation == ObjectManipulationGoal.PLACE:
                rospy.loginfo("ObjectManipulation: PLACE!")
                self.place_operation()
            else:
                rospy.logerr("ObjectManipulation: Erroneous operation!")
            #finished, get rid of goal
            self.current_goal = None

    def cancel_callback(self, goal):
        #TODO stop motions?
        self.current_goal.set_canceled()

    def pick_operation(self):
        """Execute pick operation"""
        if self.message_fields_ok():
            self.as_result = ObjectManipulationResult()
            goal_message_field = self.current_goal.get_goal()
            self.update_feedback("Checking hand to use")
            # Check which arm group was requested and if it's currently free of objects
            if 'right' in goal_message_field.group:
                if self.right_hand_object:  # Something already in the hand
                    self.update_aborted("Right hand already contains an object.")
                    return  # necessary?
                self.current_side = 'right'
            elif 'left' in goal_message_field.group:
                if self.left_hand_object:
                    self.update_aborted("Left hand already contains an object.")
                    return
                self.current_side = 'left'

            # Publish pose of goal position
            if DEBUG_MODE:
                self.to_grasp_object_pose_pub.publish(goal_message_field.target_pose)
            self.update_feedback("Detecting clusters")
            if not self.wait_for_recognized_array(wait_time=5, timeout_time=10):  # wait until we get clusters published
                self.update_aborted("Failed detecting clusters")

            # Search closer cluster
            # transform pose to base_link if needed
            if goal_message_field.target_pose.header.frame_id != "base_link":
                self.tf_listener.waitForTransform("base_link", goal_message_field.target_pose.header.frame_id, goal_message_field.target_pose.header.stamp, rospy.Duration(5))
                object_to_grasp_pose = self.tf_listener.transformPose("base_link", goal_message_field.target_pose)
            else:
                object_to_grasp_pose = goal_message_field.target_pose.pose

            self.update_feedback("Searching closer cluster while clustering")
            (closest_cluster_id, (object_points, obj_bbox_dims, object_bounding_box, object_pose)) = self.get_id_of_closest_cluster_to_pose(object_to_grasp_pose)

            rospy.logdebug("in AS: Closest cluster id is: " + str(closest_cluster_id))
            #TODO visualize bbox
            #TODO publish filtered pointcloud?
            rospy.logdebug("BBOX: " + str(obj_bbox_dims))
            ########
            self.update_feedback("Check reachability")
            ########
            self.update_feedback("Generating grasps")
            rospy.logdebug("Object pose before tf thing is: " + str(object_pose))
            #transform pose to base_link, IS THIS NECESSARY?? should be a function in any case
            self.tf_listener.waitForTransform("base_link", object_pose.header.frame_id, object_pose.header.stamp, rospy.Duration(15))
            trans_pose = self.tf_listener.transformPose("base_link", object_pose)
            object_pose = trans_pose
            #HACK remove orientation -> pose is aligned with parent(base_link)
            object_pose.pose.orientation.w = 1.0
            object_pose.pose.orientation.x = 0.0
            object_pose.pose.orientation.y = 0.0
            object_pose.pose.orientation.z = 0.0
            # shift object pose up by halfway, clustering code gives obj frame on the bottom because of too much noise on the table cropping (2 1pixel lines behind the objects)
            # TODO remove this hack, fix it in table filtering
            object_pose.pose.position.z += obj_bbox_dims[2] / 2.0
            grasp_list = self.generate_grasps(object_pose, obj_bbox_dims[0])  # width is the bbox size on x
            # check if there are grasps, if not, abort
            if len(grasp_list) == 0:
                self.update_aborted("No grasps received")
                return
            if DEBUG_MODE:  # TODO: change to dynamic param
                publish_grasps_as_poses(grasp_list)
            self.current_goal.publish_feedback(self.as_feedback)
            ########
            self.update_feedback("Setup planning scene")
            # Add object to grasp to planning scene
            self.scene.add_box(self.current_side + "_hand_object", object_pose,
                               (obj_bbox_dims[0], obj_bbox_dims[1], obj_bbox_dims[2]))
            self.as_result.object_scene_name = self.current_side + "_hand_object"
            ########
            self.update_feedback("Execute grasps")
            pug = createPickupGoal(self.current_side + "_hand_object", grasp_list, group=goal_message_field.group)
            rospy.loginfo("Sending pickup goal")
            self.pickup_ac.send_goal(pug)
            rospy.loginfo("Waiting for result...")
            self.pickup_ac.wait_for_result()
            result = self.pickup_ac.get_result()
            rospy.loginfo("Human readable error: " + str(moveit_error_dict[result.error_code.val]))
            ########
            self.update_feedback("finished")
            self.as_result.object_pose = object_pose
            self.as_result.error_code = result.error_code
            self.as_result.error_message = str(moveit_error_dict[result.error_code.val])
            if result.error_code.val == MoveItErrorCodes.SUCCESS:
                if self.current_side == 'right':
                    self.right_hand_object = self.current_side + "_hand_object"
                elif self.current_side == 'left':
                    self.left_hand_object = self.current_side + "_hand_object"
                self.current_goal.set_succeeded(result=self.as_result)
            else:
                # Remove object as it has not been picked
                self.scene.remove_world_object(self.current_side + "_hand_object")
                self.update_aborted(text="MoveIt pick failed", manipulation_result=self.as_result)
        else:
            self.update_aborted("Goal fields not correctly fulfilled")

    def place_operation(self):
        """Execute the place operation"""
        if self.message_fields_ok():
            self.as_result = ObjectManipulationResult()
            goal_message_field = self.current_goal.get_goal()
            self.update_feedback("Checking arm to use")
            # Check which arm group was requested and if it currently has an object
            if 'right' in goal_message_field.group:
                if not self.right_hand_object:  # Something already in the hand
                    self.update_aborted("Right hand does not contain an object.")
                    return  # necessary?
                self.current_side = 'right'
                current_target = self.right_hand_object
            elif 'left' in goal_message_field.group:
                if not self.left_hand_object:
                    self.update_aborted("Left hand does not contain an object.")
                    return
                self.current_side = 'left'
                current_target = self.left_hand_object

            # transform pose to base_link if needed
            if goal_message_field.target_pose.header.frame_id != "base_link":
                self.tf_listener.waitForTransform("base_link", goal_message_field.target_pose.header.frame_id, goal_message_field.target_pose.header.stamp, rospy.Duration(5))
                placing_pose = self.tf_listener.transformPose("base_link", goal_message_field.target_pose)
            else:
                placing_pose = goal_message_field.target_pose.pose
            ####  TODO: ADD HERE LOGIC ABOUT SEARCHING GOOD PLACE POSE ####
            self.update_feedback("Sending place order to MoveIt!")
            placing_pose = PoseStamped(header=Header(frame_id="base_link"), pose=placing_pose)
            goal = createPlaceGoal(placing_pose, group=goal_message_field.group, target=current_target)
            rospy.loginfo("Sending place goal")
            self.place_ac.send_goal(goal)
            rospy.loginfo("Waiting for result...")
            self.place_ac.wait_for_result()
            result = self.place_ac.get_result()
            rospy.loginfo("Human readable error: " + str(moveit_error_dict[result.error_code.val]))
            self.as_result.object_pose = placing_pose
            self.as_result.error_code = result.error_code
            self.as_result.error_message = str(moveit_error_dict[result.error_code.val])
            if result.error_code.val == MoveItErrorCodes.SUCCESS:
                # Emptying hand
                self.update_feedback("Emptying hand")
                if self.current_side == 'right':
                    self.as_result.object_scene_name = current_target
                    self.right_hand_object = None
                elif self.current_side == 'left':
                    self.as_result.object_scene_name = current_target
                    self.left_hand_object = None
                # Removing object from the collision world
                self.scene.remove_world_object(current_target)
                self.current_goal.set_succeeded(result=self.as_result)
            else:
                self.update_aborted(text="MoveIt place failed", manipulation_result=self.as_result)
        else:
            self.update_aborted("Goal fields not correctly fulfilled")

    def message_fields_ok(self):
        """Check if the minimal fields for the message are fulfilled"""
        if self.current_goal:
            goal_message_field = self.current_goal.get_goal()
            if len(goal_message_field.group) == 0:
                rospy.logwarn("Group field empty.")
                return False
            if goal_message_field.operation != ObjectManipulationGoal.PICK and goal_message_field.operation != ObjectManipulationGoal.PLACE:
                rospy.logwarn("Asked for an operation different from PICK or PLACE: " + str(goal_message_field.operation))
                return False
            if len(goal_message_field.target_pose.header.frame_id) == 0:
                rospy.logwarn("Target pose frame_id is empty")
                return False
            return True

    def update_feedback(self, text):
        """Publish feedback with given text"""
        self.as_feedback.last_state = text
        self.current_goal.publish_feedback(self.as_feedback)

    def update_aborted(self, text="", manipulation_result=None):
        """Publish feedback and abort with the text give and optionally an ObjectManipulationResult already fulfilled"""
        self.update_feedback("aborted." + text)
        if manipulation_result == None:
            aborted_result = ObjectManipulationResult()
            aborted_result.error_message = text
            self.current_goal.set_aborted(result=aborted_result)
        else:
            self.current_goal.set_aborted(result=manipulation_result)

    def generate_grasps(self, pose, width):
        """Send request to block grasp generator service"""
        goal = GenerateGraspsGoal()
        goal.pose = pose.pose
        goal.width = width
        grasp_options = GraspGeneratorOptions()
        grasp_options.grasp_axis = GraspGeneratorOptions.GRASP_AXIS_Y
        grasp_options.grasp_direction = GraspGeneratorOptions.GRASP_DIRECTION_DOWN
        grasp_options.grasp_rotation = GraspGeneratorOptions.GRASP_ROTATION_HALF
        goal.options.append(grasp_options)
        self.grasps_ac.send_goal(goal)
        if DEBUG_MODE:
            rospy.loginfo("Sent goal, waiting:\n" + str(goal))
        self.grasps_ac.wait_for_result()
        grasp_list = self.grasps_ac.get_result().grasps
        return grasp_list

    def get_id_of_closest_cluster_to_pose(self, input_pose):
        """Returns the id of the closest cluster to the pose provided (in Pose or PoseStamped)
        and all the associated clustering information"""
        current_id = 0
        closest_pose = None
        closest_object_points = None
        closest_id = 0
        closest_bbox = None
        closest_bbox_dims = None
        closest_distance = 99999.9
        for myobject in self.last_clusters.objects:
            (object_points, obj_bbox_dims, object_bounding_box, object_pose) = self.cbbf.find_object_frame_and_bounding_box(myobject.point_clouds[0])
            self.tf_listener.waitForTransform("base_link", object_pose.header.frame_id, object_pose.header.stamp, rospy.Duration(15))
            trans_pose = self.tf_listener.transformPose("base_link", object_pose)
            object_pose = trans_pose
            rospy.loginfo("id: " + str(current_id) + "\n pose:\n" + str(object_pose))
            if closest_pose == None:  # first loop iteration
                closest_object_points = object_points
                closest_pose = object_pose
                closest_bbox = object_bounding_box
                closest_bbox_dims = obj_bbox_dims
                closest_distance = dist_between_poses(closest_pose, input_pose)
            else:
                if dist_between_poses(object_pose, input_pose) < closest_distance:
                    closest_object_points = object_points
                    closest_distance = dist_between_poses(object_pose, input_pose)
                    closest_pose = object_pose
                    closest_bbox = object_bounding_box
                    closest_bbox_dims = obj_bbox_dims
                    closest_id = current_id
            current_id += 1
        rospy.loginfo("Closest id is: " + str(closest_id) + " at " + str(closest_pose))
        return closest_id, (closest_object_points, closest_bbox_dims, closest_bbox_dims, closest_pose)

    def wait_for_recognized_array(self, wait_time=6, timeout_time=10):
        """Ask for depth images until we get a recognized array
        wait for wait_time between depth throtle calls
        stop if timeout_time is achieved
        If we dont find it in the correspondent time return false, true otherwise"""
        initial_time = rospy.Time.now()
        self.last_clusters = None
        count = 0
        num_calls = 1
        self.depth_service.call(EmptyRequest())
        rospy.loginfo("Depth throtle server call #" + str(num_calls))
        rospy.loginfo("Waiting for a recognized array...")
        while rospy.Time.now() - initial_time < rospy.Duration(timeout_time) and self.last_clusters == None:
            rospy.sleep(0.1)
            count += 1

            if count >= wait_time / 10:
                self.depth_service.call(EmptyRequest())
                num_calls += 1
                rospy.loginfo("Depth throtle server call #" + str(num_calls))
        if self.last_clusters == None:
            return False
        else:
            return True
import copy
import random
from moveit_simple_grasps.msg import GenerateGraspsAction, GenerateGraspsGoal, GenerateGraspsResult

moveit_error_dict = {}
for name in MoveItErrorCodes.__dict__.keys():
    if not name[:1] == '_':
        code = MoveItErrorCodes.__dict__[name]
        moveit_error_dict[code] = name


if __name__=='__main__':
    rospy.init_node("part2_attacher")

    
    scene = PlanningSceneInterface()
    
    rospy.sleep(1)
    
    # publish a demo scene
    p = PoseStamped()
    p.header.frame_id = '/base_link'
    
    p.pose.position.x = 0.6
    p.pose.position.y = 0.0    
    p.pose.position.z = 0.45
    p.pose.orientation.w = 1.0
    #scene.add_box("table", p, (0.5, 1.5, 0.9))
    p.pose.position.x = 0.45
    p.pose.position.y = -0.1
    p.pose.position.z = 1.0
    def __init__(self):
        # 初始化move_group的API
        moveit_commander.roscpp_initialize(sys.argv)
        
        # 初始化ROS节点
        rospy.init_node('moveit_ik_demo')

        # 初始化场景对象
        scene = PlanningSceneInterface()
        rospy.sleep(1)
                
        # 初始化需要使用move group控制的机械臂中的arm group
        arm = moveit_commander.MoveGroupCommander('manipulator')
                        
        # 设置目标位置所使用的参考坐标系
        reference_frame = 'kinect_frame_optical'
        arm.set_pose_reference_frame(reference_frame)
                
        # 当运动规划失败后,允许重新规划
        arm.allow_replanning(True)
        arm.set_planning_time(10)
        
        # 设置位置(单位:米)和姿态(单位:弧度)的允许误差
        arm.set_goal_position_tolerance(0.001)
        arm.set_goal_orientation_tolerance(0.01)
       
        # 设置允许的最大速度和加速度
        arm.set_max_acceleration_scaling_factor(0.5)
        arm.set_max_velocity_scaling_factor(0.5)

        # 获取终端link的名称
        end_effector_link = arm.get_end_effector_link()    

        # 控制机械臂先回到初始化位置
        arm.set_named_target('forward')
        arm.go()
        rospy.sleep(1)

        #===============加入墙面====================
        # 移除场景中之前运行残留的物体
        scene.remove_world_object('wall')

        # 设置wall的三维尺寸
        wall_size = [2, 2, 0.001]

        # 将wall加入场景当中
        wall_pose = PoseStamped()
        wall_pose.header.frame_id = 'wall'
        wall_pose.pose.position.x = 0
        wall_pose.pose.position.y = 0
        wall_pose.pose.position.z = 0
        wall_pose.pose.orientation.w = 1.0
        scene.add_box('wall', wall_pose, wall_size)
        
        rospy.sleep(2)  
        
        #=========移动到起点状态============= 
        # 更新当前的位姿
        arm.set_start_state_to_current_state()
        #从参数服务器读取墙面信息
        wall_tf = rospy.get_param("wall_tf")
        wall_coeff = rospy.get_param("wall_coeff")#墙面方程参数ABCD
      
        # 设置机械臂工作空间中的目标位姿,位置使用x、y、z坐标描述,
        # 姿态使用四元数描述,基于base_link坐标系
        target_pose = PoseStamped()
        target_pose.header.frame_id = reference_frame
        target_pose.header.stamp = rospy.Time.now()     
        target_pose.pose.position.x = -0.35
        target_pose.pose.position.y = 0.42
        #由平面方程求Z并-0.2
        target_pose.pose.position.z = -(wall_coeff[0]*target_pose.pose.position.x+wall_coeff[1]*target_pose.pose.position.y+wall_coeff[3])/wall_coeff[2]-0.2
        target_pose.pose.orientation.x = wall_tf[3]
        target_pose.pose.orientation.y = wall_tf[4] #-0.70729
        target_pose.pose.orientation.z = wall_tf[5]
        target_pose.pose.orientation.w = wall_tf[6] # 0.70729
        
        # 设置机器臂当前的状态作为运动初始状态
        arm.set_start_state_to_current_state()
        
        # 设置机械臂终端运动的目标位姿
        arm.set_pose_target(target_pose, end_effector_link)
        
        # 规划运动路径
        traj = arm.plan()
        
        # 按照规划的运动路径控制机械臂运动
        arm.execute(traj)
        rospy.sleep(1)

        #=================设置路径点====================================
        # 初始化路点列表
        waypoints = []
        wpose = deepcopy(target_pose.pose)

        # 将初始位姿加入路点列表
        waypoints.append(deepcopy(wpose))

        # 设置路点数据,并加入路点列表
        wpose.position.y -= 0.2
        wpose.position.z = -(wall_coeff[0]*wpose.position.x+wall_coeff[1]*wpose.position.y+wall_coeff[3])/wall_coeff[2]-0.2
        waypoints.append(deepcopy(wpose))

        wpose.position.x += 0.1
        wpose.position.z = -(wall_coeff[0]*wpose.position.x+wall_coeff[1]*wpose.position.y+wall_coeff[3])/wall_coeff[2]-0.2
        waypoints.append(deepcopy(wpose))

        wpose.position.y += 0.2
        wpose.position.z = -(wall_coeff[0]*wpose.position.x+wall_coeff[1]*wpose.position.y+wall_coeff[3])/wall_coeff[2]-0.2
        waypoints.append(deepcopy(wpose))

        wpose.position.x += 0.1
        wpose.position.z = -(wall_coeff[0]*wpose.position.x+wall_coeff[1]*wpose.position.y+wall_coeff[3])/wall_coeff[2]-0.2
        waypoints.append(deepcopy(wpose))

        wpose.position.y -= 0.2
        wpose.position.z = -(wall_coeff[0]*wpose.position.x+wall_coeff[1]*wpose.position.y+wall_coeff[3])/wall_coeff[2]-0.2
        waypoints.append(deepcopy(wpose))

        wpose.position.x += 0.1
        wpose.position.z = -(wall_coeff[0]*wpose.position.x+wall_coeff[1]*wpose.position.y+wall_coeff[3])/wall_coeff[2]-0.2
        waypoints.append(deepcopy(wpose))

        wpose.position.y += 0.2
        wpose.position.z = -(wall_coeff[0]*wpose.position.x+wall_coeff[1]*wpose.position.y+wall_coeff[3])/wall_coeff[2]-0.2
        waypoints.append(deepcopy(wpose))

        wpose.position.x += 0.1
        wpose.position.z = -(wall_coeff[0]*wpose.position.x+wall_coeff[1]*wpose.position.y+wall_coeff[3])/wall_coeff[2]-0.2
        waypoints.append(deepcopy(wpose))

        wpose.position.y -= 0.2
        wpose.position.z = -(wall_coeff[0]*wpose.position.x+wall_coeff[1]*wpose.position.y+wall_coeff[3])/wall_coeff[2]-0.2
        waypoints.append(deepcopy(wpose))

        wpose.position.x += 0.1
        wpose.position.z = -(wall_coeff[0]*wpose.position.x+wall_coeff[1]*wpose.position.y+wall_coeff[3])/wall_coeff[2]-0.2
        waypoints.append(deepcopy(wpose))

        wpose.position.y += 0.2
        wpose.position.z = -(wall_coeff[0]*wpose.position.x+wall_coeff[1]*wpose.position.y+wall_coeff[3])/wall_coeff[2]-0.2
        waypoints.append(deepcopy(wpose))

        wpose.position.x += 0.1
        wpose.position.z = -(wall_coeff[0]*wpose.position.x+wall_coeff[1]*wpose.position.y+wall_coeff[3])/wall_coeff[2]-0.2
        waypoints.append(deepcopy(wpose))

        wpose.position.y -= 0.2
        wpose.position.z = -(wall_coeff[0]*wpose.position.x+wall_coeff[1]*wpose.position.y+wall_coeff[3])/wall_coeff[2]-0.2
        waypoints.append(deepcopy(wpose))

        wpose.position.x += 0.1
        wpose.position.z = -(wall_coeff[0]*wpose.position.x+wall_coeff[1]*wpose.position.y+wall_coeff[3])/wall_coeff[2]-0.2
        waypoints.append(deepcopy(wpose))

        wpose.position.y += 0.2
        wpose.position.z = -(wall_coeff[0]*wpose.position.x+wall_coeff[1]*wpose.position.y+wall_coeff[3])/wall_coeff[2]-0.2
        waypoints.append(deepcopy(wpose))
        #=====================================================
        #笛卡尔路径规划
        fraction = 0.0   #路径规划覆盖率
        maxtries = 100   #最大尝试规划次数
        attempts = 0     #已经尝试规划次数
        
        # 设置机器臂当前的状态作为运动初始状态
        arm.set_start_state_to_current_state()

        # 尝试规划一条笛卡尔空间下的路径,依次通过所有路点
        while fraction < 1.0 and attempts < maxtries:
		    (plan, fraction) = arm.compute_cartesian_path (
		                            waypoints,   # waypoint poses,路点列表
		                            0.01,        # eef_step,终端步进值
		                            0.0,         # jump_threshold,跳跃阈值
		                            True)        # avoid_collisions,避障规划
		    
		    # 尝试次数累加
		    attempts += 1
		    
		    # 打印运动规划进程
		    if attempts % 10 == 0:
		        rospy.loginfo("Still trying after " + str(attempts) + " attempts...")
		             
		# 如果路径规划成功(覆盖率100%),则开始控制机械臂运动
        if fraction == 1.0:
		    rospy.loginfo("Path computed successfully. Moving the arm.")
		    arm.execute(plan)
		    rospy.loginfo("Path execution complete.")
		# 如果路径规划失败,则打印失败信息
        else:
		    rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.")  

        rospy.sleep(1)

        #=====================================================

        # 控制机械臂回到初始化位置
        arm.set_named_target('forward')
        arm.go()
        rospy.sleep(1)

        # 移除场景中之前运行残留的物体
        scene.remove_world_object('wall')
        rospy.sleep(1)

        # 关闭并退出moveit
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)
Beispiel #51
0
#!/usr/bin/env python


import sys
import rospy
from moveit_commander import RobotCommander, MoveGroupCommander
from moveit_commander import PlanningSceneInterface, roscpp_initialize, roscpp_shutdown
from geometry_msgs.msg import PoseStamped
from moveit_msgs.msg import Grasp, GripperTranslation, PlaceLocation
from trajectory_msgs.msg import JointTrajectoryPoint


if __name__ == "__main__":
    roscpp_initialize(sys.argv)
    rospy.init_node("moveit_py_demo", anonymous=True)
    scene = PlanningSceneInterface()
    robot = RobotCommander()
    right_arm = MoveGroupCommander("right_arm")
    rospy.sleep(1)

    # clean the scene
    scene.remove_world_object("table")
    scene.remove_world_object("part")
    rospy.logwarn("cleaning world")
    # right_arm.set_named_target("r_start")
    # right_arm.go()

    # right_gripper.set_named_target("open")
    # right_gripper.go()

    rospy.sleep(3)
    def __init__(self):
        # Retrieve params:
        self._table_object_name = rospy.get_param('~table_object_name', 'Grasp_Table')
        self._grasp_object_name = rospy.get_param('~grasp_object_name', 'Grasp_Object')

        self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.01)

        self._arm_group     = rospy.get_param('~arm', 'arm')
        self._gripper_group = rospy.get_param('~gripper', 'gripper')

        self._approach_retreat_desired_dist = rospy.get_param('~approach_retreat_desired_dist', 0.6)
        self._approach_retreat_min_dist = rospy.get_param('~approach_retreat_min_dist', 0.4)

        # Create (debugging) publishers:
        self._grasps_pub = rospy.Publisher('grasps', PoseArray, queue_size=1, latch=True)
        self._places_pub = rospy.Publisher('places', PoseArray, queue_size=1, latch=True)

        # Create planning scene and robot commander:
        self._scene = PlanningSceneInterface()
        self._robot = RobotCommander()

        rospy.sleep(1.0)

        # Clean the scene:
        self._scene.remove_world_object(self._table_object_name)
        self._scene.remove_world_object(self._grasp_object_name)

        # Add table and Coke can objects to the planning scene:
        self._pose_table    = self._add_table(self._table_object_name)
        self._pose_coke_can = self._add_coke_can(self._grasp_object_name)

        rospy.sleep(1.0)

        # Define target place pose:
        self._pose_place = Pose()

        self._pose_place.position.x = self._pose_coke_can.position.x 
        self._pose_place.position.y = self._pose_coke_can.position.y + 0.02
        self._pose_place.position.z = self._pose_coke_can.position.z

        self._pose_place.orientation = Quaternion(*quaternion_from_euler(0.0, 0.0, 0.0))

        # Retrieve groups (arm and gripper):
        self._arm     = self._robot.get_group(self._arm_group)
        self._gripper = self._robot.get_group(self._gripper_group)

        # Create grasp generator 'generate' action client:
        self._grasps_ac = SimpleActionClient('/moveit_simple_grasps_server/generate', GenerateGraspsAction)
        if not self._grasps_ac.wait_for_server(rospy.Duration(5.0)):
            rospy.logerr('Grasp generator action client not available!')
            rospy.signal_shutdown('Grasp generator action client not available!')
            return

        # Create move group 'pickup' action client:
        self._pickup_ac = SimpleActionClient('/pickup', PickupAction)
        if not self._pickup_ac.wait_for_server(rospy.Duration(5.0)):
            rospy.logerr('Pick up action client not available!')
            rospy.signal_shutdown('Pick up action client not available!')
            return

        # Create move group 'place' action client:
        self._place_ac = SimpleActionClient('/place', PlaceAction)
        if not self._place_ac.wait_for_server(rospy.Duration(5.0)):
            rospy.logerr('Place action client not available!')
            rospy.signal_shutdown('Place action client not available!')
            return

        # Pick Coke can object:
        while not self._pickup(self._arm_group, self._grasp_object_name, self._grasp_object_width):
            rospy.logwarn('Pick up failed! Retrying ...')
            rospy.sleep(1.0)

        rospy.loginfo('Pick up successfully')

        # Place Coke can object on another place on the support surface (table):
        while not self._place(self._arm_group, self._grasp_object_name, self._pose_place):
            rospy.logwarn('Place failed! Retrying ...')
            rospy.sleep(1.0)

        rospy.loginfo('Place successfully')
Beispiel #53
0
#!/usr/bin/env python
import rospy, sys, tf
from onine_arm import Arm
from geometry_msgs.msg import *
from tf.transformations import quaternion_from_euler, euler_from_quaternion
from moveit_commander import RobotCommander, PlanningSceneInterface
import moveit_commander

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

scene = PlanningSceneInterface()
rospy.sleep(2)

scene.remove_world_object("target")

onine_arm = Arm()
onine_arm.tilt_food()

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

Beispiel #54
0
    def __init__(self):
        moveit_commander.roscpp_initialize(sys.argv)
        rospy.init_node('moveit_demo')
        self.scene = PlanningSceneInterface()
        pub_traj = rospy.Publisher('/joint_path_command',
                                   trajectory_msgs.msg.JointTrajectory,
                                   queue_size=10)
        #pub_ratio_sig= rospy.Publisher('/enable_source_change',Int64,queue_size=1)
        self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
        self.gripperCtrl = rospy.ServiceProxy(
            "/two_finger/gripper/gotoPositionUntilTouch", SetPosition)
        self.colors = dict()
        rospy.sleep(1)
        self.robot = moveit_commander.robot.RobotCommander()
        self.arm = MoveGroupCommander('arm')
        self.arm.allow_replanning(True)
        cartesian = rospy.get_param('~cartesian', True)
        self.arm_end_effector_link = self.arm.get_end_effector_link()
        self.arm.set_goal_position_tolerance(0.005)
        self.arm.set_goal_orientation_tolerance(0.025)
        self.arm.allow_replanning(True)
        self.reference_frame = 'base_link'
        self.arm.set_pose_reference_frame(self.reference_frame)
        self.arm.set_planning_time(5)
        #shaking
        self.joint_names = [[]]
        self.joint_names[0] = rospy.get_param('controller_joint_names')
        self.traj = trajectory_msgs.msg.JointTrajectory()
        self.sub_jpc = rospy.Publisher('/joint_path_command',
                                       trajectory_msgs.msg.JointTrajectory,
                                       queue_size=10)
        #scene planning
        self.l_id = 'l_tool'
        self.r_id = 'r_tool'
        self.table_id = 'table'
        self.target1_id = 'target1_object'
        self.target2_id = 'target2_object'
        #~ self.target3_id='target3_object'
        #~ self.target4_id='target4_object'
        self.f_target_id = 'receive_container'
        self.scene.remove_world_object(self.l_id)
        self.scene.remove_world_object(self.r_id)
        self.scene.remove_world_object(self.table_id)
        self.scene.remove_world_object(self.target1_id)
        self.scene.remove_world_object(self.target2_id)
        #~ self.scene.remove_world_object(self.target3_id)
        #~ self.scene.remove_world_object(self.target4_id)
        self.scene.remove_world_object(self.f_target_id)
        #self.scene.remove_attached_object(self.arm_end_effector_link,self.target_id)

        self.table_ground = 0.13
        self.table_size = [0.9, 0.6, 0.018]
        self.setupSence()
        target1_size = [0.035, 0.035, 0.19]
        target2_size = target1_size
        #~ target3_size=target1_size
        #~ target4_size=target1_size
        self.f_target_size = [0.2, 0.2, 0.04]

        f_target_pose = PoseStamped()
        pre_pour_pose = PoseStamped()
        target1_pose = PoseStamped()
        target2_pose = PoseStamped()
        #~ target3_pose=PoseStamped()
        #~ target4_pose=PoseStamped()

        joint_names = [
            'joint_' + jkey for jkey in ('s', 'l', 'e', 'u', 'r', 'b', 't')
        ]
        joint_names = rospy.get_param('controller_joint_names')
        traj = trajectory_msgs.msg.JointTrajectory()
        traj.joint_names = joint_names

        #final target
        l_gripper = 0.18
        #self.add_target(f_target_pose,self.f_target_size,self.reference_frame,-0.184+0.27,0.62+0.1,0,0,0,1)
        #~ self.add_target(f_target_pose,self.f_target_size,self.reference_frame,0.24,0.6-l_gripper,0,0,0,1)
        #~ self.scene.add_box(self.f_target_id,f_target_pose,self.f_target_size)
        #self.add_target(pre_pour_pose,self.reference_frame,x,y,0,0,0,1)

        #target localization
        msg1 = rospy.wait_for_message('/aruco_simple/pose', Pose)
        rospy.sleep(1)
        msg2 = rospy.wait_for_message('/aruco_simple/pose2', Pose)
        rospy.sleep(1)
        self.add_target(f_target_pose, self.f_target_size,
                        self.reference_frame, -msg2.position.y - 0.257,
                        -msg2.position.x + 0.81 - 0.1 - l_gripper, 0, 0, 0, 1)
        self.scene.add_box(self.f_target_id, f_target_pose, self.f_target_size)
        self.add_target(target1_pose, target1_size, self.reference_frame,
                        -msg1.position.y - 0.257,
                        -msg1.position.x + 0.81 - 0.065, 0, 0, 0, 1)
        self.scene.add_box(self.target1_id, target1_pose, target1_size)
        #~ self.add_target(target2_pose,target2_size,self.reference_frame,-msg2.position.y-0.257,-msg2.position.x+0.81-0.065,0,0,0,1)
        #~ self.scene.add_box(self.target2_id,target2_pose,target2_size)
        #~ self.add_target(target3_pose,target3_size,self.reference_frame,-0.01,0.76-0.01,0,0,0,1)
        #~ self.scene.add_box(self.target3_id,target3_pose,target3_size)
        #~ self.add_target(target4_pose,target4_size,self.reference_frame,0.25,0.80,0,0,0,1)
        #~ self.scene.add_box(self.target4_id,target4_pose,target4_size)

        #attach_pose
        g_p = PoseStamped()
        g_p.header.frame_id = self.arm_end_effector_link
        g_p.pose.position.x = 0.00
        g_p.pose.position.y = -0.00
        g_p.pose.position.z = 0.025
        g_p.pose.orientation.w = 0.707
        g_p.pose.orientation.x = 0
        g_p.pose.orientation.y = -0.707
        g_p.pose.orientation.z = 0

        #set color
        self.setColor(self.target1_id, 0.8, 0, 0, 1.0)
        self.setColor(self.target2_id, 0.8, 0, 0, 1.0)
        #~ self.setColor(self.target3_id,0.8,0,0,1.0)
        #self.setColor(self.target4_id,0.8,0,0,1.0)
        self.setColor(self.f_target_id, 0.8, 0.3, 0, 1.0)
        self.setColor('r_tool', 0.8, 0, 0)
        self.setColor('l_tool', 0.8, 0, 0)
        self.sendColors()
        self.gripperCtrl(255)
        rospy.sleep(3)
        self.arm.set_named_target("initial_arm")
        self.arm.go()
        rospy.sleep(3)

        #j_ori_state=[-1.899937629699707, -0.5684762597084045, 0.46537330746650696, 2.3229329586029053, -0.057941947132349014, -1.2867668867111206, 0.2628822326660156]
        #j_ori_state=[-2.161055326461792, -0.6802523136138916, -1.7733728885650635, -2.3315746784210205, -0.5292841196060181, 1.4411976337432861, -2.2327845096588135]
        j_ori_state = [
            -1.2628753185272217, -0.442996621131897, -0.1326361745595932,
            2.333048105239868, -0.15598002076148987, -1.2167049646377563,
            3.1414425373077393
        ]

        #signal= True
        self.arm.set_joint_value_target(j_ori_state)
        self.arm.go()
        rospy.sleep(3)
        #parameter setup
        tar_num = 1
        maxtries = 300
        r_grasp = 0.16
        #~ topic_name=["/ratio_carrot","/ratio_lettuce","ratio_pepper"]
        #~ save_data=["carrot.txt","pepper.txt","cucumber.txt"]
        #ratio_table=[0.03,0.025,0.04]
        for i in range(0, tar_num):
            #grasp target
            rospy.loginfo("Choosing source...")
            #~ dfile=open(save_data[i],"a")
            if i == 0:
                target_pose = target1_pose
                target_id = self.target1_id
                target_size = target1_size
                #~ amp=0.13
                #~ freq=2.75
                #~ r_angle=40.0
            elif i == 1:
                target_pose = target2_pose
                target_id = self.target2_id
                target_size = target2_size
                #~ amp=0.15
                #~ freq=2.5
                #~ r_angle=45.0
            elif i == 2:
                target_pose = target3_pose
                target_id = self.target3_id
                target_size = target3_size
                amp = 0.15
                freq = 2.75
                r_angle = 40.0
            elif i == 3:
                target_pose = target4_pose
                target_id = self.target4_id
                target_size = target4_size
                amp = 0.1
                freq = 2.75
                r_angle = 45.0

            r_pour = 0.13
            r_bottle = 0.1
            h_pour = 0.1

            pour_angle = [0.0, -15.0, 15.0, -30.0, 30.0, -45.0, 45.0]
            #~ tip_angle=[10.0,20.0,30.0,40.0,50.0,60.0,70.0,80.0,90.0]
            #~ tip_angle=[15.0,30.0,45.0,60.0,75.0,90.0]
            tip_angle = [20.0, 30.0, 40.0, 50.0, 60.0, 70.00]
            shake_freq = [2.0, 2.25, 2.50, 2.75]
            shake_amp = [0.05, 0.075, 0.1, 0.125, 0.15]
            shake_per_times = 5
            shake_times_tgt = 2
            rospy.loginfo("Params loaded.")
            rospy.loginfo("Current Source: " + str(i + 1) + "  ")
            #grasp and back
            ori_pose, g_pose = self.pg_g_pp(target_pose, r_grasp)
            ori_joint_state = self.arm.get_current_joint_values()
            #move to target position for pouring
            for m in range(len(pour_angle)):

                for j in range(len(tip_angle)):
                    for k in range(len(shake_freq)):
                        dfile = open(
                            "pepper_tipangle_" + str(tip_angle[j]) +
                            "_shaking_freq_" + str(shake_freq[k]) + "_1", "a")
                        for n in range(len(shake_amp)):
                            pp_ps_sgn = self.pp_ps(f_target_pose,
                                                   pour_angle[m], r_pour,
                                                   h_pour)
                            if pp_ps_sgn == 0:
                                rospy.loginfo(
                                    "pre-Pouring angle not correct,  back for replanning..."
                                )
                                self.rotate_back(ori_joint_state)
                                rospy.sleep(2)
                                continue
                            initial_pose = self.arm.get_current_pose(
                                self.arm_end_effector_link)
                            pour_signal = self.pour_rotate(
                                initial_pose, pour_angle[m], tip_angle[j],
                                r_bottle, maxtries)
                            if pour_signal != 1:
                                rospy.loginfo(
                                    "Pouring angle not correct,  back for replanning..."
                                )
                                self.rotate_back(ori_joint_state)
                                rospy.sleep(2)
                                continue
                            amp = shake_amp[n]
                            freq = shake_freq[k]
                            rospy.loginfo("shaking degree : " +
                                          str(tip_angle[j]) +
                                          ", shaking amp :  " + str(amp) +
                                          ", shaking freq : " + str(freq))
                            shake_times = 0
                            signal = True
                            start_joint_state = self.arm.get_current_joint_values(
                            )
                            signal, end_joint_state = self.shake_a(
                                pour_angle[m], tip_angle[j], amp)
                            while signal == 1:
                                #~ area_ratio= rospy.wait_for_message('/ratio_carrot',Float64)
                                #if (area_ratio>ratio_table[i]) or (shake_times>shake_times_tgt):
                                if (shake_times < shake_times_tgt):

                                    self.shaking(start_joint_state,
                                                 end_joint_state, freq,
                                                 shake_per_times)
                                    rospy.sleep(2)
                                    shake_times += 1
                                    rospy.loginfo("shaking times :  " +
                                                  str(shake_times) + " .")
                                    area_ratio = rospy.wait_for_message(
                                        '/ratio_carrot', Float64)
                                    if shake_times == shake_times_tgt:
                                        dfile.write(
                                            "%f %f \r\n" %
                                            (shake_amp[n], area_ratio.data))

                                        self.rotate_back(ori_joint_state)
                                        rospy.sleep(2)
                                else:
                                    signal = 0

                    self.rotate_back(ori_joint_state)
                    rospy.sleep(2)
                self.rotate_back(ori_joint_state)
                rospy.sleep(2)

        #remove and shut down
        self.scene.remove_attached_object(self.arm_end_effector_link, 'l_tool')
        self.scene.remove_attached_object(self.arm_end_effector_link, 'r_tool')
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)
    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)
        
        rospy.init_node('moveit_demo')
        
        # Use the planning scene object to add or remove objects
        scene = PlanningSceneInterface()
        
        # Create a scene publisher to push changes to the scene
        self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
        
        # Create a publisher for displaying gripper poses
        self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped)
        
        # Create a dictionary to hold object colors
        self.colors = dict()
                        
        # Initialize the move group for the right arm
        right_arm = MoveGroupCommander(GROUP_NAME_ARM)
        
        # Initialize the move group for the right gripper
        right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
        
        # Get the name of the end-effector link
        end_effector_link = right_arm.get_end_effector_link()
 
        # Allow some leeway in position (meters) and orientation (radians)
        right_arm.set_goal_position_tolerance(0.05)
        right_arm.set_goal_orientation_tolerance(0.2) #0.1

        # 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(REFERENCE_FRAME)
        
        # Allow 5 seconds per planning attempt
        right_arm.set_planning_time(5)
        
        # Set a limit on the number of pick attempts before bailing
        max_pick_attempts = 5
        
        # Set a limit on the number of place attempts
        max_place_attempts = 5
                
        # Give the scene a chance to catch up
        rospy.sleep(2)

        # Give each of the scene objects a unique name        
        table_id = 'table'
        box1_id = 'box1'
        box2_id = 'box2'
        target_id = 'pick_target'
        tool_id = 'tool'
                
        # Remove leftover objects from a previous run
        scene.remove_world_object(table_id)
        scene.remove_world_object(box1_id)
        scene.remove_world_object(box2_id)
        scene.remove_world_object(target_id)
        scene.remove_world_object(tool_id)
        
        # Remove any attached objects from a previous session
        scene.remove_attached_object(GRIPPER_FRAME, target_id)
        
        # Give the scene a chance to catch up    
        rospy.sleep(1)
        
        # Start the arm in the "resting" pose stored in the SRDF file
        right_arm.set_named_target('right_arm_home')
        right_arm.go()
        
        # Open the gripper to the neutral position
        try:
            right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
        except:
            rospy.loginfo("=============> DEBUG EXCEPT: Ignoring set_joint_value_target error")
        try:
            right_gripper.go()
        except:
            rospy.loginfo("=============> DEBUG EXCEPT: Ignoring right_gripper.go error")
       
        rospy.sleep(1)

        # Set the height of the table off the ground
        table_ground = 0.45 # 0.75 # DAVES

        # DAVES ADDED        
        table_offset_x = 0.30 # meters (centemeters / 100)
        table_offset_y = -0.30 # meters (positive = left)

        
        # Set the dimensions of the scene objects [l, w, h]
        table_size = [0.2, 0.7, 0.01]
        box1_size = [0.1, 0.05, 0.05]
        box2_size = [0.05, 0.05, 0.15]
        
        # Set the target size [l, w, h]
        target_size = [0.02, 0.01, 0.12]
        
        # Add a table top and two boxes to the scene
        table_pose = PoseStamped()
        table_pose.header.frame_id = REFERENCE_FRAME
        table_pose.pose.position.x = 0.25 + table_offset_x
        table_pose.pose.position.y = 0.0 + table_offset_y
        table_pose.pose.position.z = table_ground + table_size[2] / 2.0
        table_pose.pose.orientation.w = 1.0
        scene.add_box(table_id, table_pose, table_size)
        
        box1_pose = PoseStamped()
        box1_pose.header.frame_id = REFERENCE_FRAME
        box1_pose.pose.position.x = 0.21 + table_offset_x
        box1_pose.pose.position.y = -0.1 + table_offset_y
        box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0
        box1_pose.pose.orientation.w = 1.0   
        scene.add_box(box1_id, box1_pose, box1_size)
        
        box2_pose = PoseStamped()
        box2_pose.header.frame_id = REFERENCE_FRAME
        box2_pose.pose.position.x = 0.19 + table_offset_x
        box2_pose.pose.position.y = 0.13 + table_offset_y
        box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0
        box2_pose.pose.orientation.w = 1.0   
        scene.add_box(box2_id, box2_pose, box2_size)       
        
        # Set the target pose in between the boxes and on the table
        target_pose = PoseStamped()
        target_pose.header.frame_id = REFERENCE_FRAME
        target_pose.pose.position.x = 0.22 + table_offset_x
        target_pose.pose.position.y = 0.0 + table_offset_y
        target_pose.pose.position.z = table_ground + table_size[2] + target_size[2] / 2.0
        target_pose.pose.orientation.w = 1.0
        
        # Add the target object to the scene
        scene.add_box(target_id, target_pose, target_size)
        
        # Make the table red and the boxes orange
        self.setColor(table_id, 0.8, 0, 0, 1.0)
        self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
        self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
        
        # Make the target yellow
        self.setColor(target_id, 0.9, 0.9, 0, 1.0)


        
        # Send the colors to the planning scene
        self.sendColors()
        
        # Set the support surface name to the table object
        right_arm.set_support_surface_name(table_id)

        rospy.loginfo("=============> DEBUG Target and table setup done")
        rospy.sleep(3)



        
        # Specify a pose to place the target after being picked up
        place_pose = PoseStamped()
        place_pose.header.frame_id = REFERENCE_FRAME
        place_pose.pose.position.x = 0.18 + table_offset_x
        place_pose.pose.position.y = -0.18 + table_offset_y
        place_pose.pose.position.z = table_ground + table_size[2] + target_size[2] / 2.0
        place_pose.pose.orientation.w = 1.0

        # Initialize the grasp pose to the target pose
        grasp_pose = target_pose
        
        # Shift the grasp pose by half the width of the target to center it
        grasp_pose.pose.position.y -= target_size[1] / 2.0
                
        # Generate a list of grasps
        grasps = self.make_grasps(grasp_pose, [target_id])

        # Publish the grasp poses so they can be viewed in RViz
        for grasp in grasps:
            self.gripper_pose_pub.publish(grasp.grasp_pose)
            rospy.sleep(0.2)
    
        # Track success/failure and number of attempts for pick operation
        result = None
        n_attempts = 0
        
        # Repeat until we succeed or run out of attempts
        while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
            n_attempts += 1
            rospy.loginfo("Pick attempt: " +  str(n_attempts))
            result = right_arm.pick(target_id, grasps)
            rospy.sleep(0.2)
        
        # If the pick was successful, attempt the place operation   
        if result == MoveItErrorCodes.SUCCESS:
            result = None
            n_attempts = 0
            
            # Generate valid place poses
            places = self.make_places(place_pose)
            
            # Repeat until we succeed or run out of attempts
            while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts:
                n_attempts += 1
                rospy.loginfo("Place attempt: " +  str(n_attempts))
                for place in places:
                    result = right_arm.place(target_id, place)
                    if result == MoveItErrorCodes.SUCCESS:
                        break
                rospy.sleep(0.2)
                
            if result != MoveItErrorCodes.SUCCESS:
                rospy.loginfo("Place operation failed after " + str(n_attempts) + " attempts.")
        else:
            rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.")
                
        # Return the arm to the "resting" pose stored in the SRDF file
        right_arm.set_named_target('right_arm_home')
        right_arm.go()
        
        # Open the gripper to the neutral position
        # Open the gripper to the neutral position
        try:
            right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
        except:
            rospy.loginfo("=============> DEBUG EXCEPT: Ignoring set_joint_value_target error")
        try:
            right_gripper.go()
        except:
            rospy.loginfo("=============> DEBUG EXCEPT: Ignoring right_gripper.go error")
       
       
        rospy.sleep(1)

        # Shut down MoveIt cleanly
        moveit_commander.roscpp_shutdown()
        
        # Exit the script
        moveit_commander.os._exit(0)
Beispiel #56
0
	def __init__(self):
		moveit_commander.roscpp_initialize(sys.argv)
		rospy.init_node('moveit_demo')
		self.scene=PlanningSceneInterface()
		pub_traj= rospy.Publisher('/joint_path_command', trajectory_msgs.msg.JointTrajectory, queue_size=10)
		#pub_ratio_sig= rospy.Publisher('/enable_source_change',Int64,queue_size=1)
		self.scene_pub=rospy.Publisher('planning_scene',PlanningScene)
		self.gripperCtrl=rospy.ServiceProxy("/two_finger/gripper/gotoPositionUntilTouch",SetPosition)
		self.colors=dict()
		rospy.sleep(1)
		self.robot=moveit_commander.robot.RobotCommander()
		self.arm=MoveGroupCommander('arm')
		self.arm.allow_replanning(True)
		cartesian=rospy.get_param('~cartesian',True)
		self.arm_end_effector_link=self.arm.get_end_effector_link()
		self.arm.set_goal_position_tolerance(0.005)
		self.arm.set_goal_orientation_tolerance(0.025)
		self.arm.allow_replanning(True)
		self.reference_frame='base_link'
		self.arm.set_pose_reference_frame(self.reference_frame)
		self.arm.set_planning_time(5)
		#shaking
		self.joint_names= [[]]
		self.joint_names[0]= rospy.get_param('controller_joint_names')
		self.traj= trajectory_msgs.msg.JointTrajectory()
		self.sub_jpc= rospy.Publisher('/joint_path_command', trajectory_msgs.msg.JointTrajectory,queue_size=10)	
		#scene planning	
		self.l_id='l_tool'
		self.r_id='r_tool'
		self.table_id='table'
		self.target1_id='target1_object'
		self.target2_id='target2_object'
		self.target3_id='target3_object'
		#self.target4_id='target4_object'
		self.f_target_id='receive_container'
		self.scene.remove_world_object(self.l_id)
		self.scene.remove_world_object(self.r_id)
		self.scene.remove_world_object(self.table_id)
		self.scene.remove_world_object(self.target1_id)
		self.scene.remove_world_object(self.target2_id)
		self.scene.remove_world_object(self.target3_id)
		#self.scene.remove_world_object(self.target4_id)
		self.scene.remove_world_object(self.f_target_id)
		#self.scene.remove_attached_object(self.arm_end_effector_link,self.target_id)

		self.table_ground=0.13
		self.table_size=[0.9,0.6,0.018]
		self.setupSence()
		target1_size=[0.035,0.035,0.19]
		target2_size=target1_size
		target3_size=target1_size
		#target4_size=target1_size
		self.f_target_size=[0.2,0.2,0.04]
		
		f_target_pose=PoseStamped()
		pre_pour_pose=PoseStamped()
		target1_pose=PoseStamped()
		target2_pose=PoseStamped()
		target3_pose=PoseStamped()
		#target4_pose=PoseStamped()
		
		joint_names= ['joint_'+jkey for jkey in ('s','l','e','u','r','b','t')]
   		joint_names= rospy.get_param('controller_joint_names')
		traj= trajectory_msgs.msg.JointTrajectory()
		traj.joint_names= joint_names
		
		#final target
		l_gripper=0.18
		#self.add_target(f_target_pose,self.f_target_size,self.reference_frame,-0.184+0.27,0.62+0.1,0,0,0,1)
		self.add_target(f_target_pose,self.f_target_size,self.reference_frame,0.24,0.6-l_gripper,0,0,0,1)
		self.scene.add_box(self.f_target_id,f_target_pose,self.f_target_size)
		#self.add_target(pre_pour_pose,self.reference_frame,x,y,0,0,0,1)
		
		#target localization
		msg1 = rospy.wait_for_message('/aruco_simple/pose',Pose)	
		rospy.sleep(2)
		msg2 = rospy.wait_for_message('/aruco_simple/pose2',Pose)	
		
		self.add_target(target1_pose,target1_size,self.reference_frame,-msg1.position.y-0.257,-msg1.position.x+0.81-0.065,0,0,0,1)
		self.scene.add_box(self.target1_id,target1_pose,target1_size)
		self.add_target(target2_pose,target2_size,self.reference_frame,-msg2.position.y-0.257,-msg2.position.x+0.81-0.065,0,0,0,1)
		self.scene.add_box(self.target2_id,target2_pose,target2_size)
		self.add_target(target3_pose,target3_size,self.reference_frame,-0.01,0.76-0.01,0,0,0,1)
		self.scene.add_box(self.target3_id,target3_pose,target3_size)
		#self.add_target(target4_pose,target4_size,self.reference_frame,0.25,0.80,0,0,0,1)
		#self.scene.add_box(self.target4_id,target4_pose,target4_size)
		
		#attach_pose
		g_p=PoseStamped()
		g_p.header.frame_id=self.arm_end_effector_link
		g_p.pose.position.x=0.00
		g_p.pose.position.y=-0.00
		g_p.pose.position.z=0.025
		g_p.pose.orientation.w=0.707
		g_p.pose.orientation.x=0
		g_p.pose.orientation.y=-0.707
		g_p.pose.orientation.z=0
		
		#set color
		self.setColor(self.target1_id,0.8,0,0,1.0)
		self.setColor(self.target2_id,0.8,0,0,1.0)
		self.setColor(self.target3_id,0.8,0,0,1.0)
		#self.setColor(self.target4_id,0.8,0,0,1.0)
		self.setColor(self.f_target_id,0.8,0.3,0,1.0)
		self.setColor('r_tool',0.8,0,0)
		self.setColor('l_tool',0.8,0,0)
		self.sendColors()
		self.gripperCtrl(255)
		rospy.sleep(3)
		self.arm.set_named_target("initial_arm")
		self.arm.go()
		rospy.sleep(3)
		
		#j_ori_state=[-1.899937629699707, -0.5684762597084045, 0.46537330746650696, 2.3229329586029053, -0.057941947132349014, -1.2867668867111206, 0.2628822326660156]
		#j_ori_state=[-2.161055326461792, -0.6802523136138916, -1.7733728885650635, -2.3315746784210205, -0.5292841196060181, 1.4411976337432861, -2.2327845096588135]
		j_ori_state=[-1.2628753185272217, -0.442996621131897, -0.1326361745595932, 2.333048105239868, -0.15598002076148987, -1.2167049646377563, 3.1414425373077393]
		
		#signal= True
		self.arm.set_joint_value_target(j_ori_state)
		self.arm.go()
		rospy.sleep(3)		
		#parameter setup
		tar_num=3
		maxtries=300
		r_grasp=0.15
		topic_name=["/ratio_carrot","/ratio_lettuce","ratio_pepper"]
		save_data=["ratio_carrot.txt","ratio_lettuce.txt","ratio_pepper.txt"]
		ratio_table=[0.03,0.025,0.04]
		for i in range(0,tar_num):
			#grasp target
			rospy.loginfo("Choosing source...")	
			dfile=open(save_data[i],"a")
			if i==0:
				target_pose=target1_pose
				target_id=self.target1_id
				target_size=target1_size
				amp=0.13
				freq=2.75
				r_angle=40.0
			elif i==1:
				target_pose=target2_pose
				target_id=self.target2_id
				target_size=target2_size
				amp=0.15
				freq=2.5
				r_angle=45.0
			elif i==2:
				target_pose=target3_pose
				target_id=self.target3_id
				target_size=target3_size
				amp=0.15
				freq=2.75
				r_angle=40.0
			elif i==3:
				target_pose=target4_pose
				target_id=self.target4_id
				target_size=target4_size
				amp=0.1
				freq=2.75
				r_angle=45.0
			r_pour=0.1
			r_bottle=0.1
			h_pour=0.1
			
			#pour_angle=[15.0,30.0,45.0,60.0,75.0]
			pour_angle=[-15.0,-10.0,-5.0,0.0,10.0,15.0,30.0,45.0,60.0,75.0]
			rospy.loginfo("Params loaded.")	
			rospy.loginfo("Current Source: "+str(i+1)+"  ")	
			#grasp and back
			ori_pose,g_pose=self.pg_g_pp(target_pose,r_grasp)
			#move to target position for pouring
			pre_p_angle=self.pp_ps(f_target_pose,pour_angle,r_pour,h_pour)
			rospy.loginfo("pre_pour_angle : "+str(pre_p_angle))
			#rotation and shaking
			ps_signal=self.pour_rotate(pre_p_angle,r_angle,r_bottle,maxtries)
			if ps_signal !=1:
				 rospy.loginfo("Pre_shaking_rotation failed,  back to ori_pose...")
				 self.go_back(ori_pose,g_pose)
				 rospy.sleep(5)
				 continue
			else:
				rospy.loginfo("Shaking planning...")
				shake_per_times=3
				shake_times=0
				shake_times_tgt=5
				signal=True
				rospy.loginfo("Parameter loaded")	
				rospy.sleep(1)
				start_shake_pose=self.arm.get_current_pose(self.arm_end_effector_link)# for trajectory of shaking
				start_joint_state=self.arm.get_current_joint_values() # for state[6] to rotate
				shift_pose=deepcopy(start_shake_pose)
				r_angle=(r_angle/180.0)*3.14
				pre_p_angle=(pre_p_angle/180.0)*3.14
				shift_pose.pose.position.x+=amp*math.sin(r_angle)*math.cos(pre_p_angle)# in verticle direction
				shift_pose.pose.position.y-=amp*math.sin(r_angle)*math.sin(pre_p_angle)
				shift_pose.pose.position.z+=amp*math.cos(r_angle)#...
				if cartesian: 
					signal,end_joint_state=self.cts(start_shake_pose,shift_pose,300)
				#shaking process
				while signal==1:
					area_ratio= rospy.wait_for_message(topic_name[i],Float64)
					dfile.write("%f %f\r\n" % (shake_times,area_ratio.data))
					if (area_ratio.data<ratio_table[i]) and (shake_times<shake_times_tgt):
					#if (shake_times<shake_times_tgt):
						start_joint_state_1=self.arm.get_current_joint_values()
						self.shaking(start_joint_state_1,end_joint_state,freq,shake_per_times)
						shake_times+=1
						
						rospy.loginfo("shaking times :  "+str(shake_times)+ " .")
					else:
						signal=0
						
			self.rotate_back(r_angle)	
			self.go_back(ori_pose,g_pose)
			rospy.sleep(2)
			continue
			
			
		#remove and shut down
		self.scene.remove_attached_object(self.arm_end_effector_link,'l_tool')
		self.scene.remove_attached_object(self.arm_end_effector_link,'r_tool')
		moveit_commander.roscpp_shutdown()
		moveit_commander.os._exit(0)
class CokeCanPickAndPlace:
    def __init__(self):
        # Retrieve params:
        self._table_object_name = rospy.get_param('~table_object_name', 'Grasp_Table')
        self._grasp_object_name = rospy.get_param('~grasp_object_name', 'Grasp_Object')

        self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.01)

        self._arm_group     = rospy.get_param('~arm', 'arm')
        self._gripper_group = rospy.get_param('~gripper', 'gripper')

        self._approach_retreat_desired_dist = rospy.get_param('~approach_retreat_desired_dist', 0.6)
        self._approach_retreat_min_dist = rospy.get_param('~approach_retreat_min_dist', 0.4)

        # Create (debugging) publishers:
        self._grasps_pub = rospy.Publisher('grasps', PoseArray, queue_size=1, latch=True)
        self._places_pub = rospy.Publisher('places', PoseArray, queue_size=1, latch=True)

        # Create planning scene and robot commander:
        self._scene = PlanningSceneInterface()
        self._robot = RobotCommander()

        rospy.sleep(1.0)

        # Clean the scene:
        self._scene.remove_world_object(self._table_object_name)
        self._scene.remove_world_object(self._grasp_object_name)

        # Add table and Coke can objects to the planning scene:
        self._pose_table    = self._add_table(self._table_object_name)
        self._pose_coke_can = self._add_coke_can(self._grasp_object_name)

        rospy.sleep(1.0)

        # Define target place pose:
        self._pose_place = Pose()

        self._pose_place.position.x = self._pose_coke_can.position.x 
        self._pose_place.position.y = self._pose_coke_can.position.y + 0.02
        self._pose_place.position.z = self._pose_coke_can.position.z

        self._pose_place.orientation = Quaternion(*quaternion_from_euler(0.0, 0.0, 0.0))

        # Retrieve groups (arm and gripper):
        self._arm     = self._robot.get_group(self._arm_group)
        self._gripper = self._robot.get_group(self._gripper_group)

        # Create grasp generator 'generate' action client:
        self._grasps_ac = SimpleActionClient('/moveit_simple_grasps_server/generate', GenerateGraspsAction)
        if not self._grasps_ac.wait_for_server(rospy.Duration(5.0)):
            rospy.logerr('Grasp generator action client not available!')
            rospy.signal_shutdown('Grasp generator action client not available!')
            return

        # Create move group 'pickup' action client:
        self._pickup_ac = SimpleActionClient('/pickup', PickupAction)
        if not self._pickup_ac.wait_for_server(rospy.Duration(5.0)):
            rospy.logerr('Pick up action client not available!')
            rospy.signal_shutdown('Pick up action client not available!')
            return

        # Create move group 'place' action client:
        self._place_ac = SimpleActionClient('/place', PlaceAction)
        if not self._place_ac.wait_for_server(rospy.Duration(5.0)):
            rospy.logerr('Place action client not available!')
            rospy.signal_shutdown('Place action client not available!')
            return

        # Pick Coke can object:
        while not self._pickup(self._arm_group, self._grasp_object_name, self._grasp_object_width):
            rospy.logwarn('Pick up failed! Retrying ...')
            rospy.sleep(1.0)

        rospy.loginfo('Pick up successfully')

        # Place Coke can object on another place on the support surface (table):
        while not self._place(self._arm_group, self._grasp_object_name, self._pose_place):
            rospy.logwarn('Place failed! Retrying ...')
            rospy.sleep(1.0)

        rospy.loginfo('Place successfully')

    def __del__(self):
        # Clean the scene:
        self._scene.remove_world_object(self._grasp_object_name)
        self._scene.remove_world_object(self._table_object_name)

    def _add_table(self, name):
        p = PoseStamped()
        p.header.frame_id = self._robot.get_planning_frame()
        p.header.stamp = rospy.Time.now()

        p.pose.position.x = 0.5
        p.pose.position.y = 0.0
        p.pose.position.z = 0.22

        q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0))
        p.pose.orientation = Quaternion(*q)

        # Table size from ~/.gazebo/models/table/model.sdf, using the values
        # for the surface link.
        self._scene.add_box(name, p, (0.5, 0.4, 0.02))

        return p.pose

    def _add_coke_can(self, name):
        p = PoseStamped()
        p.header.frame_id = self._robot.get_planning_frame()
        p.header.stamp = rospy.Time.now()

        p.pose.position.x = 0.25   
        p.pose.position.y = 0
        p.pose.position.z = 0.32

        q = quaternion_from_euler(0.0, 0.0, 0.0)
        p.pose.orientation = Quaternion(*q)

        # Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae,
        # using the measure tape tool from meshlab.
        # The box is the bounding box of the coke cylinder.
        # The values are taken from the cylinder base diameter and height.
        self._scene.add_box(name, p, (0.01, 0.01, 0.009))

        return p.pose

    def _generate_grasps(self, pose, width):
        """
        Generate grasps by using the grasp generator generate action; based on
        server_test.py example on moveit_simple_grasps pkg.
        """

        # Create goal:
        goal = GenerateGraspsGoal()

        goal.pose  = pose
        goal.width = width

        options = GraspGeneratorOptions()
        # simple_graps.cpp doesn't implement GRASP_AXIS_Z!
        #options.grasp_axis      = GraspGeneratorOptions.GRASP_AXIS_Z
        options.grasp_direction = GraspGeneratorOptions.GRASP_DIRECTION_UP
        options.grasp_rotation  = GraspGeneratorOptions.GRASP_ROTATION_FULL

        # @todo disabled because it works better with the default options
        #goal.options.append(options)

        # Send goal and wait for result:
        state = self._grasps_ac.send_goal_and_wait(goal)
        if state != GoalStatus.SUCCEEDED:
            rospy.logerr('Grasp goal failed!: %s' % self._grasps_ac.get_goal_status_text())
            return None

        grasps = self._grasps_ac.get_result().grasps

        # Publish grasps (for debugging/visualization purposes):
        self._publish_grasps(grasps)

        return grasps

    def _generate_places(self, target):
        """
        Generate places (place locations), based on
        https://github.com/davetcoleman/baxter_cpp/blob/hydro-devel/
        baxter_pick_place/src/block_pick_place.cpp
        """

        # Generate places:
        places = []
        now = rospy.Time.now()
        for angle in numpy.arange(0.0, numpy.deg2rad(360.0), numpy.deg2rad(1.0)):
            # Create place location:
            place = PlaceLocation()

            place.place_pose.header.stamp = now
            place.place_pose.header.frame_id = self._robot.get_planning_frame()

            # Set target position:
            place.place_pose.pose = copy.deepcopy(target)

            # Generate orientation (wrt Z axis):
            q = quaternion_from_euler(0.0, 0.0, angle)
            place.place_pose.pose.orientation = Quaternion(*q)

            # Generate pre place approach:
            place.pre_place_approach.desired_distance = self._approach_retreat_desired_dist
            place.pre_place_approach.min_distance = self._approach_retreat_min_dist

            place.pre_place_approach.direction.header.stamp = now
            place.pre_place_approach.direction.header.frame_id = self._robot.get_planning_frame()

            place.pre_place_approach.direction.vector.x =  0
            place.pre_place_approach.direction.vector.y =  0
            place.pre_place_approach.direction.vector.z = -1

            # Generate post place approach:
            place.post_place_retreat.direction.header.stamp = now
            place.post_place_retreat.direction.header.frame_id = self._robot.get_planning_frame()

            place.post_place_retreat.desired_distance = self._approach_retreat_desired_dist
            place.post_place_retreat.min_distance = self._approach_retreat_min_dist

            place.post_place_retreat.direction.vector.x = 0
            place.post_place_retreat.direction.vector.y = 0
            place.post_place_retreat.direction.vector.z = 1

            # Add place:
            places.append(place)

        # Publish places (for debugging/visualization purposes):
        self._publish_places(places)

        return places

    def _create_pickup_goal(self, group, target, grasps):
        """
        Create a MoveIt! PickupGoal
        """

        # Create goal:
        goal = PickupGoal()

        goal.group_name  = group
        goal.target_name = target

        goal.possible_grasps.extend(grasps)

        goal.allowed_touch_objects.append(target)

        goal.support_surface_name = self._table_object_name

        # Configure goal planning options:
        goal.allowed_planning_time = 7.0

        goal.planning_options.planning_scene_diff.is_diff = True
        goal.planning_options.planning_scene_diff.robot_state.is_diff = True
        goal.planning_options.plan_only = False
        goal.planning_options.replan = True
        goal.planning_options.replan_attempts = 20

        return goal

    def _create_place_goal(self, group, target, places):
        """
        Create a MoveIt! PlaceGoal
        """

        # Create goal:
        goal = PlaceGoal()

        goal.group_name           = group
        goal.attached_object_name = target

        goal.place_locations.extend(places)

        # Configure goal planning options:
        goal.allowed_planning_time = 7.0

        goal.planning_options.planning_scene_diff.is_diff = True
        goal.planning_options.planning_scene_diff.robot_state.is_diff = True
        goal.planning_options.plan_only = False
        goal.planning_options.replan = True
        goal.planning_options.replan_attempts = 20

        return goal

    def _pickup(self, group, target, width):
        """
        Pick up a target using the planning group
        """

        # Obtain possible grasps from the grasp generator server:
        grasps = self._generate_grasps(self._pose_coke_can, width)

        # Create and send Pickup goal:
        goal = self._create_pickup_goal(group, target, grasps)

        state = self._pickup_ac.send_goal_and_wait(goal)
        if state != GoalStatus.SUCCEEDED:
            rospy.logerr('Pick up goal failed!: %s' % self._pickup_ac.get_goal_status_text())
            return None

        result = self._pickup_ac.get_result()

        # Check for error:
        err = result.error_code.val
        if err != MoveItErrorCodes.SUCCESS:
            rospy.logwarn('Group %s cannot pick up target %s!: %s' % (group, target, str(moveit_error_dict[err])))

            return False

        return True

    def _place(self, group, target, place):
        """
        Place a target using the planning group
        """

        # Obtain possible places:
        places = self._generate_places(place)

        # Create and send Place goal:
        goal = self._create_place_goal(group, target, places)

        state = self._place_ac.send_goal_and_wait(goal)
        if state != GoalStatus.SUCCEEDED:
            rospy.logerr('Place goal failed!: %s' % self._place_ac.get_goal_status_text())
            return None

        result = self._place_ac.get_result()

        # Check for error:
        err = result.error_code.val
        if err != MoveItErrorCodes.SUCCESS:
            rospy.logwarn('Group %s cannot place target %s!: %s' % (group, target, str(moveit_error_dict[err])))

            return False

        return True

    def _publish_grasps(self, grasps):
        """
        Publish grasps as poses, using a PoseArray message
        """

        if self._grasps_pub.get_num_connections() > 0:
            msg = PoseArray()
            msg.header.frame_id = self._robot.get_planning_frame()
            msg.header.stamp = rospy.Time.now()

            for grasp in grasps:
                p = grasp.grasp_pose.pose

                msg.poses.append(Pose(p.position, p.orientation))

            self._grasps_pub.publish(msg)

    def _publish_places(self, places):
        """
        Publish places as poses, using a PoseArray message
        """

        if self._places_pub.get_num_connections() > 0:
            msg = PoseArray()
            msg.header.frame_id = self._robot.get_planning_frame()
            msg.header.stamp = rospy.Time.now()

            for place in places:
                msg.poses.append(place.place_pose.pose)

            self._places_pub.publish(msg)
Beispiel #58
0
    plan, fraction = cartesian_path(-x_distance, -y_distance, -z_distance)
    move_group.execute(plan, wait=True)

    rospy.sleep(3)
    (result_xyz, result_rot) = get_TF('/odom', 'ee_link')
    print 'xyz_result=', result_xyz[0], result_xyz[1], result_xyz[2]
    print "Grasping complete!, Go to home pose..!"


if __name__ == '__main__':

    #GROUP_NAME_GRIPPER = "NAME OF GRIPPER"
    roscpp_initialize(sys.argv)
    rospy.init_node('control_Husky_UR3', anonymous=True)
    robot = RobotCommander()
    scene = PlanningSceneInterface()

    ##모바일 파트 관련 변수 선언
    x = 0.0
    y = 0.0
    theta = 0.0

    ## 매니퓰레이터 변수 선언

    group_name = "ur3_manipulator"
    move_group = MoveGroupCommander(group_name)
    FIXED_FRAME = 'world'

    display_trajectory_publisher = rospy.Publisher(
        '/move_group/display_planned_path', DisplayTrajectory, queue_size=20)
Beispiel #59
0
#!/usr/bin/env python

import sys
import rospy
from moveit_commander import RobotCommander, PlanningSceneInterface, roscpp_initialize, roscpp_shutdown
from geometry_msgs.msg import PoseStamped

if __name__=='__main__':
    roscpp_initialize(sys.argv)
    rospy.init_node('moveit_py_demo', anonymous=True)
    scene = PlanningSceneInterface()
    robot = RobotCommander()
    rospy.sleep(1)
    # clean the scene
    #scene.remove_world_object("block1")
    #scene.remove_world_object("block2")
    #scene.remove_world_object("block3")
    #scene.remove_world_object("block4")
    #scene.remove_world_object("table")
    #scene.remove_world_object("bowl")
    #scene.remove_world_object("box")
    # publish a demo scene
    p = PoseStamped()
    p.header.frame_id = robot.get_planning_frame()
    p.pose.position.x = 0.7
    p.pose.position.y = -0.4
    p.pose.position.z = 0.85
    p.pose.orientation.w = 1.57
    scene.add_box("block1", p, (0.044, 0.044, 0.044))
    p.pose.position.y = -0.2
    p.pose.position.z = 0.175
Beispiel #60
0
class PickAndPlace:
    def setColor(self, name, r, g, b, a=0.9):
        color = ObjectColor()
        color.id = name
        color.color.r = r
        color.color.g = g
        color.color.b = b
        color.color.a = a
        self.colors[name] = color

    def sendColors(self):
        p = PlanningScene()
        p.is_diff = True
        for color in self.colors.values():
            p.object_colors.append(color)
        self.scene_pub.publish(p)

    def add_point(self, traj, time, positions, velocities=None):
        point = trajectory_msgs.msg.JointTrajectoryPoint()
        point.positions = copy.deepcopy(positions)
        if velocities is not None:
            point.velocities = copy.deepcopy(velocities)
            point.time_from_start = rospy.Duration(time)
            traj.points.append(point)

    def FollowQTraj(self, q_traj, t_traj):
        assert (len(q_traj) == len(t_traj))

        #Insert current position to beginning.
        if t_traj[0] > 1.0e-2:
            t_traj.insert(0, 0.0)
            q_traj.insert(0, self.Q(arm=arm))

        self.dq_traj = self.QTrajToDQTraj(q_traj, t_traj)

    def QTrajToDQTraj(self, q_traj, t_traj):
        dof = len(q_traj[0])
        #Modeling the trajectory with spline.
        splines = [TCubicHermiteSpline() for d in range(dof)]
        for d in range(len(splines)):
            data_d = [[t, q[d]] for q, t in zip(q_traj, t_traj)]
            splines[d].Initialize(data_d,
                                  tan_method=splines[d].CARDINAL,
                                  c=0.0,
                                  m=0.0)

#NOTE: We don't have to make spline models as we just want velocities at key points.
#  They can be obtained by computing tan_method, which will be more efficient.         with_tan=True
        dq_traj = []
        for t in t_traj:
            dq = [splines[d].Evaluate(t, with_tan=True)[1] for d in range(dof)]
            dq_traj.append(dq)

#print dq_traj
        return dq_traj

    def JointNames(self):
        #0arm= 0
        return self.joint_names[0]

    def ROSGetJTP(self, q, t, dq=None):
        jp = trajectory_msgs.msg.JointTrajectoryPoint()
        jp.positions = q
        jp.time_from_start = rospy.Duration(t)
        if dq is not None: jp.velocities = dq
        return jp

    def ToROSTrajectory(self, joint_names, q_traj, t_traj, dq_traj=None):
        assert (len(q_traj) == len(t_traj))
        if dq_traj is not None: (len(dq_traj) == len(t_traj))
        #traj= trajectory_msgs.msg.JointTrajectory()
        self.traj.joint_names = joint_names
        if dq_traj is not None:
            self.traj.points = [
                self.ROSGetJTP(q, t, dq)
                for q, t, dq in zip(q_traj, t_traj, dq_traj)
            ]
        else:
            self.traj.points = [
                self.ROSGetJTP(q, t) for q, t in zip(q_traj, t_traj)
            ]
            self.traj.header.stamp = rospy.Time.now()
            #print self.traj
        return self.traj

    def SmoothQTraj(self, q_traj):
        if len(q_traj) == 0: return
        q_prev = np.array(q_traj[0])
        q_offset = np.array([0] * len(q_prev))
        for q in q_traj:
            q_diff = np.array(q) - q_prev
            for d in range(len(q_prev)):
                if q_diff[d] < -math.pi: q_offset[d] += 1
                elif q_diff[d] > math.pi: q_offset[d] -= 1
            q_prev = copy.deepcopy(q)
            q[:] = q + q_offset * 2.0 * math.pi

    def add_target(self, target_pose, target_size, frame, x, y, o1, o2, o3,
                   o4):
        target_pose.header.frame_id = frame
        target_pose.pose.position.x = x
        target_pose.pose.position.y = y
        target_pose.pose.position.z = self.table_ground + self.table_size[
            2] + target_size[2] / 2.0
        target_pose.pose.orientation.x = o1
        target_pose.pose.orientation.y = o2
        target_pose.pose.orientation.z = o3
        target_pose.pose.orientation.w = o4
        #self.scene.add_box(f_target_id,f_target_pose,f_target_size)

    def cts(self, start_pose, end_pose, maxtries, exe_signal=False):
        waypoints = []
        fraction = 0.0
        attempts = 0
        waypoints.append(start_pose.pose)
        waypoints.append(end_pose.pose)
        while fraction != 1 and attempts < maxtries:
            (plan, fraction) = self.arm.compute_cartesian_path(
                waypoints, 0.005, 0.0, True)
            attempts += 1
            if (attempts % maxtries == 0 and fraction != 1):
                rospy.loginfo("path planning failed with  " +
                              str(fraction * 100) + "% success.")
                return 0, 0
                continue
            elif fraction == 1:
                rospy.loginfo("path compute successfully with " +
                              str(attempts) + " attempts.")
                if exe_signal:
                    q_traj = [self.arm.get_current_joint_values()]
                    t_traj = [0.0]
                    for i in range(2, len(plan.joint_trajectory.points)):
                        q_traj.append(
                            plan.joint_trajectory.points[i].positions)
                        t_traj.append(t_traj[-1] + 0.08)
                    self.FollowQTraj(q_traj, t_traj)
                    self.sub_jpc.publish(
                        self.ToROSTrajectory(self.JointNames(), q_traj, t_traj,
                                             self.dq_traj))
                    rospy.sleep(5)
                end_joint_state = plan.joint_trajectory.points[-1].positions
                signal = 1
                return signal, end_joint_state

    #move and rotate
    def cts_rotate(self,
                   start_pose,
                   end_pose,
                   angle_r,
                   maxtries,
                   exe_signal=False):
        angle = angle_r * 3.14 / 180.0
        waypoints = []
        fraction = 0.0
        attempts = 0
        waypoints.append(start_pose.pose)
        waypoints.append(end_pose.pose)
        while fraction != 1 and attempts < maxtries:
            (plan, fraction) = self.arm.compute_cartesian_path(
                waypoints, 0.005, 0.0, True)
            attempts += 1
            if (attempts % maxtries == 0 and fraction != 1):
                rospy.loginfo("path planning failed with  " +
                              str(fraction * 100) + "% success.")
                return 0, 0.0
                continue
            elif fraction == 1:
                rospy.loginfo("path compute successfully with " +
                              str(attempts) + " attempts.")
                if exe_signal:
                    q_traj = [self.arm.get_current_joint_values()]
                    t_traj = [0.0]
                    per_angle = angle / (len(plan.joint_trajectory.points) - 2)
                    for i in range(2, len(plan.joint_trajectory.points)):
                        joint_inc_list = [
                            j
                            for j in plan.joint_trajectory.points[i].positions
                        ]
                        #plan.joint_trajectory.points[i].positions[6]-=per_angle*(i-1)
                        joint_inc_list[6] -= per_angle * (i - 1)
                        q_traj.append(joint_inc_list)
                        t_traj.append(t_traj[-1] + 0.07)
                    self.FollowQTraj(q_traj, t_traj)
                    self.sub_jpc.publish(
                        self.ToROSTrajectory(self.JointNames(), q_traj, t_traj,
                                             self.dq_traj))
                    rospy.sleep(5)
                end_joint_state = plan.joint_trajectory.points[-1].positions
                signal = 1
                return signal, end_joint_state

    def cts_rotate_delta(self,
                         start_pose,
                         end_pose,
                         angle_pre,
                         angle_r,
                         maxtries,
                         exe_signal=False):
        angle = (angle_r - angle_pre) * 3.14 / 180.0
        waypoints = []
        fraction = 0.0
        attempts = 0
        waypoints.append(start_pose.pose)
        waypoints.append(end_pose.pose)
        while fraction != 1 and attempts < maxtries:
            (plan, fraction) = self.arm.compute_cartesian_path(
                waypoints, 0.005, 0.0, True)
            attempts += 1
            if (attempts % maxtries == 0 and fraction != 1):
                rospy.loginfo("path planning failed with  " +
                              str(fraction * 100) + "% success.")
                return 0, 0.0
                continue
            elif fraction == 1:
                rospy.loginfo("path compute successfully with " +
                              str(attempts) + " attempts.")
                if exe_signal:
                    end_joint_state = plan.joint_trajectory.points[
                        -1].positions
                    q_traj = [self.arm.get_current_joint_values()]
                    t_traj = [0.0]
                    per_angle = angle / (len(plan.joint_trajectory.points) - 2)
                    for i in range(2, len(plan.joint_trajectory.points)):
                        joint_inc_list = [
                            j
                            for j in plan.joint_trajectory.points[i].positions
                        ]
                        #~ plan.joint_trajectory.points[i].positions[6]-=per_angle*(i-1)
                        #~ if i==len(plan.joint_trajectory.points)-1:
                        #~ joint_inc_list[6]-=angle
                        #~ q_traj.append(joint_inc_list)
                        #~ t_traj.append(1.5)
                        joint_inc_list[6] -= per_angle * (i - 1) + (
                            angle_pre * 3.14 / 180.0)
                        q_traj.append(joint_inc_list)
                        t_traj.append(t_traj[-1] + 0.1)
                    self.FollowQTraj(q_traj, t_traj)
                    self.sub_jpc.publish(
                        self.ToROSTrajectory(self.JointNames(), q_traj, t_traj,
                                             self.dq_traj))
                    rospy.sleep(5)
                signal = 1
                return signal, end_joint_state

    # shaking function:
    # freq : shaking freqence
    # times : shaking time per action
    def shaking(self, initial_state, end_joint_state, freq, times):
        q_traj = [initial_state]
        t_traj = [0.0]
        for i in range(times):
            q_traj.append(end_joint_state)
            t_traj.append(t_traj[-1] + 0.5 / freq)
            q_traj.append(initial_state)
            t_traj.append(t_traj[-1] + 0.5 / freq)
        self.FollowQTraj(q_traj, t_traj)
        self.sub_jpc.publish(
            self.ToROSTrajectory(self.JointNames(), q_traj, t_traj,
                                 self.dq_traj))
        rospy.sleep(6)

    def shake_a(self, pre_p_angle, r_angle, amp):
        start_shake_pose = self.arm.get_current_pose(
            self.arm_end_effector_link)  # for trajectory of shaking
        start_joint_state = self.arm.get_current_joint_values(
        )  # for state[6] to rotate
        shift_pose = deepcopy(start_shake_pose)
        r_angle = (r_angle / 180.0) * 3.14
        pre_p_angle = (pre_p_angle / 180.0) * 3.14
        shift_pose.pose.position.x += amp * math.sin(r_angle) * math.cos(
            pre_p_angle)  # in verticle direction
        shift_pose.pose.position.y -= amp * math.sin(r_angle) * math.sin(
            pre_p_angle)
        shift_pose.pose.position.z += amp * math.cos(r_angle)  #...
        signal, end_joint_state = self.cts(start_shake_pose, shift_pose, 300)
        return signal, end_joint_state

    def shake_b(self, pre_p_angle, r_angle, amp):
        start_shake_pose = self.arm.get_current_pose(
            self.arm_end_effector_link)  # for trajectory of shaking
        start_joint_state = self.arm.get_current_joint_values(
        )  # for state[6] to rotate
        shift_pose = deepcopy(start_shake_pose)
        r_angle = (r_angle / 180.0) * 3.14
        pre_p_angle = (pre_p_angle / 180.0) * 3.14
        shift_pose.pose.position.x -= amp * math.cos(r_angle) * math.cos(
            pre_p_angle)  # in verticle direction
        shift_pose.pose.position.y += amp * math.cos(r_angle) * math.sin(
            pre_p_angle)
        shift_pose.pose.position.z += amp * math.sin(r_angle)  #...
        signal, end_joint_state = self.cts(start_shake_pose, shift_pose, 300)
        return signal, end_joint_state

    def setupSence(self):
        r_tool_size = [0.03, 0.02, 0.18]
        l_tool_size = [0.03, 0.02, 0.18]
        #real scene table
        table_pose = PoseStamped()
        table_pose.header.frame_id = self.reference_frame
        table_pose.pose.position.x = -0.052
        table_pose.pose.position.y = 0.65
        table_pose.pose.position.z = self.table_ground + self.table_size[
            2] / 2.0
        table_pose.pose.orientation.w = 1.0
        self.scene.add_box(self.table_id, table_pose, self.table_size)
        #left gripper
        l_p = PoseStamped()
        l_p.header.frame_id = self.arm_end_effector_link
        l_p.pose.position.x = 0.00
        l_p.pose.position.y = 0.057
        l_p.pose.position.z = 0.09
        l_p.pose.orientation.w = 1
        self.scene.attach_box(self.arm_end_effector_link, self.l_id, l_p,
                              l_tool_size)
        #right gripper
        r_p = PoseStamped()
        r_p.header.frame_id = self.arm_end_effector_link
        r_p.pose.position.x = 0.00
        r_p.pose.position.y = -0.057
        r_p.pose.position.z = 0.09
        r_p.pose.orientation.w = 1
        self.scene.attach_box(self.arm_end_effector_link, self.r_id, r_p,
                              r_tool_size)

    #Params: pose of bottle, grasp_radius, grasp_height, grasp_theta
    def get_grasp_pose(self, pose, r, theta):
        g_p = PoseStamped()
        g_p.header.frame_id = self.arm_end_effector_link
        g_p.pose.position.x = pose.pose.position.x + r * math.sin(theta)
        g_p.pose.position.y = pose.pose.position.y - r * math.cos(theta)
        g_p.pose.position.z = pose.pose.position.z
        g_p.pose.orientation.w = 0.5 * (math.cos(0.5 * theta) -
                                        math.sin(0.5 * theta))
        g_p.pose.orientation.x = -0.5 * (math.cos(0.5 * theta) +
                                         math.sin(0.5 * theta))
        g_p.pose.orientation.y = 0.5 * (math.cos(0.5 * theta) -
                                        math.sin(0.5 * theta))
        g_p.pose.orientation.z = 0.5 * (math.sin(0.5 * theta) +
                                        math.cos(0.5 * theta))
        return g_p

    def get_pour_pose(self, pose, h, r, theta):
        p_p = PoseStamped()
        p_p.header.frame_id = self.arm_end_effector_link
        p_p.pose.position.x = pose.pose.position.x - r * math.cos(theta)
        p_p.pose.position.y = pose.pose.position.y + r * math.sin(theta)
        p_p.pose.position.z = pose.pose.position.z + h
        theta *= -1
        p_p.pose.orientation.w = 0.5 * (math.cos(0.5 * theta) -
                                        math.sin(0.5 * theta))
        p_p.pose.orientation.x = -0.5 * (math.cos(0.5 * theta) +
                                         math.sin(0.5 * theta))
        p_p.pose.orientation.y = 0.5 * (math.cos(0.5 * theta) -
                                        math.sin(0.5 * theta))
        p_p.pose.orientation.z = 0.5 * (math.sin(0.5 * theta) +
                                        math.cos(0.5 * theta))
        return p_p

    def pour_rotate(self, initial_pose, angle_pre, angle_r, r, maxtries):
        angle_pre = (angle_pre / 180.0) * 3.14
        angle_r_1 = (angle_r / 180.0) * 3.14
        cur_pose = self.arm.get_current_pose(self.arm_end_effector_link)
        final_pose = deepcopy(initial_pose)
        final_pose.pose.position.x += r * (
            1 - math.cos(angle_r_1)) * math.cos(angle_pre)
        final_pose.pose.position.y += r * (
            1 - math.cos(angle_r_1)) * math.sin(angle_pre)
        final_pose.pose.position.z += r * math.sin(angle_r_1)
        #~ signal,e_j_s=self.cts_rotate_delta(cur_pose,final_pose,angle_r_pre,angle_r,maxtries,True)
        signal, e_j_s = self.cts_rotate(cur_pose, final_pose, angle_r,
                                        maxtries, True)
        return signal

    def p_r_trail(self, angle_pre, angle_r, r, maxtries):
        angle_pre = (angle_pre / 180.0) * 3.14
        angle_r_1 = (angle_r / 180.0) * 3.14
        initial_pose = self.arm.get_current_pose(self.arm_end_effector_link)
        final_pose = deepcopy(initial_pose)
        final_pose.pose.position.x += r * (
            1 - math.cos(angle_r_1)) * math.cos(angle_pre)
        final_pose.pose.position.y += r * (
            1 - math.cos(angle_r_1)) * math.sin(angle_pre)
        final_pose.pose.position.z += r * math.sin(angle_r_1)
        signal, e_j_s = self.cts_rotate(initial_pose, final_pose, angle_r,
                                        maxtries, True)
        return signal

    def move_back(self, back_pose):
        current_pose = self.arm.get_current_pose(self.arm_end_effector_link)
        signal, end_j = self.cts(current_pose, back_pose, 300, True)
        return signal

    def pg_g_pp(self, pose, r):
        start_pose = self.arm.get_current_pose(self.arm_end_effector_link)
        p_i_radian = np.arctan(abs(pose.pose.position.x /
                                   pose.pose.position.y))
        p_i_angle = p_i_radian * 180.0 / 3.14
        pick_list = [
            p_i_angle, 5.0, 25.0, 45.0, 65.0, 15.0, 35.0, 55.0, 75.0, 10.0,
            20.0, 30.0, 40.0, 50.0, 60.0
        ]

        for i in range(len(pick_list)):
            theta = (pick_list[i] / 180.0) * 3.14
            if pose.pose.position.x > 0:
                theta *= -1.0
            grasp_pose = self.get_grasp_pose(pose, r, theta)
            #pick up

            signal, e_j_s = self.cts(start_pose, grasp_pose, 300, True)
            if signal == 0: continue
            break
        rospy.sleep(1)
        self.gripperCtrl(0)
        rospy.sleep(2)
        #move to ori_pose
        current_pose = self.arm.get_current_pose(self.arm_end_effector_link)
        signal, e_j_s = self.cts(current_pose, start_pose, 300, True)
        rospy.sleep(2)
        rospy.loginfo("Grasping done.")
        return start_pose, grasp_pose

    def pp_ps_old(self, target_pose, pour_angle, r_pour, h_pour):
        for i in range(len(pour_angle)):
            maxtries = 300
            start_pose = self.arm.get_current_pose(self.arm_end_effector_link)
            theta = (pour_angle[i] / 180.0) * 3.14
            pour_pose = self.get_pour_pose(target_pose, h_pour, r_pour, theta)
            #move to pose
            signal_1, e_j_s = self.cts_rotate(start_pose, pour_pose, 90.0,
                                              maxtries, True)
            if signal_1 == 0: continue
            pre_pour_angle = pour_angle[i]
            rospy.loginfo("Ready for pouring.")
            return pre_pour_angle

    def pp_ps(self, target_pose, pour_angle, r_pour, h_pour):
        maxtries = 300
        start_pose = self.arm.get_current_pose(self.arm_end_effector_link)
        theta = (pour_angle / 180.0) * 3.14
        pour_pose = self.get_pour_pose(target_pose, h_pour, r_pour, theta)
        #move to pose
        signal_1, e_j_s = self.cts_rotate(start_pose, pour_pose, 90.0,
                                          maxtries, True)
        return signal_1

    def go_back(self, ori_pose, pre_grasp_pose):
        cur_pose = self.arm.get_current_pose(self.arm_end_effector_link)
        signal, e1 = self.cts(cur_pose, ori_pose, 300, True)
        rospy.loginfo("back to pre_grasp pose, ready for placing bottle..")
        rospy.sleep(1)
        signal_1, e2 = self.cts(ori_pose, pre_grasp_pose, 300, True)
        rospy.loginfo("back to grasp pose, open gripper..")
        rospy.sleep(1)
        self.gripperCtrl(255)
        rospy.sleep(1)
        signal_2, e3 = self.cts(pre_grasp_pose, ori_pose, 300, True)
        rospy.loginfo("back to pre_grasp pose, ready for next kind of source.")

    def rotate_back(self, joint_state):
        q_traj = [self.arm.get_current_joint_values()]
        t_traj = [0.0]
        q_traj.append(joint_state)
        t_traj.append(t_traj[-1] + 4)
        self.FollowQTraj(q_traj, t_traj)
        self.sub_jpc.publish(
            self.ToROSTrajectory(self.JointNames(), q_traj, t_traj,
                                 self.dq_traj))
        rospy.sleep(5)

    def __init__(self):
        moveit_commander.roscpp_initialize(sys.argv)
        rospy.init_node('moveit_demo')
        self.scene = PlanningSceneInterface()
        pub_traj = rospy.Publisher('/joint_path_command',
                                   trajectory_msgs.msg.JointTrajectory,
                                   queue_size=10)
        #pub_ratio_sig= rospy.Publisher('/enable_source_change',Int64,queue_size=1)
        self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
        self.gripperCtrl = rospy.ServiceProxy(
            "/two_finger/gripper/gotoPositionUntilTouch", SetPosition)
        self.colors = dict()
        rospy.sleep(1)
        self.robot = moveit_commander.robot.RobotCommander()
        self.arm = MoveGroupCommander('arm')
        self.arm.allow_replanning(True)
        cartesian = rospy.get_param('~cartesian', True)
        self.arm_end_effector_link = self.arm.get_end_effector_link()
        self.arm.set_goal_position_tolerance(0.005)
        self.arm.set_goal_orientation_tolerance(0.025)
        self.arm.allow_replanning(True)
        self.reference_frame = 'base_link'
        self.arm.set_pose_reference_frame(self.reference_frame)
        self.arm.set_planning_time(5)
        #shaking
        self.joint_names = [[]]
        self.joint_names[0] = rospy.get_param('controller_joint_names')
        self.traj = trajectory_msgs.msg.JointTrajectory()
        self.sub_jpc = rospy.Publisher('/joint_path_command',
                                       trajectory_msgs.msg.JointTrajectory,
                                       queue_size=10)
        #scene planning
        self.l_id = 'l_tool'
        self.r_id = 'r_tool'
        self.table_id = 'table'
        self.target1_id = 'target1_object'
        self.target2_id = 'target2_object'
        #~ self.target3_id='target3_object'
        #~ self.target4_id='target4_object'
        self.f_target_id = 'receive_container'
        self.scene.remove_world_object(self.l_id)
        self.scene.remove_world_object(self.r_id)
        self.scene.remove_world_object(self.table_id)
        self.scene.remove_world_object(self.target1_id)
        self.scene.remove_world_object(self.target2_id)
        #~ self.scene.remove_world_object(self.target3_id)
        #~ self.scene.remove_world_object(self.target4_id)
        self.scene.remove_world_object(self.f_target_id)
        #self.scene.remove_attached_object(self.arm_end_effector_link,self.target_id)

        self.table_ground = 0.13
        self.table_size = [0.9, 0.6, 0.018]
        self.setupSence()
        target1_size = [0.035, 0.035, 0.19]
        target2_size = target1_size
        #~ target3_size=target1_size
        #~ target4_size=target1_size
        self.f_target_size = [0.2, 0.2, 0.04]

        f_target_pose = PoseStamped()
        pre_pour_pose = PoseStamped()
        target1_pose = PoseStamped()
        target2_pose = PoseStamped()
        #~ target3_pose=PoseStamped()
        #~ target4_pose=PoseStamped()

        joint_names = [
            'joint_' + jkey for jkey in ('s', 'l', 'e', 'u', 'r', 'b', 't')
        ]
        joint_names = rospy.get_param('controller_joint_names')
        traj = trajectory_msgs.msg.JointTrajectory()
        traj.joint_names = joint_names

        #final target
        l_gripper = 0.18
        #self.add_target(f_target_pose,self.f_target_size,self.reference_frame,-0.184+0.27,0.62+0.1,0,0,0,1)
        #~ self.add_target(f_target_pose,self.f_target_size,self.reference_frame,0.24,0.6-l_gripper,0,0,0,1)
        #~ self.scene.add_box(self.f_target_id,f_target_pose,self.f_target_size)
        #self.add_target(pre_pour_pose,self.reference_frame,x,y,0,0,0,1)

        #target localization
        msg1 = rospy.wait_for_message('/aruco_simple/pose', Pose)
        rospy.sleep(1)
        msg2 = rospy.wait_for_message('/aruco_simple/pose2', Pose)
        rospy.sleep(1)
        self.add_target(f_target_pose, self.f_target_size,
                        self.reference_frame, -msg2.position.y - 0.257,
                        -msg2.position.x + 0.81 - 0.1 - l_gripper, 0, 0, 0, 1)
        self.scene.add_box(self.f_target_id, f_target_pose, self.f_target_size)
        self.add_target(target1_pose, target1_size, self.reference_frame,
                        -msg1.position.y - 0.257,
                        -msg1.position.x + 0.81 - 0.065, 0, 0, 0, 1)
        self.scene.add_box(self.target1_id, target1_pose, target1_size)
        #~ self.add_target(target2_pose,target2_size,self.reference_frame,-msg2.position.y-0.257,-msg2.position.x+0.81-0.065,0,0,0,1)
        #~ self.scene.add_box(self.target2_id,target2_pose,target2_size)
        #~ self.add_target(target3_pose,target3_size,self.reference_frame,-0.01,0.76-0.01,0,0,0,1)
        #~ self.scene.add_box(self.target3_id,target3_pose,target3_size)
        #~ self.add_target(target4_pose,target4_size,self.reference_frame,0.25,0.80,0,0,0,1)
        #~ self.scene.add_box(self.target4_id,target4_pose,target4_size)

        #attach_pose
        g_p = PoseStamped()
        g_p.header.frame_id = self.arm_end_effector_link
        g_p.pose.position.x = 0.00
        g_p.pose.position.y = -0.00
        g_p.pose.position.z = 0.025
        g_p.pose.orientation.w = 0.707
        g_p.pose.orientation.x = 0
        g_p.pose.orientation.y = -0.707
        g_p.pose.orientation.z = 0

        #set color
        self.setColor(self.target1_id, 0.8, 0, 0, 1.0)
        self.setColor(self.target2_id, 0.8, 0, 0, 1.0)
        #~ self.setColor(self.target3_id,0.8,0,0,1.0)
        #self.setColor(self.target4_id,0.8,0,0,1.0)
        self.setColor(self.f_target_id, 0.8, 0.3, 0, 1.0)
        self.setColor('r_tool', 0.8, 0, 0)
        self.setColor('l_tool', 0.8, 0, 0)
        self.sendColors()
        self.gripperCtrl(255)
        rospy.sleep(3)
        self.arm.set_named_target("initial_arm")
        self.arm.go()
        rospy.sleep(3)

        #j_ori_state=[-1.899937629699707, -0.5684762597084045, 0.46537330746650696, 2.3229329586029053, -0.057941947132349014, -1.2867668867111206, 0.2628822326660156]
        #j_ori_state=[-2.161055326461792, -0.6802523136138916, -1.7733728885650635, -2.3315746784210205, -0.5292841196060181, 1.4411976337432861, -2.2327845096588135]
        j_ori_state = [
            -1.2628753185272217, -0.442996621131897, -0.1326361745595932,
            2.333048105239868, -0.15598002076148987, -1.2167049646377563,
            3.1414425373077393
        ]

        #signal= True
        self.arm.set_joint_value_target(j_ori_state)
        self.arm.go()
        rospy.sleep(3)
        #parameter setup
        tar_num = 1
        maxtries = 300
        r_grasp = 0.16
        #~ topic_name=["/ratio_carrot","/ratio_lettuce","ratio_pepper"]
        #~ save_data=["carrot.txt","pepper.txt","cucumber.txt"]
        #ratio_table=[0.03,0.025,0.04]
        for i in range(0, tar_num):
            #grasp target
            rospy.loginfo("Choosing source...")
            #~ dfile=open(save_data[i],"a")
            if i == 0:
                target_pose = target1_pose
                target_id = self.target1_id
                target_size = target1_size
                #~ amp=0.13
                #~ freq=2.75
                #~ r_angle=40.0
            elif i == 1:
                target_pose = target2_pose
                target_id = self.target2_id
                target_size = target2_size
                #~ amp=0.15
                #~ freq=2.5
                #~ r_angle=45.0
            elif i == 2:
                target_pose = target3_pose
                target_id = self.target3_id
                target_size = target3_size
                amp = 0.15
                freq = 2.75
                r_angle = 40.0
            elif i == 3:
                target_pose = target4_pose
                target_id = self.target4_id
                target_size = target4_size
                amp = 0.1
                freq = 2.75
                r_angle = 45.0

            r_pour = 0.13
            r_bottle = 0.1
            h_pour = 0.1

            pour_angle = [0.0, -15.0, 15.0, -30.0, 30.0, -45.0, 45.0]
            #~ tip_angle=[10.0,20.0,30.0,40.0,50.0,60.0,70.0,80.0,90.0]
            #~ tip_angle=[15.0,30.0,45.0,60.0,75.0,90.0]
            tip_angle = [20.0, 30.0, 40.0, 50.0, 60.0, 70.00]
            shake_freq = [2.0, 2.25, 2.50, 2.75]
            shake_amp = [0.05, 0.075, 0.1, 0.125, 0.15]
            shake_per_times = 5
            shake_times_tgt = 2
            rospy.loginfo("Params loaded.")
            rospy.loginfo("Current Source: " + str(i + 1) + "  ")
            #grasp and back
            ori_pose, g_pose = self.pg_g_pp(target_pose, r_grasp)
            ori_joint_state = self.arm.get_current_joint_values()
            #move to target position for pouring
            for m in range(len(pour_angle)):

                for j in range(len(tip_angle)):
                    for k in range(len(shake_freq)):
                        dfile = open(
                            "pepper_tipangle_" + str(tip_angle[j]) +
                            "_shaking_freq_" + str(shake_freq[k]) + "_1", "a")
                        for n in range(len(shake_amp)):
                            pp_ps_sgn = self.pp_ps(f_target_pose,
                                                   pour_angle[m], r_pour,
                                                   h_pour)
                            if pp_ps_sgn == 0:
                                rospy.loginfo(
                                    "pre-Pouring angle not correct,  back for replanning..."
                                )
                                self.rotate_back(ori_joint_state)
                                rospy.sleep(2)
                                continue
                            initial_pose = self.arm.get_current_pose(
                                self.arm_end_effector_link)
                            pour_signal = self.pour_rotate(
                                initial_pose, pour_angle[m], tip_angle[j],
                                r_bottle, maxtries)
                            if pour_signal != 1:
                                rospy.loginfo(
                                    "Pouring angle not correct,  back for replanning..."
                                )
                                self.rotate_back(ori_joint_state)
                                rospy.sleep(2)
                                continue
                            amp = shake_amp[n]
                            freq = shake_freq[k]
                            rospy.loginfo("shaking degree : " +
                                          str(tip_angle[j]) +
                                          ", shaking amp :  " + str(amp) +
                                          ", shaking freq : " + str(freq))
                            shake_times = 0
                            signal = True
                            start_joint_state = self.arm.get_current_joint_values(
                            )
                            signal, end_joint_state = self.shake_a(
                                pour_angle[m], tip_angle[j], amp)
                            while signal == 1:
                                #~ area_ratio= rospy.wait_for_message('/ratio_carrot',Float64)
                                #if (area_ratio>ratio_table[i]) or (shake_times>shake_times_tgt):
                                if (shake_times < shake_times_tgt):

                                    self.shaking(start_joint_state,
                                                 end_joint_state, freq,
                                                 shake_per_times)
                                    rospy.sleep(2)
                                    shake_times += 1
                                    rospy.loginfo("shaking times :  " +
                                                  str(shake_times) + " .")
                                    area_ratio = rospy.wait_for_message(
                                        '/ratio_carrot', Float64)
                                    if shake_times == shake_times_tgt:
                                        dfile.write(
                                            "%f %f \r\n" %
                                            (shake_amp[n], area_ratio.data))

                                        self.rotate_back(ori_joint_state)
                                        rospy.sleep(2)
                                else:
                                    signal = 0

                    self.rotate_back(ori_joint_state)
                    rospy.sleep(2)
                self.rotate_back(ori_joint_state)
                rospy.sleep(2)

        #remove and shut down
        self.scene.remove_attached_object(self.arm_end_effector_link, 'l_tool')
        self.scene.remove_attached_object(self.arm_end_effector_link, 'r_tool')
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)