9 #include <crocoddyl/core/utils/exception.hpp>
10 #include <pinocchio/algorithm/frames-derivatives.hpp>
11 #include <pinocchio/algorithm/frames.hpp>
12 #include <pinocchio/algorithm/kinematics-derivatives.hpp>
14 #include "sobec/residual-center-of-friction.hpp"
18 using namespace crocoddyl;
20 template <
typename Scalar>
21 ResidualModelCenterOfFrictionTpl<Scalar>::ResidualModelCenterOfFrictionTpl(
22 boost::shared_ptr<StateMultibody> state,
23 const pinocchio::FrameIndex contact_id,
const std::size_t nu)
24 : Base(state, 2, nu, true, true, true), contact_id_(contact_id) {}
26 template <
typename Scalar>
27 ResidualModelCenterOfFrictionTpl<Scalar>::~ResidualModelCenterOfFrictionTpl() {}
29 template <
typename Scalar>
30 void ResidualModelCenterOfFrictionTpl<Scalar>::calc(
31 const boost::shared_ptr<ResidualDataAbstract> &data,
32 const Eigen::Ref<const VectorXs> & ,
33 const Eigen::Ref<const VectorXs> &) {
34 Data *d =
static_cast<Data *
>(data.get());
35 Force f = d->contact->jMf.actInv(d->contact->f);
37 data->r[0] = f.linear()[1] / f.linear()[2];
38 data->r[1] = -f.linear()[0] / f.linear()[2];
41 template <
typename Scalar>
42 void ResidualModelCenterOfFrictionTpl<Scalar>::calcDiff(
43 const boost::shared_ptr<ResidualDataAbstract> &data,
44 const Eigen::Ref<const VectorXs> &,
const Eigen::Ref<const VectorXs> &) {
45 Data *d =
static_cast<Data *
>(data.get());
46 Force f = d->contact->jMf.actInv(d->contact->f);
47 const MatrixXs &df_dx = d->contact->df_dx;
48 const MatrixXs &df_du = d->contact->df_du;
52 data->Rx.row(0) = df_dx.row(1);
53 data->Rx.row(1) = -df_dx.row(0);
54 data->Rx.row(0) -= data->r[0] * df_dx.row(2);
55 data->Rx.row(1) -= data->r[1] * df_dx.row(2);
56 data->Rx /= f.linear()[2];
58 data->Ru.row(0) = df_du.row(1);
59 data->Ru.row(1) = -df_du.row(0);
60 data->Ru.row(0) -= data->r[0] * df_du.row(2);
61 data->Ru.row(1) -= data->r[1] * df_du.row(2);
62 data->Ru /= f.linear()[2];
65 template <
typename Scalar>
66 boost::shared_ptr<ResidualDataAbstractTpl<Scalar> >
67 ResidualModelCenterOfFrictionTpl<Scalar>::createData(
68 DataCollectorAbstract *
const data) {
69 return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(),
this,