Loading...
Searching...
No Matches
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
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, 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_;
76 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
117template <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
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 &cent_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