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>
20 using namespace crocoddyl;
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;
Cost penalizing high horizontal velocity near zero altitude.
Definition: residual-fly-high.hpp:32
const Scalar getSlope() const
Definition: residual-fly-high.hpp:110
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar Scalar
Definition: residual-fly-high.hpp:36
MathBaseTpl< Scalar > MathBase
Definition: residual-fly-high.hpp:37
MathBase::Matrix3s Matrix3s
Definition: residual-fly-high.hpp:46
ResidualDataAbstractTpl< Scalar > ResidualDataAbstract
Definition: residual-fly-high.hpp:41
StateMultibodyTpl< Scalar > StateMultibody
Definition: residual-fly-high.hpp:40
DataCollectorAbstractTpl< Scalar > DataCollectorAbstract
Definition: residual-fly-high.hpp:42
ResidualModelAbstractTpl< Scalar > Base
Definition: residual-fly-high.hpp:38
MathBase::VectorXs VectorXs
Definition: residual-fly-high.hpp:44
MathBase::MatrixXs MatrixXs
Definition: residual-fly-high.hpp:45
ResidualDataFlyHighTpl< Scalar > Data
Definition: residual-fly-high.hpp:39
MathBase::Vector3s Vector3s
Definition: residual-fly-high.hpp:43
void setSlope(const Scalar s)
Definition: residual-fly-high.hpp:111
Definition: activation-quad-ref.hpp:19
Definition: residual-fly-high.hpp:128
MathBase::VectorXs VectorXs
Definition: residual-fly-high.hpp:137
pinocchio::DataTpl< Scalar > * pinocchio
Pinocchio data.
Definition: residual-fly-high.hpp:174
Matrix6xs l_dnu_dq
Definition: residual-fly-high.hpp:176
ResidualDataFlyHighTpl(Model< Scalar > *const model, DataCollectorAbstract *const data)
Definition: residual-fly-high.hpp:140
MathBase::Matrix6xs Matrix6xs
Definition: residual-fly-high.hpp:135
MathBaseTpl< Scalar > MathBase
Definition: residual-fly-high.hpp:132
MathBase::Matrix3xs Matrix3xs
Definition: residual-fly-high.hpp:136
DataCollectorAbstractTpl< Scalar > DataCollectorAbstract
Definition: residual-fly-high.hpp:134
Scalar ez
Definition: residual-fly-high.hpp:179
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar Scalar
Definition: residual-fly-high.hpp:131
ResidualDataAbstractTpl< Scalar > Base
Definition: residual-fly-high.hpp:133
Matrix3xs o_dv_dq
Definition: residual-fly-high.hpp:177
Matrix6xs d_dq
Definition: residual-fly-high.hpp:175