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_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 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
