def plot_original_and_warped_demo(best_demo, warped_demo, traj):
arms_used = best_demo["arms_used"].value
if arms_used in "lb":
pose_array = conversions.array_to_pose_array(asarray(best_demo["l_gripper_xyzs"]), 'base_footprint')
Globals.handles.append(Globals.rviz.draw_curve(pose_array, rgba = (1,0,0,1),ns = "make_verb_traj_service"))
pose_array = conversions.array_to_pose_array(asarray(warped_demo["l_gripper_xyzs"]), 'base_footprint')
Globals.handles.append(Globals.rviz.draw_curve(pose_array, rgba = (0,1,0,1),ns = "make_verb_traj_service"))
if arms_used in "rb":
pose_array = conversions.array_to_pose_array(asarray(best_demo["r_gripper_xyzs"]), 'base_footprint')
Globals.handles.append(Globals.rviz.draw_curve(pose_array, rgba = (1,0,0,1),ns = "make_verb_traj_service"))
pose_array = conversions.array_to_pose_array(asarray(warped_demo["r_gripper_xyzs"]), 'base_footprint')
Globals.handles.append(Globals.rviz.draw_curve(pose_array, rgba = (0,1,0,1),ns = "make_verb_traj_service"))
Globals.handles.extend(Globals.rviz.draw_trajectory(traj.gripper0_poses, traj.gripper0_angles, ns = "make_verb_traj_service_grippers"))
if arms_used == 'b':
Globals.handles.extend(Globals.rviz.draw_trajectory(traj.gripper1_poses, traj.gripper1_angles, ns = "make_verb_traj_service_grippers"))
for (clouds,rgba) in [(sorted_values(best_demo["object_clouds"]),(1,0,0,.5)),(sorted_values(warped_demo["object_clouds"]),(0,1,0,.5))]:
cloud = []
for subcloud in clouds:
cloud.extend(np.asarray(subcloud).reshape(-1,3))
cloud = np.array(cloud)
cloud = voxel_downsample(cloud, .02)
pose_array = conversions.array_to_pose_array(cloud, 'base_footprint')
Globals.handles.append(Globals.rviz.draw_curve(pose_array, rgba = rgba,width=.01,type=Marker.CUBE_LIST))
def tps_rpm(x_nd, y_md, n_iter = 5, reg_init = .1, reg_final = .001, rad_init = .2, rad_final = .001, plotting = False, verbose=True, f_init = None):
n,d = x_nd.shape
regs = loglinspace(reg_init, reg_final, n_iter)
rads = loglinspace(rad_init, rad_final, n_iter)
f = ThinPlateSpline.identity(d)
for i in xrange(n_iter):
if f.d==2 and i%plotting==0:
import matplotlib.pyplot as plt
plt.clf()
if i==0 and f_init is not None:
xwarped_nd = f_init(x_nd)
print xwarped_nd.max(axis=0)
else:
xwarped_nd = f.transform_points(x_nd)
# targ_nd = find_targets(x_nd, y_md, corr_opts = dict(r = rads[i], p = .1))
corr_nm = calc_correspondence_matrix(xwarped_nd, y_md, r=rads[i], p=.2)
wt_n = corr_nm.sum(axis=1)
targ_nd = np.dot(corr_nm/wt_n[:,None], y_md)
# if plotting:
# plot_correspondence(x_nd, targ_nd)
f.fit(x_nd, targ_nd, regs[i], wt_n = wt_n, angular_spring = regs[i]*200, verbose=verbose)
if plotting and i%plotting==0:
if f.d==2:
plt.plot(x_nd[:,1], x_nd[:,0],'r.')
plt.plot(y_md[:,1], y_md[:,0], 'b.')
pred = f.transform_points(x_nd)
if f.d==2:
plt.plot(pred[:,1], pred[:,0], 'g.')
if f.d == 2:
plot_warped_grid_2d(f.transform_points, x_nd.min(axis=0), x_nd.max(axis=0))
plt.ginput()
elif f.d == 3:
from lfd import warping
from brett2.ros_utils import Marker
from utils import conversions
Globals.setup()
mins = x_nd.min(axis=0)
maxes = x_nd.max(axis=0)
mins[2] -= .1
maxes[2] += .1
Globals.handles = warping.draw_grid(Globals.rviz, f.transform_points, mins, maxes, 'base_footprint', xres=.1, yres=.1)
orig_pose_array = conversions.array_to_pose_array(x_nd, "base_footprint")
target_pose_array = conversions.array_to_pose_array(y_md, "base_footprint")
warped_pose_array = conversions.array_to_pose_array(f.transform_points(x_nd), 'base_footprint')
Globals.handles.append(Globals.rviz.draw_curve(orig_pose_array,rgba=(1,0,0,1),type=Marker.CUBE_LIST))
Globals.handles.append(Globals.rviz.draw_curve(target_pose_array,rgba=(0,0,1,1),type=Marker.CUBE_LIST))
Globals.handles.append(Globals.rviz.draw_curve(warped_pose_array,rgba=(0,1,0,1),type=Marker.CUBE_LIST))
f.corr = corr_nm
return f
def execute(self,userdata):
"""
- move head to the right place
- get a point cloud
returns: success, failure
"""
global HANDLES
HANDLES=[]
# Globals.pr2.head.set_pan_tilt(0, HEAD_TILT)
Globals.pr2.larm.goto_posture('side')
Globals.pr2.rarm.goto_posture('side')
Globals.pr2.join_all()
if HUMAN_GET_ROPE:
xyz,frame = human_get_rope()
xyz = ros_utils.transform_points(xyz, Globals.pr2.tf_listener, "base_footprint", frame)
pose_array = conversions.array_to_pose_array(xyz,"base_footprint")
HANDLES.append(Globals.rviz.draw_curve(pose_array,id=3250864,rgba=(0,0,1,1)))
xyz = curves.unif_resample(xyz, 100,tol=.01)
else:
msg = rospy.wait_for_message("/preprocessor/points", sensor_msgs.msg.PointCloud2)
xyz, rgb = ros_utils.pc2xyzrgb(msg)
xyz = xyz.reshape(-1,3)
xyz = ros_utils.transform_points(xyz, Globals.pr2.tf_listener, "base_footprint", msg.header.frame_id)
userdata.points = xyz
return "success"
def show_objects_and_trajectory(traj_xyz, obj_xyzs, obj_quats, obj_dims, rgba):
for (i,obj_xyz, obj_quat,obj_dim) in zip(itertools.count(), obj_xyzs, obj_quats,obj_dims):
ps = gm.PoseStamped(pose = gm.Pose(
position = gm.Point(*obj_xyz),
orientation = gm.Quaternion(*obj_quat)))
ps.header.frame_id = 'base_footprint'
HANDLES.append(Globals.rviz.draw_marker(
ps,
type=Marker.CUBE,
rgba = rgba,
scale = asarray(obj_dim)))
pose_array = conversions.array_to_pose_array(asarray(traj_xyz), 'base_footprint')
HANDLES.append(Globals.rviz.draw_curve(
pose_array,
rgba = rgba))
def draw_grid(rviz, f, mins, maxes, frame_id, xres = .1, yres = .1, zres = .04):
grid_handles = []
xmin, ymin, zmin = mins
xmax, ymax, zmax = maxes
ncoarse = 10
nfine = 30
xcoarse = np.arange(xmin, xmax, xres)
ycoarse = np.arange(ymin, ymax, yres)
zcoarse = np.arange(zmin, zmax, zres)
xfine = np.linspace(xmin, xmax, nfine)
yfine = np.linspace(ymin, ymax, nfine)
zfine = np.linspace(zmin, zmax, nfine)
lines = []
for x in xcoarse:
for y in ycoarse:
xyz = np.zeros((nfine, 3))
xyz[:,0] = x
xyz[:,1] = y
xyz[:,2] = zfine
lines.append(f(xyz))
for y in ycoarse:
for z in zcoarse:
xyz = np.zeros((nfine, 3))
xyz[:,0] = xfine
xyz[:,1] = y
xyz[:,2] = z
lines.append(f(xyz))
for z in zcoarse:
for x in xcoarse:
xyz = np.zeros((nfine, 3))
xyz[:,0] = x
xyz[:,1] = yfine
xyz[:,2] = z
lines.append(f(xyz))
for line in lines:
grid_handles.append(rviz.draw_curve(conversions.array_to_pose_array(line, frame_id),width=.001,rgba=(1,1,0,1)))
return grid_handles
def show_object_and_trajectory(traj_xyz, obj_xyz, obj_quat, obj_dim, id):
ps = gm.PoseStamped(pose=gm.Pose(position=gm.Point(*obj_xyz), orientation=gm.Quaternion(*obj_quat)))
ps.header.frame_id = "base_footprint"
HANDLES.append(Globals.rviz.draw_marker(ps, type=Marker.CYLINDER, rgba=COLORS[id], scale=asarray(obj_dim)))
pose_array = conversions.array_to_pose_array(asarray(traj_xyz), "base_footprint")
HANDLES.append(Globals.rviz.draw_curve(pose_array, rgba=COLORS[id]))
traj = h5py.File(args.file, 'r')[args.group]
ps = gm.PoseStamped(pose = gm.Pose(
position = gm.Point(*traj["object_pose"]),
orientation = gm.Quaternion(*traj["object_orientation"])))
ps.header.frame_id = 'base_link'
rviz.draw_marker(
ps,
id=1,
type=Marker.CUBE,
rgba = (0,1,0,1),
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)
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"
zsel = xyz[yl:yl+h, xl:xl+w, 2]
mask = (mask%2==1) & np.isfinite(zsel)# & (zsel - table_height > -1)
resp = ProcessCloudResponse()
xyz_sel = xyz[yl:yl+h, xl:xl+w,:][mask.astype('bool')]
rgb_sel = rgb[yl:yl+h, xl:xl+w,:][mask.astype('bool')]
resp.cloud_out = xyzrgb2pc(xyz_sel, rgb_sel, req.cloud_in.header.frame_id)
return resp
if __name__ == "__main__":
if args.test:
if rospy.get_name() == "/unnamed":
rospy.init_node("test_interactive_segmentation_service",disable_signals=True)
listener = tf.TransformListener()
pc = rospy.wait_for_message("/drop/points", sm.PointCloud2)
pc_tf = transformPointCloud2(pc, listener, "base_footprint", pc.header.frame_id)
Globals.setup()
req = ProcessCloudRequest()
req.cloud_in = pc_tf
resp = callback(req)
xyz, rgb = pc2xyzrgb(resp.cloud_out)
pose_array = conversions.array_to_pose_array(xyz.reshape(-1,3), 'base_footprint')
Globals.handles.append(Globals.rviz.draw_curve(pose_array,rgba=(1,0,0,1),type=Marker.CUBE_LIST,width=.002, ns = 'segmentation'))
else:
rospy.init_node("test_interactive_segmentation_service",disable_signals=True)
Globals.setup()
service = rospy.Service("interactive_segmentation", ProcessCloud, callback)
rospy.spin()
from brett2 import ros_utils
import geometry_msgs.msg as gm
import rospy
import numpy as np
rospy.init_node("test_draw_gripper")
from utils.conversions import array_to_pose_array
from brett2.ros_utils import RvizWrapper
rviz = RvizWrapper.create()
rospy.sleep(0.5)
array = np.array([[0, 0, 0], [0.2, 0, 0], [0.4, 0, 0]])
pose_array = array_to_pose_array(array, "base_footprint")
handles = rviz.draw_trajectory(pose_array, [0, 0.04, 0.08], "r")
