forked from RobotLocomotion/drake
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Create CostShim, move ConstraintImpl to FunctionCost, add supporting …
…test. PR 1 for RobotLocomotion#5890.
- Loading branch information
1 parent
44ede42
commit 89123d6
Showing
11 changed files
with
531 additions
and
105 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,18 @@ | ||
| #include "drake/solvers/cost.h" | ||
|
|
||
| namespace drake { | ||
| namespace solvers { | ||
|
|
||
| void Cost::DoEval(const Eigen::Ref<const Eigen::VectorXd>& x, | ||
| // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references). | ||
| Eigen::VectorXd& y) const { | ||
| impl_->Eval(x, y); | ||
| } | ||
| void Cost::DoEval(const Eigen::Ref<const AutoDiffVecXd>& x, | ||
| // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references). | ||
| AutoDiffVecXd& y) const { | ||
| impl_->Eval(x, y); | ||
| } | ||
|
|
||
| } // namespace solvers | ||
| } // namespace drake |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,145 @@ | ||
| #pragma once | ||
|
|
||
| #include <memory> | ||
| #include <utility> | ||
|
|
||
| #include "drake/solvers/constraint.h" | ||
| #include "drake/solvers/function.h" | ||
|
|
||
| namespace drake { | ||
| namespace solvers { | ||
|
|
||
| // TODO(eric.cousineau): Remove stopgap, and actually have Constraint and | ||
| // Cost be different classes. Consider using some common evaluation base. | ||
|
|
||
| /** | ||
| * Stopgap definition that permits Cost to use functionality in Constraint. | ||
| * Using an internal implementation permits child costs to inherit directly | ||
| * from cost, thus be convertible to a cost. | ||
| */ | ||
| class Cost : public Constraint { | ||
| public: | ||
| DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Cost) | ||
|
|
||
| explicit Cost(const std::shared_ptr<Constraint>& impl) | ||
| : Constraint(impl->num_constraints(), impl->num_vars()), impl_(impl) { | ||
| // Costs may only be scalar. | ||
| DRAKE_DEMAND(impl->num_constraints() == 1); | ||
| } | ||
|
|
||
| protected: | ||
| void DoEval(const Eigen::Ref<const Eigen::VectorXd>& x, | ||
| // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references). | ||
| Eigen::VectorXd& y) const override; | ||
|
|
||
| void DoEval(const Eigen::Ref<const AutoDiffVecXd>& x, | ||
| // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references). | ||
| AutoDiffVecXd& y) const override; | ||
|
|
||
| private: | ||
| std::shared_ptr<Constraint> impl_; | ||
| }; | ||
|
|
||
| /** | ||
| * Stopgap class to provide functionality as constraint, but allow templates to | ||
| * detect a difference from results from CreateConstraint and CreateCost. | ||
| * @tparam C Constraint type to inherit from. | ||
| */ | ||
| template <typename C> | ||
| class CostShim : public Cost { | ||
| public: | ||
| DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(CostShim) | ||
|
|
||
| template <typename... Args> | ||
| explicit CostShim(Args&&... args) | ||
| : Cost(std::make_shared<C>(std::forward<Args>(args)...)) {} | ||
| }; | ||
|
|
||
| class LinearCost : public CostShim<LinearConstraint> { | ||
| public: | ||
| DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(LinearCost) | ||
|
|
||
| // Inherit forwarding constructor. | ||
| using CostShim::CostShim; | ||
| }; | ||
|
|
||
| class QuadraticCost : public CostShim<QuadraticConstraint> { | ||
| public: | ||
| DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(QuadraticCost) | ||
|
|
||
| // Inherit forwarding constructor. | ||
| using CostShim::CostShim; | ||
| }; | ||
|
|
||
| class PolynomialCost : public CostShim<PolynomialConstraint> { | ||
| public: | ||
| DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(PolynomialCost) | ||
|
|
||
| // Inherit forwarding constructor. | ||
| using CostShim::CostShim; | ||
| }; | ||
|
|
||
| /** | ||
| * A constraint that may be specified using a callable object | ||
| * @tparam F The function / functor's type | ||
| * @note This is presently in Cost as it is the only place used. Once Cost is | ||
| * its own proper class, this name will be transitioned to FunctionCost. | ||
| */ | ||
| template <typename F> | ||
| class FunctionConstraint : public Constraint { | ||
| public: | ||
| DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(FunctionConstraint) | ||
|
|
||
| // Construct by copying from an lvalue. | ||
| template <typename... Args> | ||
| FunctionConstraint(const F& f, Args&&... args) | ||
| : Constraint(detail::FunctionTraits<F>::numOutputs(f), | ||
| detail::FunctionTraits<F>::numInputs(f), | ||
| std::forward<Args>(args)...), | ||
| f_(f) {} | ||
|
|
||
| // Construct by moving from an rvalue. | ||
| template <typename... Args> | ||
| FunctionConstraint(F&& f, Args&&... args) | ||
| : Constraint(detail::FunctionTraits<F>::numOutputs(f), | ||
| detail::FunctionTraits<F>::numInputs(f), | ||
| std::forward<Args>(args)...), | ||
| f_(std::forward<F>(f)) {} | ||
|
|
||
| protected: | ||
| void DoEval(const Eigen::Ref<const Eigen::VectorXd>& x, | ||
| Eigen::VectorXd& y) const override { | ||
| y.resize(detail::FunctionTraits<F>::numOutputs(f_)); | ||
| DRAKE_ASSERT(static_cast<size_t>(x.rows()) == | ||
| detail::FunctionTraits<F>::numInputs(f_)); | ||
| DRAKE_ASSERT(static_cast<size_t>(y.rows()) == | ||
| detail::FunctionTraits<F>::numOutputs(f_)); | ||
| detail::FunctionTraits<F>::eval(f_, x, y); | ||
| } | ||
| void DoEval(const Eigen::Ref<const AutoDiffVecXd>& x, | ||
| AutoDiffVecXd& y) const override { | ||
| y.resize(detail::FunctionTraits<F>::numOutputs(f_)); | ||
| DRAKE_ASSERT(static_cast<size_t>(x.rows()) == | ||
| detail::FunctionTraits<F>::numInputs(f_)); | ||
| DRAKE_ASSERT(static_cast<size_t>(y.rows()) == | ||
| detail::FunctionTraits<F>::numOutputs(f_)); | ||
| detail::FunctionTraits<F>::eval(f_, x, y); | ||
| } | ||
|
|
||
| private: | ||
| const F f_; | ||
| }; | ||
|
|
||
| template <typename F> | ||
| class FunctionCost : public CostShim<FunctionConstraint<F>> { | ||
| public: | ||
| DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(FunctionCost) | ||
|
|
||
| // Inherit forwarding constructor. | ||
| // Must explicitly qualify type due to class-level template. | ||
| using Base = CostShim<FunctionConstraint<F>>; | ||
| using Base::Base; | ||
| }; | ||
|
|
||
| }; // namespace solvers | ||
| } // namespace drake |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,90 @@ | ||
| #pragma once | ||
|
|
||
| #include <memory> | ||
| #include <type_traits> | ||
| #include <utility> | ||
|
|
||
| #include "drake/common/symbolic_expression.h" | ||
| #include "drake/solvers/binding.h" | ||
| #include "drake/solvers/constraint.h" | ||
| #include "drake/solvers/cost.h" | ||
| #include "drake/solvers/function.h" | ||
|
|
||
| namespace drake { | ||
| namespace solvers { | ||
|
|
||
| /** \addtogroup FunctionCostCreators */ | ||
| /*@{*/ | ||
|
|
||
| /** | ||
| * Converts an input of type @p F to a FunctionCost object. | ||
| * @tparam F This class should have functions numInputs(), numOutputs and | ||
| * eval(x, y). | ||
| * @see detail::FunctionTraits | ||
| */ | ||
| template <typename F> | ||
| std::shared_ptr<Cost> CreateFunctionCost(F&& f) { | ||
| return std::make_shared< | ||
| FunctionCost<typename std::remove_reference<F>::type>>( | ||
| std::forward<F>(f)); | ||
| } | ||
|
|
||
| namespace detail { | ||
|
|
||
| // From: drake-distro (git sha: 24452c1) | ||
| // //drake/solvers/mathematical_program.h:739 | ||
| // libstdc++ 4.9 evaluates | ||
| // `std::is_convertible<std::unique_ptr<Unrelated>, | ||
| // std::shared_ptr<Constraint>>::value` | ||
| // incorrectly as `true` so our enable_if overload is not used. | ||
| // Provide an explicit alternative for this case. | ||
| template <typename A, typename B> | ||
| struct is_convertible_workaround : std::is_convertible<A, B> {}; | ||
| template <typename A, typename B> | ||
| struct is_convertible_workaround<std::unique_ptr<A>, std::shared_ptr<B>> | ||
| : std::is_convertible<A*, B*> {}; | ||
|
|
||
| /** | ||
| * Template condition to check if @p F is a candidate to be used to construct a | ||
| * FunctionCost object for generic costs. | ||
| * @note Constraint is used to ensure that we do not preclude cost objects | ||
| * that lost their CostShim type somewhere in the process. | ||
| */ | ||
| template <typename F> | ||
| struct is_cost_functor_candidate | ||
| : std::integral_constant< | ||
| bool, | ||
| (!is_convertible_workaround<F, Constraint>::value) && | ||
| (!is_convertible_workaround< | ||
| F, std::shared_ptr<Constraint>>::value) && | ||
| (!is_convertible_workaround< | ||
| F, std::unique_ptr<Constraint>>::value) && | ||
| (!is_convertible_workaround<F, Binding<Constraint>>::value) && | ||
| (!is_convertible_workaround<F, symbolic::Expression>::value)> {}; | ||
|
|
||
| } // namespace detail | ||
|
|
||
| // TODO(eric.cousineau): For is_cost_functor_candiate, consider | ||
| // changing implementation to simply check if F is callable (after removing | ||
| // pointers, decaying, etc.) | ||
| // @ref http://stackoverflow.com/a/5117641/7829525 | ||
|
|
||
| /** | ||
| * Add costs to the optimization program on decision variables as dictated | ||
| * by the Binding constructor. | ||
| * | ||
| * @tparam F it should define functions numInputs, numOutputs and eval. | ||
| * | ||
| * @see detail::FunctionTraits for more detail. | ||
| */ | ||
| template <typename F> | ||
| typename std::enable_if<detail::is_cost_functor_candidate<F>::value, | ||
| std::shared_ptr<Cost>>::type | ||
| CreateCost(F&& f) { | ||
| return CreateFunctionCost(std::forward<F>(f)); | ||
| } | ||
|
|
||
| /*@}*/ // \addtogroup FunctionCostCreators | ||
|
|
||
| } // namespace solvers | ||
| } // namespace drake |
Oops, something went wrong.