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]