residual-feet-collision.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-feet-collision.hpp"
14 
15 namespace sobec {
16 using namespace crocoddyl;
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),
23  frame_id1(frame_id1),
24  frame_id2(frame_id2),
25  pin_model_(*state->get_pinocchio()) {}
26 
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),
33  frame_id1(frame_id1),
34  frame_id2(frame_id2),
35  pin_model_(*state->get_pinocchio()) {}
36 
37 template <typename Scalar>
38 ResidualModelFeetCollisionTpl<Scalar>::~ResidualModelFeetCollisionTpl() {}
39 
40 template <typename Scalar>
41 void ResidualModelFeetCollisionTpl<Scalar>::calc(
42  const boost::shared_ptr<ResidualDataAbstract>& data,
43  const Eigen::Ref<const VectorXs>& /*x*/,
44  const Eigen::Ref<const VectorXs>&) {
45  // Compute the residual residual give the reference CoM velocity
46 
47  Data* d = static_cast<Data*>(data.get());
48 
49  pinocchio::updateFramePlacement(pin_model_, *d->pinocchio, frame_id1);
50  pinocchio::updateFramePlacement(pin_model_, *d->pinocchio, frame_id2);
51 
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();
56  d->p1p2 = p1 - p2;
57  d->r[0] = d->p1p2.template head<2>().norm();
58 }
59 
60 template <typename Scalar>
61 void ResidualModelFeetCollisionTpl<Scalar>::calcDiff(
62  const boost::shared_ptr<ResidualDataAbstract>& data,
63  const Eigen::Ref<const VectorXs>& /*x*/,
64  const Eigen::Ref<const VectorXs>&) {
65  Data* d = static_cast<Data*>(data.get());
66  // const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic>
67  // q = x.head(state_->get_nq()); const Eigen::VectorBlock<const
68  // Eigen::Ref<const VectorXs>, Eigen::Dynamic> v = x.tail(state_->get_nv());
69 
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);
75 
76  d->dJ = d->J1.template topRows<2>() - d->J2.template topRows<2>();
77 
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]);
80 }
81 
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,
87  data);
88 }
89 
90 template <typename Scalar>
91 const typename pinocchio::FrameIndex&
92 ResidualModelFeetCollisionTpl<Scalar>::get_frame_id1() const {
93  return frame_id1;
94 }
95 
96 template <typename Scalar>
97 void ResidualModelFeetCollisionTpl<Scalar>::set_frame_id1(
98  const pinocchio::FrameIndex& fid) {
99  frame_id1 = fid;
100 }
101 
102 template <typename Scalar>
103 const typename pinocchio::FrameIndex&
104 ResidualModelFeetCollisionTpl<Scalar>::get_frame_id2() const {
105  return frame_id2;
106 }
107 
108 template <typename Scalar>
109 void ResidualModelFeetCollisionTpl<Scalar>::set_frame_id2(
110  const pinocchio::FrameIndex& fid) {
111  frame_id2 = fid;
112 }
113 
114 } // namespace sobec
Definition: contact-force.hxx:11