9 #include <crocoddyl/core/utils/exception.hpp> 10 #include <pinocchio/algorithm/frames-derivatives.hpp> 11 #include <pinocchio/algorithm/frames.hpp> 13 #include "sobec/residual-feet-collision.hpp" 17 template <
typename Scalar>
18 ResidualModelFeetCollisionTpl<Scalar>::ResidualModelFeetCollisionTpl(
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>
28 ResidualModelFeetCollisionTpl<Scalar>::ResidualModelFeetCollisionTpl(
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>
38 ResidualModelFeetCollisionTpl<Scalar>::~ResidualModelFeetCollisionTpl() {}
40 template <
typename Scalar>
41 void ResidualModelFeetCollisionTpl<Scalar>::calc(
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>
61 void ResidualModelFeetCollisionTpl<Scalar>::calcDiff(
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> >
84 ResidualModelFeetCollisionTpl<Scalar>::createData(
85 DataCollectorAbstract*
const data) {
86 return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(),
this,
90 template <
typename Scalar>
91 const typename pinocchio::FrameIndex&
92 ResidualModelFeetCollisionTpl<Scalar>::get_frame_id1()
const {
96 template <
typename Scalar>
97 void ResidualModelFeetCollisionTpl<Scalar>::set_frame_id1(
98 const pinocchio::FrameIndex& fid) {
102 template <
typename Scalar>
103 const typename pinocchio::FrameIndex&
104 ResidualModelFeetCollisionTpl<Scalar>::get_frame_id2()
const {
108 template <
typename Scalar>
109 void ResidualModelFeetCollisionTpl<Scalar>::set_frame_id2(
110 const pinocchio::FrameIndex& fid) {
Definition: contact-force.hxx:11