def make_finger(nameSuffix, mat, L2, L3, L4): i = str(nameSuffix) return JointDOF2('MCP'+i, mat, (-Pi/9, Pi/9), (-Pi/2, 0), [ Joint('PIP'+i, Joint.make_mat((L2, 0, 0), -Pi/6), (-Pi/2, 0), [ Joint('DIP'+i, Joint.make_mat((L3, 0, 0), -Pi/6), (-Pi/2, 0), [ Joint('EP'+i, Joint.make_mat((L3, 0, 0), -Pi/6), None, []) ]) ]) ])
def make_hand(): def make_finger(nameSuffix, mat, L2, L3, L4): i = str(nameSuffix) return JointDOF2('MCP'+i, mat, (-Pi/9, Pi/9), (-Pi/2, 0), [ Joint('PIP'+i, Joint.make_mat((L2, 0, 0), -Pi/6), (-Pi/2, 0), [ Joint('DIP'+i, Joint.make_mat((L3, 0, 0), -Pi/6), (-Pi/2, 0), [ Joint('EP'+i, Joint.make_mat((L3, 0, 0), -Pi/6), None, []) ]) ]) ]) # All lengths are in meters poss = [(), (L1 * scales[1], 0, HB/2), (L1 * scales[2], 0, HB/6), (L1 * scales[3], 0, -HB/6), (L1 * scales[4], 0, -HB/2), ] hand = JointDOF3('W', np.array([ # wrist matrix [1, 0, 0, Larm], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]), None, None, None, [ make_finger(i, Joint.make_mat(poss[i], 0.), L2 * scales[i], L3 * scales[i], L4 * scales[i]) for i in range(1, 5) ]) arm = JointDOF3('ARM', np.array([ [0, 0, -1, .04 - config.x0], [1, 0, 0, .03 - config.y0 - Larm], [0, -1, 0, -config.z0], [0, 0, 0, 1], ]), None, None, None, [hand]) root = arm return root