def __init__(self): self.episode_num = 0 motion, mcfg, wcfg, stepsPerFrame, config, frame_rate = create_biped() self.model = DeepDartModel(wcfg, motion[0], mcfg, config, True) # self.model.initializeHybridDynamics() state_num = self.model.get_deep_num_state() action_num = self.model.get_deep_num_action() # state_high = np.array([1.] * state_num) # action_high = np.array([np.finfo(np.float32).max] * action_num) state_high = np.array([np.finfo(np.float32).max] * state_num) action_high = np.array([1.4] * action_num) self.action_space = spaces.Box(-action_high, action_high) self.observation_space = spaces.Box(-state_high, state_high) self.seed() self.episode_times = np.array([0. for i in range(32)])
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=4, linewidth=1000, threshold=np.inf) #motion, mcfg, wcfg, stepsPerFrame, config = mit.create_vchain_5() motion, mcfg, wcfg, stepsPerFrame, config, frame_rate= mit.create_biped() #motion, mcfg, wcfg, stepsPerFrame, config = mit.create_jump_biped() frame_step_size = 1./frame_rate pydart.init() dartModel = cdm.DartModel(wcfg, motion[0], mcfg, DART_CONTACT_ON) dartMotionModel = cdm.DartModel(wcfg, motion[0], mcfg, DART_CONTACT_ON) # wcfg.lockingVel = 0.01 time_step = dartModel.world.time_step() # dartModel.initializeForwardDynamics() # dartModel.initializeHybridDynamics() #controlToMotionOffset = (1.5, -0.02, 0) controlToMotionOffset = (1.5, 0, 0) # dartModel.translateByOffset(controlToMotionOffset) totalDOF = dartModel.getTotalDOF() DOFs = dartModel.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']) constBody = motion[0].skeleton.getJointIndex('RightFoot') # momentum matrix linkMasses = dartModel.getBodyMasses() print([body.name for body in dartModel.skeleton.bodynodes]) print(linkMasses) totalMass = dartModel.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.)] CP_des = [None] dCP_des = [np.zeros(3)] # penalty method # bodyIDsToCheck = range(dartModel.getBodyNum()) bodyIDsToCheck = [supL, supR] #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 = 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.DartModelRenderer(dartMotionModel, (150,150,255), yr.POLYGON_FILL)) viewer.doc.addRenderer('controlModel', yr.DartRenderer(dartModel.world, (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,0,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 = 11. # initKh = 22. initKl = 100. initKh = 100. initBl = .1 initBh = .13 initSupKt = 17. # initSupKt = 2.5 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) pdcontroller = PDController(dartModel, dartModel.skeleton, wcfg.timeStep, Kt, Dt) def getParamVal(paramname): return viewer.objectInfoWnd.getVal(paramname) def getParamVals(paramnames): return (getParamVal(name) for name in paramnames) ik_solver = hikd.numIkSolver(dartMotionModel) body_num = dartModel.getBodyNum() # dJsys = np.zeros((6*body_num, totalDOF)) # dJsupL = np.zeros((6, totalDOF)) # dJsupR = np.zeros((6, totalDOF)) # Jpre = [np.zeros((6*body_num, totalDOF)), np.zeros((6, totalDOF)), np.zeros((6, totalDOF))] extendedFootName = ['Foot'] 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} lIDlist = [motion[0].skeleton.getJointIndex('Left'+name) for name in extendedFootName] rIDlist = [motion[0].skeleton.getJointIndex('Right'+name) for name in extendedFootName] bodyIDs, contactPositions, contactPositionLocals, contactForces = [], [], [], [] ################################### # simulate ################################### def simulateCallback(frame): # print() # print(dartModel.getJointVelocityGlobal(0)) # print(dartModel.getDOFVelocities()[0]) # print(dartModel.get_dq()[:6]) dartMotionModel.update(motion[frame]) global g_initFlag global forceShowTime global preFootCenter global maxContactChangeCount global contactChangeCount global contact global contactChangeType # print('contactstate:', contact, contactChangeCount) Kt, Kl, Kh, Bl, Bh, kt_sup = getParamVals(['Kt', 'Kl', 'Kh', 'Bl', 'Bh', 'SupKt']) Dt = 2.*(Kt**.5) Dl = (Kl**.5) Dh = (Kh**.5) dt_sup = 2.*(kt_sup**.5) # Dt = .2*(Kt**.5) # Dl = .2*(Kl**.5) # Dh = .2*(Kh**.5) # dt_sup = .2*(kt_sup**.5) pdcontroller.setKpKd(Kt, Dt) footHeight = dartModel.getBody(supL).shapenodes[0].shape.size()[1]/2. doubleTosingleOffset = 0.15 singleTodoubleOffset = 0.30 #doubleTosingleOffset = 0.09 doubleTosingleVelOffset = 0.0 com_offset_x, com_offset_y, com_offset_z = getParamVals(['com X offset', 'com Y offset', 'com Z offset']) footOffset = np.array((com_offset_x, com_offset_y, com_offset_z)) des_com = dartMotionModel.getCOM() + footOffset footCenterL = dartMotionModel.getBodyPositionGlobal(supL) footCenterR = dartMotionModel.getBodyPositionGlobal(supR) footBodyOriL = dartMotionModel.getBodyOrientationGlobal(supL) footBodyOriR = dartMotionModel.getBodyOrientationGlobal(supR) torso_pos = dartMotionModel.getBodyPositionGlobal(4) torso_ori = dartMotionModel.getBodyOrientationGlobal(4) # tracking # th_r = motion.getDOFPositions(frame) th_r = dartMotionModel.getDOFPositions() th = dartModel.getDOFPositions() th_r_flat = dartMotionModel.get_q() # dth_r = motion.getDOFVelocities(frame) dth = dartModel.getDOFVelocities() # ddth_r = motion.getDOFAccelerations(frame) # ddth_des = yct.getDesiredDOFAccelerations(th_r, th, None, dth, None, Kt, Dt) dth_flat = dartModel.get_dq() # dth_flat = np.concatenate(dth) # ddth_des_flat = pdcontroller.compute(dartMotionModel.get_q()) # ddth_des_flat = pdcontroller.compute(th_r) ddth_des_flat = pdcontroller.compute_flat(th_r_flat) # ype.flatten(ddth_des, ddth_des_flat) # ype.flatten(dth, dth_flat) ################################################# # jacobian ################################################# contact_des_ids = [dartModel.skeleton.bodynode_index("LeftFoot")] contact_ids = list() # temp idx for balancing contact_ids.extend(contact_des_ids) contact_joint_ori = list(map(dartModel.getJointOrientationGlobal, contact_ids)) contact_joint_pos = list(map(dartModel.getJointPositionGlobal, contact_ids)) contact_body_ori = list(map(dartModel.getBodyOrientationGlobal, contact_ids)) contact_body_pos = list(map(dartModel.getBodyPositionGlobal, contact_ids)) contact_body_vel = list(map(dartModel.getBodyVelocityGlobal, contact_ids)) contact_body_angvel = list(map(dartModel.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(dartMotionModel.getBodyOrientationGlobal, contact_ids)) ref_body_pos = list(map(dartMotionModel.getBodyPositionGlobal, contact_ids)) for idx in range(len(ref_body_pos)): ref_body_pos[idx] = dartModel.skeleton.body("RightFoot").shapenodes[0].shape.size()[1]/2. # 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 footOriL = dartModel.getJointOrientationGlobal(supL) footOriR = dartModel.getJointOrientationGlobal(supR) footCenterL = dartModel.getBodyPositionGlobal(supL) footCenterR = dartModel.getBodyPositionGlobal(supR) footBodyOriL = dartModel.getBodyOrientationGlobal(supL) footBodyOriR = dartModel.getBodyOrientationGlobal(supR) footBodyVelL = dartModel.getBodyVelocityGlobal(supL) footBodyVelR = dartModel.getBodyVelocityGlobal(supR) footBodyAngVelL = dartModel.getBodyAngVelocityGlobal(supL) footBodyAngVelR = dartModel.getBodyAngVelocityGlobal(supR) refFootL = dartMotionModel.getBodyPositionGlobal(supL) refFootR = dartMotionModel.getBodyPositionGlobal(supR) # refFootAngVelL = motion.getJointAngVelocityGlobal(supL, frame) # refFootAngVelR = motion.getJointAngVelocityGlobal(supR, frame) refFootAngVelL = np.zeros(3) refFootAngVelR = np.zeros(3) refFootJointVelR = motion.getJointVelocityGlobal(supR, frame) refFootJointAngVelR = motion.getJointAngVelocityGlobal(supR, frame) refFootJointR = motion.getJointPositionGlobal(supR, frame) # refFootVelR = refFootJointVelR + np.cross(refFootJointAngVelR, (refFootR-refFootJointR)) refFootVelR = np.zeros(3) refFootJointVelL = motion.getJointVelocityGlobal(supL, frame) refFootJointAngVelL = motion.getJointAngVelocityGlobal(supL, frame) refFootJointL = motion.getJointPositionGlobal(supL, frame) # refFootVelL = refFootJointVelL + np.cross(refFootJointAngVelL, (refFootL-refFootJointL)) refFootVelL = np.zeros(3) contactR = 1 contactL = 1 # contMotionOffset = th[0][0] - th_r[0][0] contMotionOffset = dartModel.getBodyPositionGlobal(0) - dartMotionModel.getBodyPositionGlobal(0) contMotionOffset = np.zeros(3) linkPositions = dartModel.getBodyPositionsGlobal() linkVelocities = dartModel.getBodyVelocitiesGlobal() linkAngVelocities = dartModel.getBodyAngVelocitiesGlobal() linkInertias = dartModel.getBodyInertiasGlobal() CM = dartModel.skeleton.com() dCM = dartModel.skeleton.com_velocity() 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 body_num = dartModel.getBodyNum() Jsys = np.zeros((6*body_num, totalDOF)) dJsys = np.zeros((6*body_num, totalDOF)) Jsys_, dJsysdq = compute_J_dJdq(dartModel.skeleton) # dJsys = np.zeros((6*body_num, totalDOF)) for i in range(dartModel.getBodyNum()): Jsys[6*i:6*i+6, :] = dartModel.getBody(i).world_jacobian()[range(-3, 3), :] dJsys[6*i:6*i+6, :] = dartModel.getBody(i).world_jacobian_classic_deriv()[range(-3, 3), :] dJsysdq = np.dot(dJsys, dartModel.skeleton.dq) # print(Jsys_ - Jsys) # print(Jsys_.dot(dth_flat)) # print(Jsys.dot(dth_flat)) # print(dartModel.getBody(0).world_linear_velocity()) # print(np.dot(Jsys[:3, :3], Jsys[0:3, 3:6].T)) print('dq', np.asarray(dartModel.skeleton.dq)[6:9]) print('joint vel', dartModel.skeleton.joint(1).velocity()) # print('bjoint', mm.exp(dartModel.skeleton.q[6:9]).dot(get_bjoint_jacobian(dartModel.skeleton.q[6:9]).dot(np.asarray(dartModel.skeleton.dq)[6:9]))) # print('not bjoint', mm.exp(dartModel.skeleton.q[6:9]).dot(np.asarray(dartModel.skeleton.dq)[6:9])) print('frombody', dartModel.getJointOrientationGlobal(1).T.dot(dartModel.getJointAngVelocityGlobal(1) - dartModel.getJointAngVelocityGlobal(0))) print('ddq', np.asarray(dartModel.skeleton.ddq)[6:9]) # print('bjoint', mm.exp(dartModel.skeleton.q[6:9]).dot(get_bjoint_jacobian(dartModel.skeleton.q[6:9]).dot(np.asarray(dartModel.skeleton.dq)[6:9]))) # print('not bjoint', mm.exp(dartModel.skeleton.q[6:9]).dot(np.asarray(dartModel.skeleton.dq)[6:9])) bodybody = dartModel.skeleton.body(1) joint_trans = dartModel.skeleton.joint(1).get_world_frame_after_transform() joint_pos = bodybody.to_local(joint_trans[:3, 3]) print('com spati', dartModel.getJointOrientationGlobal(1).T.dot( dartModel.skeleton.body(1).world_angular_acceleration() - dartModel.skeleton.body(0).world_angular_acceleration())) 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(dJsysdq[6*contact_id:6*contact_id + 6]) #calculate footCenter footCenter = .5 * (footCenterL + footCenterR) + footOffset if contact == 2: footCenter = footCenterL.copy() + footOffset #elif contact == 1 or footCenterL[1] > doubleTosingleOffset/2: if contact == 1: footCenter = footCenterR.copy() + footOffset footCenter[1] = 0. if contactChangeCount > 0 and contactChangeType == 'StoD': # change footcenter gradually footCenter = preFootCenter + (maxContactChangeCount - contactChangeCount)*(footCenter-preFootCenter)/maxContactChangeCount preFootCenter = footCenter.copy() # linear momentum # CM_ref_plane = footCenter # dL_des_plane = Kl*totalMass*(CM_ref_plane - CM_plane) - Dl*totalMass*dCM_plane # dL_des_plane[1] = 0. CM_ref_plane = footCenter CM_ref_plane[1] = dartMotionModel.skeleton.com()[1] dL_des_plane = Kl*totalMass*(CM_ref_plane - CM) - Dl*totalMass*dCM # dL_des_plane[1] = 0. # angular momentum CP_ref = footCenter CP = yrp.getCP(contactPositions, contactForces) if CP_old[0] is None or CP is None: dCP = None else: dCP = (CP - CP_old[0])/frame_step_size CP_old[0] = CP CP_des[0] = None if CP is not None and dCP is not None: ddCP_des = Kh*(CP_ref - CP) - Dh*(dCP) CP_des[0] = CP + dCP * frame_step_size + .5 * ddCP_des*(frame_step_size**2) dH_des = mm.cross(CP_des[0] - CM, dL_des_plane + totalMass*mm.s2v(wcfg.gravity)) else: dH_des = None # set up equality constraint a_oris = list(map(mm.logSO3, [mm.getSO3FromVectors(np.dot(contact_body_ori[i], mm.unitY()), mm.unitY()) for i in range(len(contact_body_ori))])) KT_SUP = np.diag([kt_sup/10., kt_sup, kt_sup/10.]) 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))] # print(a_sups) # print(np.asarray(dartModel.skeleton.dq)[0:3]) # print(dartModel.getJointAngVelocityGlobal(0)) # print(dartModel.getJointAngVelocityLocal(0)) # 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, dJsysdq) 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 # print('vel2', np.dot(dartModel.getJointOrientationGlobal(0).T, dartModel.skeleton.body(0).world_linear_velocity())) 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) mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R, r_bias) if dH_des is not None: 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], a_sups[c_idx]) if contactChangeCount > 0: contactChangeCount -= 1 if contactChangeCount == 0: maxContactChangeCount = 30 contactChangeType = 0 r = problem.solve() problem.clear() ddth_sol_flat = np.asarray(r['x']) # ype.nested(r['x'], ddth_sol) # ddth_sol[:6] = np.zeros(6) rootPos[0] = dartModel.getBodyPositionGlobal(selectedBody) localPos = [[0, 0, 0]] inv_h = 1./time_step _bodyIDs, _contactPositions, _contactPositionLocals, _contactForces = [], [], [], [] for i in range(stepsPerFrame): # apply penalty force _ddq, _tau, _bodyIDs, _contactPositions, _contactPositionLocals, _contactForces = hqp.calc_QP(dartModel.skeleton, ddth_sol_flat, inv_h) # _bodyIDs, _contactPositions, _contactPositionLocals, _contactForces = dartModel.calcPenaltyForce(bodyIDsToCheck,mus, Ks, Ds) dartModel.applyPenaltyForce(_bodyIDs, _contactPositionLocals, _contactForces) dartModel.skeleton.set_forces(_tau) # dartModel.setDOFAccelerations(ddth_sol) 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 dartModel.applyPenaltyForce(selectedBodyId, localPos, extraForce) dartModel.step() del bodyIDs[:] del contactPositions[:] del contactPositionLocals[:] del contactForces[:] bodyIDs.extend(_bodyIDs) contactPositions.extend(_contactPositions) contactPositionLocals.extend(_contactPositionLocals) contactForces.extend(_contactForces) # 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[0] 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]/100) 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] = dartModel.getBodyPositionGlobal(selectedBody) viewer.setSimulateCallback(simulateCallback) viewer.startTimer(1/30.) viewer.show() Fl.run()
def main(): np.set_printoptions(precision=4, linewidth=200) # np.set_printoptions(precision=4, linewidth=1000, threshold=np.inf) #motion, mcfg, wcfg, stepsPerFrame, config = mit.create_vchain_5() motion, mcfg, wcfg, stepsPerFrame, config, frame_rate= mit.create_biped() #motion, mcfg, wcfg, stepsPerFrame, config = mit.create_jump_biped() frame_step_size = 1./frame_rate pydart.init() dartModel = cdm.DartModel(wcfg, motion[0], mcfg, DART_CONTACT_ON) dartMotionModel = cdm.DartModel(wcfg, motion[0], mcfg, DART_CONTACT_ON) # wcfg.lockingVel = 0.01 # dartModel.initializeHybridDynamics() if not DART_CONTACT_ON: dartModel.initContactPoint() #controlToMotionOffset = (1.5, -0.02, 0) # controlToMotionOffset = (1.5, 0, 0) # dartModel.translateByOffset(controlToMotionOffset) totalDOF = dartModel.getTotalDOF() DOFs = dartModel.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']) constBody = motion[0].skeleton.getJointIndex('RightFoot') # momentum matrix linkMasses = dartModel.getBodyMasses() totalMass = dartModel.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.)] CP_des = [None] dCP_des = [np.zeros(3)] # penalty method # bodyIDsToCheck = range(dartModel.getBodyNum()) bodyIDsToCheck = [supL, supR] #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', cvr.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', cvr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_FILL)) # viewer.doc.addRenderer('motionModel', yr.DartModelRenderer(dartMotionModel, (150,150,255), yr.POLYGON_FILL)) # viewer.doc.addRenderer('controlModel', yr.DartModelRenderer(dartModel, (255,240,255), yr.POLYGON_FILL)) viewer.doc.addRenderer('controlModel', yr.DartRenderer(dartModel.world, (150,150,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,0,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 = 11. # initKh = 22. initKl = 100. initKh = 100. initBl = .1 initBh = .13 initSupKt = 17. # initSupKt = 2.5 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.reset = False def viewer_reset(object): viewer.reset = True viewer.objectInfoWnd.addBtn('Force on', viewer_SetForceState) viewer.objectInfoWnd.addBtn('reset', viewer_reset) 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) pdcontroller = PDController(dartModel, dartModel.skeleton, wcfg.timeStep, Kt, Dt, config['PDweightMap']) def getParamVal(paramname): return viewer.objectInfoWnd.getVal(paramname) def getParamVals(paramnames): return (getParamVal(name) for name in paramnames) ik_solver = hikd.numIkSolver(dartMotionModel) body_num = dartModel.getBodyNum() # dJsys = np.zeros((6*body_num, totalDOF)) # dJsupL = np.zeros((6, totalDOF)) # dJsupR = np.zeros((6, totalDOF)) # Jpre = [np.zeros((6*body_num, totalDOF)), np.zeros((6, totalDOF)), np.zeros((6, totalDOF))] ################################### #simulate ################################### def simulateCallback(frame): print(frame) # print() # print(dartModel.getJointVelocityGlobal(0)) # print(dartModel.getDOFVelocities()[0]) # print(dartModel.get_dq()[:6]) dartMotionModel.update(motion[frame]) global g_initFlag global forceShowTime global preFootCenter global maxContactChangeCount global contactChangeCount global contact global contactChangeType # print('contactstate:', contact, contactChangeCount) Kt, Kl, Kh, Bl, Bh, kt_sup = getParamVals(['Kt', 'Kl', 'Kh', 'Bl', 'Bh', 'SupKt']) # Dt = 2.*(Kt**.5) Dt = Kt/100. Dl = (Kl**.5) Dh = (Kh**.5) dt_sup = 2.*(kt_sup**.5) # Dt = .2*(Kt**.5) # Dl = .2*(Kl**.5) # Dh = .2*(Kh**.5) # dt_sup = .2*(kt_sup**.5) pdcontroller.setKpKd(Kt, Dt) footHeight = dartModel.getBody(supL).shapenodes[0].shape.size()[1]/2. doubleTosingleOffset = 0.15 singleTodoubleOffset = 0.30 #doubleTosingleOffset = 0.09 doubleTosingleVelOffset = 0.0 com_offset_x, com_offset_y, com_offset_z = getParamVals(['com X offset', 'com Y offset', 'com Z offset']) footOffset = np.array((com_offset_x, com_offset_y, com_offset_z)) des_com = dartMotionModel.getCOM() + footOffset footCenterL = dartMotionModel.getBodyPositionGlobal(supL) footCenterR = dartMotionModel.getBodyPositionGlobal(supR) footBodyOriL = dartMotionModel.getBodyOrientationGlobal(supL) footBodyOriR = dartMotionModel.getBodyOrientationGlobal(supR) torso_pos = dartMotionModel.getBodyPositionGlobal(4) torso_ori = dartMotionModel.getBodyOrientationGlobal(4) # ik_solver.setInitPose(motion[frame]) # ik_solver.addConstraints(supL, np.zeros(3), footCenterL, footBodyOriL, (True, True, True, True)) # ik_solver.addConstraints(supR, np.zeros(3), footCenterR, footBodyOriR, (True, True, True, True)) # ik_solver.addConstraints(4, np.zeros(3), torso_pos, torso_ori, (False, False, False, True)) # ik_solver.solve(des_com) # ik_solver.clear() # tracking # th_r = motion.getDOFPositions(frame) th_r = dartMotionModel.getDOFPositions() th = dartModel.getDOFPositions() th_r_flat = dartMotionModel.get_q() # dth_r = motion.getDOFVelocities(frame) # dth = dartModel.getDOFVelocities() # ddth_r = motion.getDOFAccelerations(frame) # ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt) dth_flat = dartModel.get_dq() # dth_flat = np.concatenate(dth) # ddth_des_flat = pdcontroller.compute(dartMotionModel.get_q()) # ddth_des_flat = pdcontroller.compute(th_r) ddth_des_flat = pdcontroller.compute_flat(th_r_flat) # ype.flatten(ddth_des, ddth_des_flat) # ype.flatten(dth, dth_flat) ################################################# # jacobian ################################################# footOriL = dartModel.getJointOrientationGlobal(supL) footOriR = dartModel.getJointOrientationGlobal(supR) footCenterL = dartModel.getBodyPositionGlobal(supL) footCenterR = dartModel.getBodyPositionGlobal(supR) footBodyOriL = dartModel.getBodyOrientationGlobal(supL) footBodyOriR = dartModel.getBodyOrientationGlobal(supR) footBodyVelL = dartModel.getBodyVelocityGlobal(supL) footBodyVelR = dartModel.getBodyVelocityGlobal(supR) footBodyAngVelL = dartModel.getBodyAngVelocityGlobal(supL) footBodyAngVelR = dartModel.getBodyAngVelocityGlobal(supR) refFootL = dartMotionModel.getBodyPositionGlobal(supL) refFootR = dartMotionModel.getBodyPositionGlobal(supR) # refFootAngVelL = motion.getJointAngVelocityGlobal(supL, frame) # refFootAngVelR = motion.getJointAngVelocityGlobal(supR, frame) refFootAngVelL = np.zeros(3) refFootAngVelR = np.zeros(3) refFootJointVelR = motion.getJointVelocityGlobal(supR, frame) refFootJointAngVelR = motion.getJointAngVelocityGlobal(supR, frame) refFootJointR = motion.getJointPositionGlobal(supR, frame) # refFootVelR = refFootJointVelR + np.cross(refFootJointAngVelR, (refFootR-refFootJointR)) refFootVelR = np.zeros(3) refFootJointVelL = motion.getJointVelocityGlobal(supL, frame) refFootJointAngVelL = motion.getJointAngVelocityGlobal(supL, frame) refFootJointL = motion.getJointPositionGlobal(supL, frame) # refFootVelL = refFootJointVelL + np.cross(refFootJointAngVelL, (refFootL-refFootJointL)) refFootVelL = np.zeros(3) contactR = 1 contactL = 1 if refFootVelR[1] < 0 and refFootVelR[1]*frame_step_size + refFootR[1] > singleTodoubleOffset: contactR = 0 if refFootVelL[1] < 0 and refFootVelL[1]*frame_step_size + refFootL[1] > singleTodoubleOffset: contactL = 0 if refFootVelR[1] > 0 and refFootVelR[1]*frame_step_size + refFootR[1] > doubleTosingleOffset: contactR = 0 if refFootVelL[1] > 0 and refFootVelL[1]*frame_step_size + refFootL[1] > doubleTosingleOffset: contactL = 0 # contactR = 0 # contMotionOffset = th[0][0] - th_r[0][0] contMotionOffset = dartModel.getBodyPositionGlobal(0) - dartMotionModel.getBodyPositionGlobal(0) linkPositions = dartModel.getBodyPositionsGlobal() linkVelocities = dartModel.getBodyVelocitiesGlobal() linkAngVelocities = dartModel.getBodyAngVelocitiesGlobal() linkInertias = dartModel.getBodyInertiasGlobal() CM = dartModel.skeleton.com() dCM = dartModel.skeleton.com_velocity() 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: softConstPoint = footCenterR.copy() 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 if refFootR[1] < footHeight: contact +=1 if refFootL[1] < footHeight: contact +=2 g_initFlag = 1 #calculate jacobian body_num = dartModel.getBodyNum() Jsys = np.zeros((6*body_num, totalDOF)) dJsys = np.zeros((6*body_num, totalDOF)) for i in range(dartModel.getBodyNum()): # body_i_jacobian = dartModel.getBody(i).world_jacobian()[range(-3, 3), :] # body_i_jacobian_deriv = dartModel.getBody(i).world_jacobian_classic_deriv()[range(-3, 3), :] # Jsys[6*i:6*i+6, :] = body_i_jacobian # dJsys[6*i:6*i+6, :] = body_i_jacobian_deriv Jsys[6*i:6*i+6, :] = dartModel.getBody(i).world_jacobian()[range(-3, 3), :] dJsys[6*i:6*i+6, :] = dartModel.getBody(i).world_jacobian_classic_deriv()[range(-3, 3), :] # dJsys = (Jsys - Jpre[0])/frame_step_size # Jpre[0] = Jsys.copy() JsupL = dartModel.getBody(supL).world_jacobian()[range(-3, 3), :] dJsupL = dartModel.getBody(supL).world_jacobian_classic_deriv()[range(-3, 3), :] # dJsupL = np.zeros_like(JsupL) # dJsupL = (JsupL - Jpre[1])/frame_step_size # Jpre[1] = JsupL.copy() JsupR = dartModel.getBody(supR).world_jacobian()[range(-3, 3), :] dJsupR = dartModel.getBody(supR).world_jacobian_classic_deriv()[range(-3, 3), :] # dJsupR = np.zeros_like(JsupR) # dJsupR = (JsupR - Jpre[2])/frame_step_size # Jpre[2] = JsupR.copy() #calculate footCenter footCenter = .5 * (footCenterL + footCenterR) + footOffset #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() + footOffset #elif contact == 1 or footCenterL[1] > doubleTosingleOffset/2: if contact == 1: footCenter = footCenterR.copy() + footOffset 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. # CM_ref = footCenter.copy() # CM_ref[1] = dartMotionModel.getCOM()[1] # CM_ref += np.array((0., com_offset_y, 0.)) # dL_des_plane = Kl*totalMass*(CM_ref - CM) - Dl*totalMass*dCM # angular momentum CP_ref = footCenter bodyIDs, contactPositions, contactPositionLocals, contactForces = [], [], [], [] if DART_CONTACT_ON: bodyIDs, contactPositions, contactPositionLocals, contactForces = dartModel.get_dart_contact_info() else: bodyIDs, contactPositions, contactPositionLocals, contactForces = dartModel.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])/frame_step_size CP_old[0] = CP CP_des[0] = None # if CP_des[0] is None: # CP_des[0] = footCenter if CP is not None and dCP is not None: ddCP_des = Kh*(CP_ref - CP) - Dh*(dCP) CP_des[0] = CP + dCP * frame_step_size + .5 * ddCP_des*(frame_step_size**2) # dCP_des[0] += ddCP_des * frame_step_size # CP_des[0] += dCP_des[0] * frame_step_size + .5 * ddCP_des*(frame_step_size ** 2) dH_des = np.cross(CP_des[0] - 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]))) footErrorL = refFootL.copy() footErrorL[1] = dartModel.getBody(supL).shapenodes[0].shape.size()[1]/2. footErrorL += -footCenterL + contMotionOffset footErrorR = refFootR.copy() footErrorR[1] = dartModel.getBody(supR).shapenodes[0].shape.size()[1]/2. footErrorR += -footCenterR + contMotionOffset a_supL = np.append(kt_sup*footErrorL + dt_sup*(refFootVelL - footBodyVelL), kt_sup*a_oriL+dt_sup*(refFootAngVelL-footBodyAngVelL)) a_supR = np.append(kt_sup*footErrorR + dt_sup*(refFootVelR - footBodyVelR), kt_sup*a_oriR+dt_sup*(refFootAngVelR-footBodyAngVelR)) if contactChangeCount > 0 and contactChangeType == 'DtoS': 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': linkt = (13.*contactChangeCount)/(maxContactChangeCount)+1. lindt = 2*(linkt**.5) angkt = (13.*contactChangeCount)/(maxContactChangeCount)+1. angdt = 2*(angkt**.5) 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)) # 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) ####################################################### # 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) mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R, r_bias) if dH_des is not None: 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 contact & 1: #if refFootR[1] < doubleTosingleOffset: mot.addConstraint(problem, totalDOF, JsupR, dJsupR, dth_flat, a_supR) if contact & 2: #if refFootL[1] < doubleTosingleOffset: mot.addConstraint(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) ddth_sol = np.asarray(r['x']) # ddth_sol[:6] = np.zeros(6) rootPos[0] = dartModel.getBodyPositionGlobal(selectedBody) localPos = [[0, 0, 0]] for i in range(stepsPerFrame): # apply penalty force if not DART_CONTACT_ON: bodyIDs, contactPositions, contactPositionLocals, contactForces = dartModel.calcPenaltyForce(bodyIDsToCheck, mus, Ks, Ds) dartModel.applyPenaltyForce(bodyIDs, contactPositionLocals, contactForces) #bodyIDs, contactPositions, contactPositionLocals, contactForces, contactVelocities = vpWorld.calcManyPenaltyForce(0, bodyIDsToCheck, mus, Ks, Ds) # dartModel.skeleton.set_accelerations(ddth_sol) # dartModel.skeleton.set_accelerations(ddth_des_flat) # dartModel.skeleton.set_forces(np.zeros(totalDOF)) # ddth_des_flat[:6] = np.zeros(6) th_r_flat = dartMotionModel.get_q() ddth_des_flat = pdcontroller.compute_flat(th_r_flat) dartModel.skeleton.set_forces(ddth_des_flat) 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 dartModel.applyPenaltyForce(selectedBodyId, localPos, extraForce) dartModel.step() if DART_CONTACT_ON: bodyIDs, contactPositions, contactPositionLocals, contactForces = dartModel.get_dart_contact_info() else: bodyIDs, contactPositions, contactPositionLocals, contactForces = dartModel.calcPenaltyForce(bodyIDsToCheck, mus, Ks, Ds) if viewer.reset: viewer.reset = False dartModel.reset() # print(dartModel.getCOM()) # 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[0] 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]/100) 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] = dartModel.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() vpWorld = cvw.VpWorld(wcfg) motionModel = cvm.VpMotionModel(vpWorld, motion[0], mcfg) controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg) #controlModel2 = cvm.VpControlModel(vpWorld, motion[0], mcfg) vpWorld.initialize() controlModel.initializeHybridDynamics() controlModel.translateByOffset((1.5, 0, 0)) #controlModel2.initializeHybridDynamics() #controlModel2.translateByOffset((2.5,0,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']) sup = motion[0].skeleton.getJointIndex(config['supLink']) sup2 = motion[0].skeleton.getJointIndex(config['supLink2']) # jacobian Jsup = yjc.makeEmptyJacobian(DOFs, 1) dJsup = Jsup.copy() Jsys = yjc.makeEmptyJacobian(DOFs, controlModel.getBodyNum()) dJsys = Jsys.copy() supJointMasks = [yjc.getLinkJointMask(motion[0].skeleton, sup)] allLinkJointMasks = yjc.getAllLinkJointMasks(motion[0].skeleton) Jsup2 = yjc.makeEmptyJacobian(DOFs, 1) dJsup2 = Jsup2.copy() supJointMasks2 = [yjc.getLinkJointMask(motion[0].skeleton, sup2)] CMJointMask = supJointMasks for iMask in range(len(CMJointMask[0])): if supJointMasks2[0][iMask] == 1: CMJointMask[0][iMask] = 1 Jt = yjc.makeEmptyJacobian(DOFs, 1) # 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) CP_old = [mm.v3(0., 0., 0.)] # penalty method bodyIDsToCheck = range(vpWorld.getBodyNum()) mus = [1.] * len(bodyIDsToCheck) ##### Parameter ''' (0, 'Hips') (1, 'LeftUpLeg') (2, 'LeftLeg') (3, 'LeftFoot') (4, 'RightUpLeg') (5, 'RightLeg') (6, 'RightFoot') (7, 'Spine') (8, 'Spine1') (9, 'LeftArm') (10, 'LeftForeArm') (11, 'RightArm') (12, 'RightForeArm') ''' Kp = [None] * len(bodyIDsToCheck) Kd = [None] * len(bodyIDsToCheck) #Kp = [0, 400, 450, 400, 400, 450, 400, 300, 250, 60, 25, 60, 25] #Kp = [0, 450, 450, 400, 450, 450, 400, 450, 300, 100, 25, 100, 25] UpLeg = 310 Leg = 400 Foot = 310 Spine = 350 Neck = 200 Arm = 10 Hand = 10 Kp = [ 0, UpLeg, Leg, Foot, UpLeg, Leg, Foot, Spine, Neck, Arm, Hand, Arm, Hand ] ''' for i in range(0, len(Kp)) : Kp[i] = Kp[i]*0.8 ''' #Kd = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] #Kd = [0, 2.0, 1.5, 1.2, 2.0, 1.5, 1.2, 2.0, 0.1, 0.1, 0.1, 0.1, 0.1] #Kd = [0, 1.5, 2.0, 1.2, 1.5, 2.0, 1.2, 1.5, 0.4, 0.5, 0.2, 0.5, 0.2] ''' dUpLeg = 1.7 dLeg = 1.7 dFoot = 1.2 dSpine = 1.4 dNeck = 0.9 dArm = 0.8 dHand = 0.2 ''' dUpLeg = 2 * (UpLeg**.5) dLeg = 2 * (Leg**.5) dFoot = 2 * (Foot**.5) dSpine = 2 * (Spine**.5) dNeck = 2 * (Neck**.5) dArm = 2 * (Arm**.5) dHand = 2 * (Hand**.5) Kd = [ 0, dUpLeg, dLeg, dFoot, dUpLeg, dLeg, dFoot, dSpine, dNeck, dArm, dHand, dArm, dHand ] for i in range(0, len(Kd)): Kd[i] = Kd[i] * 0.022 ''' for ii in bodyIDsToCheck : print(ii, controlModel.index2name(ii)) ''' # flat data structure ddth_des_flat = ype.makeFlatList(totalDOF) dth_flat = ype.makeFlatList(totalDOF) ddth_sol = ype.makeNestedList(DOFs) # viewer rd_footCenter = [None] rd_footCenter1 = [None] rd_footCenter2 = [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_vf = [None] rd_contactPoint1 = [None] rd_contactPoint2 = [None] viewer = ysv.SimpleViewer() # viewer.record(False) # viewer.doc.addRenderer('control', 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('controlModel2', cvr.VpModelRenderer(controlModel2, (155,100,100), yr.POLYGON_FILL)) #viewer.doc.addRenderer('rd_footCenter', yr.PointsRenderer(rd_footCenter)) #viewer.doc.addRenderer('rd_footCenter1', yr.PointsRenderer(rd_footCenter1, (255, 0, 255))) #viewer.doc.addRenderer('rd_footCenter2', yr.PointsRenderer(rd_footCenter2, (0, 255, 255))) #viewer.doc.addRenderer('rd_CM_plane', yr.PointsRenderer(rd_CM_plane, (255,255,0))) #viewer.doc.addRenderer('rd_CM', yr.PointsRenderer(rd_CM, (255,255,0))) #viewer.doc.addRenderer('rd_contactPoint', yr.PointsRenderer(rd_contactPoint1, (0,0,255))) #viewer.doc.addRenderer('rd_contactPoint2', yr.PointsRenderer(rd_contactPoint2, (0,255,255))) #viewer.doc.addRenderer('rd_CP', yr.PointsRenderer(rd_CP, (255,0,255))) # 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_vf', yr.ForcesRenderer(rd_vf, rd_CM, (0, 0, 255), .005)) global stage stage = 0 def simulateCallback(frame): motionModel.update(motion[frame]) # 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) ''' th2 = controlModel2.getDOFPositions() dth2 = controlModel2.getDOFVelocities() ddth_des2 = yct.getDesiredDOFAccelerations(th_r, th2, dth_r, dth2, ddth_r, Kt, Dt) ''' #ype.flatten(ddth_des, ddth_des_flat) #ype.flatten(dth, dth_flat) #Control #tracking control #print(Kt, Dt) #Tpd = yct.getDesiredDOFTorques(th_r, th, dth_r, dth, 100.0, 0.1)#0.65, 0.031) 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) footCenter1 = controlModel.getBodyPositionGlobal(sup) footCenter2 = controlModel.getBodyPositionGlobal(sup2) footCenter = (footCenter1 + footCenter2) / 2 ''' yjc.computeJacobian2(Jt, DOFs, jointPositions, jointAxeses, [CM], CMJointMask) pHatCom = CM - footCenter vCom = dCM ''' CM_plane = copy.copy(CM) CM_plane[1] = 0. dCM_plane = copy.copy(dCM) dCM_plane[1] = 0. CM_ref_plane = footCenter dL_des_plane = Kl * totalMass * (CM_ref_plane - CM_plane) - Dl * totalMass * dCM_plane dL_des_plane[1] = 0. CP_ref = footCenter bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce( bodyIDsToCheck, mus, Ks, Ds) CP = yrp.getCP(contactPositions, contactForces) if CP_old[0] == None or CP == None: dCP = None else: dCP = (CP - CP_old[0]) / (1 / 30.) CP_old[0] = CP if CP != None and dCP != 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))) else: dH_des = None ################ ''' linkPositions = motionModel.getBodyPositionsGlobal() linkVelocities = motionModel.getBodyVelocitiesGlobal() linkAngVelocities = motionModel.getBodyAngVelocitiesGlobal() linkInertias = motionModel.getBodyInertiasGlobal() CM2 = yrp.getCM(linkPositions, linkMasses, totalMass) dCM2 = yrp.getCM(linkVelocities, linkMasses, totalMass) footCenter1 = motionModel.getBodyPositionGlobal(sup) footCenter2 = motionModel.getBodyPositionGlobal(sup2) footCenter = (footCenter1+footCenter2)/2 pHatComDes = CM2 - footCenter vComDes = dCM2 Wcp = -750 Wcv = -10 fCom = Wcp*(pHatComDes - pHatCom) + Wcv*(vComDes - vCom) ''' #print("VirtualForce", fCom) #fCom[0] = 0. #fCom[1] = 0. #fCom[2] = -20. #fCom = [0., 0., 100.] #print("VirtualForce", fCom) 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_des) 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) footCenter1 = controlModel.getBodyPositionGlobal(sup) footCenter2 = controlModel.getBodyPositionGlobal(sup2) footCenter = (footCenter1 + footCenter2) / 2 CM_plane = copy.copy(CM) CM_plane[1] = 0. footCenter_plane = copy.copy(footCenter) footCenter_plane[1] = 0. yjc.computeJacobian2(Jt, DOFs, jointPositions, jointAxeses, [CM], CMJointMask) pHatCom = CM - footCenter vCom = dCM linkPositions2 = motionModel.getBodyPositionsGlobal() linkVelocities2 = motionModel.getBodyVelocitiesGlobal() linkAngVelocities2 = motionModel.getBodyAngVelocitiesGlobal() linkInertias2 = motionModel.getBodyInertiasGlobal() CM2 = yrp.getCM(linkPositions2, linkMasses, totalMass) dCM2 = yrp.getCM(linkVelocities2, linkMasses, totalMass) footCenter1 = motionModel.getBodyPositionGlobal(sup) footCenter2 = motionModel.getBodyPositionGlobal(sup2) footCenter = (footCenter1 + footCenter2) / 2 pHatComDes = CM2 - footCenter vComDes = dCM2 Wcp = 0 Wcv = 0 Wcm = 0 ''' 0 : initial 1 : contact 2 : fly 3 : landing ''' global stage if len(contactForces) == 0: if stage == 1: stage = 2 print("fly") else: if stage == 0: stage = 1 print("contact") elif stage == 2: stage = 3 print("landing") Wcp = -550 Wcv = -100 Wcm = 0 if stage == 1: Wcp = -550 Wcv = -100 Wcm = 0 #Wcp = -550 #Wcv = -100 elif stage == 3: #Wcp = -950 #Wcv = -300 Wcp = -950 Wcv = -300 Wcm = 0 # COM Position control fCom = Wcp * (pHatComDes - pHatCom) + Wcv * ( vComDes - vCom) + Wcm * (footCenter_plane - CM_plane) if len(contactForces) == 0: fCom[0] = 0. fCom[1] = -10. #-10 fCom[2] = 0. ''' fCom[0] = 10. fCom[1] = 0. fCom[2] = 250. ''' # Angular Momentum control L_ref = 0 L_con = 0 #R,i for i in range(1, controlModel.getBodyNum()): L_ref += ( linkMasses[i] * np.cross(linkPositions2[i] - [CM], linkVelocities2[i]) + linkInertias2[i] * linkAngVelocities2[i]) L_con += ( linkMasses[i] * np.cross(linkPositions[i] - [CM], linkVelocities[i]) + linkInertias[i] * linkAngVelocities[i]) for iJoint in range(1, 7): # from 'LeftUpLeg'(1) to 'RightFoot'(6) JpT1 = (Jt[0][6 + 3 * (iJoint - 1)], Jt[1][6 + 3 * (iJoint - 1)], Jt[2][6 + 3 * (iJoint - 1)]) JpT2 = (Jt[0][6 + 3 * (iJoint - 1) + 1], Jt[1][6 + 3 * (iJoint - 1) + 1], Jt[2][6 + 3 * (iJoint - 1) + 1]) JpT3 = (Jt[0][6 + 3 * (iJoint - 1) + 2], Jt[1][6 + 3 * (iJoint - 1) + 2], Jt[2][6 + 3 * (iJoint - 1) + 2]) Tfi = (np.dot(JpT1, fCom), np.dot(JpT2, fCom), np.dot(JpT3, fCom)) currentT = controlModel.getJointAngAccelerationLocal(iJoint) controlModel.setJointAngAccelerationLocal( iJoint, currentT + Tfi) currentT = controlModel.getJointAngAccelerationLocal(0) #print(currentT) #JpT = ( Jt[0][0], Jt[1][0], Jt[2][0]) #Tfi = JpT*fCom #controlModel.setJointAngAccelerationLocal(0, currentT+Tfi) ''' if (len(contactForces) != 0) : for iJoint in range(1, 7): # from 'LeftUpLeg'(1) to 'RightFoot'(6) JpT = ( Jt[0][6+3*(iJoint-1)], Jt[1][6+3*(iJoint-1)], Jt[2][6+3*(iJoint-1)]) Tfi = JpT*fCom currentT = controlModel.getJointAngAccelerationLocal(iJoint) controlModel.setJointAngAccelerationLocal(iJoint, currentT+Tfi) else: print("No Contact force!!") ''' controlModel.solveHybridDynamics() vpWorld.step() # rendering rd_CM[0] = CM rd_CM_plane[0] = CM.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_vf[0] = fCom 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() vpWorld = cvw.VpWorld(wcfg) motionModel = cvm.VpMotionModel(vpWorld, motion[0], mcfg) motionModel.recordVelByFiniteDiff() controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg) vpWorld.initialize() controlModel.initializeHybridDynamics() ModelOffset = (1.5, -0.01, 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']) foreSupL = motion[0].skeleton.getJointIndex(config['leftForeFoot']) foreSupR = motion[0].skeleton.getJointIndex(config['rightForeFoot']) rearSupL = motion[0].skeleton.getJointIndex(config['leftRearFoot']) rearSupR = motion[0].skeleton.getJointIndex(config['rightRearFoot']) selectedBody = motion[0].skeleton.getJointIndex(config['end']) #constBody = motion[0].skeleton.getJointIndex('LeftForeArm') constBody = motion[0].skeleton.getJointIndex(config['const']) # 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() JforeSupL = yjc.makeEmptyJacobian(DOFs, 1) dJforeSupL = JforeSupL.copy() JforeSupR = yjc.makeEmptyJacobian(DOFs, 1) dJforeSupR = JforeSupR.copy() JrearSupL = yjc.makeEmptyJacobian(DOFs, 1) dJrearSupL = JrearSupL.copy() JrearSupR = yjc.makeEmptyJacobian(DOFs, 1) dJrearSupR = JrearSupR.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) foreSupLJointMasks = [yjc.getLinkJointMask(motion[0].skeleton, foreSupL)] foreSupRJointMasks = [yjc.getLinkJointMask(motion[0].skeleton, foreSupR)] rearSupLJointMasks = [yjc.getLinkJointMask(motion[0].skeleton, rearSupL)] rearSupLJointMasks = [yjc.getLinkJointMask(motion[0].skeleton, rearSupR)] # 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 = [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_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] rootPos = [None] selectedBodyId = [selectedBody] extraForce = [None] applyedExtraForce = [None] applyedExtraForce[0] = [0, 0, 0] normalVector = [[0, 2, 0]] 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, (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_CM', yr.PointsRenderer(rd_CM, (255, 255, 0))) 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,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, (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_footLVec', yr.VectorsRenderer(rd_footL_vec, rd_footCenterL, (255, 0, 0), 3)) viewer.doc.addRenderer( 'rd_footRVec', yr.VectorsRenderer(rd_footR_vec, rd_footCenterL, (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)) stage = STATIC_BALANCING def simulateCallback(frame): global g_initFlag global forceShowFrame global forceApplyFrame global JsysPre global JsupPreL global JsupPreR global JsupPre global softConstPoint global stage motionModel.update(motion[frame]) Kt, Kk, Kl, Kh, Ksc, Bt, Bl, Bh, Bsc = viewer.GetParam() Dt = 2 * (Kt**.5) Dk = 2 * (Kk**.5) Dl = 2 * (Kl**.5) Dh = 2 * (Kh**.5) Dsc = 2 * (Ksc**.5) if Bsc == 0.0: viewer.doc.showRenderer('softConstraint', False) viewer.motionViewWnd.update(1, viewer.doc) else: viewer.doc.showRenderer('softConstraint', True) renderer1 = viewer.doc.getRenderer('softConstraint') renderer1.rc.setLineWidth(0.1 + Bsc * 3) viewer.motionViewWnd.update(1, viewer.doc) # tracking th_r = motion.getDOFPositions(frame) th = controlModel.getDOFPositions() dth_r = motion.getDOFVelocities(frame) dth = controlModel.getDOFVelocities() ddth_r = motion.getDOFAccelerations(frame) ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt) ddth_c = controlModel.getDOFAccelerations() ype.flatten(ddth_des, ddth_des_flat) ype.flatten(dth, dth_flat) ype.flatten(ddth_c, ddth_c_flat) # jacobian footCenterL = controlModel.getBodyPositionGlobal(supL) footCenterR = controlModel.getBodyPositionGlobal(supR) refFootL = motionModel.getBodyPositionGlobal(supL) refFootR = motionModel.getBodyPositionGlobal(supR) footCenter = footCenterL + (footCenterR - footCenterL) / 2.0 footCenter[1] = 0. footCenter_ref = refFootL + (refFootR - refFootL) / 2.0 #footCenter_ref[1] = 0. foreFootCenterL = controlModel.getBodyPositionGlobal(foreSupL) foreFootCenterR = controlModel.getBodyPositionGlobal(foreSupR) rearFootCenterL = controlModel.getBodyPositionGlobal(rearSupL) rearFootCenterR = controlModel.getBodyPositionGlobal(rearSupR) linkPositions = controlModel.getBodyPositionsGlobal() linkVelocities = controlModel.getBodyVelocitiesGlobal() linkAngVelocities = controlModel.getBodyAngVelocitiesGlobal() linkInertias = controlModel.getBodyInertiasGlobal() jointPositions = controlModel.getJointPositionsGlobal() jointAxeses = controlModel.getDOFAxeses() CM = yrp.getCM(linkPositions, linkMasses, totalMass) dCM = yrp.getCM(linkVelocities, linkMasses, totalMass) CM_plane = copy.copy(CM) CM_plane[1] = 0. dCM_plane = copy.copy(dCM) dCM_plane[1] = 0. linkPositions_ref = motionModel.getBodyPositionsGlobal() CM_ref = yrp.getCM(linkPositions_ref, linkMasses, totalMass) CM_plane_ref = copy.copy(CM_ref) CM_plane_ref[1] = 0. P = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions, CM, linkInertias) dP = ymt.getPureInertiaMatrixDerivative(dTO, linkMasses, linkVelocities, dCM, linkAngVelocities, linkInertias) yjc.computeJacobian2(Jsys, DOFs, jointPositions, jointAxeses, linkPositions, allLinkJointMasks) #dJsys = (Jsys - JsysPre)/(1/30.) #JsysPre = Jsys yjc.computeJacobianDerivative2(dJsys, DOFs, jointPositions, jointAxeses, linkAngVelocities, linkPositions, allLinkJointMasks) if g_initFlag == 0: softConstPoint = controlModel.getBodyPositionGlobal(constBody) softConstPoint[1] -= .3 g_initFlag = 1 yjc.computeJacobian2(JsupL, DOFs, jointPositions, jointAxeses, [footCenterL], supLJointMasks) #dJsupL = (JsupL - JsupPreL)/(1/30.) #JsupPreL = JsupL yjc.computeJacobianDerivative2(dJsupL, DOFs, jointPositions, jointAxeses, linkAngVelocities, [footCenterL], supLJointMasks, False) yjc.computeJacobian2(JsupR, DOFs, jointPositions, jointAxeses, [footCenterR], supRJointMasks) #dJsupR = (JsupR - JsupPreR)/(1/30.) #JsupPreR = JsupR yjc.computeJacobianDerivative2(dJsupR, DOFs, jointPositions, jointAxeses, linkAngVelocities, [footCenterR], supRJointMasks, False) bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce( bodyIDsToCheck, mus, Ks, Ds) CP = yrp.getCP(contactPositions, contactForces) for i in range(len(bodyIDsToCheck)): controlModel.SetBodyColor(bodyIDsToCheck[i], 0, 0, 0) flagForeSupLContact = 0 flagForeSupRContact = 0 flagRearSupLContact = 0 flagRearSupLContact = 0 for i in range(len(bodyIDs)): controlModel.SetBodyColor(bodyIDs[i], 255, 105, 105) index = controlModel.id2index(bodyIDs[i]) if index == foreSupL: flagForeSupLContact = 1 yjc.computeJacobian2(JforeSupL, DOFs, jointPositions, jointAxeses, [foreFootCenterL], foreSupLJointMasks) yjc.computeJacobianDerivative2(dJforeSupL, DOFs, jointPositions, jointAxeses, linkAngVelocities, [foreFootCenterL], foreSupLJointMasks, False) elif index == foreSupR: flagForeSupRContact = 1 yjc.computeJacobian2(JforeSupR, DOFs, jointPositions, jointAxeses, [foreFootCenterR], foreSupRJointMasks) yjc.computeJacobianDerivative2(dJforeSupR, DOFs, jointPositions, jointAxeses, linkAngVelocities, [foreFootCenterR], foreSupRJointMasks, False) elif index == rearSupL: flagRearSupLContact = 1 yjc.computeJacobian2(JrearSupL, DOFs, jointPositions, jointAxeses, [rearFootCenterL], rearSupLJointMasks) yjc.computeJacobianDerivative2(dJrearSupL, DOFs, jointPositions, jointAxeses, linkAngVelocities, [rearFootCenterL], rearSupLJointMasks, False) elif index == rearSupR: flagRearSupRContact = 1 yjc.computeJacobian2(JrearSupR, DOFs, jointPositions, jointAxeses, [rearFootCenterR], rearSupRJointMasks) yjc.computeJacobianDerivative2(dJrearSupR, DOFs, jointPositions, jointAxeses, linkAngVelocities, [rearFootCenterR], rearSupRJointMasks, False) # desForeSupLAcc = [0, 0, 0] desForeSupRAcc = [0, 0, 0] totalNormalForce = [0, 0, 0] for i in range(len(contactForces)): totalNormalForce[0] += contactForces[i][0] totalNormalForce[1] += contactForces[i][1] totalNormalForce[2] += contactForces[i][2] # linear momentum CM_ref_plane = footCenter dL_des_plane = Kl * totalMass * (CM_ref_plane - CM_plane) - Dl * totalMass * dCM_plane # angular momentum CP_ref = footCenter timeStep = 30. if CP_old[0] == None or CP == None: dCP = None else: dCP = (CP - CP_old[0]) / (1 / timeStep) CP_old[0] = CP if CP != None and dCP != None: ddCP_des = Kh * (CP_ref - CP) - Dh * (dCP) CP_des = CP + dCP * (1 / timeStep) + .5 * ddCP_des * ( (1 / timeStep)**2) dH_des = np.cross( (CP_des - CM), (dL_des_plane + totalMass * mm.s2v(wcfg.gravity))) #dH_des = np.cross((CP_des - CM_plane), (dL_des_plane + totalMass*mm.s2v(wcfg.gravity))) else: dH_des = None CMP = yrp.getCMP(contactForces, CM) r = [0, 0, 0] if CP != None and np.any(np.isnan(CMP)) != True: r = CP - CMP # momentum matrix RS = np.dot(P, Jsys) R, S = np.vsplit(RS, 2) rs = np.dot((np.dot(dP, Jsys) + np.dot(P, dJsys)), dth_flat) r_bias, s_bias = np.hsplit(rs, 2) ############################## # soft point constraint P_des = softConstPoint P_cur = controlModel.getBodyPositionGlobal(constBody) dP_des = [0, 0, 0] dP_cur = controlModel.getBodyVelocityGlobal(constBody) ddP_des1 = Ksc * (P_des - P_cur) - Dsc * (dP_cur - dP_des) r = P_des - P_cur I = np.vstack(([1, 0, 0], [0, 1, 0], [0, 0, 1])) Z = np.hstack((I, mm.getCrossMatrixForm(-r))) yjc.computeJacobian2(Jconst, DOFs, jointPositions, jointAxeses, [softConstPoint], constJointMasks) JL, JA = np.vsplit(Jconst, 2) Q1 = np.dot(Z, Jconst) q1 = np.dot(JA, dth_flat) q2 = np.dot(mm.getCrossMatrixForm(q1), np.dot(mm.getCrossMatrixForm(q1), r)) yjc.computeJacobianDerivative2(dJconst, DOFs, jointPositions, jointAxeses, linkAngVelocities, [softConstPoint], constJointMasks, False) q_bias1 = np.dot(np.dot(Z, dJconst), dth_flat) + q2 ############################## flagContact = True if dH_des == None or np.any(np.isnan(dH_des)) == True: flagContact = False viewer.doc.showRenderer('rd_grf_des', False) viewer.motionViewWnd.update(1, viewer.doc) else: viewer.doc.showRenderer('rd_grf_des', True) viewer.motionViewWnd.update(1, viewer.doc) ''' 0 : initial 1 : contact 2 : fly 3 : landing ''' #MOTION = FORWARD_JUMP if mit.MOTION == mit.FORWARD_JUMP: frame_index = [136, 100] elif mit.MOTION == mit.TAEKWONDO: frame_index = [130, 100] else: frame_index = [1000000, 1000000] #MOTION = TAEKWONDO #frame_index = [135, 100] if frame > frame_index[0]: stage = POWERFUL_BALANCING Kk = Kk * 2 Dk = 2 * (Kk**.5) elif frame > frame_index[1]: stage = MOTION_TRACKING trackingW = w if stage == MOTION_TRACKING: trackingW = w2 # optimization mot.addTrackingTerms(problem, totalDOF, Bt, trackingW, ddth_des_flat) mot.addSoftPointConstraintTerms(problem, totalDOF, Bsc, ddP_des1, Q1, q_bias1) if flagContact == True: if stage != MOTION_TRACKING: mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R, r_bias) mot.addAngularTerms(problem, totalDOF, Bh, dH_des, S, s_bias) ############################## # Hard constraint desLinearAccL = getDesFootLinearAcc(motionModel, controlModel, supL, ModelOffset, CM_ref, CM, Kk, Dk) desLinearAccR = getDesFootLinearAcc(motionModel, controlModel, supR, ModelOffset, CM_ref, CM, Kk, Dk) desAngularAccL = getDesFootAngularAcc(motionModel, controlModel, supL, Kk, Dk) desAngularAccR = getDesFootAngularAcc(motionModel, controlModel, supR, Kk, Dk) ''' desLinearAccL = [0,0,0] desAngularAccL = [0,0,0] desLinearAccR = [0,0,0] desAngularAccR = [0,0,0] refPos = motionModel.getBodyPositionGlobal(supL) curPos = controlModel.getBodyPositionGlobal(supL) refRootPos = motionModel.getBodyPositionGlobal(rootB) curRootPos = controlModel.getBodyPositionGlobal(rootB) refVecL = refPos - CM_ref if stage == MOTION_TRACKING: refPos = CM + refVecL refPos[1] += 0.05 refPos[0] -= 0.05 elif stage == POWERFUL_BALANCING: refPos = copy.copy(curPos) refPos[1] = 0 else: refPos[0] += ModelOffset[0] rd_root[0] = curRootPos refVel = motionModel.getBodyVelocityGlobal(supL) rd_footCenterL[0] = copy.copy(curPos) #rd_footCenterL[0][2] += 0.2 curVel = controlModel.getBodyVelocityGlobal(supL) #refAcc = (0,0,0) refAcc = motionModel.getBodyAccelerationGlobal(supL) if stage != MOTION_TRACKING: refPos[1] = 0 if refPos[1] < 0.0 : refPos[1] = 0.0 rd_DesPosL[0] = refPos desLinearAccL = yct.getDesiredAcceleration(refPos, curPos, refVel, curVel, refAcc, Kk, Dk) refPos = motionModel.getBodyPositionGlobal(supR) curPos = controlModel.getBodyPositionGlobal(supR) refVecR = refPos - CM_ref if stage == MOTION_TRACKING: refPos = CM + refVecR refPos[1] += 0.05 refPos[0] -= 0.05 elif stage == POWERFUL_BALANCING: refPos = copy.copy(curPos) refPos[1] = 0 else : refPos[0] += ModelOffset[0] refVel = motionModel.getBodyVelocityGlobal(supR) curVel = controlModel.getBodyVelocityGlobal(supR) refAcc = motionModel.getBodyAccelerationGlobal(supR) if stage != MOTION_TRACKING: refPos[1] = 0 if refPos[1] < 0.0 : refPos[1] = 0.0 rd_DesPosR[0] = refPos desLinearAccR = yct.getDesiredAcceleration(refPos, curPos, refVel, curVel, refAcc, Kk, Dk) curAng = [controlModel.getBodyOrientationGlobal(supL)] refAngVel = motionModel.getBodyAngVelocityGlobal(supL) curAngVel = controlModel.getBodyAngVelocityGlobal(supL) refAngAcc = (0,0,0) curAngY = np.dot(curAng, np.array([0,1,0])) refAngY = np.array([0,1,0]) if stage == DYNAMIC_BALANCING: refAng = [motionModel.getBodyOrientationGlobal(supL)] refAngY2 = np.dot(refAng, np.array([0,1,0])) refAngY = refAngY2[0] rd_footL_vec[0] = refAngY rd_footR_vec[0] = curAngY[0] aL = mm.logSO3(mm.getSO3FromVectors(curAngY[0], refAngY)) desAngularAccL = [Kk*aL + Dk*(refAngVel-curAngVel)] curAng = [controlModel.getBodyOrientationGlobal(supR)] refAngVel = motionModel.getBodyAngVelocityGlobal(supR) curAngVel = controlModel.getBodyAngVelocityGlobal(supR) refAngAcc = (0,0,0) curAngY = np.dot(curAng, np.array([0,1,0])) refAngY = np.array([0,1,0]) if stage == DYNAMIC_BALANCING: refAng = [motionModel.getBodyOrientationGlobal(supR)] refAngY2 = np.dot(refAng, np.array([0,1,0])) refAngY = refAngY2[0] aL = mm.logSO3(mm.getSO3FromVectors(curAngY[0], refAngY)) desAngularAccR = [Kk*aL + Dk*(refAngVel-curAngVel)] a_sup_2 = [desLinearAccL[0], desLinearAccL[1], desLinearAccL[2], desAngularAccL[0][0], desAngularAccL[0][1], desAngularAccL[0][2], desLinearAccR[0], desLinearAccR[1], desLinearAccR[2], desAngularAccR[0][0], desAngularAccR[0][1], desAngularAccR[0][2]] ''' a_sup_2 = np.hstack((np.hstack((desLinearAccL, desAngularAccL)), np.hstack((desLinearAccR, desAngularAccR)))) Jsup_2 = np.vstack((JsupL, JsupR)) dJsup_2 = np.vstack((dJsupL, dJsupR)) mot.setConstraint(problem, totalDOF, Jsup_2, dJsup_2, dth_flat, a_sup_2) ############################## ############################## # Additional constraint desLinearAccL = [0, 0, 0] desAngularAccL = [0, 0, 0] desLinearAccR = [0, 0, 0] desAngularAccR = [0, 0, 0] refPos = motionModel.getBodyPositionGlobal(supL) curPos = controlModel.getBodyPositionGlobal(supL) refVecL = refPos - CM_ref if stage == MOTION_TRACKING: refPos = CM + refVecL refPos[1] += 0.05 refPos[0] -= 0.05 elif stage == POWERFUL_BALANCING: refPos = copy.copy(curPos) refPos[1] = 0 else: refPos[0] += ModelOffset[0] refVel = motionModel.getBodyVelocityGlobal(supL) curVel = controlModel.getBodyVelocityGlobal(supL) #refAcc = (0,0,0) refAcc = motionModel.getBodyAccelerationGlobal(supL) if stage != MOTION_TRACKING: refPos[1] = 0 if refPos[1] < 0.0: refPos[1] = 0.0 desLinearAccL = yct.getDesiredAcceleration(refPos, curPos, refVel, curVel, refAcc, Kk, Dk) curAng = [controlModel.getBodyOrientationGlobal(supL)] refAngVel = motionModel.getBodyAngVelocityGlobal(supL) curAngVel = controlModel.getBodyAngVelocityGlobal(supL) refAngAcc = (0, 0, 0) curAngY = np.dot(curAng, np.array([0, 1, 0])) refAngY = np.array([0, 1, 0]) if stage == DYNAMIC_BALANCING: refAng = [motionModel.getBodyOrientationGlobal(supL)] refAngY2 = np.dot(refAng, np.array([0, 1, 0])) refAngY = refAngY2[0] rd_footL_vec[0] = refAngY rd_footR_vec[0] = curAngY[0] aL = mm.logSO3(mm.getSO3FromVectors(curAngY[0], refAngY)) desAngularAccL = [Kk * aL + Dk * (refAngVel - curAngVel)] ############################## r = problem.solve() problem.clear() ype.nested(r['x'], ddth_sol) rootPos[0] = controlModel.getBodyPositionGlobal(selectedBody) localPos = [[0, 0, 0]] for i in range(stepsPerFrame): # apply penalty force bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce( bodyIDsToCheck, mus, Ks, Ds) vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals, contactForces) controlModel.setDOFAccelerations(ddth_sol) if flagForeSupLContact == 1: controlModel.setJointAngAccelerationLocal( foreSupL, (2.1, 0, 0)) if flagForeSupRContact == 1: controlModel.setJointAngAccelerationLocal( foreSupR, (2.1, 0, 0)) controlModel.solveHybridDynamics() extraForce[0] = viewer.GetForce() if (extraForce[0][0] != 0 or extraForce[0][1] != 0 or extraForce[0][2] != 0): forceApplyFrame += 1 vpWorld.applyPenaltyForce(selectedBodyId, localPos, extraForce) applyedExtraForce[0] = extraForce[0] if forceApplyFrame * wcfg.timeStep > 0.1: viewer.ResetForce() forceApplyFrame = 0 vpWorld.step() # rendering rd_footCenter[0] = footCenter rd_CM[0] = CM.copy() rd_CM_plane[0] = CM_plane.copy() rd_footCenter_ref[0] = footCenter_ref rd_CM_plane_ref[0] = CM_ref.copy() rd_CM_ref[0] = CM_ref.copy() rd_CM_ref_vec[0] = (CM_ref - footCenter_ref) * 3. rd_CM_vec[0] = (CM - footCenter) * 3 #rd_CM_plane[0][1] = 0. if CP != None and dCP != None: rd_CP[0] = CP rd_CP_des[0] = CP_des rd_dL_des_plane[0] = dL_des_plane rd_dH_des[0] = dH_des rd_grf_des[0] = totalNormalForce - totalMass * mm.s2v( wcfg.gravity) #dL_des_plane - totalMass*mm.s2v(wcfg.gravity) rd_exf_des[0] = applyedExtraForce[0] rd_root_des[0] = rootPos[0] rd_CMP[0] = softConstPoint rd_soft_const_vec[0] = controlModel.getBodyPositionGlobal( constBody) - softConstPoint if (forceApplyFrame == 0): applyedExtraForce[0] = [0, 0, 0] viewer.setSimulateCallback(simulateCallback) viewer.startTimer(1 / 60.) 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() vpWorld = cvw.VpWorld(wcfg) motionModel = cvm.VpMotionModel(vpWorld, motion[0], mcfg) motionModel.recordVelByFiniteDiff() controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg) vpWorld.initialize() controlModel.initializeHybridDynamics() #ModelOffset = (1.5, -0.01, 0) ModelOffset = (1.5, 0.05, 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() ############### footPartNum = config['FootPartNum'] indexFootL = [None] * footPartNum indexFootR = [None] * footPartNum jFootL = [None] * footPartNum dJFootL = [None] * footPartNum jFootR = [None] * footPartNum dJFootR = [None] * footPartNum jointMasksFootL = [None] * footPartNum jointMasksFootR = [None] * footPartNum jAngFootL = [None] * footPartNum dJAngFootL = [None] * footPartNum jAngFootR = [None] * footPartNum dJAngFootR = [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() jAngFootL[i] = yjc.makeEmptyJacobian(DOFs, 1, False) dJAngFootL[i] = jAngFootL[i].copy() jAngFootR[i] = yjc.makeEmptyJacobian(DOFs, 1, False) dJAngFootR[i] = jAngFootR[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) ''' 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 = [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_contactForces = [None] * 1000 rd_contactPositions = [None] * 1000 rootPos = [None] selectedBodyId = [selectedBody] extraForce = [None] applyedExtraForce = [None] applyedExtraForce[0] = [0, 0, 0] normalVector = [[0, 2, 0]] 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, (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_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_plane, (255, 255, 0))) 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,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, (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('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_footCenterL, (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)) stage = STATIC_BALANCING contactRendererName = [] for i in range(motion[0].skeleton.getJointNum()): print(i, motion[0].skeleton.getJointName(i)) def simulateCallback(frame): global g_initFlag global forceShowFrame global forceApplyFrame global JsysPre global JsupPreL global JsupPreR global JsupPre global softConstPoint global stage global contactRendererName motionModel.update(motion[frame]) Kt, Kk, Kl, Kh, Ksc, Bt, Bl, Bh, Bsc = viewer.GetParam() Dt = 2 * (Kt**.5) Dk = 2 * (Kk**.5) Dl = 2 * (Kl**.5) Dh = 2 * (Kh**.5) Dsc = 2 * (Ksc**.5) if Bsc == 0.0: viewer.doc.showRenderer('softConstraint', False) viewer.motionViewWnd.update(1, viewer.doc) else: viewer.doc.showRenderer('softConstraint', True) renderer1 = viewer.doc.getRenderer('softConstraint') renderer1.rc.setLineWidth(0.1 + Bsc * 3) viewer.motionViewWnd.update(1, viewer.doc) # tracking th_r = motion.getDOFPositions(frame) th = controlModel.getDOFPositions() dth_r = motion.getDOFVelocities(frame) dth = controlModel.getDOFVelocities() ddth_r = motion.getDOFAccelerations(frame) ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt) ddth_c = controlModel.getDOFAccelerations() ype.flatten(ddth_des, ddth_des_flat) ype.flatten(dth, dth_flat) ype.flatten(ddth_c, ddth_c_flat) # jacobian refFootL = motionModel.getBodyPositionGlobal(supL) refFootR = motionModel.getBodyPositionGlobal(supR) positionFootL = [None] * footPartNum positionFootR = [None] * footPartNum for i in range(footPartNum): positionFootL[i] = controlModel.getBodyPositionGlobal( indexFootL[i]) positionFootR[i] = controlModel.getBodyPositionGlobal( indexFootR[i]) linkPositions = controlModel.getBodyPositionsGlobal() linkVelocities = controlModel.getBodyVelocitiesGlobal() linkAngVelocities = controlModel.getBodyAngVelocitiesGlobal() linkInertias = controlModel.getBodyInertiasGlobal() jointPositions = controlModel.getJointPositionsGlobal() jointAxeses = controlModel.getDOFAxeses() CM = yrp.getCM(linkPositions, linkMasses, totalMass) dCM = yrp.getCM(linkVelocities, linkMasses, totalMass) CM_plane = copy.copy(CM) CM_plane[1] = 0. dCM_plane = copy.copy(dCM) dCM_plane[1] = 0. linkPositions_ref = motionModel.getBodyPositionsGlobal() CM_ref = yrp.getCM(linkPositions_ref, linkMasses, totalMass) CM_plane_ref = copy.copy(CM_ref) CM_plane_ref[1] = 0. P = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions, CM, linkInertias) dP = ymt.getPureInertiaMatrixDerivative(dTO, linkMasses, linkVelocities, dCM, linkAngVelocities, linkInertias) yjc.computeJacobian2(Jsys, DOFs, jointPositions, jointAxeses, linkPositions, allLinkJointMasks) yjc.computeJacobianDerivative2(dJsys, DOFs, jointPositions, jointAxeses, linkAngVelocities, linkPositions, allLinkJointMasks) yjc.computeJacobian2(jFootL[0], DOFs, jointPositions, jointAxeses, [positionFootL[0]], jointMasksFootL[0]) yjc.computeJacobianDerivative2(dJFootL[0], DOFs, jointPositions, jointAxeses, linkAngVelocities, [positionFootL[0]], jointMasksFootL[0], False) yjc.computeJacobian2(jFootR[0], DOFs, jointPositions, jointAxeses, [positionFootR[0]], jointMasksFootR[0]) yjc.computeJacobianDerivative2(dJFootR[0], DOFs, jointPositions, jointAxeses, linkAngVelocities, [positionFootR[0]], jointMasksFootR[0], False) yjc.computeAngJacobian2(jAngFootL[0], DOFs, jointPositions, jointAxeses, [positionFootL[0]], jointMasksFootL[0]) yjc.computeAngJacobianDerivative2(dJAngFootL[0], DOFs, jointPositions, jointAxeses, linkAngVelocities, [positionFootL[0]], jointMasksFootL[0], False) yjc.computeAngJacobian2(jAngFootR[0], DOFs, jointPositions, jointAxeses, [positionFootR[0]], jointMasksFootR[0]) yjc.computeAngJacobianDerivative2(dJAngFootR[0], DOFs, jointPositions, jointAxeses, linkAngVelocities, [positionFootR[0]], jointMasksFootR[0], False) bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce( bodyIDsToCheck, mus, Ks, Ds) CP = yrp.getCP(contactPositions, contactForces) for i in range(len(bodyIDsToCheck)): controlModel.SetBodyColor(bodyIDsToCheck[i], 0, 0, 0) ########################################## for i in range(len(rd_contactPositions)): rd_contactPositions[i] = [0, 0, 0] rd_contactForces[i] = [0, 0, 0] for i in range(len(contactPositions)): rd_contactPositions[i] = np.copy(contactPositions[i]) rd_contactForces[i] = np.copy(contactForces[i]) ''' if len(contactPositions) > 0: rd_contactPositions = np.copy(contactPositions) rd_contactForces = np.copy(contactForces) print("rd_contactPositions", rd_contactPositions) print("contactPositions", contactPositions) ''' ''' for i in range(len(contactRendererName)): viewer.doc.removeRenderer(contactRendererName[i]) del contactRendererName[:] for i in range(len(contactPositions)): contactRendererName.append(str(i)) #viewer.doc.addRenderer(str(i), yr.PointsRenderer([contactPositions[i]], (0,255,0))) viewer.doc.addRenderer(str(i), yr.ForcesRenderer([contactForces[i]], [contactPositions[i]], (0,255,0), .009, 0.009)) viewer.motionViewWnd.update(1, viewer.doc) ''' ########################################## contactFlagFootL = [0] * footPartNum contactFlagFootR = [0] * footPartNum partialDOFIndex = [22, 22] for i in range(len(bodyIDs)): controlModel.SetBodyColor(bodyIDs[i], 255, 105, 105) index = controlModel.id2index(bodyIDs[i]) for j in range(len(indexFootL)): if index == indexFootL[j]: contactFlagFootL[j] = 1 if j != 0: yjc.computePartialJacobian2(jFootL[j], DOFs, jointPositions, jointAxeses, [positionFootL[j]], jointMasksFootL[j], partialDOFIndex) yjc.computePartialJacobianDerivative2( dJFootL[j], DOFs, jointPositions, jointAxeses, linkAngVelocities, [positionFootL[j]], jointMasksFootL[j], False, partialDOFIndex) break for j in range(len(indexFootR)): if index == indexFootR[j]: contactFlagFootR[j] = 1 if j != 0: yjc.computePartialJacobian2(jFootR[j], DOFs, jointPositions, jointAxeses, [positionFootR[j]], jointMasksFootR[j], partialDOFIndex) yjc.computePartialJacobianDerivative2( dJFootR[j], DOFs, jointPositions, jointAxeses, linkAngVelocities, [positionFootR[j]], jointMasksFootR[j], False, partialDOFIndex) break for j in range(len(indexFootL)): yjc.computeAngJacobian2(jAngFootL[j], DOFs, jointPositions, jointAxeses, [positionFootL[j]], jointMasksFootL[j]) yjc.computeAngJacobianDerivative2(dJAngFootL[j], DOFs, jointPositions, jointAxeses, linkAngVelocities, [positionFootL[j]], jointMasksFootL[j], False) yjc.computeAngJacobian2(jAngFootR[j], DOFs, jointPositions, jointAxeses, [positionFootR[j]], jointMasksFootR[j]) yjc.computeAngJacobianDerivative2(dJAngFootR[j], DOFs, jointPositions, jointAxeses, linkAngVelocities, [positionFootR[j]], jointMasksFootR[j], False) ''' if footPartNum == 1: footCenterL = controlModel.getBodyPositionGlobal(supL) footCenterR = controlModel.getBodyPositionGlobal(supR) else: if ((contactFlagFootL[3] == 1 or contactFlagFootL[4] == 1) and contactFlagFootL[0] == 0) or ((contactFlagFootR[3] == 1 or contactFlagFootR[4] == 1) and contactFlagFootR[0] == 0): r = 0.8 footCenterL = (controlModel.getBodyPositionGlobal(supL)*r + controlModel.getBodyPositionGlobal(indexFootL[1])*(1.0-r)) footCenterR = (controlModel.getBodyPositionGlobal(supR)*r + controlModel.getBodyPositionGlobal(indexFootR[1])*(1.0-r)) #footCenterL = controlModel.getBodyPositionGlobal(indexFootL[1]) #footCenterR = controlModel.getBodyPositionGlobal(indexFootR[1]) else : #footCenterL = (controlModel.getBodyPositionGlobal(supL) + controlModel.getBodyPositionGlobal(indexFootL[1]))/2.0 #footCenterR = (controlModel.getBodyPositionGlobal(supR) + controlModel.getBodyPositionGlobal(indexFootR[1]))/2.0 #footCenterL = controlModel.getBodyPositionGlobal(indexFootL[1]) #footCenterR = controlModel.getBodyPositionGlobal(indexFootR[1]) r = 0.8 footCenterL = (controlModel.getBodyPositionGlobal(indexFootL[1])*r + controlModel.getBodyPositionGlobal(indexFootL[3])*(1.0-r)) footCenterR = (controlModel.getBodyPositionGlobal(indexFootR[1])*r + controlModel.getBodyPositionGlobal(indexFootR[3])*(1.0-r)) ''' ''' if stage == POWERFUL_BALANCING: footCenterL = controlModel.getBodyPositionGlobal(indexFootL[1]) footCenterR = controlModel.getBodyPositionGlobal(indexFootR[1]) else: footCenterL = (controlModel.getBodyPositionGlobal(indexFootL[1]) + controlModel.getBodyPositionGlobal(indexFootL[3]) )/2.0 footCenterR = (controlModel.getBodyPositionGlobal(indexFootR[1]) + controlModel.getBodyPositionGlobal(indexFootR[3]))/2.0 ''' ''' p1 = controlModel.getBodyPositionGlobal(indexFootL[0]) p2 = controlModel.getBodyPositionGlobal(indexFootR[0]) p3 = controlModel.getBodyPositionGlobal(indexFootL[4]) p4 = controlModel.getBodyPositionGlobal(indexFootR[4]) print(frame, "supL", p1[1]) print(frame, "supR", p2[1]) print(frame, "calcL", p3[1]) print(frame, "calcR", p4[1]) ''' #footCenter = footCenterL + (footCenterR - footCenterL)/2.0 #footCenter[1] = 0. # ''' if checkAll(contactFlagFootL, 0) == 1 and checkAll(contactFlagFootR, 0) == 1: footCenter = footCenter elif checkAll(contactFlagFootL, 0) == 1 : footCenter = footCenterR elif checkAll(contactFlagFootR, 0) == 1 : footCenter = footCenterL ''' if footPartNum == 1: desFCL = (controlModel.getBodyPositionGlobal(supL)) desFCR = (controlModel.getBodyPositionGlobal(supR)) else: r = .5 desFCL = (controlModel.getBodyPositionGlobal(supL) * r + controlModel.getBodyPositionGlobal(indexFootL[4]) * (1.0 - r) ) #controlModel.getBodyPositionGlobal(indexFootL[1]) desFCR = (controlModel.getBodyPositionGlobal(supR) * r + controlModel.getBodyPositionGlobal(indexFootR[4]) * (1.0 - r) ) #controlModel.getBodyPositionGlobal(indexFootR[1]) desFC = desFCL + (desFCR - desFCL) / 2.0 if checkAll(contactFlagFootL, 0) == 1 and checkAll( contactFlagFootR, 0) == 1: desFC = desFC elif checkAll(contactFlagFootL, 0) == 1: desFC = desFCR elif checkAll(contactFlagFootR, 0) == 1: desFC = desFCL #if stage == MOTION_TRACKING: # desFC = desFCL desFC[1] = 0 rd_footCenter_des[0] = desFC.copy() curRelCMVec = CM_plane - desFC vecRatio = mm.length(curRelCMVec) * 0. #print(frame, vecRatio) footCenter = desFC - curRelCMVec * (vecRatio) #/10.0 footCenter_ref = refFootL + (refFootR - refFootL) / 2.0 #footCenter_ref[1] = 0. footCenter[1] = 0. vecRatio = mm.length(curRelCMVec) * 0. softConstPointOffset = -curRelCMVec * (vecRatio) #/10.0 #print(frame, vecRatio, softConstPointOffset) desForeSupLAcc = [0, 0, 0] desForeSupRAcc = [0, 0, 0] totalNormalForce = [0, 0, 0] for i in range(len(contactForces)): totalNormalForce[0] += contactForces[i][0] totalNormalForce[1] += contactForces[i][1] totalNormalForce[2] += contactForces[i][2] # linear momentum CM_ref_plane = footCenter dL_des_plane = Kl * totalMass * (CM_ref_plane - CM_plane) - Dl * totalMass * dCM_plane # angular momentum CP_ref = footCenter timeStep = 30. if CP_old[0] == None or CP == None: dCP = None else: dCP = (CP - CP_old[0]) / (1 / timeStep) CP_old[0] = CP if CP != None and dCP != None: ddCP_des = Kh * (CP_ref - CP) - Dh * (dCP) CP_des = CP + dCP * (1 / timeStep) + .5 * ddCP_des * ( (1 / timeStep)**2) dH_des = np.cross( (CP_des - CM), (dL_des_plane + totalMass * mm.s2v(wcfg.gravity))) #dH_des = np.cross((CP_des - CM_plane), (dL_des_plane + totalMass*mm.s2v(wcfg.gravity))) else: dH_des = None # momentum matrix RS = np.dot(P, Jsys) R, S = np.vsplit(RS, 2) rs = np.dot((np.dot(dP, Jsys) + np.dot(P, dJsys)), dth_flat) r_bias, s_bias = np.hsplit(rs, 2) ############################## # soft point constraint ##################################################### P_cur = controlModel.getBodyPositionGlobal(constBody) constBodyVec = P_cur - footCenter softConstPoint = [ footCenter[0] + softConstPointOffset[0], mm.length(constBodyVec), footCenter[2] + softConstPointOffset[2] ] ##################################################### P_des = softConstPoint #P_cur = controlModel.getBodyPositionGlobal(constBody) dP_des = [0, 0, 0] dP_cur = controlModel.getBodyVelocityGlobal(constBody) ddP_des1 = Ksc * (-(P_des - P_cur)) - Dsc * (-(dP_cur - dP_des)) r = -(P_des - P_cur) I = np.vstack(([1, 0, 0], [0, 1, 0], [0, 0, 1])) Z = np.hstack((I, mm.getCrossMatrixForm(-r))) yjc.computeJacobian2(Jconst, DOFs, jointPositions, jointAxeses, [P_cur], constJointMasks) JL, JA = np.vsplit(Jconst, 2) Q1 = np.dot(Z, Jconst) q1 = np.dot(JA, dth_flat) q2 = np.dot(mm.getCrossMatrixForm(q1), np.dot(mm.getCrossMatrixForm(q1), r)) yjc.computeJacobianDerivative2(dJconst, DOFs, jointPositions, jointAxeses, linkAngVelocities, [P_cur], constJointMasks, False) q_bias1 = np.dot(np.dot(Z, dJconst), dth_flat) + q2 ############################## flagContact = True if dH_des == None or np.any(np.isnan(dH_des)) == True: flagContact = False viewer.doc.showRenderer('rd_grf_des', False) viewer.motionViewWnd.update(1, viewer.doc) else: viewer.doc.showRenderer('rd_grf_des', True) viewer.motionViewWnd.update(1, viewer.doc) ''' 0 : initial 1 : contact 2 : fly 3 : landing ''' #MOTION = FORWARD_JUMP if mit.MOTION == mit.FORWARD_JUMP: frame_index = [136, 100] #frame_index = [100000, 100000] elif mit.MOTION == mit.TAEKWONDO: frame_index = [130, 100] #frame_index = [100000, 100000] elif mit.MOTION == mit.TAEKWONDO2: frame_index = [130 + 40, 100] elif mit.MOTION == mit.WALK: frame_index = [10000, 60] else: frame_index = [1000000, 1000000] #MOTION = TAEKWONDO #frame_index = [135, 100] ''' if frame > 300 : if stage != DYNAMIC_BALANCING: print("#", frame,"-DYNAMIC_BALANCING") stage = DYNAMIC_BALANCING Kk = Kk*1 Dk = 2*(Kk**.5) ''' if frame > frame_index[0]: if stage != POWERFUL_BALANCING: print("#", frame, "-POWERFUL_BALANCING") stage = POWERFUL_BALANCING Kk = Kk * 2 Dk = 2 * (Kk**.5) elif frame > frame_index[1]: if stage != MOTION_TRACKING: print("#", frame, "-MOTION_TRACKING") stage = MOTION_TRACKING trackingW = w #if checkAll(contactFlagFootR, 0) != 1 : if stage == MOTION_TRACKING: trackingW = w2 #stage = POWERFUL_BALANCING Bt = Bt * 2 # optimization mot.addTrackingTerms(problem, totalDOF, Bt, trackingW, ddth_des_flat) mot.addSoftPointConstraintTerms(problem, totalDOF, Bsc, ddP_des1, Q1, q_bias1) if flagContact == True: if stage != MOTION_TRACKING + 10: mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R, r_bias) mot.addAngularTerms(problem, totalDOF, Bh, dH_des, S, s_bias) a_sup_2 = [None] Jsup_2 = [None] dJsup_2 = [None] ############################## # Hard constraint if stage != MOTION_TRACKING: Kk2 = Kk * 2.0 else: Kk2 = Kk * 1.5 Dk2 = 2 * (Kk2**.5) ''' desLinearAccL, desPosL = getDesFootLinearAcc(motionModel, controlModel, supL, ModelOffset, CM_ref, CM, Kk2, Dk2) desLinearAccR, desPosR = getDesFootLinearAcc(motionModel, controlModel, supR, ModelOffset, CM_ref, CM, Kk2, Dk2) desAngularAccL = getDesFootAngularAcc(motionModel, controlModel, supL, Kk2, Dk2) desAngularAccR = getDesFootAngularAcc(motionModel, controlModel, supR, Kk2, Dk2) ''' if stage != MOTION_TRACKING: idx = 0 #LEFT/RIGHT_TOES if stage != MOTION_TRACKING: desLinearAccL, desPosL = getDesFootLinearAcc( motionModel, controlModel, indexFootL[idx], ModelOffset, CM_ref, CM, Kk2, Dk2, 0.040) desLinearAccR, desPosR = getDesFootLinearAcc( motionModel, controlModel, indexFootR[idx], ModelOffset, CM_ref, CM, Kk2, Dk2, 0.040) desAngularAccL = getDesFootAngularAcc(motionModel, controlModel, indexFootL[idx], Kk2, Dk2) desAngularAccR = getDesFootAngularAcc(motionModel, controlModel, indexFootR[idx], Kk2, Dk2) a_sup_2 = np.hstack( (np.hstack((desLinearAccL, desAngularAccL)), np.hstack((desLinearAccR, desAngularAccR)))) Jsup_2 = np.vstack((jFootL[idx], jFootR[idx])) dJsup_2 = np.vstack((dJFootL[idx], dJFootR[idx])) else: desLinearAccL, desPosL = getDesFootLinearAcc( motionModel, controlModel, indexFootL[idx], ModelOffset, CM_ref, CM, Kk2, Dk2, 0.040) desLinearAccR, desPosR = getDesFootLinearAcc( motionModel, controlModel, indexFootR[idx], ModelOffset, CM_ref, CM, Kk2, Dk2, 0.040) desAngularAccL = getDesFootAngularAcc(motionModel, controlModel, indexFootL[idx], Kk2, Dk2) a_sup_2 = np.hstack((desLinearAccL, desAngularAccL)) Jsup_2 = jFootL[idx] dJsup_2 = dJFootL[idx] rd_DesPosL[0] = desPosL.copy() rd_DesPosR[0] = desPosR.copy() else: if footPartNum != 5: idx = 0 desLinearAccL, desPosL = getDesFootLinearAcc( motionModel, controlModel, indexFootL[idx], ModelOffset, CM_ref, CM, Kk2, Dk2, 0.040) desAngularAccL = getDesFootAngularAcc(motionModel, controlModel, indexFootL[idx], Kk2, Dk2) a_sup_2 = np.hstack((desLinearAccL, desAngularAccL)) Jsup_2 = (jFootL[idx]) dJsup_2 = (dJFootL[idx]) ''' idx = 4 desAngularAccL = getDesFootAngularAcc(motionModel, controlModel, indexFootL[idx], Kk2, Dk2) a_sup_2 = np.hstack(( a_sup_2, desAngularAccL)) Jsup_2 = np.vstack(( Jsup_2, jAngFootL[idx])) dJsup_2 = np.vstack(( dJsup_2, dJAngFootL[idx])) ''' ''' idx = 1 desAngularAccL = getDesFootAngularAcc(motionModel, controlModel, indexFootL[idx], Kk2, Dk2) a_sup_2 = np.hstack(( a_sup_2, desAngularAccL)) Jsup_2 = np.vstack(( Jsup_2, jAngFootL[idx])) dJsup_2 = np.vstack(( dJsup_2, dJAngFootL[idx])) ''' else: idx = 2 desAngularAccL = getDesFootAngularAcc(motionModel, controlModel, indexFootL[idx], Kk2, Dk2) desAngularAccR = getDesFootAngularAcc(motionModel, controlModel, indexFootR[idx], Kk2, Dk2) a_sup_2 = np.hstack((desAngularAccL, desAngularAccR)) Jsup_2 = np.vstack((jAngFootL[idx], jAngFootR[idx])) dJsup_2 = np.vstack((dJAngFootL[idx], dJAngFootR[idx])) ############################## ############################## # Additional constraint if stage != MOTION_TRACKING + 10: #Kk2 = Kk * 2.5 Kk2 = Kk * 2.5 Dk2 = 2 * (Kk2**.5) desForePosL = [0, 0, 0] desForePosR = [0, 0, 0] desRearPosL = [0, 0, 0] desRearPosR = [0, 0, 0] for i in range(1, footPartNum): if stage != MOTION_TRACKING: if contactFlagFootL[i] == 1: desLinearAccL, desForePosL = getDesFootLinearAcc( motionModel, controlModel, indexFootL[i], ModelOffset, CM_ref, CM, Kk2, Dk2, 0.04) desAngularAccL = getDesFootAngularAcc( motionModel, controlModel, indexFootL[i], Kk2, Dk2) a_sup_2 = np.hstack( (a_sup_2, np.hstack( (desLinearAccL, desAngularAccL)))) Jsup_2 = np.vstack((Jsup_2, jFootL[i])) dJsup_2 = np.vstack((dJsup_2, dJFootL[i])) if contactFlagFootR[i] == 1: desLinearAccR, desForePosR = getDesFootLinearAcc( motionModel, controlModel, indexFootR[i], ModelOffset, CM_ref, CM, Kk2, Dk2, 0.04) desAngularAccR = getDesFootAngularAcc( motionModel, controlModel, indexFootR[i], Kk2, Dk2) a_sup_2 = np.hstack( (a_sup_2, np.hstack( (desLinearAccR, desAngularAccR)))) Jsup_2 = np.vstack((Jsup_2, jFootR[i])) dJsup_2 = np.vstack((dJsup_2, dJFootR[i])) else: if contactFlagFootL[i] == 1: desAngularAccL = getDesFootAngularAcc( motionModel, controlModel, indexFootL[i], Kk2, Dk2) a_sup_2 = np.hstack((a_sup_2, desAngularAccL)) Jsup_2 = np.vstack((Jsup_2, jAngFootL[i])) dJsup_2 = np.vstack((dJsup_2, dJAngFootL[i])) if contactFlagFootR[i] == 1: desAngularAccR = getDesFootAngularAcc( motionModel, controlModel, indexFootR[i], Kk2, Dk2) a_sup_2 = np.hstack((a_sup_2, desAngularAccR)) Jsup_2 = np.vstack((Jsup_2, jAngFootR[i])) dJsup_2 = np.vstack((dJsup_2, dJAngFootR[i])) ''' for i in range(1, footPartNum) : if contactFlagFootL[i] == 1: desLinearAccL, desForePosL = getDesFootLinearAcc(motionModel, controlModel, indexFootL[i], ModelOffset, CM_ref, CM, Kk2, Dk2, 0.034) desAngularAccL = getDesFootAngularAcc(motionModel, controlModel, indexFootL[i], Kk2, Dk2) a_sup_2 = np.hstack(( a_sup_2, np.hstack((desLinearAccL, desAngularAccL)) )) Jsup_2 = np.vstack(( Jsup_2, jFootL[i] )) dJsup_2 = np.vstack(( dJsup_2, dJFootL[i] )) if contactFlagFootR[i] == 1: desLinearAccR, desForePosR = getDesFootLinearAcc(motionModel, controlModel, indexFootR[i], ModelOffset, CM_ref, CM, Kk2, Dk2, 0.034) desAngularAccR = getDesFootAngularAcc(motionModel, controlModel, indexFootR[i], Kk2, Dk2) a_sup_2 = np.hstack(( a_sup_2, np.hstack((desLinearAccR, desAngularAccR)) )) Jsup_2 = np.vstack(( Jsup_2, jFootR[i] )) dJsup_2 = np.vstack(( dJsup_2, dJFootR[i] )) ''' rd_DesForePosL[0] = desForePosL rd_DesForePosR[0] = desForePosR rd_DesRearPosL[0] = desRearPosL rd_DesRearPosR[0] = desRearPosR ############################## mot.setConstraint(problem, totalDOF, Jsup_2, dJsup_2, dth_flat, a_sup_2) r = problem.solve() problem.clear() ype.nested(r['x'], ddth_sol) rootPos[0] = controlModel.getBodyPositionGlobal(selectedBody) localPos = [[0, 0, 0]] for i in range(stepsPerFrame): # apply penalty force bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce( bodyIDsToCheck, mus, Ks, Ds) vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals, contactForces) extraForce[0] = viewer.GetForce() if (extraForce[0][0] != 0 or extraForce[0][1] != 0 or extraForce[0][2] != 0): forceApplyFrame += 1 #vpWorld.applyPenaltyForce(selectedBodyId, localPos, extraForce) controlModel.applyBodyForceGlobal(selectedBody, extraForce[0]) applyedExtraForce[0] = extraForce[0] if forceApplyFrame * wcfg.timeStep > 0.1: viewer.ResetForce() forceApplyFrame = 0 controlModel.setDOFAccelerations(ddth_sol) controlModel.solveHybridDynamics() ''' extraForce[0] = viewer.GetForce() if (extraForce[0][0] != 0 or extraForce[0][1] != 0 or extraForce[0][2] != 0) : forceApplyFrame += 1 vpWorld.applyPenaltyForce(selectedBodyId, localPos, extraForce) applyedExtraForce[0] = extraForce[0] if forceApplyFrame*wcfg.timeStep > 0.1: viewer.ResetForce() forceApplyFrame = 0 ''' vpWorld.step() # rendering rd_footCenter[0] = footCenter rd_CM[0] = CM.copy() rd_CM_plane[0] = CM_plane.copy() rd_footCenter_ref[0] = footCenter_ref rd_CM_plane_ref[0] = CM_ref.copy() rd_CM_ref[0] = CM_ref.copy() rd_CM_ref_vec[0] = (CM_ref - footCenter_ref) * 3. rd_CM_vec[0] = (CM - footCenter) * 3 #rd_CM_plane[0][1] = 0. if CP != None and dCP != None: rd_CP[0] = CP rd_CP_des[0] = CP_des rd_dL_des_plane[0] = dL_des_plane rd_dH_des[0] = dH_des rd_grf_des[ 0] = totalNormalForce # - totalMass*mm.s2v(wcfg.gravity)#dL_des_plane - totalMass*mm.s2v(wcfg.gravity) rd_exf_des[0] = applyedExtraForce[0] rd_root_des[0] = rootPos[0] rd_CMP[0] = softConstPoint rd_soft_const_vec[0] = controlModel.getBodyPositionGlobal( constBody) - softConstPoint if (forceApplyFrame == 0): applyedExtraForce[0] = [0, 0, 0] viewer.setSimulateCallback(simulateCallback) viewer.startTimer(1 / 60.) 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() IKModel = cvm.VpMotionModel(vpWorld, motion[0], mcfg) controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg) footPartNum = config['FootPartNum'] if footPartNum > 1: elasticity = 2000 damping = 2 * (elasticity**.5) springBody1 = 1 springBody2 = 2 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_METATARSAL_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.04, 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_IK = yjc.makeEmptyJacobian(DOFs, controlModel.getBodyNum()) 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() ############### jFootL_IK = [None] * footPartNum jFootR_IK = [None] * footPartNum indexFootL = [None] * footPartNum indexFootR = [None] * footPartNum jFootL = [None] * footPartNum dJFootL = [None] * footPartNum jFootR = [None] * footPartNum dJFootR = [None] * footPartNum jointMasksFootL = [None] * footPartNum jointMasksFootR = [None] * footPartNum jAngFootL = [None] * footPartNum dJAngFootL = [None] * footPartNum jAngFootR = [None] * footPartNum dJAngFootR = [None] * footPartNum for i in range(footPartNum): jFootL_IK[i] = yjc.makeEmptyJacobian(DOFs, 1) jFootR_IK[i] = yjc.makeEmptyJacobian(DOFs, 1) jFootL[i] = yjc.makeEmptyJacobian(DOFs, 1) dJFootL[i] = jFootL[i].copy() jFootR[i] = yjc.makeEmptyJacobian(DOFs, 1) dJFootR[i] = jFootR[i].copy() jAngFootL[i] = yjc.makeEmptyJacobian(DOFs, 1, False) dJAngFootL[i] = jAngFootL[i].copy() jAngFootR[i] = yjc.makeEmptyJacobian(DOFs, 1, False) dJAngFootR[i] = jAngFootR[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) dth_IK = 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_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( 'IKModel', cvr.VpModelRenderer(IKModel, (180, 180, 180), yr.POLYGON_FILL)) viewer.doc.addRenderer( 'controlModel', cvr.VpModelRenderer(controlModel, CHARACTER_COLOR, yr.POLYGON_FILL)) #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,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, (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_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))) 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) config['Phalange'] = [ motion[0].skeleton.getJointIndex('LeftPhalange_1'),\ motion[0].skeleton.getJointIndex('LeftPhalange_2'),\ motion[0].skeleton.getJointIndex('LeftPhalange_3'),\ motion[0].skeleton.getJointIndex('RightPhalange_1'),\ motion[0].skeleton.getJointIndex('RightPhalange_2'),\ motion[0].skeleton.getJointIndex('RightPhalange_3')] config['Metatarsal'] = [motion[0].skeleton.getJointIndex('LeftMetatarsal_1'),\ motion[0].skeleton.getJointIndex('LeftMetatarsal_2'),\ motion[0].skeleton.getJointIndex('LeftMetatarsal_3'),\ motion[0].skeleton.getJointIndex('RightMetatarsal_1'),\ motion[0].skeleton.getJointIndex('RightMetatarsal_2'),\ motion[0].skeleton.getJointIndex('RightMetatarsal_3')] config['Talus'] = [ motion[0].skeleton.getJointIndex('LeftTalus_1'),\ motion[0].skeleton.getJointIndex('LeftTalus_2'),\ motion[0].skeleton.getJointIndex('LeftTalus_3'),\ motion[0].skeleton.getJointIndex('RightTalus_1'),\ motion[0].skeleton.getJointIndex('RightTalus_2'),\ motion[0].skeleton.getJointIndex('RightTalus_3')] config['Calcaneus'] = [ motion[0].skeleton.getJointIndex('LeftCalcaneus_1'),\ motion[0].skeleton.getJointIndex('LeftCalcaneus_2'),\ motion[0].skeleton.getJointIndex('LeftCalcaneus_3'),\ motion[0].skeleton.getJointIndex('RightCalcaneus_1'),\ motion[0].skeleton.getJointIndex('RightCalcaneus_2'),\ motion[0].skeleton.getJointIndex('RightCalcaneus_3')] def simulateCallback(frame): curTime = time.time() if frame % 30 == 1: pt[0] = time.time() global g_initFlag global forceShowFrame global forceApplyFrame global JsysPre global JsupPreL global JsupPreR global JsupPre global softConstPoint global stage global contactRendererName global desCOMOffset #motionModel.update(motion[0]) Kt, Kk, Kl, Kh, Ksc, Bt, Bl, Bh, B_CM, B_CMSd, B_Toe = viewer.GetParam( ) Dt = 2 * (Kt**.5) Dk = 2 * (Kk**.5) Dl = 2 * (Kl**.5) Dh = 2 * (Kh**.5) Dsc = 2 * (Ksc**.5) ''' if Bsc == 0.0 : viewer.doc.showRenderer('softConstraint', False) viewer.motionViewWnd.update(1, viewer.doc) else: viewer.doc.showRenderer('softConstraint', True) renderer1 = viewer.doc.getRenderer('softConstraint') renderer1.rc.setLineWidth(0.1+Bsc*3) viewer.motionViewWnd.update(1, viewer.doc) ''' pose = motion[0].copy() def solveIK(desComPos, desIdxs, desPos, desOri, cmW=10., posW=1., oriW=1.): numItr = 100 dt = .5 threshold = 0.1 for i in range(0, numItr): jPart_IK = [] print '----iter num', i IKModel.update(pose) th_r_IK = pose.getDOFPositions() jointPositions_IK = pose.getJointPositionsGlobal() jointAxeses_IK = pose.getDOFAxeses() linkPositions_IK = IKModel.getBodyPositionsGlobal() linkInertias_IK = IKModel.getBodyInertiasGlobal() CM_IK = yrp.getCM(linkPositions_IK, linkMasses, totalMass) print CM_IK P_IK = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions_IK, CM_IK, linkInertias_IK) yjc.computeJacobian2(Jsys_IK, DOFs, jointPositions_IK, jointAxeses_IK, linkPositions_IK, allLinkJointMasks) for j in range(0, len(desIdxs)): jPart_IK.append(Jsys_IK[6 * desIdxs[j]:6 * desIdxs[j] + 6]) J_IK, JAngCom_IK = np.vsplit(np.dot(P_IK, Jsys_IK), 2) dv_IK = cmW * (desComPos - CM_IK) for j in range(0, len(desIdxs)): J_IK = np.vstack((J_IK, jPart_IK[j])) pos_IK = IKModel.getBodyPositionGlobal(desIdxs[j]) dv_IK = np.append(dv_IK, posW * (desPos[j] - pos_IK)) ori_IK = IKModel.getBodyOrientationGlobal(desIdxs[j]) dv_IK = np.append(dv_IK, oriW * mm.logSO3(desOri[j] * ori_IK.T)) #print dv_IK[0:3] dth_IK_solve = npl.lstsq(J_IK, dv_IK) dth_IK_x = dth_IK_solve[0][:totalDOF] ype.nested(dth_IK_x, dth_IK) #print dth_IK[0][0:3] th_IK = yct.getIntegralDOF(th_r_IK, dth_IK, dt) pose.setDOFPositions(th_IK) if np.dot(dv_IK, dv_IK) < threshold: break linkPositions_ref = motionModel.getBodyPositionsGlobal() CM_ref = yrp.getCM(linkPositions_ref, linkMasses, totalMass) footCenterOffset = np.array([ viewer.objectInfoWnd.comOffsetX.value(), viewer.objectInfoWnd.comOffsetY.value(), viewer.objectInfoWnd.comOffsetZ.value() ]) #CM_IK_ref = footCenter + footCenterOffset CM_IK_ref = CM_ref + footCenterOffset #CM_IK_ref[1] = CM_ref[1] + footCenterOffset[1] motion[0].skeleton.getJointIndex(config['supLink']) #IKidxs = [indexFootL[0], indexFootR[0]] #IKdesPos = [motionModel.getBodyPositionGlobal(indexFootL[0]), motionModel.getBodyPositionGlobal(indexFootR[0])] #for i in range(0, 2): # #IKdesPos[i] += ModelOffset # IKdesPos[i][1] = 0.069 #IKori = [motionModel.getBodyOrientationGlobal(indexFootL[0]), motionModel.getBodyOrientationGlobal(indexFootR[0])] #IKdesOri = [None]*2 #for i in range(0, 2): # IKdesOri[i] = mm.I_SO3() IKidxs = config['Phalange'][0:1] + config['Phalange'][3:4] print IKidxs IKdesPos = [None] * len(IKidxs) IKdesOri = [None] * len(IKidxs) for i in range(0, len(IKidxs)): #print i IKdesPos[i] = motionModel.getBodyPositionGlobal(IKidxs[i]) IKdesPos[i][1] = 0.03 IKdesOri[i] = mm.I_SO3() print IKdesPos solveIK(CM_IK_ref, IKidxs, IKdesPos, IKdesOri) # tracking th_r_ori = motion.getDOFPositions(frame) th_r = copy.copy(th_r_ori) global leftHipTimer if viewer.objectInfoWnd.onLeftHip: leftHipTimer = 60 viewer.objectInfoWnd.onLeftHip = False if leftHipTimer > 0: viewer.objectInfoWnd.comOffsetX.value( 0.14 * np.sin(2 * 3.14 * leftHipTimer / 60.)) #viewer.objectInfoWnd.comOffsetZ.value(0.04*np.cos(2*3.14*leftHipTimer/90.)) #B_Hipd = viewer.objectInfoWnd.labelLeftHip.value() #newR1 = mm.exp(mm.v3(0.0,1.0,0.0), 3.14*0.5*B_Hipd/100.) #idx = motion[0].skeleton.getJointIndex('LeftUpLeg') #th_r[idx] = np.dot(th_r[idx], newR1) #idx = motion[0].skeleton.getJointIndex('RightUpLeg') #th_r[idx] = np.dot(th_r[idx], newR1) leftHipTimer -= 1 timeReport[0] += time.time() - curTime curTime = time.time() th = controlModel.getDOFPositions() dth_r = motion.getDOFVelocities(frame) dth = controlModel.getDOFVelocities() ddth_r = motion.getDOFAccelerations(frame) ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt) ddth_c = controlModel.getDOFAccelerations() ype.flatten(ddth_des, ddth_des_flat) ype.flatten(dth, dth_flat) ype.flatten(ddth_c, ddth_c_flat) # jacobian refFootL = motionModel.getBodyPositionGlobal(supL) refFootR = motionModel.getBodyPositionGlobal(supR) positionFootL = [None] * footPartNum positionFootR = [None] * footPartNum for i in range(footPartNum): positionFootL[i] = controlModel.getBodyPositionGlobal( indexFootL[i]) positionFootR[i] = controlModel.getBodyPositionGlobal( indexFootR[i]) linkPositions = controlModel.getBodyPositionsGlobal() linkVelocities = controlModel.getBodyVelocitiesGlobal() linkAngVelocities = controlModel.getBodyAngVelocitiesGlobal() linkInertias = controlModel.getBodyInertiasGlobal() jointPositions = controlModel.getJointPositionsGlobal() jointAxeses = controlModel.getDOFAxeses() CM = yrp.getCM(linkPositions, linkMasses, totalMass) dCM = yrp.getCM(linkVelocities, linkMasses, totalMass) CM_plane = copy.copy(CM) CM_plane[1] = 0. dCM_plane = copy.copy(dCM) dCM_plane[1] = 0. linkPositions_ref = motionModel.getBodyPositionsGlobal() linkVelocities_ref = motionModel.getBodyVelocitiesGlobal() linkAngVelocities_ref = motionModel.getBodyAngVelocitiesGlobal() linkInertias_ref = motionModel.getBodyInertiasGlobal() CM_ref = yrp.getCM(linkPositions_ref, linkMasses, totalMass) CM_plane_ref = copy.copy(CM_ref) CM_plane_ref[1] = 0. P = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions, CM, linkInertias) dP = ymt.getPureInertiaMatrixDerivative(dTO, linkMasses, linkVelocities, dCM, linkAngVelocities, linkInertias) timeReport[1] += time.time() - curTime curTime = time.time() yjc.computeJacobian2(Jsys, DOFs, jointPositions, jointAxeses, linkPositions, allLinkJointMasks) timeReport[2] += time.time() - curTime curTime = time.time() # yjc.computeJacobianDerivative2(dJsys, DOFs, jointPositions, jointAxeses, linkAngVelocities, linkPositions, allLinkJointMasks) if frame > 0: dJsys = (Jsys - JsysPre) * 30. else: dJsys = (Jsys - Jsys) JsysPre = Jsys.copy() timeReport[3] += time.time() - curTime curTime = time.time() bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce( bodyIDsToCheck, mus, Ks, Ds) CP = yrp.getCP(contactPositions, contactForces) for i in range(len(bodyIDsToCheck)): controlModel.SetBodyColor(bodyIDsToCheck[i], 0, 0, 0, 255) contactFlagFootL = [0] * footPartNum contactFlagFootR = [0] * footPartNum partialDOFIndex = [22, 22] for i in range(len(bodyIDs)): controlModel.SetBodyColor(bodyIDs[i], 255, 105, 105, 200) index = controlModel.id2index(bodyIDs[i]) for j in range(len(indexFootL)): if index == indexFootL[j]: contactFlagFootL[j] = 1 for j in range(len(indexFootR)): if index == indexFootR[j]: contactFlagFootR[j] = 1 for j in range(0, footPartNum): jAngFootR[j] = Jsys[6 * indexFootR[j]:6 * indexFootR[j] + 6][3:] #.copy() jAngFootL[j] = Jsys[6 * indexFootL[j]:6 * indexFootL[j] + 6][3:] #.copy() dJAngFootR[j] = dJsys[6 * indexFootR[j]:6 * indexFootR[j] + 6][3:] #.copy() dJAngFootL[j] = dJsys[6 * indexFootL[j]:6 * indexFootL[j] + 6][3:] #.copy() jFootR[j] = Jsys[6 * indexFootR[j]:6 * indexFootR[j] + 6] #.copy() jFootL[j] = Jsys[6 * indexFootL[j]:6 * indexFootL[j] + 6] #.copy() dJFootR[j] = dJsys[6 * indexFootR[j]:6 * indexFootR[j] + 6] #.copy() dJFootL[j] = dJsys[6 * indexFootL[j]:6 * indexFootL[j] + 6] #.copy() if footPartNum == 1: desFCL = (controlModel.getBodyPositionGlobal(supL)) desFCR = (controlModel.getBodyPositionGlobal(supR)) else: r = .5 + desCOMOffset desFCL = (controlModel.getBodyPositionGlobal(indexFootL[0]) * r + controlModel.getBodyPositionGlobal(indexFootL[1]) * (1.0 - r) ) #controlModel.getBodyPositionGlobal(indexFootL[1]) desFCR = (controlModel.getBodyPositionGlobal(indexFootR[0]) * r + controlModel.getBodyPositionGlobal(indexFootR[1]) * (1.0 - r) ) #controlModel.getBodyPositionGlobal(indexFootR[1]) desFC = desFCL + (desFCR - desFCL) / 2.0 desFC[1] = 0 rd_footCenter_des[0] = desFC.copy() curRelCMVec = CM_plane - desFC vecRatio = mm.length(curRelCMVec) * 0. #print(frame, vecRatio) footCenter = desFC - curRelCMVec * (vecRatio) #/10.0 footCenter = ( getBodyGlobalPos(controlModel, motion, 'LeftCalcaneus_1') + getBodyGlobalPos(controlModel, motion, 'LeftPhalange_1') + getBodyGlobalPos(controlModel, motion, 'RightCalcaneus_1') + getBodyGlobalPos(controlModel, motion, 'RightPhalange_1')) / 4. #footCenter = (getBodyGlobalPos(controlModel, motion, 'LeftCalcaneus_1') + getBodyGlobalPos(controlModel, motion, 'LeftTalus_1') + getBodyGlobalPos(controlModel, motion, 'RightCalcaneus_1') + getBodyGlobalPos(controlModel, motion, 'RightTalus_1'))/4. footCenter_ref = refFootL + (refFootR - refFootL) / 2.0 #footCenter_ref[1] = 0. footCenter[1] = 0. footCenterOffset = np.array([ viewer.objectInfoWnd.comOffsetX.value(), 0, viewer.objectInfoWnd.comOffsetZ.value() ]) #footCenter += footCenterOffset vecRatio = mm.length(curRelCMVec) * 0. softConstPointOffset = -curRelCMVec * (vecRatio) #/10.0 #print(frame, vecRatio, softConstPointOffset) desForeSupLAcc = [0, 0, 0] desForeSupRAcc = [0, 0, 0] totalNormalForce = [0, 0, 0] for i in range(len(contactForces)): totalNormalForce[0] += contactForces[i][0] totalNormalForce[1] += contactForces[i][1] totalNormalForce[2] += contactForces[i][2] #print((totalMass*mm.s2v(wcfg.gravity))[1]) footCenterOffset = np.array([ viewer.objectInfoWnd.comOffsetX.value(), viewer.objectInfoWnd.comOffsetY.value(), viewer.objectInfoWnd.comOffsetZ.value() ]) ###################### # optimization terms ###################### # linear momentum CM_ref_plane = footCenter + footCenterOffset dL_des_plane = Kl * totalMass * (CM_ref_plane - CM_plane) - Dl * totalMass * dCM_plane dL_des_plane[1] = Kl * totalMass * (CM_ref[1] + footCenterOffset[1] - CM[1]) - Dl * totalMass * dCM[1] # angular momentum CP_ref = footCenter + footCenterOffset timeStep = 30. if CP_old[0] == None or CP == None: dCP = None else: dCP = (CP - CP_old[0]) / (1 / timeStep) CP_old[0] = CP if CP != None and dCP != None: ddCP_des = Kh * (CP_ref - CP) - Dh * (dCP) CP_des = CP + dCP * (1 / timeStep) + .5 * ddCP_des * ( (1 / timeStep)**2) #dH_des = np.cross((CP_des - CM), (dL_des_plane + totalMass*mm.s2v(wcfg.gravity))) dH_des = np.cross( (CP_des - CM_plane), (dL_des_plane + totalMass * mm.s2v(wcfg.gravity))) else: dH_des = None # momentum matrix RS = np.dot(P, Jsys) R, S = np.vsplit(RS, 2) rs = np.dot((np.dot(dP, Jsys) + np.dot(P, dJsys)), dth_flat) r_bias, s_bias = np.hsplit(rs, 2) flagContact = True if dH_des == None or np.any(np.isnan(dH_des)) == True: flagContact = False #viewer.doc.showRenderer('rd_grf_des', False) #viewer.motionViewWnd.update(1, viewer.doc) #else: #viewer.doc.showRenderer('rd_grf_des', True) #viewer.motionViewWnd.update(1, viewer.doc) ''' 0 : initial 1 : contact 2 : fly 3 : landing ''' trackingW = w #if checkAll(contactFlagFootR, 0) != 1 : if 0: #stage == MOTION_TRACKING: trackingW = w2 #stage = POWERFUL_BALANCING Bt = Bt * 2 ####################### # optimization ####################### mot.addTrackingTerms(problem, totalDOF, Bt, trackingW, ddth_des_flat) #if flagContact == True: # mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R, r_bias) # mot.addAngularTerms(problem, totalDOF, Bh, dH_des, S, s_bias) a_sup_2 = None Jsup_2 = None dJsup_2 = None ############################## #if Jsup_2 != None: # mot.addConstraint(problem, totalDOF, Jsup_2, dJsup_2, dth_flat, a_sup_2) timeReport[4] += time.time() - curTime curTime = time.time() r = problem.solve() problem.clear() ype.nested(r['x'], ddth_sol) rootPos[0] = controlModel.getBodyPositionGlobal(selectedBody) localPos = [[0, 0, 0]] timeReport[5] += time.time() - curTime curTime = time.time() for i in range(stepsPerFrame): # apply penalty force bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce( bodyIDsToCheck, mus, Ks, Ds) vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals, contactForces) extraForce[0] = viewer.GetForce() if (extraForce[0][0] != 0 or extraForce[0][1] != 0 or extraForce[0][2] != 0): forceApplyFrame += 1 #vpWorld.applyPenaltyForce(selectedBodyId, localPos, extraForce) controlModel.applyBodyForceGlobal(selectedBody, extraForce[0]) applyedExtraForce[0] = extraForce[0] if forceApplyFrame * wcfg.timeStep > 0.1: viewer.ResetForce() forceApplyFrame = 0 controlModel.setDOFAccelerations(ddth_sol) controlModel.solveHybridDynamics() vpWorld.step() if frame % 30 == 0: print 'elapsed time for 30 frames:', time.time() - pt[0] # rendering rd_footCenter[0] = footCenter rd_CM[0] = CM.copy() rd_CM_plane[0] = CM_plane.copy() rd_footCenter_ref[0] = footCenter_ref rd_CM_plane_ref[0] = CM_ref.copy() rd_CM_ref[0] = CM_ref.copy() rd_CM_ref_vec[0] = (CM_ref - footCenter_ref) * 3. rd_CM_vec[0] = (CM - CM_plane) rd_CM_des[0] = CM_ref_plane.copy() rd_CM_des[0][1] = .01 #rd_CM_plane[0][1] = 0. if CP != None and dCP != None: rd_CP[0] = CP rd_CP_des[0] = CP_des rd_dL_des_plane[0] = dL_des_plane rd_dH_des[0] = dH_des rd_grf_des[ 0] = totalNormalForce # - totalMass*mm.s2v(wcfg.gravity)#dL_des_plane - totalMass*mm.s2v(wcfg.gravity) rd_exf_des[0] = applyedExtraForce[0] rd_root_des[0] = rootPos[0] rd_CMP[0] = softConstPoint rd_soft_const_vec[0] = controlModel.getBodyPositionGlobal( constBody) - softConstPoint #indexL = motion[0].skeleton.getJointIndex('Hips') #indexR = motion[0].skeleton.getJointIndex('Spine1') indexL = indexFootL[0] indexR = indexFootR[0] curAng = [controlModel.getBodyOrientationGlobal(indexL)] curAngY = np.dot(curAng, np.array([0, 0, 1])) rd_footL_vec[0] = np.copy(curAngY[0]) rd_footCenterL[0] = controlModel.getBodyPositionGlobal(indexL) curAng = [controlModel.getBodyOrientationGlobal(indexR)] curAngY = np.dot(curAng, np.array([0, 0, 1])) rd_footR_vec[0] = np.copy(curAngY[0]) rd_footCenterR[0] = controlModel.getBodyPositionGlobal(indexR) if (forceApplyFrame == 0): applyedExtraForce[0] = [0, 0, 0] timeReport[6] += time.time() - curTime # print timeReport 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, frame_rate = mit.create_biped( SEGMENT_FOOT, SEGMENT_FOOT_MAG) #motion, mcfg, wcfg, stepsPerFrame, config = mit.create_jump_biped() # motion_offset = np.array((0, 0.15, 0)) # motion.translateByOffset(motion_offset, True) frame_step_size = 1. / frame_rate pydart.init() dartModel = cdm.DartModel(wcfg, motion[0], mcfg, DART_CONTACT_ON) dartMotionModel = cdm.DartModel(wcfg, motion[0], mcfg, DART_CONTACT_ON) # wcfg.lockingVel = 0.01 dartModel.initializeHybridDynamics() # controlToMotionOffset = (1.5, -0.02, 0) # controlToMotionOffset = (1.5, 0.15, 0) controlToMotionOffset = (1.5, 0.03, 0) dartModel.translateByOffset(controlToMotionOffset) totalDOF = dartModel.getTotalDOF() DOFs = dartModel.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(dartModel.getJointNum()): joint_name = dartModel.getJoint(joint_idx).name # joint_name = motion[0].skeleton.getJointName(joint_idx) if 'Foot' in joint_name: foot_dofs_temp = dartModel.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 = dartModel.getJointDOFIndexes(joint_idx) foot_seg_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) # for dart_foot_joint in (dartModel.getJoint(i) for i in foot_seg_dofs): # dart_foot_joint.set_actuator_type(pydart.Joint.FORCE) # 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']) constBody = motion[0].skeleton.getJointIndex('RightFoot') # momentum matrix linkMasses = dartModel.getBodyMasses() print([body.name for body in dartModel.skeleton.bodynodes]) print(linkMasses) totalMass = dartModel.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.)] CP_des = [None] dCP_des = [np.zeros(3)] # penalty method bodyIDsToCheck = range(dartModel.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', cvr.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', cvr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_FILL)) viewer.doc.addRenderer( 'motionModel', yr.DartModelRenderer(dartMotionModel, (150, 150, 255), yr.POLYGON_FILL)) viewer.doc.addRenderer( 'controlModel', yr.DartModelRenderer(dartModel, (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, 0, 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 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.labelKt.value(initKt) # viewer.objectInfoWnd.labelKl.value(initKl) # viewer.objectInfoWnd.labelKh.value(initKh) # viewer.objectInfoWnd.labelBl.value(initBl) # viewer.objectInfoWnd.labelBh.value(initBh) # viewer.objectInfoWnd.labelSupKt.value(initSupKt) # viewer.objectInfoWnd.labelFm.value(initFm) # # viewer.objectInfoWnd.sliderKt.value(initKt*50) # viewer.objectInfoWnd.sliderKl.value(initKl*10) # viewer.objectInfoWnd.sliderKh.value(initKh*10) # viewer.objectInfoWnd.sliderBl.value(initBl*100) # viewer.objectInfoWnd.sliderBh.value(initBh*100) # viewer.objectInfoWnd.sliderSupKt.value(initSupKt*10) # viewer.objectInfoWnd.sliderFm.value(initFm) viewer.force_on = False def viewer_SetForceState(obj): 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 * 7, 40, 20, 'X') viewer.objectInfoWnd.labelForceX.value(0) viewer.objectInfoWnd.labelForceY = Fl_Value_Input(80, 30 + offset * 7, 40, 20, 'Y') viewer.objectInfoWnd.labelForceY.value(0) viewer.objectInfoWnd.labelForceZ = Fl_Value_Input(140, 30 + offset * 7, 40, 20, 'Z') viewer.objectInfoWnd.labelForceZ.value(1) viewer.objectInfoWnd.labelForceDur = Fl_Value_Input( 220, 30 + offset * 7, 40, 20, 'Dur') viewer.objectInfoWnd.labelForceDur.value(0.1) viewer.objectInfoWnd.end() #self.sliderFm = Fl_Hor_Nice_Slider(10, 42+offset*6, 250, 10) pdcontroller = PDController(dartModel, dartModel.skeleton, wcfg.timeStep, 16., 8.) 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', 'Foot_foot_1_1', 'Foot_foot_1_2' ] 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) foot_left_idx = motion[0].skeleton.getJointIndex('LeftFoot') foot_right_idx = motion[0].skeleton.getJointIndex('RightFoot') ################################### #simulate ################################### def simulateCallback(frame): global g_initFlag global forceShowTime 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) # Dt = .2*(Kt**.5) # Dl = .2*(Kl**.5) # Dh = .2*(Kh**.5) # dt_sup = .2*(kt_sup**.5) pdcontroller.setKpKd(Kt, Dt) doubleTosingleOffset = 0.15 singleTodoubleOffset = 0.30 #doubleTosingleOffset = 0.09 doubleTosingleVelOffset = 0.0 # tracking # th_r = motion.getDOFPositions(frame) # th = dartModel.getDOFPositions() # dth_r = motion.getDOFVelocities(frame) # dth = dartModel.getDOFVelocities() # ddth_r = motion.getDOFAccelerations(frame) # ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt) dth_flat = dartModel.get_dq() # 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 = dartModel.getJointOrientationGlobal(supL) footOriR = dartModel.getJointOrientationGlobal(supR) #desire footCenter[1] = 0.041135 #desire footCenter[1] = 0.0197 footCenterL = dartModel.getBodyPositionGlobal(supL) footCenterR = dartModel.getBodyPositionGlobal(supR) footBodyOriL = dartModel.getBodyOrientationGlobal(supL) footBodyOriR = dartModel.getBodyOrientationGlobal(supR) footBodyVelL = dartModel.getBodyVelocityGlobal(supL) footBodyVelR = dartModel.getBodyVelocityGlobal(supR) footBodyAngVelL = dartModel.getBodyAngVelocityGlobal(supL) footBodyAngVelR = dartModel.getBodyAngVelocityGlobal(supR) refFootL = dartMotionModel.getBodyPositionGlobal(supL) refFootR = dartMotionModel.getBodyPositionGlobal(supR) refFootAngVelL = motion.getJointAngVelocityGlobal(supL, frame) refFootAngVelR = motion.getJointAngVelocityGlobal(supR, frame) 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] * frame_step_size + refFootR[ 1] > singleTodoubleOffset: contactR = 0 if refFootVelL[1] < 0 and refFootVelL[1] * frame_step_size + refFootL[ 1] > singleTodoubleOffset: contactL = 0 if refFootVelR[1] > 0 and refFootVelR[1] * frame_step_size + refFootR[ 1] > doubleTosingleOffset: contactR = 0 if refFootVelL[1] > 0 and refFootVelL[1] * frame_step_size + refFootL[ 1] > doubleTosingleOffset: contactL = 0 # contactR = 0 # contMotionOffset = th[0][0] - th_r[0][0] contMotionOffset = dartModel.getBodyPositionGlobal( 0) - dartMotionModel.getBodyPositionGlobal(0) linkPositions = dartModel.getBodyPositionsGlobal() linkVelocities = dartModel.getBodyVelocitiesGlobal() linkAngVelocities = dartModel.getBodyAngVelocitiesGlobal() linkInertias = dartModel.getBodyInertiasGlobal() CM = dartModel.skeleton.com() dCM = dartModel.skeleton.com_velocity() 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: softConstPoint = footCenterR.copy() 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 body_num = dartModel.getBodyNum() Jsys = np.zeros((6 * body_num, totalDOF)) dJsys = np.zeros((6 * body_num, totalDOF)) for i in range(dartModel.getBodyNum()): body_i_jacobian = dartModel.getBody(i).world_jacobian()[ range(-3, 3), :] body_i_jacobian_deriv = dartModel.getBody( i).world_jacobian_classic_deriv()[range(-3, 3), :] Jsys[6 * i:6 * i + 6, :] = body_i_jacobian dJsys[6 * i:6 * i + 6, :] = body_i_jacobian_deriv JsupL = dartModel.getBody(supL).world_jacobian()[range(-3, 3), :] dJsupL = dartModel.getBody(supL).world_jacobian_classic_deriv()[ range(-3, 3), :] JsupR = dartModel.getBody(supR).world_jacobian()[range(-3, 3), :] dJsupR = dartModel.getBody(supR).world_jacobian_classic_deriv()[ range(-3, 3), :] dartMotionModel.update(motion[frame]) # ddth_des_flat = pdcontroller.compute(dartMotionModel.get_q()) ddth_des_flat = pdcontroller.compute(motion.getDOFPositions(frame)) #calculate footCenter footCenter = .5 * (footCenterL + footCenterR) #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() # foot adjustment foot_angle_weight = 1. foot_dCM_weight = 5. foot_center_diff = CM_plane + dCM_plane * frame_step_size * foot_dCM_weight - footCenter foot_center_diff_norm = np.linalg.norm(foot_center_diff) foot_left_height = dartModel.getJointPositionGlobal(foot_left_idx)[1] foot_right_height = dartModel.getJointPositionGlobal(foot_left_idx)[1] foot_left_angle = foot_angle_weight * math.atan2( foot_center_diff_norm, foot_left_height) foot_right_angle = foot_angle_weight * math.atan2( foot_center_diff_norm, foot_right_height) foot_axis = np.cross(np.array((0., 1., 0.)), foot_center_diff) foot_left_R = mm.exp(foot_axis, foot_left_angle) foot_right_R = mm.exp(foot_axis, foot_right_angle) # motion[frame].mulJointOrientationGlobal(foot_left_idx, foot_left_R) # motion[frame].mulJointOrientationGlobal(foot_right_idx, foot_right_R) # hfi.footAdjust(motion[frame], footIdDic, SEGMENT_FOOT_MAG, SEGMENT_FOOT_RAD, 0.) # 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 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 = [], [], [], [] if DART_CONTACT_ON: bodyIDs, contactPositions, contactPositionLocals, contactForces = dartModel.get_dart_contact_info( ) else: bodyIDs, contactPositions, contactPositionLocals, contactForces = dartModel.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]) / frame_step_size CP_old[0] = CP # CP_des = None if CP_des[0] is None: CP_des[0] = footCenter if CP is not None and dCP is not None: ddCP_des = Kh * (CP_ref - CP) - Dh * dCP CP_des[0] = CP + dCP * frame_step_size + .5 * ddCP_des * ( frame_step_size**2) # dCP_des[0] += ddCP_des * frame_step_size # CP_des[0] += dCP_des[0] * frame_step_size + .5 * ddCP_des*(frame_step_size ** 2) dH_des = np.cross( CP_des[0] - 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]))) left_foot_up_vec, right_foot_up_vec = hfi.get_foot_up_vector( motion[frame], footIdDic, None) a_oriL = mm.logSO3( mm.getSO3FromVectors(left_foot_up_vec, np.array([0, 1, 0]))) a_oriR = mm.logSO3( mm.getSO3FromVectors(right_foot_up_vec, 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 * (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[3:] = 0. # a_supR[3:] = 0. 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) 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 contact & 1: #if refFootR[1] < doubleTosingleOffset: mot.addConstraint(problem, totalDOF, JsupR, dJsupR, dth_flat, a_supR) if contact & 2: #if refFootL[1] < doubleTosingleOffset: mot.addConstraint(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) ddth_sol = np.asarray(r['x']) # remove foot seg effect ddth_sol[foot_dofs] = ddth_des_flat[foot_dofs] # ddth_sol[:] = ddth_des_flat[:] rootPos[0] = dartModel.getBodyPositionGlobal(selectedBody) localPos = [[0, 0, 0]] for i in range(stepsPerFrame): # apply penalty force if not DART_CONTACT_ON: bodyIDs, contactPositions, contactPositionLocals, contactForces = dartModel.calcPenaltyForce( bodyIDsToCheck, mus, Ks, Ds) dartModel.applyPenaltyForce(bodyIDs, contactPositionLocals, contactForces) #bodyIDs, contactPositions, contactPositionLocals, contactForces, contactVelocities = vpWorld.calcManyPenaltyForce(0, bodyIDsToCheck, mus, Ks, Ds) dartModel.skeleton.set_accelerations(ddth_sol) 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 dartModel.applyPenaltyForce(selectedBodyId, localPos, extraForce) dartModel.step() if DART_CONTACT_ON: bodyIDs, contactPositions, contactPositionLocals, contactForces = dartModel.get_dart_contact_info( ) else: bodyIDs, contactPositions, contactPositionLocals, contactForces = dartModel.calcPenaltyForce( bodyIDsToCheck, mus, Ks, Ds) # 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[0] 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] / 100) 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] = dartModel.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, 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) vpWorld.initialize() controlModel.initializeHybridDynamics() #controlToMotionOffset = (1.5, -0.02, 0) controlToMotionOffset = (1.5, 0, 0) controlModel.translateByOffset(controlToMotionOffset) 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) print(joint_idx, joint_name) # 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_dofs.extend(foot_dofs_temp) elif 'Right' in joint_name: right_foot_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 = 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 = 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)) control_model_renderer = yr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_FILL) viewer.doc.addRenderer('controlModel', control_model_renderer) control_model_renderer.body_colors[supL] = (255, 0, 0) control_model_renderer.body_colors[supR] = (255, 0, 0) 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) 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 ################################### # 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 # 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 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 jacobian yjc.computeJacobian2(Jsys, DOFs, jointPositions, jointAxeses, linkPositions, allLinkJointMasks) # dJsys = (Jsys - JsysPre)/(1/30.) # JsysPre = Jsys.copy() yjc.computeJacobianDerivative2(dJsys, DOFs, jointPositions, jointAxeses, linkAngVelocities, linkPositions, allLinkJointMasks) JsupL, supLJointMasks = get_jacobianbase_and_masks(motion[0].skeleton, DOFs, supL) yjc.computeJacobian2(JsupL, DOFs, jointPositions, jointAxeses, [footCenterL], supLJointMasks) # dJsupL = (JsupL - JsupPreL)/(1/30.) # JsupPreL = JsupL.copy() yjc.computeJacobianDerivative2(dJsupL, DOFs, jointPositions, jointAxeses, linkAngVelocities, [footCenterL], supLJointMasks) JsupR, supRJointMasks = get_jacobianbase_and_masks(motion[0].skeleton, DOFs, supR) yjc.computeJacobian2(JsupR, DOFs, jointPositions, jointAxeses, [footCenterR], supRJointMasks) # dJsupR = (JsupR - JsupPreR)/(1/30.) # JsupPreR = JsupR.copy() yjc.computeJacobianDerivative2(dJsupR, DOFs, jointPositions, jointAxeses, linkAngVelocities, [footCenterR], supRJointMasks) # 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! # to do 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) else: dH_des = None # set up equality constraint # left_foot_up_vec, right_foot_up_vec = hfi.get_foot_up_vector(motion[frame], footIdDic, None) left_foot_up_vec, right_foot_up_vec = hfi.get_foot_up_vector(controlModel, footIdDic, None) # print(left_foot_up_vec, right_foot_up_vec) # 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]))) a_oriL = mm.logSO3(mm.getSO3FromVectors(left_foot_up_vec, np.array([0,1,0]))) a_oriR = mm.logSO3(mm.getSO3FromVectors(right_foot_up_vec, np.array([0,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)) if contactChangeCount > 0 and contactChangeType == 'DtoS': 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': linkt = (13.*contactChangeCount)/(maxContactChangeCount)+1. lindt = 2*(linkt**.5) angkt = (13.*contactChangeCount)/(maxContactChangeCount)+1. angdt = 2*(angkt**.5) 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)) # 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) ####################################################### # 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: mot.addConstraint(problem, totalDOF, JsupR, dJsupR, dth_flat, a_supR) if contact & 2: mot.addConstraint(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() ddth_sol_flat = np.asarray(r['x']) # ddth_sol_flat[foot_seg_dofs] = np.array(ddth_des_flat)[foot_seg_dofs] print(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.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() # 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(fm=50., fv=np.array([0., 0., 50.]), ts=2., te=2.1): np.set_printoptions(precision=4, linewidth=200) # np.set_printoptions(precision=4, linewidth=1000, threshold=np.inf) motion, mcfg, wcfg, stepsPerFrame, config, frame_rate = mit.create_biped() frame_step_size = 1. / frame_rate pydart.init() dartModel = cdm.DartModel(wcfg, motion[0], mcfg, DART_CONTACT_ON) dartMotionModel = cdm.DartModel(wcfg, motion[0], mcfg, DART_CONTACT_ON) # wcfg.lockingVel = 0.01 dartModel.initializeHybridDynamics() #controlToMotionOffset = (1.5, -0.02, 0) controlToMotionOffset = (1.5, 0, 0) dartModel.translateByOffset(controlToMotionOffset) totalDOF = dartModel.getTotalDOF() DOFs = dartModel.getDOFs() # parameter Kt = 25. Dt = 5. Ks = config['Ks'] Ds = config['Ds'] 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']) constBody = motion[0].skeleton.getJointIndex('RightFoot') # momentum matrix linkMasses = dartModel.getBodyMasses() totalMass = dartModel.getTotalMass() dartModel.world.time() # penalty method # bodyIDsToCheck = range(dartModel.getBodyNum()) bodyIDsToCheck = [supL, supR] #mus = [1.]*len(bodyIDsToCheck) mus = [.5] * len(bodyIDsToCheck) # viewer selectedBodyId = [selectedBody] extraForce = [None] extraForcePos = [None] pdcontroller = PDController(dartModel, dartModel.skeleton, wcfg.timeStep, Kt, Dt) ################################### #simulate ################################### def simulateCallback(frame): dartMotionModel.update(motion[frame]) # tracking # th_r = motion.getDOFPositions(frame) th_r = dartMotionModel.getDOFPositions() th = dartModel.getDOFPositions() th_r_flat = dartMotionModel.get_q() dth_flat = dartModel.get_dq() ddth_des_flat = pdcontroller.compute_flat(th_r_flat) localPos = [[0, 0, 0]] for i in range(stepsPerFrame): # apply penalty force if not DART_CONTACT_ON: bodyIDs, contactPositions, contactPositionLocals, contactForces = dartModel.calcPenaltyForce( bodyIDsToCheck, mus, Ks, Ds) dartModel.applyPenaltyForce(bodyIDs, contactPositionLocals, contactForces) # dartModel.skeleton.set_accelerations(ddth_sol) dartModel.skeleton.set_accelerations(ddth_des_flat) # dartModel.skeleton.set_forces(np.zeros(totalDOF)) extraForce[0] = fm * fv if ts <= dartModel.world.time() <= te: dartModel.applyPenaltyForce(selectedBodyId, localPos, extraForce) dartModel.step() if DART_CONTACT_ON: bodyIDs, contactPositions, contactPositionLocals, contactForces = dartModel.get_dart_contact_info( ) else: bodyIDs, contactPositions, contactPositionLocals, contactForces = dartModel.calcPenaltyForce( bodyIDsToCheck, mus, Ks, Ds)