crocoddyl 1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
 
Loading...
Searching...
No Matches
multibody.hpp
1
2// 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 <pinocchio/multibody/model.hpp>
13
14#include "crocoddyl/multibody/fwd.hpp"
15#include "crocoddyl/core/state-base.hpp"
16
17namespace crocoddyl {
18
30template <typename _Scalar>
31class StateMultibodyTpl : public StateAbstractTpl<_Scalar> {
32 public:
33 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
34
35 typedef _Scalar Scalar;
38 typedef pinocchio::ModelTpl<Scalar> PinocchioModel;
39 typedef typename MathBase::VectorXs VectorXs;
40 typedef typename MathBase::MatrixXs MatrixXs;
41
47 explicit StateMultibodyTpl(boost::shared_ptr<PinocchioModel> model);
49 virtual ~StateMultibodyTpl();
50
56 virtual VectorXs zero() const;
57
64 virtual VectorXs rand() const;
65
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;
72
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;
78
82 const boost::shared_ptr<PinocchioModel>& get_pinocchio() const;
83
84 protected:
86 using Base::lb_;
87 using Base::ndx_;
88 using Base::nq_;
89 using Base::nv_;
90 using Base::nx_;
91 using Base::ub_;
92
93 private:
94 boost::shared_ptr<PinocchioModel> pinocchio_;
95 VectorXs x0_;
96};
97
98} // namespace crocoddyl
99
100/* --- Details -------------------------------------------------------------- */
101/* --- Details -------------------------------------------------------------- */
102/* --- Details -------------------------------------------------------------- */
103#include "crocoddyl/multibody/states/multibody.hxx"
104
105#endif // CROCODDYL_MULTIBODY_STATES_MULTIBODY_HPP_
Abstract class for the state representation.
Definition: state-base.hpp:42
std::size_t nv_
Velocity dimension.
Definition: state-base.hpp:285
std::size_t nx_
State dimension.
Definition: state-base.hpp:282
bool has_limits_
Indicates whether any of the state limits is finite.
Definition: state-base.hpp:288
std::size_t nq_
Configuration dimension.
Definition: state-base.hpp:284
VectorXs lb_
Lower state limits.
Definition: state-base.hpp:286
VectorXs ub_
Upper state limits.
Definition: state-base.hpp:287
std::size_t ndx_
State rate dimension.
Definition: state-base.hpp:283
State multibody representation.
Definition: multibody.hpp:31
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 VectorXs zero() const
Generate a zero state.
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.
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.
const boost::shared_ptr< PinocchioModel > & get_pinocchio() const
Return the Pinocchio model (i.e., model of the rigid body system)
StateMultibodyTpl(boost::shared_ptr< PinocchioModel > model)
Initialize the multibody state.
virtual VectorXs rand() const
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
Parallel transport from integrate(x, dx) to x.
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.