def simulateCallback(frame):
# seginfo
segIndex = seg_index[0]
curState = seginfo[segIndex]['state']
curInterval = yma.offsetInterval(acc_offset[0], seginfo[segIndex]['interval'])
stanceLegs = seginfo[segIndex]['stanceHips']
swingLegs = seginfo[segIndex]['swingHips']
stanceFoots = seginfo[segIndex]['stanceFoots']
swingFoots = seginfo[segIndex]['swingFoots']
swingKnees = seginfo[segIndex]['swingKnees']
groundHeight = seginfo[segIndex]['ground_height']
maxStfPushFrame = seginfo[segIndex]['max_stf_push_frame']
prev_frame = frame-1 if frame>0 else 0
# prev_frame = frame
# information
# dCM_tar = yrp.getCM(motion_seg.getJointVelocitiesGlobal(frame), bodyMasses, upperMass, uppers)
# CM_tar = yrp.getCM(motion_seg.getJointPositionsGlobal(frame), bodyMasses, upperMass, uppers)
## dCM_tar = yrp.getCM(motion_seg.getJointVelocitiesGlobal(frame), bodyMasses, totalMass)
## CM_tar = yrp.getCM(motion_seg.getJointPositionsGlobal(frame), bodyMasses, totalMass)
# stf_tar = motion_seg.getJointPositionGlobal(stanceFoots[0], frame)
# CMr_tar = CM_tar - stf_tar
dCM_tar = motion_seg.getJointVelocityGlobal(0, prev_frame)
CM_tar = motion_seg.getJointPositionGlobal(0, prev_frame)
# dCM_tar = yrp.getCM(motion_seg.getJointVelocitiesGlobal(prev_frame), bodyMasses, upperMass, uppers)
# CM_tar = yrp.getCM(motion_seg.getJointPositionsGlobal(prev_frame), bodyMasses, upperMass, uppers)
# dCM_tar = yrp.getCM(motion_seg.getJointVelocitiesGlobal(prev_frame), bodyMasses, totalMass)
# CM_tar = yrp.getCM(motion_seg.getJointPositionsGlobal(prev_frame), bodyMasses, totalMass)
stf_tar = motion_seg.getJointPositionGlobal(stanceFoots[0], prev_frame)
CMr_tar = CM_tar - stf_tar
dCM = avg_dCM[0]
CM = controlModel.getJointPositionGlobal(0)
# CM = yrp.getCM(controlModel.getJointPositionsGlobal(), bodyMasses, upperMass, uppers)
# CM = yrp.getCM(controlModel.getJointPositionsGlobal(), bodyMasses, totalMass)
CMreal = yrp.getCM(controlModel.getJointPositionsGlobal(), bodyMasses, totalMass)
stf = controlModel.getJointPositionGlobal(stanceFoots[0])
CMr = CM - stf
diff_dCM = mm.projectionOnPlane(dCM-dCM_tar, (1,0,0), (0,0,1))
diff_dCM_axis = np.cross((0,1,0), diff_dCM)
rd_vec1[0] = diff_dCM; rd_vecori1[0] = CM_tar
diff_CMr = mm.projectionOnPlane(CMr-CMr_tar, (1,0,0), (0,0,1))
# rd_vec1[0] = diff_CMr; rd_vecori1[0] = stf_tar
diff_CMr_axis = np.cross((0,1,0), diff_CMr)
direction = mm.normalize2(mm.projectionOnPlane(dCM_tar, (1,0,0), (0,0,1)))
# direction = mm.normalize2(mm.projectionOnPlane(dCM, (1,0,0), (0,0,1)))
directionAxis = np.cross((0,1,0), direction)
diff_dCM_sag, diff_dCM_cor = mm.projectionOnVector2(diff_dCM, direction)
# rd_vec1[0] = diff_dCM_sag; rd_vecori1[0] = CM_tar
diff_dCM_sag_axis = np.cross((0,1,0), diff_dCM_sag)
diff_dCM_cor_axis = np.cross((0,1,0), diff_dCM_cor)
diff_CMr_sag, diff_CMr_cor = mm.projectionOnVector2(diff_CMr, direction)
diff_CMr_sag_axis = np.cross((0,1,0), diff_CMr_sag)
diff_CMr_cor_axis = np.cross((0,1,0), diff_CMr_cor)
t = (frame-curInterval[0])/float(curInterval[1]-curInterval[0])
t_raw = t
if t>1.: t=1.
p_root = motion_stitch[frame].getJointPositionGlobal(0)
R_root = motion_stitch[frame].getJointOrientationGlobal(0)
motion_seg_orig.goToFrame(frame)
motion_seg.goToFrame(frame)
motion_stitch.goToFrame(frame)
motion_debug1.append(motion_stitch[frame].copy())
motion_debug1.goToFrame(frame)
motion_debug2.append(motion_stitch[frame].copy())
motion_debug2.goToFrame(frame)
motion_debug3.append(motion_stitch[frame].copy())
motion_debug3.goToFrame(frame)
# paper implementation
M_tc.append(motion_stitch[prev_frame])
M_tc.goToFrame(frame)
P_hat.append(M_tc[frame].copy())
P_hat.goToFrame(frame)
p_temp = ym.JointPosture(skeleton)
p_temp.rootPos = controlModel.getJointPositionGlobal(0)
p_temp.setJointOrientationsLocal(controlModel.getJointOrientationsLocal())
P.append(p_temp)
P.goToFrame(frame)
# stance foot stabilize
motion_stf_stabilize.append(motion_stitch[frame].copy())
motion_stf_stabilize.goToFrame(frame)
if STANCE_FOOT_STABILIZE:
for stanceFoot in stanceFoots:
R_target_foot = motion_seg[frame].getJointOrientationGlobal(stanceFoot)
R_current_foot = motion_stf_stabilize[frame].getJointOrientationGlobal(stanceFoot)
motion_stf_stabilize[frame].setJointOrientationGlobal(stanceFoot, cm.slerp(R_current_foot, R_target_foot , stf_stabilize_func(t)))
# R_target_foot = motion_seg[frame].getJointOrientationLocal(stanceFoot)
# R_current_foot = motion_stf_stabilize[frame].getJointOrientationLocal(stanceFoot)
# motion_stf_stabilize[frame].setJointOrientationLocal(stanceFoot, cm.slerp(R_current_foot, R_target_foot , stf_stabilize_func(t)))
# match stance leg
motion_match_stl.append(motion_stf_stabilize[frame].copy())
motion_match_stl.goToFrame(frame)
if MATCH_STANCE_LEG:
if curState!=yba.GaitState.STOP:
for i in range(len(stanceLegs)):
stanceLeg = stanceLegs[i]
stanceFoot = stanceFoots[i]
# # motion stance leg -> character stance leg as time goes
R_motion = motion_match_stl[frame].getJointOrientationGlobal(stanceLeg)
R_character = controlModel.getJointOrientationGlobal(stanceLeg)
motion_match_stl[frame].setJointOrientationGlobal(stanceLeg, cm.slerp(R_motion, R_character, match_stl_func(t)))
# t_y = match_stl_func_y(t)
# t_xz = match_stl_func(t)
#
# R_motion = motion_match_stl[frame].getJointOrientationGlobal(stanceLeg)
# R_character = controlModel.getJointOrientationGlobal(stanceLeg)
# R = np.dot(R_character, R_motion.T)
# R_y, R_xz = mm.projectRotation((0,1,0), R)
# motion_match_stl[frame].mulJointOrientationGlobal(stanceLeg, mm.scaleSO3(R_xz, t_xz))
# motion_match_stl[frame].mulJointOrientationGlobal(stanceLeg, mm.scaleSO3(R_y, t_y))
# swing foot placement
motion_swf_placement.append(motion_match_stl[frame].copy())
motion_swf_placement.goToFrame(frame)
if SWING_FOOT_PLACEMENT:
t_swing_foot_placement = swf_placement_func(t);
if extended[0]:
R_swp_sag = prev_R_swp[0][0]
R_swp_cor = prev_R_swp[0][1]
else:
R_swp_sag = mm.I_SO3(); R_swp_cor = mm.I_SO3()
R_swp_sag = np.dot(R_swp_sag, mm.exp(diff_dCM_sag_axis * K_swp_vel_sag * -t_swing_foot_placement))
R_swp_cor = np.dot(R_swp_cor, mm.exp(diff_dCM_cor_axis * K_swp_vel_cor * -t_swing_foot_placement))
if np.dot(direction, diff_CMr_sag) < 0:
R_swp_sag = np.dot(R_swp_sag, mm.exp(diff_CMr_sag_axis * K_swp_pos_sag * -t_swing_foot_placement))
R_swp_cor = np.dot(R_swp_cor, mm.exp(diff_CMr_cor_axis * K_swp_pos_cor * -t_swing_foot_placement))
for i in range(len(swingLegs)):
swingLeg = swingLegs[i]
swingFoot = swingFoots[i]
# save swing foot global orientation
# R_swf = motion_swf_placement[frame].getJointOrientationGlobal(swingFoot)
# rotate swing leg
motion_swf_placement[frame].mulJointOrientationGlobal(swingLeg, R_swp_sag)
motion_swf_placement[frame].mulJointOrientationGlobal(swingLeg, R_swp_cor)
# restore swing foot global orientation
# motion_swf_placement[frame].setJointOrientationGlobal(swingFoot, R_swf)
prev_R_swp[0] = (R_swp_sag, R_swp_cor)
# swing foot height
motion_swf_height.append(motion_swf_placement[frame].copy())
motion_swf_height.goToFrame(frame)
if SWING_FOOT_HEIGHT:
for swingFoot in swingFoots:
stanceFoot = stanceFoots[0]
# save foot global orientation
R_foot = motion_swf_height[frame].getJointOrientationGlobal(swingFoot)
R_stance_foot = motion_swf_height[frame].getJointOrientationGlobal(stanceFoot)
height_tar = motion_swf_height[frame].getJointPositionGlobal(swingFoot)[1] - motion_swf_height[frame].getJointPositionGlobal(stanceFoot)[1]
# motion_debug1[frame] = motion_swf_height[frame].copy()
# rotate
motion_swf_height[frame].rotateByTarget(controlModel.getJointOrientationGlobal(0))
# motion_debug2[frame] = motion_swf_height[frame].copy()
# motion_debug2[frame].translateByTarget(controlModel.getJointPositionGlobal(0))
height_cur = motion_swf_height[frame].getJointPositionGlobal(swingFoot)[1] - motion_swf_height[frame].getJointPositionGlobal(stanceFoot)[1]
offset_height = (height_tar - height_cur) * swf_height_func(t) * c_swf_stability
offset_sine = c_swf_mid_offset * swf_height_sine_func(t)
offset = 0.
offset += offset_height
offset += offset_sine
if offset > 0.:
newPosition = motion_swf_height[frame].getJointPositionGlobal(swingFoot)
newPosition[1] += offset
aik.ik_analytic(motion_swf_height[frame], swingFoot, newPosition)
else:
newPosition = motion_swf_height[frame].getJointPositionGlobal(stanceFoot)
newPosition[1] -= offset
aik.ik_analytic(motion_swf_height[frame], stanceFoot, newPosition)
# return
# motion_debug3[frame] = motion_swf_height[frame].copy()
# motion_debug3[frame].translateByTarget(controlModel.getJointPositionGlobal(0))
motion_swf_height[frame].rotateByTarget(R_root)
# restore foot global orientation
motion_swf_height[frame].setJointOrientationGlobal(swingFoot, R_foot)
motion_swf_height[frame].setJointOrientationGlobal(stanceFoot, R_stance_foot)
if plot!=None:
plot.addDataPoint('debug1', frame, height_tar)
plot.addDataPoint('debug2', frame, height_cur)
# plot.addDataPoint('diff', frame, diff)
# swing foot orientation
motion_swf_orientation.append(motion_swf_height[frame].copy())
motion_swf_orientation.goToFrame(frame)
if SWING_FOOT_ORIENTATION:
# swf_orientation_func = yfg.concatenate([yfg.zero, yfg.hermite2nd, yfg.one], [.25, .75])
for swingFoot in swingFoots:
R_target_foot = motion_seg[curInterval[1]].getJointOrientationGlobal(swingFoot)
R_current_foot = motion_swf_orientation[frame].getJointOrientationGlobal(swingFoot)
motion_swf_orientation[frame].setJointOrientationGlobal(swingFoot, cm.slerp(R_current_foot, R_target_foot, swf_orientation_func(t)))
# swf_stabilize_func = yfg.concatenate([yfg.hermite2nd, yfg.one], [c_taking_duration])
# push orientation
# for swingFoot in swingFoots:
# R_target_foot = motion_seg[frame].getJointOrientationGlobal(swingFoot)
# R_current_foot = motion_swf_orientation[frame].getJointOrientationGlobal(swingFoot)
# motion_swf_orientation[frame].setJointOrientationGlobal(swingFoot, cm.slerp(R_current_foot, R_target_foot , swf_stabilize_func(t)))
# stance foot push
motion_stf_push.append(motion_swf_orientation[frame].copy())
motion_stf_push.goToFrame(frame)
if STANCE_FOOT_PUSH:
for swingFoot in swingFoots:
# max_t = (maxStfPushFrame)/float(curInterval[1]-curInterval[0])
# stf_push_func = yfg.concatenate([yfg.sine, yfg.zero], [max_t*2])
stf_push_func = yfg.concatenate([yfg.sine, yfg.zero], [c_taking_duration*2])
R_swp_sag = mm.I_SO3()
# R_swp_sag = np.dot(R_swp_sag, mm.exp(diff_dCM_sag_axis * K_stp_vel * -stf_push_func(t)))
# if step_length_cur[0] < step_length_tar[0]:
# ratio = step_length_cur[0] / step_length_tar[0]
# R_max = maxmaxStfPushFrame
# R_zero =
R_swp_sag = np.dot(R_swp_sag, mm.exp((step_length_tar[0] - step_length_cur[0])*step_axis[0] * K_stp_pos * -stf_push_func(t)))
motion_stf_push[frame].mulJointOrientationGlobal(swingFoot, R_swp_sag)
# stance foot balancing
motion_stf_balancing.append(motion_stf_push[frame].copy())
motion_stf_balancing.goToFrame(frame)
if STANCE_FOOT_BALANCING:
R_stb = mm.exp(diff_dCM_axis * K_stb_vel * stf_balancing_func(t))
for stanceFoot in stanceFoots:
if frame < 5: continue
motion_stf_balancing[frame].mulJointOrientationGlobal(stanceFoot, R_stb)
# control trajectory
motion_control.append(motion_stf_balancing[frame].copy())
motion_control.goToFrame(frame)
#=======================================================================
# tracking with inverse dynamics
#=======================================================================
th_r = motion_control.getDOFPositions(frame)
th = controlModel.getDOFPositions()
dth_r = motion_control.getDOFVelocities(frame)
dth = controlModel.getDOFVelocities()
ddth_r = motion_control.getDOFAccelerations(frame)
ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt)
#=======================================================================
# simulation
#=======================================================================
CP = mm.v3(0.,0.,0.)
F = mm.v3(0.,0.,0.)
avg_dCM[0] = mm.v3(0.,0.,0.)
# external force rendering info
del rd_forces[:]; del rd_force_points[:]
for fi in forceInfos:
if fi.startFrame <= frame and frame < fi.startFrame + fi.duration*(1/frameTime):
rd_forces.append(fi.force)
rd_force_points.append(controlModel.getBodyPositionGlobal(fi.targetBody))
for i in range(stepsPerFrame):
bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(bodyIDsToCheck, mus, Ks, Ds)
vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals, contactForces)
# apply external force
for fi in forceInfos:
if fi.startFrame <= frame and frame < fi.startFrame + fi.duration*(1/frameTime):
controlModel.applyBodyForceGlobal(fi.targetBody, fi.force)
controlModel.setDOFAccelerations(ddth_des)
controlModel.solveHybridDynamics()
if TORQUE_PLOT:
rhip_torques[frame] += mm.length(controlModel.getJointTorqueLocal(rUpLeg))
rknee_torques[frame] += mm.length(controlModel.getJointTorqueLocal(rKnee))
rankle_torques[frame] += mm.length(controlModel.getJointTorqueLocal(rFoot))
rd_torques[:] = [controlModel.getJointTorqueLocal(i)/100. for i in range(skeleton.getJointNum())]
rd_joint_positions[:] = controlModel.getJointPositionsGlobal()
vpWorld.step()
# yvu.align2D(controlModel)
if len(contactForces) > 0:
CP += yrp.getCP(contactPositions, contactForces)
F += sum(contactForces)
avg_dCM[0] += controlModel.getJointVelocityGlobal(0)
# avg_dCM[0] += yrp.getCM(controlModel.getJointVelocitiesGlobal(), bodyMasses, upperMass, uppers)
# avg_dCM[0] += yrp.getCM(controlModel.getJointVelocitiesGlobal(), bodyMasses, totalMass)
# if len(stanceFoots)>0:
# avg_stf_v[0] += controlModel.getJointVelocityGlobal(stanceFoots[0])
# avg_stf_av[0] += controlModel.getJointAngVelocityGlobal(stanceFoots[0])
CP /= stepsPerFrame
F /= stepsPerFrame
avg_dCM[0] /= stepsPerFrame
# if len(stanceFoots)>0:
# avg_stf_v[0] /= stepsPerFrame
# avg_stf_av[0] /= stepsPerFrame
# rd_vec1[0] = avg_stf_av[0]; rd_vec1[0][0] = 0.; rd_vec1[0][2] = 0.
# rd_vecori1[0]= controlModel.getJointPositionGlobal(stanceFoots[0])
#=======================================================================
# segment editing
#=======================================================================
lastFrame = False
if SEGMENT_EDITING:
if curState==yba.GaitState.STOP:
if frame == len(motion_seg)-1:
lastFrame = True
elif (curState==yba.GaitState.LSWING or curState==yba.GaitState.RSWING) and t>c_min_contact_time:
swingID = lID if curState==yba.GaitState.LSWING else rID
contact = False
if swingID in bodyIDs:
minContactVel = 1000.
for i in range(len(bodyIDs)):
if bodyIDs[i]==swingID:
vel = controlModel.getBodyVelocityGlobal(swingID, contactPositionLocals[i])
vel[1] = 0
contactVel = mm.length(vel)
if contactVel < minContactVel: minContactVel = contactVel
if minContactVel < c_min_contact_vel: contact = True
extended[0] = False
if contact:
# print frame, 'foot touch'
lastFrame = True
acc_offset[0] += frame - curInterval[1]
elif frame == len(motion_seg)-1:
print frame, 'extend frame', frame+1
preserveJoints = []
# preserveJoints = [lFoot, rFoot]
# preserveJoints = [lFoot, rFoot, lKnee, rKnee]
# preserveJoints = [lFoot, rFoot, lKnee, rKnee, lUpLeg, rUpLeg]
stanceKnees = [rKnee] if curState==yba.GaitState.LSWING else [lKnee]
preserveJoints = [stanceFoots[0], stanceKnees[0], stanceLegs[0]]
diff = 3
motion_seg_orig.extend([motion_seg_orig[-1]])
motion_seg.extend(ymt.extendByIntegration_root(motion_seg, 1, diff))
motion_stitch.extend(ymt.extendByIntegration_constant(motion_stitch, 1, preserveJoints, diff))
# # extend for swing foot ground speed matching & swing foot height lower
## extendedPostures = ymt.extendByIntegration(motion_stitch, 1, preserveJoints, diff)
## extendedPostures = [motion_stitch[-1]]
##
# extendFrameNum = frame - curInterval[1] + 1
# k = 1.-extendFrameNum/5.
# if k<0.: k=0.
# extendedPostures = ymt.extendByIntegrationAttenuation(motion_stitch, 1, preserveJoints, diff, k)
#
## if len(swingFoots)>0 and np.inner(dCM_tar, dCM)>0.:
## print frame, 'speed matching'
## R_swf = motion_stitch[-1].getJointOrientationGlobal(swingFoots[0])
##
## p_swf = motion_stitch[-1].getJointPositionGlobal(swingFoots[0])
## v_swf = motion_stitch.getJointVelocityGlobal(swingFoots[0], frame-diff, frame)
## a_swf = motion_stitch.getJointAccelerationGlobal(swingFoots[0], frame-diff, frame)
## p_swf += v_swf * (frameTime) + a_swf * (frameTime)*(frameTime)
## aik.ik_analytic(extendedPostures[0], swingFoots[0], p_swf)
##
## extendedPostures[0].setJointOrientationGlobal(swingFoots[0], R_swf)
#
# motion_stitch.extend(extendedPostures)
extended[0] = True
else:
if frame == len(motion_seg)-1: lastFrame = True
if lastFrame:
if segIndex < len(segments)-1:
print '%d (%d): end of %dth seg (%s, %s)'%(frame, frame-curInterval[1],segIndex, yba.GaitState.text[curState], curInterval)
if plot!=None: plot.addDataPoint('diff', frame, (frame-curInterval[1])*.01)
if len(stanceFoots)>0 and len(swingFoots)>0:
# step_cur = controlModel.getJointPositionGlobal(swingFoots[0]) - controlModel.getJointPositionGlobal(stanceFoots[0])
# step_tar = motion_seg[curInterval[1]].getJointPositionGlobal(swingFoots[0]) - motion_seg[curInterval[1]].getJointPositionGlobal(stanceFoots[0])
step_cur = controlModel.getJointPositionGlobal(0) - controlModel.getJointPositionGlobal(stanceFoots[0])
step_tar = motion_seg[curInterval[1]].getJointPositionGlobal(0) - motion_seg[curInterval[1]].getJointPositionGlobal(stanceFoots[0])
step_cur = mm.projectionOnPlane(step_cur, (1,0,0), (0,0,1))
step_tar = mm.projectionOnPlane(step_tar, (1,0,0), (0,0,1))
step_cur_sag, step_cur_cor = mm.projectionOnVector2(step_cur, direction)
step_tar_sag, step_tar_cor = mm.projectionOnVector2(step_tar, direction)
step_length_tar[0] = mm.length(step_tar_sag)
if np.inner(step_tar_sag, step_cur_sag) > 0:
step_length_cur[0] = mm.length(step_cur_sag)
else:
step_length_cur[0] = -mm.length(step_cur_sag)
step_axis[0] = directionAxis
# rd_vec1[0] = step_tar_sag
# rd_vecori1[0] = motion_seg[curInterval[1]].getJointPositionGlobal(stanceFoots[0])
# rd_vec2[0] = step_cur_sag
# rd_vecori2[0] = controlModel.getJointPositionGlobal(stanceFoots[0])
seg_index[0] += 1
curSeg = segments[seg_index[0]]
stl_y_limit_num[0] = 0
stl_xz_limit_num[0] = 0
del motion_seg_orig[frame+1:]
motion_seg_orig.extend(ymb.getAttachedNextMotion(curSeg, motion_seg_orig[-1], False, False))
del motion_seg[frame+1:]
del motion_stitch[frame+1:]
transitionLength = len(curSeg)-1
# motion_seg.extend(ymb.getAttachedNextMotion(curSeg, motion_seg[-1], False, False))
# motion_stitch.extend(ymb.getStitchedNextMotion(curSeg, motion_control[-1], transitionLength, stitch_func, True, False))
d = motion_seg[-1] - curSeg[0]
d.rootPos[1] = 0.
motion_seg.extend(ymb.getAttachedNextMotion(curSeg, d, True, False))
if NO_FOOT_SLIDING:
if segIndex == len(segments)-2:
Rl = motion_control[-1].getJointOrientationLocal(lUpLeg)
Rr = motion_control[-1].getJointOrientationLocal(rUpLeg)
Rlk = motion_control[-1].getJointOrientationLocal(lKnee)
Rrk = motion_control[-1].getJointOrientationLocal(rKnee)
Rlf = motion_control[-1].getJointOrientationLocal(lFoot)
Rrf = motion_control[-1].getJointOrientationLocal(rFoot)
for p in curSeg:
p.setJointOrientationLocal(lUpLeg, Rl, False)
p.setJointOrientationLocal(rUpLeg, Rr, False)
p.setJointOrientationLocal(lKnee, Rlk, False)
p.setJointOrientationLocal(rKnee, Rrk, False)
p.setJointOrientationLocal(lFoot, Rlf, False)
p.setJointOrientationLocal(rFoot, Rrf, False)
p.updateGlobalT()
d = motion_control[-1] - curSeg[0]
d.rootPos[1] = 0.
motion_stitch.extend(ymb.getStitchedNextMotion(curSeg, d, transitionLength, stitch_func, True, False))
# motion_seg.extend(ymb.getAttachedNextMotion(curSeg, motion_seg[-1], False, True))
# motion_stitch.extend(ymb.getStitchedNextMotion(curSeg, motion_control[-1], transitionLength, stitch_func, True, True))
else:
motion_seg_orig.append(motion_seg_orig[-1])
motion_seg.append(motion_seg[-1])
motion_stitch.append(motion_control[-1])
# rendering
motionModel.update(motion_ori[frame])
# motionModel.update(motion_seg[frame])
if SAVE_SIMULATION:
p_temp = ym.JointPosture(skeleton)
p_temp.initLocalRs()
p_temp.rootPos = controlModel.getJointPositionGlobal(0)
p_temp.setJointOrientationsLocal(controlModel.getJointOrientationsLocal())
motion_simulation.append(p_temp)
if frame == viewer.getMaxFrame():
saveFilePath = SAVE_DIR+'simulated_'+filename
yf.writeBvhFile(saveFilePath, motion_simulation)
print saveFilePath, 'saved'
def preprocess():
tasks = []
# outputDir = './ppmotion/'
#
# dir = '../Data/woody2/Motion/Physics2/'
# config = {'repeat':True, 'footRot': mm.rotX(-.4), 'yOffset':0., 'halfFootHeight': 0.0444444444444, 'scale':.01, 'type':'woody2'}
# paths = []
# paths.append(dir+'wd2_WalkSameSame01.bvh')
# paths.append(dir+'wd2_WalkForwardSlow01.bvh')
# paths.append(dir+'wd2_WalkForwardNormal00.bvh')
# paths.append(dir+'wd2_WalkHandWav00.bvh')
# paths.append(dir+'wd2_WalkForwardFast00.bvh')
# paths.append(dir+'wd2_WalkForwardVFast00.bvh')
# paths.append(dir+'wd2_WalkLean00.bvh')
# paths.append(dir+'wd2_WalkAzuma01.bvh')
# paths.append(dir+'wd2_WalkSoldier00.bvh')
## paths.append(dir+'wd2_WalkSukiko00.bvh')
# paths.append(dir+'wd2_WalkBackward00.bvh')
## paths.append(dir+'wd2_WalkTongTong00.bvh')
## tasks.append({'config':config, 'paths':paths})
##
# dir = '../Data/woody2/Motion/Balancing/'
# config = {'footRot': mm.exp(mm.v3(1,-.5,0), -.6), 'yOffset': .0, 'halfFootHeight': 0.0444444444444, 'scale':.01, 'type':'woody2'}
# paths = []
# paths.append(dir+'wd2_2foot_walk_turn.bvh')
# paths.append(dir+'wd2_2foot_walk_turn2.bvh')
# paths.append(dir+'wd2_slow_2foot_hop.bvh')
# paths.append(dir+'wd2_short_broad_jump.bvh')
# paths.append(dir+'wd2_long_broad_jump.bvh')
# paths.append(dir+'wd2_ffast_cancan_run.bvh')
# paths.append(dir+'wd2_fast_cancan_run.bvh')
# paths.append(dir+'wd2_fast_2foot_hop.bvh')
# paths.append(dir+'wd2_1foot_contact_run.bvh')
# paths.append(dir+'wd2_1foot_contact_run2.bvh')
## tasks.append({'config':config, 'paths':paths})
#
## dir = '../Data/woody2/Motion/Samsung/'
## config = {'footRot': mm.rotX(-.46), 'yOffset': .0, 'halfFootHeight': 0.0444444444444, 'scale':.01*2.54, 'type':'woody2amc'}
## paths = []
## paths.append(dir+'wd2_left_turn.bvh')
## paths.append(dir+'wd2_right_turn.bvh')
## paths.append(dir+'wd2_pose_inner1.bvh')
## paths.append(dir+'wd2_pose_inner2.bvh')
## tasks.append({'config':config, 'paths':paths})
#
# dir = '../Data/woody2/Motion/Picking/'
# config = {'footRot': mm.rotX(-.5), 'yOffset': .0, 'halfFootHeight': 0.0444444444444, 'scale':.01, 'type':'woody2'}
# paths = []
# paths.append(dir+'wd2_pick_walk_1.bvh')
## tasks.append({'config':config, 'paths':paths})
#
# dir = '../Data/woody2/Motion/VideoMotion/'
# config = {'footRot': mm.rotX(-.48), 'yOffset': .0, 'halfFootHeight': 0.0444444444444, 'scale':.01*2.54, 'type':'woody2amc'}
## paths = glob.glob(dir+'*.bvh')
# paths = []
# paths.append(dir+'wd2_cross_walk_90d_fast_27.bvh')
## tasks.append({'config':config, 'paths':paths})
#
# dir = '../Data/woody2/Motion/Walking/'
# config = {'footRot': mm.rotX(-.40), 'yOffset': .0, 'halfFootHeight': 0.0444444444444, 'scale':.01*2.54, 'type':'woody2amc'}
# paths = glob.glob(dir+'*.bvh')
## tasks.append({'config':config, 'paths':paths})
#
## dir = '../Data/woody2/Motion/samsung_boxing/round/'
## config = {'footRot': mm.rotX(-.65), 'yOffset': .0, 'halfFootHeight': 0.0444444444444, 'scale':.01*2.54, 'type':'woody2amc'}
## paths = glob.glob(dir+'*.bvh')
## tasks.append({'config':config, 'paths':paths})
#
## dir = '../Data/woody2/Motion/samsung_boxing/boxman/'
## config = {'footRot': mm.rotX(-.5), 'yOffset': .0, 'halfFootHeight': 0.0444444444444, 'scale':.01*2.54, 'type':'woody2amc'}
## paths = glob.glob(dir+'*.bvh')
## tasks.append({'config':config, 'paths':paths})
#
# dir = '../Data/woody2/Motion/motion_edit/'
# config = {'footRot': mm.rotX(-.5), 'yOffset': .0, 'halfFootHeight': 0.0444444444444, 'scale':.01*2.54, 'type':'woody2amc'}
# paths = glob.glob(dir+'*.bvh')
## tasks.append({'config':config, 'paths':paths})
#
## outputDir = './icmotion_test/'
## dir = './rawmotion/'
## config = {'footRot': mm.rotX(-.5), 'yOffset': .0, 'halfFootHeight': 0.0444444444444, 'scale':1., 'type':'woody2_new'}
## paths = glob.glob(dir+'*.temp')
## tasks.append({'config':config, 'paths':paths})
#
# outputDir = './icmotion_last/'
# dir = './lastmotion/add/'
## config = {'rootRot':mm.rotZ(.05), 'footRot': mm.rotX(-.5), 'leftFootR':mm.rotZ(-.1), \
# config = {'rootRot':mm.rotZ(.0), 'footRot': np.dot(mm.rotX(-.5), mm.rotZ(.04)), 'leftFootR':mm.rotZ(-.2), \
# 'halfFootHeight': 0.0444444444444, 'scale':1., 'type':'woody2_new'}
# paths = glob.glob(dir+'*.temp')
# tasks.append({'config':config, 'paths':paths})
# outputDir = './ppmotion_slope/'
# dir = './rawmotion_slope_extracted/'
# config = {'rootRot':mm.rotZ(.0), 'footRot': np.dot(mm.rotX(-.55), mm.rotZ(.04)), 'leftFootR':mm.rotZ(-.2), \
# 'halfFootHeight': 0.0444444444444, 'scale':1., 'type':'woody2_new'}
# paths = glob.glob(dir+'*.bvh')
# tasks.append({'config':config, 'paths':paths})
VISUALIZE = False
for task in tasks:
config = task['config']
paths = task['paths']
for path in paths:
motion = yf.readBvhFile(path)
normalizeSkeleton(motion, config)
adjustHeight(motion, config['halfFootHeight'])
additionalEdit(motion, path)
outputPath = outputDir + os.path.basename(path)
yf.writeBvhFile(outputPath, motion)
print outputPath, 'done'
if 'repeat' in config and config['repeat']:
hRef = .1; vRef = .3
lc = yma.getElementContactStates(motion, 'LeftFoot', hRef, vRef)
interval = yba.getWalkingCycle2(motion, lc)
transitionLength = 20 if 'wd2_WalkAzuma01.bvh' in path else 10
motion = ymt.repeatCycle(motion, interval, 50, transitionLength)
outputName = os.path.splitext(os.path.basename(path))[0]+'_REPEATED.bvh'
outputPath = outputDir + outputName
yf.writeBvhFile(outputPath, motion)
print outputPath, 'done'
if VISUALIZE:
viewer = ysv.SimpleViewer()
viewer.record(False)
viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion, (0,100,255), yr.LINK_LINE))
viewer.doc.addObject('motion', motion)
viewer.startTimer(1/30.)
viewer.show()
Fl.run()
print 'FINISHED'
def simulateCallback(frame):
# seginfo
segIndex = seg_index[0]
curState = seginfo[segIndex]['state']
curInterval = yma.offsetInterval(acc_offset[0],
seginfo[segIndex]['interval'])
stanceLegs = seginfo[segIndex]['stanceHips']
swingLegs = seginfo[segIndex]['swingHips']
stanceFoots = seginfo[segIndex]['stanceFoots']
swingFoots = seginfo[segIndex]['swingFoots']
swingKnees = seginfo[segIndex]['swingKnees']
groundHeight = seginfo[segIndex]['ground_height']
maxStfPushFrame = seginfo[segIndex]['max_stf_push_frame']
prev_frame = frame - 1 if frame > 0 else 0
# prev_frame = frame
# information
# dCM_tar = yrp.getCM(motion_seg.getJointVelocitiesGlobal(frame), bodyMasses, upperMass, uppers)
# CM_tar = yrp.getCM(motion_seg.getJointPositionsGlobal(frame), bodyMasses, upperMass, uppers)
## dCM_tar = yrp.getCM(motion_seg.getJointVelocitiesGlobal(frame), bodyMasses, totalMass)
## CM_tar = yrp.getCM(motion_seg.getJointPositionsGlobal(frame), bodyMasses, totalMass)
# stf_tar = motion_seg.getJointPositionGlobal(stanceFoots[0], frame)
# CMr_tar = CM_tar - stf_tar
dCM_tar = motion_seg.getJointVelocityGlobal(0, prev_frame)
CM_tar = motion_seg.getJointPositionGlobal(0, prev_frame)
# dCM_tar = yrp.getCM(motion_seg.getJointVelocitiesGlobal(prev_frame), bodyMasses, upperMass, uppers)
# CM_tar = yrp.getCM(motion_seg.getJointPositionsGlobal(prev_frame), bodyMasses, upperMass, uppers)
# dCM_tar = yrp.getCM(motion_seg.getJointVelocitiesGlobal(prev_frame), bodyMasses, totalMass)
# CM_tar = yrp.getCM(motion_seg.getJointPositionsGlobal(prev_frame), bodyMasses, totalMass)
stf_tar = motion_seg.getJointPositionGlobal(stanceFoots[0], prev_frame)
CMr_tar = CM_tar - stf_tar
dCM = avg_dCM[0]
CM = controlModel.getJointPositionGlobal(0)
# CM = yrp.getCM(controlModel.getJointPositionsGlobal(), bodyMasses, upperMass, uppers)
# CM = yrp.getCM(controlModel.getJointPositionsGlobal(), bodyMasses, totalMass)
CMreal = yrp.getCM(controlModel.getJointPositionsGlobal(), bodyMasses,
totalMass)
stf = controlModel.getJointPositionGlobal(stanceFoots[0])
CMr = CM - stf
diff_dCM = mm.projectionOnPlane(dCM - dCM_tar, (1, 0, 0), (0, 0, 1))
diff_dCM_axis = np.cross((0, 1, 0), diff_dCM)
rd_vec1[0] = diff_dCM
rd_vecori1[0] = CM_tar
diff_CMr = mm.projectionOnPlane(CMr - CMr_tar, (1, 0, 0), (0, 0, 1))
# rd_vec1[0] = diff_CMr; rd_vecori1[0] = stf_tar
diff_CMr_axis = np.cross((0, 1, 0), diff_CMr)
direction = mm.normalize2(
mm.projectionOnPlane(dCM_tar, (1, 0, 0), (0, 0, 1)))
# direction = mm.normalize2(mm.projectionOnPlane(dCM, (1,0,0), (0,0,1)))
directionAxis = np.cross((0, 1, 0), direction)
diff_dCM_sag, diff_dCM_cor = mm.projectionOnVector2(
diff_dCM, direction)
# rd_vec1[0] = diff_dCM_sag; rd_vecori1[0] = CM_tar
diff_dCM_sag_axis = np.cross((0, 1, 0), diff_dCM_sag)
diff_dCM_cor_axis = np.cross((0, 1, 0), diff_dCM_cor)
diff_CMr_sag, diff_CMr_cor = mm.projectionOnVector2(
diff_CMr, direction)
diff_CMr_sag_axis = np.cross((0, 1, 0), diff_CMr_sag)
diff_CMr_cor_axis = np.cross((0, 1, 0), diff_CMr_cor)
t = (frame - curInterval[0]) / float(curInterval[1] - curInterval[0])
t_raw = t
if t > 1.: t = 1.
p_root = motion_stitch[frame].getJointPositionGlobal(0)
R_root = motion_stitch[frame].getJointOrientationGlobal(0)
motion_seg_orig.goToFrame(frame)
motion_seg.goToFrame(frame)
motion_stitch.goToFrame(frame)
motion_debug1.append(motion_stitch[frame].copy())
motion_debug1.goToFrame(frame)
motion_debug2.append(motion_stitch[frame].copy())
motion_debug2.goToFrame(frame)
motion_debug3.append(motion_stitch[frame].copy())
motion_debug3.goToFrame(frame)
# paper implementation
M_tc.append(motion_stitch[prev_frame])
M_tc.goToFrame(frame)
P_hat.append(M_tc[frame].copy())
P_hat.goToFrame(frame)
p_temp = ym.JointPosture(skeleton)
p_temp.rootPos = controlModel.getJointPositionGlobal(0)
p_temp.setJointOrientationsLocal(
controlModel.getJointOrientationsLocal())
P.append(p_temp)
P.goToFrame(frame)
# stance foot stabilize
motion_stf_stabilize.append(motion_stitch[frame].copy())
motion_stf_stabilize.goToFrame(frame)
if STANCE_FOOT_STABILIZE:
for stanceFoot in stanceFoots:
R_target_foot = motion_seg[frame].getJointOrientationGlobal(
stanceFoot)
R_current_foot = motion_stf_stabilize[
frame].getJointOrientationGlobal(stanceFoot)
motion_stf_stabilize[frame].setJointOrientationGlobal(
stanceFoot,
cm.slerp(R_current_foot, R_target_foot,
stf_stabilize_func(t)))
# R_target_foot = motion_seg[frame].getJointOrientationLocal(stanceFoot)
# R_current_foot = motion_stf_stabilize[frame].getJointOrientationLocal(stanceFoot)
# motion_stf_stabilize[frame].setJointOrientationLocal(stanceFoot, cm.slerp(R_current_foot, R_target_foot , stf_stabilize_func(t)))
# match stance leg
motion_match_stl.append(motion_stf_stabilize[frame].copy())
motion_match_stl.goToFrame(frame)
if MATCH_STANCE_LEG:
if curState != yba.GaitState.STOP:
for i in range(len(stanceLegs)):
stanceLeg = stanceLegs[i]
stanceFoot = stanceFoots[i]
# # motion stance leg -> character stance leg as time goes
R_motion = motion_match_stl[
frame].getJointOrientationGlobal(stanceLeg)
R_character = controlModel.getJointOrientationGlobal(
stanceLeg)
motion_match_stl[frame].setJointOrientationGlobal(
stanceLeg,
cm.slerp(R_motion, R_character, match_stl_func(t)))
# t_y = match_stl_func_y(t)
# t_xz = match_stl_func(t)
#
# R_motion = motion_match_stl[frame].getJointOrientationGlobal(stanceLeg)
# R_character = controlModel.getJointOrientationGlobal(stanceLeg)
# R = np.dot(R_character, R_motion.T)
# R_y, R_xz = mm.projectRotation((0,1,0), R)
# motion_match_stl[frame].mulJointOrientationGlobal(stanceLeg, mm.scaleSO3(R_xz, t_xz))
# motion_match_stl[frame].mulJointOrientationGlobal(stanceLeg, mm.scaleSO3(R_y, t_y))
# swing foot placement
motion_swf_placement.append(motion_match_stl[frame].copy())
motion_swf_placement.goToFrame(frame)
if SWING_FOOT_PLACEMENT:
t_swing_foot_placement = swf_placement_func(t)
if extended[0]:
R_swp_sag = prev_R_swp[0][0]
R_swp_cor = prev_R_swp[0][1]
else:
R_swp_sag = mm.I_SO3()
R_swp_cor = mm.I_SO3()
R_swp_sag = np.dot(
R_swp_sag,
mm.exp(diff_dCM_sag_axis * K_swp_vel_sag *
-t_swing_foot_placement))
R_swp_cor = np.dot(
R_swp_cor,
mm.exp(diff_dCM_cor_axis * K_swp_vel_cor *
-t_swing_foot_placement))
if np.dot(direction, diff_CMr_sag) < 0:
R_swp_sag = np.dot(
R_swp_sag,
mm.exp(diff_CMr_sag_axis * K_swp_pos_sag *
-t_swing_foot_placement))
R_swp_cor = np.dot(
R_swp_cor,
mm.exp(diff_CMr_cor_axis * K_swp_pos_cor *
-t_swing_foot_placement))
for i in range(len(swingLegs)):
swingLeg = swingLegs[i]
swingFoot = swingFoots[i]
# save swing foot global orientation
# R_swf = motion_swf_placement[frame].getJointOrientationGlobal(swingFoot)
# rotate swing leg
motion_swf_placement[frame].mulJointOrientationGlobal(
swingLeg, R_swp_sag)
motion_swf_placement[frame].mulJointOrientationGlobal(
swingLeg, R_swp_cor)
# restore swing foot global orientation
# motion_swf_placement[frame].setJointOrientationGlobal(swingFoot, R_swf)
prev_R_swp[0] = (R_swp_sag, R_swp_cor)
# swing foot height
motion_swf_height.append(motion_swf_placement[frame].copy())
motion_swf_height.goToFrame(frame)
if SWING_FOOT_HEIGHT:
for swingFoot in swingFoots:
stanceFoot = stanceFoots[0]
# save foot global orientation
R_foot = motion_swf_height[frame].getJointOrientationGlobal(
swingFoot)
R_stance_foot = motion_swf_height[
frame].getJointOrientationGlobal(stanceFoot)
height_tar = motion_swf_height[frame].getJointPositionGlobal(
swingFoot)[1] - motion_swf_height[
frame].getJointPositionGlobal(stanceFoot)[1]
# motion_debug1[frame] = motion_swf_height[frame].copy()
# rotate
motion_swf_height[frame].rotateByTarget(
controlModel.getJointOrientationGlobal(0))
# motion_debug2[frame] = motion_swf_height[frame].copy()
# motion_debug2[frame].translateByTarget(controlModel.getJointPositionGlobal(0))
height_cur = motion_swf_height[frame].getJointPositionGlobal(
swingFoot)[1] - motion_swf_height[
frame].getJointPositionGlobal(stanceFoot)[1]
offset_height = (height_tar - height_cur
) * swf_height_func(t) * c_swf_stability
offset_sine = c_swf_mid_offset * swf_height_sine_func(t)
offset = 0.
offset += offset_height
offset += offset_sine
if offset > 0.:
newPosition = motion_swf_height[
frame].getJointPositionGlobal(swingFoot)
newPosition[1] += offset
aik.ik_analytic(motion_swf_height[frame], swingFoot,
newPosition)
else:
newPosition = motion_swf_height[
frame].getJointPositionGlobal(stanceFoot)
newPosition[1] -= offset
aik.ik_analytic(motion_swf_height[frame], stanceFoot,
newPosition)
# return
# motion_debug3[frame] = motion_swf_height[frame].copy()
# motion_debug3[frame].translateByTarget(controlModel.getJointPositionGlobal(0))
motion_swf_height[frame].rotateByTarget(R_root)
# restore foot global orientation
motion_swf_height[frame].setJointOrientationGlobal(
swingFoot, R_foot)
motion_swf_height[frame].setJointOrientationGlobal(
stanceFoot, R_stance_foot)
if plot != None:
plot.addDataPoint('debug1', frame, height_tar)
plot.addDataPoint('debug2', frame, height_cur)
# plot.addDataPoint('diff', frame, diff)
# swing foot orientation
motion_swf_orientation.append(motion_swf_height[frame].copy())
motion_swf_orientation.goToFrame(frame)
if SWING_FOOT_ORIENTATION:
# swf_orientation_func = yfg.concatenate([yfg.zero, yfg.hermite2nd, yfg.one], [.25, .75])
for swingFoot in swingFoots:
R_target_foot = motion_seg[
curInterval[1]].getJointOrientationGlobal(swingFoot)
R_current_foot = motion_swf_orientation[
frame].getJointOrientationGlobal(swingFoot)
motion_swf_orientation[frame].setJointOrientationGlobal(
swingFoot,
cm.slerp(R_current_foot, R_target_foot,
swf_orientation_func(t)))
# swf_stabilize_func = yfg.concatenate([yfg.hermite2nd, yfg.one], [c_taking_duration])
# push orientation
# for swingFoot in swingFoots:
# R_target_foot = motion_seg[frame].getJointOrientationGlobal(swingFoot)
# R_current_foot = motion_swf_orientation[frame].getJointOrientationGlobal(swingFoot)
# motion_swf_orientation[frame].setJointOrientationGlobal(swingFoot, cm.slerp(R_current_foot, R_target_foot , swf_stabilize_func(t)))
# stance foot push
motion_stf_push.append(motion_swf_orientation[frame].copy())
motion_stf_push.goToFrame(frame)
if STANCE_FOOT_PUSH:
for swingFoot in swingFoots:
# max_t = (maxStfPushFrame)/float(curInterval[1]-curInterval[0])
# stf_push_func = yfg.concatenate([yfg.sine, yfg.zero], [max_t*2])
stf_push_func = yfg.concatenate([yfg.sine, yfg.zero],
[c_taking_duration * 2])
R_swp_sag = mm.I_SO3()
# R_swp_sag = np.dot(R_swp_sag, mm.exp(diff_dCM_sag_axis * K_stp_vel * -stf_push_func(t)))
# if step_length_cur[0] < step_length_tar[0]:
# ratio = step_length_cur[0] / step_length_tar[0]
# R_max = maxmaxStfPushFrame
# R_zero =
R_swp_sag = np.dot(
R_swp_sag,
mm.exp((step_length_tar[0] - step_length_cur[0]) *
step_axis[0] * K_stp_pos * -stf_push_func(t)))
motion_stf_push[frame].mulJointOrientationGlobal(
swingFoot, R_swp_sag)
# stance foot balancing
motion_stf_balancing.append(motion_stf_push[frame].copy())
motion_stf_balancing.goToFrame(frame)
if STANCE_FOOT_BALANCING:
R_stb = mm.exp(diff_dCM_axis * K_stb_vel * stf_balancing_func(t))
for stanceFoot in stanceFoots:
if frame < 5: continue
motion_stf_balancing[frame].mulJointOrientationGlobal(
stanceFoot, R_stb)
# control trajectory
motion_control.append(motion_stf_balancing[frame].copy())
motion_control.goToFrame(frame)
#=======================================================================
# tracking with inverse dynamics
#=======================================================================
th_r = motion_control.getDOFPositions(frame)
th = controlModel.getDOFPositions()
dth_r = motion_control.getDOFVelocities(frame)
dth = controlModel.getDOFVelocities()
ddth_r = motion_control.getDOFAccelerations(frame)
ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r,
Kt, Dt)
#=======================================================================
# simulation
#=======================================================================
CP = mm.v3(0., 0., 0.)
F = mm.v3(0., 0., 0.)
avg_dCM[0] = mm.v3(0., 0., 0.)
# external force rendering info
del rd_forces[:]
del rd_force_points[:]
for fi in forceInfos:
if fi.startFrame <= frame and frame < fi.startFrame + fi.duration * (
1 / frameTime):
rd_forces.append(fi.force)
rd_force_points.append(
controlModel.getBodyPositionGlobal(fi.targetBody))
for i in range(stepsPerFrame):
bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(
bodyIDsToCheck, mus, Ks, Ds)
vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals,
contactForces)
# apply external force
for fi in forceInfos:
if fi.startFrame <= frame and frame < fi.startFrame + fi.duration * (
1 / frameTime):
controlModel.applyBodyForceGlobal(fi.targetBody, fi.force)
controlModel.setDOFAccelerations(ddth_des)
controlModel.solveHybridDynamics()
if TORQUE_PLOT:
rhip_torques[frame] += mm.length(
controlModel.getJointTorqueLocal(rUpLeg))
rknee_torques[frame] += mm.length(
controlModel.getJointTorqueLocal(rKnee))
rankle_torques[frame] += mm.length(
controlModel.getJointTorqueLocal(rFoot))
rd_torques[:] = [
controlModel.getJointTorqueLocal(i) / 100.
for i in range(skeleton.getJointNum())
]
rd_joint_positions[:] = controlModel.getJointPositionsGlobal()
vpWorld.step()
# yvu.align2D(controlModel)
if len(contactForces) > 0:
CP += yrp.getCP(contactPositions, contactForces)
F += sum(contactForces)
avg_dCM[0] += controlModel.getJointVelocityGlobal(0)
# avg_dCM[0] += yrp.getCM(controlModel.getJointVelocitiesGlobal(), bodyMasses, upperMass, uppers)
# avg_dCM[0] += yrp.getCM(controlModel.getJointVelocitiesGlobal(), bodyMasses, totalMass)
# if len(stanceFoots)>0:
# avg_stf_v[0] += controlModel.getJointVelocityGlobal(stanceFoots[0])
# avg_stf_av[0] += controlModel.getJointAngVelocityGlobal(stanceFoots[0])
CP /= stepsPerFrame
F /= stepsPerFrame
avg_dCM[0] /= stepsPerFrame
# if len(stanceFoots)>0:
# avg_stf_v[0] /= stepsPerFrame
# avg_stf_av[0] /= stepsPerFrame
# rd_vec1[0] = avg_stf_av[0]; rd_vec1[0][0] = 0.; rd_vec1[0][2] = 0.
# rd_vecori1[0]= controlModel.getJointPositionGlobal(stanceFoots[0])
#=======================================================================
# segment editing
#=======================================================================
lastFrame = False
if SEGMENT_EDITING:
if curState == yba.GaitState.STOP:
if frame == len(motion_seg) - 1:
lastFrame = True
elif (curState == yba.GaitState.LSWING or curState
== yba.GaitState.RSWING) and t > c_min_contact_time:
swingID = lID if curState == yba.GaitState.LSWING else rID
contact = False
if swingID in bodyIDs:
minContactVel = 1000.
for i in range(len(bodyIDs)):
if bodyIDs[i] == swingID:
vel = controlModel.getBodyVelocityGlobal(
swingID, contactPositionLocals[i])
vel[1] = 0
contactVel = mm.length(vel)
if contactVel < minContactVel:
minContactVel = contactVel
if minContactVel < c_min_contact_vel: contact = True
extended[0] = False
if contact:
# print frame, 'foot touch'
lastFrame = True
acc_offset[0] += frame - curInterval[1]
elif frame == len(motion_seg) - 1:
print frame, 'extend frame', frame + 1
preserveJoints = []
# preserveJoints = [lFoot, rFoot]
# preserveJoints = [lFoot, rFoot, lKnee, rKnee]
# preserveJoints = [lFoot, rFoot, lKnee, rKnee, lUpLeg, rUpLeg]
stanceKnees = [
rKnee
] if curState == yba.GaitState.LSWING else [lKnee]
preserveJoints = [
stanceFoots[0], stanceKnees[0], stanceLegs[0]
]
diff = 3
motion_seg_orig.extend([motion_seg_orig[-1]])
motion_seg.extend(
ymt.extendByIntegration_root(motion_seg, 1, diff))
motion_stitch.extend(
ymt.extendByIntegration_constant(
motion_stitch, 1, preserveJoints, diff))
# # extend for swing foot ground speed matching & swing foot height lower
## extendedPostures = ymt.extendByIntegration(motion_stitch, 1, preserveJoints, diff)
## extendedPostures = [motion_stitch[-1]]
##
# extendFrameNum = frame - curInterval[1] + 1
# k = 1.-extendFrameNum/5.
# if k<0.: k=0.
# extendedPostures = ymt.extendByIntegrationAttenuation(motion_stitch, 1, preserveJoints, diff, k)
#
## if len(swingFoots)>0 and np.inner(dCM_tar, dCM)>0.:
## print frame, 'speed matching'
## R_swf = motion_stitch[-1].getJointOrientationGlobal(swingFoots[0])
##
## p_swf = motion_stitch[-1].getJointPositionGlobal(swingFoots[0])
## v_swf = motion_stitch.getJointVelocityGlobal(swingFoots[0], frame-diff, frame)
## a_swf = motion_stitch.getJointAccelerationGlobal(swingFoots[0], frame-diff, frame)
## p_swf += v_swf * (frameTime) + a_swf * (frameTime)*(frameTime)
## aik.ik_analytic(extendedPostures[0], swingFoots[0], p_swf)
##
## extendedPostures[0].setJointOrientationGlobal(swingFoots[0], R_swf)
#
# motion_stitch.extend(extendedPostures)
extended[0] = True
else:
if frame == len(motion_seg) - 1: lastFrame = True
if lastFrame:
if segIndex < len(segments) - 1:
print '%d (%d): end of %dth seg (%s, %s)' % (
frame, frame - curInterval[1], segIndex,
yba.GaitState.text[curState], curInterval)
if plot != None:
plot.addDataPoint('diff', frame,
(frame - curInterval[1]) * .01)
if len(stanceFoots) > 0 and len(swingFoots) > 0:
# step_cur = controlModel.getJointPositionGlobal(swingFoots[0]) - controlModel.getJointPositionGlobal(stanceFoots[0])
# step_tar = motion_seg[curInterval[1]].getJointPositionGlobal(swingFoots[0]) - motion_seg[curInterval[1]].getJointPositionGlobal(stanceFoots[0])
step_cur = controlModel.getJointPositionGlobal(
0) - controlModel.getJointPositionGlobal(
stanceFoots[0])
step_tar = motion_seg[
curInterval[1]].getJointPositionGlobal(0) - motion_seg[
curInterval[1]].getJointPositionGlobal(
stanceFoots[0])
step_cur = mm.projectionOnPlane(step_cur, (1, 0, 0),
(0, 0, 1))
step_tar = mm.projectionOnPlane(step_tar, (1, 0, 0),
(0, 0, 1))
step_cur_sag, step_cur_cor = mm.projectionOnVector2(
step_cur, direction)
step_tar_sag, step_tar_cor = mm.projectionOnVector2(
step_tar, direction)
step_length_tar[0] = mm.length(step_tar_sag)
if np.inner(step_tar_sag, step_cur_sag) > 0:
step_length_cur[0] = mm.length(step_cur_sag)
else:
step_length_cur[0] = -mm.length(step_cur_sag)
step_axis[0] = directionAxis
# rd_vec1[0] = step_tar_sag
# rd_vecori1[0] = motion_seg[curInterval[1]].getJointPositionGlobal(stanceFoots[0])
# rd_vec2[0] = step_cur_sag
# rd_vecori2[0] = controlModel.getJointPositionGlobal(stanceFoots[0])
seg_index[0] += 1
curSeg = segments[seg_index[0]]
stl_y_limit_num[0] = 0
stl_xz_limit_num[0] = 0
del motion_seg_orig[frame + 1:]
motion_seg_orig.extend(
ymb.getAttachedNextMotion(curSeg, motion_seg_orig[-1],
False, False))
del motion_seg[frame + 1:]
del motion_stitch[frame + 1:]
transitionLength = len(curSeg) - 1
# motion_seg.extend(ymb.getAttachedNextMotion(curSeg, motion_seg[-1], False, False))
# motion_stitch.extend(ymb.getStitchedNextMotion(curSeg, motion_control[-1], transitionLength, stitch_func, True, False))
d = motion_seg[-1] - curSeg[0]
d.rootPos[1] = 0.
motion_seg.extend(
ymb.getAttachedNextMotion(curSeg, d, True, False))
if NO_FOOT_SLIDING:
if segIndex == len(segments) - 2:
Rl = motion_control[-1].getJointOrientationLocal(
lUpLeg)
Rr = motion_control[-1].getJointOrientationLocal(
rUpLeg)
Rlk = motion_control[-1].getJointOrientationLocal(
lKnee)
Rrk = motion_control[-1].getJointOrientationLocal(
rKnee)
Rlf = motion_control[-1].getJointOrientationLocal(
lFoot)
Rrf = motion_control[-1].getJointOrientationLocal(
rFoot)
for p in curSeg:
p.setJointOrientationLocal(lUpLeg, Rl, False)
p.setJointOrientationLocal(rUpLeg, Rr, False)
p.setJointOrientationLocal(lKnee, Rlk, False)
p.setJointOrientationLocal(rKnee, Rrk, False)
p.setJointOrientationLocal(lFoot, Rlf, False)
p.setJointOrientationLocal(rFoot, Rrf, False)
p.updateGlobalT()
d = motion_control[-1] - curSeg[0]
d.rootPos[1] = 0.
motion_stitch.extend(
ymb.getStitchedNextMotion(curSeg, d, transitionLength,
stitch_func, True, False))
# motion_seg.extend(ymb.getAttachedNextMotion(curSeg, motion_seg[-1], False, True))
# motion_stitch.extend(ymb.getStitchedNextMotion(curSeg, motion_control[-1], transitionLength, stitch_func, True, True))
else:
motion_seg_orig.append(motion_seg_orig[-1])
motion_seg.append(motion_seg[-1])
motion_stitch.append(motion_control[-1])
# rendering
motionModel.update(motion_ori[frame])
# motionModel.update(motion_seg[frame])
if SAVE_SIMULATION:
p_temp = ym.JointPosture(skeleton)
p_temp.initLocalRs()
p_temp.rootPos = controlModel.getJointPositionGlobal(0)
p_temp.setJointOrientationsLocal(
controlModel.getJointOrientationsLocal())
motion_simulation.append(p_temp)
if frame == viewer.getMaxFrame():
saveFilePath = SAVE_DIR + 'simulated_' + filename
yf.writeBvhFile(saveFilePath, motion_simulation)
print saveFilePath, 'saved'
def preprocess():
tasks = []
# outputDir = './ppmotion/'
#
# dir = '../Data/woody2/Motion/Physics2/'
# config = {'repeat':True, 'footRot': mm.rotX(-.4), 'yOffset':0., 'halfFootHeight': 0.0444444444444, 'scale':.01, 'type':'woody2'}
# paths = []
# paths.append(dir+'wd2_WalkSameSame01.bvh')
# paths.append(dir+'wd2_WalkForwardSlow01.bvh')
# paths.append(dir+'wd2_WalkForwardNormal00.bvh')
# paths.append(dir+'wd2_WalkHandWav00.bvh')
# paths.append(dir+'wd2_WalkForwardFast00.bvh')
# paths.append(dir+'wd2_WalkForwardVFast00.bvh')
# paths.append(dir+'wd2_WalkLean00.bvh')
# paths.append(dir+'wd2_WalkAzuma01.bvh')
# paths.append(dir+'wd2_WalkSoldier00.bvh')
## paths.append(dir+'wd2_WalkSukiko00.bvh')
# paths.append(dir+'wd2_WalkBackward00.bvh')
## paths.append(dir+'wd2_WalkTongTong00.bvh')
## tasks.append({'config':config, 'paths':paths})
##
# dir = '../Data/woody2/Motion/Balancing/'
# config = {'footRot': mm.exp(mm.v3(1,-.5,0), -.6), 'yOffset': .0, 'halfFootHeight': 0.0444444444444, 'scale':.01, 'type':'woody2'}
# paths = []
# paths.append(dir+'wd2_2foot_walk_turn.bvh')
# paths.append(dir+'wd2_2foot_walk_turn2.bvh')
# paths.append(dir+'wd2_slow_2foot_hop.bvh')
# paths.append(dir+'wd2_short_broad_jump.bvh')
# paths.append(dir+'wd2_long_broad_jump.bvh')
# paths.append(dir+'wd2_ffast_cancan_run.bvh')
# paths.append(dir+'wd2_fast_cancan_run.bvh')
# paths.append(dir+'wd2_fast_2foot_hop.bvh')
# paths.append(dir+'wd2_1foot_contact_run.bvh')
# paths.append(dir+'wd2_1foot_contact_run2.bvh')
## tasks.append({'config':config, 'paths':paths})
#
## dir = '../Data/woody2/Motion/Samsung/'
## config = {'footRot': mm.rotX(-.46), 'yOffset': .0, 'halfFootHeight': 0.0444444444444, 'scale':.01*2.54, 'type':'woody2amc'}
## paths = []
## paths.append(dir+'wd2_left_turn.bvh')
## paths.append(dir+'wd2_right_turn.bvh')
## paths.append(dir+'wd2_pose_inner1.bvh')
## paths.append(dir+'wd2_pose_inner2.bvh')
## tasks.append({'config':config, 'paths':paths})
#
# dir = '../Data/woody2/Motion/Picking/'
# config = {'footRot': mm.rotX(-.5), 'yOffset': .0, 'halfFootHeight': 0.0444444444444, 'scale':.01, 'type':'woody2'}
# paths = []
# paths.append(dir+'wd2_pick_walk_1.bvh')
## tasks.append({'config':config, 'paths':paths})
#
# dir = '../Data/woody2/Motion/VideoMotion/'
# config = {'footRot': mm.rotX(-.48), 'yOffset': .0, 'halfFootHeight': 0.0444444444444, 'scale':.01*2.54, 'type':'woody2amc'}
## paths = glob.glob(dir+'*.bvh')
# paths = []
# paths.append(dir+'wd2_cross_walk_90d_fast_27.bvh')
## tasks.append({'config':config, 'paths':paths})
#
# dir = '../Data/woody2/Motion/Walking/'
# config = {'footRot': mm.rotX(-.40), 'yOffset': .0, 'halfFootHeight': 0.0444444444444, 'scale':.01*2.54, 'type':'woody2amc'}
# paths = glob.glob(dir+'*.bvh')
## tasks.append({'config':config, 'paths':paths})
#
## dir = '../Data/woody2/Motion/samsung_boxing/round/'
## config = {'footRot': mm.rotX(-.65), 'yOffset': .0, 'halfFootHeight': 0.0444444444444, 'scale':.01*2.54, 'type':'woody2amc'}
## paths = glob.glob(dir+'*.bvh')
## tasks.append({'config':config, 'paths':paths})
#
## dir = '../Data/woody2/Motion/samsung_boxing/boxman/'
## config = {'footRot': mm.rotX(-.5), 'yOffset': .0, 'halfFootHeight': 0.0444444444444, 'scale':.01*2.54, 'type':'woody2amc'}
## paths = glob.glob(dir+'*.bvh')
## tasks.append({'config':config, 'paths':paths})
#
# dir = '../Data/woody2/Motion/motion_edit/'
# config = {'footRot': mm.rotX(-.5), 'yOffset': .0, 'halfFootHeight': 0.0444444444444, 'scale':.01*2.54, 'type':'woody2amc'}
# paths = glob.glob(dir+'*.bvh')
## tasks.append({'config':config, 'paths':paths})
#
## outputDir = './icmotion_test/'
## dir = './rawmotion/'
## config = {'footRot': mm.rotX(-.5), 'yOffset': .0, 'halfFootHeight': 0.0444444444444, 'scale':1., 'type':'woody2_new'}
## paths = glob.glob(dir+'*.temp')
## tasks.append({'config':config, 'paths':paths})
#
# outputDir = './icmotion_last/'
# dir = './lastmotion/add/'
## config = {'rootRot':mm.rotZ(.05), 'footRot': mm.rotX(-.5), 'leftFootR':mm.rotZ(-.1), \
# config = {'rootRot':mm.rotZ(.0), 'footRot': np.dot(mm.rotX(-.5), mm.rotZ(.04)), 'leftFootR':mm.rotZ(-.2), \
# 'halfFootHeight': 0.0444444444444, 'scale':1., 'type':'woody2_new'}
# paths = glob.glob(dir+'*.temp')
# tasks.append({'config':config, 'paths':paths})
# outputDir = './ppmotion_slope/'
# dir = './rawmotion_slope_extracted/'
# config = {'rootRot':mm.rotZ(.0), 'footRot': np.dot(mm.rotX(-.55), mm.rotZ(.04)), 'leftFootR':mm.rotZ(-.2), \
# 'halfFootHeight': 0.0444444444444, 'scale':1., 'type':'woody2_new'}
# paths = glob.glob(dir+'*.bvh')
# tasks.append({'config':config, 'paths':paths})
VISUALIZE = False
for task in tasks:
config = task['config']
paths = task['paths']
for path in paths:
motion = yf.readBvhFile(path)
normalizeSkeleton(motion, config)
adjustHeight(motion, config['halfFootHeight'])
additionalEdit(motion, path)
outputPath = outputDir + os.path.basename(path)
yf.writeBvhFile(outputPath, motion)
print outputPath, 'done'
if 'repeat' in config and config['repeat']:
hRef = .1
vRef = .3
lc = yma.getElementContactStates(motion, 'LeftFoot', hRef,
vRef)
interval = yba.getWalkingCycle2(motion, lc)
transitionLength = 20 if 'wd2_WalkAzuma01.bvh' in path else 10
motion = ymt.repeatCycle(motion, interval, 50,
transitionLength)
outputName = os.path.splitext(
os.path.basename(path))[0] + '_REPEATED.bvh'
outputPath = outputDir + outputName
yf.writeBvhFile(outputPath, motion)
print outputPath, 'done'
if VISUALIZE:
viewer = ysv.SimpleViewer()
viewer.record(False)
viewer.doc.addRenderer(
'motion',
yr.JointMotionRenderer(motion, (0, 100, 255),
yr.LINK_LINE))
viewer.doc.addObject('motion', motion)
viewer.startTimer(1 / 30.)
viewer.show()
Fl.run()
print 'FINISHED'