[C++] Add Frame::previousFrame and update class Model

src/multibody/frame.hpp

@@ -58,12 +58,14 @@ namespace se3
     ///
     /// \param[in] name Name of the frame.
     /// \param[in] parent Index of the parent joint in the kinematic tree.
+    /// \param[in] previousFrame Index of the parent frame in the kinematic tree.
     /// \param[in] placement Placement of the frame wrt the parent joint frame.
     /// \param[in] type The type of the frame, see the enum FrameType
     ///
-    Frame(const std::string & name, const JointIndex parent, const SE3 & frame_placement, const FrameType type)
+    Frame(const std::string & name, const JointIndex parent, const FrameIndex previousFrame, const SE3 & frame_placement, const FrameType type)
     : name(name)
     , parent(parent)
+    , previousFrame(previousFrame)
     , placement(frame_placement)
     , type(type)
     {}
@@ -76,6 +78,7 @@ namespace se3
     bool operator == (const Frame & other) const
     {
       return name == other.name && parent == other.parent
+      && previousFrame == other.previousFrame
       && placement == other.placement
       && type == other.type ;
     }
@@ -86,6 +89,9 @@ namespace se3
     /// \brief Index of the parent joint.
     JointIndex parent;
     
+    /// \brief Index of the previous frame.
+    FrameIndex previousFrame;
+    
     /// \brief Placement of the frame wrt the parent joint.
     SE3 placement;
 

src/multibody/model.hpp

@@ -124,13 +124,15 @@ namespace se3
       , gravity( gravity981,Eigen::Vector3d::Zero() )
     {
       names[0]     = "universe";     // Should be "universe joint (trivial)"
+      addFrame(Frame("universe", 0, 0, SE3::Identity(), FIXED_JOINT));
     }
     ~Model() {} // std::cout << "Destroy model" << std::endl; }
     
     ///
     /// \brief Add a joint to the kinematic tree.
     ///
-    /// \remark This method also adds a Frame of same name to the vector of frames.
+    /// \remark This method does not add a Frame of same name to the vector of frames.
+    ///         Use Model::addJointFrame.
     /// \remark The inertia supported by the joint is set to Zero.
     ///
     /// \tparam JointModelDerived The type of the joint model.
@@ -146,7 +148,7 @@ namespace se3
     ///
     /// \return The index of the new joint.
     ///
-    /// \sa Model::appendBodyToJoint
+    /// \sa Model::appendBodyToJoint, Model::addJointFrame
     ///
     template<typename JointModelDerived>
     JointIndex addJoint(const JointIndex parent, const JointModelBase<JointModelDerived> & joint_model, const SE3 & joint_placement,
@@ -157,6 +159,18 @@ namespace se3
                         const Eigen::VectorXd & max_config = Eigen::VectorXd::Constant(JointModelDerived::NQ,std::numeric_limits<double>::max())
                         );
 
+    ///
+    /// \brief Add a joint to the frame tree.
+    ///
+    /// \param[in] jointIndex Index of the joint.
+    /// \param[in] frameIndex Index of the parent frame. If negative,
+    ///            the parent frame is the frame of the parent joint.
+    ///
+    /// \return The index of the new frame.
+    ///
+    FrameIndex addJointFrame (const JointIndex& jointIndex,
+                                    int         frameIndex = -1);
+
     ///
     /// \brief Append a body to a given joint of the kinematic tree.
     ///
@@ -165,13 +179,27 @@ namespace se3
     /// \param[in] joint_index Index of the supporting joint.
     /// \param[in] Y Spatial inertia of the body.
     /// \param[in] body_placement The relative placement of the body regarding to the parent joint. Set default to the Identity placement.
-    /// \param[in] body_name Name of the body. If empty, the name is random.
     ///
     /// \sa Model::addJoint
     ///
     void appendBodyToJoint(const JointIndex joint_index, const Inertia & Y,
-                           const SE3 & body_placement = SE3::Identity(),
-                           const std::string & body_name = "");
+                           const SE3 & body_placement = SE3::Identity());
+
+    ///
+    /// \brief Add a body to the frame tree.
+    ///
+    /// \param[in] body_name Name of the body.
+    /// \param[in] parentJoint Index of the parent joint.
+    /// \param[in] body_placement The relative placement of the body regarding to the parent joint. Set default to the Identity placement.
+    /// \param[in] previousFrame Index of the parent frame. If negative,
+    ///            the parent frame is the frame of the parent joint.
+    ///
+    /// \return The index of the new frame.
+    ///
+    FrameIndex addBodyFrame (const std::string& body_name,
+                             const JointIndex& parentJoint,
+                             const SE3 & body_placement = SE3::Identity(),
+                                   int         previousFrame = -1);
 
     ///
     /// \brief Return the index of a body given by its name.

src/multibody/model.hxx

@@ -87,15 +87,41 @@ namespace se3
     return idx;
   }
 
+  inline FrameIndex Model::addJointFrame (const JointIndex& jidx,
+                                                int         fidx)
+  {
+    if (fidx < 0) {
+      fidx = getFrameId(names[parents[jidx]]);
+    }
+    if (fidx >= frames.size())
+      throw std::invalid_argument ("Frame not found");
+    // Add a the joint frame attached to itself to the frame vector - redundant information but useful.
+    addFrame(Frame(names[jidx],jidx,fidx,SE3::Identity(),JOINT));
+    return frames.size() - 1;
+  }
+
   inline void Model::appendBodyToJoint(const Model::JointIndex joint_index,
                                        const Inertia & Y,
-                                       const SE3 & body_placement,
-                                       const std::string & body_name)
+                                       const SE3 & body_placement)
   {
     const Inertia & iYf = Y.se3Action(body_placement);
     inertias[joint_index] += iYf;
     nbody++;
   }
+
+  inline FrameIndex Model::addBodyFrame (const std::string& body_name,
+                                         const JointIndex& parentJoint,
+                                         const SE3 & body_placement,
+                                               int         previousFrame)
+  {
+    if (previousFrame < 0) {
+      previousFrame = getFrameId(names[parentJoint]);
+    }
+    if (previousFrame >= frames.size())
+      throw std::invalid_argument ("Frame not found");
+    addFrame(Frame(body_name, parentJoint, previousFrame, body_placement, BODY));
+    return frames.size() - 1;
+  }
   
   inline Model::JointIndex Model::getBodyId (const std::string & name) const
   {