Merge pull request #752 from jmirabel/devel

[C++] Add algo appendModel to merge two Models.

CMakeLists.txt

@@ -89,10 +89,10 @@ IF(BUILD_WITH_URDF_SUPPORT)
   ADD_REQUIRED_DEPENDENCY("urdfdom >= 0.2.0")
 ENDIF(BUILD_WITH_URDF_SUPPORT)
 
-IF(BUILD_WITH_HPP_FCL_SUPPORT)
+IF(BUILD_WITH_COLLISION_SUPPORT)
   find_package(catkin QUIET COMPONENTS hpp-fcl)  # compatability with catkin-packaged hpp-fcl
   ADD_REQUIRED_DEPENDENCY("hpp-fcl >= 1.0.0")
-ENDIF(BUILD_WITH_HPP_FCL_SUPPORT)
+ENDIF(BUILD_WITH_COLLISION_SUPPORT)
 
 IF(BUILD_WITH_AUTODIFF_SUPPORT)
   ADD_REQUIRED_DEPENDENCY("cppad >= 20180000.0")

src/algorithm/model.hpp

+//
+// Copyright (c) 2019 CNRS
+//
+
+#ifndef __pinocchio_algorithm_model_hpp__
+#define __pinocchio_algorithm_model_hpp__
+
+#include "pinocchio/multibody/model.hpp"
+#include "pinocchio/multibody/geometry.hpp"
+
+namespace pinocchio
+{
+  /**
+   *  \brief Append a child model into a parent model, after a specific frame.
+   *  the model given as reference argument.
+   *
+   *  \param[in] modelA the parent model
+   *  \param[in] modelB the child model
+   *  \param[in] geomModelA the parent geometry model
+   *  \param[in] geomModelB the child geometry model
+   *  \param[in] frameInModelA index of the frame of modelA where to append modelB.
+   *  \param[in] aMb pose of modelB universe joint (index 0) in frameInModelA.
+   *  \param[out] model the resulting model
+   *
+   */
+  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
+  void
+  appendModel(const ModelTpl<Scalar,Options,JointCollectionTpl> & modelA,
+              const ModelTpl<Scalar,Options,JointCollectionTpl> & modelB,
+              FrameIndex frameInModelA,
+              const SE3Tpl<Scalar, Options>& aMb,
+              ModelTpl<Scalar,Options,JointCollectionTpl>& model);
+
+  /**
+   *  \copydoc pinocchio::appendModel(const ModelTpl<Scalar,Options,JointCollectionTpl>&, const ModelTpl<Scalar,Options,JointCollectionTpl> & modelB, FrameIndex frameInModelA, const SE3Tpl<Scalar, Options>& aMb, ModelTpl<Scalar,Options,JointCollectionTpl>& model);
+   *
+   *  \param[in] geomModelA the parent geometry model
+   *  \param[in] geomModelB the child geometry model
+   *  \param[out] geomModel
+   */
+  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
+  void
+  appendModel(const ModelTpl<Scalar,Options,JointCollectionTpl> & modelA,
+              const ModelTpl<Scalar,Options,JointCollectionTpl> & modelB,
+              const GeometryModel& geomModelA,
+              const GeometryModel& geomModelB,
+              FrameIndex frameInModelA,
+              const SE3Tpl<Scalar, Options>& aMb,
+              ModelTpl<Scalar,Options,JointCollectionTpl>& model,
+              GeometryModel& geomModel);
+
+} // namespace pinocchio
+
+#include "pinocchio/algorithm/model.hxx"
+
+#endif // ifndef __pinocchio_algorithm_model_hpp__

src/algorithm/model.hxx

+//
+// Copyright (c) 2019 CNRS
+//
+
+#ifndef __pinocchio_algorithm_model_hxx__
+#define __pinocchio_algorithm_model_hxx__
+
+namespace pinocchio
+{
+  namespace details {
+    template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
+    void appendUniverseToModel (const ModelTpl<Scalar,Options,JointCollectionTpl> & modelAB,
+                                const GeometryModel& geomModelAB,
+                                FrameIndex parentFrame,
+                                const SE3Tpl<Scalar, Options>& pfMAB,
+                                ModelTpl<Scalar,Options,JointCollectionTpl> & model,
+                                GeometryModel& geomModel)
+    {
+      typedef FrameTpl<Scalar, Options> Frame;
+
+      const Frame& pframe = model.frames[parentFrame];
+      JointIndex jid = pframe.parent;
+      assert (jid < model.joints.size());
+
+      // If inertia is not NaN, add it.
+      if (modelAB.inertias[0] == modelAB.inertias[0])
+        model.appendBodyToJoint (jid, modelAB.inertias[0], pframe.placement * pfMAB);
+
+      // Add all frames whose parent is this joint.
+      for (FrameIndex fid = 1; fid < modelAB.frames.size(); ++fid) {
+        Frame frame = modelAB.frames[fid];
+        if (frame.parent == 0) {
+          frame.parent = jid;
+          if (frame.previousFrame != 0) {
+            frame.previousFrame = model.getFrameId (
+                modelAB.frames[frame.previousFrame].name,
+                modelAB.frames[frame.previousFrame].type);
+          } else {
+            frame.previousFrame = parentFrame;
+          }
+          // Modify frame placement
+          frame.placement = pframe.placement * pfMAB * frame.placement;
+          model.addFrame (frame);
+        }
+      }
+      // Add all geometries whose parent is this joint.
+      for (GeomIndex gid = 0; gid < geomModelAB.geometryObjects.size(); ++gid) {
+        GeometryObject go = geomModelAB.geometryObjects[gid];
+        if (go.parentJoint == 0) {
+          go.parentJoint = jid;
+          if (go.parentFrame != 0) {
+            go.parentFrame = model.getFrameId (
+                modelAB.frames[go.parentFrame].name,
+                modelAB.frames[go.parentFrame].type);
+          } else {
+            go.parentFrame = parentFrame;
+          }
+          go.placement = pframe.placement * pfMAB * go.placement;
+          geomModel.addGeometryObject (go);
+        }
+      }
+    }
+
+    template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
+    struct AppendJointOfModelAlgoTpl : public fusion::JointVisitorBase< AppendJointOfModelAlgoTpl<Scalar,Options,JointCollectionTpl> >
+    {
+      typedef boost::fusion::vector<
+          const ModelTpl<Scalar,Options,JointCollectionTpl> &,
+          const GeometryModel&,
+          JointIndex,
+          const SE3Tpl<Scalar, Options>&,
+          ModelTpl<Scalar,Options,JointCollectionTpl> &,
+          GeometryModel& > ArgsType;
+
+      template <typename JointModel>
+      static void algo (const JointModelBase<JointModel> & jmodel,
+          const ModelTpl<Scalar,Options,JointCollectionTpl> & modelAB,
+          const GeometryModel& geomModelAB,
+          JointIndex parentId,
+          const SE3Tpl<Scalar, Options>& pMi,
+          ModelTpl<Scalar,Options,JointCollectionTpl> & model,
+          GeometryModel& geomModel)
+      {
+        // If old parent is universe, use what's provided in the input.
+        // otherwise, get the parent from modelAB.
+        if (modelAB.parents[jmodel.id()] > 0)
+          parentId = model.getJointId(modelAB.names[modelAB.parents[jmodel.id()]]);
+        JointIndex jid = model.addJoint (
+            parentId,
+            jmodel,
+            pMi * modelAB.jointPlacements[jmodel.id()],
+            modelAB.names[jmodel.id()],
+            jmodel.jointVelocitySelector(modelAB.effortLimit),
+            jmodel.jointVelocitySelector(modelAB.velocityLimit),
+            jmodel.jointConfigSelector(modelAB.lowerPositionLimit),
+            jmodel.jointConfigSelector(modelAB.upperPositionLimit));
+        assert (jid < model.joints.size());
+
+        model.appendBodyToJoint (jid, modelAB.inertias[jmodel.id()]);
+        // Add all frames whose parent is this joint.
+        for (FrameIndex fid = 1; fid < modelAB.frames.size(); ++fid) {
+          FrameTpl<Scalar, Options> frame = modelAB.frames[fid];
+          if (frame.parent == jmodel.id()) {
+            frame.parent = jid;
+            assert (frame.previousFrame > 0 || frame.type == JOINT);
+            if (frame.previousFrame != 0) {
+              frame.previousFrame = model.getFrameId (
+                  modelAB.frames[frame.previousFrame].name,
+                  modelAB.frames[frame.previousFrame].type);
+            }
+            model.addFrame (frame);
+          }
+        }
+        // Add all geometries whose parent is this joint.
+        for (GeomIndex gid = 0; gid < geomModelAB.geometryObjects.size(); ++gid) {
+          GeometryObject go = geomModelAB.geometryObjects[gid];
+          if (go.parentJoint == jmodel.id()) {
+            go.parentJoint = jid;
+            assert (go.parentFrame > 0);
+            if (go.parentFrame != 0) {
+              go.parentFrame = model.getFrameId (
+                  modelAB.frames[go.parentFrame].name,
+                  modelAB.frames[go.parentFrame].type);
+            }
+            geomModel.addGeometryObject (go);
+          }
+        }
+      }
+    };
+  }
+
+  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
+  void
+  appendModel(const ModelTpl<Scalar,Options,JointCollectionTpl> & modelA,
+              const ModelTpl<Scalar,Options,JointCollectionTpl> & modelB,
+              FrameIndex frameInModelA,
+              const SE3Tpl<Scalar, Options>& aMb,
+              ModelTpl<Scalar,Options,JointCollectionTpl>& model)
+  {
+    GeometryModel geomModelA, geomModelB, geomModel;
+
+    appendModel(modelA, modelB, geomModelA, geomModelB, frameInModelA, aMb,
+         model, geomModel);
+  }
+
+  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
+  void
+  appendModel(const ModelTpl<Scalar,Options,JointCollectionTpl> & modelA,
+              const ModelTpl<Scalar,Options,JointCollectionTpl> & modelB,
+              const GeometryModel& geomModelA,
+              const GeometryModel& geomModelB,
+              FrameIndex frameInModelA,
+              const SE3Tpl<Scalar, Options>& aMb,
+              ModelTpl<Scalar,Options,JointCollectionTpl>& model,
+              GeometryModel& geomModel)
+  {
+    typedef details::AppendJointOfModelAlgoTpl<Scalar, Options, JointCollectionTpl> AppendJointOfModelAlgo;
+    typedef typename AppendJointOfModelAlgo::ArgsType ArgsType;
+    typedef SE3Tpl<Scalar, Options> SE3;
+
+    const FrameTpl<Scalar, Options>& frame = modelA.frames[frameInModelA];
+    static const SE3 id = SE3::Identity();
+
+    int njoints = modelA.njoints + modelB.njoints - 1;
+    model.names                 .reserve (njoints);
+    model.joints                .reserve (njoints);
+    model.jointPlacements       .reserve (njoints);
+    model.parents               .reserve (njoints);
+    model.inertias              .reserve (njoints);
+    int nframes = modelA.nframes + modelB.nframes - 1;
+    model.frames                .reserve (nframes);
+
+    geomModel.geometryObjects.reserve (geomModelA.ngeoms + geomModelB.ngeoms);
+
+    // Copy modelA joints until frame.parentJoint
+    details::appendUniverseToModel (modelA, geomModelA, 0, id, model, geomModel);
+    for (JointIndex jid = 1; jid <= frame.parent; ++jid) {
+      ArgsType args (modelA, geomModelA, 0, id, model, geomModel);
+      AppendJointOfModelAlgo::run (modelA.joints[jid], args);
+    }
+
+    // Copy modelB joints
+    details::appendUniverseToModel (modelB, geomModelB,
+        model.getFrameId (frame.name, frame.type), aMb, model, geomModel);
+    for (JointIndex jid = 1; jid < modelB.joints.size(); ++jid) {
+      SE3 pMi = (jid == 1 ? frame.placement * aMb : id);
+      ArgsType args (modelB, geomModelB, frame.parent, pMi, model, geomModel);
+      AppendJointOfModelAlgo::run (modelB.joints[jid], args);
+    }
+
+    // Copy remaining joints of modelA
+    for (JointIndex jid = frame.parent+1; jid < modelA.joints.size(); ++jid) {
+      ArgsType args (modelA, geomModelA, 0, id, model, geomModel);
+      AppendJointOfModelAlgo::run (modelA.joints[jid], args);
+    }
+
+    // Add collision pairs of geomModelA and geomModelB
+    geomModel.collisionPairs.reserve(geomModelA.collisionPairs.size()
+       + geomModelB.collisionPairs.size()
+       + geomModelA.geometryObjects.size() * geomModelB.geometryObjects.size());
+    for (std::size_t icp = 0; icp < geomModelA.collisionPairs.size(); ++icp) {
+      const CollisionPair& cp (geomModelA.collisionPairs[icp]);
+      GeomIndex go1 = geomModel.getGeometryId(geomModelA.geometryObjects[cp.first ].name);
+      GeomIndex go2 = geomModel.getGeometryId(geomModelA.geometryObjects[cp.second].name);
+      geomModel.addCollisionPair (CollisionPair (go1, go2));
+    }
+    for (std::size_t icp = 0; icp < geomModelB.collisionPairs.size(); ++icp) {
+      const CollisionPair& cp (geomModelB.collisionPairs[icp]);
+      GeomIndex go1 = geomModel.getGeometryId(geomModelB.geometryObjects[cp.first ].name);
+      GeomIndex go2 = geomModel.getGeometryId(geomModelB.geometryObjects[cp.second].name);
+      geomModel.addCollisionPair (CollisionPair (go1, go2));
+    }
+
+    // add the collision pairs between geomModelA and geomModelB.
+    for (Index i = 0; i < geomModelA.geometryObjects.size(); ++i) {
+      GeomIndex go1 = geomModel.getGeometryId(geomModelA.geometryObjects[i].name);
+      for (Index j = 0; j < geomModelB.geometryObjects.size(); ++j) {
+        GeomIndex go2 = geomModel.getGeometryId(geomModelB.geometryObjects[j].name);
+        if (   geomModel.geometryObjects[go1].parentJoint
+            != geomModel.geometryObjects[go2].parentJoint)
+          geomModel.addCollisionPair(CollisionPair(go1, go2));
+      }
+    }
+  }
+
+} // namespace pinocchio
+
+#endif // ifndef __pinocchio_algorithm_model_hxx__

src/multibody/model.hxx

@@ -48,7 +48,7 @@ namespace pinocchio
     os << "Nb joints = " << model.njoints << " (nq="<< model.nq<<",nv="<<model.nv<<")" << std::endl;
     for(Index i=0;i<(Index)(model.njoints);++i)
     {
-      os << "  Joint "<< model.names[i] << ": parent=" << model.parents[i]  << std::endl;
+      os << "  Joint " << i << " " << model.names[i] << ": parent=" << model.parents[i]  << std::endl;
     }
     
     return os;

src/parsers/sample-models.hxx

@@ -337,6 +337,7 @@ namespace pinocchio
         jff.addJoint(typename JC::JointModelSphericalZYX());
         ffidx = model.addJoint(0,jff,SE3::Identity(),"freeflyer_joint");
       }
+      model.appendBodyToJoint(ffidx,Ijoint);
       model.addJointFrame(ffidx);
       
       /* --- Lower limbs --- */

unittest/model.cpp

@@ -3,6 +3,7 @@
 //
 
 #include "pinocchio/multibody/model.hpp"
+#include "pinocchio/algorithm/model.hpp"
 #include "pinocchio/parsers/sample-models.hpp"
 
 #include <boost/test/unit_test.hpp>
@@ -43,4 +44,110 @@ BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
     BOOST_CHECK(model.cast<long double>().cast<double>() == model);
   }
 
+#ifdef PINOCCHIO_WITH_HPP_FCL
+  struct AddPrefix {
+    std::string p;
+    std::string operator() (const std::string& n) { return p + n; }
+    Frame operator() (const Frame& _f) { Frame f (_f); f.name = p + f.name; return f; }
+    AddPrefix (const char* _p) : p(_p) {}
+  };
+
+  BOOST_AUTO_TEST_CASE(append)
+  {
+    Model manipulator, humanoid, model;
+    GeometryModel geomManipulator, geomHumanoid, geomModel;
+
+    buildModels::manipulator(manipulator);
+    buildModels::manipulatorGeometries(manipulator, geomManipulator);
+    // Add prefix to joint and frame names
+    AddPrefix addManipulatorPrefix ("manipulator/");
+    std::transform (++manipulator.names.begin(), manipulator.names.end(),
+        ++manipulator.names.begin(), addManipulatorPrefix);
+    std::transform (++manipulator.frames.begin(), manipulator.frames.end(),
+        ++manipulator.frames.begin(), addManipulatorPrefix);
+
+    BOOST_MESSAGE(manipulator);
+
+    buildModels::humanoid(humanoid);
+    buildModels::humanoidGeometries(humanoid, geomHumanoid);
+    // Add prefix to joint and frame names
+    AddPrefix addHumanoidPrefix ("humanoid/");
+    std::transform (++humanoid.names.begin(), humanoid.names.end(),
+        ++humanoid.names.begin(), addHumanoidPrefix);
+    std::transform (++humanoid.frames.begin(), humanoid.frames.end(),
+        ++humanoid.frames.begin(), addHumanoidPrefix);
+
+    BOOST_MESSAGE(humanoid);
+    
+    //TODO fix inertia of the base
+    manipulator.inertias[0].setRandom();
+    SE3 aMb = SE3::Random();
+    FrameIndex fid = humanoid.addFrame (Frame ("humanoid/add_manipulator", 
+          humanoid.getJointId("humanoid/chest2_joint"),
+          humanoid.getFrameId("humanoid/chest2_joint"), aMb,
+          OP_FRAME));
+
+    appendModel (humanoid, manipulator, geomHumanoid, geomManipulator, fid,
+        SE3::Identity(), model, geomModel);
+
+    BOOST_MESSAGE(model);
+
+    // Check the model
+    BOOST_CHECK_EQUAL(model.getJointId("humanoid/chest2_joint"),
+        model.parents[model.getJointId("manipulator/shoulder1_joint")]);
+
+    // check the joint order and the inertias
+    JointIndex chest2 = model.getJointId("humanoid/chest2_joint");
+    for (JointIndex jid = 1; jid < chest2; ++jid) {
+      BOOST_MESSAGE("Checking joint " << jid << " " << model.names[jid]);
+      BOOST_CHECK_EQUAL(model.names[jid], humanoid.names[jid]);
+      BOOST_CHECK_EQUAL(model.inertias[jid], humanoid.inertias[jid]);
+      BOOST_CHECK_EQUAL(model.jointPlacements[jid], humanoid.jointPlacements[jid]);
+    }
+    BOOST_MESSAGE("Checking joint " << chest2 << " " << model.names[chest2]);
+    BOOST_CHECK_EQUAL(model.names[chest2], humanoid.names[chest2]);
+    BOOST_CHECK_MESSAGE(model.inertias[chest2].isApprox(manipulator.inertias[0].se3Action(aMb) + humanoid.inertias[chest2]),
+        model.inertias[chest2] << " != " << manipulator.inertias[0].se3Action(aMb) + humanoid.inertias[chest2]);
+    BOOST_CHECK_EQUAL(model.jointPlacements[chest2], humanoid.jointPlacements[chest2]);
+
+    for (JointIndex jid = 1; jid < manipulator.joints.size(); ++jid) {
+      BOOST_MESSAGE("Checking joint " << chest2+jid << " " << model.names[chest2+jid]);
+      BOOST_CHECK_EQUAL(model.names[chest2+jid], manipulator.names[jid]);
+      BOOST_CHECK_EQUAL(model.inertias[chest2+jid], manipulator.inertias[jid]);
+      if (jid==1)
+        BOOST_CHECK_EQUAL(model.jointPlacements[chest2+jid], aMb*manipulator.jointPlacements[jid]);
+      else
+        BOOST_CHECK_EQUAL(model.jointPlacements[chest2+jid], manipulator.jointPlacements[jid]);
+    }
+    for (JointIndex jid = chest2+1; jid < humanoid.joints.size(); ++jid) {
+      BOOST_MESSAGE("Checking joint " << jid+manipulator.joints.size()-1 << " " << model.names[jid+manipulator.joints.size()-1]);
+      BOOST_CHECK_EQUAL(model.names[jid+manipulator.joints.size()-1], humanoid.names[jid]);
+      BOOST_CHECK_EQUAL(model.inertias[jid+manipulator.joints.size()-1], humanoid.inertias[jid]);
+      BOOST_CHECK_EQUAL(model.jointPlacements[jid+manipulator.joints.size()-1], humanoid.jointPlacements[jid]);
+    }
+
+    // Check the frames
+    for (FrameIndex fid = 1; fid < humanoid.frames.size(); ++fid) {
+      const Frame& frame  = humanoid.frames[fid],
+                   parent = humanoid.frames[frame.previousFrame];
+      BOOST_CHECK(model.existFrame (frame.name, frame.type));
+      const Frame& nframe  = model.frames[model.getFrameId(frame.name, frame.type)],
+                   nparent = model.frames[nframe.previousFrame];
+      BOOST_CHECK_EQUAL(parent.name, nparent.name);
+      BOOST_CHECK_EQUAL(frame.placement, nframe.placement);
+    }
+    for (FrameIndex fid = 1; fid < manipulator.frames.size(); ++fid) {
+      const Frame& frame  = manipulator.frames[fid],
+                   parent = manipulator.frames[frame.previousFrame];
+      BOOST_CHECK(model.existFrame (frame.name, frame.type));
+      const Frame& nframe  = model.frames[model.getFrameId(frame.name, frame.type)],
+                   nparent = model.frames[nframe.previousFrame];
+      if (frame.previousFrame > 0) {
+        BOOST_CHECK_EQUAL(parent.name, nparent.name);
+        BOOST_CHECK_EQUAL(frame.placement, nframe.placement);
+      }
+    }
+  }
+#endif
+
 BOOST_AUTO_TEST_SUITE_END()