On the Constraint Problem of Friction Cone

[Supergithubber]

Hello! Glad you can open source this project. But, in the code, I found that the friction cone constraint equation you wrote is not the same as the one I pushed down. Please check whether you wrote it correctly? Below is your source code：

linear_constraints.resize(MPC_CONSTRAINT_DIM * PLAN_HORIZON, NUM_DOF * PLAN_HORIZON);    
    for (int i = 0; i < NUM_LEG * PLAN_HORIZON; ++i) {
        linear_constraints.insert(0 + 5 * i, 0 + 3 * i) = 1;
        linear_constraints.insert(1 + 5 * i, 0 + 3 * i) = 1;
        linear_constraints.insert(2 + 5 * i, 1 + 3 * i) = 1;
        linear_constraints.insert(3 + 5 * i, 1 + 3 * i) = 1;
        linear_constraints.insert(4 + 5 * i, 2 + 3 * i) = 1;

        linear_constraints.insert(0 + 5 * i, 2 + 3 * i) = mu;
        linear_constraints.insert(1 + 5 * i, 2 + 3 * i) = -mu;
        linear_constraints.insert(2 + 5 * i, 2 + 3 * i) = mu;
        linear_constraints.insert(3 + 5 * i, 2 + 3 * i) = -mu;
    }

    Eigen::VectorXd lb_one_horizon(MPC_CONSTRAINT_DIM);
    Eigen::VectorXd ub_one_horizon(MPC_CONSTRAINT_DIM);
    for (int i = 0; i < NUM_LEG; ++i) {
        lb_one_horizon.segment<5>(i * 5) << 0,
                -OsqpEigen::INFTY,
                0,
                -OsqpEigen::INFTY,
                fz_min * state.contacts[i];
        ub_one_horizon.segment<5>(i * 5) << OsqpEigen::INFTY,
                0,
                OsqpEigen::INFTY,
                0,
                fz_max * state.contacts[i];
    }
    // std:: cout << lb_one_horizon.transpose() << std::endl;
    // std:: cout << ub_one_horizon.transpose() << std::endl;
    for (int i = 0; i < PLAN_HORIZON; ++i) {
        lb.segment<MPC_CONSTRAINT_DIM>(i * MPC_CONSTRAINT_DIM) = lb_one_horizon;
        ub.segment<MPC_CONSTRAINT_DIM>(i * MPC_CONSTRAINT_DIM) = ub_one_horizon;
    }

The friction cone constraints I derived are as follows：

Looking forward to your reply！ 如果可以回复中文就再好不过了