12 namespace newcontacts {
14 template <
typename Scalar>
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>
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>
29 template <
typename Scalar>
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) {
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);
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>
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>();