crocoddyl  1.5.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
full.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2018-2019, LAAS-CNRS
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/multibody/states/multibody.hpp"
15 
16 namespace crocoddyl {
17 
18 template <typename _Scalar>
20  public:
21  typedef _Scalar Scalar;
26  typedef typename MathBase::VectorXs VectorXs;
27  typedef typename MathBase::MatrixXs MatrixXs;
28  explicit ActuationModelFullTpl(boost::shared_ptr<StateMultibody> state) : Base(state, state->get_nv()) {
29  pinocchio::JointModelFreeFlyerTpl<Scalar> ff_joint;
30  if (state->get_pinocchio()->joints[1].shortname() == ff_joint.shortname()) {
31  throw_pretty("Invalid argument: "
32  << "the first joint cannot be free-flyer");
33  }
34  };
35 
36  virtual ~ActuationModelFullTpl(){};
37 
38  virtual void calc(const boost::shared_ptr<Data>& data, const Eigen::Ref<const VectorXs>& /*x*/,
39  const Eigen::Ref<const VectorXs>& u) {
40  if (static_cast<std::size_t>(u.size()) != nu_) {
41  throw_pretty("Invalid argument: "
42  << "u has wrong dimension (it should be " + std::to_string(nu_) + ")");
43  }
44  data->tau = u;
45  };
46 
47 #ifndef NDEBUG
48  virtual void calcDiff(const boost::shared_ptr<Data>& data, const Eigen::Ref<const VectorXs>& /*x*/,
49  const Eigen::Ref<const VectorXs>& /*u*/) {
50 #else
51  virtual void calcDiff(const boost::shared_ptr<Data>&, const Eigen::Ref<const VectorXs>& /*x*/,
52  const Eigen::Ref<const VectorXs>& /*u*/) {
53 #endif
54  // The derivatives has constant values which were set in createData.
55  assert_pretty(data->dtau_dx == MatrixXs::Zero(state_->get_nv(), state_->get_ndx()), "dtau_dx has wrong value");
56  assert_pretty(data->dtau_du == MatrixXs::Identity(state_->get_nv(), nu_), "dtau_du has wrong value");
57  };
58 
59  virtual boost::shared_ptr<Data> createData() {
60  boost::shared_ptr<Data> data = boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
61  data->dtau_du.diagonal().fill((Scalar)1);
62  return data;
63  };
64 
65  protected:
66  using Base::nu_;
67  using Base::state_;
68 };
69 
70 } // namespace crocoddyl
71 
72 #endif // CROCODDYL_MULTIBODY_ACTUATIONS_FULL_HPP_