Go to the documentation of this file. 1 #ifndef __quadruped_walkgen_quadruped_hpp__
2 #define __quadruped_walkgen_quadruped_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 Eigen::Matrix<Scalar, 3, 1>::Zero());
25 virtual void calc(
const boost::shared_ptr<ActionDataAbstract>& data,
26 const Eigen::Ref<const typename MathBase::VectorXs>& x,
27 const Eigen::Ref<const typename MathBase::VectorXs>& u);
28 virtual void calcDiff(
const boost::shared_ptr<ActionDataAbstract>& data,
29 const Eigen::Ref<const typename MathBase::VectorXs>& x,
30 const Eigen::Ref<const typename MathBase::VectorXs>& u);
31 virtual boost::shared_ptr<ActionDataAbstract>
createData();
52 const typename Eigen::Matrix<Scalar, 3, 3>&
get_gI()
const;
53 void set_gI(
const typename MathBase::Matrix3s& inertia_matrix);
76 void update_model(
const Eigen::Ref<const typename MathBase::MatrixXs>& l_feet,
77 const Eigen::Ref<const typename MathBase::MatrixXs>& xref,
78 const Eigen::Ref<const typename MathBase::MatrixXs>& S);
81 const typename Eigen::Matrix<Scalar, 12, 12>&
get_A()
const;
82 const typename Eigen::Matrix<Scalar, 12, 12>&
get_B()
const;
85 using Base::has_control_limits_;
101 bool relative_forces;
102 bool implicit_integration;
104 typename Eigen::Matrix<Scalar, 12, 1> uref_;
106 typename Eigen::Matrix<Scalar, 12, 1> force_weights_;
107 typename Eigen::Matrix<Scalar, 12, 1> state_weights_;
109 typename Eigen::Matrix<Scalar, 12, 12> A;
110 typename Eigen::Matrix<Scalar, 12, 12> B;
111 typename Eigen::Matrix<Scalar, 12, 1> g;
112 typename Eigen::Matrix<Scalar, 3, 3> I_inv;
113 typename MathBase::Matrix3s R_tmp;
114 typename Eigen::Matrix<Scalar, 3, 3> gI;
116 typename Eigen::Matrix<Scalar, 3, 4> lever_arms;
117 typename MathBase::Vector3s lever_tmp;
118 typename MathBase::MatrixXs xref_;
120 typename Eigen::Matrix<Scalar, 24, 1> ub;
122 typename Eigen::Matrix<Scalar, 24, 1> Fa_x_u;
123 typename Eigen::Matrix<Scalar, 24, 1> rub_max_;
124 typename Eigen::Matrix<Scalar, 24, 24> Arr;
125 typename Eigen::Matrix<Scalar, 6, 1> r;
128 typename Eigen::Matrix<Scalar, 2, 4> pshoulder_0;
129 typename Eigen::Matrix<Scalar, 3, 4> psh;
130 typename Eigen::Matrix<Scalar, 4, 1> sh_ub_max_;
131 typename Eigen::Matrix<Scalar, 4, 1> gait;
132 typename Eigen::Matrix<Scalar, 3, 1> offset_com;
137 template <
typename _Scalar>
139 :
public crocoddyl::ActionDataAbstractTpl<_Scalar> {
140 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
144 typedef crocoddyl::ActionDataAbstractTpl<Scalar>
Base;
156 template <
template <
typename Scalar>
class Model>
158 : crocoddyl::ActionDataAbstractTpl<
Scalar>(model) {}
void set_implicit_integration(const bool &implicit)
Definition: quadruped.hxx:431
const Scalar & get_mu() const
Definition: quadruped.hxx:298
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar Scalar
Definition: quadruped.hpp:142
const Eigen::Matrix< Scalar, 12, 12 > & get_A() const
Definition: quadruped.hxx:375
ActionModelQuadrupedTpl< double > ActionModelQuadruped
Definition: quadruped.hpp:165
void set_force_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped.hxx:261
~ActionModelQuadrupedTpl()
Definition: quadruped.hxx:72
void set_friction_weight(const Scalar &weight)
Definition: quadruped.hxx:292
const Eigen::Matrix< Scalar, 12, 12 > & get_B() const
Definition: quadruped.hxx:380
const Scalar & get_dt() const
Definition: quadruped.hxx:317
ActionDataQuadrupedTpl(Model< Scalar > *const model)
Definition: quadruped.hpp:157
crocoddyl::MathBaseTpl< Scalar > MathBase
Definition: quadruped.hpp:19
crocoddyl::ActionDataAbstractTpl< double > ActionDataAbstract
Definition: quadruped.hpp:168
ActionDataQuadrupedTpl< double > ActionDataQuadruped
Definition: quadruped.hpp:166
Definition: quadruped.hpp:11
const Scalar & get_max_fz_contact() const
Definition: quadruped.hxx:356
const Eigen::Matrix< Scalar, 12, 1 > & get_state_weights() const
Definition: quadruped.hxx:273
ActionModelQuadrupedTpl(typename Eigen::Matrix< Scalar, 3, 1 > offset_CoM=Eigen::Matrix< Scalar, 3, 1 >::Zero())
Definition: quadruped.hxx:8
const Scalar & get_shoulder_hlim() const
Definition: quadruped.hxx:385
void set_mass(const Scalar &m)
Definition: quadruped.hxx:311
const Eigen::Matrix< Scalar, 12, 1 > & get_force_weights() const
Definition: quadruped.hxx:257
const Scalar & get_min_fz_contact() const
Definition: quadruped.hxx:345
void update_model(const Eigen::Ref< const typename MathBase::MatrixXs > &l_feet, const Eigen::Ref< const typename MathBase::MatrixXs > &xref, const Eigen::Ref< const typename MathBase::MatrixXs > &S)
Definition: quadruped.hxx:441
void set_shoulder_weight(const Scalar &weight)
Definition: quadruped.hxx:399
virtual void calcDiff(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const typename MathBase::VectorXs > &x, const Eigen::Ref< const typename MathBase::VectorXs > &u)
Definition: quadruped.hxx:155
crocoddyl::ActionDataAbstractTpl< Scalar > ActionDataAbstract
Definition: quadruped.hpp:17
const Eigen::Matrix< Scalar, 3, 3 > & get_gI() const
Definition: quadruped.hxx:330
const bool & get_implicit_integration() const
Definition: quadruped.hxx:427
crocoddyl::StateAbstractTpl< double > StateAbstract
Definition: quadruped.hpp:169
void set_gI(const typename MathBase::Matrix3s &inertia_matrix)
Definition: quadruped.hxx:334
void set_dt(const Scalar &dt)
Definition: quadruped.hxx:321
_Scalar Scalar
Definition: quadruped.hpp:16
crocoddyl::MathBaseTpl< Scalar > MathBase
Definition: quadruped.hpp:143
crocoddyl::ActionDataUnicycleTpl< double > ActionDataUnicycle
Definition: quadruped.hpp:172
crocoddyl::ActionDataAbstractTpl< Scalar > Base
Definition: quadruped.hpp:144
const Scalar & get_mass() const
Definition: quadruped.hxx:307
Definition: quadruped.hpp:13
Definition: quadruped.hpp:138
virtual void calc(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const typename MathBase::VectorXs > &x, const Eigen::Ref< const typename MathBase::VectorXs > &u)
Definition: quadruped.hxx:75
crocoddyl::ActionModelAbstractTpl< Scalar > Base
Definition: quadruped.hpp:18
crocoddyl::ActionModelUnicycleTpl< double > ActionModelUnicycle
Definition: quadruped.hpp:171
void set_mu(const Scalar &mu_coeff)
Definition: quadruped.hxx:302
virtual boost::shared_ptr< ActionDataAbstract > createData()
Definition: quadruped.hxx:247
const Scalar & get_shoulder_weight() const
Definition: quadruped.hxx:395
void set_shoulder_hlim(const Scalar &hlim)
Definition: quadruped.hxx:389
void set_max_fz_contact(const Scalar &max_fz_)
Definition: quadruped.hxx:361
void set_relative_forces(const bool &rel_forces)
Definition: quadruped.hxx:412
void set_min_fz_contact(const Scalar &min_fz)
Definition: quadruped.hxx:350
const Scalar & get_friction_weight() const
Definition: quadruped.hxx:288
void set_state_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped.hxx:277
crocoddyl::ActionModelAbstractTpl< double > ActionModelAbstract
Definition: quadruped.hpp:167
const bool & get_relative_forces() const
Definition: quadruped.hxx:408