diff --git a/include/crocoddyl/core/numdiff/action.hpp b/include/crocoddyl/core/numdiff/action.hpp
index 0a61f3db83146759487164128ed4e2ef794920bb..0ae52a8975d2df9b428a01fafd47db5e6d675126 100644
--- a/include/crocoddyl/core/numdiff/action.hpp
+++ b/include/crocoddyl/core/numdiff/action.hpp
@@ -15,8 +15,45 @@
namespace crocoddyl {
+/**
+ * @brief This class computes the numerical differentiation of an ActionModel.
+ *
+ * It computes the same quantity as a normal model would do but using numerical
+ * differentiation.
+ * The subtility is in the computation of the Hessian of the cost. Let us
+ * concider that the ActionModel owns a cost residual. This means that the cost
+ * is the square of a residual \f$ l(x,u) = .5 r(x,u)**2 \f$, with
+ * \f$ r(x,u) \f$ being a vector. Therefore the derivatives of the cost
+ * \f$ l \f$ can be expressed in function of the derivatives of the residuals
+ * (jacobians), denoted by \f$ R_x \f$ and \f$ R_u \f$. Which would be:
+ * \f{eqnarray*}{
+ * L_x &=& R_x^T r \\
+ * L_u &=& R_u^T r \\
+ * L_{xx} &=& R_x^T R_x + R_{xx} r
+ * \f}
+ * with \f$ R_{xx} \f$ the derivatives of the jacobian (i.e. not a matrix, but a
+ * dim-3 tensor). The Gauss approximation boils down to neglecting this terms.
+ * So \f$ L_{xx} \sim R_x^T R_x \f$. Similarly for \f$ L_{xu} \sim R_x^T R_u \f$
+ * and \f$ L_{uu} \sim R_u^T R_u \f$. The above set of equations becomes:
+ * \f{eqnarray*}{
+ * L_x &=& R_x^T r \\
+ * L_u &=& R_u^T r \\
+ * L_{xx} &\sim& R_x^T R_x \\
+ * L_{xu} &\sim& R_x^T R_u \\
+ * L_{uu} &\sim& R_u^T R_u
+ * \f}
+ * In the case that the cost does not have a residual we set the Hessian to
+ * \f$ 0 \f$, i.e. \f$ L_{xx} = L_{xu} = L_{uu} = 0 \f$.
+ */
class ActionModelNumDiff : public ActionModelAbstract {
public:
+ /**
+ * @brief Construct a new ActionModelNumDiff object
+ *
+ * @param model
+ * @param with_gauss_approx defines if we use the Gauss approximation of the
+ * cost hessian or not.
+ */
explicit ActionModelNumDiff(ActionModelAbstract& model, bool with_gauss_approx = false);
~ActionModelNumDiff();
@@ -48,7 +85,6 @@ class ActionModelNumDiff : public ActionModelAbstract {
* For full discussions see issue
* https://gepgitlab.laas.fr/loco-3d/crocoddyl/issues/139
*
- * @param model object to be checked.
* @param x is the state at which the check is performed.
*/
void assertStableStateFD(const Eigen::Ref<const Eigen::VectorXd>& x);
@@ -91,14 +127,14 @@ struct ActionDataNumDiff : public ActionDataAbstract {
}
}
- Eigen::MatrixXd Rx; //!< Cost Jacobian:
- Eigen::MatrixXd Ru;
- Eigen::VectorXd dx;
- Eigen::VectorXd du;
- Eigen::VectorXd xp;
- boost::shared_ptr<ActionDataAbstract> data_0;
- std::vector<boost::shared_ptr<ActionDataAbstract> > data_x;
- std::vector<boost::shared_ptr<ActionDataAbstract> > data_u;
+ Eigen::MatrixXd Rx; //!< Cost residual jacobian: d r / dx
+ Eigen::MatrixXd Ru; //!< Cost residual jacobian: d r / du
+ Eigen::VectorXd dx; //!< State disturbance
+ Eigen::VectorXd du; //!< Control disturbance
+ Eigen::VectorXd xp; //!< The integrated state from the disturbance on one DoF "\f$ \int x dx_i \f$"
+ boost::shared_ptr<ActionDataAbstract> data_0; //!< The data that contains the final results
+ std::vector<boost::shared_ptr<ActionDataAbstract> > data_x; //!< The temporary data associated with the state variation
+ std::vector<boost::shared_ptr<ActionDataAbstract> > data_u; //!< The temporary data associated with the control variation
};
} // namespace crocoddyl
diff --git a/src/core/numdiff/action.cpp b/src/core/numdiff/action.cpp
index 756d3c32c9e6c19d5f9ac3063afd6efd43af4369..82418e3bf815f15907bca5419fd89ad038870cb1 100644
--- a/src/core/numdiff/action.cpp
+++ b/src/core/numdiff/action.cpp
@@ -58,7 +58,9 @@ void ActionModelNumDiff::calcDiff(const boost::shared_ptr<ActionDataAbstract>& d
model_.get_state().diff(xn0, xn, data_nd->Fx.col(ix));
data->Lx(ix) = (c - c0) / disturbance_;
- data_nd->Rx.col(ix) = (data_nd->data_x[ix]->r - data_nd->data_0->r) / disturbance_;
+ if (with_gauss_approx_) {
+ data_nd->Rx.col(ix) = (data_nd->data_x[ix]->r - data_nd->data_0->r) / disturbance_;
+ }
data_nd->dx(ix) = 0.0;
}
data->Fx /= disturbance_;
@@ -74,7 +76,9 @@ void ActionModelNumDiff::calcDiff(const boost::shared_ptr<ActionDataAbstract>& d
model_.get_state().diff(xn0, xn, data_nd->Fu.col(iu));
data->Lu(iu) = (c - c0) / disturbance_;
- data_nd->Ru.col(iu) = (data_nd->data_u[iu]->r - data_nd->data_0->r) / disturbance_;
+ if (with_gauss_approx_) {
+ data_nd->Ru.col(iu) = (data_nd->data_u[iu]->r - data_nd->data_0->r) / disturbance_;
+ }
data_nd->du(iu) = 0.0;
}
data->Fu /= disturbance_;
@@ -100,33 +104,7 @@ const double& ActionModelNumDiff::get_disturbance() const { return disturbance_;
bool ActionModelNumDiff::get_with_gauss_approx() { return with_gauss_approx_; }
void ActionModelNumDiff::assertStableStateFD(const Eigen::Ref<const Eigen::VectorXd>& /** x */) {
- // TODO(mnaveau): make this method virtual and this one should do nothing, update the documentation.
- // md = model_.differential_;
- // if isinstance(md, DifferentialActionModelFloatingInContact)
- // {
- // if hasattr(md, "costs")
- // {
- // mc = md.costs;
- // if isinstance(mc, CostModelState)
- // {
- // assert (~np.isclose(model.State.diff(mc.ref, x)[3:6], np.ones(3) * np.pi, atol=1e-6).any())
- // assert (~np.isclose(model.State.diff(mc.ref, x)[3:6], -np.ones(3) * np.pi, atol=1e-6).any())
- // }
- // else if isinstance(mc, CostModelSum)
- // {
- // for (key, cost) in mc.costs.items()
- // {
- // if isinstance(cost.cost, CostModelState)
- // {
- // assert (~np.isclose(
- // model.State.diff(cost.cost.ref, x)[3:6], np.ones(3) * np.pi, atol=1e-6).any())
- // assert (~np.isclose(
- // model.State.diff(cost.cost.ref, x)[3:6], -np.ones(3) * np.pi, atol=1e-6).any())
- // }
- // }
- // }
- // }
- // }
+ // do nothing in the general case
}
boost::shared_ptr<ActionDataAbstract> ActionModelNumDiff::createData() {
diff --git a/unittest/test_actions.cpp b/unittest/test_actions.cpp
index e9e6f4a0ae2bdd8c82604adbcea9bb339a21548b..c3e44b68eac3fb20e97eb4e9af8338019e5f3ba6 100644
--- a/unittest/test_actions.cpp
+++ b/unittest/test_actions.cpp
@@ -79,6 +79,10 @@ void test_partial_derivatives_against_numdiff(crocoddyl::ActionModelAbstract& mo
BOOST_CHECK((data->Lxx - data_num_diff->Lxx).isMuchSmallerThan(1.0, tol));
BOOST_CHECK((data->Lxu - data_num_diff->Lxu).isMuchSmallerThan(1.0, tol));
BOOST_CHECK((data->Luu - data_num_diff->Luu).isMuchSmallerThan(1.0, tol));
+ }else{
+ BOOST_CHECK((data_num_diff->Lxx).isMuchSmallerThan(1.0, tol));
+ BOOST_CHECK((data_num_diff->Lxu).isMuchSmallerThan(1.0, tol));
+ BOOST_CHECK((data_num_diff->Luu).isMuchSmallerThan(1.0, tol));
}
}