def set_traj_fields_for_response(warped_stage_data, resp, arm, frame_id):
traj = resp.traj
gripper_data_key = "%s_gripper_tool_frame" % (arm)
gripper_joint_key = "%s_gripper_joint" % (arm)
if arm == 'r':
traj.r_gripper_poses.poses = juc.xyzs_quats_to_poses(warped_stage_data[gripper_data_key]["position"], warped_stage_data[gripper_data_key]["orientation"])
traj.r_gripper_angles = warped_stage_data[gripper_joint_key]
traj.r_gripper_poses.header.frame_id = frame_id
elif arm == 'l':
traj.l_gripper_poses.poses = juc.xyzs_quats_to_poses(warped_stage_data[gripper_data_key]["position"], warped_stage_data[gripper_data_key]["orientation"])
traj.l_gripper_angles = warped_stage_data[gripper_joint_key]
traj.l_gripper_poses.header.frame_id = frame_id
def set_traj_fields_for_response(warped_stage_data, resp, arm, frame_id):
traj = resp.traj
gripper_data_key = "%s_gripper_tool_frame" % (arm)
gripper_joint_key = "%s_gripper_joint" % (arm)
if arm == 'r':
traj.r_gripper_poses.poses = juc.xyzs_quats_to_poses(warped_stage_data[gripper_data_key]["position"], warped_stage_data[gripper_data_key]["orientation"])
traj.r_gripper_angles = warped_stage_data[gripper_joint_key]
traj.r_gripper_poses.header.frame_id = frame_id
elif arm == 'l':
traj.l_gripper_poses.poses = juc.xyzs_quats_to_poses(warped_stage_data[gripper_data_key]["position"], warped_stage_data[gripper_data_key]["orientation"])
traj.l_gripper_angles = warped_stage_data[gripper_joint_key]
traj.l_gripper_poses.header.frame_id = frame_id
def make_traj(req):
"""
Generate a trajectory using warping
See MakeTrajectory service description
(TODO) should be able to specify a specific demo
"""
assert isinstance(req, MakeTrajectoryRequest)
new_object_clouds = [pc2xyzrgb(cloud)[0] for cloud in req.object_clouds]
scene_info = "PLACEHOLDER"
best_demo_name, best_demo_info = verbs.get_closest_demo(
req.verb, scene_info)
best_demo_data = verbs.get_demo_data(best_demo_name)
transform_type = "tps"
old_object_clouds = [
best_demo_data["object_clouds"][obj_name]["xyz"]
for obj_name in best_demo_data["object_clouds"].keys()
]
if len(old_object_clouds) > 1:
raise Exception("i don't know what to do with multiple object clouds")
x_nd = voxel_downsample(old_object_clouds[0], .02)
y_md = voxel_downsample(new_object_clouds[0], .02)
if transform_type == "tps":
#warp = registration.tps_rpm_zrot(x_nd, y_md, plotting=2,reg_init=2,reg_final=.05, n_iter=10, verbose=False)
warp = registration.tps_rpm(x_nd,
y_md,
plotting=2,
reg_init=2,
reg_final=.05,
n_iter=10,
verbose=False)
elif transform_type == "translation2d":
warp = registration.Translation2d()
warp.fit(x_nd, y_md)
elif transform_type == "rigid2d":
warp = registration.Rigid2d()
warp.fit(x_nd, y_md)
else:
raise Exception("transform type %s is not yet implemented" %
transform_type)
l_offset, r_offset = np.zeros(3), np.zeros(3)
#if "r_tool" in best_demo_info:
#r_offset = asarray(tool_info[this_verb_info["r_tool"]]["translation"])
#if "l_tool" in best_demo_info:
#l_offset = asarray(tool_info[this_verb_info["l_tool"]]["translation"])
arms_used = best_demo_info["arms_used"]
warped_demo_data = warping.transform_verb_demo(warp, best_demo_data)
resp = MakeTrajectoryResponse()
traj = resp.traj
traj.arms_used = arms_used
if arms_used in "lb":
traj.l_gripper_poses.poses = juc.xyzs_quats_to_poses(
warped_demo_data["l_gripper_tool_frame"]["position"],
warped_demo_data["l_gripper_tool_frame"]["orientation"])
traj.l_gripper_angles = warped_demo_data["l_gripper_joint"]
traj.l_gripper_poses.header.frame_id = req.object_clouds[
0].header.frame_id
if "l_tool" in best_demo_info: traj.l_gripper_angles *= 0
if arms_used in "rb":
traj.r_gripper_poses.poses = juc.xyzs_quats_to_poses(
warped_demo_data["r_gripper_tool_frame"]["position"],
warped_demo_data["r_gripper_tool_frame"]["orientation"])
traj.r_gripper_angles = warped_demo_data["r_gripper_joint"]
traj.r_gripper_poses.header.frame_id = req.object_clouds[
0].header.frame_id
if "r_tool" in best_demo_info: traj.r_gripper_angles *= 0
Globals.handles = []
plot_original_and_warped_demo(best_demo_data, warped_demo_data, traj)
pose_array = conversions.array_to_pose_array(y_md, 'base_footprint')
Globals.handles.append(
Globals.rviz.draw_curve(pose_array,
rgba=(0, 0, 1, 1),
width=.01,
type=Marker.CUBE_LIST))
return resp
def make_traj(req):
"""
Generate a trajectory using warping
See MakeTrajectory service description
(TODO) should be able to specify a specific demo
"""
assert isinstance(req, MakeTrajectoryRequest)
new_object_clouds = [pc2xyzrgb(cloud)[0] for cloud in req.object_clouds]
scene_info = "PLACEHOLDER"
best_demo_name, best_demo_info = verbs.get_closest_demo(req.verb, scene_info)
best_demo_data = verbs.get_demo_data(best_demo_name)
transform_type = "tps"
old_object_clouds = [best_demo_data["object_clouds"][obj_name]["xyz"]
for obj_name in best_demo_data["object_clouds"].keys()]
if len(old_object_clouds) > 1:
raise Exception("i don't know what to do with multiple object clouds")
x_nd = voxel_downsample(old_object_clouds[0],.02)
y_md = voxel_downsample(new_object_clouds[0],.02)
if transform_type == "tps":
#warp = registration.tps_rpm_zrot(x_nd, y_md, plotting=2,reg_init=2,reg_final=.05, n_iter=10, verbose=False)
warp = registration.tps_rpm(x_nd, y_md, plotting=2,reg_init=2,reg_final=.05, n_iter=10, verbose=False)
elif transform_type == "translation2d":
warp = registration.Translation2d()
warp.fit(x_nd, y_md)
elif transform_type == "rigid2d":
warp = registration.Rigid2d()
warp.fit(x_nd, y_md)
else:
raise Exception("transform type %s is not yet implemented"%transform_type)
l_offset,r_offset = np.zeros(3), np.zeros(3)
#if "r_tool" in best_demo_info:
#r_offset = asarray(tool_info[this_verb_info["r_tool"]]["translation"])
#if "l_tool" in best_demo_info:
#l_offset = asarray(tool_info[this_verb_info["l_tool"]]["translation"])
arms_used = best_demo_info["arms_used"]
warped_demo_data = warping.transform_verb_demo(warp, best_demo_data)
resp = MakeTrajectoryResponse()
traj = resp.traj
traj.arms_used = arms_used
if arms_used in "lb":
traj.l_gripper_poses.poses = juc.xyzs_quats_to_poses(warped_demo_data["l_gripper_tool_frame"]["position"], warped_demo_data["l_gripper_tool_frame"]["orientation"])
traj.l_gripper_angles = warped_demo_data["l_gripper_joint"]
traj.l_gripper_poses.header.frame_id = req.object_clouds[0].header.frame_id
if "l_tool" in best_demo_info: traj.l_gripper_angles *= 0
if arms_used in "rb":
traj.r_gripper_poses.poses = juc.xyzs_quats_to_poses(warped_demo_data["r_gripper_tool_frame"]["position"], warped_demo_data["r_gripper_tool_frame"]["orientation"])
traj.r_gripper_angles = warped_demo_data["r_gripper_joint"]
traj.r_gripper_poses.header.frame_id = req.object_clouds[0].header.frame_id
if "r_tool" in best_demo_info: traj.r_gripper_angles *= 0
Globals.handles = []
plot_original_and_warped_demo(best_demo_data, warped_demo_data, traj)
pose_array = conversions.array_to_pose_array(y_md, 'base_footprint')
Globals.handles.append(Globals.rviz.draw_curve(pose_array, rgba = (0,0,1,1),width=.01,type=Marker.CUBE_LIST))
return resp