9 #include <crocoddyl/core/utils/exception.hpp> 10 #include <pinocchio/algorithm/frames-derivatives.hpp> 11 #include <pinocchio/algorithm/frames.hpp> 17 template <
typename Scalar>
19 boost::shared_ptr<StateMultibody> state,
20 const pinocchio::FrameIndex frame_id,
const Scalar slope,
22 :
Base(state, 2, nu, true, true, false),
25 pin_model_(*state->get_pinocchio()) {}
27 template <
typename Scalar>
29 boost::shared_ptr<StateMultibody> state,
30 const pinocchio::FrameIndex frame_id,
const Scalar slope)
31 :
Base(state, 2, true, true, false),
34 pin_model_(*state->get_pinocchio()) {}
36 template <
typename Scalar>
39 template <
typename Scalar>
41 const boost::shared_ptr<ResidualDataAbstract>& data,
42 const Eigen::Ref<const VectorXs>& ,
43 const Eigen::Ref<const VectorXs>&) {
46 Data* d =
static_cast<Data*
>(data.get());
48 pinocchio::updateFramePlacement(pin_model_, *d->
pinocchio, frame_id);
49 data->r = pinocchio::getFrameVelocity(pin_model_, *d->
pinocchio, frame_id,
50 pinocchio::LOCAL_WORLD_ALIGNED)
53 d->
ez = exp(-d->
pinocchio->oMf[frame_id].translation()[2] * slope);
57 template <
typename Scalar>
59 const boost::shared_ptr<ResidualDataAbstract>& data,
60 const Eigen::Ref<const VectorXs>& ,
61 const Eigen::Ref<const VectorXs>&) {
62 Data* d =
static_cast<Data*
>(data.get());
63 const std::size_t nv = state_->get_nv();
75 pinocchio::getFrameVelocityDerivatives(pin_model_, *d->
pinocchio, frame_id,
79 frame_id, pinocchio::LOCAL)
84 d->
vxJ.noalias() = pinocchio::skew(-v) * d->
l_dnu_dv.template bottomRows<3>();
90 data->Rx.leftCols(nv) = d->
o_dv_dq.template topRows<2>();
91 data->Rx.rightCols(nv) = d->
o_dv_dv.template topRows<2>();
95 data->Rx.leftCols(nv).row(0) -= data->r[0] * slope * d->
o_dv_dv.row(2);
96 data->Rx.leftCols(nv).row(1) -= data->r[1] * slope * d->
o_dv_dv.row(2);
99 template <
typename Scalar>
100 boost::shared_ptr<ResidualDataAbstractTpl<Scalar> >
102 return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(),
this,
106 template <
typename Scalar>
107 const typename pinocchio::FrameIndex&
112 template <
typename Scalar>
114 const pinocchio::FrameIndex& fid) {
Matrix6xs l_dnu_dq
Definition: residual-fly-high.hpp:176
Matrix6xs l_dnu_dv
Definition: residual-fly-high.hpp:176
MathBase::Vector3s Vector3s
Definition: residual-fly-high.hpp:43
Matrix3xs o_dv_dq
Definition: residual-fly-high.hpp:177
DataCollectorAbstractTpl< Scalar > DataCollectorAbstract
Definition: residual-fly-high.hpp:42
Matrix3xs vxJ
Definition: residual-fly-high.hpp:177
virtual void calcDiff(const boost::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the derivatives of the CoM velocity residual.
Definition: residual-fly-high.hxx:58
const pinocchio::FrameIndex & get_frame_id() const
Return the frame index.
Definition: residual-fly-high.hxx:108
Scalar ez
Definition: residual-fly-high.hpp:179
Definition: residual-fly-high.hpp:128
virtual ~ResidualModelFlyHighTpl()
Definition: residual-fly-high.hxx:37
void set_frame_id(const pinocchio::FrameIndex &fid)
Modify the frame index.
Definition: residual-fly-high.hxx:113
pinocchio::DataTpl< Scalar > * pinocchio
Pinocchio data.
Definition: residual-fly-high.hpp:174
ResidualModelAbstractTpl< Scalar > Base
Definition: residual-fly-high.hpp:38
Definition: activation-quad-ref.hpp:19
ResidualModelFlyHighTpl(boost::shared_ptr< StateMultibody > state, const pinocchio::FrameIndex frame_id, const Scalar slope, const std::size_t nu)
Initialize the residual model.
Definition: residual-fly-high.hxx:18
MathBase::Matrix3s Matrix3s
Definition: residual-fly-high.hpp:46
virtual void calc(const boost::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the residual.
Definition: residual-fly-high.hxx:40
virtual boost::shared_ptr< ResidualDataAbstract > createData(DataCollectorAbstract *const data)
Definition: residual-fly-high.hxx:101
Matrix3xs o_dv_dv
Definition: residual-fly-high.hpp:177