residual-fly-high.hxx
Go to the documentation of this file.
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2022, LAAS-CNRS
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #include <crocoddyl/core/utils/exception.hpp>
10 #include <pinocchio/algorithm/frames-derivatives.hpp>
11 #include <pinocchio/algorithm/frames.hpp>
12 
13 #include "sobec/residual-fly-high.hpp"
14 
15 namespace sobec {
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,
21  const std::size_t nu)
22  : Base(state, 2, nu, true, true, false),
23  frame_id(frame_id),
24  slope(slope),
25  pin_model_(*state->get_pinocchio()) {}
26 
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),
32  frame_id(frame_id),
33  slope(slope),
34  pin_model_(*state->get_pinocchio()) {}
35 
36 template <typename Scalar>
37 ResidualModelFlyHighTpl<Scalar>::~ResidualModelFlyHighTpl() {}
38 
39 template <typename Scalar>
40 void ResidualModelFlyHighTpl<Scalar>::calc(
41  const boost::shared_ptr<ResidualDataAbstract>& data,
42  const Eigen::Ref<const VectorXs>& /*x*/,
43  const Eigen::Ref<const VectorXs>&) {
44  // Compute the residual residual give the reference CoM velocity
45 
46  Data* d = static_cast<Data*>(data.get());
47 
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)
51  .linear()
52  .head(2);
53  d->ez = exp(-d->pinocchio->oMf[frame_id].translation()[2] * slope);
54  data->r *= d->ez;
55 }
56 
57 template <typename Scalar>
58 void ResidualModelFlyHighTpl<Scalar>::calcDiff(
59  const boost::shared_ptr<ResidualDataAbstract>& data,
60  const Eigen::Ref<const VectorXs>& /*x*/,
61  const Eigen::Ref<const VectorXs>&) {
62  Data* d = static_cast<Data*>(data.get());
63  const std::size_t nv = state_->get_nv();
64 
65  /* Let' s do a little bit of maths ...
66  * r = v/e with e=exp(z/2)
67  * r' = v'/e - v/e z'/2 = v'/e - r/2 z'
68  *
69  * Wrong, we should consider l_v the local velocity, with o_v = oRl l_v
70  * Then v=R l_v
71  * v' = R l_v' + R' l_v = R l_v' - l_v x Jr
72  * Then r' = v'/e - r/2 z' = R l_v'/e - l_v x Jr/e - r/2 z'
73  */
74 
75  pinocchio::getFrameVelocityDerivatives(pin_model_, *d->pinocchio, frame_id,
76  pinocchio::LOCAL, d->l_dnu_dq,
77  d->l_dnu_dv);
78  const Vector3s& v = pinocchio::getFrameVelocity(pin_model_, *d->pinocchio,
79  frame_id, pinocchio::LOCAL)
80  .linear();
81  const Matrix3s& R = d->pinocchio->oMf[frame_id].rotation();
82 
83  // First compute LWA derivatives of the velocity
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>();
88 
89  // First term with derivative of v
90  data->Rx.leftCols(nv) = d->o_dv_dq.template topRows<2>();
91  data->Rx.rightCols(nv) = d->o_dv_dv.template topRows<2>();
92  data->Rx *= d->ez;
93 
94  // Second term with derivative of z
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);
97 }
98 
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,
103  data);
104 }
105 
106 template <typename Scalar>
107 const typename pinocchio::FrameIndex&
108 ResidualModelFlyHighTpl<Scalar>::get_frame_id() const {
109  return frame_id;
110 }
111 
112 template <typename Scalar>
113 void ResidualModelFlyHighTpl<Scalar>::set_frame_id(
114  const pinocchio::FrameIndex& fid) {
115  frame_id = fid;
116 }
117 
118 } // namespace sobec
sobec
Definition: contact-force.hxx:11