# OPTIONAL: play it safe and go to `pose_0_b` first, THEN `pose_0`. This is # usually a good idea. #if True: # robot.move_to_pose(pose0+'_b', velocity_factor=VEL) robot.move_to_pose(pose0, velocity_factor=VEL) def get_arm_straight(): """Move robot so its arm extends outwards. I used this to test if we can reset the zero joints, according to Fetch support instructions. """ robot.torso.set_height(0.20) zeros = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] zeros_list = [(x, y) for (x, y) in zip(robot.arm_joints.names(), zeros)] robot.arm.move_to_joint_goal(zeros_list, velocity_factor=0.3) if __name__ == "__main__": robot = Robot_Skeleton() #robot.body_start_pose(start_height=0.20, end_height=0.20, velocity_factor=VEL) #robot.head_start_pose(pan=0.0, tilt=45.0*DEG_TO_RAD) #basic_camera_grippers() #moving_to_poses() get_arm_straight() rospy.spin()
print("move_to_pose1_b pose0") rospy.sleep(1) rospy.loginfo("Just moved to pose: {}_b".format(pose1)) robot.move_to_pose(pose1, velocity_factor=VEL) print("move_to_pose1 pose0") rospy.sleep(1) # Grip object robot.close_gripper(width=0.06) rospy.sleep(1) # Move back up to the original pose robot.move_to_pose(pose0, velocity_factor=VEL) if __name__ == "__main__": robot = Robot_Skeleton() # set torso height high to start to hopefully avoid collisions with base robot.body_start_pose(start_height=0.25, end_height=0.25, velocity_factor=VEL) robot.head_start_pose(pan=0.0, tilt=0.0) rospy.loginfo("Finished robot body and head start poses.") robot.close_gripper(width=0.04) robot.open_gripper() grab_item() pose_group = moveit_commander.MoveGroupCommander("arm") position = pose_group.get_current_pose("wrist_roll_link").pose.position print(position) print("done, just spinning now ...") rospy.spin()
""" Test different movement heuristics for moving the Fetch if it can't reach something from its existing position. """ from fetch_core.skeleton import Robot_Skeleton import cv2, os, sys, time, rospy import numpy as np DEG_TO_RAD = np.pi / 180 RAD_TO_DEG = 180 / np.pi # Adjust to change robot's speed. VEL = 1.0 def move_from_camera_pixels(): """Move to this pose as specified by user. """ pass if __name__ == "__main__": robot = Robot_Skeleton() robot.body_start_pose() robot.head_start_pose() move_from_camera_pixels() print("done, just spinning now ...") rospy.spin()
orientation_constraint=None, plan_only=False, replan=True, replan_attempts=20, tolerance=0.01, velocity_factor=0.4): error = robot.move_to_pose(pose_name=ps, 654 execution_timeout=execution_timeout, group_name=group_name, num_planning_attempts=num_planning_attempts, orientation_constraint=orientation_constraint, plan_only=plan_only, replan=replan, replan_attempts=replan_attempts, tolerance=tolerance, velocity_factor=velocity_factor) print(error) if __name__ == "__main__": robot = Robot_Skeleton() robot.body_start_pose(start_height=0.18, end_height=0.18, velocity_factor=0.4) rospy.loginfo("Finished robot starting config.") robot.close_gripper(width=0.04) robot.open_gripper() robot.arm.move_to_joint_goal() #map_coordinates = [0.64364554, 0.40790289, 0.26959194] #pose0 = robot.gripper.create_grasp_pose(320, 400, 0.7, 45, use_world_frame=True, world_coordinates=map_coordinates) #can_plan(pose0)
""" Use for basic testing of the Fetch.""" from fetch_core.skeleton import Robot_Skeleton import cv2, os, sys, time, rospy import numpy as np DEG_TO_RAD = np.pi / 180 RAD_TO_DEG = 180 / np.pi # Adjust to change robot's speed. VEL = 0.5 def save_camera_images(): # Get some camera images and save them. c_img, d_img = robot.get_img_data() cv2.imwrite("rgb_img.png", c_img) cv2.imwrite("depth_img.png", d_img) print("Saved images...check them out!") if __name__ == "__main__": robot = Robot_Skeleton() robot.body_start_pose(start_height=0.20, end_height=0.20, velocity_factor=VEL) robot.head_start_pose(pan=0.0, tilt=45.0 * DEG_TO_RAD) save_camera_images() rospy.spin()
if __name__ == "__main__": num_rollouts = len([x for x in os.listdir(OUTDIR) if 'rollout_' in x]) os.makedirs(os.path.join(OUTDIR, 'rollout_{}'.format(num_rollouts))) out_path = os.path.join(OUTDIR, 'rollout_{}/rollout.p'.format(num_rollouts)) print("\n\n\n\n\n\n\n\n\n\nGet things set up to save at: {}".format( out_path)) style, perc, side = set_up_bed() rollout = [] # Set to stuff from `prep_fetch.py`, while we adjust the bed. (This is # specific to our setup, so different code bases will likely have to adjust. # See our data collection protocol for more details.) robot = Robot_Skeleton() robot.body_start_pose(start_height=0.00, end_height=0.00, velocity_factor=VEL) robot.head_start_pose(pan=0.0 * DEG_TO_RAD, tilt=45.0 * DEG_TO_RAD) # Get RGB and depth images, then do a bunch of debug prints. This one is a # placeholder as we may not have finished with the initial sheet setup. # ACTIONABLE: get the initial sheet set up, then press any key (except ESC). c_img, _ = robot.get_img_data() cv2.imwrite("tmp/cimg_{}.png".format(len(os.listdir('tmp/'))), c_img) call_wait_key( cv2.imshow( "(A placeholder to stop code, finish sheet and press a key)", c_img))
print("move_to_pose1_b pose0") rospy.sleep(1) rospy.loginfo("Just moved to pose: {}_b".format(pose1)) robot.move_to_pose(pose1, velocity_factor=VEL) print("move_to_pose1 pose0") rospy.sleep(1) # Grip object robot.close_gripper(width=0.06) rospy.sleep(1) # Move back up to the original pose robot.move_to_pose(pose0, velocity_factor=VEL) if __name__ == "__main__": robot = Robot_Skeleton() # set torso height high to start to hopefully avoid collisions with base # robot.body_start_pose(start_height=0.25, end_height=0.25, velocity_factor=VEL) # robot.head_start_pose(pan=0.0, tilt=0.0) # rospy.loginfo("Finished robot body and head start poses.") # robot.close_gripper(width=0.1) #close0.0---open0.1 # robot.open_gripper() # grab_item() # pose_group = moveit_commander.MoveGroupCommander("arm") # position = pose_group.get_current_pose("wrist_roll_link").pose.position # print(position) # print("done, just spinning now ...") x, y, z = (0.5, 0.0, 0.5) # x, y, z = (1.0, 0.0, 0.8) rot_x, rot_y, rot_z = (0.0, 90.0, 0.0)
# Move to poses, adjust height as needed. robot.move_to_pose(pose0, velocity_factor=VEL) rospy.loginfo("Just moved to pose: {}".format(pose0)) robot.torso.set_height(0.15) robot.move_to_pose(pose1 + '_b', velocity_factor=VEL) rospy.loginfo("Just moved to pose: {}_b".format(pose1)) robot.move_to_pose(pose1, velocity_factor=VEL) # Grip object robot.close_gripper(width=0.06) rospy.sleep(1) # Move back up to the original pose robot.move_to_pose(pose0, velocity_factor=VEL) if __name__ == "__main__": robot = Robot_Skeleton() # set torso height high to start to hopefully avoid collisions with base robot.head_start_pose(pan=0.0, tilt=0.0) robot.body_start_pose() rospy.loginfo("Finished robot body and head start poses.") #robot.close_gripper(width=0.04) robot.open_gripper() grab_item() rospy.loginfo("done, just spinning now ...") rospy.spin()
"""Use to test the camera to base frame mapping.""" from fetch_core.skeleton import Robot_Skeleton import cv2, os, sys, time, rospy import numpy as np DEG_TO_RAD = np.pi / 180 RAD_TO_DEG = 180 / np.pi # Adjust to change robot's speed. VEL = 1.0 def move_from_camera_pixels(): """Move to this pose as specified by user. """ pass if __name__ == "__main__": robot = Robot_Skeleton() #robot.body_start_pose() robot.head_start_pose(tilt=45 * DEG_TO_RAD) #move_from_camera_pixels() print("done, just spinning now ...") rospy.spin()
""" Use for basic testing of the Fetch.""" from fetch_core.skeleton import Robot_Skeleton import cv2, os, sys, time, rospy import numpy as np DEG_TO_RAD = np.pi / 180 RAD_TO_DEG = 180 / np.pi # Adjust to change robot's speed. VEL = 0.5 if __name__ == "__main__": robot = Robot_Skeleton() robot.body_start_pose(start_height=0.20, end_height=0, velocity_factor=VEL) rospy.loginfo("Finished moving robot to start pose.") # default: robot.body_start_pose() # robot.head_start_pose(pan=0.0, tilt=45.0*DEG_TO_RAD) rospy.spin() # yields activity to other threads