Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/crocoddyl/multibody/actions/free-invdyn.hpp	Lines:	57	68	83.8 %
Date:	2024-02-13 11:12:33	Branches:	36	72	50.0 %


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

#ifndef CROCODDYL_MULTIBODY_ACTIONS_FREE_INVDYN_HPP_
#define CROCODDYL_MULTIBODY_ACTIONS_FREE_INVDYN_HPP_

#include <stdexcept>

#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/residual-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 inverse dynamics in multibody
 * systems.
 *
 * This class implements forward kinematic with an inverse-dynamics computed
 * using the Recursive Newton Euler Algorithm (RNEA). The stack of cost and
 * constraint functions are implemented in `CostModelSumTpl` and
 * `ConstraintModelManagerTpl`, respectively. The accelerations are the decision
 * variables defined as the control inputs, and the under-actuation constraint
 * is under the name `tau`, thus the user is not allowed to use it.
 *
 *
 * \sa `DifferentialActionModelAbstractTpl`, `calc()`, `calcDiff()`,
 * `createData()`
 */
template <typename _Scalar>
class DifferentialActionModelFreeInvDynamicsTpl
    : public DifferentialActionModelAbstractTpl<_Scalar> {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

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

  /**
   * @brief Initialize the free inverse-dynamics action model
   *
   * It describes the kinematic evolution of the multibody system and computes
   * the needed torques using inverse dynamics.
   *
   * @param[in] state      State of the multibody system
   * @param[in] actuation  Actuation model
   * @param[in] costs      Cost model
   */
  DifferentialActionModelFreeInvDynamicsTpl(
      boost::shared_ptr<StateMultibody> state,
      boost::shared_ptr<ActuationModelAbstract> actuation,
      boost::shared_ptr<CostModelSum> costs);

  /**
   * @brief Initialize the free inverse-dynamics action model
   *
   * @param[in] state        State of the multibody system
   * @param[in] actuation    Actuation model
   * @param[in] costs        Cost model
   * @param[in] constraints  Constraints model
   */
  DifferentialActionModelFreeInvDynamicsTpl(
      boost::shared_ptr<StateMultibody> state,
      boost::shared_ptr<ActuationModelAbstract> actuation,
      boost::shared_ptr<CostModelSum> costs,
      boost::shared_ptr<ConstraintModelManager> constraints);
  virtual ~DifferentialActionModelFreeInvDynamicsTpl();

  /**
   * @brief Compute the system acceleration, cost value and constraint residuals
   *
   * It extracts the acceleration value from control vector and also computes
   * the cost and constraints.
   *
   * @param[in] data  Free inverse-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 dynamics, cost and constraint
   * functions
   *
   * It computes the partial derivatives of the dynamical system and the cost
   * and contraint functions. It assumes that `calc()` has been run first. This
   * function builds a quadratic approximation of the time-continuous action
   * model (i.e., dynamical system, cost and constraint functions).
   *
   * @param[in] data  Free inverse-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 inverse-dynamics data
   *
   * @return free inverse-dynamics data
   */
  virtual boost::shared_ptr<DifferentialActionDataAbstract> createData();

  /**
   * @brief Checks that a specific data belongs to the free inverse-dynamics
   * model
   */
  virtual bool checkData(
      const boost::shared_ptr<DifferentialActionDataAbstract>& data);

  /**
   * @brief Computes the quasic static commands
   *
   * The quasic static commands are the ones produced for a reference posture as
   * an equilibrium point with zero acceleration, i.e., for
   * \f$\mathbf{f^q_x}\delta\mathbf{q}+\mathbf{f_u}\delta\mathbf{u}=\mathbf{0}\f$
   *
   * @param[in] data     Free inverse-dynamics data
   * @param[out] u       Quasic-static commands
   * @param[in] x        State point (velocity has to be zero)
   * @param[in] maxiter  Maximum allowed number of iterations (default 100)
   * @param[in] tol      Tolerance (default 1e-9)
   */
  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 Print relevant information of the free inverse-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::ng_;     //!< Number of inequality constraints
  using Base::nh_;     //!< Number of equality constraints
  using Base::nu_;     //!< Control dimension
  using Base::state_;  //!< Model of the state

 private:
  void init(const boost::shared_ptr<StateMultibody>& state);
  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

 public:
  /**
   * @brief Actuation residual
   *
   * This residual function enforces the torques of under-actuated joints (e.g.,
   * floating-base joints) to be zero. We compute these torques and their
   * derivatives using RNEA inside `DifferentialActionModelFreeInvDynamicsTpl`.
   *
   * As described in `ResidualModelAbstractTpl`, the residual value and its
   * Jacobians are calculated by `calc` and `calcDiff`, respectively.
   *
   * \sa `ResidualModelAbstractTpl`, `calc()`, `calcDiff()`, `createData()`
   */
  class ResidualModelActuation : public ResidualModelAbstractTpl<_Scalar> {
   public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    typedef _Scalar Scalar;
    typedef MathBaseTpl<Scalar> MathBase;
    typedef ResidualModelAbstractTpl<Scalar> Base;
    typedef StateMultibodyTpl<Scalar> StateMultibody;
    typedef ResidualDataAbstractTpl<Scalar> ResidualDataAbstract;
    typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
    typedef ActuationModelAbstractTpl<Scalar> ActuationModelAbstract;
    typedef typename MathBase::VectorXs VectorXs;
    typedef typename MathBase::MatrixXs MatrixXs;

    /**
     * @brief Initialize the actuation residual model
     *
     * @param[in] state  State of the multibody system
     * @param[in] nu     Dimension of the joint torques
     */
    ResidualModelActuation(boost::shared_ptr<StateMultibody> state,
                           const std::size_t nu)
        : Base(state, state->get_nv() - nu, state->get_nv(), true, true, true),
          na_(nu) {}
    virtual ~ResidualModelActuation() {}

    /**
     * @brief Compute the actuation residual
     *
     * @param[in] data  Actuation residual 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}^{nv+nu}\f$
     */
    virtual void calc(const boost::shared_ptr<ResidualDataAbstract>& data,
                      const Eigen::Ref<const VectorXs>&,
                      const Eigen::Ref<const VectorXs>&) {
      typename Data::ResidualDataActuation* d =
          static_cast<typename Data::ResidualDataActuation*>(data.get());
      // Update the under-actuation set and residual
      std::size_t nrow = 0;
      for (std::size_t k = 0;
           k < static_cast<std::size_t>(d->actuation->tau_set.size()); ++k) {
        if (!d->actuation->tau_set[k]) {
          data->r(nrow) = d->pinocchio->tau(k);
          nrow += 1;
        }
      }
    }

    /**
     * @brief @copydoc Base::calc(const boost::shared_ptr<ResidualDataAbstract>&
     * data, const Eigen::Ref<const VectorXs>& x)
     */
    virtual void calc(const boost::shared_ptr<ResidualDataAbstract>& data,
                      const Eigen::Ref<const VectorXs>&) {
      data->r.setZero();
    }

    /**
     * @brief Compute the derivatives of the actuation residual
     *
     * @param[in] data  Actuation residual 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<ResidualDataAbstract>& data,
                          const Eigen::Ref<const VectorXs>&,
                          const Eigen::Ref<const VectorXs>&) {
      typename Data::ResidualDataActuation* d =
          static_cast<typename Data::ResidualDataActuation*>(data.get());
      std::size_t nrow = 0;
      const std::size_t nv = state_->get_nv();
      d->dtau_dx.leftCols(nv) = d->pinocchio->dtau_dq;
      d->dtau_dx.rightCols(nv) = d->pinocchio->dtau_dv;
      d->dtau_dx -= d->actuation->dtau_dx;
      for (std::size_t k = 0;
           k < static_cast<std::size_t>(d->actuation->tau_set.size()); ++k) {
        if (!d->actuation->tau_set[k]) {
          d->Rx.row(nrow) = d->dtau_dx.row(k);
          d->Ru.row(nrow) = d->pinocchio->M.row(k);
          nrow += 1;
        }
      }
    }

    /**
     * @brief @copydoc Base::calcDiff(const
     * boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const
     * VectorXs>& x)
     */
    virtual void calcDiff(const boost::shared_ptr<ResidualDataAbstract>& data,
                          const Eigen::Ref<const VectorXs>&) {
      data->Rx.setZero();
      data->Ru.setZero();
    }

    /**
     * @brief Create the actuation residual data
     *
     * @return Actuation residual data
     */
    virtual boost::shared_ptr<ResidualDataAbstract> createData(
        DataCollectorAbstract* const data) {
      return boost::allocate_shared<typename Data::ResidualDataActuation>(
          Eigen::aligned_allocator<typename Data::ResidualDataActuation>(),
          this, data);
    }

    /**
     * @brief Print relevant information of the actuation residual model
     *
     * @param[out] os  Output stream object
     */
    virtual void print(std::ostream& os) const {
      os << "ResidualModelActuation {nx=" << state_->get_nx()
         << ", ndx=" << state_->get_ndx() << ", nu=" << nu_ << ", na=" << na_
         << "}";
    }

   protected:
    std::size_t na_;  //!< Dimension of the joint torques
    using Base::nu_;
    using Base::state_;
  };
};

template <typename _Scalar>
struct DifferentialActionDataFreeInvDynamicsTpl
    : 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 CostDataSumTpl<Scalar> CostDataSum;
  typedef ConstraintDataManagerTpl<Scalar> ConstraintDataManager;
  typedef typename MathBase::VectorXs VectorXs;

  template <template <typename Scalar> class Model>
  explicit DifferentialActionDataFreeInvDynamicsTpl(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)),
        constraints(model->get_constraints()->createData(&multibody)),
        tmp_xstatic(model->get_state()->get_nx()) {
    const std::size_t nv = model->get_state()->get_nv();
    Fu.leftCols(nv).diagonal().setOnes();
    multibody.joint->da_du.leftCols(nv).diagonal().setOnes();
    costs->shareMemory(this);
    constraints->shareMemory(this);
    tmp_xstatic.setZero();
  }

  pinocchio::DataTpl<Scalar> pinocchio;                  //!< Pinocchio data
  DataCollectorJointActMultibody multibody;              //!< Multibody data
  boost::shared_ptr<CostDataSum> costs;                  //!< Costs data
  boost::shared_ptr<ConstraintDataManager> constraints;  //!< Constraints data
  VectorXs
      tmp_xstatic;  //!< State point used for computing the quasi-static input
  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;

  struct ResidualDataActuation : public ResidualDataAbstractTpl<_Scalar> {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    typedef _Scalar Scalar;
    typedef MathBaseTpl<Scalar> MathBase;
    typedef ResidualDataAbstractTpl<Scalar> Base;
    typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
    typedef DataCollectorActMultibodyTpl<Scalar> DataCollectorActMultibody;
    typedef ActuationDataAbstractTpl<Scalar> ActuationDataAbstract;
    typedef typename MathBase::MatrixXs MatrixXs;

    template <template <typename Scalar> class Model>
    ResidualDataActuation(Model<Scalar>* const model,
                          DataCollectorAbstract* const data)
        : Base(model, data),
          dtau_dx(model->get_state()->get_nv(), model->get_state()->get_ndx()) {
      dtau_dx.setZero();
      // Check that proper shared data has been passed
      DataCollectorActMultibody* d =
          dynamic_cast<DataCollectorActMultibody*>(shared);
      if (d == NULL) {
        throw_pretty(
            "Invalid argument: the shared data should be derived from "
            "DataCollectorActMultibody");
      }

      // Avoids data casting at runtime
      pinocchio = d->pinocchio;
      actuation = d->actuation;
    }

    pinocchio::DataTpl<Scalar>* pinocchio;               //!< Pinocchio data
    boost::shared_ptr<ActuationDataAbstract> actuation;  //!< Actuation data
    MatrixXs dtau_dx;
    using Base::r;
    using Base::Ru;
    using Base::Rx;
    using Base::shared;
  };
};

}  // namespace crocoddyl

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

#endif  // CROCODDYL_MULTIBODY_ACTIONS_FREE_INVDYN_HPP_


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2021-2022, Heriot-Watt University, University of Edinburgh
5			// Copyright note valid unless otherwise stated in individual files.
6			// All rights reserved.
7			///////////////////////////////////////////////////////////////////////////////
8
9			#ifndef CROCODDYL_MULTIBODY_ACTIONS_FREE_INVDYN_HPP_
10			#define CROCODDYL_MULTIBODY_ACTIONS_FREE_INVDYN_HPP_
11
12			#include <stdexcept>
13
14			#include "crocoddyl/core/actuation-base.hpp"
15			#include "crocoddyl/core/constraints/constraint-manager.hpp"
16			#include "crocoddyl/core/costs/cost-sum.hpp"
17			#include "crocoddyl/core/diff-action-base.hpp"
18			#include "crocoddyl/core/residual-base.hpp"
19			#include "crocoddyl/core/utils/exception.hpp"
20			#include "crocoddyl/multibody/data/multibody.hpp"
21			#include "crocoddyl/multibody/fwd.hpp"
22			#include "crocoddyl/multibody/states/multibody.hpp"
23
24			namespace crocoddyl {
25
26			/**
27			* @brief Differential action model for free inverse dynamics in multibody
28			* systems.
29			*
30			* This class implements forward kinematic with an inverse-dynamics computed
31			* using the Recursive Newton Euler Algorithm (RNEA). The stack of cost and
32			* constraint functions are implemented in `CostModelSumTpl` and
33			* `ConstraintModelManagerTpl`, respectively. The accelerations are the decision
34			* variables defined as the control inputs, and the under-actuation constraint
35			* is under the name `tau`, thus the user is not allowed to use it.
36			*
37			*
38			* \sa `DifferentialActionModelAbstractTpl`, `calc()`, `calcDiff()`,
39			* `createData()`
40			*/
41			template <typename _Scalar>
42			class DifferentialActionModelFreeInvDynamicsTpl
43			: public DifferentialActionModelAbstractTpl<_Scalar> {
44			public:
45			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
46
47			typedef _Scalar Scalar;
48			typedef MathBaseTpl<Scalar> MathBase;
49			typedef DifferentialActionModelAbstractTpl<Scalar> Base;
50			typedef DifferentialActionDataFreeInvDynamicsTpl<Scalar> Data;
51			typedef DifferentialActionDataAbstractTpl<Scalar>
52			DifferentialActionDataAbstract;
53			typedef CostModelSumTpl<Scalar> CostModelSum;
54			typedef ConstraintModelManagerTpl<Scalar> ConstraintModelManager;
55			typedef StateMultibodyTpl<Scalar> StateMultibody;
56			typedef ActuationModelAbstractTpl<Scalar> ActuationModelAbstract;
57			typedef ConstraintModelResidualTpl<Scalar> ConstraintModelResidual;
58			typedef typename MathBase::VectorXs VectorXs;
59
60			/**
61			* @brief Initialize the free inverse-dynamics action model
62			*
63			* It describes the kinematic evolution of the multibody system and computes
64			* the needed torques using inverse dynamics.
65			*
66			* @param[in] state State of the multibody system
67			* @param[in] actuation Actuation model
68			* @param[in] costs Cost model
69			*/
70			DifferentialActionModelFreeInvDynamicsTpl(
71			boost::shared_ptr<StateMultibody> state,
72			boost::shared_ptr<ActuationModelAbstract> actuation,
73			boost::shared_ptr<CostModelSum> costs);
74
75			/**
76			* @brief Initialize the free inverse-dynamics action model
77			*
78			* @param[in] state State of the multibody system
79			* @param[in] actuation Actuation model
80			* @param[in] costs Cost model
81			* @param[in] constraints Constraints model
82			*/
83			DifferentialActionModelFreeInvDynamicsTpl(
84			boost::shared_ptr<StateMultibody> state,
85			boost::shared_ptr<ActuationModelAbstract> actuation,
86			boost::shared_ptr<CostModelSum> costs,
87			boost::shared_ptr<ConstraintModelManager> constraints);
88			virtual ~DifferentialActionModelFreeInvDynamicsTpl();
89
90			/**
91			* @brief Compute the system acceleration, cost value and constraint residuals
92			*
93			* It extracts the acceleration value from control vector and also computes
94			* the cost and constraints.
95			*
96			* @param[in] data Free inverse-dynamics data
97			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
98			* @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
99			*/
100			virtual void calc(
101			const boost::shared_ptr<DifferentialActionDataAbstract>& data,
102			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u);
103
104			/**
105			* @brief @copydoc Base::calc(const
106			* boost::shared_ptr<DifferentialActionDataAbstract>& data, const
107			* Eigen::Ref<const VectorXs>& x)
108			*/
109			virtual void calc(
110			const boost::shared_ptr<DifferentialActionDataAbstract>& data,
111			const Eigen::Ref<const VectorXs>& x);
112
113			/**
114			* @brief Compute the derivatives of the dynamics, cost and constraint
115			* functions
116			*
117			* It computes the partial derivatives of the dynamical system and the cost
118			* and contraint functions. It assumes that `calc()` has been run first. This
119			* function builds a quadratic approximation of the time-continuous action
120			* model (i.e., dynamical system, cost and constraint functions).
121			*
122			* @param[in] data Free inverse-dynamics data
123			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
124			* @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
125			*/
126			virtual void calcDiff(
127			const boost::shared_ptr<DifferentialActionDataAbstract>& data,
128			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u);
129
130			/**
131			* @brief @copydoc Base::calcDiff(const
132			* boost::shared_ptr<DifferentialActionDataAbstract>& data, const
133			* Eigen::Ref<const VectorXs>& x)
134			*/
135			virtual void calcDiff(
136			const boost::shared_ptr<DifferentialActionDataAbstract>& data,
137			const Eigen::Ref<const VectorXs>& x);
138
139			/**
140			* @brief Create the free inverse-dynamics data
141			*
142			* @return free inverse-dynamics data
143			*/
144			virtual boost::shared_ptr<DifferentialActionDataAbstract> createData();
145
146			/**
147			* @brief Checks that a specific data belongs to the free inverse-dynamics
148			* model
149			*/
150			virtual bool checkData(
151			const boost::shared_ptr<DifferentialActionDataAbstract>& data);
152
153			/**
154			* @brief Computes the quasic static commands
155			*
156			* The quasic static commands are the ones produced for a reference posture as
157			* an equilibrium point with zero acceleration, i.e., for
158			* \f$\mathbf{f^q_x}\delta\mathbf{q}+\mathbf{f_u}\delta\mathbf{u}=\mathbf{0}\f$
159			*
160			* @param[in] data Free inverse-dynamics data
161			* @param[out] u Quasic-static commands
162			* @param[in] x State point (velocity has to be zero)
163			* @param[in] maxiter Maximum allowed number of iterations (default 100)
164			* @param[in] tol Tolerance (default 1e-9)
165			*/
166			virtual void quasiStatic(
167			const boost::shared_ptr<DifferentialActionDataAbstract>& data,
168			Eigen::Ref<VectorXs> u, const Eigen::Ref<const VectorXs>& x,
169			const std::size_t maxiter = 100, const Scalar tol = Scalar(1e-9));
170
171			/**
172			* @brief Return the number of inequality constraints
173			*/
174			virtual std::size_t get_ng() const;
175
176			/**
177			* @brief Return the number of equality constraints
178			*/
179			virtual std::size_t get_nh() const;
180
181			/**
182			* @brief Return the lower bound of the inequality constraints
183			*/
184			virtual const VectorXs& get_g_lb() const;
185
186			/**
187			* @brief Return the upper bound of the inequality constraints
188			*/
189			virtual const VectorXs& get_g_ub() const;
190
191			/**
192			* @brief Return the actuation model
193			*/
194			const boost::shared_ptr<ActuationModelAbstract>& get_actuation() const;
195
196			/**
197			* @brief Return the cost model
198			*/
199			const boost::shared_ptr<CostModelSum>& get_costs() const;
200
201			/**
202			* @brief Return the constraint model manager
203			*/
204			const boost::shared_ptr<ConstraintModelManager>& get_constraints() const;
205
206			/**
207			* @brief Return the Pinocchio model
208			*/
209			pinocchio::ModelTpl<Scalar>& get_pinocchio() const;
210
211			/**
212			* @brief Print relevant information of the free inverse-dynamics model
213			*
214			* @param[out] os Output stream object
215			*/
216			virtual void print(std::ostream& os) const;
217
218			protected:
219			using Base::g_lb_; //!< Lower bound of the inequality constraints
220			using Base::g_ub_; //!< Upper bound of the inequality constraints
221			using Base::ng_; //!< Number of inequality constraints
222			using Base::nh_; //!< Number of equality constraints
223			using Base::nu_; //!< Control dimension
224			using Base::state_; //!< Model of the state
225
226			private:
227			void init(const boost::shared_ptr<StateMultibody>& state);
228			boost::shared_ptr<ActuationModelAbstract> actuation_; //!< Actuation model
229			boost::shared_ptr<CostModelSum> costs_; //!< Cost model
230			boost::shared_ptr<ConstraintModelManager> constraints_; //!< Constraint model
231			pinocchio::ModelTpl<Scalar>& pinocchio_; //!< Pinocchio model
232
233			public:
234			/**
235			* @brief Actuation residual
236			*
237			* This residual function enforces the torques of under-actuated joints (e.g.,
238			* floating-base joints) to be zero. We compute these torques and their
239			* derivatives using RNEA inside `DifferentialActionModelFreeInvDynamicsTpl`.
240			*
241			* As described in `ResidualModelAbstractTpl`, the residual value and its
242			* Jacobians are calculated by `calc` and `calcDiff`, respectively.
243			*
244			* \sa `ResidualModelAbstractTpl`, `calc()`, `calcDiff()`, `createData()`
245			*/
246			class ResidualModelActuation : public ResidualModelAbstractTpl<_Scalar> {
247			public:
248			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
249
250			typedef _Scalar Scalar;
251			typedef MathBaseTpl<Scalar> MathBase;
252			typedef ResidualModelAbstractTpl<Scalar> Base;
253			typedef StateMultibodyTpl<Scalar> StateMultibody;
254			typedef ResidualDataAbstractTpl<Scalar> ResidualDataAbstract;
255			typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
256			typedef ActuationModelAbstractTpl<Scalar> ActuationModelAbstract;
257			typedef typename MathBase::VectorXs VectorXs;
258			typedef typename MathBase::MatrixXs MatrixXs;
259
260			/**
261			* @brief Initialize the actuation residual model
262			*
263			* @param[in] state State of the multibody system
264			* @param[in] nu Dimension of the joint torques
265			*/
266		51	ResidualModelActuation(boost::shared_ptr<StateMultibody> state,
267			const std::size_t nu)
268		51	: Base(state, state->get_nv() - nu, state->get_nv(), true, true, true),
269	✓✗	102	na_(nu) {}
270		102	virtual ~ResidualModelActuation() {}
271
272			/**
273			* @brief Compute the actuation residual
274			*
275			* @param[in] data Actuation residual data
276			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
277			* @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nv+nu}\f$
278			*/
279		2311	virtual void calc(const boost::shared_ptr<ResidualDataAbstract>& data,
280			const Eigen::Ref<const VectorXs>&,
281			const Eigen::Ref<const VectorXs>&) {
282			typename Data::ResidualDataActuation* d =
283		2311	static_cast<typename Data::ResidualDataActuation*>(data.get());
284			// Update the under-actuation set and residual
285		2311	std::size_t nrow = 0;
286		16177	for (std::size_t k = 0;
287	✓✓	16177	k < static_cast<std::size_t>(d->actuation->tau_set.size()); ++k) {
288	✓✓	13866	if (!d->actuation->tau_set[k]) {
289		4622	data->r(nrow) = d->pinocchio->tau(k);
290		4622	nrow += 1;
291			}
292			}
293		2311	}
294
295			/**
296			* @brief @copydoc Base::calc(const boost::shared_ptr<ResidualDataAbstract>&
297			* data, const Eigen::Ref<const VectorXs>& x)
298			*/
299		177	virtual void calc(const boost::shared_ptr<ResidualDataAbstract>& data,
300			const Eigen::Ref<const VectorXs>&) {
301		177	data->r.setZero();
302		177	}
303
304			/**
305			* @brief Compute the derivatives of the actuation residual
306			*
307			* @param[in] data Actuation residual data
308			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
309			* @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
310			*/
311		627	virtual void calcDiff(const boost::shared_ptr<ResidualDataAbstract>& data,
312			const Eigen::Ref<const VectorXs>&,
313			const Eigen::Ref<const VectorXs>&) {
314			typename Data::ResidualDataActuation* d =
315		627	static_cast<typename Data::ResidualDataActuation*>(data.get());
316		627	std::size_t nrow = 0;
317		627	const std::size_t nv = state_->get_nv();
318	✓✗	627	d->dtau_dx.leftCols(nv) = d->pinocchio->dtau_dq;
319	✓✗	627	d->dtau_dx.rightCols(nv) = d->pinocchio->dtau_dv;
320		627	d->dtau_dx -= d->actuation->dtau_dx;
321		4389	for (std::size_t k = 0;
322	✓✓	4389	k < static_cast<std::size_t>(d->actuation->tau_set.size()); ++k) {
323	✓✓	3762	if (!d->actuation->tau_set[k]) {
324	✓✗✓✗	1254	d->Rx.row(nrow) = d->dtau_dx.row(k);
325	✓✗✓✗	1254	d->Ru.row(nrow) = d->pinocchio->M.row(k);
326		1254	nrow += 1;
327			}
328			}
329		627	}
330
331			/**
332			* @brief @copydoc Base::calcDiff(const
333			* boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const
334			* VectorXs>& x)
335			*/
336		22	virtual void calcDiff(const boost::shared_ptr<ResidualDataAbstract>& data,
337			const Eigen::Ref<const VectorXs>&) {
338		22	data->Rx.setZero();
339		22	data->Ru.setZero();
340		22	}
341
342			/**
343			* @brief Create the actuation residual data
344			*
345			* @return Actuation residual data
346			*/
347		3230	virtual boost::shared_ptr<ResidualDataAbstract> createData(
348			DataCollectorAbstract* const data) {
349			return boost::allocate_shared<typename Data::ResidualDataActuation>(
350			Eigen::aligned_allocator<typename Data::ResidualDataActuation>(),
351	✓✗	3230	this, data);
352			}
353
354			/**
355			* @brief Print relevant information of the actuation residual model
356			*
357			* @param[out] os Output stream object
358			*/
359			virtual void print(std::ostream& os) const {
360			os << "ResidualModelActuation {nx=" << state_->get_nx()
361			<< ", ndx=" << state_->get_ndx() << ", nu=" << nu_ << ", na=" << na_
362			<< "}";
363			}
364
365			protected:
366			std::size_t na_; //!< Dimension of the joint torques
367			using Base::nu_;
368			using Base::state_;
369			};
370			};
371
372			template <typename _Scalar>
373			struct DifferentialActionDataFreeInvDynamicsTpl
374			: public DifferentialActionDataAbstractTpl<_Scalar> {
375			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
376			typedef _Scalar Scalar;
377			typedef MathBaseTpl<Scalar> MathBase;
378			typedef DifferentialActionDataAbstractTpl<Scalar> Base;
379			typedef JointDataAbstractTpl<Scalar> JointDataAbstract;
380			typedef DataCollectorJointActMultibodyTpl<Scalar>
381			DataCollectorJointActMultibody;
382			typedef CostDataSumTpl<Scalar> CostDataSum;
383			typedef ConstraintDataManagerTpl<Scalar> ConstraintDataManager;
384			typedef typename MathBase::VectorXs VectorXs;
385
386			template <template <typename Scalar> class Model>
387		9894	explicit DifferentialActionDataFreeInvDynamicsTpl(Model<Scalar>* const model)
388			: Base(model),
389		9894	pinocchio(pinocchio::DataTpl<Scalar>(model->get_pinocchio())),
390			multibody(
391		9894	&pinocchio, model->get_actuation()->createData(),
392			boost::make_shared<JointDataAbstract>(
393			model->get_state(), model->get_actuation(), model->get_nu())),
394		9894	costs(model->get_costs()->createData(&multibody)),
395		9894	constraints(model->get_constraints()->createData(&multibody)),
396	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	29682	tmp_xstatic(model->get_state()->get_nx()) {
397		9894	const std::size_t nv = model->get_state()->get_nv();
398	✓✗✓✗ ✓✗	9894	Fu.leftCols(nv).diagonal().setOnes();
399	✓✗✓✗ ✓✗	9894	multibody.joint->da_du.leftCols(nv).diagonal().setOnes();
400	✓✗	9894	costs->shareMemory(this);
401	✓✗	9894	constraints->shareMemory(this);
402	✓✗	9894	tmp_xstatic.setZero();
403		9894	}
404
405			pinocchio::DataTpl<Scalar> pinocchio; //!< Pinocchio data
406			DataCollectorJointActMultibody multibody; //!< Multibody data
407			boost::shared_ptr<CostDataSum> costs; //!< Costs data
408			boost::shared_ptr<ConstraintDataManager> constraints; //!< Constraints data
409			VectorXs
410			tmp_xstatic; //!< State point used for computing the quasi-static input
411			using Base::cost;
412			using Base::Fu;
413			using Base::Fx;
414			using Base::Lu;
415			using Base::Luu;
416			using Base::Lx;
417			using Base::Lxu;
418			using Base::Lxx;
419			using Base::r;
420			using Base::xout;
421
422			struct ResidualDataActuation : public ResidualDataAbstractTpl<_Scalar> {
423			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
424
425			typedef _Scalar Scalar;
426			typedef MathBaseTpl<Scalar> MathBase;
427			typedef ResidualDataAbstractTpl<Scalar> Base;
428			typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
429			typedef DataCollectorActMultibodyTpl<Scalar> DataCollectorActMultibody;
430			typedef ActuationDataAbstractTpl<Scalar> ActuationDataAbstract;
431			typedef typename MathBase::MatrixXs MatrixXs;
432
433			template <template <typename Scalar> class Model>
434		3230	ResidualDataActuation(Model<Scalar>* const model,
435			DataCollectorAbstract* const data)
436			: Base(model, data),
437	✓✗	3230	dtau_dx(model->get_state()->get_nv(), model->get_state()->get_ndx()) {
438	✓✗	3230	dtau_dx.setZero();
439			// Check that proper shared data has been passed
440			DataCollectorActMultibody* d =
441	✓✗	3230	dynamic_cast<DataCollectorActMultibody*>(shared);
442	✗✓	3230	if (d == NULL) {
443			throw_pretty(
444			"Invalid argument: the shared data should be derived from "
445			"DataCollectorActMultibody");
446			}
447
448			// Avoids data casting at runtime
449		3230	pinocchio = d->pinocchio;
450		3230	actuation = d->actuation;
451		3230	}
452
453			pinocchio::DataTpl<Scalar>* pinocchio; //!< Pinocchio data
454			boost::shared_ptr<ActuationDataAbstract> actuation; //!< Actuation data
455			MatrixXs dtau_dx;
456			using Base::r;
457			using Base::Ru;
458			using Base::Rx;
459			using Base::shared;
460			};
461			};
462
463			} // namespace crocoddyl
464
465			/* --- Details -------------------------------------------------------------- */
466			/* --- Details -------------------------------------------------------------- */
467			/* --- Details -------------------------------------------------------------- */
468			#include <crocoddyl/multibody/actions/free-invdyn.hxx>
469
470			#endif // CROCODDYL_MULTIBODY_ACTIONS_FREE_INVDYN_HPP_