def addJointSO3FromBvhJoint(self, jointPosture, bvhJoint, channelValues, scale=1.0): localR = mm.I_SO3() for channel in bvhJoint.channels: if channel.channelType == 'XPOSITION': jointPosture.rootPos[0] = channelValues[ channel.channelIndex] * scale elif channel.channelType == 'YPOSITION': jointPosture.rootPos[1] = channelValues[ channel.channelIndex] * scale elif channel.channelType == 'ZPOSITION': jointPosture.rootPos[2] = channelValues[ channel.channelIndex] * scale elif channel.channelType == 'XROTATION': localR = numpy.dot( localR, mm.rotX(mm.RAD * channelValues[channel.channelIndex])) elif channel.channelType == 'YROTATION': localR = numpy.dot( localR, mm.rotY(mm.RAD * channelValues[channel.channelIndex])) elif channel.channelType == 'ZROTATION': localR = numpy.dot( localR, mm.rotZ(mm.RAD * channelValues[channel.channelIndex])) jointPosture.setLocalR( jointPosture.skeleton.getElementIndex(bvhJoint.name), localR) for i in range(len(bvhJoint.children)): self.addJointSO3FromBvhJoint(jointPosture, bvhJoint.children[i], channelValues)
def _calcJointLocalR(joint, extendedPoints, initRot, jointPosture): if len(joint.children)!=1: curRot = mmMath.I_SO3() else: p = extendedPoints[_getValidParentName(joint)] - extendedPoints[joint.name] c = extendedPoints[joint.children[0].name] - extendedPoints[joint.name] curRot = mmMath.getSO3FromVectors(p, c) jointPosture.setLocalR(joint.name, numpy.dot(curRot, initRot[joint.name].transpose())) for childJoint in joint.children: _calcJointLocalR(childJoint, extendedPoints, initRot, jointPosture)
def test_zyx(): x, y, z = (1.,.5,2.) print 'orig ', x, y, z R = mm.I_SO3() R = np.dot(R, mm.rotZ(z)) R = np.dot(R, mm.rotX(x)) R = np.dot(R, mm.rotY(y)) nz, nx, ny = cm.R2zxy_r(R) print 'zxy_r', nx, ny, nz R = mm.I_SO3() R = np.dot(R, mm.rotY(y)) R = np.dot(R, mm.rotX(x)) R = np.dot(R, mm.rotZ(z)) nz, nx, ny = cm.R2zxy_s(R) print 'zxy_s', nx, ny, nz R = mm.I_SO3() R = np.dot(R, mm.rotX(x)) R = np.dot(R, mm.rotY(y)) R = np.dot(R, mm.rotZ(z)) nx, ny, nz = cm.R2xyz_r(R) print 'xyz_r', nx, ny, nz
def PDControl3(frame): # Kp = 100.*70; # Kd = 10.*3; Kp = 1000. Kd = 10. desiredOri1 = mm.exp(axis1, deg1[0] * M_RADIAN) desiredOriX = mm.exp(axisX, deg1[0] * M_RADIAN) desiredOriY = mm.exp(axisY, deg1[0] * M_RADIAN) desiredOriZ = mm.exp(axisZ, deg1[0] * M_RADIAN) calcPDTorque(mm.I_SO3(), mm.exp(axisX, -90 * M_RADIAN), base.getRotation(), body1.getRotation(), base.getAngularVel(), body1.getAngularVel(), Kp, Kd, M1)
def mm2Joint_Posture(pointPosture, pointTPose, jointSkeleton): jointPosture = ym.JointPosture(jointSkeleton) jointPosture.rootPos = pointPosture.pointMap[jointSkeleton.root.name] extendedPoints = {} for jointName, joint in jointSkeleton.joints.items(): if joint.parent == None: extendedPoints[ 'parent_' + jointName] = pointTPose.pointMap[jointName] + numpy.array( [1., 0., 0.]) extendedPoints[jointName] = pointTPose.pointMap[jointName] elif len(joint.parent.children) > 1: extendedPoints[jointName] = pointTPose.pointMap[joint.parent.name] else: extendedPoints[jointName] = pointTPose.pointMap[jointName] initRot = {} for jointName, joint in jointSkeleton.joints.items(): if len(joint.children) != 1: curRot = mmMath.I_SO3() else: p = extendedPoints[_getValidParentName(joint)] - extendedPoints[ joint.name] c = extendedPoints[joint.children[0].name] - extendedPoints[ joint.name] curRot = mmMath.getSO3FromVectors(p, c) initRot[joint.name] = curRot extendedPoints2 = {} for jointName, joint in jointSkeleton.joints.items(): if joint.parent == None: extendedPoints2[ 'parent_' + jointName] = pointPosture.pointMap[jointName] + numpy.array( [1., 0., 0.]) extendedPoints2[jointName] = pointPosture.pointMap[jointName] elif len(joint.parent.children) > 1: extendedPoints2[jointName] = pointPosture.pointMap[ joint.parent.name] else: extendedPoints2[jointName] = pointPosture.pointMap[jointName] _calcJointLocalR(jointSkeleton.root, extendedPoints2, initRot, jointPosture) return jointPosture
RIGHT_FOOT, RIGHT_TOES, RIGHT_TARSUS, RIGHT_FOOT_SIDE_L, RIGHT_FOOT_SIDE_R ] return motion, mcfg, wcfg, stepsPerFrame, config #=============================================================================== # biped config #=============================================================================== # motion, mesh config g_motionDirConfigMap = {} g_motionDirConfigMap['../Data/woody2/Motion/Physics2/'] = \ {'footRot': mm.exp(mm.v3(1,0,0), .05), 'yOffset': .0, 'scale':1.,\ 'rootRot': mm.I_SO3()} g_motionDirConfigMap['../Data/woody2/Motion/Balancing/'] = \ {'footRot': mm.exp(mm.v3(1,-.5,0), -.6), 'yOffset': .0, 'scale':1.,\ 'rootRot': mm.exp(mm.v3(1,0,0), .01)} g_motionDirConfigMap['../Data/woody2/Motion/VideoMotion/'] = \ {'footRot': mm.exp(mm.v3(1,0,0), -.05), 'yOffset': .01, 'scale':2.53999905501,\ 'rootRot': mm.exp(mm.v3(1,0,0), .0)} g_motionDirConfigMap['../Data/woody2/Motion/Samsung/'] = \ {'footRot': mm.exp(mm.v3(1,0,0), -.03), 'yOffset': .0, 'scale':2.53999905501,\ 'rootRot': mm.exp(mm.v3(1,0,0), .03)} #=============================================================================== # # reloadable config #=============================================================================== def buildMassMap():
node = mcfg.getNode('Spine') node.width = .22 node = mcfg.getNode('RightFoot') node.length = .25 node = mcfg.getNode('LeftFoot') node.length = .25 return mcfg g_motionDirConfigMap = {} g_motionDirConfigMap['../../../Data/woody2/Motion/Physics2/'] = \ {'footRot': mm.exp(mm.v3(1,0,0), -.4), 'yOffset': .0, 'scale':1.,'rootRot': mm.I_SO3()} g_motionDirConfigMap['../../../Data/woody2/Motion/Balancing/'] = \ {'footRot': mm.exp(mm.v3(1,-.5,0), -.6), 'yOffset': .0, 'scale':1.,'rootRot': mm.exp(mm.v3(1,0,0), .01)} g_motionDirConfigMap['../../../Data/woody2/Motion/Picking/'] = \ {'footRot': mm.exp(mm.v3(1,0,0), -.5), 'yOffset': .0, 'scale':1.,'rootRot': mm.I_SO3()} g_motionDirConfigMap['../../../Data/woody2/Motion/Samsung/'] = \ {'footRot': mm.rotX(-.5), 'yOffset': .0, 'scale':2.53999905501,'rootRot': mm.I_SO3()} g_motionDirConfigMap['../../../Data/woody2/Motion/VideoMotion/'] = \ {'footRot': mm.exp(mm.v3(1,0,0), -.05), 'yOffset': .01, 'scale':2.53999905501,'rootRot': mm.exp(mm.v3(1,0,0), .0)} if __name__=='__main__': #=============================================================================== # initialize motion #=============================================================================== motionFiles = []
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_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_dCM_sag_axis * K_swp_vel_sag * -t_swing_foot_placement)) R_swp_sag = np.dot(R_swp_sag, mm.exp(diff_CMr_sag_axis * K_swp_pos_sag * -t_swing_foot_placement)) else: R_swp_sag = np.dot(R_swp_sag, mm.exp(diff_dCM_sag_axis * K_swp_vel_sag_faster * -t_swing_foot_placement)) R_swp_sag = np.dot(R_swp_sag, mm.exp(diff_CMr_sag_axis * K_swp_pos_sag_faster * -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) if OLD_SWING_HEIGHT: height_tar = motion_swf_height[frame].getJointPositionGlobal(swingFoot)[1] - motion_swf_height[frame].getJointPositionGlobal(stanceFoot)[1] else: height_tar = motion_swf_height[prev_frame].getJointPositionGlobal(swingFoot)[1] - groundHeight d_height_tar = motion_swf_height.getJointVelocityGlobal(swingFoot, prev_frame)[1] # height_tar += c_swf_mid_offset * swf_height_sine_func(t) # 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)) if OLD_SWING_HEIGHT: height_cur = motion_swf_height[frame].getJointPositionGlobal(swingFoot)[1] - motion_swf_height[frame].getJointPositionGlobal(stanceFoot)[1] else: height_cur = controlModel.getJointPositionGlobal(swingFoot)[1] - halfFootHeight - c_swf_offset d_height_cur = controlModel.getJointVelocityGlobal(swingFoot)[1] if OLD_SWING_HEIGHT: offset_height = (height_tar - height_cur) * swf_height_func(t) * c5 else: offset_height = ((height_tar - height_cur) * c5 + (d_height_tar - d_height_cur) * c6) * swf_height_func(t) offset_sine = c_swf_mid_offset * swf_height_sine_func(t) # offset_sine = 0. 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: if HIGHER_OFFSET: 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, offset_height) plot.addDataPoint('debug2', frame, height_tar - 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)) R_stb = np.dot(R_stb, mm.exp(diff_CMr_axis * K_stb_pos * 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) + -mm.normalize2(fi.force)*.2) 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() # # 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) 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)) 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]) rd_CP[0] = CP rd_CMP[0] = (CMreal[0] - (F[0]/F[1])*CMreal[1], 0, CMreal[2] - (F[2]/F[1])*CMreal[1]) if plot!=None: plot.addDataPoint('zero', frame, 0) plot.updatePoints()
def calcPDTorque(self, posture): torques = {} joint = posture.skeleton.root R = mm.I_SO3() self._calcPDTorqueJoint(joint, R, posture, torques) return torques
def simulateCallback(frame): curTime = time.time() if frame % 30 == 1: pt[0] = time.time() global g_initFlag global forceShowFrame global forceApplyFrame global JsysPre global JsupPreL global JsupPreR global JsupPre global softConstPoint global stage global contactRendererName global desCOMOffset #motionModel.update(motion[0]) Kt, Kk, Kl, Kh, Ksc, Bt, Bl, Bh, B_CM, B_CMSd, B_Toe = viewer.GetParam( ) Dt = 2 * (Kt**.5) Dk = 2 * (Kk**.5) Dl = 2 * (Kl**.5) Dh = 2 * (Kh**.5) Dsc = 2 * (Ksc**.5) ''' if Bsc == 0.0 : viewer.doc.showRenderer('softConstraint', False) viewer.motionViewWnd.update(1, viewer.doc) else: viewer.doc.showRenderer('softConstraint', True) renderer1 = viewer.doc.getRenderer('softConstraint') renderer1.rc.setLineWidth(0.1+Bsc*3) viewer.motionViewWnd.update(1, viewer.doc) ''' pose = motion[0].copy() def solveIK(desComPos, desIdxs, desPos, desOri, cmW=10., posW=1., oriW=1.): numItr = 100 dt = .5 threshold = 0.1 for i in range(0, numItr): jPart_IK = [] print '----iter num', i IKModel.update(pose) th_r_IK = pose.getDOFPositions() jointPositions_IK = pose.getJointPositionsGlobal() jointAxeses_IK = pose.getDOFAxeses() linkPositions_IK = IKModel.getBodyPositionsGlobal() linkInertias_IK = IKModel.getBodyInertiasGlobal() CM_IK = yrp.getCM(linkPositions_IK, linkMasses, totalMass) print CM_IK P_IK = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions_IK, CM_IK, linkInertias_IK) yjc.computeJacobian2(Jsys_IK, DOFs, jointPositions_IK, jointAxeses_IK, linkPositions_IK, allLinkJointMasks) for j in range(0, len(desIdxs)): jPart_IK.append(Jsys_IK[6 * desIdxs[j]:6 * desIdxs[j] + 6]) J_IK, JAngCom_IK = np.vsplit(np.dot(P_IK, Jsys_IK), 2) dv_IK = cmW * (desComPos - CM_IK) for j in range(0, len(desIdxs)): J_IK = np.vstack((J_IK, jPart_IK[j])) pos_IK = IKModel.getBodyPositionGlobal(desIdxs[j]) dv_IK = np.append(dv_IK, posW * (desPos[j] - pos_IK)) ori_IK = IKModel.getBodyOrientationGlobal(desIdxs[j]) dv_IK = np.append(dv_IK, oriW * mm.logSO3(desOri[j] * ori_IK.T)) #print dv_IK[0:3] dth_IK_solve = npl.lstsq(J_IK, dv_IK) dth_IK_x = dth_IK_solve[0][:totalDOF] ype.nested(dth_IK_x, dth_IK) #print dth_IK[0][0:3] th_IK = yct.getIntegralDOF(th_r_IK, dth_IK, dt) pose.setDOFPositions(th_IK) if np.dot(dv_IK, dv_IK) < threshold: break linkPositions_ref = motionModel.getBodyPositionsGlobal() CM_ref = yrp.getCM(linkPositions_ref, linkMasses, totalMass) footCenterOffset = np.array([ viewer.objectInfoWnd.comOffsetX.value(), viewer.objectInfoWnd.comOffsetY.value(), viewer.objectInfoWnd.comOffsetZ.value() ]) #CM_IK_ref = footCenter + footCenterOffset CM_IK_ref = CM_ref + footCenterOffset #CM_IK_ref[1] = CM_ref[1] + footCenterOffset[1] motion[0].skeleton.getJointIndex(config['supLink']) #IKidxs = [indexFootL[0], indexFootR[0]] #IKdesPos = [motionModel.getBodyPositionGlobal(indexFootL[0]), motionModel.getBodyPositionGlobal(indexFootR[0])] #for i in range(0, 2): # #IKdesPos[i] += ModelOffset # IKdesPos[i][1] = 0.069 #IKori = [motionModel.getBodyOrientationGlobal(indexFootL[0]), motionModel.getBodyOrientationGlobal(indexFootR[0])] #IKdesOri = [None]*2 #for i in range(0, 2): # IKdesOri[i] = mm.I_SO3() IKidxs = config['Phalange'][0:1] + config['Phalange'][3:4] print IKidxs IKdesPos = [None] * len(IKidxs) IKdesOri = [None] * len(IKidxs) for i in range(0, len(IKidxs)): #print i IKdesPos[i] = motionModel.getBodyPositionGlobal(IKidxs[i]) IKdesPos[i][1] = 0.03 IKdesOri[i] = mm.I_SO3() print IKdesPos solveIK(CM_IK_ref, IKidxs, IKdesPos, IKdesOri) # tracking th_r_ori = motion.getDOFPositions(frame) th_r = copy.copy(th_r_ori) global leftHipTimer if viewer.objectInfoWnd.onLeftHip: leftHipTimer = 60 viewer.objectInfoWnd.onLeftHip = False if leftHipTimer > 0: viewer.objectInfoWnd.comOffsetX.value( 0.14 * np.sin(2 * 3.14 * leftHipTimer / 60.)) #viewer.objectInfoWnd.comOffsetZ.value(0.04*np.cos(2*3.14*leftHipTimer/90.)) #B_Hipd = viewer.objectInfoWnd.labelLeftHip.value() #newR1 = mm.exp(mm.v3(0.0,1.0,0.0), 3.14*0.5*B_Hipd/100.) #idx = motion[0].skeleton.getJointIndex('LeftUpLeg') #th_r[idx] = np.dot(th_r[idx], newR1) #idx = motion[0].skeleton.getJointIndex('RightUpLeg') #th_r[idx] = np.dot(th_r[idx], newR1) leftHipTimer -= 1 timeReport[0] += time.time() - curTime curTime = time.time() th = controlModel.getDOFPositions() dth_r = motion.getDOFVelocities(frame) dth = controlModel.getDOFVelocities() ddth_r = motion.getDOFAccelerations(frame) ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt) ddth_c = controlModel.getDOFAccelerations() ype.flatten(ddth_des, ddth_des_flat) ype.flatten(dth, dth_flat) ype.flatten(ddth_c, ddth_c_flat) # jacobian refFootL = motionModel.getBodyPositionGlobal(supL) refFootR = motionModel.getBodyPositionGlobal(supR) positionFootL = [None] * footPartNum positionFootR = [None] * footPartNum for i in range(footPartNum): positionFootL[i] = controlModel.getBodyPositionGlobal( indexFootL[i]) positionFootR[i] = controlModel.getBodyPositionGlobal( indexFootR[i]) linkPositions = controlModel.getBodyPositionsGlobal() linkVelocities = controlModel.getBodyVelocitiesGlobal() linkAngVelocities = controlModel.getBodyAngVelocitiesGlobal() linkInertias = controlModel.getBodyInertiasGlobal() jointPositions = controlModel.getJointPositionsGlobal() jointAxeses = controlModel.getDOFAxeses() CM = yrp.getCM(linkPositions, linkMasses, totalMass) dCM = yrp.getCM(linkVelocities, linkMasses, totalMass) CM_plane = copy.copy(CM) CM_plane[1] = 0. dCM_plane = copy.copy(dCM) dCM_plane[1] = 0. linkPositions_ref = motionModel.getBodyPositionsGlobal() linkVelocities_ref = motionModel.getBodyVelocitiesGlobal() linkAngVelocities_ref = motionModel.getBodyAngVelocitiesGlobal() linkInertias_ref = motionModel.getBodyInertiasGlobal() CM_ref = yrp.getCM(linkPositions_ref, linkMasses, totalMass) CM_plane_ref = copy.copy(CM_ref) CM_plane_ref[1] = 0. P = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions, CM, linkInertias) dP = ymt.getPureInertiaMatrixDerivative(dTO, linkMasses, linkVelocities, dCM, linkAngVelocities, linkInertias) timeReport[1] += time.time() - curTime curTime = time.time() yjc.computeJacobian2(Jsys, DOFs, jointPositions, jointAxeses, linkPositions, allLinkJointMasks) timeReport[2] += time.time() - curTime curTime = time.time() # yjc.computeJacobianDerivative2(dJsys, DOFs, jointPositions, jointAxeses, linkAngVelocities, linkPositions, allLinkJointMasks) if frame > 0: dJsys = (Jsys - JsysPre) * 30. else: dJsys = (Jsys - Jsys) JsysPre = Jsys.copy() timeReport[3] += time.time() - curTime curTime = time.time() bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce( bodyIDsToCheck, mus, Ks, Ds) CP = yrp.getCP(contactPositions, contactForces) for i in range(len(bodyIDsToCheck)): controlModel.SetBodyColor(bodyIDsToCheck[i], 0, 0, 0, 255) contactFlagFootL = [0] * footPartNum contactFlagFootR = [0] * footPartNum partialDOFIndex = [22, 22] for i in range(len(bodyIDs)): controlModel.SetBodyColor(bodyIDs[i], 255, 105, 105, 200) index = controlModel.id2index(bodyIDs[i]) for j in range(len(indexFootL)): if index == indexFootL[j]: contactFlagFootL[j] = 1 for j in range(len(indexFootR)): if index == indexFootR[j]: contactFlagFootR[j] = 1 for j in range(0, footPartNum): jAngFootR[j] = Jsys[6 * indexFootR[j]:6 * indexFootR[j] + 6][3:] #.copy() jAngFootL[j] = Jsys[6 * indexFootL[j]:6 * indexFootL[j] + 6][3:] #.copy() dJAngFootR[j] = dJsys[6 * indexFootR[j]:6 * indexFootR[j] + 6][3:] #.copy() dJAngFootL[j] = dJsys[6 * indexFootL[j]:6 * indexFootL[j] + 6][3:] #.copy() jFootR[j] = Jsys[6 * indexFootR[j]:6 * indexFootR[j] + 6] #.copy() jFootL[j] = Jsys[6 * indexFootL[j]:6 * indexFootL[j] + 6] #.copy() dJFootR[j] = dJsys[6 * indexFootR[j]:6 * indexFootR[j] + 6] #.copy() dJFootL[j] = dJsys[6 * indexFootL[j]:6 * indexFootL[j] + 6] #.copy() if footPartNum == 1: desFCL = (controlModel.getBodyPositionGlobal(supL)) desFCR = (controlModel.getBodyPositionGlobal(supR)) else: r = .5 + desCOMOffset desFCL = (controlModel.getBodyPositionGlobal(indexFootL[0]) * r + controlModel.getBodyPositionGlobal(indexFootL[1]) * (1.0 - r) ) #controlModel.getBodyPositionGlobal(indexFootL[1]) desFCR = (controlModel.getBodyPositionGlobal(indexFootR[0]) * r + controlModel.getBodyPositionGlobal(indexFootR[1]) * (1.0 - r) ) #controlModel.getBodyPositionGlobal(indexFootR[1]) desFC = desFCL + (desFCR - desFCL) / 2.0 desFC[1] = 0 rd_footCenter_des[0] = desFC.copy() curRelCMVec = CM_plane - desFC vecRatio = mm.length(curRelCMVec) * 0. #print(frame, vecRatio) footCenter = desFC - curRelCMVec * (vecRatio) #/10.0 footCenter = ( getBodyGlobalPos(controlModel, motion, 'LeftCalcaneus_1') + getBodyGlobalPos(controlModel, motion, 'LeftPhalange_1') + getBodyGlobalPos(controlModel, motion, 'RightCalcaneus_1') + getBodyGlobalPos(controlModel, motion, 'RightPhalange_1')) / 4. #footCenter = (getBodyGlobalPos(controlModel, motion, 'LeftCalcaneus_1') + getBodyGlobalPos(controlModel, motion, 'LeftTalus_1') + getBodyGlobalPos(controlModel, motion, 'RightCalcaneus_1') + getBodyGlobalPos(controlModel, motion, 'RightTalus_1'))/4. footCenter_ref = refFootL + (refFootR - refFootL) / 2.0 #footCenter_ref[1] = 0. footCenter[1] = 0. footCenterOffset = np.array([ viewer.objectInfoWnd.comOffsetX.value(), 0, viewer.objectInfoWnd.comOffsetZ.value() ]) #footCenter += footCenterOffset vecRatio = mm.length(curRelCMVec) * 0. softConstPointOffset = -curRelCMVec * (vecRatio) #/10.0 #print(frame, vecRatio, softConstPointOffset) desForeSupLAcc = [0, 0, 0] desForeSupRAcc = [0, 0, 0] totalNormalForce = [0, 0, 0] for i in range(len(contactForces)): totalNormalForce[0] += contactForces[i][0] totalNormalForce[1] += contactForces[i][1] totalNormalForce[2] += contactForces[i][2] #print((totalMass*mm.s2v(wcfg.gravity))[1]) footCenterOffset = np.array([ viewer.objectInfoWnd.comOffsetX.value(), viewer.objectInfoWnd.comOffsetY.value(), viewer.objectInfoWnd.comOffsetZ.value() ]) ###################### # optimization terms ###################### # linear momentum CM_ref_plane = footCenter + footCenterOffset dL_des_plane = Kl * totalMass * (CM_ref_plane - CM_plane) - Dl * totalMass * dCM_plane dL_des_plane[1] = Kl * totalMass * (CM_ref[1] + footCenterOffset[1] - CM[1]) - Dl * totalMass * dCM[1] # angular momentum CP_ref = footCenter + footCenterOffset timeStep = 30. if CP_old[0] == None or CP == None: dCP = None else: dCP = (CP - CP_old[0]) / (1 / timeStep) CP_old[0] = CP if CP != None and dCP != None: ddCP_des = Kh * (CP_ref - CP) - Dh * (dCP) CP_des = CP + dCP * (1 / timeStep) + .5 * ddCP_des * ( (1 / timeStep)**2) #dH_des = np.cross((CP_des - CM), (dL_des_plane + totalMass*mm.s2v(wcfg.gravity))) dH_des = np.cross( (CP_des - CM_plane), (dL_des_plane + totalMass * mm.s2v(wcfg.gravity))) else: dH_des = None # momentum matrix RS = np.dot(P, Jsys) R, S = np.vsplit(RS, 2) rs = np.dot((np.dot(dP, Jsys) + np.dot(P, dJsys)), dth_flat) r_bias, s_bias = np.hsplit(rs, 2) flagContact = True if dH_des == None or np.any(np.isnan(dH_des)) == True: flagContact = False #viewer.doc.showRenderer('rd_grf_des', False) #viewer.motionViewWnd.update(1, viewer.doc) #else: #viewer.doc.showRenderer('rd_grf_des', True) #viewer.motionViewWnd.update(1, viewer.doc) ''' 0 : initial 1 : contact 2 : fly 3 : landing ''' trackingW = w #if checkAll(contactFlagFootR, 0) != 1 : if 0: #stage == MOTION_TRACKING: trackingW = w2 #stage = POWERFUL_BALANCING Bt = Bt * 2 ####################### # optimization ####################### mot.addTrackingTerms(problem, totalDOF, Bt, trackingW, ddth_des_flat) #if flagContact == True: # mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R, r_bias) # mot.addAngularTerms(problem, totalDOF, Bh, dH_des, S, s_bias) a_sup_2 = None Jsup_2 = None dJsup_2 = None ############################## #if Jsup_2 != None: # mot.addConstraint(problem, totalDOF, Jsup_2, dJsup_2, dth_flat, a_sup_2) timeReport[4] += time.time() - curTime curTime = time.time() r = problem.solve() problem.clear() ype.nested(r['x'], ddth_sol) rootPos[0] = controlModel.getBodyPositionGlobal(selectedBody) localPos = [[0, 0, 0]] timeReport[5] += time.time() - curTime curTime = time.time() for i in range(stepsPerFrame): # apply penalty force bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce( bodyIDsToCheck, mus, Ks, Ds) vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals, contactForces) extraForce[0] = viewer.GetForce() if (extraForce[0][0] != 0 or extraForce[0][1] != 0 or extraForce[0][2] != 0): forceApplyFrame += 1 #vpWorld.applyPenaltyForce(selectedBodyId, localPos, extraForce) controlModel.applyBodyForceGlobal(selectedBody, extraForce[0]) applyedExtraForce[0] = extraForce[0] if forceApplyFrame * wcfg.timeStep > 0.1: viewer.ResetForce() forceApplyFrame = 0 controlModel.setDOFAccelerations(ddth_sol) controlModel.solveHybridDynamics() vpWorld.step() if frame % 30 == 0: print 'elapsed time for 30 frames:', time.time() - pt[0] # rendering rd_footCenter[0] = footCenter rd_CM[0] = CM.copy() rd_CM_plane[0] = CM_plane.copy() rd_footCenter_ref[0] = footCenter_ref rd_CM_plane_ref[0] = CM_ref.copy() rd_CM_ref[0] = CM_ref.copy() rd_CM_ref_vec[0] = (CM_ref - footCenter_ref) * 3. rd_CM_vec[0] = (CM - CM_plane) rd_CM_des[0] = CM_ref_plane.copy() rd_CM_des[0][1] = .01 #rd_CM_plane[0][1] = 0. if CP != None and dCP != None: rd_CP[0] = CP rd_CP_des[0] = CP_des rd_dL_des_plane[0] = dL_des_plane rd_dH_des[0] = dH_des rd_grf_des[ 0] = totalNormalForce # - totalMass*mm.s2v(wcfg.gravity)#dL_des_plane - totalMass*mm.s2v(wcfg.gravity) rd_exf_des[0] = applyedExtraForce[0] rd_root_des[0] = rootPos[0] rd_CMP[0] = softConstPoint rd_soft_const_vec[0] = controlModel.getBodyPositionGlobal( constBody) - softConstPoint #indexL = motion[0].skeleton.getJointIndex('Hips') #indexR = motion[0].skeleton.getJointIndex('Spine1') indexL = indexFootL[0] indexR = indexFootR[0] curAng = [controlModel.getBodyOrientationGlobal(indexL)] curAngY = np.dot(curAng, np.array([0, 0, 1])) rd_footL_vec[0] = np.copy(curAngY[0]) rd_footCenterL[0] = controlModel.getBodyPositionGlobal(indexL) curAng = [controlModel.getBodyOrientationGlobal(indexR)] curAngY = np.dot(curAng, np.array([0, 0, 1])) rd_footR_vec[0] = np.copy(curAngY[0]) rd_footCenterR[0] = controlModel.getBodyPositionGlobal(indexR) if (forceApplyFrame == 0): applyedExtraForce[0] = [0, 0, 0] timeReport[6] += time.time() - curTime