Thursday, January 5, 2012

Pyppet2 - Biped Solver - Part2




def update(self, context):
AbstractArmature.update(self,context)

step_left = step_right = False

if not self.left_foot_loc:
self.left_foot_loc = self.left_foot.shadow_parent.location
step_left = True
if not self.right_foot_loc:
self.right_foot_loc = self.right_foot.shadow_parent.location
step_right = True

x,y,z = self.chest.get_velocity_local()

sideways = None
sideways_rate = abs( x )
if x < -2: sideways = 'RIGHT'
elif x > 2: sideways = 'LEFT'

moving = None
motion_rate = abs( y )
if y < -0.5: moving = 'FORWARD'
elif y > 0.5: moving = 'BACKWARD'

loc,rot,scl = self.pelvis.shadow.matrix_world.decompose()
euler = rot.to_euler()
tilt = sum( [abs(math.degrees(euler.x)), abs(math.degrees(euler.y))] ) / 2.0 # 0-45


x,y,z = self.pelvis.get_location()
current_pelvis_height = z
falling = current_pelvis_height < self.pelvis.rest_height * (1.0-self.standing_height_threshold)

hx,hy,hz = self.head.get_location()
x = (x+hx)/2.0
y = (y+hy)/2.0
ob = self.pelvis.shadow
ob.location = (x,y,-0.5)
loc,rot,scale = ob.matrix_world.decompose()
euler = rot.to_euler()

heading = math.degrees( euler.z )
spin = self.prev_heading - heading
self.prev_heading = heading
turning = None
turning_rate = abs(spin) #/ 360.0
if abs(spin) < 300: # ignore euler flip
if spin < -1.0: turning = 'LEFT'
elif spin > 1.0: turning = 'RIGHT'


if turning == 'LEFT':
if moving == 'BACKWARD':
self.left_foot.shadow.location.x = -(motion_rate * 0.25)
self.right_foot.shadow.location.x = -0.5

elif moving == 'FORWARD':
self.left_foot.shadow.location.x = 0.1
self.right_foot.shadow.location.x = 0.2
if motion_rate > 2:
if random() > 0.8:
step_right = True
self.left_foot.shadow.location.x = -(motion_rate * 0.25)
self.right_foot.shadow.location.x = motion_rate * 0.25

if not step_right and random() > 0.2:
if random() > 0.1: step_left = True
else: step_right = True

elif turning == 'RIGHT':
if moving == 'BACKWARD':
self.right_foot.shadow.location.x = -(motion_rate * 0.25)
self.left_foot.shadow.location.x = -0.5

elif moving == 'FORWARD':
self.right_foot.shadow.location.x = 0.1
self.left_foot.shadow.location.x = 0.2
if motion_rate > 2:
if random() > 0.8:
step_left = True
self.right_foot.shadow.location.x = -(motion_rate * 0.25)
self.left_foot.shadow.location.x = motion_rate * 0.25

if not step_left and random() > 0.2:
if random() > 0.1: step_right = True
else: step_left = True


hand_swing_targets = []
v = self.left_hand.biped_solver['swing-target'].location
hand_swing_targets.append( v )
v.x = -( self.left_foot.biped_solver['target'].location.x )

v = self.right_hand.biped_solver['swing-target'].location
hand_swing_targets.append( v )
v.x = -( self.right_foot.biped_solver['target'].location.x )

v = self.left_foot.biped_solver['target'].location
if v.x < 0: # if foot moving backward only pull on heel/foot
self.left_toe.biped_solver['TARGET'].weight = 0.0
elif v.x > 0: # if foot moving forward only pull on toe
self.left_foot.biped_solver['TARGET'].weight = 0.0

v = self.right_foot.biped_solver['target'].location
if v.x < 0: # if foot moving backward only pull on heel/foot
self.right_toe.biped_solver['TARGET'].weight = 0.0
elif v.x > 0: # if foot moving forward only pull on toe
self.right_foot.biped_solver['TARGET'].weight = 0.0


if moving == 'BACKWARD': # hands forward if moving backwards
for v in hand_swing_targets: v.x += 0.1

if step_left:
rad = euler.z - math.radians(90+self.primary_heading)
cx = math.sin( -rad )
cy = math.cos( -rad )
v = self.left_foot.shadow_parent.location
v.x = x+cx
v.y = y+cy
v.z = .0
self.left_foot_loc = v
if step_right:
rad = euler.z + math.radians(90+self.primary_heading)
cx = math.sin( -rad )
cy = math.cos( -rad )
v = self.right_foot.shadow_parent.location
v.x = x+cx
v.y = y+cy
v.z = .0
self.right_foot_loc = v


#################### falling ####################
if falling:

if current_pelvis_height < 0.2:
for foot in (self.left_foot, self.right_foot):
target = foot.biped_solver[ 'TARGET:pelvis' ]
if target.weight < 50: target.weight += 1.0

foot.add_local_torque( -30, 0, 0 )

else:
for foot in (self.left_foot, self.right_foot):
target = foot.biped_solver[ 'TARGET:pelvis' ]
target.weight *= 0.9


for target in self.foot_solver_targets: # reduce foot step force
target.weight *= 0.9

#for target in self.hand_solver_targets: # increase hand plant force
# if target.weight < self.when_falling_hand_target_goal_weight:
# target.weight += 1

for hand in (self.left_hand, self.right_hand):
self.head.add_local_torque( -self.when_falling_head_curl, 0, 0 )
u = self.when_falling_pull_hands_down_by_tilt_factor * tilt
hand.add_force( 0,0, -u )

x,y,z = hand.get_location()
if z < 0.1:
self.head.add_force(
0,
0,
tilt * self.when_falling_and_hands_down_lift_head_by_tilt_factor
)
hand.add_local_force( 0, -10, 0 )
else:
hand.add_local_force( 0, 3, 0 )

else: # standing

for foot in (self.left_foot, self.right_foot):
target = foot.biped_solver[ 'TARGET:pelvis' ]
target.weight *= 0.9


for target in self.foot_solver_targets:
if target.weight < self.when_standing_foot_target_goal_weight:
target.weight += 1

#for target in self.hand_solver_targets: # reduce hand plant force
# target.weight *= 0.9


head_lift = self.when_standing_head_lift
for toe in ( self.left_toe, self.right_toe ):
x,y,z = toe.get_location()
if z < 0.1:
self.head.add_force( 0,0, head_lift*0.5 )

## lift feet ##
foot = self.left_foot
v1 = foot.get_location().copy()
if v1.z < 0.1: self.head.add_force( 0,0, head_lift*0.5 )

v2 = self.left_foot_loc.copy()
v1.z = .0; v2.z = .0
dist = (v1 - v2).length
if dist > 0.5:
foot.add_force( 0, 0, self.when_standing_foot_step_far_lift)
elif dist < 0.25:
foot.add_force( 0, 0, -self.when_standing_foot_step_near_pull)

foot = self.right_foot
v1 = foot.get_location().copy()
if v1.z < 0.1: self.head.add_force( 0,0, head_lift*0.5 )

v2 = self.right_foot_loc.copy()
v1.z = .0; v2.z = .0
dist = (v1 - v2).length
if dist > 0.5:
foot.add_force( 0, 0, self.when_standing_foot_step_far_lift)
elif dist < 0.25:
foot.add_force( 0, 0, -self.when_standing_foot_step_near_pull)


No comments:

Post a Comment