Crocoddyl
full.hpp
1 // 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.
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 
29 template <typename _Scalar>
31  public:
32  typedef _Scalar Scalar;
37  typedef typename MathBase::VectorXs VectorXs;
38  typedef typename MathBase::MatrixXs MatrixXs;
39 
45  explicit ActuationModelFullTpl(boost::shared_ptr<StateAbstract> state)
46  : Base(state, state->get_nv()) {};
47  virtual ~ActuationModelFullTpl() {};
48 
56  virtual void calc(const boost::shared_ptr<Data>& data,
57  const Eigen::Ref<const VectorXs>& /*x*/,
58  const Eigen::Ref<const VectorXs>& u) {
59  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  data->tau = u;
65  };
66 
74 #ifndef NDEBUG
75  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  assert_pretty(data->dtau_dx.isZero(), "dtau_dx has wrong value");
85  assert_pretty(MatrixXs(data->dtau_du)
86  .isApprox(MatrixXs::Identity(state_->get_nv(), nu_)),
87  "dtau_du has wrong value");
88  };
89 
90  virtual void commands(const boost::shared_ptr<Data>& data,
91  const Eigen::Ref<const VectorXs>&,
92  const Eigen::Ref<const VectorXs>& tau) {
93  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  data->u = tau;
99  }
100 
101 #ifndef NDEBUG
102  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  assert_pretty(MatrixXs(data->Mtau).isApprox(MatrixXs::Identity(nu_, nu_)),
112  "Mtau has wrong value");
113  }
114 
121  virtual boost::shared_ptr<Data> createData() {
122  boost::shared_ptr<Data> data =
123  boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
124  data->dtau_du.diagonal().setOnes();
125  data->Mtau.setIdentity();
126  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_
Abstract class for the actuation-mapping model.
boost::shared_ptr< StateAbstract > state_
Model of the state.
std::size_t nu_
Dimension of joint torque inputs.
Full actuation model.
Definition: full.hpp:30
ActuationModelFullTpl(boost::shared_ptr< StateAbstract > state)
Initialize the full actuation model.
Definition: full.hpp:45
boost::shared_ptr< StateAbstract > state_
Model of the state.
std::size_t nu_
Dimension of joint torque inputs.
virtual void torqueTransform(const boost::shared_ptr< Data > &data, const Eigen::Ref< const VectorXs > &, const Eigen::Ref< const VectorXs > &)
Compute the torque transform from generalized torques to joint torque inputs.
Definition: full.hpp:102
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:56
virtual void commands(const boost::shared_ptr< Data > &data, const Eigen::Ref< const VectorXs > &, const Eigen::Ref< const VectorXs > &tau)
Compute the joint torque input from the generalized torques.
Definition: full.hpp:90
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:75
virtual boost::shared_ptr< Data > createData()
Create the full actuation data.
Definition: full.hpp:121
Abstract class for the state representation.
Definition: state-base.hpp:46