def get_palm_z_normals(self, palm_poses=None, *args):
"""
Gets the updated world frame normal direction of the palms
"""
normal_z = util.list2pose_stamped([0, 0, 1, 0, 0, 0, 1])
normal_z_poses_world = {}
wrist_poses = {}
for arm in ['right', 'left']:
if palm_poses is None:
wrist_pos_world = self.get_ee_pose(arm=arm)[0].tolist()
wrist_ori_world = self.get_ee_pose(arm=arm)[1].tolist()
wrist_poses[arm] = util.list2pose_stamped(wrist_pos_world +
wrist_ori_world)
else:
wrist_poses[arm] = palm_poses[arm]
tip_poses = self.wrist_to_tip(wrist_poses)
normal_z_poses_world['right'] = util.transform_pose(
normal_z, tip_poses['right'])
normal_z_poses_world['left'] = util.transform_pose(
normal_z, tip_poses['left'])
return normal_z_poses_world
def main(args):
#repo_dir = '/home/anthony/repos/research/mpalm_affordances/catkin_ws/src/primitives/'
repo_dir = '/root/catkin_ws/src/primitives/'
i = 0
# grasp_data_dir = os.path.join(repo_dir, 'data/grasp/face_ind_test_0_fixed')
full_data_dir = os.path.join(repo_dir, args.grasp_dir)
#full_data_dir = os.path.join(repo_dir, args.pull_dir)
for filename in os.listdir(full_data_dir):
#if filename.endswith('.pkl') and filename != 'metadata.pkl':
if filename != 'metadata.pkl':
#print(filename)
#continue
with open(
os.path.join(full_data_dir, filename, 'pkl',
filename + '.pkl'), 'rb') as f:
try:
data = pickle.load(f)
except EOFError:
continue
print("i: " + str(i))
start_mat = util.matrix_from_pose(
util.list2pose_stamped(data['start']))
goal_mat = util.matrix_from_pose(
util.list2pose_stamped(data['goal']))
T_mat = np.matmul(goal_mat, np.linalg.inv(start_mat))
keypoints_world_homog = np.hstack(
(data['keypoints_start'],
np.ones((data['keypoints_start'].shape[0], 1))))
data['keypoints_start'] = np.matmul(
start_mat, keypoints_world_homog.T).T[:, :3]
data['keypoints_goal_corrected'] = np.matmul(
goal_mat, keypoints_world_homog.T).T[:, :3]
#data['keypoints_goal_corrected'] = np.matmul(
# T_mat, keypoints_start_homog.T).T[:, :3]
#data['transformation_corrected'] = util.pose_stamped2list(util.pose_from_matrix(T_mat))
with open(
os.path.join(full_data_dir, filename, 'pkl',
filename + '.pkl'), 'wb') as new_f:
pickle.dump(data, new_f)
i += 1
def wrist_to_tip(self, wrist_poses):
"""
Transform a pose from the Yumi wrist joint to the
Gelslim tip to the
Args:
wrist_poses (dict): Dictionary of PoseStamped values
corresponding to each arm, keyed by 'right' and
'left'
Returns:
dict: Keyed by 'right' and 'left', values are PoseStamped
"""
wrist_to_tip = util.list2pose_stamped(self.cfg.WRIST_TO_TIP_TF, '')
tip_left = util.convert_reference_frame(wrist_to_tip, util.unit_pose(),
wrist_poses['left'],
"yumi_body")
tip_right = util.convert_reference_frame(wrist_to_tip,
util.unit_pose(),
wrist_poses['right'],
"yumi_body")
tip_poses = {}
tip_poses['right'] = tip_right
tip_poses['left'] = tip_left
return tip_poses
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 tip_to_wrist(self, tip_poses):
"""
Transform a pose from the Yumi Gelslim tip to the
wrist joint
Args:
tip_poses (dict): Dictionary of PoseStamped values
corresponding to each arm, keyed by 'right' and
'left'
Returns:
dict: Keyed by 'right' and 'left', values are PoseStamped
"""
tip_to_wrist = util.list2pose_stamped(self.cfg.TIP_TO_WRIST_TF, '')
world_to_world = util.unit_pose()
wrist_left = util.convert_reference_frame(tip_to_wrist, world_to_world,
tip_poses['left'],
"yumi_body")
wrist_right = util.convert_reference_frame(tip_to_wrist,
world_to_world,
tip_poses['right'],
"yumi_body")
wrist_poses = {}
wrist_poses['right'] = wrist_right
wrist_poses['left'] = wrist_left
return wrist_poses
def get_current_tip_poses(self):
wrist_poses = {}
for arm in ['right', 'left']:
wrist_pos_world = self.get_ee_pose(arm=arm)[0].tolist()
wrist_ori_world = self.get_ee_pose(arm=arm)[1].tolist()
wrist_poses[arm] = util.list2pose_stamped(wrist_pos_world +
wrist_ori_world)
tip_poses = self.wrist_to_tip(wrist_poses)
return tip_poses
def get_wrist_to_tip(wrist_poses, cfg):
"""
[summary]
Args:
tip_poses ([type]): [description]
cfg ([type]): [description]
Returns:
[type]: [description]
"""
wrist_to_tip = util.list2pose_stamped(cfg.WRIST_TO_TIP_TF, '')
tip_left = util.convert_reference_frame(wrist_to_tip, util.unit_pose(),
wrist_poses['left'], "yumi_body")
tip_right = util.convert_reference_frame(wrist_to_tip, util.unit_pose(),
wrist_poses['right'], "yumi_body")
tip_poses = {}
tip_poses['right'] = tip_right
tip_poses['left'] = tip_left
return tip_poses
def get_tip_to_wrist(tip_poses, cfg):
"""
[summary]
Args:
tip_poses ([type]): [description]
cfg ([type]): [description]
Returns:
[type]: [description]
"""
tip_to_wrist = util.list2pose_stamped(cfg.TIP_TO_WRIST_TF, '')
world_to_world = util.unit_pose()
wrist_left = util.convert_reference_frame(tip_to_wrist, world_to_world,
tip_poses['left'], "yumi_body")
wrist_right = util.convert_reference_frame(tip_to_wrist, world_to_world,
tip_poses['right'], "yumi_body")
wrist_poses = {}
wrist_poses['right'] = wrist_right
wrist_poses['left'] = wrist_left
return wrist_poses
def update_yumi_cart(poses):
wrist_to_tip = util.list2pose_stamped(
[0.0, 0.071399, -0.14344421, 0.0, 0.0, 0.0, 1.0], '')
world_to_world = util.unit_pose()
wrist_left = util.convert_reference_frame(wrist_to_tip, world_to_world,
poses[0], "yumi_body")
wrist_right = util.convert_reference_frame(wrist_to_tip, world_to_world,
poses[1], "yumi_body")
visualize_object(
wrist_left,
filepath=
"package://config/descriptions/meshes/mpalm/mpalms_all_coarse.stl",
name="/gripper_left",
color=(0., 1., 0., 1.),
frame_id="/yumi_body")
visualize_object(
wrist_right,
filepath=
"package://config/descriptions/meshes/mpalm/mpalms_all_coarse.stl",
name="/gripper_right",
color=(0., 0., 1., 1.),
frame_id="/yumi_body")
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 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 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 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 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()
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)
# 4. pulling primitive
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')
if args.simulate:
import simulation
import rospy
rospy.init_node("test")
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)
from helper.util import pose_stamped2list, list2pose_stamped
planner_args = data['planner_args']
object_start_pose_list = data['start']
object_goal_pose_list = data['goal']
print(planner_args.keys())
print(planner_args['primitive_name'])
primitive_name = planner_args['primitive_name']
pb_util.reset_body(body_id=box_id,
base_pos=object_start_pose_list[:3],
base_quat=object_start_pose_list[3:])
new_args = {}
new_args['object_pose1_world'] = list2pose_stamped(object_start_pose_list)
new_args['object_pose2_world'] = list2pose_stamped(object_goal_pose_list)
new_args['primitive_name'] = 'pull'
new_args['palm_pose_r_object'] = list2pose_stamped(data['contact_obj_frame'])
new_args['palm_pose_l_object'] = list2pose_stamped(cfg.PALM_LEFT)
new_args['object'] = None
new_args['init'] = True
new_args['N'] = 32
new_args['table_face'] = 0
from planning import pulling_planning
manipulated_object = new_args['object']
object_pose1_world = new_args['object_pose1_world']
object_pose2_world = new_args['object_pose2_world']
palm_pose_l_object = new_args['palm_pose_l_object']
def main(args):
global joint_lock
global cfg
joint_lock = threading.RLock()
print(args)
rospy.init_node('test')
object_urdf = args.config_package_path + \
'descriptions/urdf/'+args.object_name+'.urdf'
object_mesh = args.config_package_path + \
'descriptions/meshes/objects'+args.object_name+'.stl'
moveit_robot = moveit_commander.RobotCommander()
moveit_scene = moveit_commander.PlanningSceneInterface()
moveit_planner = 'RRTconnectkConfigDefault'
# moveit_planner = 'RRTstarkConfigDefault'
mp_left = GroupPlanner('left_arm',
moveit_robot,
moveit_planner,
moveit_scene,
max_attempts=50,
planning_time=5.0,
goal_tol=0.5,
eef_delta=0.01,
jump_thresh=10.0)
mp_right = GroupPlanner('right_arm',
moveit_robot,
moveit_planner,
moveit_scene,
max_attempts=50,
planning_time=5.0,
goal_tol=0.5,
eef_delta=0.01,
jump_thresh=10.0)
cfg_file = os.path.join(args.example_config_path, args.primitive) + ".yaml"
cfg = get_cfg_defaults()
cfg.merge_from_file(cfg_file)
cfg.freeze()
print(cfg)
real_object_init = [
0.3, -0.2, 0.02, -0.007008648232757453, 0.0019098461832132076,
0.5471545719627792, 0.8370000631527658
]
manipulated_object = None
object_pose1_world = util.list2pose_stamped(cfg.OBJECT_INIT)
# object_pose1_world = util.list2pose_stamped(real_object_init)
object_pose2_world = util.list2pose_stamped(cfg.OBJECT_FINAL)
palm_pose_l_object = util.list2pose_stamped(cfg.PALM_LEFT)
palm_pose_r_object = util.list2pose_stamped(cfg.PALM_RIGHT)
active_arm, unactive_arm = get_active_arm(cfg.OBJECT_INIT)
planner_args = {}
planner_args['object_pose1_world'] = object_pose1_world
planner_args['object_pose2_world'] = object_pose2_world
planner_args['palm_pose_l_object'] = palm_pose_l_object
planner_args['palm_pose_r_object'] = palm_pose_r_object
planner_args['object'] = manipulated_object
planner_args['N'] = 60 # 60
planner_args['init'] = True
global initial_plan
initial_plan = get_primitive_plan(args.primitive, planner_args,
args.config_package_path, active_arm)
box_id = None
yumi = Robot('yumi',
pb=True,
arm_cfg={
'render': True,
'self_collision': False
})
# yumi.arm.go_home()
yumi.arm.set_jpos(cfg.RIGHT_INIT + cfg.LEFT_INIT)
global both_pos
global single_pos
global joint_lock
global palm_contact
joint_lock.acquire()
both_pos = cfg.RIGHT_INIT + cfg.LEFT_INIT
single_pos = {}
single_pos['right'] = cfg.RIGHT_INIT
single_pos['left'] = cfg.LEFT_INIT
joint_lock.release()
if args.object:
box_id = pb_util.load_urdf(
args.config_package_path + 'descriptions/urdf/' +
args.object_name + '.urdf', cfg.OBJECT_INIT[0:3],
cfg.OBJECT_INIT[3:])
# box_id = pb_util.load_urdf(
# args.config_package_path +
# 'descriptions/urdf/'+args.object_name+'.urdf',
# real_object_init[0:3],
# real_object_init[3:]
# )
global trans_box_id
global trans_box_id_final
trans_box_id = pb_util.load_urdf(
args.config_package_path + 'descriptions/urdf/' +
args.object_name + '_trans.urdf', cfg.OBJECT_INIT[0:3],
cfg.OBJECT_INIT[3:])
trans_box_id_final = pb_util.load_urdf(
args.config_package_path + 'descriptions/urdf/' +
args.object_name + '_trans.urdf', cfg.OBJECT_INIT[0:3],
cfg.OBJECT_INIT[3:])
sleep_t = 0.005
loop_t = 0.125
ik = IKHelper()
if args.primitive == 'push' or args.primitive == 'pull':
execute_thread = threading.Thread(target=_yumi_execute_arm,
args=(yumi, active_arm))
else:
execute_thread = threading.Thread(target=_yumi_execute_both,
args=(yumi, ))
execute_thread.daemon = True
execute_thread.start()
box_thread = threading.Thread(target=execute_planned_pull, args=(yumi, ))
box_thread.daemon = True
palm_contact = False
box_thread.start()
replan = args.replan
embed()
for plan_number, plan_dict in enumerate(initial_plan):
intermediate_object_final = util.pose_stamped2list(
plan_dict['object_poses_world'][-1])
if plan_number == len(initial_plan) - 1:
intermediate_object_final = cfg.OBJECT_FINAL
tip_poses = plan_dict['palm_poses_world']
r_wrist_pos_world = yumi.arm.get_ee_pose(arm='right')[0].tolist()
r_wrist_ori_world = yumi.arm.get_ee_pose(arm='right')[1].tolist()
l_wrist_pos_world = yumi.arm.get_ee_pose(arm='left')[0].tolist()
l_wrist_ori_world = yumi.arm.get_ee_pose(arm='left')[1].tolist()
current_wrist_poses = {}
current_wrist_poses['right'] = util.list2pose_stamped(
r_wrist_pos_world + r_wrist_ori_world)
current_wrist_poses['left'] = util.list2pose_stamped(
l_wrist_pos_world + l_wrist_ori_world)
current_tip_poses = get_wrist_to_tip(current_wrist_poses, cfg)
tip_right = []
tip_left = []
tip_right.append(current_tip_poses['right'].pose)
tip_left.append(current_tip_poses['left'].pose)
for i in range(len(tip_poses)):
tip_left.append(tip_poses[i][0].pose)
tip_right.append(tip_poses[i][1].pose)
l_current = yumi.arm.get_jpos()[7:]
r_current = yumi.arm.get_jpos()[:7]
if args.primitive == 'pivot' or args.primitive == 'grasp':
print("Planning with MoveIt! Plan number: " + str(plan_number))
traj_right = mp_right.plan_waypoints(tip_right,
force_start=l_current +
r_current,
avoid_collisions=False)
traj_left = mp_left.plan_waypoints(tip_left,
force_start=l_current +
r_current,
avoid_collisions=False)
unified = unify_arm_trajectories(traj_left, traj_right, tip_poses)
aligned_left = unified['left']['aligned_joints']
aligned_right = unified['right']['aligned_joints']
if aligned_left.shape != aligned_right.shape:
raise ValueError('Could not aligned joint trajectories')
reached_final = False
for i in range(aligned_right.shape[0]):
# r_pos = aligned_right[i, :]
# l_pos = aligned_left[i, :]
planned_r = aligned_right[i, :]
planned_l = aligned_left[i, :]
if both_in_contact(
yumi, box_id
) and args.primitive == 'grasp' and plan_number > 0:
reached_final, pos_err_total, ori_err_total = reach_pose_goal(
intermediate_object_final[:3],
intermediate_object_final[3:],
yumi.arm.p.getBasePositionAndOrientation,
box_id,
pos_tol=0.025,
ori_tol=0.01)
print("Reached final: " + str(reached_final))
timeout = 30
start_while = time.time()
pos_err = pos_err_total
ori_err = ori_err_total
timed_out = False
not_perturbed = 0
none_count = 0
if replan:
while not reached_final and not timed_out and both_in_contact(
yumi, box_id):
# if i == 40:
palm_contact = True
pose_ref_r = util.pose_stamped2list(
ik_helper.compute_fk(
joints=yumi.arm.arm_dict['right'].get_jpos(
),
arm='r'))
diffs_r = util.pose_difference_np(
pose=unified['right']['aligned_fk'][i:],
pose_ref=pose_ref_r)
pose_ref_l = util.pose_stamped2list(
ik_helper.compute_fk(
joints=yumi.arm.arm_dict['left'].get_jpos(
),
arm='l'))
diffs_l = util.pose_difference_np(
pose=unified['left']['aligned_fk'][i:],
pose_ref=pose_ref_l)
seed_ind_r = np.argmin(diffs_r[0])
seed_ind_l = np.argmin(diffs_l[0])
seed = {}
seed['right'] = aligned_right[i:, :][seed_ind_r, :]
seed['left'] = aligned_left[i:, :][seed_ind_l, :]
# yumi.arm.p.resetBasePositionAndOrientation(
# box_id,
# util.pose_stamped2list(initial_plan[0]['object_poses_world'][-1])[:3],
# util.pose_stamped2list(
# initial_plan[0]['object_poses_world'][-1])[3:])
# frac_complete = ori_err/ori_err_total
# frac_complete = pos_err/pos_err_total
frac_complete = (ori_err / ori_err_total +
pos_err / pos_err_total) / 2
joints_execute, new_plan = greedy_replan(
yumi,
active_arm,
box_id,
args.primitive,
object_pose2_world,
args.config_package_path,
ik,
seed,
frac_complete=frac_complete,
plan_iteration=plan_number)
if ori_err / ori_err_total < 0.8 and not_perturbed == 0 and plan_number == 1:
perturb_box(
yumi,
box_id, [0.0, 0.0, 0.0],
delta_ori_euler=[np.pi / 8, 0.0, 0.0])
not_perturbed += 1
#############################################
# if plan_number == 2:
if False:
new_tip_poses = new_plan[plan_number][
'palm_poses_world']
new_tip_right = []
new_tip_left = []
for k in range(len(new_tip_poses)):
new_tip_left.append(
new_tip_poses[k][0].pose)
new_tip_right.append(
new_tip_poses[k][1].pose)
l_current = yumi.arm.get_jpos()[7:]
r_current = yumi.arm.get_jpos()[:7]
new_traj_right = mp_right.plan_waypoints(
new_tip_right,
force_start=l_current + r_current,
avoid_collisions=False)
new_traj_left = mp_left.plan_waypoints(
new_tip_left,
force_start=l_current + r_current,
avoid_collisions=False)
new_unified = unify_arm_trajectories(
new_traj_left, new_traj_right,
new_tip_poses)
new_aligned_left = new_unified['left'][
'aligned_joints']
new_aligned_right = new_unified['right'][
'aligned_joints']
if new_aligned_left.shape != new_aligned_right.shape:
raise ValueError(
'Could not aligned joint trajectories')
for j in range(new_aligned_right.shape[0]):
r_pos = new_aligned_right[j, :].tolist()
l_pos = new_aligned_left[j, :].tolist()
joint_lock.acquire()
both_pos = r_pos + l_pos
joint_lock.release()
time.sleep(loop_t)
##############################################
r_pos = joints_execute['right']
l_pos = joints_execute['left']
if r_pos is not None and l_pos is not None:
joint_lock.acquire()
both_pos = r_pos + l_pos
joint_lock.release()
none_count = 0
else:
none_count += 1
if none_count >= 10:
raise ValueError(
'IK returned none more than 10 times!')
pos_tol = 0.05 if plan_number == 1 else 0.01
reached_final, pos_err, ori_err = reach_pose_goal(
intermediate_object_final[:3],
intermediate_object_final[3:],
yumi.arm.p.getBasePositionAndOrientation,
box_id,
pos_tol=pos_tol,
ori_tol=0.001)
timed_out = time.time() - start_while > timeout
if reached_final:
print("REACHED FINAL")
break
if timed_out:
print("TIMED OUT")
time.sleep(0.005)
r_pos = planned_r.tolist()
l_pos = planned_l.tolist()
else:
r_pos = planned_r.tolist()
l_pos = planned_l.tolist()
if reached_final:
break
joint_lock.acquire()
both_pos = r_pos + l_pos
joint_lock.release()
time.sleep(loop_t)
else:
if active_arm == 'right':
traj = mp_right.plan_waypoints(tip_right,
force_start=l_current +
r_current,
avoid_collisions=False)
else:
traj = mp_left.plan_waypoints(tip_left,
force_start=l_current +
r_current,
avoid_collisions=False)
# make numpy array of each arm joint trajectory for each comp
joints_np = np.zeros((len(traj.points), 7))
# make numpy array of each arm pose trajectory, based on fk
fk_np = np.zeros((len(traj.points), 7))
for i, point in enumerate(traj.points):
joints_np[i, :] = point.positions
pose = ik_helper.compute_fk(point.positions, arm=active_arm[0])
fk_np[i, :] = util.pose_stamped2list(pose)
for i, point in enumerate(traj.points):
# j_pos = point.positions
planned_pos = point.positions
if replan and is_in_contact(
yumi, box_id) and (args.primitive == 'pull'
or args.primitive == 'push'):
palm_contact = True
reached_final, pos_err_total, _ = reach_pose_goal(
cfg.OBJECT_FINAL[:3],
cfg.OBJECT_FINAL[3:],
yumi.arm.p.getBasePositionAndOrientation,
box_id,
pos_tol=0.025,
ori_tol=0.01)
print("Reached final: " + str(reached_final))
timeout = 30
start_while = time.time()
pos_err = pos_err_total
timed_out = False
not_perturbed = 0
none_count = 0
while not reached_final and not timed_out:
# if is_in_contact(yumi, box_id) and (args.primitive == 'pull' or args.primitive == 'push'):
if True:
palm_contact = True
pose_ref = util.pose_stamped2list(
ik_helper.compute_fk(
joints=yumi.arm.arm_dict[active_arm].
get_jpos(),
arm=active_arm[0]))
diffs = util.pose_difference_np(
pose=fk_np, pose_ref=pose_ref)[0]
seed_ind = np.argmin(diffs)
seed = {}
seed[active_arm] = joints_np[seed_ind, :]
seed[unactive_arm] = yumi.arm.arm_dict[
unactive_arm].get_jpos()
joints_execute, _ = greedy_replan(
yumi,
active_arm,
box_id,
args.primitive,
object_pose2_world,
args.config_package_path,
ik,
seed,
frac_complete=pos_err / pos_err_total)
if joints_execute[active_arm] is not None:
joint_lock.acquire()
single_pos[active_arm] = joints_execute[
active_arm]
joint_lock.release()
none_count = 0
else:
none_count += 1
if none_count >= 10:
raise ValueError(
'IK returned none more than 10 times!')
reached_final, pos_err, _ = reach_pose_goal(
cfg.OBJECT_FINAL[:3],
cfg.OBJECT_FINAL[3:],
yumi.arm.p.getBasePositionAndOrientation,
box_id,
pos_tol=0.003,
ori_tol=0.001)
# if pos_err/pos_err_total < 0.3 and not_perturbed==1:
# perturb_box(yumi, box_id,
# [0.0, -0.02, 0.0],
# delta_ori_euler=[0.0, 0.0, -np.pi/6])
# not_perturbed += 1
# if pos_err/pos_err_total < 0.8 and not_perturbed==0:
# perturb_box(yumi, box_id,
# [0.0, -0.05, 0.0],
# delta_ori_euler=[0.0, 0.0, np.pi/3])
# not_perturbed += 1
timed_out = time.time() - start_while > timeout
if reached_final:
print("REACHED FINAL")
return
if timed_out:
print("TIMED OUT")
else:
pass
# joint_lock.acquire()
# single_pos[active_arm] = seed[active_arm]
# joint_lock.release()
time.sleep(0.005)
# j_pos = planned_pos
else:
j_pos = planned_pos
joint_lock.acquire()
single_pos[active_arm] = j_pos
joint_lock.release()
time.sleep(loop_t)
def main(args):
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)
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')
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 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 = {}
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):
print(args)
yumi = Robot('yumi',
pb=True,
arm_cfg={
'render': True,
'self_collision': False
})
yumi.arm.go_home()
cfg_file = os.path.join(args.example_config_path, args.primitive) + ".yaml"
cfg = get_cfg_defaults()
cfg.merge_from_file(cfg_file)
cfg.freeze()
print(cfg)
yumi.arm.set_jpos(cfg.RIGHT_INIT + cfg.LEFT_INIT)
time.sleep(1.0)
manipulated_object = None
object_pose1_world = util.list2pose_stamped(cfg.OBJECT_INIT)
object_pose2_world = util.list2pose_stamped(cfg.OBJECT_FINAL)
palm_pose_l_object = util.list2pose_stamped(cfg.PALM_LEFT)
palm_pose_r_object = util.list2pose_stamped(cfg.PALM_RIGHT)
if args.primitive == 'push':
plan = pushing_planning(object=manipulated_object,
object_pose1_world=object_pose1_world,
object_pose2_world=object_pose2_world,
palm_pose_l_object=palm_pose_l_object,
palm_pose_r_object=palm_pose_r_object)
elif args.primitive == 'grasp':
plan = grasp_planning(object=manipulated_object,
object_pose1_world=object_pose1_world,
object_pose2_world=object_pose2_world,
palm_pose_l_object=palm_pose_l_object,
palm_pose_r_object=palm_pose_r_object)
elif args.primitive == 'pivot':
gripper_name = args.config_package_path + \
'descriptions/meshes/mpalm/mpalms_all_coarse.stl'
table_name = args.config_package_path + \
'descriptions/meshes/table/table_top.stl'
manipulated_object = collisions.CollisionBody(
args.config_package_path +
'descriptions/meshes/objects/realsense_box_experiments.stl')
plan = levering_planning(object=manipulated_object,
object_pose1_world=object_pose1_world,
object_pose2_world=object_pose2_world,
palm_pose_l_object=palm_pose_l_object,
palm_pose_r_object=palm_pose_r_object,
gripper_name=gripper_name,
table_name=table_name)
elif args.primitive == 'pull':
plan = pulling_planning(object=manipulated_object,
object_pose1_world=object_pose1_world,
object_pose2_world=object_pose2_world,
palm_pose_l_object=palm_pose_l_object,
palm_pose_r_object=palm_pose_r_object,
arm='r')
else:
raise NotImplementedError
object_loaded = False
for plan_dict in plan:
for i, t in enumerate(plan_dict['t']):
if i == 0 and not object_loaded and args.object:
time.sleep(2.0)
pb_util.load_urdf(
args.config_package_path +
'descriptions/urdf/realsense_box.urdf',
cfg.OBJECT_INIT[0:3], cfg.OBJECT_INIT[3:])
time.sleep(2.0)
object_loaded = True
tip_poses = plan_dict['palm_poses_world'][i]
r_joints, l_joints, wrist_right, wrist_left = get_joint_poses(
tip_poses, yumi, cfg, nullspace=False)
loop_time = 0.125
sleep_time = 0.005
start = time.time()
if args.primitive != 'push':
while (time.time() - start < loop_time):
yumi.arm.set_jpos(list(r_joints) + list(l_joints),
wait=False)
time.sleep(sleep_time)
else:
while (time.time() - start < loop_time):
yumi.arm.set_jpos(r_joints, arm='right', wait=False)
time.sleep(sleep_time)
