quadruped_step_time.hpp
Go to the documentation of this file.
1#ifndef __quadruped_walkgen_quadruped_step_time_hpp__
2#define __quadruped_walkgen_quadruped_step_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
11namespace quadruped_walkgen {
12template <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, 4, 1>& get_step_weights() const;
37 void set_step_weights(const typename MathBase::VectorXs& weights);
38
39 const typename Eigen::Matrix<Scalar, 8, 1>& get_heuristic_weights() const;
40 void set_heuristic_weights(const typename MathBase::VectorXs& weights);
41
42 // Update the model depending if the foot in contact with the ground
43 // or the new lever arms
44 void update_model(
45 const Eigen::Ref<const typename MathBase::MatrixXs>& l_feet,
46 const Eigen::Ref<const typename MathBase::MatrixXs>& velocity,
47 const Eigen::Ref<const typename MathBase::MatrixXs>& acceleration,
48 const Eigen::Ref<const typename MathBase::MatrixXs>& xref,
49 const Eigen::Ref<const typename MathBase::VectorXs>& S);
50
51 const bool& get_symmetry_term() const;
52 void set_symmetry_term(const bool& sym_term);
53
54 const bool& get_centrifugal_term() const;
55 void set_centrifugal_term(const bool& cent_term);
56
57 const Scalar& get_T_gait() const;
58 void set_T_gait(const Scalar& T_gait_);
59
60 const Scalar& get_nb_nodes() const;
61 void set_nb_nodes(const Scalar& nodes_);
62
63 const Scalar& get_vlim() const;
64 void set_vlim(const Scalar& vlim_);
65
66 const Scalar& get_speed_weight() const;
67 void set_speed_weight(const Scalar& weight_);
68
69 const bool& get_first_step() const;
70 void set_first_step(const bool& first);
71
72 // get cost
73 const typename Eigen::Matrix<Scalar, 7, 1>& get_cost() const;
74
75 protected:
76 using Base::has_control_limits_;
78 using Base::nr_;
79 using Base::nu_;
80 using Base::state_;
81 using Base::u_lb_;
82 using Base::u_ub_;
83 using Base::unone_;
84
85 private:
86 Scalar T_gait;
87 Scalar speed_weight;
88 Scalar nb_nodes;
89 Scalar vlim;
90 Scalar beta_lim;
91 int nb_alpha_;
92 bool centrifugal_term;
93 bool symmetry_term;
94 // indicates whether it t the 1st step, otherwise the cost function is much
95 // simpler (acc, speed = 0)
96 bool first_step;
97
98 typename Eigen::Matrix<Scalar, 12, 1> state_weights_;
99 typename Eigen::Matrix<Scalar, 4, 1> step_weights_;
100 typename Eigen::Matrix<Scalar, 8, 1> heuristicWeights;
101 typename MathBase::Matrix3s R_tmp;
102
103 // typename Eigen::Array<Scalar, 3, 1 > alpha ;
104 // typename Eigen::Array<Scalar, 3, 4 > alpha2 ;
105 // typename Eigen::Array<Scalar, 3, 3 > b_coeff ;
106 typename MathBase::ArrayXs alpha;
107 typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> alpha2;
108 typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> b_coeff;
109 typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> b_coeff_x0;
110 typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> b_coeff_y0;
111 typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> b_coeff_x1;
112 typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> b_coeff_y1;
113 typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> b_coeff_x2;
114 typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> b_coeff_y2;
115
116 typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> rub_max_first_x;
117 typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> rub_max_first_y;
118 typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> rub_max_first_2;
119 typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>
120 rub_max_first_bool;
121
122 // typename Eigen::Array<Scalar, 3, 12 > b_coeff2 ;
123 typename Eigen::Matrix<Scalar, 3, 4> lfeet;
124 // typename Eigen::Array<Scalar, 3, 4 > rub_max_first ;
125 // typename Eigen::Array<Scalar, 3, 2 > rub_max_first_2 ;
126 // typename Eigen::Array<Scalar, 3, 2 > rub_max_bool_first ;
127
128 typename Eigen::Matrix<Scalar, 8, 8> B;
129
130 typename MathBase::MatrixXs xref_;
131 typename MathBase::VectorXs S_; // Containing the flying feet
132 typename Eigen::Matrix<Scalar, 8, 1> pheuristic_;
133
134 // Compute heuristic inside update Model
135 // typename Eigen::Matrix<Scalar, 2 , 4 > pshoulder_0;
136 // typename Eigen::Matrix<Scalar, 2 , 4 > pshoulder_tmp;
137 // typename Eigen::Matrix<Scalar, 3 , 1 > pcentrifugal_tmp;
138 // typename Eigen::Matrix<Scalar, 3 , 1 > pcentrifugal_tmp_1;
139 // typename Eigen::Matrix<Scalar, 3 , 1 > pcentrifugal_tmp_2;
140
141 typename Eigen::Matrix<Scalar, 4, 1> rub_max_;
142 typename Eigen::Matrix<Scalar, 4, 1> rub_max_bool;
143
144 // Log cost
145 bool log_cost;
146 typename Eigen::Matrix<Scalar, 7, 1> cost_;
147};
148
149template <typename _Scalar>
151 : public crocoddyl::ActionDataAbstractTpl<_Scalar> {
152 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
153
154 typedef _Scalar Scalar;
155 typedef crocoddyl::MathBaseTpl<Scalar> MathBase;
156 typedef crocoddyl::ActionDataAbstractTpl<Scalar> Base;
157 using Base::cost;
158 using Base::Fu;
159 using Base::Fx;
160 using Base::Lu;
161 using Base::Luu;
162 using Base::Lx;
163 using Base::Lxu;
164 using Base::Lxx;
165 using Base::r;
166 using Base::xnext;
167
168 template <template <typename Scalar> class Model>
169 explicit ActionDataQuadrupedStepTimeTpl(Model<Scalar>* const model)
170 : crocoddyl::ActionDataAbstractTpl<Scalar>(model) {}
171};
172
173/* --- Details -------------------------------------------------------------- */
174/* --- Details -------------------------------------------------------------- */
175/* --- Details -------------------------------------------------------------- */
176
179
180} // namespace quadruped_walkgen
181
183
184#endif
Definition: quadruped_step_time.hpp:14
const Eigen::Matrix< Scalar, 7, 1 > & get_cost() const
Definition: quadruped_step_time.hxx:443
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_time.hxx:194
crocoddyl::ActionDataAbstractTpl< Scalar > ActionDataAbstract
Definition: quadruped_step_time.hpp:17
void set_state_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_step_time.hxx:327
void set_step_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_step_time.hxx:342
const Scalar & get_speed_weight() const
Definition: quadruped_step_time.hxx:405
const Scalar & get_vlim() const
Definition: quadruped_step_time.hxx:428
void set_heuristic_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_step_time.hxx:357
ActionModelQuadrupedStepTimeTpl()
Definition: quadruped_step_time.hxx:8
const bool & get_centrifugal_term() const
Definition: quadruped_step_time.hxx:378
void update_model(const Eigen::Ref< const typename MathBase::MatrixXs > &l_feet, const Eigen::Ref< const typename MathBase::MatrixXs > &velocity, const Eigen::Ref< const typename MathBase::MatrixXs > &acceleration, const Eigen::Ref< const typename MathBase::MatrixXs > &xref, const Eigen::Ref< const typename MathBase::VectorXs > &S)
Definition: quadruped_step_time.hxx:465
~ActionModelQuadrupedStepTimeTpl()
Definition: quadruped_step_time.hxx:100
void set_first_step(const bool &first)
Definition: quadruped_step_time.hxx:454
virtual boost::shared_ptr< ActionDataAbstract > createData()
Definition: quadruped_step_time.hxx:313
void set_symmetry_term(const bool &sym_term)
Definition: quadruped_step_time.hxx:371
const Scalar & get_T_gait() const
Definition: quadruped_step_time.hxx:390
const bool & get_symmetry_term() const
Definition: quadruped_step_time.hxx:367
const Eigen::Matrix< Scalar, 8, 1 > & get_heuristic_weights() const
Definition: quadruped_step_time.hxx:353
void set_vlim(const Scalar &vlim_)
Definition: quadruped_step_time.hxx:432
const Eigen::Matrix< Scalar, 12, 1 > & get_state_weights() const
Definition: quadruped_step_time.hxx:323
const Scalar & get_nb_nodes() const
Definition: quadruped_step_time.hxx:416
const Eigen::Matrix< Scalar, 4, 1 > & get_step_weights() const
Definition: quadruped_step_time.hxx:338
void set_nb_nodes(const Scalar &nodes_)
Definition: quadruped_step_time.hxx:420
crocoddyl::ActionModelAbstractTpl< Scalar > Base
Definition: quadruped_step_time.hpp:18
_Scalar Scalar
Definition: quadruped_step_time.hpp:16
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_time.hxx:103
const bool & get_first_step() const
Definition: quadruped_step_time.hxx:450
void set_centrifugal_term(const bool &cent_term)
Definition: quadruped_step_time.hxx:383
void set_T_gait(const Scalar &T_gait_)
Definition: quadruped_step_time.hxx:395
crocoddyl::MathBaseTpl< Scalar > MathBase
Definition: quadruped_step_time.hpp:19
void set_speed_weight(const Scalar &weight_)
Definition: quadruped_step_time.hxx:410
Definition: quadruped.hpp:11
ActionDataQuadrupedStepTimeTpl< double > ActionDataQuadrupedStepTime
Definition: quadruped_step_time.hpp:178
ActionModelQuadrupedStepTimeTpl< double > ActionModelQuadrupedStepTime
Definition: quadruped_step_time.hpp:177
Definition: quadruped_step_time.hpp:151
crocoddyl::MathBaseTpl< Scalar > MathBase
Definition: quadruped_step_time.hpp:155
crocoddyl::ActionDataAbstractTpl< Scalar > Base
Definition: quadruped_step_time.hpp:156
ActionDataQuadrupedStepTimeTpl(Model< Scalar > *const model)
Definition: quadruped_step_time.hpp:169
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar Scalar
Definition: quadruped_step_time.hpp:154