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