quadruped_time.hpp
Go to the documentation of this file.
1 #ifndef __quadruped_walkgen_quadruped_time_hpp__
2 #define __quadruped_walkgen_quadruped_time_hpp__
3 #include <stdexcept>
4 
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"
10 
11 namespace quadruped_walkgen {
12 template <typename _Scalar>
14  : public crocoddyl::ActionModelAbstractTpl<_Scalar> {
15  public:
16  typedef _Scalar Scalar;
17  typedef crocoddyl::ActionDataAbstractTpl<Scalar> ActionDataAbstract;
18  typedef crocoddyl::ActionModelAbstractTpl<Scalar> Base;
19  typedef crocoddyl::MathBaseTpl<Scalar> MathBase;
20 
23 
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();
31 
32  // Get and Set weights vectors : state , force & friction cone :
33  const typename Eigen::Matrix<Scalar, 12, 1>& get_state_weights() const;
34  void set_state_weights(const typename MathBase::VectorXs& weights);
35 
36  const typename Eigen::Matrix<Scalar, 8, 1>& get_heuristic_weights() const;
37  void set_heuristic_weights(const typename MathBase::VectorXs& weights);
38 
39  // Update the model depending if the foot in contact with the ground
40  // or the new lever arms
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);
44 
45  const bool& get_symmetry_term() const;
46  void set_symmetry_term(const bool& sym_term);
47 
48  const bool& get_centrifugal_term() const;
49  void set_centrifugal_term(const bool& cent_term);
50 
51  const Scalar& get_T_gait() const;
52  void set_T_gait(const Scalar& T_gait_);
53 
54  const Scalar& get_dt_ref() const;
55  void set_dt_ref(const Scalar& dt);
56 
57  const Scalar& get_dt_min() const;
58  void set_dt_min(const Scalar& dt);
59 
60  const Scalar& get_dt_max() const;
61  void set_dt_max(const Scalar& dt);
62 
63  // Command weights
64  const Scalar& get_dt_weight_cmd() const;
65  void set_dt_weight_cmd(const Scalar& weight_);
66 
67  const Scalar& get_dt_bound_weight_cmd() const;
68  void set_dt_bound_weight_cmd(const Scalar& weight_);
69 
70  // get cost
71  const typename Eigen::Matrix<Scalar, 7, 1>& get_cost() const;
72 
73  protected:
74  using Base::has_control_limits_;
75  using Base::nr_;
77  using Base::nu_;
78  using Base::state_;
79  using Base::u_lb_;
80  using Base::u_ub_;
81  using Base::unone_;
82 
83  private:
84  Scalar T_gait;
85  Scalar dt_weight_cmd;
86  Scalar dt_bound_weight_cmd;
87  bool centrifugal_term;
88  bool symmetry_term;
89 
90  // Log cost
91  bool log_cost;
92  typename Eigen::Matrix<Scalar, 7, 1> cost_;
93 
94  typename Eigen::Matrix<Scalar, 12, 1> state_weights_;
95  typename Eigen::Matrix<Scalar, 8, 1> heuristic_weights_;
96 
97  typename MathBase::Matrix3s R_tmp;
98 
99  typename MathBase::MatrixXs xref_;
100  typename Eigen::Matrix<Scalar, 8, 1> pheuristic_;
101  typename Eigen::Matrix<Scalar, 8, 1> gait_double_;
102  // typename Eigen::Matrix<Scalar, 2 , 4 > pshoulder_0;
103  // typename Eigen::Matrix<Scalar, 2 , 4 > pshoulder_tmp;
104 
105  // typename Eigen::Matrix<Scalar, 3 , 1 > pcentrifugal_tmp;
106  // typename Eigen::Matrix<Scalar, 3 , 1 > pcentrifugal_tmp_1;
107  // typename Eigen::Matrix<Scalar, 3 , 1 > pcentrifugal_tmp_2;
108 
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_;
112 
113  typename Eigen::Matrix<Scalar, 2, 1> rub_max_;
114  typename Eigen::Matrix<Scalar, 2, 1> rub_max_bool;
115 };
116 
117 template <typename _Scalar>
119  : public crocoddyl::ActionDataAbstractTpl<_Scalar> {
120  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
121 
122  typedef _Scalar Scalar;
123  typedef crocoddyl::MathBaseTpl<Scalar> MathBase;
124  typedef crocoddyl::ActionDataAbstractTpl<Scalar> Base;
125  using Base::cost;
126  using Base::Fu;
127  using Base::Fx;
128  using Base::Lu;
129  using Base::Luu;
130  using Base::Lx;
131  using Base::Lxu;
132  using Base::Lxx;
133  using Base::r;
134  using Base::xnext;
135 
136  template <template <typename Scalar> class Model>
137  explicit ActionDataQuadrupedTimeTpl(Model<Scalar>* const model)
138  : crocoddyl::ActionDataAbstractTpl<Scalar>(model) {}
139 };
140 
141 /* --- Details -------------------------------------------------------------- */
142 /* --- Details -------------------------------------------------------------- */
143 /* --- Details -------------------------------------------------------------- */
144 
147 
148 } // namespace quadruped_walkgen
149 
150 #include "quadruped_time.hxx"
151 
152 #endif
quadruped_time.hxx
quadruped_walkgen::ActionModelQuadrupedTimeTpl::get_dt_max
const Scalar & get_dt_max() const
Definition: quadruped_time.hxx:263
quadruped_walkgen::ActionDataQuadrupedTimeTpl::Base
crocoddyl::ActionDataAbstractTpl< Scalar > Base
Definition: quadruped_time.hpp:124
quadruped_walkgen::ActionModelQuadrupedTimeTpl::get_centrifugal_term
const bool & get_centrifugal_term() const
Definition: quadruped_time.hxx:220
quadruped_walkgen::ActionModelQuadrupedTimeTpl::get_dt_weight_cmd
const Scalar & get_dt_weight_cmd() const
Definition: quadruped_time.hxx:288
quadruped_walkgen::ActionModelQuadrupedTimeTpl::set_dt_weight_cmd
void set_dt_weight_cmd(const Scalar &weight_)
Definition: quadruped_time.hxx:292
quadruped_walkgen
Definition: quadruped.hpp:11
quadruped_walkgen::ActionModelQuadrupedTimeTpl::set_symmetry_term
void set_symmetry_term(const bool &sym_term)
Definition: quadruped_time.hxx:213
quadruped_walkgen::ActionModelQuadrupedTimeTpl::get_cost
const Eigen::Matrix< Scalar, 7, 1 > & get_cost() const
Definition: quadruped_time.hxx:302
quadruped_walkgen::ActionDataQuadrupedTime
ActionDataQuadrupedTimeTpl< double > ActionDataQuadrupedTime
Definition: quadruped_time.hpp:146
quadruped_walkgen::ActionDataQuadrupedTimeTpl
Definition: quadruped_time.hpp:118
quadruped_walkgen::ActionModelQuadrupedTimeTpl::ActionModelQuadrupedTimeTpl
ActionModelQuadrupedTimeTpl()
Definition: quadruped_time.hxx:8
quadruped_walkgen::ActionModelQuadrupedTimeTpl::get_dt_ref
const Scalar & get_dt_ref() const
Definition: quadruped_time.hxx:243
quadruped_walkgen::ActionModelQuadrupedTimeTpl::get_T_gait
const Scalar & get_T_gait() const
Definition: quadruped_time.hxx:231
quadruped_walkgen::ActionModelQuadrupedTimeTpl::set_T_gait
void set_T_gait(const Scalar &T_gait_)
Definition: quadruped_time.hxx:236
quadruped_walkgen::ActionDataQuadrupedTimeTpl::Scalar
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar Scalar
Definition: quadruped_time.hpp:122
quadruped_walkgen::ActionModelQuadrupedTime
ActionModelQuadrupedTimeTpl< double > ActionModelQuadrupedTime
Definition: quadruped_time.hpp:145
quadruped_walkgen::ActionModelQuadrupedTimeTpl::~ActionModelQuadrupedTimeTpl
~ActionModelQuadrupedTimeTpl()
Definition: quadruped_time.hxx:54
quadruped_walkgen::ActionModelQuadrupedTimeTpl::Base
crocoddyl::ActionModelAbstractTpl< Scalar > Base
Definition: quadruped_time.hpp:18
quadruped_walkgen::ActionModelQuadrupedTimeTpl::calcDiff
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
quadruped_walkgen::ActionModelQuadrupedTimeTpl::calc
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
quadruped_walkgen::ActionModelQuadrupedTimeTpl::update_model
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
quadruped_walkgen::ActionModelQuadrupedTimeTpl::set_centrifugal_term
void set_centrifugal_term(const bool &cent_term)
Definition: quadruped_time.hxx:224
quadruped_walkgen::ActionModelQuadrupedTimeTpl::ActionDataAbstract
crocoddyl::ActionDataAbstractTpl< Scalar > ActionDataAbstract
Definition: quadruped_time.hpp:17
quadruped_walkgen::ActionModelQuadrupedTimeTpl::MathBase
crocoddyl::MathBaseTpl< Scalar > MathBase
Definition: quadruped_time.hpp:19
quadruped_walkgen::ActionModelQuadrupedTimeTpl::get_dt_bound_weight_cmd
const Scalar & get_dt_bound_weight_cmd() const
Definition: quadruped_time.hxx:274
quadruped_walkgen::ActionModelQuadrupedTimeTpl::get_heuristic_weights
const Eigen::Matrix< Scalar, 8, 1 > & get_heuristic_weights() const
Definition: quadruped_time.hxx:191
quadruped_walkgen::ActionModelQuadrupedTimeTpl::get_dt_min
const Scalar & get_dt_min() const
Definition: quadruped_time.hxx:255
quadruped_walkgen::ActionDataQuadrupedTimeTpl::MathBase
crocoddyl::MathBaseTpl< Scalar > MathBase
Definition: quadruped_time.hpp:123
quadruped_walkgen::ActionModelQuadrupedTimeTpl::set_state_weights
void set_state_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_time.hxx:180
quadruped_walkgen::ActionModelQuadrupedTimeTpl::set_dt_min
void set_dt_min(const Scalar &dt)
Definition: quadruped_time.hxx:259
quadruped_walkgen::ActionModelQuadrupedTimeTpl::get_state_weights
const Eigen::Matrix< Scalar, 12, 1 > & get_state_weights() const
Definition: quadruped_time.hxx:176
quadruped_walkgen::ActionModelQuadrupedTimeTpl
Definition: quadruped_time.hpp:13
quadruped_walkgen::ActionModelQuadrupedTimeTpl::set_dt_max
void set_dt_max(const Scalar &dt)
Definition: quadruped_time.hxx:267
quadruped_walkgen::ActionModelQuadrupedTimeTpl::createData
virtual boost::shared_ptr< ActionDataAbstract > createData()
Definition: quadruped_time.hxx:166
quadruped_walkgen::ActionModelQuadrupedTimeTpl::set_heuristic_weights
void set_heuristic_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_time.hxx:195
quadruped_walkgen::ActionDataQuadrupedTimeTpl::ActionDataQuadrupedTimeTpl
ActionDataQuadrupedTimeTpl(Model< Scalar > *const model)
Definition: quadruped_time.hpp:137
quadruped_walkgen::ActionModelQuadrupedTimeTpl::get_symmetry_term
const bool & get_symmetry_term() const
Definition: quadruped_time.hxx:209
quadruped_walkgen::ActionModelQuadrupedTimeTpl::set_dt_bound_weight_cmd
void set_dt_bound_weight_cmd(const Scalar &weight_)
Definition: quadruped_time.hxx:279
quadruped_walkgen::ActionModelQuadrupedTimeTpl::set_dt_ref
void set_dt_ref(const Scalar &dt)
Definition: quadruped_time.hxx:247
quadruped_walkgen::ActionModelQuadrupedTimeTpl::Scalar
_Scalar Scalar
Definition: quadruped_time.hpp:16