9 #include <crocoddyl/core/utils/exception.hpp>
10 #include <pinocchio/algorithm/frames-derivatives.hpp>
11 #include <pinocchio/algorithm/frames.hpp>
13 #include "sobec/residual-fly-high.hpp"
16 using namespace crocoddyl;
17 template <
typename Scalar>
18 ResidualModelFlyHighTpl<Scalar>::ResidualModelFlyHighTpl(
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>
28 ResidualModelFlyHighTpl<Scalar>::ResidualModelFlyHighTpl(
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>
37 ResidualModelFlyHighTpl<Scalar>::~ResidualModelFlyHighTpl() {}
39 template <
typename Scalar>
40 void ResidualModelFlyHighTpl<Scalar>::calc(
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>
58 void ResidualModelFlyHighTpl<Scalar>::calcDiff(
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,
76 pinocchio::LOCAL, d->l_dnu_dq,
78 const Vector3s& v = pinocchio::getFrameVelocity(pin_model_, *d->pinocchio,
79 frame_id, pinocchio::LOCAL)
81 const Matrix3s& R = d->pinocchio->oMf[frame_id].rotation();
84 d->vxJ.noalias() = pinocchio::skew(-v) * d->l_dnu_dv.template bottomRows<3>();
85 d->vxJ += d->l_dnu_dq.template topRows<3>();
86 d->o_dv_dq = R * d->vxJ;
87 d->o_dv_dv = R * d->l_dnu_dv.template topRows<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> >
101 ResidualModelFlyHighTpl<Scalar>::createData(DataCollectorAbstract*
const data) {
102 return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(),
this,
106 template <
typename Scalar>
107 const typename pinocchio::FrameIndex&
108 ResidualModelFlyHighTpl<Scalar>::get_frame_id()
const {
112 template <
typename Scalar>
113 void ResidualModelFlyHighTpl<Scalar>::set_frame_id(
114 const pinocchio::FrameIndex& fid) {
Definition: contact-force.hxx:11