Segments removal in trees

orocos_kdl/src/tree.cpp

@@ -21,6 +21,7 @@
 
 #include "tree.hpp"
 #include <sstream>
+#include <algorithm>
 
 namespace KDL {
 using namespace std;
@@ -162,6 +163,65 @@ bool Tree::addTreeRecursive(SegmentMap::const_iterator root, const std::string&
         }
         return true;
     }
+
+void Tree::deleteSegmentsRecursive(SegmentMap::const_iterator segment, unsigned int& ns, unsigned int& nj) {
+  // delete all children (if any)
+  SegmentMap::const_iterator child;
+  for(unsigned int i=0; i<GetTreeElementChildren(segment->second).size(); i++) {
+    // delete i-th child
+    child = GetTreeElementChildren(segment->second)[i];
+    deleteSegmentsRecursive(child, ns, nj);
+  }
+
+  // update ns and nj
+  ns++;
+  if(GetTreeElementSegment(segment->second).getJoint().getType() != Joint::None)
+    nj++;
+  // remove the segment from the map
+  segments.erase(segment);
+}
+
+unsigned int Tree::deleteSegmentsFrom(SegmentMap::const_iterator segment) {
+  if(segment == segments.end() || segment == getRootSegment())
+    return 0;
+
+  // remove references to this segment from its parent
+  auto parent = segments.find(GetTreeElementParent(segment->second)->first);
+  auto& parent_children = GetTreeElementChildren(parent->second);
+  // parent_children.erase(std::find_if(
+  //   parent_children.begin(), parent_children.end(),
+  //   [&](SegmentMap::const_iterator it) { return it->first == segment->first; }
+  // ));
+  parent_children.erase(std::remove(parent_children.begin(), parent_children.end(), segment));
+
+  // delete children recursively
+  unsigned int ns=0, nj=0;
+  deleteSegmentsRecursive(segment, ns, nj);
+
+  // update number o joints and segments
+  nrOfSegments -= ns;
+  nrOfJoints -= nj;
+
+  if(nj > 0) {
+    unsigned int nq = 0;
+    for(SegmentMap::iterator s=segments.begin(); s!=segments.end(); s++) {
+      if(GetTreeElementSegment(s->second).getJoint().getType() != Joint::None) {
+        GetTreeElementQNr(s->second) = nq;
+        nq++;
+      }
+    }
+  }
+
+  return ns;
+}
+
+unsigned int Tree::deleteSegmentsFrom(const std::string& name) {
+  // prevent to remove the root segment
+  if(name == root_name)
+    return 0;
+  // delete segments using the iterator version
+  return deleteSegmentsFrom(segments.find(name));
+}
 
 }//end of namespace
 

orocos_kdl/src/tree.hpp

@@ -106,6 +106,17 @@ namespace KDL
         std::string root_name;
 
         bool addTreeRecursive(SegmentMap::const_iterator root, const std::string& hook_name);
+
+        /** Removes all child segments of `segment` (and `segment` itself).
+          *
+          * @param segment Iterator pointing to the segment to be deleted.
+          * @param ns Total number of segments that are removed in this way.
+          * @param nj Total number of moving joints that are removed in this way.
+          *
+          * @note A part from the `segments` map, no internal variables are
+          * modeified here, *ie*, `nrOfJoints` and `nrOfSegments` are untouched.
+          */
+        void deleteSegmentsRecursive(SegmentMap::const_iterator segment, unsigned int& ns, unsigned int& nj);
 
     public:
         /**
@@ -206,6 +217,39 @@ namespace KDL
         {
             return segments;
         }
+
+        /**
+         * Request to delete all child segments starting from the given element.
+         *
+         * @param segment iterator of the first segment to delete (all children
+         * will be removed as well).
+         *
+         * @return the number of deleted segments.
+         *
+         * @note The root segment cannot be removed from the tree.
+         *
+         * @note If moving joints are removed in this way, joint indices
+         * are recomputed internally.
+         *
+         * @warning The behavior is undefined if `segment` is not a valid
+         * iterator (note that `getSegments().end()` is valid).
+         */
+        unsigned int deleteSegmentsFrom(SegmentMap::const_iterator segment);
+
+        /**
+         * Request to delete all child segments starting from the one with given name.
+         *
+         * @param name the name of the first segment to delete (all children
+         * will be removed as well).
+         *
+         * @return the number of deleted segments.
+         *
+         * @note The root segment cannot be removed from the tree.
+         *
+         * @note If moving joints are removed in this way, joint indices
+         * are recomputed internally.
+         */
+        unsigned int deleteSegmentsFrom(const std::string& name);
 
         virtual ~Tree(){};
 

orocos_kdl/tests/kinfamtest.cpp

@@ -276,6 +276,50 @@ void KinFamTest::TreeTest()
     solver1.JntToCart(jnt1, f1);
     solver2.JntToCart(jnt2, f2);
     CPPUNIT_ASSERT(f1 == f2.Inverse());
+
+    Tree tree3("root");
+    tree3.addSegment(Segment("S1", Joint("J1", Joint::RotX)), "root");
+    tree3.addSegment(Segment("S2", Joint("J2", Joint::RotX)), "root");
+    tree3.addSegment(Segment("S3", Joint("J3", Joint::RotX)), "S2");
+    tree3.addSegment(Segment("S4", Joint("J4", Joint::None)), "S3");
+    tree3.addSegment(Segment("S5", Joint("J5", Joint::RotX)), "S2");
+    tree3.addSegment(Segment("S6", Joint("J6", Joint::RotX)), "S5");
+    tree3.addSegment(Segment("S7", Joint("J7", Joint::None)), "S5");
+    cout << "Tree 3:" << endl << tree3 << endl;
+
+    Tree tree4(tree3);
+    tree4.deleteSegmentsFrom("S1");
+    CPPUNIT_ASSERT_EQUAL(tree4.getNrOfSegments(), (uint)6);
+    CPPUNIT_ASSERT_EQUAL(tree4.getNrOfJoints(), (uint)4);
+    cout << "After removing S1:" << endl << tree4 << endl;
+
+    tree4 = tree3;
+    tree4.deleteSegmentsFrom("S2");
+    CPPUNIT_ASSERT_EQUAL(tree4.getNrOfSegments(), (uint)1);
+    CPPUNIT_ASSERT_EQUAL(tree4.getNrOfJoints(), (uint)1);
+    cout << "After removing S2:" << endl << tree4 << endl;
+
+    tree4 = tree3;
+    tree4.deleteSegmentsFrom("S3");
+    CPPUNIT_ASSERT_EQUAL(tree4.getNrOfSegments(), (uint)5);
+    CPPUNIT_ASSERT_EQUAL(tree4.getNrOfJoints(), (uint)4);
+    cout << "After removing S3:" << endl << tree4 << endl;
+
+    tree4 = tree3;
+    tree4.deleteSegmentsFrom("S7");
+    CPPUNIT_ASSERT_EQUAL(tree4.getNrOfSegments(), (uint)6);
+    CPPUNIT_ASSERT_EQUAL(tree4.getNrOfJoints(), (uint)5);
+    cout << "After removing S7:" << endl << tree4 << endl;
+
+    tree4 = tree3;
+    tree4.deleteSegmentsFrom("ABCDEF");
+    CPPUNIT_ASSERT_EQUAL(tree4.getNrOfSegments(), tree3.getNrOfSegments());
+    CPPUNIT_ASSERT_EQUAL(tree4.getNrOfJoints(), tree3.getNrOfJoints());
+
+    tree4 = tree3;
+    tree4.deleteSegmentsFrom("root");
+    CPPUNIT_ASSERT_EQUAL(tree4.getNrOfSegments(), tree3.getNrOfSegments());
+    CPPUNIT_ASSERT_EQUAL(tree4.getNrOfJoints(), tree3.getNrOfJoints());
 }