def simulateCallback(frame): global g_initFlag global forceShowFrame global forceApplyFrame global JsysPre global JsupPreL global JsupPreR global JsupPre global softConstPoint global stage motionModel.update(motion[frame]) Kt, Kk, Kl, Kh, Ksc, Bt, Bl, Bh, Bsc = 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) # tracking th_r = motion.getDOFPositions(frame) 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 ''' if stage == POWERFUL_BALANCING: #if stage != MOTION_TRACKING: footCenterL = controlModel.getBodyPositionGlobal(supL) footCenterR = controlModel.getBodyPositionGlobal(supR) else: footCenterL = controlModel.getBodyPositionGlobal(indexFootL[1]) footCenterR = controlModel.getBodyPositionGlobal(indexFootR[1]) ''' if footPartNum == 1: footCenterL = controlModel.getBodyPositionGlobal(supL) footCenterR = controlModel.getBodyPositionGlobal(supR) else: if stage == POWERFUL_BALANCING: footCenterL = controlModel.getBodyPositionGlobal(supL) footCenterR = controlModel.getBodyPositionGlobal(supR) else: footCenterL = ( controlModel.getBodyPositionGlobal(supL) + controlModel.getBodyPositionGlobal(indexFootL[1])) / 2.0 footCenterR = ( controlModel.getBodyPositionGlobal(supR) + controlModel.getBodyPositionGlobal(indexFootR[1])) / 2.0 refFootL = motionModel.getBodyPositionGlobal(supL) refFootR = motionModel.getBodyPositionGlobal(supR) footCenter = footCenterL + (footCenterR - footCenterL) / 2.0 footCenter[1] = 0. footCenter_ref = refFootL + (refFootR - refFootL) / 2.0 #footCenter_ref[1] = 0. 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() 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) yjc.computeJacobian2(Jsys, DOFs, jointPositions, jointAxeses, linkPositions, allLinkJointMasks) yjc.computeJacobianDerivative2(dJsys, DOFs, jointPositions, jointAxeses, linkAngVelocities, linkPositions, allLinkJointMasks) if g_initFlag == 0: softConstPoint = controlModel.getBodyPositionGlobal(constBody) softConstPoint[1] -= .3 g_initFlag = 1 yjc.computeJacobian2(jFootL[0], DOFs, jointPositions, jointAxeses, [positionFootL[0]], jointMasksFootL[0]) yjc.computeJacobianDerivative2(dJFootL[0], DOFs, jointPositions, jointAxeses, linkAngVelocities, [positionFootL[0]], jointMasksFootL[0], False) yjc.computeJacobian2(jFootR[0], DOFs, jointPositions, jointAxeses, [positionFootR[0]], jointMasksFootR[0]) yjc.computeJacobianDerivative2(dJFootR[0], DOFs, jointPositions, jointAxeses, linkAngVelocities, [positionFootR[0]], jointMasksFootR[0], False) yjc.computeAngJacobian2(jAngFootL[0], DOFs, jointPositions, jointAxeses, [positionFootL[0]], jointMasksFootL[0]) yjc.computeAngJacobianDerivative2(dJAngFootL[0], DOFs, jointPositions, jointAxeses, linkAngVelocities, [positionFootL[0]], jointMasksFootL[0], False) yjc.computeAngJacobian2(jAngFootR[0], DOFs, jointPositions, jointAxeses, [positionFootR[0]], jointMasksFootR[0]) yjc.computeAngJacobianDerivative2(dJAngFootR[0], DOFs, jointPositions, jointAxeses, linkAngVelocities, [positionFootR[0]], jointMasksFootR[0], False) 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) contactFlagFootL = [0] * footPartNum contactFlagFootR = [0] * footPartNum for i in range(len(bodyIDs)): controlModel.SetBodyColor(bodyIDs[i], 255, 105, 105) index = controlModel.id2index(bodyIDs[i]) for j in range(len(indexFootL)): if index == indexFootL[j]: contactFlagFootL[j] = 1 if j != 0: yjc.computeJacobian2(jFootL[j], DOFs, jointPositions, jointAxeses, [positionFootL[j]], jointMasksFootL[j]) yjc.computeJacobianDerivative2( dJFootL[j], DOFs, jointPositions, jointAxeses, linkAngVelocities, [positionFootL[j]], jointMasksFootL[j], False) break for j in range(len(indexFootR)): if index == indexFootR[j]: contactFlagFootR[j] = 1 if j != 0: yjc.computeJacobian2(jFootR[j], DOFs, jointPositions, jointAxeses, [positionFootR[j]], jointMasksFootR[j]) yjc.computeJacobianDerivative2( dJFootR[j], DOFs, jointPositions, jointAxeses, linkAngVelocities, [positionFootR[j]], jointMasksFootR[j], False) break for j in range(len(indexFootL)): yjc.computeAngJacobian2(jAngFootL[j], DOFs, jointPositions, jointAxeses, [positionFootL[j]], jointMasksFootL[j]) yjc.computeAngJacobianDerivative2(dJAngFootL[j], DOFs, jointPositions, jointAxeses, linkAngVelocities, [positionFootL[j]], jointMasksFootL[j], False) yjc.computeAngJacobian2(jAngFootR[j], DOFs, jointPositions, jointAxeses, [positionFootR[j]], jointMasksFootR[j]) yjc.computeAngJacobianDerivative2(dJAngFootR[j], DOFs, jointPositions, jointAxeses, linkAngVelocities, [positionFootR[j]], jointMasksFootR[j], False) # if checkAll(contactFlagFootL, 0) == 1 and checkAll( contactFlagFootR, 0) == 1: footCenter = footCenter elif checkAll(contactFlagFootL, 0) == 1: footCenter = footCenterR elif checkAll(contactFlagFootR, 0) == 1: footCenter = footCenterL footCenter[1] = 0. 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] # linear momentum CM_ref_plane = footCenter dL_des_plane = Kl * totalMass * (CM_ref_plane - CM_plane) - Dl * totalMass * dCM_plane # angular momentum CP_ref = footCenter 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) ############################## # soft point constraint P_des = softConstPoint P_cur = controlModel.getBodyPositionGlobal(constBody) dP_des = [0, 0, 0] dP_cur = controlModel.getBodyVelocityGlobal(constBody) ddP_des1 = Ksc * (P_des - P_cur) - Dsc * (dP_cur - dP_des) r = P_des - P_cur I = np.vstack(([1, 0, 0], [0, 1, 0], [0, 0, 1])) Z = np.hstack((I, mm.getCrossMatrixForm(-r))) yjc.computeJacobian2(Jconst, DOFs, jointPositions, jointAxeses, [softConstPoint], constJointMasks) JL, JA = np.vsplit(Jconst, 2) Q1 = np.dot(Z, Jconst) q1 = np.dot(JA, dth_flat) q2 = np.dot(mm.getCrossMatrixForm(q1), np.dot(mm.getCrossMatrixForm(q1), r)) yjc.computeJacobianDerivative2(dJconst, DOFs, jointPositions, jointAxeses, linkAngVelocities, [softConstPoint], constJointMasks, False) q_bias1 = np.dot(np.dot(Z, dJconst), dth_flat) + q2 ############################## 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 ''' #MOTION = FORWARD_JUMP if mit.MOTION == mit.FORWARD_JUMP: frame_index = [136, 100] #frame_index = [100000, 100000] elif mit.MOTION == mit.TAEKWONDO: frame_index = [130, 100] #frame_index = [100000, 100000] else: frame_index = [1000000, 1000000] #MOTION = TAEKWONDO #frame_index = [135, 100] ''' if frame > 300 : if stage != DYNAMIC_BALANCING: print("#", frame,"-DYNAMIC_BALANCING") stage = DYNAMIC_BALANCING Kk = Kk*1 Dk = 2*(Kk**.5) ''' if frame > frame_index[0]: if stage != POWERFUL_BALANCING: print("#", frame, "-POWERFUL_BALANCING") stage = POWERFUL_BALANCING Kk = Kk * 2 Dk = 2 * (Kk**.5) elif frame > frame_index[1]: if stage != MOTION_TRACKING: print("#", frame, "-MOTION_TRACKING") stage = MOTION_TRACKING trackingW = w if stage == MOTION_TRACKING: trackingW = w2 Bt = Bt * 2 # optimization mot.addTrackingTerms(problem, totalDOF, Bt, trackingW, ddth_des_flat) mot.addSoftPointConstraintTerms(problem, totalDOF, Bsc, ddP_des1, Q1, q_bias1) if flagContact == True: if stage != MOTION_TRACKING + 10: 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] ############################## # Hard constraint if stage != MOTION_TRACKING: Kk2 = Kk * 2.0 else: Kk2 = Kk * 1.5 Dk2 = 2 * (Kk2**.5) ''' desLinearAccL, desPosL = getDesFootLinearAcc(motionModel, controlModel, supL, ModelOffset, CM_ref, CM, Kk2, Dk2) desLinearAccR, desPosR = getDesFootLinearAcc(motionModel, controlModel, supR, ModelOffset, CM_ref, CM, Kk2, Dk2) desAngularAccL = getDesFootAngularAcc(motionModel, controlModel, supL, Kk2, Dk2) desAngularAccR = getDesFootAngularAcc(motionModel, controlModel, supR, Kk2, Dk2) ''' if stage != MOTION_TRACKING: idx = 0 #LEFT/RIGHT_TOES desLinearAccL, desPosL = getDesFootLinearAcc( motionModel, controlModel, indexFootL[idx], ModelOffset, CM_ref, CM, Kk2, Dk2) desLinearAccR, desPosR = getDesFootLinearAcc( motionModel, controlModel, indexFootR[idx], ModelOffset, CM_ref, CM, Kk2, Dk2) desAngularAccL = getDesFootAngularAcc(motionModel, controlModel, indexFootL[idx], Kk2, Dk2) desAngularAccR = getDesFootAngularAcc(motionModel, controlModel, indexFootR[idx], Kk2, Dk2) a_sup_2 = np.hstack((np.hstack((desLinearAccL, desAngularAccL)), np.hstack((desLinearAccR, desAngularAccR)))) Jsup_2 = np.vstack((jFootL[idx], jFootR[idx])) dJsup_2 = np.vstack((dJFootL[idx], dJFootR[idx])) rd_DesPosL[0] = desPosL.copy() rd_DesPosR[0] = desPosR.copy() else: if footPartNum != 1: idx = 1 else: idx = 0 desAngularAccL = getDesFootAngularAcc(motionModel, controlModel, indexFootL[idx], Kk2, Dk2) desAngularAccR = getDesFootAngularAcc(motionModel, controlModel, indexFootR[idx], Kk2, Dk2) a_sup_2 = np.hstack((desAngularAccL, desAngularAccR)) Jsup_2 = np.vstack((jAngFootL[idx], jAngFootR[idx])) dJsup_2 = np.vstack((dJAngFootL[idx], dJAngFootR[idx])) ############################## ############################## # Additional constraint if stage != MOTION_TRACKING: Kk2 = Kk * 1.5 Dk2 = 2 * (Kk2**.5) desForePosL = [0, 0, 0] desForePosR = [0, 0, 0] desRearPosL = [0, 0, 0] desRearPosR = [0, 0, 0] for i in range(1, footPartNum): if contactFlagFootL[i] == 1: desLinearAccL, desForePosL = getDesFootLinearAcc( motionModel, controlModel, indexFootL[i], ModelOffset, CM_ref, CM, Kk2, Dk2) desAngularAccL = getDesFootAngularAcc( motionModel, controlModel, indexFootL[i], Kk2, Dk2) a_sup_2 = np.hstack( (a_sup_2, np.hstack((desLinearAccL, desAngularAccL)))) Jsup_2 = np.vstack((Jsup_2, jFootL[i])) dJsup_2 = np.vstack((dJsup_2, dJFootL[i])) if contactFlagFootR[i] == 1: desLinearAccR, desForePosR = getDesFootLinearAcc( motionModel, controlModel, indexFootR[i], ModelOffset, CM_ref, CM, Kk2, Dk2) desAngularAccR = getDesFootAngularAcc( motionModel, controlModel, indexFootR[i], Kk2, Dk2) a_sup_2 = np.hstack( (a_sup_2, np.hstack((desLinearAccR, desAngularAccR)))) Jsup_2 = np.vstack((Jsup_2, jFootR[i])) dJsup_2 = np.vstack((dJsup_2, dJFootR[i])) rd_DesForePosL[0] = desForePosL rd_DesForePosR[0] = desForePosR rd_DesRearPosL[0] = desRearPosL rd_DesRearPosR[0] = desRearPosR ############################## mot.setConstraint(problem, totalDOF, Jsup_2, dJsup_2, dth_flat, a_sup_2) r = problem.solve() problem.clear() ype.nested(r['x'], ddth_sol) rootPos[0] = controlModel.getBodyPositionGlobal(selectedBody) localPos = [[0, 0, 0]] 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() ''' 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) applyedExtraForce[0] = extraForce[0] if forceApplyFrame*wcfg.timeStep > 0.1: viewer.ResetForce() forceApplyFrame = 0 ''' vpWorld.step() # 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 - footCenter) * 3 #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 if (forceApplyFrame == 0): applyedExtraForce[0] = [0, 0, 0]
def setupQP(self, frame, motion, mcfg, model, world, config, timestep): motionModel = cvm.VpMotionModel(world, motion[frame], mcfg) # constants invdt = 1./timestep # dofs and flat data structure totalDOF = model.getTotalDOF() DOFs = model.getDOFs() self.totalDOF = totalDOF self.ddth_des_flat = ype.makeFlatList(totalDOF) self.ddth_r_flat = ype.makeFlatList(totalDOF) self.dth_flat = ype.makeFlatList(totalDOF) self.dth_r_flat = ype.makeFlatList(totalDOF) self.ddth_sol = ype.makeNestedList(DOFs) # momentum matrix linkMasses = model.getBodyMasses() totalMass = model.getTotalMass() TO = ymt.make_TO(linkMasses) dTO = ymt.make_dTO(len(linkMasses)) # optimization self.qp.clear() Vc_tmp = self.Vc_tmp # tracking w = self.getTrackingWeight(DOFs, motion[0].skeleton, config['weightMap']) th_r = motion.getDOFPositionsLocal(frame) th = model.getDOFPositionsLocal() dth_r = motion.getDOFVelocitiesLocal(frame) dth = model.getDOFVelocitiesLocal() ddth_r = motion.getDOFAccelerationsLocal(frame) ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r, self.Kt, self.Dt) linkPositions = model.getBodyPositionsGlobal() linkVelocities = model.getBodyVelocitiesGlobal() linkAngVelocities = model.getBodyAngVelocitiesGlobal() linkInertias = model.getBodyInertiasGlobal() jointPositions = model.getJointPositionsGlobal() jointAxeses = model.getDOFAxesesLocal() #linkPositions_ref = motionModel.getBodyPositionsGlobal() #linkVelocities_ref = motionModel.getBodyVelocitiesGlobal() #linkAngVelocities_ref = motionModel.getBodyAngVelocitiesGlobal() #linkInertias_ref = motionModel.getBodyInertiasGlobal() #jointPositions_ref = motionModel.getJointPositionsGlobal() #jointAxeses_ref = motionModel.getDOFAxesesLocal() ype.flatten(ddth_des, self.ddth_des_flat) ype.flatten(dth, self.dth_flat) ype.flatten(dth_r, self.dth_r_flat) ype.flatten(ddth_r, self.ddth_r_flat) # get CoM 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. P = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions, CM, linkInertias) dP = ymt.getPureInertiaMatrixDerivative(dTO, linkMasses, linkVelocities, dCM, linkAngVelocities, linkInertias) # jacobian Jsup = yjc.makeEmptyJacobian(DOFs, 1) dJsup = Jsup.copy() Jsys_old = None if self.Jsys != None: Jsys_old = self.Jsys.copy() if self.Jsys == None: self.Jsys = yjc.makeEmptyJacobian(DOFs, model.getBodyNum()) self.dJsys = self.Jsys.copy() allLinkJointMasks = yjc.getAllLinkJointMasks(motion[0].skeleton) yjc.computeJacobian2(self.Jsys, DOFs, jointPositions, jointAxeses, linkPositions, allLinkJointMasks) if Jsys_old ==None: self.dJsys = self.Jsys-self.Jsys else: self.dJsys = (self.Jsys - Jsys_old)*invdt #yjc.computeJacobianDerivative2(self.dJsys, DOFs, jointPositions, jointAxeses, linkAngVelocities, linkPositions, allLinkJointMasks) #CM_ref = yrp.getCM(linkPositions_ref, linkMasses, totalMass) #dCM_ref = yrp.getCM(linkVelocities_ref, linkMasses, totalMass) #CM_ref_plane = copy.copy(CM_ref); CM_ref_plane[1]=0. #dCM_ref_plane = copy.copy(dCM_ref); dCM_ref_plane[1]=0. #P_ref = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions_ref, CM_ref, linkInertias_ref) #dP_ref = ymt.getPureInertiaMatrixDerivative(dTO, linkMasses, linkVelocities_ref, dCM_ref, linkAngVelocities_ref, linkInertias_ref) # get EoM totalActuator = totalDOF invM = np.zeros((totalActuator,totalDOF)) invMc = np.zeros(totalDOF) model.getInverseEquationOfMotion(invM, invMc) #print invMc # contact detection Ks = 1 Ds = 1 supsupR = motion[0].skeleton.getJointIndex('RightLeg') supsupL = motion[0].skeleton.getJointIndex('LeftLeg') supR = motion[0].skeleton.getJointIndex('RightFoot') supL = motion[0].skeleton.getJointIndex('LeftFoot') bodyIDsToCheck = range(world.getBodyNum()) #print bodyIDsToCheck #bodyIDsToCheck = [supsupR, supsupL] #bodyIDsToCheck = [supR, supL] mus = [.5]*len(bodyIDsToCheck) bodyIDs, contactPositions, contactPositionLocals, contactForces, contactVelocities = world.calcPenaltyForce(bodyIDsToCheck, mus, Ks, Ds) #bodyIDs, contactPositions, contactPositionLocals, contactForces, contactVelocities = world.calcManyPenaltyForce(self.contactPerSide, bodyIDsToCheck, mus, Ks, Ds) #bodyIDs, contactPositions, contactPositionLocals, contactForces, contactVelocities = world.calcOnePenaltyForce(bodyIDsToCheck, mus, Ks, Ds) #print bodyIDs, contactPositions footCenterL = model.getBodyPositionGlobal(supL) footCenterR = model.getBodyPositionGlobal(supR) footCenter = footCenterL.copy() footRefCenterL = motionModel.getBodyPositionGlobal(supL) footRefCenterR = motionModel.getBodyPositionGlobal(supR) #if supL in bodyIDs: #if supR in bodyIDs: #footCenter = footCenterL + (footCenterR-footCenterL)/2. #else: #footCenter = footCenterL.copy() #else: #if supR in bodyIDs: #footCenter = footCenterR.copy() #else: #footCenter = np.array((0,0,0)) contactL = 1 contactR = 1 if footRefCenterL[1] < 0.2: if footRefCenterR[1] < 0.2: footCenter = footCenterL + (footCenterR-footCenterL)/2. else: footCenter = footCenterL.copy() contactR = 0 else: contactL = 0 if footRefCenterR[1] < 0.2: footCenter = footCenterR.copy() else: footCenter = np.array((0,0,0)) contactR = 0 #print(contactR, contactL) footCenter[1] = 0. # linear momentum CM_ref = footCenter #CM_ref = #dL_des_plane = self.Kl*totalMass*(CM_ref - CM) + self.Dl*totalMass*(dCM_ref - dCM) dL_des_plane = self.Kl*totalMass*(CM_ref - CM) + self.Dl*totalMass*(-dCM) dL_des_plane[1] = 0. # angular momentum CP_ref = footCenter #bodyIDs, contactPositions, contactPositionLocals, contactForces = world.calcManyPenaltyForce(bodyIDsToCheck, mus, Ks, Ds) #CP = yrp.getCP(contactPositions, contactForces) CP = yrp.getSimpleCP(contactPositions) if self.CP_old==None or CP==None: dCP = None else: dCP = (CP - self.CP_old[0])/(1/30.) self.CP_old = CP if CP!=None and dCP!=None: ddCP_des = self.Kh*(CP_ref - CP) - self.Dh*(dCP) CP_des = CP + dCP*(1/30.) + .5*ddCP_des*((1/30.)**2) dH_des = np.cross((CP_des - CM), (dL_des_plane + totalMass*np.array((0,-9.8,0)))) #if contactChangeCount >0: # and contactChangeType == 'DtoS': ##dH_des *= (maxContactChangeCount - contactChangeCount)/(maxContactChangeCount*10) #dH_des *= (self.maxContactChangeCount - self.contactChangeCount)/(self.maxContactChangeCount) ##dH_des *= (contactChangeCount)/(maxContactChangeCount)*.9+.1 else: dH_des = None H = np.dot(P, np.dot(self.Jsys, self.dth_flat)) dH_des = -self.Kh*H[3:] # equality constraints JcTVc_append = np.zeros((totalDOF, 0)) VcTJc_list = [] VcTdJc_list = [] dVcTJc_list = [] ac_offset_list = [] totalContact = 4*len(bodyIDs) totalProblem = totalDOF+totalActuator+totalContact preSup = -1 for i in range(len(contactPositions)): sup = bodyIDs[i] supJointMasks = [yjc.getLinkJointMask(motion[0].skeleton, sup)] if preSup != sup: bodyPos = linkPositions[sup] bodyVel = linkVelocities[sup] #yjc.computeJacobian2(Jsup, DOFs, jointPositions, jointAxeses, [bodyPos], supJointMasks) #yjc.computeJacobianDerivative2(dJsup, DOFs, jointPositions, jointAxeses, linkAngVelocities, [bodyPos], supJointMasks) Jsup = getPartJacobian(self.Jsys, sup) dJsup = getPartJacobian(self.dJsys, sup) R_dAd = np.hstack( (np.vstack( (np.eye(3), mm.getCrossMatrixForm(-bodyPos)) ), np.vstack( (np.zeros((3,3)), np.eye(3)) ) ) ) dR_dAd = np.hstack( (np.vstack( (np.eye(3), mm.getCrossMatrixForm(-bodyVel)) ), np.vstack( (np.zeros((3,3)), np.eye(3)) ) ) ) #R_dAd = np.hstack( (np.vstack( (np.eye(3), mm.getCrossMatrixForm(-contactPositions[i])) ), np.vstack( (np.zeros((3,3)), np.eye(3)) ) ) ) #dR_dAd = np.hstack( (np.vstack( (np.eye(3), mm.getCrossMatrixForm(-contactVelocities[i])) ), np.vstack( (np.zeros((3,3)), np.eye(3)) ) ) ) p = contactPositions[i] dotp = contactVelocities[i] VcT_tmp = np.zeros((4,6)) dVcT_tmp = VcT_tmp.copy() for ii in range(4): n = Vc_tmp[ii] pcn = np.cross(contactPositions[i], Vc_tmp[ii]) VcT_tmp[ii][:3] =n VcT_tmp[ii][3:] =pcn dotpcn = np.cross(contactVelocities[i], Vc_tmp[ii]) dVcT_tmp[ii][3:] = dotpcn Vc = np.dot(R_dAd, VcT_tmp.T) dVc = np.dot(R_dAd, dVcT_tmp.T) + np.dot(dR_dAd, VcT_tmp.T) JcTVc = np.dot( Jsup.T, Vc) JcTVc_append = np.hstack((JcTVc_append, JcTVc)) VcTJc_list.append( JcTVc.T ) VcTdJc_list.append( np.dot(Vc.T, dJsup) ) dVcTJc_list.append( np.dot(dVc.T, Jsup) ) #TODO: # when friction cones and velocity cones differ? #JcTVc = np.dot( Jsup.T, VcT.T) #JcTVc_append = np.hstack((JcTVc_append, JcTVc)) #VcTJc_list.append( JcTVc.T ) #VcTdJc_list.append( np.dot(VcT, dJsup) ) #dVcTJc_list.append( np.dot(dVcT, Jsup) ) penDepth = -0.05-contactPositions[i][1] if penDepth < 0.: penDepth = 0. #penDepth = 0. ac_offset = 1000.*penDepth*np.ones(4) ac_offset_list.append(ac_offset) preSup = sup extForce = np.zeros(totalActuator) if self.extDuration > 0: Jext = yjc.makeEmptyJacobian(DOFs, 1) extForcePos = model.getBodyPositionGlobal(self.extForceBody) extJointMasks = [yjc.getLinkJointMask(motion[0].skeleton, self.extForceBody)] yjc.computeJacobian2(Jext, DOFs, jointPositions, jointAxeses, [extForcePos], extJointMasks) extForce = np.dot(Jext.T, self.extForce) self.extDuration -= timestep if self.extDuration < 0: self.extDuration = 0 self.addQPEqualityInverseEomConstraint(totalProblem, totalDOF, totalActuator, totalContact, invM, invMc, JcTVc_append, extForce) # inequality constraints if totalContact> 0: self.addQPInequalityContactForceConstraint(totalProblem, totalDOF, totalActuator, totalContact) self.addQPInequalityVelocityConstraint(totalProblem, totalDOF, totalActuator, totalContact, VcTJc_list, VcTdJc_list,dVcTJc_list, self.dth_flat, ac_offset_list, invdt) #self.addQPInequalityVelocityConstraint(totalProblem, totalDOF, totalActuator, totalContact, VcTJc_vel_list, VcTdJc_vel_list,dVcTJc_vel_list, self.dth_flat, ac_offset_list, 30.) torqueMax = 1000.*np.ones(totalActuator-6) torqueMin = -torqueMax self.addQPInequalityTorqueConstraint(totalProblem, totalDOF, totalActuator, totalContact, torqueMax, torqueMin) # objectives self.addQPTrackingTerms(totalProblem, 0, totalDOF, self.Bt, w, self.ddth_des_flat) self.addQPTorqueTerms(totalProblem, totalDOF, totalActuator, self.Btau, w) if totalContact > 0: self.addQPContactForceTerms(totalProblem, totalDOF+totalActuator, totalContact, self.Bcon) #if dH_des !=None: # allLinkJointMasks = yjc.getAllLinkJointMasks(motion[0].skeleton) # yjc.computeJacobian2(Jsys, DOFs, jointPositions, jointAxeses, linkPositions, allLinkJointMasks) # yjc.computeJacobianDerivative2(dJsys, DOFs, jointPositions, jointAxeses, linkAngVelocities, linkPositions, allLinkJointMasks) # self.addLinearAndAngularBalancigTerms(totalProblem, 0, totalDOF, self.Bl, self.Bh, P, self.Jsys, self.dth_flat, dP, self.dJsys, dL_des_plane, dH_des) # end effector #TODO: eeList = [supR, supL] #eeList = [] #if totalContact > 0: for ee in eeList: eeCenter = model.getBodyPositionGlobal(ee) eeJointMasks = [yjc.getLinkJointMask(motion[0].skeleton, ee)] yjc.computeJacobian2(Jsup, DOFs, jointPositions, jointAxeses, [eeCenter], eeJointMasks) yjc.computeJacobianDerivative2(dJsup, DOFs, jointPositions, jointAxeses, linkAngVelocities, [eeCenter], eeJointMasks, False) ee_genvel_ref = np.dot(Jsup, self.dth_r_flat) ee_genacc_ref = np.dot(Jsup, self.ddth_r_flat) + np.dot(dJsup, self.dth_r_flat) ee_pos_ref = motionModel.getBodyPositionGlobal(ee) ee_pos = model.getBodyPositionGlobal(ee) ee_vel_ref = ee_genvel_ref[:3] ee_vel = model.getBodyVelocityGlobal(ee) ee_acc_ref = ee_genacc_ref[:3] ddp_des_pos = self.Ke*( (ee_pos_ref-th_r[0][0]) - (ee_pos-th[0][0]) ) ddp_des_pos += self.De*(ee_vel_ref - ee_vel) ddp_des_pos += ee_acc_ref eeOri = model.getBodyOrientationGlobal(ee) eeAngVel = model.getBodyAngVelocityGlobal(ee) ee_angvel_ref = ee_genvel_ref[3:] ee_angacc_ref = ee_genacc_ref[3:] a_ori_diff = mm.logSO3(mm.getSO3FromVectors(np.dot(eeOri, np.array([0,1,0])), np.array([0,1,0]))) ddp_des_ang = self.Ke*a_ori_diff + self.De*(-eeAngVel) #ddp_des_ang = self.Ke*a_ori_diff + self.De*(ee_angvel_ref-eeAngVel) #ddp_des_ang += ee_angacc_ref ddp_des = np.hstack( (ddp_des_pos, ddp_des_ang) ) #self.addEndEffectorTerms(totalProblem, 0, totalDOF, Jsup, dJsup, self.dth_flat, ddp_des, self.Be) self.addEqualityEndEffectorTerms(totalProblem, 0, totalDOF, Jsup, dJsup, self.dth_flat, ddp_des, self.Be) return contactPositions, CP, CM, footCenter, dL_des_plane, CM_ref
def simulateCallback(frame): global g_initFlag global forceShowFrame global forceApplyFrame global JsysPre global JsupPreL global JsupPreR global JsupPre global softConstPoint global stage global contactRendererName motionModel.update(motion[frame]) Kt, Kk, Kl, Kh, Ksc, Bt, Bl, Bh, Bsc = 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) # tracking th_r = motion.getDOFPositions(frame) 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() 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) yjc.computeJacobian2(Jsys, DOFs, jointPositions, jointAxeses, linkPositions, allLinkJointMasks) yjc.computeJacobianDerivative2(dJsys, DOFs, jointPositions, jointAxeses, linkAngVelocities, linkPositions, allLinkJointMasks) yjc.computeJacobian2(jFootL[0], DOFs, jointPositions, jointAxeses, [positionFootL[0]], jointMasksFootL[0]) yjc.computeJacobianDerivative2(dJFootL[0], DOFs, jointPositions, jointAxeses, linkAngVelocities, [positionFootL[0]], jointMasksFootL[0], False) yjc.computeJacobian2(jFootR[0], DOFs, jointPositions, jointAxeses, [positionFootR[0]], jointMasksFootR[0]) yjc.computeJacobianDerivative2(dJFootR[0], DOFs, jointPositions, jointAxeses, linkAngVelocities, [positionFootR[0]], jointMasksFootR[0], False) yjc.computeAngJacobian2(jAngFootL[0], DOFs, jointPositions, jointAxeses, [positionFootL[0]], jointMasksFootL[0]) yjc.computeAngJacobianDerivative2(dJAngFootL[0], DOFs, jointPositions, jointAxeses, linkAngVelocities, [positionFootL[0]], jointMasksFootL[0], False) yjc.computeAngJacobian2(jAngFootR[0], DOFs, jointPositions, jointAxeses, [positionFootR[0]], jointMasksFootR[0]) yjc.computeAngJacobianDerivative2(dJAngFootR[0], DOFs, jointPositions, jointAxeses, linkAngVelocities, [positionFootR[0]], jointMasksFootR[0], False) 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) ########################################## for i in range(len(rd_contactPositions)): rd_contactPositions[i] = [0,0,0] rd_contactForces[i] = [0,0,0] for i in range(len(contactPositions)): rd_contactPositions[i] = np.copy(contactPositions[i]) rd_contactForces[i] = np.copy(contactForces[i]) ''' if len(contactPositions) > 0: rd_contactPositions = np.copy(contactPositions) rd_contactForces = np.copy(contactForces) print("rd_contactPositions", rd_contactPositions) print("contactPositions", contactPositions) ''' ''' for i in range(len(contactRendererName)): viewer.doc.removeRenderer(contactRendererName[i]) del contactRendererName[:] for i in range(len(contactPositions)): contactRendererName.append(str(i)) #viewer.doc.addRenderer(str(i), yr.PointsRenderer([contactPositions[i]], (0,255,0))) viewer.doc.addRenderer(str(i), yr.ForcesRenderer([contactForces[i]], [contactPositions[i]], (0,255,0), .009, 0.009)) viewer.motionViewWnd.update(1, viewer.doc) ''' ########################################## contactFlagFootL = [0]*footPartNum contactFlagFootR = [0]*footPartNum partialDOFIndex = [22, 22] for i in range(len(bodyIDs)) : controlModel.SetBodyColor(bodyIDs[i], 255, 105, 105) index = controlModel.id2index(bodyIDs[i]) for j in range(len(indexFootL)): if index == indexFootL[j]: contactFlagFootL[j] = 1 if j != 0: yjc.computePartialJacobian2(jFootL[j], DOFs, jointPositions, jointAxeses, [positionFootL[j]], jointMasksFootL[j], partialDOFIndex) yjc.computePartialJacobianDerivative2(dJFootL[j], DOFs, jointPositions, jointAxeses, linkAngVelocities, [positionFootL[j]], jointMasksFootL[j], False, partialDOFIndex) break for j in range(len(indexFootR)): if index == indexFootR[j]: contactFlagFootR[j] = 1 if j != 0: yjc.computePartialJacobian2(jFootR[j], DOFs, jointPositions, jointAxeses, [positionFootR[j]], jointMasksFootR[j], partialDOFIndex) yjc.computePartialJacobianDerivative2(dJFootR[j], DOFs, jointPositions, jointAxeses, linkAngVelocities, [positionFootR[j]], jointMasksFootR[j], False, partialDOFIndex) break for j in range(len(indexFootL)): yjc.computeAngJacobian2(jAngFootL[j], DOFs, jointPositions, jointAxeses, [positionFootL[j]], jointMasksFootL[j]) yjc.computeAngJacobianDerivative2(dJAngFootL[j], DOFs, jointPositions, jointAxeses, linkAngVelocities, [positionFootL[j]], jointMasksFootL[j], False) yjc.computeAngJacobian2(jAngFootR[j], DOFs, jointPositions, jointAxeses, [positionFootR[j]], jointMasksFootR[j]) yjc.computeAngJacobianDerivative2(dJAngFootR[j], DOFs, jointPositions, jointAxeses, linkAngVelocities, [positionFootR[j]], jointMasksFootR[j], False) ''' if footPartNum == 1: footCenterL = controlModel.getBodyPositionGlobal(supL) footCenterR = controlModel.getBodyPositionGlobal(supR) else: if ((contactFlagFootL[3] == 1 or contactFlagFootL[4] == 1) and contactFlagFootL[0] == 0) or ((contactFlagFootR[3] == 1 or contactFlagFootR[4] == 1) and contactFlagFootR[0] == 0): r = 0.8 footCenterL = (controlModel.getBodyPositionGlobal(supL)*r + controlModel.getBodyPositionGlobal(indexFootL[1])*(1.0-r)) footCenterR = (controlModel.getBodyPositionGlobal(supR)*r + controlModel.getBodyPositionGlobal(indexFootR[1])*(1.0-r)) #footCenterL = controlModel.getBodyPositionGlobal(indexFootL[1]) #footCenterR = controlModel.getBodyPositionGlobal(indexFootR[1]) else : #footCenterL = (controlModel.getBodyPositionGlobal(supL) + controlModel.getBodyPositionGlobal(indexFootL[1]))/2.0 #footCenterR = (controlModel.getBodyPositionGlobal(supR) + controlModel.getBodyPositionGlobal(indexFootR[1]))/2.0 #footCenterL = controlModel.getBodyPositionGlobal(indexFootL[1]) #footCenterR = controlModel.getBodyPositionGlobal(indexFootR[1]) r = 0.8 footCenterL = (controlModel.getBodyPositionGlobal(indexFootL[1])*r + controlModel.getBodyPositionGlobal(indexFootL[3])*(1.0-r)) footCenterR = (controlModel.getBodyPositionGlobal(indexFootR[1])*r + controlModel.getBodyPositionGlobal(indexFootR[3])*(1.0-r)) ''' ''' if stage == POWERFUL_BALANCING: footCenterL = controlModel.getBodyPositionGlobal(indexFootL[1]) footCenterR = controlModel.getBodyPositionGlobal(indexFootR[1]) else: footCenterL = (controlModel.getBodyPositionGlobal(indexFootL[1]) + controlModel.getBodyPositionGlobal(indexFootL[3]) )/2.0 footCenterR = (controlModel.getBodyPositionGlobal(indexFootR[1]) + controlModel.getBodyPositionGlobal(indexFootR[3]))/2.0 ''' ''' p1 = controlModel.getBodyPositionGlobal(indexFootL[0]) p2 = controlModel.getBodyPositionGlobal(indexFootR[0]) p3 = controlModel.getBodyPositionGlobal(indexFootL[3]) p4 = controlModel.getBodyPositionGlobal(indexFootR[3]) print(frame, "supL", p1[1]) print(frame, "supR", p2[1]) print(frame, "calcL", p3[1]) print(frame, "calcR", p4[1]) ''' #footCenter = footCenterL + (footCenterR - footCenterL)/2.0 #footCenter[1] = 0. # ''' if checkAll(contactFlagFootL, 0) == 1 and checkAll(contactFlagFootR, 0) == 1: footCenter = footCenter elif checkAll(contactFlagFootL, 0) == 1 : footCenter = footCenterR elif checkAll(contactFlagFootR, 0) == 1 : footCenter = footCenterL ''' if footPartNum == 1: desFCL = (controlModel.getBodyPositionGlobal(supL)) desFCR = (controlModel.getBodyPositionGlobal(supR)) else : r = .4 desFCL = (controlModel.getBodyPositionGlobal(indexFootL[1])*r + controlModel.getBodyPositionGlobal(indexFootL[3])*(1.0-r))#controlModel.getBodyPositionGlobal(indexFootL[1]) desFCR = (controlModel.getBodyPositionGlobal(indexFootR[1])*r + controlModel.getBodyPositionGlobal(indexFootR[3])*(1.0-r))#controlModel.getBodyPositionGlobal(indexFootR[1]) desFC = desFCL + (desFCR - desFCL)/2.0 if checkAll(contactFlagFootL, 0) == 1 and checkAll(contactFlagFootR, 0) == 1: desFC = desFC elif checkAll(contactFlagFootL, 0) == 1 : desFC = desFCR elif checkAll(contactFlagFootR, 0) == 1 : desFC = desFCL #if stage == MOTION_TRACKING: # desFC = desFCL 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_ref = refFootL + (refFootR - refFootL)/2.0 #footCenter_ref[1] = 0. footCenter[1] = 0. 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]) print("totalNormalForce=", totalNormalForce[1]) print("F_Diff=", (totalMass*mm.s2v(wcfg.gravity))[1]+totalNormalForce[1]) # linear momentum CM_ref_plane = footCenter dL_des_plane = Kl*totalMass*(CM_ref_plane - CM_plane) - Dl*totalMass*dCM_plane print("CM_Diff=",mm.length(CM_ref_plane - CM_plane)) # angular momentum CP_ref = footCenter 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))) print("CP_Diff=",mm.length(CP_des - CP)) 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) ############################## # soft point constraint ##################################################### P_cur = controlModel.getBodyPositionGlobal(constBody) constBodyVec = P_cur - footCenter softConstPoint = [footCenter[0]+softConstPointOffset[0], mm.length(constBodyVec), footCenter[2]+softConstPointOffset[2]] ##################################################### P_des = softConstPoint #P_cur = controlModel.getBodyPositionGlobal(constBody) dP_des = [0, 0, 0] dP_cur = controlModel.getBodyVelocityGlobal(constBody) ddP_des1 = Ksc*(-(P_des - P_cur)) - Dsc*(-(dP_cur - dP_des)) r = -(P_des - P_cur) I = np.vstack(([1,0,0],[0,1,0],[0,0,1])) Z = np.hstack((I, mm.getCrossMatrixForm(-r))) yjc.computeJacobian2(Jconst, DOFs, jointPositions, jointAxeses, [P_cur], constJointMasks) JL, JA = np.vsplit(Jconst, 2) Q1 = np.dot(Z, Jconst) q1 = np.dot(JA, dth_flat) q2 = np.dot(mm.getCrossMatrixForm(q1), np.dot(mm.getCrossMatrixForm(q1), r)) yjc.computeJacobianDerivative2(dJconst, DOFs, jointPositions, jointAxeses, linkAngVelocities, [P_cur], constJointMasks, False) q_bias1 = np.dot(np.dot(Z, dJconst), dth_flat) + q2 ############################## 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 ''' #MOTION = FORWARD_JUMP if mit.MOTION == mit.FORWARD_JUMP : frame_index = [136, 100] #frame_index = [100000, 100000] elif mit.MOTION == mit.TAEKWONDO: frame_index = [130, 100] #frame_index = [100000, 100000] elif mit.MOTION == mit.TAEKWONDO2: frame_index = [130+40, 100] elif mit.MOTION == mit.WALK: frame_index = [10000, 60] else : frame_index = [1000000, 1000000] #MOTION = TAEKWONDO #frame_index = [135, 100] ''' if frame > 300 : if stage != DYNAMIC_BALANCING: print("#", frame,"-DYNAMIC_BALANCING") stage = DYNAMIC_BALANCING Kk = Kk*1 Dk = 2*(Kk**.5) ''' if frame > frame_index[0] : if stage != POWERFUL_BALANCING: print("#", frame,"-POWERFUL_BALANCING") stage = POWERFUL_BALANCING Kk = Kk*2 Dk = 2*(Kk**.5) elif frame > frame_index[1]: if stage != MOTION_TRACKING: print("#", frame,"-MOTION_TRACKING") stage = MOTION_TRACKING trackingW = w #if checkAll(contactFlagFootR, 0) != 1 : if stage == MOTION_TRACKING: trackingW = w2 #stage = POWERFUL_BALANCING Bt = Bt*2 # optimization mot.addTrackingTerms(problem, totalDOF, Bt, trackingW, ddth_des_flat) mot.addSoftPointConstraintTerms(problem, totalDOF, Bsc, ddP_des1, Q1, q_bias1) if flagContact == True: if stage != MOTION_TRACKING+10: 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] ############################## # Hard constraint if stage != MOTION_TRACKING: Kk2 = Kk * 2.0 else : Kk2 = Kk * 1.5 Dk2 = 2*(Kk2**.5) ''' desLinearAccL, desPosL = getDesFootLinearAcc(motionModel, controlModel, supL, ModelOffset, CM_ref, CM, Kk2, Dk2) desLinearAccR, desPosR = getDesFootLinearAcc(motionModel, controlModel, supR, ModelOffset, CM_ref, CM, Kk2, Dk2) desAngularAccL = getDesFootAngularAcc(motionModel, controlModel, supL, Kk2, Dk2) desAngularAccR = getDesFootAngularAcc(motionModel, controlModel, supR, Kk2, Dk2) ''' if stage != MOTION_TRACKING: idx = 0 #LEFT/RIGHT_TOES if stage != MOTION_TRACKING: desLinearAccL, desPosL = getDesFootLinearAcc(motionModel, controlModel, indexFootL[idx], ModelOffset, CM_ref, CM, Kk2, Dk2, 0.14)#0.076) desLinearAccR, desPosR = getDesFootLinearAcc(motionModel, controlModel, indexFootR[idx], ModelOffset, CM_ref, CM, Kk2, Dk2, 0.14) desAngularAccL = getDesFootAngularAcc(motionModel, controlModel, indexFootL[idx], Kk2, Dk2, [0,0,-1], [0,1,1.5]) desAngularAccR = getDesFootAngularAcc(motionModel, controlModel, indexFootR[idx], Kk2, Dk2, [0,0,-1], [0,1,1.5]) a_sup_2 = np.hstack(( np.hstack((desLinearAccL, desAngularAccL)), np.hstack((desLinearAccR, desAngularAccR)) )) Jsup_2 = np.vstack((jFootL[idx], jFootR[idx])) dJsup_2 = np.vstack((dJFootL[idx], dJFootR[idx])) else: desLinearAccL, desPosL = getDesFootLinearAcc(motionModel, controlModel, indexFootL[idx], ModelOffset, CM_ref, CM, Kk2, Dk2, 0.040) desLinearAccR, desPosR = getDesFootLinearAcc(motionModel, controlModel, indexFootR[idx], ModelOffset, CM_ref, CM, Kk2, Dk2, 0.040) desAngularAccL = getDesFootAngularAcc(motionModel, controlModel, indexFootL[idx], Kk2, Dk2) a_sup_2 = np.hstack((desLinearAccL, desAngularAccL)) Jsup_2 = jFootL[idx] dJsup_2 = dJFootL[idx] rd_DesPosL[0] = desPosL.copy() rd_DesPosR[0] = desPosR.copy() else: if footPartNum != 5: idx = 0 desLinearAccL, desPosL = getDesFootLinearAcc(motionModel, controlModel, indexFootL[idx], ModelOffset, CM_ref, CM, Kk2, Dk2, 0.045) desAngularAccL = getDesFootAngularAcc(motionModel, controlModel, indexFootL[idx], Kk2, Dk2) a_sup_2 = np.hstack(( desLinearAccL, desAngularAccL)) Jsup_2 = (jFootL[idx]) dJsup_2 = (dJFootL[idx]) ''' idx = 4 desAngularAccL = getDesFootAngularAcc(motionModel, controlModel, indexFootL[idx], Kk2, Dk2) a_sup_2 = np.hstack(( a_sup_2, desAngularAccL)) Jsup_2 = np.vstack(( Jsup_2, jAngFootL[idx])) dJsup_2 = np.vstack(( dJsup_2, dJAngFootL[idx])) ''' ''' idx = 1 desAngularAccL = getDesFootAngularAcc(motionModel, controlModel, indexFootL[idx], Kk2, Dk2) a_sup_2 = np.hstack(( a_sup_2, desAngularAccL)) Jsup_2 = np.vstack(( Jsup_2, jAngFootL[idx])) dJsup_2 = np.vstack(( dJsup_2, dJAngFootL[idx])) ''' else: idx = 0 desAngularAccL = getDesFootAngularAcc(motionModel, controlModel, indexFootL[idx], Kk2, Dk2) desAngularAccR = getDesFootAngularAcc(motionModel, controlModel, indexFootR[idx], Kk2, Dk2) a_sup_2 = np.hstack(( desAngularAccL, desAngularAccR )) Jsup_2 = np.vstack((jAngFootL[idx], jAngFootR[idx])) dJsup_2 = np.vstack((dJAngFootL[idx], dJAngFootR[idx])) ############################## ############################## # Additional constraint if stage != MOTION_TRACKING+10: #Kk2 = Kk * 2.5 Kk2 = Kk * 2.5 Dk2 = 2*(Kk2**.5) desForePosL = [0,0,0] desForePosR = [0,0,0] desRearPosL = [0,0,0] desRearPosR = [0,0,0] for i in range(1, footPartNum) : if stage != MOTION_TRACKING: axis = [0,1,0] desAng = [0,1,0] desY = 0.04 if i == 1 or i == 2: desAng = [0,1,1.2] desY = 0.076 if contactFlagFootL[i] == 1: desLinearAccL, desForePosL = getDesFootLinearAcc(motionModel, controlModel, indexFootL[i], ModelOffset, CM_ref, CM, Kk2, Dk2, desY) desAngularAccL = getDesFootAngularAcc(motionModel, controlModel, indexFootL[i], Kk2, Dk2, axis, desAng) a_sup_2 = np.hstack(( a_sup_2, np.hstack((desLinearAccL, desAngularAccL)) )) Jsup_2 = np.vstack(( Jsup_2, jFootL[i] )) dJsup_2 = np.vstack(( dJsup_2, dJFootL[i] )) if contactFlagFootR[i] == 1: desLinearAccR, desForePosR = getDesFootLinearAcc(motionModel, controlModel, indexFootR[i], ModelOffset, CM_ref, CM, Kk2, Dk2, desY) desAngularAccR = getDesFootAngularAcc(motionModel, controlModel, indexFootR[i], Kk2, Dk2, axis, desAng) a_sup_2 = np.hstack(( a_sup_2, np.hstack((desLinearAccR, desAngularAccR)) )) Jsup_2 = np.vstack(( Jsup_2, jFootR[i] )) dJsup_2 = np.vstack(( dJsup_2, dJFootR[i] )) else: if contactFlagFootL[i] == 1: desAngularAccL = getDesFootAngularAcc(motionModel, controlModel, indexFootL[i], Kk2, Dk2) a_sup_2 = np.hstack(( a_sup_2, desAngularAccL )) Jsup_2 = np.vstack(( Jsup_2, jAngFootL[i] )) dJsup_2 = np.vstack(( dJsup_2, dJAngFootL[i] )) if contactFlagFootR[i] == 1: desAngularAccR = getDesFootAngularAcc(motionModel, controlModel, indexFootR[i], Kk2, Dk2) a_sup_2 = np.hstack(( a_sup_2, desAngularAccR )) Jsup_2 = np.vstack(( Jsup_2, jAngFootR[i] )) dJsup_2 = np.vstack(( dJsup_2, dJAngFootR[i] )) ''' for i in range(1, footPartNum) : if contactFlagFootL[i] == 1: desLinearAccL, desForePosL = getDesFootLinearAcc(motionModel, controlModel, indexFootL[i], ModelOffset, CM_ref, CM, Kk2, Dk2, 0.034) desAngularAccL = getDesFootAngularAcc(motionModel, controlModel, indexFootL[i], Kk2, Dk2) a_sup_2 = np.hstack(( a_sup_2, np.hstack((desLinearAccL, desAngularAccL)) )) Jsup_2 = np.vstack(( Jsup_2, jFootL[i] )) dJsup_2 = np.vstack(( dJsup_2, dJFootL[i] )) if contactFlagFootR[i] == 1: desLinearAccR, desForePosR = getDesFootLinearAcc(motionModel, controlModel, indexFootR[i], ModelOffset, CM_ref, CM, Kk2, Dk2, 0.034) desAngularAccR = getDesFootAngularAcc(motionModel, controlModel, indexFootR[i], Kk2, Dk2) a_sup_2 = np.hstack(( a_sup_2, np.hstack((desLinearAccR, desAngularAccR)) )) Jsup_2 = np.vstack(( Jsup_2, jFootR[i] )) dJsup_2 = np.vstack(( dJsup_2, dJFootR[i] )) ''' rd_DesForePosL[0] = desForePosL rd_DesForePosR[0] = desForePosR rd_DesRearPosL[0] = desRearPosL rd_DesRearPosR[0] = desRearPosR ############################## mot.setConstraint(problem, totalDOF, Jsup_2, dJsup_2, dth_flat, a_sup_2) r = problem.solve() problem.clear() ype.nested(r['x'], ddth_sol) rootPos[0] = controlModel.getBodyPositionGlobal(selectedBody) localPos = [[0, 0, 0]] rd_Joint[0] = controlModel.getJointPositionGlobal(motion[0].skeleton.getJointIndex('LeftMetatarsal_1')) rd_Joint2[0] = controlModel.getJointPositionGlobal(motion[0].skeleton.getJointIndex('LeftMetatarsal_3')) rd_Joint3[0] = controlModel.getJointPositionGlobal(motion[0].skeleton.getJointIndex('LeftPhalange_1')) rd_Joint4[0] = controlModel.getJointPositionGlobal(motion[0].skeleton.getJointIndex('LeftPhalange_3')) 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() ''' 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) applyedExtraForce[0] = extraForce[0] if forceApplyFrame*wcfg.timeStep > 0.1: viewer.ResetForce() forceApplyFrame = 0 ''' vpWorld.step() # 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 - footCenter)*3 #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]
def simulateCallback(frame): global g_initFlag global forceShowFrame global forceApplyFrame global JsysPre global JsupPreL global JsupPreR global JsupPre global softConstPoint global stage motionModel.update(motion[frame]) Kt, Kk, Kl, Kh, Ksc, Bt, Bl, Bh, Bsc = 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) # tracking th_r = motion.getDOFPositions(frame) 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 footCenterL = controlModel.getBodyPositionGlobal(supL) footCenterR = controlModel.getBodyPositionGlobal(supR) refFootL = motionModel.getBodyPositionGlobal(supL) refFootR = motionModel.getBodyPositionGlobal(supR) footCenter = footCenterL + (footCenterR - footCenterL) / 2.0 footCenter[1] = 0. footCenter_ref = refFootL + (refFootR - refFootL) / 2.0 #footCenter_ref[1] = 0. foreFootCenterL = controlModel.getBodyPositionGlobal(foreSupL) foreFootCenterR = controlModel.getBodyPositionGlobal(foreSupR) rearFootCenterL = controlModel.getBodyPositionGlobal(rearSupL) rearFootCenterR = controlModel.getBodyPositionGlobal(rearSupR) 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() 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) yjc.computeJacobian2(Jsys, DOFs, jointPositions, jointAxeses, linkPositions, allLinkJointMasks) #dJsys = (Jsys - JsysPre)/(1/30.) #JsysPre = Jsys yjc.computeJacobianDerivative2(dJsys, DOFs, jointPositions, jointAxeses, linkAngVelocities, linkPositions, allLinkJointMasks) if g_initFlag == 0: softConstPoint = controlModel.getBodyPositionGlobal(constBody) softConstPoint[1] -= .3 g_initFlag = 1 yjc.computeJacobian2(JsupL, DOFs, jointPositions, jointAxeses, [footCenterL], supLJointMasks) #dJsupL = (JsupL - JsupPreL)/(1/30.) #JsupPreL = JsupL yjc.computeJacobianDerivative2(dJsupL, DOFs, jointPositions, jointAxeses, linkAngVelocities, [footCenterL], supLJointMasks, False) yjc.computeJacobian2(JsupR, DOFs, jointPositions, jointAxeses, [footCenterR], supRJointMasks) #dJsupR = (JsupR - JsupPreR)/(1/30.) #JsupPreR = JsupR yjc.computeJacobianDerivative2(dJsupR, DOFs, jointPositions, jointAxeses, linkAngVelocities, [footCenterR], supRJointMasks, False) 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) flagForeSupLContact = 0 flagForeSupRContact = 0 flagRearSupLContact = 0 flagRearSupLContact = 0 for i in range(len(bodyIDs)): controlModel.SetBodyColor(bodyIDs[i], 255, 105, 105) index = controlModel.id2index(bodyIDs[i]) if index == foreSupL: flagForeSupLContact = 1 yjc.computeJacobian2(JforeSupL, DOFs, jointPositions, jointAxeses, [foreFootCenterL], foreSupLJointMasks) yjc.computeJacobianDerivative2(dJforeSupL, DOFs, jointPositions, jointAxeses, linkAngVelocities, [foreFootCenterL], foreSupLJointMasks, False) elif index == foreSupR: flagForeSupRContact = 1 yjc.computeJacobian2(JforeSupR, DOFs, jointPositions, jointAxeses, [foreFootCenterR], foreSupRJointMasks) yjc.computeJacobianDerivative2(dJforeSupR, DOFs, jointPositions, jointAxeses, linkAngVelocities, [foreFootCenterR], foreSupRJointMasks, False) elif index == rearSupL: flagRearSupLContact = 1 yjc.computeJacobian2(JrearSupL, DOFs, jointPositions, jointAxeses, [rearFootCenterL], rearSupLJointMasks) yjc.computeJacobianDerivative2(dJrearSupL, DOFs, jointPositions, jointAxeses, linkAngVelocities, [rearFootCenterL], rearSupLJointMasks, False) elif index == rearSupR: flagRearSupRContact = 1 yjc.computeJacobian2(JrearSupR, DOFs, jointPositions, jointAxeses, [rearFootCenterR], rearSupRJointMasks) yjc.computeJacobianDerivative2(dJrearSupR, DOFs, jointPositions, jointAxeses, linkAngVelocities, [rearFootCenterR], rearSupRJointMasks, False) # 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] # linear momentum CM_ref_plane = footCenter dL_des_plane = Kl * totalMass * (CM_ref_plane - CM_plane) - Dl * totalMass * dCM_plane # angular momentum CP_ref = footCenter 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 CMP = yrp.getCMP(contactForces, CM) r = [0, 0, 0] if CP != None and np.any(np.isnan(CMP)) != True: r = CP - CMP # 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) ############################## # soft point constraint P_des = softConstPoint P_cur = controlModel.getBodyPositionGlobal(constBody) dP_des = [0, 0, 0] dP_cur = controlModel.getBodyVelocityGlobal(constBody) ddP_des1 = Ksc * (P_des - P_cur) - Dsc * (dP_cur - dP_des) r = P_des - P_cur I = np.vstack(([1, 0, 0], [0, 1, 0], [0, 0, 1])) Z = np.hstack((I, mm.getCrossMatrixForm(-r))) yjc.computeJacobian2(Jconst, DOFs, jointPositions, jointAxeses, [softConstPoint], constJointMasks) JL, JA = np.vsplit(Jconst, 2) Q1 = np.dot(Z, Jconst) q1 = np.dot(JA, dth_flat) q2 = np.dot(mm.getCrossMatrixForm(q1), np.dot(mm.getCrossMatrixForm(q1), r)) yjc.computeJacobianDerivative2(dJconst, DOFs, jointPositions, jointAxeses, linkAngVelocities, [softConstPoint], constJointMasks, False) q_bias1 = np.dot(np.dot(Z, dJconst), dth_flat) + q2 ############################## 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 ''' #MOTION = FORWARD_JUMP if mit.MOTION == mit.FORWARD_JUMP: frame_index = [136, 100] elif mit.MOTION == mit.TAEKWONDO: frame_index = [130, 100] else: frame_index = [1000000, 1000000] #MOTION = TAEKWONDO #frame_index = [135, 100] if frame > frame_index[0]: stage = POWERFUL_BALANCING Kk = Kk * 2 Dk = 2 * (Kk**.5) elif frame > frame_index[1]: stage = MOTION_TRACKING trackingW = w if stage == MOTION_TRACKING: trackingW = w2 # optimization mot.addTrackingTerms(problem, totalDOF, Bt, trackingW, ddth_des_flat) mot.addSoftPointConstraintTerms(problem, totalDOF, Bsc, ddP_des1, Q1, q_bias1) if flagContact == True: if stage != MOTION_TRACKING: mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R, r_bias) mot.addAngularTerms(problem, totalDOF, Bh, dH_des, S, s_bias) ############################## # Hard constraint desLinearAccL = getDesFootLinearAcc(motionModel, controlModel, supL, ModelOffset, CM_ref, CM, Kk, Dk) desLinearAccR = getDesFootLinearAcc(motionModel, controlModel, supR, ModelOffset, CM_ref, CM, Kk, Dk) desAngularAccL = getDesFootAngularAcc(motionModel, controlModel, supL, Kk, Dk) desAngularAccR = getDesFootAngularAcc(motionModel, controlModel, supR, Kk, Dk) ''' desLinearAccL = [0,0,0] desAngularAccL = [0,0,0] desLinearAccR = [0,0,0] desAngularAccR = [0,0,0] refPos = motionModel.getBodyPositionGlobal(supL) curPos = controlModel.getBodyPositionGlobal(supL) refRootPos = motionModel.getBodyPositionGlobal(rootB) curRootPos = controlModel.getBodyPositionGlobal(rootB) refVecL = refPos - CM_ref if stage == MOTION_TRACKING: refPos = CM + refVecL refPos[1] += 0.05 refPos[0] -= 0.05 elif stage == POWERFUL_BALANCING: refPos = copy.copy(curPos) refPos[1] = 0 else: refPos[0] += ModelOffset[0] rd_root[0] = curRootPos refVel = motionModel.getBodyVelocityGlobal(supL) rd_footCenterL[0] = copy.copy(curPos) #rd_footCenterL[0][2] += 0.2 curVel = controlModel.getBodyVelocityGlobal(supL) #refAcc = (0,0,0) refAcc = motionModel.getBodyAccelerationGlobal(supL) if stage != MOTION_TRACKING: refPos[1] = 0 if refPos[1] < 0.0 : refPos[1] = 0.0 rd_DesPosL[0] = refPos desLinearAccL = yct.getDesiredAcceleration(refPos, curPos, refVel, curVel, refAcc, Kk, Dk) refPos = motionModel.getBodyPositionGlobal(supR) curPos = controlModel.getBodyPositionGlobal(supR) refVecR = refPos - CM_ref if stage == MOTION_TRACKING: refPos = CM + refVecR refPos[1] += 0.05 refPos[0] -= 0.05 elif stage == POWERFUL_BALANCING: refPos = copy.copy(curPos) refPos[1] = 0 else : refPos[0] += ModelOffset[0] refVel = motionModel.getBodyVelocityGlobal(supR) curVel = controlModel.getBodyVelocityGlobal(supR) refAcc = motionModel.getBodyAccelerationGlobal(supR) if stage != MOTION_TRACKING: refPos[1] = 0 if refPos[1] < 0.0 : refPos[1] = 0.0 rd_DesPosR[0] = refPos desLinearAccR = yct.getDesiredAcceleration(refPos, curPos, refVel, curVel, refAcc, Kk, Dk) curAng = [controlModel.getBodyOrientationGlobal(supL)] refAngVel = motionModel.getBodyAngVelocityGlobal(supL) curAngVel = controlModel.getBodyAngVelocityGlobal(supL) refAngAcc = (0,0,0) curAngY = np.dot(curAng, np.array([0,1,0])) refAngY = np.array([0,1,0]) if stage == DYNAMIC_BALANCING: refAng = [motionModel.getBodyOrientationGlobal(supL)] refAngY2 = np.dot(refAng, np.array([0,1,0])) refAngY = refAngY2[0] rd_footL_vec[0] = refAngY rd_footR_vec[0] = curAngY[0] aL = mm.logSO3(mm.getSO3FromVectors(curAngY[0], refAngY)) desAngularAccL = [Kk*aL + Dk*(refAngVel-curAngVel)] curAng = [controlModel.getBodyOrientationGlobal(supR)] refAngVel = motionModel.getBodyAngVelocityGlobal(supR) curAngVel = controlModel.getBodyAngVelocityGlobal(supR) refAngAcc = (0,0,0) curAngY = np.dot(curAng, np.array([0,1,0])) refAngY = np.array([0,1,0]) if stage == DYNAMIC_BALANCING: refAng = [motionModel.getBodyOrientationGlobal(supR)] refAngY2 = np.dot(refAng, np.array([0,1,0])) refAngY = refAngY2[0] aL = mm.logSO3(mm.getSO3FromVectors(curAngY[0], refAngY)) desAngularAccR = [Kk*aL + Dk*(refAngVel-curAngVel)] a_sup_2 = [desLinearAccL[0], desLinearAccL[1], desLinearAccL[2], desAngularAccL[0][0], desAngularAccL[0][1], desAngularAccL[0][2], desLinearAccR[0], desLinearAccR[1], desLinearAccR[2], desAngularAccR[0][0], desAngularAccR[0][1], desAngularAccR[0][2]] ''' a_sup_2 = np.hstack((np.hstack((desLinearAccL, desAngularAccL)), np.hstack((desLinearAccR, desAngularAccR)))) Jsup_2 = np.vstack((JsupL, JsupR)) dJsup_2 = np.vstack((dJsupL, dJsupR)) mot.setConstraint(problem, totalDOF, Jsup_2, dJsup_2, dth_flat, a_sup_2) ############################## ############################## # Additional constraint desLinearAccL = [0, 0, 0] desAngularAccL = [0, 0, 0] desLinearAccR = [0, 0, 0] desAngularAccR = [0, 0, 0] refPos = motionModel.getBodyPositionGlobal(supL) curPos = controlModel.getBodyPositionGlobal(supL) refVecL = refPos - CM_ref if stage == MOTION_TRACKING: refPos = CM + refVecL refPos[1] += 0.05 refPos[0] -= 0.05 elif stage == POWERFUL_BALANCING: refPos = copy.copy(curPos) refPos[1] = 0 else: refPos[0] += ModelOffset[0] refVel = motionModel.getBodyVelocityGlobal(supL) curVel = controlModel.getBodyVelocityGlobal(supL) #refAcc = (0,0,0) refAcc = motionModel.getBodyAccelerationGlobal(supL) if stage != MOTION_TRACKING: refPos[1] = 0 if refPos[1] < 0.0: refPos[1] = 0.0 desLinearAccL = yct.getDesiredAcceleration(refPos, curPos, refVel, curVel, refAcc, Kk, Dk) curAng = [controlModel.getBodyOrientationGlobal(supL)] refAngVel = motionModel.getBodyAngVelocityGlobal(supL) curAngVel = controlModel.getBodyAngVelocityGlobal(supL) refAngAcc = (0, 0, 0) curAngY = np.dot(curAng, np.array([0, 1, 0])) refAngY = np.array([0, 1, 0]) if stage == DYNAMIC_BALANCING: refAng = [motionModel.getBodyOrientationGlobal(supL)] refAngY2 = np.dot(refAng, np.array([0, 1, 0])) refAngY = refAngY2[0] rd_footL_vec[0] = refAngY rd_footR_vec[0] = curAngY[0] aL = mm.logSO3(mm.getSO3FromVectors(curAngY[0], refAngY)) desAngularAccL = [Kk * aL + Dk * (refAngVel - curAngVel)] ############################## r = problem.solve() problem.clear() ype.nested(r['x'], ddth_sol) rootPos[0] = controlModel.getBodyPositionGlobal(selectedBody) localPos = [[0, 0, 0]] for i in range(stepsPerFrame): # apply penalty force bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce( bodyIDsToCheck, mus, Ks, Ds) vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals, contactForces) controlModel.setDOFAccelerations(ddth_sol) if flagForeSupLContact == 1: controlModel.setJointAngAccelerationLocal( foreSupL, (2.1, 0, 0)) if flagForeSupRContact == 1: controlModel.setJointAngAccelerationLocal( foreSupR, (2.1, 0, 0)) controlModel.solveHybridDynamics() 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) applyedExtraForce[0] = extraForce[0] if forceApplyFrame * wcfg.timeStep > 0.1: viewer.ResetForce() forceApplyFrame = 0 vpWorld.step() # 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 - footCenter) * 3 #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 if (forceApplyFrame == 0): applyedExtraForce[0] = [0, 0, 0]
def simulateCallback(frame): global g_initFlag global preFootCenterL, preFootCenterR global preFootOrientationL, preFootOrientationR global forceShowFrame global forceApplyFrame global JsysPre global JsupPreL global JsupPreR global JsupPre global softConstPoint global stage motionModel.update(motion[frame]) Kt, Kk, Kl, Kh, Ksc, Bt, Bl, Bh, Bsc = viewer.GetParam() if stage == 3: Bsc = 0 #Kl *= 1.5 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) # tracking th_r = motion.getDOFPositions(frame) 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 footCenterL = controlModel.getBodyPositionGlobal(supL) footCenterR = controlModel.getBodyPositionGlobal(supR) refFootL = motionModel.getBodyPositionGlobal(supL) refFootR = motionModel.getBodyPositionGlobal(supR) footCenter = footCenterL + (footCenterR - footCenterL)/2.0 footCenter[1] = 0. footCenter_ref = refFootL + (refFootR - refFootL)/2.0 footCenter_ref[1] = 0. 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() CM_plane_ref = yrp.getCM(linkPositions_ref, linkMasses, totalMass) CM_plane_ref[1] = 0. P = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions, CM, linkInertias) dP = ymt.getPureInertiaMatrixDerivative(dTO, linkMasses, linkVelocities, dCM, linkAngVelocities, linkInertias) yjc.computeJacobian2(Jsys, DOFs, jointPositions, jointAxeses, linkPositions, allLinkJointMasks) dJsys = (Jsys - JsysPre)/(1/30.) JsysPre = Jsys #yjc.computeJacobianDerivative2(dJsys, DOFs, jointPositions, jointAxeses, linkAngVelocities, linkPositions, allLinkJointMasks) if g_initFlag == 0: preFootCenterL = footCenterL preFootCenterR = footCenterR preFootCenterL[1] -= 0.02 preFootCenterR[1] -= 0.02 preFootOrientationL = controlModel.getBodyOrientationGlobal(supL) preFootOrientationR = controlModel.getBodyOrientationGlobal(supR) softConstPoint = controlModel.getBodyPositionGlobal(constBody) #softConstPoint[2] += 0.3 #softConstPoint[1] -= 1.1 #softConstPoint[0] += 0.1 softConstPoint[1] -= .3 #softConstPoint[0] -= .1 #softConstPoint[1] -= 1. #softConstPoint[0] -= .5 g_initFlag = 1 yjc.computeJacobian2(JsupL, DOFs, jointPositions, jointAxeses, [footCenterL], supLJointMasks) dJsupL = (JsupL - JsupPreL)/(1/30.) JsupPreL = JsupL #yjc.computeJacobianDerivative2(dJsupL, DOFs, jointPositions, jointAxeses, linkAngVelocities, [footCenterL], supLJointMasks, False) yjc.computeJacobian2(JsupR, DOFs, jointPositions, jointAxeses, [footCenterR], supRJointMasks) dJsupR = (JsupR - JsupPreR)/(1/30.) JsupPreR = JsupR #yjc.computeJacobianDerivative2(dJsupR, DOFs, jointPositions, jointAxeses, linkAngVelocities, [footCenterR], supRJointMasks, False) preFootCenter = preFootCenterL + (preFootCenterR - preFootCenterL)/2.0 preFootCenter[1] = 0 bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(bodyIDsToCheck, mus, Ks, Ds) CP = yrp.getCP(contactPositions, contactForces) # linear momentum CM_ref_plane = footCenter #CM_ref_plane = preFootCenter dL_des_plane = Kl*totalMass*(CM_ref_plane - CM_plane) - Dl*totalMass*dCM_plane #print("dL_des_plane ", dL_des_plane ) #dL_des_plane[1] = 0. # angular momentum CP_ref = footCenter 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))) #dH_des = [0, 0, 0] else: dH_des = None CMP = yrp.getCMP(contactForces, CM) r = [0,0,0] if CP!= None and np.any(np.isnan(CMP))!=True : r = CP - CMP #print("r.l", mm.length(r)) #Bba = Bh*(mm.length(r)) Bba = Bh # 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) ############################## # soft point constraint ''' cmDiff = footCenter - CM_plane print("cmDiff", cmDiff) if stage == 3: softConstPoint += ''' P_des = softConstPoint P_cur = controlModel.getBodyPositionGlobal(constBody) dP_des = [0, 0, 0] dP_cur = controlModel.getBodyVelocityGlobal(constBody) ddP_des1 = Ksc*(P_des - P_cur) - Dsc*(dP_cur - dP_des) r = P_des - P_cur I = np.vstack(([1,0,0],[0,1,0],[0,0,1])) Z = np.hstack((I, mm.getCrossMatrixForm(-r))) yjc.computeJacobian2(Jconst, DOFs, jointPositions, jointAxeses, [softConstPoint], constJointMasks) JL, JA = np.vsplit(Jconst, 2) Q1 = np.dot(Z, Jconst) q1 = np.dot(JA, dth_flat) q2 = np.dot(mm.getCrossMatrixForm(q1), np.dot(mm.getCrossMatrixForm(q1), r)) yjc.computeJacobianDerivative2(dJconst, DOFs, jointPositions, jointAxeses, linkAngVelocities, [softConstPoint], constJointMasks, False) q_bias1 = np.dot(np.dot(Z, dJconst), dth_flat) + q2 ''' P_des = preFootCenterR P_cur = controlModel.getBodyPositionGlobal(supR) P_cur[1] = 0 dP_des = [0, 0, 0] dP_cur = controlModel.getBodyVelocityGlobal(supR) ddP_des2 = Kp*(P_des - P_cur) - Dp*(dP_cur - dP_des) r = P_des - P_cur #print("r2", r) I = np.vstack(([1,0,0],[0,1,0],[0,0,1])) Z = np.hstack((I, mm.getCrossMatrixForm(-r))) JL, JA = np.vsplit(JsupR, 2) Q2 = np.dot(Z, JsupR) q1 = np.dot(JA, dth_flat) q2 = np.dot(mm.getCrossMatrixForm(q1), np.dot(mm.getCrossMatrixForm(q1), r)) q_bias2 = np.dot(np.dot(Z, dJsupR), dth_flat) + q2 ''' #print("Q1", Q1) ''' print("ddP_des1", ddP_des1) q_ddth1 = np.dot(Q1, ddth_c_flat) print("q_ddth1", q_ddth1) print("q_bias1", q_bias1) ddp1 = q_ddth1+q_bias1 print("ddp1", ddp1) print("diff1", ddP_des1-ddp1) ''' ''' print("ddP_des2", ddP_des2) q_ddth2 = np.dot(Q2, ddth_c_flat) print("q_ddth2", q_ddth2) print("q_bias2", q_bias2) ddp2 = q_ddth2+q_bias2 print("ddp2", ddp2) print("diff2", ddP_des2-ddp2) ''' ############################## ############################ # IK ''' P_des = preFootCenterL P_cur = controlModel.getJointPositionGlobal(supL) r = P_des - P_cur Q_des = preFootOrientationL Q_cur = controlModel.getJointOrientationGlobal(supL) rv = mm.logSO3(np.dot(Q_cur.transpose(), Q_des)) #print("rv", rv) des_v_sup = (r[0],r[1],r[2], rv[0], rv[1], rv[2]) A_large = np.dot(JsupL.T, JsupL) b_large = np.dot(JsupL.T, des_v_sup) des_d_th = npl.lstsq(A_large, b_large) ype.nested(des_d_th[0], d_th_IK_L) P_des2 = preFootCenterR P_cur2 = controlModel.getJointPositionGlobal(supR) r2 = P_des2 - P_cur2 Q_des2 = preFootOrientationR Q_cur2 = controlModel.getJointOrientationGlobal(supR) rv2 = mm.logSO3(np.dot(Q_cur2.transpose(), Q_des2)) #print("Q_des2", Q_des2) #print("Q_cur2", Q_cur2) #print("rv2", rv2) des_v_sup2 = (r2[0],r2[1],r2[2], rv2[0], rv2[1], rv[2]) A_large = np.dot(JsupR.T, JsupR) b_large = np.dot(JsupR.T, des_v_sup2) des_d_th = npl.lstsq(A_large, b_large) ype.nested(des_d_th[0], d_th_IK_R) for i in range(len(d_th_IK_L)): for j in range(len(d_th_IK_L[i])): d_th_IK[i][j] = d_th_IK_L[i][j] + d_th_IK_R[i][j] th_IK = yct.getIntegralDOF(th, d_th_IK, 1/timeStep) dd_th_IK = yct.getDesiredDOFAccelerations(th_IK, th, d_th_IK, dth, ddth_r, Kk, Dk) ype.flatten(d_th_IK, d_th_IK_flat) ype.flatten(dd_th_IK, dd_th_IK_flat) ''' ############################ flagContact = True if dH_des==None or np.any(np.isnan(dH_des)) == True: flagContact = False ''' 0 : initial 1 : contact 2 : fly 3 : landing ''' if flagContact == False : if stage == 1: stage = 2 print("fly") else: if stage == 0: stage = 1 print("contact") elif stage == 2: stage = 3 print("landing") if stage == 3: Bt = Bt*0.8 Bl = Bl*1 # optimization mot.addTrackingTerms(problem, totalDOF, Bt, w, ddth_des_flat) #mot.addTrackingTerms(problem, totalDOF, Bk, w_IK, dd_th_IK_flat) mot.addSoftPointConstraintTerms(problem, totalDOF, Bsc, ddP_des1, Q1, q_bias1) #mot.addSoftPointConstraintTerms(problem, totalDOF, Bp, ddP_des2, Q2, q_bias2) #mot.addConstraint(problem, totalDOF, JsupL, dJsupL, dth_flat, a_sup) #mot.addConstraint(problem, totalDOF, JsupR, dJsupR, dth_flat, a_sup2) desLinearAccL = [0,0,0] desAngularAccL = [0,0,0] desLinearAccR = [0,0,0] desAngularAccR = [0,0,0] refPos = motionModel.getBodyPositionGlobal(supL) refPos[0] += ModelOffset[0] refPos[1] = 0 refVel = motionModel.getBodyVelocityGlobal(supL) curPos = controlModel.getBodyPositionGlobal(supL) #curPos[1] = 0 curVel = controlModel.getBodyVelocityGlobal(supL) refAcc = (0,0,0) if stage == 3: refPos = curPos refPos[1] = 0 if curPos[1] < 0.0: curPos[1] = 0 else : curPos[1] = 0 rd_DesPosL[0] = refPos #(p_r, p, v_r, v, a_r, Kt, Dt) desLinearAccL = yct.getDesiredAcceleration(refPos, curPos, refVel, curVel, refAcc, Kk, Dk) #desLinearAccL[1] = 0 refPos = motionModel.getBodyPositionGlobal(supR) refPos[0] += ModelOffset[0] refPos[1] = 0 refVel = motionModel.getBodyVelocityGlobal(supR) curPos = controlModel.getBodyPositionGlobal(supR) #curPos[1] = 0 curVel = controlModel.getBodyVelocityGlobal(supR) if stage == 3: refPos = curPos refPos[1] = 0 if curPos[1] < 0.0: curPos[1] = 0 else : curPos[1] = 0 rd_DesPosR[0] = refPos desLinearAccR = yct.getDesiredAcceleration(refPos, curPos, refVel, curVel, refAcc, Kk, Dk) #desLinearAccR[1] = 0 #(th_r, th, dth_r, dth, ddth_r, Kt, Dt) refAng = [preFootOrientationL] curAng = [controlModel.getBodyOrientationGlobal(supL)] refAngVel = motionModel.getBodyAngVelocityGlobal(supL) curAngVel = controlModel.getBodyAngVelocityGlobal(supL) refAngAcc = (0,0,0) #desAngularAccL = yct.getDesiredAngAccelerations(refAng, curAng, refAngVel, curAngVel, refAngAcc, Kk, Dk) curAngY = np.dot(curAng, np.array([0,1,0])) aL = mm.logSO3(mm.getSO3FromVectors(curAngY[0], np.array([0,1,0]))) print("curAngYL=",curAngY, "aL=", aL) desAngularAccL = [Kk*aL + Dk*(refAngVel-curAngVel)] refAng = [preFootOrientationR] curAng = [controlModel.getBodyOrientationGlobal(supR)] refAngVel = motionModel.getBodyAngVelocityGlobal(supR) curAngVel = controlModel.getBodyAngVelocityGlobal(supR) refAngAcc = (0,0,0) #desAngularAccR = yct.getDesiredAngAccelerations(refAng, curAng, refAngVel, curAngVel, refAngAcc, Kk, Dk) curAngY = np.dot(curAng, np.array([0,1,0])) aL = mm.logSO3(mm.getSO3FromVectors(curAngY[0], np.array([0,1,0]))) desAngularAccR = [Kk*aL + Dk*(refAngVel-curAngVel)] print("curAngYR=",curAngY, "aL=", aL) a_sup_2 = [desLinearAccL[0], desLinearAccL[1], desLinearAccL[2], desAngularAccL[0][0], desAngularAccL[0][1], desAngularAccL[0][2], desLinearAccR[0], desLinearAccR[1], desLinearAccR[2], desAngularAccR[0][0], desAngularAccR[0][1], desAngularAccR[0][2]] if stage == 2 :#or stage == 3: refAccL = motionModel.getBodyAccelerationGlobal(supL) refAndAccL = motionModel.getBodyAngAccelerationGlobal(supL) refAccR = motionModel.getBodyAccelerationGlobal(supR) refAndAccR = motionModel.getBodyAngAccelerationGlobal(supR) a_sup_2 = [refAccL[0], refAccL[1], refAccL[2], refAndAccL[0], refAndAccL[1], refAndAccL[2], refAccR[0], refAccR[1], refAccR[2], refAndAccR[0], refAndAccR[1], refAndAccR[2]] ''' a_sup_2 = [0,0,0, desAngularAccL[0][0], desAngularAccL[0][1], desAngularAccL[0][2], 0,0,0, desAngularAccR[0][0], desAngularAccR[0][1], desAngularAccR[0][2]] ''' Jsup_2 = np.vstack((JsupL, JsupR)) dJsup_2 = np.vstack((dJsupL, dJsupR)) if flagContact == True: mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R, r_bias) mot.addAngularTerms(problem, totalDOF, Bh, dH_des, S, s_bias) mot.setConstraint(problem, totalDOF, Jsup_2, dJsup_2, dth_flat, a_sup_2) #mot.setConstraint(problem, totalDOF, JsupR, dJsupR, dth_flat, a_sup2) #mot.setConstraint(problem, totalDOF, Jsup_2, dJsup_2, dth_flat, a_sup_2) #mot.addConstraint(problem, totalDOF, Jsup_2, dJsup_2, d_th_IK_flat, a_sup_2) ''' jZ = np.dot(dJsup_2.T, dJsup_2) lamda = 0.001 for i in range(len(jZ)): for j in range(len(jZ[0])): if i == j : jZ[i][j] += lamda jZInv = npl.pinv(jZ) jA = np.dot(Jsup_2, np.dot(jZInv, np.dot(dJsup_2.T, -Jsup_2))) mot.addConstraint2(problem, totalDOF, jA, a_sup_2) ''' r = problem.solve() problem.clear() ype.nested(r['x'], ddth_sol) rootPos[0] = controlModel.getBodyPositionGlobal(selectedBody) localPos = [[0, 0, 0]] for i in range(stepsPerFrame): # apply penalty force bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(bodyIDsToCheck, mus, Ks, Ds) vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals, contactForces) controlModel.setDOFAccelerations(ddth_sol) controlModel.solveHybridDynamics() 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) applyedExtraForce[0] = extraForce[0] if forceApplyFrame*wcfg.timeStep > 0.1: viewer.ResetForce() forceApplyFrame = 0 vpWorld.step() # rendering rd_footCenter[0] = footCenter rd_footCenterL[0] = preFootCenterL rd_footCenterR[0] = preFootCenterR rd_CM[0] = CM rd_CM_plane[0] = CM_plane.copy() rd_footCenter_ref[0] = footCenter_ref rd_CM_plane_ref[0] = CM_plane_ref.copy() #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] = dL_des_plane - totalMass*mm.s2v(wcfg.gravity) rd_exf_des[0] = applyedExtraForce[0] #print("rd_exf_des", rd_exf_des[0]) rd_root_des[0] = rootPos[0] rd_CMP[0] = softConstPoint rd_soft_const_vec[0] = controlModel.getBodyPositionGlobal(constBody)-softConstPoint #if (applyedExtraForce[0][0] != 0 or applyedExtraForce[0][1] != 0 or applyedExtraForce[0][2] != 0) : if (forceApplyFrame == 0) : applyedExtraForce[0] = [0, 0, 0]