1 #ifndef __quadruped_walkgen_quadruped_time_hpp__
2 #define __quadruped_walkgen_quadruped_time_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();
41 void update_model(
const Eigen::Ref<const typename MathBase::MatrixXs>& l_feet,
42 const Eigen::Ref<const typename MathBase::MatrixXs>& xref,
43 const Eigen::Ref<const typename MathBase::VectorXs>& S);
71 const typename Eigen::Matrix<Scalar, 7, 1>&
get_cost()
const;
74 using Base::has_control_limits_;
86 Scalar dt_bound_weight_cmd;
87 bool centrifugal_term;
92 typename Eigen::Matrix<Scalar, 7, 1> cost_;
94 typename Eigen::Matrix<Scalar, 12, 1> state_weights_;
95 typename Eigen::Matrix<Scalar, 8, 1> heuristic_weights_;
97 typename MathBase::Matrix3s R_tmp;
99 typename MathBase::MatrixXs xref_;
100 typename Eigen::Matrix<Scalar, 8, 1> pheuristic_;
101 typename Eigen::Matrix<Scalar, 8, 1> gait_double_;
109 typename Eigen::Matrix<Scalar, 1, 1> dt_ref_;
110 typename Eigen::Matrix<Scalar, 1, 1> dt_min_;
111 typename Eigen::Matrix<Scalar, 1, 1> dt_max_;
113 typename Eigen::Matrix<Scalar, 2, 1> rub_max_;
114 typename Eigen::Matrix<Scalar, 2, 1> rub_max_bool;
117 template <
typename _Scalar>
119 :
public crocoddyl::ActionDataAbstractTpl<_Scalar> {
120 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
124 typedef crocoddyl::ActionDataAbstractTpl<Scalar>
Base;
136 template <
template <
typename Scalar>
class Model>
138 : crocoddyl::ActionDataAbstractTpl<
Scalar>(model) {}
Definition: quadruped_time.hpp:14
ActionModelQuadrupedTimeTpl()
Definition: quadruped_time.hxx:8
void set_heuristic_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_time.hxx:195
~ActionModelQuadrupedTimeTpl()
Definition: quadruped_time.hxx:54
virtual boost::shared_ptr< ActionDataAbstract > createData()
Definition: quadruped_time.hxx:166
void set_T_gait(const Scalar &T_gait_)
Definition: quadruped_time.hxx:236
const Scalar & get_T_gait() const
Definition: quadruped_time.hxx:231
const Scalar & get_dt_max() const
Definition: quadruped_time.hxx:263
const Scalar & get_dt_weight_cmd() const
Definition: quadruped_time.hxx:288
const Scalar & get_dt_bound_weight_cmd() const
Definition: quadruped_time.hxx:274
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_time.hxx:57
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::VectorXs > &S)
Definition: quadruped_time.hxx:311
void set_dt_weight_cmd(const Scalar &weight_)
Definition: quadruped_time.hxx:292
void set_dt_max(const Scalar &dt)
Definition: quadruped_time.hxx:267
void set_symmetry_term(const bool &sym_term)
Definition: quadruped_time.hxx:213
void set_centrifugal_term(const bool ¢_term)
Definition: quadruped_time.hxx:224
const Eigen::Matrix< Scalar, 8, 1 > & get_heuristic_weights() const
Definition: quadruped_time.hxx:191
const Scalar & get_dt_min() const
Definition: quadruped_time.hxx:255
crocoddyl::ActionModelAbstractTpl< Scalar > Base
Definition: quadruped_time.hpp:18
void set_dt_ref(const Scalar &dt)
Definition: quadruped_time.hxx:247
const Eigen::Matrix< Scalar, 7, 1 > & get_cost() const
Definition: quadruped_time.hxx:302
_Scalar Scalar
Definition: quadruped_time.hpp:16
crocoddyl::MathBaseTpl< Scalar > MathBase
Definition: quadruped_time.hpp:19
const bool & get_symmetry_term() const
Definition: quadruped_time.hxx:209
const bool & get_centrifugal_term() const
Definition: quadruped_time.hxx:220
const Scalar & get_dt_ref() const
Definition: quadruped_time.hxx:243
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_time.hxx:117
void set_state_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_time.hxx:180
void set_dt_min(const Scalar &dt)
Definition: quadruped_time.hxx:259
const Eigen::Matrix< Scalar, 12, 1 > & get_state_weights() const
Definition: quadruped_time.hxx:176
void set_dt_bound_weight_cmd(const Scalar &weight_)
Definition: quadruped_time.hxx:279
crocoddyl::ActionDataAbstractTpl< Scalar > ActionDataAbstract
Definition: quadruped_time.hpp:17
Definition: quadruped.hpp:11
ActionModelQuadrupedTimeTpl< double > ActionModelQuadrupedTime
Definition: quadruped_time.hpp:145
ActionDataQuadrupedTimeTpl< double > ActionDataQuadrupedTime
Definition: quadruped_time.hpp:146
Definition: quadruped_time.hpp:119
crocoddyl::MathBaseTpl< Scalar > MathBase
Definition: quadruped_time.hpp:123
ActionDataQuadrupedTimeTpl(Model< Scalar > *const model)
Definition: quadruped_time.hpp:137
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar Scalar
Definition: quadruped_time.hpp:122
crocoddyl::ActionDataAbstractTpl< Scalar > Base
Definition: quadruped_time.hpp:124