def transform_demo(reg, demo, left=True, right=True, cloud_xyz=False, object_clouds=False, l_offset = None, r_offset = None):
"""
reg: NonrigidRegistration object
demo: array with the following fields: r_gripper_xyzs, l_gripper_xyzs, r_gripper_quats, l_gripper_quats, cloud_xyz
(todo: replace 'cloud_xyz' with 'object_points')
"""
warped_demo = group_to_dict(demo)
if left:
if l_offset is None:
l_tool_xyzs = demo["l_gripper_xyzs"]
else:
l_tool_xyzs = np.array([xyz + np.dot(rot[:3,:3], l_offset) for (xyz, rot) in zip(demo["l_gripper_xyzs"], quats2mats(demo["l_gripper_quats"]))])
l_tool_xyzs_warped, rot_l_warped = reg.transform_frames(l_tool_xyzs, quats2mats(demo["l_gripper_quats"]))
l_gripper_quats_warped = mats2quats(rot_l_warped)
if l_offset is None:
l_gripper_xyzs_warped = l_tool_xyzs_warped
else:
l_gripper_xyzs_warped = np.array([xyz - np.dot(rot[:3,:3], l_offset) for (xyz, rot) in zip(l_tool_xyzs_warped, rot_l_warped)])
warped_demo["l_gripper_xyzs"] = l_gripper_xyzs_warped
warped_demo["l_gripper_quats"] = l_gripper_quats_warped
if right:
if r_offset is None:
r_tool_xyzs = demo["r_gripper_xyzs"]
else:
r_tool_xyzs = np.array([xyz + np.dot(rot[:3,:3], r_offset) for (xyz, rot) in zip(demo["r_gripper_xyzs"], quats2mats(demo["r_gripper_quats"]))])
r_tool_xyzs_warped, rot_r_warped = reg.transform_frames(r_tool_xyzs, quats2mats(demo["r_gripper_quats"]))
r_gripper_quats_warped = mats2quats(rot_r_warped)
if r_offset is None:
r_gripper_xyzs_warped = r_tool_xyzs_warped
else:
r_gripper_xyzs_warped = np.array([xyz - np.dot(rot[:3,:3], r_offset) for (xyz, rot) in zip(r_tool_xyzs_warped, rot_r_warped)])
warped_demo["r_gripper_xyzs"] = r_gripper_xyzs_warped
warped_demo["r_gripper_quats"] = r_gripper_quats_warped
if cloud_xyz:
old_cloud_xyz_pts = np.asarray(demo["cloud_xyz"]).reshape(-1,3)
new_cloud_xyz_pts = reg.transform_points(old_cloud_xyz_pts)
warped_demo["cloud_xyz"] = new_cloud_xyz_pts.reshape(demo["cloud_xyz"].shape)
if object_clouds:
for key in sorted(demo["object_clouds"].keys()):
old_cloud_xyz_pts = np.asarray(demo["object_clouds"][key]).reshape(-1,3)
new_cloud_xyz_pts = reg.transform_points(old_cloud_xyz_pts)
warped_demo["object_clouds"][key] = new_cloud_xyz_pts
return warped_demo
def callback(request):
global HANDLES
HANDLES = []
xyzs, _ = pc2xyzrgb(request.point_cloud)
new_mins = xyzs.reshape(-1, 3).min(axis=0)
new_maxes = xyzs.reshape(-1, 3).max(axis=0)
new_obj_xyz = (new_mins + new_maxes) / 2
new_obj_dim = new_maxes - new_mins
f.fit(np.atleast_2d(obj_xyz), np.atleast_2d(new_obj_xyz), 0.001, 0.001)
new_xyzs, new_mats = f.transform_frames(old_xyzs, conversions.quats2mats(old_quats))
new_quats = conversions.mats2quats(new_mats)
show_object_and_trajectory(new_xyzs, new_obj_xyz, obj_quat, new_obj_dim, NEW_ID)
show_object_and_trajectory(xyzquat[:, :3], obj_xyz, obj_quat, obj_dim, OLD_ID)
Globals.pr2.update_rave()
joint_positions, inds = trajectories.make_joint_traj(
new_xyzs, new_quats, Globals.pr2.rarm.manip, "base_link", "r_wrist_roll_link", filter_options=18
)
response = PushResponse()
if joint_positions is not None:
Globals.pr2.rarm.goto_joint_positions(joint_positions[0])
# current_joints = Globals.pr2.rarm.get_joint_positions()[None,:]
joint_positions, joint_velocities, times = retiming.make_traj_with_limits(
joint_positions, Globals.pr2.rarm.vel_limits, Globals.pr2.rarm.acc_limits, smooth=True
)
# joint_velocities = kinematics_utils.get_velocities(joint_positions, times, tol=.001)
Globals.pr2.rarm.follow_timed_joint_trajectory(joint_positions, joint_velocities, times)
response.success = True
else:
response.success = False
return response
def callback(request):
Globals.pr2.rarm.goto_posture('side')
Globals.pr2.larm.goto_posture('side')
#Globals.rviz.remove_all_markers()
#draw_table()
new_cloud1, _ = pc2xyzrgb(request.object_clouds[0])
new_cloud2, _ = pc2xyzrgb(request.object_clouds[1])
new_cloud1, new_cloud2 = sorted([new_cloud1, new_cloud2], key = lambda x: np.squeeze(x).ptp(axis=0).prod())
new_cloud1 = new_cloud1.reshape(-1,3)
new_cloud2 = new_cloud2.reshape(-1,3)
new_xyz1 = (new_cloud1.max(axis=0) + new_cloud1.min(axis=0))/2
new_xyz2 = (new_cloud2.max(axis=0) + new_cloud2.min(axis=0))/2
f.fit(np.array([xyz1, xyz2]), np.array([new_xyz1, new_xyz2]), 1e6, 1e-3)
new_gripper_xyzs, new_gripper_mats = f.transform_frames(old_gripper_xyzs, conversions.quats2mats(old_gripper_quats))
new_gripper_quats = conversions.mats2quats(new_gripper_mats)
#print "warning: using old oreitnations"
show_objects_and_trajectory(new_gripper_xyzs, np.array([new_xyz1, new_xyz2]), np.array([quat1, quat2]), obj_dims, (0,1,0,1))
show_objects_and_trajectory(old_gripper_xyzs, np.array([xyz1, xyz2]), np.array([quat1, quat2]), obj_dims, (0,0,1,1))
grid_handle = draw_grid(Globals.rviz, f.transform_points, old_gripper_xyzs.min(axis=0), old_gripper_xyzs.max(axis=0), "base_footprint")
HANDLES.append(grid_handle)
Globals.pr2.join_all()
Globals.pr2.update_rave()
best_traj_info, best_feasible_frac = None, 0
env = Globals.pr2.robot.GetEnv()
Globals.pr2.update_rave()
collision_env = create_obstacle_env(env)
basemanip = openravepy.interfaces.BaseManipulation(collision_env.GetRobot("pr2"),plannername=None)
rospy.sleep(.1)
#collision_env.SetViewer("qtcoin")
#raw_input("press enter to continue")
for (lr, arm) in zip("lr",[Globals.pr2.larm,Globals.pr2.rarm]):
name = arm.manip.GetName()
manip = collision_env.GetRobot('pr2').GetManipulator(name)
rospy.loginfo("trying to plan to initial position with %s",name)
first_mat1 = np.eye(4)
first_mat1[:3,:3] = new_gripper_mats[0]
first_mat1[:3,3] = new_gripper_xyzs[0]
first_mat = transform_relative_pose_for_ik(manip, first_mat1, "world", "%s_gripper_tool_frame"%lr)
collision_env.GetRobot("pr2").SetActiveManipulator(name)
trajobj = None
try:
trajobj = basemanip.MoveToHandPosition([first_mat],seedik=16,outputtrajobj=True,execute=0)
rospy.loginfo("planning succeeded")
except Exception:
rospy.loginfo("planning failed")
traceback.print_exc()
print "initial ik result", manip.FindIKSolutions(first_mat,0)
continue
rospy.loginfo("trying ik")
Globals.pr2.update_rave()
joint_positions, inds = trajectories.make_joint_traj(new_gripper_xyzs, new_gripper_quats, manip, 'base_footprint', '%s_gripper_tool_frame'%lr, filter_options = 1+16)
feasible_frac = len(inds)/len(new_gripper_xyzs)
print inds
if feasible_frac > best_feasible_frac:
best_feasible_frac = feasible_frac
best_traj_info = dict(
feasible_frac = feasible_frac,
lr = 'l' if name == 'leftarm' else 'r',
manip = manip,
arm = arm,
initial_traj = trajobj,
joint_positions = joint_positions)
collision_env.Destroy()
response = PourResponse()
if best_feasible_frac > .75:
if best_traj_info["initial_traj"] is not None:
follow_rave_traj(best_traj_info["arm"], best_traj_info["initial_traj"])
else:
print "no initial traj"
#print "feasible inds", best_traj_info["inds"]
body_traj = np.zeros(len(best_traj_info["joint_positions"]),dtype=trajectories.BodyState)
lr = best_traj_info["lr"]
body_traj["%s_arm"%lr] = best_traj_info["joint_positions"]
body_traj["%s_gripper"%lr] = gripper_angles
trajectories.follow_body_traj(Globals.pr2, body_traj, times,
r_arm = (lr=='r'), r_gripper = (lr=='r'), l_arm = (lr=='l'), l_gripper= (lr=='l'))
Globals.pr2.join_all()
response.success = True
else:
rospy.logerr("could not execute trajectory because not enough points are reachable")
response.success = False
return response
def transform_demo_with_fingertips(reg, demo, left=True, right=True, cloud_xyz=False):
"""
reg: NonrigidRegistration object
demo: array with the following fields: r_gripper_xyzs, l_gripper_xyzs, r_gripper_quats, l_gripper_quats, cloud_xyz
(todo: replace 'cloud_xyz' with 'object_points')
"""
warped_demo = dict([(key,np.asarray(value)) for (key, value) in demo.items()])
if left:
_, ori_warped = reg.transform_frames(demo["l_gripper_xyzs"], quats2mats(demo["l_gripper_quats"]))
l_xyzs1 = reg.transform_points(demo["l_gripper_xyzs1"])
l_xyzs2 = reg.transform_points(demo["l_gripper_xyzs2"])
tool_xyzs = []
tool_quats = []
tool_angles = []
for (pos1, pos2, ori) in zip(l_xyzs1, l_xyzs2, ori_warped):
hmat, ang = calc_hand_pose(pos1, pos2, ori)
xyz, quat = conversions.hmat_to_trans_rot(hmat)
tool_xyzs.append(xyz)
tool_quats.append(quat)
tool_angles.append(ang)
warped_demo["l_gripper_xyzs"] = tool_xyzs
warped_demo["l_gripper_xyzs1"] = l_xyzs1
warped_demo["l_gripper_xyzs2"] = l_xyzs2
warped_demo["l_gripper_quats"] = tool_quats
warped_demo["l_gripper_angles"] = tool_angles
if right:
_, ori_warped = reg.transform_frames(demo["r_gripper_xyzs"], quats2mats(demo["r_gripper_quats"]))
r_xyzs1 = reg.transform_points(demo["r_gripper_xyzs1"])
r_xyzs2 = reg.transform_points(demo["r_gripper_xyzs2"])
tool_xyzs = []
tool_quats = []
tool_angles = []
for (pos1, pos2, ori) in zip(r_xyzs1, r_xyzs2, ori_warped):
hmat, ang = calc_hand_pose(pos1, pos2, ori)
xyz, quat = conversions.hmat_to_trans_rot(hmat)
tool_xyzs.append(xyz)
tool_quats.append(quat)
tool_angles.append(ang)
warped_demo["r_gripper_xyzs"] = tool_xyzs
warped_demo["r_gripper_xyzs1"] = r_xyzs1
warped_demo["r_gripper_xyzs2"] = r_xyzs2
warped_demo["r_gripper_quats"] = tool_quats
warped_demo["r_gripper_angles"] = tool_angles
if cloud_xyz:
old_cloud_xyz_pts = demo["cloud_xyz"].reshape(-1,3)
new_cloud_xyz_pts = reg.transform_points(old_cloud_xyz_pts)
warped_demo["cloud_xyz"] = new_cloud_xyz_pts.reshape(demo["cloud_xyz"].shape)
return warped_demo
if __name__ == "__main__":
if rospy.get_name() == "/unnamed": rospy.init_node("pour_service")
Globals.setup()
HANDLES = []
traj = h5py.File(args.file, 'r')[args.group]
decimation = 5
old_gripper_quats = asarray(traj["r_gripper_orientations"])[::decimation]
old_gripper_mats = conversions.quats2mats(old_gripper_quats)
old_gripper_xyzs = asarray(traj["r_gripper_positions"])[::decimation] + .18*old_gripper_mats[:,:,0]
obj_dims = asarray(traj["object_dimension"][0])
sort_inds = np.argsort(obj_dims.prod(axis=1))
print sort_inds
obj_dims = obj_dims[sort_inds]
times = asarray(traj["time_stamp"])[::decimation]
times -= times[0]
gripper_index = list(traj["joint_names"][0]).index("r_gripper_joint")
gripper_angles = asarray(traj["joint_positions"][::decimation,gripper_index])
gripper_angles[gripper_angles < .08] = 0
gripper_angles[gripper_angles > .08] = .088
