Merge branch 'segments-removal' of https://github.com/francofusco/oro…

…cos_kinematics_dynamics into francofusco-segments-removal

orocos_kdl/src/chain.cpp

@@ -75,6 +75,37 @@ namespace KDL {
         return segments[nr];
     }
 
+    unsigned int Chain::deleteSegmentsFrom(unsigned int nr)
+    {
+      // make sure the index is valid
+      if(nr >= nrOfSegments)
+        return 0;
+      // decrease the number of joints (once for each moving joint that is removed)
+      for(unsigned int i=nr; i<segments.size(); i++) {
+        if(segments[i].getJoint().getType() != Joint::None)
+          nrOfJoints--;
+      }
+      // number of segments to be deleted
+      unsigned int to_del = nrOfSegments - nr;
+      // reset the number of segments
+      nrOfSegments = nr;
+      segments.resize(nr);
+      return to_del;
+    }
+
+    unsigned int Chain::deleteSegmentsFrom(const std::string& name)
+    {
+      unsigned int irev;
+      for(unsigned int i=0; i<nrOfSegments; i++) {
+        irev = nrOfSegments-i-1;
+        if(segments[irev].getName() == name) {
+          // remove the segment
+          return deleteSegmentsFrom(irev);
+        }
+      }
+      return 0;
+    }
+
     Chain::~Chain()
     {
     }

orocos_kdl/src/chain.hpp

@@ -95,6 +95,28 @@ namespace KDL {
          */
         Segment& getSegment(unsigned int nr);
 
+        /**
+         * Request to delete all child segments starting from the given index.
+         *
+         * @param nr the nr of the first segment to delete (all children will
+         * be removed as well).
+         *
+         * @return the number of deleted segments.
+         */
+        unsigned int deleteSegmentsFrom(unsigned int nr);
+
+        /**
+         * 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). Note that multiple segments with the same name
+         * are allowed in a `Chain`. The search in this function starts
+         * <strong>from the tip</strong> and stops at the first match (if any).
+         *
+         * @return the number of deleted segments.
+         */
+        unsigned int deleteSegmentsFrom(const std::string& name);
+
         virtual ~Chain();
     };
 

orocos_kdl/src/tree.cpp

@@ -21,6 +21,7 @@
 
 #include "tree.hpp"
 #include <sstream>
+#include <algorithm>
 
 namespace KDL {
 using namespace std;
@@ -174,4 +175,59 @@ bool Tree::getSubTree(const std::string& segment_name, Tree& tree) const
   return tree.addTreeRecursive(root, segment_name);
 }
 
+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->first);
+}
+
+unsigned int Tree::deleteSegmentsFrom(SegmentMap::const_iterator segment) {
+  // prevent to remove the root segment or a segment that does not exist
+  if(segment == segments.end() || segment == getRootSegment())
+    return 0;
+
+  // remove references to this segment from its parent
+  SegmentMap::iterator parent = segments.find(GetTreeElementParent(segment->second)->first);
+  std::vector<SegmentMap::const_iterator>& parent_children = GetTreeElementChildren(parent->second);
+  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 of segments
+  nrOfSegments -= ns;
+
+  if(nj > 0) {
+    // update joints indices if needed
+    nrOfJoints -= nj;
+    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) {
+  // delete segments using the iterator version; if name is the root
+  // or an invalid segment, this overload will exit immediately 
+  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:
         /**
@@ -219,6 +230,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

@@ -153,6 +153,57 @@ void KinFamTest::ChainTest()
     chain2.addChain(chain1);
     CPPUNIT_ASSERT_EQUAL(chain2.getNrOfJoints(),chain1.getNrOfJoints()*(uint)2);
     CPPUNIT_ASSERT_EQUAL(chain2.getNrOfSegments(),chain1.getNrOfSegments()*(uint)2);
+
+    // test segment removal from chains
+    Chain chain3(chain1);
+    // try to remove an inexistent segment
+    CPPUNIT_ASSERT_EQUAL(chain3.deleteSegmentsFrom("Non existent segment"), (uint)0);
+    CPPUNIT_ASSERT_EQUAL(chain3.getNrOfJoints(), chain1.getNrOfJoints());
+    CPPUNIT_ASSERT_EQUAL(chain3.getNrOfSegments(), chain1.getNrOfSegments());
+    // try to from an invalid index
+    CPPUNIT_ASSERT_EQUAL(chain3.deleteSegmentsFrom(chain3.getNrOfSegments()), (uint)0);
+    CPPUNIT_ASSERT_EQUAL(chain3.getNrOfJoints(), chain1.getNrOfJoints());
+    CPPUNIT_ASSERT_EQUAL(chain3.getNrOfSegments(), chain1.getNrOfSegments());
+    // remove the last segment (which is attached to a fixed joint)
+    CPPUNIT_ASSERT_EQUAL(chain3.deleteSegmentsFrom(chain3.getNrOfSegments()-1), (uint)1);
+    CPPUNIT_ASSERT_EQUAL(chain3.getNrOfJoints(), chain1.getNrOfJoints());
+    CPPUNIT_ASSERT_EQUAL(chain3.getNrOfSegments(), chain1.getNrOfSegments()-1);
+    // reset the chain, then try to remove all segments/joints
+    chain3 = chain1;
+    CPPUNIT_ASSERT_EQUAL(chain3.deleteSegmentsFrom(0), chain1.getNrOfSegments());
+    CPPUNIT_ASSERT_EQUAL(chain3.getNrOfJoints(), (uint)0);
+    CPPUNIT_ASSERT_EQUAL(chain3.getNrOfSegments(), (uint)0);
+    CPPUNIT_ASSERT(chain3.segments.empty());
+    // reset the chain, then try to remove the last 3 segments (having 2 moving joints)
+    chain3 = chain1;
+    CPPUNIT_ASSERT_EQUAL(chain3.deleteSegmentsFrom("Segment 4"), (uint)3);
+    CPPUNIT_ASSERT_EQUAL(chain3.getNrOfJoints(), chain1.getNrOfJoints()-2);
+    CPPUNIT_ASSERT_EQUAL(chain3.getNrOfSegments(), chain1.getNrOfSegments()-3);
+    CPPUNIT_ASSERT_EQUAL((uint)chain3.segments.size(), chain3.getNrOfSegments());
+    // create a new chain with some segment names whith repetitions
+    Chain chain4(chain1);
+    chain4.addSegment(Segment("SegmentX", Joint("JointX", Joint::None)));
+    chain4.addSegment(Segment("SegmentY", Joint("JointY", Joint::None)));
+    chain4.addSegment(Segment("SegmentY", Joint("JointY", Joint::None)));
+    chain4.addSegment(Segment("SegmentZ", Joint("JointZ", Joint::None)));
+    chain4.addSegment(Segment("SegmentX", Joint("JointX", Joint::None)));
+    chain4.addSegment(Segment("SegmentY", Joint("JointY", Joint::None)));
+    CPPUNIT_ASSERT_EQUAL(chain4.getNrOfSegments(), chain1.getNrOfSegments()+6);
+    CPPUNIT_ASSERT_EQUAL(chain4.getNrOfJoints(), chain1.getNrOfJoints());
+    chain3 = chain4;
+    CPPUNIT_ASSERT_EQUAL(chain3.deleteSegmentsFrom("SegmentY"), (uint)1);
+    CPPUNIT_ASSERT_EQUAL(chain3.deleteSegmentsFrom("SegmentX"), (uint)1);
+    CPPUNIT_ASSERT_EQUAL(chain3.deleteSegmentsFrom("SegmentY"), (uint)2);
+    CPPUNIT_ASSERT_EQUAL(chain3.deleteSegmentsFrom("SegmentY"), (uint)1);
+    CPPUNIT_ASSERT_EQUAL(chain3.deleteSegmentsFrom("SegmentX"), (uint)1);
+    CPPUNIT_ASSERT_EQUAL(chain3.getNrOfJoints(), chain4.getNrOfJoints());
+    CPPUNIT_ASSERT_EQUAL(chain3.getNrOfSegments(), chain4.getNrOfSegments()-6);
+    // reset the chain, then remove similarly to before
+    chain3  = chain4;
+    CPPUNIT_ASSERT_EQUAL(chain3.deleteSegmentsFrom("SegmentX"), (uint)2);
+    CPPUNIT_ASSERT_EQUAL(chain3.deleteSegmentsFrom("SegmentX"), (uint)4);
+    CPPUNIT_ASSERT_EQUAL(chain3.getNrOfJoints(), chain4.getNrOfJoints());
+    CPPUNIT_ASSERT_EQUAL(chain3.getNrOfSegments(), chain4.getNrOfSegments()-6);
 }
 
 // forward declaration, see below
@@ -244,6 +295,50 @@ void KinFamTest::TreeTest()
     cout << "Subtree (rooted at " << subroot << "):" << endl << tree2str(subtree) << endl;
     CPPUNIT_ASSERT(!isSubtree(tree1.getSegment(subroot), subtree.getRootSegment()));
     CPPUNIT_ASSERT(isSubtree(subtree.getRootSegment(), tree1.getSegment(subroot)));
+
+    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());
 }
 
 //Utility to check if the set of segments in contained is a subset of container.

orocos_kdl/tests/solvertest.cpp

@@ -326,6 +326,75 @@ void SolverTest::UpdateChainTest()
     CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= dynparam.JntToCoriolis(q_in, q_in2, q_out));
     CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= dynparam.JntToGravity(q_in, q_out));
     CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= dynparam.JntToMass(q_in, m));
+
+    chain2.deleteSegmentsFrom("Segment 6");
+
+    CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH,fksolverpos.JntToCart(q_in, T, chain2.getNrOfSegments()));
+    CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH,fksolvervel.JntToCart(q_in3, T2, chain2.getNrOfSegments()));
+    CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, jacsolver1.JntToJac(q_in, jac, chain2.getNrOfSegments()));
+    CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, jacdotsolver1.JntToJacDot(q_in3, jac, chain2.getNrOfSegments()));
+    CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, jacdotsolver1.JntToJacDot(q_in3, t, chain2.getNrOfSegments()));
+    CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, iksolver2.CartToJnt(q_in,t,q_out));
+    CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, iksolver_pinv_givens2.CartToJnt(q_in,t,q_out));
+    CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, iksolver_pinv_nso.CartToJnt(q_in,t,q_out));
+    CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, iksolver_wdls.CartToJnt(q_in,t,q_out));
+    CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, iksolverpos.CartToJnt(q_in,T,q_out));
+    CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, iksolverpos2.CartToJnt(q_in,T,q_out));
+    CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, iksolverpos3.CartToJnt(q_in,T,q_out));
+    CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, idsolver1.CartToJnt(q_in,q_in2,q_out,wrenches,q_out2));
+    CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, idsolver2.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches,q_out2));
+    CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, dynparam.JntToCoriolis(q_in, q_in2, q_out));
+    CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, dynparam.JntToGravity(q_in, q_out));
+    CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, dynparam.JntToMass(q_in, m));
+
+    fksolverpos.updateInternalDataStructures();
+    fksolvervel.updateInternalDataStructures();
+    jacsolver1.updateInternalDataStructures();
+    jacdotsolver1.updateInternalDataStructures();
+    iksolver2.updateInternalDataStructures();
+    iksolver_pinv_givens2.updateInternalDataStructures();
+    iksolver_pinv_nso.updateInternalDataStructures();
+    iksolver_wdls.updateInternalDataStructures();
+    iksolverpos.updateInternalDataStructures();
+    iksolverpos2.updateInternalDataStructures();
+    iksolverpos3.updateInternalDataStructures();
+    idsolver1.updateInternalDataStructures();
+    idsolver2.updateInternalDataStructures();
+    dynparam.updateInternalDataStructures();
+
+    q_in.resize(chain2.getNrOfJoints());
+    q_in2.resize(chain2.getNrOfJoints());
+    q_in3.resize(chain2.getNrOfJoints());
+    jac.resize(chain2.getNrOfJoints());
+    q_out.resize(chain2.getNrOfJoints());
+    q_out2.resize(chain2.getNrOfJoints());
+    wrenches.resize(chain2.getNrOfJoints());
+    m.resize(chain2.getNrOfJoints());
+
+    CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR,fksolverpos.JntToCart(q_in, T, chain2.getNrOfSegments()));
+    CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR,fksolvervel.JntToCart(q_in3, T2, chain2.getNrOfSegments()));
+    CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR,fksolverpos.JntToCart(q_in, T, chain2.getNrOfSegments()));
+    CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR,fksolvervel.JntToCart(q_in3, T2, chain2.getNrOfSegments()));
+    CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, jacsolver1.JntToJac(q_in, jac, chain2.getNrOfSegments()));
+    CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, jacdotsolver1.JntToJacDot(q_in3, jac, chain2.getNrOfSegments()));
+    CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, jacdotsolver1.JntToJacDot(q_in3, t, chain2.getNrOfSegments()));
+    CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= iksolver2.CartToJnt(q_in,t,q_out));
+    CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= iksolver_pinv_givens2.CartToJnt(q_in,t,q_out));
+    CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= iksolver_pinv_nso.CartToJnt(q_in,t,q_out));
+    CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= iksolver_wdls.CartToJnt(q_in,t,q_out));
+    CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= iksolverpos.CartToJnt(q_in,T,q_out));
+    CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= iksolverpos2.CartToJnt(q_in,T,q_out));
+    CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= iksolverpos3.CartToJnt(q_in,T,q_out));
+    CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= iksolverpos2.CartToJnt(q_in,T,q_out));
+    CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= iksolverpos3.CartToJnt(q_in,T,q_out));
+    CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= idsolver1.CartToJnt(q_in,q_in2,q_out,wrenches,q_out2));
+    CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= idsolver2.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches,q_out2));
+    CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= dynparam.JntToCoriolis(q_in, q_in2, q_out));
+    CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= dynparam.JntToGravity(q_in, q_out));
+    CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= dynparam.JntToMass(q_in, m));
+
+    chain2.addSegment(Segment("Segment 6", Joint("Joint 6", Joint::RotX),
+            Frame(Vector(0.0,0.0,0.1))));
 }
 void SolverTest::FkPosAndJacTest()
 {