1 #ifndef __quadruped_walkgen_quadruped_nl_hpp__
2 #define __quadruped_walkgen_quadruped_nl_hpp__
5 #include "crocoddyl/core/action-base.hpp"
6 #include "crocoddyl/core/fwd.hpp"
7 #include "crocoddyl/core/states/euclidean.hpp"
8 #include "crocoddyl/core/utils/timer.hpp"
9 #include "crocoddyl/multibody/friction-cone.hpp"
12 template <
typename _Scalar>
14 :
public crocoddyl::ActionModelAbstractTpl<_Scalar> {
18 typedef crocoddyl::ActionModelAbstractTpl<Scalar>
Base;
19 typedef crocoddyl::MathBaseTpl<Scalar>
MathBase;
22 typename Eigen::Matrix<Scalar, 3, 1> offset_CoM =
23 Eigen::Matrix<Scalar, 3, 1>::Zero());
26 virtual void calc(
const boost::shared_ptr<ActionDataAbstract>& data,
27 const Eigen::Ref<const typename MathBase::VectorXs>& x,
28 const Eigen::Ref<const typename MathBase::VectorXs>& u);
29 virtual void calcDiff(
const boost::shared_ptr<ActionDataAbstract>& data,
30 const Eigen::Ref<const typename MathBase::VectorXs>& x,
31 const Eigen::Ref<const typename MathBase::VectorXs>& u);
32 virtual boost::shared_ptr<ActionDataAbstract>
createData();
53 const typename Eigen::Matrix<Scalar, 3, 3>&
get_gI()
const;
54 void set_gI(
const typename MathBase::Matrix3s& inertia_matrix);
77 void update_model(
const Eigen::Ref<const typename MathBase::MatrixXs>& l_feet,
78 const Eigen::Ref<const typename MathBase::MatrixXs>& xref,
79 const Eigen::Ref<const typename MathBase::MatrixXs>& S);
82 const typename Eigen::Matrix<Scalar, 12, 12>&
get_A()
const;
83 const typename Eigen::Matrix<Scalar, 12, 12>&
get_B()
const;
86 using Base::has_control_limits_;
102 bool relative_forces;
103 bool implicit_integration;
105 typename Eigen::Matrix<Scalar, 12, 1> uref_;
107 typename Eigen::Matrix<Scalar, 12, 1> force_weights_;
108 typename Eigen::Matrix<Scalar, 12, 1> state_weights_;
110 typename Eigen::Matrix<Scalar, 12, 12> A;
111 typename Eigen::Matrix<Scalar, 12, 12> B;
112 typename Eigen::Matrix<Scalar, 12, 1> g;
113 typename Eigen::Matrix<Scalar, 3, 3> I_inv;
114 typename MathBase::Matrix3s R_tmp;
115 typename Eigen::Matrix<Scalar, 3, 3> gI;
117 typename Eigen::Matrix<Scalar, 3, 4> lever_arms;
118 typename MathBase::Vector3s lever_tmp;
119 typename MathBase::MatrixXs xref_;
121 typename Eigen::Matrix<Scalar, 24, 1> ub;
123 typename Eigen::Matrix<Scalar, 24, 1> Fa_x_u;
124 typename Eigen::Matrix<Scalar, 24, 1> rub_max_;
125 typename Eigen::Matrix<Scalar, 24, 24> Arr;
126 typename Eigen::Matrix<Scalar, 6, 1> r;
127 typename Eigen::Matrix<Scalar, 4, 1> gait;
129 typename Eigen::Matrix<Scalar, 3, 1> base_vector_x;
130 typename Eigen::Matrix<Scalar, 3, 1> base_vector_y;
131 typename Eigen::Matrix<Scalar, 3, 1> base_vector_z;
132 typename Eigen::Matrix<Scalar, 3, 1> forces_3d;
135 typename Eigen::Matrix<Scalar, 2, 4> pshoulder_0;
136 typename Eigen::Matrix<Scalar, 3, 4> psh;
137 typename Eigen::Matrix<Scalar, 4, 1> sh_ub_max_;
138 typename Eigen::Matrix<Scalar, 3, 1> offset_com;
143 template <
typename _Scalar>
145 :
public crocoddyl::ActionDataAbstractTpl<_Scalar> {
146 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
150 typedef crocoddyl::ActionDataAbstractTpl<Scalar>
Base;
162 template <
template <
typename Scalar>
class Model>
164 : crocoddyl::ActionDataAbstractTpl<
Scalar>(model) {}