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
