Got tests passing, removed most TODOs

include/crocoddyl/multibody/actions/contact-invdyn.hxx

@@ -106,7 +106,8 @@ void DifferentialActionModelContactInvDynamicsTpl<Scalar>::init(
          ++it_m) {
       const boost::shared_ptr<ContactItem>& contact = it_m->second;
       const std::string name = contact->name;
-      const pinocchio::FrameIndex id = contact->contact->get_id();
+      // TODO(jfoster): should this be generalised to account for loops? How would that work? Just indexing the first force for now
+      const pinocchio::FrameIndex id = contact->contact->get_id(0);
       const std::size_t nc_i = contact->contact->get_nc();
       const bool active = contact->active;
       constraints_->addConstraint(

include/crocoddyl/multibody/contact-base.hpp

@@ -140,12 +140,12 @@ class ContactModelAbstractTpl {
   /**
    * @brief Return the reference frame id
    */
-  pinocchio::FrameIndex get_id() const;
+  pinocchio::FrameIndex get_id(const int force_index) const;
 
   /**
    * @brief Modify the reference frame id
    */
-  void set_id(const pinocchio::FrameIndex id);
+  void set_id(const int force_index, const pinocchio::FrameIndex id);
 
   /**
    * @brief Modify the type of contact
@@ -178,7 +178,9 @@ class ContactModelAbstractTpl {
   std::size_t nf_;
   std::vector<pinocchio::FrameIndex>
       id_;  //!< Reference frame id of the contact
-  std::vector<pinocchio::ReferenceFrame> type_;  //!< Type of contact
+  std::vector<pinocchio::SE3> placements_;  //!< Placement of contact relative to parent
+  // TODO(jfoster): only allowing one type per contact model
+  pinocchio::ReferenceFrame type_;  //!< Type of contact
 };
 
 template <typename _Scalar>
@@ -196,7 +198,7 @@ struct ContactDataAbstractTpl : public InteractionDataAbstractTpl<_Scalar> {
   template <template <typename Scalar> class Model>
   ContactDataAbstractTpl(Model<Scalar>* const model,
                          pinocchio::DataTpl<Scalar>* const data, const int nf)
-      : InteractionDataAbstractTpl<Scalar>(model, data, nf),
+      : InteractionDataAbstractTpl<Scalar>(model, nf),
         pinocchio(data),
         a0(model->get_nc()),
         da0_dx(model->get_nc(), model->get_state()->get_ndx()),

include/crocoddyl/multibody/contact-base.hxx

@@ -15,17 +15,28 @@ namespace crocoddyl {
 template <typename Scalar>
 ContactModelAbstractTpl<Scalar>::ContactModelAbstractTpl(
     boost::shared_ptr<StateMultibody> state,
-    const pinocchio::ReferenceFrame type,
-    const std::size_t nf,
-    const std::size_t nc,
-    const std::size_t nu)
-    : state_(state), nc_(nc), nu_(nu), nf_(nf), id_(nf, 0), type_(nf, type) {}
+    const pinocchio::ReferenceFrame type, const std::size_t nf,
+    const std::size_t nc, const std::size_t nu)
+    : state_(state),
+      nc_(nc),
+      nu_(nu),
+      nf_(nf),
+      id_(nf, 0),
+      placements_(nf, pinocchio::SE3::Identity()),
+      type_(type) {}
 
 template <typename Scalar>
 ContactModelAbstractTpl<Scalar>::ContactModelAbstractTpl(
     boost::shared_ptr<StateMultibody> state,
-    const pinocchio::ReferenceFrame type, const std::size_t nf, const std::size_t nc)
-    : state_(state), nc_(nc), nu_(state->get_nv()), nf_(nf), id_(nf,0), type_(nf, type) {}
+    const pinocchio::ReferenceFrame type, const std::size_t nf,
+    const std::size_t nc)
+    : state_(state),
+      nc_(nc),
+      nu_(state->get_nv()),
+      nf_(nf),
+      id_(nf, 0),
+      placements_(nf, pinocchio::SE3::Identity()),
+      type_(type) {}
 
 template <typename Scalar>
 ContactModelAbstractTpl<Scalar>::ContactModelAbstractTpl(
@@ -76,8 +87,7 @@ void ContactModelAbstractTpl<Scalar>::updateForceDiff(
 template <typename Scalar>
 void ContactModelAbstractTpl<Scalar>::setZeroForce(
     const boost::shared_ptr<ContactDataAbstract>& data) const {
-  for (int i = 0; i < nf_; ++i)
-  {
+  for (size_t i = 0; i < nf_; ++i) {
     data->force_datas[i].f.setZero();
     data->force_datas[i].fext.setZero();
   }
@@ -125,24 +135,26 @@ std::size_t ContactModelAbstractTpl<Scalar>::get_nu() const {
 }
 
 template <typename Scalar>
-pinocchio::FrameIndex ContactModelAbstractTpl<Scalar>::get_id() const {
-  return id_[0]; // TODO(jfoster): index properly
+pinocchio::FrameIndex ContactModelAbstractTpl<Scalar>::get_id(
+    const int force_index) const {
+  return id_[force_index];
 }
 
 template <typename Scalar>
 pinocchio::ReferenceFrame ContactModelAbstractTpl<Scalar>::get_type() const {
-  return type_[0]; // TODO(jfoster): index properly
+  return type_;
 }
 
 template <typename Scalar>
-void ContactModelAbstractTpl<Scalar>::set_id(const pinocchio::FrameIndex id) {
-  id_[0] = id; // TODO(jfoster): index properly
+void ContactModelAbstractTpl<Scalar>::set_id(const int force_index,
+                                             const pinocchio::FrameIndex id) {
+  id_[force_index] = id;
 }
 
 template <typename Scalar>
 void ContactModelAbstractTpl<Scalar>::set_type(
     const pinocchio::ReferenceFrame type) {
-  type_[0] = type; // TODO(jfoster): index properly
+  type_ = type;
 }
 
 template <class Scalar>

include/crocoddyl/multibody/contacts/contact-1d.hpp

@@ -196,30 +196,20 @@ struct ContactData1DTpl : public ContactDataAbstractTpl<_Scalar> {
         v(Motion::Zero()),
         f_local(Force::Zero()),
         da0_local_dx(3, model->get_state()->get_ndx()),
-        fJf(6, model->get_state()->get_nv()),
-        v_partial_dq(6, model->get_state()->get_nv()),
-        a_partial_dq(6, model->get_state()->get_nv()),
-        a_partial_dv(6, model->get_state()->get_nv()),
-        a_partial_da(6, model->get_state()->get_nv()),
         fXjdv_dq(6, model->get_state()->get_nv()),
         fXjda_dq(6, model->get_state()->get_nv()),
         fXjda_dv(6, model->get_state()->get_nv()),
         fJf_df(3, model->get_state()->get_nv()) {
     // There is only one element in the force_datas vector
     ForceDataAbstract& fdata = force_datas[0];
-    fdata.frame = model->get_id();
+    fdata.frame = model->get_id(0);
     fdata.jMf = model->get_state()->get_pinocchio()->frames[fdata.frame].placement;
     fdata.fXj = fdata.jMf.inverse().toActionMatrix();
 
     a0_local.setZero();
     dp.setZero();
     dp_local.setZero();
     da0_local_dx.setZero();
-    fJf.setZero();
-    v_partial_dq.setZero();
-    a_partial_dq.setZero();
-    a_partial_dv.setZero();
-    a_partial_da.setZero();
     vv_skew.setZero();
     vw_skew.setZero();
     a0_skew.setZero();
@@ -244,11 +234,6 @@ struct ContactData1DTpl : public ContactDataAbstractTpl<_Scalar> {
   Vector3s dp_local;
   Force f_local;
   Matrix3xs da0_local_dx;
-  Matrix6xs fJf;
-  Matrix6xs v_partial_dq;
-  Matrix6xs a_partial_dq;
-  Matrix6xs a_partial_dv;
-  Matrix6xs a_partial_da;
   Matrix3s vv_skew;
   Matrix3s vw_skew;
   Matrix3s a0_skew;

include/crocoddyl/multibody/contacts/contact-1d.hxx

@@ -19,7 +19,7 @@ ContactModel1DTpl<Scalar>::ContactModel1DTpl(
       Raxis_(rotation),
       gains_(gains) {
   id_[0] = id;
-  type_[0] = type;
+  type_ = type;
 }
 
 template <typename Scalar>
@@ -28,7 +28,7 @@ ContactModel1DTpl<Scalar>::ContactModel1DTpl(
     Scalar xref, const pinocchio::ReferenceFrame type, const Vector2s& gains)
     : Base(state, type, 1, 1), xref_(xref), gains_(gains) {
   id_[0] = id;
-  type_[0] = type;
+  type_= type;
   Raxis_ = Matrix3s::Identity();
 }
 
@@ -78,7 +78,7 @@ void ContactModel1DTpl<Scalar>::calc(
   pinocchio::updateFramePlacement(*state_->get_pinocchio().get(), *d->pinocchio,
                                   id_[0]);
   pinocchio::getFrameJacobian(*state_->get_pinocchio().get(), *d->pinocchio,
-                              id_[0], pinocchio::LOCAL, d->fJf);
+                              id_[0], pinocchio::LOCAL, fdata.fJf);
   d->v = pinocchio::getFrameVelocity(*state_->get_pinocchio().get(),
                                      *d->pinocchio, id_[0]);
 
@@ -97,14 +97,14 @@ void ContactModel1DTpl<Scalar>::calc(
   if (gains_[1] != 0.) {
     d->a0_local += gains_[1] * d->v.linear();
   }
-  switch (type_[0]) {
+  switch (type_) {
     case pinocchio::ReferenceFrame::LOCAL:
-      d->Jc.row(0) = (Raxis_ * d->fJf.template topRows<3>()).row(2);
+      d->Jc.row(0) = (Raxis_ * fdata.fJf.template topRows<3>()).row(2);
       d->a0[0] = (Raxis_ * d->a0_local)[2];
       break;
     case pinocchio::ReferenceFrame::WORLD:
     case pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED:
-      d->Jc.row(0) = (Raxis_ * oRf * d->fJf.template topRows<3>()).row(2);
+      d->Jc.row(0) = (Raxis_ * oRf * fdata.fJf.template topRows<3>()).row(2);
       d->a0[0] = (Raxis_ * oRf * d->a0_local)[2];
       break;
   }
@@ -125,39 +125,39 @@ void ContactModel1DTpl<Scalar>::calcDiff(
 #endif
   pinocchio::getJointAccelerationDerivatives(
       *state_->get_pinocchio().get(), *d->pinocchio, joint, pinocchio::LOCAL,
-      d->v_partial_dq, d->a_partial_dq, d->a_partial_dv, d->a_partial_da);
+      fdata.v_partial_dq, fdata.a_partial_dq, fdata.a_partial_dv, fdata.a_partial_da);
   const std::size_t nv = state_->get_nv();
   pinocchio::skew(d->v.linear(), d->vv_skew);
   pinocchio::skew(d->v.angular(), d->vw_skew);
-  d->fXjdv_dq.noalias() = fdata.fXj * d->v_partial_dq;
-  d->fXjda_dq.noalias() = fdata.fXj * d->a_partial_dq;
-  d->fXjda_dv.noalias() = fdata.fXj * d->a_partial_dv;
+  d->fXjdv_dq.noalias() = fdata.fXj * fdata.v_partial_dq;
+  d->fXjda_dq.noalias() = fdata.fXj * fdata.a_partial_dq;
+  d->fXjda_dv.noalias() = fdata.fXj * fdata.a_partial_dv;
   d->da0_local_dx.leftCols(nv) = d->fXjda_dq.template topRows<3>();
   d->da0_local_dx.leftCols(nv).noalias() +=
       d->vw_skew * d->fXjdv_dq.template topRows<3>();
   d->da0_local_dx.leftCols(nv).noalias() -=
       d->vv_skew * d->fXjdv_dq.template bottomRows<3>();
   d->da0_local_dx.rightCols(nv) = d->fXjda_dv.template topRows<3>();
   d->da0_local_dx.rightCols(nv).noalias() +=
-      d->vw_skew * d->fJf.template topRows<3>();
+      d->vw_skew * fdata.fJf.template topRows<3>();
   d->da0_local_dx.rightCols(nv).noalias() -=
-      d->vv_skew * d->fJf.template bottomRows<3>();
+      d->vv_skew * fdata.fJf.template bottomRows<3>();
   const Eigen::Ref<const Matrix3s> oRf = d->pinocchio->oMf[id_[0]].rotation();
 
   if (gains_[0] != 0.) {
     pinocchio::skew(d->dp_local, d->dp_skew);
     d->da0_local_dx.leftCols(nv).noalias() +=
-        gains_[0] * d->dp_skew * d->fJf.template bottomRows<3>();
+        gains_[0] * d->dp_skew * fdata.fJf.template bottomRows<3>();
     d->da0_local_dx.leftCols(nv).noalias() +=
-        gains_[0] * d->fJf.template topRows<3>();
+        gains_[0] * fdata.fJf.template topRows<3>();
   }
   if (gains_[1] != 0.) {
     d->da0_local_dx.leftCols(nv).noalias() +=
         gains_[1] * d->fXjdv_dq.template topRows<3>();
     d->da0_local_dx.rightCols(nv).noalias() +=
-        gains_[1] * d->fJf.template topRows<3>();
+        gains_[1] * fdata.fJf.template topRows<3>();
   }
-  switch (type_[0]) {
+  switch (type_) {
     case pinocchio::ReferenceFrame::LOCAL:
       d->da0_dx.row(0) = (Raxis_ * d->da0_local_dx).row(2);
       break;
@@ -182,7 +182,7 @@ void ContactModel1DTpl<Scalar>::calcDiff(
       d->a0_world_skew.noalias() = d->a0_skew * Raxis_ * oRf;
       d->da0_dx.row(0) = (Raxis_ * oRf * d->da0_local_dx).row(2);
       d->da0_dx.leftCols(nv).row(0) -=
-          (d->a0_world_skew * d->fJf.template bottomRows<3>()).row(2);
+          (d->a0_world_skew * fdata.fJf.template bottomRows<3>()).row(2);
       break;
   }
 }
@@ -201,7 +201,7 @@ void ContactModel1DTpl<Scalar>::updateForce(
   fdata.f.linear()[2] = force[0];
   fdata.f.linear().template head<2>().setZero();
   fdata.f.angular().setZero();
-  switch (type_[0]) {
+  switch (type_) {
     case pinocchio::ReferenceFrame::LOCAL:
       fdata.fext.linear() = (R * Raxis_.transpose()).col(2) * force[0];
       fdata.fext.angular() = fdata.jMf.translation().cross(fdata.fext.linear());
@@ -216,9 +216,9 @@ void ContactModel1DTpl<Scalar>::updateForce(
       d->f_local.angular().setZero();
       fdata.fext = fdata.jMf.act(d->f_local);
       pinocchio::skew(d->f_local.linear(), d->f_skew);
-      d->fJf_df.noalias() = d->f_skew * d->fJf.template bottomRows<3>();
+      d->fJf_df.noalias() = d->f_skew * fdata.fJf.template bottomRows<3>();
       data->dtau_dq.noalias() =
-          -d->fJf.template topRows<3>().transpose() * d->fJf_df;
+          -fdata.fJf.template topRows<3>().transpose() * d->fJf_df;
       break;
   }
 }

include/crocoddyl/multibody/contacts/contact-2d.hpp

@@ -149,25 +149,15 @@ struct ContactData2DTpl : public ContactDataAbstractTpl<_Scalar> {
   ContactData2DTpl(Model<Scalar>* const model,
                    pinocchio::DataTpl<Scalar>* const data)
       : Base(model, data, 1),
-        fJf(6, model->get_state()->get_nv()),
-        v_partial_dq(6, model->get_state()->get_nv()),
-        a_partial_dq(6, model->get_state()->get_nv()),
-        a_partial_dv(6, model->get_state()->get_nv()),
-        a_partial_da(6, model->get_state()->get_nv()),
         fXjdv_dq(6, model->get_state()->get_nv()),
         fXjda_dq(6, model->get_state()->get_nv()),
         fXjda_dv(6, model->get_state()->get_nv()) {
     // There is only one element in the force_datas vector
     ForceDataAbstract& fdata = force_datas[0];
-    fdata.frame = model->get_id();
+    fdata.frame = model->get_id(0);
     fdata.jMf = model->get_state()->get_pinocchio()->frames[fdata.frame].placement;
     fdata.fXj = fdata.jMf.inverse().toActionMatrix();
 
-    fJf.setZero();
-    v_partial_dq.setZero();
-    a_partial_dq.setZero();
-    a_partial_dv.setZero();
-    a_partial_da.setZero();
     fXjdv_dq.setZero();
     fXjda_dq.setZero();
     fXjda_dv.setZero();
@@ -187,11 +177,6 @@ struct ContactData2DTpl : public ContactDataAbstractTpl<_Scalar> {
 
   pinocchio::MotionTpl<Scalar> v;
   pinocchio::MotionTpl<Scalar> a;
-  Matrix6xs fJf;
-  Matrix6xs v_partial_dq;
-  Matrix6xs a_partial_dq;
-  Matrix6xs a_partial_dv;
-  Matrix6xs a_partial_da;
   Matrix6xs fXjdv_dq;
   Matrix6xs fXjda_dq;
   Matrix6xs fXjda_dv;