def do_stage(demo_base_name, demo_index, stage_num, prev_stage_info, prev_exp_clouds):
demo_name = demo_base_name + str(demo_index)
stage_info = verb_data_accessor.get_stage_info(demo_name, stage_num)
pc_sel = do_segmentation(stage_info.item)
make_req = get_trajectory_request(stage_info.verb, pc_sel)
make_resp = make_verb_traj.make_traj_multi_stage(make_req, stage_info, stage_num, prev_stage_info, prev_exp_clouds, verb_data_accessor)
yn = yes_or_no("continue?")
if yn:
exec_req = ExecTrajectoryRequest()
exec_req.traj = make_resp.traj
exec_verb_traj.exec_traj(exec_req, traj_ik_func=exec_verb_traj.do_traj_ik_graph_search)
# return stage info and object clouds so they can be saved for use in the next stage if necessary
return (stage_info, make_req.object_clouds)
def do_stage(demo_name, stage_num, prev_stage_info, prev_exp_pc2, cur_exp_pc2, verb_data_accessor):
stage_info = verb_data_accessor.get_stage_info(demo_name, stage_num)
make_req = get_trajectory_request(stage_info.verb, cur_exp_pc2)
make_resp = multi_item_make_verb_traj.make_traj_multi_stage(
make_req, demo_name, stage_num, prev_stage_info, prev_exp_pc2, verb_data_accessor, "tps_zrot"
)
can_move_lower = stage_num == 0
yn = yes_or_no("continue?")
if yn:
exec_req = ExecTrajectoryRequest()
exec_req.traj = make_resp.traj
exec_verb_traj.exec_traj(
exec_req,
traj_ik_func=ik_functions.do_traj_ik_graph_search,
obj_pc=cur_exp_pc2,
obj_name=stage_info.item,
can_move_lower=can_move_lower,
)
def do_stage(demo_name, stage_num, prev_stage_info, prev_exp_pc2, cur_exp_pc2, verb_data_accessor):
stage_info = verb_data_accessor.get_stage_info(demo_name, stage_num)
make_req = get_trajectory_request(stage_info.verb, cur_exp_pc2)
if stage_num == 0:
grip_to_world_transform_func = None
else:
grip_to_world_transform_func = multi_item_make_verb_traj.make_grip_to_world_transform_tf("%s_gripper_tool_frame" %
verb_data_accessor.get_stage_info(demo_name, stage_num).arms_used)
make_resp = multi_item_make_verb_traj.make_traj_multi_stage(make_req, demo_name,
stage_num, prev_stage_info,
prev_exp_pc2, verb_data_accessor,
"tps_zrot")
can_move_lower = (stage_num == 0)
yn = yes_or_no("continue?")
if yn:
exec_req = ExecTrajectoryRequest()
exec_req.traj = make_resp.traj
exec_verb_traj.exec_traj(exec_req, traj_ik_func=ik_functions.do_traj_ik_graph_search,
obj_pc=cur_exp_pc2, obj_name=stage_info.item, can_move_lower=can_move_lower)
for obj_name in demo_info["args"]:
print "select the %s" % obj_name
pc_sel = seg_svc.call(ProcessCloudRequest(cloud_in=pc_tf)).cloud_out
make_req.object_clouds.append(pc_sel)
else:
object_clouds = [
demo_data["object_clouds"][obj_name]["xyz"]
for obj_name in demo_data["object_clouds"].keys()
]
for i in xrange(len(object_clouds)):
cloud = object_clouds[i].reshape(-1, 3)
#translation = np.random.randn(1,3) * np.array([[.1,.1,0]])
#cloud += translation
center = cloud.mean(axis=0)
from numpy import cos, sin, pi
rotation = np.array([[cos(pi / 4), sin(pi / 4), 0],
[-sin(pi / 4), cos(pi / 4), 0], [0, 0, 1]])
cloud = center[None, :] + np.dot(cloud - center[None, :], rotation.T)
make_req.object_clouds.append(ru.xyz2pc(cloud, 'base_footprint'))
make_resp = make_verb_traj.make_traj(make_req)
yn = yes_or_no("continue?")
if yn:
#exec_verb_traj.Globals.pr2.rarm.vel_limits *= .5
#exec_verb_traj.Globals.pr2.larm.vel_limits *= .5
exec_req = ExecTrajectoryRequest()
exec_req.traj = make_resp.traj
exec_verb_traj.exec_traj(exec_req)
pc = rospy.wait_for_message("/drop/points", sm.PointCloud2)
pc_tf = ru.transformPointCloud2(pc, exec_verb_traj.Globals.pr2.tf_listener, "base_footprint", pc.header.frame_id)
for obj_name in demo_info["args"]:
print "select the %s"%obj_name
pc_sel = seg_svc.call(ProcessCloudRequest(cloud_in = pc_tf)).cloud_out
make_req.object_clouds.append(pc_sel)
else:
object_clouds = [demo_data["object_clouds"][obj_name]["xyz"]
for obj_name in demo_data["object_clouds"].keys()]
for i in xrange(len(object_clouds)):
cloud = object_clouds[i].reshape(-1,3)
#translation = np.random.randn(1,3) * np.array([[.1,.1,0]])
#cloud += translation
center = cloud.mean(axis=0)
from numpy import cos,sin,pi
rotation = np.array([[cos(pi/4), sin(pi/4), 0], [-sin(pi/4), cos(pi/4), 0], [0, 0, 1]])
cloud = center[None,:] + np.dot(cloud - center[None,:], rotation.T)
make_req.object_clouds.append(ru.xyz2pc(cloud,'base_footprint'))
make_resp = make_verb_traj.make_traj(make_req)
yn = yes_or_no("continue?")
if yn:
#exec_verb_traj.Globals.pr2.rarm.vel_limits *= .5
#exec_verb_traj.Globals.pr2.larm.vel_limits *= .5
exec_req = ExecTrajectoryRequest()
exec_req.traj = make_resp.traj
exec_verb_traj.exec_traj(exec_req)
