GCC Code Coverage Report

Directory: ./
File: include/quadruped-walkgen/quadruped_time.hpp
Date: 2024-12-02 00:24:10
Exec Total Coverage
Lines: 0 2 0.0%
Branches: 0 0 -%
Line Branch Exec Source
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>
13 class ActionModelQuadrupedTimeTpl
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
21 ActionModelQuadrupedTimeTpl();
22 ~ActionModelQuadrupedTimeTpl();
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_; //!< Indicates whether any of the control
75 //!< limits
76 using Base::nr_; //!< Dimension of the cost residual
77 using Base::nu_; //!< Control dimension
78 using Base::state_; //!< Model of the state
79 using Base::u_lb_; //!< Lower control limits
80 using Base::u_ub_; //!< Upper control limits
81 using Base::unone_; //!< Neutral state
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>
118 struct ActionDataQuadrupedTimeTpl
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
145 typedef ActionModelQuadrupedTimeTpl<double> ActionModelQuadrupedTime;
146 typedef ActionDataQuadrupedTimeTpl<double> ActionDataQuadrupedTime;
147
148 } // namespace quadruped_walkgen
149
150 #include "quadruped_time.hxx"
151
152 #endif
153