def calc_hand_pose(lpos, rpos, ori): """ Do IK on gripper using fingertips lpos: left fingertip position rpos: right fingertip position ori: desired orientation. note that open/close direction is determined by lpos and rpos argument """ 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 calc_hand_pose(lpos, rpos, ori): """ Do IK on gripper using fingertips lpos: left fingertip position rpos: right fingertip position ori: desired orientation. note that open/close direction is determined by lpos and rpos argument """ 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 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]