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" 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());
67 const std::size_t nv = state_->get_nv();
84 pinocchio::getFrameVelocityDerivatives(pin_model_, *d->pinocchio, frame_id,
85 pinocchio::LOCAL, d->l_dnu_dq,
87 const Vector3s& v = pinocchio::getFrameVelocity(pin_model_, *d->pinocchio,
88 frame_id, pinocchio::LOCAL)
90 const Matrix3s& R = d->pinocchio->oMf[frame_id].rotation();
93 d->vxJ.noalias() = pinocchio::skew(-v) * d->l_dnu_dv.template bottomRows<3>();
94 d->vxJ += d->l_dnu_dq.template topRows<3>();
95 d->o_dv_dq = R * d->vxJ;
96 d->o_dv_dv = R * d->l_dnu_dv.template topRows<3>();
99 data->Rx.leftCols(nv) = d->o_dv_dq.template topRows<2>();
100 data->Rx.rightCols(nv) = d->o_dv_dv.template topRows<2>();
104 data->Rx.leftCols(nv).row(0) -=
105 data->r[0] * slope * d->o_dv_dv.row(2);
106 data->Rx.leftCols(nv).row(1) -=
107 data->r[1] * slope * d->o_dv_dv.row(2);
110 template <
typename Scalar>
111 boost::shared_ptr<ResidualDataAbstractTpl<Scalar> >
112 ResidualModelFlyHighTpl<Scalar>::createData(DataCollectorAbstract*
const data) {
113 return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(),
this,
117 template <
typename Scalar>
118 const typename pinocchio::FrameIndex&
119 ResidualModelFlyHighTpl<Scalar>::get_frame_id()
const {
123 template <
typename Scalar>
124 void ResidualModelFlyHighTpl<Scalar>::set_frame_id(
125 const pinocchio::FrameIndex& fid) {
Definition: contact-force.hxx:11