Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/crocoddyl/multibody/actions/free-fwddyn.hpp	Lines:	18	21	85.7 %
Date:	2024-02-13 11:12:33	Branches:	20	38	52.6 %


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019-2022, LAAS-CNRS, University of Edinburgh,
//                          Heriot-Watt University
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#ifndef CROCODDYL_MULTIBODY_ACTIONS_FREE_FWDDYN_HPP_
#define CROCODDYL_MULTIBODY_ACTIONS_FREE_FWDDYN_HPP_

#include <stdexcept>

#ifdef PINOCCHIO_WITH_CPPAD_SUPPORT  // TODO(cmastalli): Removed after merging
                                     // Pinocchio v.2.4.8
#include <pinocchio/codegen/cppadcg.hpp>
#endif

#include "crocoddyl/core/actuation-base.hpp"
#include "crocoddyl/core/constraints/constraint-manager.hpp"
#include "crocoddyl/core/costs/cost-sum.hpp"
#include "crocoddyl/core/diff-action-base.hpp"
#include "crocoddyl/core/utils/exception.hpp"
#include "crocoddyl/multibody/data/multibody.hpp"
#include "crocoddyl/multibody/fwd.hpp"
#include "crocoddyl/multibody/states/multibody.hpp"

namespace crocoddyl {

/**
 * @brief Differential action model for free forward dynamics in multibody
 * systems.
 *
 * This class implements free forward dynamics, i.e.,
 * \f[
 * \mathbf{M}\dot{\mathbf{v}} + \mathbf{h}(\mathbf{q},\mathbf{v}) =
 * \boldsymbol{\tau}, \f] where \f$\mathbf{q}\in Q\f$,
 * \f$\mathbf{v}\in\mathbb{R}^{nv}\f$ are the configuration point and
 * generalized velocity (its tangent vector), respectively;
 * \f$\boldsymbol{\tau}\f$ is the torque inputs and
 * \f$\mathbf{h}(\mathbf{q},\mathbf{v})\f$ are the Coriolis effect and gravity
 * field.
 *
 * The derivatives of the system acceleration is computed efficiently based on
 * the analytical derivatives of Articulate Body Algorithm (ABA) as described in
 * \cite carpentier-rss18.
 *
 * The stack of cost functions is implemented in `CostModelSumTpl`. The
 * computation of the free forward dynamics and its derivatives are carrying out
 * inside `calc()` and `calcDiff()` functions, respectively. It is also
 * important to remark that `calcDiff()` computes the derivatives using the
 * latest stored values by `calc()`. Thus, we need to run `calc()` first.
 *
 * \sa `DifferentialActionModelAbstractTpl`, `calc()`, `calcDiff()`,
 * `createData()`
 */
template <typename _Scalar>
class DifferentialActionModelFreeFwdDynamicsTpl
    : public DifferentialActionModelAbstractTpl<_Scalar> {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  typedef _Scalar Scalar;
  typedef DifferentialActionModelAbstractTpl<Scalar> Base;
  typedef DifferentialActionDataFreeFwdDynamicsTpl<Scalar> Data;
  typedef DifferentialActionDataAbstractTpl<Scalar>
      DifferentialActionDataAbstract;
  typedef StateMultibodyTpl<Scalar> StateMultibody;
  typedef CostModelSumTpl<Scalar> CostModelSum;
  typedef ConstraintModelManagerTpl<Scalar> ConstraintModelManager;
  typedef ActuationModelAbstractTpl<Scalar> ActuationModelAbstract;
  typedef MathBaseTpl<Scalar> MathBase;
  typedef typename MathBase::VectorXs VectorXs;
  typedef typename MathBase::MatrixXs MatrixXs;

  DifferentialActionModelFreeFwdDynamicsTpl(
      boost::shared_ptr<StateMultibody> state,
      boost::shared_ptr<ActuationModelAbstract> actuation,
      boost::shared_ptr<CostModelSum> costs,
      boost::shared_ptr<ConstraintModelManager> constraints = nullptr);
  virtual ~DifferentialActionModelFreeFwdDynamicsTpl();

  /**
   * @brief Compute the system acceleration, and cost value
   *
   * It computes the system acceleration using the free forward-dynamics.
   *
   * @param[in] data  Free forward-dynamics data
   * @param[in] x     State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
   * @param[in] u     Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
   */
  virtual void calc(
      const boost::shared_ptr<DifferentialActionDataAbstract>& data,
      const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u);

  /**
   * @brief @copydoc Base::calc(const
   * boost::shared_ptr<DifferentialActionDataAbstract>& data, const
   * Eigen::Ref<const VectorXs>& x)
   */
  virtual void calc(
      const boost::shared_ptr<DifferentialActionDataAbstract>& data,
      const Eigen::Ref<const VectorXs>& x);

  /**
   * @brief Compute the derivatives of the contact dynamics, and cost function
   *
   * @param[in] data  Free forward-dynamics data
   * @param[in] x     State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
   * @param[in] u     Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
   */
  virtual void calcDiff(
      const boost::shared_ptr<DifferentialActionDataAbstract>& data,
      const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u);

  /**
   * @brief @copydoc Base::calcDiff(const
   * boost::shared_ptr<DifferentialActionDataAbstract>& data, const
   * Eigen::Ref<const VectorXs>& x)
   */
  virtual void calcDiff(
      const boost::shared_ptr<DifferentialActionDataAbstract>& data,
      const Eigen::Ref<const VectorXs>& x);

  /**
   * @brief Create the free forward-dynamics data
   *
   * @return free forward-dynamics data
   */
  virtual boost::shared_ptr<DifferentialActionDataAbstract> createData();

  /**
   * @brief Check that the given data belongs to the free forward-dynamics data
   */
  virtual bool checkData(
      const boost::shared_ptr<DifferentialActionDataAbstract>& data);

  /**
   * @brief @copydoc Base::quasiStatic()
   */
  virtual void quasiStatic(
      const boost::shared_ptr<DifferentialActionDataAbstract>& data,
      Eigen::Ref<VectorXs> u, const Eigen::Ref<const VectorXs>& x,
      const std::size_t maxiter = 100, const Scalar tol = Scalar(1e-9));

  /**
   * @brief Return the number of inequality constraints
   */
  virtual std::size_t get_ng() const;

  /**
   * @brief Return the number of equality constraints
   */
  virtual std::size_t get_nh() const;

  /**
   * @brief Return the lower bound of the inequality constraints
   */
  virtual const VectorXs& get_g_lb() const;

  /**
   * @brief Return the upper bound of the inequality constraints
   */
  virtual const VectorXs& get_g_ub() const;

  /**
   * @brief Return the actuation model
   */
  const boost::shared_ptr<ActuationModelAbstract>& get_actuation() const;

  /**
   * @brief Return the cost model
   */
  const boost::shared_ptr<CostModelSum>& get_costs() const;

  /**
   * @brief Return the constraint model manager
   */
  const boost::shared_ptr<ConstraintModelManager>& get_constraints() const;

  /**
   * @brief Return the Pinocchio model
   */
  pinocchio::ModelTpl<Scalar>& get_pinocchio() const;

  /**
   * @brief Return the armature vector
   */
  const VectorXs& get_armature() const;

  /**
   * @brief Modify the armature vector
   */
  void set_armature(const VectorXs& armature);

  /**
   * @brief Print relevant information of the free forward-dynamics model
   *
   * @param[out] os  Output stream object
   */
  virtual void print(std::ostream& os) const;

 protected:
  using Base::g_lb_;   //!< Lower bound of the inequality constraints
  using Base::g_ub_;   //!< Upper bound of the inequality constraints
  using Base::nu_;     //!< Control dimension
  using Base::state_;  //!< Model of the state

 private:
  boost::shared_ptr<ActuationModelAbstract> actuation_;    //!< Actuation model
  boost::shared_ptr<CostModelSum> costs_;                  //!< Cost model
  boost::shared_ptr<ConstraintModelManager> constraints_;  //!< Constraint model
  pinocchio::ModelTpl<Scalar>& pinocchio_;                 //!< Pinocchio model
  bool without_armature_;  //!< Indicate if we have defined an armature
  VectorXs armature_;      //!< Armature vector
};

template <typename _Scalar>
struct DifferentialActionDataFreeFwdDynamicsTpl
    : public DifferentialActionDataAbstractTpl<_Scalar> {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  typedef _Scalar Scalar;
  typedef MathBaseTpl<Scalar> MathBase;
  typedef DifferentialActionDataAbstractTpl<Scalar> Base;
  typedef JointDataAbstractTpl<Scalar> JointDataAbstract;
  typedef DataCollectorJointActMultibodyTpl<Scalar>
      DataCollectorJointActMultibody;
  typedef typename MathBase::VectorXs VectorXs;
  typedef typename MathBase::MatrixXs MatrixXs;

  template <template <typename Scalar> class Model>
  explicit DifferentialActionDataFreeFwdDynamicsTpl(Model<Scalar>* const model)
      : Base(model),
        pinocchio(pinocchio::DataTpl<Scalar>(model->get_pinocchio())),
        multibody(
            &pinocchio, model->get_actuation()->createData(),
            boost::make_shared<JointDataAbstract>(
                model->get_state(), model->get_actuation(), model->get_nu())),
        costs(model->get_costs()->createData(&multibody)),
        Minv(model->get_state()->get_nv(), model->get_state()->get_nv()),
        u_drift(model->get_state()->get_nv()),
        dtau_dx(model->get_state()->get_nv(), model->get_state()->get_ndx()),
        tmp_xstatic(model->get_state()->get_nx()) {
    multibody.joint->dtau_du.diagonal().setOnes();
    costs->shareMemory(this);
    if (model->get_constraints() != nullptr) {
      constraints = model->get_constraints()->createData(&multibody);
      constraints->shareMemory(this);
    }
    Minv.setZero();
    u_drift.setZero();
    dtau_dx.setZero();
    tmp_xstatic.setZero();
  }

  pinocchio::DataTpl<Scalar> pinocchio;
  DataCollectorJointActMultibody multibody;
  boost::shared_ptr<CostDataSumTpl<Scalar> > costs;
  boost::shared_ptr<ConstraintDataManagerTpl<Scalar> > constraints;
  MatrixXs Minv;
  VectorXs u_drift;
  MatrixXs dtau_dx;
  VectorXs tmp_xstatic;

  using Base::cost;
  using Base::Fu;
  using Base::Fx;
  using Base::Lu;
  using Base::Luu;
  using Base::Lx;
  using Base::Lxu;
  using Base::Lxx;
  using Base::r;
  using Base::xout;
};

}  // namespace crocoddyl

/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
#include <crocoddyl/multibody/actions/free-fwddyn.hxx>

#endif  // CROCODDYL_MULTIBODY_ACTIONS_FREE_FWDDYN_HPP_


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2019-2022, LAAS-CNRS, University of Edinburgh,
5			// Heriot-Watt University
6			// Copyright note valid unless otherwise stated in individual files.
7			// All rights reserved.
8			///////////////////////////////////////////////////////////////////////////////
9
10			#ifndef CROCODDYL_MULTIBODY_ACTIONS_FREE_FWDDYN_HPP_
11			#define CROCODDYL_MULTIBODY_ACTIONS_FREE_FWDDYN_HPP_
12
13			#include <stdexcept>
14
15			#ifdef PINOCCHIO_WITH_CPPAD_SUPPORT // TODO(cmastalli): Removed after merging
16			// Pinocchio v.2.4.8
17			#include <pinocchio/codegen/cppadcg.hpp>
18			#endif
19
20			#include "crocoddyl/core/actuation-base.hpp"
21			#include "crocoddyl/core/constraints/constraint-manager.hpp"
22			#include "crocoddyl/core/costs/cost-sum.hpp"
23			#include "crocoddyl/core/diff-action-base.hpp"
24			#include "crocoddyl/core/utils/exception.hpp"
25			#include "crocoddyl/multibody/data/multibody.hpp"
26			#include "crocoddyl/multibody/fwd.hpp"
27			#include "crocoddyl/multibody/states/multibody.hpp"
28
29			namespace crocoddyl {
30
31			/**
32			* @brief Differential action model for free forward dynamics in multibody
33			* systems.
34			*
35			* This class implements free forward dynamics, i.e.,
36			* \f[
37			* \mathbf{M}\dot{\mathbf{v}} + \mathbf{h}(\mathbf{q},\mathbf{v}) =
38			* \boldsymbol{\tau}, \f] where \f$\mathbf{q}\in Q\f$,
39			* \f$\mathbf{v}\in\mathbb{R}^{nv}\f$ are the configuration point and
40			* generalized velocity (its tangent vector), respectively;
41			* \f$\boldsymbol{\tau}\f$ is the torque inputs and
42			* \f$\mathbf{h}(\mathbf{q},\mathbf{v})\f$ are the Coriolis effect and gravity
43			* field.
44			*
45			* The derivatives of the system acceleration is computed efficiently based on
46			* the analytical derivatives of Articulate Body Algorithm (ABA) as described in
47			* \cite carpentier-rss18.
48			*
49			* The stack of cost functions is implemented in `CostModelSumTpl`. The
50			* computation of the free forward dynamics and its derivatives are carrying out
51			* inside `calc()` and `calcDiff()` functions, respectively. It is also
52			* important to remark that `calcDiff()` computes the derivatives using the
53			* latest stored values by `calc()`. Thus, we need to run `calc()` first.
54			*
55			* \sa `DifferentialActionModelAbstractTpl`, `calc()`, `calcDiff()`,
56			* `createData()`
57			*/
58			template <typename _Scalar>
59			class DifferentialActionModelFreeFwdDynamicsTpl
60			: public DifferentialActionModelAbstractTpl<_Scalar> {
61			public:
62			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
63
64			typedef _Scalar Scalar;
65			typedef DifferentialActionModelAbstractTpl<Scalar> Base;
66			typedef DifferentialActionDataFreeFwdDynamicsTpl<Scalar> Data;
67			typedef DifferentialActionDataAbstractTpl<Scalar>
68			DifferentialActionDataAbstract;
69			typedef StateMultibodyTpl<Scalar> StateMultibody;
70			typedef CostModelSumTpl<Scalar> CostModelSum;
71			typedef ConstraintModelManagerTpl<Scalar> ConstraintModelManager;
72			typedef ActuationModelAbstractTpl<Scalar> ActuationModelAbstract;
73			typedef MathBaseTpl<Scalar> MathBase;
74			typedef typename MathBase::VectorXs VectorXs;
75			typedef typename MathBase::MatrixXs MatrixXs;
76
77			DifferentialActionModelFreeFwdDynamicsTpl(
78			boost::shared_ptr<StateMultibody> state,
79			boost::shared_ptr<ActuationModelAbstract> actuation,
80			boost::shared_ptr<CostModelSum> costs,
81			boost::shared_ptr<ConstraintModelManager> constraints = nullptr);
82			virtual ~DifferentialActionModelFreeFwdDynamicsTpl();
83
84			/**
85			* @brief Compute the system acceleration, and cost value
86			*
87			* It computes the system acceleration using the free forward-dynamics.
88			*
89			* @param[in] data Free forward-dynamics data
90			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
91			* @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
92			*/
93			virtual void calc(
94			const boost::shared_ptr<DifferentialActionDataAbstract>& data,
95			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u);
96
97			/**
98			* @brief @copydoc Base::calc(const
99			* boost::shared_ptr<DifferentialActionDataAbstract>& data, const
100			* Eigen::Ref<const VectorXs>& x)
101			*/
102			virtual void calc(
103			const boost::shared_ptr<DifferentialActionDataAbstract>& data,
104			const Eigen::Ref<const VectorXs>& x);
105
106			/**
107			* @brief Compute the derivatives of the contact dynamics, and cost function
108			*
109			* @param[in] data Free forward-dynamics data
110			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
111			* @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
112			*/
113			virtual void calcDiff(
114			const boost::shared_ptr<DifferentialActionDataAbstract>& data,
115			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u);
116
117			/**
118			* @brief @copydoc Base::calcDiff(const
119			* boost::shared_ptr<DifferentialActionDataAbstract>& data, const
120			* Eigen::Ref<const VectorXs>& x)
121			*/
122			virtual void calcDiff(
123			const boost::shared_ptr<DifferentialActionDataAbstract>& data,
124			const Eigen::Ref<const VectorXs>& x);
125
126			/**
127			* @brief Create the free forward-dynamics data
128			*
129			* @return free forward-dynamics data
130			*/
131			virtual boost::shared_ptr<DifferentialActionDataAbstract> createData();
132
133			/**
134			* @brief Check that the given data belongs to the free forward-dynamics data
135			*/
136			virtual bool checkData(
137			const boost::shared_ptr<DifferentialActionDataAbstract>& data);
138
139			/**
140			* @brief @copydoc Base::quasiStatic()
141			*/
142			virtual void quasiStatic(
143			const boost::shared_ptr<DifferentialActionDataAbstract>& data,
144			Eigen::Ref<VectorXs> u, const Eigen::Ref<const VectorXs>& x,
145			const std::size_t maxiter = 100, const Scalar tol = Scalar(1e-9));
146
147			/**
148			* @brief Return the number of inequality constraints
149			*/
150			virtual std::size_t get_ng() const;
151
152			/**
153			* @brief Return the number of equality constraints
154			*/
155			virtual std::size_t get_nh() const;
156
157			/**
158			* @brief Return the lower bound of the inequality constraints
159			*/
160			virtual const VectorXs& get_g_lb() const;
161
162			/**
163			* @brief Return the upper bound of the inequality constraints
164			*/
165			virtual const VectorXs& get_g_ub() const;
166
167			/**
168			* @brief Return the actuation model
169			*/
170			const boost::shared_ptr<ActuationModelAbstract>& get_actuation() const;
171
172			/**
173			* @brief Return the cost model
174			*/
175			const boost::shared_ptr<CostModelSum>& get_costs() const;
176
177			/**
178			* @brief Return the constraint model manager
179			*/
180			const boost::shared_ptr<ConstraintModelManager>& get_constraints() const;
181
182			/**
183			* @brief Return the Pinocchio model
184			*/
185			pinocchio::ModelTpl<Scalar>& get_pinocchio() const;
186
187			/**
188			* @brief Return the armature vector
189			*/
190			const VectorXs& get_armature() const;
191
192			/**
193			* @brief Modify the armature vector
194			*/
195			void set_armature(const VectorXs& armature);
196
197			/**
198			* @brief Print relevant information of the free forward-dynamics model
199			*
200			* @param[out] os Output stream object
201			*/
202			virtual void print(std::ostream& os) const;
203
204			protected:
205			using Base::g_lb_; //!< Lower bound of the inequality constraints
206			using Base::g_ub_; //!< Upper bound of the inequality constraints
207			using Base::nu_; //!< Control dimension
208			using Base::state_; //!< Model of the state
209
210			private:
211			boost::shared_ptr<ActuationModelAbstract> actuation_; //!< Actuation model
212			boost::shared_ptr<CostModelSum> costs_; //!< Cost model
213			boost::shared_ptr<ConstraintModelManager> constraints_; //!< Constraint model
214			pinocchio::ModelTpl<Scalar>& pinocchio_; //!< Pinocchio model
215			bool without_armature_; //!< Indicate if we have defined an armature
216			VectorXs armature_; //!< Armature vector
217			};
218
219			template <typename _Scalar>
220			struct DifferentialActionDataFreeFwdDynamicsTpl
221			: public DifferentialActionDataAbstractTpl<_Scalar> {
222			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
223			typedef _Scalar Scalar;
224			typedef MathBaseTpl<Scalar> MathBase;
225			typedef DifferentialActionDataAbstractTpl<Scalar> Base;
226			typedef JointDataAbstractTpl<Scalar> JointDataAbstract;
227			typedef DataCollectorJointActMultibodyTpl<Scalar>
228			DataCollectorJointActMultibody;
229			typedef typename MathBase::VectorXs VectorXs;
230			typedef typename MathBase::MatrixXs MatrixXs;
231
232			template <template <typename Scalar> class Model>
233		10259	explicit DifferentialActionDataFreeFwdDynamicsTpl(Model<Scalar>* const model)
234			: Base(model),
235		10259	pinocchio(pinocchio::DataTpl<Scalar>(model->get_pinocchio())),
236			multibody(
237		10259	&pinocchio, model->get_actuation()->createData(),
238			boost::make_shared<JointDataAbstract>(
239			model->get_state(), model->get_actuation(), model->get_nu())),
240		10259	costs(model->get_costs()->createData(&multibody)),
241		20518	Minv(model->get_state()->get_nv(), model->get_state()->get_nv()),
242		10259	u_drift(model->get_state()->get_nv()),
243		20518	dtau_dx(model->get_state()->get_nv(), model->get_state()->get_ndx()),
244	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	20518	tmp_xstatic(model->get_state()->get_nx()) {
245	✓✗✓✗	10259	multibody.joint->dtau_du.diagonal().setOnes();
246	✓✗	10259	costs->shareMemory(this);
247	✓✓	10259	if (model->get_constraints() != nullptr) {
248	✓✗	6663	constraints = model->get_constraints()->createData(&multibody);
249	✓✗	6663	constraints->shareMemory(this);
250			}
251	✓✗	10259	Minv.setZero();
252	✓✗	10259	u_drift.setZero();
253	✓✗	10259	dtau_dx.setZero();
254	✓✗	10259	tmp_xstatic.setZero();
255		10259	}
256
257			pinocchio::DataTpl<Scalar> pinocchio;
258			DataCollectorJointActMultibody multibody;
259			boost::shared_ptr<CostDataSumTpl<Scalar> > costs;
260			boost::shared_ptr<ConstraintDataManagerTpl<Scalar> > constraints;
261			MatrixXs Minv;
262			VectorXs u_drift;
263			MatrixXs dtau_dx;
264			VectorXs tmp_xstatic;
265
266			using Base::cost;
267			using Base::Fu;
268			using Base::Fx;
269			using Base::Lu;
270			using Base::Luu;
271			using Base::Lx;
272			using Base::Lxu;
273			using Base::Lxx;
274			using Base::r;
275			using Base::xout;
276			};
277
278			} // namespace crocoddyl
279
280			/* --- Details -------------------------------------------------------------- */
281			/* --- Details -------------------------------------------------------------- */
282			/* --- Details -------------------------------------------------------------- */
283			#include <crocoddyl/multibody/actions/free-fwddyn.hxx>
284
285			#endif // CROCODDYL_MULTIBODY_ACTIONS_FREE_FWDDYN_HPP_