9 #include "contact-force.hpp"
12 namespace newcontacts {
14 template <
typename Scalar>
15 ResidualModelContactForceTpl<Scalar>::ResidualModelContactForceTpl(
16 boost::shared_ptr<StateMultibody> state,
const pinocchio::FrameIndex
id,
17 const Force& fref,
const std::size_t nc,
const std::size_t nu)
18 : Base(state,
id, fref, nc, nu) {}
20 template <
typename Scalar>
21 ResidualModelContactForceTpl<Scalar>::ResidualModelContactForceTpl(
22 boost::shared_ptr<StateMultibody> state,
const pinocchio::FrameIndex
id,
23 const Force& fref,
const std::size_t nc)
24 : Base(state,
id, fref, nc) {}
26 template <
typename Scalar>
27 ResidualModelContactForceTpl<Scalar>::~ResidualModelContactForceTpl() {}
29 template <
typename Scalar>
30 void ResidualModelContactForceTpl<Scalar>::calc(
31 const boost::shared_ptr<ResidualDataAbstract>& data,
32 const Eigen::Ref<const VectorXs>&,
const Eigen::Ref<const VectorXs>&) {
33 Data* d =
static_cast<Data*
>(data.get());
36 switch (d->contact_type) {
38 ContactData1DTpl<Scalar>* d1d =
39 static_cast<ContactData1DTpl<Scalar>*
>(d->contact.get());
40 if (d1d->type == pinocchio::LOCAL) {
41 data->r = d->contact->jMf.rotation().transpose().row(d1d->mask) *
42 d->contact->f.linear() -
43 this->get_reference().linear().row(d1d->mask);
44 }
else if (d1d->type == pinocchio::WORLD ||
45 d1d->type == pinocchio::LOCAL_WORLD_ALIGNED) {
47 (d1d->oRf * d->contact->jMf.rotation().transpose()).row(d1d->mask) *
48 d->contact->f.linear() -
49 this->get_reference().linear().row(d1d->mask);
54 ContactData3DTpl<Scalar>* d3d =
55 static_cast<ContactData3DTpl<Scalar>*
>(d->contact.get());
56 if (d3d->type == pinocchio::LOCAL) {
58 (d->contact->jMf.rotation().transpose() * d->contact->f.linear() -
59 this->get_reference().linear());
60 }
else if (d3d->type == pinocchio::WORLD ||
61 d3d->type == pinocchio::LOCAL_WORLD_ALIGNED) {
62 data->r = (d3d->oRf * d->contact->jMf.rotation().transpose() *
63 d->contact->f.linear() -
64 this->get_reference().linear());
69 data->r = (d->contact->jMf.actInv(d->contact->f) - this->get_reference())
77 template <
typename Scalar>
78 void ResidualModelContactForceTpl<Scalar>::calcDiff(
79 const boost::shared_ptr<ResidualDataAbstract>& data,
80 const Eigen::Ref<const VectorXs>&,
const Eigen::Ref<const VectorXs>&) {
81 Data* d =
static_cast<Data*
>(data.get());
83 const MatrixXs& df_dx = d->contact->df_dx;
84 const MatrixXs& df_du = d->contact->df_du;
86 switch (d->contact_type) {
88 data->Rx = df_dx.template topRows<1>();
89 data->Ru = df_du.template topRows<1>();
93 data->Rx = df_dx.template topRows<3>();
94 data->Ru = df_du.template topRows<3>();
Definition: contact-force.hxx:11