9 #ifndef SOBEC_RESIDUAL_FLY_HIGH_HPP_ 10 #define SOBEC_RESIDUAL_FLY_HIGH_HPP_ 12 #include <crocoddyl/core/residual-base.hpp> 13 #include <crocoddyl/multibody/data/multibody.hpp> 14 #include <crocoddyl/multibody/fwd.hpp> 15 #include <crocoddyl/multibody/states/multibody.hpp> 31 template <
typename _Scalar>
34 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
38 typedef ResidualModelAbstractTpl<Scalar>
Base;
58 const pinocchio::FrameIndex frame_id,
59 const Scalar slope,
const std::size_t nu);
72 const pinocchio::FrameIndex frame_id,
83 virtual void calc(
const boost::shared_ptr<ResidualDataAbstract>& data,
84 const Eigen::Ref<const VectorXs>&
x,
85 const Eigen::Ref<const VectorXs>& u);
94 virtual void calcDiff(
const boost::shared_ptr<ResidualDataAbstract>& data,
95 const Eigen::Ref<const VectorXs>&
x,
96 const Eigen::Ref<const VectorXs>& u);
97 virtual boost::shared_ptr<ResidualDataAbstract> createData(
103 const pinocchio::FrameIndex& get_frame_id()
const;
108 void set_frame_id(
const pinocchio::FrameIndex& fid);
116 using Base::u_dependent_;
118 using Base::v_dependent_;
121 pinocchio::FrameIndex frame_id;
123 typename StateMultibody::PinocchioModel
127 template <
typename _Scalar>
129 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
133 typedef ResidualDataAbstractTpl<Scalar>
Base;
139 template <
template <
typename Scalar>
class Model>
143 d_dq(6, model->get_state()->get_nv()),
144 d_dv(6, model->get_state()->get_nv()),
145 l_dnu_dq(6, model->get_state()->get_nv()),
146 l_dnu_dv(6, model->get_state()->get_nv()),
147 o_dv_dq(3, model->get_state()->get_nv()),
148 o_dv_dv(3, model->get_state()->get_nv()),
149 o_Jw(3, model->get_state()->get_nv()),
150 vxJ(3, model->get_state()->get_nv()) {
153 DataCollectorMultibodyTpl<Scalar>* d =
154 dynamic_cast<DataCollectorMultibodyTpl<Scalar>*
>(shared);
157 "Invalid argument: the shared data should be derived from " 158 "DataCollectorMultibody");
162 pinocchio = d->pinocchio;
194 #endif // SOBEC_RESIDUAL_FLY_HIGH_HPP_ MathBase::Matrix3xs Matrix3xs
Definition: residual-fly-high.hpp:136
Matrix6xs l_dnu_dv
Definition: residual-fly-high.hpp:176
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar Scalar
Definition: residual-fly-high.hpp:36
Cost penalizing high horizontal velocity near zero altitude.
Definition: residual-fly-high.hpp:32
MathBase::Vector3s Vector3s
Definition: residual-fly-high.hpp:43
ResidualDataFlyHighTpl< Scalar > Data
Definition: residual-fly-high.hpp:39
DataCollectorAbstractTpl< Scalar > DataCollectorAbstract
Definition: residual-fly-high.hpp:42
Matrix3xs vxJ
Definition: residual-fly-high.hpp:177
MathBase::Matrix6xs Matrix6xs
Definition: residual-fly-high.hpp:135
ResidualDataFlyHighTpl(Model< Scalar > *const model, DataCollectorAbstract *const data)
Definition: residual-fly-high.hpp:140
Scalar ez
Definition: residual-fly-high.hpp:179
DataCollectorAbstractTpl< Scalar > DataCollectorAbstract
Definition: residual-fly-high.hpp:134
Definition: residual-fly-high.hpp:128
MathBaseTpl< Scalar > MathBase
Definition: residual-fly-high.hpp:37
pinocchio::DataTpl< Scalar > * pinocchio
Pinocchio data.
Definition: residual-fly-high.hpp:174
ResidualModelAbstractTpl< Scalar > Base
Definition: residual-fly-high.hpp:38
MathBase::VectorXs VectorXs
Definition: residual-fly-high.hpp:137
Definition: activation-quad-ref.hpp:19
StateMultibodyTpl< Scalar > StateMultibody
Definition: residual-fly-high.hpp:40
MathBaseTpl< Scalar > MathBase
Definition: residual-fly-high.hpp:132
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar Scalar
Definition: residual-fly-high.hpp:131
MathBase::Matrix3s Matrix3s
Definition: residual-fly-high.hpp:46
MathBase::VectorXs VectorXs
Definition: residual-fly-high.hpp:44
const Scalar getSlope() const
Definition: residual-fly-high.hpp:110
ResidualDataAbstractTpl< Scalar > ResidualDataAbstract
Definition: residual-fly-high.hpp:41
ResidualDataAbstractTpl< Scalar > Base
Definition: residual-fly-high.hpp:133
Matrix6xs d_dv
Definition: residual-fly-high.hpp:175
MathBase::MatrixXs MatrixXs
Definition: residual-fly-high.hpp:45
void setSlope(const Scalar s)
Definition: residual-fly-high.hpp:111