def reset(self): """ Resets the state of the environment and returns an initial observation. # Returns observation (object): The initial observation of the space. Initial reward is assumed to be 0. """ motionFile = '../data/wd2_tiptoe_zygote.bvh' motion, mcfg, wcfg, stepsPerFrame, config, frame_rate = mit.create_biped(motionFile, SEGMENT_FOOT_MAG=0.01, SEGMENT_FOOT_RAD=0.008, motion_scale=.01) self.world = cvw.VpWorld(wcfg) self.world.SetGlobalDamping(0.999) self.skel = cvm.VpControlModel(self.world, motion[0], mcfg) self.world.initialize() self.skel.initializeHybridDynamics() self.continue_from_now_by_phase(random() if self.rsi else 0.) # self.skel.set_positions(self.ref_motion.get_q(self.phase_frame)) self.skel.set_q(self.ref_motion.get_q(self.phase_frame)) self.skel.setDOFVelocities(self.ref_motion.getDOFVelocities(self.phase_frame)) # self.skel.set_velocities(self.ref_motion.get_dq_dart(self.phase_frame)) self.update_ref_skel(True) return self.state()
def main(): np.set_printoptions(precision=4, linewidth=200) # motion, mcfg, wcfg, stepsPerFrame, config = mit.create_vchain_5() motion, mcfg, wcfg, stepsPerFrame, config = mit.create_biped() vpWorld = cvw.VpWorld(wcfg) motionModel = cvm.VpMotionModel(vpWorld, motion[0], mcfg) controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg) vpWorld.initialize() controlModel.initializeHybridDynamics() ModelOffset = (1.5, -0.02, 0) #ModelOffset = (1.5, 1.02, 0) #ModelOffset = (1.5, 0.02, 0) controlModel.translateByOffset(ModelOffset) #controlModel.translateByOffset((1.5,-0.0328,0))#(1.5,-0.02,0)) totalDOF = controlModel.getTotalDOF() DOFs = controlModel.getDOFs() # parameter Kt = config['Kt'] Dt = config['Dt'] # tracking gain Kl = config['Kl'] Dl = config['Dl'] # linear balance gain Kh = config['Kh'] Dh = config['Dh'] # angular balance gain Ks = config['Ks'] Ds = config['Ds'] # penalty force spring gain Bt = config['Bt'] Bl = config['Bl'] Bh = config['Bh'] w = mot.getTrackingWeight(DOFs, motion[0].skeleton, config['weightMap']) w_IK = mot.getTrackingWeight(DOFs, motion[0].skeleton, config['IKweightMap']) supL = motion[0].skeleton.getJointIndex(config['supLink']) supR = motion[0].skeleton.getJointIndex(config['supLink2']) #controlModel.SetGround(supL, True) #controlModel.SetGround(supR, True) selectedBody = motion[0].skeleton.getJointIndex(config['end']) #constBody = motion[0].skeleton.getJointIndex('LeftForeArm') constBody = motion[0].skeleton.getJointIndex('Hips') # jacobian JsupL = yjc.makeEmptyJacobian(DOFs, 1) dJsupL = JsupL.copy() JsupPreL = JsupL.copy() JsupR = yjc.makeEmptyJacobian(DOFs, 1) dJsupR = JsupR.copy() JsupPreR = JsupR.copy() Jsup = yjc.makeEmptyJacobian(DOFs, 1) dJsup = Jsup.copy() JsupPre = Jsup.copy() Jsys = yjc.makeEmptyJacobian(DOFs, controlModel.getBodyNum()) dJsys = Jsys.copy() JsysPre = Jsys.copy() Jconst = yjc.makeEmptyJacobian(DOFs, 1) dJconst = Jconst.copy() supLJointMasks = [yjc.getLinkJointMask(motion[0].skeleton, supL)] supRJointMasks = [yjc.getLinkJointMask(motion[0].skeleton, supR)] constJointMasks = [yjc.getLinkJointMask(motion[0].skeleton, constBody)] allLinkJointMasks = yjc.getAllLinkJointMasks(motion[0].skeleton) # momentum matrix linkMasses = controlModel.getBodyMasses() totalMass = controlModel.getTotalMass() TO = ymt.make_TO(linkMasses) dTO = ymt.make_dTO(len(linkMasses)) # optimization problem = yac.LSE(totalDOF, 6) a_sup = (0, 0, 0, 0, 0, 0) #L #a_sup2 = (0,0,0, 0,0,0)#R a_sup2 = [0, 0, 0, 0, 0, 0] #R a_sup_2 = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] CP_old = [mm.v3(0., 0., 0.)] # penalty method bodyIDsToCheck = range(vpWorld.getBodyNum()) mus = [1.] * len(bodyIDsToCheck) # flat data structure ddth_des_flat = ype.makeFlatList(totalDOF) dth_flat = ype.makeFlatList(totalDOF) ddth_sol = ype.makeNestedList(DOFs) d_th_IK = ype.makeNestedList(DOFs) d_th_IK_L = ype.makeNestedList(DOFs) d_th_IK_R = ype.makeNestedList(DOFs) dd_th_IK = ype.makeNestedList(DOFs) dd_th_IK_flat = ype.makeFlatList(totalDOF) d_th_IK_flat = ype.makeFlatList(totalDOF) ddth_c_flat = ype.makeFlatList(totalDOF) # viewer rd_footCenter = [None] rd_footCenter_ref = [None] rd_footCenterL = [None] rd_footCenterR = [None] rd_CM_plane = [None] rd_CM_plane_ref = [None] rd_CM = [None] rd_CP = [None] rd_CP_des = [None] rd_dL_des_plane = [None] rd_dH_des = [None] rd_grf_des = [None] rd_exf_des = [None] rd_root_des = [None] rd_soft_const_vec = [None] rd_CMP = [None] rd_DesPosL = [None] rd_DesPosR = [None] rootPos = [None] selectedBodyId = [selectedBody] extraForce = [None] applyedExtraForce = [None] applyedExtraForce[0] = [0, 0, 0] viewer = ysv.SimpleViewer(rect=(0, 0, 1000, 800)) # viewer.record(False) # viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion, (0,255,255), yr.LINK_BONE)) viewer.doc.addObject('motion', motion) viewer.doc.addRenderer( 'motionModel', cvr.VpModelRenderer(motionModel, (150, 150, 255), yr.POLYGON_FILL)) viewer.doc.addRenderer( 'controlModel', cvr.VpModelRenderer(controlModel, (255, 240, 255), yr.POLYGON_FILL)) viewer.doc.addRenderer('rd_footCenter', yr.PointsRenderer(rd_footCenter)) #viewer.doc.addRenderer('rd_footCenterL', yr.PointsRenderer(rd_footCenterL)) #viewer.doc.addRenderer('rd_footCenterR', yr.PointsRenderer(rd_footCenterR)) viewer.doc.addRenderer('rd_CM_plane', yr.PointsRenderer(rd_CM_plane, (255, 255, 0))) viewer.doc.addRenderer('rd_CP', yr.PointsRenderer(rd_CP, (0, 255, 0))) #viewer.doc.addRenderer('rd_CP_des', yr.PointsRenderer(rd_CP_des, (255,0,255))) # viewer.doc.addRenderer('rd_dL_des_plane', yr.VectorsRenderer(rd_dL_des_plane, rd_CM, (255,255,0))) # viewer.doc.addRenderer('rd_dH_des', yr.VectorsRenderer(rd_dH_des, rd_CM, (0,255,0))) viewer.doc.addRenderer( 'rd_grf_des', yr.ForcesRenderer(rd_grf_des, rd_CP_des, (0, 255, 255), .001)) viewer.doc.addRenderer( 'rd_exf_des', yr.ForcesRenderer(rd_exf_des, rd_root_des, (0, 255, 0), .009, 0.05)) #viewer.doc.addRenderer('rd_CMP', yr.PointsRenderer(rd_CMP, (0,0,255))) viewer.doc.addRenderer('rd_DesPosL', yr.PointsRenderer(rd_DesPosL, (0, 0, 255))) viewer.doc.addRenderer('rd_DesPosR', yr.PointsRenderer(rd_DesPosR, (0, 100, 255))) #viewer.doc.addRenderer('softConstraint', yr.VectorsRenderer(rd_soft_const_vec, rd_CMP, (255,0,0), 3)) viewer.doc.addRenderer('rd_footCenter_ref', yr.PointsRenderer(rd_footCenter_ref)) viewer.doc.addRenderer('rd_CM_plane_ref', yr.PointsRenderer(rd_CM_plane_ref, (255, 255, 0))) stage = 0 def preCallback(frame): motionModel.update(motion[frame]) 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] viewer.setPreFrameCallback_Always(preCallback) viewer.setSimulateCallback(simulateCallback) viewer.startTimer(1 / 60.) viewer.show() Fl.run()
def main(): # np.set_printoptions(precision=4, linewidth=200) np.set_printoptions(precision=5, threshold=np.inf, suppress=True, linewidth=3000) motionFile = 'wd2_tiptoe.bvh' motionFile = 'wd2_tiptoe_zygote.bvh' # motion, mcfg, wcfg, stepsPerFrame, config, frame_rate = mit.create_biped(motionFile, SEGMENT_FOOT_RAD=0.008) motion, mcfg, wcfg, stepsPerFrame, config, frame_rate = mit.create_biped( motionFile, SEGMENT_FOOT_MAG=0.01, SEGMENT_FOOT_RAD=0.008) # motion, mcfg, wcfg, stepsPerFrame, config, frame_rate = mit.create_biped() # motion, mcfg, wcfg, stepsPerFrame, config = mit.create_jump_biped() vpWorld = cvw.VpWorld(wcfg) sphere_radius = 0.5 # vpWorld.add_sphere_bump(sphere_radius, (1.6361, -sphere_radius + 0.08, -0.3209)) # vpWorld.add_sphere_bump(sphere_radius, (1.4543, -sphere_radius + 0.08, -0.3301)) vpWorld.add_sphere_bump(sphere_radius, (1.6361, -sphere_radius + 0.08, -0.2909)) vpWorld.add_sphere_bump(sphere_radius, (1.4543, -sphere_radius + 0.08, -0.2901)) vpWorld.SetGlobalDamping(0.999) motionModel = cvm.VpMotionModel(vpWorld, motion[0], mcfg) controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg) # controlModel_shadow_for_ik = cvm.VpControlModel(vpWorld, motion[0], mcfg) vpWorld.initialize() controlModel.initializeHybridDynamics() # controlToMotionOffset = (1.5, -0.02, 0) controlToMotionOffset = (1.5, 0., 0) controlModel.translateByOffset(controlToMotionOffset) # controlModel_shadow_for_ik.set_q(controlModel.get_q()) # controlModel_shadow_for_ik.computeJacobian(0, np.array([0., 0., 0.])) wcfg_ik = copy.deepcopy(wcfg) vpWorld_ik = cvw.VpWorld(wcfg_ik) controlModel_ik = cvm.VpControlModel(vpWorld_ik, motion[0], mcfg) vpWorld_ik.initialize() controlModel_ik.set_q(np.zeros_like(controlModel.get_q())) totalDOF = controlModel.getTotalDOF() DOFs = controlModel.getDOFs() print(totalDOF) print(controlModel.getTotalMass()) foot_dofs = [] left_foot_dofs = [] right_foot_dofs = [] foot_seg_dofs = [] left_foot_seg_dofs = [] right_foot_seg_dofs = [] # for joint_idx in range(motion[0].skeleton.getJointNum()): for joint_idx in range(controlModel.getJointNum()): joint_name = controlModel.index2name(joint_idx) # joint_name = motion[0].skeleton.getJointName(joint_idx) if 'Foot' in joint_name: foot_dofs_temp = controlModel.getJointDOFIndexes(joint_idx) foot_dofs.extend(foot_dofs_temp) if 'Left' in joint_name: left_foot_dofs.extend(foot_dofs_temp) elif 'Right' in joint_name: right_foot_dofs.extend(foot_dofs_temp) if 'foot' in joint_name: foot_dofs_temp = controlModel.getJointDOFIndexes(joint_idx) foot_seg_dofs.extend(foot_dofs_temp) if 'Left' in joint_name: left_foot_seg_dofs.extend(foot_dofs_temp) elif 'Right' in joint_name: right_foot_seg_dofs.extend(foot_dofs_temp) # parameter Kt = config['Kt'] Dt = config['Dt'] # tracking gain Kl = config['Kl'] Dl = config['Dl'] # linear balance gain Kh = config['Kh'] Dh = config['Dh'] # angular balance gain Ks = config['Ks'] Ds = config['Ds'] # penalty force spring gain Bt = config['Bt'] Bl = config['Bl'] Bh = config['Bh'] # selectedBody = motion[0].skeleton.getJointIndex(config['end']) selectedBody = motion[0].skeleton.getJointIndex('Spine') constBody = motion[0].skeleton.getJointIndex('RightFoot') supL = motion[0].skeleton.getJointIndex('LeftFoot') supR = motion[0].skeleton.getJointIndex('RightFoot') # momentum matrix linkMasses = controlModel.getBodyMasses() totalMass = controlModel.getTotalMass() TO = ymt.make_TO(linkMasses) dTO = ymt.make_dTO(len(linkMasses)) # optimization problem = yac.LSE(totalDOF, 12) # a_sup = (0,0,0, 0,0,0) #ori # a_sup = (0,0,0, 0,0,0) #L CP_old = [mm.v3(0., 0., 0.)] # penalty method bodyIDsToCheck = list(range(vpWorld.getBodyNum())) # mus = [1.]*len(bodyIDsToCheck) mus = [2.] * len(bodyIDsToCheck) # flat data structure ddth_des_flat = ype.makeFlatList(totalDOF) dth_flat = ype.makeFlatList(totalDOF) ddth_sol = ype.makeNestedList(DOFs) # viewer rd_footCenter = [None] rd_footCenter_ref = [None] rd_footCenterL = [None] rd_footCenterR = [None] rd_CM_plane = [None] rd_CM = [None] rd_CP = [None] rd_CP_des = [None] rd_dL_des_plane = [None] rd_dH_des = [None] rd_grf_des = [None] rd_exf_des = [None] rd_exfen_des = [None] rd_root_des = [None] rd_foot_ori = [None] rd_foot_pos = [None] rd_root_ori = [None] rd_root_pos = [None] rd_CF = [None] rd_CF_pos = [None] rootPos = [None] selectedBodyId = [selectedBody] extraForce = [None] extraForcePos = [None] rightFootVectorX = [None] rightFootVectorY = [None] rightFootVectorZ = [None] rightFootPos = [None] rightVectorX = [None] rightVectorY = [None] rightVectorZ = [None] rightPos = [None] def makeEmptyBasicSkeletonTransformDict(init=None): Ts = dict() Ts['pelvis'] = init Ts['spine_ribs'] = init Ts['head'] = init Ts['thigh_R'] = init Ts['shin_R'] = init Ts['foot_heel_R'] = init Ts['foot_R'] = init Ts['heel_R'] = init Ts['outside_metatarsal_R'] = init Ts['outside_phalanges_R'] = init Ts['inside_metatarsal_R'] = init Ts['inside_phalanges_R'] = init Ts['upper_limb_R'] = init Ts['lower_limb_R'] = init Ts['thigh_L'] = init Ts['shin_L'] = init Ts['foot_heel_L'] = init Ts['foot_L'] = init Ts['heel_L'] = init Ts['outside_metatarsal_L'] = init Ts['outside_phalanges_L'] = init Ts['inside_metatarsal_L'] = init Ts['inside_phalanges_L'] = init Ts['upper_limb_L'] = init Ts['lower_limb_L'] = init return Ts # viewer = ysv.SimpleViewer() # viewer = hsv.hpSimpleViewer(rect=[0, 0, 1024, 768], viewForceWnd=False) viewer = hsv.hpSimpleViewer(rect=[0, 0, 960 + 300, 1 + 1080 + 55], viewForceWnd=False) # viewer.record(False) # viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion, (0,255,255), yr.LINK_BONE)) viewer.doc.addObject('motion', motion) viewer.doc.addRenderer('world', yr.VpWorldRenderer(vpWorld, (150, 150, 150))) viewer.doc.addRenderer( 'motionModel', yr.VpModelRenderer(motionModel, (150, 150, 255), yr.POLYGON_FILL)) viewer.doc.setRendererVisible('motionModel', False) viewer.doc.addRenderer( 'ikModel', yr.VpModelRenderer(controlModel_ik, (150, 150, 255), yr.POLYGON_LINE)) viewer.doc.setRendererVisible('ikModel', False) # viewer.doc.addRenderer('controlModel', cvr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_LINE)) control_model_renderer = yr.VpModelRenderer(controlModel, (255, 240, 255), yr.POLYGON_FILL) viewer.doc.addRenderer('controlModel', control_model_renderer) skeleton_renderer = None if SKELETON_ON: # skeleton_renderer = yr.BasicSkeletonRenderer(makeEmptyBasicSkeletonTransformDict(np.eye(4)), offset_Y=-0.08) # skeleton_renderer = yr.BasicSkeletonRenderer(makeEmptyBasicSkeletonTransformDict(np.eye(4)), color=(230, 230, 230), offset_draw=(0.8, -0.02, 0.)) skeleton_renderer = yr.BasicSkeletonRenderer( makeEmptyBasicSkeletonTransformDict(np.eye(4)), color=(230, 230, 230), offset_draw=(0., -0.0, 0.)) viewer.doc.addRenderer('skeleton', skeleton_renderer) viewer.doc.addRenderer('rd_footCenter', yr.PointsRenderer(rd_footCenter)) # viewer.doc.setRendererVisible('rd_footCenter', False) viewer.doc.addRenderer('rd_footCenter_ref', yr.PointsRenderer(rd_footCenter_ref)) # viewer.doc.setRendererVisible('rd_footCenter_ref', False) viewer.doc.addRenderer('rd_CM_plane', yr.PointsRenderer(rd_CM_plane, (255, 255, 0))) # viewer.doc.setRendererVisible('rd_CM_plane', False) viewer.doc.addRenderer('rd_CP', yr.PointsRenderer(rd_CP, (0, 255, 0))) # viewer.doc.setRendererVisible('rd_CP', False) viewer.doc.addRenderer('rd_CP_des', yr.PointsRenderer(rd_CP_des, (255, 0, 255))) # viewer.doc.setRendererVisible('rd_CP_des', False) viewer.doc.addRenderer( 'rd_dL_des_plane', yr.VectorsRenderer(rd_dL_des_plane, rd_CM, (255, 255, 0))) # viewer.doc.setRendererVisible('rd_dL_des_plane', False) viewer.doc.addRenderer('rd_dH_des', yr.VectorsRenderer(rd_dH_des, rd_CM, (0, 255, 0))) # viewer.doc.setRendererVisible('rd_dH_des', False) # viewer.doc.addRenderer('rd_grf_des', yr.ForcesRenderer(rd_grf_des, rd_CP_des, (0,255,0), .001)) viewer.doc.addRenderer('rd_CF', yr.VectorsRenderer(rd_CF, rd_CF_pos, (255, 255, 0))) # viewer.doc.setRendererVisible('rd_CF', False) viewer.doc.addRenderer( 'rd_foot_ori', yr.OrientationsRenderer(rd_foot_ori, rd_foot_pos, (255, 255, 0))) viewer.doc.setRendererVisible('rd_foot_ori', False) viewer.doc.addRenderer( 'rd_root_ori', yr.OrientationsRenderer(rd_root_ori, rd_root_pos, (255, 255, 0))) viewer.doc.setRendererVisible('rd_root_ori', False) viewer.doc.addRenderer( 'extraForce', yr.VectorsRenderer(rd_exf_des, extraForcePos, (0, 255, 0))) viewer.doc.setRendererVisible('extraForce', False) # viewer.doc.addRenderer('extraForceEnable', yr.VectorsRenderer(rd_exfen_des, extraForcePos, (255,0,0))) viewer.doc.addRenderer( 'extraForceEnable', yr.WideArrowRenderer(rd_exfen_des, extraForcePos, (255, 0, 0), lineWidth=.05, fromPoint=False)) # viewer.doc.addRenderer('right_foot_oriX', yr.VectorsRenderer(rightFootVectorX, rightFootPos, (255,0,0))) # viewer.doc.addRenderer('right_foot_oriY', yr.VectorsRenderer(rightFootVectorY, rightFootPos, (0,255,0))) # viewer.doc.addRenderer('right_foot_oriZ', yr.VectorsRenderer(rightFootVectorZ, rightFootPos, (0,0,255))) # viewer.doc.addRenderer('right_oriX', yr.VectorsRenderer(rightVectorX, rightPos, (255,0,0))) # viewer.doc.addRenderer('right_oriY', yr.VectorsRenderer(rightVectorY, rightPos, (0,255,0))) # viewer.doc.addRenderer('right_oriZ', yr.VectorsRenderer(rightVectorZ, rightPos, (0,0,255))) # foot_viewer = FootWindow(viewer.x() + viewer.w() + 20, viewer.y(), 300, 400, 'foot contact modifier', controlModel) foot_viewer = None # type: FootWindow initKt = 25. # initKt = 60. initKl = 100. initKh = 100. initBl = .1 initBh = .13 # initSupKt = 17 initSupKt = 22 initFm = 50.0 initComX = 0. initComY = 0. initComZ = 0. viewer.objectInfoWnd.add1DSlider("Kt", 0., 300., 1., initKt) viewer.objectInfoWnd.add1DSlider("Kl", 0., 300., 1., initKl) viewer.objectInfoWnd.add1DSlider("Kh", 0., 300., 1., initKh) viewer.objectInfoWnd.add1DSlider("Bl", 0., 1., .001, initBl) viewer.objectInfoWnd.add1DSlider("Bh", 0., 1., .001, initBh) viewer.objectInfoWnd.add1DSlider("SupKt", 0., 300., 0.1, initSupKt) viewer.objectInfoWnd.add1DSlider("Fm", 0., 1000., 1., initFm) viewer.objectInfoWnd.add1DSlider("com X offset", -1., 1., 0.01, initComX) viewer.objectInfoWnd.add1DSlider("com Y offset", -1., 1., 0.01, initComY) viewer.objectInfoWnd.add1DSlider("com Z offset", -1., 1., 0.01, initComZ) viewer.objectInfoWnd.add1DSlider("tiptoe angle", -0.5, .5, 0.001, 0.) viewer.objectInfoWnd.add1DSlider("left tilt angle", -0.5, .5, 0.001, 0.) viewer.objectInfoWnd.add1DSlider("right tilt angle", -0.5, .5, 0.001, 0.) viewer.force_on = False def viewer_SetForceState(object): viewer.force_on = True def viewer_GetForceState(): return viewer.force_on def viewer_ResetForceState(): viewer.force_on = False viewer.objectInfoWnd.addBtn('Force on', viewer_SetForceState) viewer_ResetForceState() offset = 60 viewer.objectInfoWnd.begin() viewer.objectInfoWnd.labelForceX = Fl_Value_Input(20, 30 + offset * 9, 40, 20, 'X') viewer.objectInfoWnd.labelForceX.value(0) viewer.objectInfoWnd.labelForceY = Fl_Value_Input(80, 30 + offset * 9, 40, 20, 'Y') viewer.objectInfoWnd.labelForceY.value(0) viewer.objectInfoWnd.labelForceZ = Fl_Value_Input(140, 30 + offset * 9, 40, 20, 'Z') viewer.objectInfoWnd.labelForceZ.value(-1) viewer.objectInfoWnd.labelForceDur = Fl_Value_Input( 220, 30 + offset * 9, 40, 20, 'Dur') viewer.objectInfoWnd.labelForceDur.value(0.4) viewer.objectInfoWnd.end() # self.sliderFm = Fl_Hor_Nice_Slider(10, 42+offset*6, 250, 10) def getParamVal(paramname): return viewer.objectInfoWnd.getVal(paramname) def getParamVals(paramnames): return (getParamVal(name) for name in paramnames) def setParamVal(paramname, val): viewer.objectInfoWnd.setVal(paramname, val) idDic = dict() for i in range(motion[0].skeleton.getJointNum()): idDic[motion[0].skeleton.getJointName(i)] = i # extendedFootName = ['Foot_foot_0_0', 'Foot_foot_0_1', 'Foot_foot_0_0_0', 'Foot_foot_0_1_0', 'Foot_foot_1_0'] extendedFootName = [ 'Foot_foot_0_0', 'Foot_foot_0_0_0', 'Foot_foot_0_1_0', 'Foot_foot_1_0' ] lIDdic = { 'Left' + name: motion[0].skeleton.getJointIndex('Left' + name) for name in extendedFootName } rIDdic = { 'Right' + name: motion[0].skeleton.getJointIndex('Right' + name) for name in extendedFootName } footIdDic = lIDdic.copy() footIdDic.update(rIDdic) lIDlist = [ motion[0].skeleton.getJointIndex('Left' + name) for name in extendedFootName ] rIDlist = [ motion[0].skeleton.getJointIndex('Right' + name) for name in extendedFootName ] footIdlist = [] footIdlist.extend(lIDlist) footIdlist.extend(rIDlist) foot_left_idx = motion[0].skeleton.getJointIndex('LeftFoot') foot_right_idx = motion[0].skeleton.getJointIndex('RightFoot') foot_left_idx_temp = motion[0].skeleton.getJointIndex('LeftFoot_foot_1_0') foot_right_idx_temp = motion[0].skeleton.getJointIndex( 'RightFoot_foot_1_0') # ik_solver = hik.numIkSolver(dartIkModel) # ik_solver.clear() # bodyIDsToCheck = rIDlist.copy() joint_names = [ motion[0].skeleton.getJointName(i) for i in range(motion[0].skeleton.getJointNum()) ] def fix_dofs(_DOFs, nested_dof_values, _mcfg, _joint_names): fixed_nested_dof_values = list() fixed_nested_dof_values.append(nested_dof_values[0]) for i in range(1, len(_DOFs)): dof = _DOFs[i] if dof == 1: node = _mcfg.getNode(_joint_names[i]) axis = mm.unitZ() if node.jointAxes[0] == 'X': axis = mm.unitX() elif node.jointAxes[0] == 'Y': axis = mm.unitY() fixed_nested_dof_values.append( np.array([np.dot(nested_dof_values[i], axis)])) else: fixed_nested_dof_values.append(nested_dof_values[i]) return fixed_nested_dof_values start_frame = 200 up_vec_in_each_link = dict() for foot_id in footIdlist: up_vec_in_each_link[ foot_id] = controlModel_ik.getBodyOrientationGlobal(foot_id)[1, :] controlModel_ik.set_q(controlModel.get_q()) ################################### # simulate ################################### def simulateCallback(frame): # print(frame) # print(motion[frame].getJointOrientationLocal(footIdDic['RightFoot_foot_0_1_0'])) # hfi.footAdjust(motion[frame], idDic, SEGMENT_FOOT_MAG=.03, SEGMENT_FOOT_RAD=.015, baseHeight=0.02) if abs(getParamVal('tiptoe angle')) > 0.001: tiptoe_angle = getParamVal('tiptoe angle') motion[frame].mulJointOrientationLocal( idDic['LeftFoot_foot_0_0_0'], mm.exp(mm.unitX(), -math.pi * tiptoe_angle)) motion[frame].mulJointOrientationLocal( idDic['LeftFoot_foot_0_1_0'], mm.exp(mm.unitX(), -math.pi * tiptoe_angle)) motion[frame].mulJointOrientationLocal( idDic['RightFoot_foot_0_0_0'], mm.exp(mm.unitX(), -math.pi * tiptoe_angle)) motion[frame].mulJointOrientationLocal( idDic['RightFoot_foot_0_1_0'], mm.exp(mm.unitX(), -math.pi * tiptoe_angle)) # motion[frame].mulJointOrientationLocal(idDic['LeftFoot'], mm.exp(mm.unitX(), math.pi * tiptoe_angle * 0.95)) # motion[frame].mulJointOrientationLocal(idDic['RightFoot'], mm.exp(mm.unitX(), math.pi * tiptoe_angle * 0.95)) # motion[frame].mulJointOrientationLocal(idDic['LeftFoot'], mm.exp(mm.unitX(), math.pi * tiptoe_angle)) # motion[frame].mulJointOrientationLocal(idDic['RightFoot'], mm.exp(mm.unitX(), math.pi * tiptoe_angle)) if getParamVal('left tilt angle') > 0.001: left_tilt_angle = getParamVal('left tilt angle') if motion[0].skeleton.getJointIndex( 'LeftFoot_foot_0_1') is not None: motion[frame].mulJointOrientationLocal( idDic['LeftFoot_foot_0_1'], mm.exp(mm.unitZ(), -math.pi * left_tilt_angle)) else: motion[frame].mulJointOrientationLocal( idDic['LeftFoot_foot_0_1_0'], mm.exp(mm.unitZ(), -math.pi * left_tilt_angle)) motion[frame].mulJointOrientationLocal( idDic['LeftFoot'], mm.exp(mm.unitZ(), math.pi * left_tilt_angle)) elif getParamVal('left tilt angle') < -0.001: left_tilt_angle = getParamVal('left tilt angle') motion[frame].mulJointOrientationLocal( idDic['LeftFoot_foot_0_0'], mm.exp(mm.unitZ(), -math.pi * left_tilt_angle)) if motion[0].skeleton.getJointIndex( 'LeftFoot_foot_0_1') is not None: motion[frame].mulJointOrientationLocal( idDic['LeftFoot_foot_0_1'], mm.exp(mm.unitZ(), math.pi * left_tilt_angle)) else: motion[frame].mulJointOrientationLocal( idDic['LeftFoot_foot_0_1_0'], mm.exp(mm.unitZ(), math.pi * left_tilt_angle)) motion[frame].mulJointOrientationLocal( idDic['LeftFoot'], mm.exp(mm.unitZ(), math.pi * left_tilt_angle)) if getParamVal('right tilt angle') > 0.001: right_tilt_angle = getParamVal('right tilt angle') if motion[0].skeleton.getJointIndex( 'RightFoot_foot_0_1') is not None: motion[frame].mulJointOrientationLocal( idDic['RightFoot_foot_0_1'], mm.exp(mm.unitZ(), math.pi * right_tilt_angle)) else: motion[frame].mulJointOrientationLocal( idDic['RightFoot_foot_0_1_0'], mm.exp(mm.unitZ(), math.pi * right_tilt_angle)) motion[frame].mulJointOrientationLocal( idDic['RightFoot'], mm.exp(mm.unitZ(), -math.pi * right_tilt_angle)) elif getParamVal('right tilt angle') < -0.001: right_tilt_angle = getParamVal('right tilt angle') motion[frame].mulJointOrientationLocal( idDic['RightFoot_foot_0_0'], mm.exp(mm.unitZ(), math.pi * right_tilt_angle)) if motion[0].skeleton.getJointIndex( 'RightFoot_foot_0_1') is not None: motion[frame].mulJointOrientationLocal( idDic['RightFoot_foot_0_1'], mm.exp(mm.unitZ(), -math.pi * right_tilt_angle)) # else: # motion[frame].mulJointOrientationLocal(idDic['RightFoot_foot_0_1_0'], mm.exp(mm.unitZ(), -math.pi * right_tilt_angle)) motion[frame].mulJointOrientationLocal( idDic['RightFoot'], mm.exp(mm.unitZ(), -math.pi * right_tilt_angle)) motionModel.update(motion[frame]) motionModel.translateByOffset( np.array([ getParamVal('com X offset'), getParamVal('com Y offset'), getParamVal('com Z offset') ])) controlModel_ik.set_q(controlModel.get_q()) global g_initFlag global forceShowTime global JsysPre global JsupPreL global JsupPreR global JconstPre global preFootCenter global maxContactChangeCount global contactChangeCount global contact global contactChangeType Kt, Kl, Kh, Bl, Bh, kt_sup = getParamVals( ['Kt', 'Kl', 'Kh', 'Bl', 'Bh', 'SupKt']) Dt = 2 * (Kt**.5) Dl = 2 * (Kl**.5) Dh = 2 * (Kh**.5) dt_sup = 2 * (kt_sup**.5) # 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) # ype.flatten(fix_dofs(DOFs, ddth_des, mcfg, joint_names), ddth_des_flat) # ype.flatten(fix_dofs(DOFs, dth, mcfg, joint_names), dth_flat) ype.flatten(ddth_des, ddth_des_flat) ype.flatten(dth, dth_flat) ################################################# # jacobian ################################################# contact_des_ids = list() # desired contact segments if foot_viewer.check_om_l.value(): contact_des_ids.append( motion[0].skeleton.getJointIndex('LeftFoot_foot_0_0')) if foot_viewer.check_op_l.value(): contact_des_ids.append( motion[0].skeleton.getJointIndex('LeftFoot_foot_0_0_0')) if foot_viewer.check_im_l is not None and foot_viewer.check_im_l.value( ): contact_des_ids.append( motion[0].skeleton.getJointIndex('LeftFoot_foot_0_1')) if foot_viewer.check_ip_l.value(): contact_des_ids.append( motion[0].skeleton.getJointIndex('LeftFoot_foot_0_1_0')) if foot_viewer.check_h_l.value(): contact_des_ids.append( motion[0].skeleton.getJointIndex('LeftFoot_foot_1_0')) if foot_viewer.check_om_r.value(): contact_des_ids.append( motion[0].skeleton.getJointIndex('RightFoot_foot_0_0')) if foot_viewer.check_op_r.value(): contact_des_ids.append( motion[0].skeleton.getJointIndex('RightFoot_foot_0_0_0')) if foot_viewer.check_im_r is not None and foot_viewer.check_im_r.value( ): contact_des_ids.append( motion[0].skeleton.getJointIndex('RightFoot_foot_0_1')) if foot_viewer.check_ip_r.value(): contact_des_ids.append( motion[0].skeleton.getJointIndex('RightFoot_foot_0_1_0')) if foot_viewer.check_h_r.value(): contact_des_ids.append( motion[0].skeleton.getJointIndex('RightFoot_foot_1_0')) contact_ids = list() # temp idx for balancing contact_ids.extend(contact_des_ids) contact_joint_ori = list( map(controlModel.getJointOrientationGlobal, contact_ids)) contact_joint_pos = list( map(controlModel.getJointPositionGlobal, contact_ids)) contact_body_ori = list( map(controlModel.getBodyOrientationGlobal, contact_ids)) contact_body_pos = list( map(controlModel.getBodyPositionGlobal, contact_ids)) contact_body_vel = list( map(controlModel.getBodyVelocityGlobal, contact_ids)) contact_body_angvel = list( map(controlModel.getBodyAngVelocityGlobal, contact_ids)) ref_joint_ori = list( map(motion[frame].getJointOrientationGlobal, contact_ids)) ref_joint_pos = list( map(motion[frame].getJointPositionGlobal, contact_ids)) ref_joint_vel = [ motion.getJointVelocityGlobal(joint_idx, frame) for joint_idx in contact_ids ] ref_joint_angvel = [ motion.getJointAngVelocityGlobal(joint_idx, frame) for joint_idx in contact_ids ] ref_body_ori = list( map(motionModel.getBodyOrientationGlobal, contact_ids)) ref_body_pos = list(map(motionModel.getBodyPositionGlobal, contact_ids)) # ref_body_vel = list(map(controlModel.getBodyVelocityGlobal, contact_ids)) ref_body_angvel = [ motion.getJointAngVelocityGlobal(joint_idx, frame) for joint_idx in contact_ids ] ref_body_vel = [ ref_joint_vel[i] + np.cross(ref_joint_angvel[i], ref_body_pos[i] - ref_joint_pos[i]) for i in range(len(ref_joint_vel)) ] is_contact = [1] * len(contact_ids) contact_right = len(set(contact_des_ids).intersection(rIDlist)) > 0 contact_left = len(set(contact_des_ids).intersection(lIDlist)) > 0 contMotionOffset = th[0][0] - th_r[0][0] linkPositions = controlModel.getBodyPositionsGlobal() linkVelocities = controlModel.getBodyVelocitiesGlobal() linkAngVelocities = controlModel.getBodyAngVelocitiesGlobal() linkInertias = controlModel.getBodyInertiasGlobal() 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) # calculate jacobian Jsys, dJsys = controlModel.computeCom_J_dJdq() J_contacts = [] # type: list[np.ndarray] dJ_contacts = [] # type: list[np.ndarray] for contact_id in contact_ids: J_contacts.append(Jsys[6 * contact_id:6 * contact_id + 6, :]) dJ_contacts.append(dJsys[6 * contact_id:6 * contact_id + 6]) # calculate footCenter footCenter = sum(contact_body_pos) / len(contact_body_pos) if len(contact_body_pos) > 0 \ else .5 * (controlModel.getBodyPositionGlobal(supL) + controlModel.getBodyPositionGlobal(supR)) footCenter[1] = 0. # if len(contact_body_pos) > 2: # hull = ConvexHull(contact_body_pos) footCenter_ref = sum(ref_body_pos) / len(ref_body_pos) if len(ref_body_pos) > 0 \ else .5 * (motionModel.getBodyPositionGlobal(supL) + motionModel.getBodyPositionGlobal(supR)) footCenter_ref = footCenter_ref + contMotionOffset # if len(ref_body_pos) > 2: # hull = ConvexHull(ref_body_pos) footCenter_ref[1] = 0. # footCenter[0] = footCenter[0] + getParamVal('com X offset') # footCenter[1] = footCenter[0] + getParamVal('com Y offset') # footCenter[2] = footCenter[2] + getParamVal('com Z offset') # initialization if g_initFlag == 0: preFootCenter[0] = footCenter.copy() g_initFlag = 1 # if contactChangeCount == 0 and np.linalg.norm(footCenter - preFootCenter[0]) > 0.01: # contactChangeCount += 30 if contactChangeCount > 0: # change footcenter gradually footCenter = preFootCenter[0] + ( maxContactChangeCount - contactChangeCount) * ( footCenter - preFootCenter[0]) / maxContactChangeCount else: preFootCenter[0] = footCenter.copy() # linear momentum # TODO: # We should consider dCM_ref, shouldn't we? # add getBodyPositionGlobal and getBodyPositionsGlobal in csVpModel! # to do that, set joint velocities to vpModel CM_ref_plane = footCenter # CM_ref_plane = footCenter_ref CM_ref = footCenter + np.array([ getParamVal('com X offset'), motionModel.getCOM()[1] + getParamVal('com Y offset'), getParamVal('com Z offset') ]) dL_des_plane = Kl * totalMass * (CM_ref - CM) - Dl * totalMass * dCM # dL_des_plane = Kl * totalMass * (CM_ref_plane - CM_plane) - Dl * totalMass * dCM_plane # dL_des_plane[1] = 0. # print('dCM_plane : ', np.linalg.norm(dCM_plane)) # angular momentum CP_ref = footCenter # CP_ref = footCenter_ref bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce( bodyIDsToCheck, mus, Ks, Ds) CP = yrp.getCP(contactPositions, contactForces) if CP_old[0] is None or CP is None: dCP = None else: dCP = (CP - CP_old[0]) / (1 / 30.) CP_old[0] = CP if CP is not None and dCP is not None: ddCP_des = Kh * (CP_ref - CP) - Dh * dCP dCP_des = dCP + ddCP_des * (1 / 30.) CP_des = CP + dCP_des * (1 / 30.) # CP_des = footCenter CP_des = CP + dCP * (1 / 30.) + .5 * ddCP_des * ((1 / 30.)**2) dH_des = np.cross( (CP_des - CM), (dL_des_plane + totalMass * mm.s2v(wcfg.gravity))) if contactChangeCount > 0: # and contactChangeType == 'DtoS': dH_des *= (maxContactChangeCount - contactChangeCount) / maxContactChangeCount else: dH_des = None # convex hull contact_pos_2d = np.asarray([ np.array([contactPosition[0], contactPosition[2]]) for contactPosition in contactPositions ]) p = np.array([CM_plane[0], CM_plane[2]]) # hull = None # type: Delaunay # if contact_pos_2d.shape[0] > 0: # hull = Delaunay(contact_pos_2d) # print(hull.find_simplex(p) >= 0) # set up equality constraint # TODO: # logSO3 is just q'', not acceleration. # To make a_oris acceleration, q'' -> a will be needed # body_ddqs = list(map(mm.logSO3, [mm.getSO3FromVectors(np.dot(body_ori, mm.unitY()), mm.unitY()) for body_ori in contact_body_ori])) # body_ddqs = list(map(mm.logSO3, [np.dot(contact_body_ori[i].T, np.dot(ref_body_ori[i], mm.getSO3FromVectors(np.dot(ref_body_ori[i], mm.unitY()), mm.unitY()))) for i in range(len(contact_body_ori))])) # body_ddqs = list(map(mm.logSO3, [np.dot(contact_body_ori[i].T, np.dot(ref_body_ori[i], mm.getSO3FromVectors(np.dot(ref_body_ori[i], up_vec_in_each_link[contact_ids[i]]), mm.unitY()))) for i in range(len(contact_body_ori))])) a_oris = list( map(mm.logSO3, [ np.dot( contact_body_ori[i].T, np.dot( ref_body_ori[i], mm.getSO3FromVectors( np.dot(ref_body_ori[i], up_vec_in_each_link[contact_ids[i]]), mm.unitY()))) for i in range(len(contact_body_ori)) ])) a_oris = list( map(mm.logSO3, [ np.dot( np.dot( ref_body_ori[i], mm.getSO3FromVectors( np.dot(ref_body_ori[i], up_vec_in_each_link[contact_ids[i]]), mm.unitY())), contact_body_ori[i].T) for i in range(len(contact_body_ori)) ])) body_qs = list(map(mm.logSO3, contact_body_ori)) body_angs = [ np.dot(contact_body_ori[i], contact_body_angvel[i]) for i in range(len(contact_body_ori)) ] body_dqs = [ mm.vel2qd(body_angs[i], body_qs[i]) for i in range(len(body_angs)) ] # a_oris = [np.dot(contact_body_ori[i], mm.qdd2accel(body_ddqs[i], body_dqs[i], body_qs[i])) for i in range(len(contact_body_ori))] # body_ddq = body_ddqs[0] # body_ori = contact_body_ori[0] # body_ang = np.dot(body_ori.T, contact_body_angvel[0]) # # body_q = mm.logSO3(body_ori) # body_dq = mm.vel2qd(body_ang, body_q) # a_ori = np.dot(body_ori, mm.qdd2accel(body_ddq, body_dq, body_q)) KT_SUP = np.diag([kt_sup / 10., kt_sup, kt_sup / 10.]) # KT_SUP = np.diag([kt_sup, kt_sup, kt_sup]) # a_oris = list(map(mm.logSO3, [mm.getSO3FromVectors(np.dot(body_ori, mm.unitY()), mm.unitY()) for body_ori in contact_body_ori])) # a_oris = list(map(mm.logSO3, [mm.getSO3FromVectors(np.dot(contact_body_ori[i], up_vec_in_each_link[contact_ids[i]]), mm.unitY()) for i in range(len(contact_body_ori))])) # a_sups = [np.append(kt_sup*(ref_body_pos[i] - contact_body_pos[i] + contMotionOffset) + dt_sup*(ref_body_vel[i] - contact_body_vel[i]), # kt_sup*a_oris[i]+dt_sup*(ref_body_angvel[i]-contact_body_angvel[i])) for i in range(len(a_oris))] # a_sups = [np.append(kt_sup*(ref_body_pos[i] - contact_body_pos[i] + contMotionOffset) - dt_sup * contact_body_vel[i], # kt_sup*a_oris[i] - dt_sup * contact_body_angvel[i]) for i in range(len(a_oris))] a_sups = [ np.append( np.dot(KT_SUP, (ref_body_pos[i] - contact_body_pos[i] + contMotionOffset)) - dt_sup * contact_body_vel[i], kt_sup * a_oris[i] - dt_sup * contact_body_angvel[i]) for i in range(len(a_oris)) ] # for i in range(len(a_sups)): # a_sups[i][1] = -kt_sup * contact_body_pos[i][1] - dt_sup * contact_body_vel[i][1] # 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) rs = np.dot(dP, np.dot(Jsys, dth_flat)) + np.dot(P, dJsys) r_bias, s_bias = np.hsplit(rs, 2) ####################################################### # optimization ####################################################### # if contact == 2 and footCenterR[1] > doubleTosingleOffset/2: if contact_left and not contact_right: config['weightMap']['RightUpLeg'] = .8 config['weightMap']['RightLeg'] = .8 config['weightMap']['RightFoot'] = .8 else: config['weightMap']['RightUpLeg'] = .1 config['weightMap']['RightLeg'] = .25 config['weightMap']['RightFoot'] = .2 # if contact == 1 and footCenterL[1] > doubleTosingleOffset/2: if contact_right and not contact_left: config['weightMap']['LeftUpLeg'] = .8 config['weightMap']['LeftLeg'] = .8 config['weightMap']['LeftFoot'] = .8 else: config['weightMap']['LeftUpLeg'] = .1 config['weightMap']['LeftLeg'] = .25 config['weightMap']['LeftFoot'] = .2 w = mot.getTrackingWeight(DOFs, motion[0].skeleton, config['weightMap']) mot.addTrackingTerms(problem, totalDOF, Bt, w, ddth_des_flat) if dH_des is not None: mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R, r_bias) mot.addAngularTerms(problem, totalDOF, Bh, dH_des, S, s_bias) if True: for c_idx in range(len(contact_ids)): mot.addConstraint2(problem, totalDOF, J_contacts[c_idx], dJ_contacts[c_idx], dth_flat, a_sups[c_idx]) if contactChangeCount > 0: contactChangeCount = contactChangeCount - 1 if contactChangeCount == 0: maxContactChangeCount = 30 contactChangeType = 0 r = problem.solve() problem.clear() ddth_sol_flat = np.asarray(r['x']) # ddth_sol_flat[foot_seg_dofs] = np.array(ddth_des_flat)[foot_seg_dofs] ype.nested(ddth_sol_flat, 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) # bodyIDs, contactPositions, contactPositionLocals, contactForces, contactVelocities = vpWorld.calcManyPenaltyForce(0, bodyIDsToCheck, mus, Ks, Ds) vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals, contactForces) controlModel.setDOFAccelerations(ddth_sol) # controlModel.setDOFAccelerations(ddth_des) # controlModel.set_ddq(ddth_sol_flat) # controlModel.set_ddq(ddth_des_flat) controlModel.solveHybridDynamics() if forceShowTime > viewer.objectInfoWnd.labelForceDur.value(): forceShowTime = 0 viewer_ResetForceState() forceforce = np.array([ viewer.objectInfoWnd.labelForceX.value(), viewer.objectInfoWnd.labelForceY.value(), viewer.objectInfoWnd.labelForceZ.value() ]) extraForce[0] = getParamVal('Fm') * mm.normalize2(forceforce) if viewer_GetForceState(): forceShowTime += wcfg.timeStep vpWorld.applyPenaltyForce(selectedBodyId, localPos, extraForce) vpWorld.step() controlModel_ik.set_q(controlModel.get_q()) if foot_viewer is not None: foot_viewer.foot_pressure_gl_window.refresh_foot_contact_info( frame, vpWorld, bodyIDsToCheck, mus, Ks, Ds) foot_viewer.foot_pressure_gl_window.goToFrame(frame) # rendering for foot_seg_id in footIdlist: control_model_renderer.body_colors[foot_seg_id] = (255, 240, 255) for contact_id in contact_ids: control_model_renderer.body_colors[contact_id] = (255, 0, 0) rd_footCenter[0] = footCenter rd_footCenter_ref[0] = footCenter_ref rd_CM[0] = CM rd_CM_plane[0] = CM.copy() rd_CM_plane[0][1] = 0. if CP is not None and dCP is not None: rd_CP[0] = CP rd_CP_des[0] = CP_des rd_dL_des_plane[0] = [ dL_des_plane[0] / 100, dL_des_plane[1] / 100, dL_des_plane[2] / 100 ] rd_dH_des[0] = dH_des rd_grf_des[0] = dL_des_plane - totalMass * mm.s2v(wcfg.gravity) del rd_foot_ori[:] del rd_foot_pos[:] # for seg_foot_id in footIdlist: # rd_foot_ori.append(controlModel.getJointOrientationGlobal(seg_foot_id)) # rd_foot_pos.append(controlModel.getJointPositionGlobal(seg_foot_id)) rd_foot_ori.append(controlModel.getJointOrientationGlobal(supL)) rd_foot_ori.append(controlModel.getJointOrientationGlobal(supR)) rd_foot_pos.append(controlModel.getJointPositionGlobal(supL)) rd_foot_pos.append(controlModel.getJointPositionGlobal(supR)) rd_root_des[0] = rootPos[0] rd_root_ori[0] = controlModel.getBodyOrientationGlobal(0) rd_root_pos[0] = controlModel.getBodyPositionGlobal(0) del rd_CF[:] del rd_CF_pos[:] for i in range(len(contactPositions)): rd_CF.append(contactForces[i] / 400) rd_CF_pos.append(contactPositions[i].copy()) if viewer_GetForceState(): rd_exfen_des[0] = [ extraForce[0][0] / 100, extraForce[0][1] / 100, extraForce[0][2] / 100 ] rd_exf_des[0] = [0, 0, 0] else: rd_exf_des[0] = [ extraForce[0][0] / 100, extraForce[0][1] / 100, extraForce[0][2] / 100 ] rd_exfen_des[0] = [0, 0, 0] # extraForcePos[0] = controlModel.getBodyPositionGlobal(selectedBody) extraForcePos[0] = controlModel.getBodyPositionGlobal( selectedBody) - 0.1 * np.array([ viewer.objectInfoWnd.labelForceX.value(), 0., viewer.objectInfoWnd.labelForceZ.value() ]) # render contact_ids # render skeleton if SKELETON_ON: Ts = dict() Ts['pelvis'] = controlModel.getJointTransform(idDic['Hips']) Ts['thigh_R'] = controlModel.getJointTransform(idDic['RightUpLeg']) Ts['shin_R'] = controlModel.getJointTransform(idDic['RightLeg']) Ts['foot_R'] = controlModel.getJointTransform(idDic['RightFoot']) Ts['foot_heel_R'] = controlModel.getJointTransform( idDic['RightFoot']) Ts['heel_R'] = np.eye(4) Ts['outside_metatarsal_R'] = controlModel.getJointTransform( idDic['RightFoot_foot_0_0']) Ts['outside_phalanges_R'] = controlModel.getJointTransform( idDic['RightFoot_foot_0_0_0']) # Ts['inside_metatarsal_R'] = controlModel.getJointTransform(idDic['RightFoot_foot_0_1']) Ts['inside_metatarsal_R'] = np.eye(4) Ts['inside_phalanges_R'] = controlModel.getJointTransform( idDic['RightFoot_foot_0_1_0']) Ts['spine_ribs'] = controlModel.getJointTransform(idDic['Spine']) Ts['head'] = controlModel.getJointTransform(idDic['Spine1']) Ts['upper_limb_R'] = controlModel.getJointTransform( idDic['RightArm']) Ts['lower_limb_R'] = controlModel.getJointTransform( idDic['RightForeArm']) Ts['thigh_L'] = controlModel.getJointTransform(idDic['LeftUpLeg']) Ts['shin_L'] = controlModel.getJointTransform(idDic['LeftLeg']) Ts['foot_L'] = controlModel.getJointTransform(idDic['LeftFoot']) Ts['foot_heel_L'] = controlModel.getJointTransform( idDic['LeftFoot']) Ts['heel_L'] = np.eye(4) Ts['outside_metatarsal_L'] = controlModel.getJointTransform( idDic['LeftFoot_foot_0_0']) Ts['outside_phalanges_L'] = controlModel.getJointTransform( idDic['LeftFoot_foot_0_0_0']) # Ts['inside_metatarsal_L'] = controlModel.getJointTransform(idDic['LeftFoot_foot_0_1']) Ts['inside_metatarsal_L'] = np.eye(4) Ts['inside_phalanges_L'] = controlModel.getJointTransform( idDic['LeftFoot_foot_0_1_0']) Ts['upper_limb_L'] = controlModel.getJointTransform( idDic['LeftArm']) Ts['lower_limb_L'] = controlModel.getJointTransform( idDic['LeftForeArm']) skeleton_renderer.appendFrameState(Ts) viewer.setSimulateCallback(simulateCallback) viewer.startTimer(1 / 30.) # viewer.play() viewer.show() foot_viewer = FootWindow(viewer.x() + viewer.w() + 20, viewer.y(), 300, 500, 'foot contact modifier', controlModel) foot_viewer.show() foot_viewer.check_all_seg() viewer.motionViewWnd.goToFrame(0) Fl.run()
def main(): np.set_printoptions(precision=4, linewidth=200) # motion, mcfg, wcfg, stepsPerFrame, config = mit.create_vchain_5() motion, mcfg, wcfg, stepsPerFrame, config = mit.create_biped() # motion, mcfg, wcfg, stepsPerFrame, config = mit.create_jump_biped() vpWorld = cvw.VpWorld(wcfg) motionModel = cvm.VpMotionModel(vpWorld, motion[0], mcfg) controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg) vpWorld.initialize() controlModel.initializeHybridDynamics() # controlToMotionOffset = (1.5, -0.02, 0) controlToMotionOffset = (1.5, 0, 0) controlModel.translateByOffset(controlToMotionOffset) totalDOF = controlModel.getTotalDOF() DOFs = controlModel.getDOFs() # parameter Kt = config['Kt'] Dt = config['Dt'] # tracking gain Kl = config['Kl'] Dl = config['Dl'] # linear balance gain Kh = config['Kh'] Dh = config['Dh'] # angular balance gain Ks = config['Ks'] Ds = config['Ds'] # penalty force spring gain Bt = config['Bt'] Bl = config['Bl'] Bh = config['Bh'] w = mot.getTrackingWeight(DOFs, motion[0].skeleton, config['weightMap']) supL = motion[0].skeleton.getJointIndex(config['supLink1']) supR = motion[0].skeleton.getJointIndex(config['supLink2']) selectedBody = motion[0].skeleton.getJointIndex(config['end']) # selectedBody = motion[0].skeleton.getJointIndex('Hips') constBody = motion[0].skeleton.getJointIndex('RightFoot') # jacobian JsupL = yjc.makeEmptyJacobian(DOFs, 1) dJsupL = JsupL.copy() JsupPreL = JsupL.copy() JsupR = yjc.makeEmptyJacobian(DOFs, 1) dJsupR = JsupR.copy() JsupPreR = JsupR.copy() Jconst = yjc.makeEmptyJacobian(DOFs, 1) dJconst = Jconst.copy() JconstPre = Jconst.copy() Jsys = yjc.makeEmptyJacobian(DOFs, controlModel.getBodyNum()) dJsys = Jsys.copy() JsysPre = Jsys.copy() supLJointMasks = [yjc.getLinkJointMask(motion[0].skeleton, supL)] supRJointMasks = [yjc.getLinkJointMask(motion[0].skeleton, supR)] constJointMasks = [yjc.getLinkJointMask(motion[0].skeleton, constBody)] allLinkJointMasks = yjc.getAllLinkJointMasks(motion[0].skeleton) # momentum matrix linkMasses = controlModel.getBodyMasses() totalMass = controlModel.getTotalMass() TO = ymt.make_TO(linkMasses) dTO = ymt.make_dTO(len(linkMasses)) # optimization problem = yac.LSE(totalDOF, 12) # a_sup = (0,0,0, 0,0,0) #ori # a_sup = (0,0,0, 0,0,0) #L a_supL = (0, 0, 0, 0, 0, 0) a_supR = (0, 0, 0, 0, 0, 0) a_sup_2 = (0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0) CP_old = [mm.v3(0., 0., 0.)] # penalty method bodyIDsToCheck = list(range(vpWorld.getBodyNum())) # mus = [1.]*len(bodyIDsToCheck) mus = [.5] * len(bodyIDsToCheck) # flat data structure ddth_des_flat = ype.makeFlatList(totalDOF) dth_flat = ype.makeFlatList(totalDOF) ddth_sol = ype.makeNestedList(DOFs) # viewer rd_footCenter = [None] rd_footCenterL = [None] rd_footCenterR = [None] rd_CM_plane = [None] rd_CM = [None] rd_CP = [None] rd_CP_des = [None] rd_dL_des_plane = [None] rd_dH_des = [None] rd_grf_des = [None] rd_exf_des = [None] rd_exfen_des = [None] rd_root_des = [None] rd_CF = [None] rd_CF_pos = [None] rootPos = [None] selectedBodyId = [selectedBody] extraForce = [None] extraForcePos = [None] rightFootVectorX = [None] rightFootVectorY = [None] rightFootVectorZ = [None] rightFootPos = [None] rightVectorX = [None] rightVectorY = [None] rightVectorZ = [None] rightPos = [None] # viewer = ysv.SimpleViewer() viewer = hsv.hpSimpleViewer(viewForceWnd=False) # viewer.record(False) # viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion, (0,255,255), yr.LINK_BONE)) viewer.doc.addObject('motion', motion) viewer.doc.addRenderer( 'motionModel', yr.VpModelRenderer(motionModel, (150, 150, 255), yr.POLYGON_FILL)) # viewer.doc.addRenderer('controlModel', cvr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_LINE)) viewer.doc.addRenderer( 'controlModel', yr.VpModelRenderer(controlModel, (255, 240, 255), yr.POLYGON_FILL)) viewer.doc.addRenderer('rd_footCenter', yr.PointsRenderer(rd_footCenter)) viewer.doc.addRenderer('rd_CM_plane', yr.PointsRenderer(rd_CM_plane, (255, 255, 0))) viewer.doc.addRenderer('rd_CP', yr.PointsRenderer(rd_CP, (0, 255, 0))) viewer.doc.addRenderer('rd_CP_des', yr.PointsRenderer(rd_CP_des, (255, 0, 255))) viewer.doc.addRenderer( 'rd_dL_des_plane', yr.VectorsRenderer(rd_dL_des_plane, rd_CM, (255, 255, 0))) viewer.doc.addRenderer('rd_dH_des', yr.VectorsRenderer(rd_dH_des, rd_CM, (0, 255, 0))) # viewer.doc.addRenderer('rd_grf_des', yr.ForcesRenderer(rd_grf_des, rd_CP_des, (0,255,0), .001)) viewer.doc.addRenderer('rd_CF', yr.VectorsRenderer(rd_CF, rd_CF_pos, (255, 255, 0))) viewer.doc.addRenderer( 'extraForce', yr.VectorsRenderer(rd_exf_des, extraForcePos, (0, 255, 0))) viewer.doc.addRenderer( 'extraForceEnable', yr.VectorsRenderer(rd_exfen_des, extraForcePos, (255, 0, 0))) # viewer.doc.addRenderer('right_foot_oriX', yr.VectorsRenderer(rightFootVectorX, rightFootPos, (255,0,0))) # viewer.doc.addRenderer('right_foot_oriY', yr.VectorsRenderer(rightFootVectorY, rightFootPos, (0,255,0))) # viewer.doc.addRenderer('right_foot_oriZ', yr.VectorsRenderer(rightFootVectorZ, rightFootPos, (0,0,255))) # viewer.doc.addRenderer('right_oriX', yr.VectorsRenderer(rightVectorX, rightPos, (255,0,0))) # viewer.doc.addRenderer('right_oriY', yr.VectorsRenderer(rightVectorY, rightPos, (0,255,0))) # viewer.doc.addRenderer('right_oriZ', yr.VectorsRenderer(rightVectorZ, rightPos, (0,0,255))) # success!! # initKt = 50 # initKl = 10.1 # initKh = 3.1 # initBl = .1 # initBh = .1 # initSupKt = 21.6 # initFm = 100.0 # success!! -- 2015.2.12. double stance # initKt = 50 # initKl = 37.1 # initKh = 41.8 # initBl = .1 # initBh = .13 # initSupKt = 21.6 # initFm = 165.0 # single stance # initKt = 25 # initKl = 80.1 # initKh = 10.8 # initBl = .1 # initBh = .13 # initSupKt = 21.6 # initFm = 50.0 # single stance -> double stance # initKt = 25 # initKl = 60. # initKh = 20. # initBl = .1 # initBh = .13 # initSupKt = 21.6 # initFm = 50.0 initKt = 25 initKl = 100. initKh = 100. initBl = .1 initBh = .13 initSupKt = 17 initFm = 50.0 initComX = 0. initComY = 0. initComZ = 0. viewer.objectInfoWnd.add1DSlider("Kt", 0., 300., 1., initKt) viewer.objectInfoWnd.add1DSlider("Kl", 0., 300., 1., initKl) viewer.objectInfoWnd.add1DSlider("Kh", 0., 300., 1., initKh) viewer.objectInfoWnd.add1DSlider("Bl", 0., 1., .001, initBl) viewer.objectInfoWnd.add1DSlider("Bh", 0., 1., .001, initBh) viewer.objectInfoWnd.add1DSlider("SupKt", 0., 100., 0.1, initSupKt) viewer.objectInfoWnd.add1DSlider("Fm", 0., 1000., 10., initFm) viewer.objectInfoWnd.add1DSlider("com X offset", -1., 1., 0.01, initComX) viewer.objectInfoWnd.add1DSlider("com Y offset", -1., 1., 0.01, initComY) viewer.objectInfoWnd.add1DSlider("com Z offset", -1., 1., 0.01, initComZ) viewer.force_on = False def viewer_SetForceState(object): viewer.force_on = True def viewer_GetForceState(): return viewer.force_on def viewer_ResetForceState(): viewer.force_on = False viewer.objectInfoWnd.addBtn('Force on', viewer_SetForceState) viewer_ResetForceState() offset = 60 viewer.objectInfoWnd.begin() viewer.objectInfoWnd.labelForceX = Fl_Value_Input(20, 30 + offset * 9, 40, 20, 'X') viewer.objectInfoWnd.labelForceX.value(0) viewer.objectInfoWnd.labelForceY = Fl_Value_Input(80, 30 + offset * 9, 40, 20, 'Y') viewer.objectInfoWnd.labelForceY.value(0) viewer.objectInfoWnd.labelForceZ = Fl_Value_Input(140, 30 + offset * 9, 40, 20, 'Z') viewer.objectInfoWnd.labelForceZ.value(1) viewer.objectInfoWnd.labelForceDur = Fl_Value_Input( 220, 30 + offset * 9, 40, 20, 'Dur') viewer.objectInfoWnd.labelForceDur.value(0.1) viewer.objectInfoWnd.end() # self.sliderFm = Fl_Hor_Nice_Slider(10, 42+offset*6, 250, 10) def getParamVal(paramname): return viewer.objectInfoWnd.getVal(paramname) def getParamVals(paramnames): return (getParamVal(name) for name in paramnames) ################################### # simulate ################################### def simulateCallback(frame): motionModel.update(motion[frame]) global g_initFlag global forceShowTime global JsysPre global JsupPreL global JsupPreR global JsupPre global JconstPre global preFootCenter global maxContactChangeCount global contactChangeCount global contact global contactChangeType # Kt, Kl, Kh, Bl, Bh, kt_sup = viewer.GetParam() Kt, Kl, Kh, Bl, Bh, kt_sup = getParamVals( ['Kt', 'Kl', 'Kh', 'Bl', 'Bh', 'SupKt']) Dt = 2 * (Kt**.5) Dl = 2 * (Kl**.5) Dh = 2 * (Kh**.5) dt_sup = 2 * (kt_sup**.5) doubleTosingleOffset = 0.15 singleTodoubleOffset = 0.30 # doubleTosingleOffset = 0.09 doubleTosingleVelOffset = 0.0 # 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) ype.flatten(ddth_des, ddth_des_flat) ype.flatten(dth, dth_flat) ################################################# # jacobian ################################################# # caution!! body orientation and joint orientation of foot are totally different!! footOriL = controlModel.getJointOrientationGlobal(supL) footOriR = controlModel.getJointOrientationGlobal(supR) # desire footCenter[1] = 0.041135 # desire footCenter[1] = 0.0197 footCenterL = controlModel.getBodyPositionGlobal(supL) footCenterR = controlModel.getBodyPositionGlobal(supR) footBodyOriL = controlModel.getBodyOrientationGlobal(supL) footBodyOriR = controlModel.getBodyOrientationGlobal(supR) footBodyVelL = controlModel.getBodyVelocityGlobal(supL) footBodyVelR = controlModel.getBodyVelocityGlobal(supR) footBodyAngVelL = controlModel.getBodyAngVelocityGlobal(supL) footBodyAngVelR = controlModel.getBodyAngVelocityGlobal(supR) refFootL = motionModel.getBodyPositionGlobal(supL) refFootR = motionModel.getBodyPositionGlobal(supR) refFootVelL = motionModel.getBodyVelocityGlobal(supL) refFootVelR = motionModel.getBodyVelocityGlobal(supR) refFootAngVelL = motionModel.getBodyAngVelocityGlobal(supL) refFootAngVelR = motionModel.getBodyAngVelocityGlobal(supR) refFootJointVelR = motion.getJointVelocityGlobal(supR, frame) refFootJointAngVelR = motion.getJointAngVelocityGlobal(supR, frame) refFootJointR = motion.getJointPositionGlobal(supR, frame) refFootVelR = refFootJointVelR + np.cross(refFootJointAngVelR, (refFootR - refFootJointR)) refFootJointVelL = motion.getJointVelocityGlobal(supL, frame) refFootJointAngVelL = motion.getJointAngVelocityGlobal(supL, frame) refFootJointL = motion.getJointPositionGlobal(supL, frame) refFootVelL = refFootJointVelL + np.cross(refFootJointAngVelL, (refFootL - refFootJointL)) contactR = 1 contactL = 1 if refFootVelR[1] < 0 and refFootVelR[1] / 30. + refFootR[ 1] > singleTodoubleOffset: contactR = 0 if refFootVelL[1] < 0 and refFootVelL[1] / 30. + refFootL[ 1] > singleTodoubleOffset: contactL = 0 if refFootVelR[1] > 0 and refFootVelR[1] / 30. + refFootR[ 1] > doubleTosingleOffset: contactR = 0 if refFootVelL[1] > 0 and refFootVelL[1] / 30. + refFootL[ 1] > doubleTosingleOffset: contactL = 0 # if 32 < frame < 147: # contactR = 0 contMotionOffset = th[0][0] - th_r[0][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. P = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions, CM, linkInertias) dP = ymt.getPureInertiaMatrixDerivative(dTO, linkMasses, linkVelocities, dCM, linkAngVelocities, linkInertias) # calculate jacobian Jsys, dJsys = controlModel.computeCom_J_dJdq() JsupL = Jsys[6 * supL:6 * supL + 6, :] dJsupL = dJsys[6 * supL:6 * supL + 6] JsupR = Jsys[6 * supR:6 * supR + 6, :] dJsupR = dJsys[6 * supR:6 * supR + 6] # calculate contact state # if g_initFlag == 1 and contact == 1 and refFootR[1] < doubleTosingleOffset and footCenterR[1] < 0.08: if g_initFlag == 1: # contact state # 0: flying 1: right only 2: left only 3: double # if contact == 2 and refFootR[1] < doubleTosingleOffset: if contact == 2 and contactR == 1: contact = 3 maxContactChangeCount += 30 contactChangeCount += maxContactChangeCount contactChangeType = 'StoD' # elif contact == 3 and refFootL[1] < doubleTosingleOffset: elif contact == 1 and contactL == 1: contact = 3 maxContactChangeCount += 30 contactChangeCount += maxContactChangeCount contactChangeType = 'StoD' # elif contact == 3 and refFootR[1] > doubleTosingleOffset: elif contact == 3 and contactR == 0: contact = 2 contactChangeCount += maxContactChangeCount contactChangeType = 'DtoS' # elif contact == 3 and refFootL[1] > doubleTosingleOffset: elif contact == 3 and contactL == 0: contact = 1 contactChangeCount += maxContactChangeCount contactChangeType = 'DtoS' else: contact = 0 # if refFootR[1] < doubleTosingleOffset: if contactR == 1: contact += 1 # if refFootL[1] < doubleTosingleOffset: if contactL == 1: contact += 2 # initialization if g_initFlag == 0: JsysPre = Jsys.copy() JsupPreL = JsupL.copy() JsupPreR = JsupR.copy() JconstPre = Jconst.copy() softConstPoint = footCenterR.copy() # yjc.computeJacobian2(JsysPre, DOFs, jointPositions, jointAxeses, linkPositions, allLinkJointMasks) # yjc.computeJacobian2(JsupPreL, DOFs, jointPositions, jointAxeses, [footCenterL], supLJointMasks) # yjc.computeJacobian2(JsupPreR, DOFs, jointPositions, jointAxeses, [footCenterR], supRJointMasks) # yjc.computeJacobian2(JconstPre, DOFs, jointPositions, jointAxeses, [softConstPoint], constJointMasks) footCenter = footCenterL + (footCenterR - footCenterL) / 2.0 footCenter[1] = 0. preFootCenter = footCenter.copy() # footToBodyFootRotL = np.dot(np.transpose(footOriL), footBodyOriL) # footToBodyFootRotR = np.dot(np.transpose(footOriR), footBodyOriR) if refFootR[1] < doubleTosingleOffset: contact += 1 if refFootL[1] < doubleTosingleOffset: contact += 2 g_initFlag = 1 # calculate footCenter footCenter = footCenterL + (footCenterR - footCenterL) / 2.0 # if refFootR[1] >doubleTosingleOffset: # if refFootR[1] > doubleTosingleOffset or footCenterR[1] > 0.08: # if contact == 1 or footCenterR[1] > 0.08: # if contact == 2 or footCenterR[1] > doubleTosingleOffset/2: if contact == 2: footCenter = footCenterL.copy() # elif contact == 1 or footCenterL[1] > doubleTosingleOffset/2: if contact == 1: footCenter = footCenterR.copy() footCenter[1] = 0. if contactChangeCount > 0 and contactChangeType == 'StoD': # change footcenter gradually footCenter = preFootCenter + ( maxContactChangeCount - contactChangeCount) * ( footCenter - preFootCenter) / maxContactChangeCount preFootCenter = footCenter.copy() # linear momentum # TODO: # We should consider dCM_ref, shouldn't we? # add getBodyPositionGlobal and getBodyPositionsGlobal in csVpModel! # todo that, set joint velocities to vpModel CM_ref_plane = footCenter dL_des_plane = Kl * totalMass * (CM_ref_plane - CM_plane) - Dl * totalMass * dCM_plane # dL_des_plane[1] = 0. # angular momentum CP_ref = footCenter bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce( bodyIDsToCheck, mus, Ks, Ds) # bodyIDs, contactPositions, contactPositionLocals, contactForces, contactVelocities = vpWorld.calcManyPenaltyForce(0, bodyIDsToCheck, mus, Ks, Ds) CP = yrp.getCP(contactPositions, contactForces) if CP_old[0] is None or CP is None: dCP = None else: dCP = (CP - CP_old[0]) / (1 / 30.) CP_old[0] = CP if CP is not None and dCP is not None: ddCP_des = Kh * (CP_ref - CP) - 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 * mm.s2v(wcfg.gravity))) if contactChangeCount > 0: # and contactChangeType == 'DtoS': # dH_des *= (maxContactChangeCount - contactChangeCount)/(maxContactChangeCount*10) dH_des *= (maxContactChangeCount - contactChangeCount) / (maxContactChangeCount) # dH_des *= (contactChangeCount)/(maxContactChangeCount)*.9+.1 else: dH_des = None # H = np.dot(P, np.dot(Jsys, dth_flat)) # dH_des = -Kh* H[3:] # soft point constraint #softConstPoint = refFootR.copy() ##softConstPoint[0] += 0.2 #Ksc = 50 #Dsc = 2*(Ksc**.5) #Bsc = 1. #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_des - dP_cur) #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) #dJconst = (Jconst - Jconst)/(1/30.) #JconstPre = Jconst.copy() ##yjc.computeJacobianDerivative2(dJconst, DOFs, jointPositions, jointAxeses, linkAngVelocities, [softConstPoint], constJointMasks, False) #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)) #q_bias1 = np.dot(np.dot(Z, dJconst), dth_flat) + q2 #set up equality constraint a_oriL = mm.logSO3( mm.getSO3FromVectors(np.dot(footBodyOriL, np.array([0, 1, 0])), np.array([0, 1, 0]))) a_oriR = mm.logSO3( mm.getSO3FromVectors(np.dot(footBodyOriR, np.array([0, 1, 0])), np.array([0, 1, 0]))) #if contact == 3 and contactChangeCount < maxContactChangeCount/4 and contactChangeCount >=1: #kt_sup = 30 #viewer.objectInfoWnd.labelSupKt.value(kt_sup) #viewer.objectInfoWnd.sliderSupKt.value(initSupKt*10) # a_supL = np.append(kt_sup*(refFootL - footCenterL + contMotionOffset) + dt_sup*(refFootVelL - footBodyVelL), kt_sup*a_oriL+dt_sup*(refFootAngVelL-footBodyAngVelL)) # a_supR = np.append(kt_sup*(refFootR - footCenterR + contMotionOffset) + dt_sup*(refFootVelR - footBodyVelR), kt_sup*a_oriR+dt_sup*(refFootAngVelR-footBodyAngVelR)) a_supL = np.append( kt_sup * (refFootL - footCenterL + contMotionOffset) - dt_sup * footBodyVelL, kt_sup * a_oriL - dt_sup * footBodyAngVelL) a_supR = np.append( kt_sup * (refFootR - footCenterR + contMotionOffset) - dt_sup * footBodyVelR, kt_sup * a_oriR - dt_sup * footBodyAngVelR) if contactChangeCount > 0 and contactChangeType == 'DtoS': #refFootR += (footCenter-CM_plane)/2. #refFootR[1] = 0 #pre contact value are needed #if contact == 2: ##refFootR[0] += 0.2 ##refFootR[2] -= 0.05 #offsetDropR = (footCenter-CM_plane)/2. #refFootR += offsetDropR #refFootR[1] = 0. ##refFootR[2] = footCenterR[2] - contMotionOffset[2] ##refFootR[0] = footCenterR[0] - contMotionOffset[0] #refFootL[0] += 0.05 #refFootL[2] -= 0.05 #elif contact == 1: #offsetDropL = (footCenter-CM_plane)/2. #refFootL += offsetDropL #refFootL[1] = 0. #a_supL = np.append(kt_sup*(refFootL - footCenterL + contMotionOffset) + dt_sup*(refFootVelL - footBodyVelL), kt_sup*a_oriL+dt_sup*(refFootAngVelL-footBodyAngVelL)) #a_supR = np.append(kt_sup*(refFootR - footCenterR + contMotionOffset) + dt_sup*(refFootVelR - footBodyVelR), kt_sup*a_oriR+dt_sup*(refFootAngVelR-footBodyAngVelR)) #a_supL = np.append(kt_sup*(refFootL - footCenterL + contMotionOffset) + dt_sup*(refFootVelL - footBodyVelL), 16*kt_sup*a_oriL+4*dt_sup*(refFootAngVelL-footBodyAngVelL)) #a_supR = np.append(kt_sup*(refFootR - footCenterR + contMotionOffset) + dt_sup*(refFootVelR - footBodyVelR), 16*kt_sup*a_oriR+4*dt_sup*(refFootAngVelR-footBodyAngVelR)) a_supL = np.append( kt_sup * (refFootL - footCenterL + contMotionOffset) + dt_sup * (refFootVelL - footBodyVelL), 4 * kt_sup * a_oriL + 2 * dt_sup * (refFootAngVelL - footBodyAngVelL)) a_supR = np.append( kt_sup * (refFootR - footCenterR + contMotionOffset) + dt_sup * (refFootVelR - footBodyVelR), 4 * kt_sup * a_oriR + 2 * dt_sup * (refFootAngVelR - footBodyAngVelR)) elif contactChangeCount > 0 and contactChangeType == 'StoD': #refFootR[0] +=0.05 #refFootR[2] +=0.05 linkt = (13. * contactChangeCount) / (maxContactChangeCount) + 1. lindt = 2 * (linkt**.5) angkt = (13. * contactChangeCount) / (maxContactChangeCount) + 1. angdt = 2 * (angkt**.5) #a_supL = np.append(4*kt_sup*(refFootL - footCenterL + contMotionOffset) + 2*dt_sup*(refFootVelL - footBodyVelL), 16*kt_sup*a_oriL+4*dt_sup*(refFootAngVelL-footBodyAngVelL)) #a_supR = np.append(4*kt_sup*(refFootR - footCenterR + contMotionOffset) + 2*dt_sup*(refFootVelR - footBodyVelR), 16*kt_sup*a_oriR+4*dt_sup*(refFootAngVelR-footBodyAngVelR)) a_supL = np.append( linkt * kt_sup * (refFootL - footCenterL + contMotionOffset) + lindt * dt_sup * (refFootVelL - footBodyVelL), angkt * kt_sup * a_oriL + angdt * dt_sup * (refFootAngVelL - footBodyAngVelL)) a_supR = np.append( linkt * kt_sup * (refFootR - footCenterR + contMotionOffset) + lindt * dt_sup * (refFootVelR - footBodyVelR), angkt * kt_sup * a_oriR + angdt * dt_sup * (refFootAngVelR - footBodyAngVelR)) #a_supL = np.append(16*kt_sup*(refFootL - footCenterL + contMotionOffset) + 4*dt_sup*(refFootVelL - footBodyVelL), 16*kt_sup*a_oriL+4*dt_sup*(refFootAngVelL-footBodyAngVelL)) #a_supR = np.append(16*kt_sup*(refFootR - footCenterR + contMotionOffset) + 4*dt_sup*(refFootVelR - footBodyVelR), 16*kt_sup*a_oriR+4*dt_sup*(refFootAngVelR-footBodyAngVelR)) #a_supL = np.append(4*kt_sup*(refFootL - footCenterL + contMotionOffset) + 2*dt_sup*(refFootVelL - footBodyVelL), 32*kt_sup*a_oriL+5.6*dt_sup*(refFootAngVelL-footBodyAngVelL)) #a_supR = np.append(4*kt_sup*(refFootR - footCenterR + contMotionOffset) + 2*dt_sup*(refFootVelR - footBodyVelR), 32*kt_sup*a_oriR+5.6*dt_sup*(refFootAngVelR-footBodyAngVelR)) #a_supL[1] = kt_sup*(refFootL[1] - footCenterL[1] + contMotionOffset[1]) + dt_sup*(refFootVelL[1] - footBodyVelL[1]) #a_supR[1] = kt_sup*(refFootR[1] - footCenterR[1] + contMotionOffset[1]) + dt_sup*(refFootVelR[1] - footBodyVelR[1]) ##if contact == 2: #if refFootR[1] <doubleTosingleOffset : #Jsup = np.vstack((JsupL, JsupR)) #dJsup = np.vstack((dJsupL, dJsupR)) #a_sup = np.append(a_supL, a_supR) #else: #Jsup = JsupL.copy() #dJsup = dJsupL.copy() #a_sup = a_supL.copy() # 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) rs = np.dot(dP, np.dot(Jsys, dth_flat)) + np.dot(P, dJsys) r_bias, s_bias = np.hsplit(rs, 2) ####################################################### # optimization ####################################################### #if contact == 2 and footCenterR[1] > doubleTosingleOffset/2: if contact == 2: config['weightMap']['RightUpLeg'] = .8 config['weightMap']['RightLeg'] = .8 config['weightMap']['RightFoot'] = .8 else: config['weightMap']['RightUpLeg'] = .1 config['weightMap']['RightLeg'] = .25 config['weightMap']['RightFoot'] = .2 #if contact == 1 and footCenterL[1] > doubleTosingleOffset/2: if contact == 1: config['weightMap']['LeftUpLeg'] = .8 config['weightMap']['LeftLeg'] = .8 config['weightMap']['LeftFoot'] = .8 else: config['weightMap']['LeftUpLeg'] = .1 config['weightMap']['LeftLeg'] = .25 config['weightMap']['LeftFoot'] = .2 w = mot.getTrackingWeight(DOFs, motion[0].skeleton, config['weightMap']) #if contact == 2: #mot.addSoftPointConstraintTerms(problem, totalDOF, Bsc, ddP_des1, Q1, q_bias1) mot.addTrackingTerms(problem, totalDOF, Bt, w, ddth_des_flat) if dH_des is not None: mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R, r_bias) mot.addAngularTerms(problem, totalDOF, Bh, dH_des, S, s_bias) #if contact & 1 and contactChangeCount == 0: if contact & 1: #if refFootR[1] < doubleTosingleOffset: mot.addConstraint2(problem, totalDOF, JsupR, dJsupR, dth_flat, a_supR) if contact & 2: #if refFootL[1] < doubleTosingleOffset: mot.addConstraint2(problem, totalDOF, JsupL, dJsupL, dth_flat, a_supL) if contactChangeCount > 0: contactChangeCount -= 1 if contactChangeCount == 0: maxContactChangeCount = 30 contactChangeType = 0 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) # print(contactForces) #bodyIDs, contactPositions, contactPositionLocals, contactForces, contactVelocities = vpWorld.calcManyPenaltyForce(0, bodyIDsToCheck, mus, Ks, Ds) vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals, contactForces) controlModel.setDOFAccelerations(ddth_sol) controlModel.solveHybridDynamics() if forceShowTime > viewer.objectInfoWnd.labelForceDur.value(): forceShowTime = 0 viewer_ResetForceState() forceforce = np.array([ viewer.objectInfoWnd.labelForceX.value(), viewer.objectInfoWnd.labelForceY.value(), viewer.objectInfoWnd.labelForceZ.value() ]) extraForce[0] = getParamVal('Fm') * mm.normalize2(forceforce) # extraForce[0] = viewer.objectInfoWnd.labelFm.value() * mm.normalize2(forceforce) if viewer_GetForceState(): forceShowTime += wcfg.timeStep vpWorld.applyPenaltyForce(selectedBodyId, localPos, extraForce) vpWorld.step() # rendering rightFootVectorX[0] = np.dot(footOriL, np.array([.1, 0, 0])) rightFootVectorY[0] = np.dot(footOriL, np.array([0, .1, 0])) rightFootVectorZ[0] = np.dot(footOriL, np.array([0, 0, .1])) rightFootPos[0] = footCenterL rightVectorX[0] = np.dot(footBodyOriL, np.array([.1, 0, 0])) rightVectorY[0] = np.dot(footBodyOriL, np.array([0, .1, 0])) rightVectorZ[0] = np.dot(footBodyOriL, np.array([0, 0, .1])) rightPos[0] = footCenterL + np.array([.1, 0, 0]) rd_footCenter[0] = footCenter rd_footCenterL[0] = footCenterL rd_footCenterR[0] = footCenterR rd_CM[0] = CM rd_CM_plane[0] = CM.copy() rd_CM_plane[0][1] = 0. if CP is not None and dCP is not None: rd_CP[0] = CP rd_CP_des[0] = CP_des rd_dL_des_plane[0] = [ dL_des_plane[0] / 100, dL_des_plane[1] / 100, dL_des_plane[2] / 100 ] rd_dH_des[0] = dH_des rd_grf_des[0] = dL_des_plane - totalMass * mm.s2v(wcfg.gravity) rd_root_des[0] = rootPos[0] del rd_CF[:] del rd_CF_pos[:] for i in range(len(contactPositions)): rd_CF.append(contactForces[i] / 400) rd_CF_pos.append(contactPositions[i].copy()) if viewer_GetForceState(): rd_exfen_des[0] = [ extraForce[0][0] / 100, extraForce[0][1] / 100, extraForce[0][2] / 100 ] rd_exf_des[0] = [0, 0, 0] else: rd_exf_des[0] = [ extraForce[0][0] / 100, extraForce[0][1] / 100, extraForce[0][2] / 100 ] rd_exfen_des[0] = [0, 0, 0] extraForcePos[0] = controlModel.getBodyPositionGlobal(selectedBody) viewer.setSimulateCallback(simulateCallback) viewer.startTimer(1 / 30.) viewer.show() Fl.run()
def main(): np.set_printoptions(precision=4, linewidth=200) # motion, mcfg, wcfg, stepsPerFrame, config = mit.create_vchain_5() motion, mcfg, wcfg, stepsPerFrame, config = mit.create_biped() mcfg_motion = mit.normal_mcfg() vpWorld = cvw.VpWorld(wcfg) motionModel = cvm.VpMotionModel(vpWorld, motion[0], mcfg) motionModel.recordVelByFiniteDiff() controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg) footPartNum = config['FootPartNum'] if footPartNum > 1: elasticity = 2000 damping = 2 * (elasticity**.5) springBody1 = 5 springBody2 = 6 springBody1Pos = motionModel.getBodyPositionGlobal( motion[0].skeleton.getJointIndex(config['FootLPart'][springBody1])) springBody2Pos = motionModel.getBodyPositionGlobal( motion[0].skeleton.getJointIndex(config['FootLPart'][springBody2])) initialDist = mm.length(springBody1Pos - springBody2Pos) * 1. node = mcfg.getNode(mit.LEFT_PHALANGE_1) initialDist -= node.width #0.084 v1 = (-node.width * 0.5, 0.0, node.length * 0.4) v2 = (node.width * 0.5, 0.0, node.length * 0.4) controlModel.setSpring( motion[0].skeleton.getJointIndex(config['FootLPart'][springBody1]), motion[0].skeleton.getJointIndex(config['FootLPart'][springBody2]), elasticity, damping, v2, v1, initialDist) controlModel.setSpring( motion[0].skeleton.getJointIndex(config['FootRPart'][springBody1]), motion[0].skeleton.getJointIndex(config['FootRPart'][springBody2]), elasticity, damping, v1, v2, initialDist) #elasticity = 10 #damping = 2*(elasticity**.5) #springBody1 = 3 #springBody2 = 4 #node = mcfg.getNode(mit.LEFT_PHALANGE_1) #springBody1Pos = motionModel.getBodyPositionGlobal(motion[0].skeleton.getJointIndex(config['FootLPart'][springBody1])) #springBody2Pos = motionModel.getBodyPositionGlobal(motion[0].skeleton.getJointIndex(config['FootLPart'][springBody2])) #initialDist = mm.length(springBody1Pos - springBody2Pos)*1. #initialDist -= node.width#0.084 #v1 = (-node.width*0.5,0.0,-node.length*0.4) #v2 = (node.width*0.5,0.0,-node.length*0.4) ##controlModel.setSpring(motion[0].skeleton.getJointIndex(config['FootLPart'][springBody1]), motion[0].skeleton.getJointIndex(config['FootLPart'][springBody2]), elasticity, damping, v2, v1, initialDist) ##controlModel.setSpring(motion[0].skeleton.getJointIndex(config['FootRPart'][springBody1]), motion[0].skeleton.getJointIndex(config['FootRPart'][springBody2]), elasticity, damping, v1, v2, initialDist) vpWorld.initialize() controlModel.initializeHybridDynamics() #ModelOffset = (1.5, -0.01, 0) ModelOffset = (1.5, 0.4, 0) controlModel.translateByOffset(ModelOffset) totalDOF = controlModel.getTotalDOF() DOFs = controlModel.getDOFs() # parameter Kt = config['Kt'] Dt = config['Dt'] # tracking gain Kl = config['Kl'] Dl = config['Dl'] # linear balance gain Kh = config['Kh'] Dh = config['Dh'] # angular balance gain Ks = config['Ks'] Ds = config['Ds'] # penalty force spring gain Bt = config['Bt'] Bl = config['Bl'] Bh = config['Bh'] w = mot.getTrackingWeight(DOFs, motion[0].skeleton, config['weightMap']) w2 = mot.getTrackingWeight(DOFs, motion[0].skeleton, config['weightMap2']) #w_IK = mot.getTrackingWeight(DOFs, motion[0].skeleton, config['IKweightMap']) supL = motion[0].skeleton.getJointIndex(config['supLink']) supR = motion[0].skeleton.getJointIndex(config['supLink2']) rootB = motion[0].skeleton.getJointIndex(config['root']) selectedBody = motion[0].skeleton.getJointIndex(config['end']) #constBody = motion[0].skeleton.getJointIndex('LeftForeArm') constBody = motion[0].skeleton.getJointIndex(config['const']) # jacobian Jsup = yjc.makeEmptyJacobian(DOFs, 1) dJsup = Jsup.copy() JsupPre = Jsup.copy() Jsys = yjc.makeEmptyJacobian(DOFs, controlModel.getBodyNum()) dJsys = Jsys.copy() JsysPre = Jsys.copy() Jconst = yjc.makeEmptyJacobian(DOFs, 1) dJconst = Jconst.copy() Jcom = yjc.makeEmptyJacobian(DOFs, 1, False) dJcom = Jcom.copy() JcomAng = yjc.makeEmptyJacobian(DOFs, 1, False) dJcomAng = JcomAng.copy() ############### indexFootL = [None] * footPartNum indexFootR = [None] * footPartNum jFootL = [None] * footPartNum dJFootL = [None] * footPartNum jFootR = [None] * footPartNum dJFootR = [None] * footPartNum jointMasksFootL = [None] * footPartNum jointMasksFootR = [None] * footPartNum for i in range(footPartNum): jFootL[i] = yjc.makeEmptyJacobian(DOFs, 1) dJFootL[i] = jFootL[i].copy() jFootR[i] = yjc.makeEmptyJacobian(DOFs, 1) dJFootR[i] = jFootR[i].copy() indexFootL[i] = motion[0].skeleton.getJointIndex( config['FootLPart'][i]) indexFootR[i] = motion[0].skeleton.getJointIndex( config['FootRPart'][i]) jointMasksFootL[i] = [ yjc.getLinkJointMask(motion[0].skeleton, indexFootL[i]) ] jointMasksFootR[i] = [ yjc.getLinkJointMask(motion[0].skeleton, indexFootR[i]) ] constJointMasks = [ yjc.getLinksJointMask(motion[0].skeleton, [indexFootL[0], indexFootR[0]]) ] #constJointMasks = [yjc.getLinksJointMask(motion[0].skeleton, [indexFootL[0]])] #constJointMasks = [yjc.getLinkJointMask(motion[0].skeleton, constBody)] allLinkJointMasks = yjc.getAllLinkJointMasks(motion[0].skeleton) #comLowerJointMasks = [yjc.getLinksJointMask(motion[0].skeleton, [motion[0].skeleton.getJointIndex('LeftLeg'), motion[0].skeleton.getJointIndex('RightLeg')])] comUpperJointMasks = [ yjc.getLinkJointMask(motion[0].skeleton, selectedBody) ] #comLowerJointMasks = [yjc.getLinksJointMask(motion[0].skeleton, [motion[0].skeleton.getJointIndex('LeftLeg'), motion[0].skeleton.getJointIndex('RightLeg')])] comUpperJointMasks[0][0] = 0 #comUpperJointMasks[0][1] = 1 #comUpperJointMasks[0][10] = 1 comUpperJointMasks[0][2] = 1 comUpperJointMasks[0][11] = 1 #print(comUpperJointMasks) comLowerJointMasks = [ yjc.getLinksJointMask(motion[0].skeleton, [ motion[0].skeleton.getJointIndex('LeftLeg'), motion[0].skeleton.getJointIndex('RightLeg') ]) ] ''' maskArray = [foreSupLJointMasks, foreSupRJointMasks, rearSupLJointMasks, rearSupRJointMasks] parentArray = [supL, supR, supL, supR] effectorArray = [foreSupL, foreSupR, rearSupL, rearSupR] for j in range(4) : for i in range(len(foreSupLJointMasks)) : if i == parentArray[j] or i == effectorArray[j] : maskArray[j][0][i] = 1 else : maskArray[j][0][i] = 0 ''' # momentum matrix linkMasses = controlModel.getBodyMasses() totalMass = controlModel.getTotalMass() TO = ymt.make_TO(linkMasses) dTO = ymt.make_dTO(len(linkMasses)) # optimization problem = yac.LSE(totalDOF, 6) a_sup = (0, 0, 0, 0, 0, 0) #L #a_sup2 = (0,0,0, 0,0,0)#R a_sup2 = [0, 0, 0, 0, 0, 0] #R a_sup_2 = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] CP_old = [mm.v3(0., 0., 0.)] # penalty method bodyIDsToCheck = range(vpWorld.getBodyNum()) mus = [1.] * len(bodyIDsToCheck) # flat data structure ddth_des_flat = ype.makeFlatList(totalDOF) dth_flat = ype.makeFlatList(totalDOF) ddth_sol = ype.makeNestedList(DOFs) d_th_IK = ype.makeNestedList(DOFs) d_th_IK_L = ype.makeNestedList(DOFs) d_th_IK_R = ype.makeNestedList(DOFs) dd_th_IK = ype.makeNestedList(DOFs) dd_th_IK_flat = ype.makeFlatList(totalDOF) d_th_IK_flat = ype.makeFlatList(totalDOF) ddth_c_flat = ype.makeFlatList(totalDOF) # viewer rd_footCenter = [None] rd_footCenter_ref = [None] rd_footCenterL = [None] rd_footCenterR = [None] rd_CM_plane = [None] rd_CM_plane_ref = [None] rd_CM_ref = [None] rd_CM_des = [None] rd_CM = [None] rd_CM_vec = [None] rd_CM_ref_vec = [None] rd_CP = [None] rd_CP_des = [None] rd_dL_des_plane = [None] rd_dH_des = [None] rd_grf_des = [None] rd_footCenter_des = [None] rd_exf_des = [None] rd_root_des = [None] rd_soft_const_vec = [None] rd_root = [None] rd_footL_vec = [None] rd_footR_vec = [None] rd_CMP = [None] rd_DesPosL = [None] rd_DesPosR = [None] rd_DesForePosL = [None] rd_DesForePosR = [None] rd_DesRearPosL = [None] rd_DesRearPosR = [None] rd_Joint = [None] rd_Joint2 = [None] rd_Joint3 = [None] rd_Joint4 = [None] rd_desPoints = [None] #rd_contactForces = [None]*10000 #rd_contactPositions = [None]*10000 rd_virtualForce = [None] rootPos = [None] selectedBodyId = [selectedBody] extraForce = [None] applyedExtraForce = [None] applyedExtraForce[0] = [0, 0, 0] normalVector = [[0, 2, 0]] if MULTI_VIEWER: viewer = ymv.MultiViewer(800, 655) #viewer = ymv.MultiViewer(1600, 1255) viewer.setRenderers1([ cvr.VpModelRenderer(motionModel, CHARACTER_COLOR, yr.POLYGON_FILL) ]) viewer.setRenderers2([ cvr.VpModelRenderer(controlModel, CHARACTER_COLOR, yr.POLYGON_FILL) ]) else: viewer = ysv.SimpleViewer() # viewer.record(False) # viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion, (0,255,255), yr.LINK_BONE)) viewer.doc.addObject('motion', motion) viewer.doc.addRenderer( 'motionModel', cvr.VpModelRenderer(motionModel, (100, 100, 100), yr.POLYGON_FILL)) #(150,150,255) viewer.doc.addRenderer( 'controlModel', cvr.VpModelRenderer(controlModel, CHARACTER_COLOR, yr.POLYGON_FILL)) #viewer.doc.addRenderer('controlModel', cvr.VpModelRenderer(controlModel, CHARACTER_COLOR, yr.POLYGON_LINE)) #viewer.doc.addRenderer('rd_footCenter', yr.PointsRenderer(rd_footCenter)) #viewer.doc.addRenderer('rd_footCenter_des', yr.PointsRenderer(rd_footCenter_des, (150,0,150)) ) #viewer.doc.addRenderer('rd_footCenterL', yr.PointsRenderer(rd_footCenterL)) #viewer.doc.addRenderer('rd_footCenterR', yr.PointsRenderer(rd_footCenterR)) viewer.doc.addRenderer('rd_CM_plane', yr.PointsRenderer(rd_CM_plane, (255, 255, 0))) viewer.doc.addRenderer('rd_CM', yr.PointsRenderer(rd_CM, (255, 0, 255))) viewer.doc.addRenderer('rd_CM_des', yr.PointsRenderer(rd_CM_des, (64, 64, 255))) viewer.doc.addRenderer( 'rd_CM_vec', yr.VectorsRenderer(rd_CM_vec, rd_CM_plane, (255, 0, 0), 3)) #viewer.doc.addRenderer('rd_CP_des', yr.PointsRenderer(rd_CP_des, (0,255,0))) viewer.doc.addRenderer('rd_CP_des', yr.PointsRenderer(rd_CP_des, (255, 0, 128))) # viewer.doc.addRenderer('rd_dL_des_plane', yr.VectorsRenderer(rd_dL_des_plane, rd_CM, (255,255,0))) # viewer.doc.addRenderer('rd_dH_des', yr.VectorsRenderer(rd_dH_des, rd_CM, (0,255,0))) #viewer.doc.addRenderer('rd_grf_des', yr.ForcesRenderer(rd_grf_des, rd_CP, (0,255,255), .001)) viewer.doc.addRenderer( 'rd_exf_des', yr.ForcesRenderer(rd_exf_des, rd_root_des, (0, 255, 0), .009, 0.04)) #viewer.doc.addRenderer('rd_CMP', yr.PointsRenderer(rd_CMP, (0,0,255))) #viewer.doc.addRenderer('rd_DesPosL', yr.PointsRenderer(rd_DesPosL, (0,0,255))) #viewer.doc.addRenderer('rd_DesPosR', yr.PointsRenderer(rd_DesPosR, (0,100,255))) #viewer.doc.addRenderer('rd_DesForePosL', yr.PointsRenderer(rd_DesForePosL, (150,0,200))) #viewer.doc.addRenderer('rd_DesForePosR', yr.PointsRenderer(rd_DesForePosR, (150,0,250))) #viewer.doc.addRenderer('rd_DesRearPosL', yr.PointsRenderer(rd_DesRearPosL, (0,150,200))) #viewer.doc.addRenderer('rd_DesRearPosR', yr.PointsRenderer(rd_DesRearPosR, (0,150,250))) #viewer.doc.addRenderer('softConstraint', yr.VectorsRenderer(rd_soft_const_vec, rd_CMP, (150,100,100), 3)) #viewer.doc.addRenderer('rd_footLVec', yr.VectorsRenderer(rd_footL_vec, rd_footCenterL, (255,0,0), 3)) #viewer.doc.addRenderer('rd_footRVec', yr.VectorsRenderer(rd_footR_vec, rd_footCenterR, (255,255,0), 3)) #viewer.doc.addRenderer('rd_footCenter_ref', yr.PointsRenderer(rd_footCenter_ref)) #viewer.doc.addRenderer('rd_CM_plane_ref', yr.PointsRenderer(rd_CM_plane_ref, (255,255,0))) #viewer.doc.addRenderer('rd_refNormalVec', yr.VectorsRenderer(normalVector, rd_footCenter_ref, (255,0,0), 3)) #viewer.doc.addRenderer('rd_refCMVec', yr.VectorsRenderer(rd_CM_ref_vec, rd_footCenter_ref, (255,0,255), 3)) #viewer.doc.addRenderer('rd_curNormalVec', yr.VectorsRenderer(normalVector, rd_footCenter, (255,0,0), 3)) #viewer.doc.addRenderer('rd_CMVec', yr.VectorsRenderer(rd_CM_vec, rd_footCenter, (255,0,255), 3)) #viewer.doc.addRenderer('rd_contactForces', yr.ForcesRenderer(rd_contactForces, rd_contactPositions, (0,255,0), .009, 0.009)) #viewer.doc.addRenderer('rd_virtualForce', yr.ForcesRenderer(rd_virtualForce, rd_CM, (50,255,0), 0.5, 0.02)) #viewer.doc.addRenderer('rd_Joint', yr.PointsRenderer(rd_Joint, (255,0,0))) #viewer.doc.addRenderer('rd_Joint2', yr.PointsRenderer(rd_Joint2, (0,255,0))) #viewer.doc.addRenderer('rd_Joint3', yr.PointsRenderer(rd_Joint3, (0,0,255))) #viewer.doc.addRenderer('rd_Joint4', yr.PointsRenderer(rd_Joint4, (255,255,0))) viewer.doc.addRenderer('rd_desPoints', yr.PointsRenderer(rd_desPoints, (255, 0, 0))) stage = STATIC_BALANCING contactRendererName = [] for i in range(motion[0].skeleton.getJointNum()): print(i, motion[0].skeleton.getJointName(i)) desCOMOffset = 0.0 pt = [0.] timeReport = [0.] * 7 viewer.objectInfoWnd.comOffsetY.value(-0.05) viewer.objectInfoWnd.comOffsetZ.value(0.00) viewer.objectInfoWnd.begin() viewer.objectInfoWnd.Bc = Fl_Value_Input(100, 450, 40, 10, 'Bc') viewer.objectInfoWnd.Bc.value(0.1) viewer.objectInfoWnd.end() viewer.objectInfoWnd.labelKt.value(50) viewer.objectInfoWnd.labelKk.value(17) 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) # tracking th_r_ori = motion.getDOFPositions(frame) th_r = copy.copy(th_r_ori) ############################ #Reference motion modulation dCM_k = 10. linkVelocities = controlModel.getBodyVelocitiesGlobal() dCM = yrp.getCM(linkVelocities, linkMasses, totalMass) dCM_plane = copy.copy(dCM) dCM_plane[1] = 0. global leftHipTimer if viewer.objectInfoWnd.onLeftHip: leftHipTimer = 60 viewer.objectInfoWnd.onLeftHip = False if leftHipTimer > 0: viewer.objectInfoWnd.comOffsetX.value( 0.08 * 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) if CP != None: CP[1] = 0. for i in range(len(bodyIDsToCheck)): controlModel.SetBodyColor(bodyIDsToCheck[i], 0, 0, 0, 255) contactFlagFootL = [0] * footPartNum contactFlagFootR = [0] * footPartNum 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): 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] #dL_des_plane[1] = 0. #print 'dL_des_plane', dL_des_plane # angular momentum CP_ref = footCenter + footCenterOffset CP_ref[1] = 0. timeStep = 30. if CP_old[0] == None or CP == None: dCP = None else: dCP = (CP - CP_old[0]) * 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) #print 'dCP: ', dCP #print 'ddCP_des: ', ddCP_des #print 'CP_des: ', CP_des #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 ''' #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] elif mit.MOTION == mit.TIPTOE: frame_index = [1000000, 1000000] #frame_index = [10000, 165] else: frame_index = [1000000, 1000000] #MOTION = TAEKWONDO #frame_index = [135, 100] 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 0: #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) # using || dH ||^2 instead mot.addAnotherTerms(problem, totalDOF, Bh, S, -(s_bias + Kh * np.dot(S, dth_flat))) a_sup_2 = None Jsup_2 = None dJsup_2 = None ############################## # Hard constraint Kk2 = Kk * 4.0 Dk2 = 2 * (Kk2**.5) ankleW = 0 ankleOffset = ankleW * curRelCMVec[2] metatarW = 0 metatarOffset = metatarW * curRelCMVec[2] ############################## ############################## # Additional constraint if stage != MOTION_TRACKING and frame > 5: # ankle strategy idx = 0 #LEFT/RIGHT_TOES if mit.FOOT_PART_NUM == 1: yOffset = 0.03 else: yOffset = 0.069 #yOffset = 0.06 # ankleOffset = (footCenter - CM_plane)*4. ankleOffset = footCenterOffset * 10. ankleOffset[1] = 0. #ankleOffset[2] = 0. ankleOffset[2] = ankleOffset[2] * 20. ankleOffsetL = ankleOffset.copy() ankleOffsetR = ankleOffset.copy() #ankleOffset= np.array((0,0,0)) if footCenterOffset[0] > 0.0: ankleOffsetL[0] = 0. else: ankleOffsetR[0] = 0. # print 'ankleOffset=', ankleOffset desLinearAccL, desPosL = getDesFootLinearAcc( motionModel, controlModel, indexFootL[idx], ModelOffset, CM_ref, CM, Kk, Dk, yOffset) #0.076) #0.14) desLinearAccR, desPosR = getDesFootLinearAcc( motionModel, controlModel, indexFootR[idx], ModelOffset, CM_ref, CM, Kk, Dk, yOffset) ax = [0, 0, -1] aaa = getBodyGlobalOri(controlModel, motion, 'RightFoot') #print np.dot(aaa, ax) if mit.FOOT_PART_NUM == 1: ax = [0, 1, 0] desAngularAccL = getDesFootAngularAcc( motionModel, controlModel, indexFootL[idx], Kk, Dk, ax, mm.normalize([0, 1, 0] + ankleOffsetL)) desAngularAccR = getDesFootAngularAcc( motionModel, controlModel, indexFootR[idx], Kk, Dk, ax, mm.normalize([0, 1, 0] + ankleOffsetR)) 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])) #mot.addConstraint(problem, totalDOF, Jsup_2, dJsup_2, dth_flat, a_sup_2) #mot.addConstraint(problem, totalDOF, Jsup_2[:1], dJsup_2[:1], dth_flat, a_sup_2[:1]) #mot.addConstraint(problem, totalDOF, Jsup_2[2:], dJsup_2[2:], dth_flat, a_sup_2[2:]) #mot.addConstraint(problem, totalDOF, Jsup_2[3:], dJsup_2[3:], dth_flat, a_sup_2[3:]) mot.addAnotherTerms(problem, totalDOF, viewer.objectInfoWnd.Bc.value(), Jsup_2[3:], a_sup_2[3:] - np.dot(dJsup_2[3:], dth_flat)) #mot.addAnotherTerms(problem, totalDOF, viewer.objectInfoWnd.Bc.value(), Jsup_2, a_sup_2 - np.dot(dJsup_2, dth_flat)) #mot.addAnotherTerms(problem, totalDOF, 1.*viewer.objectInfoWnd.Bc.value(), Jsup_2[0:1], a_sup_2[0:1] - np.dot(dJsup_2[0:1] , dth_flat)) #mot.addAnotherTerms(problem, totalDOF, 1.*viewer.objectInfoWnd.Bc.value(), Jsup_2[2:], a_sup_2[2:] - np.dot(dJsup_2[2:] , dth_flat)) desCOMOffset = 0.0 rd_DesPosL[0] = desPosL.copy() rd_DesPosR[0] = desPosR.copy() if stage == STATIC_BALANCING and frame > 10: # and False: del rd_desPoints[:] # foot strategy #Kk2 = Kk * 2.5 #Kk2 = Kk * .2 #Dk2 = 2*(Kk2**.5) desForePosL = [0, 0, 0] desForePosR = [0, 0, 0] desRearPosL = [0, 0, 0] desRearPosR = [0, 0, 0] footPartPos = [] footPartPos.append( controlModel.getBodyPositionGlobal( motion[0].skeleton.getJointIndex('LeftCalcaneus_1'))) footPartPos.append( controlModel.getBodyPositionGlobal( motion[0].skeleton.getJointIndex('LeftPhalange_1'))) footPartPos.append( controlModel.getBodyPositionGlobal( motion[0].skeleton.getJointIndex('RightCalcaneus_1'))) footPartPos.append( controlModel.getBodyPositionGlobal( motion[0].skeleton.getJointIndex('RightPhalange_1'))) for i in range(1, footPartNum): contactFlagFootL[i] = 1 contactFlagFootR[i] = 1 SupPts = np.vstack( (np.array((footPartPos[0][0], footPartPos[1][0], footPartPos[2][0], footPartPos[3][0])), np.array( (footPartPos[0][2], footPartPos[1][2], footPartPos[2][2], footPartPos[3][2])), np.array((1., 1., 1., 1.)))) coordWidthLen = 2. coordLengthLen = 1.5 SupUV = np.vstack( (np.array((-coordWidthLen, -coordWidthLen, coordWidthLen, coordWidthLen)), np.array((-coordLengthLen, coordLengthLen, -coordLengthLen, coordLengthLen)), np.array((1., 1., 1., 1.)))) SupMap = np.dot(np.dot(SupUV, SupUV.T), np.linalg.inv(np.dot(SupPts, SupUV.T))) #print SupMap desFootCenter = footCenter + footCenterOffset footCenterPts = np.array((desFootCenter[0], desFootCenter[2], 1)) #print np.dot(SupMap, footCenterPts) #print np.dot(getBodyGlobalOri(controlModel, motion, 'LeftMetatarsal_1'), np.array((0,1,0))) CM_plane_2D = np.array((CM[0], CM[2], 1)) # CM_plane_UV = np.dot(SupMap, CM_plane_2D) CM_plane_UV = np.dot(SupMap, footCenterPts) # print CM_plane_UV # for i in range(1, footPartNum): if CM_plane_UV[1] > .5: # com is in front for i in range(1, 5): contactFlagFootL[i] = 0 contactFlagFootR[i] = 0 elif CM_plane_UV[1] < -.5: # com is back for i in range(3, footPartNum): contactFlagFootL[i] = 0 contactFlagFootR[i] = 0 else: # com is in middle position for i in range(3, 5): contactFlagFootL[i] = 0 contactFlagFootR[i] = 0 contactFlagFoot = contactFlagFootL if CM_plane_UV[0] < 0.: contactFlagFoot = contactFlagFootR # CM_plane_UV[0] = -CM_plane_UV[0] if abs(CM_plane_UV[0]) > 1.: for j in range(0, 3): contactFlagFoot[2 * j + 2] = 0 # print 'footL : ',contactFlagFootL # print 'footR : ',contactFlagFootR for i in range(1, footPartNum): axis = [0, 0, 1] if i == 1 or i == 2: axis = [0, 0, -1] desAng = [0, 0, 1] if i == 1 or i == 2: desAng = [0, 0, -1] desY = 0.029 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((desLinearAccL, desAngularAccL)) Jsup_2 = jFootL[i].copy() dJsup_2 = dJFootL[i].copy() mot.addConstraint(problem, totalDOF, Jsup_2, dJsup_2, dth_flat, a_sup_2) #mot.addAnotherTerms(problem, totalDOF, viewer.objectInfoWnd.Bc.value(), Jsup_2, a_sup_2 - np.dot(dJsup_2, dth_flat)) #mot.addAnotherTerms(problem, totalDOF, viewer.objectInfoWnd.Bc.value(), Jsup_2[3:], a_sup_2[3:] - np.dot(dJsup_2[3:] , dth_flat)) rd_desPoints.append(desForePosL.copy()) 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((desLinearAccR, desAngularAccR)) Jsup_2 = jFootR[i].copy() dJsup_2 = dJFootR[i].copy() mot.addConstraint(problem, totalDOF, Jsup_2, dJsup_2, dth_flat, a_sup_2) #mot.addAnotherTerms(problem, totalDOF, viewer.objectInfoWnd.Bc.value(), Jsup_2, a_sup_2 - np.dot(dJsup_2, dth_flat)) #mot.addAnotherTerms(problem, totalDOF, viewer.objectInfoWnd.Bc.value(), Jsup_2[3:], a_sup_2[3:] - np.dot(dJsup_2[3:], dth_flat)) rd_desPoints.append(desForePosR.copy()) rd_DesForePosL[0] = desForePosL rd_DesForePosR[0] = desForePosR rd_DesRearPosL[0] = desRearPosL rd_DesRearPosR[0] = desRearPosR ############################## #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() #print np.NAN r = problem.solve() #print frame #Ashape = np.shape(problem.A) #if len(Ashape) >0 : # for i in range(0, Ashape[0]): # print problem.A[i] #print problem.A[] #print problem.b #print r problem.clear() #print r['x'] ype.nested(r['x'], ddth_sol) #print ddth_sol rootPos[0] = controlModel.getBodyPositionGlobal(selectedBody) localPos = [[0, 0, 0]] ########################################### ##Jacobian Transpose control # COM Position control #fCom = Wcp*(pHatComDes - pHatCom) + Wcv*(vComDes - vCom) + Wcm*(footCenter_plane - CM_plane) w1 = 10 #10.1 w2 = 1 #1#2*(w1**.5) if frame > 100: w1 = 10.1 #10.1 w2 = 1 footToCMVec = CM - footCenter desCMPos = [footCenter[0], mm.length(footToCMVec), footCenter[2]] #print("desCMPos", desCMPos) #print("CM", CM) fCom = w1 * (desCMPos - CM) + w2 * (-dCM) #print("fCom", fCom) #fCom[0] = 0. #fCom[1] = 0 #fCom[2] = 0 rd_virtualForce[0] = fCom.copy() #hipPos = controlModel.getBodyPositionGlobal(rootB) headPos = controlModel.getBodyPositionGlobal(selectedBody) hipPos = controlModel.getBodyPositionGlobal(rootB) yjc.computeJacobian2(Jcom, DOFs, jointPositions, jointAxeses, [headPos], comUpperJointMasks) #yjc.computeJacobianDerivative2(dJcom, DOFs, jointPositions, jointAxeses, linkAngVelocities, [CM], comUpperJointMasks, False) JcomT = Jcom.T TauJT = np.dot(JcomT, fCom) # Angular Momentum Hc = ymt.getAngularMomentum(CM, linkInertias, linkAngVelocities, linkPositions, linkMasses, linkVelocities) Href = ymt.getAngularMomentum(CM_ref, linkInertias_ref, linkAngVelocities_ref, linkPositions_ref, linkMasses, linkVelocities_ref) Wam = .05 Tam = Wam * (Href - Hc) #print("Tam", Tam) yjc.computeAngJacobian2(JcomAng, DOFs, jointPositions, jointAxeses, [headPos], comUpperJointMasks) TauAM = np.dot(JcomAng.T, Tam) 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) #print frame, bodyIDs, contactPositions, contactPositionLocals, contactForces 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 #print ddth_sol controlModel.setDOFAccelerations(ddth_sol) controlModel.solveHybridDynamics() ''' if (frame > 5): tau = controlModel.getJointTorqueLocal(indexFootL[3]) tau2 = controlModel.getJointTorqueLocal(indexFootL[4]) tau3 = controlModel.getJointTorqueLocal(indexFootR[3]) tau4 = controlModel.getJointTorqueLocal(indexFootR[4]) torques = controlModel.getInternalJointTorquesLocal() if (frame > 100 and frame < 110) or (frame > 165 and frame < 190): Wcal1 = 0.05 Wcal2 = 0.05 dC = fCom[2]*Wcal1-dCM[2]*Wcal2 print("dC", dC) torques[indexFootL[5]-1]+= (dC, 0.0, 0.0) torques[indexFootR[5]-1]+= (dC, 0.0, 0.0) if (frame > 50 and frame < 75) or (frame > 110 and frame <140) or (frame > 185 and frame < 220): metatarR = [controlModel.getBodyOrientationGlobal(indexFootL[1])] phalangeR = [controlModel.getBodyOrientationGlobal(indexFootL[3])] metatarR2 = np.dot(metatarR, np.array([0,0,1])) phalangeR2 = np.dot(phalangeR, np.array([0,0,1])) metatarRZ = mm.normalize(metatarR2[0]) phalangeRZ = mm.normalize(phalangeR2[0]) lean = np.dot(metatarRZ, phalangeRZ) Wlean = 2 dt = -0.02*(lean)*Wlean torques[indexFootL[3]-1]+= (dt, 0.0, 0.0) torques[indexFootL[4]-1]+= (dt, 0.0, 0.0) torques[indexFootR[3]-1]+= (dt, 0.0, 0.0) torques[indexFootR[4]-1]+= (dt, 0.0, 0.0) i = 0 t = 6 while t < len(TauJT) : torques[i] += (TauJT[t]+TauAM[t], TauJT[t+1]+TauAM[t+1], TauJT[t+2]+TauAM[t+2]) i+=1 t+=3 #totalTorques = [a + b for a, b in zip(torques, TauJT)] #print("torques2", torques) #print("TauJT", TauJT[16], TauJT[17], TauJT[18]) #print("torques", torques[16]) #print("totalTorques", totalTorques[16]) controlModel.setInternalJointTorquesLocal(torques) ''' ''' 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() 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 # print timeReport viewer.setSimulateCallback(simulateCallback) viewer.startTimer(1 / 30.) viewer.show() Fl.run()
def main(): # np.set_printoptions(precision=4, linewidth=200) np.set_printoptions(precision=5, threshold=np.inf, suppress=True, linewidth=3000) motionFile = 'wd2_tiptoe.bvh' motionFile = 'wd2_tiptoe_zygote.bvh' # motion, mcfg, wcfg, stepsPerFrame, config, frame_rate = mit.create_biped(motionFile, SEGMENT_FOOT_RAD=0.008) motion, mcfg, wcfg, stepsPerFrame, config, frame_rate = mit.create_biped( motionFile, SEGMENT_FOOT_MAG=0.01, SEGMENT_FOOT_RAD=0.008) # motion, mcfg, wcfg, stepsPerFrame, config, frame_rate = mit.create_biped() # motion, mcfg, wcfg, stepsPerFrame, config = mit.create_jump_biped() vpWorld = cvw.VpWorld(wcfg) vpWorld.SetGlobalDamping(0.999) motionModel = cvm.VpMotionModel(vpWorld, motion[0], mcfg) controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg) # controlModel_shadow_for_ik = cvm.VpControlModel(vpWorld, motion[0], mcfg) vpWorld.initialize() controlModel.initializeHybridDynamics() # controlToMotionOffset = (1.5, -0.02, 0) controlToMotionOffset = (0., 0., 0) controlModel.translateByOffset(controlToMotionOffset) # controlModel_shadow_for_ik.set_q(controlModel.get_q()) # controlModel_shadow_for_ik.computeJacobian(0, np.array([0., 0., 0.])) wcfg_ik = copy.deepcopy(wcfg) vpWorld_ik = cvw.VpWorld(wcfg_ik) controlModel_ik = cvm.VpControlModel(vpWorld_ik, motion[0], mcfg) vpWorld_ik.initialize() controlModel_ik.set_q(np.zeros_like(controlModel.get_q())) controlModel_q = np.zeros_like(controlModel.get_q()) controlModel_q[4] = controlModel_q[4] + 1.2 - 0.24 controlModel.set_q(controlModel_q) print( controlModel.getBodyPositionGlobal( motion[0].skeleton.getJointIndex('RightFoot_foot_1_0'))) totalDOF = controlModel.getTotalDOF() DOFs = controlModel.getDOFs() foot_dofs = [] left_foot_dofs = [] right_foot_dofs = [] foot_seg_dofs = [] left_foot_seg_dofs = [] right_foot_seg_dofs = [] # for joint_idx in range(motion[0].skeleton.getJointNum()): for joint_idx in range(controlModel.getJointNum()): joint_name = controlModel.index2name(joint_idx) # joint_name = motion[0].skeleton.getJointName(joint_idx) if 'Foot' in joint_name: foot_dofs_temp = controlModel.getJointDOFIndexes(joint_idx) foot_dofs.extend(foot_dofs_temp) if 'Left' in joint_name: left_foot_dofs.extend(foot_dofs_temp) elif 'Right' in joint_name: right_foot_dofs.extend(foot_dofs_temp) if 'foot' in joint_name: foot_dofs_temp = controlModel.getJointDOFIndexes(joint_idx) foot_seg_dofs.extend(foot_dofs_temp) if 'Left' in joint_name: left_foot_seg_dofs.extend(foot_dofs_temp) elif 'Right' in joint_name: right_foot_seg_dofs.extend(foot_dofs_temp) # parameter Kt = config['Kt'] Dt = config['Dt'] # tracking gain Kl = config['Kl'] Dl = config['Dl'] # linear balance gain Kh = config['Kh'] Dh = config['Dh'] # angular balance gain Ks = config['Ks'] Ds = config['Ds'] # penalty force spring gain Bt = config['Bt'] Bl = config['Bl'] Bh = config['Bh'] selectedBody = motion[0].skeleton.getJointIndex(config['end']) constBody = motion[0].skeleton.getJointIndex('RightFoot') supL = motion[0].skeleton.getJointIndex('LeftFoot') supR = motion[0].skeleton.getJointIndex('RightFoot') # momentum matrix linkMasses = controlModel.getBodyMasses() totalMass = controlModel.getTotalMass() TO = ymt.make_TO(linkMasses) dTO = ymt.make_dTO(len(linkMasses)) # optimization problem = yac.LSE(totalDOF, 12) # a_sup = (0,0,0, 0,0,0) #ori # a_sup = (0,0,0, 0,0,0) #L CP_old = [mm.v3(0., 0., 0.)] # penalty method bodyIDsToCheck = list(range(vpWorld.getBodyNum())) # mus = [1.]*len(bodyIDsToCheck) mus = [.5] * len(bodyIDsToCheck) # flat data structure ddth_des_flat = ype.makeFlatList(totalDOF) dth_flat = ype.makeFlatList(totalDOF) ddth_sol = ype.makeNestedList(DOFs) # viewer rd_footCenter = [None] rd_footCenter_ref = [None] rd_footCenterL = [None] rd_footCenterR = [None] rd_CM_plane = [None] rd_CM = [None] rd_CP = [None] rd_CP_des = [None] rd_dL_des_plane = [None] rd_dH_des = [None] rd_grf_des = [None] rd_exf_des = [None] rd_exfen_des = [None] rd_root_des = [None] rd_foot_ori = [None] rd_foot_pos = [None] rd_root_ori = [None] rd_root_pos = [None] rd_CF = [None] rd_CF_pos = [None] rootPos = [None] selectedBodyId = [selectedBody] extraForce = [None] extraForcePos = [None] rightFootVectorX = [None] rightFootVectorY = [None] rightFootVectorZ = [None] rightFootPos = [None] rightVectorX = [None] rightVectorY = [None] rightVectorZ = [None] rightPos = [None] def makeEmptyBasicSkeletonTransformDict(init=None): Ts = dict() Ts['pelvis'] = init Ts['spine_ribs'] = init Ts['head'] = init Ts['thigh_R'] = init Ts['shin_R'] = init Ts['foot_heel_R'] = init Ts['foot_R'] = init Ts['heel_R'] = init Ts['outside_metatarsal_R'] = init Ts['outside_phalanges_R'] = init Ts['inside_metatarsal_R'] = init Ts['inside_phalanges_R'] = init Ts['upper_limb_R'] = init Ts['lower_limb_R'] = init Ts['thigh_L'] = init Ts['shin_L'] = init Ts['foot_heel_L'] = init Ts['foot_L'] = init Ts['heel_L'] = init Ts['outside_metatarsal_L'] = init Ts['outside_phalanges_L'] = init Ts['inside_metatarsal_L'] = init Ts['inside_phalanges_L'] = init Ts['upper_limb_L'] = init Ts['lower_limb_L'] = init return Ts # viewer = ysv.SimpleViewer() # viewer = hsv.hpSimpleViewer(rect=[0, 0, 1024, 768], viewForceWnd=False) viewer = hsv.hpSimpleViewer(rect=[0, 0, 1920 + 300, 1 + 1080 + 55], viewForceWnd=False) # viewer.record(False) # viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion, (0,255,255), yr.LINK_BONE)) viewer.doc.addObject('motion', motion) motion_model_renderer = yr.VpModelRenderer(motionModel, (255, 240, 255), yr.POLYGON_FILL) viewer.doc.addRenderer('motionModel', motion_model_renderer) # viewer.doc.setRendererVisible('motionModel', False) viewer.doc.addRenderer( 'ikModel', yr.VpModelRenderer(controlModel_ik, (150, 150, 255), yr.POLYGON_LINE)) viewer.doc.setRendererVisible('ikModel', False) # viewer.doc.addRenderer('controlModel', cvr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_LINE)) control_model_renderer = yr.VpModelRenderer(controlModel, (255, 240, 255), yr.POLYGON_FILL) viewer.doc.addRenderer('controlModel', control_model_renderer) viewer.doc.setRendererVisible('controlModel', False) skeleton_renderer = None if SKELETON_ON: # skeleton_renderer = yr.BasicSkeletonRenderer(makeEmptyBasicSkeletonTransformDict(np.eye(4)), offset_Y=-0.08) # skeleton_renderer = yr.BasicSkeletonRenderer(makeEmptyBasicSkeletonTransformDict(np.eye(4)), color=(230, 230, 230), offset_draw=(0.8, -0.02, 0.)) skeleton_renderer = yr.BasicSkeletonRenderer( makeEmptyBasicSkeletonTransformDict(np.eye(4)), color=(230, 230, 230), offset_draw=(0., -0.0, 0.)) viewer.doc.addRenderer('skeleton', skeleton_renderer) viewer.doc.setRendererVisible('skeleton', False) viewer.doc.addRenderer('rd_footCenter', yr.PointsRenderer(rd_footCenter)) viewer.doc.setRendererVisible('rd_footCenter', False) viewer.doc.addRenderer('rd_footCenter_ref', yr.PointsRenderer(rd_footCenter_ref)) viewer.doc.setRendererVisible('rd_footCenter_ref', False) viewer.doc.addRenderer('rd_CM_plane', yr.PointsRenderer(rd_CM_plane, (255, 255, 0))) viewer.doc.setRendererVisible('rd_CM_plane', False) viewer.doc.addRenderer('rd_CP', yr.PointsRenderer(rd_CP, (0, 255, 0))) viewer.doc.setRendererVisible('rd_CP', False) viewer.doc.addRenderer('rd_CP_des', yr.PointsRenderer(rd_CP_des, (255, 0, 255))) viewer.doc.setRendererVisible('rd_CP_des', False) viewer.doc.addRenderer( 'rd_dL_des_plane', yr.VectorsRenderer(rd_dL_des_plane, rd_CM, (255, 255, 0))) viewer.doc.setRendererVisible('rd_dL_des_plane', False) viewer.doc.addRenderer('rd_dH_des', yr.VectorsRenderer(rd_dH_des, rd_CM, (0, 255, 0))) viewer.doc.setRendererVisible('rd_dH_des', False) # viewer.doc.addRenderer('rd_grf_des', yr.ForcesRenderer(rd_grf_des, rd_CP_des, (0,255,0), .001)) viewer.doc.addRenderer('rd_CF', yr.VectorsRenderer(rd_CF, rd_CF_pos, (255, 255, 0))) viewer.doc.setRendererVisible('rd_CF', False) viewer.doc.addRenderer( 'rd_foot_ori', yr.OrientationsRenderer(rd_foot_ori, rd_foot_pos, (255, 255, 0))) viewer.doc.setRendererVisible('rd_foot_ori', False) viewer.doc.addRenderer( 'rd_root_ori', yr.OrientationsRenderer(rd_root_ori, rd_root_pos, (255, 255, 0))) viewer.doc.setRendererVisible('rd_root_ori', False) viewer.doc.addRenderer( 'extraForce', yr.VectorsRenderer(rd_exf_des, extraForcePos, (0, 255, 0))) viewer.doc.setRendererVisible('extraForce', False) viewer.doc.addRenderer( 'extraForceEnable', yr.VectorsRenderer(rd_exfen_des, extraForcePos, (255, 0, 0))) # viewer.doc.addRenderer('right_foot_oriX', yr.VectorsRenderer(rightFootVectorX, rightFootPos, (255,0,0))) # viewer.doc.addRenderer('right_foot_oriY', yr.VectorsRenderer(rightFootVectorY, rightFootPos, (0,255,0))) # viewer.doc.addRenderer('right_foot_oriZ', yr.VectorsRenderer(rightFootVectorZ, rightFootPos, (0,0,255))) # viewer.doc.addRenderer('right_oriX', yr.VectorsRenderer(rightVectorX, rightPos, (255,0,0))) # viewer.doc.addRenderer('right_oriY', yr.VectorsRenderer(rightVectorY, rightPos, (0,255,0))) # viewer.doc.addRenderer('right_oriZ', yr.VectorsRenderer(rightVectorZ, rightPos, (0,0,255))) # foot_viewer = FootWindow(viewer.x() + viewer.w() + 20, viewer.y(), 300, 400, 'foot contact modifier', controlModel) foot_viewer = None # type: FootWindow initKt = 25. # initKt = 60. initKl = 100. initKh = 100. initBl = .1 initBh = .13 # initSupKt = 17 initSupKt = 22 initFm = 50.0 initComX = 0. initComY = 0. initComZ = 0. viewer.objectInfoWnd.add1DSlider("Kt", 0., 300., 1., initKt) viewer.objectInfoWnd.add1DSlider("Kl", 0., 300., 1., initKl) viewer.objectInfoWnd.add1DSlider("Kh", 0., 300., 1., initKh) viewer.objectInfoWnd.add1DSlider("Bl", 0., 1., .001, initBl) viewer.objectInfoWnd.add1DSlider("Bh", 0., 1., .001, initBh) viewer.objectInfoWnd.add1DSlider("SupKt", 0., 300., 0.1, initSupKt) viewer.objectInfoWnd.add1DSlider("Fm", 0., 1000., 10., initFm) viewer.objectInfoWnd.add1DSlider("com X offset", -1., 1., 0.001, initComX) viewer.objectInfoWnd.add1DSlider("com Y offset", -1., 1., 0.001, initComY) viewer.objectInfoWnd.add1DSlider("com Z offset", -1., 1., 0.001, initComZ) viewer.objectInfoWnd.add1DSlider("tiptoe angle", -0.5, .5, 0.001, 0.) viewer.objectInfoWnd.add1DSlider("left tilt angle", -0.5, .5, 0.001, 0.) viewer.objectInfoWnd.add1DSlider("right tilt angle", -0.5, .5, 0.001, 0.) viewer.force_on = False def viewer_SetForceState(object): viewer.force_on = True def viewer_GetForceState(): return viewer.force_on def viewer_ResetForceState(): viewer.force_on = False viewer.objectInfoWnd.addBtn('Force on', viewer_SetForceState) viewer_ResetForceState() offset = 60 viewer.objectInfoWnd.begin() viewer.objectInfoWnd.labelForceX = Fl_Value_Input(20, 30 + offset * 9, 40, 20, 'X') viewer.objectInfoWnd.labelForceX.value(0) viewer.objectInfoWnd.labelForceY = Fl_Value_Input(80, 30 + offset * 9, 40, 20, 'Y') viewer.objectInfoWnd.labelForceY.value(0) viewer.objectInfoWnd.labelForceZ = Fl_Value_Input(140, 30 + offset * 9, 40, 20, 'Z') viewer.objectInfoWnd.labelForceZ.value(1) viewer.objectInfoWnd.labelForceDur = Fl_Value_Input( 220, 30 + offset * 9, 40, 20, 'Dur') viewer.objectInfoWnd.labelForceDur.value(0.1) viewer.objectInfoWnd.end() # self.sliderFm = Fl_Hor_Nice_Slider(10, 42+offset*6, 250, 10) def getParamVal(paramname): return viewer.objectInfoWnd.getVal(paramname) def getParamVals(paramnames): return (getParamVal(name) for name in paramnames) def setParamVal(paramname, val): viewer.objectInfoWnd.setVal(paramname, val) idDic = dict() for i in range(motion[0].skeleton.getJointNum()): idDic[motion[0].skeleton.getJointName(i)] = i # extendedFootName = ['Foot_foot_0_0', 'Foot_foot_0_1', 'Foot_foot_0_0_0', 'Foot_foot_0_1_0', 'Foot_foot_1_0'] extendedFootName = [ 'Foot_foot_0_0', 'Foot_foot_0_0_0', 'Foot_foot_0_1_0', 'Foot_foot_1_0' ] lIDdic = { 'Left' + name: motion[0].skeleton.getJointIndex('Left' + name) for name in extendedFootName } rIDdic = { 'Right' + name: motion[0].skeleton.getJointIndex('Right' + name) for name in extendedFootName } footIdDic = lIDdic.copy() footIdDic.update(rIDdic) lIDlist = [ motion[0].skeleton.getJointIndex('Left' + name) for name in extendedFootName ] rIDlist = [ motion[0].skeleton.getJointIndex('Right' + name) for name in extendedFootName ] footIdlist = [] footIdlist.extend(lIDlist) footIdlist.extend(rIDlist) foot_left_idx = motion[0].skeleton.getJointIndex('LeftFoot') foot_right_idx = motion[0].skeleton.getJointIndex('RightFoot') foot_left_idx_temp = motion[0].skeleton.getJointIndex('LeftFoot_foot_1_0') foot_right_idx_temp = motion[0].skeleton.getJointIndex( 'RightFoot_foot_1_0') # ik_solver = hik.numIkSolver(dartIkModel) # ik_solver.clear() # bodyIDsToCheck = rIDlist.copy() joint_names = [ motion[0].skeleton.getJointName(i) for i in range(motion[0].skeleton.getJointNum()) ] def fix_dofs(_DOFs, nested_dof_values, _mcfg, _joint_names): fixed_nested_dof_values = list() fixed_nested_dof_values.append(nested_dof_values[0]) for i in range(1, len(_DOFs)): dof = _DOFs[i] if dof == 1: node = _mcfg.getNode(_joint_names[i]) axis = mm.unitZ() if node.jointAxes[0] == 'X': axis = mm.unitX() elif node.jointAxes[0] == 'Y': axis = mm.unitY() fixed_nested_dof_values.append( np.array([np.dot(nested_dof_values[i], axis)])) else: fixed_nested_dof_values.append(nested_dof_values[i]) return fixed_nested_dof_values start_frame = 50 up_vec_in_each_link = dict() for foot_id in footIdlist: up_vec_in_each_link[ foot_id] = controlModel_ik.getBodyOrientationGlobal(foot_id)[1, :] controlModel_ik.set_q(controlModel.get_q()) ################################### # simulate ################################### def preFrameCallback_Always(frame): global COLOR_ON viewer.motionViewWnd.glWindow.camera.rotateX = math.pi / 180. * -25. viewer.motionViewWnd.glWindow.camera.rotateY = mm.deg2Rad(45.) viewer.motionViewWnd.glWindow.camera.distance = .5 viewer.motionViewWnd.glWindow.camera.center = np.array( [0.04347, -0.01005, -0.31936]) if frame < start_frame: setParamVal('com Y offset', -0.11) viewer.doc.setRendererVisible('motionModel', False) viewer.doc.setRendererVisible('skeleton', True) elif frame < start_frame + 20: viewer.doc.setRendererVisible('motionModel', True) elif frame < start_frame + 50: setParamVal('com X offset', (frame - start_frame - 20) * 0.003) elif frame < start_frame + 70: pass elif frame < start_frame + 90: COLOR_ON = True elif frame < start_frame + 105: setParamVal('com Y offset', -0.11 + (frame - start_frame - 90) * 0.0032) setParamVal('tiptoe angle', (frame - start_frame - 90) * 0.012) elif frame < start_frame + 120: pass elif frame < start_frame + 135: setParamVal('com Y offset', -0.065 - (frame - start_frame - 120) * 0.0032) setParamVal('tiptoe angle', 0.168 - (frame - start_frame - 120) * 0.012) elif frame < start_frame + 150: pass elif frame < start_frame + 165: setParamVal('com Y offset', -0.11 + (frame - start_frame - 150) * 0.0004) setParamVal('left tilt angle', -(frame - start_frame - 150) * 0.012) elif frame < start_frame + 180: pass elif frame < start_frame + 195: setParamVal('com Y offset', -0.104 - (frame - start_frame - 180) * 0.0004) setParamVal('left tilt angle', -0.168 + (frame - start_frame - 180) * 0.012) # print(viewer.motionViewWnd.glWindow.camera.rotateX/math.pi * 180.) # print(viewer.motionViewWnd.glWindow.camera.rotateY/math.pi * 180.) # print(viewer.motionViewWnd.glWindow.camera.distance) # print(viewer.motionViewWnd.glWindow.camera.center) # if frame > start_frame: # viewer.motionViewWnd.glWindow.camera.rotateY = mm.deg2Rad((frame-start_frame)*3+45.) # else: # viewer.motionViewWnd.glWindow.camera.rotateY = mm.deg2Rad(45.) # # elif frame > 340: # viewer.doc.setRendererVisible('controlModel', False) # viewer.doc.setRendererVisible('skeleton', True) # else: # if 0 <= frame % 50 < 22: # viewer.doc.setRendererVisible('controlModel', True) # viewer.doc.setRendererVisible('skeleton', False) # elif 22 <= frame % 50 < 25: # viewer.doc.setRendererVisible('controlModel', False) # viewer.doc.setRendererVisible('skeleton', False) # elif 25 <= frame % 50 < 47: # viewer.doc.setRendererVisible('controlModel', False) # viewer.doc.setRendererVisible('skeleton', True) # elif 47 <= frame % 50 < 50: # viewer.doc.setRendererVisible('controlModel', False) # viewer.doc.setRendererVisible('skeleton', False) def simulateCallback(frame): global COLOR_ON # print(frame) # print(motion[frame].getJointOrientationLocal(footIdDic['RightFoot_foot_0_1_0'])) if False: if frame == 200: if motionFile == 'wd2_tiptoe.bvh': setParamVal('tiptoe angle', 0.3) if motionFile == 'wd2_tiptoe_zygote.bvh': setParamVal('tiptoe angle', 0.3) # elif 210 < frame < 240: # if motionFile == 'wd2_tiptoe_zygote.bvh': # setParamVal('com Y offset', 0.01/30. * (frame-110)) elif frame == 400: setParamVal('com Y offset', 0.) setParamVal('tiptoe angle', 0.) elif frame == 430: foot_viewer.check_all_seg() # setParamVal('SupKt', 30.) # elif frame == 400: # setParamVal('SupKt', 17.) # hfi.footAdjust(motion[frame], idDic, SEGMENT_FOOT_MAG=.03, SEGMENT_FOOT_RAD=.015, baseHeight=0.02) if abs(getParamVal('tiptoe angle')) > 0.001: tiptoe_angle = getParamVal('tiptoe angle') motion[frame].mulJointOrientationLocal( idDic['LeftFoot_foot_0_0_0'], mm.exp(mm.unitX(), -math.pi * tiptoe_angle)) motion[frame].mulJointOrientationLocal( idDic['LeftFoot_foot_0_1_0'], mm.exp(mm.unitX(), -math.pi * tiptoe_angle)) motion[frame].mulJointOrientationLocal( idDic['RightFoot_foot_0_0_0'], mm.exp(mm.unitX(), -math.pi * tiptoe_angle)) motion[frame].mulJointOrientationLocal( idDic['RightFoot_foot_0_1_0'], mm.exp(mm.unitX(), -math.pi * tiptoe_angle)) # motion[frame].mulJointOrientationLocal(idDic['LeftFoot'], mm.exp(mm.unitX(), math.pi * tiptoe_angle * 0.95)) # motion[frame].mulJointOrientationLocal(idDic['RightFoot'], mm.exp(mm.unitX(), math.pi * tiptoe_angle * 0.95)) motion[frame].mulJointOrientationLocal( idDic['LeftFoot'], mm.exp(mm.unitX(), math.pi * tiptoe_angle)) motion[frame].mulJointOrientationLocal( idDic['RightFoot'], mm.exp(mm.unitX(), math.pi * tiptoe_angle)) if getParamVal('left tilt angle') > 0.001: left_tilt_angle = getParamVal('left tilt angle') if motion[0].skeleton.getJointIndex( 'LeftFoot_foot_0_1') is not None: motion[frame].mulJointOrientationLocal( idDic['LeftFoot_foot_0_1'], mm.exp(mm.unitZ(), -math.pi * left_tilt_angle)) else: motion[frame].mulJointOrientationLocal( idDic['LeftFoot_foot_0_1_0'], mm.exp(mm.unitZ(), -math.pi * left_tilt_angle)) motion[frame].mulJointOrientationLocal( idDic['LeftFoot_foot_1_0'], mm.exp(mm.unitZ(), -math.pi * left_tilt_angle)) motion[frame].mulJointOrientationLocal( idDic['LeftFoot'], mm.exp(mm.unitZ(), math.pi * left_tilt_angle)) elif getParamVal('left tilt angle') < -0.001: left_tilt_angle = getParamVal('left tilt angle') motion[frame].mulJointOrientationLocal( idDic['LeftFoot_foot_0_0'], mm.exp(mm.unitZ(), -math.pi * left_tilt_angle)) if motion[0].skeleton.getJointIndex( 'LeftFoot_foot_0_1') is not None: motion[frame].mulJointOrientationLocal( idDic['LeftFoot_foot_0_1'], mm.exp(mm.unitZ(), math.pi * left_tilt_angle)) # else: # motion[frame].mulJointOrientationLocal(idDic['LeftFoot_foot_0_1_0'], mm.exp(mm.unitZ(), math.pi * left_tilt_angle)) motion[frame].mulJointOrientationLocal( idDic['LeftFoot_foot_1_0'], mm.exp(mm.unitZ(), -math.pi * left_tilt_angle)) motion[frame].mulJointOrientationLocal( idDic['LeftFoot'], mm.exp(mm.unitZ(), math.pi * left_tilt_angle)) if getParamVal('right tilt angle') > 0.001: right_tilt_angle = getParamVal('right tilt angle') if motion[0].skeleton.getJointIndex( 'RightFoot_foot_0_1') is not None: motion[frame].mulJointOrientationLocal( idDic['RightFoot_foot_0_1'], mm.exp(mm.unitZ(), math.pi * right_tilt_angle)) else: motion[frame].mulJointOrientationLocal( idDic['RightFoot_foot_0_1_0'], mm.exp(mm.unitZ(), math.pi * right_tilt_angle)) motion[frame].mulJointOrientationLocal( idDic['RightFoot_foot_1_0'], mm.exp(mm.unitZ(), math.pi * right_tilt_angle)) motion[frame].mulJointOrientationLocal( idDic['RightFoot'], mm.exp(mm.unitZ(), -math.pi * right_tilt_angle)) elif getParamVal('right tilt angle') < -0.001: right_tilt_angle = getParamVal('right tilt angle') motion[frame].mulJointOrientationLocal( idDic['RightFoot_foot_0_0'], mm.exp(mm.unitZ(), math.pi * right_tilt_angle)) if motion[0].skeleton.getJointIndex( 'RightFoot_foot_0_1') is not None: motion[frame].mulJointOrientationLocal( idDic['RightFoot_foot_0_1'], mm.exp(mm.unitZ(), -math.pi * right_tilt_angle)) # else: # motion[frame].mulJointOrientationLocal(idDic['RightFoot_foot_0_1_0'], mm.exp(mm.unitZ(), -math.pi * right_tilt_angle)) motion[frame].mulJointOrientationLocal( idDic['RightFoot_foot_1_0'], mm.exp(mm.unitZ(), math.pi * right_tilt_angle)) motion[frame].mulJointOrientationLocal( idDic['RightFoot'], mm.exp(mm.unitZ(), -math.pi * right_tilt_angle)) motionModel.update(motion[frame]) motionModel.translateByOffset( np.array([ getParamVal('com X offset'), getParamVal('com Y offset'), getParamVal('com Z offset') ])) controlModel.update(motion[frame]) # controlModel_ik.set_q(controlModel.get_q()) # controlModel_ik.set_q(controlModel.get_q()) controlModel_ik.update(motion[frame]) controlModel_ik.translateByOffset( np.array([ -getParamVal('com X offset') * 2., getParamVal('com Y offset'), getParamVal('com Z offset') ])) global g_initFlag global forceShowTime global JsysPre global JsupPreL global JsupPreR global JconstPre global preFootCenter global maxContactChangeCount global contactChangeCount global contact global contactChangeType Kt, Kl, Kh, Bl, Bh, kt_sup = getParamVals( ['Kt', 'Kl', 'Kh', 'Bl', 'Bh', 'SupKt']) Dt = 2 * (Kt**.5) Dl = 2 * (Kl**.5) Dh = 2 * (Kh**.5) dt_sup = 2 * (kt_sup**.5) # 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) # ype.flatten(fix_dofs(DOFs, ddth_des, mcfg, joint_names), ddth_des_flat) # ype.flatten(fix_dofs(DOFs, dth, mcfg, joint_names), dth_flat) ype.flatten(ddth_des, ddth_des_flat) ype.flatten(dth, dth_flat) ################################################# # jacobian ################################################# contact_des_ids = list() # desired contact segments if foot_viewer.check_om_l.value(): contact_des_ids.append( motion[0].skeleton.getJointIndex('LeftFoot_foot_0_0')) if foot_viewer.check_op_l.value(): contact_des_ids.append( motion[0].skeleton.getJointIndex('LeftFoot_foot_0_0_0')) if foot_viewer.check_im_l is not None and foot_viewer.check_im_l.value( ): contact_des_ids.append( motion[0].skeleton.getJointIndex('LeftFoot_foot_0_1')) if foot_viewer.check_ip_l.value(): contact_des_ids.append( motion[0].skeleton.getJointIndex('LeftFoot_foot_0_1_0')) if foot_viewer.check_h_l.value(): contact_des_ids.append( motion[0].skeleton.getJointIndex('LeftFoot_foot_1_0')) if foot_viewer.check_om_r.value(): contact_des_ids.append( motion[0].skeleton.getJointIndex('RightFoot_foot_0_0')) if foot_viewer.check_op_r.value(): contact_des_ids.append( motion[0].skeleton.getJointIndex('RightFoot_foot_0_0_0')) if foot_viewer.check_im_r is not None and foot_viewer.check_im_r.value( ): contact_des_ids.append( motion[0].skeleton.getJointIndex('RightFoot_foot_0_1')) if foot_viewer.check_ip_r.value(): contact_des_ids.append( motion[0].skeleton.getJointIndex('RightFoot_foot_0_1_0')) if foot_viewer.check_h_r.value(): contact_des_ids.append( motion[0].skeleton.getJointIndex('RightFoot_foot_1_0')) contact_ids = list() # temp idx for balancing contact_ids.extend(contact_des_ids) contact_joint_ori = list( map(controlModel.getJointOrientationGlobal, contact_ids)) contact_joint_pos = list( map(controlModel.getJointPositionGlobal, contact_ids)) contact_body_ori = list( map(controlModel.getBodyOrientationGlobal, contact_ids)) contact_body_pos = list( map(controlModel.getBodyPositionGlobal, contact_ids)) contact_body_vel = list( map(controlModel.getBodyVelocityGlobal, contact_ids)) contact_body_angvel = list( map(controlModel.getBodyAngVelocityGlobal, contact_ids)) ref_joint_ori = list( map(motion[frame].getJointOrientationGlobal, contact_ids)) ref_joint_pos = list( map(motion[frame].getJointPositionGlobal, contact_ids)) ref_joint_vel = [ motion.getJointVelocityGlobal(joint_idx, frame) for joint_idx in contact_ids ] ref_joint_angvel = [ motion.getJointAngVelocityGlobal(joint_idx, frame) for joint_idx in contact_ids ] ref_body_ori = list( map(motionModel.getBodyOrientationGlobal, contact_ids)) ref_body_pos = list(map(motionModel.getBodyPositionGlobal, contact_ids)) # ref_body_vel = list(map(controlModel.getBodyVelocityGlobal, contact_ids)) ref_body_angvel = [ motion.getJointAngVelocityGlobal(joint_idx, frame) for joint_idx in contact_ids ] ref_body_vel = [ ref_joint_vel[i] + np.cross(ref_joint_angvel[i], ref_body_pos[i] - ref_joint_pos[i]) for i in range(len(ref_joint_vel)) ] is_contact = [1] * len(contact_ids) contact_right = len(set(contact_des_ids).intersection(rIDlist)) > 0 contact_left = len(set(contact_des_ids).intersection(lIDlist)) > 0 contMotionOffset = th[0][0] - th_r[0][0] linkPositions = controlModel.getBodyPositionsGlobal() linkVelocities = controlModel.getBodyVelocitiesGlobal() linkAngVelocities = controlModel.getBodyAngVelocitiesGlobal() linkInertias = controlModel.getBodyInertiasGlobal() 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) if foot_viewer is not None: foot_viewer.foot_pressure_gl_window.refresh_foot_contact_info( frame, vpWorld, bodyIDsToCheck, mus, Ks, Ds) foot_viewer.foot_pressure_gl_window.goToFrame(frame) # rendering for foot_seg_id in footIdlist: motion_model_renderer.body_colors[foot_seg_id] = (255, 240, 255) for contact_id in contact_ids: motion_model_renderer.body_colors[contact_id] = (255, 0, 0) pallete2 = list() pallete2.append((244, 198, 61)) pallete2.append((4, 105, 113)) pallete2.append((234, 219, 196)) pallete2.append((216, 1, 6)) pallete2.append((230, 230, 230)) pallete = pallete2 color = dict() color['RightFoot'] = pallete[0] color['RightFoot_foot_1_0'] = pallete[4] color['RightFoot_foot_0_0'] = pallete[1] color['RightFoot_foot_0_0_0'] = pallete[2] color['RightFoot_foot_0_1_0'] = pallete[3] color['LeftFoot'] = pallete[0] color['LeftFoot_foot_1_0'] = pallete[4] color['LeftFoot_foot_0_0'] = pallete[1] color['LeftFoot_foot_0_0_0'] = pallete[2] color['LeftFoot_foot_0_1_0'] = pallete[3] if COLOR_ON: for color_key in color.keys(): motion_model_renderer.body_colors[ idDic[color_key]] = color[color_key] rd_CM[0] = CM rd_CM_plane[0] = CM.copy() rd_CM_plane[0][1] = 0. del rd_foot_ori[:] del rd_foot_pos[:] # for seg_foot_id in footIdlist: # rd_foot_ori.append(controlModel.getJointOrientationGlobal(seg_foot_id)) # rd_foot_pos.append(controlModel.getJointPositionGlobal(seg_foot_id)) rd_foot_ori.append(controlModel.getJointOrientationGlobal(supL)) rd_foot_ori.append(controlModel.getJointOrientationGlobal(supR)) rd_foot_pos.append(controlModel.getJointPositionGlobal(supL)) rd_foot_pos.append(controlModel.getJointPositionGlobal(supR)) rd_root_des[0] = rootPos[0] rd_root_ori[0] = controlModel.getBodyOrientationGlobal(0) rd_root_pos[0] = controlModel.getBodyPositionGlobal(0) del rd_CF[:] del rd_CF_pos[:] # render contact_ids # render skeleton if SKELETON_ON: Ts = dict() Ts['pelvis'] = controlModel_ik.getJointTransform(idDic['Hips']) Ts['thigh_R'] = controlModel_ik.getJointTransform( idDic['RightUpLeg']) Ts['shin_R'] = controlModel_ik.getJointTransform(idDic['RightLeg']) Ts['foot_R'] = controlModel_ik.getJointTransform( idDic['RightFoot']) Ts['foot_heel_R'] = controlModel_ik.getJointTransform( idDic['RightFoot']) Ts['heel_R'] = np.eye(4) Ts['outside_metatarsal_R'] = controlModel_ik.getJointTransform( idDic['RightFoot_foot_0_0']) Ts['outside_phalanges_R'] = controlModel_ik.getJointTransform( idDic['RightFoot_foot_0_0_0']) # Ts['inside_metatarsal_R'] = controlModel.getJointTransform(idDic['RightFoot_foot_0_1']) Ts['inside_metatarsal_R'] = np.eye(4) Ts['inside_phalanges_R'] = controlModel_ik.getJointTransform( idDic['RightFoot_foot_0_1_0']) Ts['spine_ribs'] = controlModel_ik.getJointTransform( idDic['Spine']) Ts['head'] = controlModel_ik.getJointTransform(idDic['Spine1']) Ts['upper_limb_R'] = controlModel_ik.getJointTransform( idDic['RightArm']) Ts['lower_limb_R'] = controlModel_ik.getJointTransform( idDic['RightForeArm']) Ts['thigh_L'] = controlModel_ik.getJointTransform( idDic['LeftUpLeg']) Ts['shin_L'] = controlModel_ik.getJointTransform(idDic['LeftLeg']) Ts['foot_L'] = controlModel_ik.getJointTransform(idDic['LeftFoot']) Ts['foot_heel_L'] = controlModel_ik.getJointTransform( idDic['LeftFoot']) Ts['heel_L'] = np.eye(4) Ts['outside_metatarsal_L'] = controlModel_ik.getJointTransform( idDic['LeftFoot_foot_0_0']) Ts['outside_phalanges_L'] = controlModel_ik.getJointTransform( idDic['LeftFoot_foot_0_0_0']) # Ts['inside_metatarsal_L'] = controlModel.getJointTransform(idDic['LeftFoot_foot_0_1']) Ts['inside_metatarsal_L'] = np.eye(4) Ts['inside_phalanges_L'] = controlModel_ik.getJointTransform( idDic['LeftFoot_foot_0_1_0']) Ts['upper_limb_L'] = controlModel_ik.getJointTransform( idDic['LeftArm']) Ts['lower_limb_L'] = controlModel_ik.getJointTransform( idDic['LeftForeArm']) color = dict() color['foot_R'] = pallete[0] color['heel_R'] = pallete[4] color['outside_metatarsal_R'] = pallete[1] color['outside_phalanges_R'] = pallete[2] color['inside_phalanges_R'] = pallete[3] color['foot_L'] = pallete[0] color['heel_L'] = pallete[4] color['outside_metatarsal_L'] = pallete[1] color['outside_phalanges_L'] = pallete[2] color['inside_phalanges_L'] = pallete[3] if COLOR_ON: skeleton_renderer.appendFrameState(Ts, color) else: skeleton_renderer.appendFrameState(Ts) viewer.setSimulateCallback(simulateCallback) viewer.setPreFrameCallback_Always(preFrameCallback_Always) viewer.startTimer(1 / 30.) # viewer.play() viewer.show() foot_viewer = FootWindow(viewer.x() + viewer.w() + 20, viewer.y(), 300, 500, 'foot contact modifier', controlModel) foot_viewer.show() foot_viewer.check_op_l.value(True) foot_viewer.check_ip_l.value(True) foot_viewer.check_op_r.value(True) foot_viewer.check_ip_r.value(True) foot_viewer.check_not_all_seg() viewer.motionViewWnd.goToFrame(0) Fl.run()
def init(): global motion global mcfg global wcfg global stepsPerFrame global config global mcfg_motion global vpWorld global controlModel global totalDOF global DOFs global bodyIDsToCheck global torques_nested global ddth_des_flat global dth_flat global ddth_sol global rd_cForces global rd_cPositions global rd_jointPos global rd_cForcesControl global rd_cPositionsControl global rd_ForceControl global rd_ForceDes global rd_Position global rd_PositionDes global viewer global motionModel global solver global IKModel global dartModel global pdController np.set_printoptions(precision=4, linewidth=200) # motion, mcfg, wcfg, stepsPerFrame, config = mit.create_vchain_1() # motion, mcfg, wcfg, stepsPerFrame, config = mit.create_vchain_5() # motion, mcfg, wcfg, stepsPerFrame, config = mit.create_biped() # motion, mcfg, wcfg, stepsPerFrame, config = mit.create_chiken_foot() # motion, mcfg, wcfg, stepsPerFrame, config = mit.create_foot('fastswim.bvh') # motion, mcfg, wcfg, stepsPerFrame, config = mit.create_foot_2('simpleJump_2.bvh') # motion, mcfg, wcfg, stepsPerFrame, config = mit.create_capsule('simpleJump_onebody.bvh') # motion, mcfg, wcfg, stepsPerFrame, config = mit.create_foot('simpleJump.bvh') # motion, mcfg, wcfg, stepsPerFrame, config = mit.create_foot('simpleJump_long.bvh') motion, mcfg, wcfg, stepsPerFrame, config = mit.create_foot('simpleJump_long.bvh') mcfg_motion = mit.normal_mcfg() vpWorld = cvw.VpWorld(wcfg) controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg) motionModel = cvm.VpMotionModel(vpWorld, motion[0], mcfg) IKModel = cvm.VpMotionModel(vpWorld, motion[0], mcfg) dartModel = cpm.DartModel(wcfg, motion[0], mcfg) pdController = PDController(dartModel.skeleton, dartModel.world.time_step()) # dartWorld.skeletons[1].controller = PDController(dartWorld.skeletons[1], dartWorld.dt) # solver = hik.numIkSolver(wcfg, motion[0], mcfg) vpWorld.SetGlobalDamping(0.9999) # controlModel.initializeHybridDynamics() controlModel.initializeForwardDynamics() ModelOffset = np.array([0., .12, 0.]) controlModel.translateByOffset(ModelOffset) motionModel.translateByOffset(ModelOffset) dartModel.translateByOffset(ModelOffset) vpWorld.SetIntegrator("RK4") # vpWorld.SetIntegrator("IMPLICIT_EULER_FAST") # vpWorld.SetIntegrator("EULER") controlModel.SetJointsDamping(0.2) controlModel.SetJointsElasticity(.01) vpWorld.initialize() totalDOF = controlModel.getTotalDOF() DOFs = controlModel.getDOFs() bodyIDsToCheck = range(vpWorld.getBodyNum()) # flat data structure ddth_des_flat = ype.makeFlatList(totalDOF) dth_flat = ype.makeFlatList(totalDOF) ddth_sol = ype.makeNestedList(DOFs) torques_nested = ype.makeNestedList(DOFs) rd_cForces = [None] rd_cPositions = [None] rd_cForcesControl = [None] rd_cPositionsControl = [None] rd_ForceControl = [None] rd_ForceDes = [None] rd_Position = [None] rd_PositionDes = [None] rd_jointPos = [None] viewer = hsv.hpSimpleViewer(title='main_Test') viewer.doc.addObject('motion', motion) # viewer.doc.addRenderer('motionModel', cvr.VpModelRenderer( # motionModel, MOTION_COLOR, yr.POLYGON_FILL)) # viewer.doc.addRenderer('IKModel', cvr.VpModelRenderer( # solver.model, MOTION_COLOR, yr.POLYGON_FILL)) # viewer.doc.addRenderer('dartModel', yr.DartModelRenderer(dartWorld, CHARACTER_COLOR2)) viewer.doc.addRenderer('dartModel', yr.DartModelRenderer(dartModel, CHARACTER_COLOR2)) viewer.doc.addRenderer('rd_contactForcesControl', yr.VectorsRenderer(rd_cForcesControl, rd_cPositionsControl, (255, 0, 0), .1, 'rd_c1')) viewer.doc.addRenderer('rd_contactForces', yr.VectorsRenderer(rd_cForces, rd_cPositions, (0, 255, 0), .1, 'rd_c2')) viewer.doc.addRenderer('rd_contactForceControl', yr.VectorsRenderer(rd_ForceControl, rd_Position, (0, 0, 255), .1, 'rd_c3')) # viewer.doc.addRenderer('rd_contactForceDes', yr.VectorsRenderer(rd_ForceDes, rd_PositionDes, (255, 0, 255), .1)) # viewer.doc.addRenderer('rd_jointPos', yr.PointsRenderer(rd_jointPos)) viewer.objectInfoWnd.add1DSlider('PD gain', minVal=0., maxVal=200., initVal=10., valStep=.1) viewer.objectInfoWnd.add1DSlider('Joint Damping', minVal=1., maxVal=2000., initVal=35., valStep=1.) viewer.objectInfoWnd.add1DSlider('steps per frame', minVal=1., maxVal=200., initVal=config['stepsPerFrame'], valStep=1.) viewer.objectInfoWnd.add1DSlider('1/simul speed', minVal=1., maxVal=100., initVal=config['simulSpeedInv'], valStep=1.) viewer.objectInfoWnd.add1DSlider('normal des force min', minVal=0., maxVal=1000., initVal=40., valStep=1.) viewer.objectInfoWnd.add1DSlider('normal des force max', minVal=0., maxVal=1000., initVal=40., valStep=1.) viewer.objectInfoWnd.add1DSlider('des force begin', minVal=0., maxVal=len(motion) - 1, initVal=50., valStep=1.) viewer.objectInfoWnd.add1DSlider('des force dur', minVal=1., maxVal=len(motion) - 1, initVal=5., valStep=1.) viewer.objectInfoWnd.add1DSlider('force weight', minVal=-10., maxVal=10., initVal=-1.3, valStep=.01) viewer.objectInfoWnd.add1DSlider('LCP weight', minVal=-10., maxVal=10., initVal=1.3, valStep=.01) viewer.objectInfoWnd.add1DSlider('tau weight', minVal=-10., maxVal=10., initVal=-3., valStep=.01) viewer.objectInfoWnd.add1DSlider('ref', minVal=-10., maxVal=10., initVal=0., valStep=.01) viewer.objectInfoWnd.addBtn('image', viewer.motionViewWnd.dump) viewer.objectInfoWnd.addBtn('image seq dump', viewer.motionViewWnd.dumpMov) viewer.cForceWnd.addDataSet('expForce', FL_BLACK) viewer.cForceWnd.addDataSet('desForceMin', FL_RED) viewer.cForceWnd.addDataSet('desForceMax', FL_RED) viewer.cForceWnd.addDataSet('realForce', FL_GREEN) for i in range(motion[0].skeleton.getJointNum()): print(i, motion[0].skeleton.getJointName(i)) print("(index, id, name)") for i in range(controlModel.getBodyNum()): print(i, controlModel.index2vpid(i), controlModel.index2name(i))
def __init__(self, env_name='walk'): motionFile = '../data/wd2_tiptoe_zygote.bvh' motion, mcfg, wcfg, stepsPerFrame, config, frame_rate = mit.create_biped(motionFile, SEGMENT_FOOT_MAG=0.01, SEGMENT_FOOT_RAD=0.008, motion_scale=.01) self.world = cvw.VpWorld(wcfg) self.world.SetGlobalDamping(0.999) self.skel = cvm.VpControlModel(self.world, motion[0], mcfg) self.world.initialize() self.skel.initializeHybridDynamics() self.Kp, self.Kd = 400., 40. self.Ks, self.Ds = config['Ks'], config['Ds'] self.env_name = env_name self.ref_motion = None # type: ym.Motion motion_name = None if env_name == 'walk': motion_name = "../data/segfoot_wd2_WalkForwardNormal00.bvh" elif env_name == 'walk_fast': motion_name = "../data/segfoot_wd2_WalkForwardVFast00.bvh" elif env_name == 'walk_sukiko': motion_name = '../data/segfoot_wd2_WalkSukiko00.bvh' SEGMENT_FOOT_MAG = 0.01 SEGMENT_FOOT_RAD = 0.008 bvh = yf.readBvhFileAsBvh(motion_name) current_path = os.path.dirname(os.path.abspath(__file__)) partBvhFilePath = current_path + '/../../PyCommon/modules/samples/' partBvhFilePath = partBvhFilePath + 'foot_model_real_joint_01.bvh' partBvh = yf.readBvhFileAsBvh(partBvhFilePath) partSkeleton = partBvh.toJointSkeleton(1., False) bvh.replaceJointFromBvh('RightFoot', partBvh, SEGMENT_FOOT_MAG) partBvh = yf.readBvhFileAsBvh(partBvhFilePath) partBvh.mirror('YZ') bvh.replaceJointFromBvh('LeftFoot', partBvh, SEGMENT_FOOT_MAG) self.ref_motion = bvh.toJointMotion(1., False) # type: ym.JointMotion if env_name == 'walk': # self.ref_motion = self.ref_motion[20:] self.ref_motion.translateByOffset([0., -0.03, 0.]) elif env_name == 'jump': self.ref_motion = self.ref_motion[164:280] elif env_name == 'walk_u_turn': self.ref_motion = self.ref_motion[25:214] self.ref_motion.translateByOffset([0., 0.03, 0.]) elif env_name == 'jump_whole': self.ref_motion = self.ref_motion[315:966] elif env_name == 'walk_u_turn_whole': self.ref_motion.translateByOffset([0., 0.03, 0.]) ref_wcfg = copy.deepcopy(wcfg) self.ref_world = cvw.VpWorld(ref_wcfg) self.ref_skel = cvm.VpControlModel(self.ref_world, motion[0], mcfg) self.ref_world.initialize() self.ref_skel.set_q(np.zeros_like(self.skel.get_q())) self.step_per_frame = stepsPerFrame fix_motion_data_by_foot(self.ref_motion, self.ref_skel, self.ref_world) self.rsi = True self.w_p = 0.65 self.w_v = 0.1 self.w_e = 0.15 self.w_c = 0.1 self.exp_p = 2. self.exp_v = 0.1 self.exp_e = 40. self.exp_c = 10. self.body_num = self.skel.getBodyNum() self.idx_e = [self.skel.name2index('LeftFoot'), self.skel.name2index('RightFoot'), self.skel.name2index('LeftForeArm'), self.skel.name2index('RightForeArm')] # self.body_e = list(map(self.skel.body, self.idx_e)) # self.ref_body_e = list(map(self.ref_skel.body, self.idx_e)) self.motion_len = len(self.ref_motion) self.motion_time = len(self.ref_motion) / self.ref_motion.fps self.time_offset = 0. self.viewer = None self.phase_frame = 0 self.prev_ref_q = self.ref_motion.getDOFPositions(self.phase_frame) self.prev_ref_dq = self.ref_motion.getDOFVelocities(self.phase_frame) self.prev_ref_p_e_hat = np.asarray([self.ref_skel.getBodyPositionGlobal(body_idx) for body_idx in self.idx_e]).flatten() self.prev_ref_com = self.ref_skel.getCOM() # setting for reward self.reward_joint = list() # self.reward_joint.append('Hips') self.reward_joint.append('RightUpLeg') self.reward_joint.append('RightLeg') self.reward_joint.append('LeftUpLeg') self.reward_joint.append('LeftLeg') self.reward_joint.append('Spine') self.reward_joint.append('Spine1') self.reward_joint.append('RightArm') self.reward_joint.append('RightForeArm') self.reward_joint.append('LeftArm') self.reward_joint.append('LeftForeArm') # setting for pd gain self.foot_joint = list() self.foot_joint.append('RightFoot') self.foot_joint.append('RightFoot_foot_0_0') self.foot_joint.append('RightFoot_foot_0_0_0') self.foot_joint.append('RightFoot_foot_0_1_0') self.foot_joint.append('RightFoot_foot_1_0') self.foot_joint.append('LeftFoot') self.foot_joint.append('LeftFoot_foot_0_0') self.foot_joint.append('LeftFoot_foot_0_0_0') self.foot_joint.append('LeftFoot_foot_0_1_0') self.foot_joint.append('LeftFoot_foot_1_0') state_num = 1 + (3*3 + 4) * self.body_num action_num = self.skel.getTotalDOF() - 6 state_high = np.array([np.finfo(np.float32).max] * state_num) action_high = np.array([pi*10.] * action_num) self.action_space = gym.spaces.Box(-action_high, action_high, dtype=np.float32) self.observation_space = gym.spaces.Box(-state_high, state_high, dtype=np.float32)
def init(): global motion global mcfg global wcfg global stepsPerFrame global config global vpWorld global controlModel global totalDOF global DOFs global bodyIDsToCheck global torques_nested global ddth_des_flat global dth_flat global ddth_sol global rd_cForces global rd_cPositions global rd_jointPos global rd_cForcesControl global rd_cPositionsControl global rd_ForceControl global rd_ForceDes global rd_Position global rd_PositionDes global rd_point2 global viewer global motionModel global solver global IKModel def create_foot(motionFile='foot3.bvh'): # motion motion = yf.readBvhFile(motionFile, .05) # world, model mcfg = ypc.ModelConfig() mcfg.defaultDensity = 1000. mcfg.defaultBoneRatio = 1. for i in range(motion[0].skeleton.getElementNum()): mcfg.addNode(motion[0].skeleton.getElementName(i)) node = mcfg.getNode('root') node.geom = 'MyFoot3' node.geom = 'MyBox' # node.length = 1. node.mass = 1. node = mcfg.getNode('foot00') node.geom = 'MyFoot4' node.geom = 'MyBox' node.mass = 1. node = mcfg.getNode('foot01') node.geom = 'MyFoot4' node.geom = 'MyBox' node.mass = 1. def mcfgFix(_mcfg): for v in _mcfg.nodes.itervalues(): if len(v.geoms) == 0: v.geoms.append(v.geom) v.geomMass.append(v.mass) v.geomTs.append(None) # mcfgFix(mcfg) wcfg = ypc.WorldConfig() wcfg.planeHeight = 0. wcfg.useDefaultContactModel = False stepsPerFrame = 40 simulSpeedInv = 1. wcfg.timeStep = (1/30.*simulSpeedInv)/stepsPerFrame # parameter config = dict([]) config['Kt'] = 20; config['Dt'] = 2*(config['Kt']**.5) # tracking gain config['Kl'] = 1; config['Dl'] = 2*(config['Kl']**.5) # linear balance gain config['Kh'] = 1; config['Dh'] = 2*(config['Kh']**.5) # angular balance gain config['Ks'] = 5000; config['Ds'] = 2*(config['Ks']**.5) # penalty force spring gain config['Bt'] = 1. config['Bl'] = 1. config['Bh'] = 1. config['stepsPerFrame'] = stepsPerFrame config['simulSpeedInv'] = simulSpeedInv # etc config['weightMap'] = {'root': 1., 'foot00': 1., 'foot01': 1.} config['weightMapTuple'] = (1., 1., 1.) # config['supLink'] = 'link0' return motion, mcfg, wcfg, stepsPerFrame, config np.set_printoptions(precision=4, linewidth=200) motion, mcfg, wcfg, stepsPerFrame, config = create_foot('test2.bvh') vpWorld = cvw.VpWorld(wcfg) controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg) motionModel = cvm.VpMotionModel(vpWorld, motion[0], mcfg) # IKModel = cvm.VpMotionModel(vpWorld, motion[0], mcfg) # solver = hik.numIkSolver(wcfg, motion[0], mcfg) vpWorld.SetGlobalDamping(0.9999) # controlModel.initializeHybridDynamics() controlModel.initializeForwardDynamics() ModelOffset = np.array([0., 1.42, 0.]) controlModel.translateByOffset(ModelOffset) motionModel.translateByOffset(ModelOffset) # ModelRotateOffset = np.array([[0.707, 0., 0.707],[0., 1., 0.],[-0.707, 0., 0.707]]) # ModelRotateOffset = np.array([[1., 0., 0.],[0., 0., -1.],[0, 1., 0.]]) ModelRotateOffset = np.array([[1., 0., 0.],[0., 0.707, -0.707],[0, 0.707, 0.707]]) controlModel.rotate(ModelRotateOffset) # motionModel.rotate(ModelRotateOffset) vpWorld.SetIntegrator("RK4") # vpWorld.SetIntegrator("IMPLICIT_EULER_FAST") # vpWorld.SetIntegrator("EULER") vpWorld.initialize() totalDOF = controlModel.getTotalDOF() DOFs = controlModel.getDOFs() bodyIDsToCheck = range(vpWorld.getBodyNum()) # flat data structure ddth_des_flat = ype.makeFlatList(totalDOF) dth_flat = ype.makeFlatList(totalDOF) ddth_sol = ype.makeNestedList(DOFs) torques_nested = ype.makeNestedList(DOFs) rd_cForces = [None] rd_cPositions = [None] rd_cForcesControl = [None] rd_cPositionsControl = [None] rd_ForceControl = [None] rd_ForceDes = [None] rd_Position = [None] rd_PositionDes = [None] rd_jointPos = [None] rd_point2 = [None] viewer = hsv.hpSimpleViewer(title='main_Test') viewer.doc.addObject('motion', motion) # viewer.doc.addRenderer('motionModel', cvr.VpModelRenderer( # motionModel, MOTION_COLOR, yr.POLYGON_FILL)) # viewer.doc.addRenderer('IKModel', cvr.VpModelRenderer( # solver.model, MOTION_COLOR, yr.POLYGON_FILL)) viewer.doc.addRenderer('controlModel', cvr.VpModelRenderer( controlModel, CHARACTER_COLOR, yr.POLYGON_FILL)) viewer.doc.addRenderer('rd_contactForcesControl', yr.VectorsRenderer( rd_cForcesControl, rd_cPositionsControl, (255, 0, 0), .1)) viewer.doc.addRenderer('rd_contactForces', yr.VectorsRenderer( rd_cForces, rd_cPositions, (0, 255, 0), .1)) viewer.doc.addRenderer('rd_contactForceControl', yr.VectorsRenderer( rd_ForceControl, rd_Position, (0, 0, 255), .1)) # viewer.doc.addRenderer('rd_contactForceDes', yr.VectorsRenderer(rd_ForceDes, rd_PositionDes, (255, 0, 255), .1)) # viewer.doc.addRenderer('rd_jointPos', yr.PointsRenderer(rd_jointPos)) viewer.doc.addRenderer('rd_point2', yr.PointsRenderer(rd_point2, (255,0,0))) viewer.objectInfoWnd.add1DSlider( 'PD gain', minVal=0., maxVal=200., initVal=10., valStep=.1) viewer.objectInfoWnd.add1DSlider( 'Joint Damping', minVal=1., maxVal=2000., initVal=35., valStep=1.) viewer.objectInfoWnd.add1DSlider( 'steps per frame', minVal=1., maxVal=200., initVal=config['stepsPerFrame'], valStep=1.) viewer.objectInfoWnd.add1DSlider( '1/simul speed', minVal=1., maxVal=100., initVal=config['simulSpeedInv'], valStep=1.) viewer.objectInfoWnd.add1DSlider( 'normal des force min', minVal=0., maxVal=1000., initVal=0., valStep=1.) viewer.objectInfoWnd.add1DSlider( 'normal des force max', minVal=0., maxVal=1000., initVal=80., valStep=1.) viewer.objectInfoWnd.add1DSlider( 'des force begin', minVal=0., maxVal=len(motion) - 1, initVal=70., valStep=1.) viewer.objectInfoWnd.add1DSlider( 'des force dur', minVal=1., maxVal=len(motion) - 1, initVal=5., valStep=1.) viewer.objectInfoWnd.add1DSlider( 'force weight', minVal=-10., maxVal=10., initVal=0., valStep=.01) viewer.objectInfoWnd.add1DSlider( 'LCP weight', minVal=-10., maxVal=10., initVal=0., valStep=.01) viewer.objectInfoWnd.add1DSlider( 'tau weight', minVal=-10., maxVal=10., initVal=0., valStep=.01) viewer.objectInfoWnd.addBtn('image', viewer.motionViewWnd.dump) viewer.objectInfoWnd.addBtn('image seq dump', viewer.motionViewWnd.dumpMov) viewer.cForceWnd.addDataSet('expForce', FL_BLACK) viewer.cForceWnd.addDataSet('desForceMin', FL_RED) viewer.cForceWnd.addDataSet('desForceMax', FL_RED) viewer.cForceWnd.addDataSet('realForce', FL_GREEN) for i in range(motion[0].skeleton.getJointNum()): print(i, motion[0].skeleton.getJointName(i)) print motion[0].skeleton.getJointNum() print("(index, id, name)") for i in range(controlModel.getBodyNum()): print(i, controlModel.index2id(i), controlModel.index2name(i)) print controlModel.getBodyNum()
def main(): # np.set_printoptions(precision=4, linewidth=200) np.set_printoptions(precision=5, threshold=np.inf, suppress=True, linewidth=3000) motion, mcfg, wcfg, stepsPerFrame, config, frame_rate = mit.create_biped() # motion, mcfg, wcfg, stepsPerFrame, config = mit.create_jump_biped() vpWorld = cvw.VpWorld(wcfg) motionModel = cvm.VpMotionModel(vpWorld, motion[0], mcfg) controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg) # controlModel_shadow_for_ik = cvm.VpControlModel(vpWorld, motion[0], mcfg) vpWorld.initialize() controlModel.initializeHybridDynamics() # controlToMotionOffset = (1.5, -0.02, 0) controlToMotionOffset = (1.5, 0, 0) controlModel.translateByOffset(controlToMotionOffset) # controlModel_shadow_for_ik.set_q(controlModel.get_q()) # controlModel_shadow_for_ik.computeJacobian(0, np.array([0., 0., 0.])) wcfg_ik = copy.deepcopy(wcfg) vpWorld_ik = cvw.VpWorld(wcfg_ik) controlModel_ik = cvm.VpControlModel(vpWorld_ik, motion[0], mcfg) vpWorld_ik.initialize() controlModel_ik.set_q(controlModel.get_q()) totalDOF = controlModel.getTotalDOF() DOFs = controlModel.getDOFs() foot_dofs = [] left_foot_dofs = [] right_foot_dofs = [] foot_seg_dofs = [] left_foot_seg_dofs = [] right_foot_seg_dofs = [] # for joint_idx in range(motion[0].skeleton.getJointNum()): for joint_idx in range(controlModel.getJointNum()): joint_name = controlModel.index2name(joint_idx) # joint_name = motion[0].skeleton.getJointName(joint_idx) if 'Foot' in joint_name: foot_dofs_temp = controlModel.getJointDOFIndexes(joint_idx) foot_dofs.extend(foot_dofs_temp) if 'Left' in joint_name: left_foot_dofs.extend(foot_dofs_temp) elif 'Right' in joint_name: right_foot_dofs.extend(foot_dofs_temp) if 'foot' in joint_name: foot_dofs_temp = controlModel.getJointDOFIndexes(joint_idx) foot_seg_dofs.extend(foot_dofs_temp) if 'Left' in joint_name: left_foot_seg_dofs.extend(foot_dofs_temp) elif 'Right' in joint_name: right_foot_seg_dofs.extend(foot_dofs_temp) # parameter Kt = config['Kt'] Dt = config['Dt'] # tracking gain Kl = config['Kl'] Dl = config['Dl'] # linear balance gain Kh = config['Kh'] Dh = config['Dh'] # angular balance gain Ks = config['Ks'] Ds = config['Ds'] # penalty force spring gain Bt = config['Bt'] Bl = config['Bl'] Bh = config['Bh'] supL = motion[0].skeleton.getJointIndex(config['supLink1']) supR = motion[0].skeleton.getJointIndex(config['supLink2']) selectedBody = motion[0].skeleton.getJointIndex(config['end']) constBody = motion[0].skeleton.getJointIndex('RightFoot') # jacobian # JsupL = yjc.makeEmptyJacobian(DOFs, 1) # dJsupL = JsupL.copy() # JsupPreL = JsupL.copy() # # JsupR = yjc.makeEmptyJacobian(DOFs, 1) # dJsupR = JsupR.copy() # JsupPreR = JsupR.copy() Jconst = yjc.makeEmptyJacobian(DOFs, 1) dJconst = Jconst.copy() JconstPre = Jconst.copy() Jsys = yjc.makeEmptyJacobian(DOFs, controlModel.getBodyNum()) dJsys_temp = Jsys.copy() JsysPre = Jsys.copy() constJointMasks = [yjc.getLinkJointMask(motion[0].skeleton, constBody)] allLinkJointMasks = yjc.getAllLinkJointMasks(motion[0].skeleton) # momentum matrix linkMasses = controlModel.getBodyMasses() totalMass = controlModel.getTotalMass() TO = ymt.make_TO(linkMasses) dTO = ymt.make_dTO(len(linkMasses)) # optimization problem = yac.LSE(totalDOF, 12) # a_sup = (0,0,0, 0,0,0) #ori # a_sup = (0,0,0, 0,0,0) #L CP_old = [mm.v3(0., 0., 0.)] # penalty method bodyIDsToCheck = list(range(vpWorld.getBodyNum())) # mus = [1.]*len(bodyIDsToCheck) mus = [.5] * len(bodyIDsToCheck) # flat data structure ddth_des_flat = ype.makeFlatList(totalDOF) dth_flat = ype.makeFlatList(totalDOF) ddth_sol = ype.makeNestedList(DOFs) # viewer rd_footCenter = [None] rd_footCenter_ref = [None] rd_footCenterL = [None] rd_footCenterR = [None] rd_CM_plane = [None] rd_CM = [None] rd_CP = [None] rd_CP_des = [None] rd_dL_des_plane = [None] rd_dH_des = [None] rd_grf_des = [None] rd_exf_des = [None] rd_exfen_des = [None] rd_root_des = [None] rd_root_ori = [None] rd_root_pos = [None] rd_CF = [None] rd_CF_pos = [None] rootPos = [None] selectedBodyId = [selectedBody] extraForce = [None] extraForcePos = [None] rightFootVectorX = [None] rightFootVectorY = [None] rightFootVectorZ = [None] rightFootPos = [None] rightVectorX = [None] rightVectorY = [None] rightVectorZ = [None] rightPos = [None] def makeEmptyBasicSkeletonTransformDict(init=None): Ts = dict() Ts['pelvis'] = init Ts['spine_ribs'] = init Ts['head'] = init Ts['thigh_R'] = init Ts['shin_R'] = init Ts['foot_R'] = init Ts['upper_limb_R'] = init Ts['lower_limb_R'] = init Ts['thigh_L'] = init Ts['shin_L'] = init Ts['foot_L'] = init Ts['upper_limb_L'] = init Ts['lower_limb_L'] = init return Ts # viewer = ysv.SimpleViewer() viewer = hsv.hpSimpleViewer(rect=[0, 0, 1024, 768], viewForceWnd=False) # viewer.record(False) # viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion, (0,255,255), yr.LINK_BONE)) viewer.doc.addObject('motion', motion) viewer.doc.addRenderer( 'motionModel', yr.VpModelRenderer(motionModel, (150, 150, 255), yr.POLYGON_FILL)) viewer.doc.addRenderer( 'ikModel', yr.VpModelRenderer(controlModel_ik, (150, 150, 255), yr.POLYGON_LINE)) # viewer.doc.addRenderer('controlModel', cvr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_LINE)) control_model_renderer = yr.VpModelRenderer(controlModel, (255, 240, 255), yr.POLYGON_FILL) viewer.doc.addRenderer('controlModel', control_model_renderer) skeleton_renderer = None if SKELETON_ON: skeleton_renderer = yr.BasicSkeletonRenderer( makeEmptyBasicSkeletonTransformDict(np.eye(4)), offset_Y=-0.08) viewer.doc.addRenderer('skeleton', skeleton_renderer) viewer.doc.addRenderer('rd_footCenter', yr.PointsRenderer(rd_footCenter)) viewer.doc.addRenderer('rd_footCenter_ref', yr.PointsRenderer(rd_footCenter_ref)) viewer.doc.addRenderer('rd_CM_plane', yr.PointsRenderer(rd_CM_plane, (255, 255, 0))) viewer.doc.addRenderer('rd_CP', yr.PointsRenderer(rd_CP, (0, 255, 0))) viewer.doc.addRenderer('rd_CP_des', yr.PointsRenderer(rd_CP_des, (255, 0, 255))) viewer.doc.addRenderer( 'rd_dL_des_plane', yr.VectorsRenderer(rd_dL_des_plane, rd_CM, (255, 255, 0))) viewer.doc.addRenderer('rd_dH_des', yr.VectorsRenderer(rd_dH_des, rd_CM, (0, 255, 0))) # viewer.doc.addRenderer('rd_grf_des', yr.ForcesRenderer(rd_grf_des, rd_CP_des, (0,255,0), .001)) viewer.doc.addRenderer('rd_CF', yr.VectorsRenderer(rd_CF, rd_CF_pos, (255, 255, 0))) viewer.doc.addRenderer( 'rd_root_ori', yr.OrientationsRenderer(rd_root_ori, rd_root_pos, (255, 255, 0))) viewer.doc.addRenderer( 'extraForce', yr.VectorsRenderer(rd_exf_des, extraForcePos, (0, 255, 0))) viewer.doc.addRenderer( 'extraForceEnable', yr.VectorsRenderer(rd_exfen_des, extraForcePos, (255, 0, 0))) # viewer.doc.addRenderer('right_foot_oriX', yr.VectorsRenderer(rightFootVectorX, rightFootPos, (255,0,0))) # viewer.doc.addRenderer('right_foot_oriY', yr.VectorsRenderer(rightFootVectorY, rightFootPos, (0,255,0))) # viewer.doc.addRenderer('right_foot_oriZ', yr.VectorsRenderer(rightFootVectorZ, rightFootPos, (0,0,255))) # viewer.doc.addRenderer('right_oriX', yr.VectorsRenderer(rightVectorX, rightPos, (255,0,0))) # viewer.doc.addRenderer('right_oriY', yr.VectorsRenderer(rightVectorY, rightPos, (0,255,0))) # viewer.doc.addRenderer('right_oriZ', yr.VectorsRenderer(rightVectorZ, rightPos, (0,0,255))) # foot_viewer = FootWindow(viewer.x() + viewer.w() + 20, viewer.y(), 300, 400, 'foot contact modifier', controlModel) foot_viewer = None # type: FootWindow # success!! # initKt = 50 # initKl = 10.1 # initKh = 3.1 # initBl = .1 # initBh = .1 # initSupKt = 21.6 # initFm = 100.0 # success!! -- 2015.2.12. double stance # initKt = 50 # initKl = 37.1 # initKh = 41.8 # initBl = .1 # initBh = .13 # initSupKt = 21.6 # initFm = 165.0 # single stance # initKt = 25 # initKl = 80.1 # initKh = 10.8 # initBl = .1 # initBh = .13 # initSupKt = 21.6 # initFm = 50.0 # single stance -> double stance # initKt = 25 # initKl = 60. # initKh = 20. # initBl = .1 # initBh = .13 # initSupKt = 21.6 # initFm = 50.0 initKt = 25 initKl = 100. initKh = 100. initBl = .1 initBh = .13 initSupKt = 17 initFm = 50.0 initComX = 0. initComY = 0. initComZ = 0. viewer.objectInfoWnd.add1DSlider("Kt", 0., 300., 1., initKt) viewer.objectInfoWnd.add1DSlider("Kl", 0., 300., 1., initKl) viewer.objectInfoWnd.add1DSlider("Kh", 0., 300., 1., initKh) viewer.objectInfoWnd.add1DSlider("Bl", 0., 1., .001, initBl) viewer.objectInfoWnd.add1DSlider("Bh", 0., 1., .001, initBh) viewer.objectInfoWnd.add1DSlider("SupKt", 0., 300., 0.1, initSupKt) viewer.objectInfoWnd.add1DSlider("Fm", 0., 1000., 10., initFm) viewer.objectInfoWnd.add1DSlider("com X offset", -1., 1., 0.01, initComX) viewer.objectInfoWnd.add1DSlider("com Y offset", -1., 1., 0.01, initComY) viewer.objectInfoWnd.add1DSlider("com Z offset", -1., 1., 0.01, initComZ) viewer.force_on = False def viewer_SetForceState(object): viewer.force_on = True def viewer_GetForceState(): return viewer.force_on def viewer_ResetForceState(): viewer.force_on = False viewer.objectInfoWnd.addBtn('Force on', viewer_SetForceState) viewer_ResetForceState() offset = 60 viewer.objectInfoWnd.begin() viewer.objectInfoWnd.labelForceX = Fl_Value_Input(20, 30 + offset * 9, 40, 20, 'X') viewer.objectInfoWnd.labelForceX.value(0) viewer.objectInfoWnd.labelForceY = Fl_Value_Input(80, 30 + offset * 9, 40, 20, 'Y') viewer.objectInfoWnd.labelForceY.value(0) viewer.objectInfoWnd.labelForceZ = Fl_Value_Input(140, 30 + offset * 9, 40, 20, 'Z') viewer.objectInfoWnd.labelForceZ.value(1) viewer.objectInfoWnd.labelForceDur = Fl_Value_Input( 220, 30 + offset * 9, 40, 20, 'Dur') viewer.objectInfoWnd.labelForceDur.value(0.1) viewer.objectInfoWnd.end() # self.sliderFm = Fl_Hor_Nice_Slider(10, 42+offset*6, 250, 10) def getParamVal(paramname): return viewer.objectInfoWnd.getVal(paramname) def getParamVals(paramnames): return (getParamVal(name) for name in paramnames) extendedFootName = [ 'Foot_foot_0_0', 'Foot_foot_0_1', 'Foot_foot_0_0_0', 'Foot_foot_0_1_0', 'Foot_foot_1_0' ] lIDdic = { 'Left' + name: motion[0].skeleton.getJointIndex('Left' + name) for name in extendedFootName } rIDdic = { 'Right' + name: motion[0].skeleton.getJointIndex('Right' + name) for name in extendedFootName } footIdDic = lIDdic.copy() footIdDic.update(rIDdic) lIDlist = [ motion[0].skeleton.getJointIndex('Left' + name) for name in extendedFootName ] rIDlist = [ motion[0].skeleton.getJointIndex('Right' + name) for name in extendedFootName ] footIdlist = [] footIdlist.extend(lIDlist) footIdlist.extend(rIDlist) foot_left_idx = motion[0].skeleton.getJointIndex('LeftFoot') foot_right_idx = motion[0].skeleton.getJointIndex('RightFoot') foot_left_idx_temp = motion[0].skeleton.getJointIndex('LeftFoot_foot_1_0') foot_right_idx_temp = motion[0].skeleton.getJointIndex( 'RightFoot_foot_1_0') def get_jacobianbase_and_masks(skeleton, DOFs, joint_idx): J = yjc.makeEmptyJacobian(DOFs, 1) joint_masks = [yjc.getLinkJointMask(skeleton, joint_idx)] return J, joint_masks # ik_solver = hik.numIkSolver(dartIkModel) # ik_solver.clear() ################################### # simulate ################################### def simulateCallback(frame): # print(frame) # print(motion[frame].getJointOrientationLocal(footIdDic['RightFoot_foot_0_1_0'])) if False and viewer_GetForceState(): # print('force on, frame: ', frame) motion[frame].mulJointOrientationLocal( footIdDic['LeftFoot_foot_0_0_0'], mm.exp(mm.unitX(), -math.pi * mm.SCALAR_1_6)) motion[frame].mulJointOrientationLocal( footIdDic['LeftFoot_foot_0_1_0'], mm.exp(mm.unitX(), -math.pi * mm.SCALAR_1_6)) motion[frame].mulJointOrientationLocal( footIdDic['RightFoot_foot_0_0_0'], mm.exp(mm.unitX(), -math.pi * mm.SCALAR_1_6)) motion[frame].mulJointOrientationLocal( footIdDic['RightFoot_foot_0_1_0'], mm.exp(mm.unitX(), -math.pi * mm.SCALAR_1_6)) # print(motion[frame].getJointOrientationLocal(footIdDic['RightFoot_foot_0_1_0'])) motionModel.update(motion[frame]) controlModel_ik.set_q(controlModel.get_q()) global g_initFlag global forceShowTime global JsysPre global JsupPreL global JsupPreR global JconstPre global preFootCenter global maxContactChangeCount global contactChangeCount global contact global contactChangeType # Kt, Kl, Kh, Bl, Bh, kt_sup = viewer.GetParam() Kt, Kl, Kh, Bl, Bh, kt_sup = getParamVals( ['Kt', 'Kl', 'Kh', 'Bl', 'Bh', 'SupKt']) Dt = 2 * (Kt**.5) Dl = 2 * (Kl**.5) Dh = 2 * (Kh**.5) dt_sup = 2 * (kt_sup**.5) doubleTosingleOffset = 0.15 singleTodoubleOffset = 0.30 # doubleTosingleOffset = 0.09 doubleTosingleVelOffset = 0.0 # 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) ype.flatten(ddth_des, ddth_des_flat) # ddth_des_flat = Kt * (motion.get_q(frame) - np.array(controlModel.get_q())) - Dt * np.array(controlModel.get_dq()) ype.flatten(dth, dth_flat) # dth_flat = np.array(controlModel.get_dq()) ################################################# # jacobian ################################################# contact_des_ids = list() # desired contact segments if foot_viewer.check_om_l.value(): contact_des_ids.append( motion[0].skeleton.getJointIndex('LeftFoot_foot_0_0')) if foot_viewer.check_op_l.value(): contact_des_ids.append( motion[0].skeleton.getJointIndex('LeftFoot_foot_0_0_0')) if foot_viewer.check_im_l.value(): contact_des_ids.append( motion[0].skeleton.getJointIndex('LeftFoot_foot_0_1')) if foot_viewer.check_ip_l.value(): contact_des_ids.append( motion[0].skeleton.getJointIndex('LeftFoot_foot_0_1_0')) if foot_viewer.check_h_l.value(): contact_des_ids.append( motion[0].skeleton.getJointIndex('LeftFoot_foot_1_0')) if foot_viewer.check_om_r.value(): contact_des_ids.append( motion[0].skeleton.getJointIndex('RightFoot_foot_0_0')) if foot_viewer.check_op_r.value(): contact_des_ids.append( motion[0].skeleton.getJointIndex('RightFoot_foot_0_0_0')) if foot_viewer.check_im_r.value(): contact_des_ids.append( motion[0].skeleton.getJointIndex('RightFoot_foot_0_1')) if foot_viewer.check_ip_r.value(): contact_des_ids.append( motion[0].skeleton.getJointIndex('RightFoot_foot_0_1_0')) if foot_viewer.check_h_r.value(): contact_des_ids.append( motion[0].skeleton.getJointIndex('RightFoot_foot_1_0')) contact_ids = list() # temp idx for balancing contact_ids.extend(contact_des_ids) contact_joint_ori = list( map(controlModel.getJointOrientationGlobal, contact_ids)) contact_joint_pos = list( map(controlModel.getJointPositionGlobal, contact_ids)) contact_body_ori = list( map(controlModel.getBodyOrientationGlobal, contact_ids)) contact_body_pos = list( map(controlModel.getBodyPositionGlobal, contact_ids)) contact_body_vel = list( map(controlModel.getBodyVelocityGlobal, contact_ids)) contact_body_angvel = list( map(controlModel.getBodyAngVelocityGlobal, contact_ids)) ref_joint_ori = list( map(motion[frame].getJointOrientationGlobal, contact_ids)) ref_joint_pos = list( map(motion[frame].getJointPositionGlobal, contact_ids)) ref_joint_vel = [ motion.getJointVelocityGlobal(joint_idx, frame) for joint_idx in contact_ids ] ref_joint_angvel = [ motion.getJointAngVelocityGlobal(joint_idx, frame) for joint_idx in contact_ids ] ref_body_ori = list( map(motionModel.getBodyOrientationGlobal, contact_ids)) ref_body_pos = list(map(motionModel.getBodyPositionGlobal, contact_ids)) # ref_body_vel = list(map(controlModel.getBodyVelocityGlobal, contact_ids)) ref_body_angvel = [ motion.getJointAngVelocityGlobal(joint_idx, frame) for joint_idx in contact_ids ] ref_body_vel = [ ref_joint_vel[i] + np.cross(ref_joint_angvel[i], ref_body_pos[i] - ref_joint_pos[i]) for i in range(len(ref_joint_vel)) ] J_contacts = [ yjc.makeEmptyJacobian(DOFs, 1) for i in range(len(contact_ids)) ] dJ_contacts = [ yjc.makeEmptyJacobian(DOFs, 1) for i in range(len(contact_ids)) ] joint_masks = [ yjc.getLinkJointMask(motion[0].skeleton, joint_idx) for joint_idx in contact_ids ] # caution!! body orientation and joint orientation of foot are totally different!! footOriL = controlModel.getJointOrientationGlobal(supL) footOriR = controlModel.getJointOrientationGlobal(supR) # desire footCenter[1] = 0.041135 # desire footCenter[1] = 0.0197 footCenterL = controlModel.getBodyPositionGlobal(supL) footCenterR = controlModel.getBodyPositionGlobal(supR) footBodyOriL = controlModel.getBodyOrientationGlobal(supL) footBodyOriR = controlModel.getBodyOrientationGlobal(supR) footBodyVelL = controlModel.getBodyVelocityGlobal(supL) footBodyVelR = controlModel.getBodyVelocityGlobal(supR) footBodyAngVelL = controlModel.getBodyAngVelocityGlobal(supL) footBodyAngVelR = controlModel.getBodyAngVelocityGlobal(supR) refFootL = motionModel.getBodyPositionGlobal(supL) refFootR = motionModel.getBodyPositionGlobal(supR) refFootVelL = motionModel.getBodyVelocityGlobal(supL) refFootVelR = motionModel.getBodyVelocityGlobal(supR) refFootAngVelL = motionModel.getBodyAngVelocityGlobal(supL) refFootAngVelR = motionModel.getBodyAngVelocityGlobal(supR) refFootJointVelR = motion.getJointVelocityGlobal(supR, frame) refFootJointAngVelR = motion.getJointAngVelocityGlobal(supR, frame) refFootJointR = motion.getJointPositionGlobal(supR, frame) refFootVelR = refFootJointVelR + np.cross(refFootJointAngVelR, (refFootR - refFootJointR)) refFootJointVelL = motion.getJointVelocityGlobal(supL, frame) refFootJointAngVelL = motion.getJointAngVelocityGlobal(supL, frame) refFootJointL = motion.getJointPositionGlobal(supL, frame) refFootVelL = refFootJointVelL + np.cross(refFootJointAngVelL, (refFootL - refFootJointL)) is_contact = [1] * len(contact_ids) contactR = 1 contactL = 1 if refFootVelR[1] < 0 and refFootVelR[1] / 30. + refFootR[ 1] > singleTodoubleOffset: contactR = 0 if refFootVelL[1] < 0 and refFootVelL[1] / 30. + refFootL[ 1] > singleTodoubleOffset: contactL = 0 if refFootVelR[1] > 0 and refFootVelR[1] / 30. + refFootR[ 1] > doubleTosingleOffset: contactR = 0 if refFootVelL[1] > 0 and refFootVelL[1] / 30. + refFootL[ 1] > doubleTosingleOffset: contactL = 0 # contactR = 1 contMotionOffset = th[0][0] - th_r[0][0] linkPositions = controlModel.getBodyPositionsGlobal() linkVelocities = controlModel.getBodyVelocitiesGlobal() linkAngVelocities = controlModel.getBodyAngVelocitiesGlobal() linkInertias = controlModel.getBodyInertiasGlobal() 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) # calculate contact state # if g_initFlag == 1 and contact == 1 and refFootR[1] < doubleTosingleOffset and footCenterR[1] < 0.08: if g_initFlag == 1: # contact state # 0: flying 1: right only 2: left only 3: double # if contact == 2 and refFootR[1] < doubleTosingleOffset: if contact == 2 and contactR == 1: contact = 3 maxContactChangeCount += 30 contactChangeCount += maxContactChangeCount contactChangeType = 'StoD' # elif contact == 3 and refFootL[1] < doubleTosingleOffset: elif contact == 1 and contactL == 1: contact = 3 maxContactChangeCount += 30 contactChangeCount += maxContactChangeCount contactChangeType = 'StoD' # elif contact == 3 and refFootR[1] > doubleTosingleOffset: elif contact == 3 and contactR == 0: contact = 2 contactChangeCount += maxContactChangeCount contactChangeType = 'DtoS' # elif contact == 3 and refFootL[1] > doubleTosingleOffset: elif contact == 3 and contactL == 0: contact = 1 contactChangeCount += maxContactChangeCount contactChangeType = 'DtoS' else: contact = 0 # if refFootR[1] < doubleTosingleOffset: if contactR == 1: contact += 1 # if refFootL[1] < doubleTosingleOffset: if contactL == 1: contact += 2 # initialization if g_initFlag == 0: # JsysPre = Jsys.copy() JconstPre = Jconst.copy() softConstPoint = footCenterR.copy() # yjc.computeJacobian2(JsysPre, DOFs, jointPositions, jointAxeses, linkPositions, allLinkJointMasks) # yjc.computeJacobian2(JconstPre, DOFs, jointPositions, jointAxeses, [softConstPoint], constJointMasks) footCenter = footCenterL + (footCenterR - footCenterL) / 2.0 footCenter[1] = 0. preFootCenter = footCenter.copy() # footToBodyFootRotL = np.dot(np.transpose(footOriL), footBodyOriL) # footToBodyFootRotR = np.dot(np.transpose(footOriR), footBodyOriR) if refFootR[1] < doubleTosingleOffset: contact += 1 if refFootL[1] < doubleTosingleOffset: contact += 2 g_initFlag = 1 # calculate jacobian Jsys, dJsys = controlModel.computeCom_J_dJdq() for i in range(len(J_contacts)): J_contacts[i] = Jsys[6 * contact_ids[i]:6 * contact_ids[i] + 6, :] dJ_contacts[i] = dJsys[6 * contact_ids[i]:6 * contact_ids[i] + 6] # calculate footCenter footCenter = sum(contact_body_pos) / len(contact_body_pos) if len(contact_body_pos) > 0 \ else .5 * (controlModel.getBodyPositionGlobal(supL) + controlModel.getBodyPositionGlobal(supR)) # if len(contact_body_pos) > 2: # hull = ConvexHull(contact_body_pos) footCenter_ref = sum(ref_body_pos) / len(ref_body_pos) if len(ref_body_pos) > 0 \ else .5 * (motionModel.getBodyPositionGlobal(supL) + motionModel.getBodyPositionGlobal(supR)) footCenter_ref = footCenter_ref + contMotionOffset # if len(ref_body_pos) > 2: # hull = ConvexHull(ref_body_pos) footCenter_ref[1] = 0. # footCenter = footCenterL + (footCenterR - footCenterL)/2.0 # if refFootR[1] >doubleTosingleOffset: # if refFootR[1] > doubleTosingleOffset or footCenterR[1] > 0.08: # if contact == 1 or footCenterR[1] > 0.08: # if contact == 2 or footCenterR[1] > doubleTosingleOffset/2: if contact == 2: footCenter = footCenterL.copy() # elif contact == 1 or footCenterL[1] > doubleTosingleOffset/2: if contact == 1: footCenter = footCenterR.copy() footCenter[1] = 0. footCenter[0] = footCenter[0] + getParamVal('com X offset') if contactChangeCount > 0 and contactChangeType == 'StoD': # change footcenter gradually footCenter = preFootCenter + ( maxContactChangeCount - contactChangeCount) * ( footCenter - preFootCenter) / maxContactChangeCount preFootCenter = footCenter.copy() # linear momentum # TODO: # We should consider dCM_ref, shouldn't we? # add getBodyPositionGlobal and getBodyPositionsGlobal in csVpModel! # to do that, set joint velocities to vpModel CM_ref_plane = footCenter # CM_ref_plane = footCenter_ref dL_des_plane = Kl * totalMass * (CM_ref_plane - CM_plane) - Dl * totalMass * dCM_plane # dL_des_plane[1] = 0. # print('dCM_plane : ', np.linalg.norm(dCM_plane)) # angular momentum CP_ref = footCenter # CP_ref = footCenter_ref bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce( bodyIDsToCheck, mus, Ks, Ds) # bodyIDs, contactPositions, contactPositionLocals, contactForces, contactVelocities = vpWorld.calcManyPenaltyForce(0, bodyIDsToCheck, mus, Ks, Ds) CP = yrp.getCP(contactPositions, contactForces) if CP_old[0] is None or CP is None: dCP = None else: dCP = (CP - CP_old[0]) / (1 / 30.) CP_old[0] = CP if CP is not None and dCP is not None: ddCP_des = Kh * (CP_ref - CP) - Dh * dCP dCP_des = dCP + ddCP_des * (1 / 30.) CP_des = CP + dCP_des * (1 / 30.) # CP_des = footCenter CP_des = CP + dCP * (1 / 30.) + .5 * ddCP_des * ((1 / 30.)**2) dH_des = np.cross( (CP_des - CM), (dL_des_plane + totalMass * mm.s2v(wcfg.gravity))) if contactChangeCount > 0: # and contactChangeType == 'DtoS': dH_des *= (maxContactChangeCount - contactChangeCount) / maxContactChangeCount else: dH_des = None # convex hull contact_pos_2d = np.asarray([ np.array([contactPosition[0], contactPosition[2]]) for contactPosition in contactPositions ]) p = np.array([CM_plane[0], CM_plane[2]]) # hull = None # type: Delaunay # if contact_pos_2d.shape[0] > 0: # hull = Delaunay(contact_pos_2d) # print(hull.find_simplex(p) >= 0) # set up equality constraint # TODO: # logSO3 is just q'', not acceleration. # To make a_oris acceleration, q'' -> a will be needed # body_ddqs = list(map(mm.logSO3, [mm.getSO3FromVectors(np.dot(body_ori, mm.unitY()), mm.unitY()) for body_ori in contact_body_ori])) body_ddqs = list( map(mm.logSO3, [ np.dot( contact_body_ori[i].T, np.dot( ref_body_ori[i], mm.getSO3FromVectors( np.dot(ref_body_ori[i], mm.unitY()), mm.unitY()))) for i in range(len(contact_body_ori)) ])) body_qs = list(map(mm.logSO3, contact_body_ori)) body_angs = [ np.dot(contact_body_ori[i], contact_body_angvel[i]) for i in range(len(contact_body_ori)) ] body_dqs = [ mm.vel2qd(body_angs[i], body_qs[i]) for i in range(len(body_angs)) ] a_oris = [ np.dot(contact_body_ori[i], mm.qdd2accel(body_ddqs[i], body_dqs[i], body_qs[i])) for i in range(len(contact_body_ori)) ] # body_ddq = body_ddqs[0] # body_ori = contact_body_ori[0] # body_ang = np.dot(body_ori.T, contact_body_angvel[0]) # # body_q = mm.logSO3(body_ori) # body_dq = mm.vel2qd(body_ang, body_q) # a_ori = np.dot(body_ori, mm.qdd2accel(body_ddq, body_dq, body_q)) # a_oris = list(map(mm.logSO3, [mm.getSO3FromVectors(np.dot(body_ori, mm.unitY()), mm.unitY()) for body_ori in contact_body_ori])) a_sups = [ np.append( kt_sup * (ref_body_pos[i] - contact_body_pos[i] + contMotionOffset) + dt_sup * (ref_body_vel[i] - contact_body_vel[i]), kt_sup * a_oris[i] + dt_sup * (ref_body_angvel[i] - contact_body_angvel[i])) for i in range(len(a_oris)) ] # 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) rs = np.dot(dP, np.dot(Jsys, dth_flat)) + np.dot(P, dJsys) r_bias, s_bias = np.hsplit(rs, 2) ####################################################### # optimization ####################################################### # if contact == 2 and footCenterR[1] > doubleTosingleOffset/2: if contact == 2: config['weightMap']['RightUpLeg'] = .8 config['weightMap']['RightLeg'] = .8 config['weightMap']['RightFoot'] = .8 else: config['weightMap']['RightUpLeg'] = .1 config['weightMap']['RightLeg'] = .25 config['weightMap']['RightFoot'] = .2 # if contact == 1 and footCenterL[1] > doubleTosingleOffset/2: if contact == 1: config['weightMap']['LeftUpLeg'] = .8 config['weightMap']['LeftLeg'] = .8 config['weightMap']['LeftFoot'] = .8 else: config['weightMap']['LeftUpLeg'] = .1 config['weightMap']['LeftLeg'] = .25 config['weightMap']['LeftFoot'] = .2 w = mot.getTrackingWeight(DOFs, motion[0].skeleton, config['weightMap']) # if contact == 2: # mot.addSoftPointConstraintTerms(problem, totalDOF, Bsc, ddP_des1, Q1, q_bias1) mot.addTrackingTerms(problem, totalDOF, Bt, w, ddth_des_flat) if dH_des is not None: 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, dJsup, dth_flat, a_sup) # mot.addConstraint(problem, totalDOF, Jsup, dJsup, dth_flat, a_sup) # if contact & 1 and contactChangeCount == 0: if True: for c_idx in range(len(contact_ids)): # mot.addConstraint(problem, totalDOF, J_contacts[c_idx], dJ_contacts[c_idx], dth_flat, a_sups[c_idx]) mot.addConstraint2(problem, totalDOF, J_contacts[c_idx], dJ_contacts[c_idx], dth_flat, a_sups[c_idx]) if contactChangeCount > 0: contactChangeCount = contactChangeCount - 1 if contactChangeCount == 0: maxContactChangeCount = 30 contactChangeType = 0 r = problem.solve() problem.clear() ddth_sol_flat = np.asarray(r['x']) # ddth_sol_flat[foot_seg_dofs] = np.array(ddth_des_flat)[foot_seg_dofs] ype.nested(ddth_sol_flat, 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) # bodyIDs, contactPositions, contactPositionLocals, contactForces, contactVelocities = vpWorld.calcManyPenaltyForce(0, bodyIDsToCheck, mus, Ks, Ds) vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals, contactForces) controlModel.setDOFAccelerations(ddth_sol) # controlModel.setDOFAccelerations(ddth_des) # controlModel.set_ddq(ddth_sol_flat) # controlModel.set_ddq(ddth_des_flat) controlModel.solveHybridDynamics() if forceShowTime > viewer.objectInfoWnd.labelForceDur.value(): forceShowTime = 0 viewer_ResetForceState() forceforce = np.array([ viewer.objectInfoWnd.labelForceX.value(), viewer.objectInfoWnd.labelForceY.value(), viewer.objectInfoWnd.labelForceZ.value() ]) extraForce[0] = getParamVal('Fm') * mm.normalize2(forceforce) if viewer_GetForceState(): forceShowTime += wcfg.timeStep vpWorld.applyPenaltyForce(selectedBodyId, localPos, extraForce) vpWorld.step() controlModel_ik.set_q(controlModel.get_q()) if foot_viewer is not None: foot_viewer.foot_pressure_gl_window.refresh_foot_contact_info( frame, vpWorld, bodyIDsToCheck, mus, Ks, Ds) foot_viewer.foot_pressure_gl_window.goToFrame(frame) # rendering for foot_seg_id in footIdlist: control_model_renderer.body_colors[foot_seg_id] = (255, 240, 255) for contact_id in contact_ids: control_model_renderer.body_colors[contact_id] = (255, 0, 0) rightFootVectorX[0] = np.dot(footOriL, np.array([.1, 0, 0])) rightFootVectorY[0] = np.dot(footOriL, np.array([0, .1, 0])) rightFootVectorZ[0] = np.dot(footOriL, np.array([0, 0, .1])) rightFootPos[0] = footCenterL rightVectorX[0] = np.dot(footBodyOriL, np.array([.1, 0, 0])) rightVectorY[0] = np.dot(footBodyOriL, np.array([0, .1, 0])) rightVectorZ[0] = np.dot(footBodyOriL, np.array([0, 0, .1])) rightPos[0] = footCenterL + np.array([.1, 0, 0]) rd_footCenter[0] = footCenter rd_footCenter_ref[0] = footCenter_ref rd_footCenterL[0] = footCenterL rd_footCenterR[0] = footCenterR rd_CM[0] = CM rd_CM_plane[0] = CM.copy() rd_CM_plane[0][1] = 0. if CP is not None and dCP is not None: rd_CP[0] = CP rd_CP_des[0] = CP_des rd_dL_des_plane[0] = [ dL_des_plane[0] / 100, dL_des_plane[1] / 100, dL_des_plane[2] / 100 ] rd_dH_des[0] = dH_des rd_grf_des[0] = dL_des_plane - totalMass * mm.s2v(wcfg.gravity) rd_root_des[0] = rootPos[0] rd_root_ori[0] = controlModel.getBodyOrientationGlobal(0) rd_root_pos[0] = controlModel.getBodyPositionGlobal(0) del rd_CF[:] del rd_CF_pos[:] for i in range(len(contactPositions)): rd_CF.append(contactForces[i] / 400) rd_CF_pos.append(contactPositions[i].copy()) if viewer_GetForceState(): rd_exfen_des[0] = [ extraForce[0][0] / 100, extraForce[0][1] / 100, extraForce[0][2] / 100 ] rd_exf_des[0] = [0, 0, 0] else: rd_exf_des[0] = [ extraForce[0][0] / 100, extraForce[0][1] / 100, extraForce[0][2] / 100 ] rd_exfen_des[0] = [0, 0, 0] extraForcePos[0] = controlModel.getBodyPositionGlobal(selectedBody) # render contact_ids # render skeleton if SKELETON_ON: Ts = dict() Ts['pelvis'] = controlModel.getJointTransform(0) Ts['thigh_R'] = controlModel.getJointTransform(1) Ts['shin_R'] = controlModel.getJointTransform(2) Ts['foot_R'] = controlModel.getJointTransform(3) Ts['spine_ribs'] = controlModel.getJointTransform(9) Ts['head'] = controlModel.getJointTransform(10) Ts['upper_limb_R'] = controlModel.getJointTransform(13) Ts['lower_limb_R'] = controlModel.getJointTransform(14) Ts['thigh_L'] = controlModel.getJointTransform(15) Ts['shin_L'] = controlModel.getJointTransform(16) Ts['foot_L'] = controlModel.getJointTransform(17) Ts['upper_limb_L'] = controlModel.getJointTransform(11) Ts['lower_limb_L'] = controlModel.getJointTransform(12) skeleton_renderer.appendFrameState(Ts) viewer.setSimulateCallback(simulateCallback) viewer.startTimer(1 / 30.) # viewer.play() viewer.show() foot_viewer = FootWindow(viewer.x() + viewer.w() + 20, viewer.y(), 300, 500, 'foot contact modifier', controlModel) foot_viewer.show() foot_viewer.check_op_l.value(True) foot_viewer.check_om_l.value(True) foot_viewer.check_h_l.value(True) foot_viewer.check_op_r.value(True) foot_viewer.check_om_r.value(True) foot_viewer.check_h_r.value(True) Fl.run()
def simulation_test(): Kt = 20. Dt = 2 * (Kt**.5) Ks = 2000. Ds = 2 * (Ks**.5) mu = 1. dir = './icmotion_last/' filename = 'stop_left_normal.temp' # filename = 'left_left_normal.temp' # filename = 'right_left_fast.temp' motion_ori = yf.readBvhFile(dir + filename) frameTime = 1 / motion_ori.fps motion_ori[0:0] = [motion_ori[0]] * 20 mcfgfile = open(dir + 'mcfg', 'rb') mcfg = pickle.load(mcfgfile) mcfgfile.close() wcfg = ypc.WorldConfig() wcfg.planeHeight = 0. wcfg.useDefaultContactModel = False # wcfg.lockingVel = c_locking_vel stepsPerFrame = 30 wcfg.timeStep = (frameTime) / stepsPerFrame vpWorld = cvw.VpWorld(wcfg) motionModel = cvm.VpMotionModel(vpWorld, motion_ori[0], mcfg) controlModel = cvm.VpControlModel(vpWorld, motion_ori[0], mcfg) vpWorld.initialize() print(controlModel) controlModel.initializeHybridDynamics() bodyIDsToCheck = range(vpWorld.getBodyNum()) mus = [mu] * len(bodyIDsToCheck) viewer = ysv.SimpleViewer() # viewer.record(False) # viewer.doc.addRenderer('motion_ori', yr.JointMotionRenderer(motion_ori, (0,100,255), yr.LINK_BONE)) viewer.doc.addObject('motion_ori', motion_ori) viewer.doc.addRenderer( 'motionModel', cvr.VpModelRenderer(motionModel, (0, 150, 255), yr.POLYGON_LINE)) viewer.doc.addRenderer( 'controlModel', cvr.VpModelRenderer(controlModel, (200, 200, 200), yr.POLYGON_LINE)) def simulateCallback(frame): th_r = motion_ori.getDOFPositions(frame) th = controlModel.getDOFPositions() dth_r = motion_ori.getDOFVelocities(frame) dth = controlModel.getDOFVelocities() ddth_r = motion_ori.getDOFAccelerations(frame) ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt) for i in range(stepsPerFrame): bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce( bodyIDsToCheck, mus, Ks, Ds) vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals, contactForces) controlModel.setDOFAccelerations(ddth_des) controlModel.solveHybridDynamics() vpWorld.step() motionModel.update(motion_ori[frame]) viewer.setSimulateCallback(simulateCallback) viewer.startTimer(frameTime / 1.4) viewer.show() Fl.run()
def main(): # np.set_printoptions(precision=4, linewidth=200) np.set_printoptions(precision=5, threshold=np.inf, suppress=True, linewidth=3000) motionFile = 'wd2_tiptoe_zygote.bvh' # motion, mcfg, wcfg, stepsPerFrame, config, frame_rate = mit.create_biped(motionFile, SEGMENT_FOOT_RAD=0.008) motion, mcfg, wcfg, stepsPerFrame, config, frame_rate = mit.create_biped(motionFile, SEGMENT_FOOT_MAG=0.01, SEGMENT_FOOT_RAD=0.008) # motion, mcfg, wcfg, stepsPerFrame, config, frame_rate = mit.create_biped() # motion, mcfg, wcfg, stepsPerFrame, config = mit.create_jump_biped() vpWorld = cvw.VpWorld(wcfg) vpWorld.SetGlobalDamping(0.999) motionModel = cvm.VpMotionModel(vpWorld, motion[0], mcfg) controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg) # controlModel_shadow_for_ik = cvm.VpControlModel(vpWorld, motion[0], mcfg) vpWorld.initialize() controlModel.initializeHybridDynamics() # controlToMotionOffset = (1.5, -0.02, 0) controlToMotionOffset = (1.5, 0., 0) controlModel.translateByOffset(controlToMotionOffset) # controlModel_shadow_for_ik.set_q(controlModel.get_q()) # controlModel_shadow_for_ik.computeJacobian(0, np.array([0., 0., 0.])) wcfg_ik = copy.deepcopy(wcfg) vpWorld_ik = cvw.VpWorld(wcfg_ik) controlModel_ik = cvm.VpControlModel(vpWorld_ik, motion[0], mcfg) vpWorld_ik.initialize() controlModel_ik.set_q(np.zeros_like(controlModel.get_q())) print('<?xml version="1.0" ?>') print('<skel version="1.0">') print('<world name="world 1">') print(' <physics>') print(' <time_step>0.001</time_step>') print(' <gravity>0 -9.81 0</gravity>') print(' <collision_detector>fcl_mesh</collision_detector>') print(' </physics>') print(' <skeleton name="grount skeleton">') print(' <mobile>false</mobile>') print(' <body name="ground">') print(' <transformation>0 -0.025 0 0 0 0</transformation>') print(' <visualization_shape name=" - visual - 0">') print(' <transformation>0.0 0.0 0.0 0.0 0.0 0.0 </transformation>') print(' <geometry>') print(' <box>') print(' <size>10000.0 0.05 10000.0</size>') print(' </box>') print(' </geometry>') print(' </visualization_shape>') print(' <collision_shape name=" - collision - 0">') print(' <transformation>0.0 0.0 0.0 0.0 0.0 0.0 </transformation>') print(' <geometry>') print(' <box>') print(' <size>100.0 0.05 100.0</size>') print(' </box>') print(' </geometry>') print(' </collision_shape>') print(' </body>') print(' <joint name="joint 1" type="free">') print(' <parent>world</parent>') print(' <child>ground</child>') print(' </joint>') print(' </skeleton>') print(' <skeleton name="dartModel">') for i in range(controlModel.getJointNum()): body_name = controlModel_ik.index2name(i) print(' <body name="'+body_name+'">') print(' <transformation>'+str(np.concatenate((controlModel_ik.getBodyPositionGlobal(i), mm.logSO3(controlModel_ik.getBodyOrientationGlobal(i)))))[1:-1]+'</transformation>') print(' <inertia>') print(' <mass>' + str(controlModel_ik.getBodyMass(i))+'</mass>') print(' <offset>'+str(controlModel_ik.getBodyLocalCom(i))[1:-1]+'</offset>') print(' </inertia>') geom_types = controlModel_ik.getBodyGeomsType(i) geom_sizes = controlModel_ik.getBodyGeomsSize(i) geom_local_frames = controlModel_ik.getBodyGeomsLocalFrame(i) geom_count = 0 for j in range(len(geom_types)): if geom_types[j] == 'B': print(' <visualization_shape name="'+body_name+' - visual shape '+str(geom_count)+'">') print(' <transformation>'+str(np.concatenate((geom_local_frames[j][:3, 3], mm.logSO3(geom_local_frames[j][:3, :3]))))[1:-1]+'</transformation>') print(' <geometry>') print(' <box>') print(' <size>'+str(geom_sizes[j])[1:-1]+'</size>') print(' </box>') print(' </geometry>') print(' </visualization_shape>') geom_count += 1 elif geom_types[j] in ('C', 'D', 'E', 'F'): print(' <visualization_shape name="'+body_name+' - visual shape '+str(geom_count)+'">') print(' <transformation>'+str(np.concatenate((geom_local_frames[j][:3, 3], mm.logSO3(geom_local_frames[j][:3, :3]))))[1:-1]+'</transformation>') print(' <geometry>') print(' <capsule>') print(' <radius>'+str(geom_sizes[j][0])+'</radius>') print(' <height>'+str(geom_sizes[j][1]-2.*geom_sizes[j][0])+'</height>') print(' </capsule>') print(' </geometry>') print(' </visualization_shape>') geom_count += 1 geom_count = 0 for j in range(len(geom_types)): if geom_types[j] == 'B': print(' <collision_shape name="'+body_name+' - collision shape '+str(geom_count)+'">') print(' <transformation>'+str(np.concatenate((geom_local_frames[j][:3, 3], mm.logSO3(geom_local_frames[j][:3, :3]))))[1:-1]+'</transformation>') print(' <geometry>') print(' <box>') print(' <size>'+str(geom_sizes[j])[1:-1]+'</size>') print(' </box>') print(' </geometry>') print(' </collision_shape>') geom_count += 1 if geom_types[j] in ('C', 'D'): print(' <collision_shape name="'+body_name+' - collision shape '+str(geom_count)+'">') print(' <transformation>'+str(np.concatenate((np.dot(geom_local_frames[j], mm.TransVToSE3((geom_sizes[j][1]/2.-geom_sizes[j][0])*mm.unitZ()))[:3, 3], mm.logSO3(geom_local_frames[j][:3, :3]))))[1:-1]+'</transformation>') print(' <geometry>') print(' <sphere>') print(' <radius>'+str(geom_sizes[j][0])+'</radius>') print(' </sphere>') print(' </geometry>') print(' </collision_shape>') geom_count += 1 if geom_types[j] in ('C', 'F'): print(' <collision_shape name="'+body_name+' - collision shape '+str(geom_count)+'">') print(' <transformation>'+str(np.concatenate((np.dot(geom_local_frames[j], mm.TransVToSE3(-(geom_sizes[j][1]/2.-geom_sizes[j][0])*mm.unitZ()))[:3, 3], mm.logSO3(geom_local_frames[j][:3, :3]))))[1:-1]+'</transformation>') print(' <geometry>') print(' <sphere>') print(' <radius>'+str(geom_sizes[j][0])+'</radius>') print(' </sphere>') print(' </geometry>') print(' </collision_shape>') geom_count += 1 print(' </body>') for i in range(controlModel.getJointNum()): name = controlModel_ik.index2name(i) inv_bone_R, inv_bone_p = controlModel_ik.getInvBoneT(i) if i == 0: print(' <joint name="j_'+name+'" type="free">') print(' <transformation>'+str(np.concatenate((inv_bone_p, mm.logSO3(inv_bone_R))))[1:-1]+'</transformation>') print(' <parent>world</parent>') print(' <child>'+name+'</child>') print(' </joint>') else: parent_name = controlModel_ik.index2name(controlModel_ik.getParentIndex(i)) print(' <joint name="j_'+name+'" type="ball">') print(' <transformation>'+str(np.concatenate((inv_bone_p, mm.logSO3(inv_bone_R))))[1:-1]+'</transformation>') print(' <parent>'+parent_name+'</parent>') print(' <child>'+name+'</child>') print(' </joint>') print(' </skeleton>') print('</world>') print('</skel>')