1 #ifndef __quadruped_walkgen_quadruped_step_hpp__
2 #define __quadruped_walkgen_quadruped_step_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();
45 const Eigen::Ref<const typename MathBase::MatrixXs>& l_feet,
46 const Eigen::Ref<const typename MathBase::MatrixXs>& xref,
47 const Eigen::Ref<const typename MathBase::VectorXs>& S,
48 const Eigen::Ref<const typename MathBase::MatrixXs>& position,
49 const Eigen::Ref<const typename MathBase::MatrixXs>& velocity,
50 const Eigen::Ref<const typename MathBase::MatrixXs>& acceleration,
51 const Eigen::Ref<const typename MathBase::MatrixXs>& jerk,
52 const Eigen::Ref<const typename MathBase::MatrixXs>& oRh,
53 const Eigen::Ref<const typename MathBase::MatrixXs>& oTh,
68 const typename Eigen::Matrix<Scalar, 2, 1>&
get_acc_lim()
const;
69 void set_acc_lim(
const typename MathBase::VectorXs& acceleration_lim_);
77 const typename Eigen::Matrix<Scalar, 2, 1>&
get_vel_lim()
const;
78 void set_vel_lim(
const typename MathBase::VectorXs& velocity_lim_);
92 using Base::has_control_limits_;
103 bool centrifugal_term;
106 typename Eigen::Matrix<Scalar, 12, 1> state_weights_;
107 typename Eigen::Matrix<Scalar, 8, 1> step_weights_;
108 typename Eigen::Matrix<Scalar, 8, 1> heuristic_weights_;
109 typename MathBase::Matrix3s R_tmp;
111 typename Eigen::Matrix<Scalar, 8, 8> B;
113 typename MathBase::MatrixXs xref_;
114 typename Eigen::Matrix<Scalar, 8, 1> pheuristic_;
124 bool is_acc_activated_;
127 typename Eigen::Matrix<Scalar, 2, 1>
129 typename Eigen::Matrix<Scalar, 4, 1> S_;
130 typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> delta_;
131 typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> gamma_;
132 typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> alpha_;
133 typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> beta_x_;
134 typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> beta_y_;
135 typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> tmp_ones_;
136 typename Eigen::Array<Scalar, 3, 4> position_;
138 typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> rb_accx_max_;
139 typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> rb_accy_max_;
140 typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>
142 typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>
146 bool is_vel_activated_;
149 typename Eigen::Matrix<Scalar, 2, 1>
151 typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> gamma_v;
152 typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> alpha_v;
153 typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> beta_x_v;
154 typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> beta_y_v;
156 typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> rb_velx_max_;
157 typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> rb_vely_max_;
158 typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>
160 typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>
164 bool is_jerk_activated_;
167 typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> beta_j;
168 typename Eigen::Array<Scalar, 3, 4> jerk_;
170 typename Eigen::Matrix<Scalar, 2, 4> rb_jerk_;
172 typename Eigen::Matrix<Scalar, 3, 3> oRh_;
173 typename Eigen::Matrix<Scalar, 3, 1> oTh_;
176 template <
typename _Scalar>
178 :
public crocoddyl::ActionDataAbstractTpl<_Scalar> {
179 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
183 typedef crocoddyl::ActionDataAbstractTpl<Scalar>
Base;
195 template <
template <
typename Scalar>
class Model>
197 : crocoddyl::ActionDataAbstractTpl<
Scalar>(model) {}
Definition: quadruped_step.hpp:14
const bool & get_centrifugal_term() const
Definition: quadruped_step.hxx:967
void set_symmetry_term(const bool &sym_term)
Definition: quadruped_step.hxx:960
void set_acc_weight(const Scalar &weight_)
Definition: quadruped_step.hxx:1019
void set_T_gait(const Scalar &T_gait_)
Definition: quadruped_step.hxx:983
const bool & get_symmetry_term() const
Definition: quadruped_step.hxx:956
_Scalar Scalar
Definition: quadruped_step.hpp:16
const Scalar & get_T_gait() const
Definition: quadruped_step.hxx:978
const Scalar & get_acc_weight() const
Definition: quadruped_step.hxx:1015
void set_sample_feet_traj(const int &n_sample)
Definition: quadruped_step.hxx:1081
const Eigen::Matrix< Scalar, 2, 1 > & get_acc_lim() const
Definition: quadruped_step.hxx:1000
void set_heuristic_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_step.hxx:946
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.hxx:128
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.hxx:345
crocoddyl::ActionModelAbstractTpl< Scalar > Base
Definition: quadruped_step.hpp:18
void set_acc_activated(const bool &is_activated)
Definition: quadruped_step.hxx:993
~ActionModelQuadrupedStepTpl()
Definition: quadruped_step.hxx:125
void set_step_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_step.hxx:931
virtual boost::shared_ptr< ActionDataAbstract > createData()
Definition: quadruped_step.hxx:902
void set_jerk_activated(const bool &is_activated)
Definition: quadruped_step.hxx:1075
crocoddyl::ActionDataAbstractTpl< Scalar > ActionDataAbstract
Definition: quadruped_step.hpp:17
const Eigen::Matrix< Scalar, 8, 1 > & get_step_weights() const
Definition: quadruped_step.hxx:927
void set_vel_activated(const bool &is_activated)
Definition: quadruped_step.hxx:1029
void set_centrifugal_term(const bool ¢_term)
Definition: quadruped_step.hxx:971
void set_state_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_step.hxx:916
ActionModelQuadrupedStepTpl()
Definition: quadruped_step.hxx:8
void set_vel_weight(const Scalar &weight_)
Definition: quadruped_step.hxx:1055
const Scalar & get_jerk_weight() const
Definition: quadruped_step.hxx:1061
void set_acc_lim(const typename MathBase::VectorXs &acceleration_lim_)
Definition: quadruped_step.hxx:1004
const bool & get_acc_activated() const
Definition: quadruped_step.hxx:989
void set_jerk_weight(const Scalar &weight_)
Definition: quadruped_step.hxx:1065
const Eigen::Matrix< Scalar, 2, 1 > & get_vel_lim() const
Definition: quadruped_step.hxx:1036
const bool & get_vel_activated() const
Definition: quadruped_step.hxx:1025
void set_vel_lim(const typename MathBase::VectorXs &velocity_lim_)
Definition: quadruped_step.hxx:1040
crocoddyl::MathBaseTpl< Scalar > MathBase
Definition: quadruped_step.hpp:19
const Eigen::Matrix< Scalar, 8, 1 > & get_heuristic_weights() const
Definition: quadruped_step.hxx:942
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, const Eigen::Ref< const typename MathBase::MatrixXs > &position, const Eigen::Ref< const typename MathBase::MatrixXs > &velocity, const Eigen::Ref< const typename MathBase::MatrixXs > &acceleration, const Eigen::Ref< const typename MathBase::MatrixXs > &jerk, const Eigen::Ref< const typename MathBase::MatrixXs > &oRh, const Eigen::Ref< const typename MathBase::MatrixXs > &oTh, const Scalar &delta_T)
Definition: quadruped_step.hxx:1157
const Scalar & get_vel_weight() const
Definition: quadruped_step.hxx:1051
const bool & get_jerk_activated() const
Definition: quadruped_step.hxx:1071
const Eigen::Matrix< Scalar, 12, 1 > & get_state_weights() const
Definition: quadruped_step.hxx:912
Definition: quadruped.hpp:11
ActionDataQuadrupedStepTpl< double > ActionDataQuadrupedStep
Definition: quadruped_step.hpp:205
ActionModelQuadrupedStepTpl< double > ActionModelQuadrupedStep
Definition: quadruped_step.hpp:204
Definition: quadruped_step.hpp:178
crocoddyl::MathBaseTpl< Scalar > MathBase
Definition: quadruped_step.hpp:182
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar Scalar
Definition: quadruped_step.hpp:181
crocoddyl::ActionDataAbstractTpl< Scalar > Base
Definition: quadruped_step.hpp:183
ActionDataQuadrupedStepTpl(Model< Scalar > *const model)
Definition: quadruped_step.hpp:196