crocoddyl  1.6.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
multibody.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2020, 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 #include "crocoddyl/multibody/fwd.hpp"
12 #include "crocoddyl/core/state-base.hpp"
13 #include <pinocchio/multibody/model.hpp>
14 
15 namespace crocoddyl {
16 
17 template <typename _Scalar>
18 class StateMultibodyTpl : public StateAbstractTpl<_Scalar> {
19  public:
20  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
21 
22  typedef _Scalar Scalar;
23  typedef MathBaseTpl<Scalar> MathBase;
24  typedef StateAbstractTpl<Scalar> Base;
25  typedef pinocchio::ModelTpl<Scalar> PinocchioModel;
26  typedef typename MathBase::VectorXs VectorXs;
27  typedef typename MathBase::MatrixXs MatrixXs;
28 
29  enum JointType { FreeFlyer = 0, Spherical, Simple };
30 
31  explicit StateMultibodyTpl(boost::shared_ptr<PinocchioModel> model);
32  StateMultibodyTpl();
33  virtual ~StateMultibodyTpl();
34 
35  virtual VectorXs zero() const;
36  virtual VectorXs rand() const;
37  virtual void diff(const Eigen::Ref<const VectorXs>& x0, const Eigen::Ref<const VectorXs>& x1,
38  Eigen::Ref<VectorXs> dxout) const;
39  virtual void integrate(const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& dx,
40  Eigen::Ref<VectorXs> xout) const;
41  virtual void Jdiff(const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&, Eigen::Ref<MatrixXs> Jfirst,
42  Eigen::Ref<MatrixXs> Jsecond, const Jcomponent firstsecond = both) const;
43 
44  virtual void Jintegrate(const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& dx,
45  Eigen::Ref<MatrixXs> Jfirst, Eigen::Ref<MatrixXs> Jsecond,
46  const Jcomponent firstsecond = both, const AssignmentOp = setto) const;
47  virtual void JintegrateTransport(const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& dx,
48  Eigen::Ref<MatrixXs> Jin, const Jcomponent firstsecond) const;
49 
50  const boost::shared_ptr<PinocchioModel>& get_pinocchio() const;
51 
52  protected:
53  using Base::has_limits_;
54  using Base::lb_;
55  using Base::ndx_;
56  using Base::nq_;
57  using Base::nv_;
58  using Base::nx_;
59  using Base::ub_;
60 
61  private:
62  boost::shared_ptr<PinocchioModel> pinocchio_;
63  VectorXs x0_;
64  JointType joint_type_;
65 };
66 
67 } // namespace crocoddyl
68 
69 /* --- Details -------------------------------------------------------------- */
70 /* --- Details -------------------------------------------------------------- */
71 /* --- Details -------------------------------------------------------------- */
72 #include "crocoddyl/multibody/states/multibody.hxx"
73 
74 #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 reference state.
bool has_limits_
Indicates whether any of the state limits is finite.
Definition: state-base.hpp:291
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 reference 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.