On the Constraint Problem of Friction Cone
Closed this issue · 0 comments
Supergithubber commented
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!
如果可以回复中文就再好不过了