def simulateCallback(frame):
        # print(frame)
        # print(motion[frame].getJointOrientationLocal(footIdDic['RightFoot_foot_0_1_0']))
        # hfi.footAdjust(motion[frame], idDic, SEGMENT_FOOT_MAG=.03, SEGMENT_FOOT_RAD=.015, baseHeight=0.02)

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

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

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

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

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

        global g_initFlag
        global forceShowTime

        global JsysPre
        global JsupPreL
        global JsupPreR

        global JconstPre

        global preFootCenter
        global maxContactChangeCount
        global contactChangeCount
        global contact
        global contactChangeType

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

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

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

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

        contact_des_ids = list()  # desired contact segments
        if foot_viewer.check_om_l.value():
            contact_des_ids.append(
                motion[0].skeleton.getJointIndex('LeftFoot_foot_0_0'))
        if foot_viewer.check_op_l.value():
            contact_des_ids.append(
                motion[0].skeleton.getJointIndex('LeftFoot_foot_0_0_0'))
        if foot_viewer.check_im_l is not None and foot_viewer.check_im_l.value(
        ):
            contact_des_ids.append(
                motion[0].skeleton.getJointIndex('LeftFoot_foot_0_1'))
        if foot_viewer.check_ip_l.value():
            contact_des_ids.append(
                motion[0].skeleton.getJointIndex('LeftFoot_foot_0_1_0'))
        if foot_viewer.check_h_l.value():
            contact_des_ids.append(
                motion[0].skeleton.getJointIndex('LeftFoot_foot_1_0'))

        if foot_viewer.check_om_r.value():
            contact_des_ids.append(
                motion[0].skeleton.getJointIndex('RightFoot_foot_0_0'))
        if foot_viewer.check_op_r.value():
            contact_des_ids.append(
                motion[0].skeleton.getJointIndex('RightFoot_foot_0_0_0'))
        if foot_viewer.check_im_r is not None and foot_viewer.check_im_r.value(
        ):
            contact_des_ids.append(
                motion[0].skeleton.getJointIndex('RightFoot_foot_0_1'))
        if foot_viewer.check_ip_r.value():
            contact_des_ids.append(
                motion[0].skeleton.getJointIndex('RightFoot_foot_0_1_0'))
        if foot_viewer.check_h_r.value():
            contact_des_ids.append(
                motion[0].skeleton.getJointIndex('RightFoot_foot_1_0'))

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

            vpWorld.step()

        controlModel_ik.set_q(controlModel.get_q())

        if foot_viewer is not None:
            foot_viewer.foot_pressure_gl_window.refresh_foot_contact_info(
                frame, vpWorld, bodyIDsToCheck, mus, Ks, Ds)
            foot_viewer.foot_pressure_gl_window.goToFrame(frame)

        # rendering
        for foot_seg_id in footIdlist:
            control_model_renderer.body_colors[foot_seg_id] = (255, 240, 255)

        for contact_id in contact_ids:
            control_model_renderer.body_colors[contact_id] = (255, 0, 0)

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

        rd_CM[0] = CM

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

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

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

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

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

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

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

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

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

        # render contact_ids

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

            skeleton_renderer.appendFrameState(Ts)
示例#2
0
    def simulateCallback(frame):
        # print(frame)
        # print(motion[frame].getJointOrientationLocal(footIdDic['RightFoot_foot_0_1_0']))
        if False and viewer_GetForceState():
            # print('force on, frame: ', frame)
            motion[frame].mulJointOrientationLocal(
                footIdDic['LeftFoot_foot_0_0_0'],
                mm.exp(mm.unitX(), -math.pi * mm.SCALAR_1_6))
            motion[frame].mulJointOrientationLocal(
                footIdDic['LeftFoot_foot_0_1_0'],
                mm.exp(mm.unitX(), -math.pi * mm.SCALAR_1_6))
            motion[frame].mulJointOrientationLocal(
                footIdDic['RightFoot_foot_0_0_0'],
                mm.exp(mm.unitX(), -math.pi * mm.SCALAR_1_6))
            motion[frame].mulJointOrientationLocal(
                footIdDic['RightFoot_foot_0_1_0'],
                mm.exp(mm.unitX(), -math.pi * mm.SCALAR_1_6))
        # print(motion[frame].getJointOrientationLocal(footIdDic['RightFoot_foot_0_1_0']))
        motionModel.update(motion[frame])
        controlModel_ik.set_q(controlModel.get_q())

        global g_initFlag
        global forceShowTime

        global JsysPre
        global JsupPreL
        global JsupPreR

        global JconstPre

        global preFootCenter
        global maxContactChangeCount
        global contactChangeCount
        global contact
        global contactChangeType

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

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

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

        ype.flatten(ddth_des, ddth_des_flat)
        # ddth_des_flat = Kt * (motion.get_q(frame) - np.array(controlModel.get_q())) - Dt * np.array(controlModel.get_dq())
        ype.flatten(dth, dth_flat)
        # dth_flat = np.array(controlModel.get_dq())

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

        contact_des_ids = list()  # desired contact segments
        if foot_viewer.check_om_l.value():
            contact_des_ids.append(
                motion[0].skeleton.getJointIndex('LeftFoot_foot_0_0'))
        if foot_viewer.check_op_l.value():
            contact_des_ids.append(
                motion[0].skeleton.getJointIndex('LeftFoot_foot_0_0_0'))
        if foot_viewer.check_im_l.value():
            contact_des_ids.append(
                motion[0].skeleton.getJointIndex('LeftFoot_foot_0_1'))
        if foot_viewer.check_ip_l.value():
            contact_des_ids.append(
                motion[0].skeleton.getJointIndex('LeftFoot_foot_0_1_0'))
        if foot_viewer.check_h_l.value():
            contact_des_ids.append(
                motion[0].skeleton.getJointIndex('LeftFoot_foot_1_0'))

        if foot_viewer.check_om_r.value():
            contact_des_ids.append(
                motion[0].skeleton.getJointIndex('RightFoot_foot_0_0'))
        if foot_viewer.check_op_r.value():
            contact_des_ids.append(
                motion[0].skeleton.getJointIndex('RightFoot_foot_0_0_0'))
        if foot_viewer.check_im_r.value():
            contact_des_ids.append(
                motion[0].skeleton.getJointIndex('RightFoot_foot_0_1'))
        if foot_viewer.check_ip_r.value():
            contact_des_ids.append(
                motion[0].skeleton.getJointIndex('RightFoot_foot_0_1_0'))
        if foot_viewer.check_h_r.value():
            contact_des_ids.append(
                motion[0].skeleton.getJointIndex('RightFoot_foot_1_0'))

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

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

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

        J_contacts = [
            yjc.makeEmptyJacobian(DOFs, 1) for i in range(len(contact_ids))
        ]
        dJ_contacts = [
            yjc.makeEmptyJacobian(DOFs, 1) for i in range(len(contact_ids))
        ]
        joint_masks = [
            yjc.getLinkJointMask(motion[0].skeleton, joint_idx)
            for joint_idx in contact_ids
        ]

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

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

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

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

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

        is_contact = [1] * len(contact_ids)
        contactR = 1
        contactL = 1
        if refFootVelR[1] < 0 and refFootVelR[1] / 30. + refFootR[
                1] > singleTodoubleOffset:
            contactR = 0
        if refFootVelL[1] < 0 and refFootVelL[1] / 30. + refFootL[
                1] > singleTodoubleOffset:
            contactL = 0
        if refFootVelR[1] > 0 and refFootVelR[1] / 30. + refFootR[
                1] > doubleTosingleOffset:
            contactR = 0
        if refFootVelL[1] > 0 and refFootVelL[1] / 30. + refFootL[
                1] > doubleTosingleOffset:
            contactL = 0
        # contactR = 1

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

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

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

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

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

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

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

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

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

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

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

            g_initFlag = 1

        # calculate jacobian
        Jsys, dJsys = controlModel.computeCom_J_dJdq()
        for i in range(len(J_contacts)):
            J_contacts[i] = Jsys[6 * contact_ids[i]:6 * contact_ids[i] + 6, :]
            dJ_contacts[i] = dJsys[6 * contact_ids[i]:6 * contact_ids[i] + 6]

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

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

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

        footCenter[0] = footCenter[0] + getParamVal('com X offset')

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

        preFootCenter = footCenter.copy()

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

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

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

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

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

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

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

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

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

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

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

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

        # if contact == 2:
        #     mot.addSoftPointConstraintTerms(problem, totalDOF, Bsc, ddP_des1, Q1, q_bias1)

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

            # mot.setConstraint(problem, totalDOF, Jsup, dJsup, dth_flat, a_sup)
            # mot.addConstraint(problem, totalDOF, Jsup, dJsup, dth_flat, a_sup)
            # if contact & 1 and contactChangeCount == 0:
            if True:
                for c_idx in range(len(contact_ids)):
                    # mot.addConstraint(problem, totalDOF, J_contacts[c_idx], dJ_contacts[c_idx], dth_flat, a_sups[c_idx])
                    mot.addConstraint2(problem, totalDOF, J_contacts[c_idx],
                                       dJ_contacts[c_idx], dth_flat,
                                       a_sups[c_idx])

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

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

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

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

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

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

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

            vpWorld.step()

        controlModel_ik.set_q(controlModel.get_q())

        if foot_viewer is not None:
            foot_viewer.foot_pressure_gl_window.refresh_foot_contact_info(
                frame, vpWorld, bodyIDsToCheck, mus, Ks, Ds)
            foot_viewer.foot_pressure_gl_window.goToFrame(frame)

        # rendering
        for foot_seg_id in footIdlist:
            control_model_renderer.body_colors[foot_seg_id] = (255, 240, 255)

        for contact_id in contact_ids:
            control_model_renderer.body_colors[contact_id] = (255, 0, 0)

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

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

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

        rd_CM[0] = CM

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

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

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

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

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

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

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

        extraForcePos[0] = controlModel.getBodyPositionGlobal(selectedBody)

        # render contact_ids

        # render skeleton
        if SKELETON_ON:
            Ts = dict()
            Ts['pelvis'] = controlModel.getJointTransform(0)
            Ts['thigh_R'] = controlModel.getJointTransform(1)
            Ts['shin_R'] = controlModel.getJointTransform(2)
            Ts['foot_R'] = controlModel.getJointTransform(3)
            Ts['spine_ribs'] = controlModel.getJointTransform(9)
            Ts['head'] = controlModel.getJointTransform(10)
            Ts['upper_limb_R'] = controlModel.getJointTransform(13)
            Ts['lower_limb_R'] = controlModel.getJointTransform(14)
            Ts['thigh_L'] = controlModel.getJointTransform(15)
            Ts['shin_L'] = controlModel.getJointTransform(16)
            Ts['foot_L'] = controlModel.getJointTransform(17)
            Ts['upper_limb_L'] = controlModel.getJointTransform(11)
            Ts['lower_limb_L'] = controlModel.getJointTransform(12)

            skeleton_renderer.appendFrameState(Ts)