Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Feature/backup lqoc man ex #131

Open
wants to merge 28 commits into
base: v3.0.3-devel
Choose a base branch
from
Open

Feature/backup lqoc man ex #131

Show file tree
Hide file tree
Changes from 5 commits
Commits
Show all changes
28 commits
Select commit Hold shift + click to select a range
6ee68c5
backing up the test
markusgft Apr 24, 2020
55d2d87
backing up working example
markusgft Apr 26, 2020
2f7a116
working ex
markusgft Apr 26, 2020
1b1fb28
backup working example
markusgft Apr 30, 2020
5779b60
completed working example of LQOC (multiple-shooting) on composite ma…
markusgft May 12, 2020
7048b4c
remove unneeded computation
acxz May 18, 2020
f8d083d
fixed bug concerning hpipm_status display
hmcm May 20, 2020
9b9cedd
Fix links to RobCoGen in the readme
May 20, 2020
ca4fb2a
Merge pull request #132 from acxz/patch-1
markusgft May 21, 2020
796cd2a
Merge pull request #134 from hmcm/patch-hpipm
markusgft May 21, 2020
6ee83d4
Merge pull request #135 from mfrigerio17/maf
markusgft May 21, 2020
cd73120
Downgrade minimum required CMake version to 3.5 (xenial) and use newe…
romainreignier May 22, 2020
600a629
Fix histogram plot crash
romainreignier May 22, 2020
6d6ea58
Update ct_core/CMakeLists.txt with suggested commits
markusgft May 23, 2020
aac898f
Merge pull request #136 from romainreignier/compatible-old-cmake
markusgft May 24, 2020
f44104c
remove repeated header install line
acxz Jun 2, 2020
1ada03d
Merge pull request #139 from acxz/patch-1
markusgft Jun 3, 2020
4edeff3
Merge branch 'v3.0.2' into feature/backup_lqoc_man_ex
markusgft Jun 4, 2020
c2fee0e
fast forward with latest fixes.
markusgft Jun 4, 2020
4525468
non-working version
markusgft Jun 20, 2020
f8ba321
update with less coeff() work going on
markusgft Jun 21, 2020
85e378a
Merge branch 'feature/backup_lqoc_man_ex' of github.com:ethz-adrl/con…
markusgft Jun 21, 2020
88cfa2f
non-working hpipm test
markusgft Jun 25, 2020
d6572d3
Add impl for eigen-type to csv export.
markusgft Jul 13, 2020
b1940a9
backup
markusgft Jul 13, 2020
39682a7
reverse backup
markusgft Jul 13, 2020
94fb5a4
kinematic case not working with intermediate cost
markusgft Jul 14, 2020
755d93a
development backup holidays.
markusgft Dec 19, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
79 changes: 37 additions & 42 deletions ct_core/include/ct/core/control/StateFeedbackController-impl.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,17 +6,17 @@ Licensed under the BSD-2 license (see LICENSE file in main directory)
namespace ct {
namespace core {

template <typename MANIFOLD, size_t CONTROL_DIM, typename TIME_T>
template <typename MANIFOLD, size_t CONTROL_DIM, TIME_TYPE TIME_T>
StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::StateFeedbackController() : Base()
{
}

template <typename MANIFOLD, size_t CONTROL_DIM, typename TIME_T>
template <typename MANIFOLD, size_t CONTROL_DIM, TIME_TYPE TIME_T>
StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::~StateFeedbackController()
{
}

template <typename MANIFOLD, size_t CONTROL_DIM, typename TIME_T>
template <typename MANIFOLD, size_t CONTROL_DIM, TIME_TYPE TIME_T>
StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::StateFeedbackController(const MANIFOLD& x_ref,
const ct::core::ControlVector<CONTROL_DIM, SCALAR>& uff,
const ct::core::FeedbackMatrix<STATE_DIM, CONTROL_DIM, SCALAR>& K)
Expand All @@ -29,16 +29,15 @@ StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::StateFeedbackController(
{
}

template <typename MANIFOLD, size_t CONTROL_DIM, typename TIME_T>
template <typename MANIFOLD, size_t CONTROL_DIM, TIME_TYPE TIME_T>
StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::StateFeedbackController(
const StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>& other)
: Base(), x_ref_(other.x_ref_), uff_(other.uff_), K_(other.K_)
{
}

template <typename MANIFOLD, size_t CONTROL_DIM, typename TIME_T>
StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::StateFeedbackController(
const StateVectorArray<STATE_DIM, SCALAR>& x_ref,
template <typename MANIFOLD, size_t CONTROL_DIM, TIME_TYPE TIME_T>
StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::StateFeedbackController(const DiscreteArray<MANIFOLD>& x_ref,
const ControlVectorArray<CONTROL_DIM, SCALAR>& uff,
const FeedbackArray<STATE_DIM, CONTROL_DIM, SCALAR>& K,
const SCALAR deltaT,
Expand All @@ -48,8 +47,8 @@ StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::StateFeedbackController(
{
}

template <typename MANIFOLD, size_t CONTROL_DIM, typename TIME_T>
void StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::update(const DiscreteArray<state_vector_t>& x_ref,
template <typename MANIFOLD, size_t CONTROL_DIM, TIME_TYPE TIME_T>
void StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::update(const DiscreteArray<MANIFOLD>& x_ref,
const DiscreteArray<control_vector_t>& uff,
const DiscreteArray<FeedbackMatrix<STATE_DIM, CONTROL_DIM, SCALAR>>& K,
const tpl::TimeArray<SCALAR>& t)
Expand All @@ -70,95 +69,91 @@ void StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::update(const Discre
K_.setTime(tshort);
}

template <typename MANIFOLD, size_t CONTROL_DIM, typename TIME_T>
template <typename MANIFOLD, size_t CONTROL_DIM, TIME_TYPE TIME_T>
void StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::computeControl(const MANIFOLD& state,
const Time_t& tn,
control_vector_t& u)
{
computeControl_specialized(state, tn, u);
}


template <typename MANIFOLD, size_t CONTROL_DIM, typename TIME_T>
template <typename MANIFOLD, size_t CONTROL_DIM, TIME_TYPE TIME_T>
StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>* StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::clone()
const
{
return new StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>(*this);
}


template <typename MANIFOLD, size_t CONTROL_DIM, typename TIME_T>
const DiscreteArray<typename StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::state_vector_t>&
StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::x_ref() const
template <typename MANIFOLD, size_t CONTROL_DIM, TIME_TYPE TIME_T>
const DiscreteArray<MANIFOLD>& StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::x_ref() const
{
return x_ref_.getDataArray();
}

template <typename MANIFOLD, size_t CONTROL_DIM, typename TIME_T>
template <typename MANIFOLD, size_t CONTROL_DIM, TIME_TYPE TIME_T>
const DiscreteArray<typename StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::control_vector_t>&
StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::uff() const
{
return uff_.getDataArray();
}

template <typename MANIFOLD, size_t CONTROL_DIM, typename TIME_T>
const DiscreteArray<FeedbackMatrix<STATE_DIM, CONTROL_DIM, SCALAR>>&
StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::K() const
template <typename MANIFOLD, size_t CONTROL_DIM, TIME_TYPE TIME_T>
auto StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::K() const
-> const DiscreteArray<FeedbackMatrix<STATE_DIM, CONTROL_DIM, SCALAR>>&
{
return K_.getDataArray();
}

template <typename MANIFOLD, size_t CONTROL_DIM, typename TIME_T>
const tpl::TimeArray<SCALAR>& StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::time() const
template <typename MANIFOLD, size_t CONTROL_DIM, TIME_TYPE TIME_T>
auto StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::time() const -> const tpl::TimeArray<SCALAR>&
{
return x_ref_.getTimeArray();
}

template <typename MANIFOLD, size_t CONTROL_DIM, typename TIME_T>
StateTrajectory<STATE_DIM, SCALAR>&
StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::getReferenceStateTrajectory()
template <typename MANIFOLD, size_t CONTROL_DIM, TIME_TYPE TIME_T>
DiscreteTrajectory<MANIFOLD>& StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::getReferenceStateTrajectory()
{
return x_ref_;
}

template <typename MANIFOLD, size_t CONTROL_DIM, typename TIME_T>
const StateTrajectory<STATE_DIM, SCALAR>&
StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::getReferenceStateTrajectory() const
template <typename MANIFOLD, size_t CONTROL_DIM, TIME_TYPE TIME_T>
auto StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::getReferenceStateTrajectory() const
-> const DiscreteTrajectory<MANIFOLD>&
{
return x_ref_;
}

template <typename MANIFOLD, size_t CONTROL_DIM, typename TIME_T>
ControlTrajectory<CONTROL_DIM, SCALAR>&
StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::getFeedforwardTrajectory()
template <typename MANIFOLD, size_t CONTROL_DIM, TIME_TYPE TIME_T>
auto StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::getFeedforwardTrajectory()
-> ControlTrajectory<CONTROL_DIM, SCALAR>&
{
return uff_;
}

template <typename MANIFOLD, size_t CONTROL_DIM, typename TIME_T>
const ControlTrajectory<CONTROL_DIM, SCALAR>&
StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::getFeedforwardTrajectory() const
template <typename MANIFOLD, size_t CONTROL_DIM, TIME_TYPE TIME_T>
auto StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::getFeedforwardTrajectory() const
-> const ControlTrajectory<CONTROL_DIM, SCALAR>&
{
return uff_;
}

template <typename MANIFOLD, size_t CONTROL_DIM, typename TIME_T>
FeedbackTrajectory<STATE_DIM, CONTROL_DIM, SCALAR>&
StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::getFeedbackTrajectory()
template <typename MANIFOLD, size_t CONTROL_DIM, TIME_TYPE TIME_T>
auto StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::getFeedbackTrajectory()
-> FeedbackTrajectory<STATE_DIM, CONTROL_DIM, SCALAR>&
{
return K_;
}

template <typename MANIFOLD, size_t CONTROL_DIM, typename TIME_T>
const FeedbackTrajectory<STATE_DIM, CONTROL_DIM, SCALAR>&
StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::getFeedbackTrajectory() const
template <typename MANIFOLD, size_t CONTROL_DIM, TIME_TYPE TIME_T>
auto StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::getFeedbackTrajectory() const
-> const FeedbackTrajectory<STATE_DIM, CONTROL_DIM, SCALAR>&
{
return K_;
}

template <typename MANIFOLD, size_t CONTROL_DIM, typename TIME_T>
template <typename MANIFOLD, size_t CONTROL_DIM, TIME_TYPE TIME_T>
void StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::extractControlTrajectory(
const StateTrajectory<STATE_DIM, SCALAR>& x_traj,
const DiscreteTrajectory<MANIFOLD>& x_traj,
ControlTrajectory<CONTROL_DIM, SCALAR>& u_traj)
{
u_traj.clear();
Expand Down
46 changes: 19 additions & 27 deletions ct_core/include/ct/core/control/StateFeedbackController.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@ Licensed under the BSD-2 license (see LICENSE file in main directory)
#pragma once

#include "Controller.h"
#include <ct/core/systems/System.h>

namespace ct {
namespace core {
Expand Down Expand Up @@ -37,8 +38,8 @@ namespace core {
* @tparam CONTROL_DIM control vector size
* @tparam SCALAR primitive type
*/
template <typename MANIFOLD, size_t CONTROL_DIM, TIME_TYPE TIME_T>
class StateFeedbackController : public Controller<MANIFOLD, CONTROL_DIM, TIME_T>,
template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
class StateFeedbackController : public Controller<MANIFOLD, CONTROL_DIM, TIME_T>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Expand Down Expand Up @@ -87,6 +88,22 @@ class StateFeedbackController : public Controller<MANIFOLD, CONTROL_DIM, TIME_T>
//! deep cloning
StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>* clone() const override;

template <ct::core::TIME_TYPE T = TIME_T>
typename std::enable_if<T == ct::core::CONTINUOUS_TIME>::type computeControl_specialized(const MANIFOLD& state,
const Time_t& t,
control_vector_t& u)
{
u = uff_.eval(t) + K_.eval(t) * (state - x_ref_.eval(t)); // TODO: move to impl
}

template <ct::core::TIME_TYPE T = TIME_T>
typename std::enable_if<T == ct::core::DISCRETE_TIME>::type computeControl_specialized(const MANIFOLD& state,
const Time_t& n,
control_vector_t& u)
{
u = uff_[n] + K_[n] * (state - x_ref_[n]); // TODO: move to impl
}

//! computes the control action in the continuous case
/*!
* evaluates the controller using interpolation where required using interpolation
Expand All @@ -96,13 +113,6 @@ class StateFeedbackController : public Controller<MANIFOLD, CONTROL_DIM, TIME_T>
*/
virtual void computeControl(const MANIFOLD& state, const Time_t& tn, control_vector_t& controlAction) override;

typename std::enable_if<TIME_T == ct::core::CONTINUOUS_TIME>::type computeControl_specialized(const MANIFOLD& state,
const Time_t& tn,
control_vector_t& controlAction);
typename std::enable_if<TIME_T == ct::core::DISCRETE_TIME>::type computeControl_specialized(const MANIFOLD& state,
const Time_t& tn,
control_vector_t& controlAction);

//! updates the controller
/*!
* sets a new feedforward and feedback controller
Expand Down Expand Up @@ -156,23 +166,5 @@ class StateFeedbackController : public Controller<MANIFOLD, CONTROL_DIM, TIME_T>
};


template <typename MANIFOLD, size_t CONTROL_DIM, TIME_TYPE TIME_T>
typename std::enable_if<TIME_T == ct::core::CONTINUOUS_TIME>::type
StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::computeControl_specialized(const state_vector_t& state,
const SCALAR& t,
control_vector_t& controlAction)
{
controlAction = uff_.eval(t) + K_.eval(t) * (state - x_ref_.eval(t));
}

template <typename MANIFOLD, size_t CONTROL_DIM, TIME_TYPE TIME_T>
typename std::enable_if<TIME_T == ct::core::CONTINUOUS_TIME>::type
StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::computeControl_specialized(const state_vector_t& state,
const int n,
control_vector_t& controlAction)
{
controlAction = uff_[n] + K_[n] * (state - x_ref_[n]);
}

} // namespace core
} // namespace ct
6 changes: 4 additions & 2 deletions ct_core/include/ct/core/systems/linearizer/DynamicsLinearizerNumDiff.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -151,7 +151,8 @@ class DynamicsLinearizerNumDiff
SCALAR dxp = mlog_ph - m_log.coeffs()(i);

t_perturbed = m_log;
t_perturbed.coeffs()(i) = mlog_ph;
t_perturbed.set_coeff(i, mlog_ph); // TODO
//t_perturbed.coeffs()(i) = mlog_ph;
Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This needs to be solved cleanly, most likely requires a change in manif.


// evaluate dynamics at perturbed state
dynamics_fct_(retract_fct_(t_perturbed), t, u, res_plus);
Expand All @@ -162,7 +163,8 @@ class DynamicsLinearizerNumDiff
SCALAR dxm = m_log.coeffs()(i) - mlog_mh;

t_perturbed = m_log;
t_perturbed.coeffs()(i) = mlog_mh;
t_perturbed.set_coeff(i, mlog_ph);
//t_perturbed.coeffs()(i) = mlog_ph;

Tangent res_minus;
dynamics_fct_(retract_fct_(t_perturbed), t, u, res_minus);
Expand Down
2 changes: 1 addition & 1 deletion ct_optcon/include/ct/optcon/costfunction/CostFunction-impl.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@ template <typename MANIFOLD, size_t CONTROL_DIM>
CostFunction<MANIFOLD, CONTROL_DIM>::CostFunction() : t_(0.0),
t_shift_(0.0)
{
x_.setZero();
x_ = MANIFOLD::NeutralElement();
u_.setZero();
}

Expand Down
6 changes: 2 additions & 4 deletions ct_optcon/include/ct/optcon/costfunction/CostFunctionAD-impl.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -219,17 +219,15 @@ auto CostFunctionAD<AD_MANIFOLD, CONTROL_DIM>::evaluateTerminal() -> SCALAR_EVAL
}

template <typename AD_MANIFOLD, size_t CONTROL_DIM>
auto CostFunctionAD<AD_MANIFOLD, CONTROL_DIM>::stateDerivativeIntermediate()
-> ct::core::StateVector<STATE_DIM, SCALAR_EVAL>
auto CostFunctionAD<AD_MANIFOLD, CONTROL_DIM>::stateDerivativeIntermediate() -> typename EVAL_MANIFOLD::Tangent
{
Eigen::Matrix<SCALAR_EVAL, 1, STATE_DIM + CONTROL_DIM + 1> jacTot =
intermediateCostCodegen_->jacobian(stateControlTime_);
return jacTot.template leftCols<STATE_DIM>().transpose() + this->stateDerivativeIntermediateBase();
}

template <typename AD_MANIFOLD, size_t CONTROL_DIM>
auto CostFunctionAD<AD_MANIFOLD, CONTROL_DIM>::stateDerivativeTerminal()
-> ct::core::StateVector<STATE_DIM, SCALAR_EVAL>
auto CostFunctionAD<AD_MANIFOLD, CONTROL_DIM>::stateDerivativeTerminal() -> typename EVAL_MANIFOLD::Tangent
{
Eigen::Matrix<SCALAR_EVAL, 1, STATE_DIM + CONTROL_DIM + 1> jacTot = finalCostCodegen_->jacobian(stateControlTime_);
return jacTot.template leftCols<STATE_DIM>().transpose() + this->stateDerivativeTerminalBase();
Expand Down
6 changes: 2 additions & 4 deletions ct_optcon/include/ct/optcon/costfunction/CostFunctionAD.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -89,13 +89,11 @@ class CostFunctionAD
*/
CostFunctionAD(const CostFunctionAD& arg);


/**
* \brief Destructor
*/
virtual ~CostFunctionAD();


/**
* @brief Initializes the AD costfunction, generates and compiles
* source code
Expand Down Expand Up @@ -133,8 +131,8 @@ class CostFunctionAD
SCALAR_EVAL evaluateIntermediate() override;
SCALAR_EVAL evaluateTerminal() override;

ct::core::StateVector<STATE_DIM, SCALAR_EVAL> stateDerivativeIntermediate() override;
ct::core::StateVector<STATE_DIM, SCALAR_EVAL> stateDerivativeTerminal() override;
typename EVAL_MANIFOLD::Tangent stateDerivativeIntermediate() override;
typename EVAL_MANIFOLD::Tangent stateDerivativeTerminal() override;

control_vector_t controlDerivativeIntermediate() override;
control_vector_t controlDerivativeTerminal() override;
Expand Down
6 changes: 2 additions & 4 deletions ct_optcon/include/ct/optcon/costfunction/CostFunctionAnalytical-impl.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -101,15 +101,13 @@ auto CostFunctionAnalytical<MANIFOLD, CONTROL_DIM>::evaluateTerminal() -> SCALAR
}

template <typename MANIFOLD, size_t CONTROL_DIM>
auto CostFunctionAnalytical<MANIFOLD, CONTROL_DIM>::stateDerivativeIntermediate()
-> ct::core::StateVector<STATE_DIM, SCALAR_EVAL>
auto CostFunctionAnalytical<MANIFOLD, CONTROL_DIM>::stateDerivativeIntermediate() -> typename MANIFOLD::Tangent
{
return this->stateDerivativeIntermediateBase();
}

template <typename MANIFOLD, size_t CONTROL_DIM>
auto CostFunctionAnalytical<MANIFOLD, CONTROL_DIM>::stateDerivativeTerminal()
-> ct::core::StateVector<STATE_DIM, SCALAR_EVAL>
auto CostFunctionAnalytical<MANIFOLD, CONTROL_DIM>::stateDerivativeTerminal() -> typename MANIFOLD::Tangent
{
return this->stateDerivativeTerminalBase();
}
Expand Down
4 changes: 2 additions & 2 deletions ct_optcon/include/ct/optcon/costfunction/CostFunctionAnalytical.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -75,8 +75,8 @@ class CostFunctionAnalytical : public CostFunctionQuadratic<MANIFOLD, CONTROL_DI
SCALAR_EVAL evaluateIntermediate() override;
SCALAR_EVAL evaluateTerminal() override;

ct::core::StateVector<STATE_DIM, SCALAR_EVAL> stateDerivativeIntermediate() override;
ct::core::StateVector<STATE_DIM, SCALAR_EVAL> stateDerivativeTerminal() override;
typename MANIFOLD::Tangent stateDerivativeIntermediate() override;
typename MANIFOLD::Tangent stateDerivativeTerminal() override;

state_matrix_t stateSecondDerivativeIntermediate() override;
state_matrix_t stateSecondDerivativeTerminal() override;
Expand Down
11 changes: 4 additions & 7 deletions ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadratic-impl.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -207,7 +207,6 @@ auto CostFunctionQuadratic<MANIFOLD, CONTROL_DIM>::evaluateIntermediateBase() ->
return y;
}


template <typename MANIFOLD, size_t CONTROL_DIM>
auto CostFunctionQuadratic<MANIFOLD, CONTROL_DIM>::evaluateTerminalBase() -> SCALAR_EVAL
{
Expand All @@ -219,11 +218,10 @@ auto CostFunctionQuadratic<MANIFOLD, CONTROL_DIM>::evaluateTerminalBase() -> SCA
return y;
}


template <typename MANIFOLD, size_t CONTROL_DIM>
auto CostFunctionQuadratic<MANIFOLD, CONTROL_DIM>::stateDerivativeIntermediateBase() -> Tangent_t
auto CostFunctionQuadratic<MANIFOLD, CONTROL_DIM>::stateDerivativeIntermediateBase() -> typename EVAL_MANIFOLD::Tangent
{
Tangent_t derivative;
typename EVAL_MANIFOLD::Tangent derivative;
derivative.setZero();

for (auto it : this->intermediateCostAnalytical_)
Expand All @@ -238,11 +236,10 @@ auto CostFunctionQuadratic<MANIFOLD, CONTROL_DIM>::stateDerivativeIntermediateBa
return derivative;
}


template <typename MANIFOLD, size_t CONTROL_DIM>
auto CostFunctionQuadratic<MANIFOLD, CONTROL_DIM>::stateDerivativeTerminalBase() -> Tangent_t
auto CostFunctionQuadratic<MANIFOLD, CONTROL_DIM>::stateDerivativeTerminalBase() -> typename EVAL_MANIFOLD::Tangent
{
Tangent_t derivative;
typename EVAL_MANIFOLD::Tangent derivative;
derivative.setZero();

for (auto it : this->finalCostAnalytical_)
Expand Down
Loading