Skip to content

Commit edbeaf8

Browse files
Meow404Meow404
committed
fix after rebase
1 parent c2c6134 commit edbeaf8

File tree

11 files changed

+199
-231
lines changed

11 files changed

+199
-231
lines changed

‎bindings/pyc3/c3_systems_py.cc‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -103,8 +103,8 @@ PYBIND11_MODULE(systems, m) {
103103
py::return_value_policy::reference)
104104
.def("get_output_port_lcs", &LCSFactorySystem::get_output_port_lcs,
105105
py::return_value_policy::reference)
106-
.def("get_output_port_lcs_contact_description",
107-
&LCSFactorySystem::get_output_port_lcs_contact_description,
106+
.def("get_output_port_lcs_contact_descriptions",
107+
&LCSFactorySystem::get_output_port_lcs_contact_descriptions,
108108
py::return_value_policy::reference);
109109

110110
py::class_<C3Output::C3Solution>(m, "C3Solution")

‎core/BUILD.bazel‎

Lines changed: 0 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -50,9 +50,6 @@ cc_library(
5050
data = [
5151
":default_solver_options",
5252
],
53-
data = [
54-
":default_solver_options",
55-
],
5653
copts = ["-fopenmp"],
5754
linkopts = ["-fopenmp"],
5855
deps = [

‎core/c3.cc‎

Lines changed: 28 additions & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -419,32 +419,39 @@ vector<VectorXd> C3::SolveQP(const VectorXd& x0, const vector<MatrixXd>& G,
419419
prog_.SetInitialGuess(x_[N_], warm_start_x_[admm_iteration][N_]);
420420
}
421421

422-
MathematicalProgramResult result = osqp_.Solve(prog_);
422+
try {
423+
MathematicalProgramResult result = osqp_.Solve(prog_);
423424

424-
if (result.is_success()) {
425-
for (int i = 0; i < N_; ++i) {
426-
if (is_final_solve) {
427-
x_sol_->at(i) = result.GetSolution(x_[i]);
428-
lambda_sol_->at(i) = result.GetSolution(lambda_[i]);
429-
u_sol_->at(i) = result.GetSolution(u_[i]);
425+
if (result.is_success()) {
426+
for (int i = 0; i < N_; ++i) {
427+
if (is_final_solve) {
428+
x_sol_->at(i) = result.GetSolution(x_[i]);
429+
lambda_sol_->at(i) = result.GetSolution(lambda_[i]);
430+
u_sol_->at(i) = result.GetSolution(u_[i]);
431+
}
432+
z_sol_->at(i).segment(0, n_x_) = result.GetSolution(x_[i]);
433+
z_sol_->at(i).segment(n_x_, n_lambda_) = result.GetSolution(lambda_[i]);
434+
z_sol_->at(i).segment(n_x_ + n_lambda_, n_u_) =
435+
result.GetSolution(u_[i]);
436+
437+
if (warm_start_) {
438+
// update warm start parameters
439+
warm_start_x_[admm_iteration][i] = result.GetSolution(x_[i]);
440+
warm_start_lambda_[admm_iteration][i] =
441+
result.GetSolution(lambda_[i]);
442+
warm_start_u_[admm_iteration][i] = result.GetSolution(u_[i]);
443+
}
430444
}
431-
z_sol_->at(i).segment(0, n_x_) = result.GetSolution(x_[i]);
432-
z_sol_->at(i).segment(n_x_, n_lambda_) = result.GetSolution(lambda_[i]);
433-
z_sol_->at(i).segment(n_x_ + n_lambda_, n_u_) = result.GetSolution(u_[i]);
434-
435445
if (warm_start_) {
436-
// update warm start parameters
437-
warm_start_x_[admm_iteration][i] = result.GetSolution(x_[i]);
438-
warm_start_lambda_[admm_iteration][i] = result.GetSolution(lambda_[i]);
439-
warm_start_u_[admm_iteration][i] = result.GetSolution(u_[i]);
446+
warm_start_x_[admm_iteration][N_] = result.GetSolution(x_[N_]);
440447
}
448+
} else {
449+
drake::log()->warn("C3::SolveQP failed to solve the QP with status: {}",
450+
result.get_solution_result());
441451
}
442-
if (warm_start_) {
443-
warm_start_x_[admm_iteration][N_] = result.GetSolution(x_[N_]);
444-
}
445-
} else {
446-
drake::log()->warn("C3::SolveQP failed to solve the QP with status: {}",
447-
result.get_solution_result());
452+
} catch (const std::exception& e) {
453+
drake::log()->error("C3::SolveQP failed with exception: {}", e.what());
454+
// return *z_sol_;
448455
}
449456

450457
return *z_sol_;

‎core/c3_miqp.cc‎

Lines changed: 83 additions & 77 deletions
Original file line numberDiff line numberDiff line change
@@ -18,100 +18,106 @@ VectorXd C3MIQP::SolveSingleProjection(const MatrixXd& U,
1818
const MatrixXd& H, const VectorXd& c,
1919
const int admm_iteration,
2020
const int& warm_start_index) {
21-
// Create an environment
22-
GRBEnv env(true);
23-
env.set("LogToConsole", "0");
24-
env.set("OutputFlag", "0");
25-
env.set("Threads", "0");
26-
env.start();
27-
// set up linear term in cost
28-
VectorXd cost_lin = -2 * delta_c.transpose() * U;
29-
30-
// set up for constraints (Ex + F \lambda + Hu + c >= 0)
31-
MatrixXd Mcons1(n_lambda_, n_x_ + n_lambda_ + n_u_);
32-
Mcons1 << E, F, H;
33-
34-
// set up for constraints (\lambda >= 0)
35-
MatrixXd MM1 = MatrixXd::Zero(n_lambda_, n_x_);
36-
MatrixXd MM2 = MatrixXd::Identity(n_lambda_, n_lambda_);
37-
MatrixXd MM3 = MatrixXd::Zero(n_lambda_, n_u_);
38-
MatrixXd Mcons2(n_lambda_, n_x_ + n_lambda_ + n_u_);
39-
Mcons2 << MM1, MM2, MM3;
40-
41-
GRBModel model = GRBModel(env);
42-
43-
GRBVar delta_k[n_x_ + n_lambda_ + n_u_];
44-
GRBVar binary[n_lambda_];
45-
46-
for (int i = 0; i < n_lambda_; i++) {
47-
binary[i] = model.addVar(0.0, 1.0, 0.0, GRB_BINARY);
48-
if (warm_start_index != -1) {
49-
binary[i].set(GRB_DoubleAttr_Start,
50-
warm_start_binary_[admm_iteration][warm_start_index](i));
21+
try {
22+
// Create an environment
23+
GRBEnv env(true);
24+
env.set("LogToConsole", "0");
25+
env.set("OutputFlag", "0");
26+
env.set("Threads", "0");
27+
env.start();
28+
// set up linear term in cost
29+
VectorXd cost_lin = -2 * delta_c.transpose() * U;
30+
31+
// set up for constraints (Ex + F \lambda + Hu + c >= 0)
32+
MatrixXd Mcons1(n_lambda_, n_x_ + n_lambda_ + n_u_);
33+
Mcons1 << E, F, H;
34+
35+
// set up for constraints (\lambda >= 0)
36+
MatrixXd MM1 = MatrixXd::Zero(n_lambda_, n_x_);
37+
MatrixXd MM2 = MatrixXd::Identity(n_lambda_, n_lambda_);
38+
MatrixXd MM3 = MatrixXd::Zero(n_lambda_, n_u_);
39+
MatrixXd Mcons2(n_lambda_, n_x_ + n_lambda_ + n_u_);
40+
Mcons2 << MM1, MM2, MM3;
41+
42+
GRBModel model = GRBModel(env);
43+
44+
GRBVar delta_k[n_x_ + n_lambda_ + n_u_];
45+
GRBVar binary[n_lambda_];
46+
47+
for (int i = 0; i < n_lambda_; i++) {
48+
binary[i] = model.addVar(0.0, 1.0, 0.0, GRB_BINARY);
49+
if (warm_start_index != -1) {
50+
binary[i].set(GRB_DoubleAttr_Start,
51+
warm_start_binary_[admm_iteration][warm_start_index](i));
52+
}
5153
}
52-
}
5354

54-
for (int i = 0; i < n_x_ + n_lambda_ + n_u_; i++) {
55-
delta_k[i] =
56-
model.addVar(-kVariableBounds, kVariableBounds, 0.0, GRB_CONTINUOUS);
57-
if (warm_start_index != -1) {
58-
delta_k[i].set(GRB_DoubleAttr_Start,
59-
warm_start_delta_[admm_iteration][warm_start_index](i));
55+
for (int i = 0; i < n_x_ + n_lambda_ + n_u_; i++) {
56+
delta_k[i] =
57+
model.addVar(-kVariableBounds, kVariableBounds, 0.0, GRB_CONTINUOUS);
58+
if (warm_start_index != -1) {
59+
delta_k[i].set(GRB_DoubleAttr_Start,
60+
warm_start_delta_[admm_iteration][warm_start_index](i));
61+
}
6062
}
61-
}
6263

63-
GRBQuadExpr obj = 0;
64+
GRBQuadExpr obj = 0;
6465

65-
for (int i = 0; i < n_x_ + n_lambda_ + n_u_; i++) {
66-
obj.addTerm(cost_lin(i), delta_k[i]);
67-
obj.addTerm(U(i, i), delta_k[i], delta_k[i]);
68-
}
66+
for (int i = 0; i < n_x_ + n_lambda_ + n_u_; i++) {
67+
obj.addTerm(cost_lin(i), delta_k[i]);
68+
obj.addTerm(U(i, i), delta_k[i], delta_k[i]);
69+
}
6970

70-
model.setObjective(obj, GRB_MINIMIZE);
71+
model.setObjective(obj, GRB_MINIMIZE);
7172

72-
double coeff[n_x_ + n_lambda_ + n_u_];
73-
double coeff2[n_x_ + n_lambda_ + n_u_];
73+
double coeff[n_x_ + n_lambda_ + n_u_];
74+
double coeff2[n_x_ + n_lambda_ + n_u_];
7475

75-
for (int i = 0; i < n_lambda_; i++) {
76-
GRBLinExpr lambda_expr = 0;
76+
for (int i = 0; i < n_lambda_; i++) {
77+
GRBLinExpr lambda_expr = 0;
7778

78-
/// convert VectorXd to double
79-
for (int j = 0; j < n_x_ + n_lambda_ + n_u_; j++) {
80-
coeff[j] = Mcons2(i, j);
81-
}
79+
/// convert VectorXd to double
80+
for (int j = 0; j < n_x_ + n_lambda_ + n_u_; j++) {
81+
coeff[j] = Mcons2(i, j);
82+
}
8283

83-
lambda_expr.addTerms(coeff, delta_k, n_x_ + n_lambda_ + n_u_);
84-
model.addConstr(lambda_expr >= 0);
85-
model.addConstr(lambda_expr <= M_ * (1 - binary[i]));
84+
lambda_expr.addTerms(coeff, delta_k, n_x_ + n_lambda_ + n_u_);
85+
model.addConstr(lambda_expr >= 0);
86+
model.addConstr(lambda_expr <= M_ * (1 - binary[i]));
8687

87-
GRBLinExpr activation_expr = 0;
88+
GRBLinExpr activation_expr = 0;
8889

89-
/// convert VectorXd to double
90-
for (int j = 0; j < n_x_ + n_lambda_ + n_u_; j++) {
91-
coeff2[j] = Mcons1(i, j);
92-
}
90+
/// convert VectorXd to double
91+
for (int j = 0; j < n_x_ + n_lambda_ + n_u_; j++) {
92+
coeff2[j] = Mcons1(i, j);
93+
}
9394

94-
activation_expr.addTerms(coeff2, delta_k, n_x_ + n_lambda_ + n_u_);
95-
model.addConstr(activation_expr + c(i) >= 0);
96-
model.addConstr(activation_expr + c(i) <= M_ * binary[i]);
97-
}
95+
activation_expr.addTerms(coeff2, delta_k, n_x_ + n_lambda_ + n_u_);
96+
model.addConstr(activation_expr + c(i) >= 0);
97+
model.addConstr(activation_expr + c(i) <= M_ * binary[i]);
98+
}
9899

99-
model.optimize();
100+
model.optimize();
100101

101-
VectorXd delta_kc(n_x_ + n_lambda_ + n_u_);
102-
VectorXd binaryc(n_lambda_);
103-
for (int i = 0; i < n_x_ + n_lambda_ + n_u_; i++) {
104-
delta_kc(i) = delta_k[i].get(GRB_DoubleAttr_X);
105-
}
106-
for (int i = 0; i < n_lambda_; i++) {
107-
binaryc(i) = binary[i].get(GRB_DoubleAttr_X);
108-
}
102+
VectorXd delta_kc(n_x_ + n_lambda_ + n_u_);
103+
VectorXd binaryc(n_lambda_);
104+
for (int i = 0; i < n_x_ + n_lambda_ + n_u_; i++) {
105+
delta_kc(i) = delta_k[i].get(GRB_DoubleAttr_X);
106+
}
107+
for (int i = 0; i < n_lambda_; i++) {
108+
binaryc(i) = binary[i].get(GRB_DoubleAttr_X);
109+
}
109110

110-
if (warm_start_index != -1) {
111-
warm_start_delta_[admm_iteration][warm_start_index] = delta_kc;
112-
warm_start_binary_[admm_iteration][warm_start_index] = binaryc;
111+
if (warm_start_index != -1) {
112+
warm_start_delta_[admm_iteration][warm_start_index] = delta_kc;
113+
warm_start_binary_[admm_iteration][warm_start_index] = binaryc;
114+
}
115+
return delta_kc;
116+
} catch (GRBException& e) {
117+
std::cerr << "Error code = " << e.getErrorCode() << std::endl;
118+
std::cerr << e.getMessage() << std::endl;
119+
throw std::runtime_error("Gurobi optimization failed.");
113120
}
114-
return delta_kc;
115121
}
116122

117123
} // namespace c3

‎multibody/test/multibody_test.cc‎

Lines changed: 53 additions & 64 deletions
Original file line numberDiff line numberDiff line change
@@ -185,70 +185,59 @@ TEST_P(LCSFactoryPivotingTest, LinearizePlantToLCS) {
185185
EXPECT_EQ(lcs.num_lambdas(), LCSFactory::GetNumContactVariables(options));
186186
}
187187

188-
// TEST_P(LCSFactoryPivotingTest, UpdateStateAndInput) {
189-
// LCS initial_lcs = lcs_factory->GenerateLCS();
190-
// auto [initial_contact_points, initial_force_basis_per_contact] =
191-
// lcs_factory->GetWitnessPointsAndForceBasisInWorldFrame();
192-
193-
// drake::VectorX<double> state =
194-
// VectorXd::Zero(plant->num_positions() + plant->num_velocities());
195-
// state << 0, 0.75, 0.785, -0.5, 0.5, 0.5, 0.5, 0, 0, 0, 0, 0, 0, 0;
196-
// drake::VectorX<double> input = VectorXd::Zero(plant->num_actuators());
197-
// input << 0, 0, 0, 0;
198-
199-
// // Update the LCS factory with the state and input.
200-
// lcs_factory->UpdateStateAndInput(state, input);
201-
202-
// LCS updated_lcs = lcs_factory->GenerateLCS();
203-
// auto [updated_contact_points, updated_force_basis_per_contact] =
204-
// lcs_factory->GetWitnessPointsAndForceBasisInWorldFrame();
205-
206-
// EXPECT_EQ(initial_lcs.A(), updated_lcs.A());
207-
// EXPECT_EQ(initial_lcs.B(), updated_lcs.B());
208-
// EXPECT_EQ(initial_lcs.d(), updated_lcs.d());
209-
210-
// EXPECT_NE(initial_lcs.D(), updated_lcs.D());
211-
// // Except for frictionless spring, the contact model would generate different
212-
// // matrices
213-
// if (contact_model != ContactModel::kFrictionlessSpring) {
214-
// EXPECT_NE(initial_lcs.E(), updated_lcs.E());
215-
// EXPECT_NE(initial_lcs.F(), updated_lcs.F());
216-
// EXPECT_NE(initial_lcs.H(), updated_lcs.H());
217-
// EXPECT_NE(initial_lcs.c(), updated_lcs.c());
218-
// }
219-
220-
// for (size_t i = 0; i < initial_contact_points.size(); ++i) {
221-
// EXPECT_NE(initial_contact_points[i], updated_contact_points[i]);
222-
// EXPECT_NE(initial_force_basis_per_contact[i],
223-
// updated_force_basis_per_contact[i]);
224-
// }
225-
// }
226-
227-
// TEST_P(LCSFactoryPivotingTest, GetWitnessPointsAndForceBasisInWorldFrame) {
228-
// auto [contact_points, force_bases_per_contact] =
229-
// lcs_factory->GetWitnessPointsAndForceBasisInWorldFrame();
230-
231-
// int n_contacts = contact_pairs.size();
232-
// // Check for number of force variables (not including slack variables)
233-
// switch (contact_model) {
234-
// case ContactModel::kStewartAndTrinkle:
235-
// EXPECT_EQ(force_bases_per_contact[0].rows(),
236-
// 1 + 2 * options.num_friction_directions);
237-
// break;
238-
// case ContactModel::kFrictionlessSpring:
239-
// EXPECT_EQ(force_bases_per_contact[0].rows(), 1);
240-
// break;
241-
// case ContactModel::kAnitescu:
242-
// EXPECT_EQ(force_bases_per_contact[0].rows(),
243-
// 2 * options.num_friction_directions);
244-
// break;
245-
// default:
246-
// EXPECT_TRUE(false); // Something went wrong in parsing the contact model
247-
// }
248-
// EXPECT_EQ(force_bases_per_contact[0].cols(), 3);
249-
// EXPECT_EQ(force_bases_per_contact.size(), n_contacts);
250-
// EXPECT_EQ(contact_points.size(), n_contacts);
251-
// }
188+
TEST_P(LCSFactoryPivotingTest, UpdateStateAndInput) {
189+
LCS initial_lcs = lcs_factory->GenerateLCS();
190+
auto initial_contact_descriptions = lcs_factory->GetContactDescriptions();
191+
192+
drake::VectorX<double> state =
193+
VectorXd::Zero(plant->num_positions() + plant->num_velocities());
194+
state << 0, 0.75, 0.785, -0.5, 0.5, 0.5, 0.5, 0, 0, 0, 0, 0, 0, 0;
195+
drake::VectorX<double> input = VectorXd::Zero(plant->num_actuators());
196+
input << 0, 0, 0, 0;
197+
198+
// Update the LCS factory with the state and input.
199+
lcs_factory->UpdateStateAndInput(state, input);
200+
201+
LCS updated_lcs = lcs_factory->GenerateLCS();
202+
auto updated_contact_descriptions = lcs_factory->GetContactDescriptions();
203+
204+
EXPECT_EQ(initial_lcs.A(), updated_lcs.A());
205+
EXPECT_EQ(initial_lcs.B(), updated_lcs.B());
206+
EXPECT_EQ(initial_lcs.d(), updated_lcs.d());
207+
208+
EXPECT_NE(initial_lcs.D(), updated_lcs.D());
209+
// Except for frictionless spring, the contact model would generate different
210+
// matrices
211+
if (contact_model != ContactModel::kFrictionlessSpring) {
212+
EXPECT_NE(initial_lcs.E(), updated_lcs.E());
213+
EXPECT_NE(initial_lcs.F(), updated_lcs.F());
214+
EXPECT_NE(initial_lcs.H(), updated_lcs.H());
215+
EXPECT_NE(initial_lcs.c(), updated_lcs.c());
216+
}
217+
218+
for (size_t i = 0; i < initial_contact_descriptions.size(); ++i) {
219+
if (initial_contact_descriptions[i].is_slack) continue;
220+
EXPECT_NE(initial_contact_descriptions[i].witness_point_A,
221+
updated_contact_descriptions[i].witness_point_A);
222+
EXPECT_NE(initial_contact_descriptions[i].witness_point_B,
223+
updated_contact_descriptions[i].witness_point_B);
224+
EXPECT_NE(initial_contact_descriptions[i].force_basis,
225+
updated_contact_descriptions[i].force_basis);
226+
}
227+
}
228+
229+
TEST_P(LCSFactoryPivotingTest, GetContactDescriptions) {
230+
auto contact_descriptions = lcs_factory->GetContactDescriptions();
231+
232+
int n_contacts = contact_descriptions.size();
233+
EXPECT_EQ(LCSFactory::GetNumContactVariables(options), n_contacts);
234+
for (size_t i = 0; i < contact_descriptions.size(); ++i) {
235+
if (contact_descriptions[i].is_slack) continue;
236+
EXPECT_FALSE(contact_descriptions[i].witness_point_A.isZero());
237+
EXPECT_FALSE(contact_descriptions[i].witness_point_B.isZero());
238+
EXPECT_FALSE(contact_descriptions[i].force_basis.isZero());
239+
}
240+
}
252241

253242
TEST_P(LCSFactoryPivotingTest, FixSomeModes) {
254243
LCS lcs = lcs_factory->GenerateLCS();

0 commit comments

Comments
 (0)