Crocoddyl
 
Loading...
Searching...
No Matches
floating-base.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2019-2025, 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/multibody/fwd.hpp"
15#include "crocoddyl/multibody/states/multibody.hpp"
16
17namespace crocoddyl {
18
34template <typename _Scalar>
36 : public ActuationModelAbstractTpl<_Scalar> {
37 public:
38 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
40
41 typedef _Scalar Scalar;
46 typedef typename MathBase::VectorXs VectorXs;
47 typedef typename MathBase::MatrixXs MatrixXs;
48
55 explicit ActuationModelFloatingBaseTpl(std::shared_ptr<StateMultibody> state)
56 : Base(state,
57 state->get_nv() -
58 state->get_pinocchio()
59 ->joints[(
60 state->get_pinocchio()->existJointName("root_joint")
61 ? state->get_pinocchio()->getJointId("root_joint")
62 : 0)]
63 .nv()) {};
64 virtual ~ActuationModelFloatingBaseTpl() = default;
65
74 virtual void calc(const std::shared_ptr<Data>& data,
75 const Eigen::Ref<const VectorXs>& /*x*/,
76 const Eigen::Ref<const VectorXs>& u) override {
77 if (static_cast<std::size_t>(u.size()) != nu_) {
78 throw_pretty(
79 "Invalid argument: " << "u has wrong dimension (it should be " +
80 std::to_string(nu_) + ")");
81 }
82 data->tau.tail(nu_) = u;
83 };
84
92#ifndef NDEBUG
93 virtual void calcDiff(const std::shared_ptr<Data>& data,
94 const Eigen::Ref<const VectorXs>& /*x*/,
95 const Eigen::Ref<const VectorXs>& /*u*/) override {
96#else
97 virtual void calcDiff(const std::shared_ptr<Data>&,
98 const Eigen::Ref<const VectorXs>& /*x*/,
99 const Eigen::Ref<const VectorXs>& /*u*/) override {
100#endif
101 // The derivatives has constant values which were set in createData.
102 assert_pretty(data->dtau_dx.isZero(), "dtau_dx has wrong value");
103 assert_pretty(MatrixXs(data->dtau_du).isApprox(dtau_du_),
104 "dtau_du has wrong value");
105 };
106
107 virtual void commands(const std::shared_ptr<Data>& data,
108 const Eigen::Ref<const VectorXs>&,
109 const Eigen::Ref<const VectorXs>& tau) override {
110 if (static_cast<std::size_t>(tau.size()) != state_->get_nv()) {
111 throw_pretty(
112 "Invalid argument: " << "tau has wrong dimension (it should be " +
113 std::to_string(state_->get_nv()) + ")");
114 }
115 data->u = tau.tail(nu_);
116 }
117
118#ifndef NDEBUG
119 virtual void torqueTransform(const std::shared_ptr<Data>& data,
120 const Eigen::Ref<const VectorXs>&,
121 const Eigen::Ref<const VectorXs>&) override {
122#else
123 virtual void torqueTransform(const std::shared_ptr<Data>&,
124 const Eigen::Ref<const VectorXs>&,
125 const Eigen::Ref<const VectorXs>&) override {
126#endif
127 // The torque transform has constant values which were set in createData.
128 assert_pretty(MatrixXs(data->Mtau).isApprox(Mtau_), "Mtau has wrong value");
129 }
130
136 virtual std::shared_ptr<Data> createData() override {
138 std::shared_ptr<StateMultibody> state =
139 std::static_pointer_cast<StateMultibody>(state_);
140 std::shared_ptr<Data> data =
141 std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
142 const std::size_t root_joint_id =
143 state->get_pinocchio()->existJointName("root_joint")
144 ? state->get_pinocchio()->getJointId("root_joint")
145 : 0;
146 const std::size_t nfb = state->get_pinocchio()->joints[root_joint_id].nv();
147 data->dtau_du.diagonal(-nfb).setOnes();
148 data->Mtau.diagonal(nfb).setOnes();
149 for (std::size_t i = 0; i < nfb; ++i) {
150 data->tau_set[i] = false;
151 }
152#ifndef NDEBUG
153 dtau_du_ = data->dtau_du;
154 Mtau_ = data->Mtau;
155#endif
156 return data;
157 };
158
159 template <typename NewScalar>
162 typedef StateMultibodyTpl<NewScalar> StateType;
163 ReturnType ret(std::static_pointer_cast<StateType>(
164 state_->template cast<NewScalar>()));
165 return ret;
166 }
167
173 virtual void print(std::ostream& os) const override {
174 os << "ActuationModelFloatingBase {nu=" << nu_
175 << ", nv=" << state_->get_nv() << "}";
176 }
177
178 protected:
179 using Base::nu_;
180 using Base::state_;
181
182#ifndef NDEBUG
183 private:
184 MatrixXs dtau_du_;
185 MatrixXs Mtau_;
186#endif
187};
188
189} // namespace crocoddyl
190
191CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(
193
194#endif // CROCODDYL_MULTIBODY_ACTUATIONS_FLOATING_BASE_HPP_
Abstract class for the actuation-mapping model.
std::shared_ptr< StateAbstract > state_
Model of the state.
std::size_t nu_
Dimension of joint torque inputs.
Floating-base actuation model.
std::shared_ptr< StateAbstract > state_
Model of the state.
virtual void calc(const std::shared_ptr< Data > &data, const Eigen::Ref< const VectorXs > &, const Eigen::Ref< const VectorXs > &u) override
Compute the floating-base actuation signal from the joint-torque input .
virtual std::shared_ptr< Data > createData() override
Create the floating-base actuation data.
virtual void commands(const std::shared_ptr< Data > &data, const Eigen::Ref< const VectorXs > &, const Eigen::Ref< const VectorXs > &tau) override
Compute the joint torque input from the generalized torques.
virtual void torqueTransform(const std::shared_ptr< Data > &data, const Eigen::Ref< const VectorXs > &, const Eigen::Ref< const VectorXs > &) override
Compute the torque transform from generalized torques to joint torque inputs.
ActuationModelFloatingBaseTpl(std::shared_ptr< StateMultibody > state)
Initialize the floating-base actuation model.
std::size_t nu_
Dimension of joint torque inputs.
virtual void calcDiff(const std::shared_ptr< Data > &data, const Eigen::Ref< const VectorXs > &, const Eigen::Ref< const VectorXs > &) override
Compute the Jacobians of the floating-base actuation function.
virtual void print(std::ostream &os) const override
Print relevant information of the joint-effort residual.
State multibody representation.
Definition multibody.hpp:34