1 #ifndef __quadruped_walkgen_quadruped_augmented_hpp__ 2 #define __quadruped_walkgen_quadruped_augmented_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;
22 typename Eigen::Matrix<Scalar, 3, 1> offset_CoM =
23 Eigen::Matrix<Scalar, 3, 1>::Zero());
26 virtual void calc(
const boost::shared_ptr<ActionDataAbstract>& data,
27 const Eigen::Ref<const typename MathBase::VectorXs>& x,
28 const Eigen::Ref<const typename MathBase::VectorXs>& u);
29 virtual void calcDiff(
const boost::shared_ptr<ActionDataAbstract>& data,
30 const Eigen::Ref<const typename MathBase::VectorXs>& x,
31 const Eigen::Ref<const typename MathBase::VectorXs>& u);
32 virtual boost::shared_ptr<ActionDataAbstract>
createData();
59 const typename Eigen::Matrix<Scalar, 3, 3>&
get_gI()
const;
60 void set_gI(
const typename MathBase::Matrix3s& inertia_matrix);
84 const typename Eigen::Matrix<Scalar, 4, 1>& weight);
91 void update_model(
const Eigen::Ref<const typename MathBase::MatrixXs>& l_feet,
92 const Eigen::Ref<const typename MathBase::MatrixXs>& l_stop,
93 const Eigen::Ref<const typename MathBase::MatrixXs>& xref,
94 const Eigen::Ref<const typename MathBase::MatrixXs>& S);
97 const typename Eigen::Matrix<Scalar, 12, 12>&
get_A()
const;
98 const typename Eigen::Matrix<Scalar, 12, 12>&
get_B()
const;
104 using Base::has_control_limits_;
121 bool centrifugal_term;
123 bool relative_forces;
127 bool shoulder_reference_position;
129 typename Eigen::Matrix<Scalar, 12, 1> uref_;
131 typename Eigen::Matrix<Scalar, 12, 1> force_weights_;
132 typename Eigen::Matrix<Scalar, 12, 1> state_weights_;
133 typename Eigen::Matrix<Scalar, 8, 1> heuristic_weights_;
134 typename Eigen::Matrix<Scalar, 8, 1> stop_weights_;
136 typename Eigen::Matrix<Scalar, 12, 12> A;
137 typename Eigen::Matrix<Scalar, 12, 12> B;
138 typename Eigen::Matrix<Scalar, 12, 1> g;
139 typename Eigen::Matrix<Scalar, 3, 3> R;
140 typename MathBase::Matrix3s R_tmp;
141 typename Eigen::Matrix<Scalar, 3, 3> gI;
143 typename Eigen::Matrix<Scalar, 3, 4> lever_arms;
144 typename MathBase::Vector3s lever_tmp;
145 typename MathBase::MatrixXs xref_;
148 typename Eigen::Matrix<Scalar, 2, 4> pshoulder_0;
155 typename Eigen::Matrix<Scalar, 8, 1> pstop_;
156 typename Eigen::Matrix<Scalar, 8, 1> pheuristic_;
158 typename Eigen::Matrix<Scalar, 24, 1> ub;
160 typename Eigen::Matrix<Scalar, 24, 1> Fa_x_u;
161 typename Eigen::Matrix<Scalar, 20, 1> rub_;
162 typename Eigen::Matrix<Scalar, 24, 1> rub_max_;
163 typename Eigen::Matrix<Scalar, 24, 24> Arr;
164 typename Eigen::Matrix<Scalar, 6, 1> r;
165 typename Eigen::Matrix<Scalar, 4, 1> gait;
166 typename Eigen::Matrix<Scalar, 8, 1> gait_double;
168 typename Eigen::Matrix<Scalar, 3, 1> base_vector_x;
169 typename Eigen::Matrix<Scalar, 3, 1> base_vector_y;
170 typename Eigen::Matrix<Scalar, 3, 1> base_vector_z;
171 typename Eigen::Matrix<Scalar, 3, 1> forces_3d;
174 typename Eigen::Matrix<Scalar, 3, 4> psh;
175 typename Eigen::Matrix<Scalar, 4, 1> sh_ub_max_;
176 typename Eigen::Matrix<Scalar, 4, 1> sh_weight;
177 typename Eigen::Matrix<Scalar, 3, 1> offset_com;
181 template <
typename _Scalar>
183 :
public crocoddyl::ActionDataAbstractTpl<_Scalar> {
184 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
188 typedef crocoddyl::ActionDataAbstractTpl<Scalar>
Base;
200 template <
template <
typename Scalar>
class Model>
202 : crocoddyl::ActionDataAbstractTpl<
Scalar>(model) {}
crocoddyl::MathBaseTpl< Scalar > MathBase
Definition: quadruped_augmented.hpp:19
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.hxx:96
void set_state_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_augmented.hxx:471
void set_stop_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_augmented.hxx:501
void set_relative_forces(const bool &rel_forces)
Definition: quadruped_augmented.hxx:690
const Eigen::Matrix< Scalar, 12, 12 > & get_A() const
Definition: quadruped_augmented.hxx:673
void set_T_gait(const Scalar &T_gait_)
Definition: quadruped_augmented.hxx:638
const Scalar & get_shoulder_hlim() const
Definition: quadruped_augmented.hxx:645
ActionDataQuadrupedAugmentedTpl(Model< Scalar > *const model)
Definition: quadruped_augmented.hpp:201
ActionModelQuadrupedAugmentedTpl< double > ActionModelQuadrupedAugmented
Definition: quadruped_augmented.hpp:209
void set_friction_weight(const Scalar &weight)
Definition: quadruped_augmented.hxx:516
crocoddyl::ActionModelAbstractTpl< Scalar > Base
Definition: quadruped_augmented.hpp:18
const Eigen::Matrix< Scalar, 12, 12 > & get_B() const
Definition: quadruped_augmented.hxx:678
void set_shoulder_hlim(const Scalar &hlim)
Definition: quadruped_augmented.hxx:650
const Eigen::Matrix< Scalar, 12, 1 > & get_force_weights() const
Definition: quadruped_augmented.hxx:452
const Eigen::Matrix< Scalar, 4, 1 > & get_shoulder_contact_weight() const
Definition: quadruped_augmented.hxx:658
const Scalar & get_max_fz_contact() const
Definition: quadruped_augmented.hxx:582
ActionModelQuadrupedAugmentedTpl(typename Eigen::Matrix< Scalar, 3, 1 > offset_CoM=Eigen::Matrix< Scalar, 3, 1 >::Zero())
Definition: quadruped_augmented.hxx:8
const Scalar & get_mu() const
Definition: quadruped_augmented.hxx:522
Definition: quadruped.hpp:11
crocoddyl::ActionDataAbstractTpl< Scalar > Base
Definition: quadruped_augmented.hpp:188
const Eigen::Matrix< Scalar, 12, 1 > & get_state_weights() const
Definition: quadruped_augmented.hxx:467
_Scalar Scalar
Definition: quadruped_augmented.hpp:16
const Scalar & get_dt() const
Definition: quadruped_augmented.hxx:541
crocoddyl::ActionDataAbstractTpl< Scalar > ActionDataAbstract
Definition: quadruped_augmented.hpp:17
const Scalar & get_min_fz_contact() const
Definition: quadruped_augmented.hxx:569
void set_mass(const Scalar &m)
Definition: quadruped_augmented.hxx:535
Definition: quadruped_augmented.hpp:13
const Eigen::Matrix< Scalar, 8, 1 > & get_stop_weights() const
Definition: quadruped_augmented.hxx:497
void set_symmetry_term(const bool &sym_term)
Definition: quadruped_augmented.hxx:603
void set_force_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_augmented.hxx:456
const Eigen::Matrix< Scalar, 3, 3 > & get_gI() const
Definition: quadruped_augmented.hxx:554
const Scalar & get_T_gait() const
Definition: quadruped_augmented.hxx:633
crocoddyl::MathBaseTpl< Scalar > MathBase
Definition: quadruped_augmented.hpp:187
const bool & get_symmetry_term() const
Definition: quadruped_augmented.hxx:598
const bool & get_centrifugal_term() const
Definition: quadruped_augmented.hxx:610
void set_min_fz_contact(const Scalar &min_fz)
Definition: quadruped_augmented.hxx:575
const Scalar & get_mass() const
Definition: quadruped_augmented.hxx:531
void set_heuristic_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_augmented.hxx:486
void set_centrifugal_term(const bool ¢_term)
Definition: quadruped_augmented.hxx:615
const Scalar & get_friction_weight() const
Definition: quadruped_augmented.hxx:511
~ActionModelQuadrupedAugmentedTpl()
Definition: quadruped_augmented.hxx:93
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.hxx:215
void set_max_fz_contact(const Scalar &max_fz)
Definition: quadruped_augmented.hxx:588
void set_mu(const Scalar &mu_coeff)
Definition: quadruped_augmented.hxx:526
virtual boost::shared_ptr< ActionDataAbstract > createData()
Definition: quadruped_augmented.hxx:442
ActionDataQuadrupedAugmentedTpl< double > ActionDataQuadrupedAugmented
Definition: quadruped_augmented.hpp:210
const bool & get_shoulder_reference_position() const
Definition: quadruped_augmented.hxx:623
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar Scalar
Definition: quadruped_augmented.hpp:186
Definition: quadruped_augmented.hpp:182
void set_shoulder_contact_weight(const typename Eigen::Matrix< Scalar, 4, 1 > &weight)
Definition: quadruped_augmented.hxx:662
void set_gI(const typename MathBase::Matrix3s &inertia_matrix)
Definition: quadruped_augmented.hxx:558
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.hxx:708
void set_dt(const Scalar &dt)
Definition: quadruped_augmented.hxx:545
void set_shoulder_reference_position(const bool &reference)
Definition: quadruped_augmented.hxx:627
const Eigen::Matrix< Scalar, 8, 1 > & get_heuristic_weights() const
Definition: quadruped_augmented.hxx:482
const bool & get_relative_forces() const
Definition: quadruped_augmented.hxx:685