Skip to content

Commit

Permalink
Review changes - add noalias, change operation order and fix alocation
Browse files Browse the repository at this point in the history
  • Loading branch information
@LudovicDeMatteis
LudovicDeMatteis committed Oct 9, 2024
1 parent c5fdaf0 commit db0183e
Show file tree
Hide file tree
Showing 3 changed files with 60 additions and 71 deletions.
3 changes: 1 addition & 2 deletions include/crocoddyl/multibody/contacts/contact-6d-loop.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -235,6 +235,7 @@ struct ContactData6DLoopTpl : public ContactDataAbstractTpl<_Scalar> {
using Base::da0_dx;
using Base::df_du;
using Base::df_dx;
using Base::dtau_dq;
using Base::f;
using Base::Jc;
using Base::pinocchio;
Expand Down Expand Up @@ -276,8 +277,6 @@ struct ContactData6DLoopTpl : public ContactDataAbstractTpl<_Scalar> {
Force joint1_f;
Force joint2_f;
Force f_local;

Matrix6s f_cross;
};

} // namespace crocoddyl
Expand Down
120 changes: 55 additions & 65 deletions include/crocoddyl/multibody/contacts/contact-6d-loop.hxx
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,14 +23,15 @@ ContactModel6DLoopTpl<Scalar>::ContactModel6DLoopTpl(
const SE3 &joint2_placement, const pinocchio::ReferenceFrame ref,
const std::size_t nu, const Vector2s &gains)
: Base(state, pinocchio::ReferenceFrame::LOCAL, 6, nu),
gains_(gains),
joint1_id_(joint1_id),
joint2_id_(joint2_id),
joint1_placement_(joint1_placement),
joint2_placement_(joint2_placement) {
joint2_placement_(joint2_placement),
gains_(gains) {
if (ref != pinocchio::ReferenceFrame::LOCAL) {
std::cerr << "Warning: Only reference frame LOCAL is supported for 6D loop "
"contacts\n";
std::cerr << "Warning: Only reference frame pinocchio::LOCAL is supported for 6D loop "
"contacts\n"
<< std::endl;
}
}

Expand All @@ -41,14 +42,15 @@ ContactModel6DLoopTpl<Scalar>::ContactModel6DLoopTpl(
const SE3 &joint2_placement, const pinocchio::ReferenceFrame ref,
const Vector2s &gains)
: Base(state, pinocchio::ReferenceFrame::LOCAL, 6),
gains_(gains),
joint1_id_(joint1_id),
joint2_id_(joint2_id),
joint1_placement_(joint1_placement),
joint2_placement_(joint2_placement) {
joint2_placement_(joint2_placement),
gains_(gains) {
if (ref != pinocchio::ReferenceFrame::LOCAL) {
std::cerr << "Warning: Only reference frame LOCAL is supported for 6D loop "
"contacts\n";
std::cerr << "Warning: Only reference frame pinocchio::LOCAL is supported for 6D loop "
"contacts\n"
<< std::endl;
}
}

Expand All @@ -62,46 +64,40 @@ void ContactModel6DLoopTpl<Scalar>::calc(
Data *d = static_cast<Data *>(data.get());
pinocchio::updateFramePlacements<Scalar>(*state_->get_pinocchio().get(),
*d->pinocchio);
d->j1Xf1 = joint1_placement_.toActionMatrix();
d->j2Xf2 = joint2_placement_.toActionMatrix();
d->j1Xf1.noalias() = joint1_placement_.toActionMatrix();
d->j2Xf2.noalias() = joint2_placement_.toActionMatrix();

pinocchio::getJointJacobian(*state_->get_pinocchio().get(), *d->pinocchio,
joint1_id_, pinocchio::LOCAL, d->j1Jj1);
pinocchio::getJointJacobian(*state_->get_pinocchio().get(), *d->pinocchio,
joint2_id_, pinocchio::LOCAL, d->j2Jj2);
d->f1Jf1 = d->j1Xf1.inverse() * d->j1Jj1;
d->f2Jf2 = d->j2Xf2.inverse() * d->j2Jj2;
d->f1Jf1.noalias() = d->j1Xf1.inverse() * d->j1Jj1;
d->f2Jf2.noalias() = d->j2Xf2.inverse() * d->j2Jj2;

d->oMf1 = d->pinocchio->oMi[joint1_id_].act(joint1_placement_);
d->oMf2 = d->pinocchio->oMi[joint2_id_].act(joint2_placement_);
d->f1Mf2 = d->oMf1.actInv(d->oMf2);
d->f1Xf2 = d->f1Mf2.toActionMatrix();
d->f1Xf2.noalias() = d->f1Mf2.toActionMatrix();

d->Jc = d->f1Jf1 - d->f1Xf2 * d->f2Jf2;
d->Jc.noalias() = d->f1Jf1 - d->f1Xf2 * d->f2Jf2;
// Compute the acceleration drift
if (joint1_id_ > 0) {
d->f1vf1 = joint1_placement_.actInv(d->pinocchio->v[joint1_id_]);
d->f1af1 = joint1_placement_.actInv(d->pinocchio->a[joint1_id_]);
} else {
d->f1vf1.setZero();
d->f1af1.setZero();
}
if (joint2_id_ > 0) {
d->f2vf2 = joint2_placement_.actInv(d->pinocchio->v[joint2_id_]);
d->f2af2 = joint2_placement_.actInv(d->pinocchio->a[joint2_id_]);
} else {
d->f2vf2.setZero();
d->f2af2.setZero();
d->f1vf2 = d->f1Mf2.act(d->f2vf2);
d->f1af2 = d->f1Mf2.act(d->f2af2);
}
d->f1vf2 = d->f1Mf2.act(d->f2vf2);
d->f1af2 = d->f1Mf2.act(d->f2af2);
d->a0 =
d->a0.noalias() =
(d->f1af1 - d->f1Mf2.act(d->f2af2) + d->f1vf1.cross(d->f1vf2)).toVector();

if (gains_[0] != 0.) {
d->a0 += gains_[0] * (-pinocchio::log6(d->f1Mf2).toVector());
if (std::abs<Scalar>(gains_[0]) > std::numeric_limits<Scalar>::epsilon()) {
d->a0.noalias() -= gains_[0] * pinocchio::log6(d->f1Mf2).toVector();
}
if (gains_[1] != 0.) {
if (std::abs<Scalar>(gains_[1]) > std::numeric_limits<Scalar>::epsilon()) {
d->a0 += gains_[1] * (d->f1vf1 - d->f1vf2).toVector();
}
}
Expand All @@ -115,15 +111,11 @@ void ContactModel6DLoopTpl<Scalar>::calcDiff(

if (joint1_id_ > 0) {
d->f1af1 = joint1_placement_.actInv(d->pinocchio->a[joint1_id_]);
} else {
d->f1af1.setZero();
}
if (joint2_id_ > 0) {
d->f2af2 = joint2_placement_.actInv(d->pinocchio->a[joint2_id_]);
} else {
d->f2af2.setZero();
d->f1af2 = d->f1Mf2.act(d->f2af2);
}
d->f1af2 = d->f1Mf2.act(d->f2af2);

pinocchio::getJointAccelerationDerivatives(
*state_->get_pinocchio().get(), *d->pinocchio, joint1_id_,
Expand All @@ -135,43 +127,44 @@ void ContactModel6DLoopTpl<Scalar>::calcDiff(
d->a2_partial_da);

d->da0_dq_t1 =
joint1_placement_.toActionMatrixInverse() * d->a1_partial_dq; //
joint1_placement_.toActionMatrixInverse() * d->a1_partial_dq;
d->da0_dq_t2 = (d->f1af2.toActionMatrix() * (d->f1Jf1 - d->f1Xf2 * d->f2Jf2) +
d->f1Xf2 * (joint2_placement_.toActionMatrixInverse() *
d->a2_partial_dq)); //
d->a2_partial_dq));
d->da0_dq_t3 =
-d->f1vf2.toActionMatrix() * (joint1_placement_.toActionMatrixInverse() *
d->v1_partial_dq) // part 1
d->v1_partial_dq)
+ d->f1vf1.toActionMatrix() * d->f1vf2.toActionMatrix() *
(d->f1Jf1 - d->f1Xf2 * d->f2Jf2) // part 2
(d->f1Jf1 - d->f1Xf2 * d->f2Jf2)
+ d->f1vf1.toActionMatrix() * d->f1Xf2 *
(joint2_placement_.toActionMatrixInverse() *
d->v2_partial_dq); // part 3
d->v2_partial_dq);

d->da0_dx.leftCols(nv) = d->da0_dq_t1 - d->da0_dq_t2 + d->da0_dq_t3;
d->da0_dx.leftCols(nv).noalias() = d->da0_dq_t1 - d->da0_dq_t2 + d->da0_dq_t3;
d->da0_dx.rightCols(nv) =
joint1_placement_.toActionMatrixInverse() * d->a1_partial_dv -
d->f1Xf2 *
(joint2_placement_.toActionMatrixInverse() * d->a2_partial_dv) -
d->f1vf2.toActionMatrix() * d->f1Jf1 +
d->f1vf1.toActionMatrix() * d->f1Xf2 * d->f2Jf2;
if (gains_[0] != 0.) {
if (std::abs<Scalar>(gains_[0]) > std::numeric_limits<Scalar>::epsilon()) {
Matrix6s f1Mf2_log6;
pinocchio::Jlog6(d->f1Mf2, f1Mf2_log6);
d->da0_dx.leftCols(nv) +=
gains_[0] * (-f1Mf2_log6 * (-d->oMf2.toActionMatrixInverse() *
d->oMf1.toActionMatrix() * d->f1Jf1 +
d->f2Jf2));
d->da0_dx.leftCols(nv).noalias() +=
gains_[0] * f1Mf2_log6 * (d->oMf2.toActionMatrixInverse() *
d->oMf1.toActionMatrix() * d->f1Jf1 -
d->f2Jf2);
}
if (gains_[1] != 0.) {
d->da0_dx.leftCols(nv) +=
gains_[1] *
(joint1_placement_.toActionMatrixInverse() * d->v1_partial_dq -
d->f1Mf2.act(d->f2vf2).toActionMatrix() *
(d->f1Jf1 - d->f1Xf2 * d->f2Jf2) -
d->f1Xf2 * joint2_placement_.toActionMatrixInverse() *
d->v2_partial_dq);
d->da0_dx.rightCols(nv) += gains_[1] * (d->f1Jf1 - d->f1Xf2 * d->f2Jf2);
if (std::abs<Scalar>(gains_[1]) > std::numeric_limits<Scalar>::epsilon()) {
d->da0_dx.leftCols(nv).noalias() +=
gains_[1] *
(joint1_placement_.toActionMatrixInverse() * d->v1_partial_dq -
d->f1Mf2.act(d->f2vf2).toActionMatrix() *
(d->f1Jf1 - d->f1Xf2 * d->f2Jf2) -
d->f1Xf2 * joint2_placement_.toActionMatrixInverse() *
d->v2_partial_dq);
d->da0_dx.rightCols(nv).noalias() +=
gains_[1] * (d->f1Jf1 - d->f1Xf2 * d->f2Jf2);
}
}

Expand All @@ -186,32 +179,29 @@ void ContactModel6DLoopTpl<Scalar>::updateForce(
d->f = pinocchio::ForceTpl<Scalar>(-force);
switch (type_) {
case pinocchio::ReferenceFrame::LOCAL: {
data->fext = joint1_placement_.act(data->f);
d->joint1_f = -joint1_placement_.act(data->f);
d->joint2_f = (joint2_placement_ * d->f1Mf2.inverse()).act(data->f);
d->fext = joint1_placement_.act(d->f);
d->joint1_f = -joint1_placement_.act(d->f);
d->joint2_f = (joint2_placement_ * d->f1Mf2.inverse()).act(d->f);

data->dtau_dq.setZero();

d->f_cross.setZero();
d->f_cross.topRightCorner(3, 3) = pinocchio::skew(d->joint2_f.linear());
d->f_cross.bottomLeftCorner(3, 3) = pinocchio::skew(d->joint2_f.linear());
d->f_cross.bottomRightCorner(3, 3) =
pinocchio::skew(d->joint2_f.angular());
Matrix6s f_cross = Matrix6s::Zero(6, 6);
f_cross.template topRightCorner<3, 3>() = pinocchio::skew(d->joint2_f.linear());
f_cross.template bottomLeftCorner<3, 3>() = pinocchio::skew(d->joint2_f.linear());
f_cross.template bottomRightCorner<3, 3>() = pinocchio::skew(d->joint2_f.angular());

SE3 j2Mj1 =
joint2_placement_.act(d->f1Mf2.actInv(joint1_placement_.inverse()));

data->dtau_dq =
d->dtau_dq.noalias() =
d->j2Jj2.transpose() *
(-d->f_cross * (d->j2Jj2 - j2Mj1.toActionMatrix() * d->j1Jj1));
(-f_cross * (d->j2Jj2 - j2Mj1.toActionMatrix() * d->j1Jj1));
break;
}
case pinocchio::ReferenceFrame::WORLD:
throw_pretty("Reference frame WORLD is not implemented yet");
throw_pretty("Reference frame pinocchio::WORLD is not implemented, please use pinocchio::LOCAL");
break;
case pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED:
throw_pretty(
"Reference frame LOCAL_WORLD_ALIGNED is not implemented yet");
"Reference frame pinocchio::LOCAL_WORLD_ALIGNED is not implemented, please use pinocchio::LOCAL");
break;
}
}
Expand Down
8 changes: 4 additions & 4 deletions include/crocoddyl/multibody/contacts/multiple-contacts.hxx
View file
Original file line number Diff line number Diff line change
Expand Up @@ -251,10 +251,10 @@ void ContactModelMultipleTpl<Scalar>::updateForce(
const Eigen::VectorBlock<const VectorXs, Eigen::Dynamic> force_i =
force.segment(nc, nc_i);
m_i->contact->updateForce(d_i, force_i);
ContactModel6DLoopTpl<Scalar>* c =
const ContactModel6DLoopTpl<Scalar>* c =
dynamic_cast<ContactModel6DLoopTpl<Scalar>*>(m_i->contact.get());
if (c != nullptr) {
ContactData6DLoopTpl<Scalar>* dc =
const ContactData6DLoopTpl<Scalar>* dc =
static_cast<ContactData6DLoopTpl<Scalar>*>(d_i.get());
const pinocchio::JointIndex joint1 = c->get_joint1_id();
const pinocchio::JointIndex joint2 = c->get_joint2_id();
Expand Down Expand Up @@ -288,10 +288,10 @@ void ContactModelMultipleTpl<Scalar>::updateForce(
const Eigen::VectorBlock<const VectorXs, Eigen::Dynamic> force_i =
force.segment(nc, nc_i);
m_i->contact->updateForce(d_i, force_i);
ContactModel6DLoopTpl<Scalar>* c =
const ContactModel6DLoopTpl<Scalar>* c =
dynamic_cast<ContactModel6DLoopTpl<Scalar>*>(m_i->contact.get());
if (c != nullptr) {
ContactData6DLoopTpl<Scalar>* dc =
const ContactData6DLoopTpl<Scalar>* dc =
static_cast<ContactData6DLoopTpl<Scalar>*>(d_i.get());
const pinocchio::JointIndex joint1 = c->get_joint1_id();
const pinocchio::JointIndex joint2 = c->get_joint2_id();
Expand Down

0 comments on commit db0183e

Please sign in to comment.