aligator  0.16.0
A versatile and efficient C++ library for real-time constrained trajectory optimization.
Loading...
Searching...
No Matches
state-wrap.hpp
Go to the documentation of this file.
1#pragma once
2
5
6#include <crocoddyl/core/state-base.hpp>
7#include <boost/shared_ptr.hpp>
8
10
13template <typename _Scalar>
15public:
16 using Scalar = _Scalar;
18 using TangentVectorType = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
20 using Base::ndx;
21 using Base::nx;
22
23 using StateAbstract = crocoddyl::StateAbstractTpl<Scalar>;
24
25 shared_ptr<StateAbstract> croc_state;
26
27 explicit StateWrapperTpl(const shared_ptr<StateAbstract> &state)
28 : Base(int(state->get_nx()), int(state->get_ndx()))
29 , croc_state(state) {}
30
31 static crocoddyl::Jcomponent convert_to_firstsecond(int arg) {
32 if (arg == 0) {
33 return crocoddyl::first;
34 } else if (arg == 1) {
35 return crocoddyl::second;
36 } else {
37 return crocoddyl::both;
38 }
39 }
40
41protected:
42 void neutral_impl(VectorRef out) const { out = croc_state->zero(); }
43
44 void rand_impl(VectorRef out) const { out = croc_state->rand(); }
45
46 void integrate_impl(const ConstVectorRef &x, const ConstVectorRef &v,
47 VectorRef out) const {
48 croc_state->integrate(x, v, out);
49 }
50
51 void Jintegrate_impl(const ConstVectorRef &x, const ConstVectorRef &v,
52 MatrixRef Jout, int arg) const {
53 croc_state->Jintegrate(x, v, Jout, Jout, convert_to_firstsecond(arg));
54 }
55
56 void difference_impl(const ConstVectorRef &x0, const ConstVectorRef &x1,
57 VectorRef out) const {
58 croc_state->diff(x0, x1, out);
59 }
60
61 void Jdifference_impl(const ConstVectorRef &x0, const ConstVectorRef &x1,
62 MatrixRef Jout, int arg) const {
63 croc_state->Jdiff(x0, x1, Jout, Jout, convert_to_firstsecond(arg));
64 }
65
66 void JintegrateTransport_impl(const ConstVectorRef &x,
67 const ConstVectorRef &v, MatrixRef Jout,
68 int arg) const {
69 croc_state->JintegrateTransport(x, v, Jout, convert_to_firstsecond(arg));
70 }
71};
72
73#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
74extern template struct StateWrapperTpl<context::Scalar>;
75#endif
76
77} // namespace aligator::compat::croc
Headers for the Crocoddyl compatibility module.
ManifoldAbstractTpl(int nx, int ndx)
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > TangentVectorType
static crocoddyl::Jcomponent convert_to_firstsecond(int arg)
void rand_impl(VectorRef out) const
crocoddyl::StateAbstractTpl< Scalar > StateAbstract
void JintegrateTransport_impl(const ConstVectorRef &x, const ConstVectorRef &v, MatrixRef Jout, int arg) const
void Jdifference_impl(const ConstVectorRef &x0, const ConstVectorRef &x1, MatrixRef Jout, int arg) const
StateWrapperTpl(const shared_ptr< StateAbstract > &state)
void difference_impl(const ConstVectorRef &x0, const ConstVectorRef &x1, VectorRef out) const
Implementation of the manifold retraction operation.
void Jintegrate_impl(const ConstVectorRef &x, const ConstVectorRef &v, MatrixRef Jout, int arg) const
void integrate_impl(const ConstVectorRef &x, const ConstVectorRef &v, VectorRef out) const
Perform the manifold integration operation.
void neutral_impl(VectorRef out) const