1 #ifndef __quadruped_walkgen_quadruped_step_time_hpp__ 2 #define __quadruped_walkgen_quadruped_step_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();
45 const Eigen::Ref<const typename MathBase::MatrixXs>& l_feet,
46 const Eigen::Ref<const typename MathBase::MatrixXs>& velocity,
47 const Eigen::Ref<const typename MathBase::MatrixXs>& acceleration,
48 const Eigen::Ref<const typename MathBase::MatrixXs>& xref,
49 const Eigen::Ref<const typename MathBase::VectorXs>& S);
73 const typename Eigen::Matrix<Scalar, 7, 1>&
get_cost()
const;
76 using Base::has_control_limits_;
92 bool centrifugal_term;
98 typename Eigen::Matrix<Scalar, 12, 1> state_weights_;
99 typename Eigen::Matrix<Scalar, 4, 1> step_weights_;
100 typename Eigen::Matrix<Scalar, 8, 1> heuristicWeights;
101 typename MathBase::Matrix3s R_tmp;
106 typename MathBase::ArrayXs alpha;
107 typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> alpha2;
108 typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> b_coeff;
109 typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> b_coeff_x0;
110 typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> b_coeff_y0;
111 typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> b_coeff_x1;
112 typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> b_coeff_y1;
113 typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> b_coeff_x2;
114 typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> b_coeff_y2;
116 typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> rub_max_first_x;
117 typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> rub_max_first_y;
118 typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> rub_max_first_2;
119 typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>
123 typename Eigen::Matrix<Scalar, 3, 4> lfeet;
128 typename Eigen::Matrix<Scalar, 8, 8> B;
130 typename MathBase::MatrixXs xref_;
131 typename MathBase::VectorXs S_;
132 typename Eigen::Matrix<Scalar, 8, 1> pheuristic_;
141 typename Eigen::Matrix<Scalar, 4, 1> rub_max_;
142 typename Eigen::Matrix<Scalar, 4, 1> rub_max_bool;
146 typename Eigen::Matrix<Scalar, 7, 1> cost_;
149 template <
typename _Scalar>
151 :
public crocoddyl::ActionDataAbstractTpl<_Scalar> {
152 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
156 typedef crocoddyl::ActionDataAbstractTpl<Scalar>
Base;
168 template <
template <
typename Scalar>
class Model>
170 : crocoddyl::ActionDataAbstractTpl<
Scalar>(model) {}
void update_model(const Eigen::Ref< const typename MathBase::MatrixXs > &l_feet, const Eigen::Ref< const typename MathBase::MatrixXs > &velocity, const Eigen::Ref< const typename MathBase::MatrixXs > &acceleration, const Eigen::Ref< const typename MathBase::MatrixXs > &xref, const Eigen::Ref< const typename MathBase::VectorXs > &S)
Definition: quadruped_step_time.hxx:465
void set_symmetry_term(const bool &sym_term)
Definition: quadruped_step_time.hxx:371
crocoddyl::MathBaseTpl< Scalar > MathBase
Definition: quadruped_step_time.hpp:155
const Scalar & get_nb_nodes() const
Definition: quadruped_step_time.hxx:416
void set_T_gait(const Scalar &T_gait_)
Definition: quadruped_step_time.hxx:395
_Scalar Scalar
Definition: quadruped_step_time.hpp:16
const Scalar & get_vlim() const
Definition: quadruped_step_time.hxx:428
crocoddyl::ActionModelAbstractTpl< Scalar > Base
Definition: quadruped_step_time.hpp:18
void set_step_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_step_time.hxx:342
Definition: quadruped.hpp:11
virtual boost::shared_ptr< ActionDataAbstract > createData()
Definition: quadruped_step_time.hxx:313
Definition: quadruped_step_time.hpp:150
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_time.hxx:194
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_time.hxx:103
~ActionModelQuadrupedStepTimeTpl()
Definition: quadruped_step_time.hxx:100
crocoddyl::MathBaseTpl< Scalar > MathBase
Definition: quadruped_step_time.hpp:19
void set_vlim(const Scalar &vlim_)
Definition: quadruped_step_time.hxx:432
const Eigen::Matrix< Scalar, 7, 1 > & get_cost() const
Definition: quadruped_step_time.hxx:443
void set_state_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_step_time.hxx:327
ActionModelQuadrupedStepTimeTpl< double > ActionModelQuadrupedStepTime
Definition: quadruped_step_time.hpp:177
void set_first_step(const bool &first)
Definition: quadruped_step_time.hxx:454
const Eigen::Matrix< Scalar, 12, 1 > & get_state_weights() const
Definition: quadruped_step_time.hxx:323
const Scalar & get_speed_weight() const
Definition: quadruped_step_time.hxx:405
crocoddyl::ActionDataAbstractTpl< Scalar > ActionDataAbstract
Definition: quadruped_step_time.hpp:17
const bool & get_symmetry_term() const
Definition: quadruped_step_time.hxx:367
crocoddyl::ActionDataAbstractTpl< Scalar > Base
Definition: quadruped_step_time.hpp:156
ActionDataQuadrupedStepTimeTpl(Model< Scalar > *const model)
Definition: quadruped_step_time.hpp:169
const Eigen::Matrix< Scalar, 8, 1 > & get_heuristic_weights() const
Definition: quadruped_step_time.hxx:353
const bool & get_first_step() const
Definition: quadruped_step_time.hxx:450
ActionDataQuadrupedStepTimeTpl< double > ActionDataQuadrupedStepTime
Definition: quadruped_step_time.hpp:178
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar Scalar
Definition: quadruped_step_time.hpp:154
const bool & get_centrifugal_term() const
Definition: quadruped_step_time.hxx:378
const Scalar & get_T_gait() const
Definition: quadruped_step_time.hxx:390
void set_speed_weight(const Scalar &weight_)
Definition: quadruped_step_time.hxx:410
void set_heuristic_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_step_time.hxx:357
ActionModelQuadrupedStepTimeTpl()
Definition: quadruped_step_time.hxx:8
void set_nb_nodes(const Scalar &nodes_)
Definition: quadruped_step_time.hxx:420
void set_centrifugal_term(const bool ¢_term)
Definition: quadruped_step_time.hxx:383
Definition: quadruped_step_time.hpp:13
const Eigen::Matrix< Scalar, 4, 1 > & get_step_weights() const
Definition: quadruped_step_time.hxx:338