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) {}
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) {}
21 template <
typename Scalar>
22 ContactModel3DTpl<Scalar>::~ContactModel3DTpl() {}
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();
33 pinocchio::getFrameJacobian(*state_->get_pinocchio().get(), *d->pinocchio, xref_.frame, pinocchio::LOCAL, d->fJf);
34 d->Jc = d->fJf.template topRows<3>();
36 d->a = pinocchio::getFrameAcceleration(*state_->get_pinocchio().get(), *d->pinocchio, xref_.frame);
37 d->a0 = d->a.linear() + d->vw.cross(d->vv);
39 if (gains_[0] != 0.) {
40 d->a0 += gains_[0] * (d->pinocchio->oMf[xref_.frame].translation() - xref_.oxf);
42 if (gains_[1] != 0.) {
43 d->a0 += gains_[1] * d->vv;
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>();
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;
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;
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)");
82 ContactData3D* d =
static_cast<ContactData3D*
>(data.get());
83 data->f = d->jMf.act(pinocchio::ForceTpl<Scalar>(force, Vector3s::Zero()));
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);
92 template <
typename Scalar>
93 const FrameTranslationTpl<Scalar>& ContactModel3DTpl<Scalar>::get_xref()
const {
97 template <
typename Scalar>
98 const typename MathBaseTpl<Scalar>::Vector2s& ContactModel3DTpl<Scalar>::get_gains()
const {
Definition: action-base.hxx:11