# 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
