-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
generic_trivial_costs.h
79 lines (64 loc) · 2.28 KB
/
generic_trivial_costs.h
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
#pragma once
#include <cstddef>
#include <limits>
#include "drake/common/autodiff.h"
#include "drake/common/drake_assert.h"
#include "drake/common/drake_copyable.h"
#include "drake/common/eigen_types.h"
#include "drake/math/autodiff.h"
#include "drake/solvers/cost.h"
#include "drake/solvers/function.h"
namespace drake {
namespace solvers {
namespace test {
// A generic cost derived from Constraint class. This is meant for testing
// adding a cost to optimization program, and the cost is in the form of a
// derived class of Constraint.
class GenericTrivialCost1 : public Cost {
public:
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(GenericTrivialCost1)
GenericTrivialCost1() : Cost(3), private_val_(2) {}
protected:
void DoEval(const Eigen::Ref<const Eigen::VectorXd>& x,
Eigen::VectorXd* y) const override {
DoEvalGeneric(x, y);
}
void DoEval(const Eigen::Ref<const AutoDiffVecXd>& x,
AutoDiffVecXd* y) const override {
DoEvalGeneric(x, y);
}
void DoEval(const Eigen::Ref<const VectorX<symbolic::Variable>>& x,
VectorX<symbolic::Expression>* y) const override {
DoEvalGeneric(x, y);
}
private:
template <typename DerivedX, typename ScalarY>
void DoEvalGeneric(const Eigen::MatrixBase<DerivedX>& x,
VectorX<ScalarY>* y) const {
y->resize(1);
(*y)(0) = x(0) * x(1) + x(2) / x(0) * private_val_;
}
// Add a private data member to make sure no slicing on this class, derived
// from Constraint.
double private_val_{0};
};
// A generic cost. This class is meant for testing adding a cost to the
// optimization program, by calling `MathematicalProgram::MakeCost` to
// convert this class to a ConstraintImpl object.
class GenericTrivialCost2 {
public:
DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(GenericTrivialCost2)
GenericTrivialCost2() = default;
static size_t numInputs() { return 2; }
static size_t numOutputs() { return 1; }
template <typename ScalarType>
void eval(internal::VecIn<ScalarType> const& x,
internal::VecOut<ScalarType>* y) const {
DRAKE_ASSERT(static_cast<size_t>(x.rows()) == numInputs());
DRAKE_ASSERT(static_cast<size_t>(y->rows()) == numOutputs());
(*y)(0) = x(0) * x(0) - x(1) * x(1) + 2;
}
};
} // namespace test
} // namespace solvers
} // namespace drake