Exemplo n.º 1
0
    def simulateCallback(frame):
        motionModel.update(motion[frame])

        global g_initFlag
        global forceShowTime

        global JsysPre
        global JsupPreL
        global JsupPreR
        global JsupPre

        global JconstPre

        global preFootCenter
        global maxContactChangeCount
        global contactChangeCount
        global contact
        global contactChangeType

        # Kt, Kl, Kh, Bl, Bh, kt_sup = viewer.GetParam()
        Kt, Kl, Kh, Bl, Bh, kt_sup = getParamVals(
            ['Kt', 'Kl', 'Kh', 'Bl', 'Bh', 'SupKt'])
        Dt = 2 * (Kt**.5)
        Dl = 2 * (Kl**.5)
        Dh = 2 * (Kh**.5)
        dt_sup = 2 * (kt_sup**.5)

        doubleTosingleOffset = 0.15
        singleTodoubleOffset = 0.30
        # doubleTosingleOffset = 0.09
        doubleTosingleVelOffset = 0.0

        # tracking
        th_r = motion.getDOFPositions(frame)
        th = controlModel.getDOFPositions()
        dth_r = motion.getDOFVelocities(frame)
        dth = controlModel.getDOFVelocities()
        ddth_r = motion.getDOFAccelerations(frame)
        ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r,
                                                  Kt, Dt)

        ype.flatten(ddth_des, ddth_des_flat)
        ype.flatten(dth, dth_flat)

        #################################################
        # jacobian
        #################################################

        # caution!! body orientation and joint orientation of foot are totally different!!
        footOriL = controlModel.getJointOrientationGlobal(supL)
        footOriR = controlModel.getJointOrientationGlobal(supR)

        # desire footCenter[1] = 0.041135
        # desire footCenter[1] = 0.0197
        footCenterL = controlModel.getBodyPositionGlobal(supL)
        footCenterR = controlModel.getBodyPositionGlobal(supR)
        footBodyOriL = controlModel.getBodyOrientationGlobal(supL)
        footBodyOriR = controlModel.getBodyOrientationGlobal(supR)
        footBodyVelL = controlModel.getBodyVelocityGlobal(supL)
        footBodyVelR = controlModel.getBodyVelocityGlobal(supR)
        footBodyAngVelL = controlModel.getBodyAngVelocityGlobal(supL)
        footBodyAngVelR = controlModel.getBodyAngVelocityGlobal(supR)

        refFootL = motionModel.getBodyPositionGlobal(supL)
        refFootR = motionModel.getBodyPositionGlobal(supR)
        refFootVelL = motionModel.getBodyVelocityGlobal(supL)
        refFootVelR = motionModel.getBodyVelocityGlobal(supR)
        refFootAngVelL = motionModel.getBodyAngVelocityGlobal(supL)
        refFootAngVelR = motionModel.getBodyAngVelocityGlobal(supR)

        refFootJointVelR = motion.getJointVelocityGlobal(supR, frame)
        refFootJointAngVelR = motion.getJointAngVelocityGlobal(supR, frame)
        refFootJointR = motion.getJointPositionGlobal(supR, frame)
        refFootVelR = refFootJointVelR + np.cross(refFootJointAngVelR,
                                                  (refFootR - refFootJointR))

        refFootJointVelL = motion.getJointVelocityGlobal(supL, frame)
        refFootJointAngVelL = motion.getJointAngVelocityGlobal(supL, frame)
        refFootJointL = motion.getJointPositionGlobal(supL, frame)
        refFootVelL = refFootJointVelL + np.cross(refFootJointAngVelL,
                                                  (refFootL - refFootJointL))

        contactR = 1
        contactL = 1
        if refFootVelR[1] < 0 and refFootVelR[1] / 30. + refFootR[
                1] > singleTodoubleOffset:
            contactR = 0
        if refFootVelL[1] < 0 and refFootVelL[1] / 30. + refFootL[
                1] > singleTodoubleOffset:
            contactL = 0
        if refFootVelR[1] > 0 and refFootVelR[1] / 30. + refFootR[
                1] > doubleTosingleOffset:
            contactR = 0
        if refFootVelL[1] > 0 and refFootVelL[1] / 30. + refFootL[
                1] > doubleTosingleOffset:
            contactL = 0
        # if 32 < frame < 147:
        #     contactR = 0

        contMotionOffset = th[0][0] - th_r[0][0]

        linkPositions = controlModel.getBodyPositionsGlobal()
        linkVelocities = controlModel.getBodyVelocitiesGlobal()
        linkAngVelocities = controlModel.getBodyAngVelocitiesGlobal()
        linkInertias = controlModel.getBodyInertiasGlobal()

        jointPositions = controlModel.getJointPositionsGlobal()
        jointAxeses = controlModel.getDOFAxeses()

        CM = yrp.getCM(linkPositions, linkMasses, totalMass)
        dCM = yrp.getCM(linkVelocities, linkMasses, totalMass)
        CM_plane = copy.copy(CM)
        CM_plane[1] = 0.
        dCM_plane = copy.copy(dCM)
        dCM_plane[1] = 0.

        P = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions, CM,
                                     linkInertias)
        dP = ymt.getPureInertiaMatrixDerivative(dTO, linkMasses,
                                                linkVelocities, dCM,
                                                linkAngVelocities,
                                                linkInertias)

        # calculate jacobian
        Jsys, dJsys = controlModel.computeCom_J_dJdq()
        JsupL = Jsys[6 * supL:6 * supL + 6, :]
        dJsupL = dJsys[6 * supL:6 * supL + 6]
        JsupR = Jsys[6 * supR:6 * supR + 6, :]
        dJsupR = dJsys[6 * supR:6 * supR + 6]

        # calculate contact state
        # if g_initFlag == 1 and contact == 1 and refFootR[1] < doubleTosingleOffset and footCenterR[1] < 0.08:
        if g_initFlag == 1:
            # contact state
            # 0: flying 1: right only 2: left only 3: double
            # if contact == 2 and refFootR[1] < doubleTosingleOffset:
            if contact == 2 and contactR == 1:
                contact = 3
                maxContactChangeCount += 30
                contactChangeCount += maxContactChangeCount
                contactChangeType = 'StoD'

            # elif contact == 3 and refFootL[1] < doubleTosingleOffset:
            elif contact == 1 and contactL == 1:
                contact = 3
                maxContactChangeCount += 30
                contactChangeCount += maxContactChangeCount
                contactChangeType = 'StoD'

            # elif contact == 3 and refFootR[1] > doubleTosingleOffset:
            elif contact == 3 and contactR == 0:
                contact = 2
                contactChangeCount += maxContactChangeCount
                contactChangeType = 'DtoS'

            # elif contact == 3 and refFootL[1] > doubleTosingleOffset:
            elif contact == 3 and contactL == 0:
                contact = 1
                contactChangeCount += maxContactChangeCount
                contactChangeType = 'DtoS'

            else:
                contact = 0
                # if refFootR[1] < doubleTosingleOffset:
                if contactR == 1:
                    contact += 1
                # if refFootL[1] < doubleTosingleOffset:
                if contactL == 1:
                    contact += 2

        # initialization
        if g_initFlag == 0:
            JsysPre = Jsys.copy()
            JsupPreL = JsupL.copy()
            JsupPreR = JsupR.copy()
            JconstPre = Jconst.copy()
            softConstPoint = footCenterR.copy()
            # yjc.computeJacobian2(JsysPre, DOFs, jointPositions, jointAxeses, linkPositions, allLinkJointMasks)
            # yjc.computeJacobian2(JsupPreL, DOFs, jointPositions, jointAxeses, [footCenterL], supLJointMasks)
            # yjc.computeJacobian2(JsupPreR, DOFs, jointPositions, jointAxeses, [footCenterR], supRJointMasks)
            # yjc.computeJacobian2(JconstPre, DOFs, jointPositions, jointAxeses, [softConstPoint], constJointMasks)

            footCenter = footCenterL + (footCenterR - footCenterL) / 2.0
            footCenter[1] = 0.
            preFootCenter = footCenter.copy()
            # footToBodyFootRotL = np.dot(np.transpose(footOriL), footBodyOriL)
            # footToBodyFootRotR = np.dot(np.transpose(footOriR), footBodyOriR)

            if refFootR[1] < doubleTosingleOffset:
                contact += 1
            if refFootL[1] < doubleTosingleOffset:
                contact += 2

            g_initFlag = 1

        # calculate footCenter
        footCenter = footCenterL + (footCenterR - footCenterL) / 2.0
        # if refFootR[1] >doubleTosingleOffset:
        # if refFootR[1] > doubleTosingleOffset or footCenterR[1] > 0.08:
        # if contact == 1 or footCenterR[1] > 0.08:
        # if contact == 2 or footCenterR[1] > doubleTosingleOffset/2:
        if contact == 2:
            footCenter = footCenterL.copy()
        # elif contact == 1 or footCenterL[1] > doubleTosingleOffset/2:
        if contact == 1:
            footCenter = footCenterR.copy()
        footCenter[1] = 0.

        if contactChangeCount > 0 and contactChangeType == 'StoD':
            # change footcenter gradually
            footCenter = preFootCenter + (
                maxContactChangeCount - contactChangeCount) * (
                    footCenter - preFootCenter) / maxContactChangeCount

        preFootCenter = footCenter.copy()

        # linear momentum
        # TODO:
        # We should consider dCM_ref, shouldn't we?
        # add getBodyPositionGlobal and getBodyPositionsGlobal in csVpModel!
        # todo that, set joint velocities to vpModel
        CM_ref_plane = footCenter
        dL_des_plane = Kl * totalMass * (CM_ref_plane -
                                         CM_plane) - Dl * totalMass * dCM_plane
        # dL_des_plane[1] = 0.

        # angular momentum
        CP_ref = footCenter
        bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(
            bodyIDsToCheck, mus, Ks, Ds)
        # bodyIDs, contactPositions, contactPositionLocals, contactForces, contactVelocities = vpWorld.calcManyPenaltyForce(0, bodyIDsToCheck, mus, Ks, Ds)
        CP = yrp.getCP(contactPositions, contactForces)
        if CP_old[0] is None or CP is None:
            dCP = None
        else:
            dCP = (CP - CP_old[0]) / (1 / 30.)
        CP_old[0] = CP

        if CP is not None and dCP is not None:
            ddCP_des = Kh * (CP_ref - CP) - Dh * (dCP)
            CP_des = CP + dCP * (1 / 30.) + .5 * ddCP_des * ((1 / 30.)**2)
            dH_des = np.cross(
                (CP_des - CM),
                (dL_des_plane + totalMass * mm.s2v(wcfg.gravity)))
            if contactChangeCount > 0:  # and contactChangeType == 'DtoS':
                # dH_des *= (maxContactChangeCount - contactChangeCount)/(maxContactChangeCount*10)
                dH_des *= (maxContactChangeCount -
                           contactChangeCount) / (maxContactChangeCount)
                # dH_des *= (contactChangeCount)/(maxContactChangeCount)*.9+.1
        else:
            dH_des = None
        # H = np.dot(P, np.dot(Jsys, dth_flat))
        # dH_des = -Kh* H[3:]

        # soft point constraint
        #softConstPoint = refFootR.copy()
        ##softConstPoint[0] += 0.2
        #Ksc = 50
        #Dsc = 2*(Ksc**.5)
        #Bsc = 1.

        #P_des = softConstPoint
        #P_cur = controlModel.getBodyPositionGlobal(constBody)
        #dP_des = [0, 0, 0]
        #dP_cur = controlModel.getBodyVelocityGlobal(constBody)
        #ddP_des1 = Ksc*(P_des - P_cur) + Dsc*(dP_des - dP_cur)

        #r = P_des - P_cur
        #I = np.vstack(([1,0,0],[0,1,0],[0,0,1]))
        #Z = np.hstack((I, mm.getCrossMatrixForm(-r)))

        #yjc.computeJacobian2(Jconst, DOFs, jointPositions, jointAxeses, [softConstPoint], constJointMasks)
        #dJconst = (Jconst - Jconst)/(1/30.)
        #JconstPre = Jconst.copy()
        ##yjc.computeJacobianDerivative2(dJconst, DOFs, jointPositions, jointAxeses, linkAngVelocities, [softConstPoint], constJointMasks, False)

        #JL, JA = np.vsplit(Jconst, 2)
        #Q1 = np.dot(Z, Jconst)

        #q1 = np.dot(JA, dth_flat)
        #q2 = np.dot(mm.getCrossMatrixForm(q1), np.dot(mm.getCrossMatrixForm(q1), r))
        #q_bias1 = np.dot(np.dot(Z, dJconst), dth_flat) + q2

        #set up equality constraint
        a_oriL = mm.logSO3(
            mm.getSO3FromVectors(np.dot(footBodyOriL, np.array([0, 1, 0])),
                                 np.array([0, 1, 0])))
        a_oriR = mm.logSO3(
            mm.getSO3FromVectors(np.dot(footBodyOriR, np.array([0, 1, 0])),
                                 np.array([0, 1, 0])))

        #if contact == 3 and contactChangeCount < maxContactChangeCount/4 and contactChangeCount >=1:
        #kt_sup = 30
        #viewer.objectInfoWnd.labelSupKt.value(kt_sup)
        #viewer.objectInfoWnd.sliderSupKt.value(initSupKt*10)

        # a_supL = np.append(kt_sup*(refFootL - footCenterL + contMotionOffset) + dt_sup*(refFootVelL - footBodyVelL), kt_sup*a_oriL+dt_sup*(refFootAngVelL-footBodyAngVelL))
        # a_supR = np.append(kt_sup*(refFootR - footCenterR + contMotionOffset) + dt_sup*(refFootVelR - footBodyVelR), kt_sup*a_oriR+dt_sup*(refFootAngVelR-footBodyAngVelR))
        a_supL = np.append(
            kt_sup * (refFootL - footCenterL + contMotionOffset) -
            dt_sup * footBodyVelL, kt_sup * a_oriL - dt_sup * footBodyAngVelL)
        a_supR = np.append(
            kt_sup * (refFootR - footCenterR + contMotionOffset) -
            dt_sup * footBodyVelR, kt_sup * a_oriR - dt_sup * footBodyAngVelR)

        if contactChangeCount > 0 and contactChangeType == 'DtoS':
            #refFootR += (footCenter-CM_plane)/2.
            #refFootR[1] = 0
            #pre contact value are needed
            #if contact == 2:
            ##refFootR[0] += 0.2
            ##refFootR[2] -= 0.05
            #offsetDropR = (footCenter-CM_plane)/2.
            #refFootR += offsetDropR
            #refFootR[1] = 0.
            ##refFootR[2] = footCenterR[2] - contMotionOffset[2]
            ##refFootR[0] = footCenterR[0] - contMotionOffset[0]
            #refFootL[0] += 0.05
            #refFootL[2] -= 0.05
            #elif contact == 1:
            #offsetDropL = (footCenter-CM_plane)/2.
            #refFootL += offsetDropL
            #refFootL[1] = 0.
            #a_supL = np.append(kt_sup*(refFootL - footCenterL + contMotionOffset) + dt_sup*(refFootVelL - footBodyVelL), kt_sup*a_oriL+dt_sup*(refFootAngVelL-footBodyAngVelL))
            #a_supR = np.append(kt_sup*(refFootR - footCenterR + contMotionOffset) + dt_sup*(refFootVelR - footBodyVelR), kt_sup*a_oriR+dt_sup*(refFootAngVelR-footBodyAngVelR))
            #a_supL = np.append(kt_sup*(refFootL - footCenterL + contMotionOffset) + dt_sup*(refFootVelL - footBodyVelL), 16*kt_sup*a_oriL+4*dt_sup*(refFootAngVelL-footBodyAngVelL))
            #a_supR = np.append(kt_sup*(refFootR - footCenterR + contMotionOffset) + dt_sup*(refFootVelR - footBodyVelR), 16*kt_sup*a_oriR+4*dt_sup*(refFootAngVelR-footBodyAngVelR))
            a_supL = np.append(
                kt_sup * (refFootL - footCenterL + contMotionOffset) + dt_sup *
                (refFootVelL - footBodyVelL), 4 * kt_sup * a_oriL +
                2 * dt_sup * (refFootAngVelL - footBodyAngVelL))
            a_supR = np.append(
                kt_sup * (refFootR - footCenterR + contMotionOffset) + dt_sup *
                (refFootVelR - footBodyVelR), 4 * kt_sup * a_oriR +
                2 * dt_sup * (refFootAngVelR - footBodyAngVelR))
        elif contactChangeCount > 0 and contactChangeType == 'StoD':
            #refFootR[0] +=0.05
            #refFootR[2] +=0.05
            linkt = (13. * contactChangeCount) / (maxContactChangeCount) + 1.
            lindt = 2 * (linkt**.5)
            angkt = (13. * contactChangeCount) / (maxContactChangeCount) + 1.
            angdt = 2 * (angkt**.5)
            #a_supL = np.append(4*kt_sup*(refFootL - footCenterL + contMotionOffset) + 2*dt_sup*(refFootVelL - footBodyVelL), 16*kt_sup*a_oriL+4*dt_sup*(refFootAngVelL-footBodyAngVelL))
            #a_supR = np.append(4*kt_sup*(refFootR - footCenterR + contMotionOffset) + 2*dt_sup*(refFootVelR - footBodyVelR), 16*kt_sup*a_oriR+4*dt_sup*(refFootAngVelR-footBodyAngVelR))
            a_supL = np.append(
                linkt * kt_sup * (refFootL - footCenterL + contMotionOffset) +
                lindt * dt_sup * (refFootVelL - footBodyVelL),
                angkt * kt_sup * a_oriL + angdt * dt_sup *
                (refFootAngVelL - footBodyAngVelL))
            a_supR = np.append(
                linkt * kt_sup * (refFootR - footCenterR + contMotionOffset) +
                lindt * dt_sup * (refFootVelR - footBodyVelR),
                angkt * kt_sup * a_oriR + angdt * dt_sup *
                (refFootAngVelR - footBodyAngVelR))
            #a_supL = np.append(16*kt_sup*(refFootL - footCenterL + contMotionOffset) + 4*dt_sup*(refFootVelL - footBodyVelL), 16*kt_sup*a_oriL+4*dt_sup*(refFootAngVelL-footBodyAngVelL))
            #a_supR = np.append(16*kt_sup*(refFootR - footCenterR + contMotionOffset) + 4*dt_sup*(refFootVelR - footBodyVelR), 16*kt_sup*a_oriR+4*dt_sup*(refFootAngVelR-footBodyAngVelR))
            #a_supL = np.append(4*kt_sup*(refFootL - footCenterL + contMotionOffset) + 2*dt_sup*(refFootVelL - footBodyVelL), 32*kt_sup*a_oriL+5.6*dt_sup*(refFootAngVelL-footBodyAngVelL))
            #a_supR = np.append(4*kt_sup*(refFootR - footCenterR + contMotionOffset) + 2*dt_sup*(refFootVelR - footBodyVelR), 32*kt_sup*a_oriR+5.6*dt_sup*(refFootAngVelR-footBodyAngVelR))
            #a_supL[1] = kt_sup*(refFootL[1] - footCenterL[1] + contMotionOffset[1]) + dt_sup*(refFootVelL[1] - footBodyVelL[1])
            #a_supR[1] = kt_sup*(refFootR[1] - footCenterR[1] + contMotionOffset[1]) + dt_sup*(refFootVelR[1] - footBodyVelR[1])

        ##if contact == 2:
        #if refFootR[1] <doubleTosingleOffset :
        #Jsup = np.vstack((JsupL, JsupR))
        #dJsup = np.vstack((dJsupL, dJsupR))
        #a_sup = np.append(a_supL, a_supR)
        #else:
        #Jsup = JsupL.copy()
        #dJsup = dJsupL.copy()
        #a_sup = a_supL.copy()

        # momentum matrix
        RS = np.dot(P, Jsys)
        R, S = np.vsplit(RS, 2)

        # rs = np.dot((np.dot(dP, Jsys) + np.dot(P, dJsys)), dth_flat)
        rs = np.dot(dP, np.dot(Jsys, dth_flat)) + np.dot(P, dJsys)
        r_bias, s_bias = np.hsplit(rs, 2)

        #######################################################
        # optimization
        #######################################################
        #if contact == 2 and footCenterR[1] > doubleTosingleOffset/2:
        if contact == 2:
            config['weightMap']['RightUpLeg'] = .8
            config['weightMap']['RightLeg'] = .8
            config['weightMap']['RightFoot'] = .8
        else:
            config['weightMap']['RightUpLeg'] = .1
            config['weightMap']['RightLeg'] = .25
            config['weightMap']['RightFoot'] = .2

        #if contact == 1 and footCenterL[1] > doubleTosingleOffset/2:
        if contact == 1:
            config['weightMap']['LeftUpLeg'] = .8
            config['weightMap']['LeftLeg'] = .8
            config['weightMap']['LeftFoot'] = .8
        else:
            config['weightMap']['LeftUpLeg'] = .1
            config['weightMap']['LeftLeg'] = .25
            config['weightMap']['LeftFoot'] = .2

        w = mot.getTrackingWeight(DOFs, motion[0].skeleton,
                                  config['weightMap'])

        #if contact == 2:
        #mot.addSoftPointConstraintTerms(problem, totalDOF, Bsc, ddP_des1, Q1, q_bias1)
        mot.addTrackingTerms(problem, totalDOF, Bt, w, ddth_des_flat)
        if dH_des is not None:
            mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R, r_bias)
            mot.addAngularTerms(problem, totalDOF, Bh, dH_des, S, s_bias)

            #if contact & 1 and contactChangeCount == 0:
            if contact & 1:
                #if refFootR[1] < doubleTosingleOffset:
                mot.addConstraint2(problem, totalDOF, JsupR, dJsupR, dth_flat,
                                   a_supR)
            if contact & 2:
                #if refFootL[1] < doubleTosingleOffset:
                mot.addConstraint2(problem, totalDOF, JsupL, dJsupL, dth_flat,
                                   a_supL)

        if contactChangeCount > 0:
            contactChangeCount -= 1
            if contactChangeCount == 0:
                maxContactChangeCount = 30
                contactChangeType = 0

        r = problem.solve()
        problem.clear()
        ype.nested(r['x'], ddth_sol)

        rootPos[0] = controlModel.getBodyPositionGlobal(selectedBody)
        localPos = [[0, 0, 0]]

        for i in range(stepsPerFrame):
            # apply penalty force
            bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(
                bodyIDsToCheck, mus, Ks, Ds)
            # print(contactForces)
            #bodyIDs, contactPositions, contactPositionLocals, contactForces, contactVelocities = vpWorld.calcManyPenaltyForce(0, bodyIDsToCheck, mus, Ks, Ds)
            vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals,
                                      contactForces)

            controlModel.setDOFAccelerations(ddth_sol)
            controlModel.solveHybridDynamics()

            if forceShowTime > viewer.objectInfoWnd.labelForceDur.value():
                forceShowTime = 0
                viewer_ResetForceState()

            forceforce = np.array([
                viewer.objectInfoWnd.labelForceX.value(),
                viewer.objectInfoWnd.labelForceY.value(),
                viewer.objectInfoWnd.labelForceZ.value()
            ])
            extraForce[0] = getParamVal('Fm') * mm.normalize2(forceforce)
            # extraForce[0] = viewer.objectInfoWnd.labelFm.value() * mm.normalize2(forceforce)
            if viewer_GetForceState():
                forceShowTime += wcfg.timeStep
                vpWorld.applyPenaltyForce(selectedBodyId, localPos, extraForce)

            vpWorld.step()

        # rendering
        rightFootVectorX[0] = np.dot(footOriL, np.array([.1, 0, 0]))
        rightFootVectorY[0] = np.dot(footOriL, np.array([0, .1, 0]))
        rightFootVectorZ[0] = np.dot(footOriL, np.array([0, 0, .1]))
        rightFootPos[0] = footCenterL

        rightVectorX[0] = np.dot(footBodyOriL, np.array([.1, 0, 0]))
        rightVectorY[0] = np.dot(footBodyOriL, np.array([0, .1, 0]))
        rightVectorZ[0] = np.dot(footBodyOriL, np.array([0, 0, .1]))
        rightPos[0] = footCenterL + np.array([.1, 0, 0])

        rd_footCenter[0] = footCenter
        rd_footCenterL[0] = footCenterL
        rd_footCenterR[0] = footCenterR

        rd_CM[0] = CM

        rd_CM_plane[0] = CM.copy()
        rd_CM_plane[0][1] = 0.

        if CP is not None and dCP is not None:
            rd_CP[0] = CP
            rd_CP_des[0] = CP_des

            rd_dL_des_plane[0] = [
                dL_des_plane[0] / 100, dL_des_plane[1] / 100,
                dL_des_plane[2] / 100
            ]
            rd_dH_des[0] = dH_des

            rd_grf_des[0] = dL_des_plane - totalMass * mm.s2v(wcfg.gravity)

        rd_root_des[0] = rootPos[0]

        del rd_CF[:]
        del rd_CF_pos[:]
        for i in range(len(contactPositions)):
            rd_CF.append(contactForces[i] / 400)
            rd_CF_pos.append(contactPositions[i].copy())

        if viewer_GetForceState():
            rd_exfen_des[0] = [
                extraForce[0][0] / 100, extraForce[0][1] / 100,
                extraForce[0][2] / 100
            ]
            rd_exf_des[0] = [0, 0, 0]
        else:
            rd_exf_des[0] = [
                extraForce[0][0] / 100, extraForce[0][1] / 100,
                extraForce[0][2] / 100
            ]
            rd_exfen_des[0] = [0, 0, 0]

        extraForcePos[0] = controlModel.getBodyPositionGlobal(selectedBody)
    def simulateCallback(frame):
        # print(frame)
        # print(motion[frame].getJointOrientationLocal(footIdDic['RightFoot_foot_0_1_0']))
        if frame == start_frame:
            viewer.force_on = True
        if False:
            if frame == start_frame:
                setParamVal('Fm', 35)
                viewer.force_on = True
            if frame == start_frame + 150:
                setParamVal('Fm', 40)
                viewer.force_on = True
            if frame == start_frame + 300:
                setParamVal('Fm', 45)
                viewer.force_on = True
            if frame == start_frame + 450:
                setParamVal('Fm', 50)
                viewer.force_on = True
            if frame == start_frame + 600:
                setParamVal('Fm', 55)
                viewer.force_on = True
            if frame == start_frame + 750:
                setParamVal('Fm', 60)
                viewer.force_on = True

        # hfi.footAdjust(motion[frame], idDic, SEGMENT_FOOT_MAG=.03, SEGMENT_FOOT_RAD=.015, baseHeight=0.02)

        if abs(getParamVal('tiptoe angle')) > 0.001:
            tiptoe_angle = getParamVal('tiptoe angle')
            motion[frame].mulJointOrientationLocal(idDic['LeftFoot_foot_0_0_0'],
                                                   mm.exp(mm.unitX(), -math.pi * tiptoe_angle))
            motion[frame].mulJointOrientationLocal(idDic['LeftFoot_foot_0_1_0'],
                                                   mm.exp(mm.unitX(), -math.pi * tiptoe_angle))
            motion[frame].mulJointOrientationLocal(idDic['RightFoot_foot_0_0_0'],
                                                   mm.exp(mm.unitX(), -math.pi * tiptoe_angle))
            motion[frame].mulJointOrientationLocal(idDic['RightFoot_foot_0_1_0'],
                                                   mm.exp(mm.unitX(), -math.pi * tiptoe_angle))
            # motion[frame].mulJointOrientationLocal(idDic['LeftFoot'], mm.exp(mm.unitX(), math.pi * tiptoe_angle * 0.95))
            # motion[frame].mulJointOrientationLocal(idDic['RightFoot'], mm.exp(mm.unitX(), math.pi * tiptoe_angle * 0.95))
            motion[frame].mulJointOrientationLocal(idDic['LeftFoot'], mm.exp(mm.unitX(), math.pi * tiptoe_angle))
            motion[frame].mulJointOrientationLocal(idDic['RightFoot'], mm.exp(mm.unitX(), math.pi * tiptoe_angle))

        if getParamVal('left tilt angle') > 0.001:
            left_tilt_angle = getParamVal('left tilt angle')
            if motion[0].skeleton.getJointIndex('LeftFoot_foot_0_1') is not None:
                motion[frame].mulJointOrientationLocal(idDic['LeftFoot_foot_0_1'], mm.exp(mm.unitZ(), -math.pi * left_tilt_angle))
            else:
                motion[frame].mulJointOrientationLocal(idDic['LeftFoot_foot_0_1_0'], mm.exp(mm.unitZ(), -math.pi * left_tilt_angle))
            motion[frame].mulJointOrientationLocal(idDic['LeftFoot'], mm.exp(mm.unitZ(), math.pi * left_tilt_angle))

        elif getParamVal('left tilt angle') < -0.001:
            left_tilt_angle = getParamVal('left tilt angle')
            motion[frame].mulJointOrientationLocal(idDic['LeftFoot_foot_0_0'], mm.exp(mm.unitZ(), -math.pi * left_tilt_angle))
            if motion[0].skeleton.getJointIndex('LeftFoot_foot_0_1') is not None:
                motion[frame].mulJointOrientationLocal(idDic['LeftFoot_foot_0_1'], mm.exp(mm.unitZ(), math.pi * left_tilt_angle))
            else:
                motion[frame].mulJointOrientationLocal(idDic['LeftFoot_foot_0_1_0'], mm.exp(mm.unitZ(), math.pi * left_tilt_angle))
            motion[frame].mulJointOrientationLocal(idDic['LeftFoot'], mm.exp(mm.unitZ(), math.pi * left_tilt_angle))

        if getParamVal('right tilt angle') > 0.001:
            right_tilt_angle = getParamVal('right tilt angle')
            if motion[0].skeleton.getJointIndex('RightFoot_foot_0_1') is not None:
                motion[frame].mulJointOrientationLocal(idDic['RightFoot_foot_0_1'], mm.exp(mm.unitZ(), math.pi * right_tilt_angle))
            else:
                motion[frame].mulJointOrientationLocal(idDic['RightFoot_foot_0_1_0'], mm.exp(mm.unitZ(), math.pi * right_tilt_angle))
            motion[frame].mulJointOrientationLocal(idDic['RightFoot'], mm.exp(mm.unitZ(), -math.pi * right_tilt_angle))
        elif getParamVal('right tilt angle') < -0.001:
            right_tilt_angle = getParamVal('right tilt angle')
            motion[frame].mulJointOrientationLocal(idDic['RightFoot_foot_0_0'], mm.exp(mm.unitZ(), math.pi * right_tilt_angle))
            if motion[0].skeleton.getJointIndex('RightFoot_foot_0_1') is not None:
                motion[frame].mulJointOrientationLocal(idDic['RightFoot_foot_0_1'], mm.exp(mm.unitZ(), -math.pi * right_tilt_angle))
            # else:
            #     motion[frame].mulJointOrientationLocal(idDic['RightFoot_foot_0_1_0'], mm.exp(mm.unitZ(), -math.pi * right_tilt_angle))
            motion[frame].mulJointOrientationLocal(idDic['RightFoot'], mm.exp(mm.unitZ(), -math.pi * right_tilt_angle))

        motionModel.update(motion[frame])
        motionModel.translateByOffset(np.array([getParamVal('com X offset'), getParamVal('com Y offset'), getParamVal('com Z offset')]))
        controlModel_ik.set_q(controlModel.get_q())

        global g_initFlag
        global forceShowTime

        global JsysPre
        global JsupPreL
        global JsupPreR

        global JconstPre

        global preFootCenter
        global maxContactChangeCount
        global contactChangeCount
        global contact
        global contactChangeType

        Kt, Kl, Kh, Bl, Bh, kt_sup = getParamVals(['Kt', 'Kl', 'Kh', 'Bl', 'Bh', 'SupKt'])
        Dt = 2*(Kt**.5)
        Dl = 2*(Kl**.5)
        Dh = 2*(Kh**.5)
        dt_sup = 2*(kt_sup**.5)

        # tracking
        th_r = motion.getDOFPositions(frame)
        th = controlModel.getDOFPositions()
        dth_r = motion.getDOFVelocities(frame)
        dth = controlModel.getDOFVelocities()
        ddth_r = motion.getDOFAccelerations(frame)
        ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt)

        # ype.flatten(fix_dofs(DOFs, ddth_des, mcfg, joint_names), ddth_des_flat)
        # ype.flatten(fix_dofs(DOFs, dth, mcfg, joint_names), dth_flat)
        ype.flatten(ddth_des, ddth_des_flat)
        ype.flatten(dth, dth_flat)

        #################################################
        # jacobian
        #################################################

        contact_des_ids = list()  # desired contact segments
        if foot_viewer.check_op_l.value():
            contact_des_ids.append(motion[0].skeleton.getJointIndex('LeftToes'))
        if foot_viewer.check_h_l.value():
            contact_des_ids.append(motion[0].skeleton.getJointIndex('LeftFoot'))

        if foot_viewer.check_op_r.value():
            contact_des_ids.append(motion[0].skeleton.getJointIndex('RightToes'))
        if foot_viewer.check_h_r.value():
            contact_des_ids.append(motion[0].skeleton.getJointIndex('RightFoot'))

        contact_ids = list()  # temp idx for balancing
        contact_ids.extend(contact_des_ids)

        contact_joint_ori = list(map(controlModel.getJointOrientationGlobal, contact_ids))
        contact_joint_pos = list(map(controlModel.getJointPositionGlobal, contact_ids))
        contact_body_ori = list(map(controlModel.getBodyOrientationGlobal, contact_ids))
        contact_body_pos = list(map(controlModel.getBodyPositionGlobal, contact_ids))
        contact_body_vel = list(map(controlModel.getBodyVelocityGlobal, contact_ids))
        contact_body_angvel = list(map(controlModel.getBodyAngVelocityGlobal, contact_ids))

        ref_joint_ori = list(map(motion[frame].getJointOrientationGlobal, contact_ids))
        ref_joint_pos = list(map(motion[frame].getJointPositionGlobal, contact_ids))
        ref_joint_vel = [motion.getJointVelocityGlobal(joint_idx, frame) for joint_idx in contact_ids]
        ref_joint_angvel = [motion.getJointAngVelocityGlobal(joint_idx, frame) for joint_idx in contact_ids]
        ref_body_ori = list(map(motionModel.getBodyOrientationGlobal, contact_ids))
        ref_body_pos = list(map(motionModel.getBodyPositionGlobal, contact_ids))
        # ref_body_vel = list(map(controlModel.getBodyVelocityGlobal, contact_ids))
        ref_body_angvel = [motion.getJointAngVelocityGlobal(joint_idx, frame) for joint_idx in contact_ids]
        ref_body_vel = [ref_joint_vel[i] + np.cross(ref_joint_angvel[i], ref_body_pos[i] - ref_joint_pos[i])
                        for i in range(len(ref_joint_vel))]

        is_contact = [1] * len(contact_ids)
        contact_right = len(set(contact_des_ids).intersection(rIDlist)) > 0
        contact_left = len(set(contact_des_ids).intersection(lIDlist)) > 0

        contMotionOffset = th[0][0] - th_r[0][0]

        linkPositions = controlModel.getBodyPositionsGlobal()
        linkVelocities = controlModel.getBodyVelocitiesGlobal()
        linkAngVelocities = controlModel.getBodyAngVelocitiesGlobal()
        linkInertias = controlModel.getBodyInertiasGlobal()

        CM = yrp.getCM(linkPositions, linkMasses, totalMass)
        dCM = yrp.getCM(linkVelocities, linkMasses, totalMass)
        CM_plane = copy.copy(CM)
        CM_plane[1] = 0.
        dCM_plane = copy.copy(dCM)
        dCM_plane[1] = 0.

        P = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions, CM, linkInertias)
        dP = ymt.getPureInertiaMatrixDerivative(dTO, linkMasses, linkVelocities, dCM, linkAngVelocities, linkInertias)

        # calculate jacobian
        Jsys, dJsys = controlModel.computeCom_J_dJdq()
        J_contacts = []  # type: list[np.ndarray]
        dJ_contacts = []  # type: list[np.ndarray]
        for contact_id in contact_ids:
            J_contacts.append(Jsys[6*contact_id:6*contact_id + 6, :])
            dJ_contacts.append(dJsys[6*contact_id:6*contact_id + 6])

        # calculate footCenter
        footCenter = sum(contact_body_pos) / len(contact_body_pos) if len(contact_body_pos) > 0 \
            else .5 * (controlModel.getBodyPositionGlobal(supL) + controlModel.getBodyPositionGlobal(supR))
        footCenter[1] = 0.
        # if len(contact_body_pos) > 2:
        #     hull = ConvexHull(contact_body_pos)

        footCenter_ref = sum(ref_body_pos) / len(ref_body_pos) if len(ref_body_pos) > 0 \
            else .5 * (motionModel.getBodyPositionGlobal(supL) + motionModel.getBodyPositionGlobal(supR))
        footCenter_ref = footCenter_ref + contMotionOffset
        # if len(ref_body_pos) > 2:
        #     hull = ConvexHull(ref_body_pos)
        footCenter_ref[1] = 0.

        # footCenter[0] = footCenter[0] + getParamVal('com X offset')
        # footCenter[1] = footCenter[0] + getParamVal('com Y offset')
        # footCenter[2] = footCenter[2] + getParamVal('com Z offset')

        # initialization
        if g_initFlag == 0:
            preFootCenter[0] = footCenter.copy()
            g_initFlag = 1

        # if contactChangeCount == 0 and np.linalg.norm(footCenter - preFootCenter[0]) > 0.01:
        #     contactChangeCount += 30
        if contactChangeCount > 0:
            # change footcenter gradually
            footCenter = preFootCenter[0] + (maxContactChangeCount - contactChangeCount)*(footCenter-preFootCenter[0])/maxContactChangeCount
        else:
            preFootCenter[0] = footCenter.copy()

        # linear momentum
        # TODO:
        # We should consider dCM_ref, shouldn't we?
        # add getBodyPositionGlobal and getBodyPositionsGlobal in csVpModel!
        # to do that, set joint velocities to vpModel
        CM_ref_plane = footCenter
        # CM_ref_plane = footCenter_ref
        CM_ref = footCenter + np.array([getParamVal('com X offset'), motionModel.getCOM()[1] + getParamVal('com Y offset'), getParamVal('com Z offset')])
        dL_des_plane = Kl * totalMass * (CM_ref - CM) - Dl * totalMass * dCM
        # dL_des_plane = Kl * totalMass * (CM_ref_plane - CM_plane) - Dl * totalMass * dCM_plane
        # dL_des_plane[1] = 0.
        # print('dCM_plane : ', np.linalg.norm(dCM_plane))

        # angular momentum
        CP_ref = footCenter
        # CP_ref = footCenter_ref
        bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(bodyIDsToCheck, mus, Ks, Ds)
        CP = yrp.getCP(contactPositions, contactForces)
        if CP_old[0] is None or CP is None:
            dCP = None
        else:
            dCP = (CP - CP_old[0])/(1/30.)
        CP_old[0] = CP

        if CP is not None and dCP is not None:
            ddCP_des = Kh*(CP_ref - CP) - Dh * dCP
            dCP_des = dCP + ddCP_des * (1/30.)
            CP_des = CP + dCP_des * (1/30.)
            # CP_des = footCenter
            CP_des = CP + dCP*(1/30.) + .5*ddCP_des*((1/30.)**2)
            dH_des = np.cross((CP_des - CM), (dL_des_plane + totalMass * mm.s2v(wcfg.gravity)))
            if contactChangeCount > 0:  # and contactChangeType == 'DtoS':
                dH_des *= (maxContactChangeCount - contactChangeCount)/maxContactChangeCount
        else:
            dH_des = None

        # convex hull
        contact_pos_2d = np.asarray([np.array([contactPosition[0], contactPosition[2]]) for contactPosition in contactPositions])
        p = np.array([CM_plane[0], CM_plane[2]])
        # hull = None  # type: Delaunay
        # if contact_pos_2d.shape[0] > 0:
        #     hull = Delaunay(contact_pos_2d)
        #     print(hull.find_simplex(p) >= 0)

        # set up equality constraint
        # TODO:
        # logSO3 is just q'', not acceleration.
        # To make a_oris acceleration, q'' -> a will be needed
        # body_ddqs = list(map(mm.logSO3, [mm.getSO3FromVectors(np.dot(body_ori, mm.unitY()), mm.unitY()) for body_ori in contact_body_ori]))
        # body_ddqs = list(map(mm.logSO3, [np.dot(contact_body_ori[i].T, np.dot(ref_body_ori[i], mm.getSO3FromVectors(np.dot(ref_body_ori[i], mm.unitY()), mm.unitY()))) for i in range(len(contact_body_ori))]))
        # body_ddqs = list(map(mm.logSO3, [np.dot(contact_body_ori[i].T, np.dot(ref_body_ori[i], mm.getSO3FromVectors(np.dot(ref_body_ori[i], up_vec_in_each_link[contact_ids[i]]), mm.unitY()))) for i in range(len(contact_body_ori))]))
        a_oris = list(map(mm.logSO3, [np.dot(contact_body_ori[i].T, np.dot(ref_body_ori[i], mm.getSO3FromVectors(np.dot(ref_body_ori[i], up_vec_in_each_link[contact_ids[i]]), mm.unitY()))) for i in range(len(contact_body_ori))]))
        a_oris = list(map(mm.logSO3, [np.dot(np.dot(ref_body_ori[i], mm.getSO3FromVectors(np.dot(ref_body_ori[i], up_vec_in_each_link[contact_ids[i]]), mm.unitY())), contact_body_ori[i].T) for i in range(len(contact_body_ori))]))
        body_qs = list(map(mm.logSO3, contact_body_ori))
        body_angs = [np.dot(contact_body_ori[i], contact_body_angvel[i]) for i in range(len(contact_body_ori))]
        body_dqs = [mm.vel2qd(body_angs[i], body_qs[i]) for i in range(len(body_angs))]
        # a_oris = [np.dot(contact_body_ori[i], mm.qdd2accel(body_ddqs[i], body_dqs[i], body_qs[i])) for i in range(len(contact_body_ori))]

        # body_ddq = body_ddqs[0]
        # body_ori = contact_body_ori[0]
        # body_ang = np.dot(body_ori.T, contact_body_angvel[0])
        #
        # body_q = mm.logSO3(body_ori)
        # body_dq = mm.vel2qd(body_ang, body_q)
        # a_ori = np.dot(body_ori, mm.qdd2accel(body_ddq, body_dq, body_q))

        KT_SUP = np.diag([kt_sup/10., kt_sup, kt_sup/10.])
        # KT_SUP = np.diag([kt_sup, kt_sup, kt_sup])

        # a_oris = list(map(mm.logSO3, [mm.getSO3FromVectors(np.dot(body_ori, mm.unitY()), mm.unitY()) for body_ori in contact_body_ori]))
        # a_oris = list(map(mm.logSO3, [mm.getSO3FromVectors(np.dot(contact_body_ori[i], up_vec_in_each_link[contact_ids[i]]), mm.unitY()) for i in range(len(contact_body_ori))]))
        # a_sups = [np.append(kt_sup*(ref_body_pos[i] - contact_body_pos[i] + contMotionOffset) + dt_sup*(ref_body_vel[i] - contact_body_vel[i]),
        #                     kt_sup*a_oris[i]+dt_sup*(ref_body_angvel[i]-contact_body_angvel[i])) for i in range(len(a_oris))]
        # a_sups = [np.append(kt_sup*(ref_body_pos[i] - contact_body_pos[i] + contMotionOffset) - dt_sup * contact_body_vel[i],
        #                     kt_sup*a_oris[i] - dt_sup * contact_body_angvel[i]) for i in range(len(a_oris))]
        a_sups = [np.append(np.dot(KT_SUP, (ref_body_pos[i] - contact_body_pos[i] + contMotionOffset)) - dt_sup * contact_body_vel[i],
                            kt_sup*a_oris[i] - dt_sup * contact_body_angvel[i]) for i in range(len(a_oris))]
        # for i in range(len(a_sups)):
        #     a_sups[i][1] = -kt_sup * contact_body_pos[i][1] - dt_sup * contact_body_vel[i][1]

        # momentum matrix
        RS = np.dot(P, Jsys)
        R, S = np.vsplit(RS, 2)

        # rs = np.dot((np.dot(dP, Jsys) + np.dot(P, dJsys)), dth_flat)
        rs = np.dot(dP, np.dot(Jsys, dth_flat)) + np.dot(P, dJsys)
        r_bias, s_bias = np.hsplit(rs, 2)

        #######################################################
        # optimization
        #######################################################
        # if contact == 2 and footCenterR[1] > doubleTosingleOffset/2:
        if contact_left and not contact_right:
            config['weightMap']['RightUpLeg'] = .8
            config['weightMap']['RightLeg'] = .8
            config['weightMap']['RightFoot'] = .8
        else:
            config['weightMap']['RightUpLeg'] = .1
            config['weightMap']['RightLeg'] = .25
            config['weightMap']['RightFoot'] = .2

        # if contact == 1 and footCenterL[1] > doubleTosingleOffset/2:
        if contact_right and not contact_left:
            config['weightMap']['LeftUpLeg'] = .8
            config['weightMap']['LeftLeg'] = .8
            config['weightMap']['LeftFoot'] = .8
        else:
            config['weightMap']['LeftUpLeg'] = .1
            config['weightMap']['LeftLeg'] = .25
            config['weightMap']['LeftFoot'] = .2

        w = mot.getTrackingWeight(DOFs, motion[0].skeleton, config['weightMap'])

        mot.addTrackingTerms(problem, totalDOF, Bt, w, ddth_des_flat)
        if dH_des is not None:
            mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R, r_bias)
            mot.addAngularTerms(problem, totalDOF, Bh, dH_des, S, s_bias)

            if True:
                for c_idx in range(len(contact_ids)):
                    mot.addConstraint2(problem, totalDOF, J_contacts[c_idx], dJ_contacts[c_idx], dth_flat, a_sups[c_idx])

        if contactChangeCount > 0:
            contactChangeCount = contactChangeCount - 1
            if contactChangeCount == 0:
                maxContactChangeCount = 30
                contactChangeType = 0

        r = problem.solve()
        problem.clear()
        ddth_sol_flat = np.asarray(r['x'])
        # ddth_sol_flat[foot_seg_dofs] = np.array(ddth_des_flat)[foot_seg_dofs]
        ype.nested(ddth_sol_flat, ddth_sol)

        rootPos[0] = controlModel.getBodyPositionGlobal(selectedBody)
        localPos = [[0, 0, 0]]

        for i in range(stepsPerFrame):
            # apply penalty force
            bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(bodyIDsToCheck, mus, Ks, Ds)
            # bodyIDs, contactPositions, contactPositionLocals, contactForces, contactVelocities = vpWorld.calcManyPenaltyForce(0, bodyIDsToCheck, mus, Ks, Ds)
            vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals, contactForces)

            controlModel.setDOFAccelerations(ddth_sol)
            # controlModel.setDOFAccelerations(ddth_des)
            # controlModel.set_ddq(ddth_sol_flat)
            # controlModel.set_ddq(ddth_des_flat)
            controlModel.solveHybridDynamics()

            if forceShowTime > viewer.objectInfoWnd.labelForceDur.value():
                forceShowTime = 0
                viewer_ResetForceState()

            forceforce = np.array([viewer.objectInfoWnd.labelForceX.value(), viewer.objectInfoWnd.labelForceY.value(), viewer.objectInfoWnd.labelForceZ.value()])
            extraForce[0] = getParamVal('Fm') * mm.normalize2(forceforce)
            if viewer_GetForceState():
                forceShowTime += wcfg.timeStep
                vpWorld.applyPenaltyForce(selectedBodyId, localPos, extraForce)

            vpWorld.step()

        controlModel_ik.set_q(controlModel.get_q())

        # rendering
        bodyIDs, geomIDs, positionLocalsForGeom = vpWorld.getContactInfoForcePlate(bodyIDsToCheck)
        for foot_seg_id in footIdlist:
            control_model_renderer.body_colors[foot_seg_id] = (255, 240, 255)
            control_model_renderer.geom_colors[foot_seg_id] = [(255, 240, 255)] * controlModel.getBodyGeomNum(foot_seg_id)

        for i in range(len(geomIDs)):
            if controlModel.vpid2index(bodyIDs[i]) in footIdlist:
                control_model_renderer.geom_colors[controlModel.vpid2index(bodyIDs[i])][geomIDs[i]] = (255, 0, 0)
        # for foot_seg_id in footIdlist:
        #     control_model_renderer.body_colors[foot_seg_id] = (255, 240, 255)
        #
        # for contact_id in contact_ids:
        #     control_model_renderer.body_colors[contact_id] = (255, 0, 0)


        rd_footCenter[0] = footCenter
        rd_footCenter_ref[0] = footCenter_ref

        rd_CM[0] = CM

        rd_CM_plane[0] = CM.copy()
        rd_CM_plane[0][1] = 0.

        if CP is not None and dCP is not None:
            rd_CP[0] = CP
            rd_CP_des[0] = CP_des

            rd_dL_des_plane[0] = [dL_des_plane[0]/100, dL_des_plane[1]/100, dL_des_plane[2]/100]
            rd_dH_des[0] = dH_des

            rd_grf_des[0] = dL_des_plane - totalMass * mm.s2v(wcfg.gravity)

        del rd_foot_ori[:]
        del rd_foot_pos[:]
        # for seg_foot_id in footIdlist:
        #     rd_foot_ori.append(controlModel.getJointOrientationGlobal(seg_foot_id))
        #     rd_foot_pos.append(controlModel.getJointPositionGlobal(seg_foot_id))
        rd_foot_ori.append(controlModel.getJointOrientationGlobal(supL))
        rd_foot_ori.append(controlModel.getJointOrientationGlobal(supR))
        rd_foot_pos.append(controlModel.getJointPositionGlobal(supL))
        rd_foot_pos.append(controlModel.getJointPositionGlobal(supR))
        rd_foot_ori.append(controlModel.getJointOrientationGlobal(13))
        rd_foot_ori.append(controlModel.getJointOrientationGlobal(14))
        rd_foot_pos.append(controlModel.getJointPositionGlobal(13))
        rd_foot_pos.append(controlModel.getJointPositionGlobal(14))

        rd_root_des[0] = rootPos[0]
        rd_root_ori[0] = controlModel.getBodyOrientationGlobal(0)
        rd_root_pos[0] = controlModel.getBodyPositionGlobal(0)

        del rd_CF[:]
        del rd_CF_pos[:]
        for i in range(len(contactPositions)):
            rd_CF.append(contactForces[i]/400)
            rd_CF_pos.append(contactPositions[i].copy())

        if viewer_GetForceState():
            rd_exfen_des[0] = [extraForce[0][0]/100, extraForce[0][1]/100, extraForce[0][2]/100]
            rd_exf_des[0] = [0, 0, 0]
        else:
            rd_exf_des[0] = [extraForce[0][0]/100, extraForce[0][1]/100, extraForce[0][2]/100]
            rd_exfen_des[0] = [0, 0, 0]

        # extraForcePos[0] = controlModel.getBodyPositionGlobal(selectedBody)
        extraForcePos[0] = controlModel.getBodyPositionGlobal(selectedBody) - 0.1 * np.array([viewer.objectInfoWnd.labelForceX.value(), 0., viewer.objectInfoWnd.labelForceZ.value()])

        # render contact_ids

        # render skeleton
        if SKELETON_ON:
            Ts = dict()
            Ts['pelvis'] = controlModel.getJointTransform(idDic['Hips'])
            Ts['thigh_R'] = controlModel.getJointTransform(idDic['RightUpLeg'])
            Ts['shin_R'] = controlModel.getJointTransform(idDic['RightLeg'])
            Ts['foot_R'] = controlModel.getJointTransform(idDic['RightFoot'])
            Ts['foot_heel_R'] = controlModel.getJointTransform(idDic['RightFoot'])
            Ts['heel_R'] = np.eye(4)
            Ts['outside_metatarsal_R'] = controlModel.getJointTransform(idDic['RightFoot_foot_0_0'])
            Ts['outside_phalanges_R'] = controlModel.getJointTransform(idDic['RightFoot_foot_0_0_0'])
            # Ts['inside_metatarsal_R'] = controlModel.getJointTransform(idDic['RightFoot_foot_0_1'])
            Ts['inside_metatarsal_R'] = np.eye(4)
            Ts['inside_phalanges_R'] = controlModel.getJointTransform(idDic['RightFoot_foot_0_1_0'])
            Ts['spine_ribs'] = controlModel.getJointTransform(idDic['Spine'])
            Ts['head'] = controlModel.getJointTransform(idDic['Spine1'])
            Ts['upper_limb_R'] = controlModel.getJointTransform(idDic['RightArm'])
            Ts['lower_limb_R'] = controlModel.getJointTransform(idDic['RightForeArm'])
            Ts['thigh_L'] = controlModel.getJointTransform(idDic['LeftUpLeg'])
            Ts['shin_L'] = controlModel.getJointTransform(idDic['LeftLeg'])
            Ts['foot_L'] = controlModel.getJointTransform(idDic['LeftFoot'])
            Ts['foot_heel_L'] = controlModel.getJointTransform(idDic['LeftFoot'])
            Ts['heel_L'] = np.eye(4)
            Ts['outside_metatarsal_L'] = controlModel.getJointTransform(idDic['LeftFoot_foot_0_0'])
            Ts['outside_phalanges_L'] = controlModel.getJointTransform(idDic['LeftFoot_foot_0_0_0'])
            # Ts['inside_metatarsal_L'] = controlModel.getJointTransform(idDic['LeftFoot_foot_0_1'])
            Ts['inside_metatarsal_L'] = np.eye(4)
            Ts['inside_phalanges_L'] = controlModel.getJointTransform(idDic['LeftFoot_foot_0_1_0'])
            Ts['upper_limb_L'] = controlModel.getJointTransform(idDic['LeftArm'])
            Ts['lower_limb_L'] = controlModel.getJointTransform(idDic['LeftForeArm'])

            skeleton_renderer.appendFrameState(Ts)
Exemplo n.º 3
0
    def simulateCallback(frame):
        # print(frame)
        # print(motion[frame].getJointOrientationLocal(footIdDic['RightFoot_foot_0_1_0']))

        # hfi.footAdjust(motion[frame], idDic, SEGMENT_FOOT_MAG=.03, SEGMENT_FOOT_RAD=.015, baseHeight=0.02)

        # motionModel.update(motion[frame])
        motionModel.translateByOffset(
            np.array([
                getParamVal('com X offset'),
                getParamVal('com Y offset'),
                getParamVal('com Z offset')
            ]))
        # controlModel_ik.set_q(controlModel.get_q())

        global g_initFlag
        global forceShowTime

        global JsysPre
        global JsupPreL
        global JsupPreR

        global JconstPre

        global preFootCenter
        global maxContactChangeCount
        global contactChangeCount
        global contact
        global contactChangeType

        Kt, Kl, Kh, Bl, Bh, kt_sup = getParamVals(
            ['Kt', 'Kl', 'Kh', 'Bl', 'Bh', 'SupKt'])
        Dt = 2 * (Kt**.5)
        Dl = 2 * (Kl**.5)
        Dh = 2 * (Kh**.5)
        dt_sup = 2 * (kt_sup**.5)

        # tracking
        # # th_r = motion.getDOFPositions(frame)
        # th_r = motionModel.getDOFPositions()
        # th = controlModel.getDOFPositions()
        # # dth_r = motion.getDOFVelocities(frame)
        # dth = controlModel.getDOFVelocities()
        # # ddth_r = motion.getDOFAccelerations(frame)
        # ddth_des = yct.getDesiredDOFAccelerations(th_r, th, None, dth, None, Kt, Dt)
        #
        # # ype.flatten(fix_dofs(DOFs, ddth_des, mcfg, joint_names), ddth_des_flat)
        # # ype.flatten(fix_dofs(DOFs, dth, mcfg, joint_names), dth_flat)
        # # print(ddth_des)
        # ype.flatten(ddth_des, ddth_des_flat)
        # ype.flatten(dth, dth_flat)

        th_r_flat = motionModel.get_q()
        th_flat = controlModel.get_q()
        dth_flat = controlModel.get_dq()
        joint_dof_info = controlModel.getJointDOFInfo()

        ddth_des_flat = yct.getDesiredDOFAccelerations_flat(
            th_r_flat, th_flat, None, dth_flat, None, Kt, Dt, joint_dof_info)
        # print(controlModel.getCoriAndGrav())

        #################################################
        # jacobian
        #################################################

        contact_des_ids = list()  # desired contact segments
        contact_des_ids.append(supL)
        # if foot_viewer.check_h_l.value():
        #     contact_des_ids.append(motion[0].skeleton.getJointIndex('LeftFoot'))
        #
        # if foot_viewer.check_h_r.value():
        #     contact_des_ids.append(motion[0].skeleton.getJointIndex('RightFoot'))

        contact_ids = list()  # temp idx for balancing
        contact_ids.extend(contact_des_ids)

        contact_joint_ori = list(
            map(controlModel.getJointOrientationGlobal, contact_ids))
        contact_joint_pos = list(
            map(controlModel.getJointPositionGlobal, contact_ids))
        contact_body_ori = list(
            map(controlModel.getBodyOrientationGlobal, contact_ids))
        contact_body_pos = list(
            map(controlModel.getBodyPositionGlobal, contact_ids))
        contact_body_vel = list(
            map(controlModel.getBodyVelocityGlobal, contact_ids))
        contact_body_angvel = list(
            map(controlModel.getBodyAngVelocityGlobal, contact_ids))

        ref_joint_ori = list(
            map(motionModel.getJointOrientationGlobal, contact_ids))
        ref_joint_pos = list(
            map(motionModel.getJointPositionGlobal, contact_ids))
        ref_joint_vel = [
            motionModel.getJointVelocityGlobal(joint_idx)
            for joint_idx in contact_ids
        ]
        ref_joint_angvel = [
            motionModel.getJointAngVelocityGlobal(joint_idx)
            for joint_idx in contact_ids
        ]
        ref_body_ori = list(
            map(motionModel.getBodyOrientationGlobal, contact_ids))
        ref_body_pos = list(map(motionModel.getBodyPositionGlobal,
                                contact_ids))
        # ref_body_vel = list(map(controlModel.getBodyVelocityGlobal, contact_ids))
        ref_body_angvel = [
            motionModel.getJointAngVelocityGlobal(joint_idx)
            for joint_idx in contact_ids
        ]
        ref_body_vel = [
            ref_joint_vel[i] +
            np.cross(ref_joint_angvel[i], ref_body_pos[i] - ref_joint_pos[i])
            for i in range(len(ref_joint_vel))
        ]

        is_contact = [1] * len(contact_ids)
        contact_right = len(set(contact_des_ids).intersection(rIDlist)) > 0
        contact_left = len(set(contact_des_ids).intersection(lIDlist)) > 0

        # contMotionOffset = th_flat[0:3] - th_r_flat[0:3]
        contMotionOffset = np.array((1.5, 0., 0.))

        linkPositions = [
            controlModel.getBodyComPositionGlobal(i)
            for i in range(controlModel.getBodyNum())
        ]
        linkVelocities = [
            controlModel.getBodyComVelocityGlobal(i)
            for i in range(controlModel.getBodyNum())
        ]
        linkAngVelocities = [
            controlModel.getBodyAngVelocityGlobal(i)
            for i in range(controlModel.getBodyNum())
        ]
        linkInertias = [
            controlModel.getBodyInertiaGlobal(i)
            for i in range(controlModel.getBodyNum())
        ]

        CM = yrp.getCM(linkPositions, linkMasses, totalMass)
        dCM = yrp.getCM(linkVelocities, linkMasses, totalMass)
        CM_plane = copy.copy(CM)
        CM_plane[1] = 0.
        dCM_plane = copy.copy(dCM)
        dCM_plane[1] = 0.

        P = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions, CM,
                                     linkInertias)
        dP = ymt.getPureInertiaMatrixDerivative(dTO, linkMasses,
                                                linkVelocities, dCM,
                                                linkAngVelocities,
                                                linkInertias)

        # calculate jacobian
        Jsys, dJsys = controlModel.computeCom_J_dJdq()
        J_contacts = []  # type: list[np.ndarray]
        dJ_contacts = []  # type: list[np.ndarray]
        for contact_id in contact_ids:
            J_contacts.append(Jsys[6 * contact_id:6 * contact_id + 6, :])
            dJ_contacts.append(dJsys[6 * contact_id:6 * contact_id + 6])

        # calculate footCenter
        footCenter = sum(contact_body_pos) / len(contact_body_pos) if len(contact_body_pos) > 0 \
            else .5 * (controlModel.getBodyComPositionGlobal(supL) + controlModel.getBodyComPositionGlobal(supR))
        footCenter[1] = 0.
        # if len(contact_body_pos) > 2:
        #     hull = ConvexHull(contact_body_pos)

        footCenter_ref = sum(ref_body_pos) / len(ref_body_pos) if len(ref_body_pos) > 0 \
            else .5 * (motionModel.getBodyComPositionGlobal(supL) + motionModel.getBodyComPositionGlobal(supR))
        footCenter_ref = footCenter_ref + contMotionOffset
        # if len(ref_body_pos) > 2:
        #     hull = ConvexHull(ref_body_pos)
        footCenter_ref[1] = 0.

        # footCenter[0] = footCenter[0] + getParamVal('com X offset')
        # footCenter[1] = footCenter[0] + getParamVal('com Y offset')
        # footCenter[2] = footCenter[2] + getParamVal('com Z offset')

        # initialization
        if g_initFlag == 0:
            preFootCenter[0] = footCenter.copy()
            g_initFlag = 1

        # if contactChangeCount == 0 and np.linalg.norm(footCenter - preFootCenter[0]) > 0.01:
        #     contactChangeCount += 30
        if contactChangeCount > 0:
            # change footcenter gradually
            footCenter = preFootCenter[0] + (
                maxContactChangeCount - contactChangeCount) * (
                    footCenter - preFootCenter[0]) / maxContactChangeCount
        else:
            preFootCenter[0] = footCenter.copy()

        # linear momentum
        # TODO:
        # We should consider dCM_ref, shouldn't we?
        # add getBodyPositionGlobal and getBodyPositionsGlobal in csVpModel!
        # to do that, set joint velocities to vpModel
        CM_ref_plane = footCenter
        # CM_ref_plane = footCenter_ref
        CM_ref = footCenter + np.array([
            getParamVal('com X offset'),
            motionModel.getCOM()[1] + getParamVal('com Y offset'),
            getParamVal('com Z offset')
        ])
        dL_des_plane = Kl * totalMass * (CM_ref - CM) - Dl * totalMass * dCM
        # dL_des_plane = Kl * totalMass * (CM_ref_plane - CM_plane) - Dl * totalMass * dCM_plane
        # dL_des_plane[1] = 0.
        # print('dCM_plane : ', np.linalg.norm(dCM_plane))

        # angular momentum
        CP_ref = footCenter
        # CP_ref = footCenter_ref
        bodyIDs, contactPositions, contactPositionLocals, contactForces = controlModel.calcPenaltyForce(
            bodyIDsToCheck, mus, Ks, Ds)
        CP = yrp.getCP(contactPositions, contactForces)
        if CP_old[0] is None or CP is None:
            dCP = None
        else:
            dCP = (CP - CP_old[0]) / (1 / 30.)
        CP_old[0] = CP

        if CP is not None and dCP is not None:
            ddCP_des = Kh * (CP_ref - CP) - Dh * dCP
            dCP_des = dCP + ddCP_des * (1 / 30.)
            CP_des = CP + dCP_des * (1 / 30.)
            # CP_des = footCenter
            CP_des = CP + dCP * (1 / 30.) + .5 * ddCP_des * ((1 / 30.)**2)
            dH_des = np.cross(
                (CP_des - CM),
                (dL_des_plane + totalMass * controlModel.getGravity()))
            if contactChangeCount > 0:  # and contactChangeType == 'DtoS':
                dH_des *= (maxContactChangeCount -
                           contactChangeCount) / maxContactChangeCount
        else:
            dH_des = None

        # set up equality constraint
        # TODO:
        # logSO3 is just q'', not acceleration.
        # To make a_oris acceleration, q'' -> a will be needed
        # body_ddqs = list(map(mm.logSO3, [mm.getSO3FromVectors(np.dot(body_ori, mm.unitY()), mm.unitY()) for body_ori in contact_body_ori]))
        # body_ddqs = list(map(mm.logSO3, [np.dot(contact_body_ori[i].T, np.dot(ref_body_ori[i], mm.getSO3FromVectors(np.dot(ref_body_ori[i], mm.unitY()), mm.unitY()))) for i in range(len(contact_body_ori))]))
        # body_ddqs = list(map(mm.logSO3, [np.dot(contact_body_ori[i].T, np.dot(ref_body_ori[i], mm.getSO3FromVectors(np.dot(ref_body_ori[i], up_vec_in_each_link[contact_ids[i]]), mm.unitY()))) for i in range(len(contact_body_ori))]))
        a_oris = list(
            map(mm.logSO3, [
                np.dot(
                    contact_body_ori[i].T,
                    np.dot(
                        ref_body_ori[i],
                        mm.getSO3FromVectors(
                            np.dot(ref_body_ori[i],
                                   up_vec_in_each_link[contact_ids[i]]),
                            mm.unitY()))) for i in range(len(contact_body_ori))
            ]))
        a_oris = list(
            map(mm.logSO3, [
                np.dot(
                    np.dot(
                        ref_body_ori[i],
                        mm.getSO3FromVectors(
                            np.dot(ref_body_ori[i],
                                   up_vec_in_each_link[contact_ids[i]]),
                            mm.unitY())), contact_body_ori[i].T)
                for i in range(len(contact_body_ori))
            ]))
        body_qs = list(map(mm.logSO3, contact_body_ori))
        body_angs = [
            np.dot(contact_body_ori[i], contact_body_angvel[i])
            for i in range(len(contact_body_ori))
        ]
        body_dqs = [
            mm.vel2qd(body_angs[i], body_qs[i]) for i in range(len(body_angs))
        ]
        # a_oris = [np.dot(contact_body_ori[i], mm.qdd2accel(body_ddqs[i], body_dqs[i], body_qs[i])) for i in range(len(contact_body_ori))]

        # body_ddq = body_ddqs[0]
        # body_ori = contact_body_ori[0]
        # body_ang = np.dot(body_ori.T, contact_body_angvel[0])
        #
        # body_q = mm.logSO3(body_ori)
        # body_dq = mm.vel2qd(body_ang, body_q)
        # a_ori = np.dot(body_ori, mm.qdd2accel(body_ddq, body_dq, body_q))

        KT_SUP = np.diag([kt_sup / 10., kt_sup, kt_sup / 10.])
        # KT_SUP = np.diag([kt_sup, kt_sup, kt_sup])

        # a_oris = list(map(mm.logSO3, [mm.getSO3FromVectors(np.dot(body_ori, mm.unitY()), mm.unitY()) for body_ori in contact_body_ori]))
        a_oris = list(
            map(mm.logSO3, [
                mm.getSO3FromVectors(
                    np.dot(contact_body_ori[i],
                           up_vec_in_each_link[contact_ids[i]]), mm.unitY())
                for i in range(len(contact_body_ori))
            ]))
        # a_sups = [np.append(kt_sup*(ref_body_pos[i] - contact_body_pos[i] + contMotionOffset) + dt_sup*(ref_body_vel[i] - contact_body_vel[i]),
        #                     kt_sup*a_oris[i]+dt_sup*(ref_body_angvel[i]-contact_body_angvel[i])) for i in range(len(a_oris))]
        # a_sups = [np.append(kt_sup*(ref_body_pos[i] - contact_body_pos[i] + contMotionOffset) - dt_sup * contact_body_vel[i],
        #                     kt_sup*a_oris[i] - dt_sup * contact_body_angvel[i]) for i in range(len(a_oris))]
        a_sups = [
            np.append(
                np.dot(KT_SUP,
                       (ref_body_pos[i] - contact_body_pos[i] +
                        contMotionOffset)) - dt_sup * contact_body_vel[i],
                kt_sup * a_oris[i] - dt_sup * contact_body_angvel[i])
            for i in range(len(a_oris))
        ]
        # for i in range(len(a_sups)):
        #     a_sups[i][1] = -kt_sup * contact_body_pos[i][1] - dt_sup * contact_body_vel[i][1]

        # momentum matrix
        RS = np.dot(P, Jsys)
        R, S = np.vsplit(RS, 2)

        # rs = np.dot((np.dot(dP, Jsys) + np.dot(P, dJsys)), dth_flat)
        rs = np.dot(dP, np.dot(Jsys, dth_flat)) + np.dot(P, dJsys)
        r_bias, s_bias = np.hsplit(rs, 2)

        #######################################################
        # optimization
        #######################################################
        if LEG_FLEXIBLE:
            if contact == 2:
                config['weightMap']['h_thigh_right'] = .8
                config['weightMap']['h_shin_right'] = .8
                config['weightMap']['h_heel_right'] = .8
            else:
                config['weightMap']['h_thigh_right'] = .1
                config['weightMap']['h_shin_right'] = .25
                config['weightMap']['h_heel_right'] = .2

            if contact == 1:
                config['weightMap']['h_thigh_left'] = .8
                config['weightMap']['h_shin_left'] = .8
                config['weightMap']['h_heel_left'] = .8
            else:
                config['weightMap']['h_thigh_left'] = .1
                config['weightMap']['h_shin_left'] = .25
                config['weightMap']['h_heel_left'] = .2

        w = mot.getTrackingWeight(DOFs, controlModel, config['weightMap'])

        mot.addTrackingTerms(problem, totalDOF, Bt, w, ddth_des_flat)
        if dH_des is not None:
            mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R, r_bias)
            mot.addAngularTerms(problem, totalDOF, Bh, dH_des, S, s_bias)

            if True:
                for c_idx in range(len(contact_ids)):
                    mot.addConstraint2(problem, totalDOF, J_contacts[c_idx],
                                       dJ_contacts[c_idx], dth_flat,
                                       a_sups[c_idx])

        if contactChangeCount > 0:
            contactChangeCount = contactChangeCount - 1
            if contactChangeCount == 0:
                maxContactChangeCount = 30
                contactChangeType = 0

        r = problem.solve()
        problem.clear()
        ddth_sol_flat = np.asarray(r['x'])
        # ddth_sol_flat[foot_seg_dofs] = np.array(ddth_des_flat)[foot_seg_dofs]
        # ype.nested(ddth_sol_flat, ddth_sol)

        rootPos[0] = controlModel.getBodyPositionGlobal(selectedBody)
        localPos = [[0, 0, 0]]

        for _ in range(stepsPerFrame):
            bodyIDs, contactPositions, contactPositionLocals, contactForces = controlModel.calcPenaltyForce(
                bodyIDsToCheck, mus, Ks, Ds)
            controlModel.applyPenaltyForce(bodyIDs, contactPositionLocals,
                                           contactForces)

            # apply penalty force
            # controlModel.setDOFAccelerations(ddth_sol)
            # controlModel.setDOFAccelerations(ddth_des)
            controlModel.set_ddq(ddth_sol_flat)
            # controlModel.set_ddq(ddth_des_flat)
            controlModel.solveHybridDynamics()

            if forceShowTime > viewer.objectInfoWnd.labelForceDur.value():
                forceShowTime = 0
                viewer_ResetForceState()

            forceforce = np.array([
                viewer.objectInfoWnd.labelForceX.value(),
                viewer.objectInfoWnd.labelForceY.value(),
                viewer.objectInfoWnd.labelForceZ.value()
            ])
            extraForce[0] = getParamVal('Fm') * mm.normalize2(forceforce)
            if viewer_GetForceState():
                forceShowTime += controlModel.getTimeStep()
                controlModel.applyPenaltyForce(selectedBodyId, localPos,
                                               extraForce)

            controlModel.step()

        # rendering
        # bodyIDs, geomIDs, positionLocalsForGeom = vpWorld.getContactInfoForcePlate(bodyIDsToCheck)
        # for foot_seg_id in footIdlist:
        #     control_model_renderer.body_colors[foot_seg_id] = (255, 240, 255)
        #     control_model_renderer.geom_colors[foot_seg_id] = [(255, 240, 255)] * controlModel.getBodyGeomNum(foot_seg_id)

        # for i in range(len(geomIDs)):
        #     if controlModel.vpid2index(bodyIDs[i]) in footIdlist:
        #         control_model_renderer.geom_colors[controlModel.vpid2index(bodyIDs[i])][geomIDs[i]] = (255, 0, 0)
        # for foot_seg_id in footIdlist:
        #     control_model_renderer.body_colors[foot_seg_id] = (255, 240, 255)
        #
        # for contact_id in contact_ids:
        #     control_model_renderer.body_colors[contact_id] = (255, 0, 0)

        rd_footCenter[0] = footCenter
        rd_footCenter_ref[0] = footCenter_ref

        rd_CM[0] = CM

        rd_CM_plane[0] = CM.copy()
        rd_CM_plane[0][1] = 0.

        if CP is not None and dCP is not None:
            rd_CP[0] = CP
            rd_CP_des[0] = CP_des

            rd_dL_des_plane[0] = [
                dL_des_plane[0] / 100, dL_des_plane[1] / 100,
                dL_des_plane[2] / 100
            ]
            rd_dH_des[0] = dH_des

            rd_grf_des[
                0] = dL_des_plane - totalMass * controlModel.getGravity()

        del rd_foot_ori[:]
        del rd_foot_pos[:]
        # for seg_foot_id in footIdlist:
        #     rd_foot_ori.append(controlModel.getJointOrientationGlobal(seg_foot_id))
        #     rd_foot_pos.append(controlModel.getJointPositionGlobal(seg_foot_id))
        rd_foot_ori.append(controlModel.getJointOrientationGlobal(supL))
        rd_foot_ori.append(controlModel.getJointOrientationGlobal(supR))
        rd_foot_pos.append(controlModel.getJointPositionGlobal(supL))
        rd_foot_pos.append(controlModel.getJointPositionGlobal(supR))

        del rd_body_ori[:]
        del rd_body_pos[:]
        # for body_idx in range(dartModel.getBodyNum()):

        rd_root_des[0] = rootPos[0]
        rd_root_ori[0] = controlModel.getBodyOrientationGlobal(0)
        rd_root_pos[0] = controlModel.getBodyPositionGlobal(0)

        del rd_CF[:]
        del rd_CF_pos[:]
        for i in range(len(contactPositions)):
            rd_CF.append(contactForces[i] / 400)
            rd_CF_pos.append(contactPositions[i].copy())

        if viewer_GetForceState():
            rd_exfen_des[0] = [
                extraForce[0][0] / 100, extraForce[0][1] / 100,
                extraForce[0][2] / 100
            ]
            rd_exf_des[0] = [0, 0, 0]
        else:
            rd_exf_des[0] = [
                extraForce[0][0] / 100, extraForce[0][1] / 100,
                extraForce[0][2] / 100
            ]
            rd_exfen_des[0] = [0, 0, 0]

        # extraForcePos[0] = controlModel.getBodyPositionGlobal(selectedBody)
        extraForcePos[0] = controlModel.getBodyPositionGlobal(
            selectedBody) - 0.1 * np.array([
                viewer.objectInfoWnd.labelForceX.value(), 0.,
                viewer.objectInfoWnd.labelForceZ.value()
            ])