Crocoddyl
 
Loading...
Searching...
No Matches
multibody.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2019-2025, LAAS-CNRS, University of Edinburgh,
5// Heriot-Watt University
6// Copyright note valid unless otherwise stated in individual files.
7// All rights reserved.
9
10#ifndef CROCODDYL_MULTIBODY_STATES_MULTIBODY_HPP_
11#define CROCODDYL_MULTIBODY_STATES_MULTIBODY_HPP_
12
13#include "crocoddyl/core/state-base.hpp"
14#include "crocoddyl/multibody/fwd.hpp"
15
16namespace crocoddyl {
17
33template <typename _Scalar>
34class StateMultibodyTpl : public StateAbstractTpl<_Scalar> {
35 public:
36 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
37 CROCODDYL_DERIVED_CAST(StateBase, StateMultibodyTpl)
38
39 typedef _Scalar Scalar;
42 typedef pinocchio::ModelTpl<Scalar> PinocchioModel;
43 typedef typename MathBase::VectorXs VectorXs;
44 typedef typename MathBase::MatrixXs MatrixXs;
45
51 explicit StateMultibodyTpl(std::shared_ptr<PinocchioModel> model);
53 virtual ~StateMultibodyTpl();
54
60 virtual VectorXs zero() const override;
61
68 virtual VectorXs rand() const override;
69
70 virtual void diff(const Eigen::Ref<const VectorXs>& x0,
71 const Eigen::Ref<const VectorXs>& x1,
72 Eigen::Ref<VectorXs> dxout) const override;
73 virtual void integrate(const Eigen::Ref<const VectorXs>& x,
74 const Eigen::Ref<const VectorXs>& dx,
75 Eigen::Ref<VectorXs> xout) const override;
76 virtual void Jdiff(const Eigen::Ref<const VectorXs>&,
77 const Eigen::Ref<const VectorXs>&,
78 Eigen::Ref<MatrixXs> Jfirst, Eigen::Ref<MatrixXs> Jsecond,
79 const Jcomponent firstsecond = both) const override;
80
81 virtual void Jintegrate(const Eigen::Ref<const VectorXs>& x,
82 const Eigen::Ref<const VectorXs>& dx,
83 Eigen::Ref<MatrixXs> Jfirst,
84 Eigen::Ref<MatrixXs> Jsecond,
85 const Jcomponent firstsecond = both,
86 const AssignmentOp = setto) const override;
87 virtual void JintegrateTransport(const Eigen::Ref<const VectorXs>& x,
88 const Eigen::Ref<const VectorXs>& dx,
89 Eigen::Ref<MatrixXs> Jin,
90 const Jcomponent firstsecond) const override;
91
95 const std::shared_ptr<PinocchioModel>& get_pinocchio() const;
96
97 template <typename NewScalar>
99
105 virtual void print(std::ostream& os) const override;
106
107 protected:
108 using Base::has_limits_;
109 using Base::lb_;
110 using Base::ndx_;
111 using Base::nq_;
112 using Base::nv_;
113 using Base::nx_;
114 using Base::ub_;
115
116 private:
117 std::shared_ptr<PinocchioModel> pinocchio_;
118 VectorXs x0_;
119};
120
121} // namespace crocoddyl
122
123/* --- Details -------------------------------------------------------------- */
124/* --- Details -------------------------------------------------------------- */
125/* --- Details -------------------------------------------------------------- */
126#include "crocoddyl/multibody/states/multibody.hxx"
127
128extern template class CROCODDYL_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI
130extern template class CROCODDYL_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI
132
133#endif // CROCODDYL_MULTIBODY_STATES_MULTIBODY_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.
State multibody representation.
Definition multibody.hpp:34
StateMultibodyTpl(std::shared_ptr< PinocchioModel > model)
Initialize the multibody state.
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=setto) const override
Compute the Jacobian of the state manifold integration.
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.
const std::shared_ptr< PinocchioModel > & get_pinocchio() const
Return the Pinocchio model (i.e., model of the rigid body system)
virtual void Jdiff(const Eigen::Ref< const VectorXs > &, const Eigen::Ref< const VectorXs > &, Eigen::Ref< MatrixXs > Jfirst, Eigen::Ref< MatrixXs > Jsecond, const Jcomponent firstsecond=both) const override
Compute the Jacobian of the state manifold differentiation.
virtual VectorXs rand() const override
Generate a random state.
virtual void JintegrateTransport(const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &dx, Eigen::Ref< MatrixXs > Jin, const Jcomponent firstsecond) const override
Parallel transport from integrate(x, dx) to x.
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 print(std::ostream &os) const override
Print relevant information of the state multibody.