crocoddyl  1.8.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
full.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2021, LAAS-CNRS, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #ifndef CROCODDYL_MULTIBODY_ACTUATIONS_FULL_HPP_
10 #define CROCODDYL_MULTIBODY_ACTUATIONS_FULL_HPP_
11 
12 #include "crocoddyl/multibody/fwd.hpp"
13 #include "crocoddyl/core/actuation-base.hpp"
14 #include "crocoddyl/core/state-base.hpp"
15 
16 namespace crocoddyl {
17 
27 template <typename _Scalar>
29  public:
30  typedef _Scalar Scalar;
35  typedef typename MathBase::VectorXs VectorXs;
36  typedef typename MathBase::MatrixXs MatrixXs;
37 
43  explicit ActuationModelFullTpl(boost::shared_ptr<StateAbstract> state) : Base(state, state->get_nv()){};
44  virtual ~ActuationModelFullTpl(){};
45 
53  virtual void calc(const boost::shared_ptr<Data>& data, const Eigen::Ref<const VectorXs>& /*x*/,
54  const Eigen::Ref<const VectorXs>& u) {
55  if (static_cast<std::size_t>(u.size()) != nu_) {
56  throw_pretty("Invalid argument: "
57  << "u has wrong dimension (it should be " + std::to_string(nu_) + ")");
58  }
59  data->tau = u;
60  };
61 
69 #ifndef NDEBUG
70  virtual void calcDiff(const boost::shared_ptr<Data>& data, const Eigen::Ref<const VectorXs>& /*x*/,
71  const Eigen::Ref<const VectorXs>& /*u*/) {
72 #else
73  virtual void calcDiff(const boost::shared_ptr<Data>&, const Eigen::Ref<const VectorXs>& /*x*/,
74  const Eigen::Ref<const VectorXs>& /*u*/) {
75 #endif
76  // The derivatives has constant values which were set in createData.
77  assert_pretty(data->dtau_dx == MatrixXs::Zero(state_->get_nv(), state_->get_ndx()), "dtau_dx has wrong value");
78  assert_pretty(data->dtau_du == MatrixXs::Identity(state_->get_nv(), nu_), "dtau_du has wrong value");
79  };
80 
87  virtual boost::shared_ptr<Data> createData() {
88  boost::shared_ptr<Data> data = boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
89  data->dtau_du.diagonal().fill((Scalar)1);
90  return data;
91  };
92 
93  protected:
94  using Base::nu_;
95  using Base::state_;
96 };
97 
98 } // namespace crocoddyl
99 
100 #endif // CROCODDYL_MULTIBODY_ACTUATIONS_FULL_HPP_
ActuationModelFullTpl(boost::shared_ptr< StateAbstract > state)
Initialize the full actuation model.
Definition: full.hpp:43
Abstract class for the state representation.
Definition: fwd.hpp:112
virtual boost::shared_ptr< Data > createData()
Create the full actuation data.
Definition: full.hpp:87
Full actuation model.
Definition: full.hpp:28
virtual void calcDiff(const boost::shared_ptr< Data > &data, const Eigen::Ref< const VectorXs > &, const Eigen::Ref< const VectorXs > &)
Compute the Jacobians of the full actuation model.
Definition: full.hpp:70
virtual void calc(const boost::shared_ptr< Data > &data, const Eigen::Ref< const VectorXs > &, const Eigen::Ref< const VectorXs > &u)
Compute the full actuation.
Definition: full.hpp:53