Skip to content

Commit

Permalink
More kinematic loop computation simplification
Browse files Browse the repository at this point in the history
  • Loading branch information
@james-p-foster
james-p-foster committed Feb 19, 2025
1 parent 2304479 commit d77a1ee
Show file tree
Hide file tree
Showing 2 changed files with 119 additions and 134 deletions.
34 changes: 6 additions & 28 deletions include/crocoddyl/multibody/contacts/contact-6d-loop.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -192,24 +192,17 @@ struct ContactData6DLoopTpl : public ContactDataAbstractTpl<_Scalar> {
ContactData6DLoopTpl(Model<Scalar> *const model,
pinocchio::DataTpl<Scalar> *const data)
: Base(model, data, 2),
f1_v2_partial_dq(6, model->get_state()->get_nv()),
da0_dq_t1(6, model->get_state()->get_nv()),
da0_dq_t2(6, model->get_state()->get_nv()),
da0_dq_t2_tmp(6, model->get_state()->get_nv()),
da0_dq_t3(6, model->get_state()->get_nv()),
da0_dq_t3_tmp(6, model->get_state()->get_nv()),
dpos_dq(6, model->get_state()->get_nv()),
dvel_dq(6, model->get_state()->get_nv()),
dtau_dq_tmp(model->get_state()->get_nv(), model->get_state()->get_nv()),
f1Jf2(6, model->get_state()->get_nv()),
f1Mf2(SE3::Identity()),
f1Xf2(SE3ActionMatrix::Identity()),
f1vf2(Motion::Zero()),
f1af2(Motion::Zero()) {
if (force_datas.size() != 2 || nf != 2) {
f1af2(Motion::Zero()),
f_cross(6, 6) {
if (force_datas.size() != 2 || nf != 2)
throw_pretty(
"Invalid argument: " << "the force_datas has to be of size 2");
}

ForceDataAbstract &fdata1 = force_datas[0];
fdata1.frame = model->get_id(0);
Expand All @@ -223,17 +216,10 @@ struct ContactData6DLoopTpl : public ContactDataAbstractTpl<_Scalar> {
fdata2.fXj = fdata2.jMf.inverse().toActionMatrix();
fdata2.type = model->get_type();

f1_v2_partial_dq.setZero();
da0_dq_t1.setZero();
da0_dq_t2.setZero();
da0_dq_t2_tmp.setZero();
da0_dq_t3.setZero();
da0_dq_t3_tmp.setZero();
dpos_dq.setZero();
dvel_dq.setZero();
dtau_dq_tmp.setZero();
f1Jf2.setZero();
j2Jj1.setZero();
f_cross.setZero();
}

using Base::a0;
Expand All @@ -244,23 +230,15 @@ struct ContactData6DLoopTpl : public ContactDataAbstractTpl<_Scalar> {
// Intermediate calculations
Matrix6xs da0_dq_t1;
Matrix6xs da0_dq_t2;
Matrix6xs da0_dq_t2_tmp;
Matrix6xs da0_dq_t3;
Matrix6xs da0_dq_t3_tmp;
Matrix6xs dpos_dq;
Matrix6xs dvel_dq;
MatrixXs dtau_dq_tmp;

// Coupled terms
SE3 f1Mf2; //<! Relative placement of the contact frames in the first contact
// frame
SE3 f1Mf2;
SE3ActionMatrix f1Xf2; //<! Relative action matrix of the
// contact frames in the first contact frame
Motion f1vf2;
Motion f1af2;
Matrix6xs f1Jf2;
Matrix6xs j2Jj1;
Matrix6xs f1_v2_partial_dq;
Matrix6s f_cross;
};

} // namespace crocoddyl
Expand Down
219 changes: 113 additions & 106 deletions include/crocoddyl/multibody/contacts/contact-6d-loop.hxx
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,16 +23,14 @@ ContactModel6DLoopTpl<Scalar>::ContactModel6DLoopTpl(
const SE3 &joint2_placement, const pinocchio::ReferenceFrame type,
const std::size_t nu, const Vector2s &gains)
: Base(state, pinocchio::ReferenceFrame::LOCAL, 2, 6, nu), gains_(gains) {
if (type != pinocchio::ReferenceFrame::LOCAL) {
std::cerr << "Warning: Only reference frame pinocchio::LOCAL is supported "
"for 6D loop contacts"
<< std::endl;
}
if (joint1_id == 0 || joint2_id == 0) {
std::cerr << "Warning: At least one of the parents joints id is zero"
"you should use crocoddyl::ContactModel6D instead"
<< std::endl;
}
if (type != pinocchio::ReferenceFrame::LOCAL)
throw_pretty(
"Only reference frame pinocchio::LOCAL is supported "
"for 6D loop contacts");
if (joint1_id == 0 || joint2_id == 0)
throw_pretty(
"Either joint1_id or joint2_id is set to 0, cannot use form a "
"kinematic loop with the world");

id_[0] = joint1_id;
id_[1] = joint2_id;
Expand All @@ -48,21 +46,19 @@ ContactModel6DLoopTpl<Scalar>::ContactModel6DLoopTpl(
const SE3 &joint2_placement, const pinocchio::ReferenceFrame type,
const Vector2s &gains)
: Base(state, pinocchio::ReferenceFrame::LOCAL, 2, 6), gains_(gains) {
if (type != pinocchio::ReferenceFrame::LOCAL) {
std::cerr << "Warning: Only reference frame pinocchio::LOCAL is supported "
"for 6D loop contacts"
<< std::endl;
}
if (joint1_id == 0 || joint2_id == 0) {
std::cerr << "Warning: At least one of the parents joints id is zero"
"you should use crocoddyl::ContactModel6D instead"
<< std::endl;
}
if (type != pinocchio::ReferenceFrame::LOCAL)
throw_pretty(
"Only reference frame pinocchio::LOCAL is supported "
"for 6D loop contacts");
if (joint1_id == 0 || joint2_id == 0)
throw_pretty(
"Either joint1_id or joint2_id is set to 0, cannot use form a "
"kinematic loop with the world");

id_[0].frame = joint1_id;
id_[1].frame = joint2_id;
placements_[0].jMf = joint1_placement;
placements_[1].jMf = joint2_placement;
id_[0] = joint1_id;
id_[1] = joint2_id;
placements_[0] = joint1_placement;
placements_[1] = joint2_placement;
type_ = type;
}

Expand All @@ -74,39 +70,42 @@ void ContactModel6DLoopTpl<Scalar>::calc(
const boost::shared_ptr<ContactDataAbstract> &data,
const Eigen::Ref<const VectorXs> &) {
Data *d = static_cast<Data *>(data.get());
ForceDataAbstractTpl<Scalar> &fdata_1 = d->force_datas[0];
ForceDataAbstractTpl<Scalar> &fdata_2 = d->force_datas[1];
// TODO(jfoster): is this frame placement update call needed?
pinocchio::updateFramePlacements(*state_->get_pinocchio().get(),
*d->pinocchio);

for (int i = 0; i < nf_; ++i) {
ForceDataAbstractTpl<Scalar> &fdata_i = d->force_datas[i];
ForceDataAbstract &fdata_i = d->force_datas[i];
pinocchio::getJointJacobian(*state_->get_pinocchio().get(), *d->pinocchio,
fdata_i.frame, pinocchio::LOCAL, fdata_i.jJj);
fdata_i.fJf.noalias() = fdata_i.fXj * fdata_i.jJj;
fdata_i.fJf = fdata_i.fXj * fdata_i.jJj;
fdata_i.oMf = d->pinocchio->oMi[id_[i]].act(fdata_i.jMf);
}

ForceDataAbstract &fdata_1 = d->force_datas[0];
ForceDataAbstract &fdata_2 = d->force_datas[1];

d->f1Mf2 = fdata_1.oMf.actInv(fdata_2.oMf);
d->f1Xf2.noalias() = d->f1Mf2.toActionMatrix();
d->Jc.noalias() = fdata_1.fJf - d->f1Xf2 * fdata_2.fJf;
d->f1Xf2 = d->f1Mf2.toActionMatrix();
d->Jc = fdata_1.fJf - d->f1Xf2 * fdata_2.fJf;

// Compute the acceleration drift
for (int i = 0; i < nf_; ++i) {
ForceDataAbstractTpl<Scalar> &fdata_i = d->force_datas[i];
ForceDataAbstract &fdata_i = d->force_datas[i];
fdata_i.fvf = fdata_i.jMf.actInv(d->pinocchio->v[id_[i]]);
fdata_i.faf = fdata_i.jMf.actInv(d->pinocchio->a[id_[i]]);
}
d->f1vf2 = d->f1Mf2.act(fdata_2.fvf);
d->f1af2 = d->f1Mf2.act(fdata_2.faf);

d->a0.noalias() =
(fdata_1.faf - d->f1af2 + fdata_1.fvf.cross(d->f1vf2)).toVector();
d->a0 =
(fdata_1.faf - d->f1Mf2.act(fdata_2.faf) + fdata_1.fvf.cross(d->f1vf2))
.toVector();

if (std::abs<Scalar>(gains_[0]) > std::numeric_limits<Scalar>::epsilon()) {
d->a0.noalias() -= gains_[0] * pinocchio::log6(d->f1Mf2).toVector();
}
if (std::abs<Scalar>(gains_[1]) > std::numeric_limits<Scalar>::epsilon()) {
d->a0.noalias() += gains_[1] * (fdata_1.fvf - d->f1vf2).toVector();
}
if (gains_[0] != 0.0)
d->a0 += gains_[0] * -pinocchio::log6(d->f1Mf2).toVector();
if (gains_[1] != 0.0)
d->a0 += gains_[1] * (fdata_1.fvf - d->f1vf2).toVector();
}

template <typename Scalar>
Expand All @@ -115,98 +114,106 @@ void ContactModel6DLoopTpl<Scalar>::calcDiff(
const Eigen::Ref<const VectorXs> &) {
Data *d = static_cast<Data *>(data.get());
const std::size_t nv = state_->get_nv();
ForceDataAbstractTpl<Scalar> &fdata_1 = d->force_datas[0];
ForceDataAbstractTpl<Scalar> &fdata_2 = d->force_datas[1];
ForceDataAbstract &fdata_1 = d->force_datas[0];
ForceDataAbstract &fdata_2 = d->force_datas[1];

for (int i = 0; i < nf_; ++i) {
ForceDataAbstractTpl<Scalar> &fdata_i = d->force_datas[i];
ForceDataAbstract &fdata_i = d->force_datas[i];
fdata_i.faf = fdata_i.jMf.actInv(d->pinocchio->a[id_[i]]);
}
d->f1af2 = d->f1Mf2.act(d->force_datas[1].faf);
d->f1af2 = d->f1Mf2.act(fdata_2.faf);

for (int i = 0; i < nf_; ++i) {
ForceDataAbstractTpl<Scalar> &fdata_i = d->force_datas[i];
ForceDataAbstract &fdata_i = d->force_datas[i];

pinocchio::getJointAccelerationDerivatives(
*state_->get_pinocchio().get(), *d->pinocchio, fdata_i.frame,
pinocchio::LOCAL, fdata_i.v_partial_dq, fdata_i.a_partial_dq,
fdata_i.a_partial_dv, fdata_i.a_partial_da);
}

d->da0_dq_t1.noalias() =
fdata_1.jMf.toActionMatrixInverse() * fdata_1.a_partial_dq;

d->da0_dq_t2.noalias() = d->f1af2.toActionMatrix() * d->Jc;
fdata_2.f_a_partial_dq.noalias() =
fdata_2.jMf.toActionMatrixInverse() * fdata_2.a_partial_dq;
d->da0_dq_t2.noalias() += d->f1Xf2 * fdata_2.f_a_partial_dq;

fdata_1.f_v_partial_dq.noalias() =
fdata_1.jMf.toActionMatrixInverse() * fdata_1.v_partial_dq;
d->da0_dq_t3.noalias() = -d->f1vf2.toActionMatrix() * fdata_1.f_v_partial_dq;
d->da0_dq_t3_tmp.noalias() = d->f1vf2.toActionMatrix() * d->Jc;
d->da0_dq_t3.noalias() += fdata_1.fvf.toActionMatrix() * d->da0_dq_t3_tmp;
d->da0_dq_t3_tmp.noalias() =
fdata_2.jMf.toActionMatrixInverse() * fdata_2.v_partial_dq;
d->da0_dq_t3.noalias() +=
fdata_1.fvf.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;

fdata_2.f_a_partial_dv.noalias() =
fdata_2.jMf.toActionMatrixInverse() * fdata_2.a_partial_dv;
d->f1Jf2.noalias() = d->f1Xf2 * fdata_2.fJf;
d->da0_dx.rightCols(nv).noalias() =
fdata_1.jMf.toActionMatrixInverse() * fdata_1.a_partial_dv;
d->da0_dx.rightCols(nv).noalias() -= d->f1Xf2 * fdata_2.f_a_partial_dv;
d->da0_dx.rightCols(nv).noalias() -= d->f1vf2.toActionMatrix() * fdata_1.fJf;
d->da0_dx.rightCols(nv).noalias() += fdata_1.fvf.toActionMatrix() * d->f1Jf2;

if (std::abs<Scalar>(gains_[0]) > std::numeric_limits<Scalar>::epsilon()) {
d->da0_dq_t1 = fdata_1.jMf.toActionMatrixInverse() * fdata_1.a_partial_dq;

d->da0_dq_t2 =
d->f1af2.toActionMatrix() * (fdata_1.fJf - d->f1Xf2 * fdata_2.fJf) +
d->f1Xf2 * (fdata_2.jMf.toActionMatrixInverse() * fdata_2.a_partial_dq);
d->da0_dq_t3 =
-d->f1vf2.toActionMatrix() * (fdata_1.jMf.toActionMatrixInverse() *
fdata_1.v_partial_dq) // part 1
+ fdata_1.fvf.toActionMatrix() * d->f1vf2.toActionMatrix() *
(fdata_1.fJf - d->f1Xf2 * fdata_2.fJf) // part 2
+ fdata_1.fvf.toActionMatrix() * d->f1Xf2 *
(fdata_2.jMf.toActionMatrixInverse() *
fdata_2.v_partial_dq); // part 3

d->da0_dx.leftCols(nv) = d->da0_dq_t1 - d->da0_dq_t2 + d->da0_dq_t3;
d->da0_dx.rightCols(nv) =
fdata_1.jMf.toActionMatrixInverse() * fdata_1.a_partial_dv -
d->f1Xf2 * (fdata_2.jMf.toActionMatrixInverse() * fdata_2.a_partial_dv) -
d->f1vf2.toActionMatrix() * fdata_1.fJf +
fdata_1.fvf.toActionMatrix() * d->f1Xf2 * fdata_2.fJf;

if (gains_[0] != 0.0) {
Matrix6s f1Mf2_log6;
pinocchio::Jlog6(d->f1Mf2, f1Mf2_log6);
d->dpos_dq.noalias() = fdata_2.oMf.toActionMatrixInverse() *
fdata_1.oMf.toActionMatrix() * fdata_1.fJf;
d->dpos_dq.noalias() -= fdata_2.fJf;
d->da0_dx.leftCols(nv).noalias() += gains_[0] * f1Mf2_log6 * d->dpos_dq;
d->da0_dx.leftCols(nv) +=
gains_[0] *
(-f1Mf2_log6 * (-fdata_2.oMf.toActionMatrixInverse() *
fdata_1.oMf.toActionMatrix() * fdata_1.fJf +
fdata_2.fJf));
}
if (std::abs<Scalar>(gains_[1]) > std::numeric_limits<Scalar>::epsilon()) {
fdata_2.f_v_partial_dq.noalias() =
fdata_2.jMf.toActionMatrixInverse() * fdata_2.v_partial_dq;
d->f1_v2_partial_dq.noalias() = d->f1Xf2 * fdata_2.f_v_partial_dq;
d->dvel_dq.noalias() = fdata_1.f_v_partial_dq - d->f1_v2_partial_dq;
d->dvel_dq.noalias() -= d->f1vf2.toActionMatrix() * d->Jc;
d->da0_dx.leftCols(nv).noalias() += gains_[1] * d->dvel_dq;
d->da0_dx.rightCols(nv).noalias() += gains_[1] * d->Jc;
if (gains_[1] != 0.0) {
d->da0_dx.leftCols(nv) +=
gains_[1] *
(fdata_1.jMf.toActionMatrixInverse() * fdata_1.v_partial_dq -
d->f1Mf2.act(fdata_2.fvf).toActionMatrix() *
(fdata_1.fJf - d->f1Xf2 * fdata_2.fJf) -
d->f1Xf2 * fdata_2.jMf.toActionMatrixInverse() * fdata_2.v_partial_dq);
d->da0_dx.rightCols(nv) +=
gains_[1] * (fdata_1.fJf - d->f1Xf2 * fdata_2.fJf);
}
}

template <typename Scalar>
void ContactModel6DLoopTpl<Scalar>::updateForce(
const boost::shared_ptr<ContactDataAbstract> &data, const VectorXs &force) {
if (force.size() != 6) {
throw_pretty("Contact force vector has wrong dimension (expected 6 got "
<< force.size() << ")");
throw_pretty(
"Invalid argument: " << "lambda has wrong dimension (it should be 6)");
}
Data *d = static_cast<Data *>(data.get());
ForceDataAbstractTpl<Scalar> &fdata_1 = d->force_datas[0];
ForceDataAbstractTpl<Scalar> &fdata_2 = d->force_datas[1];

fdata_1.f = pinocchio::ForceTpl<Scalar>(-force);
fdata_1.fext = fdata_1.jMf.act(fdata_1.f);
fdata_2.f = -fdata_1.jMf.act(fdata_1.f);
fdata_2.fext = (fdata_2.jMf * d->f1Mf2.inverse()).act(fdata_1.f);

Matrix6s f_cross = Matrix6s::Zero(6, 6);
f_cross.template topRightCorner<3, 3>() = pinocchio::skew(fdata_2.f.linear());
f_cross.template bottomLeftCorner<3, 3>() =
pinocchio::skew(fdata_2.f.linear());
f_cross.template bottomRightCorner<3, 3>() =
pinocchio::skew(fdata_2.f.angular());

SE3 j2Mj1 = fdata_2.jMf.act(d->f1Mf2.actInv(fdata_1.jMf.inverse()));
d->j2Jj1.noalias() = j2Mj1.toActionMatrix() * fdata_1.jJj;
d->dtau_dq_tmp.noalias() = -f_cross * (fdata_2.jJj - d->j2Jj1);
d->dtau_dq.noalias() = fdata_2.jJj.transpose() * d->dtau_dq_tmp;
ForceDataAbstract &fdata_1 = d->force_datas[0];
ForceDataAbstract &fdata_2 = d->force_datas[1];

pinocchio::ForceTpl<Scalar> f = pinocchio::ForceTpl<Scalar>(-force);
switch (type_) {
case pinocchio::ReferenceFrame::LOCAL: {
// TODO(jfoster): unsure if this logic is correct
fdata_1.f = fdata_1.jMf.act(f);
fdata_1.fext = -fdata_1.jMf.act(f);
fdata_2.f = -(fdata_2.jMf * d->f1Mf2.inverse()).act(f);
fdata_2.fext = (fdata_2.jMf * d->f1Mf2.inverse()).act(f);

d->dtau_dq.setZero();
d->f_cross.setZero();
d->f_cross.topRightCorner(3, 3) = pinocchio::skew(fdata_2.fext.linear());
d->f_cross.bottomLeftCorner(3, 3) =
pinocchio::skew(fdata_2.fext.linear());
d->f_cross.bottomRightCorner(3, 3) =
pinocchio::skew(fdata_2.fext.angular());

SE3 j2Mj1 = fdata_2.jMf.act(d->f1Mf2.actInv(fdata_1.jMf.inverse()));
d->dtau_dq = fdata_2.jJj.transpose() * (-d->f_cross * (fdata_2.jJj - j2Mj1.toActionMatrix() * fdata_1.jJj));
break;
}
case pinocchio::ReferenceFrame::WORLD:
throw_pretty(
"Reference frame WORLD is not implemented for kinematic loops");
case pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED:
throw_pretty(
"Reference frame LOCAL_WORLD_ALIGNED is not implemented for "
"kinematic loops");
}
}

template <typename Scalar>
Expand Down

0 comments on commit d77a1ee

Please sign in to comment.