contact-3d.hxx
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2018-2020, LAAS-CNRS, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 namespace crocoddyl {
10 
11 template <typename Scalar>
12 ContactModel3DTpl<Scalar>::ContactModel3DTpl(boost::shared_ptr<StateMultibody> state, const FrameTranslation& xref,
13  const std::size_t& nu, const Vector2s& gains)
14  : Base(state, 3, nu), xref_(xref), gains_(gains) {}
15 
16 template <typename Scalar>
17 ContactModel3DTpl<Scalar>::ContactModel3DTpl(boost::shared_ptr<StateMultibody> state, const FrameTranslation& xref,
18  const Vector2s& gains)
19  : Base(state, 3), xref_(xref), gains_(gains) {}
20 
21 template <typename Scalar>
22 ContactModel3DTpl<Scalar>::~ContactModel3DTpl() {}
23 
24 template <typename Scalar>
25 void ContactModel3DTpl<Scalar>::calc(const boost::shared_ptr<ContactDataAbstract>& data,
26  const Eigen::Ref<const VectorXs>&) {
27  ContactData3D* d = static_cast<ContactData3D*>(data.get());
28  pinocchio::updateFramePlacement(*state_->get_pinocchio().get(), *d->pinocchio, xref_.frame);
29  d->v = pinocchio::getFrameVelocity(*state_->get_pinocchio().get(), *d->pinocchio, xref_.frame);
30  d->vw = d->v.angular();
31  d->vv = d->v.linear();
32 
33  pinocchio::getFrameJacobian(*state_->get_pinocchio().get(), *d->pinocchio, xref_.frame, pinocchio::LOCAL, d->fJf);
34  d->Jc = d->fJf.template topRows<3>();
35 
36  d->a = pinocchio::getFrameAcceleration(*state_->get_pinocchio().get(), *d->pinocchio, xref_.frame);
37  d->a0 = d->a.linear() + d->vw.cross(d->vv);
38 
39  if (gains_[0] != 0.) {
40  d->a0 += gains_[0] * (d->pinocchio->oMf[xref_.frame].translation() - xref_.oxf);
41  }
42  if (gains_[1] != 0.) {
43  d->a0 += gains_[1] * d->vv;
44  }
45 }
46 
47 template <typename Scalar>
48 void ContactModel3DTpl<Scalar>::calcDiff(const boost::shared_ptr<ContactDataAbstract>& data,
49  const Eigen::Ref<const VectorXs>&) {
50  ContactData3D* d = static_cast<ContactData3D*>(data.get());
51  pinocchio::getJointAccelerationDerivatives(*state_->get_pinocchio().get(), *d->pinocchio, d->joint, pinocchio::LOCAL,
52  d->v_partial_dq, d->a_partial_dq, d->a_partial_dv, d->a_partial_da);
53  const std::size_t& nv = state_->get_nv();
54  pinocchio::skew(d->vv, d->vv_skew);
55  pinocchio::skew(d->vw, d->vw_skew);
56  d->fXjdv_dq.noalias() = d->fXj * d->v_partial_dq;
57  d->fXjda_dq.noalias() = d->fXj * d->a_partial_dq;
58  d->fXjda_dv.noalias() = d->fXj * d->a_partial_dv;
59  d->da0_dx.leftCols(nv).noalias() = d->fXjda_dq.template topRows<3>() +
60  d->vw_skew * d->fXjdv_dq.template topRows<3>() -
61  d->vv_skew * d->fXjdv_dq.template bottomRows<3>();
62  d->da0_dx.rightCols(nv).noalias() =
63  d->fXjda_dv.template topRows<3>() + d->vw_skew * d->Jc - d->vv_skew * d->fJf.template bottomRows<3>();
64 
65  if (gains_[0] != 0.) {
66  d->oRf = d->pinocchio->oMf[xref_.frame].rotation();
67  d->da0_dx.leftCols(nv).noalias() += gains_[0] * d->oRf * d->Jc;
68  }
69  if (gains_[1] != 0.) {
70  d->da0_dx.leftCols(nv).noalias() += gains_[1] * d->fXj.template topRows<3>() * d->v_partial_dq;
71  d->da0_dx.rightCols(nv).noalias() += gains_[1] * d->fXj.template topRows<3>() * d->a_partial_da;
72  }
73 }
74 
75 template <typename Scalar>
76 void ContactModel3DTpl<Scalar>::updateForce(const boost::shared_ptr<ContactDataAbstract>& data,
77  const VectorXs& force) {
78  if (force.size() != 3) {
79  throw_pretty("Invalid argument: "
80  << "lambda has wrong dimension (it should be 3)");
81  }
82  ContactData3D* d = static_cast<ContactData3D*>(data.get());
83  data->f = d->jMf.act(pinocchio::ForceTpl<Scalar>(force, Vector3s::Zero()));
84 }
85 
86 template <typename Scalar>
87 boost::shared_ptr<ContactDataAbstractTpl<Scalar> > ContactModel3DTpl<Scalar>::createData(
88  pinocchio::DataTpl<Scalar>* const data) {
89  return boost::make_shared<ContactData3D>(this, data);
90 }
91 
92 template <typename Scalar>
93 const FrameTranslationTpl<Scalar>& ContactModel3DTpl<Scalar>::get_xref() const {
94  return xref_;
95 }
96 
97 template <typename Scalar>
98 const typename MathBaseTpl<Scalar>::Vector2s& ContactModel3DTpl<Scalar>::get_gains() const {
99  return gains_;
100 }
101 
102 } // namespace crocoddyl
Definition: action-base.hxx:11