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> 20 template <
typename Scalar>
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>
29 template <
typename Scalar>
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());
37 data->r[0] = f.linear()[1] / f.linear()[2];
38 data->r[1] = -f.linear()[0] / f.linear()[2];
41 template <
typename Scalar>
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());
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> >
69 return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(),
this,
ResidualModelAbstractTpl< Scalar > Base
Definition: residual-center-of-friction.hpp:48
boost::shared_ptr< ForceDataAbstractTpl< Scalar > > contact
Definition: residual-center-of-friction.hpp:166
pinocchio::ForceTpl< Scalar > Force
Definition: residual-center-of-friction.hpp:57
virtual ~ResidualModelCenterOfFrictionTpl()
Definition: residual-center-of-friction.hxx:27
DataCollectorAbstractTpl< Scalar > DataCollectorAbstract
Definition: residual-center-of-friction.hpp:52
virtual boost::shared_ptr< ResidualDataAbstract > createData(DataCollectorAbstract *const data)
Definition: residual-center-of-friction.hxx:67
virtual void calc(const boost::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the center of friction.
Definition: residual-center-of-friction.hxx:30
MathBase::MatrixXs MatrixXs
Definition: residual-center-of-friction.hpp:56
virtual void calcDiff(const boost::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the derivatives of residual.
Definition: residual-center-of-friction.hxx:42
Definition: activation-quad-ref.hpp:19
Definition: residual-center-of-friction.hpp:118
ResidualModelCenterOfFrictionTpl(boost::shared_ptr< StateMultibody > state, const pinocchio::FrameIndex contact_id, const std::size_t nu)
Initialize the residual model.
Definition: residual-center-of-friction.hxx:21