Friday, December 16, 2011

Pyppet2 - Biped Solver - Part1



http://pyppet.googlecode.com/files/pyppet-1.9.2f.tar.bz2


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

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

x1,y1,z1 = self.pelvis.get_location()
current_pelvis_height = z1
x2,y2,z2 = self.head.get_location()
x = (x1+x2)/2.0
y = (y1+y2)/2.0
ob = self.pelvis.shadow
ob.location = (x,y,0)
loc,rot,scale = ob.matrix_world.decompose()
euler = rot.to_euler()

rad = euler.z - math.radians(90)
cx = math.sin( -rad )
cy = math.cos( -rad )
if not self.left_foot_loc or random() > 0.9:
v = self.left_foot.shadow.location
v.x = x+cx
v.y = y+cy
v.z = .0
self.left_foot_loc = v

rad = euler.z + math.radians(90)
cx = math.sin( -rad )
cy = math.cos( -rad )
if not self.right_foot_loc or random() > 0.9:
v = self.right_foot.shadow.location
v.x = x+cx
v.y = y+cy
v.z = .0
self.right_foot_loc = v

Updates foot targets, and measures tilt of head/pelvis offset.



## falling ##
if current_pelvis_height < self.pelvis.rest_height * (1.0-self.standing_height_threshold):

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 )

If falling pull hands down to break fall, then try to sit up.




else: # standing
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


## lift feet ##
head_lift = self.when_standing_head_lift

foot = self.left_foot
v1 = foot.get_location().copy()
if v1.z < 0.1: self.head.add_force( 0,0, head_lift )
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)
#self.pelvis.add_force( 0,0, -head_lift*0.25 )
elif dist < 0.25:
foot.add_force( 0, 0, -self.when_standing_foot_step_near_pull)
#self.head.add_force( 0,0, head_lift )

foot = self.right_foot
v1 = foot.get_location().copy()
if v1.z < 0.1: self.head.add_force( 0,0, head_lift )
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)
#self.pelvis.add_force( 0,0, -head_lift*0.25 )
elif dist < 0.25:
foot.add_force( 0, 0, -self.when_standing_foot_step_near_pull)
#self.head.add_force( 0,0, head_lift )




If standing take step to next foot target, and lift head if foot is touching the ground.

No comments:

Post a Comment