contact-6d.hxx
1 
3 // BSD 3-Clause License
4 //
5 // Copyright (C) 2018-2020, LAAS-CNRS, University of Edinburgh
6 // Copyright note valid unless otherwise stated in individual files.
7 // All rights reserved.
9 
10 #include "crocoddyl/core/utils/exception.hpp"
11 #include "crocoddyl/multibody/contacts/contact-6d.hpp"
12 
13 #include <pinocchio/algorithm/frames.hpp>
14 #include <pinocchio/algorithm/kinematics-derivatives.hpp>
15 
16 namespace crocoddyl {
17 
18 template <typename Scalar>
19 ContactModel6DTpl<Scalar>::ContactModel6DTpl(boost::shared_ptr<StateMultibody> state, const FramePlacement& Mref,
20  const std::size_t& nu, const Vector2s& gains)
21  : Base(state, 6, nu), Mref_(Mref), gains_(gains) {}
22 
23 template <typename Scalar>
24 ContactModel6DTpl<Scalar>::ContactModel6DTpl(boost::shared_ptr<StateMultibody> state, const FramePlacement& Mref,
25  const Vector2s& gains)
26  : Base(state, 6), Mref_(Mref), gains_(gains) {}
27 
28 template <typename Scalar>
29 ContactModel6DTpl<Scalar>::~ContactModel6DTpl() {}
30 
31 template <typename Scalar>
32 void ContactModel6DTpl<Scalar>::calc(const boost::shared_ptr<ContactDataAbstract>& data,
33  const Eigen::Ref<const VectorXs>&) {
34  ContactData6D* d = static_cast<ContactData6D*>(data.get());
35  pinocchio::updateFramePlacement(*state_->get_pinocchio().get(), *d->pinocchio, Mref_.frame);
36  pinocchio::getFrameJacobian(*state_->get_pinocchio().get(), *d->pinocchio, Mref_.frame, pinocchio::LOCAL, d->Jc);
37 
38  d->a = pinocchio::getFrameAcceleration(*state_->get_pinocchio().get(), *d->pinocchio, Mref_.frame);
39  d->a0 = d->a.toVector();
40 
41  if (gains_[0] != 0.) {
42  d->rMf = Mref_.oMf.inverse() * d->pinocchio->oMf[Mref_.frame];
43  d->a0 += gains_[0] * pinocchio::log6(d->rMf).toVector();
44  }
45  if (gains_[1] != 0.) {
46  d->v = pinocchio::getFrameVelocity(*state_->get_pinocchio().get(), *d->pinocchio, Mref_.frame);
47  d->a0 += gains_[1] * d->v.toVector();
48  }
49 }
50 
51 template <typename Scalar>
52 void ContactModel6DTpl<Scalar>::calcDiff(const boost::shared_ptr<ContactDataAbstract>& data,
53  const Eigen::Ref<const VectorXs>&) {
54  ContactData6D* d = static_cast<ContactData6D*>(data.get());
55  pinocchio::getJointAccelerationDerivatives(*state_->get_pinocchio().get(), *d->pinocchio, d->joint, pinocchio::LOCAL,
56  d->v_partial_dq, d->a_partial_dq, d->a_partial_dv, d->a_partial_da);
57  const std::size_t& nv = state_->get_nv();
58  d->da0_dx.leftCols(nv).noalias() = d->fXj * d->a_partial_dq;
59  d->da0_dx.rightCols(nv).noalias() = d->fXj * d->a_partial_dv;
60 
61  if (gains_[0] != 0.) {
62  pinocchio::Jlog6(d->rMf, d->rMf_Jlog6);
63  d->da0_dx.leftCols(nv).noalias() += gains_[0] * d->rMf_Jlog6 * d->Jc;
64  }
65  if (gains_[1] != 0.) {
66  d->da0_dx.leftCols(nv).noalias() += gains_[1] * d->fXj * d->v_partial_dq;
67  d->da0_dx.rightCols(nv).noalias() += gains_[1] * d->fXj * d->a_partial_da;
68  }
69 }
70 
71 template <typename Scalar>
72 void ContactModel6DTpl<Scalar>::updateForce(const boost::shared_ptr<ContactDataAbstract>& data,
73  const VectorXs& force) {
74  if (force.size() != 6) {
75  throw_pretty("Invalid argument: "
76  << "lambda has wrong dimension (it should be 6)");
77  }
78  ContactData6D* d = static_cast<ContactData6D*>(data.get());
79  data->f = d->jMf.act(pinocchio::ForceTpl<Scalar>(force));
80 }
81 
82 template <typename Scalar>
83 boost::shared_ptr<ContactDataAbstractTpl<Scalar> > ContactModel6DTpl<Scalar>::createData(
84  pinocchio::DataTpl<Scalar>* const data) {
85  return boost::make_shared<ContactData6D>(this, data);
86 }
87 
88 template <typename Scalar>
89 const FramePlacementTpl<Scalar>& ContactModel6DTpl<Scalar>::get_Mref() const {
90  return Mref_;
91 }
92 
93 template <typename Scalar>
94 const typename MathBaseTpl<Scalar>::Vector2s& ContactModel6DTpl<Scalar>::get_gains() const {
95  return gains_;
96 }
97 
98 } // namespace crocoddyl
Definition: action-base.hxx:11