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]
