def get_joint_poses(tip_poses, robot, cfg, nullspace=True):
tip_to_wrist = util.list2pose_stamped(cfg.TIP_TO_WRIST_TF, '')
world_to_world = util.unit_pose()
r_joints, l_joints = None, None
wrist_left = util.convert_reference_frame(tip_to_wrist, world_to_world,
tip_poses[0], "yumi_body")
wrist_right = util.convert_reference_frame(tip_to_wrist, world_to_world,
tip_poses[1], "yumi_body")
wrist_left = util.pose_stamped2list(wrist_left)
wrist_right = util.pose_stamped2list(wrist_right)
# r_joints = robot._accurate_ik(
# wrist_right[0:3],
# wrist_right[3:],
# arm='right',
# nullspace=nullspace)[:7]
# l_joints = robot._accurate_ik(
# wrist_left[0:3],
# wrist_left[3:],
# arm='left',
# nullspace=nullspace)[7:]
r_joints = robot.compute_ik(wrist_right[0:3],
wrist_right[3:],
arm='right',
nullspace=nullspace)[:7]
l_joints = robot.compute_ik(wrist_left[0:3],
wrist_left[3:],
arm='left',
nullspace=nullspace)[7 + 2:-2]
return r_joints, l_joints, wrist_right, wrist_left
def joints_from_pose(pose, arm, seed=None):
dict_ik_pose = {}
counter = 0
if seed is None:
# seed_both_arms = [1.6186728267172805, -0.627957447708273, -2.181258817200459, 0.6194790456943977, 3.286937582146461, 0.7514464459671322, -3.038332444527036, -0.8759314115583274, -1.008775144531517, 1.5611336697750984, 0.5415813719525806, 2.4631588845619383, 0.5179680763672723, 1.5100636942652095]
# seed_both_arms = [1.2845762921016486, -1.728097616287859, -1.484556117795128, -0.060662408807661716,
# 0.9363691436943, 1.045354483503344, 2.9032784162076473,
# -1.427503049876694, -1.7222228380256484, 1.1440022381582904, -0.034734670104648924,
# -0.4423711522101721, 0.5704434127183402, -0.17426065383625167]
if arm == "l":
seed = helper.helper.yumi2robot_joints(
rospy.get_param('grasping_left/yumi_convention/joints'),
deg=True)
# seed = seed_both_arms[7:14]
else:
seed = helper.helper.yumi2robot_joints(
rospy.get_param('grasping_right/yumi_convention/joints'),
deg=True)
# seed = seed_both_arms[0:7]
# while True:
# joints = compute_ik(helper.roshelper.pose_stamped2list(pose), seed, arm=arm)
joints = compute_ik(util.pose_stamped2list(pose), seed, arm=arm)
# counter += 1
if len(joints) > 2: # or counter > 10:
dict_ik_pose['joints'] = joints
# break
if len(joints) == 7:
dict_ik_pose['is_execute'] = True
else:
dict_ik_pose['is_execute'] = False
return dict_ik_pose
def get_joint_poses(tip_poses, robot, cfg, nullspace=True):
"""
[summary]
Args:
tip_poses ([type]): [description]
robot ([type]): [description]
cfg ([type]): [description]
nullspace (bool, optional): [description]. Defaults to True.
Returns:
[type]: [description]
"""
tip_to_wrist = util.list2pose_stamped(cfg.TIP_TO_WRIST_TF, '')
world_to_world = util.unit_pose()
r_joints, l_joints = None, None
wrist_left = util.convert_reference_frame(tip_to_wrist, world_to_world,
tip_poses[0], "yumi_body")
wrist_right = util.convert_reference_frame(tip_to_wrist, world_to_world,
tip_poses[1], "yumi_body")
wrist_left = util.pose_stamped2list(wrist_left)
wrist_right = util.pose_stamped2list(wrist_right)
r_joints = robot.arm.compute_ik(wrist_right[0:3],
wrist_right[3:],
arm='right',
ns=nullspace)
l_joints = robot.arm.compute_ik(wrist_left[0:3],
wrist_left[3:],
arm='left',
ns=nullspace)
return r_joints, l_joints, wrist_right, wrist_left
def joints_from_pose_list(pose_list, seed_both_arms=None):
arm_list = ['l', 'r']
dict_ik_pose = {}
dict_ik_pose['joints'] = []
dict_ik_pose['is_execute'] = []
for pose, arm in zip(pose_list, arm_list):
# roshelper.handle_block_pose(pose.pose, self.br, "/yumi_body", "/gripper_" + arm)
counter = 0
if seed_both_arms == None:
seed_both_arms = [
1.2845762921016486, -1.728097616287859, -1.484556117795128,
-0.060662408807661716, 0.9363691436943, 1.045354483503344,
2.9032784162076473, -1.427503049876694, -1.7222228380256484,
1.1440022381582904, -0.034734670104648924, -0.4423711522101721,
0.5704434127183402, -0.17426065383625167
]
while True:
if arm == "l":
seed = helper.helper.yumi2robot_joints(
rospy.get_param('grasping_left/yumi_convention/joints'),
deg=True)
else:
seed = helper.helper.yumi2robot_joints(
rospy.get_param('grasping_right/yumi_convention/joints'),
deg=True)
# joints = compute_ik(helper.roshelper.pose_stamped2list(pose), seed, arm=arm)
joints = compute_ik(util.pose_stamped2list(pose), seed, arm=arm)
counter += 1
if len(joints) > 2 or counter > 20:
dict_ik_pose['joints'].append(joints)
break
if len(joints) == 7:
dict_ik_pose['is_execute'].append(True)
else:
dict_ik_pose['is_execute'].append(False)
return dict_ik_pose
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 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')
signal.signal(signal.SIGINT, signal_handler)
data_seed = args.np_seed
primitive_name = args.primitive
pickle_path = os.path.join(args.data_dir, primitive_name,
args.experiment_name)
if args.save_data:
suf_i = 0
original_pickle_path = pickle_path
while True:
if os.path.exists(pickle_path):
suffix = '_%d' % suf_i
pickle_path = original_pickle_path + suffix
suf_i += 1
data_seed += 1
else:
os.makedirs(pickle_path)
break
if not os.path.exists(pickle_path):
os.makedirs(pickle_path)
np.random.seed(data_seed)
yumi_ar = Robot('yumi_palms',
pb=True,
pb_cfg={
'gui': args.visualize,
'opengl_render': False
},
arm_cfg={
'self_collision': False,
'seed': data_seed
})
r_gel_id = cfg.RIGHT_GEL_ID
l_gel_id = cfg.LEFT_GEL_ID
table_id = cfg.TABLE_ID
alpha = cfg.ALPHA
K = cfg.GEL_CONTACT_STIFFNESS
restitution = cfg.GEL_RESTITUION
p.changeDynamics(yumi_ar.arm.robot_id,
r_gel_id,
restitution=restitution,
contactStiffness=K,
contactDamping=alpha * K,
rollingFriction=args.rolling)
p.changeDynamics(yumi_ar.arm.robot_id,
l_gel_id,
restitution=restitution,
contactStiffness=K,
contactDamping=alpha * K,
rollingFriction=args.rolling)
yumi_gs = YumiCamsGS(yumi_ar,
cfg,
exec_thread=False,
sim_step_repeat=args.sim_step_repeat)
for _ in range(10):
yumi_gs.update_joints(cfg.RIGHT_INIT + cfg.LEFT_INIT)
cuboid_sampler = CuboidSampler(os.path.join(
os.environ['CODE_BASE'],
'catkin_ws/src/config/descriptions/meshes/objects/cuboids/nominal_cuboid.stl'
),
pb_client=yumi_ar.pb_client)
cuboid_fname_template = os.path.join(
os.environ['CODE_BASE'],
'catkin_ws/src/config/descriptions/meshes/objects/cuboids/')
cuboid_manager = MultiBlockManager(cuboid_fname_template,
cuboid_sampler,
robot_id=yumi_ar.arm.robot_id,
table_id=27,
r_gel_id=r_gel_id,
l_gel_id=l_gel_id)
if args.multi:
cuboid_fname = cuboid_manager.get_cuboid_fname()
# cuboid_fname = '/root/catkin_ws/src/config/descriptions/meshes/objects/cuboids/test_cuboid_smaller_4479.stl'
else:
cuboid_fname = args.config_package_path + 'descriptions/meshes/objects/' + \
args.object_name + '_experiments.stl'
mesh_file = cuboid_fname
print("Cuboid file: " + cuboid_fname)
if args.goal_viz:
goal_visualization = True
else:
goal_visualization = False
obj_id, sphere_ids, mesh, goal_obj_id = \
cuboid_sampler.sample_cuboid_pybullet(
cuboid_fname,
goal=goal_visualization,
keypoints=False)
cuboid_manager.filter_collisions(obj_id, goal_obj_id)
p.changeDynamics(obj_id, -1, lateralFriction=0.4)
# goal_face = 0
goal_faces = [0, 1, 2, 3, 4, 5]
from random import shuffle
shuffle(goal_faces)
goal_face = goal_faces[0]
exp_single = SingleArmPrimitives(cfg, yumi_ar.pb_client.get_client_id(),
obj_id, cuboid_fname)
k = 0
while True:
k += 1
if k > 10:
print('FAILED TO BUILD GRASPING GRAPH')
return
try:
exp_double = DualArmPrimitives(cfg,
yumi_ar.pb_client.get_client_id(),
obj_id,
cuboid_fname,
goal_face=goal_face)
break
except ValueError as e:
print(e)
yumi_ar.pb_client.remove_body(obj_id)
if goal_visualization:
yumi_ar.pb_client.remove_body(goal_obj_id)
cuboid_fname = cuboid_manager.get_cuboid_fname()
print("Cuboid file: " + cuboid_fname)
obj_id, sphere_ids, mesh, goal_obj_id = \
cuboid_sampler.sample_cuboid_pybullet(
cuboid_fname,
goal=goal_visualization,
keypoints=False)
cuboid_manager.filter_collisions(obj_id, goal_obj_id)
p.changeDynamics(obj_id, -1, lateralFriction=0.4)
if primitive_name == 'grasp':
exp_running = exp_double
else:
exp_running = exp_single
action_planner = ClosedLoopMacroActions(cfg,
yumi_gs,
obj_id,
yumi_ar.pb_client.get_client_id(),
args.config_package_path,
replan=args.replan,
object_mesh_file=mesh_file)
if goal_visualization:
trans_box_lock = threading.RLock()
goal_viz = GoalVisual(trans_box_lock, goal_obj_id,
action_planner.pb_client, cfg.OBJECT_POSE_3)
action_planner.update_object(obj_id, mesh_file)
exp_single.initialize_object(obj_id, cuboid_fname)
dynamics_info = {}
dynamics_info['contactDamping'] = alpha * K
dynamics_info['contactStiffness'] = K
dynamics_info['rollingFriction'] = args.rolling
dynamics_info['restitution'] = restitution
data = {}
data['saved_data'] = []
data['metadata'] = {}
data['metadata']['mesh_file'] = mesh_file
data['metadata']['cfg'] = cfg
data['metadata']['dynamics'] = dynamics_info
data['metadata']['cam_cfg'] = yumi_gs.cam_setup_cfg
data['metadata']['step_repeat'] = args.sim_step_repeat
data['metadata']['seed'] = data_seed
data['metadata']['seed_original'] = args.np_seed
metadata = data['metadata']
data_manager = DataManager(pickle_path)
pred_dir = os.path.join(os.getcwd(), 'predictions')
obs_dir = os.path.join(os.getcwd(), 'observations')
if args.save_data:
with open(os.path.join(pickle_path, 'metadata.pkl'), 'wb') as mdata_f:
pickle.dump(metadata, mdata_f)
total_trials = 0
successes = 0
for _ in range(args.num_blocks):
# for goal_face in goal_faces:
for _ in range(1):
goal_face = np.random.randint(6)
try:
print('New object!')
exp_double.initialize_object(obj_id, cuboid_fname, goal_face)
except ValueError as e:
print(e)
print('Goal face: ' + str(goal_face))
continue
for _ in range(args.num_obj_samples):
total_trials += 1
if primitive_name == 'grasp':
start_face = exp_double.get_valid_ind()
if start_face is None:
print('Could not find valid start face')
continue
plan_args = exp_double.get_random_primitive_args(
ind=start_face, random_goal=True, execute=True)
elif primitive_name == 'pull':
plan_args = exp_single.get_random_primitive_args(
ind=goal_face, random_goal=True, execute=True)
start_pose = plan_args['object_pose1_world']
goal_pose = plan_args['object_pose2_world']
if goal_visualization:
goal_viz.update_goal_state(
util.pose_stamped2list(goal_pose))
if args.debug:
import simulation
plan = action_planner.get_primitive_plan(
primitive_name, plan_args, 'right')
for i in range(10):
simulation.visualize_object(
start_pose,
filepath=
"package://config/descriptions/meshes/objects/cuboids/"
+ cuboid_fname.split('objects/cuboids')[1],
name="/object_initial",
color=(1., 0., 0., 1.),
frame_id="/yumi_body",
scale=(1., 1., 1.))
simulation.visualize_object(
goal_pose,
filepath=
"package://config/descriptions/meshes/objects/cuboids/"
+ cuboid_fname.split('objects/cuboids')[1],
name="/object_final",
color=(0., 0., 1., 1.),
frame_id="/yumi_body",
scale=(1., 1., 1.))
rospy.sleep(.1)
simulation.simulate(
plan,
cuboid_fname.split('objects/cuboids')[1])
else:
success = False
attempts = 0
while True:
attempts += 1
time.sleep(0.1)
for _ in range(10):
yumi_gs.update_joints(cfg.RIGHT_INIT +
cfg.LEFT_INIT)
p.resetBasePositionAndOrientation(
obj_id,
util.pose_stamped2list(start_pose)[:3],
util.pose_stamped2list(start_pose)[3:])
if attempts > 15:
break
print('attempts: ' + str(attempts))
try:
obs, pcd = yumi_gs.get_observation(
obj_id=obj_id,
robot_table_id=(yumi_ar.arm.robot_id,
table_id))
obj_pose_world = start_pose
obj_pose_final = goal_pose
start = util.pose_stamped2list(obj_pose_world)
goal = util.pose_stamped2list(obj_pose_final)
start_mat = util.matrix_from_pose(obj_pose_world)
goal_mat = util.matrix_from_pose(obj_pose_final)
T_mat = np.matmul(goal_mat,
np.linalg.inv(start_mat))
transformation = np.asarray(util.pose_stamped2list(
util.pose_from_matrix(T_mat)),
dtype=np.float32)
# model takes in observation, and predicts:
pointcloud_pts = np.asarray(
obs['down_pcd_pts'][:100, :], dtype=np.float32)
obs_fname = os.path.join(
obs_dir,
str(total_trials) + '.npz')
np.savez(obs_fname,
pointcloud_pts=pointcloud_pts,
transformation=transformation)
# embed()
got_file = False
pred_fname = os.path.join(
pred_dir,
str(total_trials) + '.npz')
start = time.time()
while True:
try:
prediction = np.load(pred_fname)
got_file = True
except:
pass
if got_file or (time.time() - start > 300):
break
time.sleep(0.01)
if not got_file:
wait = raw_input(
'waiting for predictions to come back online'
)
continue
os.remove(pred_fname)
# embed()
ind = np.random.randint(10)
# contact_obj_frame_r = prediction['prediction'][ind, :7]
# contact_obj_frame_l = prediction['prediction'][ind, 7:]
contact_prediction_r = prediction['prediction'][
ind, :7]
contact_prediction_l = prediction['prediction'][
ind, 7:]
contact_world_pos_r = contact_prediction_r[:3] + np.mean(
pointcloud_pts, axis=0)
contact_world_pos_l = contact_prediction_l[:3] + np.mean(
pointcloud_pts, axis=0)
contact_world_pos_pred = {}
contact_world_pos_pred[
'right'] = contact_world_pos_r
contact_world_pos_pred[
'left'] = contact_world_pos_l
contact_world_pos_corr = correct_grasp_pos(
contact_world_pos_pred, obs['pcd_pts'])
contact_world_pos_r = contact_world_pos_corr[
'right']
contact_world_pos_l = contact_world_pos_corr[
'left']
contact_world_r = contact_world_pos_r.tolist(
) + contact_prediction_r[3:].tolist()
contact_world_l = contact_world_pos_l.tolist(
) + contact_prediction_l[3:].tolist()
palm_poses_world = {}
# palm_poses_world['right'] = util.convert_reference_frame(
# util.list2pose_stamped(contact_obj_frame_r),
# util.unit_pose(),
# obj_pose_world)
# palm_poses_world['left'] = util.convert_reference_frame(
# util.list2pose_stamped(contact_obj_frame_l),
# util.unit_pose(),
# obj_pose_world)
palm_poses_world['right'] = util.list2pose_stamped(
contact_world_r)
palm_poses_world['left'] = util.list2pose_stamped(
contact_world_l)
# obj_pose_final = self.goal_pose_world_frame_mod``
palm_poses_obj_frame = {}
# delta = 10e-3
penetration_delta = 7.5e-3
# delta = np.random.random_sample() * \
# (penetration_delta - 0.5*penetration_delta) + \
# penetration_delta
delta = penetration_delta
y_normals = action_planner.get_palm_y_normals(
palm_poses_world)
for key in palm_poses_world.keys():
# try to penetrate the object a small amount
palm_poses_world[
key].pose.position.x -= delta * y_normals[
key].pose.position.x
palm_poses_world[
key].pose.position.y -= delta * y_normals[
key].pose.position.y
palm_poses_world[
key].pose.position.z -= delta * y_normals[
key].pose.position.z
palm_poses_obj_frame[
key] = util.convert_reference_frame(
palm_poses_world[key], obj_pose_world,
util.unit_pose())
# plan_args['palm_pose_r_object'] = util.list2pose_stamped(contact_obj_frame_r)
# plan_args['palm_pose_l_object'] = util.list2pose_stamped(contact_obj_frame_l)
plan_args[
'palm_pose_r_object'] = palm_poses_obj_frame[
'right']
plan_args[
'palm_pose_l_object'] = palm_poses_obj_frame[
'left']
plan = action_planner.get_primitive_plan(
primitive_name, plan_args, 'right')
# import simulation
# for i in range(10):
# simulation.visualize_object(
# start_pose,
# filepath="package://config/descriptions/meshes/objects/cuboids/" +
# cuboid_fname.split('objects/cuboids')[1],
# name="/object_initial",
# color=(1., 0., 0., 1.),
# frame_id="/yumi_body",
# scale=(1., 1., 1.))
# simulation.visualize_object(
# goal_pose,
# filepath="package://config/descriptions/meshes/objects/cuboids/" +
# cuboid_fname.split('objects/cuboids')[1],
# name="/object_final",
# color=(0., 0., 1., 1.),
# frame_id="/yumi_body",
# scale=(1., 1., 1.))
# rospy.sleep(.1)
# simulation.simulate(plan, cuboid_fname.split('objects/cuboids')[1])
# continue
result = action_planner.execute(
primitive_name, plan_args)
if result is None:
continue
print('Result: ' + str(result[0]) +
' Pos Error: ' + str(result[1]) +
' Ori Error: ' + str(result[2]))
if result[0]:
successes += 1
print('Success rate: ' +
str(successes * 100.0 / total_trials))
break
except ValueError as e:
print("Value error: ")
print(e)
if args.nice_pull_release:
time.sleep(1.0)
pose = util.pose_stamped2list(
yumi_gs.compute_fk(yumi_gs.get_jpos(arm='right')))
pos, ori = pose[:3], pose[3:]
pos[2] += 0.001
r_jnts = yumi_gs.compute_ik(
pos, ori, yumi_gs.get_jpos(arm='right'))
l_jnts = yumi_gs.get_jpos(arm='left')
if r_jnts is not None:
for _ in range(10):
pos[2] += 0.001
r_jnts = yumi_gs.compute_ik(
pos, ori, yumi_gs.get_jpos(arm='right'))
l_jnts = yumi_gs.get_jpos(arm='left')
if r_jnts is not None:
yumi_gs.update_joints(
list(r_jnts) + l_jnts)
time.sleep(0.1)
time.sleep(0.1)
for _ in range(10):
yumi_gs.update_joints(cfg.RIGHT_INIT + cfg.LEFT_INIT)
while True:
try:
yumi_ar.pb_client.remove_body(obj_id)
if goal_visualization:
yumi_ar.pb_client.remove_body(goal_obj_id)
cuboid_fname = cuboid_manager.get_cuboid_fname()
print("Cuboid file: " + cuboid_fname)
obj_id, sphere_ids, mesh, goal_obj_id = \
cuboid_sampler.sample_cuboid_pybullet(
cuboid_fname,
goal=goal_visualization,
keypoints=False)
cuboid_manager.filter_collisions(obj_id, goal_obj_id)
p.changeDynamics(obj_id, -1, lateralFriction=0.4)
action_planner.update_object(obj_id, mesh_file)
exp_single.initialize_object(obj_id, cuboid_fname)
# exp_double.initialize_object(obj_id, cuboid_fname, goal_face)
if goal_visualization:
goal_viz.update_goal_obj(goal_obj_id)
break
except ValueError as e:
print(e)
def unify_arm_trajectories(self, left_arm, right_arm, tip_poses):
"""
Function to return a right arm and left arm trajectory
of the same number of points, where the index of the points
that align with the goal cartesian poses of each arm are the
same for both trajectories
Args:
left_arm (JointTrajectory): left arm joint trajectory from
left move group after calling compute_cartesian_path
right_arm (JointTrajectory): right arm joint trajectory from
right move group after calling compute_cartesian_path
tip_poses (list): list of desired end effector poses for
both arms for a particular segment of a primitive plan
Returns:
dict: Dictionary of combined trajectories in different formats.
Keys for each arm, 'right' and 'left', which each are
themselves dictionary. Deeper keys ---
'fk': Cartesian path numpy array, unaligned
'joints': C-space path numpy array, unaligned
'aligned_fk': Cartesian path numpy array, aligned
'aligned_joints': C-space path numpy array, aligned
'closest_inds': Indices of each original path corresponding
to the closest value in the other arms trajectory
"""
# find the longer trajectory
long_traj = 'left' if len(left_arm.points) > len(right_arm.points) \
else 'right'
# make numpy array of each arm joint trajectory for each comp
left_arm_joints_np = np.zeros((len(left_arm.points), 7))
right_arm_joints_np = np.zeros((len(right_arm.points), 7))
# make numpy array of each arm pose trajectory, based on fk
left_arm_fk_np = np.zeros((len(left_arm.points), 7))
right_arm_fk_np = np.zeros((len(right_arm.points), 7))
for i, point in enumerate(left_arm.points):
left_arm_joints_np[i, :] = point.positions
pose = self.compute_fk(point.positions, arm='left')
left_arm_fk_np[i, :] = util.pose_stamped2list(pose)
for i, point in enumerate(right_arm.points):
right_arm_joints_np[i, :] = point.positions
pose = self.compute_fk(point.positions, arm='right')
right_arm_fk_np[i, :] = util.pose_stamped2list(pose)
closest_left_inds = []
closest_right_inds = []
# for each tip_pose, find the index in the longer trajectory that
# most closely matches the pose (using fk)
for i in range(len(tip_poses)):
r_waypoint = util.pose_stamped2list(tip_poses[i][1])
l_waypoint = util.pose_stamped2list(tip_poses[i][0])
r_pos_diffs = util.pose_difference_np(
pose=right_arm_fk_np, pose_ref=np.array(r_waypoint))[0]
l_pos_diffs = util.pose_difference_np(
pose=left_arm_fk_np, pose_ref=np.array(l_waypoint))[0]
# r_pos_diffs, r_ori_diffs = util.pose_difference_np(
# pose=right_arm_fk_np, pose_ref=np.array(r_waypoint))
# l_pos_diffs, l_ori_diffs = util.pose_difference_np(
# pose=left_arm_fk_np, pose_ref=np.array(l_waypoint))
r_index = np.argmin(r_pos_diffs)
l_index = np.argmin(l_pos_diffs)
# r_index = np.argmin(r_pos_diffs + r_ori_diffs)
# l_index = np.argmin(l_pos_diffs + l_ori_diffs)
closest_right_inds.append(r_index)
closest_left_inds.append(l_index)
# Create a new trajectory for the shorter trajectory, that is the same
# length as the longer trajectory.
if long_traj == 'left':
new_right = np.zeros((left_arm_joints_np.shape))
prev_r_ind = 0
prev_new_ind = 0
for i, r_ind in enumerate(closest_right_inds):
# Put the joint values from the short
# trajectory at the indices corresponding to the path waypoints
# at the corresponding indices found for the longer trajectory
new_ind = closest_left_inds[i]
new_right[new_ind, :] = right_arm_joints_np[r_ind, :]
# For the missing values in between the joint waypoints,
# interpolate to fill the trajectory
# if new_ind - prev_new_ind > -1:
interp = np.linspace(right_arm_joints_np[prev_r_ind, :],
right_arm_joints_np[r_ind],
num=new_ind - prev_new_ind)
new_right[prev_new_ind:new_ind, :] = interp
prev_r_ind = r_ind
prev_new_ind = new_ind
aligned_right_joints = new_right
aligned_left_joints = left_arm_joints_np
else:
new_left = np.zeros((right_arm_joints_np.shape))
prev_l_ind = 0
prev_new_ind = 0
for i, l_ind in enumerate(closest_left_inds):
new_ind = closest_right_inds[i]
new_left[new_ind, :] = left_arm_joints_np[l_ind, :]
interp = np.linspace(left_arm_joints_np[prev_l_ind, :],
left_arm_joints_np[l_ind],
num=new_ind - prev_new_ind)
new_left[prev_new_ind:new_ind, :] = interp
prev_l_ind = l_ind
prev_new_ind = new_ind
aligned_right_joints = right_arm_joints_np
aligned_left_joints = new_left
# get aligned poses of the end effector as well
aligned_right_fk = np.zeros((aligned_right_joints.shape[0], 7))
aligned_left_fk = np.zeros((aligned_left_joints.shape[0], 7))
for i in range(aligned_right_joints.shape[0]):
pose_r = self.compute_fk(aligned_right_joints[i, :], arm='right')
aligned_right_fk[i, :] = util.pose_stamped2list(pose_r)
pose_l = self.compute_fk(aligned_left_joints[i, :], arm='left')
aligned_left_fk[i, :] = util.pose_stamped2list(pose_l)
unified = {}
unified['right'] = {}
unified['right']['fk'] = right_arm_fk_np
unified['right']['joints'] = right_arm_joints_np
unified['right']['aligned_fk'] = aligned_right_fk
unified['right']['aligned_joints'] = aligned_right_joints
unified['right']['inds'] = closest_right_inds
unified['left'] = {}
unified['left']['fk'] = left_arm_fk_np
unified['left']['joints'] = left_arm_joints_np
unified['left']['aligned_fk'] = aligned_left_fk
unified['left']['aligned_joints'] = aligned_left_joints
unified['left']['inds'] = closest_left_inds
return unified
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 greedy_replan(yumi,
active_arm,
box_id,
primitive,
object_final_pose,
config_path,
ik,
seed,
frac_complete=1.0,
plan_iteration=0):
"""
[summary]
"""
global initial_plan
# get the current inputs to the planner
# both palms in the object frame, current pose of the object, active arm
object_pos = list(yumi.arm.p.getBasePositionAndOrientation(box_id)[0])
object_ori = list(yumi.arm.p.getBasePositionAndOrientation(box_id)[1])
# r_tip_pos_world = list(yumi.arm.p.getLinkState(yumi.arm.robot_id, 13)[0])
# r_tip_ori_world = list(yumi.arm.p.getLinkState(yumi.arm.robot_id, 13)[1])
# l_tip_pos_world = list(yumi.arm.p.getLinkState(yumi.arm.robot_id, 26)[0])
# l_tip_ori_world = list(yumi.arm.p.getLinkState(yumi.arm.robot_id, 26)[1])
# r_tip_pose_object_frame = util.convert_reference_frame(
# util.list2pose_stamped(r_tip_pos_world + r_tip_ori_world),
# util.list2pose_stamped(object_pos + object_ori),
# util.unit_pose()
# )
# l_tip_pose_object_frame = util.convert_reference_frame(
# util.list2pose_stamped(l_tip_pos_world + l_tip_ori_world),
# util.list2pose_stamped(object_pos + object_ori),
# util.unit_pose()
# )
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)
r_tip_pose_object_frame = util.convert_reference_frame(
current_tip_poses['right'],
util.list2pose_stamped(object_pos + object_ori), util.unit_pose())
l_tip_pose_object_frame = util.convert_reference_frame(
current_tip_poses['left'],
util.list2pose_stamped(object_pos + object_ori), util.unit_pose())
object_pose_current = util.list2pose_stamped(object_pos + object_ori)
primitive_args = {}
primitive_args['object_pose1_world'] = object_pose_current
primitive_args['object_pose2_world'] = object_final_pose
primitive_args['palm_pose_l_object'] = l_tip_pose_object_frame
primitive_args['palm_pose_r_object'] = r_tip_pose_object_frame
primitive_args['object'] = None
primitive_args['N'] = int(
len(initial_plan[plan_iteration]['palm_poses_world']) * frac_complete)
primitive_args['init'] = False
new_plan = get_primitive_plan(primitive, primitive_args, config_path,
active_arm)
if primitive == 'grasp':
next_step = 1 if plan_iteration == 1 else 14
else:
next_step = 1
new_tip_poses = new_plan[plan_iteration]['palm_poses_world'][next_step]
seed_r = seed['right']
seed_l = seed['left']
r_joints = ik.compute_ik(util.pose_stamped2list(new_tip_poses[1])[:3],
util.pose_stamped2list(new_tip_poses[1])[3:],
seed_r,
arm='right')
l_joints = ik.compute_ik(util.pose_stamped2list(new_tip_poses[0])[:3],
util.pose_stamped2list(new_tip_poses[0])[3:],
seed_l,
arm='left')
joints = {}
joints['right'] = r_joints
joints['left'] = l_joints
return joints, new_plan
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')
signal.signal(signal.SIGINT, signal_handler)
data_seed = args.np_seed
np.random.seed(data_seed)
yumi_ar = Robot('yumi_palms',
pb=True,
pb_cfg={'gui': args.visualize},
arm_cfg={
'self_collision': False,
'seed': data_seed
})
r_gel_id = cfg.RIGHT_GEL_ID
l_gel_id = cfg.LEFT_GEL_ID
alpha = cfg.ALPHA
K = cfg.GEL_CONTACT_STIFFNESS
restitution = cfg.GEL_RESTITUION
p.changeDynamics(yumi_ar.arm.robot_id,
r_gel_id,
restitution=restitution,
contactStiffness=K,
contactDamping=alpha * K,
rollingFriction=args.rolling)
p.changeDynamics(yumi_ar.arm.robot_id,
l_gel_id,
restitution=restitution,
contactStiffness=K,
contactDamping=alpha * K,
rollingFriction=args.rolling)
yumi_gs = YumiGelslimPybulet(yumi_ar, cfg, exec_thread=False)
for _ in range(10):
yumi_gs.update_joints(cfg.RIGHT_INIT + cfg.LEFT_INIT)
cuboid_sampler = CuboidSampler(
'/root/catkin_ws/src/primitives/objects/cuboids/nominal_cuboid.stl',
pb_client=yumi_ar.pb_client)
cuboid_fname_template = '/root/catkin_ws/src/config/descriptions/meshes/objects/cuboids/'
cuboid_manager = MultiBlockManager(cuboid_fname_template,
cuboid_sampler,
robot_id=yumi_ar.arm.robot_id,
table_id=27,
r_gel_id=r_gel_id,
l_gel_id=l_gel_id)
cuboid_fname = cuboid_manager.get_cuboid_fname()
print("Cuboid file: " + cuboid_fname)
# cuboid_fname = os.path.join(cuboid_fname_template, 'test_cuboid_smaller_4206.stl')
obj_id, sphere_ids, mesh, goal_obj_id = \
cuboid_sampler.sample_cuboid_pybullet(
cuboid_fname,
goal=True,
keypoints=False)
cuboid_manager.filter_collisions(obj_id, goal_obj_id)
p.changeDynamics(obj_id, -1, lateralFriction=0.4)
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
example_args['init'] = True
example_args['table_face'] = 0
primitive_name = args.primitive
goal_face = 0
# mesh_file = args.config_package_path + 'descriptions/meshes/objects/' + \
# args.object_name + '_experiments.stl'
mesh_file = cuboid_fname
exp_single = SingleArmPrimitives(cfg, yumi_ar.pb_client.get_client_id(),
obj_id, mesh_file)
k = 0
while True:
k += 1
if k > 10:
print('FAILED TO BUILD GRASPING GRAPH')
return
try:
exp_double = DualArmPrimitives(cfg,
yumi_ar.pb_client.get_client_id(),
obj_id,
mesh_file,
goal_face=goal_face)
break
except ValueError as e:
print(e)
yumi_ar.pb_client.remove_body(obj_id)
yumi_ar.pb_client.remove_body(goal_obj_id)
cuboid_fname = cuboid_manager.get_cuboid_fname()
print("Cuboid file: " + cuboid_fname)
obj_id, sphere_ids, mesh, goal_obj_id = \
cuboid_sampler.sample_cuboid_pybullet(
cuboid_fname,
goal=True,
keypoints=False)
cuboid_manager.filter_collisions(obj_id, goal_obj_id)
p.changeDynamics(obj_id, -1, lateralFriction=0.4)
if primitive_name == 'grasp':
exp_running = exp_double
else:
exp_running = exp_single
action_planner = ClosedLoopMacroActions(cfg,
yumi_gs,
obj_id,
yumi_ar.pb_client.get_client_id(),
args.config_package_path,
replan=args.replan,
object_mesh_file=mesh_file)
trans_box_lock = threading.RLock()
goal_viz = GoalVisual(trans_box_lock, goal_obj_id,
action_planner.pb_client, cfg.OBJECT_POSE_3)
action_planner.update_object(obj_id, mesh_file)
exp_single.initialize_object(obj_id, cuboid_fname)
exp_double.initialize_object(obj_id, cuboid_fname, goal_face)
print('Reset multi block!')
for _ in range(args.num_blocks):
for _ in range(args.num_obj_samples):
if primitive_name == 'grasp':
start_face = exp_double.get_valid_ind()
if start_face is None:
print('Could not find valid start face')
continue
plan_args = exp_double.get_random_primitive_args(
ind=start_face, random_goal=True, execute=True)
elif primitive_name == 'pull':
plan_args = exp_single.get_random_primitive_args(
ind=None, random_goal=True, execute=True)
start_pose = plan_args['object_pose1_world']
goal_pose = plan_args['object_pose2_world']
goal_viz.update_goal_state(util.pose_stamped2list(goal_pose))
if args.debug:
import simulation
plan = action_planner.get_primitive_plan(
primitive_name, plan_args, 'right')
for i in range(10):
simulation.visualize_object(
start_pose,
filepath=
"package://config/descriptions/meshes/objects/cuboids/"
+ cuboid_fname.split('objects/cuboids')[1],
name="/object_initial",
color=(1., 0., 0., 1.),
frame_id="/yumi_body",
scale=(1., 1., 1.))
simulation.visualize_object(
goal_pose,
filepath=
"package://config/descriptions/meshes/objects/cuboids/"
+ cuboid_fname.split('objects/cuboids')[1],
name="/object_final",
color=(0., 0., 1., 1.),
frame_id="/yumi_body",
scale=(1., 1., 1.))
rospy.sleep(.1)
simulation.simulate(plan,
cuboid_fname.split('objects/cuboids')[1])
else:
try:
result = action_planner.execute(primitive_name, plan_args)
if result is not None:
print(str(result[0]))
except ValueError as e:
print("Value error: ")
print(e)
if args.nice_pull_release:
time.sleep(1.0)
pose = util.pose_stamped2list(
yumi_gs.compute_fk(yumi_gs.get_jpos(arm='right')))
pos, ori = pose[:3], pose[3:]
pos[2] += 0.001
r_jnts = yumi_gs.compute_ik(pos, ori,
yumi_gs.get_jpos(arm='right'))
l_jnts = yumi_gs.get_jpos(arm='left')
if r_jnts is not None:
for _ in range(10):
pos[2] += 0.001
r_jnts = yumi_gs.compute_ik(
pos, ori, yumi_gs.get_jpos(arm='right'))
l_jnts = yumi_gs.get_jpos(arm='left')
if r_jnts is not None:
yumi_gs.update_joints(list(r_jnts) + l_jnts)
time.sleep(0.1)
time.sleep(0.1)
for _ in range(10):
yumi_gs.update_joints(cfg.RIGHT_INIT + cfg.LEFT_INIT)
while True:
try:
yumi_ar.pb_client.remove_body(obj_id)
yumi_ar.pb_client.remove_body(goal_obj_id)
cuboid_fname = cuboid_manager.get_cuboid_fname()
print("Cuboid file: " + cuboid_fname)
obj_id, sphere_ids, mesh, goal_obj_id = \
cuboid_sampler.sample_cuboid_pybullet(
cuboid_fname,
goal=True,
keypoints=False)
cuboid_manager.filter_collisions(obj_id, goal_obj_id)
p.changeDynamics(obj_id, -1, lateralFriction=0.4)
action_planner.update_object(obj_id, mesh_file)
exp_single.initialize_object(obj_id, cuboid_fname)
exp_double.initialize_object(obj_id, cuboid_fname, goal_face)
goal_viz.update_goal_obj(goal_obj_id)
break
except ValueError as e:
print(e)
def main(args):
# get configuration
cfg_file = osp.join(args.example_config_path, args.primitive) + ".yaml"
cfg = get_cfg_defaults()
cfg.merge_from_file(cfg_file)
cfg.freeze()
rospy.init_node('EvalSubgoal')
signal.signal(signal.SIGINT, signal_handler)
# setup data saving paths
data_seed = args.np_seed
primitive_name = args.primitive
problems_file = '/root/catkin_ws/src/primitives/data/planning/test_problems_0/demo_0.pkl'
with open(problems_file, 'rb') as f:
problems_data = pickle.load(f)
prob_inds = np.arange(len(problems_data), dtype=np.int64).tolist()
data_inds = np.arange(len(problems_data[0]['problems']),
dtype=np.int64).tolist()
pickle_path = osp.join(args.data_dir, primitive_name, args.experiment_name)
if args.save_data:
suf_i = 0
original_pickle_path = pickle_path
# while True:
# if osp.exists(pickle_path):
# suffix = '_%d' % suf_i
# pickle_path = original_pickle_path + suffix
# suf_i += 1
# data_seed += 1
# else:
# os.makedirs(pickle_path)
# break
if not osp.exists(pickle_path):
os.makedirs(pickle_path)
np.random.seed(data_seed)
# initialize airobot and modify dynamics
yumi_ar = Robot('yumi_palms',
pb=True,
pb_cfg={
'gui': args.visualize,
'opengl_render': False
},
arm_cfg={
'self_collision': False,
'seed': data_seed
})
r_gel_id = cfg.RIGHT_GEL_ID
l_gel_id = cfg.LEFT_GEL_ID
table_id = cfg.TABLE_ID
alpha = cfg.ALPHA
K = cfg.GEL_CONTACT_STIFFNESS
restitution = cfg.GEL_RESTITUION
p.changeDynamics(yumi_ar.arm.robot_id,
r_gel_id,
restitution=restitution,
contactStiffness=K,
contactDamping=alpha * K,
rollingFriction=args.rolling,
lateralFriction=0.5)
p.changeDynamics(yumi_ar.arm.robot_id,
l_gel_id,
restitution=restitution,
contactStiffness=K,
contactDamping=alpha * K,
rollingFriction=args.rolling,
lateralFriction=0.5)
# initialize PyBullet + MoveIt! + ROS yumi interface
yumi_gs = YumiCamsGS(yumi_ar,
cfg,
exec_thread=False,
sim_step_repeat=args.sim_step_repeat)
yumi_ar.arm.go_home(ignore_physics=True)
# initialize object sampler
cuboid_sampler = CuboidSampler(osp.join(
os.environ['CODE_BASE'],
'catkin_ws/src/config/descriptions/meshes/objects/cuboids/nominal_cuboid.stl'
),
pb_client=yumi_ar.pb_client)
cuboid_fname_template = osp.join(
os.environ['CODE_BASE'],
'catkin_ws/src/config/descriptions/meshes/objects/cuboids/')
cuboid_manager = MultiBlockManager(cuboid_fname_template,
cuboid_sampler,
robot_id=yumi_ar.arm.robot_id,
table_id=table_id,
r_gel_id=r_gel_id,
l_gel_id=l_gel_id)
if args.multi:
# cuboid_fname = cuboid_manager.get_cuboid_fname()
# cuboid_fname = str(osp.join(
# '/root/catkin_ws/src/config/descriptions/meshes/objects/cuboids',
# problems_data[0]['object_name']))
# get object name
k = 0
prob_inds = copy.deepcopy(
list(np.arange(len(problems_data), dtype=np.int64)))
shuffle(prob_inds)
while True:
if len(prob_inds) == 0:
print('Done with test problems!')
return
prob_ind = prob_inds.pop()
obj_name = problems_data[prob_ind]['object_name'].split('.stl')[0]
if osp.exists(osp.join(pickle_path, obj_name)):
continue
os.makedirs(osp.join(pickle_path, obj_name))
break
cuboid_fname = str(
osp.join(
'/root/catkin_ws/src/config/descriptions/meshes/objects/cuboids',
obj_name + '.stl'))
else:
cuboid_fname = args.config_package_path + 'descriptions/meshes/objects/' + \
args.object_name + '.stl'
mesh_file = cuboid_fname
print("Cuboid file: " + cuboid_fname)
if args.goal_viz:
goal_visualization = True
else:
goal_visualization = False
obj_id, sphere_ids, mesh, goal_obj_id = \
cuboid_sampler.sample_cuboid_pybullet(
cuboid_fname,
goal=goal_visualization,
keypoints=False)
cuboid_manager.filter_collisions(obj_id, goal_obj_id)
p.changeDynamics(obj_id, -1, lateralFriction=1.0)
goal_faces = [0, 1, 2, 3, 4, 5]
# shuffle(goal_faces)
goal_face = goal_faces[0]
# initialize primitive args samplers
exp_single = SingleArmPrimitives(cfg, yumi_ar.pb_client.get_client_id(),
obj_id, cuboid_fname)
k = 0
while True:
k += 1
if k > 10:
print('FAILED TO BUILD GRASPING GRAPH')
return
try:
exp_double = DualArmPrimitives(cfg,
yumi_ar.pb_client.get_client_id(),
obj_id,
cuboid_fname,
goal_face=goal_face)
break
except ValueError as e:
print(e)
yumi_ar.pb_client.remove_body(obj_id)
if goal_visualization:
yumi_ar.pb_client.remove_body(goal_obj_id)
cuboid_fname = cuboid_manager.get_cuboid_fname()
print("Cuboid file: " + cuboid_fname)
obj_id, sphere_ids, mesh, goal_obj_id = \
cuboid_sampler.sample_cuboid_pybullet(
cuboid_fname,
goal=goal_visualization,
keypoints=False)
cuboid_manager.filter_collisions(obj_id, goal_obj_id)
p.changeDynamics(obj_id, -1, lateralFriction=1.0)
if primitive_name == 'grasp':
exp_running = exp_double
else:
exp_running = exp_single
# initialize macro action interface
action_planner = ClosedLoopMacroActions(cfg,
yumi_gs,
obj_id,
yumi_ar.pb_client.get_client_id(),
args.config_package_path,
replan=args.replan,
object_mesh_file=mesh_file)
if goal_visualization:
trans_box_lock = threading.RLock()
goal_viz = GoalVisual(trans_box_lock, goal_obj_id,
action_planner.pb_client, cfg.OBJECT_POSE_3)
action_planner.update_object(obj_id, mesh_file)
exp_single.initialize_object(obj_id, cuboid_fname)
# prep save info
dynamics_info = {}
dynamics_info['contactDamping'] = alpha * K
dynamics_info['contactStiffness'] = K
dynamics_info['rollingFriction'] = args.rolling
dynamics_info['restitution'] = restitution
data = {}
data['saved_data'] = []
data['metadata'] = {}
data['metadata']['mesh_file'] = mesh_file
data['metadata']['cfg'] = cfg
data['metadata']['dynamics'] = dynamics_info
data['metadata']['cam_cfg'] = yumi_gs.cam_setup_cfg
data['metadata']['step_repeat'] = args.sim_step_repeat
data['metadata']['seed'] = data_seed
data['metadata']['seed_original'] = args.np_seed
metadata = data['metadata']
data_manager = DataManager(pickle_path)
# pred_dir = osp.join(os.environ['CODE_BASE'], cfg.PREDICTION_DIR)
# obs_dir = osp.join(os.environ['CODE_BASE'], cfg.OBSERVATION_DIR)
pred_dir = cfg.PREDICTION_DIR
obs_dir = cfg.OBSERVATION_DIR
if not osp.exists(pred_dir):
os.makedirs(pred_dir)
if not osp.exists(obs_dir):
os.makedirs(obs_dir)
if args.save_data:
with open(osp.join(pickle_path, 'metadata.pkl'), 'wb') as mdata_f:
pickle.dump(metadata, mdata_f)
# prep visualization tools
palm_mesh_file = osp.join(os.environ['CODE_BASE'], cfg.PALM_MESH_FILE)
table_mesh_file = osp.join(os.environ['CODE_BASE'], cfg.TABLE_MESH_FILE)
viz_palms = PalmVis(palm_mesh_file, table_mesh_file, cfg)
viz_pcd = PCDVis()
# pull_sampler = PullSamplerBasic()
pull_sampler = PullSamplerVAEPubSub(obs_dir=obs_dir, pred_dir=pred_dir)
grasp_sampler = GraspSamplerVAEPubSub(default_target=None,
obs_dir=obs_dir,
pred_dir=pred_dir,
pointnet=args.pointnet)
# grasp_sampler = GraspSamplerTransVAEPubSub(
# None,
# obs_dir,
# pred_dir,
# pointnet=args.pointnet
# )
# grasp_sampler = GraspSamplerBasic(
# default_target=None
# )
parent, child = Pipe(duplex=True)
work_queue, result_queue = Queue(), Queue()
experiment_manager = GraspEvalManager(yumi_gs,
yumi_ar.pb_client.get_client_id(),
pickle_path, args.exp_name, parent,
child, work_queue, result_queue, cfg)
experiment_manager.set_object_id(obj_id, cuboid_fname)
# begin runs
total_trials = 0
total_executions = 0
total_face_success = 0
# for _ in range(args.num_blocks):
for problem_ind in range(1, len(problems_data)):
for goal_face in goal_faces:
try:
exp_double.initialize_object(obj_id, cuboid_fname, goal_face)
except ValueError as e:
print('Goal face: ' + str(goal_face), e)
continue
for _ in range(args.num_obj_samples):
yumi_ar.arm.go_home(ignore_physics=True)
obj_data = experiment_manager.get_object_data()
if obj_data['trials'] > 0:
kvs = {}
kvs['trials'] = obj_data['trials']
kvs['grasp_success'] = obj_data[
'grasp_success'] * 100.0 / obj_data['trials']
kvs['mp_success'] = obj_data[
'mp_success'] * 100.0 / obj_data['trials']
kvs['face_success'] = obj_data[
'face_success'] * 100.0 / obj_data['trials']
kvs['pos_err'] = np.mean(obj_data['final_pos_error'])
kvs['ori_err'] = np.mean(obj_data['final_ori_error'])
string = ''
for k, v in kvs.items():
string += "%s: %.3f, " % (k, v)
print(string)
total_trials += 1
if primitive_name == 'grasp':
start_face = exp_double.get_valid_ind()
if start_face is None:
print('Could not find valid start face')
continue
plan_args = exp_double.get_random_primitive_args(
ind=start_face, random_goal=True, execute=True)
elif primitive_name == 'pull':
plan_args = exp_single.get_random_primitive_args(
ind=goal_face, random_goal=True, execute=True)
start_pose = plan_args['object_pose1_world']
goal_pose = plan_args['object_pose2_world']
if goal_visualization:
goal_viz.update_goal_state(
util.pose_stamped2list(goal_pose))
goal_viz.hide_goal_obj()
attempts = 0
# embed()
# yumi_ar.pb_client.remove_body(obj_id)
# start_pos = [0.4, 0.0, 0.1]
# un_norm_ori = np.random.rand(4)
# start_ori = un_norm_ori/(np.linalg.norm(un_norm_ori))
# start_pose = util.list2pose_stamped(list(start_pos) + list(start_ori))
# bandu_names = [
# '/root/catkin_ws/src/config/descriptions/bandu/Bandu Block/Bandu Block.urdf',
# '/root/catkin_ws/src/config/descriptions/bandu/Big Ring/Big Ring.urdf',
# '/root/catkin_ws/src/config/descriptions/bandu/Double Wedge/Double Wedge.urdf',
# '/root/catkin_ws/src/config/descriptions/bandu/Egg/Egg.urdf',
# '/root/catkin_ws/src/config/descriptions/bandu/Knight Shape/Knight Shape.urdf',
# '/root/catkin_ws/src/config/descriptions/bandu/Pencil/Pencil.urdf',
# '/root/catkin_ws/src/config/descriptions/bandu/Skewed Rectangular Prism/Skewed Rectangular Prism.urdf',
# '/root/catkin_ws/src/config/descriptions/bandu/Skewed Triangular Prism/Skewed Triangular Prism.urdf',
# '/root/catkin_ws/src/config/descriptions/bandu/Skewed Wedge/Skewed Wedge.urdf',
# ]
# obj_id = yumi_ar.pb_client.load_urdf(
# bandu_names[0],
# start_pos,
# start_ori
# )
# pcd1 = trimesh.PointCloud(pointcloud_pts)
# pcd2 = trimesh.PointCloud(pointcloud_pts[np.where(start_state.pointcloud_mask)[0], :])
# pcd1.colors = [255, 0, 0, 255]
# pcd2.colors = [0, 0, 255, 255]
# scene_full = trimesh.Scene([pcd1, pcd2])
# scene1 = trimesh.Scene([pcd1])
# scene2 = trimesh.Scene([pcd2])
# scene_full.show()
# embed()
while True:
# if attempts > cfg.ATTEMPT_MAX:
if attempts > 4:
experiment_manager.set_mp_success(False, attempts)
experiment_manager.end_trial(None, False)
break
# print('attempts: ' + str(attempts))
attempts += 1
time.sleep(0.1)
yumi_ar.arm.go_home(ignore_physics=True)
if goal_visualization:
goal_viz.update_goal_state(
util.pose_stamped2list(goal_pose))
goal_viz.hide_goal_obj()
time.sleep(1.0)
p.resetBasePositionAndOrientation(
obj_id,
util.pose_stamped2list(start_pose)[:3],
util.pose_stamped2list(start_pose)[3:])
time.sleep(1.0)
obs, pcd = yumi_gs.get_observation(
obj_id=obj_id,
robot_table_id=(yumi_ar.arm.robot_id, table_id))
goal_pose_global = util.pose_stamped2list(goal_pose)
goal_mat_global = util.matrix_from_pose(goal_pose)
start_mat = util.matrix_from_pose(start_pose)
T_mat_global = np.matmul(goal_mat_global,
np.linalg.inv(start_mat))
transformation_global = util.pose_stamped2np(
util.pose_from_matrix(T_mat_global))
# model takes in observation, and predicts:
pointcloud_pts = np.asarray(obs['down_pcd_pts'][:100, :],
dtype=np.float32)
pointcloud_pts_full = np.asarray(np.concatenate(
obs['pcd_pts']),
dtype=np.float32)
table_pts_full = np.concatenate(obs['table_pcd_pts'],
axis=0)
grasp_sampler.update_default_target(
table_pts_full[::500, :])
# sample from model
start_state = PointCloudNode()
start_state.set_pointcloud(pcd=pointcloud_pts,
pcd_full=pointcloud_pts_full)
if primitive_name == 'grasp':
# prediction = grasp_sampler.sample(start_state.pointcloud)
prediction = grasp_sampler.sample(
state=start_state.pointcloud,
state_full=start_state.pointcloud_full)
elif primitive_name == 'pull':
prediction = pull_sampler.sample(
start_state.pointcloud)
start_state.pointcloud_mask = prediction['mask']
new_state = PointCloudNode()
new_state.init_state(start_state,
prediction['transformation'])
correction = False
if primitive_name == 'grasp':
correction = True
new_state.init_palms(
prediction['palms'],
correction=correction,
prev_pointcloud=start_state.pointcloud_full)
trans_execute = util.pose_from_matrix(
new_state.transformation)
if args.final_subgoal:
trans_execute = util.pose_from_matrix(T_mat_global)
if primitive_name == 'grasp':
local_plan = grasp_planning_wf(
util.list2pose_stamped(new_state.palms[7:]),
util.list2pose_stamped(new_state.palms[:7]),
trans_execute)
elif primitive_name == 'pull':
local_plan = pulling_planning_wf(
util.list2pose_stamped(new_state.palms[:7]),
util.list2pose_stamped(new_state.palms[:7]),
trans_execute)
if args.rviz_viz:
import simulation
for i in range(10):
simulation.visualize_object(
start_pose,
filepath=
"package://config/descriptions/meshes/objects/cuboids/"
+ cuboid_fname.split('objects/cuboids')[1],
name="/object_initial",
color=(1., 0., 0., 1.),
frame_id="/yumi_body",
scale=(1., 1., 1.))
simulation.visualize_object(
goal_pose,
filepath=
"package://config/descriptions/meshes/objects/cuboids/"
+ cuboid_fname.split('objects/cuboids')[1],
name="/object_final",
color=(0., 0., 1., 1.),
frame_id="/yumi_body",
scale=(1., 1., 1.))
rospy.sleep(.1)
simulation.simulate(
local_plan,
cuboid_fname.split('objects/cuboids')[1])
embed()
if args.plotly_viz:
plot_data = {}
plot_data['start'] = pointcloud_pts
plot_data[
'object_mask_down'] = start_state.pointcloud_mask
fig, _ = viz_pcd.plot_pointcloud(plot_data,
downsampled=True)
fig.show()
embed()
# embed()
# trans_list = []
# for i in range(50):
# pred = pull_sampler.sample(start_state.pointcloud)
# trans_list.append(util.pose_stamped2np(util.pose_from_matrix(pred['transformation'])))
if args.trimesh_viz:
viz_data = {}
viz_data[
'contact_world_frame_right'] = new_state.palms_raw[:
7]
viz_data[
'contact_world_frame_left'] = new_state.palms_raw[
7:]
# viz_data['contact_world_frame_left'] = new_state.palms_raw[:7]
viz_data['start_vis'] = util.pose_stamped2np(
start_pose)
viz_data['transformation'] = util.pose_stamped2np(
util.pose_from_matrix(
prediction['transformation']))
# viz_data['transformation'] = np.asarray(trans_list).squeeze()
viz_data['mesh_file'] = cuboid_fname
viz_data['object_pointcloud'] = pointcloud_pts_full
viz_data['start'] = pointcloud_pts
# viz_data['start'] = pointcloud_pts_full
viz_data['object_mask'] = prediction['mask']
embed()
scene = viz_palms.vis_palms(viz_data,
world=True,
corr=False,
full_path=True,
goal_number=1)
scene_pcd = viz_palms.vis_palms_pcd(viz_data,
world=True,
corr=False,
full_path=True,
show_mask=True,
goal_number=1)
scene_pcd.show()
# scene.show()
embed()
real_start_pos = p.getBasePositionAndOrientation(obj_id)[0]
real_start_ori = p.getBasePositionAndOrientation(obj_id)[1]
real_start_pose = list(real_start_pos) + list(
real_start_ori)
real_start_mat = util.matrix_from_pose(
util.list2pose_stamped(real_start_pose))
des_goal_pose = util.transform_pose(
util.list2pose_stamped(real_start_pose),
util.pose_from_matrix(prediction['transformation']))
if goal_visualization:
goal_viz.update_goal_state(
util.pose_stamped2list(goal_pose))
goal_viz.show_goal_obj()
# create trial data
trial_data = {}
trial_data['start_pcd'] = pointcloud_pts_full
trial_data['start_pcd_down'] = pointcloud_pts
trial_data['start_pcd_mask'] = start_state.pointcloud_mask
trial_data['obj_fname'] = cuboid_fname
trial_data['start_pose'] = np.asarray(real_start_pose)
trial_data['goal_pose'] = util.pose_stamped2np(
des_goal_pose)
trial_data['goal_pose_global'] = np.asarray(
goal_pose_global)
trial_data['table_pcd'] = table_pts_full[::500, :]
trial_data['trans_des'] = util.pose_stamped2np(
util.pose_from_matrix(prediction['transformation']))
trial_data['trans_des_global'] = transformation_global
# experiment_manager.start_trial()
action_planner.active_arm = 'right'
action_planner.inactive_arm = 'left'
if primitive_name == 'grasp':
# try to execute the action
yumi_ar.arm.set_jpos([
0.9936, -2.1848, -0.9915, 0.8458, 3.7618, 1.5486,
0.1127, -1.0777, -2.1187, 0.995, 1.002, -3.6834,
1.8132, 2.6405
],
ignore_physics=True)
grasp_success = False
try:
for k, subplan in enumerate(local_plan):
time.sleep(1.0)
action_planner.playback_dual_arm(
'grasp', subplan, k)
if k > 0 and experiment_manager.still_grasping(
):
grasp_success = True
real_final_pos = p.getBasePositionAndOrientation(
obj_id)[0]
real_final_ori = p.getBasePositionAndOrientation(
obj_id)[1]
real_final_pose = list(real_final_pos) + list(
real_final_ori)
real_final_mat = util.matrix_from_pose(
util.list2pose_stamped(real_final_pose))
real_T_mat = np.matmul(
real_final_mat, np.linalg.inv(real_start_mat))
real_T_pose = util.pose_stamped2np(
util.pose_from_matrix(real_T_mat))
trial_data['trans_executed'] = real_T_mat
trial_data['final_pose'] = real_final_pose
experiment_manager.set_mp_success(True, attempts)
experiment_manager.end_trial(
trial_data, grasp_success)
# embed()
except ValueError as e:
# print('Value Error: ', e)
continue
elif primitive_name == 'pull':
try:
yumi_ar.arm.set_jpos(cfg.RIGHT_INIT +
cfg.LEFT_INIT,
ignore_physics=True)
time.sleep(0.5)
action_planner.playback_single_arm(
'pull', local_plan[0])
time.sleep(0.5)
action_planner.single_arm_retract()
except ValueError as e:
continue
time.sleep(3.0)
yumi_ar.arm.go_home(ignore_physics=True)
break
embed()
obj_data = experiment_manager.get_object_data()
# obj_name = problems_data[problem_ind]['object_name'].split('.stl')[0]
obj_data_fname = osp.join(pickle_path, obj_name,
obj_name + '_eval_data.pkl')
# print('Object data: ')
# for key in obj_data.keys():
# print(key, obj_data[key])
if args.save_data:
print('Saving to: ' + str(obj_data_fname))
with open(obj_data_fname, 'wb') as f:
pickle.dump(obj_data, f)
yumi_ar.pb_client.remove_body(obj_id)
if goal_visualization:
yumi_ar.pb_client.remove_body(goal_obj_id)
# cuboid_fname = cuboid_manager.get_cuboid_fname()
# cuboid_fname = str(osp.join(
# '/root/catkin_ws/src/config/descriptions/meshes/objects/cuboids',
# problems_data[problem_ind]['object_name']))
while True:
if len(prob_inds) == 0:
print('Done with test problems!')
return
prob_ind = prob_inds.pop()
obj_name = problems_data[prob_ind]['object_name'].split('.stl')[0]
if osp.exists(osp.join(pickle_path, obj_name)):
continue
os.makedirs(osp.join(pickle_path, obj_name))
break
cuboid_fname = str(
osp.join(
'/root/catkin_ws/src/config/descriptions/meshes/objects/cuboids',
obj_name + '.stl'))
obj_id, sphere_ids, mesh, goal_obj_id = \
cuboid_sampler.sample_cuboid_pybullet(
cuboid_fname,
goal=goal_visualization,
keypoints=False)
cuboid_manager.filter_collisions(obj_id, goal_obj_id)
p.changeDynamics(obj_id, -1, lateralFriction=1.0)
action_planner.update_object(obj_id, cuboid_fname)
exp_single.initialize_object(obj_id, cuboid_fname)
experiment_manager.set_object_id(obj_id, cuboid_fname)
if goal_visualization:
goal_viz.update_goal_obj(goal_obj_id)
def worker_yumi(child_conn, work_queue, result_queue, cfg, args, seed,
worker_id, global_info_dict, worker_flag_dict, planning_lock):
while True:
try:
if not child_conn.poll(0.0001):
continue
msg = child_conn.recv()
except (EOFError, KeyboardInterrupt):
break
if msg == "RESET":
data_seed = seed
np.random.seed(data_seed)
yumi_ar = Robot('yumi_palms',
pb=True,
pb_cfg={'gui': args.visualize},
arm_cfg={
'self_collision': False,
'seed': data_seed
})
r_gel_id = cfg.RIGHT_GEL_ID
l_gel_id = cfg.LEFT_GEL_ID
alpha = cfg.ALPHA
K = cfg.GEL_CONTACT_STIFFNESS
restitution = cfg.GEL_RESTITUION
p.changeDynamics(yumi_ar.arm.robot_id,
r_gel_id,
restitution=restitution,
contactStiffness=K,
contactDamping=alpha * K,
rollingFriction=args.rolling)
p.changeDynamics(yumi_ar.arm.robot_id,
l_gel_id,
restitution=restitution,
contactStiffness=K,
contactDamping=alpha * K,
rollingFriction=args.rolling)
dynamics_info = {}
dynamics_info['contactDamping'] = alpha * K
dynamics_info['contactStiffness'] = K
dynamics_info['rollingFriction'] = args.rolling
dynamics_info['restitution'] = restitution
yumi_gs = YumiCamsGS(yumi_ar,
cfg,
exec_thread=False,
sim_step_repeat=args.step_repeat)
for _ in range(10):
yumi_gs.update_joints(cfg.RIGHT_INIT + cfg.LEFT_INIT)
obj_id = yumi_ar.pb_client.load_urdf(
args.config_package_path + 'descriptions/urdf/' +
args.object_name + '.urdf', cfg.OBJECT_POSE_3[0:3],
cfg.OBJECT_POSE_3[3:])
# goal_obj_id = yumi_ar.pb_client.load_urdf(
# args.config_package_path +
# 'descriptions/urdf/'+args.object_name+'_trans.urdf',
# cfg.OBJECT_POSE_3[0:3],
# cfg.OBJECT_POSE_3[3:]
# )
# p.setCollisionFilterPair(yumi_ar.arm.robot_id, goal_obj_id, r_gel_id, -1, enableCollision=False)
# p.setCollisionFilterPair(obj_id, goal_obj_id, -1, -1, enableCollision=False)
p.setCollisionFilterPair(yumi_ar.arm.robot_id,
obj_id,
r_gel_id,
-1,
enableCollision=True)
p.setCollisionFilterPair(yumi_ar.arm.robot_id,
obj_id,
27,
-1,
enableCollision=True)
yumi_ar.pb_client.reset_body(obj_id, cfg.OBJECT_POSE_3[:3],
cfg.OBJECT_POSE_3[3:])
# yumi_ar.pb_client.reset_body(
# goal_obj_id,
# cfg.OBJECT_POSE_3[:3],
# cfg.OBJECT_POSE_3[3:])
primitive_name = args.primitive
mesh_file = '/root/catkin_ws/src/config/descriptions/meshes/objects/cuboids/realsense_box_experiments.stl'
cuboid_fname = mesh_file
face = 0
# setup macro_planner
action_planner = ClosedLoopMacroActions(
cfg,
yumi_gs,
obj_id,
yumi_ar.pb_client.get_client_id(),
args.config_package_path,
object_mesh_file=mesh_file,
replan=args.replan)
exp_single = SingleArmPrimitives(cfg,
yumi_ar.pb_client.get_client_id(),
obj_id, mesh_file)
exp_double = DualArmPrimitives(cfg,
yumi_ar.pb_client.get_client_id(),
obj_id, mesh_file)
if primitive_name == 'grasp' or primitive_name == 'pivot':
exp_running = exp_double
else:
exp_running = exp_single
action_planner.update_object(obj_id, mesh_file)
exp_single.initialize_object(obj_id, cuboid_fname)
exp_double.initialize_object(obj_id, cuboid_fname, face)
# goal_viz = GoalVisual(
# trans_box_lock,
# goal_obj_id,
# action_planner.pb_client,
# cfg.OBJECT_POSE_3)
pickle_path = os.path.join(args.data_dir, primitive_name,
args.experiment_name)
if not os.path.exists(pickle_path):
os.makedirs(pickle_path)
data_manager = DataManager(pickle_path)
continue
if msg == "HOME":
yumi_gs.update_joints(cfg.RIGHT_INIT + cfg.LEFT_INIT)
continue
if msg == "OBJECT_POSE":
continue
if msg == "SAMPLE":
global_info_dict['trial'] += 1
# print('Success: ' + str(global_info_dict['success']) +
# ' Trial number: ' + str(global_info_dict['trial']) +
# ' Worker ID: ' + str(worker_id))
worker_flag_dict[worker_id] = False
success = False
start_face = 1
plan_args = exp_running.get_random_primitive_args(
ind=start_face, random_goal=False, execute=True)
try:
obs, pcd = yumi_gs.get_observation(obj_id=obj_id)
obj_pose_world = plan_args['object_pose1_world']
obj_pose_final = plan_args['object_pose2_world']
contact_obj_frame, contact_world_frame = {}, {}
contact_obj_frame['right'] = plan_args['palm_pose_r_object']
contact_world_frame['right'] = plan_args['palm_pose_r_world']
contact_obj_frame['left'] = plan_args['palm_pose_l_object']
contact_world_frame['left'] = plan_args['palm_pose_l_world']
start = util.pose_stamped2list(obj_pose_world)
goal = util.pose_stamped2list(obj_pose_final)
keypoints_start = np.array(
exp_running.mesh_world.vertices.tolist())
keypoints_start_homog = np.hstack(
(keypoints_start, np.ones((keypoints_start.shape[0], 1))))
start_mat = util.matrix_from_pose(obj_pose_world)
goal_mat = util.matrix_from_pose(obj_pose_final)
T_mat = np.matmul(goal_mat, np.linalg.inv(start_mat))
keypoints_goal = np.matmul(T_mat,
keypoints_start_homog.T).T[:, :3]
contact_obj_frame_dict = {}
contact_world_frame_dict = {}
nearest_pt_world_dict = {}
corner_norms = {}
down_pcd_norms = {}
if primitive_name == 'pull':
# active_arm, inactive_arm = action_planner.get_active_arm(
# util.pose_stamped2list(obj_pose_world)
# )
active_arm = action_planner.active_arm
inactive_arm = action_planner.inactive_arm
contact_obj_frame_dict[
active_arm] = util.pose_stamped2list(
contact_obj_frame[active_arm])
contact_world_frame_dict[
active_arm] = util.pose_stamped2list(
contact_world_frame[active_arm])
# contact_pos = open3d.utility.DoubleVector(np.array(contact_world_frame_dict[active_arm][:3]))
# kdtree = open3d.geometry.KDTreeFlann(pcd)
# nearest_pt_ind = kdtree.search_knn_vector_3d(contact_pos, 1)[1][0]
# nearest_pt_world_dict[active_arm] = np.asarray(pcd.points)[nearest_pt_ind]
contact_pos = np.array(
contact_world_frame_dict[active_arm][:3])
corner_dists = (np.asarray(keypoints_start) - contact_pos)
corner_norms[active_arm] = np.linalg.norm(corner_dists,
axis=1)
down_pcd_dists = (obs['down_pcd_pts'] - contact_pos)
down_pcd_norms[active_arm] = np.linalg.norm(down_pcd_dists,
axis=1)
contact_obj_frame_dict[inactive_arm] = None
contact_world_frame_dict[inactive_arm] = None
nearest_pt_world_dict[inactive_arm] = None
corner_norms[inactive_arm] = None
down_pcd_norms[inactive_arm] = None
elif primitive_name == 'grasp':
for key in contact_obj_frame.keys():
contact_obj_frame_dict[key] = util.pose_stamped2list(
contact_obj_frame[key])
contact_world_frame_dict[key] = util.pose_stamped2list(
contact_world_frame[key])
contact_pos = np.array(
contact_world_frame_dict[key][:3])
corner_dists = (np.asarray(keypoints_start) -
contact_pos)
corner_norms[key] = np.linalg.norm(corner_dists,
axis=1)
down_pcd_dists = (obs['down_pcd_pts'] - contact_pos)
down_pcd_norms[key] = np.linalg.norm(down_pcd_dists,
axis=1)
# contact_pos = open3d.utility.DoubleVector(np.array(contact_world_frame_dict[key][:3]))
# kdtree = open3d.geometry.KDTreeFlann(pcd)
# nearest_pt_ind = kdtree.search_knn_vector_3d(contact_pos, 1)[1][0]
# nearest_pt_world_dict[key] = np.asarray(pcd.points)[nearest_pt_ind]
print('Worker ID: %d, Waiting to plan...' % worker_id)
while True:
time.sleep(0.01)
work_queue_empty = work_queue.empty()
if work_queue_empty:
planner_available = True
break
if planner_available:
print('Worker ID: %d, Planning!...' % worker_id)
work_queue.put(True)
time.sleep(1.0)
result = action_planner.execute(primitive_name, plan_args)
time.sleep(1.0)
while not work_queue.empty():
work_queue.get()
if result is not None:
if result[0]:
success = True
global_info_dict['success'] += 1
sample = {}
sample['obs'] = obs
sample['start'] = start
sample['goal'] = goal
sample['keypoints_start'] = keypoints_start
sample['keypoints_goal'] = keypoints_goal
sample['keypoint_dists'] = corner_norms
sample['down_pcd_pts'] = obs['down_pcd_pts']
sample['down_pcd_dists'] = down_pcd_norms
sample['transformation'] = util.pose_from_matrix(T_mat)
sample['contact_obj_frame'] = contact_obj_frame_dict
sample[
'contact_world_frame'] = contact_world_frame_dict
# sample['contact_pcd'] = nearest_pt_world_dict
sample['result'] = result
if primitive_name == 'grasp':
sample['goal_face'] = exp_double.goal_face
if args.save_data:
data_manager.save_observation(
sample, str(global_info_dict['trial']))
# print("Success: " + str(global_info_dict['success']))
except ValueError as e:
print("Value error: ")
print(e)
result = None
time.sleep(1.0)
while not work_queue.empty():
work_queue.get()
worker_result = {}
worker_result['result'] = result
worker_result['id'] = worker_id
worker_result['success'] = success
result_queue.put(worker_result)
child_conn.send('DONE')
continue
if msg == "END":
break
time.sleep(0.001)
print('Breaking Worker ID: ' + str(worker_id))
child_conn.close()
def main(args):
pull_cfg_file = os.path.join(args.example_config_path, 'pull') + ".yaml"
pull_cfg = get_cfg_defaults()
pull_cfg.merge_from_file(pull_cfg_file)
pull_cfg.freeze()
grasp_cfg_file = os.path.join(args.example_config_path, 'grasp') + ".yaml"
grasp_cfg = get_cfg_defaults()
grasp_cfg.merge_from_file(grasp_cfg_file)
grasp_cfg.freeze()
# cfg = grasp_cfg
cfg = pull_cfg
rospy.init_node('MultiStep')
signal.signal(signal.SIGINT, signal_handler)
data_seed = args.np_seed
np.random.seed(data_seed)
yumi_ar = Robot('yumi_palms',
pb=True,
pb_cfg={'gui': args.visualize},
arm_cfg={
'self_collision': False,
'seed': data_seed
})
r_gel_id = cfg.RIGHT_GEL_ID
l_gel_id = cfg.LEFT_GEL_ID
alpha = cfg.ALPHA
K = cfg.GEL_CONTACT_STIFFNESS
restitution = cfg.GEL_RESTITUION
p.changeDynamics(yumi_ar.arm.robot_id,
r_gel_id,
restitution=restitution,
contactStiffness=K,
contactDamping=alpha * K,
rollingFriction=args.rolling)
p.changeDynamics(yumi_ar.arm.robot_id,
l_gel_id,
restitution=restitution,
contactStiffness=K,
contactDamping=alpha * K,
rollingFriction=args.rolling)
yumi_gs = YumiGelslimPybulet(yumi_ar, cfg, exec_thread=False)
for _ in range(10):
yumi_gs.update_joints(cfg.RIGHT_INIT + cfg.LEFT_INIT)
obj_id = yumi_ar.pb_client.load_urdf(
args.config_package_path + 'descriptions/urdf/' + args.object_name +
'.urdf', cfg.OBJECT_POSE_3[0:3], cfg.OBJECT_POSE_3[3:])
goal_obj_id = yumi_ar.pb_client.load_urdf(
args.config_package_path + 'descriptions/urdf/' + args.object_name +
'_trans.urdf', cfg.OBJECT_POSE_3[0:3], cfg.OBJECT_POSE_3[3:])
p.setCollisionFilterPair(yumi_ar.arm.robot_id,
goal_obj_id,
r_gel_id,
-1,
enableCollision=False)
p.setCollisionFilterPair(obj_id,
goal_obj_id,
-1,
-1,
enableCollision=False)
p.setCollisionFilterPair(yumi_ar.arm.robot_id,
obj_id,
r_gel_id,
-1,
enableCollision=True)
p.setCollisionFilterPair(yumi_ar.arm.robot_id,
obj_id,
27,
-1,
enableCollision=True)
yumi_ar.pb_client.reset_body(obj_id, cfg.OBJECT_POSE_3[:3],
cfg.OBJECT_POSE_3[3:])
yumi_ar.pb_client.reset_body(goal_obj_id, cfg.OBJECT_POSE_3[:3],
cfg.OBJECT_POSE_3[3:])
# 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
# example_args['init'] = True
# example_args['table_face'] = 0
primitive_name = args.primitive
face = np.random.randint(6)
# face = 3
mesh_file = args.config_package_path + 'descriptions/meshes/objects/cuboids/' + \
args.object_name + '_experiments.stl'
cuboid_fname = mesh_file
exp_single = SingleArmPrimitives(pull_cfg,
yumi_ar.pb_client.get_client_id(), obj_id,
mesh_file)
exp_double = DualArmPrimitives(grasp_cfg,
yumi_ar.pb_client.get_client_id(),
obj_id,
mesh_file,
goal_face=face)
action_planner = ClosedLoopMacroActions(cfg,
yumi_gs,
obj_id,
yumi_ar.pb_client.get_client_id(),
args.config_package_path,
replan=args.replan,
object_mesh_file=mesh_file)
trans_box_lock = threading.RLock()
goal_viz = GoalVisual(trans_box_lock, goal_obj_id,
action_planner.pb_client, cfg.OBJECT_POSE_3)
multistep_planner = MultiStepManager(cfg, grasp_cfg, pull_cfg)
action_planner.update_object(obj_id, mesh_file)
exp_single.initialize_object(obj_id, cuboid_fname)
exp_double.initialize_object(obj_id, cuboid_fname, face)
print('Reset multi block!')
for _ in range(args.num_obj_samples):
# get grasp sample
# start_face_index = np.random.randint(len(grasp_cfg.VALID_GRASP_PAIRS[face]))
# start_face = grasp_cfg.VALID_GRASP_PAIRS[face][start_face_index]
# grasp_args = exp_double.get_random_primitive_args(ind=start_face,
# random_goal=True,
# execute=False)
# # pull_args_start = exp_single.get_random_primitive_args(ind=cfg.GRASP_TO_PULL[start_face],
# random_goal=True)
# pull_args_goal = exp_single.get_random_primitive_args(ind=cfg.GRASP_TO_PULL[face],
# random_goal=True)
# pull_args_start['object_pose2_world'] = grasp_args['object_pose1_world']
# pull_args_goal['object_pose1_world'] = grasp_args['object_pose2_world']
# full_args = [pull_args_start, grasp_args, pull_args_goal]
# full_args = [grasp_args]
full_args = multistep_planner.get_args(exp_single,
exp_double,
face,
execute=False)
# obj_pose_final = exp_running.goal_pose_world_frame_mod
# palm_poses_obj_frame = {}
# y_normals = action_planner.get_palm_y_normals(palm_poses_world)
# delta = 2e-3
# for key in palm_poses_world.keys():
# palm_pose_world = palm_poses_world[key]
# # try to penetrate the object a small amount
# palm_pose_world.pose.position.x -= delta*y_normals[key].pose.position.x
# palm_pose_world.pose.position.y -= delta*y_normals[key].pose.position.y
# palm_pose_world.pose.position.z -= delta*y_normals[key].pose.position.z
# palm_poses_obj_frame[key] = util.convert_reference_frame(
# palm_pose_world, obj_pose_world, util.unit_pose())
valid_subplans = 0
valid_plans = []
for plan_args in full_args:
plan = action_planner.get_primitive_plan(plan_args['name'],
plan_args, 'right')
goal_viz.update_goal_state(
util.pose_stamped2list(plan_args['object_pose2_world']))
start_pose = plan_args['object_pose1_world']
goal_pose = plan_args['object_pose2_world']
if args.debug:
for _ in range(10):
simulation.visualize_object(
start_pose,
filepath=
"package://config/descriptions/meshes/objects/cuboids/"
+ cuboid_fname.split('objects/cuboids')[1],
name="/object_initial",
color=(1., 0., 0., 1.),
frame_id="/yumi_body",
scale=(1., 1., 1.))
simulation.visualize_object(
goal_pose,
filepath=
"package://config/descriptions/meshes/objects/cuboids/"
+ cuboid_fname.split('objects/cuboids')[1],
name="/object_final",
color=(0., 0., 1., 1.),
frame_id="/yumi_body",
scale=(1., 1., 1.))
rospy.sleep(.1)
simulation.simulate(plan,
cuboid_fname.split('objects/cuboids')[1])
else:
primitive_name = plan_args['name']
start_joints = {}
if primitive_name == 'pull':
start_joints['right'] = pull_cfg.RIGHT_INIT
start_joints['left'] = pull_cfg.LEFT_INIT
elif primitive_name == 'grasp':
start_joints['right'] = grasp_cfg.RIGHT_INIT
start_joints['left'] = grasp_cfg.LEFT_INIT
subplan_valid = action_planner.full_mp_check(
plan, primitive_name, start_joints=start_joints)
if subplan_valid:
print("subplan valid!")
valid_subplans += 1
valid_plans.append(plan)
else:
print("not valid, primitive: " + primitive_name)
if valid_subplans == len(full_args) and not args.debug:
yumi_ar.pb_client.reset_body(
obj_id,
util.pose_stamped2list(full_args[0]['object_pose1_world'])[:3],
util.pose_stamped2list(full_args[0]['object_pose1_world'])[3:])
for plan_args in full_args:
# teleport to start state, to avoid colliding with object during transit
primitive_name = plan_args['name']
time.sleep(0.1)
for _ in range(10):
if primitive_name == 'pull':
# yumi_gs.update_joints(pull_cfg.RIGHT_INIT + pull_cfg.LEFT_INIT)
yumi_gs.yumi_pb.arm.set_jpos(pull_cfg.RIGHT_INIT +
pull_cfg.LEFT_INIT,
ignore_physics=True)
elif primitive_name == 'grasp':
# yumi_gs.update_joints(grasp_cfg.RIGHT_INIT + grasp_cfg.LEFT_INIT)
yumi_gs.yumi_pb.arm.set_jpos(grasp_cfg.RIGHT_INIT +
grasp_cfg.LEFT_INIT,
ignore_physics=True)
try:
# should update the plan args start state to be the current simulator state
result = action_planner.execute(primitive_name, plan_args)
if result is not None:
print(str(result[0]))
except ValueError as e:
print("Value error: ")
print(e)
time.sleep(1.0)
if primitive_name == 'pull':
pose = util.pose_stamped2list(
yumi_gs.compute_fk(yumi_gs.get_jpos(arm='right')))
pos, ori = pose[:3], pose[3:]
# pose = yumi_gs.get_ee_pose()
# pos, ori = pose[0], pose[1]
# pos[2] -= 0.0714
pos[2] += 0.001
r_jnts = yumi_gs.compute_ik(pos, ori,
yumi_gs.get_jpos(arm='right'))
l_jnts = yumi_gs.get_jpos(arm='left')
if r_jnts is not None:
for _ in range(10):
pos[2] += 0.001
r_jnts = yumi_gs.compute_ik(
pos, ori, yumi_gs.get_jpos(arm='right'))
l_jnts = yumi_gs.get_jpos(arm='left')
if r_jnts is not None:
yumi_gs.update_joints(list(r_jnts) + l_jnts)
time.sleep(0.1)
time.sleep(0.1)
for _ in range(10):
yumi_gs.update_joints(cfg.RIGHT_INIT + cfg.LEFT_INIT)
def main(args):
print(args)
yumi = ar.create_robot('yumi',
robot_cfg={
'render': True,
'self_collision': False
})
yumi.go_home()
# while not yumi._reach_jnt_goal(yumi.cfgs.HOME_POSITION):
# yumi.p.stepSimulation()
# time.sleep(0.001)
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)
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
# plan_dict = plan[0]
# i = 1
# tip_poses = plan_dict['palm_poses_world'][i]
# r_joints, l_joints = get_joint_poses(tip_poses, yumi, cfg, nullspace=True)
# from IPython import embed
# embed()
with open('./data/' + args.primitive + '_object_poses_tip.pkl', 'rb') as f:
data = pickle.load(f)
# print("data: ")
# print(data)
object_poses_palm = data['object_pose_palm']
palm_poses_world = data['palm_pose_world']
object_poses_world = []
for i, pose in enumerate(object_poses_palm):
tmp_obj_pose = util.list2pose_stamped(pose)
palm_pose = util.list2pose_stamped(palm_poses_world[i])
tmp_obj_pose_world = util.convert_reference_frame(
tmp_obj_pose, palm_pose, util.unit_pose())
obj_pose_world = util.pose_stamped2list(tmp_obj_pose_world)
object_poses_world.append(obj_pose_world)
box_id = yumi.load_object(
args.config_package_path + 'descriptions/urdf/realsense_box.urdf',
cfg.OBJECT_INIT[0:3], cfg.OBJECT_INIT[3:])
for i, pose in enumerate(object_poses_world):
yumi.p.resetBasePositionAndOrientation(box_id, pose[:3], pose[3:])
time.sleep(0.01)
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, sim_step_repeat=args.step_repeat)
obj_id = yumi_ar.pb_client.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 = yumi_ar.pb_client.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,
obj_id,
yumi_ar.pb_client.get_client_id(),
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(
obj_id,
yumi_ar.pb_client.get_client_id())[0])
obj_ori_world = list(p.getBasePositionAndOrientation(
obj_id,
yumi_ar.pb_client.get_client_id())[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_running = SingleArmPrimitives(cfg, obj_id, mesh_file)
k = 0
while True:
# sample a random stable pose, and get the corresponding
# stable orientation index
k += 1
# init_id = exp_running.get_rand_init()[-1]
init_id = exp_running.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_running.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_running.get_palm_poses_world_frame(
point,
normal,
primitive_name=primitive_name)
obj_pos_world = list(p.getBasePositionAndOrientation(
obj_id,
yumi_ar.pb_client.get_client_id())[0])
obj_ori_world = list(p.getBasePositionAndOrientation(
obj_id,
yumi_ar.pb_client.get_client_id())[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_running.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_running.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()
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')
signal.signal(signal.SIGINT, signal_handler)
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
# data_seed = 1
data_seed = args.np_seed
np.random.seed(data_seed)
yumi_ar = Robot('yumi_palms',
pb=True,
pb_cfg={'gui': args.visualize},
arm_cfg={'self_collision': False,
'seed': data_seed})
r_gel_id = cfg.RIGHT_GEL_ID
l_gel_id = cfg.LEFT_GEL_ID
alpha = cfg.ALPHA
K = cfg.GEL_CONTACT_STIFFNESS
restitution = cfg.GEL_RESTITUION
p.changeDynamics(
yumi_ar.arm.robot_id,
r_gel_id,
restitution=restitution,
contactStiffness=K,
contactDamping=alpha*K,
rollingFriction=args.rolling
)
p.changeDynamics(
yumi_ar.arm.robot_id,
l_gel_id,
restitution=restitution,
contactStiffness=K,
contactDamping=alpha*K,
rollingFriction=args.rolling
)
dynamics_info = {}
dynamics_info['contactDamping'] = alpha*K
dynamics_info['contactStiffness'] = K
dynamics_info['rollingFriction'] = args.rolling
dynamics_info['restitution'] = restitution
yumi_gs = YumiCamsGS(
yumi_ar,
cfg,
exec_thread=False,
sim_step_repeat=args.step_repeat)
for _ in range(10):
yumi_gs.update_joints(cfg.RIGHT_INIT + cfg.LEFT_INIT)
if args.object:
obj_id = yumi_ar.pb_client.load_urdf(
args.config_package_path +
'descriptions/urdf/'+args.object_name+'.urdf',
cfg.OBJECT_POSE_3[0:3],
cfg.OBJECT_POSE_3[3:]
)
goal_obj_id = yumi_ar.pb_client.load_urdf(
args.config_package_path +
'descriptions/urdf/'+args.object_name+'_trans.urdf',
cfg.OBJECT_POSE_3[0:3],
cfg.OBJECT_POSE_3[3:]
)
p.setCollisionFilterPair(yumi_ar.arm.robot_id, goal_obj_id, r_gel_id, -1, enableCollision=False)
p.setCollisionFilterPair(obj_id, goal_obj_id, -1, -1, enableCollision=False)
p.setCollisionFilterPair(yumi_ar.arm.robot_id, obj_id, r_gel_id, -1, enableCollision=True)
p.setCollisionFilterPair(yumi_ar.arm.robot_id, obj_id, 27, -1, enableCollision=True)
yumi_ar.pb_client.reset_body(
obj_id,
cfg.OBJECT_POSE_3[:3],
cfg.OBJECT_POSE_3[3:])
yumi_ar.pb_client.reset_body(
goal_obj_id,
cfg.OBJECT_POSE_3[:3],
cfg.OBJECT_POSE_3[3:])
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
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,
yumi_ar.pb_client.get_client_id(),
obj_id,
mesh_file)
if primitive_name == 'grasp' or primitive_name == 'pivot':
exp_double = DualArmPrimitives(
cfg,
yumi_ar.pb_client.get_client_id(),
obj_id,
mesh_file)
exp_running = exp_double
else:
exp_running = exp_single
# setup macro_planner
action_planner = ClosedLoopMacroActions(
cfg,
yumi_gs,
obj_id,
yumi_ar.pb_client.get_client_id(),
args.config_package_path,
replan=args.replan,
object_mesh_file=mesh_file
)
data['metadata']['mesh_file'] = mesh_file
data['metadata']['cfg'] = cfg
data['metadata']['dynamics'] = dynamics_info
data['metadata']['cam_cfg'] = yumi_gs.cam_setup_cfg
data['metadata']['step_repeat'] = args.step_repeat
delta_z_height = 0.95
with open(args.config_package_path+'descriptions/urdf/'+args.object_name+'.urdf', 'rb') as f:
urdf_txt = f.read()
data['metadata']['object_urdf'] = urdf_txt
data['metadata']['delta_z_height'] = delta_z_height
data['metadata']['step_repeat'] = args.step_repeat
data['metadata']['seed'] = data_seed
metadata = data['metadata']
if args.multi:
cuboid_sampler = CuboidSampler(
'/root/catkin_ws/src/primitives/objects/cuboids/nominal_cuboid.stl',
pb_client=yumi_ar.pb_client)
cuboid_fname_template = '/root/catkin_ws/src/primitives/objects/cuboids/'
cuboid_manager = MultiBlockManager(
cuboid_fname_template,
cuboid_sampler,
robot_id=yumi_ar.arm.robot_id,
table_id=27,
r_gel_id=r_gel_id,
l_gel_id=l_gel_id)
yumi_ar.pb_client.remove_body(obj_id)
yumi_ar.pb_client.remove_body(goal_obj_id)
cuboid_fname = cuboid_manager.get_cuboid_fname()
obj_id, sphere_ids, mesh, goal_obj_id = \
cuboid_sampler.sample_cuboid_pybullet(
cuboid_fname,
goal=True,
keypoints=False)
cuboid_manager.filter_collisions(obj_id, goal_obj_id)
action_planner.update_object(obj_id, mesh_file)
trans_box_lock = threading.RLock()
goal_viz = GoalVisual(
trans_box_lock,
goal_obj_id,
action_planner.pb_client,
cfg.OBJECT_POSE_3)
pickle_path = os.path.join(
args.data_dir,
primitive_name,
args.experiment_name
)
if not os.path.exists(pickle_path):
os.makedirs(pickle_path)
data_manager = DataManager(pickle_path)
if args.save_data:
with open(os.path.join(pickle_path, 'metadata.pkl'), 'wb') as mdata_f:
pickle.dump(metadata, mdata_f)
if args.debug:
if args.multi:
cuboid_sampler.delete_cuboid(obj_id, goal_obj_id, sphere_ids)
cuboid_fname = cuboid_manager.get_cuboid_fname()
obj_id, sphere_ids, mesh, goal_obj_id = cuboid_sampler.sample_cuboid_pybullet(
cuboid_fname,
goal=True,
keypoints=False)
cuboid_manager.filter_collisions(obj_id, goal_obj_id)
goal_viz.update_goal_obj(goal_obj_id)
p.changeDynamics(
obj_id,
-1,
lateralFriction=0.4
)
action_planner.update_object(obj_id, mesh_file)
exp_running.initialize_object(obj_id, cuboid_fname)
print('Reset multi block!')
else:
cuboid_fname = '/root/catkin_ws/src/config/descriptions/meshes/objects/cuboids/realsense_box_experiments.stl'
for _ in range(args.num_obj_samples):
if primitive_name == 'pull':
init_id = exp_running.get_rand_init(ind=2)[-1]
obj_pose_final = util.list2pose_stamped(exp_running.init_poses[init_id])
point, normal, face = exp_running.sample_contact(primitive_name)
world_pose = exp_running.get_palm_poses_world_frame(
point,
normal,
primitive_name=primitive_name)
obj_pos_world = list(p.getBasePositionAndOrientation(
obj_id, yumi_ar.pb_client.get_client_id())[0])
obj_ori_world = list(p.getBasePositionAndOrientation(
obj_id, yumi_ar.pb_client.get_client_id())[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_running.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
elif primitive_name == 'grasp':
k = 0
have_contact = False
contact_face = None
while True:
x, y, dq, q, init_id = exp_running.get_rand_init()
obj_pose_world_nom = exp_running.get_obj_pose()[0]
palm_poses_world = exp_running.get_palm_poses_world_frame(
init_id,
obj_pose_world_nom,
[x, y, dq])
# get_palm_poses_world_frame may adjust the
# initial object pose, so need to check it again
obj_pose_world = exp_running.get_obj_pose()[0]
if palm_poses_world is not None:
have_contact = True
break
k += 1
if k >= 10:
print("FAILED")
break
if have_contact:
obj_pose_final = exp_running.goal_pose_world_frame_mod
palm_poses_obj_frame = {}
for key in palm_poses_world.keys():
palm_poses_obj_frame[key] = util.convert_reference_frame(
palm_poses_world[key], obj_pose_world, util.unit_pose())
example_args['palm_pose_r_object'] = palm_poses_obj_frame['right']
example_args['palm_pose_l_object'] = palm_poses_obj_frame['left']
example_args['object_pose1_world'] = obj_pose_world
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/cuboids/" + cuboid_fname.split('objects/cuboids')[1],
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/cuboids/" + cuboid_fname.split('objects/cuboids')[1],
name="/object_final",
color=(0., 0., 1., 1.),
frame_id="/yumi_body",
scale=(1., 1., 1.))
rospy.sleep(.1)
simulation.simulate(plan, cuboid_fname.split('objects/cuboids')[1])
else:
global_start = time.time()
face = 0
# exp_double.reset_graph(face)
start_time = time.time()
success = 0
for trial in range(args.num_trials):
k = 0
if args.multi:
cuboid_sampler.delete_cuboid(obj_id, goal_obj_id, sphere_ids)
cuboid_fname = cuboid_manager.get_cuboid_fname()
obj_id, sphere_ids, mesh, goal_obj_id = cuboid_sampler.sample_cuboid_pybullet(
cuboid_fname,
goal=True,
keypoints=False)
cuboid_manager.filter_collisions(obj_id, goal_obj_id)
goal_viz.update_goal_obj(goal_obj_id)
p.changeDynamics(
obj_id,
-1,
lateralFriction=0.4
)
action_planner.update_object(obj_id, mesh_file)
exp_running.initialize_object(obj_id, cuboid_fname)
print('Reset multi block!')
for _ in range(args.num_obj_samples):
while True:
have_contact = False
# sample a random stable pose, and get the corresponding
# stable orientation index
k += 1
if primitive_name == 'pull':
# init_id = exp_running.get_rand_init()[-1]
init_id = exp_running.get_rand_init()[-1]
# sample a point on the object that is valid
# for the primitive action being executed
point, normal, face = exp_running.sample_contact(
primitive_name=primitive_name)
if point is not None:
break
elif primitive_name == 'grasp':
x, y, dq, q, init_id = exp_double.get_rand_init()
obj_pose_world_nom = exp_double.get_obj_pose()[0]
palm_poses_world = exp_double.get_palm_poses_world_frame(
init_id,
obj_pose_world_nom,
[x, y, dq])
obj_pose_world = exp_double.get_obj_pose()[0]
if palm_poses_world is not None:
have_contact = True
break
if k >= 10:
print("FAILED")
return
# for _ in range(10):
# yumi_gs.update_joints(cfg.RIGHT_INIT + cfg.LEFT_INIT)
if primitive_name == 'pull':
# get the full 6D pose palm in world, at contact location
palm_pose_world = exp_running.get_palm_poses_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(
obj_id,
yumi_ar.pb_client.get_client_id())[0])
obj_ori_world = list(p.getBasePositionAndOrientation(
obj_id,
yumi_ar.pb_client.get_client_id())[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_running.init_poses[init_id])
x, y, q, _ = exp_running.get_rand_init(execute=False, ind=init_id)
final_nominal = exp_running.init_poses[init_id]
final_nominal[0] = x
final_nominal[1] = y
final_nominal[3:] = q
obj_pose_final = util.list2pose_stamped(final_nominal)
goal_viz.update_goal_state(final_nominal)
obj_pose_final.pose.position.z += cfg.TABLE_HEIGHT
example_args['object_pose2_world'] = obj_pose_final
example_args['table_face'] = init_id
example_args['primitive_name'] = primitive_name
example_args['N'] = exp_running.calc_n(
obj_pose_world, obj_pose_final)
elif primitive_name == 'grasp':
if have_contact:
obj_pose_final = exp_double.goal_pose_world_frame_mod
palm_poses_obj_frame = {}
for key in palm_poses_world.keys():
palm_poses_obj_frame[key] = util.convert_reference_frame(
palm_poses_world[key], obj_pose_world, util.unit_pose()
)
example_args['palm_pose_r_object'] = palm_poses_obj_frame['right']
example_args['palm_pose_l_object'] = palm_poses_obj_frame['left']
example_args['object_pose1_world'] = obj_pose_world
example_args['object_pose2_world'] = obj_pose_final
example_args['table_face'] = init_id
else:
continue
try:
obs, pcd = yumi_gs.get_observation(obj_id=obj_id)
start = util.pose_stamped2list(obj_pose_world)
goal = util.pose_stamped2list(obj_pose_final)
keypoints_start = np.array(exp_running.mesh_world.vertices.tolist())
keypoints_start_homog = np.hstack(
(keypoints_start, np.ones((keypoints_start.shape[0], 1)))
)
start_mat = util.matrix_from_pose(obj_pose_world)
goal_mat = util.matrix_from_pose(obj_pose_final)
T_mat = np.matmul(goal_mat, np.linalg.inv(start_mat))
keypoints_goal = np.matmul(T_mat, keypoints_start_homog.T).T[:, :3]
contact_obj_frame_dict = {}
contact_world_frame_dict = {}
nearest_pt_world_dict = {}
if primitive_name == 'pull':
active_arm, inactive_arm = action_planner.get_active_arm(
util.pose_stamped2list(obj_pose_world)
)
# get contact (palm pose object frame)
contact_obj_frame_dict[active_arm] = util.pose_stamped2list(contact_obj_frame)
contact_world_frame_dict[active_arm] = util.pose_stamped2list(palm_pose_world)
contact_pos = open3d.utility.DoubleVector(np.array(contact_world_frame_dict[active_arm][:3]))
kdtree = open3d.geometry.KDTreeFlann(pcd)
# nearest_pt_ind = kdtree.search_knn_vector_3d(contact_pos, 1)[1][0]
# nearest_pt_world_dict[active_arm] = np.asarray(pcd.points)[nearest_pt_ind]
contact_obj_frame_dict[inactive_arm] = None
contact_world_frame_dict[inactive_arm] = None
nearest_pt_world_dict[inactive_arm] = None
elif primitive_name == 'grasp':
for key in palm_poses_obj_frame.keys():
contact_obj_frame_dict[key] = util.pose_stamped2list(palm_poses_obj_frame[key])
contact_world_frame_dict[key] = util.pose_stamped2list(palm_poses_world[key])
contact_pos = open3d.utility.DoubleVector(np.array(contact_world_frame_dict[key][:3]))
kdtree = open3d.geometry.KDTreeFlann(pcd)
# nearest_pt_ind = kdtree.search_knn_vector_3d(contact_pos, 1)[1][0]
# nearest_pt_world_dict[key] = np.asarray(pcd.points)[nearest_pt_ind]
result = action_planner.execute(primitive_name, example_args)
if result is not None:
print('Trial number: ' + str(trial) + ', reached final: ' + str(result[0]))
print('Time so far: ' + str(time.time() - start_time))
if result[0]:
success += 1
sample = {}
sample['obs'] = obs
sample['start'] = start
sample['goal'] = goal
sample['keypoints_start'] = keypoints_start
sample['keypoints_goal'] = keypoints_goal
sample['transformation'] = util.pose_from_matrix(T_mat)
sample['contact_obj_frame'] = contact_obj_frame_dict
sample['contact_world_frame'] = contact_world_frame_dict
# sample['contact_pcd'] = nearest_pt_world_dict
sample['result'] = result
if primitive_name == 'grasp':
sample['goal_face'] = exp_double.goal_face
if args.save_data:
data_manager.save_observation(sample, str(trial))
print("Success: " + str(success))
else:
continue
# 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)
except ValueError as e:
print("Value error: ")
print(e)
# time.sleep(1.0)
# pose = util.pose_stamped2list(yumi_gs.compute_fk(yumi_gs.get_jpos(arm='right')))
# pos, ori = pose[:3], pose[3:]
# # pose = yumi_gs.get_ee_pose()
# # pos, ori = pose[0], pose[1]
# # pos[2] -= 0.0714
# pos[2] += 0.001
# r_jnts = yumi_gs.compute_ik(pos, ori, yumi_gs.get_jpos(arm='right'))
# l_jnts = yumi_gs.get_jpos(arm='left')
# if r_jnts is not None:
# for _ in range(10):
# pos[2] += 0.001
# r_jnts = yumi_gs.compute_ik(pos, ori, yumi_gs.get_jpos(arm='right'))
# l_jnts = yumi_gs.get_jpos(arm='left')
# if r_jnts is not None:
# yumi_gs.update_joints(list(r_jnts) + l_jnts)
# time.sleep(0.1)
time.sleep(0.1)
for _ in range(10):
yumi_gs.update_joints(cfg.RIGHT_INIT + cfg.LEFT_INIT)
# for _ in range(10):
# 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]
# )
# yumi_gs.update_joints(cfg.RIGHT_INIT + cfg.LEFT_INIT)
# 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)
print("TOTAL TIME: " + str(time.time() - global_start))
def main(args):
cfg_file = osp.join(args.example_config_path, args.primitive) + ".yaml"
cfg = get_cfg_defaults()
cfg.merge_from_file(cfg_file)
cfg.freeze()
rospy.init_node('EvalMultiStep')
signal.signal(signal.SIGINT, signal_handler)
data_seed = args.np_seed
primitive_name = args.primitive
pickle_path = osp.join(args.data_dir, primitive_name, args.experiment_name)
if args.save_data:
suf_i = 0
original_pickle_path = pickle_path
# while True:
# if osp.exists(pickle_path):
# suffix = '_%d' % suf_i
# pickle_path = original_pickle_path + suffix
# suf_i += 1
# data_seed += 1
# else:
# os.makedirs(pickle_path)
# break
if not osp.exists(pickle_path):
os.makedirs(pickle_path)
np.random.seed(data_seed)
yumi_ar = Robot('yumi_palms',
pb=True,
pb_cfg={
'gui': args.visualize,
'opengl_render': False
},
arm_cfg={
'self_collision': False,
'seed': data_seed
})
r_gel_id = cfg.RIGHT_GEL_ID
l_gel_id = cfg.LEFT_GEL_ID
table_id = cfg.TABLE_ID
alpha = cfg.ALPHA
K = cfg.GEL_CONTACT_STIFFNESS
restitution = cfg.GEL_RESTITUION
p.changeDynamics(yumi_ar.arm.robot_id,
r_gel_id,
restitution=restitution,
contactStiffness=K,
contactDamping=alpha * K,
rollingFriction=args.rolling)
p.changeDynamics(yumi_ar.arm.robot_id,
l_gel_id,
restitution=restitution,
contactStiffness=K,
contactDamping=alpha * K,
rollingFriction=args.rolling)
yumi_gs = YumiCamsGS(yumi_ar,
cfg,
exec_thread=False,
sim_step_repeat=args.sim_step_repeat)
for _ in range(10):
yumi_gs.update_joints(cfg.RIGHT_INIT + cfg.LEFT_INIT)
cuboid_sampler = CuboidSampler(osp.join(
os.environ['CODE_BASE'],
'catkin_ws/src/config/descriptions/meshes/objects/cuboids/nominal_cuboid.stl'
),
pb_client=yumi_ar.pb_client)
cuboid_fname_template = osp.join(
os.environ['CODE_BASE'],
'catkin_ws/src/config/descriptions/meshes/objects/cuboids/')
cuboid_manager = MultiBlockManager(cuboid_fname_template,
cuboid_sampler,
robot_id=yumi_ar.arm.robot_id,
table_id=table_id,
r_gel_id=r_gel_id,
l_gel_id=l_gel_id)
if args.multi:
cuboid_fname = cuboid_manager.get_cuboid_fname()
# cuboid_fname = '/root/catkin_ws/src/config/descriptions/meshes/objects/cuboids/test_cuboid_smaller_4479.stl'
else:
cuboid_fname = args.config_package_path + 'descriptions/meshes/objects/' + \
args.object_name + '_experiments.stl'
mesh_file = cuboid_fname
goal_visualization = False
if args.goal_viz:
goal_visualization = True
obj_id, sphere_ids, mesh, goal_obj_id = \
cuboid_sampler.sample_cuboid_pybullet(
cuboid_fname,
goal=goal_visualization,
keypoints=False)
cuboid_manager.filter_collisions(obj_id, goal_obj_id)
p.changeDynamics(obj_id, -1, lateralFriction=1.0)
# goal_face = 0
goal_faces = [0, 1, 2, 3, 4, 5]
from random import shuffle
shuffle(goal_faces)
goal_face = goal_faces[0]
exp_single = SingleArmPrimitives(cfg, yumi_ar.pb_client.get_client_id(),
obj_id, cuboid_fname)
k = 0
while True:
k += 1
if k > 10:
print('FAILED TO BUILD GRASPING GRAPH')
return
try:
exp_double = DualArmPrimitives(cfg,
yumi_ar.pb_client.get_client_id(),
obj_id,
cuboid_fname,
goal_face=goal_face)
break
except ValueError as e:
print(e)
yumi_ar.pb_client.remove_body(obj_id)
if goal_visualization:
yumi_ar.pb_client.remove_body(goal_obj_id)
cuboid_fname = cuboid_manager.get_cuboid_fname()
print("Cuboid file: " + cuboid_fname)
obj_id, sphere_ids, mesh, goal_obj_id = \
cuboid_sampler.sample_cuboid_pybullet(
cuboid_fname,
goal=goal_visualization,
keypoints=False)
cuboid_manager.filter_collisions(obj_id, goal_obj_id)
p.changeDynamics(obj_id, -1, lateralFriction=1.0)
if primitive_name == 'grasp':
exp_running = exp_double
else:
exp_running = exp_single
action_planner = ClosedLoopMacroActions(cfg,
yumi_gs,
obj_id,
yumi_ar.pb_client.get_client_id(),
args.config_package_path,
replan=args.replan,
object_mesh_file=mesh_file)
if goal_visualization:
trans_box_lock = threading.RLock()
goal_viz = GoalVisual(trans_box_lock,
goal_obj_id,
action_planner.pb_client,
cfg.OBJECT_POSE_3,
show_init=False)
action_planner.update_object(obj_id, mesh_file)
exp_single.initialize_object(obj_id, cuboid_fname)
dynamics_info = {}
dynamics_info['contactDamping'] = alpha * K
dynamics_info['contactStiffness'] = K
dynamics_info['rollingFriction'] = args.rolling
dynamics_info['restitution'] = restitution
data = {}
data['saved_data'] = []
data['metadata'] = {}
data['metadata']['mesh_file'] = mesh_file
data['metadata']['cfg'] = cfg
data['metadata']['dynamics'] = dynamics_info
data['metadata']['cam_cfg'] = yumi_gs.cam_setup_cfg
data['metadata']['step_repeat'] = args.sim_step_repeat
data['metadata']['seed'] = data_seed
data['metadata']['seed_original'] = args.np_seed
metadata = data['metadata']
data_manager = DataManager(pickle_path)
pred_dir = osp.join(os.environ['CODE_BASE'], cfg.PREDICTION_DIR)
obs_dir = osp.join(os.environ['CODE_BASE'], cfg.OBSERVATION_DIR)
if args.save_data:
with open(osp.join(pickle_path, 'metadata.pkl'), 'wb') as mdata_f:
pickle.dump(metadata, mdata_f)
total_trials = 0
successes = 0
# prep visualization tools
palm_mesh_file = osp.join(os.environ['CODE_BASE'], cfg.PALM_MESH_FILE)
table_mesh_file = osp.join(os.environ['CODE_BASE'], cfg.TABLE_MESH_FILE)
viz_palms = PalmVis(palm_mesh_file, table_mesh_file, cfg)
viz_pcd = PCDVis()
if args.skeleton == 'pg':
skeleton = ['pull', 'grasp']
elif args.skeleton == 'gp':
skeleton = ['grasp', 'pull']
elif args.skeleton == 'pgp':
skeleton = ['pull', 'grasp', 'pull']
else:
raise ValueError('Unrecognized plan skeleton!')
pull_sampler = PullSamplerBasic()
grasp_sampler = GraspSamplerBasic(None)
# pull_sampler = PullSamplerVAEPubSub(
# obs_dir=obs_dir,
# pred_dir=pred_dir
# )
# grasp_sampler = GraspSamplerVAEPubSub(
# default_target=None,
# obs_dir=obs_dir,
# pred_dir=pred_dir
# )
pull_skill = PullRightSkill(pull_sampler, yumi_gs, pulling_planning_wf)
pull_skill_no_mp = PullRightSkill(pull_sampler,
yumi_gs,
pulling_planning_wf,
ignore_mp=True)
grasp_skill = GraspSkill(grasp_sampler, yumi_gs, grasp_planning_wf)
skills = {}
# skills['pull'] = pull_skill_no_mp
skills['pull'] = pull_skill
skills['grasp'] = grasp_skill
problems_file = '/root/catkin_ws/src/primitives/data/planning/test_problems_0/demo_0.pkl'
with open(problems_file, 'rb') as f:
problems_data = pickle.load(f)
prob_inds = np.arange(len(problems_data), dtype=np.int64).tolist()
data_inds = np.arange(len(problems_data[0]['problems']),
dtype=np.int64).tolist()
experiment_manager = GraspEvalManager(yumi_gs,
yumi_ar.pb_client.get_client_id(),
pickle_path, args.exp_name, None,
None, None, None, cfg)
# experiment_manager.set_object_id(
# obj_id,
# cuboid_fname
# )
total_trial_number = 0
for _ in range(len(problems_data)):
# prob_ind = 3
# obj_fname = str(osp.join(
# '/root/catkin_ws/src/config/descriptions/meshes/objects/cuboids',
# problems_data[prob_ind]['object_name']))
# print(obj_fname)
# for j, problem_data in enumerate(problems_data[prob_ind]['problems']):
for _ in range(len(problems_data[0]['problems'])):
total_trial_number += 1
# prob_ind = 8
# data_ind = 15
prob_ind = prob_inds[np.random.randint(len(prob_inds))]
data_ind = data_inds[np.random.randint(len(data_inds))]
problem_data = problems_data[prob_ind]['problems'][data_ind]
obj_fname = str(
osp.join(
'/root/catkin_ws/src/config/descriptions/meshes/objects/cuboids',
problems_data[prob_ind]['object_name']))
obj_name = problems_data[prob_ind]['object_name'].split('.stl')[0]
print(obj_fname)
start_pose = problem_data['start_vis'].tolist()
# put object into work at start_pose, with known obj_fname
yumi_ar.pb_client.remove_body(obj_id)
if goal_visualization:
yumi_ar.pb_client.remove_body(goal_obj_id)
obj_id, sphere_ids, mesh, goal_obj_id = \
cuboid_sampler.sample_cuboid_pybullet(
obj_fname,
goal=goal_visualization,
keypoints=False)
if goal_visualization:
goal_viz.update_goal_obj(goal_obj_id)
goal_viz.hide_goal_obj()
cuboid_manager.filter_collisions(obj_id, goal_obj_id)
exp_single.initialize_object(obj_id, obj_fname)
experiment_manager.set_object_id(obj_id, obj_fname)
p.resetBasePositionAndOrientation(obj_id, start_pose[:3],
start_pose[3:])
p.changeDynamics(obj_id, -1, lateralFriction=1.0)
yumi_ar.arm.set_jpos(cfg.RIGHT_OUT_OF_FRAME +
cfg.LEFT_OUT_OF_FRAME,
ignore_physics=True)
time.sleep(0.2)
real_start_pos = p.getBasePositionAndOrientation(obj_id)[0]
real_start_ori = p.getBasePositionAndOrientation(obj_id)[1]
real_start_pose = list(real_start_pos) + list(real_start_ori)
transformation_des = util.matrix_from_pose(
util.list2pose_stamped(
problem_data['transformation'].tolist()))
# #### BEGIN BLOCK FOR GETTING INTRO FIGURE
# R_3 = common.euler2rot([0.0, 0.0, np.pi/4])
# t_3 = np.array([0.03, 0.25, 0.0])
# T_3 = np.eye(4)
# T_3[:-1, :-1] = R_3
# # T_3[:-1, -1] = t_3
# print(T_3)
# trans_des = np.matmul(T_3, transformation_des)
# goal_pose = util.pose_stamped2list(util.transform_pose(
# util.list2pose_stamped(real_start_pose),
# util.pose_from_matrix(trans_des)
# ))
# if goal_visualization:
# goal_viz.update_goal_state(goal_pose)
# goal_viz.show_goal_obj()
# embed()
# #### END BLOCK FOR GETTING INTRO FIGURE
goal_pose = util.pose_stamped2list(
util.transform_pose(
util.list2pose_stamped(real_start_pose),
util.list2pose_stamped(problem_data['transformation'])))
# if skeleton is 'pull' 'grasp' 'pull', add an additional SE(2) transformation to the task
if args.skeleton == 'pgp':
while True:
x, y, dq = exp_single.get_rand_trans_yaw()
goal_pose_2_list = copy.deepcopy(goal_pose)
goal_pose_2_list[0] = x
goal_pose_2_list[1] = y
goal_pose_2_list[3:] = common.quat_multiply(
dq, np.asarray(goal_pose[3:]))
if goal_pose_2_list[0] > 0.2 and goal_pose_2_list[0] < 0.4 and \
goal_pose_2_list[1] > -0.3 and goal_pose_2_list[1] < 0.1:
rot = common.quat2rot(dq)
T_2 = np.eye(4)
T_2[:-1, :-1] = rot
T_2[:2, -1] = [x - goal_pose[0], y - goal_pose[1]]
break
goal_pose = goal_pose_2_list
transformation_des = np.matmul(T_2, transformation_des)
# # if skeleton is 'grasp' first, invert the desired trans and flip everything
if args.skeleton == 'gp':
transformation_des = np.linalg.inv(transformation_des)
start_tmp = copy.deepcopy(start_pose)
start_pose = goal_pose
goal_pose = start_tmp
p.resetBasePositionAndOrientation(obj_id, start_pose[:3],
start_pose[3:])
real_start_pos = p.getBasePositionAndOrientation(obj_id)[0]
real_start_ori = p.getBasePositionAndOrientation(obj_id)[1]
real_start_pose = list(real_start_pos) + list(real_start_ori)
time.sleep(0.5)
# get observation
obs, pcd = yumi_gs.get_observation(
obj_id=obj_id, robot_table_id=(yumi_ar.arm.robot_id, table_id))
pointcloud_pts = np.asarray(obs['down_pcd_pts'][:100, :],
dtype=np.float32)
pointcloud_pts_full = np.asarray(np.concatenate(obs['pcd_pts']),
dtype=np.float32)
grasp_sampler.update_default_target(
np.concatenate(obs['table_pcd_pts'], axis=0)[::500, :])
trial_data = {}
trial_data['start_pcd'] = pointcloud_pts_full
trial_data['start_pcd_down'] = pointcloud_pts
trial_data['obj_fname'] = cuboid_fname
trial_data['start_pose'] = np.asarray(real_start_pose)
trial_data['goal_pose'] = np.asarray(goal_pose)
trial_data['goal_pose_global'] = np.asarray(goal_pose)
trial_data['trans_des_global'] = transformation_des
trial_data['skeleton'] = args.skeleton
# plan!
planner = PointCloudTree(pointcloud_pts,
transformation_des,
skeleton,
skills,
start_pcd_full=pointcloud_pts_full)
start_plan_time = time.time()
plan_total = planner.plan()
trial_data['planning_time'] = time.time() - start_plan_time
if plan_total is None:
print('Could not find plan')
experiment_manager.set_mp_success(False, 1)
obj_data = experiment_manager.get_object_data()
# obj_data_fname = osp.join(
# pickle_path,
# obj_name + '_' + str(total_trial_number),
# obj_name + '_' + str(total_trial_number) + '_ms_eval_data.pkl')
obj_data_fname = osp.join(
pickle_path, obj_name + '_' + str(total_trial_number) +
'_ms_eval_data.pkl')
if args.save_data:
print('Saving to: ' + str(obj_data_fname))
with open(obj_data_fname, 'wb') as f:
pickle.dump(obj_data, f)
continue
plan = copy.deepcopy(plan_total[1:])
if args.trimesh_viz:
# from multistep_planning_eval_cfg import get_cfg_defaults
# import os.path as osp
# from eval_utils.visualization_tools import PCDVis, PalmVis
# cfg = get_cfg_defaults()
# palm_mesh_file = osp.join(os.environ['CODE_BASE'],
# cfg.PALM_MESH_FILE)
# table_mesh_file = osp.join(os.environ['CODE_BASE'],
# cfg.TABLE_MESH_FILE)
# viz_palms = PalmVis(palm_mesh_file, table_mesh_file, cfg)
# viz_pcd = PCDVis()
ind = 0
pcd_data = copy.deepcopy(problem_data)
pcd_data['start'] = plan_total[ind].pointcloud_full
pcd_data['object_pointcloud'] = plan_total[ind].pointcloud_full
pcd_data['transformation'] = np.asarray(
util.pose_stamped2list(
util.pose_from_matrix(plan_total[ind +
1].transformation)))
pcd_data['contact_world_frame_right'] = np.asarray(
plan_total[ind + 1].palms[:7])
if skeleton[ind] == 'grasp':
pcd_data['contact_world_frame_left'] = np.asarray(
plan_total[ind + 1].palms[:7])
elif skeleton[ind] == 'pull':
pcd_data['contact_world_frame_left'] = np.asarray(
plan_total[ind + 1].palms[7:])
scene = viz_palms.vis_palms_pcd(pcd_data,
world=True,
centered=False,
corr=False)
scene.show()
# embed()
# execute plan if one is found...
pose_plan = [(real_start_pose,
util.list2pose_stamped(real_start_pose))]
for i in range(1, len(plan) + 1):
pose = util.transform_pose(
pose_plan[i - 1][1],
util.pose_from_matrix(plan[i - 1].transformation))
pose_list = util.pose_stamped2list(pose)
pose_plan.append((pose_list, pose))
# get palm poses from plan
palm_pose_plan = []
for i, node in enumerate(plan):
palms = copy.deepcopy(node.palms)
# palms = copy.deepcopy(node.palms_raw) if node.palms_raw is not None else copy.deepcopy(node.palms)
if skeleton[i] == 'pull':
palms[2] -= 0.002
palm_pose_plan.append(palms)
# observe results
full_plan = []
for i in range(len(plan)):
if skeleton[i] == 'pull':
local_plan = pulling_planning_wf(
util.list2pose_stamped(palm_pose_plan[i]),
util.list2pose_stamped(palm_pose_plan[i]),
util.pose_from_matrix(plan[i].transformation))
elif skeleton[i] == 'grasp':
local_plan = grasp_planning_wf(
util.list2pose_stamped(palm_pose_plan[i][7:]),
util.list2pose_stamped(palm_pose_plan[i][:7]),
util.pose_from_matrix(plan[i].transformation))
full_plan.append(local_plan)
grasp_success = True
action_planner.active_arm = 'right'
action_planner.inactive_arm = 'left'
if goal_visualization:
goal_viz.update_goal_state(goal_pose)
goal_viz.show_goal_obj()
if goal_visualization:
goal_viz.update_goal_state(goal_pose)
goal_viz.show_goal_obj()
real_start_pos = p.getBasePositionAndOrientation(obj_id)[0]
real_start_ori = p.getBasePositionAndOrientation(obj_id)[1]
real_start_pose = list(real_start_pos) + list(real_start_ori)
real_start_mat = util.matrix_from_pose(
util.list2pose_stamped(real_start_pose))
# embed()
try:
start_playback_time = time.time()
for playback in range(args.playback_num):
if playback > 0 and goal_visualization:
goal_viz.hide_goal_obj()
yumi_ar.pb_client.reset_body(obj_id, pose_plan[0][0][:3],
pose_plan[0][0][3:])
p.changeDynamics(obj_id, -1, lateralFriction=1.0)
for i, skill in enumerate(skeleton):
if skill == 'pull':
# set arm configuration to good start state
yumi_ar.arm.set_jpos(cfg.RIGHT_INIT +
cfg.LEFT_INIT,
ignore_physics=True)
time.sleep(0.5)
# move to making contact, and ensure contact is made
# try:
# _, _ = action_planner.single_arm_setup(full_plan[i][0], pre=True)
# except ValueError as e:
# print(e)
# break
_, _ = action_planner.single_arm_setup(
full_plan[i][0], pre=True)
start_playback_time = time.time()
if not experiment_manager.still_pulling():
while True:
if experiment_manager.still_pulling(
) or time.time(
) - start_playback_time > 20.0:
break
action_planner.single_arm_approach()
time.sleep(0.075)
new_plan = pulling_planning_wf(
yumi_gs.get_current_tip_poses()
['left'],
yumi_gs.get_current_tip_poses()
['right'],
util.pose_from_matrix(
plan[i].transformation))
pull_plan = new_plan[0]
else:
pull_plan = full_plan[i][0]
# try:
# action_planner.playback_single_arm('pull', pull_plan, pre=False)
# except ValueError as e:
# print(e)
# break
action_planner.playback_single_arm('pull',
pull_plan,
pre=False)
grasp_success = grasp_success and experiment_manager.still_pulling(
n=False)
print('grasp success: ' + str(grasp_success))
time.sleep(0.5)
action_planner.single_arm_retract()
elif skill == 'grasp':
yumi_ar.arm.set_jpos([
0.9936, -2.1848, -0.9915, 0.8458, 3.7618,
1.5486, 0.1127, -1.0777, -2.1187, 0.995, 1.002,
-3.6834, 1.8132, 2.6405
],
ignore_physics=True)
time.sleep(0.5)
# try:
# _, _ = action_planner.dual_arm_setup(full_plan[i][0], 0, pre=True)
# except ValueError as e:
# print(e)
# break
_, _ = action_planner.dual_arm_setup(
full_plan[i][0], 0, pre=True)
start_playback_time = time.time()
if not experiment_manager.still_grasping():
while True:
if experiment_manager.still_grasping(
) or time.time(
) - start_playback_time > 20.0:
break
action_planner.dual_arm_approach()
time.sleep(0.075)
new_plan = grasp_planning_wf(
yumi_gs.get_current_tip_poses()
['left'],
yumi_gs.get_current_tip_poses()
['right'],
util.pose_from_matrix(
plan[i].transformation))
grasp_plan = new_plan
else:
grasp_plan = full_plan[i]
for k, subplan in enumerate(grasp_plan):
# try:
# action_planner.playback_dual_arm('grasp', subplan, k, pre=False)
# except ValueError as e:
# print(e)
# break
action_planner.playback_dual_arm('grasp',
subplan,
k,
pre=False)
if k == 1:
grasp_success = grasp_success and experiment_manager.still_grasping(
n=False)
print('grasp success: ' +
str(grasp_success))
time.sleep(1.0)
except ValueError as e:
print(e)
experiment_manager.set_mp_success(True, 1)
experiment_manager.set_execute_success(False)
obj_data = experiment_manager.get_object_data()
# obj_data_fname = osp.join(
# pickle_path,
# obj_name + '_' + str(total_trial_number),
# obj_name + '_' + str(total_trial_number) + '_ms_eval_data.pkl')
obj_data_fname = osp.join(
pickle_path, obj_name + '_' + str(total_trial_number) +
'_ms_eval_data.pkl')
if args.save_data:
print('Saving to: ' + str(obj_data_fname))
with open(obj_data_fname, 'wb') as f:
pickle.dump(obj_data, f)
continue
real_final_pos = p.getBasePositionAndOrientation(obj_id)[0]
real_final_ori = p.getBasePositionAndOrientation(obj_id)[1]
real_final_pose = list(real_final_pos) + list(real_final_ori)
real_final_mat = util.matrix_from_pose(
util.list2pose_stamped(real_final_pose))
real_T_mat = np.matmul(real_final_mat,
np.linalg.inv(real_start_mat))
real_T_pose = util.pose_stamped2np(
util.pose_from_matrix(real_T_mat))
trial_data['trans_executed'] = real_T_mat
trial_data['final_pose'] = real_final_pose
experiment_manager.set_mp_success(True, 1)
experiment_manager.set_execute_success(True)
experiment_manager.end_trial(trial_data, grasp_success)
time.sleep(3.0)
obj_data = experiment_manager.get_object_data()
kvs = {}
kvs['grasp_success'] = obj_data['grasp_success']
kvs['pos_err'] = np.mean(obj_data['final_pos_error'])
kvs['ori_err'] = np.mean(obj_data['final_ori_error'])
kvs['planning_time'] = obj_data['planning_time']
string = ''
for k, v in kvs.items():
string += "%s: %.3f, " % (k, v)
print(string)
# obj_data_fname = osp.join(
# pickle_path,
# obj_name + '_' + str(total_trial_number),
# obj_name + '_' + str(total_trial_number) + '_ms_eval_data.pkl')
obj_data_fname = osp.join(
pickle_path,
obj_name + '_' + str(total_trial_number) + '_ms_eval_data.pkl')
if args.save_data:
print('Saving to: ' + str(obj_data_fname))
with open(obj_data_fname, 'wb') as f:
pickle.dump(obj_data, f)