diff --git a/tactile/code/fingers_sim/pinky_sim.py b/tactile/code/fingers_sim/pinky_sim.py new file mode 100644 index 00000000..dcee23b4 --- /dev/null +++ b/tactile/code/fingers_sim/pinky_sim.py @@ -0,0 +1,106 @@ +import numpy as np +from math import pi, cos, sin + +def actuation_pinky(distance, q, links_lenghts, R): + q1 = q[0] + q2 = q[1] + q3 = q[2] + + offset_pinky_x = links_lenghts[0] + offset_pinky_y = links_lenghts[1] + offset_pinky_z = links_lenghts[2] + L1y_pinky = links_lenghts[3] + L1z_pinky = links_lenghts[4] + L2y_pinky = links_lenghts[5] + L2z_pinky = links_lenghts[6] + L3y_pinky = links_lenghts[7] + L3z_pinky = links_lenghts[8] + + #print(np.linalg.norm(current_pose - np.array([0, 0, 1]))) + + py_j2 = - L1y_pinky*cos(q1) - L1z_pinky*sin(q1) + pz_j2 = - L1y_pinky*sin(q1) + L1z_pinky*cos(q1) + + py_j3 = py_j2 - L2y_pinky*cos(q1+q2) - L2z_pinky*sin(q1+q2) + pz_j3 = pz_j2 - L2y_pinky*sin(q1+q2) + L2z_pinky*cos(q1+q2) + + py_tip = py_j3 - L3y_pinky*cos(q1+q2+q3) - L3z_pinky*sin(q1+q2+q3) + pz_tip = pz_j3 - L3y_pinky*sin(q1+q2+q3) + L3z_pinky*cos(q1+q2+q3) + + pos = np.array([[0], [py_tip], [pz_tip + 0.03]]) + + p = R.dot(pos) + np.array([[offset_pinky_x], [offset_pinky_y], [offset_pinky_z]]) + # Task Jacobian: p_dot = Jp(q)q_dot + Jp = np.array([[0, 0, 0], + [L1y_pinky*sin(q1) - L1z_pinky*cos(q1) + L2y_pinky*sin(q1+q2) - L2z_pinky*cos(q1+q2) + L3y_pinky*sin(q1+q2+q3) - L3z_pinky*cos(q1+q2+q3), + L2y_pinky*sin(q1+q2) - L2z_pinky*cos(q1+q2) + L3y_pinky*sin(q1+q2+q3) - L3z_pinky*cos(q1+q2+q3), + L3y_pinky*sin(q1+q2+q3) - L3z_pinky*cos(q1+q2+q3)], + [-L1y_pinky*cos(q1) - L1z_pinky*sin(q1) - L2y_pinky*cos(q1+q2) - L2z_pinky*sin(q1+q2) - L3y_pinky*cos(q1+q2+q3) - L3z_pinky*sin(q1+q2+q3), + -L2y_pinky*cos(q1+q2) - L2z_pinky*sin(q1+q2) - L3y_pinky*cos(q1+q2+q3) - L3z_pinky*sin(q1+q2+q3), + -L3y_pinky*cos(q1+q2+q3) - L3z_pinky*sin(q1+q2+q3)]]) + + Jp = R.dot(Jp) + # Distance from origin d + d = np.linalg.norm(p) + + W = [[6.5, 0, 0], + [0, 5.5, 0], + [0, 0, 7.5]] + + #print(d - distance) + + # Extended jacobian for distance: d_dot = Jd(q)q_dot + Jd = 1/d*p.transpose().dot(Jp) + + W_inv = np.linalg.inv(W) + + Jd_trans = Jd.transpose() + detJ = np.float64(Jd.dot(W_inv.dot(Jd_trans))) + if detJ <= 1e-4: + mu = (detJ + 1.0)/1000 + else: + mu = 0 + Jinv = W_inv.dot(Jd_trans).dot(1/(detJ + mu**2)) + + # Proportional Gain + K = 100 + + # Distance measured from WeArt + dr = distance + + w = np.array([(5*pi/16 - q1)/(3*pi/8), (3*pi/8 - q2)/(pi/4), (5*pi/16 - q3)/(3*pi/8)]) + + u_vinc = np.reshape((np.eye(3) - Jinv.dot(Jd)).dot(w.T),[3,1]) + qdot = Jinv*K*(dr - d) + u_vinc + + return qdot + +def move_pinky(distance,data,model,joint_ids,palm,links,R, hand): + off = 7 if hand=="Right_" else 14 + q1 = data.qpos[joint_ids[hand+'Pinky_J1']+off] + q2 = data.qpos[joint_ids[hand+'Pinky_J2']+off] + q3 = data.qpos[joint_ids[hand+'Pinky_J3']+off] + + L1y_pinky = links[0] + L1z_pinky = links[1] + L2y_pinky = links[2] + L2z_pinky = links[3] + L3y_pinky = links[4] + L3z_pinky = links[5] + + offset_pinky = data.xpos[model.body(hand+'Pinky_J1.stl').id] + offx_pinky = offset_pinky[0] - palm[0] + offy_pinky = offset_pinky[1] - palm[1] + offz_pinky = offset_pinky[2] - palm[2] + + links = [offx_pinky, offy_pinky, offz_pinky, L1y_pinky, L1z_pinky, L2y_pinky, L2z_pinky, L3y_pinky, L3z_pinky] + + qdot = actuation_pinky(distance, [q1, q2, q3], links, R) + + v1 = qdot[0][0] + v2 = qdot[1][0] + v3 = qdot[2][0] + + data.ctrl[joint_ids[hand+'Pinky_J1']] = v1 + data.ctrl[joint_ids[hand+'Pinky_J2']] = v2 + data.ctrl[joint_ids[hand+'Pinky_J3']] = v3 \ No newline at end of file diff --git a/tactile/code/fingers_sim/thumb_sim.py b/tactile/code/fingers_sim/thumb_sim.py new file mode 100644 index 00000000..f658fa98 --- /dev/null +++ b/tactile/code/fingers_sim/thumb_sim.py @@ -0,0 +1,130 @@ +import numpy as np +from math import pi, cos, sin + +def actuation_thumb(distance, q, links_lenghts, R, hand_side): + q1 = q[0] + q2 = q[1] + + offset_thumb_x = links_lenghts[0] + offset_thumb_y = links_lenghts[1] + offset_thumb_z = links_lenghts[2] + L2x_thumb = links_lenghts[3] + L2y_thumb = links_lenghts[4] + L3x_thumb = links_lenghts[5] + L3y_thumb = links_lenghts[6] + + px_j2 = L2x_thumb*cos(q1) - L2y_thumb*sin(q1) + py_j2 = - L2x_thumb*sin(q1) - L2y_thumb*cos(q1) + + px_tip = px_j2 + L3x_thumb*cos(q1+q2) - L3y_thumb*sin(q1+q2) + py_tip = py_j2 - L3x_thumb*sin(q1+q2) - L3y_thumb*cos(q1+q2) + + pos = np.array([[px_tip + 0.05], [py_tip], [0]]) + pos *= hand_side + + p = R.dot(pos) + np.array([[offset_thumb_x], [offset_thumb_y], [offset_thumb_z]]) + + # Task Jacobian: p_dot = Jp(q)q_dot + Jp = np.array([[- L2x_thumb*sin(q1) - L2y_thumb*cos(q1) - L3x_thumb*sin(q1+q2) - L3y_thumb*cos(q1+q2), + - L3x_thumb*sin(q1+q2) - L3y_thumb*cos(q1+q2)], + [- L2x_thumb*cos(q1) - L2y_thumb*sin(q1) - L3x_thumb*cos(q1+q2) + L3y_thumb*sin(q1+q2), + - L3x_thumb*cos(q1+q2) + L3y_thumb*sin(q1+q2)], + [0,0]]) + + Jp = R.dot(Jp) + Jp *= hand_side + # Distance from origin d + d = np.linalg.norm(p) + + W = [[0.5, 0], + [0, 0.5]] + + + + # Extended jacobian for distance: d_dot = Jd(q)q_dot + Jd = 1/d*p.transpose().dot(Jp) + + W_inv = np.linalg.inv(W) + + Jd_trans = Jd.transpose() + detJ = np.float64(Jd.dot(W_inv.dot(Jd_trans))) + if detJ <= 1e-3: + mu = (detJ + 1.0)/20 + # print("sing", detJ) + else: + mu = 0 + Jinv = W_inv.dot(Jd_trans) + Jinv = Jinv * (1/(detJ + mu**2)) + + #print(d, distance) + + # Proportional Gain + K = 100 + + # Distance measured from WeArt + dr = distance + + w = np.array([(pi/4 - q1)/(pi/2), (pi/4 - q2)/(pi/2)]) + + u_vinc = np.reshape((np.eye(2) - Jinv.dot(Jd)).dot(w.T),[2,1]) + qdot = Jinv*K*(dr - d) + u_vinc + # print((qdot)) + return qdot + +def actuation_thumb_abd(abduction, q1,Ly,Lz): + + z = - Ly*sin(q1) + Lz*cos(q1)-0.01 + Jz = -Ly*cos(q1)-Lz*sin(q1) + + detJ = Jz**2 + if detJ <= 1e-3: + mu = (detJ + 1.0)/50 + else: + mu = 0 + Jinv = Jz/(detJ + mu**2) + + # Proportional Gain + K = 50 + + # Distance measured from WeArt + dr_dot = 0 + + qdot = Jinv*( dr_dot + K*(abduction - z)) + + return qdot + +def move_thumb(distance,abduction,data,model,joint_ids,palm,links,R, hand_side): + hand = 'Right_' if hand_side==1 else 'Left_' + + off = 7 if hand=="Right_" else 14 + q1 = data.qpos[joint_ids[hand+'Thumb_J2']+off] + q2 = data.qpos[joint_ids[hand+'Thumb_J3']+off] + + L2x_thumb = links[0] + L2y_thumb = links[1] + L3x_thumb = links[2] + L3y_thumb = links[3] + + + q = data.qpos[joint_ids[hand+'Thumb_J1']+off] + Ly = links[4] + Lz = links[5] + + offset_thumb = data.xpos[model.body(hand+'Thumb_J1.stl').id] + offx_thumb = offset_thumb[0] - palm[0] + offy_thumb = offset_thumb[1] - palm[1] + offz_thumb = offset_thumb[2] - palm[2] + + links = [offx_thumb, offy_thumb, offz_thumb, L2x_thumb, L2y_thumb, L3x_thumb, L3y_thumb] + + Rq = np.array([[1,0,0],[0, cos(q), -sin(q)],[0,sin(q),cos(q)]]) + R = R.dot(Rq) + qdot = actuation_thumb(distance, [q1, q2], links,R, hand_side) + + v1 = qdot[0][0] + v2 = qdot[1][0] + + #print(abduction) + data.ctrl[joint_ids[hand + 'Thumb_J1']] = actuation_thumb_abd(abduction, q,Ly,Lz) + data.ctrl[joint_ids[hand + 'Thumb_J2']] = v1 + data.ctrl[joint_ids[hand + 'Thumb_J3']] = v2 \ No newline at end of file