crocoddyl 1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
 
Loading...
Searching...
No Matches
floating-base.hpp
1
2// 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_FLOATING_BASE_HPP_
10#define CROCODDYL_MULTIBODY_ACTUATIONS_FLOATING_BASE_HPP_
11
12#include "crocoddyl/multibody/fwd.hpp"
13#include "crocoddyl/core/utils/exception.hpp"
14#include "crocoddyl/core/actuation-base.hpp"
15#include "crocoddyl/multibody/states/multibody.hpp"
16
17namespace crocoddyl {
18
31template <typename _Scalar>
33 public:
34 typedef _Scalar Scalar;
39 typedef typename MathBase::VectorXs VectorXs;
40 typedef typename MathBase::MatrixXs MatrixXs;
41
48 explicit ActuationModelFloatingBaseTpl(boost::shared_ptr<StateMultibody> state)
49 : Base(state, state->get_nv() - state->get_pinocchio()->joints[1].nv()){};
51
59 virtual void calc(const boost::shared_ptr<Data>& data, const Eigen::Ref<const VectorXs>& /*x*/,
60 const Eigen::Ref<const VectorXs>& u) {
61 if (static_cast<std::size_t>(u.size()) != nu_) {
62 throw_pretty("Invalid argument: "
63 << "u has wrong dimension (it should be " + std::to_string(nu_) + ")");
64 }
65 data->tau.tail(nu_) = u;
66 };
67
75#ifndef NDEBUG
76 virtual void calcDiff(const boost::shared_ptr<Data>& data, const Eigen::Ref<const VectorXs>& /*x*/,
77 const Eigen::Ref<const VectorXs>& /*u*/) {
78#else
79 virtual void calcDiff(const boost::shared_ptr<Data>&, const Eigen::Ref<const VectorXs>& /*x*/,
80 const Eigen::Ref<const VectorXs>& /*u*/) {
81#endif
82 // The derivatives has constant values which were set in createData.
83 assert_pretty(data->dtau_dx.isZero(), "dtau_dx has wrong value");
84 assert_pretty(MatrixXs(data->dtau_du).isApprox(dtau_du_), "dtau_du has wrong value");
85 };
86
92 virtual boost::shared_ptr<Data> createData() {
94 boost::shared_ptr<StateMultibody> state = boost::static_pointer_cast<StateMultibody>(state_);
95 boost::shared_ptr<Data> data = boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
96 data->dtau_du.diagonal(-state->get_pinocchio()->joints[1].nv()).setOnes();
97#ifndef NDEBUG
98 dtau_du_ = data->dtau_du;
99#endif
100 return data;
101 };
102
103 protected:
104 using Base::nu_;
105 using Base::state_;
106
107#ifndef NDEBUG
108 private:
109 MatrixXs dtau_du_;
110#endif
111};
112
113} // namespace crocoddyl
114
115#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_
Control dimension.
Floating-base actuation model.
virtual boost::shared_ptr< Data > createData()
Create the floating-base actuation data.
boost::shared_ptr< StateAbstract > state_
Model of the state.
std::size_t nu_
Control dimension.
ActuationModelFloatingBaseTpl(boost::shared_ptr< StateMultibody > state)
Initialize the floating-base actuation model.
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 control input .
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.
State multibody representation.
Definition: multibody.hpp:31