From 904dda968d948802516a3b42758c32576837cc1a Mon Sep 17 00:00:00 2001 From: Franco Fusco Date: Mon, 28 Oct 2019 16:25:34 +0100 Subject: [PATCH 1/6] Segments removal in chains --- orocos_kdl/src/chain.cpp | 32 +++++++++++++++ orocos_kdl/src/chain.hpp | 22 +++++++++++ orocos_kdl/tests/kinfamtest.cpp | 51 ++++++++++++++++++++++++ orocos_kdl/tests/solvertest.cpp | 69 +++++++++++++++++++++++++++++++++ 4 files changed, 174 insertions(+) diff --git a/orocos_kdl/src/chain.cpp b/orocos_kdl/src/chain.cpp index 2b6320b27..94d3dc01a 100644 --- a/orocos_kdl/src/chain.cpp +++ b/orocos_kdl/src/chain.cpp @@ -20,6 +20,7 @@ // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA #include "chain.hpp" +#include namespace KDL { using namespace std; @@ -75,6 +76,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) + nrOfJoints -= std::count_if( + segments.begin()+nr, segments.end(), + [](const Segment& seg) { return seg.getJoint().getType() != Joint::None; } + ); + // 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; 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/tests/kinfamtest.cpp b/orocos_kdl/tests/kinfamtest.cpp index 2e2867616..07ef67d4a 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); } void KinFamTest::TreeTest() 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() { From 0465c90777af453414ae239e02eb270efcbc0ba2 Mon Sep 17 00:00:00 2001 From: Franco Fusco Date: Mon, 28 Oct 2019 19:00:20 +0100 Subject: [PATCH 2/6] Segments removal in trees --- orocos_kdl/src/tree.cpp | 60 +++++++++++++++++++++++++++++++++ orocos_kdl/src/tree.hpp | 44 ++++++++++++++++++++++++ orocos_kdl/tests/kinfamtest.cpp | 44 ++++++++++++++++++++++++ 3 files changed, 148 insertions(+) diff --git a/orocos_kdl/src/tree.cpp b/orocos_kdl/src/tree.cpp index 1ca751489..561291d3e 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; @@ -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; 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); +} + +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 diff --git a/orocos_kdl/src/tree.hpp b/orocos_kdl/src/tree.hpp index 3e33cc7f2..1a214f35c 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: /** @@ -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(){}; diff --git a/orocos_kdl/tests/kinfamtest.cpp b/orocos_kdl/tests/kinfamtest.cpp index 07ef67d4a..dbf0d984f 100644 --- a/orocos_kdl/tests/kinfamtest.cpp +++ b/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()); } From 53124907e85331e8dd5009e7504856968d6eca19 Mon Sep 17 00:00:00 2001 From: Franco Fusco Date: Tue, 29 Oct 2019 09:35:31 +0100 Subject: [PATCH 3/6] Minor updates --- orocos_kdl/src/tree.cpp | 16 ++++++---------- 1 file changed, 6 insertions(+), 10 deletions(-) diff --git a/orocos_kdl/src/tree.cpp b/orocos_kdl/src/tree.cpp index 561291d3e..99d8142e0 100644 --- a/orocos_kdl/src/tree.cpp +++ b/orocos_kdl/src/tree.cpp @@ -182,27 +182,25 @@ void Tree::deleteSegmentsRecursive(SegmentMap::const_iterator segment, unsigned } 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 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 + // update number of segments nrOfSegments -= ns; - nrOfJoints -= nj; 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) { @@ -216,10 +214,8 @@ unsigned int Tree::deleteSegmentsFrom(SegmentMap::const_iterator segment) { } 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 + // 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)); } From 15ba3b2478f75bf74ad4deff53bd937a807b789e Mon Sep 17 00:00:00 2001 From: Franco Fusco Date: Tue, 29 Oct 2019 09:57:08 +0100 Subject: [PATCH 4/6] Removed lambdas in chain.cpp --- orocos_kdl/src/chain.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/orocos_kdl/src/chain.cpp b/orocos_kdl/src/chain.cpp index 94d3dc01a..4cd0bad7c 100644 --- a/orocos_kdl/src/chain.cpp +++ b/orocos_kdl/src/chain.cpp @@ -20,7 +20,7 @@ // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA #include "chain.hpp" -#include +// #include namespace KDL { using namespace std; @@ -82,10 +82,10 @@ namespace KDL { if(nr >= nrOfSegments) return 0; // decrease the number of joints (once for each moving joint that is removed) - nrOfJoints -= std::count_if( - segments.begin()+nr, segments.end(), - [](const Segment& seg) { return seg.getJoint().getType() != Joint::None; } - ); + for(unsigned int i=nr; i Date: Tue, 29 Oct 2019 10:17:10 +0100 Subject: [PATCH 5/6] Removed auto keyword --- orocos_kdl/src/chain.cpp | 1 - orocos_kdl/src/tree.cpp | 6 +++--- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/orocos_kdl/src/chain.cpp b/orocos_kdl/src/chain.cpp index 4cd0bad7c..bba7150ac 100644 --- a/orocos_kdl/src/chain.cpp +++ b/orocos_kdl/src/chain.cpp @@ -20,7 +20,6 @@ // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA #include "chain.hpp" -// #include namespace KDL { using namespace std; diff --git a/orocos_kdl/src/tree.cpp b/orocos_kdl/src/tree.cpp index 99d8142e0..36e43cbff 100644 --- a/orocos_kdl/src/tree.cpp +++ b/orocos_kdl/src/tree.cpp @@ -178,7 +178,7 @@ void Tree::deleteSegmentsRecursive(SegmentMap::const_iterator segment, unsigned if(GetTreeElementSegment(segment->second).getJoint().getType() != Joint::None) nj++; // remove the segment from the map - segments.erase(segment); + segments.erase(segment->first); } unsigned int Tree::deleteSegmentsFrom(SegmentMap::const_iterator segment) { @@ -187,8 +187,8 @@ unsigned int Tree::deleteSegmentsFrom(SegmentMap::const_iterator segment) { return 0; // remove references to this segment from its parent - auto parent = segments.find(GetTreeElementParent(segment->second)->first); - auto& parent_children = GetTreeElementChildren(parent->second); + 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 From 4df48f12323b1d9e00a0e36c0033bb25ed5a5df4 Mon Sep 17 00:00:00 2001 From: Franco Fusco Date: Sat, 22 Feb 2020 16:03:31 +0100 Subject: [PATCH 6/6] Fixed issue in solvertest --- orocos_kdl/tests/solvertest.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/orocos_kdl/tests/solvertest.cpp b/orocos_kdl/tests/solvertest.cpp index fa3c27a5a..e07140ece 100644 --- a/orocos_kdl/tests/solvertest.cpp +++ b/orocos_kdl/tests/solvertest.cpp @@ -225,6 +225,10 @@ void SolverTest::UpdateChainTest() CPPUNIT_ASSERT_EQUAL((int)SolverI::E_OUT_OF_RANGE, jacdotsolver1.JntToJacDot(q_in3, jac, chain2.getNrOfSegments()+1)); chain2.addSegment(Segment("Segment 6", Joint("Joint 6", Joint::RotX), Frame(Vector(0.0,0.0,0.1)))); + chain2.addSegment(Segment("Segment 7", Joint("Joint 7", Joint::RotY), + Frame(Vector(0.1,0.2,0.1)))); + chain2.addSegment(Segment("Segment 8", Joint("Joint 8", Joint::RotZ), + Frame(Vector(-0.1,0,0)))); 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())); @@ -327,7 +331,7 @@ void SolverTest::UpdateChainTest() 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"); + chain2.deleteSegmentsFrom("Segment 7"); 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())); @@ -392,9 +396,6 @@ 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.addSegment(Segment("Segment 6", Joint("Joint 6", Joint::RotX), - Frame(Vector(0.0,0.0,0.1)))); } void SolverTest::FkPosAndJacTest() {