crocoddyl  1.7.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
multibody.hpp
1 // 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_STATES_MULTIBODY_HPP_
10 #define CROCODDYL_MULTIBODY_STATES_MULTIBODY_HPP_
11 
12 #include "crocoddyl/multibody/fwd.hpp"
13 #include "crocoddyl/core/state-base.hpp"
14 #include <pinocchio/multibody/model.hpp>
15 
16 namespace crocoddyl {
17 
29 template <typename _Scalar>
30 class StateMultibodyTpl : public StateAbstractTpl<_Scalar> {
31  public:
32  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
33 
34  typedef _Scalar Scalar;
35  typedef MathBaseTpl<Scalar> MathBase;
36  typedef StateAbstractTpl<Scalar> Base;
37  typedef pinocchio::ModelTpl<Scalar> PinocchioModel;
38  typedef typename MathBase::VectorXs VectorXs;
39  typedef typename MathBase::MatrixXs MatrixXs;
40 
46  explicit StateMultibodyTpl(boost::shared_ptr<PinocchioModel> model);
48  virtual ~StateMultibodyTpl();
49 
55  virtual VectorXs zero() const;
56 
63  virtual VectorXs rand() const;
64 
65  virtual void diff(const Eigen::Ref<const VectorXs>& x0, const Eigen::Ref<const VectorXs>& x1,
66  Eigen::Ref<VectorXs> dxout) const;
67  virtual void integrate(const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& dx,
68  Eigen::Ref<VectorXs> xout) const;
69  virtual void Jdiff(const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&, Eigen::Ref<MatrixXs> Jfirst,
70  Eigen::Ref<MatrixXs> Jsecond, const Jcomponent firstsecond = both) const;
71 
72  virtual void Jintegrate(const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& dx,
73  Eigen::Ref<MatrixXs> Jfirst, Eigen::Ref<MatrixXs> Jsecond,
74  const Jcomponent firstsecond = both, const AssignmentOp = setto) const;
75  virtual void JintegrateTransport(const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& dx,
76  Eigen::Ref<MatrixXs> Jin, const Jcomponent firstsecond) const;
77 
81  const boost::shared_ptr<PinocchioModel>& get_pinocchio() const;
82 
83  protected:
84  using Base::has_limits_;
85  using Base::lb_;
86  using Base::ndx_;
87  using Base::nq_;
88  using Base::nv_;
89  using Base::nx_;
90  using Base::ub_;
91 
92  private:
93  boost::shared_ptr<PinocchioModel> pinocchio_;
94  VectorXs x0_;
95 };
96 
97 } // namespace crocoddyl
98 
99 /* --- Details -------------------------------------------------------------- */
100 /* --- Details -------------------------------------------------------------- */
101 /* --- Details -------------------------------------------------------------- */
102 #include "crocoddyl/multibody/states/multibody.hxx"
103 
104 #endif // CROCODDYL_MULTIBODY_STATES_MULTIBODY_HPP_
VectorXs lb_
Lower state limits.
Definition: state-base.hpp:289
VectorXs ub_
Upper state limits.
Definition: state-base.hpp:290
virtual VectorXs zero() const
Generate a zero state.
StateMultibodyTpl(boost::shared_ptr< PinocchioModel > model)
Initialize the multibody state.
bool has_limits_
Indicates whether any of the state limits is finite.
Definition: state-base.hpp:291
const boost::shared_ptr< PinocchioModel > & get_pinocchio() const
Return the Pinocchio model (i.e., model of the rigid body system)
std::size_t nx_
State dimension.
Definition: state-base.hpp:285
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
Compute the Jacobian of the state manifold differentiation.
std::size_t nv_
Velocity dimension.
Definition: state-base.hpp:288
virtual void JintegrateTransport(const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &dx, Eigen::Ref< MatrixXs > Jin, const Jcomponent firstsecond) const
Parallel transport from x + dx to x.
virtual void diff(const Eigen::Ref< const VectorXs > &x0, const Eigen::Ref< const VectorXs > &x1, Eigen::Ref< VectorXs > dxout) const
Compute the state manifold differentiation.
std::size_t ndx_
State rate dimension.
Definition: state-base.hpp:286
virtual VectorXs rand() const
Generate a random state.
std::size_t nq_
Configuration dimension.
Definition: state-base.hpp:287
virtual void integrate(const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &dx, Eigen::Ref< VectorXs > xout) const
Compute the state manifold integration.
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
Compute the Jacobian of the state manifold integration.