9 #ifndef CROCODDYL_MULTIBODY_STATES_MULTIBODY_HPP_ 10 #define CROCODDYL_MULTIBODY_STATES_MULTIBODY_HPP_ 12 #include <pinocchio/multibody/model.hpp> 14 #include "crocoddyl/multibody/fwd.hpp" 15 #include "crocoddyl/core/state-base.hpp" 30 template <
typename _Scalar>
31 class StateMultibodyTpl :
public StateAbstractTpl<_Scalar> {
33 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
35 typedef _Scalar Scalar;
36 typedef MathBaseTpl<Scalar> MathBase;
37 typedef StateAbstractTpl<Scalar> Base;
38 typedef pinocchio::ModelTpl<Scalar> PinocchioModel;
39 typedef typename MathBase::VectorXs VectorXs;
40 typedef typename MathBase::MatrixXs MatrixXs;
56 virtual VectorXs
zero()
const;
64 virtual VectorXs
rand()
const;
66 virtual void diff(
const Eigen::Ref<const VectorXs>& x0,
const Eigen::Ref<const VectorXs>& x1,
67 Eigen::Ref<VectorXs> dxout)
const;
68 virtual void integrate(
const Eigen::Ref<const VectorXs>& x,
const Eigen::Ref<const VectorXs>& dx,
69 Eigen::Ref<VectorXs> xout)
const;
70 virtual void Jdiff(
const Eigen::Ref<const VectorXs>&,
const Eigen::Ref<const VectorXs>&, Eigen::Ref<MatrixXs> Jfirst,
71 Eigen::Ref<MatrixXs> Jsecond,
const Jcomponent firstsecond = both)
const;
73 virtual void Jintegrate(
const Eigen::Ref<const VectorXs>& x,
const Eigen::Ref<const VectorXs>& dx,
74 Eigen::Ref<MatrixXs> Jfirst, Eigen::Ref<MatrixXs> Jsecond,
75 const Jcomponent firstsecond = both,
const AssignmentOp = setto)
const;
76 virtual void JintegrateTransport(
const Eigen::Ref<const VectorXs>& x,
const Eigen::Ref<const VectorXs>& dx,
77 Eigen::Ref<MatrixXs> Jin,
const Jcomponent firstsecond)
const;
82 const boost::shared_ptr<PinocchioModel>&
get_pinocchio()
const;
94 boost::shared_ptr<PinocchioModel> pinocchio_;
103 #include "crocoddyl/multibody/states/multibody.hxx" 105 #endif // CROCODDYL_MULTIBODY_STATES_MULTIBODY_HPP_ VectorXs lb_
Lower state limits.
VectorXs ub_
Upper state limits.
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.
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.
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.
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 integrate(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.
virtual VectorXs rand() const
Generate a random state.
std::size_t nq_
Configuration dimension.
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.