def _look_at(self, x, y):
eye = np.array([x, y, 0])
center = np.array([x, y, 1])
up = np.array([0, -1, 0])
z = normalize(eye - center)
x = normalize(np.cross(up, z))
y = normalize(np.cross(z, x))
rc = np.array([
[x[0], x[1], x[2], np.dot(-x, eye)],
[y[0], -y[1], y[2], np.dot(-y, eye)],
[z[0], z[1], z[2], np.dot(-z, eye)],
[0, 0, 0, 1]]).transpose()
return rc
def calc_hand_pose(lpos, rpos, ori):
ynew0 = normalize(lpos - rpos)
xnew0 = normalize(ori[:,0] - np.dot(ynew0, ori[:,0]) * ynew0)
znew0 = np.cross(xnew0, ynew0)
tool_ori = np.c_[xnew0, ynew0, znew0]
midpt = (lpos + rpos)/2
new_open = np.linalg.norm(lpos - rpos) - 0.030284 # distance between frames when gripper closed
midpt_to_tool = interp_between(new_open, .002,.087, 0.01179, 0.02581)
#np.linalg.norm(((brett.robot.GetLink("r_gripper_r_finger_tip_link").GetTransform()+brett.robot.GetLink("r_gripper_l_finger_tip_link").GetTransform())/2 - brett.robot.GetLink("r_gripper_tool_frame").GetTransform())[:3,3])
tool_pos = tool_ori[:3,0] * midpt_to_tool + midpt
new_pose = np.eye(4)
new_pose[:3,:3] = tool_ori
new_pose[:3,3] = tool_pos
return new_pose, new_open
def plot_problem(self):
figure, plot = self.create_plot()
# Calculate points for the problem
x_points = []
y_points = []
z_points = []
min_index = self.search_range.get_min()
max_index = self.search_range.get_max()
num_points = float(abs(max_index - min_index))/self.plot_resolution
x = np.arange(min_index, max_index, num_points, dtype=float)
y = np.arange(min_index, max_index, num_points, dtype=float)
for idx in itertools.product(*[x,y]):
x_points.append(idx[0])
y_points.append(idx[1])
z_points.append(self.problem.eval(list(idx)))
z_points = np.array(normalize(z_points))
plot.plot_trisurf(x_points,y_points,z_points)
return figure, plot
def make_basis(a,b):
p = normalize(a)
q = normalize(b - np.dot(p,b)*p)
r = np.cross(p,q)
return c_[p,q,r]
