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