crocoddyl 1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
 
Loading...
Searching...
No Matches
residual-base.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2021, University of Edinburgh
5// Copyright note valid unless otherwise stated in individual files.
6// All rights reserved.
8
9#ifndef CROCODDYL_CORE_RESIDUAL_BASE_HPP_
10#define CROCODDYL_CORE_RESIDUAL_BASE_HPP_
11
12#include <boost/shared_ptr.hpp>
13#include <boost/make_shared.hpp>
14
15#include "crocoddyl/core/fwd.hpp"
16#include "crocoddyl/core/state-base.hpp"
17#include "crocoddyl/core/data-collector-base.hpp"
18
19namespace crocoddyl {
20
37template <typename _Scalar>
39 public:
40 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
41
42 typedef _Scalar Scalar;
47 typedef typename MathBase::VectorXs VectorXs;
48 typedef typename MathBase::MatrixXs MatrixXs;
49
60 ResidualModelAbstractTpl(boost::shared_ptr<StateAbstract> state, const std::size_t nr, const std::size_t nu,
61 const bool q_dependent = true, const bool v_dependent = true,
62 const bool u_dependent = true);
63
75 ResidualModelAbstractTpl(boost::shared_ptr<StateAbstract> state, const std::size_t nr, const bool q_dependent = true,
76 const bool v_dependent = true, const bool u_dependent = true);
78
86 virtual void calc(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
87 const Eigen::Ref<const VectorXs>& u);
88
98 virtual void calc(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
99
109 virtual void calcDiff(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
110 const Eigen::Ref<const VectorXs>& u);
111
121 virtual void calcDiff(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
122
133 virtual boost::shared_ptr<ResidualDataAbstract> createData(DataCollectorAbstract* const data);
134
138 const boost::shared_ptr<StateAbstract>& get_state() const;
139
143 std::size_t get_nr() const;
144
148 std::size_t get_nu() const;
149
153 bool get_q_dependent() const;
154
158 bool get_v_dependent() const;
159
163 bool get_u_dependent() const;
164
168 template <class Scalar>
169 friend std::ostream& operator<<(std::ostream& os, const ResidualModelAbstractTpl<Scalar>& model);
170
176 virtual void print(std::ostream& os) const;
177
178 protected:
179 boost::shared_ptr<StateAbstract> state_;
180 std::size_t nr_;
181 std::size_t nu_;
182 VectorXs unone_;
186};
187
188template <typename _Scalar>
190 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
191
192 typedef _Scalar Scalar;
195 typedef typename MathBase::VectorXs VectorXs;
196 typedef typename MathBase::MatrixXs MatrixXs;
197
198 template <template <typename Scalar> class Model>
199 ResidualDataAbstractTpl(Model<Scalar>* const model, DataCollectorAbstract* const data)
200 : shared(data),
201 r(model->get_nr()),
202 Rx(model->get_nr(), model->get_state()->get_ndx()),
203 Ru(model->get_nr(), model->get_nu()) {
204 r.setZero();
205 Rx.setZero();
206 Ru.setZero();
207 }
208 virtual ~ResidualDataAbstractTpl() {}
209
211 VectorXs r;
212 MatrixXs Rx;
213 MatrixXs Ru;
214};
215
216} // namespace crocoddyl
217
218/* --- Details -------------------------------------------------------------- */
219/* --- Details -------------------------------------------------------------- */
220/* --- Details -------------------------------------------------------------- */
221#include "crocoddyl/core/residual-base.hxx"
222
223#endif // CROCODDYL_CORE_RESIDUAL_BASE_HPP_
Abstract class for residual models.
ResidualModelAbstractTpl(boost::shared_ptr< StateAbstract > state, const std::size_t nr, const bool q_dependent=true, const bool v_dependent=true, const bool u_dependent=true)
friend std::ostream & operator<<(std::ostream &os, const ResidualModelAbstractTpl< Scalar > &model)
Print information on the residual model.
bool get_q_dependent() const
Return true if the residual function depends on q.
virtual void calc(const boost::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
Compute the residual vector for nodes that depends only on the state.
virtual boost::shared_ptr< ResidualDataAbstract > createData(DataCollectorAbstract *const data)
Create the residual data.
virtual void calcDiff(const boost::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
Compute the Jacobian of the residual functions with respect to the state only.
virtual void print(std::ostream &os) const
Print relevant information of the residual model.
ResidualModelAbstractTpl(boost::shared_ptr< StateAbstract > state, const std::size_t nr, const std::size_t nu, const bool q_dependent=true, const bool v_dependent=true, const bool u_dependent=true)
Initialize the residual model.
bool u_dependent_
Label that indicates if the residual function depends on u.
virtual void calc(const boost::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the residual vector.
const boost::shared_ptr< StateAbstract > & get_state() const
Return the state.
bool get_v_dependent() const
Return true if the residual function depends on v.
boost::shared_ptr< StateAbstract > state_
State description.
std::size_t nu_
Control dimension.
virtual void calcDiff(const boost::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the Jacobian of the residual vector.
VectorXs unone_
No control vector.
bool q_dependent_
Label that indicates if the residual function depends on q.
std::size_t nr_
Residual vector dimension.
bool get_u_dependent() const
Return true if the residual function depends on u.
std::size_t get_nr() const
Return the dimension of the residual vector.
bool v_dependent_
Label that indicates if the residual function depends on v.
std::size_t get_nu() const
Return the dimension of the control input.
Abstract class for the state representation.
Definition: state-base.hpp:42
MatrixXs Ru
Jacobian of the residual vector with respect the control.
MatrixXs Rx
Jacobian of the residual vector with respect the state.
DataCollectorAbstract * shared
Shared data allocated by the action model.