diff --git a/orocos_kdl/src/chain.cpp b/orocos_kdl/src/chain.cpp index 2b6320b27..bba7150ac 100644 --- a/orocos_kdl/src/chain.cpp +++ b/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; ifrom the tip and stops at the first match (if any). + * + * @return the number of deleted segments. + */ + unsigned int deleteSegmentsFrom(const std::string& name); + virtual ~Chain(); }; diff --git a/orocos_kdl/src/tree.cpp b/orocos_kdl/src/tree.cpp index feb097bc0..f8c201550 100644 --- a/orocos_kdl/src/tree.cpp +++ b/orocos_kdl/src/tree.cpp @@ -21,6 +21,7 @@ #include "tree.hpp" #include +#include 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; isecond).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& 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 diff --git a/orocos_kdl/src/tree.hpp b/orocos_kdl/src/tree.hpp index fd664e879..38abeb870 100644 --- a/orocos_kdl/src/tree.hpp +++ b/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(){}; diff --git a/orocos_kdl/tests/kinfamtest.cpp b/orocos_kdl/tests/kinfamtest.cpp index 19e441f51..9ab808888 100644 --- a/orocos_kdl/tests/kinfamtest.cpp +++ b/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. diff --git a/orocos_kdl/tests/solvertest.cpp b/orocos_kdl/tests/solvertest.cpp index 7864e9096..fa3c27a5a 100644 --- a/orocos_kdl/tests/solvertest.cpp +++ b/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() {