From a9ca20dc8bd7ebd5fcd6cd6aa1c497358404232d Mon Sep 17 00:00:00 2001
From: Maximilien Naveau <maximilien.naveau@tuebingen.mpg.de>
Date: Wed, 25 Sep 2019 18:49:52 +0200
Subject: [PATCH] start fixing the gaus approx and the documentation
---
.gitignore | 2 +-
include/crocoddyl/core/action-base.hpp | 10 ++++++----
include/crocoddyl/core/numdiff/action.hpp | 10 +++++++++-
src/core/numdiff/action.cpp | 7 +++++++
4 files changed, 23 insertions(+), 6 deletions(-)
diff --git a/.gitignore b/.gitignore
index 91ea4571..89a563ee 100644
--- a/.gitignore
+++ b/.gitignore
@@ -4,7 +4,7 @@
*~
*.orig
*.ipynb_checkpoints
-build/*
+build*/*
*.aux
*.log
*.pdf
diff --git a/include/crocoddyl/core/action-base.hpp b/include/crocoddyl/core/action-base.hpp
index 07cf9ff3..29090184 100644
--- a/include/crocoddyl/core/action-base.hpp
+++ b/include/crocoddyl/core/action-base.hpp
@@ -41,10 +41,10 @@ class ActionModelAbstract {
StateAbstract& get_state() const;
protected:
- unsigned int nu_;
- unsigned int nr_;
- StateAbstract& state_;
- Eigen::VectorXd unone_;
+ unsigned int nu_; //!< Control dimension
+ unsigned int nr_; //!< Dimension of the cost residual
+ StateAbstract& state_; //!< Model of the state
+ Eigen::VectorXd unone_; //!< Neutral state
#ifdef PYTHON_BINDINGS
@@ -90,6 +90,7 @@ struct ActionDataAbstract {
template <typename Model>
explicit ActionDataAbstract(Model* const model)
: cost(0.),
+ costResidual(model->get_nr()),
xnext(model->get_state().get_nx()),
r(model->get_nr()),
Fx(model->get_state().get_ndx(), model->get_state().get_ndx()),
@@ -122,6 +123,7 @@ struct ActionDataAbstract {
const Eigen::MatrixXd& get_Fu() const { return Fu; }
double cost;
+ Eigen::VectorXd costResidual;
Eigen::VectorXd xnext;
Eigen::VectorXd r;
Eigen::MatrixXd Fx;
diff --git a/include/crocoddyl/core/numdiff/action.hpp b/include/crocoddyl/core/numdiff/action.hpp
index 5f71cd00..0a61f3db 100644
--- a/include/crocoddyl/core/numdiff/action.hpp
+++ b/include/crocoddyl/core/numdiff/action.hpp
@@ -22,6 +22,14 @@ class ActionModelNumDiff : public ActionModelAbstract {
void calc(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x,
const Eigen::Ref<const Eigen::VectorXd>& u);
+ /**
+ * @brief calcDiff computes the
+ *
+ * @param data
+ * @param x
+ * @param u
+ * @param recalc
+ */
void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x,
const Eigen::Ref<const Eigen::VectorXd>& u, const bool& recalc = true);
boost::shared_ptr<ActionDataAbstract> createData();
@@ -83,7 +91,7 @@ struct ActionDataNumDiff : public ActionDataAbstract {
}
}
- Eigen::MatrixXd Rx;
+ Eigen::MatrixXd Rx; //!< Cost Jacobian:
Eigen::MatrixXd Ru;
Eigen::VectorXd dx;
Eigen::VectorXd du;
diff --git a/src/core/numdiff/action.cpp b/src/core/numdiff/action.cpp
index afffde41..756d3c32 100644
--- a/src/core/numdiff/action.cpp
+++ b/src/core/numdiff/action.cpp
@@ -84,6 +84,13 @@ void ActionModelNumDiff::calcDiff(const boost::shared_ptr<ActionDataAbstract>& d
data->Lxu = data_nd->Rx.transpose() * data_nd->Ru;
data->Luu = data_nd->Ru.transpose() * data_nd->Ru;
}
+ else
+ {
+ data->Lxx.fill(0.0);
+ data->Lxu.fill(0.0);
+ data->Luu.fill(0.0);
+ }
+
}
ActionModelAbstract& ActionModelNumDiff::get_model() const { return model_; }
--
GitLab