def main(args):
cfg_file = os.path.join(args.example_config_path, args.primitive) + ".yaml"
cfg = get_cfg_defaults()
cfg.merge_from_file(cfg_file)
cfg.freeze()
rospy.init_node('replay')
with open(os.join(args.data_dir, args.data_file), 'rb') as data_f:
data = pickle.load(data_f)
yumi_ar = Robot('yumi',
pb=True,
arm_cfg={
'render': True,
'self_collision': False
})
if args.object:
box_id = pb_util.load_urdf(
args.config_package_path + 'descriptions/urdf/' +
args.object_name + '.urdf', cfg.OBJECT_POSE_3[0:3],
cfg.OBJECT_POSE_3[3:])
def main(args):
cfg_file = os.path.join(args.example_config_path, args.primitive) + ".yaml"
cfg = get_cfg_defaults()
cfg.merge_from_file(cfg_file)
cfg.freeze()
rospy.init_node('MacroActions')
data = {}
data['saved_data'] = []
data['metadata'] = {}
# parent1, child1 = Pipe()
# parent2, child2 = Pipe()
# work_queue = Queue()
# result_queue = Queue()
# p1 = Process(target=worker_yumi, args=(child1, work_queue, result_queue, cfg, args))
# p2 = Process(target=worker_yumi, args=(child2, work_queue, result_queue, cfg, args))
# p1.start()
# p2.start()
# parent1.send("RESET")
# parent2.send("RESET")
# print("started workers")
# time.sleep(15.0)
# embed()
# # setup yumi
yumi_ar = Robot('yumi',
pb=True,
arm_cfg={
'render': args.visualize,
'self_collision': False,
'rt_simulation': False
})
yumi_ar.arm.set_jpos(cfg.RIGHT_INIT + cfg.LEFT_INIT)
gel_id = 12
alpha = 0.01
K = 500
restitution = 0.99
dynamics_info = {}
dynamics_info['contactDamping'] = alpha * K
dynamics_info['contactStiffness'] = K
dynamics_info['rollingFriction'] = args.rolling
dynamics_info['restitution'] = restitution
p.changeDynamics(yumi_ar.arm.robot_id,
gel_id,
restitution=restitution,
contactStiffness=K,
contactDamping=alpha * K,
rollingFriction=args.rolling)
# setup yumi_gs
# yumi_gs = YumiGelslimPybulet(yumi_ar, cfg, exec_thread=args.execute_thread)
yumi_gs = YumiCamsGS(yumi_ar, cfg, exec_thread=args.execute_thread)
if args.object:
box_id = pb_util.load_urdf(
args.config_package_path + 'descriptions/urdf/' +
args.object_name + '.urdf', cfg.OBJECT_POSE_3[0:3],
cfg.OBJECT_POSE_3[3:])
# trans_box_id = pb_util.load_urdf(
# args.config_package_path +
# 'descriptions/urdf/'+args.object_name+'_trans.urdf',
# cfg.OBJECT_POSE_3[0:3],
# cfg.OBJECT_POSE_3[3:]
# )
# setup macro_planner
action_planner = ClosedLoopMacroActions(cfg,
yumi_gs,
box_id,
pb_util.PB_CLIENT,
args.config_package_path,
replan=args.replan)
manipulated_object = None
object_pose1_world = util.list2pose_stamped(cfg.OBJECT_INIT)
object_pose2_world = util.list2pose_stamped(cfg.OBJECT_FINAL)
palm_pose_l_object = util.list2pose_stamped(cfg.PALM_LEFT)
palm_pose_r_object = util.list2pose_stamped(cfg.PALM_RIGHT)
example_args = {}
example_args['object_pose1_world'] = object_pose1_world
example_args['object_pose2_world'] = object_pose2_world
example_args['palm_pose_l_object'] = palm_pose_l_object
example_args['palm_pose_r_object'] = palm_pose_r_object
example_args['object'] = manipulated_object
# example_args['N'] = 60 # 60
example_args['N'] = calc_n(object_pose1_world, object_pose2_world) # 60
print("N: " + str(example_args['N']))
example_args['init'] = True
example_args['table_face'] = 0
primitive_name = args.primitive
mesh_file = args.config_package_path + 'descriptions/meshes/objects/' + args.object_name + '_experiments.stl'
exp_single = SingleArmPrimitives(cfg, box_id, mesh_file)
exp_double = DualArmPrimitives(cfg, box_id, mesh_file)
# trans_box_lock = threading.RLock()
# goal_viz = GoalVisual(
# trans_box_lock,
# trans_box_id,
# action_planner.pb_client,
# cfg.OBJECT_POSE_3)
# visualize_goal_thread = threading.Thread(
# target=goal_viz.visualize_goal_state)
# visualize_goal_thread.daemon = True
# visualize_goal_thread.start()
data['metadata']['mesh_file'] = mesh_file
data['metadata']['cfg'] = cfg
data['metadata']['dynamics'] = dynamics_info
data['metadata']['cam_cfg'] = yumi_gs.cam_cfg
if args.debug:
init_id = exp.get_rand_init(ind=2)[-1]
obj_pose_final = util.list2pose_stamped(exp.init_poses[init_id])
point, normal, face = exp.sample_contact(primitive_name)
# embed()
world_pose = exp.get_palm_pose_world_frame(
point, normal, primitive_name=primitive_name)
obj_pos_world = list(
p.getBasePositionAndOrientation(box_id, pb_util.PB_CLIENT)[0])
obj_ori_world = list(
p.getBasePositionAndOrientation(box_id, pb_util.PB_CLIENT)[1])
obj_pose_world = util.list2pose_stamped(obj_pos_world + obj_ori_world)
contact_obj_frame = util.convert_reference_frame(
world_pose, obj_pose_world, util.unit_pose())
example_args['palm_pose_r_object'] = contact_obj_frame
example_args['object_pose1_world'] = obj_pose_world
obj_pose_final = util.list2pose_stamped(exp.init_poses[init_id])
obj_pose_final.pose.position.z = obj_pose_world.pose.position.z / 1.175
print("init: ")
print(util.pose_stamped2list(object_pose1_world))
print("final: ")
print(util.pose_stamped2list(obj_pose_final))
example_args['object_pose2_world'] = obj_pose_final
example_args['table_face'] = init_id
plan = action_planner.get_primitive_plan(primitive_name, example_args,
'right')
embed()
import simulation
for i in range(10):
simulation.visualize_object(
object_pose1_world,
filepath=
"package://config/descriptions/meshes/objects/realsense_box_experiments.stl",
name="/object_initial",
color=(1., 0., 0., 1.),
frame_id="/yumi_body",
scale=(1., 1., 1.))
simulation.visualize_object(
object_pose2_world,
filepath=
"package://config/descriptions/meshes/objects/realsense_box_experiments.stl",
name="/object_final",
color=(0., 0., 1., 1.),
frame_id="/yumi_body",
scale=(1., 1., 1.))
rospy.sleep(.1)
simulation.simulate(plan)
else:
global_start = time.time()
for trial in range(20):
k = 0
while True:
# sample a random stable pose, and get the corresponding
# stable orientation index
k += 1
# init_id = exp.get_rand_init()[-1]
init_id = exp.get_rand_init(ind=0)[-1]
# sample a point on the object that is valid
# for the primitive action being executed
point, normal, face = exp.sample_contact(
primitive_name=primitive_name)
if point is not None:
break
if k >= 10:
print("FAILED")
return
# get the full 6D pose palm in world, at contact location
palm_pose_world = exp.get_palm_pose_world_frame(
point, normal, primitive_name=primitive_name)
# get the object pose in the world frame
# if trial == 0:
# parent1.send("OBJECT_POSE")
# elif trial == 1:
# parent2.send("OBJECT_POSE")
obj_pos_world = list(
p.getBasePositionAndOrientation(box_id, pb_util.PB_CLIENT)[0])
obj_ori_world = list(
p.getBasePositionAndOrientation(box_id, pb_util.PB_CLIENT)[1])
obj_pose_world = util.list2pose_stamped(obj_pos_world +
obj_ori_world)
# obj_pose_world = work_queue.get(block=True)
# transform the palm pose from the world frame to the object frame
contact_obj_frame = util.convert_reference_frame(
palm_pose_world, obj_pose_world, util.unit_pose())
# set up inputs to the primitive planner, based on task
# including sampled initial object pose and contacts,
# and final object pose
example_args['palm_pose_r_object'] = contact_obj_frame
example_args['object_pose1_world'] = obj_pose_world
obj_pose_final = util.list2pose_stamped(exp.init_poses[init_id])
obj_pose_final.pose.position.z /= 1.18
print("init: ")
print(util.pose_stamped2list(object_pose1_world))
print("final: ")
print(util.pose_stamped2list(obj_pose_final))
example_args['object_pose2_world'] = obj_pose_final
example_args['table_face'] = init_id
example_args['primitive_name'] = primitive_name
example_args['N'] = calc_n(obj_pose_world, obj_pose_final)
print("N: " + str(example_args['N']))
# if trial == 0:
# goal_viz.update_goal_state(exp.init_poses[init_id])
try:
# get observation (images/point cloud)
obs = yumi_gs.get_observation(obj_id=box_id)
# get start/goal (obj_pose_world, obj_pose_final)
start = util.pose_stamped2list(obj_pose_world)
goal = util.pose_stamped2list(obj_pose_final)
# get corners (from exp? that has mesh)
keypoints_start = np.array(exp.mesh_world.vertices.tolist())
keypoints_start_homog = np.hstack(
(keypoints_start, np.ones((keypoints_start.shape[0], 1))))
goal_start_frame = util.convert_reference_frame(
pose_source=obj_pose_final,
pose_frame_target=obj_pose_world,
pose_frame_source=util.unit_pose())
goal_start_frame_mat = util.matrix_from_pose(goal_start_frame)
keypoints_goal = np.matmul(goal_start_frame_mat,
keypoints_start_homog.T).T
# get contact (palm pose object frame)
contact_obj_frame = util.pose_stamped2list(contact_obj_frame)
contact_world_frame = util.pose_stamped2list(palm_pose_world)
contact_pos = open3d.utility.DoubleVector(
np.array(contact_world_frame[:3]))
kdtree = open3d.geometry.KDTreeFlann(obs['pcd_full'])
nearest_pt_ind = kdtree.search_knn_vector_3d(contact_pos,
1)[1][0]
nearest_pt_world = np.asarray(
obs['pcd_full'].points)[nearest_pt_ind]
# embed()
result = action_planner.execute(primitive_name, example_args)
sample = {}
sample['obs'] = obs
sample['start'] = start
sample['goal'] = goal
sample['keypoints_start'] = keypoints_start
sample['keypoints_goal'] = keypoints_goal
sample['transformation'] = util.pose_stamped2list(
goal_start_frame)
sample['contact_obj_frame'] = contact_obj_frame
sample['contact_world_frame'] = contact_world_frame
sample['contact_pcd'] = nearest_pt_world
sample['result'] = result
sample['planner_args'] = example_args
data['saved_data'].append(sample)
# if trial == 0:
# parent1.send("SAMPLE")
# elif trial == 1:
# parent2.send("SAMPLE")
# result = work_queue.get(block=True)
# if trial == 0:
# parent1.send("SAMPLE")
# elif trial == 1:
# parent2.send("SAMPLE")
# parent1.send("SAMPLE")
# parent2.send("SAMPLE")
# start = time.time()
# done = False
# result_list = []
# while (time.time() - start) < cfg.TIMEOUT and not done:
# try:
# result = result_queue.get(block=True)
# result_list.append(result)
# if len(result_list) == 2:
# done = True
# except result_queue.Empty:
# continue
# time.sleep(0.001)
print("reached final: " + str(result[0]))
except ValueError:
print("moveit failed!")
# time.sleep(0.1)
# yumi_gs.update_joints(cfg.RIGHT_INIT + cfg.LEFT_INIT)
j_pos = cfg.RIGHT_INIT + cfg.LEFT_INIT
for ind, jnt_id in enumerate(yumi_ar.arm.arm_jnt_ids):
p.resetJointState(yumi_ar.arm.robot_id,
jnt_id,
targetValue=j_pos[ind])
# p.resetJointStatesMultiDof(
# yumi_ar.arm.robot_id,
# yumi_ar.arm.arm_jnt_ids,
# targetValues=j_pos)
# parent1.send("HOME")
# parent2.send("HOME")
# time.sleep(1.0)
# embed()
# embed()
print("TOTAL TIME: " + str(time.time() - global_start))
with open('data/sample_data_right_hand_pull.pkl', 'wb') as data_f:
pickle.dump(data, data_f)
def worker_yumi(child_conn, work_queue, result_queue, cfg, args):
while True:
# print("here!")
try:
if not child_conn.poll(0.0001):
continue
msg = child_conn.recv()
except (EOFError, KeyboardInterrupt):
break
if msg == "RESET":
# yumi = Robot('yumi', pb=True, arm_cfg={'render': True, 'self_collision': False})
# client_id = p.connect(p.DIRECT)
# print("\n\nfinished worker construction\n\n")
yumi_ar = Robot('yumi',
pb=True,
arm_cfg={
'render': True,
'self_collision': False
})
yumi_ar.arm.set_jpos(cfg.RIGHT_INIT + cfg.LEFT_INIT)
gel_id = 12
alpha = 0.01
K = 500
p.changeDynamics(yumi_ar.arm.robot_id,
gel_id,
restitution=0.99,
contactStiffness=K,
contactDamping=alpha * K,
rollingFriction=args.rolling)
# setup yumi_gs
yumi_gs = YumiGelslimPybulet(yumi_ar,
cfg,
exec_thread=args.execute_thread)
box_id = pb_util.load_urdf(
args.config_package_path + 'descriptions/urdf/' +
args.object_name + '.urdf', cfg.OBJECT_POSE_3[0:3],
cfg.OBJECT_POSE_3[3:])
trans_box_id = pb_util.load_urdf(
args.config_package_path + 'descriptions/urdf/' +
args.object_name + '_trans.urdf', cfg.OBJECT_POSE_3[0:3],
cfg.OBJECT_POSE_3[3:])
# setup macro_planner
action_planner = ClosedLoopMacroActions(cfg,
yumi_gs,
box_id,
pb_util.PB_CLIENT,
args.config_package_path,
replan=args.replan)
continue
if msg == "HOME":
yumi_gs.update_joints(cfg.RIGHT_INIT + cfg.LEFT_INIT)
continue
if msg == "OBJECT_POSE":
obj_pos_world = list(
p.getBasePositionAndOrientation(box_id, pb_util.PB_CLIENT)[0])
obj_ori_world = list(
p.getBasePositionAndOrientation(box_id, pb_util.PB_CLIENT)[1])
obj_pose_world = util.list2pose_stamped(obj_pos_world +
obj_ori_world)
work_queue.put(obj_pose_world)
continue
if msg == "SAMPLE":
# try:
# example_args = work_queue.get(block=True)
# primitive_name = example_args['primitive_name']
# result = action_planner.execute(primitive_name, example_args)
# work_queue.put(result)
# except work_queue.Empty:
# continue
manipulated_object = None
object_pose1_world = util.list2pose_stamped(cfg.OBJECT_INIT)
object_pose2_world = util.list2pose_stamped(cfg.OBJECT_FINAL)
palm_pose_l_object = util.list2pose_stamped(cfg.PALM_LEFT)
palm_pose_r_object = util.list2pose_stamped(cfg.PALM_RIGHT)
example_args = {}
example_args['object_pose1_world'] = object_pose1_world
example_args['object_pose2_world'] = object_pose2_world
example_args['palm_pose_l_object'] = palm_pose_l_object
example_args['palm_pose_r_object'] = palm_pose_r_object
example_args['object'] = manipulated_object
example_args['N'] = 60 # 60
example_args['init'] = True
example_args['table_face'] = 0
primitive_name = args.primitive
mesh_file = args.config_package_path + 'descriptions/meshes/objects/' + args.object_name + '_experiments.stl'
exp = EvalPrimitives(cfg, box_id, mesh_file)
k = 0
while True:
# sample a random stable pose, and get the corresponding
# stable orientation index
k += 1
# init_id = exp.get_rand_init()[-1]
init_id = exp.get_rand_init(ind=0)[-1]
# sample a point on the object that is valid
# for the primitive action being executed
point, normal, face = exp.sample_contact(
primitive_name=primitive_name)
if point is not None:
break
if k >= 10:
print("FAILED")
continue
# get the full 6D pose palm in world, at contact location
palm_pose_world = exp.get_palm_pose_world_frame(
point, normal, primitive_name=primitive_name)
obj_pos_world = list(
p.getBasePositionAndOrientation(box_id, pb_util.PB_CLIENT)[0])
obj_ori_world = list(
p.getBasePositionAndOrientation(box_id, pb_util.PB_CLIENT)[1])
obj_pose_world = util.list2pose_stamped(obj_pos_world +
obj_ori_world)
contact_obj_frame = util.convert_reference_frame(
palm_pose_world, obj_pose_world, util.unit_pose())
# set up inputs to the primitive planner, based on task
# including sampled initial object pose and contacts,
# and final object pose
example_args['palm_pose_r_object'] = contact_obj_frame
example_args['object_pose1_world'] = obj_pose_world
obj_pose_final = util.list2pose_stamped(exp.init_poses[init_id])
obj_pose_final.pose.position.z /= 1.155
print("init: ")
print(util.pose_stamped2list(object_pose1_world))
print("final: ")
print(util.pose_stamped2list(obj_pose_final))
example_args['object_pose2_world'] = obj_pose_final
example_args['table_face'] = init_id
example_args['primitive_name'] = primitive_name
# if trial == 0:
# goal_viz.update_goal_state(exp.init_poses[init_id])
result = None
try:
result = action_planner.execute(primitive_name, example_args)
# result = work_queue.get(block=True)
print("reached final: " + str(result[0]))
except ValueError:
print("moveit failed!")
result_queue.put(result)
continue
if msg == "END":
break
print("before sleep!")
time.sleep(0.01)
print("breaking")
child_conn.close()
yumi.arm.go_home()
yumi.arm.set_jpos(cfg.RIGHT_INIT + cfg.LEFT_INIT)
gel_id = 12
p.changeDynamics(yumi.arm.robot_id,
gel_id,
restitution=dynamics_info['restitution'],
contactStiffness=dynamics_info['contactStiffness'],
contactDamping=dynamics_info['contactDamping'],
rollingFriction=dynamics_info['rollingFriction'])
yumi_gs = YumiGelslimPybulet(yumi, cfg)
box_id = pb_util.load_urdf(
'/root/catkin_ws/src/config/descriptions/urdf/realsense_box.urdf',
cfg.OBJECT_POSE_3[0:3], cfg.OBJECT_POSE_3[3:])
config_pkg_path = '/root/catkin_ws/src/config/'
action_planner = ClosedLoopMacroActions(cfg,
yumi_gs,
box_id,
pb_util.PB_CLIENT,
config_pkg_path,
object_mesh_file=mesh_file,
replan=True)
from helper.util import pose_stamped2list, list2pose_stamped
planner_args = data['planner_args']
def main(ifRender=False):
"""
this function loads initial oboject on a table,
"""
np.set_printoptions(precision=3, suppress=True)
# load robot
robot = Robot('ur5e', use_eetool=False, arm_cfg={'render': ifRender, 'self_collision': True})
robot.arm.go_home()
origin = robot.arm.get_ee_pose()
def go_home():
robot.arm.set_ee_pose(home, origin[1])
# robot.arm.eetool.close()
# init robot
go_home()
# load table
table_ori = euler2quat([0, 0, np.pi/2.0])
table_pos = [table_x, table_y, table_z]
table_id = load_urdf('table/table.urdf', table_pos, table_ori, scaling=table_scaling)
# load box
box_id = load_geom('box', size=box_size, mass=1, base_pos=box_pos, rgba=[1, 0, 0, 1])
# init camera
def get_img():
# screenshot camera images
focus_pt = [0.7, 0, 1.]
robot.cam.setup_camera(focus_pt=focus_pt, dist=0.5, yaw=90, pitch=-60, roll=0)
rgb, depth = robot.cam.get_images(get_rgb=True, get_depth=True)
# crop the rgb
img = rgb[40:360, 0:640]
# dep = depth[40:360, 0:640]
# low pass filter : Gaussian blur
# blurred_img = cv2.GaussianBlur(img.astype('float32'), (5, 5), 0)
small_img = cv2.resize(img.astype('float32'), dsize=(200, 100),
interpolation=cv2.INTER_CUBIC) # numpy array dtype numpy int64 by default
# small_dep = cv2.resize(dep.astype('float32'), dsize=(200, 100),
# interpolation=cv2.INTER_CUBIC) # numpy array dtype numpy int64 by default
return small_img, depth
# test run
def test():
time_to_sleep = 0.5
go_home()
pose = robot.arm.get_ee_pose()
robot.arm.set_ee_pose([pose[0][0], pose[0][1], min_height], origin[1])
time.sleep(time_to_sleep)
get_img()
# test boundary
robot.arm.set_ee_pose([pose[0][0], pose[0][1]+table_length/2.0, min_height], origin[1])
# robot.arm.eetool.close()
time.sleep(time_to_sleep)
get_img()
robot.arm.move_ee_xyz([0, -table_length, 0])
time.sleep(time_to_sleep)
get_img()
pose = robot.arm.get_ee_pose()
robot.arm.set_ee_pose([pose[0][0], pose[0][1], rest_height], origin[1])
time.sleep(time_to_sleep)
get_img()
robot.arm.move_ee_xyz([0, table_length, 0])
time.sleep(time_to_sleep)
get_img()
pose = robot.arm.get_ee_pose()
robot.arm.set_ee_pose([pose[0][0], pose[0][1], min_height], origin[1])
time.sleep(time_to_sleep)
get_img()
# test arc
for i in list([np.pi/3.0, np.pi*2/5.0, np.pi*3/7.0, np.pi/2.0,
np.pi*4/7.0, np.pi*3/5.0, np.pi*2/3.0]):
robot.arm.set_ee_pose([arm_span*np.sin(i),
arm_span*np.cos(i), rest_height], origin[1])
time.sleep(time_to_sleep)
get_img()
robot.arm.set_ee_pose([arm_span*np.sin(i),
arm_span*np.cos(i), min_height], origin[1])
time.sleep(time_to_sleep)
get_img()
robot.arm.set_ee_pose([arm_span*np.sin(i),
arm_span*np.cos(i), rest_height], origin[1])
time.sleep(time_to_sleep)
get_img()
def get_pixel(X, Y, Z=0.975):
"""return fractional pixels representations
Z values comes from running robot.cam.get_pix.3dpt
representing the table surface height"""
ext_mat = robot.cam.get_cam_ext()
int_mat = robot.cam.get_cam_int()
XYZ = np.array([X, Y, Z, 1])
xyz = np.linalg.inv(ext_mat).dot(XYZ)[:3]
pixel_xyz = int_mat.dot(xyz)
pixel_xyz = pixel_xyz / pixel_xyz[2]
pixel_col = pixel_xyz[0] / 3.2 # due to image cropping and scaling
pixel_row = (pixel_xyz[1] - 40) / 3.2
return pixel_row, pixel_col
# # log object
# pos, quat, lin_vel, ang_vel = get_body_state(box_id)
# ar.log_info('Box:')
# ar.log_info(' position: %s' % np.array2string(pos, precision=2))
# ar.log_info(' quaternion: %s' % np.array2string(quat, precision=2))
# ar.log_info(' linear vel: %s' % np.array2string(lin_vel, precision=2))
# ar.log_info(' angular vel: %s' % np.array2string(ang_vel, precision=2))
# plt.figure()
# plt.imshow(rgb)
# plt.figure()
# plt.imshow(depth * 25, cmap='gray', vmin=0, vmax=255)
# print('Maximum Depth (m): %f' % np.max(depth))
# print('Minimum Depth (m): %f' % np.min(depth))
# plt.show()
def poke(save_dir='data/image'):
if not os.path.exists(save_dir):
os.makedirs(save_dir)
save_depth_dir = save_dir + '_depth'
if not os.path.exists(save_depth_dir):
os.makedirs(save_depth_dir)
index = 0
curr_img, curr_dep = get_img()
while True:
obj_pos, obj_quat, lin_vel, _ = get_body_state(box_id)
obj_ang = quat2euler(obj_quat)[2] # -pi ~ pi
obj_x, obj_y, obj_z = obj_pos
# check if cube is on table
if obj_z < box_z / 2.0 or lin_vel[0] > 1e-3: # important that box is still
print(obj_z, lin_vel[0])
reset_body(box_id, box_pos)
continue
while True:
# choose random poke point on the object
poke_x = np.random.random()*box_size + obj_x - box_size/2.0
poke_y = np.random.random()*box_size + obj_y - box_size/2.0
# choose poke angle along the z axis
poke_ang = np.random.random() * np.pi * 2 - np.pi
# choose poke length
poke_len = np.random.random() * poke_len_std + poke_len_mean
# calc starting poke location and ending poke loaction
start_x = poke_x - poke_len * np.cos(poke_ang)
start_y = poke_y - poke_len * np.sin(poke_ang)
end_x = poke_x + poke_len * np.cos(poke_ang)
end_y = poke_y + poke_len * np.sin(poke_ang)
if end_x > workspace_min_x and end_x < workspace_max_x \
and end_y > workspace_min_y and end_y < workspace_max_y:
break
robot.arm.move_ee_xyz([start_x-home[0], start_y-home[1], 0], 0.015)
robot.arm.move_ee_xyz([0, 0, min_height-rest_height], 0.015)
js1, js2, js3, js4, js5, js6 = robot.arm.get_jpos()
robot.arm.move_ee_xyz([end_x-start_x, end_y-start_y, 0], 0.015)
je1, je2, je3, je4, je5, je6 = robot.arm.get_jpos()
# important that we use move_ee_xyz, as set_ee_pose can throw obj in motion
robot.arm.move_ee_xyz([0, 0, rest_height-min_height], 0.015)
# move arm away from camera view
go_home() # important to have one set_ee_pose every loop to reset accu errors
next_img, next_dep = get_img()
# calc poke and obj locations in image pixel space
start_r, start_c = get_pixel(start_x, start_y)
end_r, end_c = get_pixel(end_x, end_y)
obj_r, obj_c = get_pixel(obj_x, obj_y)
with open(save_dir + '.txt', 'a') as file:
file.write('%d %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n' % \
(index, start_x, start_y, end_x, end_y, obj_x, obj_y,
obj_quat[0], obj_quat[1], obj_quat[2], obj_quat[3],
js1, js2, js3, js4, js5, js6, je1, je2, je3, je4, je5, je6,
start_r, start_c, end_r, end_c, obj_r, obj_c))
cv2.imwrite(save_dir + '/' + str(index) +'.png',
cv2.cvtColor(curr_img, cv2.COLOR_RGB2BGR))
# cv2.imwrite(save_dir + '_depth/' + str(index) +'.png', curr_dep)
np.save(save_dir + '_depth/' + str(index), curr_dep)
curr_img = next_img
curr_dep = next_dep
if index % 1000 == 0:
ar.log_info('number of pokes: %sk' % str(index/1000))
index += 1
def test_data(filename):
data = np.loadtxt(filename, unpack=True)
idx, stx, sty, edx, edy, obx, oby, qt1, qt2, qt3, qt4, js1, js2, js3, js4, js5, js6, je1, je2, je3, je4, je5, je6 = data
go_home()
reset_body(box_id, box_pos)
_, _ = get_img()
for i in range(5):
robot.arm.set_ee_pose([stx[i], sty[i], rest_height], origin[1])
robot.arm.move_ee_xyz([0, 0, min_height-rest_height], 0.015)
robot.arm.move_ee_xyz([edx[i]-stx[i], edy[i]-sty[i], 0], 0.015)
robot.arm.move_ee_xyz([0, 0, rest_height-min_height], 0.015)
_, _ = get_img()
start_jpos = robot.arm.compute_ik([stx[i], sty[i], min_height])
end_jpos = robot.arm.compute_ik([edx[i], edy[i], min_height])
print('start')
print(js1[i], js2[i], js3[i], js4[i], js5[i], js6[i])
print(start_jpos)
print('end')
print(je1[i], je2[i], je3[i], je4[i], je5[i], je6[i])
print(end_jpos)
time.sleep(1)
def eval_poke(tag, ground_truth, img_idx, poke):
# ground_truth is the entire matrix from 'image_xx.txt' file
# img_idx is image index, first column of ground_truth
# poke is the predicted 4 vector for world, 8 for pixel and joint
# second half of the 8 vector are world coordinates computed from gt pixel and joint values
# label index in the poke data array (29, 1)
# index 0 is the poke index corresponding to starting image
stx, sty, edx, edy = 1, 2, 3, 4 # ee pos of start and end poke
obx, oby, qt1, qt2, qt3, qt4 = 5, 6, 7, 8, 9, 10 # obj pose before poke
js1, js2, js3, js4, js5, js6 = 11, 12, 13, 14, 15, 16 # jpos before poke
je1, je2, je3, je4, je5, je6 = 17, 18, 19, 20, 21, 22 # jpos after poke
sr, stc, edr, edc, obr, obc = 23, 24, 25, 26, 27, 28 # row and col locations in image
gt = ground_truth[img_idx:img_idx+2]
tgt_posi = gt[1, 5:8]
tgt_posi[-1] = box_z
tgt_quat = gt[1, 7:11]
init_posi = gt[0, 5:8]
init_posi[-1] = box_z
init_quat = gt[0, 7:11]
gt_poke = gt[0, 1:5]
go_home()
reset_body(box_id, init_posi, init_quat)
box_id2 = load_geom('box', size=box_size, mass=1,
base_pos=tgt_posi, base_ori=tgt_quat, rgba=[0,0,1,0.5])
p.setCollisionFilterPair(box_id, box_id2, -1, -1, enableCollision=0)
# to check robot_id and link_id
# robot.arm.robot_id
# robot.arm.p.getNumJoints(robot_id)
# robot.arm.p.getJointInfo(robot_id, 0-max_lnik_id)
p.setCollisionFilterPair(box_id2, 1, -1, 11, enableCollision=0)
_, _ = get_img()
# time.sleep(1)
# reset_body(box_id, init_posi, init_quat)
# robot.arm.move_ee_xyz([gt_poke[0]-home[0], gt_poke[1]-home[1], 0], 0.015)
# robot.arm.move_ee_xyz([0, 0, min_height-rest_height], 0.015)
# robot.arm.move_ee_xyz([gt_poke[2]-gt_poke[0], gt_poke[3]-gt_poke[1], 0], 0.015)
# robot.arm.move_ee_xyz([0, 0, rest_height-min_height], 0.015)
# go_home()
# _, _ = get_img()
# reset_body(box_id, init_posi, init_quat)
# poke = ground_truth[img_idx, 1:5]
robot.arm.move_ee_xyz([poke[0]-home[0], poke[1]-home[1], 0], 0.015)
robot.arm.move_ee_xyz([0, 0, min_height-rest_height], 0.015)
robot.arm.move_ee_xyz([poke[2]-poke[0], poke[3]-poke[1], 0], 0.015)
robot.arm.move_ee_xyz([0, 0, rest_height-min_height], 0.015)
go_home()
_, _ = get_img()
curr_posi, _, _, _ = get_body_state(box_id)
dist = np.sqrt((tgt_posi[0]-curr_posi[0])**2 + (tgt_posi[1]-curr_posi[1])**2)
print(dist)
# time.sleep(0.5)
remove_body(box_id2)
return dist
def calc_dist(gtfile, pdfile, tag, test_num=50):
# this function generate 50 visualization sequence
true = np.loadtxt(gtfile)
pred = np.loadtxt(pdfile)
accu_dist = 0.0
# query = random.sample(range(0, pred.shape[0]), test_num)
total = pred.shape[0]
query = range(total)[::total/test_num]
for i in query:
img_idx = int(pred[i][0])
if tag == 'world':
poke = pred[i][1:5]
elif tag == 'pixel':
rows = np.array([pred[i, 1], pred[i, 3], pred[i, 7], pred[i, 9]])
cols = np.array([pred[i, 2], pred[i, 4], pred[i, 8], pred[i, 10]])
rows = ((rows*3.2 + 40) + 0.5).astype(int)
cols = ((cols*3.2) + 0.5).astype(int)
depth_file = gtfile[:-4] + '_depth/' + str(img_idx) + '.npy'
depth_im = np.load(depth_file)
pokes_3d = get_pix_3dpt(depth_im, rows, cols)
poke = pokes_3d[:, :2].flatten()
else:
raise Exception("experiment_tag has to be 'world', 'joint', or 'pixel'")
accu_dist += eval_poke(tag, true, img_idx, poke)
return accu_dist / len(query)
def get_pix_3dpt(depth_im, rs, cs, in_world=True, filter_depth=False,
k=1, ktype='median', depth_min=None, depth_max=None):
if not isinstance(rs, int) and not isinstance(rs, list) and \
not isinstance(rs, np.ndarray):
raise TypeError('rs should be an int, a list or a numpy array')
if not isinstance(cs, int) and not isinstance(cs, list) and \
not isinstance(cs, np.ndarray):
raise TypeError('cs should be an int, a list or a numpy array')
if isinstance(rs, int):
rs = [rs]
if isinstance(cs, int):
cs = [cs]
if isinstance(rs, np.ndarray):
rs = rs.flatten()
if isinstance(cs, np.ndarray):
cs = cs.flatten()
if not (isinstance(k, int) and (k % 2) == 1):
raise TypeError('k should be a positive odd integer.')
# _, depth_im = self.get_images(get_rgb=False, get_depth=True)
if k == 1:
depth_im = depth_im[rs, cs]
else:
depth_im_list = []
if ktype == 'min':
ktype_func = np.min
elif ktype == 'max':
ktype_func = np.max
elif ktype == 'median':
ktype_func = np.median
elif ktype == 'mean':
ktype_func = np.mean
else:
raise TypeError('Unsupported ktype:[%s]' % ktype)
for r, c in zip(rs, cs):
s = k // 2
rmin = max(0, r - s)
rmax = min(self.img_height, r + s + 1)
cmin = max(0, c - s)
cmax = min(self.img_width, c + s + 1)
depth_im_list.append(ktype_func(depth_im[rmin:rmax,
cmin:cmax]))
depth_im = np.array(depth_im_list)
depth = depth_im.reshape(-1) * 1 #self.depth_scale
img_pixs = np.stack((rs, cs)).reshape(2, -1)
img_pixs[[0, 1], :] = img_pixs[[1, 0], :]
depth_min = depth_min if depth_min else 0.01 #self.depth_min
depth_max = depth_max if depth_max else 10 #self.depth_max
if filter_depth:
valid = depth > depth_min
valid = np.logical_and(valid,
depth < depth_max)
depth = depth[:, valid]
img_pixs = img_pixs[:, valid]
get_img() # to set up camera matrices
cam_int_mat_inv = robot.cam.cam_int_mat_inv
cam_ext_mat = robot.cam.cam_ext_mat
uv_one = np.concatenate((img_pixs,
np.ones((1, img_pixs.shape[1]))))
uv_one_in_cam = np.dot(cam_int_mat_inv, uv_one)
pts_in_cam = np.multiply(uv_one_in_cam, depth)
if in_world:
if cam_ext_mat is None:
raise ValueError('Please call set_cam_ext() first to set up'
' the camera extrinsic matrix')
pts_in_cam = np.concatenate((pts_in_cam,
np.ones((1, pts_in_cam.shape[1]))),
axis=0)
pts_in_world = np.dot(cam_ext_mat, pts_in_cam)
pts_in_world = pts_in_world[:3, :].T
return pts_in_world
else:
return pts_in_cam.T
from IPython import embed
embed()
def main(args):
global joint_lock
global cfg
joint_lock = threading.RLock()
print(args)
rospy.init_node('test')
object_urdf = args.config_package_path + \
'descriptions/urdf/'+args.object_name+'.urdf'
object_mesh = args.config_package_path + \
'descriptions/meshes/objects'+args.object_name+'.stl'
moveit_robot = moveit_commander.RobotCommander()
moveit_scene = moveit_commander.PlanningSceneInterface()
moveit_planner = 'RRTconnectkConfigDefault'
# moveit_planner = 'RRTstarkConfigDefault'
mp_left = GroupPlanner('left_arm',
moveit_robot,
moveit_planner,
moveit_scene,
max_attempts=50,
planning_time=5.0,
goal_tol=0.5,
eef_delta=0.01,
jump_thresh=10.0)
mp_right = GroupPlanner('right_arm',
moveit_robot,
moveit_planner,
moveit_scene,
max_attempts=50,
planning_time=5.0,
goal_tol=0.5,
eef_delta=0.01,
jump_thresh=10.0)
cfg_file = os.path.join(args.example_config_path, args.primitive) + ".yaml"
cfg = get_cfg_defaults()
cfg.merge_from_file(cfg_file)
cfg.freeze()
print(cfg)
real_object_init = [
0.3, -0.2, 0.02, -0.007008648232757453, 0.0019098461832132076,
0.5471545719627792, 0.8370000631527658
]
manipulated_object = None
object_pose1_world = util.list2pose_stamped(cfg.OBJECT_INIT)
# object_pose1_world = util.list2pose_stamped(real_object_init)
object_pose2_world = util.list2pose_stamped(cfg.OBJECT_FINAL)
palm_pose_l_object = util.list2pose_stamped(cfg.PALM_LEFT)
palm_pose_r_object = util.list2pose_stamped(cfg.PALM_RIGHT)
active_arm, unactive_arm = get_active_arm(cfg.OBJECT_INIT)
planner_args = {}
planner_args['object_pose1_world'] = object_pose1_world
planner_args['object_pose2_world'] = object_pose2_world
planner_args['palm_pose_l_object'] = palm_pose_l_object
planner_args['palm_pose_r_object'] = palm_pose_r_object
planner_args['object'] = manipulated_object
planner_args['N'] = 60 # 60
planner_args['init'] = True
global initial_plan
initial_plan = get_primitive_plan(args.primitive, planner_args,
args.config_package_path, active_arm)
box_id = None
yumi = Robot('yumi',
pb=True,
arm_cfg={
'render': True,
'self_collision': False
})
# yumi.arm.go_home()
yumi.arm.set_jpos(cfg.RIGHT_INIT + cfg.LEFT_INIT)
global both_pos
global single_pos
global joint_lock
global palm_contact
joint_lock.acquire()
both_pos = cfg.RIGHT_INIT + cfg.LEFT_INIT
single_pos = {}
single_pos['right'] = cfg.RIGHT_INIT
single_pos['left'] = cfg.LEFT_INIT
joint_lock.release()
if args.object:
box_id = pb_util.load_urdf(
args.config_package_path + 'descriptions/urdf/' +
args.object_name + '.urdf', cfg.OBJECT_INIT[0:3],
cfg.OBJECT_INIT[3:])
# box_id = pb_util.load_urdf(
# args.config_package_path +
# 'descriptions/urdf/'+args.object_name+'.urdf',
# real_object_init[0:3],
# real_object_init[3:]
# )
global trans_box_id
global trans_box_id_final
trans_box_id = pb_util.load_urdf(
args.config_package_path + 'descriptions/urdf/' +
args.object_name + '_trans.urdf', cfg.OBJECT_INIT[0:3],
cfg.OBJECT_INIT[3:])
trans_box_id_final = pb_util.load_urdf(
args.config_package_path + 'descriptions/urdf/' +
args.object_name + '_trans.urdf', cfg.OBJECT_INIT[0:3],
cfg.OBJECT_INIT[3:])
sleep_t = 0.005
loop_t = 0.125
ik = IKHelper()
if args.primitive == 'push' or args.primitive == 'pull':
execute_thread = threading.Thread(target=_yumi_execute_arm,
args=(yumi, active_arm))
else:
execute_thread = threading.Thread(target=_yumi_execute_both,
args=(yumi, ))
execute_thread.daemon = True
execute_thread.start()
box_thread = threading.Thread(target=execute_planned_pull, args=(yumi, ))
box_thread.daemon = True
palm_contact = False
box_thread.start()
replan = args.replan
embed()
for plan_number, plan_dict in enumerate(initial_plan):
intermediate_object_final = util.pose_stamped2list(
plan_dict['object_poses_world'][-1])
if plan_number == len(initial_plan) - 1:
intermediate_object_final = cfg.OBJECT_FINAL
tip_poses = plan_dict['palm_poses_world']
r_wrist_pos_world = yumi.arm.get_ee_pose(arm='right')[0].tolist()
r_wrist_ori_world = yumi.arm.get_ee_pose(arm='right')[1].tolist()
l_wrist_pos_world = yumi.arm.get_ee_pose(arm='left')[0].tolist()
l_wrist_ori_world = yumi.arm.get_ee_pose(arm='left')[1].tolist()
current_wrist_poses = {}
current_wrist_poses['right'] = util.list2pose_stamped(
r_wrist_pos_world + r_wrist_ori_world)
current_wrist_poses['left'] = util.list2pose_stamped(
l_wrist_pos_world + l_wrist_ori_world)
current_tip_poses = get_wrist_to_tip(current_wrist_poses, cfg)
tip_right = []
tip_left = []
tip_right.append(current_tip_poses['right'].pose)
tip_left.append(current_tip_poses['left'].pose)
for i in range(len(tip_poses)):
tip_left.append(tip_poses[i][0].pose)
tip_right.append(tip_poses[i][1].pose)
l_current = yumi.arm.get_jpos()[7:]
r_current = yumi.arm.get_jpos()[:7]
if args.primitive == 'pivot' or args.primitive == 'grasp':
print("Planning with MoveIt! Plan number: " + str(plan_number))
traj_right = mp_right.plan_waypoints(tip_right,
force_start=l_current +
r_current,
avoid_collisions=False)
traj_left = mp_left.plan_waypoints(tip_left,
force_start=l_current +
r_current,
avoid_collisions=False)
unified = unify_arm_trajectories(traj_left, traj_right, tip_poses)
aligned_left = unified['left']['aligned_joints']
aligned_right = unified['right']['aligned_joints']
if aligned_left.shape != aligned_right.shape:
raise ValueError('Could not aligned joint trajectories')
reached_final = False
for i in range(aligned_right.shape[0]):
# r_pos = aligned_right[i, :]
# l_pos = aligned_left[i, :]
planned_r = aligned_right[i, :]
planned_l = aligned_left[i, :]
if both_in_contact(
yumi, box_id
) and args.primitive == 'grasp' and plan_number > 0:
reached_final, pos_err_total, ori_err_total = reach_pose_goal(
intermediate_object_final[:3],
intermediate_object_final[3:],
yumi.arm.p.getBasePositionAndOrientation,
box_id,
pos_tol=0.025,
ori_tol=0.01)
print("Reached final: " + str(reached_final))
timeout = 30
start_while = time.time()
pos_err = pos_err_total
ori_err = ori_err_total
timed_out = False
not_perturbed = 0
none_count = 0
if replan:
while not reached_final and not timed_out and both_in_contact(
yumi, box_id):
# if i == 40:
palm_contact = True
pose_ref_r = util.pose_stamped2list(
ik_helper.compute_fk(
joints=yumi.arm.arm_dict['right'].get_jpos(
),
arm='r'))
diffs_r = util.pose_difference_np(
pose=unified['right']['aligned_fk'][i:],
pose_ref=pose_ref_r)
pose_ref_l = util.pose_stamped2list(
ik_helper.compute_fk(
joints=yumi.arm.arm_dict['left'].get_jpos(
),
arm='l'))
diffs_l = util.pose_difference_np(
pose=unified['left']['aligned_fk'][i:],
pose_ref=pose_ref_l)
seed_ind_r = np.argmin(diffs_r[0])
seed_ind_l = np.argmin(diffs_l[0])
seed = {}
seed['right'] = aligned_right[i:, :][seed_ind_r, :]
seed['left'] = aligned_left[i:, :][seed_ind_l, :]
# yumi.arm.p.resetBasePositionAndOrientation(
# box_id,
# util.pose_stamped2list(initial_plan[0]['object_poses_world'][-1])[:3],
# util.pose_stamped2list(
# initial_plan[0]['object_poses_world'][-1])[3:])
# frac_complete = ori_err/ori_err_total
# frac_complete = pos_err/pos_err_total
frac_complete = (ori_err / ori_err_total +
pos_err / pos_err_total) / 2
joints_execute, new_plan = greedy_replan(
yumi,
active_arm,
box_id,
args.primitive,
object_pose2_world,
args.config_package_path,
ik,
seed,
frac_complete=frac_complete,
plan_iteration=plan_number)
if ori_err / ori_err_total < 0.8 and not_perturbed == 0 and plan_number == 1:
perturb_box(
yumi,
box_id, [0.0, 0.0, 0.0],
delta_ori_euler=[np.pi / 8, 0.0, 0.0])
not_perturbed += 1
#############################################
# if plan_number == 2:
if False:
new_tip_poses = new_plan[plan_number][
'palm_poses_world']
new_tip_right = []
new_tip_left = []
for k in range(len(new_tip_poses)):
new_tip_left.append(
new_tip_poses[k][0].pose)
new_tip_right.append(
new_tip_poses[k][1].pose)
l_current = yumi.arm.get_jpos()[7:]
r_current = yumi.arm.get_jpos()[:7]
new_traj_right = mp_right.plan_waypoints(
new_tip_right,
force_start=l_current + r_current,
avoid_collisions=False)
new_traj_left = mp_left.plan_waypoints(
new_tip_left,
force_start=l_current + r_current,
avoid_collisions=False)
new_unified = unify_arm_trajectories(
new_traj_left, new_traj_right,
new_tip_poses)
new_aligned_left = new_unified['left'][
'aligned_joints']
new_aligned_right = new_unified['right'][
'aligned_joints']
if new_aligned_left.shape != new_aligned_right.shape:
raise ValueError(
'Could not aligned joint trajectories')
for j in range(new_aligned_right.shape[0]):
r_pos = new_aligned_right[j, :].tolist()
l_pos = new_aligned_left[j, :].tolist()
joint_lock.acquire()
both_pos = r_pos + l_pos
joint_lock.release()
time.sleep(loop_t)
##############################################
r_pos = joints_execute['right']
l_pos = joints_execute['left']
if r_pos is not None and l_pos is not None:
joint_lock.acquire()
both_pos = r_pos + l_pos
joint_lock.release()
none_count = 0
else:
none_count += 1
if none_count >= 10:
raise ValueError(
'IK returned none more than 10 times!')
pos_tol = 0.05 if plan_number == 1 else 0.01
reached_final, pos_err, ori_err = reach_pose_goal(
intermediate_object_final[:3],
intermediate_object_final[3:],
yumi.arm.p.getBasePositionAndOrientation,
box_id,
pos_tol=pos_tol,
ori_tol=0.001)
timed_out = time.time() - start_while > timeout
if reached_final:
print("REACHED FINAL")
break
if timed_out:
print("TIMED OUT")
time.sleep(0.005)
r_pos = planned_r.tolist()
l_pos = planned_l.tolist()
else:
r_pos = planned_r.tolist()
l_pos = planned_l.tolist()
if reached_final:
break
joint_lock.acquire()
both_pos = r_pos + l_pos
joint_lock.release()
time.sleep(loop_t)
else:
if active_arm == 'right':
traj = mp_right.plan_waypoints(tip_right,
force_start=l_current +
r_current,
avoid_collisions=False)
else:
traj = mp_left.plan_waypoints(tip_left,
force_start=l_current +
r_current,
avoid_collisions=False)
# make numpy array of each arm joint trajectory for each comp
joints_np = np.zeros((len(traj.points), 7))
# make numpy array of each arm pose trajectory, based on fk
fk_np = np.zeros((len(traj.points), 7))
for i, point in enumerate(traj.points):
joints_np[i, :] = point.positions
pose = ik_helper.compute_fk(point.positions, arm=active_arm[0])
fk_np[i, :] = util.pose_stamped2list(pose)
for i, point in enumerate(traj.points):
# j_pos = point.positions
planned_pos = point.positions
if replan and is_in_contact(
yumi, box_id) and (args.primitive == 'pull'
or args.primitive == 'push'):
palm_contact = True
reached_final, pos_err_total, _ = reach_pose_goal(
cfg.OBJECT_FINAL[:3],
cfg.OBJECT_FINAL[3:],
yumi.arm.p.getBasePositionAndOrientation,
box_id,
pos_tol=0.025,
ori_tol=0.01)
print("Reached final: " + str(reached_final))
timeout = 30
start_while = time.time()
pos_err = pos_err_total
timed_out = False
not_perturbed = 0
none_count = 0
while not reached_final and not timed_out:
# if is_in_contact(yumi, box_id) and (args.primitive == 'pull' or args.primitive == 'push'):
if True:
palm_contact = True
pose_ref = util.pose_stamped2list(
ik_helper.compute_fk(
joints=yumi.arm.arm_dict[active_arm].
get_jpos(),
arm=active_arm[0]))
diffs = util.pose_difference_np(
pose=fk_np, pose_ref=pose_ref)[0]
seed_ind = np.argmin(diffs)
seed = {}
seed[active_arm] = joints_np[seed_ind, :]
seed[unactive_arm] = yumi.arm.arm_dict[
unactive_arm].get_jpos()
joints_execute, _ = greedy_replan(
yumi,
active_arm,
box_id,
args.primitive,
object_pose2_world,
args.config_package_path,
ik,
seed,
frac_complete=pos_err / pos_err_total)
if joints_execute[active_arm] is not None:
joint_lock.acquire()
single_pos[active_arm] = joints_execute[
active_arm]
joint_lock.release()
none_count = 0
else:
none_count += 1
if none_count >= 10:
raise ValueError(
'IK returned none more than 10 times!')
reached_final, pos_err, _ = reach_pose_goal(
cfg.OBJECT_FINAL[:3],
cfg.OBJECT_FINAL[3:],
yumi.arm.p.getBasePositionAndOrientation,
box_id,
pos_tol=0.003,
ori_tol=0.001)
# if pos_err/pos_err_total < 0.3 and not_perturbed==1:
# perturb_box(yumi, box_id,
# [0.0, -0.02, 0.0],
# delta_ori_euler=[0.0, 0.0, -np.pi/6])
# not_perturbed += 1
# if pos_err/pos_err_total < 0.8 and not_perturbed==0:
# perturb_box(yumi, box_id,
# [0.0, -0.05, 0.0],
# delta_ori_euler=[0.0, 0.0, np.pi/3])
# not_perturbed += 1
timed_out = time.time() - start_while > timeout
if reached_final:
print("REACHED FINAL")
return
if timed_out:
print("TIMED OUT")
else:
pass
# joint_lock.acquire()
# single_pos[active_arm] = seed[active_arm]
# joint_lock.release()
time.sleep(0.005)
# j_pos = planned_pos
else:
j_pos = planned_pos
joint_lock.acquire()
single_pos[active_arm] = j_pos
joint_lock.release()
time.sleep(loop_t)
def main(args):
print(args)
yumi = Robot('yumi',
pb=True,
arm_cfg={
'render': True,
'self_collision': False
})
yumi.arm.go_home()
cfg_file = os.path.join(args.example_config_path, args.primitive) + ".yaml"
cfg = get_cfg_defaults()
cfg.merge_from_file(cfg_file)
cfg.freeze()
print(cfg)
yumi.arm.set_jpos(cfg.RIGHT_INIT + cfg.LEFT_INIT)
time.sleep(1.0)
manipulated_object = None
object_pose1_world = util.list2pose_stamped(cfg.OBJECT_INIT)
object_pose2_world = util.list2pose_stamped(cfg.OBJECT_FINAL)
palm_pose_l_object = util.list2pose_stamped(cfg.PALM_LEFT)
palm_pose_r_object = util.list2pose_stamped(cfg.PALM_RIGHT)
if args.primitive == 'push':
plan = pushing_planning(object=manipulated_object,
object_pose1_world=object_pose1_world,
object_pose2_world=object_pose2_world,
palm_pose_l_object=palm_pose_l_object,
palm_pose_r_object=palm_pose_r_object)
elif args.primitive == 'grasp':
plan = grasp_planning(object=manipulated_object,
object_pose1_world=object_pose1_world,
object_pose2_world=object_pose2_world,
palm_pose_l_object=palm_pose_l_object,
palm_pose_r_object=palm_pose_r_object)
elif args.primitive == 'pivot':
gripper_name = args.config_package_path + \
'descriptions/meshes/mpalm/mpalms_all_coarse.stl'
table_name = args.config_package_path + \
'descriptions/meshes/table/table_top.stl'
manipulated_object = collisions.CollisionBody(
args.config_package_path +
'descriptions/meshes/objects/realsense_box_experiments.stl')
plan = levering_planning(object=manipulated_object,
object_pose1_world=object_pose1_world,
object_pose2_world=object_pose2_world,
palm_pose_l_object=palm_pose_l_object,
palm_pose_r_object=palm_pose_r_object,
gripper_name=gripper_name,
table_name=table_name)
elif args.primitive == 'pull':
plan = pulling_planning(object=manipulated_object,
object_pose1_world=object_pose1_world,
object_pose2_world=object_pose2_world,
palm_pose_l_object=palm_pose_l_object,
palm_pose_r_object=palm_pose_r_object,
arm='r')
else:
raise NotImplementedError
object_loaded = False
for plan_dict in plan:
for i, t in enumerate(plan_dict['t']):
if i == 0 and not object_loaded and args.object:
time.sleep(2.0)
pb_util.load_urdf(
args.config_package_path +
'descriptions/urdf/realsense_box.urdf',
cfg.OBJECT_INIT[0:3], cfg.OBJECT_INIT[3:])
time.sleep(2.0)
object_loaded = True
tip_poses = plan_dict['palm_poses_world'][i]
r_joints, l_joints, wrist_right, wrist_left = get_joint_poses(
tip_poses, yumi, cfg, nullspace=False)
loop_time = 0.125
sleep_time = 0.005
start = time.time()
if args.primitive != 'push':
while (time.time() - start < loop_time):
yumi.arm.set_jpos(list(r_joints) + list(l_joints),
wait=False)
time.sleep(sleep_time)
else:
while (time.time() - start < loop_time):
yumi.arm.set_jpos(r_joints, arm='right', wait=False)
time.sleep(sleep_time)