9 #include "contact-force.hpp" 13 template <
typename Scalar>
14 ResidualModelContactForceTpl<Scalar>::ResidualModelContactForceTpl(
15 boost::shared_ptr<StateMultibody> state,
const pinocchio::FrameIndex
id,
16 const Force& fref,
const std::size_t nc,
const std::size_t nu)
17 : Base(state,
id, fref, nc, nu) {}
19 template <
typename Scalar>
20 ResidualModelContactForceTpl<Scalar>::ResidualModelContactForceTpl(
21 boost::shared_ptr<StateMultibody> state,
const pinocchio::FrameIndex
id,
22 const Force& fref,
const std::size_t nc)
23 : Base(state,
id, fref, nc) {}
25 template <
typename Scalar>
26 ResidualModelContactForceTpl<Scalar>::~ResidualModelContactForceTpl() {}
28 template <
typename Scalar>
29 void ResidualModelContactForceTpl<Scalar>::calc(
30 const boost::shared_ptr<ResidualDataAbstract>& data,
31 const Eigen::Ref<const VectorXs>&,
const Eigen::Ref<const VectorXs>&) {
32 Data* d =
static_cast<Data*
>(data.get());
35 switch (d->contact_type) {
37 sobec::ContactData1DTpl<Scalar>* d1d =
38 static_cast<sobec::ContactData1DTpl<Scalar>*
>(d->contact.get());
39 if (d1d->type == pinocchio::LOCAL) {
40 data->r = d->contact->jMf.rotation().transpose().row(d1d->mask) *
41 d->contact->f.linear() -
42 this->get_reference().linear().row(d1d->mask);
43 }
else if (d1d->type == pinocchio::WORLD ||
44 d1d->type == pinocchio::LOCAL_WORLD_ALIGNED) {
46 (d1d->oRf * d->contact->jMf.rotation().transpose()).row(d1d->mask) *
47 d->contact->f.linear() -
48 this->get_reference().linear().row(d1d->mask);
53 sobec::ContactData3DTpl<Scalar>* d3d =
54 static_cast<sobec::ContactData3DTpl<Scalar>*
>(d->contact.get());
55 if (d3d->type == pinocchio::LOCAL) {
57 (d->contact->jMf.rotation().transpose() * d->contact->f.linear() -
58 this->get_reference().linear());
59 }
else if (d3d->type == pinocchio::WORLD ||
60 d3d->type == pinocchio::LOCAL_WORLD_ALIGNED) {
61 data->r = (d3d->oRf * d->contact->jMf.rotation().transpose() *
62 d->contact->f.linear() -
63 this->get_reference().linear());
68 data->r = (d->contact->jMf.actInv(d->contact->f) - this->get_reference())
76 template <
typename Scalar>
77 void ResidualModelContactForceTpl<Scalar>::calcDiff(
78 const boost::shared_ptr<ResidualDataAbstract>& data,
79 const Eigen::Ref<const VectorXs>&,
const Eigen::Ref<const VectorXs>&) {
80 Data* d =
static_cast<Data*
>(data.get());
82 const MatrixXs& df_dx = d->contact->df_dx;
83 const MatrixXs& df_du = d->contact->df_du;
85 switch (d->contact_type) {
87 data->Rx = df_dx.template topRows<1>();
88 data->Ru = df_du.template topRows<1>();
92 data->Rx = df_dx.template topRows<3>();
93 data->Ru = df_du.template topRows<3>();
Definition: contact-force.hxx:11