forked from pz4kybsvg/Conception
You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
68 lines
2.5 KiB
68 lines
2.5 KiB
2 years ago
|
#pragma once
|
||
|
|
|
||
|
|
#include <optional>
|
||
|
|
#include <utility>
|
||
|
|
#include <vector>
|
||
|
|
|
||
|
|
#include <gtest/gtest.h>
|
||
|
|
|
||
|
|
#include "drake/solvers/mathematical_program.h"
|
||
|
|
#include "drake/solvers/solver_interface.h"
|
||
|
|
|
||
|
|
namespace drake {
|
||
|
|
namespace solvers {
|
||
|
|
namespace test {
|
||
|
|
/// Run solver.Solve() on the given @p prog. If the solver is absent or does
|
||
|
|
/// not find a solution, stop immediately via an exception. (Were we to
|
||
|
|
/// continue, testing statements that examine the results would be likely to
|
||
|
|
/// fail with confusing messages, so best to avoid them entirely.)
|
||
|
|
MathematicalProgramResult RunSolver(
|
||
|
|
const MathematicalProgram& prog, const SolverInterface& solver,
|
||
|
|
const std::optional<Eigen::VectorXd>& initial_guess = {},
|
||
|
|
const std::optional<SolverOptions>& solver_options = {});
|
||
|
|
|
||
|
|
/// Determine if two bindings are the same. Two bindings are the same if
|
||
|
|
///
|
||
|
|
/// 1. They contain the same constraint pointer.
|
||
|
|
/// 2. Their bound variables are the same.
|
||
|
|
template <typename Constraint>
|
||
|
|
::testing::AssertionResult IsBindingEqual(const Binding<Constraint>& binding1,
|
||
|
|
const Binding<Constraint>& binding2) {
|
||
|
|
if (binding1.evaluator() != binding2.evaluator()) {
|
||
|
|
return ::testing::AssertionFailure()
|
||
|
|
<< "Constraint pointers are not the same.";
|
||
|
|
}
|
||
|
|
if (binding1.variables().rows() != binding2.variables().rows()) {
|
||
|
|
return ::testing::AssertionFailure()
|
||
|
|
<< "Constraint variable sizes are not the same.";
|
||
|
|
}
|
||
|
|
for (int i = 0; i < binding1.variables().rows(); ++i) {
|
||
|
|
if (!binding1.variables()(i).equal_to(binding2.variables()(i))) {
|
||
|
|
return ::testing::AssertionFailure()
|
||
|
|
<< "Constraint variable mismatch:(" << binding1.variables()(i)
|
||
|
|
<< " vs. " << binding2.variables()(i) << ")";
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return ::testing::AssertionSuccess() << "Same binding.";
|
||
|
|
}
|
||
|
|
|
||
|
|
/// Determines if two vectors of bindings are the same.
|
||
|
|
template <typename Constraint>
|
||
|
|
::testing::AssertionResult IsVectorOfBindingEqual(
|
||
|
|
const std::vector<Binding<Constraint>>& bindings1,
|
||
|
|
const std::vector<Binding<Constraint>>& bindings2) {
|
||
|
|
if (bindings1.size() != bindings2.size()) {
|
||
|
|
return ::testing::AssertionFailure()
|
||
|
|
<< "Size mismatches: (" << bindings1.size() << " vs. "
|
||
|
|
<< bindings2.size() << ").";
|
||
|
|
}
|
||
|
|
for (int i = 0; i < static_cast<int>(bindings1.size()); ++i) {
|
||
|
|
auto result = IsBindingEqual(bindings1[i], bindings2[i]);
|
||
|
|
if (!result) return result;
|
||
|
|
}
|
||
|
|
return ::testing::AssertionSuccess() << " Same bindings.";
|
||
|
|
}
|
||
|
|
} // namespace test
|
||
|
|
} // namespace solvers
|
||
|
|
} // namespace drake
|