10 #include "crocoddyl/core/utils/exception.hpp" 11 #include "crocoddyl/multibody/contacts/contact-6d.hpp" 13 #include <pinocchio/algorithm/frames.hpp> 14 #include <pinocchio/algorithm/kinematics-derivatives.hpp> 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) {}
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) {}
28 template <
typename Scalar>
29 ContactModel6DTpl<Scalar>::~ContactModel6DTpl() {}
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);
38 d->a = pinocchio::getFrameAcceleration(*state_->get_pinocchio().get(), *d->pinocchio, Mref_.frame);
39 d->a0 = d->a.toVector();
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();
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();
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;
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;
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;
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)");
78 ContactData6D* d =
static_cast<ContactData6D*
>(data.get());
79 data->f = d->jMf.act(pinocchio::ForceTpl<Scalar>(force));
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);
88 template <
typename Scalar>
89 const FramePlacementTpl<Scalar>& ContactModel6DTpl<Scalar>::get_Mref()
const {
93 template <
typename Scalar>
94 const typename MathBaseTpl<Scalar>::Vector2s& ContactModel6DTpl<Scalar>::get_gains()
const {
Definition: action-base.hxx:11