def do_traj_ik_default(lr, gripper_poses):
gripper_xyzs, gripper_quats = [], []
for pose in gripper_poses:
xyz, quat = conversions.pose_to_trans_rot(pose)
gripper_xyzs.append(xyz)
gripper_quats.append(quat)
manip = get_manipulator(lr)
joint_positions, inds = trajectories.make_joint_traj(gripper_xyzs, gripper_quats, manip, "base_footprint", "%s_gripper_tool_frame"%lr, filter_options = 1+18)
return joint_positions
def do_traj_ik_default(pr2, lr, gripper_poses):
gripper_xyzs, gripper_quats = [], []
for pose in gripper_poses:
xyz, quat = juc.pose_to_trans_rot(pose)
gripper_xyzs.append(xyz)
gripper_quats.append(quat)
manip = get_manipulator(pr2, lr)
joint_positions, inds = trajectories.make_joint_traj(gripper_xyzs, gripper_quats, manip, "base_footprint", "%s_gripper_tool_frame"%lr, filter_options = 1+18)
return joint_positions
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
scale = asarray(traj["object_dimension"]))
pose_array = conversions.array_to_pose_array(asarray(traj["gripper_positions"]), 'base_link')
rviz.draw_curve(
pose_array,
id=0)
n_waypoints = 20
xyzquat = np.c_[traj["gripper_positions"],traj["gripper_orientations"]]
xyzquat_rs = kinematics_utils.unif_resample(xyzquat, n_waypoints, weights = np.ones(7), tol=.001)
times = np.linspace(0,10,n_waypoints)
pr2.torso.go_up()
pr2.join_all()
pr2.update_rave()
joint_positions,_ = trajectories.make_joint_traj(xyzquat_rs[:,0:3], xyzquat_rs[:,3:7], pr2.rarm.manip, 'base_link', 'r_wrist_roll_link', filter_options = 18)
joint_velocities = kinematics_utils.get_velocities(joint_positions, times, tol=.001)
pr2.rarm.follow_timed_joint_trajectory(joint_positions, joint_velocities, times)
#for xyzq in xyzquat_rs:
#xyz = xyzq[:3]
#quat = xyzq[3:]
#hmat = conversions.trans_rot_to_hmat(xyz,quat)
#try:
#pr2.rarm.goto_pose_matrix(hmat, 'base_link', 'r_wrist_roll_link')
#pr2.join_all()
#except IKFail:
#pass
pr2.join_all()
rviz.draw_curve(pose_array, id=0)
n_waypoints = 20
xyzquat = np.c_[traj["gripper_positions"], traj["gripper_orientations"]]
xyzquat_rs = kinematics_utils.unif_resample(xyzquat,
n_waypoints,
weights=np.ones(7),
tol=.001)
times = np.linspace(0, 10, n_waypoints)
pr2.torso.go_up()
pr2.join_all()
pr2.update_rave()
joint_positions, _ = trajectories.make_joint_traj(xyzquat_rs[:, 0:3],
xyzquat_rs[:, 3:7],
pr2.rarm.manip,
'base_link',
'r_wrist_roll_link',
filter_options=18)
joint_velocities = kinematics_utils.get_velocities(joint_positions,
times,
tol=.001)
pr2.rarm.follow_timed_joint_trajectory(joint_positions, joint_velocities,
times)
#for xyzq in xyzquat_rs:
#xyz = xyzq[:3]
#quat = xyzq[3:]
#hmat = conversions.trans_rot_to_hmat(xyz,quat)
#try:
#pr2.rarm.goto_pose_matrix(hmat, 'base_link', 'r_wrist_roll_link')
#pr2.join_all()
def execute(self,userdata):
"""
- lookup closest trajectory from database
- if it's a terminal state, we're done
- warp it based on the current rope
returns: done, not_done, failure
visualization:
- show all library states
- warping visualization from matlab
"""
xyz_new = np.squeeze(np.asarray(userdata.points))
if args.obj == "cloth": xyz_new = voxel_downsample(xyz_new, .025)
xyz_new_ds, ds_inds = downsample(xyz_new)
dists_new = recognition.calc_geodesic_distances_downsampled(xyz_new,xyz_new_ds, ds_inds)
if HUMAN_SELECT_DEMO:
seg_name = trajectory_library.interactive_select_demo(self.library)
best_demo = self.library.root[seg_name]
pts0,_ = downsample(np.asarray(best_demo["cloud_xyz"]))
pts1,_ = downsample(xyz_new)
self.f = registration.tps_rpm(pts0, pts1,
plotting = 4, reg_init=1,reg_final=.025,n_iter=40)
else:
best_f = None
best_cost = np.inf
best_name = None
for (seg_name,candidate_demo) in self.library.root.items():
xyz_demo = np.squeeze(np.asarray(candidate_demo["cloud_xyz"]))
if args.obj == "cloth": xyz_demo = voxel_downsample(xyz_demo, .025)
xyz_demo_ds, ds_inds = downsample(xyz_demo)#voxel_downsample(xyz_demo, DS_LENGTH, return_inds = True)
dists_demo = recognition.calc_geodesic_distances_downsampled(xyz_demo, xyz_demo_ds, ds_inds)
cost = recognition.calc_match_score(xyz_new_ds, xyz_demo_ds, dists0 = dists_new, dists1 = dists_demo)
print "seg_name: %s. cost: %s"%(seg_name, cost)
if cost < best_cost:
best_cost = cost
best_name = seg_name
#if best_name.startswith("done"): return "done"
best_demo = self.library.root[best_name]
xyz_demo_ds,_ = downsample(np.asarray(best_demo["cloud_xyz"][0]))
self.f = registration.tps_rpm(xyz_demo_ds, xyz_new_ds,
plotting = 10, reg_init=1,reg_final=.01,n_iter=200,verbose=True)
print "best segment:", best_name
orig_pose_array = conversions.array_to_pose_array(best_demo["cloud_xyz"][0], "base_footprint")
warped_pose_array = conversions.array_to_pose_array(self.f.transform_points(best_demo["cloud_xyz"][0]), "base_footprint")
HANDLES.append(Globals.rviz.draw_curve(orig_pose_array,rgba=(1,0,0,1),id=19024,type=Marker.CUBE_LIST))
HANDLES.append(Globals.rviz.draw_curve(warped_pose_array,rgba=(0,1,0,1),id=2983,type=Marker.CUBE_LIST))
mins = best_demo["cloud_xyz"][0].min(axis=0)
maxes = best_demo["cloud_xyz"][0].max(axis=0)
mins[2] -= .1
maxes[2] += .1
grid_handle = warping.draw_grid(Globals.rviz, self.f.transform_points, mins, maxes, 'base_footprint')
HANDLES.append(grid_handle)
#self.f = fit_tps(demo["rope"][0], userdata.points)
userdata.left_used = left_used = best_demo["arms_used"].value in "lb"
userdata.right_used = right_used = best_demo["arms_used"].value in "rb"
print "left_used", left_used
print "right_used", right_used
warped_demo = warping.transform_demo_with_fingertips(self.f, best_demo, left=left_used, right=right_used)
trajectory = np.zeros(len(best_demo["times"]), dtype=trajectories.BodyState)
Globals.pr2.update_rave()
if left_used:
l_arm_traj, feas_inds = trajectories.make_joint_traj(warped_demo["l_gripper_xyzs"], warped_demo["l_gripper_quats"], Globals.pr2.robot.GetManipulator("leftarm"),"base_footprint","l_gripper_tool_frame",1+16)
if len(feas_inds) == 0: return "failure"
trajectory["l_arm"] = l_arm_traj
rospy.loginfo("left arm: %i of %i points feasible", len(feas_inds), len(trajectory))
trajectory["l_gripper"] = fix_gripper(warped_demo["l_gripper_angles"])
HANDLES.append(Globals.rviz.draw_curve(
conversions.array_to_pose_array(
alternate(warped_demo["l_gripper_xyzs1"],warped_demo["l_gripper_xyzs2"]),
"base_footprint"),
width=.001, rgba = (1,0,1,.4),type=Marker.LINE_LIST))
if right_used:
r_arm_traj,feas_inds = trajectories.make_joint_traj(warped_demo["r_gripper_xyzs"], warped_demo["r_gripper_quats"], Globals.pr2.robot.GetManipulator("rightarm"),"base_footprint","r_gripper_tool_frame",1+16)
if len(feas_inds) == 0: return "failure"
trajectory["r_arm"] = r_arm_traj
rospy.loginfo("right arm: %i of %i points feasible", len(feas_inds), len(trajectory))
trajectory["r_gripper"] = fix_gripper(warped_demo["r_gripper_angles"])
HANDLES.append(Globals.rviz.draw_curve(
conversions.array_to_pose_array(
alternate(warped_demo["l_gripper_xyzs1"],warped_demo["l_gripper_xyzs2"]),
"base_footprint"),
width=.001, rgba = (1,0,1,.4),type=Marker.LINE_LIST))
userdata.trajectory = trajectory
#userdata.base_xya = (x,y,0)
# todo: draw pr2
# consent = yes_or_no("trajectory ok?")
consent = True
if consent: return "not_done"
else: return "failure"
def callback(req):
Globals.handles = []
traj = req.traj
if traj.arms_used in "rl":
lr = traj.arms_used
manip_name = {"l": "leftarm", "r": "rightarm"}[lr]
manip = Globals.pr2.robot.GetManipulator(manip_name)
gripper0_xyzs, gripper0_quats = [], []
for pose in traj.gripper0_poses.poses:
xyz, quat = conversions.pose_to_trans_rot(pose)
gripper0_xyzs.append(xyz)
gripper0_quats.append(quat)
joint_positions, inds = trajectories.make_joint_traj(
gripper0_xyzs, gripper0_quats, manip, "base_footprint", "%s_gripper_tool_frame" % lr, filter_options=1
)
feasible_frac = len(inds) / len(gripper0_xyzs)
best_feasible_frac, best_lr, best_joint_positions = feasible_frac, lr, joint_positions
if best_feasible_frac < 0.7:
rospy.logerr("unacceptably low fraction of ik success: %.4f", best_feasible_frac)
return ExecTrajectoryResponse(success=False)
rospy.loginfo("using %s hand", {"l": "left", "r": "right"}[best_lr])
gripper_angles = np.array(traj.gripper0_angles)
rospy.logwarn("warning! gripper angle hack")
gripper_angles[gripper_angles < 0.04] = gripper_angles[gripper_angles < 0.04] - 0.02
body_traj = np.zeros(len(best_joint_positions), dtype=trajectories.BodyState)
body_traj["%s_arm" % best_lr] = best_joint_positions
body_traj["%s_gripper" % best_lr] = gripper_angles
pose_array = gm.PoseArray()
pose_array.header.frame_id = "base_footprint"
pose_array.poses = traj.gripper0_poses.poses
Globals.handles.append(Globals.rviz.draw_curve(pose_array, rgba=(1, 0, 0, 1)))
trajectories.follow_body_traj(
Globals.pr2,
body_traj,
times=None,
r_arm=(best_lr == "r"),
r_gripper=(best_lr == "r"),
l_arm=(best_lr == "l"),
l_gripper=(best_lr == "l"),
)
elif traj.arms_used in "b":
body_traj = np.zeros(len(traj.gripper0_poses.poses), dtype=trajectories.BodyState)
for (lr, gripper_poses, gripper_angles) in zip(
"lr", [traj.gripper0_poses.poses, traj.gripper1_poses.poses], [traj.gripper0_angles, traj.gripper1_angles]
):
gripper_angles = np.array(gripper_angles)
rospy.logwarn("warning! gripper angle hack")
gripper_angles[gripper_angles < 0.04] = gripper_angles[gripper_angles < 0.04] - 0.02
manip_name = {"l": "leftarm", "r": "rightarm"}[lr]
manip = Globals.pr2.robot.GetManipulator(manip_name)
gripper_xyzs, gripper_quats = [], []
for pose in gripper_poses:
xyz, quat = conversions.pose_to_trans_rot(pose)
gripper_xyzs.append(xyz)
gripper_quats.append(quat)
joint_positions, inds = trajectories.make_joint_traj(
gripper_xyzs, gripper_quats, manip, "base_footprint", "%s_gripper_tool_frame" % lr, filter_options=1
)
if len(inds) == 0:
return ExecTrajectoryResponse(success=False)
feasible_frac = len(inds) / len(gripper_xyzs)
body_traj["%s_arm" % lr] = joint_positions
body_traj["%s_gripper" % lr] = gripper_angles
pose_array = gm.PoseArray()
pose_array.header.frame_id = "base_footprint"
pose_array.header.stamp = rospy.Time.now()
pose_array.poses = traj.gripper0_poses.poses
Globals.handles.append(Globals.rviz.draw_curve(pose_array, rgba=(1, 0, 0, 1)))
trajectories.follow_body_traj(
Globals.pr2, body_traj, times=None, r_arm=True, r_gripper=True, l_arm=True, l_gripper=True
)
else:
raise NotImplementedError
Globals.pr2.join_all()
return ExecTrajectoryResponse(success=True)
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
