#
# July 4 2010
#
# Moves body to hold foot fixed during steps.
#
# usage:
#    Create the initial stationary walk in place using
#    walk designer.  Next, set the scale of the figure and
#    adjust the height so that the feet are at ground level.
#    Remove the key frames for orientation and translation except
#    for the first one, which cannot be removed.
#
#    Inverse kinematics must be on for left and right leg.  The
#    script will turn on inverse kinematics when it runs.
#
#    The timing of the walk cycle must be 31 frames per cycle.  The
#    pose must be the same at frames 1,32,63,94, etc. The total
#    number of frames in the walk animation should be
#		f = N*31 + 1
#			where:
#				f = total frames
#				N = number of cycles
#
#    Finally, run this script.  The figure should wak straight 
#    ahead without slipping or sliding of the feet.
#
#    Do not retime the animation until after you have run this
#    script.  The script depends on the walk cycle being exactly
#    31 frames long, which is the natural cycle for walk designer. 
#
import math
import poser
scene = poser.Scene()
figure = scene.CurrentFigure()
figure.SetIkStatus(2,1)
figure.SetIkStatus(3,1)
actor = figure.Actor("Right Foot")
body = figure.Actor("Body")
px_body = body.Parameter("xTran")
py_body = body.Parameter("yTran")
p_hdg = body.Parameter("yRotate")
pz_body = body.Parameter("zTran")
px_actor = actor.Parameter("xTran")
pz_actor = actor.Parameter("zTran")
scale = 21.0
actor = figure.Actor("Body")
pScale = actor.Parameter("Scale")
print "Scale = ",pScale.Value()
scale = pScale.Value()
nf_per_step = 92  # 20x scale
nf_per_step = 31  # normal scale

first_frame = scene.Frame();
NF = scene.NumFrames()
i = first_frame
scene.SetFrame(i)
new_y = py_body.Value()
while i<NF :
	scene.SetFrame(i)
	py_body.SetValue(new_y)
	i = i + 1
scene.SetFrame(0)
def half_step(actor,ef) :
	global scene
	global z_body,nf_per_step
	i = 0
	kf = scene.Frame()
	step = nf_per_step/2 + ef
        while i < step :
		i = i +1
                x_body = px_body.Value()
		z_body = pz_body.Value()
                px_actor = actor.Parameter("xTran")
		pz_actor = actor.Parameter("zTran")
#		print "kf = ",kf+i," z_body before = ",z_body
		z_foot = pz_actor.Value()
		scene.SetFrame(kf+i)
		error = z_foot - pz_actor.Value()
		hdg = p_hdg.Value()/57.297
		x_body = x_body + error*scale*math.sin(hdg)
		z_body = z_body + error*scale*math.cos(hdg)
                px_body.SetValue(x_body)
		pz_body.SetValue(z_body)
#		print "z_body after = ",pz_body.Value()
#		print "z_body,error,error*scale = ",z_body," ",error," ",error*scale


i = 0
NF = scene.NumFrames();

# need to remove keyframes for x,y,z except first one.
actor = figure.Actor("Body")
f=1
while f < NF :
	actor.DeleteKeyFrame(f)
	f = f + 1

NS = (NF-1)/(nf_per_step-1)
print "Number of cycles = ",NS
while i < NS :
	actor = figure.Actor("Right Foot")
	half_step(actor,0)
#	half_step(actor,1)

	actor = figure.Actor("Left Foot")
#	half_step(actor,0)
	half_step(actor,1)
	i = i + 1
actor = figure.Actor("Body")
pScale = actor.Parameter("Scale")
print "Scale = ",pScale.Value()
scene.SetFrame(0)
scene.DrawAll()