def setup_solver(solver): Pi = np.pi def get_pos(stringIdx, fretIdx, z=0.): fp = FretPos(stringIdx, fretIdx) x, y = fretSp.pos_fret_to_plane(fp) return np.dot(fretSp.localMat, [x, y, z, 1])[:3] X1 = [ ('EP1', (6, 1)), ('EP2', (2, 2)), ('EP3', (3, 3)), ('EP4', (1, 3)), ] Am = [ ('EP1', (2, 1)), ('EP2', (4, 2)), ('EP3', (3, 2)), ] F = [ ('EP1', (6, 1)), ('EP2', (3, 2)), ('EP3', (5, 3)), ('EP4', (4, 3)), ] C = [ ('EP1', (2, 1)), ('EP2', (4, 2)), ('EP3', (5, 3)), ] bar = 0 C7 = [ ('EP1', (2, bar+1)), ('EP2', (4, bar+2)), ('EP3', (5, bar+3)), ('EP4', (1, bar+3)), ] G = [ ('EP3', (6, 3)), ('EP2', (5, 2)), ('EP4', (1, 3)), ] Em = [ ('EP2', (5, 2)), ('EP3', (4, 2)), ] Dm = [ ('EP1', (1, 1)), ('EP2', (3, 2)), ('EP4', (2, 3)), ('EP3', (4, 3)), ] GB = [ ('EP1', (6, 3)), ('EP4', (1, 7)), ] B7 = [ ('EP1', (4, 1)), ('EP2', (5, 2)), ('EP3', (3, 2)), ('EP4', (1, 2)), ] A = [('EP1', (4, 2)),('EP2', (3, 2)),('EP3', (2, 2))] # Choose an arrangement arrangement = A fretSp.set_marks([FretPos(i, j) for (_, (i, j)) in arrangement]) # Prepare the solver solver.set_joints(model.root) solver.set_target_pos([(f, get_pos(i, j)) for (f, (i, j)) in arrangement]) for (f, (i, j)) in arrangement: joint = Joint.get(f) s = model.scales[['EP0', 'EP1', 'EP2', 'EP3', 'EP4'].index(f)] pos = get_pos(i, j, s * model.L4) solver.set_constaints([ RefPosConstraint(joint.parent, pos, .8) ]) # define constraints solver.set_constaints([ # RefAngleConstraint(Joint.get('MCP1z'), -Pi/6, 1), # RefAngleConstraint(Joint.get('MCP2z'), -Pi/6, 1), # RefAngleConstraint(Joint.get('MCP3z'), -Pi/6, 1), # RefAngleConstraint(Joint.get('MCP4z'), -Pi/6, 1), RefAngleConstraint(Joint.get('MCP1y'), -Pi/8, 1), RefAngleConstraint(Joint.get('MCP2y'), 0, 1), RefAngleConstraint(Joint.get('MCP3y'), 0, 1), RefAngleConstraint(Joint.get('MCP4y'), Pi/8, 1), ]) return solver
def set_target_pos(self, jointPosPairs): """ @param jointPosPairs: [(str, Pos3D)], a list of (jointName, effectorPos) pairs. """ self.m = m = 3 * len(jointPosPairs) self.targetPoss = T = np.zeros(m, dtype=np.double) for i, (jname, pos) in enumerate(jointPosPairs): self.targets.append(Joint.get(jname)) T[3 * i : 3 * i + 3] = pos