Loading...
Searching...
No Matches
quadruped_step_period.hpp
Go to the documentation of this file.
1#ifndef __quadruped_walkgen_quadruped_step_period_hpp__
2#define __quadruped_walkgen_quadruped_step_period_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_shoulder_weights() const;
40 void set_shoulder_weights(const typename MathBase::VectorXs& weights);
41
42 const typename Eigen::Matrix<Scalar, 8, 1>& get_shoulder_position() const;
43 void set_shoulder_position(const typename MathBase::VectorXs& weights);
44
45 // Update the model depending if the foot in contact with the ground
46 // or the new lever arms
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);
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_dt_ref() const;
61 void set_dt_ref(const Scalar& dt);
62
63 const Scalar& get_dt_min() const;
64 void set_dt_min(const Scalar& dt);
65
66 const Scalar& get_dt_max() const;
67 void set_dt_max(const Scalar& dt);
68
69 const Scalar& get_dt_weight() const;
70 void set_dt_weight(const Scalar& weight_);
71
72 const Scalar& get_dt_bound_weight() const;
73 void set_dt_bound_weight(const Scalar& weight_);
74
75 const Scalar& get_nb_nodes() const;
76 void set_nb_nodes(const Scalar& nodes_);
77
78 const Scalar& get_vlim() const;
79 void set_vlim(const Scalar& vlim_);
80
81 const Scalar& get_speed_weight() const;
82 void set_speed_weight(const Scalar& weight_);
83
84 protected:
85 using Base::has_control_limits_;
87 using Base::nr_;
88 using Base::nu_;
89 using Base::state_;
90 using Base::u_lb_;
91 using Base::u_ub_;
92 using Base::unone_;
93
94 private:
95 Scalar T_gait;
96 Scalar dt_weight_;
97 Scalar dt_bound_weight;
98 Scalar speed_weight;
99 bool centrifugal_term;
100 bool symmetry_term;
101
102 Scalar nb_nodes;
103 Scalar vlim;
104 Scalar beta_lim;
105
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;
110
111 typename Eigen::Matrix<Scalar, 8, 4> B;
112
113 typename MathBase::MatrixXs xref_;
114
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;
118
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;
122
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_;
126
127 typename Eigen::Matrix<Scalar, 4, 1> rub_max_;
128 typename Eigen::Matrix<Scalar, 4, 1> rub_max_bool;
129};
130
131template <typename _Scalar>
133 : public crocoddyl::ActionDataAbstractTpl<_Scalar> {
134 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
135
136 typedef _Scalar Scalar;
137 typedef crocoddyl::MathBaseTpl<Scalar> MathBase;
138 typedef crocoddyl::ActionDataAbstractTpl<Scalar> Base;
139 using Base::cost;
140 using Base::Fu;
141 using Base::Fx;
142 using Base::Lu;
143 using Base::Luu;
144 using Base::Lx;
145 using Base::Lxu;
146 using Base::Lxx;
147 using Base::r;
148 using Base::xnext;
149
150 template <template <typename Scalar> class Model>
151 explicit ActionDataQuadrupedStepPeriodTpl(Model<Scalar>* const model)
152 : crocoddyl::ActionDataAbstractTpl<Scalar>(model) {}
153};
154
155/* --- Details -------------------------------------------------------------- */
156/* --- Details -------------------------------------------------------------- */
157/* --- Details -------------------------------------------------------------- */
158
159typedef ActionModelQuadrupedStepPeriodTpl<double>
162
163} // namespace quadruped_walkgen
164
166
167#endif
Definition quadruped_step_period.hpp:14
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
crocoddyl::ActionModelAbstractTpl< Scalar > Base
Definition quadruped_step_period.hpp:18
~ActionModelQuadrupedStepPeriodTpl()
Definition quadruped_step_period.hxx:49
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_weight() const
Definition quadruped_step_period.hxx:313
const Scalar & get_speed_weight() const
Definition quadruped_step_period.hxx:325
const Scalar & get_T_gait() const
Definition quadruped_step_period.hxx:301
_Scalar Scalar
Definition quadruped_step_period.hpp:16
const Eigen::Matrix< Scalar, 8, 1 > & get_shoulder_weights() const
Definition quadruped_step_period.hxx:248
const Eigen::Matrix< Scalar, 12, 1 > & get_state_weights() const
Definition quadruped_step_period.hxx:218
void set_centrifugal_term(const bool &cent_term)
Definition quadruped_step_period.hxx:294
void set_nb_nodes(const Scalar &nodes_)
Definition quadruped_step_period.hxx:356
const Scalar & get_dt_min() const
Definition quadruped_step_period.hxx:388
const bool & get_symmetry_term() const
Definition quadruped_step_period.hxx:277
void set_dt_min(const Scalar &dt)
Definition quadruped_step_period.hxx:392
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
const Scalar & get_nb_nodes() const
Definition quadruped_step_period.hxx:351
crocoddyl::MathBaseTpl< Scalar > MathBase
Definition quadruped_step_period.hpp:19
void set_dt_max(const Scalar &dt)
Definition quadruped_step_period.hxx:402
void set_state_weights(const typename MathBase::VectorXs &weights)
Definition quadruped_step_period.hxx:222
void set_shoulder_weights(const typename MathBase::VectorXs &weights)
Definition quadruped_step_period.hxx:252
const Eigen::Matrix< Scalar, 4, 1 > & get_step_weights() const
Definition quadruped_step_period.hxx:233
void set_dt_weight(const Scalar &weight_)
Definition quadruped_step_period.hxx:318
ActionModelQuadrupedStepPeriodTpl()
Definition quadruped_step_period.hxx:8
virtual boost::shared_ptr< ActionDataAbstract > createData()
Definition quadruped_step_period.hxx:208
void set_T_gait(const Scalar &T_gait_)
Definition quadruped_step_period.hxx:306
const Scalar & get_dt_ref() const
Definition quadruped_step_period.hxx:378
void set_symmetry_term(const bool &sym_term)
Definition quadruped_step_period.hxx:282
crocoddyl::ActionDataAbstractTpl< Scalar > ActionDataAbstract
Definition quadruped_step_period.hpp:17
const Scalar & get_vlim() const
Definition quadruped_step_period.hxx:365
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
void set_dt_bound_weight(const Scalar &weight_)
Definition quadruped_step_period.hxx:344
void set_speed_weight(const Scalar &weight_)
Definition quadruped_step_period.hxx:331
void set_shoulder_position(const typename MathBase::VectorXs &weights)
Definition quadruped_step_period.hxx:267
const Eigen::Matrix< Scalar, 8, 1 > & get_shoulder_position() const
Definition quadruped_step_period.hxx:263
void set_vlim(const Scalar &vlim_)
Definition quadruped_step_period.hxx:370
const Scalar & get_dt_bound_weight() const
Definition quadruped_step_period.hxx:338
void set_dt_ref(const Scalar &dt)
Definition quadruped_step_period.hxx:382
void set_step_weights(const typename MathBase::VectorXs &weights)
Definition quadruped_step_period.hxx:237
Definition quadruped.hpp:11
ActionModelQuadrupedStepPeriodTpl< double > ActionModelQuadrupedStepPeriod
Definition quadruped_step_period.hpp:160
ActionDataQuadrupedStepPeriodTpl< double > ActionDataQuadrupedStepPeriod
Definition quadruped_step_period.hpp:161
Definition quadruped_step_period.hpp:133
crocoddyl::MathBaseTpl< Scalar > MathBase
Definition quadruped_step_period.hpp:137
crocoddyl::ActionDataAbstractTpl< Scalar > Base
Definition quadruped_step_period.hpp:138
ActionDataQuadrupedStepPeriodTpl(Model< Scalar > *const model)
Definition quadruped_step_period.hpp:151
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar Scalar
Definition quadruped_step_period.hpp:136