Esempio n. 1
0
    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)

        rospy.init_node('start_pos')

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

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

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

        rospy.sleep(1)

        # Shut down MoveIt cleanly
        moveit_commander.roscpp_shutdown()

        # Exit the script
        moveit_commander.os._exit(0)
Esempio n. 2
0
def talker_by13():
    #init
    moveit_commander.roscpp_initialize(sys.argv)
    rospy.init_node('moveit_fk_demo')
    #cartesian = rospy.get_param('~cartesian', True)
    arm = MoveGroupCommander('manipulator')

    arm.set_pose_reference_frame('base_link')

    arm.allow_replanning(True)
    arm.set_goal_position_tolerance(0.001)
    arm.set_goal_orientation_tolerance(0.001)
    # arm.set_max_acceleration_scaling_factor(0.5)
    #arm.set_max_velocity_scaling_factor(0.5)

    end_effector_link = arm.get_end_effector_link()

    #arm.set_named_target('home')
    arm.set_named_target('up')
    arm.go()
    rospy.sleep(2)

    target_pose = PoseStamped()
    target_pose.header.frame_id = 'base_link'
    target_pose.header.stamp = rospy.Time.now()
    target_pose.pose.position.x = 0.86
    target_pose.pose.position.y = 0.25
    target_pose.pose.position.z = 0.02832
    target_pose.pose.orientation.x = 0
    target_pose.pose.orientation.y = 0
    target_pose.pose.orientation.z = 0
    target_pose.pose.orientation.w = 1
    #next: find workspace

    arm.set_start_state_to_current_state()
    arm.set_pose_target(target_pose, end_effector_link)

    traj = arm.plan()

    arm.execute(traj)

    #arm.shift_pose_target(2,-0.05,end_effector_link)
    #arm.go()
    rospy.sleep(2)

    moveit_commander.roscpp_shutdown()
    moveit_commander.os._exit(0)
Esempio n. 3
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(False)

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

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

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

        scene = moveit_commander.PlanningSceneInterface()

        print "[INFO] Current pose:", arm.get_current_pose(eef_link).pose

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

        self.box_name = ''
        self.scene = scene
        self.group = arm
        self.eef_link = eef_link
        self.reference_frame = reference_frame
        self.moveit_commander = moveit_commander

        self.move_distance = 0.1
        self.back_distance = 0.15
Esempio n. 4
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)
Esempio n. 5
0
class TestMove():
    def __init__(self):
        roscpp_initialize(sys.argv)
        rospy.init_node('ur3_move', anonymous=True)

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

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

        rospy.sleep(2)
        # Allow replanning to increase the odds of a solution
        self.robot_arm.allow_replanning(True)

    def move_code(self):

        self.robot_arm.set_named_target("home")  #go to goal state.
        self.robot_arm.go(wait=True)
        print("====== move plan go to home 1 ======")
        rospy.sleep(1)

        #        print("====== move plan go to up ======")
        self.robot_arm.set_named_target("table")  #go to goal state.
        self.robot_arm.go(wait=True)
        print("====== move plan go to table 1 ======")
        rospy.sleep(1)

        range(0, 5)

        rospy.spin()
        roscpp_shutdown()

        #        robot_arm.set_named_target("up")
        #        robot_arm.go(wait=True)

        robot_state = robot_arm.get_current_pose()
        robot_angle = robot_arm.get_current_joint_values()

        print(robot_state)
Esempio n. 6
0
    def __init__(self):

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

        arm = MoveGroupCommander(ARM_GROUP)

        arm.allow_replanning(True)
        arm.set_planning_time(10)

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

        print "======== create new goal position ========"

        start_pose = PoseStamped()
        start_pose.header.frame_id = arm.get_planning_frame()

        # Test Position
        start_pose.pose.position.x = 0.2  # 20 cm
        start_pose.pose.position.y = -0.11  # -11 cm
        start_pose.pose.position.z = 0.6  # 60 cm
        q = quaternion_from_euler(0.0, 0.0, 0.0)
        start_pose.pose.orientation = Quaternion(*q)

        print start_pose

        print "====== move to position ======="


        # KDL
        # arm.set_joint_value_target(start_pose, True)

        # IK Fast
        arm.set_position_target([start_pose.pose.position.x, start_pose.pose.position.y, start_pose.pose.position.z],
                                arm.get_end_effector_link())

        arm.go()
        rospy.sleep(2)

        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)
Esempio n. 7
0
def talker_by13():
    	#init
    	moveit_commander.roscpp_initialize(sys.argv)
    	rospy.init_node('moveit_fk_demo')
        cartesian = rospy.get_param('~cartesian', True)
        arm = MoveGroupCommander('manipulator')

        #arm.set_pose_reference_frame('base_link')

        #arm.set_goal_position_tolerance(0.001)
       # arm.set_goal_orientation_tolerance(0.001)
       # arm.set_max_acceleration_scaling_factor(0.5)
        #arm.set_max_velocity_scaling_factor(0.5)

        end_effector_link = arm.get_end_effector_link()

        arm.set_named_target('home')
        arm.go()
        rospy.sleep(1)

        start_pose = arm.get_current_pose(end_effector_link).pose

        waypoints = []

        wpose = deepcopy(start_pose)
        wpose.position.x -= 0.2
        wpose.position.y -= 0.2
	#wpose.position.z += 0.6 
        #wpose.position.x = 0.8
        #wpose.position.y = 0.9
	#wpose.position.z = 0.9

        arm.set_pose_target(wpose)  
        arm.go()
        rospy.sleep(1)

        




   	moveit_commander.roscpp_shutdown()
    	moveit_commander.os._exit(0)
Esempio n. 8
0
def main():
    rospy.init_node('arm_start_up', anonymous=True)
    scene = PlanningSceneInterface()
    robot = RobotCommander()
    right_arm = MoveGroupCommander("right_arm")
    left_arm = MoveGroupCommander("left_arm")
    right_arm.set_planner_id("RRTConnect:"); 
    left_arm.set_planner_id("RRTConnect:")
    
  
    # move to a random target
    #group.set_named_target("ahead")
    #group.go()
    #rospy.sleep(1)
    right_arm.set_named_target("right_start")
    right_arm.go()
    rospy.sleep(1)
    left_arm.set_named_target("left_start")
    left_arm.go()
    rospy.sleep(1)
Esempio n. 9
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)
Esempio n. 10
0
class TestMove():
    def __init__(self):
        roscpp_initialize(sys.argv)
        rospy.init_node('ur3_move', anonymous=True)

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

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

        rospy.sleep(2)
        # Allow replanning to increase the odds of a solution
        self.robot_arm.allow_replanning(True)

    def move_code(self):

        self.robot_arm.set_named_target("home")  #go to goal state.
        self.robot_arm.go(wait=True)
        print("====== move plan go to home 1 ======")
        rospy.sleep(0.5)

        #        print("====== move plan go to up ======")
        self.robot_arm.set_named_target("up")  #go to goal state.
        self.robot_arm.go(wait=True)
        print("====== move plan go to up ======")
        rospy.sleep(0.5)

        self.robot_arm.set_named_target("home")  #go to goal state.
        self.robot_arm.go(wait=True)
        print("====== move plan go to home 1 ======")
        rospy.sleep(0.5)

        #        robot_arm.set_named_target("up")
        #        robot_arm.go(wait=True)

        robot_state = self.robot_arm.get_current_pose()
        robot_angle = self.robot_arm.get_current_joint_values()

        print(robot_state)

    def move_TF(self):

        self.ee_TF_list = [-2.046, 1.121, 0.575, 0.453, 0.561, -0.435, 0.538]
        self.pose_goal.position.x = self.ee_TF_list[0]
        self.pose_goal.position.y = self.ee_TF_list[1]
        self.pose_goal.position.z = self.ee_TF_list[2]

        self.pose_goal.orientation.x = self.ee_TF_list[3]
        self.pose_goal.orientation.y = self.ee_TF_list[4]
        self.pose_goal.orientation.z = self.ee_TF_list[5]
        self.pose_goal.orientation.w = self.ee_TF_list[6]

        self.robot_arm.set_pose_target(self.pose_goal)
        self.robot_arm.go(True)
Esempio n. 11
0
    def __init__(self):
        # 初始化move_group的API
        moveit_commander.roscpp_initialize(sys.argv)

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

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

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

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

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

        arm.set_planning_time(0.05)  # 规划时间限制

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

        print "[INFO] Current pose:", arm.get_current_pose(eef_link).pose

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

        self.group = arm
        self.robot = robot
        self.eef_link = eef_link
        self.reference_frame = reference_frame
        self.moveit_commander = moveit_commander
    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)
        
        rospy.init_node('moveit_speed_demo')
                
        # Initialize the move group for the right arm
        arm = MoveGroupCommander('manipulator')
        
        # Get the name of the end-effector link
        end_effector_link = arm.get_end_effector_link()
        
        # Allow replanning to increase the odds of a solution
        arm.allow_replanning(True)
        
        # Allow some leeway in position(meters) and orientation (radians)
        arm.set_goal_position_tolerance(0.001)
        arm.set_goal_orientation_tolerance(0.01)
        
        # Start the arm in the "home" pose stored in the SRDF file
        arm.set_named_target('home')
        arm.go()
        
        # 设置机械臂的目标位置,使用六轴的位置数据进行描述(单位:弧度)
        joint_positions = [0.391410, -0.676384, -0.376217, 0.0, 1.052834, 0.454125]
        arm.set_joint_value_target(joint_positions)
                 
        # 控制机械臂完成运动
        arm.go()

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

        # Get back the planned trajectory
        arm.set_joint_value_target(joint_positions)
        traj = arm.plan()
        
        # Scale the trajectory speed by a factor of 0.25
        new_traj = scale_trajectory_speed(traj, 0.25)

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

        arm.set_named_target('home')
        arm.go()

        # Exit MoveIt cleanly
        moveit_commander.roscpp_shutdown()
        
        # Exit the script
        moveit_commander.os._exit(0)
Esempio n. 13
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('rest')
        right_arm.go()
        
        # Move to the named "straight_forward" pose stored in the SRDF file
        right_arm.set_named_target('wave')
        right_arm.go()
        rospy.sleep(2)
        
        # Move back to the resting position at roughly 1/4 speed
        right_arm.set_named_target('rest')

        # Get back the planned trajectory
        traj = right_arm.plan()
        
        # Set the trajectory speed
        new_traj = create_tracking_trajectory(traj, 1.0)
        
        rospy.loginfo(new_traj)

        # 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)
    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)
Esempio n. 15
0
    def __init__(self):
        # 初始化move_group的API
        moveit_commander.roscpp_initialize(sys.argv)

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

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

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

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

        # 控制夹爪张开
        gripper.set_joint_value_target(GRIPPER_OPEN)
        gripper.go()

        rospy.sleep(0.5)

        # 控制机械臂到目标位置
        arm.set_named_target('test_2')
        arm.go()

        # 控制夹爪闭合
        gripper.set_joint_value_target(GRIPPER_CLOSED)
        gripper.go()

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

        # 关闭并退出moveit
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)
    def __init__(self):
        # 初始化move_group的API
        moveit_commander.roscpp_initialize(sys.argv)

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

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

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

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

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

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

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

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

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

        print start_pose

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

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

        # 设置路点数据,并加入路点列表
        wpose = deepcopy(start_pose)
        wpose.position.z -= 0.2
        waypoints.append(deepcopy(wpose))

        wpose.position.x += 0.1
        waypoints.append(deepcopy(wpose))

        wpose.position.y += 0.1
        waypoints.append(deepcopy(wpose))

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

        # 设置机器臂当前的状态作为运动初始状态
        arm.set_start_state_to_current_state()

        # 尝试规划一条笛卡尔空间下的路径,依次通过所有路点
        while fraction < 1.0 and attempts < maxtries:
            (plan, fraction) = arm.compute_cartesian_path(
                waypoints,  # waypoint poses,路点列表
                0.01,  # eef_step,终端步进值
                0.0,  # jump_threshold,跳跃阈值
                True)  # avoid_collisions,避障规划

            # 尝试次数累加
            attempts += 1

            # 打印运动规划进程
            if attempts % 10 == 0:
                rospy.loginfo("Still trying after " + str(attempts) +
                              " attempts...")

        # 如果路径规划成功(覆盖率100%),则开始控制机械臂运动
        if fraction == 1.0:
            rospy.loginfo("Path computed successfully. Moving the arm.")
            arm.execute(plan)
            rospy.loginfo("Path execution complete.")
        # 如果路径规划失败,则打印失败信息
        else:
            rospy.loginfo("Path planning failed with only " + str(fraction) +
                          " success after " + str(maxtries) + " attempts.")

        rospy.sleep(1)

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

        # 关闭并退出moveit
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)
Esempio n. 17
0
if __name__ == '__main__':

    # Initialize the move_group API
    moveit_commander.roscpp_initialize(sys.argv)
    rospy.init_node("cobra_move_pick")

    arm = MoveGroupCommander(PLANNING_GROUP)

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

    # move arm to start position
    arm.set_named_target(START_POSITION)
    arm.go()
    rospy.sleep(2)

    # Action Clients for Pick
    rospy.loginfo("Connecting to pickup AS")
    pickup_ac = actionlib.SimpleActionClient('/pickup', PickupAction)
    pickup_ac.wait_for_server()
    rospy.loginfo("Successfully connected")

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

    # clean world
    rospy.loginfo("Cleaning world objects")
Esempio n. 18
0
    punch_pose.pose.position.z = 0
    temp_eul = None

    #detach/remove current scene objects
    print 'removing objects...'
    robot.detach_object("bowl")
    rospy.sleep(0.5)
    scene.remove_world_object("bowl")
    scene.remove_world_object("punch")
    scene.remove_world_object("flat")

    rospy.sleep(0.5)

    #reset the gripper and arm position to home
    robot.set_start_state_to_current_state()
    robot.set_named_target("home")
    robot.go()
    startpose = PoseStamped()
    startpose.header.frame_id = 'world'
    startpose.header.stamp = rospy.Time.now()
    startpose = robot.get_current_pose()
    startpose.pose.position.x -= 0.3
    robot.set_pose_target(startpose)
    robot.go()
#    gripper.set_start_state_to_current_state()
#    gripper.set_named_target("default")
#    gripper.go()

    #add scene objects
    print 'adding scene objects'
    scene.add_mesh("bowl", bowl_pose, "8inhemi.STL")
Esempio n. 19
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)
Esempio n. 20
0
    def callback(self, data):
        # 初始化move_group的API
        moveit_commander.roscpp_initialize(sys.argv)

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

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

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

        # 获取终端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'
        arm.set_pose_reference_frame(reference_frame)

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

        # 设置场景物体的名称
        table_id = 'table'

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

        # 设置桌面的高度
        table_ground = 0.6

        # 设置table、box1和box2的三维尺寸
        table_size = [1.5, 0.7, 0.01]

        # 将三个物体加入场景当中
        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 = 0.825
        table_pose.pose.orientation.w = 1.0
        scene.add_box(table_id, table_pose, table_size)
        rospy.sleep(1)

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

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

        # 设置机械臂的运动目标位置,进行避障规划
        '''orient = transformations.quaternion_from_euler(0, 1.57075, 0)
        target_pose = PoseStamped()
        target_pose.header.frame_id = reference_frame
        target_pose.pose.position.x = -0.1
        target_pose.pose.position.y = 0.5
        target_pose.pose.position.z = 1.05
        target_pose.pose.orientation.x = orient[0]
        target_pose.pose.orientation.y = orient[1]
        target_pose.pose.orientation.z = orient[2]
        target_pose.pose.orientation.w = orient[3]'''

        orient = transformations.quaternion_from_euler(0, 1.57075, 0)
        target_pose = data
        target_pose.pose.orientation.x = orient[0]
        target_pose.pose.orientation.y = orient[1]
        target_pose.pose.orientation.z = orient[2]
        target_pose.pose.orientation.w = orient[3]

        # 控制机械臂运动到目标位置
        self.gripper_control(0)
        result = None
        arm.set_pose_target(target_pose, end_effector_link)
        arm.go()

        rospy.loginfo('Start grasping')
        target_pose.pose.position.z -= 0.05
        arm.set_pose_target(target_pose, end_effector_link)
        arm.go()

        self.gripper_control(0.36)
        '''arm.attach_object( box2_id)
        arm.set_named_target('home')
        arm.go()'''

        arm.set_named_target('home')
        arm.go()
        self.gripper_control(0)
        rospy.loginfo('Grasping done')
        rospy.sleep(2)
        judge_client(True)
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)
Esempio n. 21
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)
        rospy.Subscriber("chatter", Float64MultiArray, callback)

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

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

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

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

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

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

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

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

        # 设置场景物体的名称
        table_id = 'table'
        # cy_id = 'cy'
        box1_id = 'box1'
        box2_id = 'box2'
        box3_id = 'box3'
        sphere_id = 'sphere'

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

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

        # arm.set_named_target('start')
        # arm.go()
        # rospy.sleep(1)

        gripper.set_joint_value_target([0.05])
        gripper.go()
        rospy.sleep(0)

        # 设置桌面的高度
        table_ground = 0.37

        # 设置table、box1和box2的三维尺寸
        table_size = [0.2, 0.3, 0.01]
        box1_size = [0.01, 0.01, 0.19]
        box2_size = [0.01, 0.01, 0.19]
        box3_size = [0.005, 0.01, 0.3]
        sphere_R = 0.01
        error = 0.03

        # 将三个物体加入场景当中
        table_pose = PoseStamped()
        table_pose.header.frame_id = reference_frame
        table_pose.pose.position.x = -table_size[0] / 2.0
        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)
        #scene.add_cylinder

        box1_pose = PoseStamped()
        box1_pose.header.frame_id = reference_frame
        box1_pose.pose.position.x = -0.09
        box1_pose.pose.position.y = table_size[0] / 2.0
        box1_pose.pose.position.z = 0.18 + 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.09
        box2_pose.pose.position.y = -table_size[0] / 2.0
        box2_pose.pose.position.z = 0.18 + box1_size[2] / 2.0
        box2_pose.pose.orientation.w = 1.0
        scene.add_box(box2_id, box2_pose, box2_size)

        # box3_pose = PoseStamped()
        # box3_pose.header.frame_id = reference_frame
        # box3_pose.pose.position.x = pos_aim[0]
        # box3_pose.pose.position.y = pos_aim[1]
        # box3_pose.pose.position.z = box3_size[2]/2.0+0.1
        # box3_pose.pose.orientation.w = 1.0
        # scene.add_box(box3_id, box3_pose, box3_size)

        sphere_pose = PoseStamped()
        sphere_pose.header.frame_id = reference_frame
        sphere_pose.pose.position.x = pos_aim[0] + 0
        sphere_pose.pose.position.y = pos_aim[1]
        sphere_pose.pose.position.z = pos_aim[2]
        sphere_pose.pose.orientation.w = 1.0
        scene.add_sphere(sphere_id, sphere_pose, sphere_R)

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

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

        # rospy.INFO("waiting...")
        # if(Recive_FLAG==1):
        #     rospy.INFO("OK!")

        # 设置机械臂的运动目标位置
        target_pose = PoseStamped()
        target_pose.header.frame_id = reference_frame
        target_pose.pose.position.x = pos_aim[0] - error
        target_pose.pose.position.y = pos_aim[1]
        target_pose.pose.position.z = pos_aim[2]
        # target_pose.pose.orientation.w = 1.0
        #####0.3
        # 0.2     0.2     0.2     0.15     0.15
        # 0.12    0.11    0.12    0.15     0.15
        # 0.30    0.245   0.35    0.35     0.25
        # 控制机械臂运动到目标位置
        arm.set_pose_target(target_pose, end_effector_link)
        arm.go()
        rospy.sleep(1)

        gripper.set_joint_value_target([0.03])
        gripper.go()
        rospy.sleep(1)

        # # 控制机械臂终端向x移动5cm
        arm.shift_pose_target(0, 0.01, end_effector_link)
        arm.go()
        rospy.sleep(1)

        gripper.set_joint_value_target([0.05])
        gripper.go()
        rospy.sleep(1)

        # # 设置机械臂的运动目标位置,进行避障规划
        # target_pose2 = PoseStamped()
        # target_pose2.header.frame_id = reference_frame
        # target_pose2.pose.position.x = 0.15
        # target_pose2.pose.position.y = 0 #-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('init')
        arm.go()

        # 关闭并退出moveit
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)

        rospy.spin()
    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)
Esempio n. 23
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)
    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)
Esempio n. 25
0
#!/usr/bin/env python

import rospy
from moveit_commander import RobotCommander, PlanningSceneInterface, MoveGroupCommander

if __name__ == '__main__':

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

	robot = MoveGroupCommander("sia5d");
	gripper = MoveGroupCommander("gripper");
	rospy.sleep(1)
	
	while not rospy.is_shutdown():	
	
		gripper.set_named_target('pinch')
		gripper.go()
		rospy.sleep(1)
		gripper.set_named_target('default')
		gripper.go()
		rospy.sleep(1)
		
	rospy.spin()
Esempio n. 26
0
    def __init__(self):

        roscpp_initialize(sys.argv)
        rospy.init_node('moveit_py_demo', anonymous=True)
      
        scene = PlanningSceneInterface()
        robot = RobotCommander()
       
        right_arm = MoveGroupCommander("right_arm")
        right_gripper = MoveGroupCommander("right_gripper")
       
        eef = right_arm.get_end_effector_link()
       
        rospy.sleep(2)
       
        scene.remove_attached_object("right_gripper_link", "part")

   
        # clean the scene
        scene.remove_world_object("table")
        scene.remove_world_object("part")
   
        right_arm.set_named_target("start1")
        right_arm.go()
      
        right_gripper.set_named_target("open")
        right_gripper.go()
      
        rospy.sleep(1)
   
        # publish a demo scene
        p = PoseStamped()
        p.header.frame_id = robot.get_planning_frame()
   
        # add a table
        #p.pose.position.x = 0.42
        #p.pose.position.y = -0.2
        #p.pose.position.z = 0.3
        #scene.add_box("table", p, (0.5, 1.5, 0.6))

   
        # add an object to be grasped
        p.pose.position.x = 0.15

        p.pose.position.y = -0.12
        p.pose.position.z = 0.7
        scene.add_box("part", p, (0.07, 0.01, 0.2))
      
        rospy.sleep(1)
             
        g = Grasp()
        g.id = "test"
        start_pose = PoseStamped()
        start_pose.header.frame_id = FIXED_FRAME
   
        # start the gripper in a neutral pose part way to the target
        start_pose.pose.position.x = 0.0130178
        start_pose.pose.position.y = -0.125155
        start_pose.pose.position.z = 0.597653
        start_pose.pose.orientation.x = 0.0
        start_pose.pose.orientation.y = 0.388109
        start_pose.pose.orientation.z = 0.0
        start_pose.pose.orientation.w = 0.921613
          
        right_arm.set_pose_target(start_pose)
        right_arm.go()
       
        rospy.sleep(2)

        # generate a list of grasps
        #grasps = self.make_grasps(start_pose)
   
        #result = False
        #n_attempts = 0
       
        # repeat until will succeed
        #while result == False:
            #result = robot.right_arm.pick("part", grasps)      
            #n_attempts += 1
            #print "Attempts: ", n_attempts
            #rospy.sleep(0.2)
          
        rospy.spin()
        roscpp_shutdown()
class SmartGrasper(object):
    """
    This is the helper library to easily access the different functionalities of the simulated robot
    from python.
    """

    __last_joint_state = None
    __current_model_name = "cricket_ball"
    __path_to_models = "/root/.gazebo/models/"
    
    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()

    def reset_world(self):
        """
        Resets the object poses in the world and the robot joint angles.
        """
        self.__switch_ctrl.call(start_controllers=[], 
                                stop_controllers=["hand_controller", "arm_controller", "joint_state_controller"], 
                                strictness=SwitchControllerRequest.BEST_EFFORT)
        self.__pause_physics.call()
        
        joint_names = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 
                       'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint', 'H1_F1J1', 'H1_F1J2', 'H1_F1J3', 
                       'H1_F2J1', 'H1_F2J2', 'H1_F2J3', 'H1_F3J1', 'H1_F3J2', 'H1_F3J3']
        joint_positions = [1.2, 0.3, -1.5, -0.5, -1.5, 0.0, 0.0, -0.3, 0.0, 0.0, -0.3, 0.0, 0.0, -0.3, 0.0]
        
        self.__set_model.call(model_name="smart_grasping_sandbox", 
                              urdf_param_name="robot_description",
                              joint_names=joint_names, 
                              joint_positions=joint_positions)
            
        timer = Timer(0.0, self.__start_ctrl)
        timer.start()
        
        time.sleep(0.1)
        self.__unpause_physics.call()

        self.__reset_world.call()

    def get_object_pose(self):
        """
        Gets the pose of the ball in the world frame.
        
        @return The pose of the ball.
        """
        return self.__get_pose_srv.call(self.__current_model_name, "world").pose

    def get_tip_pose(self):
        """
        Gets the current pose of the robot's tooltip in the world frame.
        @return the tip pose
        """
        return self.arm_commander.get_current_pose(self.arm_commander.get_end_effector_link()).pose

    def move_tip_absolute(self, target):
        """
        Moves the tooltip to the absolute target in the world frame

        @param target is a geometry_msgs.msg.Pose
        @return True on success
        """
        self.arm_commander.set_start_state_to_current_state()
        self.arm_commander.set_pose_targets([target])
        plan = self.arm_commander.plan()
        if not self.arm_commander.execute(plan):
            return False
        return True
        
    def move_tip(self, x=0., y=0., z=0., roll=0., pitch=0., yaw=0.):
        """
        Moves the tooltip in the world frame by the given x,y,z / roll,pitch,yaw. 

        @return True on success
        """
        transform = PyKDL.Frame(PyKDL.Rotation.RPY(pitch, roll, yaw),
                                PyKDL.Vector(-x, -y, -z))
        
        tip_pose = self.get_tip_pose()
        tip_pose_kdl = posemath.fromMsg(tip_pose)
        final_pose = toMsg(tip_pose_kdl * transform)
            
        self.arm_commander.set_start_state_to_current_state()
        self.arm_commander.set_pose_targets([final_pose])
        plan = self.arm_commander.plan()
        if not  self.arm_commander.execute(plan):
            return False
        return True

    def send_command(self, command, duration=0.2):
        """
        Send a dictionnary of joint targets to the arm and hand directly.
        
        @param command: a dictionnary of joint names associated with a target:
                        {"H1_F1J1": -1.0, "shoulder_pan_joint": 1.0}
        @param duration: the amount of time it will take to get there in seconds. Needs to be bigger than 0.0
        """
        hand_goal = None
        arm_goal = None
        
        for joint, target in command.items():
            if "H1" in joint:
                if not hand_goal:
                    hand_goal = FollowJointTrajectoryGoal()
                    
                    point = JointTrajectoryPoint()
                    point.time_from_start = rospy.Duration.from_sec(duration)
                    
                    hand_goal.trajectory.points.append(point)
                    
                hand_goal.trajectory.joint_names.append(joint)
                hand_goal.trajectory.points[0].positions.append(target)
            else:
                if not arm_goal:
                    arm_goal = FollowJointTrajectoryGoal()
                    
                    point = JointTrajectoryPoint()
                    point.time_from_start = rospy.Duration.from_sec(duration)
                    
                    arm_goal.trajectory.points.append(point)
                    
                arm_goal.trajectory.joint_names.append(joint)
                arm_goal.trajectory.points[0].positions.append(target)
        
        if arm_goal:
            self.__arm_traj_client.send_goal_and_wait(arm_goal)
        if hand_goal:
            self.__hand_traj_client.send_goal_and_wait(hand_goal)

    def get_current_joint_state(self):
        """
        Gets the current state of the robot. 
        
        @return joint positions, velocity and efforts as three dictionnaries
        """
        joints_position = {n: p for n, p in
                           zip(self.__last_joint_state.name,
                               self.__last_joint_state.position)}
        joints_velocity = {n: v for n, v in
                           zip(self.__last_joint_state.name,
                           self.__last_joint_state.velocity)}
        joints_effort = {n: v for n, v in
                         zip(self.__last_joint_state.name, 
                         self.__last_joint_state.effort)}
        return joints_position, joints_velocity, joints_effort

    def open_hand(self):
        """
        Opens the hand.
        
        @return True on success
        """
        self.hand_commander.set_named_target("open")
        plan = self.hand_commander.plan()
        if not self.hand_commander.execute(plan, wait=True):
            return False

        return True

    def close_hand(self):
        """
        Closes the hand.
        
        @return True on success
        """
        self.hand_commander.set_named_target("close")
        plan = self.hand_commander.plan()
        if not self.hand_commander.execute(plan, wait=True):
            return False

        return True

    def check_fingers_collisions(self, enable=True):
        """
        Disables or enables the collisions check between the fingers and the objects / table
        
        @param enable: set to True to enable / False to disable
        @return True on success
        """
        objects = ["cricket_ball__link", "drill__link", "cafe_table__link"]

        while self.__pub_planning_scene.get_num_connections() < 1:
            rospy.loginfo("waiting for someone to subscribe to the /planning_scene")
            rospy.sleep(0.1)

        request = PlanningSceneComponents(components=PlanningSceneComponents.ALLOWED_COLLISION_MATRIX)
        response = self.__get_planning_scene(request)
        acm = response.scene.allowed_collision_matrix

        for object_name in objects:
            if object_name not in acm.entry_names:
                # add object to allowed collision matrix
                acm.entry_names += [object_name]
                for row in range(len(acm.entry_values)):
                    acm.entry_values[row].enabled += [False]

                new_row = deepcopy(acm.entry_values[0])
                acm.entry_values += {new_row}

        for index_entry_values, entry_values in enumerate(acm.entry_values):
            if "H1_F" in acm.entry_names[index_entry_values]:
                for index_value, _ in enumerate(entry_values.enabled):
                    if acm.entry_names[index_value] in objects:
                        if enable:
                            acm.entry_values[index_entry_values].enabled[index_value] = False
                        else:
                            acm.entry_values[index_entry_values].enabled[index_value] = True
            elif acm.entry_names[index_entry_values] in objects:
                for index_value, _ in enumerate(entry_values.enabled):
                    if "H1_F" in acm.entry_names[index_value]:
                        if enable:
                            acm.entry_values[index_entry_values].enabled[index_value] = False
                        else:
                            acm.entry_values[index_entry_values].enabled[index_value] = True
        
        planning_scene_diff = PlanningScene(is_diff=True, allowed_collision_matrix=acm)
        self.__pub_planning_scene.publish(planning_scene_diff)
        rospy.sleep(1.0)

        return True

    def pick(self):
        """
        Does its best to pick the ball.
        """
        rospy.loginfo("Moving to Pregrasp")
        self.open_hand()
        time.sleep(0.1)
        
        ball_pose = self.get_object_pose()
        ball_pose.position.z += 0.5
        
        #setting an absolute orientation (from the top)
        quaternion = quaternion_from_euler(-pi/2., 0.0, 0.0)
        ball_pose.orientation.x = quaternion[0]
        ball_pose.orientation.y = quaternion[1]
        ball_pose.orientation.z = quaternion[2]
        ball_pose.orientation.w = quaternion[3]
        
        self.move_tip_absolute(ball_pose)
        time.sleep(0.1)
        
        rospy.loginfo("Grasping")
        self.move_tip(y=-0.164)
        time.sleep(0.1)
        self.check_fingers_collisions(False)
        time.sleep(0.1)
        self.close_hand()
        time.sleep(0.1)
        
        rospy.loginfo("Lifting")
        for _ in range(5):
            self.move_tip(y=0.01)
            time.sleep(0.1)
            
        self.check_fingers_collisions(True)
        
    def swap_object(self, new_model_name):
        """
        Replaces the current object with a new one.Replaces
        
        @new_model_name the name of the folder in which the object is (e.g. beer)
        """
        try:
            self.__delete_model(self.__current_model_name)
        except:
            rospy.logwarn("Failed to delete: " + self.__current_model_name)
        
        try:
            sdf = None
            initial_pose = Pose()
            initial_pose.position.x = 0.15
            initial_pose.position.z = 0.82
            
            with open(self.__path_to_models + new_model_name + "/model.sdf", "r") as model:
                sdf = model.read()
            res = self.__spawn_model(new_model_name, sdf, "", initial_pose, "world")
            rospy.logerr( "RES: " + str(res) )
            self.__current_model_name = new_model_name
        except:
            rospy.logwarn("Failed to delete: " + self.__current_model_name)
   
   

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

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

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

        return arm_target

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

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

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

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

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

        self.hand_commander.attach_object("cricket_ball__link")

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

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

    def __start_ctrl(self):
        rospy.loginfo("STARTING CONTROLLERS")
        self.__switch_ctrl.call(start_controllers=["hand_controller", "arm_controller", "joint_state_controller"], 
                                stop_controllers=[], strictness=1)
                                
    def __joint_state_cb(self, msg):
        self.__last_joint_state = msg
    scene.add_mesh("pen",p,resourcepath+'objects/pen.dae')
    rospy.sleep(1)
     
    # print the existing groups 
    robot = RobotCommander()
    print "Available groups: ",robot.get_group_names()
    
    # setup the arm group and its planner
    arm = MoveGroupCommander("arm")
    arm.set_start_state_to_current_state()
    arm.set_planner_id("RRTstarkConfigDefault") 
    arm.set_planning_time(5.0)
    arm.detach_object("pen")
    
    # set the arm to a safe target
    arm.set_named_target("gamma")
    # plan and execute the motion 
    arm.go()
    
    # setup the hand group and its planner
    hand = MoveGroupCommander("hand")
    hand.set_start_state_to_current_state()
    hand.set_planner_id("LBKPIECEkConfigDefault") 
    hand.set_planning_time(10.0)
    
    # set the hand to a safe target
    hand.set_named_target("open")
    # plan and execute the motion
    hand.go()

    # load grasps
Esempio n. 29
0
class ArmTracker:
    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)
            
                    
    def update_target_pose(self, target):
        self.target = target

    def relax_all_servos(self):
        command = 'rosrun donaxi_dynamixels arbotix_relax_all_servos.py'
        args = shlex.split(command)
        subprocess.Popen(args)
           
    def shutdown(self):
        # Stop any further target messages from being processed
        self.target_subscriber.unregister()
        
        # Stop any current arm movement
        self.right_arm.stop()
        
        # Move to the resting position
        self.right_arm.set_named_target('resting')
        self.right_arm.go()
        
        # Relax the servos
        self.relax_all_servos()
        
        os._exit(0) 
Esempio n. 30
0
    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)
        
        rospy.init_node('moveit_demo')
        
        # Construct the initial scene object
        scene = PlanningSceneInterface()
        
        # Create a scene publisher to push changes to the scene
        self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
        
        # Create a dictionary to hold object colors
        self.colors = dict()
        
        # Pause for the scene to get ready
        rospy.sleep(1)
                        
        # Initialize the move group for the right arm
        right_arm = MoveGroupCommander('right_arm')
        # Initialize the move group for the left arm
        left_arm = MoveGroupCommander('left_arm')

        right_arm.set_planner_id("KPIECEkConfigDefault");
        left_arm.set_planner_id("KPIECEkConfigDefault");
        rospy.sleep(1)
        
        # Get the name of the end-effector link
        end_effector_link = right_arm.get_end_effector_link()
        
        # Allow some leeway in position (meters) and orientation (radians)
        right_arm.set_goal_position_tolerance(0.01)
        right_arm.set_goal_orientation_tolerance(0.05)
       
        # Allow replanning to increase the odds of a solution
        right_arm.allow_replanning(True)
        
        # Set the reference frame for pose targets
        reference_frame = 'base_footprint'
        
        # Set the right arm reference frame accordingly
        right_arm.set_pose_reference_frame(reference_frame)
        
        # Allow 5 seconds per planning attempt
        right_arm.set_planning_time(5)
        
        # Give each of the scene objects a unique name
        table_id = 'table'
        box1_id = 'box1'
        box2_id = 'box2'
        
        # Remove leftover objects from a previous run
        scene.remove_world_object(table_id)
        scene.remove_world_object(box1_id)
        scene.remove_world_object(box2_id)
        
        # Give the scene a chance to catch up
        rospy.sleep(1)
        
        # Start the arm in the "resting" pose stored in the SRDF file
        #left_arm.set_named_target('left_start')
        #left_arm.go()

        # Start the arm in the "resting" pose stored in the SRDF file
        right_arm.set_named_target('right_start')
        right_arm.go()
        
        rospy.sleep(2)
        
        # Set the height of the table off the ground
        table_ground = 0.75
        
        # Set the length, width and height of the table and boxes
        table_size = [0.2, 0.7, 0.01]
        box1_size = [0.1, 0.05, 0.05]
        box2_size = [0.05, 0.05, 0.15]
        
        # Add a table top and two boxes to the scene
        table_pose = PoseStamped()
        table_pose.header.frame_id = reference_frame
        table_pose.pose.position.x = 0.56
        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.51
        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.49
        box2_pose.pose.position.y = 0.15
        box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0
        box2_pose.pose.orientation.w = 1.0   
        scene.add_box(box2_id, box2_pose, box2_size)
        
        # Make the table red and the boxes orange
        self.setColor(table_id, 0.8, 0, 0, 1.0)
        self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
        self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
        
        # Send the colors to the planning scene
        self.sendColors()    
        
        # Set the target pose in between the boxes and above the table
        target_pose = PoseStamped()
        target_pose.header.frame_id = reference_frame
        target_pose.pose.position.x = 0.4
        target_pose.pose.position.y = 0.0
        target_pose.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05
        target_pose.pose.orientation.w = 1.0
        
        # Set the target pose for the arm
        right_arm.set_pose_target(target_pose, end_effector_link)
        
        # Move the arm to the target pose (if possible)
        right_arm.go()
        
        # Pause for a moment...
        rospy.sleep(2)
        

        # Return the arm to the "resting" pose stored in the SRDF file
        right_arm.set_named_target('right_start')
        right_arm.go()
        
        # Exit MoveIt cleanly
        moveit_commander.roscpp_shutdown()
        
        # Exit the script        
        moveit_commander.os._exit(0)
Esempio n. 31
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)
    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)

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

        if cartesian:
            # Append the pose to the waypoints list
            waypoints.append(deepcopy(wpose))
        else:
            right_arm.set_pose_target(wpose)
            right_arm.go()
            rospy.sleep(1)
         
        # Set the next waypoint to the right 0.15 meters
        wpose.position.x += 0.05
        wpose.position.y += 0.15
        wpose.position.z -= 0.15
          
        if cartesian:
            # Append the pose to the waypoints list
            waypoints.append(deepcopy(wpose))
        else:
            right_arm.set_pose_target(wpose)
            right_arm.go()
            rospy.sleep(1)
            
        if cartesian:
            # Append the pose to the waypoints list
            waypoints.append(deepcopy(start_pose))
        else:
            right_arm.set_pose_target(start_pose)
            right_arm.go()
            rospy.sleep(1)
            
        if cartesian:
            fraction = 0.0
            maxtries = 100
            attempts = 0
            
            # Set the internal state to the current state
            right_arm.set_start_state_to_current_state()
     
            # Plan the Cartesian path connecting the waypoints
            while fraction < 1.0 and attempts < maxtries:
                (plan, fraction) = right_arm.compute_cartesian_path (
                                        waypoints,   # waypoint poses
                                        0.01,        # eef_step
                                        0.0,         # jump_threshold
                                        True)        # avoid_collisions
                
                # Increment the number of attempts 
                attempts += 1
                
                # Print out a progress message
                if attempts % 10 == 0:
                    rospy.loginfo("Still trying after " + str(attempts) + " attempts...")
                         
            # If we have a complete plan, execute the trajectory
            if fraction == 1.0:
                rospy.loginfo("Path computed successfully. Moving the arm.")
    
                right_arm.execute(plan)
                            
                rospy.loginfo("Path execution complete.")
            else:
                rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.")  

        # Move normally back to the 'resting' position
        right_arm.set_named_target('resting')
        right_arm.go()
        rospy.sleep(1)
        
        # Shut down MoveIt cleanly
        moveit_commander.roscpp_shutdown()
        
        # Exit MoveIt
        moveit_commander.os._exit(0)
Esempio n. 33
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]
    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)
        
        rospy.init_node('moveit_demo')
        
        # Construct the initial scene object
        scene = PlanningSceneInterface()
        
        # Pause for the scene to get ready
        rospy.sleep(1)
                                
        # Initialize the MoveIt! commander for the right arm
        right_arm = MoveGroupCommander('right_arm')
                
        # Initialize the MoveIt! commander for the gripper
        right_gripper = MoveGroupCommander('right_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.01)
        right_arm.set_goal_orientation_tolerance(0.05)
       
        # Allow replanning to increase the odds of a solution
        right_arm.allow_replanning(True)
        
        # Allow 5 seconds per planning attempt
        right_arm.set_planning_time(5)
        
        # Remove leftover objects from a previous run
        scene.remove_attached_object(end_effector_link, 'tool')
        scene.remove_world_object('table') 
        scene.remove_world_object('box1')    
        scene.remove_world_object('box2')
        scene.remove_world_object('target')

        # Set the height of the table off the ground
        table_ground = 0.75
        
        # Set the length, width and height of the table
        table_size = [0.2, 0.7, 0.01]
        
        # Set the length, width and height of the object to attach
        tool_size = [0.3, 0.02, 0.02]
        
        # Create a pose for the tool relative to the end-effector
        p = PoseStamped()
        p.header.frame_id = end_effector_link
        
        scene.attach_mesh
        
        # Place the end of the object within the grasp of the gripper
        p.pose.position.x = tool_size[0] / 2.0 - 0.025
        p.pose.position.y = 0.0
        p.pose.position.z = 0.0
        
        # Align the object with the gripper (straight out)
        p.pose.orientation.x = 0
        p.pose.orientation.y = 0
        p.pose.orientation.z = 0
        p.pose.orientation.w = 1
        
        # Attach the tool to the end-effector
        scene.attach_box(end_effector_link, 'tool', p, tool_size)

        # Add a floating table top
        table_pose = PoseStamped()
        table_pose.header.frame_id = 'base_footprint'
        table_pose.pose.position.x = 0.35
        table_pose.pose.position.y = 0.0
        table_pose.pose.position.z = table_ground + table_size[2] / 2.0
        table_pose.pose.orientation.w = 1.0
        scene.add_box('table', table_pose, table_size)
        
        # Update the current state
        right_arm.set_start_state_to_current_state()

        # Move the arm with the attached object to the 'straight_forward' position
        right_arm.set_named_target('straight_forward')
        right_arm.go()
        rospy.sleep(2)  
        
        # Return the arm in the "resting" pose stored in the SRDF file
        right_arm.set_named_target('resting')
        right_arm.go()
        rospy.sleep(2)
         
        scene.remove_attached_object(end_effector_link, 'tool')   
        
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)
    def __init__(self):
        # 初始化move_group的API
        moveit_commander.roscpp_initialize(sys.argv)
        
        # 初始化ROS节点
        rospy.init_node('moveit_obstacles_demo')
        
        # 初始化场景对象
        scene = PlanningSceneInterface()
        
        # 创建一个发布场景变化信息的发布者
        self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5)
        
        # 创建一个存储物体颜色的字典对象
        self.colors = dict()
        
        # 等待场景准备就绪
        rospy.sleep(1)
                        
        # 初始化需要使用move group控制的机械臂中的arm group
        arm = MoveGroupCommander('ur_arm')
        
        # 获取终端link的名称
        end_effector_link = arm.get_end_effector_link()
        
        # 设置位置(单位:米)和姿态(单位:弧度)的允许误差
        arm.set_goal_position_tolerance(0.01)
        arm.set_goal_orientation_tolerance(0.05)
       
        # 当运动规划失败后,允许重新规划
        arm.allow_replanning(True)
        
        # 设置目标位置所使用的参考坐标系
        reference_frame = 'base_link'
        arm.set_pose_reference_frame(reference_frame)
        
        # 设置每次运动规划的时间限制:5s
        arm.set_planning_time(5)
        
        # 设置场景物体的名称
        table_id = 'table'
        box1_id = 'box1'
        box2_id = 'box2'
        
        # 移除场景中之前运行残留的物体
        scene.remove_world_object(table_id)
        scene.remove_world_object(box1_id)
        scene.remove_world_object(box2_id)    
        rospy.sleep(1)
        
        # 控制机械臂先回到初始化位置
        arm.set_named_target('home')
        arm.go()
        rospy.sleep(2)
        
        # 设置桌面的高度
        table_ground = 0.25
        
        # 设置table、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)
Esempio n. 36
0
def callback(pose):
    object_position_info = pose.position
    object_orientation_info = pose.orientation
    print object_position_info
    moveit_commander.roscpp_initialize(sys.argv)
    #rospy.init_node('moveit_cartesian', anonymous=True)
    cartesian = rospy.get_param('~cartesian', True)
                        
    #set cartesian parameters
    ur5_manipulator = MoveGroupCommander('manipulator')
    ur5_gripper = MoveGroupCommander('gripper')
    ur5_manipulator.allow_replanning(True)
    ur5_manipulator.set_pose_reference_frame('base_link')
    ur5_manipulator.set_goal_position_tolerance(0.01)
    ur5_manipulator.set_goal_orientation_tolerance(0.1)
    end_effector_link = ur5_manipulator.get_end_effector_link()    
    ur5_manipulator.set_named_target('home_j')
    ur5_manipulator.go()
    ur5_gripper.set_named_target('open')
    ur5_gripper.go()

    #get the end effort information
    start_pose = ur5_manipulator.get_current_pose(end_effector_link).pose
    print("The first waypoint:")
    print(start_pose)
    #define waypoints
    waypoints = []   
    waypoints.append(start_pose)

    wpose = deepcopy(start_pose)
    wpose.position.z = object_position_info.z+0.25
    wpose.position.x = object_position_info.x
    wpose.position.y = object_position_info.y
    print("The second waypoint:")
    print(wpose) 
    waypoints.append(deepcopy(wpose))
    print(" ")
    print(waypoints) 


    if cartesian:
        fraction = 0.0   
        maxtries = 100   
        attempts = 0     
        while fraction < 1.0 and attempts < maxtries:
            (plan, fraction) =  ur5_manipulator.compute_cartesian_path (
                                        waypoints,   
                                        0.01,        
                                        0.0,         
                                        True)              
            attempts += 1
                
                
            if attempts % 10 == 0:
                rospy.loginfo("Still trying after " + str(attempts) + " attempts...")
                         
            
        if fraction == 1.0:
            rospy.loginfo("Path computed successfully. Moving the arm.")
            ur5_manipulator.execute(plan)
            rospy.sleep(2)
            rospy.loginfo("Path execution complete.")
            
        else:
            rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.")  

    rospy.sleep(3)
    ur5_gripper.set_named_target("close")
    plan = ur5_gripper.go()
    rospy.sleep(2)
    ur5_manipulator.set_named_target('home_j')
    ur5_manipulator.go()
    rospy.sleep(3)
        
        
    moveit_commander.roscpp_shutdown()
Esempio n. 37
0
    def __init__(self):
        # 初始化move_group的API
        moveit_commander.roscpp_initialize(sys.argv)

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

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

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

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

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

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

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

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

        star_pose = PoseStamped()
        star_pose.header.frame_id = reference_frame
        star_pose.header.stamp = rospy.Time.now()
        star_pose.pose.position.x = 0.40
        star_pose.pose.position.y = 0.0
        star_pose.pose.position.z = 0.12
        star_pose.pose.orientation.w = 1.0

        # 设置机械臂终端运动的目标位姿
        arm.set_pose_target(star_pose, end_effector_link)
        arm.go()

        radius = 0.1
        centerY = 0.0
        centerX = 0.40 - radius

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

        pose_temp = star_pose
        for th in numpy.arange(0, 6.2831855, 1.2566371):
            pose_temp.pose.position.y = -(centerY + radius * math.sin(th))
            pose_temp.pose.position.x = centerX + radius * math.cos(th)
            pose_temp.pose.position.z = 0.113
            wpose = deepcopy(pose_temp.pose)
            starPoints.append(deepcopy(wpose))

        # 将圆弧上的路径点加入列表
        waypoints.append(starPoints[0])
        waypoints.append(starPoints[2])
        waypoints.append(starPoints[4])
        waypoints.append(starPoints[1])
        waypoints.append(starPoints[3])
        waypoints.append(starPoints[0])

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

        # 设置机器臂当前的状态作为运动初始状态
        arm.set_start_state_to_current_state()

        # 尝试规划一条笛卡尔空间下的路径,依次通过所有路点,完成圆弧轨迹
        while fraction < 1.0 and attempts < maxtries:
            (plan, fraction) = arm.compute_cartesian_path(
                waypoints,  # waypoint poses,路点列表
                0.01,  # eef_step,终端步进值
                0.0,  # jump_threshold,跳跃阈值
                True)  # avoid_collisions,避障规划

            # 尝试次数累加
            attempts += 1

            # 打印运动规划进程
            if attempts % 10 == 0:
                rospy.loginfo("Still trying after " + str(attempts) +
                              " attempts...")

        # 如果路径规划成功(覆盖率100%),则开始控制机械臂运动
        if fraction == 1.0:
            rospy.loginfo("Path computed successfully. Moving the arm.")
            arm.execute(plan)
            rospy.loginfo("Path execution complete.")
        # 如果路径规划失败,则打印失败信息
        else:
            rospy.loginfo("Path planning failed with only " + str(fraction) +
                          " success after " + str(maxtries) + " attempts.")

        rospy.sleep(1)

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

        # 关闭并退出moveit
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)
Esempio n. 38
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('manipulator')
        
        # 当运动规划失败后,允许重新规划
        arm.allow_replanning(True)
        
        # 设置目标位置所使用的参考坐标系
        arm.set_pose_reference_frame('base_link')
                
        # 设置位置(单位:米)和姿态(单位:弧度)的允许误差
        arm.set_goal_position_tolerance(0.001)
        arm.set_goal_orientation_tolerance(0.1)
        
        # 获取终端link的名称
        end_effector_link = arm.get_end_effector_link()
           
        # 设置每次运动规划的时间限制:1s
        arm.set_planning_time(1)
                             
        # 控制机械臂运动到之前设置的“home”姿态
        arm.set_named_target('home')
        arm.go()
        
        # 获取当前位姿数据为机械臂运动的起始位姿
        start_pose = arm.get_current_pose(end_effector_link).pose
 
        # ---------------------  第一段轨迹生成,关节空间插值  ---------------------#
        
        # 设置机械臂的目标位置,使用六轴的位置数据进行描述(单位:弧度)
        joint_positions = [0, -1, 1, 0, -1, 0]
        arm.set_joint_value_target(joint_positions)
                 
        # 控制机械臂完成运动
        arm.go()
        rospy.sleep(0.5)
        # 获取当前位姿数据为机械臂运动的起始位姿
        start_pose = arm.get_current_pose(end_effector_link).pose
        # ---------------------  第一段轨迹生成,关节空间插值  ---------------------#
       
        # 初始化路点列表
        waypoints = []
                
        # 将初始位姿加入路点列表
        if cartesian:
            waypoints.append(start_pose)
            
        # 设置第二个路点数据,并加入路点列表
        # 第二个路点需要向后运动0.3米,向右运动0.3米
        wpose = deepcopy(start_pose)

        wpose.orientation.x = 0
        wpose.orientation.y = 0.707106781186547
        wpose.orientation.z = 0
        wpose.orientation.w = 0.707106781186547
        wpose.position.x -= 0.2
        # wpose.position.y -= 0.4
        wpose.position.z -= 1

        if cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            arm.set_pose_target(wpose)
            arm.go()
            rospy.sleep(1)
         
        # 设置第三个路点数据,并加入路点列表
        wpose.orientation.x = 0
        wpose.orientation.y = 0.707106781186547
        wpose.orientation.z = 0
        wpose.orientation.w = 0.707106781186547
        # wpose.position.x -= 0.5
        wpose.position.y -= 0.5
        wpose.position.z += 0.2
        # wpose.position.x += 0.15
        # wpose.position.y += 0.1
        # wpose.position.z -= 0.15
          
        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)
    def __init__(self):
        # 初始化move_group的API
        moveit_commander.roscpp_initialize(sys.argv)

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

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

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

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

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

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

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

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

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

        # 角度弧度转换
        j1 = 90.0 / 180 * math.pi
        j2 = -18.6 / 180 * math.pi
        j3 = -28.1 / 180 * math.pi
        j4 = 1.0 / 180 * math.pi
        j5 = 47.6 / 180 * math.pi
        j6 = -0.9 / 180 * math.pi

        # 设置机械臂的目标位置,使用六轴的位置数据进行描述(单位:弧度)
        joint_positions = [j1, j2, j3, j4, j5, j6]
        arm.set_joint_value_target(joint_positions)

        arm.go()
        rospy.sleep(1)

        # 向下按压门把手
        current_pose = arm.get_current_joint_values()
        current_pose[4] += (20.0 / 180.0) * math.pi
        arm.set_joint_value_target(current_pose)
        arm.go()
        rospy.sleep(1)

        #推开门
        current_pose = arm.get_current_joint_values()
        current_pose[0] -= (42.0 / 180.0) * math.pi
        current_pose[1] += (2.0 / 180.0) * math.pi
        current_pose[2] -= (11.4 / 180.0) * math.pi
        arm.set_joint_value_target(current_pose)
        arm.go()
        rospy.sleep(1)

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

        # 关闭并退出moveit
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)
    def __init__(self):

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

        arm = MoveGroupCommander(ARM_GROUP)

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

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

        waypoints = list()

        pose = PoseStamped().pose

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

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

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

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

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

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

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

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

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

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

        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)
Esempio n. 41
0
        pass

    ### Create a handle for the Planning Scene Interface
    psi = PlanningSceneInterface()
    rospy.sleep(1.0)

    ### Create a handle for the Move Group Commander
    mgc = MoveGroupCommander("arm")
    rospy.sleep(1.0)

    ### Add virtual obstacle
    pose = gen_pose(pos=[-0.2, -0.1, 1.2])
    psi.add_box("box", pose, size=(0.15, 0.15, 0.6))
    rospy.sleep(1.0)

    ### Move to stored joint position
    mgc.set_named_target("left")
    mgc.go()

    ### Move to Cartesian position
    goal_pose = gen_pose(pos=[0.123, -0.417, 1.361],
                         euler=[3.1415, 0.0, 1.5707])
    mgc.go(goal_pose.pose)

    ### Move Cartesian linear
    goal_pose.pose.position.z -= 0.1
    (traj, frac) = mgc.compute_cartesian_path([goal_pose.pose], 0.01, 4, True)
    mgc.execute(traj)

    print "Done"
Esempio n. 42
0
	p2.pose.orientation.z = b2[2]
	p2.pose.orientation.w = b2[3]

	#detach/remove current scene objects
	print 'removing objects...'
	robot.detach_object("bowl")
	rospy.sleep(1)
	scene.remove_world_object("bowl")
	scene.remove_world_object("punch")
	scene.remove_world_object("glovebox")

	rospy.sleep(2)

	#reset the gripper and arm position to home
	robot.set_start_state_to_current_state()
	robot.set_named_target("start_glove")
	robot.go()
	gripper.set_start_state_to_current_state()
	gripper.go()

	#add scene objects	
	print 'adding scene objects'
	scene.add_mesh("bowl", p, "8inhemi.STL")
	scene.add_mesh("punch", p1, "punch.STL")
	scene.add_mesh("glovebox", p2, "GloveBox.stl")
	print 'attaching bowl...'
	robot.attach_object("bowl")
	rospy.sleep(2)
	currentbowlpose = p;

	gripper.set_named_target("pinch")
Esempio n. 43
0
class SrRobotCommander(object):
    """
    Base class for hand and arm commanders
    """
    def __init__(self, name):
        """
        Initialize MoveGroupCommander object
        @param name - name of the MoveIt group
        """
        self._name = name
        self._move_group_commander = MoveGroupCommander(name)

        self._robot_commander = RobotCommander()

        self._robot_name = self._robot_commander._r.get_robot_name()

        self.refresh_named_targets()

        self._warehouse_name_get_srv = rospy.ServiceProxy(
            "get_robot_state", GetState)
        self._planning_scene = PlanningSceneInterface()

        self._joint_states_lock = threading.Lock()
        self._joint_states_listener = \
            rospy.Subscriber("joint_states", JointState,
                             self._joint_states_callback, queue_size=1)
        self._joints_position = {}
        self._joints_velocity = {}
        self._joints_effort = {}
        self._joints_state = None
        self._clients = {}
        self.__plan = None

        self._controllers = {}

        rospy.wait_for_service('compute_ik')
        self._compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
        self._forward_k = rospy.ServiceProxy('compute_fk', GetPositionFK)

        controller_list_param = rospy.get_param("/move_group/controller_list")

        # create dictionary with name of controllers and corresponding joints
        self._controllers = {
            item["name"]: item["joints"]
            for item in controller_list_param
        }

        self._set_up_action_client(self._controllers)

        self.tf_buffer = tf2_ros.Buffer()
        self.listener = tf2_ros.TransformListener(self.tf_buffer)

        threading.Thread(None, rospy.spin)

    def set_planner_id(self, planner_id):
        self._move_group_commander.set_planner_id(planner_id)

    def set_num_planning_attempts(self, num_planning_attempts):
        self._move_group_commander.set_num_planning_attempts(
            num_planning_attempts)

    def set_planning_time(self, seconds):
        self._move_group_commander.set_planning_time(seconds)

    def get_end_effector_pose_from_named_state(self, name):
        state = self._warehouse_name_get_srv(name, self._robot_name).state
        return self.get_end_effector_pose_from_state(state)

    def get_end_effector_pose_from_state(self, state):
        header = Header()
        fk_link_names = [self._move_group_commander.get_end_effector_link()]
        header.frame_id = self._move_group_commander.get_pose_reference_frame()
        response = self._forward_k(header, fk_link_names, state)
        return response.pose_stamped[0]

    def get_planning_frame(self):
        return self._move_group_commander.get_planning_frame()

    def set_pose_reference_frame(self, reference_frame):
        self._move_group_commander.set_pose_reference_frame(reference_frame)

    def get_group_name(self):
        return self._name

    def refresh_named_targets(self):
        self._srdf_names = self.__get_srdf_names()
        self._warehouse_names = self.__get_warehouse_names()

    def set_max_velocity_scaling_factor(self, value):
        self._move_group_commander.set_max_velocity_scaling_factor(value)

    def set_max_acceleration_scaling_factor(self, value):
        self._move_group_commander.set_max_acceleration_scaling_factor(value)

    def allow_looking(self, value):
        self._move_group_commander.allow_looking(value)

    def allow_replanning(self, value):
        self._move_group_commander.allow_replanning(value)

    def execute(self):
        """
        Executes the last plan made.
        """
        if self.check_plan_is_valid():
            self._move_group_commander.execute(self.__plan)
            self.__plan = None
        else:
            rospy.logwarn("No plans were made, not executing anything.")

    def execute_plan(self, plan):
        if self.check_given_plan_is_valid(plan):
            self._move_group_commander.execute(plan)
            self.__plan = None
        else:
            rospy.logwarn("Plan is not valid, not executing anything.")

    def move_to_joint_value_target(self,
                                   joint_states,
                                   wait=True,
                                   angle_degrees=False):
        """
        Set target of the robot's links and moves to it.
        @param joint_states - dictionary with joint name and value. It can
        contain only joints values of which need to be changed.
        @param wait - should method wait for movement end or not
        @param angle_degrees - are joint_states in degrees or not
        """
        joint_states_cpy = copy.deepcopy(joint_states)

        if angle_degrees:
            joint_states_cpy.update(
                (joint, radians(i)) for joint, i in joint_states_cpy.items())
        self._move_group_commander.set_start_state_to_current_state()
        self._move_group_commander.set_joint_value_target(joint_states_cpy)
        self._move_group_commander.go(wait=wait)

    def plan_to_joint_value_target(self, joint_states, angle_degrees=False):
        """
        Set target of the robot's links and plans.
        @param joint_states - dictionary with joint name and value. It can
        contain only joints values of which need to be changed.
        @param angle_degrees - are joint_states in degrees or not
        This is a blocking method.
        """
        joint_states_cpy = copy.deepcopy(joint_states)

        if angle_degrees:
            joint_states_cpy.update(
                (joint, radians(i)) for joint, i in joint_states_cpy.items())
        self._move_group_commander.set_start_state_to_current_state()
        self._move_group_commander.set_joint_value_target(joint_states_cpy)
        self.__plan = self._move_group_commander.plan()
        return self.__plan

    def check_plan_is_valid(self):
        """
        Checks if current plan contains a valid trajectory
        """
        return (self.__plan is not None
                and len(self.__plan.joint_trajectory.points) > 0)

    def check_given_plan_is_valid(self, plan):
        """
        Checks if given plan contains a valid trajectory
        """
        return (plan is not None and len(plan.joint_trajectory.points) > 0)

    def get_robot_name(self):
        return self._robot_name

    def named_target_in_srdf(self, name):
        return name in self._srdf_names

    def set_named_target(self, name):
        if name in self._srdf_names:
            self._move_group_commander.set_named_target(name)
        elif (name in self._warehouse_names):
            response = self._warehouse_name_get_srv(name, self._robot_name)

            active_names = self._move_group_commander._g.get_active_joints()
            joints = response.state.joint_state.name
            positions = response.state.joint_state.position
            js = {}

            for n, this_name in enumerate(joints):
                if this_name in active_names:
                    js[this_name] = positions[n]
            self._move_group_commander.set_joint_value_target(js)
        else:
            rospy.logerr("Unknown named state '%s'..." % name)
            return False
        return True

    def get_named_target_joint_values(self, name):
        output = dict()

        if (name in self._srdf_names):
            output = self._move_group_commander.\
                           _g.get_named_target_values(str(name))

        elif (name in self._warehouse_names):
            js = self._warehouse_name_get_srv(
                name, self._robot_name).state.joint_state

            for x, n in enumerate(js.name):
                if n in self._move_group_commander._g.get_joints():
                    output[n] = js.position[x]

        else:
            rospy.logerr("No target named %s" % name)

            return None

        return output

    def get_end_effector_link(self):
        return self._move_group_commander.get_end_effector_link()

    def get_current_pose(self, reference_frame=None):
        """
        Get the current pose of the end effector.
        @param reference_frame - The desired reference frame in which end effector pose should be returned.
        If none is passed, it will use the planning frame as reference.
        @return geometry_msgs.msg.Pose() - current pose of the end effector
        """
        if reference_frame is not None:
            try:
                trans = self.tf_buffer.lookup_transform(
                    reference_frame,
                    self._move_group_commander.get_end_effector_link(),
                    rospy.Time(0), rospy.Duration(5.0))
                current_pose = geometry_msgs.msg.Pose()
                current_pose.position.x = trans.transform.translation.x
                current_pose.position.y = trans.transform.translation.y
                current_pose.position.z = trans.transform.translation.z
                current_pose.orientation.x = trans.transform.rotation.x
                current_pose.orientation.y = trans.transform.rotation.y
                current_pose.orientation.z = trans.transform.rotation.z
                current_pose.orientation.w = trans.transform.rotation.w
                return current_pose
            except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
                    tf2_ros.ExtrapolationException):
                rospy.logwarn(
                    "Couldn't get the pose from " +
                    self._move_group_commander.get_end_effector_link() +
                    " in " + reference_frame + " reference frame")
            return None
        else:
            return self._move_group_commander.get_current_pose().pose

    def get_current_state(self):
        """
        Get the current joint state of the group being used.
        @return a dictionary with the joint names as keys and current joint values
        """
        joint_names = self._move_group_commander._g.get_active_joints()
        joint_values = self._move_group_commander._g.get_current_joint_values()

        return dict(zip(joint_names, joint_values))

    def get_current_state_bounded(self):
        """
        Get the current joint state of the group being used, enforcing that they are within each joint limits.
        @return a dictionary with the joint names as keys and current joint values
        """
        current = self._move_group_commander._g.get_current_state_bounded()
        names = self._move_group_commander._g.get_active_joints()
        output = {n: current[n] for n in names if n in current}

        return output

    def get_robot_state_bounded(self):
        return self._move_group_commander._g.get_current_state_bounded()

    def move_to_named_target(self, name, wait=True):
        """
        Set target of the robot's links and moves to it
        @param name - name of the target pose defined in SRDF
        @param wait - should method wait for movement end or not
        """
        self._move_group_commander.set_start_state_to_current_state()
        if self.set_named_target(name):
            self._move_group_commander.go(wait=wait)

    def plan_to_named_target(self, name):
        """
        Set target of the robot's links and plans
        This is a blocking method.
        @param name - name of the target pose defined in SRDF
        """
        self._move_group_commander.set_start_state_to_current_state()
        if self.set_named_target(name):
            self.__plan = self._move_group_commander.plan()

    def __get_warehouse_names(self):
        try:
            list_srv = rospy.ServiceProxy("list_robot_states", ListStates)
            return list_srv("", self._robot_name).states

        except rospy.ServiceException as exc:
            rospy.logwarn("Couldn't access warehouse: " + str(exc))
            return list()

    def _reset_plan(self):
        self.__plan = None

    def _set_plan(self, plan):
        self.__plan = plan

    def __get_srdf_names(self):
        return self._move_group_commander._g.get_named_targets()

    def get_named_targets(self):
        """
        Get the complete list of named targets, from SRDF
        as well as warehouse poses if available.
        @return list of strings containing names of targets.
        """
        return self._srdf_names + self._warehouse_names

    def get_joints_position(self):
        """
        Returns joints position
        @return - dictionary with joints positions
        """
        with self._joint_states_lock:
            return self._joints_position

    def get_joints_velocity(self):
        """
        Returns joints velocities
        @return - dictionary with joints velocities
        """
        with self._joint_states_lock:
            return self._joints_velocity

    def _get_joints_effort(self):
        """
        Returns joints effort
        @return - dictionary with joints efforts
        """
        with self._joint_states_lock:
            return self._joints_effort

    def get_joints_state(self):
        """
        Returns joints state
        @return - JointState message
        """
        with self._joint_states_lock:
            return self._joints_state

    def run_joint_trajectory(self, joint_trajectory):
        """
        Moves robot through all joint states with specified timeouts
        @param joint_trajectory - JointTrajectory class object. Represents
        trajectory of the joints which would be executed.
        """
        plan = RobotTrajectory()
        plan.joint_trajectory = joint_trajectory
        self._move_group_commander.execute(plan)

    def make_named_trajectory(self, trajectory):
        """
        Makes joint value trajectory from specified by named poses (either from
        SRDF or from warehouse)
        @param trajectory - list of waypoints, each waypoint is a dict with
                            the following elements (n.b either name or joint_angles is required)
                            - name -> the name of the way point
                            - joint_angles -> a dict of joint names and angles
                            - interpolate_time -> time to move from last wp
                            - pause_time -> time to wait at this wp
                            - degrees -> set to true if joint_angles is specified in degrees. Assumed false if absent.
        """
        current = self.get_current_state_bounded()

        joint_trajectory = JointTrajectory()
        joint_names = current.keys()
        joint_trajectory.joint_names = joint_names

        start = JointTrajectoryPoint()
        start.positions = current.values()
        start.time_from_start = rospy.Duration.from_sec(0.001)
        joint_trajectory.points.append(start)

        time_from_start = 0.0

        for wp in trajectory:

            joint_positions = None
            if 'name' in wp.keys():
                joint_positions = self.get_named_target_joint_values(
                    wp['name'])
            elif 'joint_angles' in wp.keys():
                joint_positions = copy.deepcopy(wp['joint_angles'])
                if 'degrees' in wp.keys() and wp['degrees']:
                    for joint, angle in joint_positions.iteritems():
                        joint_positions[joint] = radians(angle)

            if joint_positions is None:
                rospy.logerr(
                    "Invalid waypoint. Must contain valid name for named target or dict of joint angles."
                )
                return None

            new_positions = {}

            for n in joint_names:
                new_positions[n] = joint_positions[
                    n] if n in joint_positions else current[n]

            trajectory_point = JointTrajectoryPoint()
            trajectory_point.positions = [
                new_positions[n] for n in joint_names
            ]

            current = new_positions

            time_from_start += wp['interpolate_time']
            trajectory_point.time_from_start = rospy.Duration.from_sec(
                time_from_start)
            joint_trajectory.points.append(trajectory_point)

            if 'pause_time' in wp and wp['pause_time'] > 0:
                extra = JointTrajectoryPoint()
                extra.positions = trajectory_point.positions
                time_from_start += wp['pause_time']
                extra.time_from_start = rospy.Duration.from_sec(
                    time_from_start)
                joint_trajectory.points.append(extra)

        return joint_trajectory

    def send_stop_trajectory_unsafe(self):
        """
        Sends a trajectory of all active joints at their current position.
        This stops the robot.
        """

        current = self.get_current_state_bounded()

        trajectory_point = JointTrajectoryPoint()
        trajectory_point.positions = current.values()
        trajectory_point.time_from_start = rospy.Duration.from_sec(0.1)

        trajectory = JointTrajectory()
        trajectory.points.append(trajectory_point)
        trajectory.joint_names = current.keys()

        self.run_joint_trajectory_unsafe(trajectory)

    def run_named_trajectory_unsafe(self, trajectory, wait=False):
        """
        Moves robot through trajectory specified by named poses, either from
        SRDF or from warehouse. Runs trajectory directly via contoller.
        @param trajectory - list of waypoints, each waypoint is a dict with
                            the following elements:
                            - name -> the name of the way point
                            - interpolate_time -> time to move from last wp
                            - pause_time -> time to wait at this wp
        """
        joint_trajectory = self.make_named_trajectory(trajectory)
        if joint_trajectory is not None:
            self.run_joint_trajectory_unsafe(joint_trajectory, wait)

    def run_named_trajectory(self, trajectory):
        """
        Moves robot through trajectory specified by named poses, either from
        SRDF or from warehouse. Runs trajectory via moveit.
        @param trajectory - list of waypoints, each waypoint is a dict with
                            the following elements:
                            - name -> the name of the way point
                            - interpolate_time -> time to move from last wp
                            - pause_time -> time to wait at this wp
        """
        joint_trajectory = self.make_named_trajectory(trajectory)
        if joint_trajectory is not None:
            self.run_joint_trajectory(joint_trajectory)

    def move_to_position_target(self, xyz, end_effector_link="", wait=True):
        """
        Specify a target position for the end-effector and moves to it
        @param xyz - new position of end-effector
        @param end_effector_link - name of the end effector link
        @param wait - should method wait for movement end or not
        """
        self._move_group_commander.set_start_state_to_current_state()
        self._move_group_commander.set_position_target(xyz, end_effector_link)
        self._move_group_commander.go(wait=wait)

    def plan_to_position_target(self, xyz, end_effector_link=""):
        """
        Specify a target position for the end-effector and plans.
        This is a blocking method.
        @param xyz - new position of end-effector
        @param end_effector_link - name of the end effector link
        """
        self._move_group_commander.set_start_state_to_current_state()
        self._move_group_commander.set_position_target(xyz, end_effector_link)
        self.__plan = self._move_group_commander.plan()

    def move_to_pose_target(self, pose, end_effector_link="", wait=True):
        """
        Specify a target pose for the end-effector and moves to it
        @param pose - new pose of end-effector: a Pose message, a PoseStamped
        message or a list of 6 floats: [x, y, z, rot_x, rot_y, rot_z] or a list
        of 7 floats [x, y, z, qx, qy, qz, qw]
        @param end_effector_link - name of the end effector link
        @param wait - should method wait for movement end or not
        """
        self._move_group_commander.set_start_state_to_current_state()
        self._move_group_commander.set_pose_target(pose, end_effector_link)
        self._move_group_commander.go(wait=wait)

    def plan_to_pose_target(self,
                            pose,
                            end_effector_link="",
                            alternative_method=False):
        """
        Specify a target pose for the end-effector and plans.
        This is a blocking method.
        @param pose - new pose of end-effector: a Pose message, a PoseStamped
        message or a list of 6 floats: [x, y, z, rot_x, rot_y, rot_z] or a list
        of 7 floats [x, y, z, qx, qy, qz, qw]
        @param end_effector_link - name of the end effector link
        @param alternative_method - use set_joint_value_target instead of set_pose_target
        """
        self._move_group_commander.set_start_state_to_current_state()
        if alternative_method:
            self._move_group_commander.set_joint_value_target(
                pose, end_effector_link)
        else:
            self._move_group_commander.set_pose_target(pose, end_effector_link)
        self.__plan = self._move_group_commander.plan()
        return self.__plan

    def _joint_states_callback(self, joint_state):
        """
        The callback function for the topic joint_states.
        It will store the received joint position, velocity and efforts
        information into dictionaries
        @param joint_state - the message containing the joints data.
        """
        with self._joint_states_lock:
            self._joints_state = joint_state
            self._joints_position = {
                n: p
                for n, p in zip(joint_state.name, joint_state.position)
            }
            self._joints_velocity = {
                n: v
                for n, v in zip(joint_state.name, joint_state.velocity)
            }
            self._joints_effort = {
                n: v
                for n, v in zip(joint_state.name, joint_state.effort)
            }

    def _set_up_action_client(self, controller_list):
        """
        Sets up an action client to communicate with the trajectory controller
        """
        self._action_running = {}

        for controller_name in controller_list.keys():
            self._action_running[controller_name] = False
            service_name = controller_name + "/follow_joint_trajectory"
            self._clients[controller_name] = SimpleActionClient(
                service_name, FollowJointTrajectoryAction)
            if self._clients[controller_name].wait_for_server(
                    timeout=rospy.Duration(4)) is False:
                err_msg = 'Failed to connect to action server ({}) in 4 sec'.format(
                    service_name)
                rospy.logwarn(err_msg)

    def move_to_joint_value_target_unsafe(self,
                                          joint_states,
                                          time=0.002,
                                          wait=True,
                                          angle_degrees=False):
        """
        Set target of the robot's links and moves to it.
        @param joint_states - dictionary with joint name and value. It can
        contain only joints values of which need to be changed.
        @param time - time in s (counting from now) for the robot to reach the
        target (it needs to be greater than 0.0 for it not to be rejected by
        the trajectory controller)
        @param wait - should method wait for movement end or not
        @param angle_degrees - are joint_states in degrees or not
        """
        # self._update_default_trajectory()
        # self._set_targets_to_default_trajectory(joint_states)
        goals = {}
        joint_states_cpy = copy.deepcopy(joint_states)

        if angle_degrees:
            joint_states_cpy.update(
                (joint, radians(i)) for joint, i in joint_states_cpy.items())

        for controller in self._controllers:
            controller_joints = self._controllers[controller]
            goal = FollowJointTrajectoryGoal()
            goal.trajectory.joint_names = []
            point = JointTrajectoryPoint()
            point.positions = []

            for x in joint_states_cpy.keys():
                if x in controller_joints:
                    goal.trajectory.joint_names.append(x)
                    point.positions.append(joint_states_cpy[x])

            point.time_from_start = rospy.Duration.from_sec(time)

            goal.trajectory.points = [point]

            goals[controller] = goal

        self._call_action(goals)

        if not wait:
            return

        for i in self._clients.keys():
            if not self._clients[i].wait_for_result():
                rospy.loginfo("Trajectory not completed")

    def action_is_running(self, controller=None):
        if controller is not None:
            return self._action_running[controller]

        for controller_running in self._action_running.values():
            if controller_running:
                return True
        return False

    def _action_done_cb(self, controller, terminal_state, result):
        self._action_running[controller] = False

    def _call_action(self, goals):
        for client in self._clients:
            self._action_running[client] = True
            self._clients[client].send_goal(
                goals[client],
                lambda terminal_state, result: self._action_done_cb(
                    client, terminal_state, result))

    def run_joint_trajectory_unsafe(self, joint_trajectory, wait=True):
        """
        Moves robot through all joint states with specified timeouts
        @param joint_trajectory - JointTrajectory class object. Represents
        trajectory of the joints which would be executed.
        @param wait - should method wait for movement end or not
        """
        goals = {}
        for controller in self._controllers:
            controller_joints = self._controllers[controller]
            goal = FollowJointTrajectoryGoal()
            goal.trajectory = copy.deepcopy(joint_trajectory)

            indices_of_joints_in_this_controller = []

            for i, joint in enumerate(joint_trajectory.joint_names):
                if joint in controller_joints:
                    indices_of_joints_in_this_controller.append(i)

            goal.trajectory.joint_names = [
                joint_trajectory.joint_names[i]
                for i in indices_of_joints_in_this_controller
            ]

            for point in goal.trajectory.points:
                if point.positions:
                    point.positions = [
                        point.positions[i]
                        for i in indices_of_joints_in_this_controller
                    ]
                if point.velocities:
                    point.velocities = [
                        point.velocities[i]
                        for i in indices_of_joints_in_this_controller
                    ]
                if point.effort:
                    point.effort = [
                        point.effort[i]
                        for i in indices_of_joints_in_this_controller
                    ]

            goals[controller] = goal

        self._call_action(goals)

        if not wait:
            return

        for i in self._clients.keys():
            if not self._clients[i].wait_for_result():
                rospy.loginfo("Trajectory not completed")

    def plan_to_waypoints_target(self,
                                 waypoints,
                                 reference_frame=None,
                                 eef_step=0.005,
                                 jump_threshold=0.0):
        """
        Specify a set of waypoints for the end-effector and plans.
        This is a blocking method.
        @param reference_frame - the reference frame in which the waypoints are given
        @param waypoints - an array of poses of end-effector
        @param eef_step - configurations are computed for every eef_step meters
        @param jump_threshold - maximum distance in configuration space between consecutive points in the resulting path
        """
        old_frame = self._move_group_commander.get_pose_reference_frame()
        if reference_frame is not None:
            self.set_pose_reference_frame(reference_frame)
        (self.__plan,
         fraction) = self._move_group_commander.compute_cartesian_path(
             waypoints, eef_step, jump_threshold)
        self.set_pose_reference_frame(old_frame)

    def set_teach_mode(self, teach):
        """
        Activates/deactivates the teach mode for the robot.
        Activation: stops the the trajectory controllers for the robot, and
        sets it to teach mode.
        Deactivation: stops the teach mode and starts trajectory controllers
        for the robot.
        Currently this method blocks for a few seconds when called on a hand,
        while the hand parameters are reloaded.
        @param teach - bool to activate or deactivate teach mode
        """

        if teach:
            mode = RobotTeachModeRequest.TEACH_MODE
        else:
            mode = RobotTeachModeRequest.TRAJECTORY_MODE
        self.change_teach_mode(mode, self._name)

    def move_to_trajectory_start(self, trajectory, wait=True):
        """
        Make and execute a plan from the current state to the first state in an pre-existing trajectory
        @param trajectory - moveit_msgs/JointTrajectory
        @param wait - Bool to specify if movement should block untill finished.
        """

        if len(trajectory.points) <= 0:
            rospy.logerr("Trajectory has no points in it, can't reverse...")
            return None

        first_point = trajectory.points[0]
        end_state = dict(zip(trajectory.joint_names, first_point.positions))
        self.move_to_joint_value_target(end_state, wait=wait)

    @staticmethod
    def change_teach_mode(mode, robot):
        teach_mode_client = rospy.ServiceProxy('/teach_mode', RobotTeachMode)

        req = RobotTeachModeRequest()
        req.teach_mode = mode
        req.robot = robot
        try:
            resp = teach_mode_client(req)
            if resp.result == RobotTeachModeResponse.ERROR:
                rospy.logerr("Failed to change robot %s to mode %d", robot,
                             mode)
            else:
                rospy.loginfo("Changed robot %s to mode %d Result = %d", robot,
                              mode, resp.result)
        except rospy.ServiceException:
            rospy.logerr("Failed to call service teach_mode")

    def get_ik(self, target_pose, avoid_collisions=False, joint_states=None):
        """
        Computes the inverse kinematics for a given pose. It returns a JointState
        @param target_pose - A given pose of type PoseStamped
        @param avoid_collisions - Find an IK solution that avoids collisions. By default, this is false
        """
        service_request = PositionIKRequest()
        service_request.group_name = self._name
        service_request.ik_link_name = self._move_group_commander.get_end_effector_link(
        )
        service_request.pose_stamped = target_pose
        service_request.timeout.secs = 0.5
        service_request.avoid_collisions = avoid_collisions
        if joint_states is None:
            service_request.robot_state.joint_state = self.get_joints_state()
        else:
            service_request.robot_state.joint_state = joint_states

        try:
            resp = self._compute_ik(ik_request=service_request)
            # Check if error_code.val is SUCCESS=1
            if resp.error_code.val != 1:
                if resp.error_code.val == -10:
                    rospy.logerr("Unreachable point: Start state in collision")
                elif resp.error_code.val == -12:
                    rospy.logerr("Unreachable point: Goal state in collision")
                elif resp.error_code.val == -31:
                    rospy.logerr("Unreachable point: No IK solution")
                else:
                    rospy.logerr("Unreachable point (error: %s)" %
                                 resp.error_code)
                return
            else:
                return resp.solution.joint_state

        except rospy.ServiceException, e:
            rospy.logerr("Service call failed: %s" % e)
    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("base_link")
        
        # Create a scene publisher to push changes to the scene
        self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
        
        # Create a publisher for displaying gripper poses
        self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped)
        
        # Create a dictionary to hold object colors
        self.colors = dict()
                        
        # Initialize the move group for the right arm
        right_arm = MoveGroupCommander(GROUP_NAME_ARM)
        
        # Initialize the move group for the right gripper
        right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
        
        # Get the name of the end-effector link
        end_effector_link = right_arm.get_end_effector_link()
 
        # Allow some leeway in position (meters) and orientation (radians)
        right_arm.set_goal_position_tolerance(0.05)
        right_arm.set_goal_orientation_tolerance(0.1)

        # Allow replanning to increase the odds of a solution
        right_arm.allow_replanning(True)
        
        # Set the right arm reference frame
        right_arm.set_pose_reference_frame(REFERENCE_FRAME)
        
        # Allow 5 seconds per planning attempt
        right_arm.set_planning_time(15)
        
        # 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 = 3
                
        # Give the scene a chance to catch up
        rospy.sleep(2)
        
        # Connect to the UBR-1 find_objects action server
        rospy.loginfo("Connecting to basic_grasping_perception/find_objects...")
        find_objects = actionlib.SimpleActionClient("basic_grasping_perception/find_objects", FindGraspableObjectsAction)
        find_objects.wait_for_server()
        rospy.loginfo("...connected")
        
        # Give the scene a chance to catch up    
        rospy.sleep(1)
        
        # Start the arm in the "resting" pose stored in the SRDF file
        right_arm.set_named_target('resting')
        right_arm.go()
        
        # Open the gripper to the neutral position
        right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
        right_gripper.go()
       
        rospy.sleep(1)
        
        # Begin the main perception and pick-and-place loop
        while not rospy.is_shutdown():
            # Initialize the grasping goal
            goal = FindGraspableObjectsGoal()
            
            # We don't use the UBR-1 grasp planner as it does not work with our gripper
            goal.plan_grasps = False
            
            # Send the goal request to the find_objects action server which will trigger
            # the perception pipeline
            find_objects.send_goal(goal)
            
            # Wait for a result
            find_objects.wait_for_result(rospy.Duration(5.0))
            
            # The result will contain support surface(s) and objects(s) if any are detected
            find_result = find_objects.get_result()
    
            # Display the number of objects found
            rospy.loginfo("Found %d objects" % len(find_result.objects))
    
            # Remove all previous objects from the planning scene
            for name in scene.getKnownCollisionObjects():
                scene.removeCollisionObject(name, False)
            for name in scene.getKnownAttachedObjects():
                scene.removeAttachedObject(name, False)
            scene.waitForSync()
            
            # Clear the virtual object colors
            scene._colors = dict()
    
            # Use the nearest object on the table as the target
            target_pose = PoseStamped()
            target_pose.header.frame_id = REFERENCE_FRAME
            target_size = None
            the_object = None
            the_object_dist = 1.0
            count = -1
            
            # Cycle through all detected objects and keep the nearest one
            for obj in find_result.objects:
                count += 1
                scene.addSolidPrimitive("object%d"%count,
                                        obj.object.primitives[0],
                                        obj.object.primitive_poses[0],
                                        wait = False)

                # Choose the object nearest to the robot
                dx = obj.object.primitive_poses[0].position.x - args.x
                dy = obj.object.primitive_poses[0].position.y
                d = math.sqrt((dx * dx) + (dy * dy))
                if d < the_object_dist:
                    the_object_dist = d
                    the_object = count
                    
                    # Get the size of the target
                    target_size = obj.object.primitives[0].dimensions
                    
                    # Set the target pose
                    target_pose.pose = obj.object.primitive_poses[0]
                    
                    # We want the gripper to be horizontal
                    target_pose.pose.orientation.x = 0.0
                    target_pose.pose.orientation.y = 0.0
                    target_pose.pose.orientation.z = 0.0
                    target_pose.pose.orientation.w = 1.0
                
                # Make sure we found at least one object before setting the target ID
                if the_object != None:
                    target_id = "object%d"%the_object
                    
            # Insert the support surface into the planning scene
            for obj in find_result.support_surfaces:
                # Extend surface to floor
                height = obj.primitive_poses[0].position.z
                obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0],
                                                2.0, # make table wider
                                                obj.primitives[0].dimensions[2] + height]
                obj.primitive_poses[0].position.z += -height/2.0
    
                # Add to scene
                scene.addSolidPrimitive(obj.name,
                                        obj.primitives[0],
                                        obj.primitive_poses[0],
                                        wait = False)
                
                # Get the table dimensions
                table_size = obj.primitives[0].dimensions
                
            # If no objects detected, try again
            if the_object == None or target_size is None:
                rospy.logerr("Nothing to grasp! try again...")
                continue
    
            # Wait for the scene to sync
            scene.waitForSync()
    
            # Set colors of the table and the object we are grabbing
            scene.setColor(target_id, 223.0/256.0, 90.0/256.0, 12.0/256.0)  # orange
            scene.setColor(find_result.objects[the_object].object.support_surface, 0.3, 0.3, 0.3, 0.7)  # grey
            scene.sendColors()
    
            # Skip pick-and-place if we are just detecting objects
            if args.objects:
                if args.once:
                    exit(0)
                else:
                    continue
    
            # Get the support surface ID
            support_surface = find_result.objects[the_object].object.support_surface
                        
            # Set the support surface name to the table object
            right_arm.set_support_surface_name(support_surface)
            
            # 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 = target_pose.pose.position.x
            place_pose.pose.position.y = 0.03
            place_pose.pose.position.z = table_size[2] + target_size[2] / 2.0 + 0.015
            place_pose.pose.orientation.w = 1.0
                            
            # Initialize the grasp pose to the target pose
            grasp_pose = target_pose
             
            # Shift the grasp pose half the size of the target to center it in the gripper
            try:
                grasp_pose.pose.position.x += target_size[0] / 2.0
                grasp_pose.pose.position.y -= 0.01
                grasp_pose.pose.position.z += target_size[2] / 2.0
            except:
                rospy.loginfo("Invalid object size so skipping")
                continue

            # 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
            
            # Set the start state to the current state
            right_arm.set_start_state_to_current_state()
             
            # Repeat until we succeed or run out of attempts
            while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
                result = right_arm.pick(target_id, grasps)
                n_attempts += 1
                rospy.loginfo("Pick attempt: " +  str(n_attempts))
                rospy.sleep(1.0)
             
            # 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)
                
                # Set the start state to the current state
                #right_arm.set_start_state_to_current_state()
                  
                # Repeat until we succeed or run out of attempts
                while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts:
                    for place in places:
                        result = right_arm.place(target_id, place)
                        if result == MoveItErrorCodes.SUCCESS:
                            break
                    n_attempts += 1
                    rospy.loginfo("Place attempt: " +  str(n_attempts))
                    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.")

            rospy.sleep(2)
            
            # Open the gripper to the neutral position
            right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
            right_gripper.go()
            
            rospy.sleep(2)
            
            # Return the arm to the "resting" pose stored in the SRDF file
            right_arm.set_named_target('resting')
            right_arm.go()
             
            rospy.sleep(2)
            
            # Give the servos a rest
            arbotix_relax_all_servos()
            
            rospy.sleep(2)
        
            if args.once:
                # Shut down MoveIt cleanly
                moveit_commander.roscpp_shutdown()
                
                # Exit the script
                moveit_commander.os._exit(0)
Esempio n. 45
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('robot')
        gripper = MoveGroupCommander("gripper")
        # 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.1)
        arm.set_goal_position_tolerance(0.01)
        arm.set_goal_orientation_tolerance(0.1)  #(0.523599)
        # Get the name of the end-effector link

        end_effector_link = arm.get_end_effector_link()

        gripper.set_named_target("GripperOpen")
        gripper.go(wait=True)
        rospy.sleep(3)  #5

        gripper.set_named_target("GripperClose")
        gripper.go(wait=True)
        rospy.sleep(1)

        # # Start in the "cartesian_z" (demo) configuration stored in the SRDF file
        arm.set_named_target('cartesian_z')
        # # Plan and execute a trajectory to the goal configuration
        arm.go(wait=True)
        rospy.sleep(5)

        # Execute the position again in order to start from there - !!!!workaround
        # arm.set_named_target('cartesian_z')
        # arm.go(wait=True)

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

        # Set the next waypoint back 0.2 meters and right 0.2 meters
        wpose.position.x -= 0.0  #05#0.01
        wpose.position.y -= 0.0
        wpose.position.z -= 0.25
        if cartesian:
            # Append the pose to the waypoints list
            waypoints.append(deepcopy(wpose))
        else:
            arm.set_pose_target(wpose)
            arm.go()
            rospy.sleep(1)

        # Set the next waypoint to the right 0.15 meters
        wpose.position.x += 0.0  #0.05#0.01
        wpose.position.y += 0.0
        wpose.position.z += 0.38
        if cartesian:
            # Append the pose to the waypoints list
            waypoints.append(deepcopy(wpose))
        else:
            arm.set_pose_target(wpose)
            arm.go()
            rospy.sleep(1)
            # Set the next waypoint to the right 0.15 meters
        wpose.position.x += 0.0  #0.05#0.01
        wpose.position.y += 0.0
        wpose.position.z -= 0.38

        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)

        wpose.position.x += 0.0
        wpose.position.y += 0.0
        wpose.position.z += 0.25

        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  #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.01,  # eef_step
                    0.0,  # jump_threshold
                    True)  # avoid_collisions

                # Increment the number of attempts
                attempts += 1

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

            # If we have a complete plan, execute the trajectory
            if fraction == 1.0:
                rospy.loginfo("Path computed successfully. Moving the arm.")

                arm.execute(plan, wait=True)

                rospy.loginfo("Path execution complete.")
            else:
                rospy.loginfo("Path planning failed with only " +
                              str(fraction) + " success after " +
                              str(maxtries) + " attempts.")

        # Move normally back to the 'transport_position' position
        # arm.set_named_target('cartesian_z')
        # arm.go(wait=True)
        rospy.sleep(22)
        arm.set_named_target('transport_position')
        arm.go(wait=True)
        rospy.sleep(27)
        gripper.set_named_target("GripperOpen")
        gripper.go(wait=True)
        rospy.sleep(5)

        gripper.set_named_target("GripperClose")
        gripper.go(wait=True)
        rospy.sleep(5)

        arm.set_named_target('transport_position')
        arm.go(wait=True)
        rospy.sleep(27)

        # Shut down MoveIt cleanly
        moveit_commander.roscpp_shutdown()

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

        # Allow replanning to increase the odds of a solution
        right_arm.allow_replanning(True)
        
        # Set the right arm reference frame
        right_arm.set_pose_reference_frame(REFERENCE_FRAME)
        
        # Allow 10 seconds per planning attempt
        right_arm.set_planning_time(10)
        
        # Set a limit on the number of pick attempts before bailing
        max_pick_attempts = 10
        
        # Set a limit on the number of place attempts
        max_place_attempts = 5
                
        # Give the scene a chance to catch up
        rospy.sleep(2)

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

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

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

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

        # Shut down MoveIt cleanly
        moveit_commander.roscpp_shutdown()
        
        # Exit the script
        moveit_commander.os._exit(0)
Esempio n. 47
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 arm
        arm = MoveGroupCommander(GROUP_NAME_ARM)

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

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

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

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

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

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

        # Set a limit on the number of pick attempts before bailing
        max_pick_attempts = 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
        arm.set_named_target('left_arm_up')
        arm.go()

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

        rospy.sleep(1)

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

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

        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.13
        box2_pose.pose.position.z = table_ground + table_size[
            2] + box2_size[2] / 2.0
        box2_pose.pose.orientation.w = 1.0
        #scene.add_box(box2_id, box2_pose, box2_size)

        # Set the target pose in between the boxes and on the table
        target_pose = PoseStamped()
        target_pose.header.frame_id = REFERENCE_FRAME
        target_pose.pose.position.x = 0.24
        target_pose.pose.position.y = 0.275
        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
        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

        # 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
        grasp_pose.pose.position.x = 0.12792118579 + .1
        grasp_pose.pose.position.y = 0.285290879999 + 0.05
        grasp_pose.pose.position.z = 0.120301181892
        #grasp_pose.pose.orientation =

        # Generate a list of grasps
        grasps = self.make_grasps(grasp_pose, [target_id, table_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)
            break

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

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

        # 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 = 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:
                # Return the arm to the "resting" pose stored in the SRDF file
                arm.set_named_target('left_arm_rest')
                arm.go()

                # Open the gripper to the open position
                gripper.set_joint_value_target(GRIPPER_OPEN)
                gripper.go()
        else:
            rospy.loginfo("Pick operation failed after " + str(n_attempts) +
                          " attempts.")

        rospy.sleep(1)

        # Shut down MoveIt cleanly
        moveit_commander.roscpp_shutdown()

        # Exit the script
        moveit_commander.os._exit(0)
Esempio n. 48
0
	while rospy.get_time() == 0.0: pass
	
	### Create a handle for the Planning Scene Interface
	psi = PlanningSceneInterface()
	rospy.sleep(1.0)
	
	### Create a handle for the Move Group Commander
	mgc = MoveGroupCommander("arm")
	rospy.sleep(1.0)
	
	
	### Add virtual obstacle
	pose = gen_pose(pos=[-0.2, -0.1, 1.2])
	psi.add_box("box", pose, size=(0.15, 0.15, 0.6))
	rospy.sleep(1.0)
	
	### Move to stored joint position
	mgc.set_named_target("left")
	mgc.go()
	
	### Move to Cartesian position
	goal_pose = gen_pose(pos=[0.123, -0.417, 1.361], euler=[3.1415, 0.0, 1.5707])
	mgc.go(goal_pose.pose)
	
	### Move Cartesian linear
	goal_pose.pose.position.z -= 0.1
	(traj,frac) = mgc.compute_cartesian_path([goal_pose.pose], 0.01, 4, True)
	mgc.execute(traj)
	
	print "Done"
Esempio n. 49
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.gripper_pose_pub = rospy.Publisher('gripper_pose',
                                                PoseStamped,
                                                queue_size=10)

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

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

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

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

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

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

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

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

        # 设置pick和place阶段的最大尝试次数
        max_pick_attempts = 5
        max_place_attempts = 5
        rospy.sleep(2)

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

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

        # 移除场景中之前与机器臂绑定的物体
        scene.remove_attached_object(GRIPPER_FRAME, target_id)
        rospy.sleep(1)

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

        # 控制夹爪张开
        gripper.set_joint_value_target(GRIPPER_OPEN)
        gripper.go()
        rospy.sleep(1)

        # 设置桌面的高度
        table_ground = 0.2

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

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

        box1_pose = PoseStamped()
        box1_pose.header.frame_id = REFERENCE_FRAME
        box1_pose.pose.position.x = 0.31
        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.29
        box2_pose.pose.position.y = -0.4
        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)

        # 设置目标物体的尺寸
        target_size = [0.04, 0.04, 0.05]

        # 设置目标物体的位置,位于桌面之上两个盒子之间
        target_pose = PoseStamped()
        target_pose.header.frame_id = REFERENCE_FRAME
        target_pose.pose.position.x = 0.32
        target_pose.pose.position.y = 0.0
        target_pose.pose.position.z = table_ground + table_size[
            2] + target_size[2] / 2.0
        target_pose.pose.orientation.w = 1.0

        # 将抓取的目标物体加入场景中
        scene.add_box(target_id, target_pose, target_size)

        # 将目标物体设置为黄色
        self.setColor(target_id, 0.9, 0.9, 0, 1.0)

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

        # 设置支持的外观
        arm.set_support_surface_name(table_id)

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

        # 将目标位置设置为机器人的抓取目标位置
        grasp_pose = target_pose

        # 生成抓取姿态
        grasps = self.make_grasps(grasp_pose, [target_id])

        # 将抓取姿态发布,可以在rviz中显示
        for grasp in grasps:
            self.gripper_pose_pub.publish(grasp.grasp_pose)
            rospy.sleep(0.2)

        # 追踪抓取成功与否,以及抓取的尝试次数
        result = None
        n_attempts = 0

        # 重复尝试抓取,直道成功或者超多最大尝试次数
        while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
            n_attempts += 1
            rospy.loginfo("Pick attempt: " + str(n_attempts))
            result = arm.pick(target_id, grasps)
            rospy.sleep(0.2)

        # 如果pick成功,则进入place阶段
        if result == MoveItErrorCodes.SUCCESS:
            result = None
            n_attempts = 0

            # 生成放置姿态
            places = self.make_places(place_pose)

            # 重复尝试放置,直道成功或者超多最大尝试次数
            while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts:
                n_attempts += 1
                rospy.loginfo("Place attempt: " + str(n_attempts))
                for place in places:
                    result = arm.place(target_id, place)
                    if result == MoveItErrorCodes.SUCCESS:
                        break
                rospy.sleep(0.2)

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

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

        # 控制夹爪回到张开的状态
        gripper.set_joint_value_target(GRIPPER_OPEN)
        gripper.go()
        rospy.sleep(1)

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

        rospy.init_node('moveit_obstacles_demo')

        # Construct the initial scene object
        scene = PlanningSceneInterface()

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

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

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

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

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

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

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

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

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

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

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

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

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

        rospy.sleep(2)

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

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

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

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

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

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

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

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

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

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

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

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

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

        # Exit MoveIt cleanly
        moveit_commander.roscpp_shutdown()

        # Exit the script
        moveit_commander.os._exit(0)
Esempio n. 51
0
class ArmTracker:
    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 left arm
        self.left_arm = MoveGroupCommander(GROUP_NAME_ARM)

        # Initialize the move group for the left gripper
        left_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 left arm reference frame accordingly
        self.left_arm.set_pose_reference_frame(self.reference_frame)

        # Allow replanning to increase the chances of a solution
        self.left_arm.allow_replanning(False)

        # Set a position tolerance in meters
        self.left_arm.set_goal_position_tolerance(0.05)

        # Set an orientation tolerance in radians
        self.left_arm.set_goal_orientation_tolerance(0.2)

        # What is the end effector link?
        self.ee_link = self.left_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
        left_gripper.set_joint_value_target(GRIPPER_CLOSED)

        # Plan and execute the gripper motion
        left_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 left_arm shoulder lift frame
            #target_arm = self.listener.transformPose('left_arm_shoulder_pan_link', target)
            target_arm = self.listener.transformPose('left_rotate', 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.left_arm.set_named_target('l_start')
                self.left_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.left_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.left_arm.set_start_state_to_current_state()

            # Set the target pose for the end-effector
            self.left_arm.set_pose_target(target_ee, self.ee_link)

            # Plan and execute the trajectory
            success = self.left_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)

    def update_target_pose(self, target):
        self.target = target

    def relax_all_servos(self):
        command = 'rosrun rbx2_dynamixels arbotix_relax_all_servos.py'
        args = shlex.split(command)
        subprocess.Popen(args)

    def shutdown(self):
        # Stop any further target messages from being processed
        self.target_subscriber.unregister()

        # Stop any current arm movement
        self.left_arm.stop()

        # Move to the r_start position
        self.left_arm.set_named_target('r_start')
        self.left_arm.go()

        # Relax the servos
        self.relax_all_servos()

        os._exit(0)
Esempio n. 52
0
    def __init__(self, script_path):
        rospy.init_node('show_time')
        rospy.on_shutdown(self.cleanup)
        # Initialize a number of parameters and variables for nav locations

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

        # Initialize the move group for the arms
        right_arm = MoveGroupCommander('right_arm')
        left_arm = MoveGroupCommander('left_arm')
        right_gripper = MoveGroupCommander('right_gripper')
        left_gripper = MoveGroupCommander('left_gripper')
        

        # Set the default TTS voice to use
        self.voice = rospy.get_param("~voice", "voice_don_diphone")
        self.robot = rospy.get_param("~robot", "robbie")
        self.start =(Pose(Point(1.7, 0, 0.0), Quaternion(0.0, 0.0, 1.000, 0.018)))
        self.left =(Pose(Point(1.960, -1.854, 0.0), Quaternion(0.0, 0.0, 0.916, 0.401)))
        self.right =(Pose(Point(2.123, 1.592, 0.0), Quaternion(0.0, 0.0, 0.877, -0.480)))
        self.auto_dock =(Pose(Point(0.5, 0, 0.0), Quaternion(0.0, 0.0, 0.002, 1.000)))
       

        

        self.client = SimpleActionClient("move_base", MoveBaseAction)
        self.client.wait_for_server()
        # Create the sound client object
        self.soundhandle = SoundClient()
        
        # Wait a moment to let the client connect to the
        # sound_play server
        rospy.sleep(2)
        
        # Make sure any lingering sound_play processes are stopped.
        self.soundhandle.stopAll()
        # Set the wave file path if used
        self.wavepath = rospy.get_param("~wavepath", script_path + "/../sounds")
         #define afternoon and morning
        self.noon = strftime("%p:", localtime())
        if self.noon == "AM:":
            self.noon1 = "Goood Morning  "
        else:
            self.noon1 ="Good After Noon   "
        self.local = strftime("%H:%M:", localtime())
        #local time
        self.local = strftime("%H:%M:", localtime())

        self.soundhandle.say(self.noon1 + self.robot +"   is on line" + " the time is   " + self.local, self.voice)

        right_arm.set_named_target('r_travel')
        right_arm.go()

        left_arm.set_named_target('l_travel')
        left_arm.go()

        rospy.sleep(5)
        self.move_to(self.start)
        self.soundhandle.say("Welcome to SHOW time.     This is where I get to demonstrate my capabilities" , self.voice)
        rospy.sleep(6)

        self.soundhandle.say("I can move to my left", self.voice)
        rospy.sleep(2)
        self.move_to(self.left)
        
        self.soundhandle.say("now back to the start position", self.voice)
        rospy.sleep(2)
        self.move_to(self.start)
        
        self.soundhandle.say("I can move to my right", self.voice)
        rospy.sleep(2)
        self.move_to(self.right)
        
        self.soundhandle.say("And again back to the start position", self.voice)
        rospy.sleep(2)
        self.move_to(self.start)

        self.soundhandle.say("I once caught a fish this big", self.voice)
        right_arm.set_named_target('r_fish')
        right_arm.go()
        left_arm.set_named_target('l_fish')
        left_arm.go()
        #rospy.sleep(2)

        self.soundhandle.say("Thank you for your time ", self.voice)
        right_arm.set_named_target('right_start')
        right_arm.go()
        left_arm.set_named_target('left_start')
        left_arm.go()

        #rospy.sleep(2)

        #rospy.sleep(2)
        self.move_to(self.auto_dock)
        # add auto dock sequence here

        # Shut down MoveIt cleanly
        moveit_commander.roscpp_shutdown()
        
        # Exit the script
        moveit_commander.os._exit(0)