def constraints(dofs):
full = (skeleton.constraints(joint, dofs, compliance = 0) +
target.constraints(target.definition, body, dofs )
)
return constraint.merge( full )
def calibrate():
print 'calibrating...'
print 'please stand still'
time.sleep(2)
# wait until we have a skeleton frame
while True:
init_data = payload[0]
if init_data: break
sys.stdout.write('.'); sys.stdout.flush()
time.sleep(1)
print 'ok'
# where to get targets from during calibration
def get_data():
# return payload[0]
return init_data
# calibration target constraints
def init_target_constraints(dofs, **kwargs):
out_data = kwargs.get('out_data', {})
t = target.create( get_data(), adaptor, world )
out_data['targets'] = t
return target.constraints(t, body, dofs, compliance = 1e-4)
h = 1e-2
eps = 1e-2
for d, t in solver.calibration(world, dofs, inertia,
constraint.merge(skeleton_constraints,
init_target_constraints),
dt = h,
eps = eps):
w.targets = t
yield d, t
print i
time.sleep(1)
frame = source['data']
print 'starting calibration'
world = target.World()
adaptor = model.kinect_adaptor(body)
def target_constraints(dofs, **kwargs):
out = kwargs['out_data']
t = target.create( frame, adaptor, world )
out['targets'] = t
return target.constraints(t, body, dofs, compliance = 1e-4)
dt = 5e-3
eps = 1e-2
for d, t in solver.calibration(world, dofs, inertia,
constraint.merge(skeleton_constraints,
target_constraints),
dt = dt,
eps = eps):
pass
calibration_filename = 'calibration.json'
with open(calibration_filename, 'w') as f:
f.write( json.dumps(world, default = lambda o: o.dump()))
print 'wrote', calibration_filename
'y': str(angles[1]),
'z': str(angles[2]),
}
data.append( chunk )
msg = json.dumps( data )
try:
print 'frame', frame
frame += 1
client.write( msg + '\r\n' )
client.flush()
except socket.error:
print 'write error'
break
occulus = threading.Thread(target = send, args = (ready, source) )
occulus.daemon = True
occulus.start()
print 'ik started'
for d, mu in solver.step(dofs, inertia,
constraint.merge(skeleton_constraints,
target_constraints),
dt = dt):
source['dofs'] = np.copy(d).view(dtype = Rigid3)
ready.set()
def source(**kwargs):
dofs = kwargs.get('dofs')
dt = kwargs.get('dt', 1e-1)
eps = kwargs.get('eps', 1e-2)
# don't enable targets just yet to get skeleton in shape
print 'assembling skeleton...'
for d, mu in solver.step(dofs, inertia,
skeleton_constraints,
dt = dt):
yield dofs, None
error = norm(mu) / dt
print error
if error <= eps: break
def calibrate():
print 'calibrating...'
print 'please stand still'
time.sleep(2)
# wait until we have a skeleton frame
while True:
init_data = payload[0]
if init_data: break
sys.stdout.write('.'); sys.stdout.flush()
time.sleep(1)
print 'ok'
# where to get targets from during calibration
def get_data():
# return payload[0]
return init_data
# calibration target constraints
def init_target_constraints(dofs, **kwargs):
out_data = kwargs.get('out_data', {})
t = target.create( get_data(), adaptor, world )
out_data['targets'] = t
return target.constraints(t, body, dofs, compliance = 1e-4)
h = 1e-2
eps = 1e-2
for d, t in solver.calibration(world, dofs, inertia,
constraint.merge(skeleton_constraints,
init_target_constraints),
dt = h,
eps = eps):
w.targets = t
yield d, t
try:
calibration_filename = 'calibration.json'
with open(calibration_filename) as f:
world.load( json.loads(f.read()) )
except:
for d, t in calibrate():
yield d, t
with open(calibration_filename, 'w') as f:
f.write( json.dumps(world, default = lambda o: o.dump()))
print 'wrote', calibration_filename
# then regular ik
print 'ik started'
out_data = {}
for d, mu in solver.step(dofs, inertia,
constraint.merge(skeleton_constraints,
target_constraints),
dt = dt,
out_data = out_data):
t = out_data['targets']
yield d, t
def constraints(dofs):
full = (skeleton.constraints(joint, dofs, compliance=0) +
target.constraints(target.definition, body, dofs))
return constraint.merge(full)