Skip to content

Commit

Permalink
[pre-commit.ci] auto fixes from pre-commit.com hooks
Browse files Browse the repository at this point in the history
for more information, see https://pre-commit.ci
  • Loading branch information
pre-commit-ci[bot] committed Oct 10, 2024
1 parent 6ea5e9e commit 62fe727
Showing 1 changed file with 13 additions and 9 deletions.
22 changes: 13 additions & 9 deletions include/crocoddyl/multibody/contacts/contact-6d-loop.hxx
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,7 @@ ContactModel6DLoopTpl<Scalar>::ContactModel6DLoopTpl(
gains_(gains) {
if (ref != pinocchio::ReferenceFrame::LOCAL) {
std::cerr << "Warning: Only reference frame pinocchio::LOCAL is supported "
"for 6D loop contacts"
"for 6D loop contacts"
<< std::endl;
}
}
Expand Down Expand Up @@ -126,19 +126,23 @@ void ContactModel6DLoopTpl<Scalar>::calcDiff(
pinocchio::LOCAL, d->v2_partial_dq, d->a2_partial_dq, d->a2_partial_dv,
d->a2_partial_da);

d->da0_dq_t1.noalias() = joint1_placement_.toActionMatrixInverse() * d->a1_partial_dq;
d->da0_dq_t1.noalias() =
joint1_placement_.toActionMatrixInverse() * d->a1_partial_dq;

d->da0_dq_t2.noalias() = d->f1af2.toActionMatrix() * d->Jc;
d->f2_a2_partial_dq.noalias() = joint2_placement_.toActionMatrixInverse() *
d->a2_partial_dq;
d->f2_a2_partial_dq.noalias() =
joint2_placement_.toActionMatrixInverse() * d->a2_partial_dq;
d->da0_dq_t2.noalias() += d->f1Xf2 * d->f2_a2_partial_dq;

d->f1_v1_partial_dq.noalias() = joint1_placement_.toActionMatrixInverse() * d->v1_partial_dq;
d->da0_dq_t3.noalias() = - d->f1vf2.toActionMatrix() * d->f1_v1_partial_dq;

d->f1_v1_partial_dq.noalias() =
joint1_placement_.toActionMatrixInverse() * d->v1_partial_dq;
d->da0_dq_t3.noalias() = -d->f1vf2.toActionMatrix() * d->f1_v1_partial_dq;
d->da0_dq_t3_tmp.noalias() = d->f1vf2.toActionMatrix() * d->Jc;
d->da0_dq_t3.noalias() += d->f1vf1.toActionMatrix() * d->da0_dq_t3_tmp;
d->da0_dq_t3_tmp.noalias() = joint2_placement_.toActionMatrixInverse() * d->v2_partial_dq;
d->da0_dq_t3.noalias() += d->f1vf1.toActionMatrix() * d->f1Xf2 * d->da0_dq_t3_tmp;
d->da0_dq_t3_tmp.noalias() =
joint2_placement_.toActionMatrixInverse() * d->v2_partial_dq;
d->da0_dq_t3.noalias() +=
d->f1vf1.toActionMatrix() * d->f1Xf2 * d->da0_dq_t3_tmp;
d->da0_dx.leftCols(nv).noalias() = d->da0_dq_t1 - d->da0_dq_t2 + d->da0_dq_t3;

d->da0_dx.rightCols(nv).noalias() =
Expand Down

0 comments on commit 62fe727

Please sign in to comment.