Go to the documentation of this file. 1 #ifndef __quadruped_walkgen_quadruped_step_period_hpp__
2 #define __quadruped_walkgen_quadruped_step_period_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;
24 virtual void calc(
const boost::shared_ptr<ActionDataAbstract>& data,
25 const Eigen::Ref<const typename MathBase::VectorXs>& x,
26 const Eigen::Ref<const typename MathBase::VectorXs>& u);
27 virtual void calcDiff(
const boost::shared_ptr<ActionDataAbstract>& data,
28 const Eigen::Ref<const typename MathBase::VectorXs>& x,
29 const Eigen::Ref<const typename MathBase::VectorXs>& u);
30 virtual boost::shared_ptr<ActionDataAbstract>
createData();
47 void update_model(
const Eigen::Ref<const typename MathBase::MatrixXs>& l_feet,
48 const Eigen::Ref<const typename MathBase::MatrixXs>& xref,
49 const Eigen::Ref<const typename MathBase::MatrixXs>& S);
85 using Base::has_control_limits_;
99 bool centrifugal_term;
106 typename Eigen::Matrix<Scalar, 12, 1> state_weights_;
107 typename Eigen::Matrix<Scalar, 4, 1> step_weights_;
108 typename Eigen::Matrix<Scalar, 8, 1> shoulder_weights_;
109 typename MathBase::Matrix3s R_tmp;
111 typename Eigen::Matrix<Scalar, 8, 4> B;
113 typename MathBase::MatrixXs xref_;
115 typename Eigen::Matrix<Scalar, 8, 1> pshoulder_;
116 typename Eigen::Matrix<Scalar, 2, 4> pshoulder_0;
117 typename Eigen::Matrix<Scalar, 2, 4> pshoulder_tmp;
119 typename Eigen::Matrix<Scalar, 3, 1> pcentrifugal_tmp;
120 typename Eigen::Matrix<Scalar, 3, 1> pcentrifugal_tmp_1;
121 typename Eigen::Matrix<Scalar, 3, 1> pcentrifugal_tmp_2;
123 typename Eigen::Matrix<Scalar, 1, 1> dt_ref_;
124 typename Eigen::Matrix<Scalar, 1, 1> dt_min_;
125 typename Eigen::Matrix<Scalar, 1, 1> dt_max_;
127 typename Eigen::Matrix<Scalar, 4, 1> rub_max_;
128 typename Eigen::Matrix<Scalar, 4, 1> rub_max_bool;
131 template <
typename _Scalar>
133 :
public crocoddyl::ActionDataAbstractTpl<_Scalar> {
134 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
138 typedef crocoddyl::ActionDataAbstractTpl<Scalar>
Base;
150 template <
template <
typename Scalar>
class Model>
152 : crocoddyl::ActionDataAbstractTpl<
Scalar>(model) {}
159 typedef ActionModelQuadrupedStepPeriodTpl<double>
const Scalar & get_dt_bound_weight() const
Definition: quadruped_step_period.hxx:338
const Eigen::Matrix< Scalar, 12, 1 > & get_state_weights() const
Definition: quadruped_step_period.hxx:218
const Scalar & get_dt_ref() const
Definition: quadruped_step_period.hxx:378
void set_nb_nodes(const Scalar &nodes_)
Definition: quadruped_step_period.hxx:356
const Scalar & get_speed_weight() const
Definition: quadruped_step_period.hxx:325
virtual boost::shared_ptr< ActionDataAbstract > createData()
Definition: quadruped_step_period.hxx:208
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_step_period.hxx:412
void set_dt_bound_weight(const Scalar &weight_)
Definition: quadruped_step_period.hxx:344
void set_state_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_step_period.hxx:222
void set_dt_max(const Scalar &dt)
Definition: quadruped_step_period.hxx:402
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar Scalar
Definition: quadruped_step_period.hpp:136
ActionModelQuadrupedStepPeriodTpl< double > ActionModelQuadrupedStepPeriod
Definition: quadruped_step_period.hpp:160
void set_vlim(const Scalar &vlim_)
Definition: quadruped_step_period.hxx:370
Definition: quadruped.hpp:11
void set_shoulder_position(const typename MathBase::VectorXs &weights)
Definition: quadruped_step_period.hxx:267
const Eigen::Matrix< Scalar, 8, 1 > & get_shoulder_weights() const
Definition: quadruped_step_period.hxx:248
void set_dt_min(const Scalar &dt)
Definition: quadruped_step_period.hxx:392
crocoddyl::MathBaseTpl< Scalar > MathBase
Definition: quadruped_step_period.hpp:19
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_step_period.hxx:52
ActionModelQuadrupedStepPeriodTpl()
Definition: quadruped_step_period.hxx:8
void set_T_gait(const Scalar &T_gait_)
Definition: quadruped_step_period.hxx:306
const Scalar & get_T_gait() const
Definition: quadruped_step_period.hxx:301
Definition: quadruped_step_period.hpp:13
ActionDataQuadrupedStepPeriodTpl< double > ActionDataQuadrupedStepPeriod
Definition: quadruped_step_period.hpp:161
crocoddyl::ActionDataAbstractTpl< Scalar > Base
Definition: quadruped_step_period.hpp:138
const Scalar & get_vlim() const
Definition: quadruped_step_period.hxx:365
const Scalar & get_nb_nodes() const
Definition: quadruped_step_period.hxx:351
void set_step_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_step_period.hxx:237
crocoddyl::ActionDataAbstractTpl< Scalar > ActionDataAbstract
Definition: quadruped_step_period.hpp:17
crocoddyl::ActionModelAbstractTpl< Scalar > Base
Definition: quadruped_step_period.hpp:18
void set_dt_weight(const Scalar &weight_)
Definition: quadruped_step_period.hxx:318
ActionDataQuadrupedStepPeriodTpl(Model< Scalar > *const model)
Definition: quadruped_step_period.hpp:151
Definition: quadruped_step_period.hpp:132
~ActionModelQuadrupedStepPeriodTpl()
Definition: quadruped_step_period.hxx:49
void set_speed_weight(const Scalar &weight_)
Definition: quadruped_step_period.hxx:331
const Eigen::Matrix< Scalar, 4, 1 > & get_step_weights() const
Definition: quadruped_step_period.hxx:233
void set_shoulder_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_step_period.hxx:252
const bool & get_centrifugal_term() const
Definition: quadruped_step_period.hxx:289
const Scalar & get_dt_max() const
Definition: quadruped_step_period.hxx:398
const Scalar & get_dt_min() const
Definition: quadruped_step_period.hxx:388
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_step_period.hxx:99
void set_centrifugal_term(const bool ¢_term)
Definition: quadruped_step_period.hxx:294
void set_dt_ref(const Scalar &dt)
Definition: quadruped_step_period.hxx:382
_Scalar Scalar
Definition: quadruped_step_period.hpp:16
void set_symmetry_term(const bool &sym_term)
Definition: quadruped_step_period.hxx:282
crocoddyl::MathBaseTpl< Scalar > MathBase
Definition: quadruped_step_period.hpp:137
const bool & get_symmetry_term() const
Definition: quadruped_step_period.hxx:277
const Scalar & get_dt_weight() const
Definition: quadruped_step_period.hxx:313
const Eigen::Matrix< Scalar, 8, 1 > & get_shoulder_position() const
Definition: quadruped_step_period.hxx:263