1 #ifndef __quadruped_walkgen_quadruped_augmented_time_hpp__
2 #define __quadruped_walkgen_quadruped_augmented_time_hpp__
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"
12 template <
typename _Scalar>
14 :
public crocoddyl::ActionModelAbstractTpl<_Scalar> {
18 typedef crocoddyl::ActionModelAbstractTpl<Scalar>
Base;
19 typedef crocoddyl::MathBaseTpl<Scalar>
MathBase;
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();
63 const typename Eigen::Matrix<Scalar, 3, 3>&
get_gI()
const;
64 void set_gI(
const typename MathBase::Matrix3s& inertia_matrix);
102 void update_model(
const Eigen::Ref<const typename MathBase::MatrixXs>& l_feet,
103 const Eigen::Ref<const typename MathBase::MatrixXs>& l_stop,
104 const Eigen::Ref<const typename MathBase::MatrixXs>& xref,
105 const Eigen::Ref<const typename MathBase::MatrixXs>& S);
108 const typename Eigen::Matrix<Scalar, 12, 12>&
get_A()
const;
109 const typename Eigen::Matrix<Scalar, 12, 12>&
get_B()
const;
112 const typename Eigen::Matrix<Scalar, 7, 1>&
get_cost()
const;
115 using Base::has_control_limits_;
133 bool centrifugal_term;
136 bool relative_forces;
137 typename Eigen::Matrix<Scalar, 12, 1> uref_;
139 typename Eigen::Matrix<Scalar, 12, 1> force_weights_;
140 typename Eigen::Matrix<Scalar, 12, 1> state_weights_;
141 typename Eigen::Matrix<Scalar, 8, 1> heuristicWeights;
142 typename Eigen::Matrix<Scalar, 8, 1> last_position_weights_;
144 typename Eigen::Matrix<Scalar, 12, 12> A;
145 typename Eigen::Matrix<Scalar, 12, 12> B;
146 typename Eigen::Matrix<Scalar, 12, 1> g;
147 typename Eigen::Matrix<Scalar, 3, 3> R;
148 typename MathBase::Matrix3s R_tmp;
149 typename Eigen::Matrix<Scalar, 3, 3> gI;
151 typename Eigen::Matrix<Scalar, 3, 4> lever_arms;
152 typename MathBase::Vector3s lever_tmp;
153 typename MathBase::MatrixXs xref_;
155 typename Eigen::Matrix<Scalar, 8, 1> pshoulder_;
156 typename Eigen::Matrix<Scalar, 8, 1> pheuristic_;
157 typename Eigen::Matrix<Scalar, 2, 4> pshoulder_0;
158 typename Eigen::Matrix<Scalar, 2, 4> pshoulder_tmp;
160 typename Eigen::Matrix<Scalar, 3, 1> pcentrifugal_tmp;
161 typename Eigen::Matrix<Scalar, 3, 1> pcentrifugal_tmp_1;
162 typename Eigen::Matrix<Scalar, 3, 1> pcentrifugal_tmp_2;
164 typename Eigen::Matrix<Scalar, 8, 1> pref_;
166 typename Eigen::Matrix<Scalar, 24, 1> ub;
168 typename Eigen::Matrix<Scalar, 24, 1> Fa_x_u;
169 typename Eigen::Matrix<Scalar, 24, 1> rub_max_;
170 typename Eigen::Matrix<Scalar, 24, 24> Arr;
171 typename Eigen::Matrix<Scalar, 6, 1> r;
172 typename Eigen::Matrix<Scalar, 4, 1> gait;
173 typename Eigen::Matrix<Scalar, 8, 1> gait_double;
175 typename Eigen::Matrix<Scalar, 3, 1> base_vector_x;
176 typename Eigen::Matrix<Scalar, 3, 1> base_vector_y;
177 typename Eigen::Matrix<Scalar, 3, 1> base_vector_z;
178 typename Eigen::Matrix<Scalar, 3, 1> forces_3d;
180 typename Eigen::Matrix<Scalar, 1, 1> dt_min_;
181 typename Eigen::Matrix<Scalar, 1, 1> dt_max_;
183 typename Eigen::Matrix<Scalar, 2, 1> rub_max_dt;
184 typename Eigen::Matrix<Scalar, 2, 1> rub_max_dt_bool;
188 typename Eigen::Matrix<Scalar, 7, 1> cost_;
191 typename Eigen::Matrix<Scalar, 3, 4> psh;
192 typename Eigen::Matrix<Scalar, 4, 1> sh_ub_max_;
197 template <
typename _Scalar>
199 :
public crocoddyl::ActionDataAbstractTpl<_Scalar> {
200 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
204 typedef crocoddyl::ActionDataAbstractTpl<Scalar>
Base;
216 template <
template <
typename Scalar>
class Model>
218 : crocoddyl::ActionDataAbstractTpl<
Scalar>(model) {}
225 typedef ActionModelQuadrupedAugmentedTimeTpl<double>
Definition: quadruped_augmented_time.hpp:14
ActionModelQuadrupedAugmentedTimeTpl()
Definition: quadruped_augmented_time.hxx:9
const Eigen::Matrix< Scalar, 8, 1 > & get_shoulder_position() const
Definition: quadruped_augmented_time.hxx:463
void set_symmetry_term(const bool &sym_term)
Definition: quadruped_augmented_time.hxx:622
const Eigen::Matrix< Scalar, 7, 1 > & get_cost() const
Definition: quadruped_augmented_time.hxx:671
void set_dt_min(const Scalar &dt)
Definition: quadruped_augmented_time.hxx:527
const Eigen::Matrix< Scalar, 12, 12 > & get_A() const
Definition: quadruped_augmented_time.hxx:680
const Scalar & get_T_gait() const
Definition: quadruped_augmented_time.hxx:641
void set_state_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_augmented_time.hxx:437
void set_shoulder_contact_weight(const Scalar &weight)
Definition: quadruped_augmented_time.hxx:607
void set_heuristic_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_augmented_time.hxx:452
const Eigen::Matrix< Scalar, 12, 1 > & get_state_weights() const
Definition: quadruped_augmented_time.hxx:433
const bool & get_relative_forces() const
Definition: quadruped_augmented_time.hxx:692
const Scalar & get_min_fz_contact() const
Definition: quadruped_augmented_time.hxx:561
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_augmented_time.hxx:231
const bool & get_symmetry_term() const
Definition: quadruped_augmented_time.hxx:617
void set_stop_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_augmented_time.hxx:482
void set_max_fz_contact(const Scalar &max_fz_)
Definition: quadruped_augmented_time.hxx:580
const Scalar & get_shoulder_hlim() const
Definition: quadruped_augmented_time.hxx:590
void set_centrifugal_term(const bool ¢_term)
Definition: quadruped_augmented_time.hxx:634
void set_T_gait(const Scalar &T_gait_)
Definition: quadruped_augmented_time.hxx:646
void set_shoulder_hlim(const Scalar &hlim)
Definition: quadruped_augmented_time.hxx:595
const Eigen::Matrix< Scalar, 12, 12 > & get_B() const
Definition: quadruped_augmented_time.hxx:685
void set_mass(const Scalar &m)
Definition: quadruped_augmented_time.hxx:517
~ActionModelQuadrupedAugmentedTimeTpl()
Definition: quadruped_augmented_time.hxx:98
void set_relative_forces(const bool &rel_forces)
Definition: quadruped_augmented_time.hxx:697
const Scalar & get_dt_max() const
Definition: quadruped_augmented_time.hxx:534
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_augmented_time.hxx:101
crocoddyl::ActionModelAbstractTpl< Scalar > Base
Definition: quadruped_augmented_time.hpp:18
void update_model(const Eigen::Ref< const typename MathBase::MatrixXs > &l_feet, const Eigen::Ref< const typename MathBase::MatrixXs > &l_stop, const Eigen::Ref< const typename MathBase::MatrixXs > &xref, const Eigen::Ref< const typename MathBase::MatrixXs > &S)
Definition: quadruped_augmented_time.hxx:715
crocoddyl::MathBaseTpl< Scalar > MathBase
Definition: quadruped_augmented_time.hpp:19
void set_dt_ref(const Scalar &dt)
const Eigen::Matrix< Scalar, 8, 1 > & get_stop_weights() const
Definition: quadruped_augmented_time.hxx:478
void set_dt_max(const Scalar &dt)
Definition: quadruped_augmented_time.hxx:538
const Scalar & get_dt_weight() const
const Scalar & get_mu() const
Definition: quadruped_augmented_time.hxx:503
const bool & get_centrifugal_term() const
Definition: quadruped_augmented_time.hxx:629
const Eigen::Matrix< Scalar, 8, 1 > & get_heuristic_weights() const
Definition: quadruped_augmented_time.hxx:448
void set_dt_bound_weight(const Scalar &weight_)
Definition: quadruped_augmented_time.hxx:662
const Scalar & get_dt_min() const
Definition: quadruped_augmented_time.hxx:523
const Scalar & get_friction_weight() const
Definition: quadruped_augmented_time.hxx:493
virtual boost::shared_ptr< ActionDataAbstract > createData()
Definition: quadruped_augmented_time.hxx:408
const Scalar & get_dt_bound_weight() const
Definition: quadruped_augmented_time.hxx:657
void set_friction_weight(const Scalar &weight)
Definition: quadruped_augmented_time.hxx:497
const Scalar & get_mass() const
Definition: quadruped_augmented_time.hxx:513
const Eigen::Matrix< Scalar, 3, 3 > & get_gI() const
Definition: quadruped_augmented_time.hxx:546
void set_force_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_augmented_time.hxx:422
_Scalar Scalar
Definition: quadruped_augmented_time.hpp:16
void set_mu(const Scalar &mu_coeff)
Definition: quadruped_augmented_time.hxx:507
void set_shoulder_position(const typename MathBase::VectorXs &weights)
Definition: quadruped_augmented_time.hxx:467
void set_gI(const typename MathBase::Matrix3s &inertia_matrix)
Definition: quadruped_augmented_time.hxx:550
void set_dt_weight(const Scalar &weight_)
void set_min_fz_contact(const Scalar &min_fz)
Definition: quadruped_augmented_time.hxx:567
const Scalar & get_shoulder_contact_weight() const
Definition: quadruped_augmented_time.hxx:603
const Scalar & get_max_fz_contact() const
Definition: quadruped_augmented_time.hxx:574
const Eigen::Matrix< Scalar, 12, 1 > & get_force_weights() const
Definition: quadruped_augmented_time.hxx:418
const Scalar & get_dt_ref() const
crocoddyl::ActionDataAbstractTpl< Scalar > ActionDataAbstract
Definition: quadruped_augmented_time.hpp:17
Definition: quadruped.hpp:11
ActionModelQuadrupedAugmentedTimeTpl< double > ActionModelQuadrupedAugmentedTime
Definition: quadruped_augmented_time.hpp:226
ActionDataQuadrupedAugmentedTimeTpl< double > ActionDataQuadrupedAugmentedTime
Definition: quadruped_augmented_time.hpp:228
Definition: quadruped_augmented_time.hpp:199
ActionDataQuadrupedAugmentedTimeTpl(Model< Scalar > *const model)
Definition: quadruped_augmented_time.hpp:217
crocoddyl::ActionDataAbstractTpl< Scalar > Base
Definition: quadruped_augmented_time.hpp:204
crocoddyl::MathBaseTpl< Scalar > MathBase
Definition: quadruped_augmented_time.hpp:203
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar Scalar
Definition: quadruped_augmented_time.hpp:202