forked from RobotLocomotion/drake
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathevaluator_base.cc
121 lines (105 loc) · 3.95 KB
/
evaluator_base.cc
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
#include "drake/solvers/evaluator_base.h"
#include <set>
#include "drake/common/drake_throw.h"
#include "drake/common/nice_type_name.h"
using std::make_shared;
using std::shared_ptr;
using Eigen::MatrixXd;
using Eigen::VectorXd;
namespace drake {
namespace solvers {
std::ostream& EvaluatorBase::Display(
std::ostream& os, const VectorX<symbolic::Variable>& vars) const {
const int num_vars = this->num_vars();
DRAKE_THROW_UNLESS(vars.rows() == num_vars || num_vars == Eigen::Dynamic);
return this->DoDisplay(os, vars);
}
std::ostream& EvaluatorBase::Display(std::ostream& os) const {
if (this->num_vars() == Eigen::Dynamic) {
symbolic::Variable dynamic_var("dynamic_sized_variable");
return this->DoDisplay(os, Vector1<symbolic::Variable>(dynamic_var));
}
return this->DoDisplay(
os, symbolic::MakeVectorContinuousVariable(this->num_vars(), "$"));
}
std::ostream& EvaluatorBase::DoDisplay(
std::ostream& os, const VectorX<symbolic::Variable>& vars) const {
// Display the evaluator's most derived type name.
os << NiceTypeName::RemoveNamespaces(NiceTypeName::Get(*this));
// Append the description (when provided).
const std::string& description = get_description();
if (!description.empty()) {
os << " described as '" << description << "'";
}
// Append the bound decision variables (when provided).
const int vars_rows = vars.rows();
os << " with " << vars_rows << " decision variables";
for (int i = 0; i < vars_rows; ++i) {
os << " " << vars(i).get_name();
}
os << "\n";
return os;
}
namespace {
// Check if each entry of gradient_sparsity_pattern is within [0, rows) and [0,
// cols), and if there are any repeated entries in gradient_sparsity_pattern.
void CheckGradientSparsityPattern(
const std::vector<std::pair<int, int>>& gradient_sparsity_pattern, int rows,
int cols) {
std::set<std::pair<int, int>> nonzero_entries;
for (const auto& nonzero_entry : gradient_sparsity_pattern) {
if (nonzero_entry.first < 0 || nonzero_entry.first >= rows) {
throw std::invalid_argument(
"Constraint::SetSparsityPattern(): row index out of range.");
}
if (nonzero_entry.second < 0 || nonzero_entry.second >= cols) {
throw std::invalid_argument(
"Constraint::SetSparsityPattern(): column index out of range.");
}
auto it = nonzero_entries.find(nonzero_entry);
if (it != nonzero_entries.end()) {
throw std::invalid_argument(
"Constraint::SetSparsityPatten(): was given entries with repeated "
"values.");
}
nonzero_entries.insert(it, nonzero_entry);
}
}
} // namespace
void EvaluatorBase::SetGradientSparsityPattern(
const std::vector<std::pair<int, int>>& gradient_sparsity_pattern) {
if (kDrakeAssertIsArmed) {
CheckGradientSparsityPattern(gradient_sparsity_pattern, num_outputs(),
num_vars());
}
gradient_sparsity_pattern_.emplace(gradient_sparsity_pattern);
}
std::ostream& operator<<(std::ostream& os, const EvaluatorBase& e) {
return e.Display(os);
}
void PolynomialEvaluator::DoEval(const Eigen::Ref<const Eigen::VectorXd>& x,
Eigen::VectorXd* y) const {
double_evaluation_point_temp_.clear();
for (size_t i = 0; i < poly_vars_.size(); i++) {
double_evaluation_point_temp_[poly_vars_[i]] = x[i];
}
y->resize(num_outputs());
for (int i = 0; i < num_outputs(); i++) {
(*y)[i] =
polynomials_[i].EvaluateMultivariate(double_evaluation_point_temp_);
}
}
void PolynomialEvaluator::DoEval(const Eigen::Ref<const AutoDiffVecXd>& x,
AutoDiffVecXd* y) const {
taylor_evaluation_point_temp_.clear();
for (size_t i = 0; i < poly_vars_.size(); i++) {
taylor_evaluation_point_temp_[poly_vars_[i]] = x[i];
}
y->resize(num_outputs());
for (int i = 0; i < num_outputs(); i++) {
(*y)[i] =
polynomials_[i].EvaluateMultivariate(taylor_evaluation_point_temp_);
}
}
} // namespace solvers
} // namespace drake