def main(): ## Initialization ## rospy.init_node("floor_to_table") trajectory_generator = TrajectoryGenerator() trajectory_executor = TrajectoryExecutor() grasping_client = GripperInterfaceClient() torso_action = FollowTrajectoryClient("torso_controller", ["torso_lift_joint"]) head_action = PointHeadClient() move_base = MoveBaseClient() ## Raise Torso ## torso_action.move_to([0.35, ]) ## Point head at floor ## head_action.look_at(0.7, .0, 0.95, "base_link") ## Declare sought object ## grasping_client.set_target_object("Box", [0.5, 0.0, 0.8, 0, 0, 0], [0.09, 0.04, .195]) ## Recognize bench object ## trajectories = grasping_client.find_graspable_object() ## Pick bench object ## trajectories = grasping_client.plan_pick() trajectory_executor.execute_trajectories(trajectories, error_checking = False) grasping_client.recognizeAttachedObject() tuck_state = JointSpaceGoal([1.32, 1.40, -0.2, 1.72, 0.0, 1.55, 0.0]) trajectory = trajectory_generator.generate_trajectories(tuck_state, get_approval=True) trajectory_executor.execute_trajectories(trajectory) torso_action.move_to([0.05, ]) move_base.goto(0, 0, 1.57) head_action.look_at(0.9, -0.7, 0.2, "base_link") grasping_client.removeCollisionObjects() trajectories = grasping_client.plan_place([0.70, -0.15, 0.65, 0.0, 0.0, 0.0]) trajectory_executor.execute_trajectories(trajectories, error_checking=False) trajectory_generator.clear_virtual_arm_state() trajectory = trajectory_generator.generate_trajectories(tuck_state, get_approval=True) trajectory_executor.execute_trajectories(trajectory)
def main(): ## Initialization ## rospy.init_node("push_button") trajectory_generator = TrajectoryGenerator() trajectory_executor = TrajectoryExecutor() button_finder = ButtonLocationSubscriber() arm_joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"] arm_action = FollowTrajectoryClient('arm_controller', arm_joints) torso_action = FollowTrajectoryClient('torso_controller', ["torso_lift_joint"]) rospy.loginfo("Waiting for get_position_ik...") rospy.wait_for_service('compute_ik') get_position_ik = rospy.ServiceProxy('compute_ik', GetPositionIK) rospy.loginfo("...connected.") ## Map button world frame pose to base_link frame pose of gripper ## button_center = None #approx coords: 0.796538999146 -0.0150021952933 0.264828975569 while button_center is None: rospy.sleep(1.0) button_center = button_finder.getButtonLocation() desired_gripper_translation = .16645 pre_push_pose = PoseGoal([button_center[0]-desired_gripper_translation, button_center[1], button_center[2]+.2, 0,0,0]) print pre_push_pose.pose pre_push_goals = [pre_push_pose] pre_push_trajectories = trajectory_generator.generate_trajectories(pre_push_goals, get_approval=True) pre_push_state = trajectory_generator.get_virtual_arm_state() trajectory_executor.execute_trajectories(pre_push_trajectories, error_checking=False) post_push_posture = GripperPostureGoal(1.0) tuck_state = JointSpaceGoal([1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]) gripper_goal = [post_push_posture] trajectory_generator.clear_virtual_arm_state() post_push_trajectories = trajectory_generator.generate_trajectories(gripper_goal, get_approval=False) trajectory_executor.execute_trajectories(post_push_trajectories, error_checking = False) torso_action.move_to([.30, ]) post_push_goals = [tuck_state] trajectory_generator.clear_virtual_arm_state() post_push_trajectories = trajectory_generator.generate_trajectories(post_push_goals, get_approval=True) trajectory_executor.execute_trajectories(post_push_trajectories, error_checking = False) torso_action.move_to([0.05, ])
def main(): ## Initialization ## rospy.init_node("push_button") trajectory_generator = TrajectoryGenerator() trajectory_executor = TrajectoryExecutor() button_finder = ButtonLocationSubscriber() arm_joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"] arm_action = FollowTrajectoryClient('arm_controller', arm_joints) #scene_pub = rospy.Publisher('planning_scene', # PlanningScene, # queue_size=10) rospy.loginfo("Waiting for get_position_ik...") rospy.wait_for_service('compute_ik') get_position_ik = rospy.ServiceProxy('compute_ik', GetPositionIK) rospy.loginfo("...connected.") ## Map button world frame pose to base_link frame pose of gripper ## button_center = None while button_center is None: rospy.sleep(1.0) button_center = button_finder.getButtonLocation() #button_center = [1.08, 0.06, 1.02] desired_gripper_translation = 0.08 push_depth = -.015 gripper_offset = .16645 desired_gripper_translation += gripper_offset push_depth -= gripper_offset pre_push_pose = PoseGoal([button_center[0]-desired_gripper_translation, button_center[1], button_center[2], 0,0,0]) print pre_push_pose.pose pre_push_posture = GripperPostureGoal(-0.5) push = [button_center[0] + push_depth, button_center[1], button_center[2]] post_push_retreat = [button_center[0]-desired_gripper_translation, button_center[1], button_center[2]] post_push_posture = GripperPostureGoal(1.0) post_push_pose = PoseGoal([post_push_retreat[0], post_push_retreat[1], post_push_retreat[2], 0, 0, 0]) tuck_state = JointSpaceGoal([1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]) pre_push_goals = [pre_push_pose, pre_push_posture] pre_push_trajectories = trajectory_generator.generate_trajectories(pre_push_goals, get_approval=True) pre_push_state = trajectory_generator.get_virtual_arm_state() print pre_push_state.joint_state.position trajectory_executor.execute_trajectories(pre_push_trajectories, error_checking=False) ps = PoseStamped() ps.header.frame_id = "base_link" ps.pose.position.x = push[0] ps.pose.position.y = push[1] ps.pose.position.z = push[2] ps.pose.orientation.w = 1.0 ik_request = PositionIKRequest() ik_request.group_name = 'arm' ik_request.pose_stamped = ps while True: response = get_position_ik(ik_request) print response user = raw_input("Accept position? Y/N") if user == 'Y': break push_state = response.solution.joint_state.position[6:13] #state = get_position_ik(group = 'arm') print response.solution.joint_state.name[6:13] print push_state print pre_push_state raw_input('Press ENTER to proceed') arm_action.move_to(push_state) ps = PoseStamped() ps.header.frame_id = "base_link" ps.pose.position.x = push[0] - .08 ps.pose.position.y = push[1] ps.pose.position.z = push[2] ps.pose.orientation.w = 1.0 ik_request = PositionIKRequest() ik_request.group_name = 'arm' ik_request.pose_stamped = ps while True: response = get_position_ik(ik_request) print response user = raw_input("Accept position? Y/N") if user == 'Y': break inter_state_1 = response.solution.joint_state.position[6:13] print inter_state_1 """ ps = PoseStamped() ps.header.frame_id = "base_link" ps.pose.position.x = push[0] - .025 * 2.0/3.0 ps.pose.position.y = push[1] ps.pose.position.z = push[2] ps.pose.orientation.w = 1.0 ik_request = PositionIKRequest() ik_request.group_name = 'arm' ik_request.pose_stamped = ps while True: response = get_position_ik(ik_request) print response user = raw_input("Accept position? Y/N") if user == 'Y': break inter_state_2 = response.solution.joint_state.position[6:13] print inter_state_2 """ """ ps = PlanningScene() ps.is_diff = True print ps ls_l = LinkScale() ls_l.link_name = "l_gripper_finger_link" print "+++++SCALE: ", ls_l.scale ls_l.scale = .01 ls_r = LinkScale() ls_r.link_name = "r_gripper_finger_link" ls_r.scale = .01 ps.link_scale.append(ls_l) ps.link_scale.append(ls_r) scene_pub.publish(ps) """ #arm_action.move_to(inter_state_1) #arm_action.move_to(inter_state_2) post_push_state = pre_push_state.joint_state.position #arm_action.move_to(post_push_state) post_push_goals = [tuck_state] """ ps = PlanningScene() ps.is_diff = True ls_l = LinkScale() ls_l.link_name = "l_gripper_finger_link" ls_l.scale = .06 ls_r = LinkScale() ls_r.link_name = "r_gripper_finger_link" ls_r.scale = .06 ps.link_scale.append(ls_l) ps.link_scale.append(ls_r) scene_pub.publish(ps) """ trajectory_generator.clear_virtual_arm_state() post_push_trajectories = trajectory_generator.generate_trajectories(post_push_goals, get_approval=True) trajectory_executor.execute_trajectories(post_push_trajectories, error_checking = False)
def main(): ## Initialization ## rospy.init_node("push_button") trajectory_generator = TrajectoryGenerator() trajectory_executor = TrajectoryExecutor() button_finder = ButtonLocationSubscriber() arm_joints = [ "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] arm_action = FollowTrajectoryClient('arm_controller', arm_joints) torso_action = FollowTrajectoryClient('torso_controller', ["torso_lift_joint"]) rospy.loginfo("Waiting for get_position_ik...") rospy.wait_for_service('compute_ik') get_position_ik = rospy.ServiceProxy('compute_ik', GetPositionIK) rospy.loginfo("...connected.") ## Map button world frame pose to base_link frame pose of gripper ## button_center = None #approx coords: 0.796538999146 -0.0150021952933 0.264828975569 while button_center is None: rospy.sleep(1.0) button_center = button_finder.getButtonLocation() desired_gripper_translation = .16645 pre_push_pose = PoseGoal([ button_center[0] - desired_gripper_translation, button_center[1], button_center[2] + .2, 0, 0, 0 ]) print pre_push_pose.pose pre_push_goals = [pre_push_pose] pre_push_trajectories = trajectory_generator.generate_trajectories( pre_push_goals, get_approval=True) pre_push_state = trajectory_generator.get_virtual_arm_state() trajectory_executor.execute_trajectories(pre_push_trajectories, error_checking=False) post_push_posture = GripperPostureGoal(1.0) tuck_state = JointSpaceGoal([1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]) gripper_goal = [post_push_posture] trajectory_generator.clear_virtual_arm_state() post_push_trajectories = trajectory_generator.generate_trajectories( gripper_goal, get_approval=False) trajectory_executor.execute_trajectories(post_push_trajectories, error_checking=False) torso_action.move_to([ .30, ]) post_push_goals = [tuck_state] trajectory_generator.clear_virtual_arm_state() post_push_trajectories = trajectory_generator.generate_trajectories( post_push_goals, get_approval=True) trajectory_executor.execute_trajectories(post_push_trajectories, error_checking=False) torso_action.move_to([ 0.05, ])