def get_nearest_ik(db, manip, xyz, quat): manip_name = manip.GetName() group = db.h5file[manip_name] pointing_axis = conversions.quat2mat(quat)[:3,0] i_closest_axis = np.dot(group["pointing_axes"],pointing_axis).argmax() x,y,z = xyz xroot, yroot, zroot = manip.GetBase().GetTransform()[:3,3] xticks = np.asarray(group["xticks"]) yticks = np.asarray(group["yticks"]) zticks = np.asarray(group["zticks"]) xind = int(round((x - xroot - xticks[0])/(xticks[1] - xticks[0]))) yind = int(round((y - yroot - yticks[0])/(yticks[1] - yticks[0]))) zind = int(round((z - zroot - zticks[0])/(zticks[1] - zticks[0]))) #zind = np.searchsorted(self.root["zticks"], z - zroot) + 1 xind = np.clip(xind, 0, xticks.size-1) yind = np.clip(yind, 0, yticks.size-1) zind = np.clip(zind, 0, zticks.size-1) ind_str = "%i_inds"%i_closest_axis i_x, i_y, i_z = group[ind_str][:,xind, yind, zind] assert group[str(i_closest_axis)][i_x, i_y, i_z] > 0 x_feas = xticks[i_x] + xroot y_feas = yticks[i_y] + yroot z_feas = zticks[i_z] + zroot old_mat = conversions.quat2mat(quat) old_x, old_y, old_z = old_mat.T new_x = group["pointing_axes"][i_closest_axis] new_y = np.cross(old_z, new_x) new_z = np.cross(new_x, new_y) pose_mat = np.eye(4) pose_mat[:3,0] = new_x pose_mat[:3,1] = new_y / np.linalg.norm(new_y) pose_mat[:3,2] = new_z / np.linalg.norm(new_z) #pose_mat[:3,:3] = make_orth_basis(new_x) # XXX not sure why it currently sometimes fails without erosion # it always succeeds when I use make_orth_basis # solution should exist regardless of wrist rotation pose_mat[:3,3] = [x_feas, y_feas, z_feas] return manip.FindIKSolution(pose_mat, 2+16)
def get_nearest_ik(db, manip, xyz, quat): manip_name = manip.GetName() group = db.h5file[manip_name] pointing_axis = conversions.quat2mat(quat)[:3, 0] i_closest_axis = np.dot(group["pointing_axes"], pointing_axis).argmax() x, y, z = xyz xroot, yroot, zroot = manip.GetBase().GetTransform()[:3, 3] xticks = np.asarray(group["xticks"]) yticks = np.asarray(group["yticks"]) zticks = np.asarray(group["zticks"]) xind = int(round((x - xroot - xticks[0]) / (xticks[1] - xticks[0]))) yind = int(round((y - yroot - yticks[0]) / (yticks[1] - yticks[0]))) zind = int(round((z - zroot - zticks[0]) / (zticks[1] - zticks[0]))) #zind = np.searchsorted(self.root["zticks"], z - zroot) + 1 xind = np.clip(xind, 0, xticks.size - 1) yind = np.clip(yind, 0, yticks.size - 1) zind = np.clip(zind, 0, zticks.size - 1) ind_str = "%i_inds" % i_closest_axis i_x, i_y, i_z = group[ind_str][:, xind, yind, zind] assert group[str(i_closest_axis)][i_x, i_y, i_z] > 0 x_feas = xticks[i_x] + xroot y_feas = yticks[i_y] + yroot z_feas = zticks[i_z] + zroot old_mat = conversions.quat2mat(quat) old_x, old_y, old_z = old_mat.T new_x = group["pointing_axes"][i_closest_axis] new_y = np.cross(old_z, new_x) new_z = np.cross(new_x, new_y) pose_mat = np.eye(4) pose_mat[:3, 0] = new_x pose_mat[:3, 1] = new_y / np.linalg.norm(new_y) pose_mat[:3, 2] = new_z / np.linalg.norm(new_z) #pose_mat[:3,:3] = make_orth_basis(new_x) # XXX not sure why it currently sometimes fails without erosion # it always succeeds when I use make_orth_basis # solution should exist regardless of wrist rotation pose_mat[:3, 3] = [x_feas, y_feas, z_feas] return manip.FindIKSolution(pose_mat, 2 + 16)
def transform_pose(xyz,quat,F): x,y,z = xyz xp, yp, zp = F.eval(np.r_[x],np.r_[y],np.r_[z]).T jac = F.grad(np.r_[x],np.r_[y],np.r_[z])[0] old_rot_mat = conv.quat2mat(quat) new_rot_mat = np.dot(jac, old_rot_mat) new_rot_mat_orth = np.linalg.qr(new_rot_mat)[0] new_quat = conv.mat2quat(new_rot_mat_orth) return np.r_[xp,yp,zp], new_quat
def transform_pose(xyz, quat, F): x, y, z = xyz xp, yp, zp = F.eval(np.r_[x], np.r_[y], np.r_[z]).T jac = F.grad(np.r_[x], np.r_[y], np.r_[z])[0] old_rot_mat = conv.quat2mat(quat) new_rot_mat = np.dot(jac, old_rot_mat) new_rot_mat_orth = np.linalg.qr(new_rot_mat)[0] new_quat = conv.mat2quat(new_rot_mat_orth) return np.r_[xp, yp, zp], new_quat
def get_warped_spec_pt_traj_mats(demo_spec_pt_traj_mats, demo_to_exp_target_transform): demo_spec_pt_traj_xyzs, demo_spec_pt_traj_oriens = [], [] for demo_spec_pt_traj_mat in demo_spec_pt_traj_mats: demo_spec_pt_traj_xyz, demo_spec_pt_traj_orien = juc.hmat_to_trans_rot(demo_spec_pt_traj_mat) demo_spec_pt_traj_xyzs.append(demo_spec_pt_traj_xyz) demo_spec_pt_traj_oriens.append(juc.quat2mat(demo_spec_pt_traj_orien)) warped_spec_pt_traj_xyzs, warped_spec_pt_traj_oriens = demo_to_exp_target_transform.transform_frames(np.array(demo_spec_pt_traj_xyzs), np.array(demo_spec_pt_traj_oriens)) warped_spec_pt_traj_mats = [juc.trans_rot_to_hmat(warped_spec_pt_traj_xyz, juc.mat2quat(warped_spec_pt_traj_orien)) for warped_spec_pt_traj_xyz, warped_spec_pt_traj_orien in zip(warped_spec_pt_traj_xyzs, warped_spec_pt_traj_oriens)] return warped_spec_pt_traj_mats
def transform_poses(pvec_n7, tps33): x_n, y_n, z_n = pvec_n7[:,:3].T quat_n4 = pvec_n7[:,3:7] xp_n, yp_n, zp_n = tps33.eval(x_n, y_n, z_n).T grad_n33 = tps33.grad(x_n, y_n, z_n) mats_n33 = [conv.quat2mat(quat) for quat in quat_n4] tmats_n33 = [np.dot(g,p) for (g,p) in zip(grad_n33, mats_n33)] tmats_n33 = [np.linalg.qr(mat)[0] for mat in tmats_n33] tquats_n4 = [conv.mat2quat(mat) for mat in tmats_n33] return np.c_[xp_n, yp_n, zp_n, tquats_n4]
def transform_poses(xyzs,quats,F): x,y,z = xyzs.T xyzp = F.eval(x,y,z) jacs = F.grad(x,y,z) new_quats = [] for (quat,jac) in zip(quats,jacs): old_rot_mat = conv.quat2mat(quat) new_rot_mat = np.dot(jac, old_rot_mat) q,r = np.linalg.qr(new_rot_mat.T) new_rot_mat_orth = np.sign(np.diag(r))[:,None]* q.T new_quat = conv.mat2quat(new_rot_mat_orth) new_quats.append(new_quat) return xyzp, np.array(new_quats)
def transform_poses(xyzs, quats, F): x, y, z = xyzs.T xyzp = F.eval(x, y, z) jacs = F.grad(x, y, z) new_quats = [] for (quat, jac) in zip(quats, jacs): old_rot_mat = conv.quat2mat(quat) new_rot_mat = np.dot(jac, old_rot_mat) q, r = np.linalg.qr(new_rot_mat.T) new_rot_mat_orth = np.sign(np.diag(r))[:, None] * q.T new_quat = conv.mat2quat(new_rot_mat_orth) new_quats.append(new_quat) return xyzp, np.array(new_quats)
def get_reachable_region(db, manip, xyz, quat): manip_name = manip.GetName() group = db.h5file[manip_name] pointing_axis = conversions.quat2mat(quat)[:3, 0] i_closest_axis = np.dot(group["pointing_axes"], pointing_axis).argmax() x, y, z = xyz xroot, yroot, zroot = manip.GetBase().GetTransform()[:3, 3] zticks = np.asarray(group["zticks"]) zind = int(round((z - zroot - zticks[0]) / (zticks[1] - zticks[0]))) if zind < 0 or zind >= zticks.size: raise Exception("z value too high") #zind = np.searchsorted(self.root["zticks"], z - zroot) + 1 ee_mask = ra.Grid2(group["xticks"], group["yticks"], group[str(i_closest_axis)][:, :, zind]) base_mask = ee_mask.flip().shift(x - xroot, y - yroot) return base_mask
def get_reachable_region(db, manip, xyz, quat): manip_name = manip.GetName() group = db.h5file[manip_name] pointing_axis = conversions.quat2mat(quat)[:3,0] i_closest_axis = np.dot(group["pointing_axes"],pointing_axis).argmax() x,y,z = xyz xroot, yroot, zroot = manip.GetBase().GetTransform()[:3,3] zticks = np.asarray(group["zticks"]) zind = int(round((z - zroot - zticks[0])/(zticks[1] - zticks[0]))) if zind < 0 or zind >= zticks.size: raise Exception("z value too high") #zind = np.searchsorted(self.root["zticks"], z - zroot) + 1 ee_mask = ra.Grid2(group["xticks"], group["yticks"], group[str(i_closest_axis)][:,:,zind]) base_mask = ee_mask.flip().shift(x - xroot, y - yroot) return base_mask
def apply_tps_transform_to_hmat(tps_transform, hmat): xyz, quat = juc.hmat_to_trans_rot(hmat) warped_xyz, warped_mat = tps_transform.transform_frames(np.array([xyz]), np.array([juc.quat2mat(quat)])) return juc.trans_rot_to_hmat(warped_xyz[0], juc.mat2quat(warped_mat[0]))
def make_traj_multi_stage_do_work(current_stage_info, cur_exp_clouds, clouds_frame_id, stage_num, prev_stage_info, prev_exp_clouds, verb_data_accessor, to_gripper_frame_func=None): arms_used = current_stage_info.arms_used verb_stage_data = verb_data_accessor.get_demo_data(current_stage_info.stage_name) if stage_num == 0: # don't do any extra transformation for the first stage prev_demo_to_exp_grip_transform_lin_rigid = np.identity(4) # no special point translation for first stage since no tool yet special_point_translation = np.identity(4) elif stage_num > 0: # make sure that the tool stage only uses one arm (the one with the tool) assert arms_used in ['r', 'l'] prev_stage_data = verb_data_accessor.get_demo_data(prev_stage_info.stage_name) prev_demo_pc = prev_stage_data["object_clouds"][prev_stage_info.item]["xyz"] prev_exp_pc = prev_exp_clouds[0] prev_demo_pc_down = voxel_downsample(prev_demo_pc, .02) prev_exp_pc_down = voxel_downsample(prev_exp_pc, .02) gripper_data_key = "%s_gripper_tool_frame" % (arms_used) # transform point cloud in base frame to gripper frame # assume right hand has the tool for now # use the last pose of the gripper in the stage to figure out the point cloud of the tool in the gripper frame when the tool was grabbed prev_demo_gripper_pos = prev_stage_data[gripper_data_key]["position"][-1] prev_demo_gripper_orien = prev_stage_data[gripper_data_key]["orientation"][-1] prev_demo_gripper_to_base_transform = juc.trans_rot_to_hmat(prev_demo_gripper_pos, prev_demo_gripper_orien) prev_demo_base_to_gripper_transform = np.linalg.inv(prev_demo_gripper_to_base_transform) prev_demo_pc_in_gripper_frame = np.array([apply_transform(prev_demo_base_to_gripper_transform, point) for point in prev_demo_pc_down]) # get the new point cloud in the new gripper frame # prev_exp_pc_in_gripper_frame = [apply_transform(prev_exp_base_to_gripper_transform, point) for point in prev_exp_pc_down] if to_gripper_frame_func is None: prev_exp_pc_in_gripper_frame = to_gripper_frame_tf_listener(prev_exp_pc_down, gripper_data_key) else: prev_exp_pc_in_gripper_frame = to_gripper_frame_func(prev_exp_pc_down, gripper_data_key) # get the transformation from the new point cloud to the old point cloud for the previous stage prev_demo_to_exp_grip_transform = get_tps_transform(prev_demo_pc_in_gripper_frame, prev_exp_pc_in_gripper_frame) # transforms gripper trajectory point into special point trajectory point if prev_stage_info.special_point is None: # if there is no special point, linearize at origin prev_demo_to_exp_grip_transform_lin_rigid = lin_rigid_tps_transform(prev_demo_to_exp_grip_transform, np.zeros(3)) # don't do a special point translation if there is no specified special point special_point_translation = np.identity(4) else: prev_demo_to_exp_grip_transform_lin_rigid = lin_rigid_tps_transform(prev_demo_to_exp_grip_transform, np.array(prev_stage_info.special_point)) # translation from gripper pose in world frame to special point pose in world frame special_point_translation = jut.translation_matrix(np.array(prev_stage_info.special_point)) if arms_used != 'b': arms_used_list = [arms_used] else: arms_used_list = ['r', 'l'] warped_stage_data = group_to_dict(verb_stage_data) # deep copy it resp = MakeTrajectoryResponse() traj = resp.traj traj.arms_used = arms_used for arm in arms_used_list: gripper_data_key = "%s_gripper_tool_frame" % (arm) # find the special point trajectory before the target transformation cur_demo_gripper_traj_xyzs = verb_stage_data[gripper_data_key]["position"] cur_demo_gripper_traj_oriens = verb_stage_data[gripper_data_key]["orientation"] cur_demo_gripper_traj_mats = [juc.trans_rot_to_hmat(trans, orien) for (trans, orien) in zip(cur_demo_gripper_traj_xyzs, cur_demo_gripper_traj_oriens)] cur_mid_spec_pt_traj_mats = [np.dot(gripper_mat, special_point_translation) for gripper_mat in cur_demo_gripper_traj_mats] # find the transformation from the new special point to the gripper frame cur_exp_inv_special_point_transformation = np.linalg.inv(np.dot(prev_demo_to_exp_grip_transform_lin_rigid, special_point_translation)) # save the demo special point traj for plotting plot_spec_pt_traj = [] for gripper_mat in cur_demo_gripper_traj_mats: spec_pt_xyz, spec_pt_orien = juc.hmat_to_trans_rot(np.dot(gripper_mat, special_point_translation)) plot_spec_pt_traj.append(spec_pt_xyz) print 'grip transform:' print prev_demo_to_exp_grip_transform_lin_rigid print 'special point translation:' print special_point_translation print 'inverse special point translation:' print cur_exp_inv_special_point_transformation # find the target transformation for the experiment scene demo_object_clouds = [verb_stage_data["object_clouds"][obj_name]["xyz"] for obj_name in verb_stage_data["object_clouds"].keys()] if len(demo_object_clouds) > 1: raise Exception("i don't know what to do with multiple object clouds") x_nd = voxel_downsample(demo_object_clouds[0], .02) y_md = voxel_downsample(cur_exp_clouds[0], .02) # transformation from old target object to new target object in world frame cur_demo_to_exp_transform = get_tps_transform(x_nd, y_md) # apply the target warping transformation to the special point trajectory cur_mid_spec_pt_traj_xyzs, cur_mid_spec_pt_traj_oriens = [], [] for cur_mid_spec_pt_traj_mat in cur_mid_spec_pt_traj_mats: cur_mid_spec_pt_traj_xyz, cur_mid_spec_pt_traj_orien = juc.hmat_to_trans_rot(cur_mid_spec_pt_traj_mat) cur_mid_spec_pt_traj_xyzs.append(cur_mid_spec_pt_traj_xyz) cur_mid_spec_pt_traj_oriens.append(juc.quat2mat(cur_mid_spec_pt_traj_orien)) cur_exp_spec_pt_traj_xyzs, cur_exp_spec_pt_traj_oriens = cur_demo_to_exp_transform.transform_frames(np.array(cur_mid_spec_pt_traj_xyzs), np.array(cur_mid_spec_pt_traj_oriens)) plot_warped_spec_pt_traj = cur_exp_spec_pt_traj_xyzs #save the special point traj for plotting cur_exp_spec_pt_traj_mats = [juc.trans_rot_to_hmat(cur_exp_spec_pt_traj_xyz, mat2quat(cur_exp_spec_pt_traj_orien)) for cur_exp_spec_pt_traj_xyz, cur_exp_spec_pt_traj_orien in zip(cur_exp_spec_pt_traj_xyzs, cur_exp_spec_pt_traj_oriens)] # transform the warped special point trajectory back to a gripper trajectory in the experiment cur_exp_gripper_traj_mats = [np.dot(spec_pt_mat, cur_exp_inv_special_point_transformation) for spec_pt_mat in cur_exp_spec_pt_traj_mats] warped_stage_data[gripper_data_key]["position"] = [] warped_stage_data[gripper_data_key]["orientation"] = [] for exp_traj_mat in cur_exp_gripper_traj_mats: warped_pos, warped_orien = juc.hmat_to_trans_rot(exp_traj_mat) warped_stage_data[gripper_data_key]["position"].append(warped_pos) warped_stage_data[gripper_data_key]["orientation"].append(warped_orien) if arm == 'r': traj.r_gripper_poses.poses = xyzs_quats_to_poses(warped_stage_data[gripper_data_key]["position"], warped_stage_data[gripper_data_key]["orientation"]) print "poses: ", len(traj.r_gripper_poses.poses) traj.r_gripper_angles = warped_stage_data["r_gripper_joint"] traj.r_gripper_poses.header.frame_id = clouds_frame_id elif arm == 'l': traj.l_gripper_poses.poses = xyzs_quats_to_poses(warped_stage_data[gripper_data_key]["position"], warped_stage_data[gripper_data_key]["orientation"]) print "poses: ", len(traj.l_gripper_poses.poses) traj.l_gripper_angles = warped_stage_data["l_gripper_joint"] traj.l_gripper_poses.header.frame_id = clouds_frame_id Globals.handles = [] plot_original_and_warped_demo_and_spec_pt(verb_stage_data, warped_stage_data, plot_spec_pt_traj, plot_warped_spec_pt_traj, 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