Crocoddyl
floating-base.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2022, LAAS-CNRS, University of Edinburgh,
5 // Heriot-Watt University
6 // Copyright note valid unless otherwise stated in individual files.
7 // All rights reserved.
9 
10 #ifndef CROCODDYL_MULTIBODY_ACTUATIONS_FLOATING_BASE_HPP_
11 #define CROCODDYL_MULTIBODY_ACTUATIONS_FLOATING_BASE_HPP_
12 
13 #include "crocoddyl/core/actuation-base.hpp"
14 #include "crocoddyl/core/utils/exception.hpp"
15 #include "crocoddyl/multibody/fwd.hpp"
16 #include "crocoddyl/multibody/states/multibody.hpp"
17 
18 namespace crocoddyl {
19 
35 template <typename _Scalar>
37  : public ActuationModelAbstractTpl<_Scalar> {
38  public:
39  typedef _Scalar Scalar;
44  typedef typename MathBase::VectorXs VectorXs;
45  typedef typename MathBase::MatrixXs MatrixXs;
46 
54  boost::shared_ptr<StateMultibody> state)
55  : Base(state,
56  state->get_nv() -
57  state->get_pinocchio()
58  ->joints[(
59  state->get_pinocchio()->existJointName("root_joint")
60  ? state->get_pinocchio()->getJointId("root_joint")
61  : 0)]
62  .nv()){};
63  virtual ~ActuationModelFloatingBaseTpl(){};
64 
73  virtual void calc(const boost::shared_ptr<Data>& data,
74  const Eigen::Ref<const VectorXs>& /*x*/,
75  const Eigen::Ref<const VectorXs>& u) {
76  if (static_cast<std::size_t>(u.size()) != nu_) {
77  throw_pretty("Invalid argument: "
78  << "u has wrong dimension (it should be " +
79  std::to_string(nu_) + ")");
80  }
81  data->tau.tail(nu_) = u;
82  };
83 
91 #ifndef NDEBUG
92  virtual void calcDiff(const boost::shared_ptr<Data>& data,
93  const Eigen::Ref<const VectorXs>& /*x*/,
94  const Eigen::Ref<const VectorXs>& /*u*/) {
95 #else
96  virtual void calcDiff(const boost::shared_ptr<Data>&,
97  const Eigen::Ref<const VectorXs>& /*x*/,
98  const Eigen::Ref<const VectorXs>& /*u*/) {
99 #endif
100  // The derivatives has constant values which were set in createData.
101  assert_pretty(data->dtau_dx.isZero(), "dtau_dx has wrong value");
102  assert_pretty(MatrixXs(data->dtau_du).isApprox(dtau_du_),
103  "dtau_du has wrong value");
104  };
105 
106  virtual void commands(const boost::shared_ptr<Data>& data,
107  const Eigen::Ref<const VectorXs>&,
108  const Eigen::Ref<const VectorXs>& tau) {
109  if (static_cast<std::size_t>(tau.size()) != state_->get_nv()) {
110  throw_pretty("Invalid argument: "
111  << "tau has wrong dimension (it should be " +
112  std::to_string(state_->get_nv()) + ")");
113  }
114  data->u = tau.tail(nu_);
115  }
116 
117 #ifndef NDEBUG
118  virtual void torqueTransform(const boost::shared_ptr<Data>& data,
119  const Eigen::Ref<const VectorXs>&,
120  const Eigen::Ref<const VectorXs>&) {
121 #else
122  virtual void torqueTransform(const boost::shared_ptr<Data>&,
123  const Eigen::Ref<const VectorXs>&,
124  const Eigen::Ref<const VectorXs>&) {
125 #endif
126  // The torque transform has constant values which were set in createData.
127  assert_pretty(MatrixXs(data->Mtau).isApprox(Mtau_), "Mtau has wrong value");
128  }
129 
135  virtual boost::shared_ptr<Data> createData() {
137  boost::shared_ptr<StateMultibody> state =
138  boost::static_pointer_cast<StateMultibody>(state_);
139  boost::shared_ptr<Data> data =
140  boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
141  const std::size_t root_joint_id =
142  state->get_pinocchio()->existJointName("root_joint")
143  ? state->get_pinocchio()->getJointId("root_joint")
144  : 0;
145  const std::size_t nfb = state->get_pinocchio()->joints[root_joint_id].nv();
146  data->dtau_du.diagonal(-nfb).setOnes();
147  data->Mtau.diagonal(nfb).setOnes();
148  for (std::size_t i = 0; i < nfb; ++i) {
149  data->tau_set[i] = false;
150  }
151 #ifndef NDEBUG
152  dtau_du_ = data->dtau_du;
153  Mtau_ = data->Mtau;
154 #endif
155  return data;
156  };
157 
158  protected:
159  using Base::nu_;
160  using Base::state_;
161 
162 #ifndef NDEBUG
163  private:
164  MatrixXs dtau_du_;
165  MatrixXs Mtau_;
166 #endif
167 };
168 
169 } // namespace crocoddyl
170 
171 #endif // CROCODDYL_MULTIBODY_ACTUATIONS_FLOATING_BASE_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.
Floating-base actuation model.
boost::shared_ptr< StateAbstract > state_
Model of the state.
std::size_t nu_
Dimension of joint torque inputs.
ActuationModelFloatingBaseTpl(boost::shared_ptr< StateMultibody > state)
Initialize the floating-base actuation model.
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.
virtual void calc(const boost::shared_ptr< Data > &data, const Eigen::Ref< const VectorXs > &, const Eigen::Ref< const VectorXs > &u)
Compute the floating-base actuation signal from the joint-torque input .
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.
virtual void calcDiff(const boost::shared_ptr< Data > &data, const Eigen::Ref< const VectorXs > &, const Eigen::Ref< const VectorXs > &)
Compute the Jacobians of the floating-base actuation function.
virtual boost::shared_ptr< Data > createData()
Create the floating-base actuation data.
State multibody representation.
Definition: multibody.hpp:35