Crocoddyl
 
Loading...
Searching...
No Matches
state.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2019-2025, LAAS-CNRS, New York University,
5// Max Planck Gesellschaft,
6// University of Edinburgh, Heriot-Watt University
7// Copyright note valid unless otherwise stated in individual files.
8// All rights reserved.
10
11#ifndef CROCODDYL_CORE_NUMDIFF_STATE_HPP_
12#define CROCODDYL_CORE_NUMDIFF_STATE_HPP_
13
14#include "crocoddyl/core/fwd.hpp"
15#include "crocoddyl/core/state-base.hpp"
16
17namespace crocoddyl {
18
19template <typename _Scalar>
20class StateNumDiffTpl : public StateAbstractTpl<_Scalar> {
21 public:
22 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
23 CROCODDYL_DERIVED_CAST(StateBase, StateNumDiffTpl)
24
25 typedef _Scalar Scalar;
28 typedef typename MathBase::VectorXs VectorXs;
29 typedef typename MathBase::MatrixXs MatrixXs;
30
31 explicit StateNumDiffTpl(std::shared_ptr<Base> state);
32 virtual ~StateNumDiffTpl();
33
34 virtual VectorXs zero() const override;
35 virtual VectorXs rand() const override;
36 virtual void diff(const Eigen::Ref<const VectorXs>& x0,
37 const Eigen::Ref<const VectorXs>& x1,
38 Eigen::Ref<VectorXs> dxout) const override;
39 virtual void integrate(const Eigen::Ref<const VectorXs>& x,
40 const Eigen::Ref<const VectorXs>& dx,
41 Eigen::Ref<VectorXs> xout) const override;
57 virtual void Jdiff(const Eigen::Ref<const VectorXs>& x0,
58 const Eigen::Ref<const VectorXs>& x1,
59 Eigen::Ref<MatrixXs> Jfirst, Eigen::Ref<MatrixXs> Jsecond,
60 Jcomponent firstsecond = both) const override;
76 virtual void Jintegrate(const Eigen::Ref<const VectorXs>& x,
77 const Eigen::Ref<const VectorXs>& dx,
78 Eigen::Ref<MatrixXs> Jfirst,
79 Eigen::Ref<MatrixXs> Jsecond,
80 const Jcomponent firstsecond = both,
81 const AssignmentOp op = setto) const override;
82
83 virtual void JintegrateTransport(
84 const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& dx,
85 Eigen::Ref<MatrixXs> Jin,
86 const Jcomponent firstsecond = both) const override;
87
88 template <typename NewScalar>
89 StateNumDiffTpl<NewScalar> cast() const;
90
95 const Scalar get_disturbance() const;
96
101 void set_disturbance(const Scalar disturbance);
102
108 virtual void print(std::ostream& os) const override;
109
110 private:
111 std::shared_ptr<Base>
112 state_;
113 Scalar e_jac_;
115
116 protected:
117 using Base::has_limits_;
118 using Base::lb_;
119 using Base::ndx_;
120 using Base::nq_;
121 using Base::nv_;
122 using Base::nx_;
123 using Base::ub_;
124};
125
126} // namespace crocoddyl
127
128/* --- Details -------------------------------------------------------------- */
129/* --- Details -------------------------------------------------------------- */
130/* --- Details -------------------------------------------------------------- */
131#include "crocoddyl/core/numdiff/state.hxx"
132
133CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::StateNumDiffTpl)
134
135#endif // CROCODDYL_CORE_NUMDIFF_STATE_HPP_
Abstract class for the state representation.
std::size_t nv_
Velocity dimension.
std::size_t nx_
State dimension.
bool has_limits_
Indicates whether any of the state limits is finite.
std::size_t nq_
Configuration dimension.
VectorXs lb_
Lower state limits.
VectorXs ub_
Upper state limits.
std::size_t ndx_
State rate dimension.
virtual void integrate(const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &dx, Eigen::Ref< VectorXs > xout) const override
Compute the state manifold integration.
virtual void Jdiff(const Eigen::Ref< const VectorXs > &x0, const Eigen::Ref< const VectorXs > &x1, Eigen::Ref< MatrixXs > Jfirst, Eigen::Ref< MatrixXs > Jsecond, Jcomponent firstsecond=both) const override
This computes the Jacobian of the diff method by finite differentiation:
const Scalar get_disturbance() const
Return the disturbance constant used in the numerical differentiation routine.
void set_disturbance(const Scalar disturbance)
Modify the disturbance constant used by the numerical differentiation routine.
virtual void Jintegrate(const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &dx, Eigen::Ref< MatrixXs > Jfirst, Eigen::Ref< MatrixXs > Jsecond, const Jcomponent firstsecond=both, const AssignmentOp op=setto) const override
This computes the Jacobian of the integrate method by finite differentiation:
virtual VectorXs rand() const override
Generate a random state.
virtual VectorXs zero() const override
Generate a zero state.
virtual void diff(const Eigen::Ref< const VectorXs > &x0, const Eigen::Ref< const VectorXs > &x1, Eigen::Ref< VectorXs > dxout) const override
Compute the state manifold differentiation.
virtual void JintegrateTransport(const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &dx, Eigen::Ref< MatrixXs > Jin, const Jcomponent firstsecond=both) const override
Parallel transport from integrate(x, dx) to x.
virtual void print(std::ostream &os) const override
Print relevant information of the state numdiff.