crocoddyl 1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
 
Loading...
Searching...
No Matches
full.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_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/core/state-base.hpp"
15
16namespace crocoddyl {
17
27template <typename _Scalar>
29 public:
30 typedef _Scalar Scalar;
35 typedef typename MathBase::VectorXs VectorXs;
36 typedef typename MathBase::MatrixXs MatrixXs;
37
43 explicit ActuationModelFullTpl(boost::shared_ptr<StateAbstract> state) : Base(state, state->get_nv()){};
44 virtual ~ActuationModelFullTpl(){};
45
53 virtual void calc(const boost::shared_ptr<Data>& data, const Eigen::Ref<const VectorXs>& /*x*/,
54 const Eigen::Ref<const VectorXs>& u) {
55 if (static_cast<std::size_t>(u.size()) != nu_) {
56 throw_pretty("Invalid argument: "
57 << "u has wrong dimension (it should be " + std::to_string(nu_) + ")");
58 }
59 data->tau = u;
60 };
61
69#ifndef NDEBUG
70 virtual void calcDiff(const boost::shared_ptr<Data>& data, const Eigen::Ref<const VectorXs>& /*x*/,
71 const Eigen::Ref<const VectorXs>&) {
72#else
73 virtual void calcDiff(const boost::shared_ptr<Data>&, const Eigen::Ref<const VectorXs>& /*x*/,
74 const Eigen::Ref<const VectorXs>&) {
75#endif
76 // The derivatives has constant values which were set in createData.
77 assert_pretty(data->dtau_dx.isZero(), "dtau_dx has wrong value");
78 assert_pretty(MatrixXs(data->dtau_du).isApprox(MatrixXs::Identity(state_->get_nv(), nu_)),
79 "dtau_du has wrong value");
80 };
81
88 virtual boost::shared_ptr<Data> createData() {
89 boost::shared_ptr<Data> data = boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
90 data->dtau_du.diagonal().setOnes();
91 return data;
92 };
93
94 protected:
95 using Base::nu_;
96 using Base::state_;
97};
98
99} // namespace crocoddyl
100
101#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_
Control dimension.
Full actuation model.
Definition: full.hpp:28
virtual boost::shared_ptr< Data > createData()
Create the full actuation data.
Definition: full.hpp:88
ActuationModelFullTpl(boost::shared_ptr< StateAbstract > state)
Initialize the full actuation model.
Definition: full.hpp:43
boost::shared_ptr< StateAbstract > state_
Model of the state.
std::size_t nu_
Control dimension.
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:53
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:70
Abstract class for the state representation.
Definition: state-base.hpp:42