def __init__(self):
     self.pub_right_hand_pose = rospy.Publisher(RIGHT_HAND_POSESTAMPED_TOPIC, PoseStamped, latch=True)
     self.pub_right_hand_pose_reference = rospy.Publisher(RIGHT_HAND_REFERENCE_POSESTAMPED_TOPIC, PoseStamped, latch=True)
     self.pub_left_hand_pose = rospy.Publisher(LEFT_HAND_POSESTAMPED_TOPIC, PoseStamped, latch=True)
     self.pub_left_hand_pose_reference = rospy.Publisher(LEFT_HAND_REFERENCE_POSESTAMPED_TOPIC, PoseStamped, latch=True)
     self.hydra_data_subs = rospy.Subscriber(HYDRA_DATA_TOPIC, Hydra, self.hydraDataCallback)
     self.pub_move_base = rospy.Publisher(MOVE_BASE_TOPIC, Twist)
     self.subs = rospy.Subscriber('/joint_states', JointState, self.getJointStates)
     self.current_joint_states = None
     rospy.loginfo("Getting first joint_states")
     while self.current_joint_states == None:
         rospy.sleep(0.1)
     rospy.loginfo("Gotten!")
     rospy.loginfo("Connecting with right hand AS")
     self.right_hand_as = actionlib.SimpleActionClient(HAND_RIGHT_AS, FollowJointTrajectoryAction)
     self.right_hand_as.wait_for_server()
     rospy.loginfo("Connecting with left hand AS")
     self.left_hand_as = actionlib.SimpleActionClient(HAND_LEFT_AS, FollowJointTrajectoryAction)
     self.left_hand_as.wait_for_server()
     rospy.loginfo("Starting up move group commander for right, left, torso and head... (slow)")
     self.right_arm_mgc = MoveGroupCommander("right_arm")
     self.right_arm_mgc.set_pose_reference_frame('base_link')
     self.left_arm_mgc = MoveGroupCommander("left_arm")
     self.left_arm_mgc.set_pose_reference_frame('base_link')
     self.torso_mgc = MoveGroupCommander("right_arm_torso")
     self.torso_mgc.set_pose_reference_frame('base_link')
     self.head_mgc = MoveGroupCommander("head")
     self.head_mgc.set_pose_reference_frame('base_link')
     self.last_hydra_message = None
     self.tmp_pose_right = PoseStamped()
     self.tmp_pose_left = PoseStamped()
     self.read_message = False
示例#2
0
 def __init__(self):
 ##########################################################################
         rospy.loginfo("BipedCommander started")
         self.legs_group = MoveGroupCommander("legs")
         self.arms_group = MoveGroupCommander("arms")
         self.rarm_group = MoveGroupCommander("RArm")
         self.larm_group = MoveGroupCommander("LArm")
class HeadMover():
    """ Moves head to specified joint values """
    def __init__(self):
        self.group_head = MoveGroupCommander('head')
        
    def move_head(self, name, joint_values):
        rospy.loginfo('Moving head to specified position')
        self.group_head.set_joint_value_target(joint_values)
        self.group_head.go()
示例#4
0
def init():
    #Wake up Baxter
    baxter_interface.RobotEnable().enable()
    rospy.sleep(0.25)
    print "Baxter is enabled"
    
    print "Intitializing clients for services"
    global ik_service_left
    ik_service_left = rospy.ServiceProxy(
            "ExternalTools/left/PositionKinematicsNode/IKService",
            SolvePositionIK)

    global ik_service_right
    ik_service_right = rospy.ServiceProxy(
            "ExternalTools/right/PositionKinematicsNode/IKService",
            SolvePositionIK)

    global obj_loc_service 
    obj_loc_service = rospy.ServiceProxy(
        "object_location_service", ObjLocation)

    global stopflag
    stopflag = False
    #Taken from the MoveIt Tutorials
    moveit_commander.roscpp_initialize(sys.argv)
    robot = moveit_commander.RobotCommander()

    global scene
    scene = moveit_commander.PlanningSceneInterface()

    #Activate Left Arm to be used with MoveIt
    global left_group
    left_group = MoveGroupCommander("left_arm")
    left_group.set_goal_position_tolerance(0.01)
    left_group.set_goal_orientation_tolerance(0.01)
    
    
    global right_group
    right_group = MoveGroupCommander("right_arm")
    pose_right = Pose()
    pose_right.position = Point(0.793, -0.586, 0.329)
    pose_right.orientation = Quaternion(1.0, 0.0, 0.0, 0.0)
    request_pose(pose_right, "right", right_group)
    

    global left_gripper
    left_gripper = baxter_interface.Gripper('left')
    left_gripper.calibrate()
    print left_gripper.parameters()

    global end_effector_subs
    end_effector_subs = rospy.Subscriber("/robot/end_effector/left_gripper/state", EndEffectorState, end_effector_callback)
    rospy.sleep(1)

    global pubpic
    pubpic = rospy.Publisher('/robot/xdisplay', Image, latch=True, queue_size=1)
    rospy.sleep(1)
def get_move_group_commander(group):
    '''
    Gets the move_group_commander for this process.

    '''
    global _mgc_dict
    with _mgc_dict_creation_lock:
        if not group in _mgc_dict:
            _mgc_group = MoveGroupCommander(group)
            _mgc_group.set_planner_id('RRTConnectkConfigDefault')
            _mgc_dict[group] = _mgc_group
	    add_ground()
        return _mgc_dict[group]
    def __init__(self):
        """
        This constructor initialises the different necessary connections to the topics and services
        and resets the world to start in a good position.
        """
        rospy.init_node("smart_grasper")
        
        self.__joint_state_sub = rospy.Subscriber("/joint_states", JointState, 
                                                  self.__joint_state_cb, queue_size=1)

        rospy.wait_for_service("/gazebo/get_model_state", 10.0)
        rospy.wait_for_service("/gazebo/reset_world", 10.0)
        self.__reset_world = rospy.ServiceProxy("/gazebo/reset_world", Empty)
        self.__get_pose_srv = rospy.ServiceProxy("/gazebo/get_model_state", GetModelState)

        rospy.wait_for_service("/gazebo/pause_physics")
        self.__pause_physics = rospy.ServiceProxy("/gazebo/pause_physics", Empty)
        rospy.wait_for_service("/gazebo/unpause_physics")
        self.__unpause_physics = rospy.ServiceProxy("/gazebo/unpause_physics", Empty)
        rospy.wait_for_service("/controller_manager/switch_controller")
        self.__switch_ctrl = rospy.ServiceProxy("/controller_manager/switch_controller", SwitchController)
        rospy.wait_for_service("/gazebo/set_model_configuration")
        self.__set_model = rospy.ServiceProxy("/gazebo/set_model_configuration", SetModelConfiguration)
        
        rospy.wait_for_service("/gazebo/delete_model")
        self.__delete_model = rospy.ServiceProxy("/gazebo/delete_model", DeleteModel)
        rospy.wait_for_service("/gazebo/spawn_sdf_model")
        self.__spawn_model = rospy.ServiceProxy("/gazebo/spawn_sdf_model", SpawnModel)
        
        rospy.wait_for_service('/get_planning_scene', 10.0)
        self.__get_planning_scene = rospy.ServiceProxy('/get_planning_scene', GetPlanningScene)
        self.__pub_planning_scene = rospy.Publisher('/planning_scene', PlanningScene, queue_size=10, latch=True)

        self.arm_commander = MoveGroupCommander("arm")
        self.hand_commander = MoveGroupCommander("hand")
        
        self.__hand_traj_client = SimpleActionClient("/hand_controller/follow_joint_trajectory", 
                                                     FollowJointTrajectoryAction)
        self.__arm_traj_client = SimpleActionClient("/arm_controller/follow_joint_trajectory", 
                                                    FollowJointTrajectoryAction)
                                              
        if self.__hand_traj_client.wait_for_server(timeout=rospy.Duration(4.0)) is False:
            rospy.logfatal("Failed to connect to /hand_controller/follow_joint_trajectory in 4sec.")
            raise Exception("Failed to connect to /hand_controller/follow_joint_trajectory in 4sec.")
                                              
        if self.__arm_traj_client.wait_for_server(timeout=rospy.Duration(4.0)) is False:
            rospy.logfatal("Failed to connect to /arm_controller/follow_joint_trajectory in 4sec.")
            raise Exception("Failed to connect to /arm_controller/follow_joint_trajectory in 4sec.")

        
        self.reset_world()
示例#7
0
    def __init__(self, config, debug=0):
        print "[INFO] Initialise Robot"
        self.DEBUG = debug
        # configuration
        self.config = config
        # initialise move groups
        self.arm = MoveGroupCommander(ARM_GROUP)
        self.gripper = MoveGroupCommander(GRIPPER_GROUP)
        # initialise pick and place manager
        if self.DEBUG is 1:
            # verbose mode
            self.pick_and_place = PickPlaceInterface(ARM_GROUP, GRIPPER_GROUP, False, True)
        else:
            # non verbose mode
            self.pick_and_place = PickPlaceInterface(ARM_GROUP, GRIPPER_GROUP, False, False)

        # tolerance: position (in m), orientation (in rad)
        self.arm.set_goal_position_tolerance(0.01)
        self.arm.set_goal_orientation_tolerance(0.1)

        self.place_manager = None
        self.pick_manager = None

        self.initialise_move_actions()
        self.start_position()
class MoveitCommanderMoveGroupState(EventState):
  """
  Uses moveit commander to plan to the given joint configuration and execute the resulting trajctory.
  """
  
  def __init__(self, planning_group, joint_names):
    """Constructor"""
    super(MoveitCommanderMoveGroupState, self).__init__(outcomes=['reached', 'failed'],
                    input_keys=['target_joint_config'])

    self._planning_group = planning_group
    self._joint_names = joint_names
    Logger.loginfo("About to make mgc in init with group %s" % self._planning_group)
    self.group_to_move = MoveGroupCommander(self._planning_group)
    Logger.loginfo("finished making mgc in init.")

    self._done = False


  def execute(self, userdata):
    """Execute this state"""
    if self._done is not False:
      return self._done
  
  def on_enter(self, userdata):
    # create the motion goal
    Logger.loginfo("Entering MoveIt Commander code!")

    if len(self._joint_names) != len(userdata.target_joint_config):
      Logger.logwarn("Number of joint names (%d) does not match number of joint values (%d)"
                      % (len(self._joint_names), len(userdata.target_joint_config)))

    self.group_to_move.set_joint_value_target(dict(zip(self._joint_names, userdata.target_joint_config)))               


    # execute the motion
    try: 
      Logger.loginfo("Moving %s to: %s" % (self._planning_group, ", ".join(map(str, userdata.target_joint_config))))
      result = self.group_to_move.go()
    except Exception as e:
      Logger.logwarn('Was unable to execute move group request:\n%s' % str(e))
      self._done = "failed"

    if result:
      self._done = "reached"
    else:
      self._done = "failed"
示例#9
0
	def __init__(self, planArm="left_arm"):
		# Initialize the move_group API
		#moveit_commander.roscpp_initialize(sys.argv)

		# Initialize the ROS node
		rospy.init_node('pick_place', anonymous=True)

		# Connect to the arm move group
		if planArm == "right_arm":
			self.arm = MoveGroupCommander('right_arm')
			self.hand = Gripper(1)
		else:
			self.arm = MoveGroupCommander('left_arm')
			self.hand = Gripper(0)

		rospy.sleep(1)
		self.hand.release()
		
		# Allow replanning to increase the odds of a solution
		self.arm.allow_replanning(True)
  def __init__(self, planning_group, joint_names):
    """Constructor"""
    super(MoveitCommanderMoveGroupState, self).__init__(outcomes=['reached', 'failed'],
                    input_keys=['target_joint_config'])

    self._planning_group = planning_group
    self._joint_names = joint_names
    Logger.loginfo("About to make mgc in init with group %s" % self._planning_group)
    self.group_to_move = MoveGroupCommander(self._planning_group)
    Logger.loginfo("finished making mgc in init.")

    self._done = False
    def __init__(self):

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

        arm = MoveGroupCommander(ARM_GROUP)

        arm.allow_replanning(True)

        rospy.sleep(2)

        pose = PoseStamped().pose

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

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

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

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

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

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

        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)
示例#12
0
	def run(self, cycle, verbose = 2):

		#enable robot if not already done
		enableProcess = subprocess.Popen(["rosrun", "baxter_tools", 
		"enable_robot.py", "-e"])
		enableProcess.wait()
		
		#start moveit servers. Since we do not know how long it will take them 
		#to start, pause for 15 seconds
		jointServer = subprocess.Popen(["rosrun", "baxter_interface", 
		"joint_trajectory_action_server.py"])
		planServer = subprocess.Popen(["roslaunch", "baxter_moveit_config", 
		"move_group.launch"])
		time.sleep(15)
		
		raw_input("press enter")
		
		try:
			#left = MoveGroupCommander("left_arm")
			right = MoveGroupCommander("right_arm")
			
			upRight = geometry_msgs.msg.Pose(position = geometry_msgs.msg.Point(
			x = 0.9667369015076861, y = -0.1190874231664102, z = -0.07489146157788866 ))
			
			#upLeft = geometry_msgs.msg.Pose(position = geometry_msgs.msg.Point(
			#x = 0.7, y = 0.35, z = 0.8))
			
			#downRight = geometry_msgs.msg.Pose(position = geometry_msgs.msg.Point(
			#x = 0.7, y = -0.45, z = 0.3))
			
			#downLeft = geometry_msgs.msg.Pose(position = geometry_msgs.msg.Point(
			#x = 0.7, y = 0.45, z = 0.3))
			
			right.set_pose_target(upRight)
			#left.set_pose_target(upLeft)
			right.go()
			#left.go()
			
			#right.set_pose_target(downRight)
			#left.set_pose_target(downLeft)
			
			right.go()
			#left.go()
			
		finally:
			#clean up - kill servers
			jointServer.kill()
			planServer.kill()
			
			sys.exit()
示例#13
0
def main():
    #Wait for the IK service to become available
    rospy.wait_for_service('compute_ik')
    rospy.init_node('service_query')
    
    #Create the function used to call the service
    compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
    
    while not rospy.is_shutdown():
        raw_input('Press [ Enter ]: ')
        
        #Construct the request
        request = GetPositionIKRequest()
        request.ik_request.group_name = "left_arm"
        request.ik_request.ik_link_name = "left_hand"
        request.ik_request.attempts = 20
        request.ik_request.pose_stamped.header.frame_id = "base"
        
        #Set the desired orientation for the end effector HERE
        request.ik_request.pose_stamped.pose.position.x = 0.5
        request.ik_request.pose_stamped.pose.position.y = 0.5
        request.ik_request.pose_stamped.pose.position.z = 0.3        
        request.ik_request.pose_stamped.pose.orientation.x = 0.0
        request.ik_request.pose_stamped.pose.orientation.y = 1.0
        request.ik_request.pose_stamped.pose.orientation.z = 0.0
        request.ik_request.pose_stamped.pose.orientation.w = 0.0
        
        try:
            #Send the request to the service
            response = compute_ik(request)
            
            #Print the response HERE
            print(response)
            group = MoveGroupCommander("left_arm")
            
            #this command tries to achieve a pose: which is position, orientation
            group.set_pose_target(request.ik_request.pose_stamped)

            #then, this command tries to achieve a position only.  orientation is airbitrary.
            group.set_position_target([0.5,0.5,0.3])
            group.go()
            
        except rospy.ServiceException, e:
            print "Service call failed: %s"%e
示例#14
0
class Robot:
    def __init__(self, config, debug=0):
        print "[INFO] Initialise Robot"
        self.DEBUG = debug
        # configuration
        self.config = config
        # initialise move groups
        self.arm = MoveGroupCommander(ARM_GROUP)
        self.gripper = MoveGroupCommander(GRIPPER_GROUP)
        # initialise pick and place manager
        if self.DEBUG is 1:
            # verbose mode
            self.pick_and_place = PickPlaceInterface(ARM_GROUP, GRIPPER_GROUP, False, True)
        else:
            # non verbose mode
            self.pick_and_place = PickPlaceInterface(ARM_GROUP, GRIPPER_GROUP, False, False)

        # tolerance: position (in m), orientation (in rad)
        self.arm.set_goal_position_tolerance(0.01)
        self.arm.set_goal_orientation_tolerance(0.1)

        self.place_manager = None
        self.pick_manager = None

        self.initialise_move_actions()
        self.start_position()

    """Initialise the place and pick manager.
    """
    def initialise_move_actions(self):
        self.place_manager = PlaceManager(self.arm, self.pick_and_place, self.config, self.DEBUG)
        self.pick_manager = PickManager(self.arm, self.pick_and_place, self.DEBUG)

    """Move robot arm to "start position".
    """
    def start_position(self):
        self.arm.set_named_target(START_POSITION)
        self.arm.go()
        rospy.sleep(2)
示例#15
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}
示例#16
0
    def __init__(self):
        group = MoveGroupCommander("arm")
        
        #group.set_orientation_tolerance([0.3,0.3,0,3])
        
        p = PoseStamped()
        p.header.frame_id = "/katana_base_link"
        p.pose.position.x = 0.4287
        #p.pose.position.y = -0.0847
        p.pose.position.y = 0.0
        p.pose.position.z = 0.4492
        
        
        q = quaternion_from_euler(2, 0, 0)
        
        p.pose.orientation.x = q[0]
        p.pose.orientation.y = q[1]
        p.pose.orientation.z = q[2]
        p.pose.orientation.w = q[3]
        


        
        print "Planning frame: " ,group.get_planning_frame()
        print "Pose reference frame: ",group.get_pose_reference_frame()
        
        #group.set_pose_reference_frame("katana_base_link")

        print "RPy target: 0,0,0"
        #group.set_rpy_target([0, 0, 0],"katana_gripper_tool_frame")
        #group.set_position_target([0.16,0,0.40], "katana_gripper_tool_frame")
        
        group.set_pose_target(p, "katana_gripper_tool_frame")
        
        group.go()
        print "Current rpy: " , group.get_current_rpy("katana_gripper_tool_frame")
示例#17
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)
示例#18
0
    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)

        rospy.init_node('moveit_demo')

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

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

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

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

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

        # Move to the named "straight_forward" pose stored in the SRDF file
        right_arm.set_named_target('straight_forward')
        right_arm.go()
        rospy.sleep(2)

        # Move back to the resting position at roughly 1/4 speed
        right_arm.set_named_target('resting')

        # Get back the planned trajectory
        traj = right_arm.plan()

        # Scale the trajectory speed by a factor of 0.25
        new_traj = scale_trajectory_speed(traj, 0.25)

        # Execute the new trajectory
        right_arm.execute(new_traj)
        rospy.sleep(1)

        # Exit MoveIt cleanly
        moveit_commander.roscpp_shutdown()

        # Exit the script
        moveit_commander.os._exit(0)
示例#19
0
positions = [
    0.2928262968941126, -0.2928262968941126, 0.2928262968941126,
    -0.2928262968941126, -0.2928262968941126, 0.2928262968941126,
    0.44286313484323736, -1.1781581860491328, 1.7121511388933512,
    1.0374444266562388, 1.5704395720709645, -2.698212672994671
]
#def default_grip(arm):

if __name__ == '__main__':

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

    scene = PlanningSceneInterface()
    robot = RobotCommander()
    arm = MoveGroupCommander("manipulator")
    eef = MoveGroupCommander("endeffector")
    rospy.sleep(1)
    #Start State Gripper
    group_variable_values = eef.get_current_joint_values()
    group_variable_values[0] = 0.0
    eef.set_joint_value_target(group_variable_values)
    plan2 = eef.plan()
    arm.execute(plan2)
    # clean the scene
    scene.remove_world_object("pole")
    scene.remove_world_object("table")
    scene.remove_world_object("part")

    # publish a demo scene
    k = PoseStamped()
示例#20
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.7

        # 设置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.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.4
        table_pose.pose.position.y = 0.0
        table_pose.pose.position.z = table_ground + table_size[2]
        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.5110620856285095, 0.32814791798591614, 0.5454912781715393,
            1.0146701335906982, 0.8498637080192566, -0.45695754885673523
        ]
        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):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)

        rospy.init_node('moveit_demo')

        self.gripper_opened = [rospy.get_param(GRIPPER_PARAM + "/max_opening")]
        self.gripper_closed = [rospy.get_param(GRIPPER_PARAM + "/min_opening")]
        self.gripper_neutral = [rospy.get_param(GRIPPER_PARAM + "/neutral")]

        self.gripper_tighten = rospy.get_param(GRIPPER_PARAM + "/tighten")

        # We need a tf listener to convert poses into arm reference base
        self.tf_listener = tf.TransformListener()

        # 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('target_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.04)
        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 = 3

        # Set a limit on the number of place attempts
        max_place_attempts = 3
        rospy.loginfo("Scaling for MoveIt timeout=" + str(
            rospy.get_param(
                '/move_group/trajectory_execution/allowed_execution_duration_scaling'
            )))

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

        # Give each of the scene objects a unique name
        #table_id = 'table'								  We also remove the table object in order to run a test
        #box1_id = 'box1'		                                                  These boxes are commented as we do not need them
        #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)                                               These boxes are commented as we do not need them
        #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 "arm_up" pose stored in the SRDF file
        rospy.loginfo("Set Arm: right_up")
        arm.set_named_target('right_up')
        if arm.go() != True:
            rospy.logwarn("  Go failed")
        rospy.sleep(2)

        # Move the gripper to the closed position
        rospy.loginfo("Set Gripper: Close " + str(self.gripper_closed))
        gripper.set_joint_value_target(self.gripper_closed)
        if gripper.go() != True:
            rospy.logwarn("  Go failed")
        rospy.sleep(2)

        # Move the gripper to the neutral position
        rospy.loginfo("Set Gripper: Neutral " + str(self.gripper_neutral))
        gripper.set_joint_value_target(self.gripper_neutral)
        if gripper.go() != True:
            rospy.logwarn("  Go failed")
        rospy.sleep(2)

        # Move the gripper to the open position
        rospy.loginfo("Set Gripper: Open " + str(self.gripper_opened))
        gripper.set_joint_value_target(self.gripper_opened)
        if gripper.go() != True:
            rospy.logwarn("  Go failed")
        rospy.sleep(2)

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

        # 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]                                                       commented for the same reasons as previously
        #box2_size = [0.05, 0.05, 0.15]

        # Set the target size [l, w, h]
        target_size = [
            0.018, 0.018, 0.018
        ]  #[0.02, 0.005, 0.12]	             original object dimensions in meters

        # 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.36
        #table_pose.pose.position.y = 0.0
        #table_pose.pose.position.z = table_ground + table_size[2] -0.08 / 2.0                #0.4+0.01/2 aka table_ground + table_size[2] + target_size[2] / 2.0
        #table_pose.pose.orientation.w = 1.0
        #scene.add_box(table_id, table_pose, table_size)

        #box1_pose = PoseStamped()						             These two blocks of code are commented as they assign postion to unwanted boxes
        #box1_pose.header.frame_id = REFERENCE_FRAME
        #box1_pose.pose.position.x = table_pose.pose.position.x - 0.04
        #box1_pose.pose.position.y = 0.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 = table_pose.pose.position.x - 0.06
        #box2_pose.pose.position.y = 0.2
        #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.03  #table_pose.pose.position.x - 0.03
        target_pose.pose.position.y = 0.1
        target_pose.pose.position.z = 0.4 + 0.01 + 0.018 - 0.08 / 2  #table_ground + table_size[2] + target_size[2] / 2.0 table_ground + table_size[2] + target_size[2] -0.08 / 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
        #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.03  #table_pose.pose.position.x - 0.03
        place_pose.pose.position.y = -0.15
        place_pose.pose.position.z = 0.4 + 0.01 + 0.018 - 0.08 / 2  #table_ground + table_size[2] + target_size[2] -0.08 / 2.0 0.4+0.01+0.018/2 aka 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],
                                  [target_size[1] - self.gripper_tighten])

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

        # Repeat until we succeed or run out of attempts
        while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
            rospy.loginfo("Pick attempt #" + str(n_attempts))
            for grasp in grasps:
                # Publish the grasp poses so they can be viewed in RViz
                self.gripper_pose_pub.publish(grasp.grasp_pose)
                rospy.sleep(0.2)

                result = arm.pick(target_id, grasps)
                if result == MoveItErrorCodes.SUCCESS:
                    break

            n_attempts += 1
            rospy.sleep(0.2)

        # If the pick was successful, attempt the place operation
        if result == MoveItErrorCodes.SUCCESS:
            rospy.loginfo("  Pick: Done!")
            # Generate valid place poses
            places = self.make_places(place_pose)

            success = False  #from here we are forcing the success cases by stting and making it always true it makes an error appear and the arm stays to 										 the middle position
            n_attempts = 0

            # Repeat until we succeed or run out of attempts
            while not success and n_attempts < max_place_attempts:  #while not has been removed   added = after operator <
                rospy.loginfo("Place attempt #" + str(n_attempts))
                for place in places:
                    # Publish the place poses so they can be viewed in RViz
                    self.gripper_pose_pub.publish(place)
                    rospy.sleep(0.2)

                    success = arm.place(target_id, place)
                    break
                    if success:
                        break

                n_attempts += 1
                rospy.sleep(0.2)

            if not success:
                rospy.logerr("Place operation failed after " +
                             str(n_attempts) + " attempts.")
            else:  #end of forcing
                rospy.loginfo("  Place: Done!")
        else:
            rospy.logerr("Pick operation failed after " + str(n_attempts) +
                         " attempts.")

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

        arm.set_named_target('resting')
        arm.go()

        # Open the gripper to the neutral position
        gripper.set_joint_value_target(self.gripper_neutral)
        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)

        # Initialize the ROS node
        rospy.init_node('moveit_constraints_demo', anonymous=True)

        robot = RobotCommander()

        # Connect to the arm move group
        arm = MoveGroupCommander(GROUP_NAME_ARM)

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

        # Increase the planning time since constraint planning can take a while
        arm.set_planning_time(5)

        # 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 some leeway in position(meters) and orientation (radians)
        arm.set_goal_position_tolerance(0.05)
        arm.set_goal_orientation_tolerance(0.1)

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

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

        # Plan and execute a trajectory to the goal configuration
        arm.go()
        rospy.sleep(1)

        # Open the gripper
        gripper.set_joint_value_target(GRIPPER_NEUTRAL)
        gripper.go()
        rospy.sleep(1)

        # Set an initial target pose with the arm up and to the right
        target_pose = PoseStamped()
        target_pose.header.frame_id = REFERENCE_FRAME
        target_pose.pose.position.x = 0.263803774718
        target_pose.pose.position.y = 0.295405791959
        target_pose.pose.position.z = 0.690438884208
        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 start state and target pose, then plan and execute
        arm.set_start_state(robot.get_current_state())
        arm.set_pose_target(target_pose, end_effector_link)
        arm.go()
        rospy.sleep(2)

        # Close the gripper
        gripper.set_joint_value_target(GRIPPER_CLOSED)
        gripper.go()
        rospy.sleep(1)

        # Store the current pose
        start_pose = arm.get_current_pose(end_effector_link)

        # Create a contraints list and give it a name
        constraints = Constraints()
        constraints.name = "Keep gripper horizontal"

        # Create an orientation constraint for the right gripper
        orientation_constraint = OrientationConstraint()
        orientation_constraint.header = start_pose.header
        orientation_constraint.link_name = arm.get_end_effector_link()
        orientation_constraint.orientation.w = 1.0
        orientation_constraint.absolute_x_axis_tolerance = 0.1
        orientation_constraint.absolute_y_axis_tolerance = 0.1
        orientation_constraint.absolute_z_axis_tolerance = 0.1
        orientation_constraint.weight = 1.0
        # q = quaternion_from_euler(0, 0, -1.57079633)
        # orientation_constraint.orientation.x = q[0]
        # orientation_constraint.orientation.y = q[1]
        # orientation_constraint.orientation.z = q[2]
        # orientation_constraint.orientation.w = q[3]

        # Append the constraint to the list of contraints
        constraints.orientation_constraints.append(orientation_constraint)

        # Set the path constraints on the arm
        arm.set_path_constraints(constraints)

        # Set a target pose for the arm
        target_pose = PoseStamped()
        target_pose.header.frame_id = REFERENCE_FRAME
        target_pose.pose.position.x = 0.39000848183
        target_pose.pose.position.y = 0.185900663329
        target_pose.pose.position.z = 0.732752341378
        target_pose.pose.orientation.w = 1

        # Set the start state and target pose, then plan and execute
        arm.set_start_state_to_current_state()
        arm.set_pose_target(target_pose, end_effector_link)
        arm.go()
        rospy.sleep(1)

        # Clear all path constraints
        arm.clear_path_constraints()

        # Open the gripper
        gripper.set_joint_value_target(GRIPPER_NEUTRAL)
        gripper.go()
        rospy.sleep(1)

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

        # Plan and execute a trajectory to the goal configuration
        arm.go()
        rospy.sleep(1)

        # Shut down MoveIt cleanly
        moveit_commander.roscpp_shutdown()

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

        # Initialize the ROS node
        rospy.init_node('moveit_demo', anonymous=True)
        
        robot = RobotCommander()

        # Connect to the right_arm move group
        right_arm = MoveGroupCommander(GROUP_NAME_ARM)
        
        # Initialize the move group for the right gripper
        right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
                                
        # Increase the planning time since contraint planning can take a while
        right_arm.set_planning_time(15)
                        
        # 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 some leeway in position(meters) and orientation (radians)
        right_arm.set_goal_position_tolerance(0.05)
        right_arm.set_goal_orientation_tolerance(0.1)
        
        # Get the name of the end-effector link
        end_effector_link = right_arm.get_end_effector_link()
        
        # Start in the "resting" configuration stored in the SRDF file
        right_arm.set_named_target('resting')
         
        # Plan and execute a trajectory to the goal configuration
        right_arm.go()
        rospy.sleep(1)
        
        # Open the gripper
        right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
        right_gripper.go()
        rospy.sleep(1)
        
        # Set an initial target pose with the arm up and to the right
        target_pose = PoseStamped()
        target_pose.header.frame_id = REFERENCE_FRAME
        target_pose.pose.position.x = 0.237012590198
        target_pose.pose.position.y = -0.0747191267505
        target_pose.pose.position.z = 0.901578401949
        target_pose.pose.orientation.w = 1.0
         
        # Set the start state and target pose, then plan and execute
        right_arm.set_start_state(robot.get_current_state())
        right_arm.set_pose_target(target_pose, end_effector_link)
        right_arm.go()
        rospy.sleep(2)
        
        # Close the gripper
        right_gripper.set_joint_value_target(GRIPPER_CLOSED)
        right_gripper.go()
        rospy.sleep(1)
        
        # Store the current pose
        start_pose = right_arm.get_current_pose(end_effector_link)
        
        # Create a contraints list and give it a name
        constraints = Constraints()
        constraints.name = "Keep gripper horizontal"
        
        # Create an orientation constraint for the right gripper 
        orientation_constraint = OrientationConstraint()
        orientation_constraint.header = start_pose.header
        orientation_constraint.link_name = right_arm.get_end_effector_link()
        orientation_constraint.orientation.w = 1.0
        orientation_constraint.absolute_x_axis_tolerance = 0.1
        orientation_constraint.absolute_y_axis_tolerance = 0.1
        orientation_constraint.absolute_z_axis_tolerance = 3.14
        orientation_constraint.weight = 1.0
        
        # Append the constraint to the list of contraints
        constraints.orientation_constraints.append(orientation_constraint)
          
        # Set the path constraints on the right_arm
        right_arm.set_path_constraints(constraints)
        
        # Set a target pose for the arm        
        target_pose = PoseStamped()
        target_pose.header.frame_id = REFERENCE_FRAME
        target_pose.pose.position.x = 0.173187824708
        target_pose.pose.position.y = -0.0159929871606
        target_pose.pose.position.z = 0.692596608605
        target_pose.pose.orientation.w = 1.0

        # Set the start state and target pose, then plan and execute
        right_arm.set_start_state_to_current_state()
        right_arm.set_pose_target(target_pose, end_effector_link)
        right_arm.go()
        rospy.sleep(1)
          
        # Clear all path constraints
        right_arm.clear_path_constraints()
        
        # Open the gripper
        right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
        right_gripper.go()
        rospy.sleep(1)
        
        # Return to the "resting" configuration stored in the SRDF file
        right_arm.set_named_target('resting')

        # Plan and execute a trajectory to the goal configuration
        right_arm.go()
        rospy.sleep(1)

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

        # Initialize the ROS node
        rospy.init_node('moveit_demo', anonymous=True)

        cartesian = rospy.get_param('~cartesian', True)

        # Connect to the arm move group
        arm = MoveGroupCommander('arm')

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

        # Set the right arm reference frame
        arm.set_pose_reference_frame('base_link')

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

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

        # Set an initial position for the arm
        start_position = [
            0.0, 0.5, -0.0074579719079, -1.67822729461, -3.1415174069, -1.1,
            3.1415174069
        ]

        # Set the goal pose of the end effector to the stored pose
        arm.set_joint_value_target(start_position)

        # Plan and execute a trajectory to the goal configuration
        arm.go()

        # Get the current pose so we can add it as a waypoint
        start_pose = arm.get_current_pose(end_effector_link).pose

        # Initialize the waypoints list
        waypoints = []

        # Set the first waypoint to be the starting pose
        if cartesian:
            # Append the pose to the waypoints list
            waypoints.append(start_pose)

        wpose = deepcopy(start_pose)

        # Move end effector to the right 0.3 meters
        wpose.position.y -= 0.3

        if cartesian:
            # Append the pose to the waypoints list
            waypoints.append(deepcopy(wpose))
        else:
            arm.set_pose_target(wpose)
            arm.go()
            rospy.sleep(1)

        # Move end effector up and back
        wpose.position.x -= 0.2
        wpose.position.z += 0.3

        if cartesian:
            # Append the pose to the waypoints list
            waypoints.append(deepcopy(wpose))
        else:
            arm.set_pose_target(wpose)
            arm.go()
            rospy.sleep(1)

        if cartesian:
            # Append the pose to the waypoints list
            waypoints.append(deepcopy(start_pose))
        else:
            arm.set_pose_target(start_pose)
            arm.go()
            rospy.sleep(1)

        if cartesian:
            fraction = 0.0
            maxtries = 100
            attempts = 0

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

            # Plan the Cartesian path connecting the waypoints
            while fraction < 1.0 and attempts < maxtries:
                (plan, fraction) = arm.compute_cartesian_path(
                    waypoints,  # waypoint poses
                    0.025,  # eef_step
                    0.0,  # jump_threshold
                    True)  # avoid_collisions

                # Increment the number of attempts
                attempts += 1

                # Print out a progress message
                if attempts % 10 == 0:
                    rospy.loginfo("Still trying after " + str(attempts) +
                                  " attempts...")

            # If we have a complete plan, execute the trajectory
            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.")

        # Shut down MoveIt cleanly
        moveit_commander.roscpp_shutdown()

        # Exit MoveIt
        moveit_commander.os._exit(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控制的机械臂中的 manipulator 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)

        # 设置目标位置所使用的参考坐标系
        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、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.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.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.19
        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.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

        # 控制机械臂运动到目标位置
        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.2
        target_pose2.pose.position.y = 0.25
        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)
示例#26
0
 def __init__(self):
     rospy.init_node('arm_tracker')
     
     rospy.on_shutdown(self.shutdown)
     
     # Maximum distance of the target before the arm will lower
     self.max_target_dist = 1.2
     
     # Arm length to center of gripper frame
     self.arm_length = 0.4
     
     # Distance between the last target and the new target before we move the arm
     self.last_target_threshold = 0.01
     
     # Distance between target and end-effector before we move the arm
     self.target_ee_threshold = 0.025
     
     # Initialize the move group for the right arm
     self.right_arm = MoveGroupCommander(GROUP_NAME_ARM)
     
     # Initialize the move group for the right gripper
     right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
     
     # Set the reference frame for pose targets
     self.reference_frame = REFERENCE_FRAME
     
     # Keep track of the last target pose
     self.last_target_pose = PoseStamped()
     
     # Set the right arm reference frame accordingly
     self.right_arm.set_pose_reference_frame(self.reference_frame)
                     
     # Allow replanning to increase the chances of a solution
     self.right_arm.allow_replanning(False)
             
     # Set a position tolerance in meters
     self.right_arm.set_goal_position_tolerance(0.05)
     
     # Set an orientation tolerance in radians
     self.right_arm.set_goal_orientation_tolerance(0.2)
     
     # What is the end effector link?
     self.ee_link = self.right_arm.get_end_effector_link()
     
     # Create the transform listener
     self.listener = tf.TransformListener()
     
     # Queue up some tf data...
     rospy.sleep(3)
     
     # Set the gripper target to closed position using a joint value target
     right_gripper.set_joint_value_target(GRIPPER_CLOSED)
      
     # Plan and execute the gripper motion
     right_gripper.go()
     rospy.sleep(1)
             
     # Subscribe to the target topic
     rospy.wait_for_message('/target_pose', PoseStamped)
     
     # Use queue_size=1 so we don't pile up outdated target messages
     self.target_subscriber = rospy.Subscriber('/target_pose', PoseStamped, self.update_target_pose, queue_size=1)
     
     rospy.loginfo("Ready for action!")
     
     while not rospy.is_shutdown():
         try:
             target = self.target
         except:
             rospy.sleep(0.5)
             continue
                     
         # Timestamp the target with the current time
         target.header.stamp = rospy.Time()
         
         # Get the target pose in the right_arm shoulder lift frame
         #target_arm = self.listener.transformPose('right_arm_shoulder_pan_link', target)
         target_arm = self.listener.transformPose('right_arm_base_link', target)
         
         # Convert the position values to a Python list
         p0 = [target_arm.pose.position.x, target_arm.pose.position.y, target_arm.pose.position.z]
          
         # Compute the distance between the target and the shoulder link
         dist_target_shoulder = euclidean(p0, [0, 0, 0])
                                      
         # If the target is too far away, then lower the arm
         if dist_target_shoulder > self.max_target_dist:
             rospy.loginfo("Target is too far away")
             self.right_arm.set_named_target('resting')
             self.right_arm.go()
             rospy.sleep(1)
             continue
         
         # Transform the pose to the base reference frame
         target_base = self.listener.transformPose(self.reference_frame, target)
         
         # Compute the distance between the current target and the last target
         p1 = [target_base.pose.position.x, target_base.pose.position.y, target_base.pose.position.z]
         p2 = [self.last_target_pose.pose.position.x, self.last_target_pose.pose.position.y, self.last_target_pose.pose.position.z]
                 
         dist_last_target = euclidean(p1, p2)
         
         # Move the arm only if we are far enough away from the previous target
         if dist_last_target < self.last_target_threshold:
             rospy.loginfo("Still close to last target")
             rospy.sleep(0.5)
             continue
         
         # Get the pose of the end effector in the base reference frame
         ee_pose = self.right_arm.get_current_pose(self.ee_link)
         
         # Convert the position values to a Python list
         p3 = [ee_pose.pose.position.x, ee_pose.pose.position.y, ee_pose.pose.position.z]
         
         # Compute the distance between the target and the end-effector
         dist_target = euclidean(p1, p3)
         
         # Only move the arm if we are far enough away from the target
         if dist_target < self.target_ee_threshold:
             rospy.loginfo("Already close enough to target")
             rospy.sleep(1)
             continue
         
         # We want the gripper somewhere on the line connecting the shoulder and the target.
         # Using a parametric form of the line, the parameter ranges from 0 to the
         # minimum of the arm length and the distance to the target.
         t_max = min(self.arm_length, dist_target_shoulder)
         
         # Bring it back 10% so we don't collide with the target
         t = 0.9 * t_max
         
         # Now compute the target positions from the parameter
         try:
             target_arm.pose.position.x *= (t / dist_target_shoulder)
             target_arm.pose.position.y *= (t / dist_target_shoulder)
             target_arm.pose.position.z *= (t / dist_target_shoulder)
         except:
             rospy.sleep(1)
             rospy.loginfo("Exception!")
             continue
         
         # Transform to the base_footprint frame
         target_ee = self.listener.transformPose(self.reference_frame, target_arm)
         
         # Set the target gripper orientation to be horizontal
         target_ee.pose.orientation.x = 0
         target_ee.pose.orientation.y = 0
         target_ee.pose.orientation.z = 0
         target_ee.pose.orientation.w = 1
         
         # Update the current start state
         self.right_arm.set_start_state_to_current_state()
         
         # Set the target pose for the end-effector
         self.right_arm.set_pose_target(target_ee, self.ee_link)
         
         # Plan and execute the trajectory
         success = self.right_arm.go()
         
         if success:
             # Store the current target as the last target
             self.last_target_pose = target
         
         # Pause a bit between motions to keep from locking up
         rospy.sleep(0.5)
示例#27
0
import sys
import rospy
import copy
import geometry_msgs.msg

from moveit_commander import MoveGroupCommander
from moveit_commander import roscpp_initialize, roscpp_shutdown

from math import sin, copysign

if __name__ == '__main__':
    print "--- Straight line gesture ---"
#     roscpp_initialize(sys.argv)
    rospy.init_node('straight_line', anonymous=True)

    right_arm = MoveGroupCommander("right_arm")
    start_pose = geometry_msgs.msg.Pose()
    start_pose.position.x = -0.0988490064784
    start_pose.position.y = 0.272349904278
    start_pose.position.z = 1.18864499931
    start_pose.orientation.x = 0.393751611087
    start_pose.orientation.y = 0.918424640162
    start_pose.orientation.z = -0.0150455838492
    start_pose.orientation.w = 0.0350639347048
#     start_pose.orientation.w = 0
#     start_pose.orientation.x = 0
#     start_pose.orientation.y = 1
#     start_pose.orientation.z = 0
#     start_pose.position.y = 0.0256415233819
#     start_pose.position.z = 1.25871460368
#     start_pose.position.x = 0.243500142238
示例#28
0
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)

    print ">>>>> remove scenes"
示例#29
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)
示例#30
0
import rospy
import copy
import geometry_msgs.msg
import moveit_msgs.msg

from moveit_commander import RobotCommander, MoveGroupCommander
from moveit_commander import PlanningSceneInterface, roscpp_initialize, roscpp_shutdown

from math import sin, copysign, sqrt, pi

if __name__ == "__main__":
    print "============ Dynamic hand gestures: Come"
    roscpp_initialize(sys.argv)
    rospy.init_node("pumpkin_planning", anonymous=True)

    right_arm = MoveGroupCommander("right_arm")
    display_trajectory_publisher = rospy.Publisher(
        "/move_group/display_planned_path", moveit_msgs.msg.DisplayTrajectory, queue_size=1
    )
    print right_arm.get_current_pose().pose

    start_pose = geometry_msgs.msg.Pose()
    start_pose.position.x = -0.102288932444
    start_pose.position.y = 0.0200830096926
    start_pose.position.z = 1.29036727185
    start_pose.orientation.x = 0.5
    start_pose.orientation.y = 0.5
    start_pose.orientation.z = 0.5
    start_pose.orientation.w = 0.5
    right_arm.set_pose_target(start_pose)
    plan_start = right_arm.plan()
示例#31
0
    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)

        # Initialize the ROS node
        rospy.init_node('moveit_demo', anonymous=True)

        robot = RobotCommander()

        # Connect to the right_arm move group
        right_arm = MoveGroupCommander(GROUP_NAME_ARM)

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

        # Increase the planning time since constraint planning can take a while
        right_arm.set_planning_time(15)

        # 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 some leeway in position(meters) and orientation (radians)
        right_arm.set_goal_position_tolerance(0.05)
        right_arm.set_goal_orientation_tolerance(0.1)

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

        # Start in the "resting" configuration stored in the SRDF file
        right_arm.set_named_target('right_arm_zero')

        # Plan and execute a trajectory to the goal configuration
        right_arm.go()
        rospy.sleep(1)

        # Open the gripper
        right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
        right_gripper.go()
        rospy.sleep(1)

        # Set an initial target pose with the arm up and to the right
        target_pose = PoseStamped()
        target_pose.header.frame_id = REFERENCE_FRAME
        target_pose.pose.position.x = 0.237012590198
        target_pose.pose.position.y = -0.0747191267505
        target_pose.pose.position.z = 0.901578401949
        target_pose.pose.orientation.w = 1.0

        # Set the start state and target pose, then plan and execute
        right_arm.set_start_state(robot.get_current_state())
        right_arm.set_pose_target(target_pose, end_effector_link)
        right_arm.go()
        rospy.sleep(2)

        # Close the gripper
        right_gripper.set_joint_value_target(GRIPPER_CLOSED)
        right_gripper.go()
        rospy.sleep(1)

        # Store the current pose
        start_pose = right_arm.get_current_pose(end_effector_link)

        # Create a contraints list and give it a name
        constraints = Constraints()
        constraints.name = "Keep gripper horizontal"

        # Create an orientation constraint for the right gripper
        orientation_constraint = OrientationConstraint()
        orientation_constraint.header = start_pose.header
        orientation_constraint.link_name = right_arm.get_end_effector_link()
        orientation_constraint.orientation.w = 1.0
        orientation_constraint.absolute_x_axis_tolerance = 0.1
        orientation_constraint.absolute_y_axis_tolerance = 0.1
        orientation_constraint.absolute_z_axis_tolerance = 3.14
        orientation_constraint.weight = 1.0

        # Append the constraint to the list of contraints
        constraints.orientation_constraints.append(orientation_constraint)

        # Set the path constraints on the right_arm
        right_arm.set_path_constraints(constraints)

        # Set a target pose for the arm
        target_pose = PoseStamped()
        target_pose.header.frame_id = REFERENCE_FRAME
        target_pose.pose.position.x = 0.173187824708
        target_pose.pose.position.y = -0.0159929871606
        target_pose.pose.position.z = 0.692596608605
        target_pose.pose.orientation.w = 1.0

        # Set the start state and target pose, then plan and execute
        right_arm.set_start_state_to_current_state()
        right_arm.set_pose_target(target_pose, end_effector_link)
        right_arm.go()
        rospy.sleep(1)

        # Clear all path constraints
        right_arm.clear_path_constraints()

        # Open the gripper
        right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
        right_gripper.go()
        rospy.sleep(1)

        # Return to the "resting" configuration stored in the SRDF file
        right_arm.set_named_target('right_arm_zero')

        # Plan and execute a trajectory to the goal configuration
        right_arm.go()
        rospy.sleep(1)

        # Shut down MoveIt cleanly
        moveit_commander.roscpp_shutdown()

        # Exit MoveIt
        moveit_commander.os._exit(0)
示例#32
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 dimensions of the scene objects [l, w, h]
        table_size = [0.2, 0.7, 0.01]
        # 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 = 1
        target_pose.pose.position.y = 0.7
        target_pose.pose.position.z = table_ground + table_size[
            2] + target_size[2] / 2.0
        target_pose.pose.orientation.w = 1
        # 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)
    def __init__(self):
        # 初始化move_group的API
        moveit_commander.roscpp_initialize(sys.argv)

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

        # 是否需要使用笛卡尔空间的运动规划
        cartesian = rospy.get_param('~cartesian', True)

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

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

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

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

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

        # 控制机械臂运动到之前设置的“forward”姿态
        arm.set_named_target('forward')
        arm.go()

        # 获取当前位姿数据最为机械臂运动的起始位姿
        start_pose = arm.get_current_pose(end_effector_link).pose

        # 初始化路点列表
        waypoints = []

        # 将初始位姿加入路点列表
        if cartesian:
            waypoints.append(start_pose)

        # 设置第二个路点数据,并加入路点列表
        # 第二个路点需要向后运动0.2米,向右运动0.2米
        wpose = deepcopy(start_pose)
        wpose.position.x -= 0.2
        wpose.position.y -= 0.2

        if cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            arm.set_pose_target(wpose)
            arm.go()
            rospy.sleep(1)

        # 设置第三个路点数据,并加入路点列表
        wpose.position.y += 0.1

        if cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            arm.set_pose_target(wpose)
            arm.go()
            rospy.sleep(1)

        # 设置第四个路点数据,回到初始位置,并加入路点列表
        if cartesian:
            waypoints.append(deepcopy(start_pose))
        else:
            arm.set_pose_target(start_pose)
            arm.go()
            rospy.sleep(1)

        if cartesian:
            fraction = 0.0  #路径规划覆盖率
            maxtries = 100  #最大尝试规划次数
            attempts = 0  #已经尝试规划次数

            # 设置机器臂当前的状态作为运动初始状态
            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.")

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

        # 关闭并退出moveit
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)
示例#34
0
def close_hand():
    msg = Float64()
    msg.data = -1.0
    for i in range(30):
        pub.publish(msg)
        rospy.sleep(0.1)

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

    # for for 5 sec
    rospy.sleep(5)

    rospy.loginfo("start program %f" % rospy.get_time())
    arm = MoveGroupCommander("ur5_arm")
    arm.set_planner_id('RRTConnectkConfigDefault')
    pub = rospy.Publisher("/r_gripper_controller/command", Float64, queue_size=1)
    client = actionlib.SimpleActionClient('arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
    client.wait_for_server()

    rospy.loginfo("init pose")
    msg = FollowJointTrajectoryGoal()
    msg.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.2)
    msg.trajectory.joint_names = ['ur5_arm_shoulder_pan_joint',
                                  'ur5_arm_shoulder_lift_joint',
                                  'ur5_arm_elbow_joint',
                                  'ur5_arm_wrist_1_joint',
                                  'ur5_arm_wrist_2_joint',
                                  'ur5_arm_wrist_3_joint']
    msg.trajectory.points.append(JointTrajectoryPoint(positions=[-1.57, -0.1745, -2.79, -1.57, 0, 0],
示例#35
0
文件: pick_test.py 项目: bjrhyqh/pick
import random
import actionlib
from moveit_commander import RobotCommander,MoveGroupCommander, PlanningSceneInterface, roscpp_initialize, roscpp_shutdown
from geometry_msgs.msg import PoseStamped
from std_msgs.msg import Float64,MultiArrayDimension
from control_msgs.msg import GripperCommandAction,FollowJointTrajectoryAction
from gazebo_ros_link_attacher.srv import Attach, AttachRequest, AttachResponse

if __name__=='__main__':

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

    scene = PlanningSceneInterface()
    robot = RobotCommander()
    arm =  MoveGroupCommander("manipulator")
    eef = MoveGroupCommander("gripper")
    rospy.sleep(1)


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

    client = actionlib.SimpleActionClient('gripper_controller/gripper_action', GripperCommandAction)
    if not client.wait_for_server(rospy.Duration(5.0)):
        rospy.logerr('Pick up action client not available!')
        rospy.signal_shutdown('Pick up action client not available!')

    # publish a demo scene
示例#36
0
#!/usr/bin/env python

import rospy

from moveit_commander import MoveGroupCommander

if __name__ == '__main__':

    #init_node()
    group = MoveGroupCommander("botharms")
    sleep_time = 0.2
    exec_vel = 1.0

    while True:
        # home
        group.set_max_velocity_scaling_factor(exec_vel * 0.2)
        group.set_joint_value_target(
            [-0.78, 0.78, 0.14, 0.0, 0.78, -0.78, 0.14, 0.0])
        group.go()
        rospy.sleep(sleep_time)

        group.set_max_velocity_scaling_factor(exec_vel)
        group.set_joint_value_target(
            [-1.5, 1.5, 0.14, 0.0, 1.5, -1.5, 0.14, 0.0])
        group.go()
        rospy.sleep(sleep_time)

        num = 0
        while num < 5:
            group.set_max_velocity_scaling_factor(exec_vel * 0.2)
            group.set_joint_value_target(
示例#37
0
    def __init__(self, debug=False, online = True, robot_frame="odom_combined"):
        
        self._tf = TransformListener()
        
        self._online = online
        
        # self.snd_handle = SoundClient()
        
        if self._online:
        
            #self._interface = ROSpeexInterface()
            #self._interface.init()
            self._speech_client = SpeechSynthesisClient_NICT()
            
        else:
            
            self.snd_handle = SoundClient()
        
        rospy.sleep(1)
        
        self.say('Hello world!')
        
        rospy.sleep(1)
        
        self._debug = debug
        self._robot_frame = robot_frame
        
        self._point_sub = rospy.Subscriber('nearest_face', PointStamped, self.face_cb)
        
        self._head_action_cl = actionlib.SimpleActionClient('/head_traj_controller/point_head_action', pr2_controllers_msgs.msg.PointHeadAction)
        self._torso_action_cl = actionlib.SimpleActionClient('/torso_controller/position_joint_action', pr2_controllers_msgs.msg.SingleJointPositionAction)
        
        self._left_arm = MoveGroupCommander("left_arm")
        self._right_arm = MoveGroupCommander("right_arm")
        
        print "r.f.: " + self._left_arm.get_pose_reference_frame()

        self.face = None
        # self.face_from = rospy.Time(0)
        self.face_last_dist = 0
        self.last_invited_at = rospy.Time(0)
        self._person_prob_left = 0
        
        self.l_home_pose = [0.283, 0.295, 0.537, -1.646, 0.468, -1.735]
        
        self.l_wave_1 = [-0.1, 0.6, 1.15, -1.7, -0.97, -1.6]
        self.l_wave_2 = [-0.1, 0.6, 1.15, 1.7, -0.97, 1.6]
        
        self.r_home_pose = [0.124, -0.481, 0.439, -1.548, 0.36, -0.035]
        self.r_advert = [0.521, -0.508, 0.845, -1.548, 0.36, -0.035]
        
        self.no_face_random_delay = None
        
        self._initialized = False
        
        self._timer = rospy.Timer(rospy.Duration(1.0), self.timer)
        
        
        self._move_buff = Queue.Queue()
        
        self._head_buff = Queue.Queue()
        
        self._move_thread = threading.Thread(target=self.movements)
        self._move_thread.daemon = True
        self._move_thread.start()
        
        self._head_thread = threading.Thread(target=self.head)
        self._head_thread.daemon = True
        self._head_thread.start()
        
        self.new_face = False
        self.face_last_dist = 0.0
        
        self.face_counter = 0
        
        self.actions = [self.stretchingAction,
                        self.introduceAction,
                        self.waveHandAction,
                        self.lookAroundAction,
                        self.lookAroundAction,
                        self.lookAroundAction,
                        self.advertAction,
                        self.numberOfFacesAction]
        
        self.goodbye_strings = ["Thanks for stopping by.",
                                "Enjoy the event.",
                                "See you later!",
                                "Have a nice day!"]
        
        self.invite_strings = ["Hello. It's nice to see you.",
                               "Come here and take some flyer.",
                               "I hope you are enjoying the event."]
        
        rospy.loginfo("Ready")
示例#38
0
class MoveIt(object):
    def __init__(self):
        moveit_commander.roscpp_initialize(sys.argv)
        self.scene = PlanningSceneInterface()
        self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty)

        self.arm = MoveGroupCommander("arm")
        # self.arm.set_goal_joint_tolerance(0.1)
        self.gripper = MoveGroupCommander("gripper")

        # already default
        self.arm.set_planner_id("RRTConnectkConfigDefault")

        self.end_effector_link = self.arm.get_end_effector_link()

        self.arm.allow_replanning(True)
        self.arm.set_planning_time(5)

        self.transformer = tf.TransformListener()

        rospy.sleep(2)  # allow some time for initialization of moveit

    def __del__(self):
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)

    def _open_gripper(self):
        joint_trajectory = JointTrajectory()
        joint_trajectory.header.stamp = rospy.get_rostime()
        joint_trajectory.joint_names = [
            "m1n6s200_joint_finger_1", "m1n6s200_joint_finger_2"
        ]

        joint_trajectory_point = JointTrajectoryPoint()
        joint_trajectory_point.positions = [0, 0]
        joint_trajectory_point.time_from_start = rospy.Duration(5.0)

        joint_trajectory.points.append(joint_trajectory_point)
        return joint_trajectory

    def _close_gripper(self):
        joint_trajectory = JointTrajectory()
        joint_trajectory.header.stamp = rospy.get_rostime()
        joint_trajectory.joint_names = [
            "m1n6s200_joint_finger_1", "m1n6s200_joint_finger_2"
        ]

        joint_trajectory_point = JointTrajectoryPoint()
        joint_trajectory_point.positions = [1.2, 1.2]
        joint_trajectory_point.time_from_start = rospy.Duration(5.0)

        joint_trajectory.points.append(joint_trajectory_point)
        return joint_trajectory

    # Template function for creating the Grasps
    def _create_grasps(self, x, y, z, z_max, rotation):
        grasps = []

        # You can create multiple grasps and add them to the grasps list
        grasp = Grasp()  # create a new grasp

        # Set the pre grasp posture (the fingers)
        grasp.pre_grasp_posture = self._open_gripper()
        # Set the grasp posture (the fingers)
        grasp.grasp_posture = self._close_gripper()
        # Set the position of where to grasp
        grasp.grasp_pose.pose.position.x = x
        grasp.grasp_pose.pose.position.y = y
        grasp.grasp_pose.pose.position.z = z
        # Set the orientation of the end effector
        q = quaternion_from_euler(math.pi, 0.0, rotation)
        grasp.grasp_pose.pose.orientation.x = q[0]
        grasp.grasp_pose.pose.orientation.y = q[1]
        grasp.grasp_pose.pose.orientation.z = q[2]
        grasp.grasp_pose.pose.orientation.w = q[3]
        grasp.grasp_pose.header.frame_id = "m1n6s200_link_base"
        # Set the pre_grasp_approach
        grasp.pre_grasp_approach.direction.header.frame_id = self.end_effector_link
        grasp.pre_grasp_approach.direction.vector.z = 1.0
        grasp.pre_grasp_approach.direction.vector.y = 0.0
        grasp.pre_grasp_approach.direction.vector.x = 0.0
        grasp.pre_grasp_approach.min_distance = 0.05
        grasp.pre_grasp_approach.desired_distance = 0.1
        # # Set the post_grasp_approach
        grasp.post_grasp_retreat.direction.header.frame_id = self.end_effector_link
        grasp.post_grasp_retreat.direction.vector.z = -1.0
        grasp.post_grasp_retreat.direction.vector.x = 0.0
        grasp.post_grasp_retreat.direction.vector.y = 0.0
        grasp.post_grasp_retreat.min_distance = 0.05
        grasp.post_grasp_retreat.desired_distance = 0.25

        grasp.grasp_pose.header.frame_id = "m1n6s200_link_base"  # setting the planning frame (Positive x is to the left, negative Y is to the front of the arm)

        grasps.append(
            grasp
        )  # add all your grasps in the grasps list, MoveIT will pick the best one

        for z_offset in np.arange(z + 0.02, z_max, 0.01):
            new_grasp = copy.deepcopy(grasp)
            new_grasp.grasp_pose.pose.position.z = z_offset
            grasps.append(new_grasp)
        return grasps

    # Template function, you can add parameters if needed!
    def grasp(self, x, y, z, z_max, rotation, size):
        print '******************* grasp'
        # Object distance:
        obj_dist = np.linalg.norm(np.asarray((x, y, z)))
        if obj_dist > 0.5:
            rospy.loginfo(
                "Object too far appart ({} m), skipping pick".format(obj_dist))
            return False

        # Add collision object, easiest to name the object, "object"
        object_pose = PoseStamped()
        object_pose.header.frame_id = "m1n6s200_link_base"
        object_pose.pose.position.x = x
        object_pose.pose.position.y = y
        object_pose.pose.position.z = z
        q = quaternion_from_euler(math.pi, 0.0, rotation)
        object_pose.pose.orientation.x = q[0]
        object_pose.pose.orientation.y = q[1]
        object_pose.pose.orientation.z = q[2]
        object_pose.pose.orientation.w = q[3]

        self.scene.add_box("object", object_pose, size)

        rospy.sleep(0.5)
        self.clear_octomap()
        rospy.sleep(1.0)

        # Create and return grasps
        # z += size[2]/2  # Focus on the top of the object only
        # z += size[2]/2 + 0.02  # Focus on the top of the object only

        grasps = self._create_grasps(x, y, z, z_max, rotation)
        print '******************************************************************************'
        result = self.arm.pick(
            'object', grasps)  # Perform pick on "object", returns result
        print '******************************************************************************'
        # self.move_to(x, y, z + 0.15, rotation)

        if result == MoveItErrorCodes.SUCCESS:
            print 'Success grasp'
            return True
        else:
            print 'Failed grasp'
            return False

    def clear_object(self, x, y, z, z_max, rotation, size):
        print '******************* clear_object'

        self.move_to_waypoint()
        success = self.grasp(x, y, z, z_max, rotation, size)

        if success:
            self.move_to_waypoint()
            success = self.move_to_drop_zone()
            if success:
                print 'success move to drop zone'
            else:
                print 'failed move to drop zone'

        self.open_fingers()
        self.remove_object()
        rospy.sleep(1.0)
        self.close_fingers()

        return success

    def open_fingers(self):
        print '******************* open_fingers'
        self.gripper.set_joint_value_target([0.0, 0.0])
        self.gripper.go(wait=True)
        rospy.sleep(2.0)

    def close_fingers(self):
        print '******************* close_fingers'
        self.gripper.set_joint_value_target([1.3, 1.3])
        self.gripper.go(wait=True)
        rospy.sleep(2.0)

    def move_to(self, x, y, z, rotation, frame_id="m1n6s200_link_base"):
        print '******************* move_to'
        q = quaternion_from_euler(math.pi, 0.0, rotation)
        pose = PoseStamped()
        pose.header.frame_id = frame_id
        pose.pose.position.x = x
        pose.pose.position.y = y
        pose.pose.position.z = z
        pose.pose.orientation.x = q[0]
        pose.pose.orientation.y = q[1]
        pose.pose.orientation.z = q[2]
        pose.pose.orientation.w = q[3]

        self.arm.set_pose_target(pose, self.end_effector_link)
        plan = self.arm.plan()
        success = self.arm.go(wait=True)
        self.arm.stop()
        self.arm.clear_pose_targets()
        return success

    def move_to_waypoint(self):
        print '******************* move_to_waypoint'
        return self.move_to(0.35, 0, 0.25, 1.57)

    def rtb(self):
        print '******************* rtb'
        self.move_to_drop_zone()
        # pose = PoseStamped()
        # pose.header.frame_id = 'base_footprint'
        # pose.pose.position.x = -0.191258927921
        # pose.pose.position.y = 0.1849306168113
        # pose.pose.position.z = 0.813729734732
        # pose.pose.orientation.x = -0.934842026356
        # pose.pose.orientation.y = 0.350652799078
        # pose.pose.orientation.z = -0.00168532388516
        # pose.pose.orientation.w = 0.0557688079539
        #
        # self.arm.set_pose_target(pose, self.end_effector_link)
        # plan = self.arm.plan()
        # self.arm.go(wait=True)
        # self.arm.stop()
        # self.arm.clear_pose_targets()

    def move_to_drop_zone(self):
        print '******************* move_to_drop_zone'
        pose = PoseStamped()
        pose.header.frame_id = "m1n6s200_link_base"
        pose.pose.position.x = 0.2175546259709541
        pose.pose.position.y = 0.18347985269448372
        pose.pose.position.z = 0.16757751444136426

        pose.pose.orientation.x = 0.6934210704552356
        pose.pose.orientation.y = 0.6589390059796749
        pose.pose.orientation.z = -0.23223137602833943
        pose.pose.orientation.w = -0.17616808290725341

        self.arm.set_pose_target(pose, self.end_effector_link)
        plan = self.arm.plan()
        success = self.arm.go(wait=True)
        self.arm.stop()
        self.arm.clear_pose_targets()
        return success

    def print_position(self):
        pose = self.arm.get_current_pose()
        self.transformer.waitForTransform("m1n6s200_link_base",
                                          "base_footprint", rospy.Time.now(),
                                          rospy.Duration(10))
        eef_pose = self.transformer.transformPose("m1n6s200_link_base", pose)

        orientation = eef_pose.pose.orientation
        orientation = [
            orientation.x, orientation.y, orientation.z, orientation.w
        ]
        euler = euler_from_quaternion(orientation)

        print "z:", eef_pose.pose.position.x
        print "y:", eef_pose.pose.position.y
        print "z:", eef_pose.pose.position.z
        print "yaw (degrees):", math.degrees(euler[2])

    def remove_object(self):
        self.scene.remove_attached_object(self.end_effector_link, "object")
        self.scene.remove_world_object("object")
        rospy.loginfo("Object removed")
示例#39
0
	def __init__(self):
		moveit_commander.roscpp_initialize(sys.argv)
		rospy.init_node('moveit_demo')
		rospy.Subscriber('/aruco_single/pose', PoseStamped, self.pose_cb,queue_size=1)
		scene=PlanningSceneInterface()
		self.scene_pub=rospy.Publisher('planning_scene',PlanningScene)
		self.colors=dict()
		rospy.sleep(1)
		arm=MoveGroupCommander('arm')
		#gripper=MoveGroupCommander('gripper')
		end_effector_link=arm.get_end_effector_link()
		arm.set_goal_position_tolerance(0.005)
		arm.set_goal_orientation_tolerance(0.025)
		arm.allow_replanning(True)
		#gripper.set_goal_position_tolerance(0.005)
		#gripper.set_goal_orientation_tolerance(0.025)
		#gripper.allow_replanning(True)
		
		reference_frame='base_link'
		arm.set_pose_reference_frame(reference_frame)
		arm.set_planning_time(5)
		
		
		#scene planning
		table_id='table'
		#cylinder_id='cylinder'
		
		box2_id='box2'
		target_id='target_object'
		#scene.remove_world_object(box1_id)
		scene.remove_world_object(box2_id)
		scene.remove_world_object(table_id)
		scene.remove_world_object(target_id)
		
		rospy.sleep(2)

		table_ground=0.59
		table_size=[0.5,1,0.01]
		#box1_size=[0.1,0.05,0.03]
		box2_size=[0.15,0.15,0.02]
		r_tool_size=[0.05,0.04,0.22]
		l_tool_size=[0.05,0.04,0.22]
		target_size=[0.05,0.05,0.1]
		
		
		
		

		table_pose=PoseStamped()
		table_pose.header.frame_id=reference_frame
		table_pose.pose.position.x=0.7
		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.7
		box1_pose.pose.position.y=-0.2
		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.55
		box2_pose.pose.position.y=-0.12
		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)	
		
		
		target_pose=PoseStamped()
		target_pose.header.frame_id=reference_frame
		target_pose.pose.position.x=0.58
		target_pose.pose.position.y=0.05
		target_pose.pose.position.z=table_ground+table_size[2]+target_size[2]/2.0
		target_pose.pose.orientation.x=0
		target_pose.pose.orientation.y=0
		target_pose.pose.orientation.z=0
		target_pose.pose.orientation.w=1
		scene.add_box(target_id,target_pose,target_size)	
		
		#left gripper
		l_p=PoseStamped()
		l_p.header.frame_id=end_effector_link
		l_p.pose.position.x=0.00
		l_p.pose.position.y=0.06
		l_p.pose.position.z=0.11
		l_p.pose.orientation.w=1
		scene.attach_box(end_effector_link,'l_tool',l_p,l_tool_size)	
		#right gripper
		r_p=PoseStamped()
		r_p.header.frame_id=end_effector_link
		r_p.pose.position.x=0.00
		r_p.pose.position.y= -0.06
		r_p.pose.position.z=0.11
		r_p.pose.orientation.w=1
		scene.attach_box(end_effector_link,'r_tool',r_p,r_tool_size)	
		
		#grasp
		g_p=PoseStamped()
		g_p.header.frame_id=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
		
		
		
		

		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.setColor('r_tool',0.8,0,0)
		self.setColor('l_tool',0.8,0,0)
		self.setColor('target_object',0,1,0)
		self.sendColors()
		
		#motion planning
		arm.set_named_target("initial_arm")
		arm.go()
		rospy.sleep(2)
		
		grasp_pose=target_pose
		grasp_pose.pose.position.x-=0.15
		#grasp_pose.pose.position.z=
		grasp_pose.pose.orientation.x=0
		grasp_pose.pose.orientation.y=0.707
		grasp_pose.pose.orientation.z=0
		grasp_pose.pose.orientation.w=0.707
		

       
		#arm.set_start_state_to_current_state()
		'''
		arm.set_pose_target(grasp_pose,end_effector_link)
		traj=arm.plan()
		arm.execute(traj)
		print arm.get_current_joint_values()
		
		'''
		pre_joint_state=[0.16588150906995922, 1.7060146047438647, -0.00961761728757362, 1.8614674591892713, -2.9556667436476847, 1.7432451233907822, 3.1415]
		arm.set_joint_value_target(pre_joint_state)
		traj=arm.plan()
		arm.execute(traj)
		rospy.sleep(2)
		arm.shift_pose_target(0,0.09,end_effector_link)
		arm.go()
		rospy.sleep(2)
		
		scene.attach_box(end_effector_link,target_id,g_p,target_size)	
		rospy.sleep(2)
		
		#grasping is over , from now is pouring
		arm.shift_pose_target(2,0.15,end_effector_link)
		arm.go()
		rospy.sleep(2)
		joint_state_1=arm.get_current_joint_values()
		joint_state_1[0]-=0.17
		arm.set_joint_value_target(joint_state_1)
		arm.go()
		rospy.sleep(1)
		joint_state_2=arm.get_current_joint_values()
		joint_state_2[6]-=1.8
		arm.set_joint_value_target(joint_state_2)
		arm.go()
		rospy.sleep(1)
		
		#print arm.get_current_joint_values()
		#pouring test
		for i in range(1,5):
			joint_state_2[6]+=0.087
			arm.set_joint_value_target(joint_state_2)
			arm.go()
			time.sleep(0.05)
			
			joint_state_2[6]-=0.087
			arm.set_joint_value_target(joint_state_2)
			arm.go()
			time.sleep(0.05)
			
			print i
		
		joint_state_2[6]+=1.8
		arm.set_joint_value_target(joint_state_2)
		arm.go()
		rospy.sleep(2)
		
		joint_state_1[0]+=0.17
		arm.set_joint_value_target(joint_state_1)
		arm.go()
		rospy.sleep(1)
		arm.shift_pose_target(2,-0.15,end_effector_link)
		arm.go()
		rospy.sleep(2)
		scene.remove_attached_object(end_effector_link,target_id)
		rospy.sleep(2)

		arm.set_named_target("initial_arm")
		arm.go()
		rospy.sleep(2)

		#remove and shut down
		scene.remove_attached_object(end_effector_link,'l_tool')
		rospy.sleep(1)
		scene.remove_attached_object(end_effector_link,'r_tool')
		rospy.sleep(1)
		moveit_commander.roscpp_shutdown()
		moveit_commander.os._exit(0)
示例#40
0
def main():
    rospy.init_node('gripper_teleop')
    wait_for_time()

    # Added by Xinyi:
    # Initialize Settings for the Test (Part 1)

    # Raise the torso to allow arm movement
    torso = fetch_api.Torso()
    torso.set_height(fetch_api.Torso.MAX_HEIGHT)

    # Set arm joints
    # Order: body ---> gripper
    # b: blue joint
    # g: gray joint
    # order: [b, g, b, g, b, g, b]
    # OPTION 1: Follow given trajectory
    arm = fetch_api.Arm()
    arm_initial_poses = [1.0, 1.25, 1.0, -2.25, -0.3, 1.0, 0.0]
    arm.move_to_joints(fetch_api.ArmJoints.from_list(arm_initial_poses))
    # for recording bag file
    # arm_viz_poses = [1.0, 1.25, 1.0, -2.25, 2.25, 2.25, 0.0]
    # arm.move_to_joints(fetch_api.ArmJoints.from_list(arm_viz_poses))

    # OPTION 2: Use motion planning
    # INITIAL_POSES = [
    #         ("shoulder_pan_joint", 1.0), ("shoulder_lift_joint", 1.2), ("upperarm_roll_joint", 1.0), ("elbow_flex_joint", -2.0), 
    #         ("forearm_roll_joint", -0.3), ("wrist_flex_joint", 1.2), ("wrist_roll_joint", 0.0)]
    # arm.move_to_joint_goal(INITIAL_POSES, replan=True)

    # INITIAL_POSES = PoseStamped()
    # INITIAL_POSES.header.frame_id = 'base_link'
    # INITIAL_POSES.pose.position.x = 0.6
    # INITIAL_POSES.pose.position.y = -0.1
    # INITIAL_POSES.pose.position.z = 0.3
    # INITIAL_POSES.pose.orientation.w = 1
    # error = arm.move_to_pose(INITIAL_POSES, replan=True)
    # if error is not None:
    #     arm.cancel_all_goals()
    #     rospy.logerr('FAIL TO MOVE TO INITIAL_POSES {}'.format(error))
    # else:
    #     rospy.loginfo('MOVED TO INITIAL_POSES')


    # Set the base position
    base = fetch_api.Base()
    # OPTION 1: Start from origin
    # move Fetch from the origin to the table
    # base.go_forward(1.6, 0.5)  # value (distance in meters)
    # base.turn(math.pi / 3)     # value (angle in degrees)
    # base.go_forward(3.3, 0.5)  # value (distance in meters)
    # base.turn(-math.pi / 3)

    # OPTION 2: Start right in front of the table
    base.align_with_x_axis_pos()
    base.go_forward(0.3, 0.5)


    # Avoid occlusion
    # OPTION 1: Freeze point cloud
    # freeze_pub = rospy.Publisher('/access_teleop/freeze_cloud', Bool, queue_size=5)
    # rospy.sleep(0.5)
    # publish freeze point cloud
    # freeze_pub.publish(Bool(data=True))

    # OPTION 2: Record a bag file of the surrounding
    head = fetch_api.Head()

    # tilt the head from -0.35 to 0.85
    # for i in range(6):
    #     head.look_at("base_link", HEAD_POSE[0], HEAD_POSE[1], -0.35 + i * 0.2)
    #     os.system("rosrun perception save_cloud world $(rospack find access_teleop)/bags/")
    # rospy.set_param("bag_file_refreshed", "true")
    # (end)

    move_group = MoveGroupCommander("arm")

    status_publisher = rospy.Publisher('/access_teleop/arm_status', String, queue_size=1)
    gripper_publisher = rospy.Publisher('/access_teleop/gripper_pixels', PX, queue_size=1)

    info_pubs = []
    for camera_name in camera_names:
        info_pubs.append([camera_name,
                          rospy.Publisher(camera_name + '/camera_info', camera_info_messages.CameraInfo, queue_size=1)])

    # Added by Xinyi
    # Debug: visualize camera positions
    vis_pub = rospy.Publisher('visualization_marker', Marker, queue_size=5)
    rospy.sleep(0.5)
    # (end)

    tb = TransformBroadcaster()

    camera_model = PinholeCameraModel()

    move_by_delta = MoveByDelta(arm, move_group)
    move_by_delta.start()

    move_by_absolute = MoveByAbsolute(arm, move_group, status_publisher)
    move_by_absolute.start()

    move_and_orient = MoveAndOrient(arm, move_group)
    move_and_orient.start()

    orient = Orient(arm, move_group)
    orient.start()

    # Added by Xinyi
    wrist_roll = WristRoll(arm, move_group)
    wrist_roll.start()

    base_switch_task = BaseSwitchTask(base)
    base_switch_task.start()

    head_tilt = HeadTilt(head)
    head_tilt.start()

    # Initialize Settings for the Test (Part 2)
    # Add the first test object to Gazebo
    os.system("$(rospack find access_teleop)/scripts/switch_object.sh " + "NONE" + " " + str(MODELS[0]))
    
    # Move arm joints to positions for test
    arm_test_poses = [1.0, 1.25, 1.0, -2.25, -0.3, 1.0, 0.0]
    arm.move_to_joints(fetch_api.ArmJoints.from_list(arm_test_poses))

    # Adjust the head to look at the table
    # OPTION 1: Tilt by angle
    # head.pan_tilt(0, math.pi / 2)

    # OPTION 2: Look at a point in space
    head.look_at("base_link", HEAD_POSE[0], HEAD_POSE[1], HEAD_POSE[2])

    rospy.sleep(0.5)
    # (end)

    rate = rospy.Rate(200)
    while not rospy.is_shutdown():
        publish_camera_transforms(tb, vis_pub)
        publish_camera_info(info_pubs)
        publish_gripper_pixels(camera_model, move_group, gripper_publisher)

        # publish freeze point cloud
        # freeze_pub.publish(Bool(data=True))

        # # get the current model position and publish a marker of current model position
        # model_state = rospy.ServiceProxy('gazebo/get_model_state', GetModelState)
        # model_pos = model_state(MODELS[current_model_idx], 'base_link')
        # if model_pos.success:
        #     marker = Marker(
        #             type=Marker.SPHERE,
        #             id=current_model_idx,
        #             pose=Pose(Point(model_pos.pose.position.x, model_pos.pose.position.y, model_pos.pose.position.z), 
        #                       Quaternion(model_pos.pose.orientation.x, model_pos.pose.orientation.y, model_pos.pose.orientation.z, model_pos.pose.orientation.w)),
        #             scale=Vector3(0.05, 0.05, 0.05),
        #             header=Header(frame_id='base_link'),
        #             color=ColorRGBA(1.0, 0.5, 1.0, 0.5))
        #     vis_pub.publish(marker)

        rate.sleep()
示例#41
0
#!/usr/bin/env python

import rospy

from moveit_commander import MoveGroupCommander

if __name__ == '__main__':

    #init_node()
    rospy.init_node('message', anonymous=True)
    group = MoveGroupCommander("manipulator")
    exec_vel = 0.5

    while True:

        rospy.loginfo("joint1 start")
        group.set_max_velocity_scaling_factor(exec_vel)
        group.set_joint_value_target([0.1, 0.1, 0.1, 0.1, 0.1, 0.1])
        group.go()
        rospy.loginfo("joint1 end")

        rospy.loginfo("pose1 start")
        group.set_max_velocity_scaling_factor(exec_vel)
        group.set_pose_target([0.5, -0.2, 0.2, 0.0, 1.0, 0.0])
        group.go()
        rospy.loginfo("pose1 end")

        rospy.loginfo("pose2 start")
        group.set_max_velocity_scaling_factor(exec_vel)
        group.set_pose_target([0.5, -0.2, 0.7, 0.0, 1.0, 0.0])
        group.go()
示例#42
0
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.

# Author: Kenji Miyake, Isaac Isao Saito

# This test script needs improved so that it becomes call-able from ROS test
# structure.

from geometry_msgs.msg import Pose, PoseStamped
from moveit_commander import MoveGroupCommander, conversions
import rospy

rospy.init_node("test_hironx_moveit")

rarm = MoveGroupCommander("right_arm")
larm = MoveGroupCommander("left_arm")

rarm_current_pose = rarm.get_current_pose().pose
larm_current_pose = larm.get_current_pose().pose


def _set_target_random(self):
    '''
    @type self: moveit_commander.MoveGroupCommander
    @param self: In this particular test script, the argument "self" is either
                 'rarm' or 'larm'.
    '''
    global current, current2, target
    current = self.get_current_pose()
    print "*current*", current
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from geometry_msgs.msg import PoseWithCovarianceStamped, Quaternion
from tf.transformations import quaternion_from_euler, euler_from_quaternion
from std_msgs.msg import String
from nav_msgs.msg import Odometry

#GROUP_NAME_GRIPPER = "NAME OF GRIPPER"
roscpp_initialize(sys.argv)
rospy.init_node('control_Husky_UR3', anonymous=True)
robot = RobotCommander()
scene = PlanningSceneInterface()

## 매니퓰레이터 변수 선언

group_name = "ur3_manipulator"
move_group = MoveGroupCommander(group_name)
FIXED_FRAME = 'world'

#모바일 로봇 변수
x = 0.0
y = 0.0
theta = 0.0


def newOdom(msg):
    global x
    global y
    global theta

    x = msg.pose.pose.position.x
    y = msg.pose.pose.position.y
示例#44
0
    return joint_out


JOINT_INDEX_BASE = 0
JOINT_INDEX_SHOULDER = 1
JOINT_INDEX_LOWER_ARM = 2
JOINT_INDEX_ELBOW = 3
JOINT_INDEX_UPPER_ARM = 4
JOINT_INDEX_WRIST_YAW = 5
JOINT_INDEX_WRIST_ROLL = 6

joint_state_topic = ['joint_states:=/j2s7s300/joint_states']
roscpp_initialize(joint_state_topic)
rospy.init_node('kinova_move', anonymous=False)

group = MoveGroupCommander('arm')

joint_target_init = [0, 2.8, 0.2, 1.4, 4.5, 1.8, 0.2]

joint_target_1 = rotate_joint(joint_target_init, 0.2, JOINT_INDEX_UPPER_ARM)
joint_target_2 = rotate_joint(joint_target_init, -0.2, JOINT_INDEX_UPPER_ARM)
joint_target_3 = rotate_joint(joint_target_init, math.pi / 4, JOINT_INDEX_BASE)
joint_target_4 = rotate_joint(joint_target_3, 0.2, JOINT_INDEX_UPPER_ARM)
joint_target_5 = rotate_joint(joint_target_3, -0.2, JOINT_INDEX_UPPER_ARM)
joint_targets = [
    joint_target_init, joint_target_1, joint_target_2, joint_target_3,
    joint_target_4, joint_target_5, joint_target_init
]

joint_targets = []
for i in range(0, 361, 45):
示例#45
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,
                                         queue_size=5)

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

        # 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 5 seconds per planning attempt
        right_arm.set_planning_time(60)

        # 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 = '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 "grasp" pose stored in the SRDF file
        right_arm.set_named_target('right_arm_up')
        right_arm.go()

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

        rospy.sleep(5)

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

        # 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]
        target_x = 0.135
        #target_y = -0.32

        target_y = -0.285290879999

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

        # 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 = target_x
        target_pose.pose.position.y = target_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 blue and the boxes orange
        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)

        # 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.18
        place_pose.pose.position.y = 0
        place_pose.pose.position.z = table_ground + table_size[
            2] + target_size[2] / 2.0
        place_pose.pose.orientation.w = 1.0

        p = PoseStamped()
        p.header.frame_id = "up1_footprint"
        p.pose.position.x = 0.12792118579
        p.pose.position.y = -0.285290879999
        p.pose.position.z = 0.120301181892

        p.pose.orientation.x = 0.0
        p.pose.orientation.y = 0.0
        p.pose.orientation.z = -0.706825181105
        p.pose.orientation.w = 0.707388269167

        right_gripper.set_pose_target(p.pose)

        # pick an object
        right_arm.allow_replanning(True)
        right_arm.allow_looking(True)
        right_arm.set_goal_tolerance(0.05)
        right_arm.set_planning_time(60)

        print "arm grasp"

        success = 0
        attempt = 0
        while not success:
            p_plan = right_arm.plan()
            attempt = attempt + 1
            print "Planning attempt: " + str(attempt)
            if p_plan.joint_trajectory.points != []:
                success = 1

        print "arm grasp"
        right_arm.execute(p_plan)

        rospy.sleep(5)

        right_gripper.set_joint_value_target(GRIPPER_GRASP)
        right_gripper.go()

        print "gripper closed"

        rospy.sleep(5)

        scene.attach_box(GRIPPER_FRAME, target_id)

        print "object attached"

        right_arm.set_named_target('right_arm_up')
        right_arm.go()

        print "arm up"

        rospy.sleep(1)

        # Shut down MoveIt cleanly
        moveit_commander.roscpp_shutdown()

        # Exit the script
        moveit_commander.os._exit(0)
示例#46
0
        controllers = [
            '/base_lift_controller', '/base_pan_controller',
            '/elbow_flex_controller', '/wrist_flex_controller'
        ]
        for controller in controllers:
            # 各コントローラ毎に,ゲインを指示するための service proxy (ROS の service を送るためのクライアント) を作成
            set_compliance_slope = rospy.ServiceProxy(
                controller + '/set_compliance_slope', SetComplianceSlope)
            temp = set_compliance_slope(sl)
    except rospy.ServiceException, e:
        print "Service call failed: %s" % e


if __name__ == '__main__':

    group = MoveGroupCommander("whole_arm")
    rospy.init_node("temp_arm_demo")

    target_position = [0.05, 0.0, 0.24]  # x, y, z それぞれの位置
    target_orientation = Quaternion(
        *quaternion_from_euler(0.0, 1.571, 0.0, 'sxyz'))
    # 位置と傾きを用いて姿勢を生成
    target_pose = Pose(Point(*target_position), target_orientation)
    group.set_pose_target(target_pose)
    # 姿勢への移動指示を実行
    set_slope(64)
    group.go()

    time.sleep(1)

    # 新たな姿勢を指定
示例#47
0
class PlannerAnnotationParser(AnnotationParserBase):
    """
    Parses the annotations files that contains the benchmarking
    information.
    """
    def __init__(self, path_to_annotation, path_to_data):
        super(PlannerAnnotationParser, self).__init__(path_to_annotation,
                                                      path_to_data)
        self.parse()

        self._load_scene()
        self._init_planning()
        self.benchmark()

    def check_results(self, results):
        """
        Returns the results from the planner, checking them against any eventual validation data
        (no validation data in our case).
        """
        return self.planner_data

    def _load_scene(self):
        """
        Loads the proper scene for the planner.
        It can be either a python static scene or bag containing an occupancy map.
        """
        scene = self._annotations["scene"]

        for element in scene:
            if element["type"] == "launch":
                self.play_launch(element["name"])
            elif element["type"] == "python":
                self.load_python(element["name"])
            elif element["type"] == "bag":
                self.play_bag(element["name"])
                for _ in range(150):
                    rospy.sleep(0.3)

        # wait for the scene to be spawned properly
        rospy.sleep(0.5)

    def _init_planning(self):
        """
        Initialises the needed connections for the planning.
        """

        self.group_id = self._annotations["group_id"]
        self.planners = self._annotations["planners"]
        self.scene = PlanningSceneInterface()
        self.robot = RobotCommander()
        self.group = MoveGroupCommander(self.group_id)
        self._marker_pub = rospy.Publisher('/visualization_marker_array',
                                           MarkerArray,
                                           queue_size=10,
                                           latch=True)
        self._planning_time_sub = rospy.Subscriber(
            '/move_group/result', MoveGroupActionResult,
            self._check_computation_time)
        rospy.sleep(1)

        self.group.set_num_planning_attempts(
            self._annotations["planning_attempts"])

        self.group.set_goal_tolerance(self._annotations["goal_tolerance"])
        self.group.set_planning_time(self._annotations["planning_time"])
        self.group.allow_replanning(self._annotations["allow_replanning"])

        self._comp_time = []

        self.planner_data = []

    def benchmark(self):
        for test_id, test in enumerate(self._annotations["tests"]):
            marker_position_1 = test["start_xyz"]
            marker_position_2 = test["goal_xyz"]

            self._add_markers(marker_position_1, "Start test \n sequence",
                              marker_position_2, "Goal")

            # Start planning in a given joint position
            joints = test["start_joints"]
            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:6] = joints
            current.joint_state.position = current_joints

            self.group.set_start_state(current)
            joints = test["goal_joints"]

            for planner in self.planners:
                if planner == "stomp":
                    planner = "STOMP"
                elif planner == "sbpl":
                    planner = "AnytimeD*"
                self.planner_id = planner
                self.group.set_planner_id(planner)
                self._plan_joints(
                    joints,
                    self._annotations["name"] + "-test_" + str(test_id))

        return self.planner_data

    def _add_markers(self, point, text1, point_2, text2):
        # add marker for start and goal pose of EE
        marker_array = MarkerArray()
        marker_1 = Marker()
        marker_1.header.frame_id = "world"
        marker_1.header.stamp = rospy.Time.now()
        marker_1.type = Marker.SPHERE
        marker_1.scale.x = 0.04
        marker_1.scale.y = 0.04
        marker_1.scale.z = 0.04
        marker_1.lifetime = rospy.Duration()

        marker_2 = deepcopy(marker_1)

        marker_1.color.g = 0.5
        marker_1.color.a = 1.0
        marker_1.id = 0
        marker_1.pose.position.x = point[0]
        marker_1.pose.position.y = point[1]
        marker_1.pose.position.z = point[2]
        marker_2.color.r = 0.5
        marker_2.color.a = 1.0
        marker_2.id = 1
        marker_2.pose.position.x = point_2[0]
        marker_2.pose.position.y = point_2[1]
        marker_2.pose.position.z = point_2[2]

        marker_3 = Marker()
        marker_3.header.frame_id = "world"
        marker_3.header.stamp = rospy.Time.now()
        marker_3.type = Marker.TEXT_VIEW_FACING
        marker_3.scale.z = 0.10
        marker_3.lifetime = rospy.Duration()

        marker_4 = deepcopy(marker_3)

        marker_3.text = text1
        marker_3.id = 2
        marker_3.color.g = 0.5
        marker_3.color.a = 1.0
        marker_3.pose.position.x = point[0]
        marker_3.pose.position.y = point[1]
        marker_3.pose.position.z = point[2] + 0.15
        marker_4.text = text2
        marker_4.id = 3
        marker_4.color.r = 0.5
        marker_4.color.a = 1.0
        marker_4.pose.position.x = point_2[0]
        marker_4.pose.position.y = point_2[1]
        marker_4.pose.position.z = point_2[2] + 0.15

        marker_array.markers = [marker_1, marker_2, marker_3, marker_4]

        self._marker_pub.publish(marker_array)
        rospy.sleep(1)

    def _plan_joints(self, joints, test_name):
        # plan to joint target and determine success
        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()
        plan_time = "N/A"
        total_joint_rotation = "N/A"
        comp_time = "N/A"

        plan_success = self._check_plan_success(plan)
        if plan_success:
            plan_time = self._check_plan_time(plan)
            total_joint_rotation = self._check_plan_total_rotation(plan)
            while not self._comp_time:
                rospy.sleep(0.5)
            comp_time = self._comp_time.pop(0)
        self.planner_data.append([
            self.planner_id, test_name,
            str(plan_success), plan_time, total_joint_rotation, comp_time
        ])

    @staticmethod
    def _check_plan_success(plan):
        if len(plan.joint_trajectory.points) > 0:
            return True
        else:
            return False

    @staticmethod
    def _check_plan_time(plan):
        # find duration of successful plan
        number_of_points = len(plan.joint_trajectory.points)
        time = plan.joint_trajectory.points[number_of_points -
                                            1].time_from_start.to_sec()
        return time

    @staticmethod
    def _check_plan_total_rotation(plan):
        # find total joint rotation in successful trajectory
        angles = [0, 0, 0, 0, 0, 0]
        number_of_points = len(plan.joint_trajectory.points)
        for i in range(number_of_points - 1):
            angles_temp = [
                abs(x - y)
                for x, y in zip(plan.joint_trajectory.points[i + 1].positions,
                                plan.joint_trajectory.points[i].positions)
            ]
            angles = [x + y for x, y in zip(angles, angles_temp)]

        total_angle_change = sum(angles)
        return total_angle_change

    def _check_computation_time(self, msg):
        # get computation time for successful plan to be found
        if msg.status.status == 3:
            self._comp_time.append(msg.result.planning_time)

    def _check_path_length(self, plan):
        # find distance travelled by end effector
        # not yet implemented
        return
示例#48
0
    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.colors = dict()

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

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

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

        # 当运动规划失败后,允许重新规划
        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()

        # 设置桌面的高度
        table_ground = 0.6

        # 设置table、box1和box2的三维尺寸
        table_size = [1.5, 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.0
        table_pose.pose.position.y = 0.5
        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.2
        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.1
        box2_pose.pose.position.y = 0.5
        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)

        # 将桌子设置成红色,两个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)

        # 设置目标物体的尺寸
        target_size = [0.02, 0.01, 0.12]

        # 设置目标物体的位置,位于桌面之上两个盒子之间
        target_pose = PoseStamped()
        target_pose.header.frame_id = REFERENCE_FRAME
        target_pose.pose.position.x = 0. - 2
        target_pose.pose.position.y = 0.4
        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
        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])

        # 追踪抓取成功与否,以及抓取的尝试次数
        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()

        # 控制夹爪回到张开的状态

        # 关闭并退出moveit
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)
示例#49
0
class DaArmServer:
    """The basic, design problem/tui agnostic arm server
    """
    gestures = {}
    pointing_height = 0.06
    grasp_height = 0.05
    drop_height = 0.07
    cruising_height = 0.1
    START_TOLERANCE = 0.05  # this is for moveit to check for change in joint angles before moving
    GOAL_TOLERANCE = 0.005
    moving = False
    paused = False
    move_group_state = "IDLE"
    last_joint_trajectory_goal = ""
    last_joint_trajectory_result = ""

    def __init__(self, num_planning_attempts=100, safe_zone=None):
        rospy.init_node("daarm_server", anonymous=True)
        self.safe_zone = safe_zone  # this is a fallback zone to drop a block on fail if nothing is passed: [{x,y},{xT,yT}]
        self.init_params()
        self.init_scene()
        self.init_publishers()
        self.init_subscribers()
        self.init_action_clients()
        self.init_service_clients()
        self.init_arm(num_planning_attempts)

    def init_arm(self, num_planning_attempts=700):
        rospy.set_param(
            "/move_group/trajectory_execution/allowed_start_tolerance",
            self.START_TOLERANCE)
        self.arm = MoveGroupCommander("arm")
        self.gripper = MoveGroupCommander("gripper")
        self.robot = RobotCommander()
        self.arm.set_num_planning_attempts(num_planning_attempts)
        self.arm.set_goal_position_tolerance(self.GOAL_TOLERANCE)
        self.arm.set_goal_orientation_tolerance(0.02)
        self.init_services()
        self.init_action_servers()

    def init_scene(self):
        world_objects = [
            "table", "tui", "monitor", "overHead", "wall", "farWall",
            "frontWall", "backWall", "blockProtector", "rearCamera"
        ]
        self.robot = RobotCommander()
        self.scene = PlanningSceneInterface()
        for obj in world_objects:
            self.scene.remove_world_object(obj)
        rospy.sleep(0.5)
        self.tuiPose = PoseStamped()
        self.tuiPose.header.frame_id = self.robot.get_planning_frame()
        self.tuiPose.pose.position = Point(0.0056, -0.343, -0.51)
        self.tuiDimension = (0.9906, 0.8382, 0.8836)
        self.overHeadPose = PoseStamped()
        self.overHeadPose.header.frame_id = self.robot.get_planning_frame()
        self.overHeadPose.pose.position = Point(0.0056, 0.0, 0.97)
        self.overHeadDimension = (0.9906, 0.8382, 0.05)
        self.blockProtectorPose = PoseStamped()
        self.blockProtectorPose.header.frame_id = self.robot.get_planning_frame(
        )
        self.blockProtectorPose.pose.position = Point(
            0.0056, -0.343, -0.51 + self.cruising_height)
        self.blockProtectorDimension = (0.9906, 0.8382, 0.8636)
        self.wallPose = PoseStamped()
        self.wallPose.header.frame_id = self.robot.get_planning_frame()
        self.wallPose.pose.position = Point(-0.858, -0.343, -0.3048)
        self.wallDimension = (0.6096, 2, 1.35)
        self.farWallPose = PoseStamped()
        self.farWallPose.header.frame_id = self.robot.get_planning_frame()
        self.farWallPose.pose.position = Point(0.9, -0.343, -0.3048)
        self.farWallDimension = (0.6096, 2, 3.35)
        self.frontWallPose = PoseStamped()
        self.frontWallPose.header.frame_id = self.robot.get_planning_frame()
        self.frontWallPose.pose.position = Point(0.0056, -0.85, -0.51)
        self.frontWallDimension = (1, 0.15, 4)
        self.backWallPose = PoseStamped()
        self.backWallPose.header.frame_id = self.robot.get_planning_frame()
        self.backWallPose.pose.position = Point(0.0056, 0.55, -0.51)
        self.backWallDimension = (1, 0.005, 4)
        self.rearCameraPose = PoseStamped()
        self.rearCameraPose.header.frame_id = self.robot.get_planning_frame()
        self.rearCameraPose.pose.position = Point(0.65, 0.45, -0.51)
        self.rearCameraDimension = (0.5, 0.5, 2)
        rospy.sleep(0.5)
        self.scene.add_box("tui", self.tuiPose, self.tuiDimension)
        self.scene.add_box("wall", self.wallPose, self.wallDimension)
        self.scene.add_box("farWall", self.farWallPose, self.farWallDimension)
        self.scene.add_box("overHead", self.overHeadPose,
                           self.overHeadDimension)
        self.scene.add_box("backWall", self.backWallPose,
                           self.backWallDimension)
        self.scene.add_box("frontWall", self.frontWallPose,
                           self.frontWallDimension)
        self.scene.add_box("rearCamera", self.rearCameraPose,
                           self.rearCameraDimension)

    def raise_table(self):
        #raises the table obstacle to protect blocks on the table during transport
        self.scene.remove_world_object("blockProtector")
        self.scene.add_box("blockProtector", self.blockProtectorPose,
                           self.blockProtectorDimension)

    def lower_table(self):
        #lowers the table to allow grasping into it
        self.scene.remove_world_object("blockProtector")

    def init_params(self):
        try:
            self.grasp_height = get_ros_param(
                "GRASP_HEIGHT", "Grasp height defaulting to 0.01")
            self.drop_height = get_ros_param("DROP_HEIGHT",
                                             "Drop height defaulting to 0.07")
            self.cruising_height = get_ros_param(
                "CRUISING_HEIGHT", "Cruising height defaulting to 0.1")
            self.pointing_height = get_ros_param(
                "POINT_HEIGHT", "Pointing height defaulting to 0.06")
        except ValueError as e:
            rospy.loginfo(e)

    def handle_param_update(self, message):
        self.init_params()

    def init_publishers(self):
        self.calibration_publisher = rospy.Publisher("/calibration_results",
                                                     CalibrationParams,
                                                     queue_size=1)
        self.action_belief_publisher = rospy.Publisher("/arm_action_beliefs",
                                                       String,
                                                       queue_size=1)
        rospy.sleep(0.5)

    def init_subscribers(self):
        self.joint_angle_subscriber = rospy.Subscriber(
            '/j2s7s300_driver/out/joint_angles', JointAngles,
            self.update_joints)

        self.joint_trajectory_subscriber = rospy.Subscriber(
            '/j2s7s300/follow_joint_trajectory/status', GoalStatusArray,
            self.update_joint_trajectory_state)

        self.joint_trajectory_goal_subscriber = rospy.Subscriber(
            '/j2s7s300/follow_joint_trajectory/goal',
            FollowJointTrajectoryActionGoal, self.update_joint_trajectory_goal)

        self.joint_trajectory_result_subscriber = rospy.Subscriber(
            '/j2s7s300/follow_joint_trajectory/result',
            FollowJointTrajectoryActionResult,
            self.update_joint_trajectory_result)

        self.finger_position_subscriber = rospy.Subscriber(
            '/j2s7s300_driver/out/finger_position', FingerPosition,
            self.update_finger_position)

        self.param_update_subscriber = rospy.Subscriber(
            "/param_update", String, self.handle_param_update)

        self.moveit_status_subscriber = rospy.Subscriber(
            '/move_group/status', GoalStatusArray,
            self.update_move_group_status)
        self.move_it_feedback_subscriber = rospy.Subscriber(
            '/move_group/feedback', MoveGroupActionFeedback,
            self.update_move_group_state)

        #Topic for getting joint torques
        rospy.Subscriber('/j2s7s300_driver/out/joint_torques', JointAngles,
                         self.monitorJointTorques)
        #Topic for getting cartesian force on end effector
        rospy.Subscriber('/j2s7s300_driver/out/tool_wrench',
                         geometry_msgs.msg.WrenchStamped,
                         self.monitorToolWrench)

    def init_action_servers(self):
        self.calibration_server = actionlib.SimpleActionServer(
            "calibrate_arm", CalibrateAction, self.calibrate, auto_start=False)
        self.calibration_server.start()
        self.move_block_server = actionlib.SimpleActionServer(
            "move_block",
            MoveBlockAction,
            self.handle_move_block,
            auto_start=False)
        self.move_block_server.start()
        #self.home_arm_server = actionlib.SimpleActionServer("home_arm", HomeArmAction, self.home_arm)
        self.move_pose_server = actionlib.SimpleActionServer(
            "move_pose",
            MovePoseAction,
            self.handle_move_pose,
            auto_start=False)
        self.move_pose_server.start()

    def init_services(self):
        self.home_arm_service = rospy.Service("/home_arm", ArmCommand,
                                              self.handle_home_arm)
        # emergency stop
        self.stop_arm_service = rospy.Service("/stop_arm", ArmCommand,
                                              self.handle_stop_arm)
        # stop and pause for a bit
        self.pause_arm_service = rospy.Service("/pause_arm", ArmCommand,
                                               self.handle_pause_arm)
        self.start_arm_service = rospy.Service("/restart_arm", ArmCommand,
                                               self.handle_restart_arm)

    def init_action_clients(self):
        # Action Client for joint control
        joint_action_address = '/j2s7s300_driver/joints_action/joint_angles'
        self.joint_action_client = actionlib.SimpleActionClient(
            joint_action_address, kinova_msgs.msg.ArmJointAnglesAction)
        rospy.loginfo('Waiting for ArmJointAnglesAction server...')
        self.joint_action_client.wait_for_server()
        rospy.loginfo('ArmJointAnglesAction Server Connected')

        # Service to move the gripper fingers
        finger_action_address = '/j2s7s300_driver/fingers_action/finger_positions'
        self.finger_action_client = actionlib.SimpleActionClient(
            finger_action_address, kinova_msgs.msg.SetFingersPositionAction)
        self.finger_action_client.wait_for_server()

        #

    def init_service_clients(self):
        self.is_simulation = False
        try:
            self.is_simulation = get_ros_param("IS_SIMULATION", "")
        except:
            self.is_simulation = False

        if self.is_simulation is True:
            # setup alternatives to jaco services for emergency stop, joint control, and finger control
            pass
        # Service to get TUI State
        rospy.wait_for_service('get_tui_blocks')
        self.get_block_state = rospy.ServiceProxy('get_tui_blocks', TuiState)

        # Service for homing the arm
        home_arm_service = '/j2s7s300_driver/in/home_arm'
        self.home_arm_client = rospy.ServiceProxy(home_arm_service, HomeArm)
        rospy.loginfo('Waiting for kinova home arm service')
        rospy.wait_for_service(home_arm_service)
        rospy.loginfo('Kinova home arm service server connected')

        # Service for emergency stop
        emergency_service = '/j2s7s300_driver/in/stop'
        self.emergency_stop = rospy.ServiceProxy(emergency_service, Stop)
        rospy.loginfo('Waiting for Stop service')
        rospy.wait_for_service(emergency_service)
        rospy.loginfo('Stop service server connected')

        # Service for restarting the arm
        start_service = '/j2s7s300_driver/in/start'
        self.restart_arm = rospy.ServiceProxy(start_service, Start)
        rospy.loginfo('Waiting for Start service')
        rospy.wait_for_service(start_service)
        rospy.loginfo('Start service server connected')

    def handle_start_arm(self, message):
        return self.restart_arm()

    def handle_stop_arm(self, message):
        return self.stop_motion()

    def handle_pause_arm(self, message):
        self.stop_motion(home=True, pause=True)
        return str(self.paused)

    def handle_restart_arm(self, message):
        self.restart_arm()
        self.paused = False
        return str(self.paused)

    def handle_home_arm(self, message):
        try:
            status = self.home_arm()
            return json.dumps(status)
        except rospy.ServiceException as e:
            rospy.loginfo("Homing arm failed")

    def home_arm(self):
        # send the arm home
        # for now, let's just use the kinova home
        #self.home_arm_client()
        self.home_arm_kinova()
        return "done"

    def custom_home_arm(self):
        angles_set = [
            629.776062012, 150.076568694, -0.13603515923, 29.8505859375,
            0.172727271914, 212.423721313, 539.743164062
        ]
        goal = kinova_msgs.msg.ArmJointAnglesGoal()

        goal.angles.joint1 = angles_set[0]
        goal.angles.joint2 = angles_set[1]
        goal.angles.joint3 = angles_set[2]
        goal.angles.joint4 = angles_set[3]
        goal.angles.joint5 = angles_set[4]
        goal.angles.joint6 = angles_set[5]
        goal.angles.joint7 = angles_set[6]

        self.joint_action_client.send_goal(goal)

    def home_arm_kinova(self):
        """Takes the arm to the kinova default home if possible
        """
        # self.arm.set_named_target("Home")
        angles_set = map(np.deg2rad, [
            629.776062012, 150.076568694, -0.13603515923, 29.8505859375,
            0.172727271914, 212.423721313, 269.743164062
        ])
        self.arm.clear_pose_targets()
        try:
            self.arm.set_joint_value_target(angles_set)
        except MoveItCommanderException as e:
            pass  #stupid bug in movegroupcommander wrapper throws an exception when trying to set joint angles
        try:
            self.arm.go()
            return "successful home"
        except:
            return "failed to home"

    # This callback function monitors the Joint Torques and stops the current execution if the Joint Torques exceed certain value
    def monitorJointTorques(self, torques):
        if abs(torques.joint1) > 1:
            return
            #self.emergency_stop() #Stop arm driver
            #rospy.sleep(1.0)
            #self.group.stop() #Stop moveit execution

    # This callback function monitors the Joint Wrench and stops the current
    # execution if the Joint Wrench exceeds certain value
    def monitorToolWrench(self, wrenchStamped):
        return
        #toolwrench = abs(wrenchStamped.wrench.force.x**2 + wrenchStamped.wrench.force.y**2 + wrenchStamped.wrench.force.z**2)
        ##print toolwrench
        #if toolwrench > 100:
        #    self.emergency_stop()  # Stop arm driver

    def move_fingers(self, finger1_pct, finger2_pct, finger3_pct):
        finger_max_turn = 6800
        goal = kinova_msgs.msg.SetFingersPositionGoal()
        goal.fingers.finger1 = float((finger1_pct / 100.0) * finger_max_turn)
        goal.fingers.finger2 = float((finger2_pct / 100.0) * finger_max_turn)
        goal.fingers.finger3 = float((finger3_pct / 100.0) * finger_max_turn)

        self.finger_action_client.send_goal(goal)
        if self.finger_action_client.wait_for_result(rospy.Duration(5.0)):
            return self.finger_action_client.get_result()
        else:
            self.finger_action_client.cancel_all_goals()
            rospy.loginfo('the gripper action timed-out')
            return None

    def move_joint_angles(self, angle_set):
        goal = kinova_msgs.msg.ArmJointAnglesGoal()

        goal.angles.joint1 = angle_set[0]
        goal.angles.joint2 = angle_set[1]
        goal.angles.joint3 = angle_set[2]
        goal.angles.joint4 = angle_set[3]
        goal.angles.joint5 = angle_set[4]
        goal.angles.joint6 = angle_set[5]
        goal.angles.joint7 = angle_set[6]

        self.joint_action_client.send_goal(goal)
        if self.joint_action_client.wait_for_result(rospy.Duration(20.0)):
            return self.joint_action_client.get_result()
        else:
            print('        the joint angle action timed-out')
            self.joint_action_client.cancel_all_goals()
            return None

    def handle_move_block(self, message):
        """msg format: {id: int,
                        source: Point {x: float,y: float},
                        target: Point {x: float, y: float}
        """
        print(message)

        pick_x = message.source.x
        pick_y = message.source.y
        pick_x_threshold = message.source_x_tolerance
        pick_y_threshold = message.source_y_tolerance
        block_id = message.id

        place_x = message.target.x
        place_y = message.target.y
        place_x_threshold = message.target_x_tolerance
        place_y_threshold = message.target_y_tolerance
        self.move_block(block_id, pick_x, pick_y, pick_x_threshold,
                        pick_y_threshold, place_x, place_y, place_x_threshold,
                        place_y_threshold, message.block_size)

    def handle_pick_failure(self, exception):
        rospy.loginfo("Pick failed, going home.")
        self.open_gripper()
        self.home_arm()
        raise exception

    def handle_place_failure(self, safe_zone, block_size, exception):
        #should probably figure out if I'm holding the block first so it doesn't look weird
        #figure out how to drop the block somewhere safe
        #pass none and none if you are certain you haven't picked up a block yet
        if safe_zone is None and block_size is None:
            self.home_arm()
            raise exception
        rospy.loginfo("HANDLING PLACE FAILURE")
        block_response = json.loads(self.get_block_state('tuio').tui_state)
        current_block_state = block_response
        drop_location = self.calculate_drop_location(
            safe_zone[0]['x'],
            safe_zone[0]['y'],
            safe_zone[1]['x_tolerance'],
            safe_zone[1]['y_tolerance'],
            current_block_state,
            block_size,
            num_attempts=1000)
        try:
            arm_drop_location = tuio_to_arm(drop_location.x, drop_location.y,
                                            get_tuio_bounds(),
                                            get_arm_bounds())
            rospy.loginfo("panic arm drop: " + str(arm_drop_location))
            self.place_block(
                Point(arm_drop_location[0], arm_drop_location[1], 0))
        except Exception as e:
            rospy.loginfo(
                "ERROR: Cannot panic place the block! Get ready to catch it!")
            self.open_gripper()
        self.home_arm()
        raise exception

    def get_candidate_blocks(self, block_id, pick_x, pick_y, pick_x_tolerance,
                             pick_y_tolerance):
        block_response = json.loads(self.get_block_state('tuio').tui_state)
        current_block_state = block_response

        candidate_blocks = []
        print("looking for ", block_id, " ", pick_x, pick_y, pick_x_tolerance,
              pick_y_tolerance)
        # get candidate blocks -- blocks with the same id and within the error bounds/threshold given
        print(current_block_state)
        for block in current_block_state:
            print(
                block,
                self.check_block_bounds(block['x'], block['y'], pick_x, pick_y,
                                        pick_x_tolerance, pick_y_tolerance))
            if block['id'] == block_id and self.check_block_bounds(
                    block['x'], block['y'], pick_x, pick_y, pick_x_tolerance,
                    pick_y_tolerance):
                candidate_blocks.append(block)
        return candidate_blocks

    def move_block(self,
                   block_id,
                   pick_x,
                   pick_y,
                   pick_x_tolerance,
                   pick_y_tolerance,
                   place_x,
                   place_y,
                   place_x_tolerance,
                   place_y_tolerance,
                   block_size=None,
                   safe_zone=None,
                   pick_tries=2,
                   place_tries=1,
                   point_at_block=True):

        if block_size is None:
            block_size = get_ros_param('DEFAULT_BLOCK_SIZE')
        block_response = json.loads(self.get_block_state('tuio').tui_state)
        current_block_state = block_response

        candidate_blocks = []
        print("looking for ", block_id, " ", pick_x, pick_y, pick_x_tolerance,
              pick_y_tolerance)
        # get candidate blocks -- blocks with the same id and within the error bounds/threshold given
        print(current_block_state)
        # check for a drop location before trying to pick, do this before checking source to prevent cases where we go for a block user
        # moved while we are checking for a drop location
        drop_location = self.calculate_drop_location(place_x,
                                                     place_y,
                                                     place_x_tolerance,
                                                     place_y_tolerance,
                                                     current_block_state,
                                                     block_size,
                                                     num_attempts=2000)

        if drop_location == None:
            self.handle_place_failure(
                None, None,
                Exception("no room in the target zone to drop the block"))

        # here we are actually building a set of candidate blocks to pick
        for block in current_block_state:
            print(
                block,
                self.check_block_bounds(block['x'], block['y'], pick_x, pick_y,
                                        pick_x_tolerance, pick_y_tolerance))
            if block['id'] == block_id and self.check_block_bounds(
                    block['x'], block['y'], pick_x, pick_y, pick_x_tolerance,
                    pick_y_tolerance):
                candidate_blocks.append(block)

        # select best block to pick and pick up
        pick_location = None
        if len(candidate_blocks) < 1:
            raise Exception("no block of id " + str(block_id) +
                            " found within the source zone")

        elif len(candidate_blocks) == 1:
            pick_location = Point(candidate_blocks[0]['x'],
                                  candidate_blocks[0]['y'], 0)
        else:
            pick_location = Point(*self.find_most_isolated_block(
                candidate_blocks, current_block_state))

        if point_at_block == True:
            try:
                arm_pick_location = tuio_to_arm(pick_location.x,
                                                pick_location.y,
                                                get_tuio_bounds(),
                                                get_arm_bounds())
                arm_drop_location = tuio_to_arm(drop_location.x,
                                                drop_location.y,
                                                get_tuio_bounds(),
                                                get_arm_bounds())
                self.close_gripper()
                self.point_at_block(location=Point(arm_pick_location[0],
                                                   arm_pick_location[1], 0))
                self.point_at_block(location=Point(arm_drop_location[0],
                                                   arm_drop_location[1], 0))
                self.home_arm()
            except Exception as e:
                rospy.loginfo(str(e))
                rospy.loginfo("failed trying to point at block. giving up.")
                self.home_arm()
            self.move_block_server.set_succeeded(
                MoveBlockResult(drop_location))
            return

        for i in range(pick_tries):
            try:
                arm_pick_location = tuio_to_arm(pick_location.x,
                                                pick_location.y,
                                                get_tuio_bounds(),
                                                get_arm_bounds())
                self.pick_block(location=Point(arm_pick_location[0],
                                               arm_pick_location[1], 0),
                                check_grasp=True)
                break
            except Exception as e:
                if i < pick_tries - 1:
                    rospy.loginfo("pick failed and trying again..." + str(e))
                else:
                    rospy.loginfo("pick failed and out of attempts..." +
                                  str(e))
                    self.handle_pick_failure(e)

        if safe_zone == None:
            if self.safe_zone == None:
                safe_zone = [{
                    'x': pick_x,
                    'y': pick_y
                }, {
                    'x_tolerance': pick_x_tolerance,
                    'y_tolerance': pick_y_tolerance
                }]
            else:
                safe_zone = self.safe_zone

        # calculate drop location

        block_response = json.loads(self.get_block_state('tuio').tui_state)
        current_block_state = block_response
        drop_location = self.calculate_drop_location(place_x,
                                                     place_y,
                                                     place_x_tolerance,
                                                     place_y_tolerance,
                                                     current_block_state,
                                                     block_size,
                                                     num_attempts=2000)
        if drop_location == None:
            self.handle_place_failure(
                safe_zone, block_size,
                Exception("no room in the target zone to drop the block"))
        rospy.loginfo("tuio drop" + str(drop_location))
        for i in range(place_tries):
            try:
                arm_drop_location = tuio_to_arm(drop_location.x,
                                                drop_location.y,
                                                get_tuio_bounds(),
                                                get_arm_bounds())
                rospy.loginfo("arm drop: " + str(arm_drop_location))
                self.place_block(
                    Point(arm_drop_location[0], arm_drop_location[1], 0))
                break
            except Exception as e:
                if i < place_tries - 1:
                    rospy.loginfo("place failed and trying again..." + str(e))
                else:
                    rospy.loginfo("place failed and out of attempts..." +
                                  str(e))
                    # check to see if we've defined a safe zone to drop the blocks

                    self.handle_place_failure(safe_zone, block_size, e)

        # assume success if we made it this far
        self.move_block_server.set_succeeded(MoveBlockResult(drop_location))

    # check if a certain x, y position is within the bounds of another x,y position
    @staticmethod
    def check_block_bounds(x, y, x_origin, y_origin, x_threshold, y_threshold):
        if x <= x_origin + x_threshold and x >= x_origin - x_threshold \
                and y <= y_origin + y_threshold and y >= y_origin - y_threshold:
            return True
        return False

    # calculate the best location to drop the block
    def calculate_drop_location(self,
                                x,
                                y,
                                x_threshold,
                                y_threshold,
                                blocks,
                                block_size,
                                num_attempts=1000):
        attempt = 0
        x_original, y_original = x, y
        while (attempt < num_attempts):
            valid = True
            for block in blocks:
                if self.check_block_bounds(block['x'], block['y'], x, y,
                                           0.8 * block_size, block_size):
                    valid = False
                    break
            if valid:
                return Point(x, y, 0)
            else:
                x = random.uniform(x_original - x_threshold,
                                   x_original + x_threshold)
                y = random.uniform(y_original - y_threshold,
                                   y_original + y_threshold)
            attempt += 1
        #if none found in num_attempts, return none
        return None

    # candidates should have more than one element in it
    @staticmethod
    def find_most_isolated_block(candidates, all_blocks):
        print(candidates)
        min_distances = []  # tuples of candidate, distance to closest block
        for candidate in candidates:
            min_dist = -1
            for block in all_blocks:
                if block['x'] == candidate['x'] and block['y'] == candidate[
                        'y']:
                    continue
                else:
                    dist = DaArmServer.block_dist(candidate, block)
                    if min_dist == -1 or dist < min_dist:
                        min_dist = dist
            min_distances.append([candidate, min_dist])
        most_isolated, _ = max(min_distances, key=lambda x: x[
            1])  # get most isolated candidate, and min_distance
        return most_isolated['x'], most_isolated['y'], 0

    @staticmethod
    def block_dist(block_1, block_2):
        return np.sqrt((block_2['x'] - block_1['x'])**2 +
                       (block_2['y'] - block_1['y'])**2)

    def update_finger_position(self, message):
        self.finger_positions = [
            message.finger1, message.finger2, message.finger3
        ]

    def check_grasp(self):
        closed_pos = 0.95 * 6800
        distance_from_closed = 0
        for fp in self.finger_positions:
            distance_from_closed += (closed_pos - fp)**2
        if np.sqrt(distance_from_closed
                   ) > 130:  #this is just some magic number for now
            return True  #if the fingers aren't fully closed, then grasp is good
        else:
            return False

    def open_gripper(self, delay=0):
        """open the gripper ALL THE WAY, then delay
        """
        if self.is_simulation is True:
            self.gripper.set_named_target("Open")
            self.gripper.go()
        else:
            try:
                rospy.loginfo("Opening Gripper!!")
                self.move_fingers(50, 50, 50)
            except Exception as e:
                rospy.loginfo("Caught it!!" + str(e))
        rospy.sleep(delay)

    def close_gripper(self, delay=0):
        """close the gripper ALL THE WAY, then delay
        """
        # self.gripper.set_named_target("Close")
        # self.gripper.go()
        try:
            rospy.loginfo("Closing Gripper!!")
            self.move_fingers(95, 95, 95)
        except Exception as e:
            rospy.loginfo("Caught it!!" + str(e))
        rospy.sleep(delay)

    def handle_gesture(self, gesture):
        # lookup the gesture from a table? or how about a message?
        # one possibility, object that contains desired deltas in pos/orientation
        # as well as specify the arm or gripper choice
        pass

    def handle_move_pose(self, message):
        # takes a geometry_msgs/Pose message
        self.move_arm_to_pose(message.target.position,
                              message.target.orientation,
                              action_server=self.move_pose_server)
        self.move_pose_server.set_succeeded()

    def check_plan_result(self, target_pose, cur_pose):
        #we'll do a very lenient check, this is to find failures, not error
        #also only checking position, not orientation
        rospy.loginfo("checking pose:" + str(target_pose) + str(cur_pose))
        if np.abs(target_pose.pose.position.x -
                  cur_pose.pose.position.x) > self.GOAL_TOLERANCE * 2:
            print("x error too far")
            return False
        if np.abs(target_pose.pose.position.y -
                  cur_pose.pose.position.y) > self.GOAL_TOLERANCE * 2:
            print("y error too far")
            return False
        if np.abs(target_pose.pose.position.z -
                  cur_pose.pose.position.z) > self.GOAL_TOLERANCE * 2:
            print("z error too far")
            return False
        print("tolerable error")
        return True

    # expects cooridinates for position to be in arm space
    def move_arm_to_pose(self,
                         position,
                         orientation,
                         delay=0.5,
                         waypoints=[],
                         corrections=4,
                         action_server=None):
        for i in range(corrections + 1):
            rospy.loginfo("TRY NUMBER " + str(i))
            if len(waypoints) > 0 and i < 1:
                # this is good for doing gestures
                plan, fraction = self.arm.compute_cartesian_path(
                    waypoints, eef_step=0.01, jump_threshold=0.0)
            else:
                p = self.arm.get_current_pose()
                p.pose.position = position
                p.pose.orientation = orientation
                rospy.loginfo("PLANNING TO " + str(p))
                self.arm.set_pose_target(p)
                last_traj_goal = self.last_joint_trajectory_goal
                rospy.loginfo("EXECUTING!")
                plan = self.arm.go(wait=False)
                timeout = 5 / 0.001
                while self.last_joint_trajectory_goal == last_traj_goal:
                    rospy.sleep(0.001)
                    timeout -= 1
                    if timeout <= 0:
                        raise (Exception(
                            "Timeout waiting for kinova to accept movement goal."
                        ))
                rospy.loginfo("KINOVA GOT THE MOVEMENT GOAL")
                current_goal = self.last_joint_trajectory_goal
                # then loop until a result for it gets published
                timeout = 90 / 0.001
                while self.last_joint_trajectory_result != current_goal:
                    rospy.sleep(0.001)
                    timeout -= 1
                    if timeout <= 0:
                        raise (Exception(
                            "Motion took longer than 90 seconds. aborting..."))
                    if self.paused is True:
                        self.arm.stop()  # cancel the moveit goals
                        return
                rospy.loginfo("LEAVING THE WHILE LOOP")
                # for i in range(corrections):
                #     rospy.loginfo("Correcting the pose")
                #     self.move_joint_angles(angle_set)
                rospy.sleep(delay)
                if (self.check_plan_result(p, self.arm.get_current_pose())):
                    break  #we're there, no need to retry
                #rospy.loginfo("OK GOT THROUGH THE PLANNING PHASE")
            if False:
                # # get the last pose to correct if desired
                # ptPos = plan.joint_trajectory.points[-1].positions
                # # print "=================================="
                # # print "Last point of the current trajectory: "
                # angle_set = list()
                # for i in range(len(ptPos)):
                #     tempPos = ptPos[i]*180/np.pi + int(round((self.joint_angles[i] - ptPos[i]*180/np.pi)/(360)))*360
                #     angle_set.append(tempPos)

                if action_server:
                    pass
                    #action_server.publish_feedback(CalibrateFeedback("Plan Found"))
                last_traj_goal = self.last_joint_trajectory_goal
                rospy.loginfo("EXECUTING!")
                self.arm.execute(plan, wait=False)
                # this is a bit naive, but I'm going to loop until a new trajectory goal gets published
                timeout = 5 / 0.001
                while self.last_joint_trajectory_goal == last_traj_goal:
                    rospy.sleep(0.001)
                    timeout -= 1
                    if timeout <= 0:
                        raise (Exception(
                            "Timeout waiting for kinova to accept movement goal."
                        ))
                rospy.loginfo("KINOVA GOT THE MOVEMENT GOAL")
                current_goal = self.last_joint_trajectory_goal
                # then loop until a result for it gets published
                timeout = 15 / 0.001
                while self.last_joint_trajectory_result != current_goal:
                    rospy.sleep(0.001)
                    timeout -= 1
                    if timeout <= 0:
                        raise (Exception(
                            "Motion took longer than 15 seconds. aborting..."))
                    if self.paused is True:
                        self.arm.stop()  # cancel the moveit goals
                        return
                rospy.loginfo("LEAVING THE WHILE LOOP")
                # for i in range(corrections):
                #     rospy.loginfo("Correcting the pose")
                #     self.move_joint_angles(angle_set)
                rospy.sleep(delay)
            else:
                if action_server:
                    #action_server.publish_feedback(CalibrateFeedback("Planning Failed"))
                    pass

    # Let's have the caller handle sequences instead.
    # def handle_move_sequence(self, message):
    #     # if the move fails, do we cancel the sequence
    #     cancellable = message.cancellable
    #     moves = message.moves
    #     for move in moves:

    #         try:
    #             self.handle_move_block(move)
    #         except Exception:
    #             if cancellable:
    #                 rospy.loginfo("Part of move failed, cancelling the rest.")
    #                 break

    def update_move_group_state(self, message):
        rospy.loginfo(message.feedback.state)
        self.move_group_state = message.feedback.state

    def update_move_group_status(self, message):
        if message.status_list:
            #rospy.loginfo("MoveGroup Status for "+str(message.status_list[0].goal_id.id)+": "+str(message.status_list[0].status))
            self.move_group_status = message.status_list[0].status

    def update_joint_trajectory_state(self, message):
        # print(message.status_list)
        if len(message.status_list) > 0:
            self.joint_trajectory_state = message.status_list[0].status
        else:
            self.joint_trajectory_state = 0

    def update_joint_trajectory_goal(self, message):
        #print("goalisasis", message.goal_id.id)
        self.last_joint_trajectory_goal = message.goal_id.id

    def update_joint_trajectory_result(self, message):
        #print("resultisasis", message.status.goal_id.id)
        self.last_joint_trajectory_result = message.status.goal_id.id

    def get_grasp_orientation(self, position):
        #return Quaternion(0, 0, 1/np.sqrt(2), 1/np.sqrt(2))
        return Quaternion(-0.707388, -0.706825, 0.0005629, 0.0005633)

    def update_joints(self, joints):
        self.joint_angles = [
            joints.joint1, joints.joint2, joints.joint3, joints.joint4,
            joints.joint5, joints.joint6, joints.joint7
        ]

    def move_z_relative(self, distance):
        p = self.arm.get_current_pose()
        p.pose.position.z += distance
        self.move_arm_to_pose(p.pose.position, p.pose.orientation, delay=0)

    def move_z_absolute(self, height):
        p = self.arm.get_current_pose()
        p.pose.position.z = height
        self.move_arm_to_pose(p.pose.position, p.pose.orientation, delay=0)

    def pick_block(self,
                   location,
                   check_grasp=False,
                   retry_attempts=0,
                   delay=0,
                   action_server=None):
        # go to a spot and pick up a block
        # if check_grasp is true, it will check torque on gripper and retry or fail if not holding
        # open the gripper
        # print('Position: ', position)
        self.open_gripper()
        position = Point(location.x, location.y, self.cruising_height)
        orientation = self.get_grasp_orientation(position)
        try:
            self.raise_table()
            self.move_arm_to_pose(position,
                                  orientation,
                                  delay=0,
                                  action_server=action_server)
            self.lower_table()
            position = Point(location.x, location.y, self.grasp_height)
            self.move_arm_to_pose(position,
                                  orientation,
                                  delay=0,
                                  action_server=action_server)
        except Exception as e:
            current_pose = self.arm.get_current_pose()
            if current_pose.pose.position.z - self.cruising_height < 0:
                self.move_z_absolute(self.crusing_height)
            self.raise_table()
            raise (
                e
            )  #problem because sometimes we get exception e.g. if we're already there
            # and it will skip the close if so.
        if action_server:
            action_server.publish_feedback()

        self.close_gripper()
        self.move_z_absolute(self.cruising_height)
        #try to wait until we're up to check grasp
        rospy.sleep(0.5)

        if check_grasp is True:
            if (self.check_grasp() is False):
                print("Grasp failed!")
                self.raise_table()
                raise (Exception("grasp failed!"))
            # for now, but we want to check finger torques
            # for i in range(retry_attempts):
            #     self.move_z(0.3)
        self.raise_table()

        rospy.sleep(delay)

    def place_block(self,
                    location,
                    check_grasp=False,
                    delay=0,
                    action_server=None):
        # go to a spot an drop a block
        # if check_grasp is true, it will check torque on gripper and fail if not holding a block
        # print('Position: ', position)
        current_pose = self.arm.get_current_pose()
        if current_pose.pose.position.z - self.cruising_height < -.02:  # if I'm significantly below cruisng height, correct
            self.move_z_absolute(self.cruising_height)
        position = Point(location.x, location.y, self.cruising_height)
        rospy.loginfo("PLACE POSITION: " + str(position) + "(DROP HEIGHT: " +
                      str(self.drop_height))
        orientation = self.get_grasp_orientation(position)
        try:
            self.raise_table(
            )  # only do this as a check in case it isn't already raised
            self.move_arm_to_pose(position,
                                  orientation,
                                  delay=0,
                                  action_server=action_server)
            self.lower_table()
            self.move_z_absolute(self.drop_height)
        except Exception as e:
            current_pose = self.arm.get_current_pose()
            if current_pose.pose.position.z - self.cruising_height < 0:
                self.move_z_absolute(self.cruising_height)
            self.raise_table()
            raise (e)
        if action_server:
            action_server.publish_feedback()
        self.open_gripper()
        self.move_z_absolute(self.cruising_height)
        self.raise_table()
        self.close_gripper()

    def point_at_block(self, location, delay=0, action_server=None):
        # go to a spot an drop a block
        # if check_grasp is true, it will check torque on gripper and fail if not holding a block
        # print('Position: ', position)
        current_pose = self.arm.get_current_pose()
        if current_pose.pose.position.z - self.cruising_height < -.02:  # if I'm significantly below cruisng height, correct
            self.move_z_absolute(self.cruising_height)
        position = Point(location.x, location.y, self.cruising_height)
        rospy.loginfo("PLACE POSITION: " + str(position) + "(DROP HEIGHT: " +
                      str(self.drop_height))
        orientation = self.get_grasp_orientation(position)
        try:
            self.raise_table(
            )  # only do this as a check in case it isn't already raised
            self.move_arm_to_pose(position,
                                  orientation,
                                  delay=0,
                                  action_server=action_server)
            self.lower_table()
            self.move_z_absolute(self.pointing_height)
            self.move_z_absolute(self.cruising_height)
        except Exception as e:
            current_pose = self.arm.get_current_pose()
            if current_pose.pose.position.z - self.cruising_height < 0:
                self.move_z_absolute(self.cruising_height)
            self.raise_table()
            raise (e)
        if action_server:
            action_server.publish_feedback()
        self.raise_table()

    def stop_motion(self, home=False, pause=False):
        rospy.loginfo("STOPPING the ARM")
        # cancel the moveit_trajectory
        # self.arm.stop()

        # call the emergency stop on kinova
        self.emergency_stop()
        # rospy.sleep(0.5)

        if pause is True:
            self.paused = True
        if home is True:
            # self.restart_arm()
            self.home_arm()

    def calibrate(self, message):
        print("calibrating ", message)
        self.place_calibration_block()
        rospy.sleep(5)  # five seconds to correct the drop if it bounced, etc.
        print("moving on...")
        self.calibration_server.publish_feedback(
            CalibrateFeedback("getting the coordinates"))
        params = self.record_calibration_params()
        self.set_arm_calibration_params(params[0], params[1])
        self.calibration_publisher.publish(
            CalibrationParams(params[0], params[1]))
        self.calibration_server.set_succeeded(CalibrateResult(params))

    def set_arm_calibration_params(self, arm_x_min, arm_y_min):
        rospy.set_param("ARM_X_MIN", arm_x_min)
        rospy.set_param("ARM_Y_MIN", arm_y_min)

    def place_calibration_block(self):
        # start by homing the arm (kinova home)
        self.calibration_server.publish_feedback(CalibrateFeedback("homing"))
        self.home_arm_kinova()
        # go to grab a block from a human
        self.open_gripper()
        self.close_gripper()
        self.open_gripper(4)
        self.close_gripper(2)
        # move to a predetermined spot
        self.calibration_server.publish_feedback(
            CalibrateFeedback("moving to drop"))
        try:
            self.move_arm_to_pose(Point(0.4, -0.4, 0.1),
                                  Quaternion(1, 0, 0, 0),
                                  corrections=0)
        except Exception as e:
            rospy.loginfo("THIS IS TH PRoblem" + str(e))
        # drop the block
        self.open_gripper()
        self.calibration_server.publish_feedback(
            CalibrateFeedback("dropped the block"))
        calibration_block = {'x': 0.4, 'y': -0.4, 'id': 0}
        calibration_action_belief = {
            "action": "add",
            "block": calibration_block
        }
        self.action_belief_publisher.publish(
            String(json.dumps(calibration_action_belief)))
        rospy.loginfo("published arm belief")
        return

    def record_calibration_params(self):
        """ Call the block_tracker service to get the current table config.
            Find the calibration block and set the appropriate params.
        """
        # make sure we know the dimensions of the table before we start
        # fixed table dimensions include tuio_min_x,tuio_min_y,tuio_dist_x,tuio_dist_y,arm_dist_x,arm_dist_y
        tuio_bounds = get_tuio_bounds()
        arm_bounds = get_arm_bounds(calibrate=False)
        try:
            block_state = json.loads(self.get_block_state("tuio").tui_state)
        except rospy.ServiceException as e:
            # self.calibration_server.set_aborted()
            raise (ValueError("Failed getting block state to calibrate: " +
                              str(e)))
        if len(block_state) != 1:
            # self.calibration_server.set_aborted()
            raise (ValueError(
                "Failed calibration, either couldn't find block or > 1 block on TUI!"
            ))

        # if we made it here, let's continue!
        # start by comparing the reported tuio coords where we dropped the block
        # with the arm's localization of the end-effector that dropped it
        # (we assume that the block fell straight below the drop point)
        drop_pose = self.arm.get_current_pose()
        end_effector_x = drop_pose.pose.position.x
        end_effector_y = drop_pose.pose.position.y

        block_tuio_x = block_state[0]['x']
        block_tuio_y = block_state[0]['y']

        x_ratio, y_ratio = tuio_to_ratio(block_tuio_x, block_tuio_y,
                                         tuio_bounds)
        arm_x_min = end_effector_x - x_ratio * arm_bounds["x_dist"]
        arm_y_min = end_effector_y - y_ratio * arm_bounds["y_dist"]

        return [arm_x_min, arm_y_min]
示例#50
0
class TestMove():
    def __init__(self):
        roscpp_initialize(sys.argv)
        rospy.init_node('ur3_move', anonymous=True)

        self.detected = {}
        self.detected_names = rospy.get_param(
            '/darknet_ros/yolo_model/detection_classes/names')
        self.object_pose_sub = rospy.Subscriber(
            '/cluster_decomposer/centroid_pose_array', PoseArray,
            self.collectJsk)
        self.listener = tf.TransformListener()

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

        self.ii = 0

        global goal_x
        global goal_y
        global goal_z

        global ori_w
        global ori_x
        global ori_y
        global ori_z

        self.distance = 0

        self.tomato = []
        self.position_list = []
        self.orientation_list = []

        self.tomato_pose = Pose()
        self.goalPoseFromTomato = Pose()
        self.br = tf.TransformBroadcaster()

        self.calculated_tomato = Pose()
        self.calculated_tomato_coor = Pose()

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

        self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
        self.robot_arm.set_goal_orientation_tolerance(0.005)
        self.robot_arm.set_planning_time(10)
        self.robot_arm.set_num_planning_attempts(10)

        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)

    def move_code(self):
        planning_frame = self.robot_arm.get_planning_frame()
        print "========== plannig frame: ", planning_frame

        self.wpose = self.robot_arm.get_current_pose()
        print "====== current pose : ", self.wpose

        marker_joint_goal = [
            2.6482303142547607, -0.3752959410296839, -2.1118043104754847,
            -0.4801228682147425, -1.4944542090045374, -4.647655431424276
        ]
        print "INIT POSE: ", self.robot_arm.get_current_pose().pose.position
        self.robot_arm.go(marker_joint_goal, wait=True)

    def pose_subscriber(self):
        rospy.loginfo("waiting for pose topic...")

        rospy.get_param('/darknet_ros/yolo_model/detection_classes/names')
        rospy.Subscriber('/cluster_decomposer/centroid_pose_array', PoseArray,
                         self.collectJsk)

    def go_to_goal_point(self, scale=1.0):
        planning_frame = self.robot_arm.get_planning_frame()

        print(">> robot arm planning frame: \n", planning_frame)

        self.calculated_tomato.position.x = (scale * goal_x)
        self.calculated_tomato.position.y = (scale * goal_y)
        self.calculated_tomato.position.z = (scale * goal_z)

        self.calculated_tomato.orientation.w = (scale * ori_w)
        self.calculated_tomato.orientation.x = (scale * ori_x)
        self.calculated_tomato.orientation.y = (scale * ori_y)
        self.calculated_tomato.orientation.z = (scale * ori_z)

        print(">> robot_pose goal frame: ", self.calculated_tomato)
        self.robot_arm.set_pose_target(self.calculated_tomato)

        tf_display_position = [
            self.calculated_tomato.position.x,
            self.calculated_tomato.position.y,
            self.calculated_tomato.position.z
        ]
        tf_display_orientation = [
            self.calculated_tomato.orientation.x,
            self.calculated_tomato.orientation.y,
            self.calculated_tomato.orientation.z,
            self.calculated_tomato.orientation.w
        ]

        ii = 0
        while ii < 5:
            ii += 1
            self.br.sendTransform(tf_display_position, tf_display_orientation,
                                  rospy.Time.now(), "goal_wpose", "world")

        tomato_waypoints = []
        tomato_waypoints.append(copy.deepcopy(self.calculated_tomato))
        (tomato_plan, tomato_fraction) = self.robot_arm.compute_cartesian_path(
            tomato_waypoints, 0.01, 0.0)
        self.display_trajectory(tomato_plan)

        print("======= Press `Enter` to if plan in correct!======")
        raw_input()
        self.robot_arm.go(True)

    '''
    def go_to_designated_coordinate(self):        
        desired_goal_pose = [-0.537,0.166, 0.248]

        Cplanning_frame = self.robot_arm.get_planning_frame()
        print(">> current planning frame: \n",Cplanning_frame)

        self.calculated_tomato_coor.position.x = desired_goal_pose[0]
        self.calculated_tomato_coor.position.y = desired_goal_pose[1]
        self.calculated_tomato_coor.position.z = desired_goal_pose[2]
        self.calculated_tomato_coor.orientation = Quaternion(*quaternion_from_euler(desired_goal_pose[0],desired_goal_pose[1],desired_goal_pose[2]))

        print(">> goal frame", self.calculated_tomato_coor)
        self.robot_arm.set_pose_target(self.calculated_tomato_coor)

        tf_display_position = [self.calculated_tomato_coor.position.x, self.calculated_tomato_coor.position.y, self.calculated_tomato_coor.position.z]
        tf_display_orientation = [self.calculated_tomato_coor.orientation.x, self.calculated_tomato_coor.orientation.y, self.calculated_tomato_coor.orientation.z, self.calculated_tomato_coor.orientation.w]

        jj = 0
        while jj < 5:
            jj += 1
            self.br.sendTransform(
                tf_display_position,
                tf_display_orientation,
                rospy.Time.now(),
                "goal_wpose",
                "world")

        tomato_waypoints = []
        tomato_waypoints.append(copy.deepcopy(self.calculated_tomato_coor))
        (plan, fraction) = self.robot_arm.compute_cartesian_path(tomato_waypoints,0.01,0.0)
        self.display_trajectory(plan)

        print("=========== Press `Enter` to if plan is correct!!...")
        raw_input()
        self.robot_arm.go(True)
    '''

    def plan_cartesian_path(self, scale=1.0):
        waypoints = []

        self.wpose = self.robot_arm.get_current_pose().pose
        self.wpose.position.x -= scale * 0.1
        self.wpose.position.y += scale * 0.1
        waypoints.append(copy.deepcopy(self.wpose))
        '''
        self.wpose.position.x -= scale*0.2
        waypoints.append(copy.deepcopy(self.wpose))
    	'''
        '''
        self.wpose.position.x -= scale*0.1
        waypoints.append(copy.deepcopy(self.wpose))
        '''

        (plan, fraction) = self.robot_arm.compute_cartesian_path(
            waypoints, 0.01, 0.0)

        return plan, fraction

    def execute_plan(self, plan):
        group = self.robot_arm
        group.execute(plan, wait=True)

    def display_trajectory(self, plan):
        display_trajectory_publisher = self.display_trajectory_publisher

        display_trajectory = moveit_msgs.msg.DisplayTrajectory()
        display_trajectory.trajectory_start = self.robot_cmd.get_current_state(
        )
        display_trajectory.trajectory.append(plan)

        display_trajectory_publisher.publish(display_trajectory)

    def collectJsk(self, msg):
        global goal_x
        global goal_y
        global goal_z

        global ori_w
        global ori_x
        global ori_y
        global ori_z

        try:
            (trans,
             rot) = self.listener.lookupTransform('base_link', 'yolo_output00',
                                                  rospy.Time(0))

            goal_x = trans[0]
            goal_y = trans[1]
            goal_z = trans[2]

            ori_w = rot[0]
            ori_x = rot[1]
            ori_y = rot[2]
            ori_z = rot[3]

            print("==== trans[x,y,z]: ", trans)
            print("==== rot[x,y,z,w]: ", rot)

        except (tf.LookupException, tf.ConnectivityException,
                tf.ExtrapolationException):
            rospy.logwarn('there is no tf')
#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)


def newOdom(msg):
    global x
    global y
    global theta

    x = msg.pose.pose.position.x
    y = msg.pose.pose.position.y

    rot_q = msg.pose.pose.orientation
示例#52
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, mid_pose, end_pose, maxtries, exe_signal=False):
        waypoints = []
        fraction = 0.0
        attempts = 0
        waypoints.append(start_pose.pose)
        if mid_pose != 0:
            waypoints.append(mid_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.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

    #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.05)
                    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

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

    #shaking_a: vertical bottle axis
    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.0
        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, angle_pre, angle_r, r, maxtries):
        angle_pre = (angle_pre / 180.0) * 3.14
        angle_r_1 = (angle_r / 180.0) * 3.14
        initial_state = self.arm.get_current_joint_values()
        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 pg_g_pp(self, pose, r, pre_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 = [
            0.0, 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
            pre_grasp_pose = self.get_grasp_pose(pose, r + pre_r, theta)
            #pick up
            grasp_pose = pre_grasp_pose
            grasp_pose.pose.position.x -= pre_r * math.sin(theta)
            grasp_pose.pose.position.y += pre_r * math.cos(theta)
            signal, e_j_s = self.cts(start_pose, pre_grasp_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, pre_grasp_pose, start_pose, 300,
                                 True)
        rospy.sleep(2)
        rospy.loginfo("Grasping done.")
        return start_pose, pre_grasp_pose, grasp_pose

    def pp_ps(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 go_back(self, ori_pose, pre_grasp_pose, grasp_pose):
        cur_pose = self.arm.get_current_pose(self.arm_end_effector_link)
        signal, e1 = self.cts(cur_pose, 0, ori_pose, 300, True)
        rospy.loginfo("back to pre_grasp pose, ready for placing bottle..")
        signal_1, e2 = self.cts(ori_pose, pre_grasp_pose, grasp_pose, 300,
                                True)
        rospy.loginfo("back to grasp pose, open gripper..")
        self.gripperCtrl(255)
        rospy.sleep(1)
        signal_2, e3 = self.cts(grasp_pose, pre_grasp_pose, ori_pose, 300,
                                True)
        rospy.loginfo("back to pre_grasp pose, ready for next kind of source.")

    def rotate_back(self, angle):
        angle = angle * 3.14 / 180.0
        current_j_state = self.arm.get_current_joint_values()
        current_j_state[6] += angle + 1.57
        self.arm.go()

    def last_step_go_back(self, ori_pose):
        cur_pose = self.arm.get_current_pose(self.arm_end_effector_link)
        signal, e1 = self.cts(cur_pose, 0, ori_pose, 300, True)
        rospy.loginfo("back to last step...")
        rospy.sleep(1)

    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
        #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.2, 0.6, 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
        #msg = rospy.wait_for_message('/aruco_single/pose',PoseStamped)
        self.add_target(target1_pose, target1_size, self.reference_frame,
                        -0.25, 0.8, 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,
                        -0.12, 0.87, 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.02,
                        0.88, 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.15,
                        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 = 4
        maxtries = 300
        r_grasp = 0.15
        r_pre_g = 0.1
        r_bottle = 0.1
        for i in range(0, tar_num):
            #grasp target
            rospy.loginfo("Choosing source...")
            if i == 0:
                target_pose = target1_pose
                target_id = self.target1_id
                target_size = target1_size
                amp = 0.1
                freq = 2.75
                r_angle = 45.0
            elif i == 1:
                target_pose = target2_pose
                target_id = self.target2_id
                target_size = target2_size
                amp = 0.15
                freq = 2.0
                r_angle = 30.0
            elif i == 2:
                target_pose = target3_pose
                target_id = self.target3_id
                target_size = target3_size
                amp = 0.1
                freq = 2.75
                r_angle = 45.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
            h_pour = 0.1

            #pour_angle=[15.0,30.0,45.0,60.0,75.0]
            pour_angle = [
                0.0, -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, mid_pose, g_pose = self.pg_g_pp(target_pose, r_grasp,
                                                      r_pre_g)
            #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.last_step_go_back(ori_pose)
                rospy.sleep(3)
                continue
            else:
                rospy.loginfo("Shaking planning...")
                shake_per_times = 3
                shake_times = 0
                shake_times_tgt = 1
                signal = True
                rospy.loginfo("Parameter loaded")
                rospy.sleep(1)
                signal, end_joint_state = self.shake_a(pre_p_angle, r_angle,
                                                       amp)
                while signal == 1:
                    #area_ratio= rospy.wait_for_message('/color_area_ratio',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)
                        shake_times += 1

                        rospy.loginfo("shaking times :  " + str(shake_times) +
                                      " .")
                    else:
                        signal = 0

            self.rotate_back(r_angle)
            self.go_back(ori_pose, mid_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)
#!/usr/bin/env python

import rospy, geometry_msgs.msg, copy

from moveit_commander import MoveGroupCommander

if __name__ == '__main__':

    rospy.init_node('commander_example', anonymous=True)
    group = MoveGroupCommander("right_arm")

    # Pose Target 1
    pose_target_1 = geometry_msgs.msg.Pose()

    pose_target_1.position.x = 0.3
    pose_target_1.position.y = -0.1
    pose_target_1.position.z = 0.15
    pose_target_1.orientation.x = 0.0
    pose_target_1.orientation.y = -0.707
    pose_target_1.orientation.z = 0.0
    pose_target_1.orientation.w = 0.707

    # Pose Target 2
    pose_target_2 = geometry_msgs.msg.Pose()

    pose_target_2 = copy.deepcopy(pose_target_1)
    pose_target_2.position.y = -0.5

    # Waypoints Motion
    waypoints = []
    waypoints.append(pose_target_1)
示例#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
        #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.2, 0.6, 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
        #msg = rospy.wait_for_message('/aruco_single/pose',PoseStamped)
        self.add_target(target1_pose, target1_size, self.reference_frame,
                        -0.25, 0.8, 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,
                        -0.12, 0.87, 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.02,
                        0.88, 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.15,
                        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 = 4
        maxtries = 300
        r_grasp = 0.15
        r_pre_g = 0.1
        r_bottle = 0.1
        for i in range(0, tar_num):
            #grasp target
            rospy.loginfo("Choosing source...")
            if i == 0:
                target_pose = target1_pose
                target_id = self.target1_id
                target_size = target1_size
                amp = 0.1
                freq = 2.75
                r_angle = 45.0
            elif i == 1:
                target_pose = target2_pose
                target_id = self.target2_id
                target_size = target2_size
                amp = 0.15
                freq = 2.0
                r_angle = 30.0
            elif i == 2:
                target_pose = target3_pose
                target_id = self.target3_id
                target_size = target3_size
                amp = 0.1
                freq = 2.75
                r_angle = 45.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
            h_pour = 0.1

            #pour_angle=[15.0,30.0,45.0,60.0,75.0]
            pour_angle = [
                0.0, -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, mid_pose, g_pose = self.pg_g_pp(target_pose, r_grasp,
                                                      r_pre_g)
            #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.last_step_go_back(ori_pose)
                rospy.sleep(3)
                continue
            else:
                rospy.loginfo("Shaking planning...")
                shake_per_times = 3
                shake_times = 0
                shake_times_tgt = 1
                signal = True
                rospy.loginfo("Parameter loaded")
                rospy.sleep(1)
                signal, end_joint_state = self.shake_a(pre_p_angle, r_angle,
                                                       amp)
                while signal == 1:
                    #area_ratio= rospy.wait_for_message('/color_area_ratio',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)
                        shake_times += 1

                        rospy.loginfo("shaking times :  " + str(shake_times) +
                                      " .")
                    else:
                        signal = 0

            self.rotate_back(r_angle)
            self.go_back(ori_pose, mid_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)
import rospy
import copy
import geometry_msgs.msg
import moveit_msgs.msg

from moveit_commander import RobotCommander, MoveGroupCommander
from moveit_commander import PlanningSceneInterface, roscpp_initialize, roscpp_shutdown

from math import sin, copysign, sqrt, pi
   
if __name__ == '__main__':
    print "============ Dynamic hand gestures: Right"
    roscpp_initialize(sys.argv)
    rospy.init_node('pumpkin_planning', anonymous=True)

    right_arm = MoveGroupCommander("right_arm")
    display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=1)
    print right_arm.get_current_pose().pose

    wpose = geometry_msgs.msg.Pose()
    wpose.orientation.w = 0.0176318609849
    wpose.orientation.x = 0.392651866792
    wpose.orientation.y = 0.918465607415
    wpose.orientation.z = 0.0439835989663
    wpose.position.y = 0.227115629827
    wpose.position.z = 1.32344046934
    wpose.position.x = -0.358178766726
    right_arm.set_pose_target(wpose)
    plan1 = right_arm.plan()

    right_arm.execute(plan1)
示例#56
0
			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"
	p2.header.frame_id = "world"
	bowl2eef.header.frame_id = "world"
	p_goal.header.frame_id = "world"
	tflist = Transform()
示例#57
0
class PR2Greeter:
    
    def __init__(self, debug=False, online = True, robot_frame="odom_combined"):
        
        self._tf = TransformListener()
        
        self._online = online
        
        # self.snd_handle = SoundClient()
        
        if self._online:
        
            #self._interface = ROSpeexInterface()
            #self._interface.init()
            self._speech_client = SpeechSynthesisClient_NICT()
            
        else:
            
            self.snd_handle = SoundClient()
        
        rospy.sleep(1)
        
        self.say('Hello world!')
        
        rospy.sleep(1)
        
        self._debug = debug
        self._robot_frame = robot_frame
        
        self._point_sub = rospy.Subscriber('nearest_face', PointStamped, self.face_cb)
        
        self._head_action_cl = actionlib.SimpleActionClient('/head_traj_controller/point_head_action', pr2_controllers_msgs.msg.PointHeadAction)
        self._torso_action_cl = actionlib.SimpleActionClient('/torso_controller/position_joint_action', pr2_controllers_msgs.msg.SingleJointPositionAction)
        
        self._left_arm = MoveGroupCommander("left_arm")
        self._right_arm = MoveGroupCommander("right_arm")
        
        print "r.f.: " + self._left_arm.get_pose_reference_frame()

        self.face = None
        # self.face_from = rospy.Time(0)
        self.face_last_dist = 0
        self.last_invited_at = rospy.Time(0)
        self._person_prob_left = 0
        
        self.l_home_pose = [0.283, 0.295, 0.537, -1.646, 0.468, -1.735]
        
        self.l_wave_1 = [-0.1, 0.6, 1.15, -1.7, -0.97, -1.6]
        self.l_wave_2 = [-0.1, 0.6, 1.15, 1.7, -0.97, 1.6]
        
        self.r_home_pose = [0.124, -0.481, 0.439, -1.548, 0.36, -0.035]
        self.r_advert = [0.521, -0.508, 0.845, -1.548, 0.36, -0.035]
        
        self.no_face_random_delay = None
        
        self._initialized = False
        
        self._timer = rospy.Timer(rospy.Duration(1.0), self.timer)
        
        
        self._move_buff = Queue.Queue()
        
        self._head_buff = Queue.Queue()
        
        self._move_thread = threading.Thread(target=self.movements)
        self._move_thread.daemon = True
        self._move_thread.start()
        
        self._head_thread = threading.Thread(target=self.head)
        self._head_thread.daemon = True
        self._head_thread.start()
        
        self.new_face = False
        self.face_last_dist = 0.0
        
        self.face_counter = 0
        
        self.actions = [self.stretchingAction,
                        self.introduceAction,
                        self.waveHandAction,
                        self.lookAroundAction,
                        self.lookAroundAction,
                        self.lookAroundAction,
                        self.advertAction,
                        self.numberOfFacesAction]
        
        self.goodbye_strings = ["Thanks for stopping by.",
                                "Enjoy the event.",
                                "See you later!",
                                "Have a nice day!"]
        
        self.invite_strings = ["Hello. It's nice to see you.",
                               "Come here and take some flyer.",
                               "I hope you are enjoying the event."]
        
        rospy.loginfo("Ready")
    
    def say(self, text):
        
        if self._online:
        
            #self._interface.say(text, 'en', 'nict')
            data = self._speech_client.request(text, 'en')
            
            try:
            
              tmp_file = open('output_tmp.dat', 'wb')
              tmp_file.write(data)
              tmp_file.close()

              # play sound
              subprocess.check_output(['ffplay', 'output_tmp.dat', '-autoexit', '-nodisp'], stderr=subprocess.STDOUT)
              
            except IOError:
              rospy.logerr('file io error.')
            except OSError:
              rospy.logerr('ffplay is not installed.')
            except subprocess.CalledProcessError:
              rospy.logerr('ffplay return error value.')
            except:
              rospy.logerr('unknown exception.')
            
        else:
            
            self.snd_handle.say(text)
    
    def getRandomFromArray(self, arr):
        
        idx = random.randint(0, len(arr) - 1)
        
        return arr[idx]
    
    def stretchingAction(self):
        
        self.say("I'm bit tired. Let's do some stretching.")
        
        self._torso_action_cl.wait_for_server()
        
        goal = pr2_controllers_msgs.msg.SingleJointPositionGoal()
        
        goal.position = 0.195
        goal.max_velocity = 0.5
        
        try:
        
          self._torso_action_cl.send_goal(goal)
          self._torso_action_cl.wait_for_result(rospy.Duration.from_sec(10.0))
          
        except:
            
            rospy.logerr("torso action error (up)")
        
        # TODO move arms
        
        self.say("I feel much better now!")
        
        goal.position = 0.0
        
        try:
        
          self._torso_action_cl.send_goal(goal)
          self._torso_action_cl.wait_for_result(rospy.Duration.from_sec(10.0))
          
        except:
            
            rospy.logerr("torso action error (down)")
    
    def numberOfFacesAction(self):
        
        string = "Today I already saw " + str(self.face_counter) + "face"
        
        if self.face_counter != 1:
            
            string = string + "s"
            
        string = string + "."
        
        self.say(string)
        rospy.sleep(1)
    
    def advertAction(self):
        
        self.say("Hello. Here are some posters for you.")
        self.go(self._right_arm, self.r_advert)
        rospy.sleep(1)
        
    def introduceAction(self):
        
        self.say("Hello. I'm PR2 robot. Come here to check me.")
        rospy.sleep(1)
        
        
    def waveHandAction(self):
        
        self.say("I'm here. Please come to see me.")
        
        rand = random.randint(1, 3)
        
        for _ in range(rand):
            
            self.wave()
            
        self.go(self._left_arm, self.l_home_pose)
        
        rospy.loginfo("Waving")
        rospy.sleep(1)
        
    def lookAroundAction(self):
        
        self.say("I'm looking for somebody. Please come closer.")
        
        p = PointStamped()
        p.header.stamp = rospy.Time.now()
        p.header.frame_id = "/base_link"
        
        p.point.x = 5.0
        
        sign = random.choice([-1, 1])
        
        p.point.y = sign * random.uniform(1.5, 0.5)
        p.point.z = random.uniform(1.7, 0.2)
        self._head_buff.put((copy.deepcopy(p), 0.1, True))
        
        p.point.y = -1 * sign * random.uniform(1.5, 0.5)
        p.point.z = random.uniform(1.7, 0.2)
        self._head_buff.put((copy.deepcopy(p), 0.1, True))
        
        p.point.y = sign * random.uniform(1.5, 0.5)
        p.point.z = random.uniform(1.7, 0.2)
        self._head_buff.put((copy.deepcopy(p), 0.1, True))
        
        p.point.y = -1 * sign * random.uniform(1.5, 0.5)
        p.point.z = random.uniform(1.7, 0.2)
        self._head_buff.put((copy.deepcopy(p), 0.1, True))
        
        rospy.loginfo("Looking around")
        rospy.sleep(1)
        
    def getPointDist(self, pt):
        
        assert(self.face is not None)
        
        # fist, get my position
        p = PoseStamped()
        
        p.header.frame_id = "base_link"
        p.header.stamp = rospy.Time.now() - rospy.Duration(0.5)
        
        p.pose.position.x = 0
        p.pose.position.y = 0
        p.pose.position.z = 0
        p.pose.orientation.x = 0
        p.pose.orientation.y = 0
        p.pose.orientation.z = 0
        p.pose.orientation.w = 1
        
        try:
            
            self._tf.waitForTransform(p.header.frame_id, self._robot_frame, p.header.stamp, rospy.Duration(2))
            p = self._tf.transformPose(self._robot_frame, p)
            
        except:
            
            rospy.logerr("TF error!")
            return None
        
        return sqrt(pow(p.pose.position.x - pt.point.x, 2) + pow(p.pose.position.y - pt.point.y, 2) + pow(p.pose.position.z - pt.point.z, 2))
        
        
        
    def getPoseStamped(self, group, c):
        
        assert(len(c) == 6)
        
        p = PoseStamped()
        
        p.header.frame_id = "base_link"
        p.header.stamp = rospy.Time.now() - rospy.Duration(0.5)
        
        p.pose.position.x = c[0]
        p.pose.position.y = c[1]
        p.pose.position.z = c[2]
        
        quat = tf.transformations.quaternion_from_euler(c[3], c[4], c[5])
        
        p.pose.orientation.x = quat[0]
        p.pose.orientation.y = quat[1]
        p.pose.orientation.z = quat[2]
        p.pose.orientation.w = quat[3]
        
        try:
            
            self._tf.waitForTransform(p.header.frame_id, group.get_pose_reference_frame(), p.header.stamp, rospy.Duration(2))
            p = self._tf.transformPose(group.get_pose_reference_frame(), p)
            
        except:
            
            rospy.logerr("TF error!")
            return None
        
        return p
    
    def go(self, group, where):
        
        self._move_buff.put((group, where))
        
    def wave(self):
        
        self.go(self._left_arm, self.l_wave_1)
        self.go(self._left_arm, self.l_wave_2)
        self.go(self._left_arm, self.l_wave_1)
        
    def head(self):
        
        self._head_action_cl.wait_for_server()
        
        while not rospy.is_shutdown():
            
            (target, vel, imp) = self._head_buff.get()
            
            # we don't need to block it here
            self._head_buff.task_done()
            
            if (not imp) and (not self._head_buff.empty()):
              
              print "skipping head goal"
              
              # head target can be skipped (follow only the latest one)  
              continue
            
            # print "head point goal"
            # print target
            
            # point PR2's head there (http://wiki.ros.org/pr2_controllers/Tutorials/Moving%20the%20Head)
            goal = pr2_controllers_msgs.msg.PointHeadGoal()
        
            goal.target = target
	          # goal.min_duration = rospy.Duration(3.0)
            goal.max_velocity = vel
            # goal.pointing_frame = "high_def_frame"
            goal.pointing_frame = "head_mount_kinect_rgb_link"
            goal.pointing_axis.x = 1
            goal.pointing_axis.y = 0
            goal.pointing_axis.z = 0
            
            try:
            
              self._head_action_cl.send_goal(goal)
              self._head_action_cl.wait_for_result(rospy.Duration.from_sec(5.0))

            except:

               rospy.logerr("head action error")
            
            #self._head_buff.task_done()
        
    def movements(self):
        
        while not rospy.is_shutdown():
            
            (group, where) = self._move_buff.get()
            
            group.set_start_state_to_current_state()
            p = self.getPoseStamped(group, where)
            if p is None:
                
                self._move_buff.task_done()
                continue
                
            group.set_pose_target(p)
            group.set_goal_tolerance(0.1)
            group.allow_replanning(True)
            
            self._move_buff.task_done()
            
            group.go(wait=True)
    
    def timer(self, event):
        
        if self._initialized is False:
            
            rospy.loginfo("Moving arms to home positions")
            
            self.init_head()
            self.go(self._left_arm, self.l_home_pose)
            self.go(self._right_arm, self.r_home_pose)
            self._move_buff.join()
            
            self.say("I'm ready for a great job.")
            self._initialized = True
        
        
        if self.face is None:
            
            if (self.no_face_random_delay is None):
                
                delay = random.uniform(30, 10)    
                self.no_face_random_delay = rospy.Time.now() + rospy.Duration(delay)
                
                rospy.loginfo("Random delay: " + str(delay))
                
                return
                
            else:
                
                if rospy.Time.now() < self.no_face_random_delay:
                    
                    return
            
            self.init_head()
            self.go(self._left_arm, self.l_home_pose)
            self.go(self._right_arm, self.r_home_pose)
                
            rospy.loginfo("Starting selected action")
                
            action = self.getRandomFromArray(self.actions)
            
            action()
            
            delay = random.uniform(30, 10)    
            self.no_face_random_delay = rospy.Time.now() + rospy.Duration(delay)
            
            return
        
        else:
            
            self.no_face_random_delay = None
        
        if self.new_face and (self.last_invited_at + rospy.Duration(10) < rospy.Time.now()):

            self.last_invited_at = rospy.Time.now()

            self.new_face = False
            rospy.loginfo("new person")            

            self.face_counter = self.face_counter + 1
            
            # cd = getPointDist(self.face)
            
            # TODO decide action based on distance ?
            self.go(self._left_arm, self.l_home_pose)  # if a person is too close, dont move hand?
            self.go(self._right_arm, self.r_advert)
            
            string = self.getRandomFromArray(self.invite_strings)
            self.say(string)
            
            # TODO wait some min. time + say something
        
        # after 20 seconds of no detected face, let's continue 
        if self.face.header.stamp + rospy.Duration(10) < rospy.Time.now():

            rospy.loginfo("person left")
            
            string = self.getRandomFromArray(self.goodbye_strings)
            self.say(string)
            
            self.init_head()
            
            self.go(self._left_arm, self.l_home_pose)
            self.go(self._right_arm, self.r_home_pose)
            self.face = None
            
            return
        
        self._head_buff.put((copy.deepcopy(self.face), 0.4, False))
            
     
    def init_head(self):
        
        p = PointStamped()
        p.header.stamp = rospy.Time.now()
        p.header.frame_id = "/base_link"
        
        p.point.x = 2.0
        p.point.y = 0.0
        p.point.z = 1.7
        
        self._head_buff.put((p, 0.1, True))
        
    def face_cb(self, point):
        
        # transform point
        
        try:
        
            self._tf.waitForTransform(point.header.frame_id, self._robot_frame, point.header.stamp, rospy.Duration(2))
            pt = self._tf.transformPoint(self._robot_frame, point)
            
        except:
            
            rospy.logerr("Transform error")
            return
        
        if self.face is not None:
        
            cd = self.getPointDist(pt)  # current distance
            dd = fabs(self.face_last_dist - cd)  # change in distance
            
            if dd < 0.5:
        
                self.face.header = pt.header
                
                # filter x,y,z values a bit
                self.face.point = pt.point
                #self.face.point.x = (self.face.point.x + pt.point.x) / 2
                #self.face.point.y = (self.face.point.y + pt.point.y) / 2
                #self.face.point.z = (self.face.point.z + pt.point.z) / 2
                
            else:
                
               if self._person_prob_left < 2:
                   
                   self._person_prob_left += 1 
                   
               else:
                
                   self._person_prob_left = 0
                
                   print "new face ddist: " + str(dd)

                   self.new_face = True
                   self.face = pt
               
            self.face_last_dist = cd
            
        else:
            
            self._person_prob_left = 0
            self.new_face = True
            self.face = pt
    rot_q = msg.pose.pose.orientation
    (roll, pitch,
     theta) = euler_from_quaternion([rot_q.x, rot_q.y, rot_q.z, rot_q.w])


sub = rospy.Subscriber("/odometry/filtered", Odometry, newOdom)
pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)

speed = Twist()

r = rospy.Rate(4)

## 매니퓰레이터 변수 선언

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)

#통합제어

joint_goal = move_group.get_current_joint_values()

#변수 선언
mobile_joints = [-pi / 3, 0.5]
joint_goal_list = [
    pi * 90 / 180, pi * -130 / 180, pi * 111 / 180, pi * -68 / 180,
    pi * -90 / 180, 0
]  #home pose
示例#59
0
文件: fk.py 项目: ALxander19/matlab
#!/usr/bin/env python
import rospy
from moveit_commander import PlanningSceneInterface, RobotCommander, MoveGroupCommander
from moveit_msgs.srv import GetPositionFK, GetPositionFKRequest
from tf.transformations import quaternion_from_euler
from moveit_msgs.msg import MoveItErrorCodes

if __name__ == '__main__':

    try:
        rospy.init_node("fk_node")
        robot = RobotCommander()
        group_name = "manipulator"
        group = MoveGroupCommander(group_name)
        connection = rospy.ServiceProxy("/compute_fk", GetPositionFK)
        rospy.loginfo("waiting for fk server")
        connection.wait_for_service()
        rospy.loginfo("server found")

        request = GetPositionFKRequest()
        request.fk_link_names = robot.get_link_names(group_name)
        request.header.frame_id = robot.get_root_link()
        request.robot_state.joint_state.name = group.get_joints()[0:-1]
        

        # get joint state
        joint_states= raw_input("enter joint state j1 j2 j3 j4 j5 j6 j7 \n")
        joints= [float(idx) for idx in joint_states.split(' ')]
        request.robot_state.joint_state.position = joints

        response = connection(request)
示例#60
0
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)

    # publish a demo scene