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 Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic>
64  // q = x.head(state_->get_nq()); const Eigen::VectorBlock<const
65  // Eigen::Ref<const VectorXs>, Eigen::Dynamic> v = x.tail(state_->get_nv());
66 
67  const std::size_t nv = state_->get_nv();
68 
69  // pinocchio::getCenterOfMassVelocityDerivatives(pin_model_, *d->pinocchio,
70  // d->dvcom_dq);
71  // data->Rx.leftCols(nv) = d->dvcom_dq;
72  // data->Rx.rightCols(nv) = d->pinocchio->Jcom;
73 
74  /* Let' s do a little bit of maths ...
75  * r = v/e with e=exp(z/2)
76  * r' = v'/e - v/e z'/2 = v'/e - r/2 z'
77  *
78  * Wrong, we should consider l_v the local velocity, with o_v = oRl l_v
79  * Then v=R l_v
80  * v' = R l_v' + R' l_v = R l_v' - l_v x Jr
81  * Then r' = v'/e - r/2 z' = R l_v'/e - l_v x Jr/e - r/2 z'
82  */
83 
84  pinocchio::getFrameVelocityDerivatives(pin_model_, *d->pinocchio, frame_id,
85  pinocchio::LOCAL, d->l_dnu_dq,
86  d->l_dnu_dv);
87  const Vector3s& v = pinocchio::getFrameVelocity(pin_model_, *d->pinocchio,
88  frame_id, pinocchio::LOCAL)
89  .linear();
90  const Matrix3s& R = d->pinocchio->oMf[frame_id].rotation();
91 
92  // First compute LWA derivatives of the velocity
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>();
97 
98  // First term with derivative of v
99  data->Rx.leftCols(nv) = d->o_dv_dq.template topRows<2>();
100  data->Rx.rightCols(nv) = d->o_dv_dv.template topRows<2>();
101  data->Rx *= d->ez;
102 
103  // Second term with derivative of z
104  data->Rx.leftCols(nv).row(0) -=
105  data->r[0] * slope * d->o_dv_dv.row(2); // d->d_dq.row(2);
106  data->Rx.leftCols(nv).row(1) -=
107  data->r[1] * slope * d->o_dv_dv.row(2); // d->d_dq.row(2);
108 }
109 
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,
114  data);
115 }
116 
117 template <typename Scalar>
118 const typename pinocchio::FrameIndex&
119 ResidualModelFlyHighTpl<Scalar>::get_frame_id() const {
120  return frame_id;
121 }
122 
123 template <typename Scalar>
124 void ResidualModelFlyHighTpl<Scalar>::set_frame_id(
125  const pinocchio::FrameIndex& fid) {
126  frame_id = fid;
127 }
128 
129 } // namespace sobec
sobec
Definition: contact-force.hxx:11