9 #include <crocoddyl/core/utils/exception.hpp> 10 #include <pinocchio/algorithm/frames-derivatives.hpp> 11 #include <pinocchio/algorithm/frames.hpp> 17 template <
typename Scalar>
19 boost::shared_ptr<StateMultibody> state,
20 const pinocchio::FrameIndex frame_id1,
21 const pinocchio::FrameIndex frame_id2,
const std::size_t nu)
22 :
Base(state, 1, nu, true, false, false),
25 pin_model_(*state->get_pinocchio()) {}
27 template <
typename Scalar>
29 boost::shared_ptr<StateMultibody> state,
30 const pinocchio::FrameIndex frame_id1,
31 const pinocchio::FrameIndex frame_id2)
32 :
Base(state, 1, true, false, false),
35 pin_model_(*state->get_pinocchio()) {}
37 template <
typename Scalar>
40 template <
typename Scalar>
42 const boost::shared_ptr<ResidualDataAbstract>& data,
43 const Eigen::Ref<const VectorXs>& ,
44 const Eigen::Ref<const VectorXs>&) {
47 Data* d =
static_cast<Data*
>(data.get());
49 pinocchio::updateFramePlacement(pin_model_, *d->
pinocchio, frame_id1);
50 pinocchio::updateFramePlacement(pin_model_, *d->
pinocchio, frame_id2);
52 const typename MathBase::Vector3s& p1 =
53 d->
pinocchio->oMf[frame_id1].translation();
54 const typename MathBase::Vector3s& p2 =
55 d->
pinocchio->oMf[frame_id2].translation();
57 d->r[0] = d->
p1p2.template head<2>().norm();
60 template <
typename Scalar>
62 const boost::shared_ptr<ResidualDataAbstract>& data,
63 const Eigen::Ref<const VectorXs>& ,
64 const Eigen::Ref<const VectorXs>&) {
65 Data* d =
static_cast<Data*
>(data.get());
70 const std::size_t nv = state_->get_nv();
71 pinocchio::getFrameJacobian(pin_model_, *d->
pinocchio, frame_id1,
72 pinocchio::LOCAL_WORLD_ALIGNED, d->
J1);
73 pinocchio::getFrameJacobian(pin_model_, *d->
pinocchio, frame_id2,
74 pinocchio::LOCAL_WORLD_ALIGNED, d->
J2);
76 d->
dJ = d->
J1.template topRows<2>() - d->
J2.template topRows<2>();
78 data->Rx.leftCols(nv) = d->
dJ.row(0) * (d->
p1p2[0] / d->r[0]);
79 data->Rx.leftCols(nv) += d->
dJ.row(1) * (d->
p1p2[1] / d->r[0]);
82 template <
typename Scalar>
83 boost::shared_ptr<ResidualDataAbstractTpl<Scalar> >
86 return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(),
this,
90 template <
typename Scalar>
91 const typename pinocchio::FrameIndex&
96 template <
typename Scalar>
98 const pinocchio::FrameIndex& fid) {
102 template <
typename Scalar>
103 const typename pinocchio::FrameIndex&
108 template <
typename Scalar>
110 const pinocchio::FrameIndex& fid) {
Matrix6xs J2
Definition: residual-feet-collision.hpp:158
void set_frame_id2(const pinocchio::FrameIndex &fid2)
Set the second frame index in the collision pair.
Definition: residual-feet-collision.hxx:109
DataCollectorAbstractTpl< Scalar > DataCollectorAbstract
Definition: residual-feet-collision.hpp:39
Matrix6xs J1
Definition: residual-feet-collision.hpp:158
const pinocchio::FrameIndex & get_frame_id2() const
Return the second frame index in the collision pair.
Definition: residual-feet-collision.hxx:104
void set_frame_id1(const pinocchio::FrameIndex &fid1)
Set the first frame index in the collision pair.
Definition: residual-feet-collision.hxx:97
pinocchio::DataTpl< Scalar > * pinocchio
Pinocchio data.
Definition: residual-feet-collision.hpp:157
Definition: residual-feet-collision.hpp:119
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 the CoM velocity residual.
Definition: residual-feet-collision.hxx:61
virtual ~ResidualModelFeetCollisionTpl()
Definition: residual-feet-collision.hxx:38
Definition: activation-quad-ref.hpp:19
const pinocchio::FrameIndex & get_frame_id1() const
Return the first frame index in the collision pair.
Definition: residual-feet-collision.hxx:92
virtual void calc(const boost::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the residual.
Definition: residual-feet-collision.hxx:41
ResidualModelAbstractTpl< Scalar > Base
Definition: residual-feet-collision.hpp:35
ResidualModelFeetCollisionTpl(boost::shared_ptr< StateMultibody > state, const pinocchio::FrameIndex frame_id1, const pinocchio::FrameIndex frame_id2, const std::size_t nu)
Initialize the residual model.
Definition: residual-feet-collision.hxx:18
Vector3s p1p2
Definition: residual-feet-collision.hpp:160
Matrix2xs dJ
Definition: residual-feet-collision.hpp:159
virtual boost::shared_ptr< ResidualDataAbstract > createData(DataCollectorAbstract *const data)
Definition: residual-feet-collision.hxx:84