contact3d.hxx
Go to the documentation of this file.
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2021, LAAS-CNRS, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #include "contact3d.hpp"
10 
11 namespace sobec {
12 
13 template <typename Scalar>
14 ContactModel3DTpl<Scalar>::ContactModel3DTpl(
15  boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
16  const Vector3s& xref, const std::size_t nu, const Vector2s& gains,
17  const pinocchio::ReferenceFrame type)
18  : Base(state, id, Vector3s::Zero(), nu, Vector2s::Zero()),
19  xref_(xref),
20  gains_(gains),
21  type_(type) {}
22 
23 template <typename Scalar>
24 ContactModel3DTpl<Scalar>::ContactModel3DTpl(
25  boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
26  const Vector3s& xref, const Vector2s& gains,
27  const pinocchio::ReferenceFrame type)
28  : Base(state, id, Vector3s::Zero(), Vector2s::Zero()),
29  xref_(xref),
30  gains_(gains),
31  type_(type) {}
32 
33 template <typename Scalar>
34 ContactModel3DTpl<Scalar>::~ContactModel3DTpl() {}
35 
36 template <typename Scalar>
37 void ContactModel3DTpl<Scalar>::calc(
38  const boost::shared_ptr<crocoddyl::ContactDataAbstractTpl<Scalar>>& data,
39  const Eigen::Ref<const VectorXs>& x) {
40  Data* d = static_cast<Data*>(data.get());
41  pinocchio::updateFramePlacement(*state_->get_pinocchio().get(), *d->pinocchio,
42  id_);
43  pinocchio::getFrameJacobian(*state_->get_pinocchio().get(), *d->pinocchio,
44  id_, pinocchio::LOCAL, d->fJf);
45  d->v = pinocchio::getFrameVelocity(*state_->get_pinocchio().get(),
46  *d->pinocchio, id_, pinocchio::LOCAL);
47  d->vv = d->v.linear();
48  d->vw = d->v.angular();
49  d->Jc = d->fJf.template topRows<3>();
50  d->a0_temp_ =
51  pinocchio::getFrameClassicalAcceleration(
52  *state_->get_pinocchio().get(), *d->pinocchio, id_, pinocchio::LOCAL)
53  .linear();
54  d->oRf = d->pinocchio->oMf[id_].rotation();
55  // BAUMGARTE P
56  if (gains_[0] != 0.) {
57  d->a0_temp_ += gains_[0] * d->oRf.transpose() *
58  (d->pinocchio->oMf[id_].translation() - xref_);
59  }
60  // BAUMGARTE V
61  if (gains_[1] != 0.) {
62  d->a0_temp_ += gains_[1] * d->vv;
63  }
64  d->a0 = d->a0_temp_;
65  if (type_ == pinocchio::LOCAL_WORLD_ALIGNED || type_ == pinocchio::WORLD) {
66  d->a0 = d->oRf * d->a0_temp_;
67  d->Jc = d->oRf * d->fJf.template topRows<3>();
68  }
69 }
70 
71 template <typename Scalar>
72 void ContactModel3DTpl<Scalar>::calcDiff(
73  const boost::shared_ptr<crocoddyl::ContactDataAbstractTpl<Scalar>>& data,
74  const Eigen::Ref<const VectorXs>&) {
75  Data* d = static_cast<Data*>(data.get());
76  const pinocchio::JointIndex joint =
77  state_->get_pinocchio()->frames[d->frame].parent;
78  pinocchio::getJointAccelerationDerivatives(
79  *state_->get_pinocchio().get(), *d->pinocchio, joint, pinocchio::LOCAL,
80  d->v_partial_dq, d->a_partial_dq, d->a_partial_dv, d->a_partial_da);
81  const std::size_t nv = state_->get_nv();
82  pinocchio::skew(d->vv, d->vv_skew);
83  pinocchio::skew(d->vw, d->vw_skew);
84 
85  d->fXjdv_dq.noalias() = d->fXj * d->v_partial_dq;
86  d->fXjda_dq.noalias() = d->fXj * d->a_partial_dq;
87  d->fXjda_dv.noalias() = d->fXj * d->a_partial_dv;
88 
89  d->da0_dx_temp_.leftCols(nv) = d->fXjda_dq.template topRows<3>();
90  d->da0_dx_temp_.leftCols(nv).noalias() +=
91  d->vw_skew * d->fXjdv_dq.template topRows<3>();
92  d->da0_dx_temp_.leftCols(nv).noalias() -=
93  d->vv_skew * d->fXjdv_dq.template bottomRows<3>();
94  d->da0_dx_temp_.rightCols(nv) = d->fXjda_dv.template topRows<3>();
95  d->da0_dx_temp_.rightCols(nv).noalias() +=
96  d->vw_skew * d->fJf.template topRows<3>();
97  d->da0_dx_temp_.rightCols(nv).noalias() -=
98  d->vv_skew * d->fJf.template bottomRows<3>();
99 
100  if (gains_[0] != 0.) {
101  pinocchio::skew(
102  d->oRf.transpose() * (d->pinocchio->oMf[id_].translation() - xref_),
103  d->tmp_skew_);
104  d->da0_dx_temp_.leftCols(nv).noalias() +=
105  gains_[0] * (d->tmp_skew_ * d->fJf.template bottomRows<3>() +
106  d->fJf.template topRows<3>());
107  }
108 
109  if (gains_[1] != 0.) {
110  d->da0_dx_temp_.leftCols(nv).noalias() +=
111  gains_[1] * d->fXjdv_dq.template topRows<3>();
112  d->da0_dx_temp_.rightCols(nv).noalias() +=
113  gains_[1] * d->fJf.template topRows<3>();
114  }
115 
116  d->da0_dx = d->da0_dx_temp_;
117 
118  if (type_ == pinocchio::LOCAL_WORLD_ALIGNED || type_ == pinocchio::WORLD) {
119  // Need to recompute classical acceleration + BG correction (may not be
120  // equal to drift a0 anymore)
121  d->a0_temp_ = pinocchio::getFrameClassicalAcceleration(
122  *state_->get_pinocchio().get(), *d->pinocchio, id_,
123  pinocchio::LOCAL)
124  .linear();
125  if (gains_[0] != 0.) {
126  d->a0_temp_ += gains_[0] * d->oRf.transpose() *
127  (d->pinocchio->oMf[id_].translation() - xref_);
128  }
129  if (gains_[1] != 0.) {
130  d->a0_temp_ += gains_[1] * d->vv;
131  }
132  // Skew term due to LWA frame (is zero when classical acceleration = 0,
133  // which is the case in ContactFwdDynamics)
134  pinocchio::skew(d->oRf * d->a0_temp_, d->tmp_skew_);
135  d->da0_dx.leftCols(nv).noalias() =
136  d->oRf * d->da0_dx_temp_.leftCols(nv) -
137  d->tmp_skew_ * d->oRf * d->fJf.template bottomRows<3>();
138  d->da0_dx.rightCols(nv).noalias() = d->oRf * d->da0_dx_temp_.rightCols(nv);
139  }
140 }
141 
142 template <typename Scalar>
143 void ContactModel3DTpl<Scalar>::updateForce(
144  const boost::shared_ptr<crocoddyl::ContactDataAbstractTpl<Scalar>>& data,
145  const VectorXs& force) {
146  if (force.size() != 3) {
147  throw_pretty("Invalid argument: "
148  << "lambda has wrong dimension (it should be 3)");
149  }
150  Data* d = static_cast<Data*>(data.get());
151  d->oRf = d->pinocchio->oMf[id_].rotation();
152  if (type_ == pinocchio::LOCAL) {
153  data->f = d->jMf.act(pinocchio::ForceTpl<Scalar>(force, Vector3s::Zero()));
154  }
155  if (type_ == pinocchio::LOCAL_WORLD_ALIGNED || type_ == pinocchio::WORLD) {
156  data->f = d->jMf.act(pinocchio::ForceTpl<Scalar>(d->oRf.transpose() * force,
157  Vector3s::Zero()));
158  // Compute skew term to be added to rnea derivatives
159  pinocchio::skew(d->oRf.transpose() * force, d->tmp_skew_);
160  d->drnea_skew_term_ =
161  -d->fJf.topRows(3).transpose() * d->tmp_skew_ * d->fJf.bottomRows(3);
162  }
163 }
164 
165 template <typename Scalar>
166 boost::shared_ptr<crocoddyl::ContactDataAbstractTpl<Scalar>>
167 ContactModel3DTpl<Scalar>::createData(pinocchio::DataTpl<Scalar>* const data) {
168  return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this,
169  data);
170 }
171 
172 template <typename Scalar>
173 void ContactModel3DTpl<Scalar>::print(std::ostream& os) const {
174  os << "ContactModel3D {frame=" << state_->get_pinocchio()->frames[id_].name
175  << "}";
176 }
177 
178 template <typename Scalar>
179 const typename crocoddyl::MathBaseTpl<Scalar>::Vector3s&
180 ContactModel3DTpl<Scalar>::get_reference() const {
181  return xref_;
182 }
183 
184 template <typename Scalar>
185 const typename crocoddyl::MathBaseTpl<Scalar>::Vector2s&
186 ContactModel3DTpl<Scalar>::get_gains() const {
187  return gains_;
188 }
189 
190 template <typename Scalar>
191 void ContactModel3DTpl<Scalar>::set_reference(const Vector3s& reference) {
192  xref_ = reference;
193 }
194 
195 template <typename Scalar>
196 void ContactModel3DTpl<Scalar>::set_type(const pinocchio::ReferenceFrame type) {
197  type_ = type;
198 }
199 
200 template <typename Scalar>
201 const pinocchio::ReferenceFrame ContactModel3DTpl<Scalar>::get_type() const {
202  return type_;
203 }
204 
205 } // namespace sobec
Definition: contact-force.hxx:11