Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/crocoddyl/multibody/actuations/full.hpp	Lines:	23	25	92.0 %
Date:	2024-02-13 11:12:33	Branches:	15	86	17.4 %


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

#ifndef CROCODDYL_MULTIBODY_ACTUATIONS_FULL_HPP_
#define CROCODDYL_MULTIBODY_ACTUATIONS_FULL_HPP_

#include "crocoddyl/core/actuation-base.hpp"
#include "crocoddyl/core/state-base.hpp"
#include "crocoddyl/multibody/fwd.hpp"

namespace crocoddyl {

/**
 * @brief Full actuation model
 *
 * This actuation model applies input controls for all the `nv` dimensions of
 * the system.
 *
 * Both actuation and Jacobians are computed analytically by `calc` and
 * `calcDiff`, respectively.
 *
 * \sa `ActuationModelAbstractTpl`, `calc()`, `calcDiff()`, `createData()`
 */
template <typename _Scalar>
class ActuationModelFullTpl : public ActuationModelAbstractTpl<_Scalar> {
 public:
  typedef _Scalar Scalar;
  typedef MathBaseTpl<Scalar> MathBase;
  typedef ActuationModelAbstractTpl<Scalar> Base;
  typedef ActuationDataAbstractTpl<Scalar> Data;
  typedef StateAbstractTpl<Scalar> StateAbstract;
  typedef typename MathBase::VectorXs VectorXs;
  typedef typename MathBase::MatrixXs MatrixXs;

  /**
   * @brief Initialize the full actuation model
   *
   * @param[in] state  State of the dynamical system
   */
  explicit ActuationModelFullTpl(boost::shared_ptr<StateAbstract> state)
      : Base(state, state->get_nv()){};
  virtual ~ActuationModelFullTpl(){};

  /**
   * @brief Compute the full actuation
   *
   * @param[in] data  Full actuation data
   * @param[in] x     State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
   * @param[in] u     Joint torque input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
   */
  virtual void calc(const boost::shared_ptr<Data>& data,
                    const Eigen::Ref<const VectorXs>& /*x*/,
                    const Eigen::Ref<const VectorXs>& u) {
    if (static_cast<std::size_t>(u.size()) != nu_) {
      throw_pretty("Invalid argument: "
                   << "u has wrong dimension (it should be " +
                          std::to_string(nu_) + ")");
    }
    data->tau = u;
  };

  /**
   * @brief Compute the Jacobians of the full actuation model
   *
   * @param[in] data  Full actuation data
   * @param[in] x     State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
   * @param[in] u     Joint torque input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
   */
#ifndef NDEBUG
  virtual void calcDiff(const boost::shared_ptr<Data>& data,
                        const Eigen::Ref<const VectorXs>& /*x*/,
                        const Eigen::Ref<const VectorXs>&) {
#else
  virtual void calcDiff(const boost::shared_ptr<Data>&,
                        const Eigen::Ref<const VectorXs>& /*x*/,
                        const Eigen::Ref<const VectorXs>&) {
#endif
    // The derivatives has constant values which were set in createData.
    assert_pretty(data->dtau_dx.isZero(), "dtau_dx has wrong value");
    assert_pretty(MatrixXs(data->dtau_du)
                      .isApprox(MatrixXs::Identity(state_->get_nv(), nu_)),
                  "dtau_du has wrong value");
  };

  virtual void commands(const boost::shared_ptr<Data>& data,
                        const Eigen::Ref<const VectorXs>&,
                        const Eigen::Ref<const VectorXs>& tau) {
    if (static_cast<std::size_t>(tau.size()) != nu_) {
      throw_pretty("Invalid argument: "
                   << "tau has wrong dimension (it should be " +
                          std::to_string(nu_) + ")");
    }
    data->u = tau;
  }

#ifndef NDEBUG
  virtual void torqueTransform(const boost::shared_ptr<Data>& data,
                               const Eigen::Ref<const VectorXs>&,
                               const Eigen::Ref<const VectorXs>&) {
#else
  virtual void torqueTransform(const boost::shared_ptr<Data>&,
                               const Eigen::Ref<const VectorXs>&,
                               const Eigen::Ref<const VectorXs>&) {
#endif
    // The torque transform has constant values which were set in createData.
    assert_pretty(MatrixXs(data->Mtau).isApprox(MatrixXs::Identity(nu_, nu_)),
                  "Mtau has wrong value");
  }

  /**
   * @brief Create the full actuation data
   *
   * @param[in] data  shared data (it should be of type DataCollectorContactTpl)
   * @return the cost data.
   */
  virtual boost::shared_ptr<Data> createData() {
    boost::shared_ptr<Data> data =
        boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
    data->dtau_du.diagonal().setOnes();
    data->Mtau.setIdentity();
    return data;
  };

 protected:
  using Base::nu_;
  using Base::state_;
};

}  // namespace crocoddyl

#endif  // CROCODDYL_MULTIBODY_ACTUATIONS_FULL_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			// Copyright note valid unless otherwise stated in individual files.
6			// All rights reserved.
7			///////////////////////////////////////////////////////////////////////////////
8
9			#ifndef CROCODDYL_MULTIBODY_ACTUATIONS_FULL_HPP_
10			#define CROCODDYL_MULTIBODY_ACTUATIONS_FULL_HPP_
11
12			#include "crocoddyl/core/actuation-base.hpp"
13			#include "crocoddyl/core/state-base.hpp"
14			#include "crocoddyl/multibody/fwd.hpp"
15
16			namespace crocoddyl {
17
18			/**
19			* @brief Full actuation model
20			*
21			* This actuation model applies input controls for all the `nv` dimensions of
22			* the system.
23			*
24			* Both actuation and Jacobians are computed analytically by `calc` and
25			* `calcDiff`, respectively.
26			*
27			* \sa `ActuationModelAbstractTpl`, `calc()`, `calcDiff()`, `createData()`
28			*/
29			template <typename _Scalar>
30			class ActuationModelFullTpl : public ActuationModelAbstractTpl<_Scalar> {
31			public:
32			typedef _Scalar Scalar;
33			typedef MathBaseTpl<Scalar> MathBase;
34			typedef ActuationModelAbstractTpl<Scalar> Base;
35			typedef ActuationDataAbstractTpl<Scalar> Data;
36			typedef StateAbstractTpl<Scalar> StateAbstract;
37			typedef typename MathBase::VectorXs VectorXs;
38			typedef typename MathBase::MatrixXs MatrixXs;
39
40			/**
41			* @brief Initialize the full actuation model
42			*
43			* @param[in] state State of the dynamical system
44			*/
45		875	explicit ActuationModelFullTpl(boost::shared_ptr<StateAbstract> state)
46	✓✗	875	: Base(state, state->get_nv()){};
47		1754	virtual ~ActuationModelFullTpl(){};
48
49			/**
50			* @brief Compute the full actuation
51			*
52			* @param[in] data Full actuation data
53			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
54			* @param[in] u Joint torque input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
55			*/
56		40428	virtual void calc(const boost::shared_ptr<Data>& data,
57			const Eigen::Ref<const VectorXs>& /x/,
58			const Eigen::Ref<const VectorXs>& u) {
59	✗✓	40428	if (static_cast<std::size_t>(u.size()) != nu_) {
60			throw_pretty("Invalid argument: "
61			<< "u has wrong dimension (it should be " +
62			std::to_string(nu_) + ")");
63			}
64		40428	data->tau = u;
65		40428	};
66
67			/**
68			* @brief Compute the Jacobians of the full actuation model
69			*
70			* @param[in] data Full actuation data
71			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
72			* @param[in] u Joint torque input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
73			*/
74			#ifndef NDEBUG
75		11617	virtual void calcDiff(const boost::shared_ptr<Data>& data,
76			const Eigen::Ref<const VectorXs>& /x/,
77			const Eigen::Ref<const VectorXs>&) {
78			#else
79			virtual void calcDiff(const boost::shared_ptr<Data>&,
80			const Eigen::Ref<const VectorXs>& /x/,
81			const Eigen::Ref<const VectorXs>&) {
82			#endif
83			// The derivatives has constant values which were set in createData.
84	✓✗✗✓ ✗✗✗✗ ✗✗✗✗	11617	assert_pretty(data->dtau_dx.isZero(), "dtau_dx has wrong value");
85	✓✗✓✗ ✗✓✗✗ ✗✗✗✗ ✗✗	11617	assert_pretty(MatrixXs(data->dtau_du)
86			.isApprox(MatrixXs::Identity(state_->get_nv(), nu_)),
87			"dtau_du has wrong value");
88		11617	};
89
90		4994	virtual void commands(const boost::shared_ptr<Data>& data,
91			const Eigen::Ref<const VectorXs>&,
92			const Eigen::Ref<const VectorXs>& tau) {
93	✗✓	4994	if (static_cast<std::size_t>(tau.size()) != nu_) {
94			throw_pretty("Invalid argument: "
95			<< "tau has wrong dimension (it should be " +
96			std::to_string(nu_) + ")");
97			}
98		4994	data->u = tau;
99		4994	}
100
101			#ifndef NDEBUG
102		1268	virtual void torqueTransform(const boost::shared_ptr<Data>& data,
103			const Eigen::Ref<const VectorXs>&,
104			const Eigen::Ref<const VectorXs>&) {
105			#else
106			virtual void torqueTransform(const boost::shared_ptr<Data>&,
107			const Eigen::Ref<const VectorXs>&,
108			const Eigen::Ref<const VectorXs>&) {
109			#endif
110			// The torque transform has constant values which were set in createData.
111	✓✗✓✗ ✗✓✗✗ ✗✗✗✗ ✗✗	1268	assert_pretty(MatrixXs(data->Mtau).isApprox(MatrixXs::Identity(nu_, nu_)),
112			"Mtau has wrong value");
113		1268	}
114
115			/**
116			* @brief Create the full actuation data
117			*
118			* @param[in] data shared data (it should be of type DataCollectorContactTpl)
119			* @return the cost data.
120			*/
121		34226	virtual boost::shared_ptr<Data> createData() {
122	✓✗	34226	boost::shared_ptr<Data> data =
123			boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
124	✓✗✓✗	34226	data->dtau_du.diagonal().setOnes();
125	✓✗	34226	data->Mtau.setIdentity();
126		34226	return data;
127			};
128
129			protected:
130			using Base::nu_;
131			using Base::state_;
132			};
133
134			} // namespace crocoddyl
135
136			#endif // CROCODDYL_MULTIBODY_ACTUATIONS_FULL_HPP_