def reach_pose_goal(pos, ori, get_func, object_id, pos_tol=0.01, ori_tol=0.02):
"""
Check if end effector reached goal or not. Returns true
if both position and orientation goals have been reached
within specified tolerance
Args:
pos (list np.ndarray): goal position
ori (list or np.ndarray): goal orientation. It can be:
**quaternion** ([qx, qy, qz, qw], shape: :math:`[4]`)
**rotation matrix** (shape: :math:`[3, 3]`)
**euler angles** ([roll, pitch, yaw], shape: :math:`[3]`)
get_func (function): name of the function with which we can get
the current pose
pos_tol (float): tolerance of position error
ori_tol (float): tolerance of orientation error
Returns:
bool: If goal pose is reached or not
"""
if not isinstance(pos, np.ndarray):
goal_pos = np.array(pos)
else:
goal_pos = pos
if not isinstance(ori, np.ndarray):
goal_ori = np.array(ori)
else:
goal_ori = ori
if goal_ori.size == 3:
goal_ori = common.euler2quat(goal_ori)
elif goal_ori.shape == (3, 3):
goal_ori = common.rot2quat(goal_ori)
elif goal_ori.size != 4:
raise TypeError('Orientation must be in one '
'of the following forms:'
'rotation matrix, euler angles, or quaternion')
goal_ori = goal_ori.flatten()
goal_pos = goal_pos.flatten()
new_ee_pos = np.array(get_func(object_id)[0])
new_ee_quat = get_func(object_id)[1]
pos_diff = new_ee_pos.flatten() - goal_pos
pos_error = np.max(np.abs(pos_diff))
quat_diff = common.quat_multiply(common.quat_inverse(goal_ori),
new_ee_quat)
rot_similarity = np.abs(quat_diff[3])
if pos_error < pos_tol and \
rot_similarity > 1 - ori_tol:
return True, pos_error, 1 - rot_similarity
else:
print("pos error: " + str(pos_error))
print("ori_error: " + str(1 - rot_similarity))
return False, pos_error, 1 - rot_similarity
def perturb_box(yumi, box_id, delta_pos, delta_ori_euler=None):
object_pos = list(yumi.arm.p.getBasePositionAndOrientation(box_id)[0])
object_ori = list(yumi.arm.p.getBasePositionAndOrientation(box_id)[1])
new_pos = np.array(object_pos) + np.array(delta_pos)
if delta_ori_euler is not None:
pass
delta_ori_quat = common.euler2quat(delta_ori_euler)
new_ori = common.quat_multiply(object_ori, delta_ori_quat)
else:
new_ori = object_ori
yumi.arm.p.resetBasePositionAndOrientation(box_id, new_pos.tolist(),
new_ori)
def apply_action(self, action):
if not isinstance(action, np.ndarray):
action = np.array(action).flatten()
if action.size != 2:
raise ValueError('Action should be [d_x, d_y].')
action = np.concatenate([action, np.array([0.])])
pos, quat, rot_mat, euler = self.robot.arm.get_ee_pose()
pos += action * self._ee_pos_scale
rot_vec = np.array([0, 0, 1]) * self.gripper_ori
rot_quat = rotvec2quat(rot_vec)
ee_ori = quat_multiply(self.ref_ee_ori, rot_quat)
jnt_pos = self.robot.arm.compute_ik(pos, ori=ee_ori)
for step in range(self._action_repeat):
self.robot.arm.set_jpos(jnt_pos)
self.robot.pb_client.stepSimulation()
def apply_action(self, action):
if not isinstance(action, np.ndarray):
action = np.array(action).flatten()
if action.size != 5:
raise ValueError('Action should be [d_x, d_y, d_z, '
'd_angle, open/close gripper].')
pos, quat, rot_mat, euler = self.robot.arm.get_ee_pose()
pos += action[:3] * self._ee_pos_scale
self.gripper_ori += action[3] * self._ee_ori_scale
self.gripper_ori = ang_in_mpi_ppi(self.gripper_ori)
rot_vec = np.array([0, 0, 1]) * self.gripper_ori
rot_quat = rotvec2quat(rot_vec)
ee_ori = quat_multiply(self.ref_ee_ori, rot_quat)
jnt_pos = self.robot.arm.compute_ik(pos, ori=ee_ori)
gripper_ang = self._scale_gripper_angle(action[4])
for step in range(self._action_repeat):
self.robot.arm.set_jpos(jnt_pos)
self.robot.arm.eetool.set_pos(gripper_ang)
self.robot.pb_client.stepSimulation()
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)
