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")