Skip to content

Commit f7fba0e

Browse files
meyerjmeyerj
committed
Merge branch 'segments-removal' of https://github.com/francofusco/orocos_kinematics_dynamics into francofusco-segments-removal
2 parents b35c424 + f591582 commit f7fba0e

File tree

6 files changed

+317
-0
lines changed

6 files changed

+317
-0
lines changed

‎orocos_kdl/src/chain.cpp‎

Lines changed: 31 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -75,6 +75,37 @@ namespace KDL {
7575
return segments[nr];
7676
}
7777

78+
unsigned int Chain::deleteSegmentsFrom(unsigned int nr)
79+
{
80+
// make sure the index is valid
81+
if(nr >= nrOfSegments)
82+
return 0;
83+
// decrease the number of joints (once for each moving joint that is removed)
84+
for(unsigned int i=nr; i<segments.size(); i++) {
85+
if(segments[i].getJoint().getType() != Joint::None)
86+
nrOfJoints--;
87+
}
88+
// number of segments to be deleted
89+
unsigned int to_del = nrOfSegments - nr;
90+
// reset the number of segments
91+
nrOfSegments = nr;
92+
segments.resize(nr);
93+
return to_del;
94+
}
95+
96+
unsigned int Chain::deleteSegmentsFrom(const std::string& name)
97+
{
98+
unsigned int irev;
99+
for(unsigned int i=0; i<nrOfSegments; i++) {
100+
irev = nrOfSegments-i-1;
101+
if(segments[irev].getName() == name) {
102+
// remove the segment
103+
return deleteSegmentsFrom(irev);
104+
}
105+
}
106+
return 0;
107+
}
108+
78109
Chain::~Chain()
79110
{
80111
}

‎orocos_kdl/src/chain.hpp‎

Lines changed: 22 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -95,6 +95,28 @@ namespace KDL {
9595
*/
9696
Segment& getSegment(unsigned int nr);
9797

98+
/**
99+
* Request to delete all child segments starting from the given index.
100+
*
101+
* @param nr the nr of the first segment to delete (all children will
102+
* be removed as well).
103+
*
104+
* @return the number of deleted segments.
105+
*/
106+
unsigned int deleteSegmentsFrom(unsigned int nr);
107+
108+
/**
109+
* Request to delete all child segments starting from the one with given name.
110+
*
111+
* @param name the name of the first segment to delete (all children will
112+
* be removed as well). Note that multiple segments with the same name
113+
* are allowed in a `Chain`. The search in this function starts
114+
* <strong>from the tip</strong> and stops at the first match (if any).
115+
*
116+
* @return the number of deleted segments.
117+
*/
118+
unsigned int deleteSegmentsFrom(const std::string& name);
119+
98120
virtual ~Chain();
99121
};
100122

‎orocos_kdl/src/tree.cpp‎

Lines changed: 56 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,7 @@
2121

2222
#include "tree.hpp"
2323
#include <sstream>
24+
#include <algorithm>
2425

2526
namespace KDL {
2627
using namespace std;
@@ -174,4 +175,59 @@ bool Tree::getSubTree(const std::string& segment_name, Tree& tree) const
174175
return tree.addTreeRecursive(root, segment_name);
175176
}
176177

178+
void Tree::deleteSegmentsRecursive(SegmentMap::const_iterator segment, unsigned int& ns, unsigned int& nj) {
179+
// delete all children (if any)
180+
SegmentMap::const_iterator child;
181+
for(unsigned int i=0; i<GetTreeElementChildren(segment->second).size(); i++) {
182+
// delete i-th child
183+
child = GetTreeElementChildren(segment->second)[i];
184+
deleteSegmentsRecursive(child, ns, nj);
185+
}
186+
187+
// update ns and nj
188+
ns++;
189+
if(GetTreeElementSegment(segment->second).getJoint().getType() != Joint::None)
190+
nj++;
191+
// remove the segment from the map
192+
segments.erase(segment->first);
193+
}
194+
195+
unsigned int Tree::deleteSegmentsFrom(SegmentMap::const_iterator segment) {
196+
// prevent to remove the root segment or a segment that does not exist
197+
if(segment == segments.end() || segment == getRootSegment())
198+
return 0;
199+
200+
// remove references to this segment from its parent
201+
SegmentMap::iterator parent = segments.find(GetTreeElementParent(segment->second)->first);
202+
std::vector<SegmentMap::const_iterator>& parent_children = GetTreeElementChildren(parent->second);
203+
parent_children.erase(std::remove(parent_children.begin(), parent_children.end(), segment));
204+
205+
// delete children recursively
206+
unsigned int ns=0, nj=0;
207+
deleteSegmentsRecursive(segment, ns, nj);
208+
209+
// update number of segments
210+
nrOfSegments -= ns;
211+
212+
if(nj > 0) {
213+
// update joints indices if needed
214+
nrOfJoints -= nj;
215+
unsigned int nq = 0;
216+
for(SegmentMap::iterator s=segments.begin(); s!=segments.end(); s++) {
217+
if(GetTreeElementSegment(s->second).getJoint().getType() != Joint::None) {
218+
GetTreeElementQNr(s->second) = nq;
219+
nq++;
220+
}
221+
}
222+
}
223+
224+
return ns;
225+
}
226+
227+
unsigned int Tree::deleteSegmentsFrom(const std::string& name) {
228+
// delete segments using the iterator version; if name is the root
229+
// or an invalid segment, this overload will exit immediately
230+
return deleteSegmentsFrom(segments.find(name));
231+
}
232+
177233
}//end of namespace

‎orocos_kdl/src/tree.hpp‎

Lines changed: 44 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -106,6 +106,17 @@ namespace KDL
106106
std::string root_name;
107107

108108
bool addTreeRecursive(SegmentMap::const_iterator root, const std::string& hook_name);
109+
110+
/** Removes all child segments of `segment` (and `segment` itself).
111+
*
112+
* @param segment Iterator pointing to the segment to be deleted.
113+
* @param ns Total number of segments that are removed in this way.
114+
* @param nj Total number of moving joints that are removed in this way.
115+
*
116+
* @note A part from the `segments` map, no internal variables are
117+
* modeified here, *ie*, `nrOfJoints` and `nrOfSegments` are untouched.
118+
*/
119+
void deleteSegmentsRecursive(SegmentMap::const_iterator segment, unsigned int& ns, unsigned int& nj);
109120

110121
public:
111122
/**
@@ -219,6 +230,39 @@ namespace KDL
219230
{
220231
return segments;
221232
}
233+
234+
/**
235+
* Request to delete all child segments starting from the given element.
236+
*
237+
* @param segment iterator of the first segment to delete (all children
238+
* will be removed as well).
239+
*
240+
* @return the number of deleted segments.
241+
*
242+
* @note The root segment cannot be removed from the tree.
243+
*
244+
* @note If moving joints are removed in this way, joint indices
245+
* are recomputed internally.
246+
*
247+
* @warning The behavior is undefined if `segment` is not a valid
248+
* iterator (note that `getSegments().end()` is valid).
249+
*/
250+
unsigned int deleteSegmentsFrom(SegmentMap::const_iterator segment);
251+
252+
/**
253+
* Request to delete all child segments starting from the one with given name.
254+
*
255+
* @param name the name of the first segment to delete (all children
256+
* will be removed as well).
257+
*
258+
* @return the number of deleted segments.
259+
*
260+
* @note The root segment cannot be removed from the tree.
261+
*
262+
* @note If moving joints are removed in this way, joint indices
263+
* are recomputed internally.
264+
*/
265+
unsigned int deleteSegmentsFrom(const std::string& name);
222266

223267
virtual ~Tree(){};
224268

‎orocos_kdl/tests/kinfamtest.cpp‎

Lines changed: 95 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -153,6 +153,57 @@ void KinFamTest::ChainTest()
153153
chain2.addChain(chain1);
154154
CPPUNIT_ASSERT_EQUAL(chain2.getNrOfJoints(),chain1.getNrOfJoints()*(uint)2);
155155
CPPUNIT_ASSERT_EQUAL(chain2.getNrOfSegments(),chain1.getNrOfSegments()*(uint)2);
156+
157+
// test segment removal from chains
158+
Chain chain3(chain1);
159+
// try to remove an inexistent segment
160+
CPPUNIT_ASSERT_EQUAL(chain3.deleteSegmentsFrom("Non existent segment"), (uint)0);
161+
CPPUNIT_ASSERT_EQUAL(chain3.getNrOfJoints(), chain1.getNrOfJoints());
162+
CPPUNIT_ASSERT_EQUAL(chain3.getNrOfSegments(), chain1.getNrOfSegments());
163+
// try to from an invalid index
164+
CPPUNIT_ASSERT_EQUAL(chain3.deleteSegmentsFrom(chain3.getNrOfSegments()), (uint)0);
165+
CPPUNIT_ASSERT_EQUAL(chain3.getNrOfJoints(), chain1.getNrOfJoints());
166+
CPPUNIT_ASSERT_EQUAL(chain3.getNrOfSegments(), chain1.getNrOfSegments());
167+
// remove the last segment (which is attached to a fixed joint)
168+
CPPUNIT_ASSERT_EQUAL(chain3.deleteSegmentsFrom(chain3.getNrOfSegments()-1), (uint)1);
169+
CPPUNIT_ASSERT_EQUAL(chain3.getNrOfJoints(), chain1.getNrOfJoints());
170+
CPPUNIT_ASSERT_EQUAL(chain3.getNrOfSegments(), chain1.getNrOfSegments()-1);
171+
// reset the chain, then try to remove all segments/joints
172+
chain3 = chain1;
173+
CPPUNIT_ASSERT_EQUAL(chain3.deleteSegmentsFrom(0), chain1.getNrOfSegments());
174+
CPPUNIT_ASSERT_EQUAL(chain3.getNrOfJoints(), (uint)0);
175+
CPPUNIT_ASSERT_EQUAL(chain3.getNrOfSegments(), (uint)0);
176+
CPPUNIT_ASSERT(chain3.segments.empty());
177+
// reset the chain, then try to remove the last 3 segments (having 2 moving joints)
178+
chain3 = chain1;
179+
CPPUNIT_ASSERT_EQUAL(chain3.deleteSegmentsFrom("Segment 4"), (uint)3);
180+
CPPUNIT_ASSERT_EQUAL(chain3.getNrOfJoints(), chain1.getNrOfJoints()-2);
181+
CPPUNIT_ASSERT_EQUAL(chain3.getNrOfSegments(), chain1.getNrOfSegments()-3);
182+
CPPUNIT_ASSERT_EQUAL((uint)chain3.segments.size(), chain3.getNrOfSegments());
183+
// create a new chain with some segment names whith repetitions
184+
Chain chain4(chain1);
185+
chain4.addSegment(Segment("SegmentX", Joint("JointX", Joint::None)));
186+
chain4.addSegment(Segment("SegmentY", Joint("JointY", Joint::None)));
187+
chain4.addSegment(Segment("SegmentY", Joint("JointY", Joint::None)));
188+
chain4.addSegment(Segment("SegmentZ", Joint("JointZ", Joint::None)));
189+
chain4.addSegment(Segment("SegmentX", Joint("JointX", Joint::None)));
190+
chain4.addSegment(Segment("SegmentY", Joint("JointY", Joint::None)));
191+
CPPUNIT_ASSERT_EQUAL(chain4.getNrOfSegments(), chain1.getNrOfSegments()+6);
192+
CPPUNIT_ASSERT_EQUAL(chain4.getNrOfJoints(), chain1.getNrOfJoints());
193+
chain3 = chain4;
194+
CPPUNIT_ASSERT_EQUAL(chain3.deleteSegmentsFrom("SegmentY"), (uint)1);
195+
CPPUNIT_ASSERT_EQUAL(chain3.deleteSegmentsFrom("SegmentX"), (uint)1);
196+
CPPUNIT_ASSERT_EQUAL(chain3.deleteSegmentsFrom("SegmentY"), (uint)2);
197+
CPPUNIT_ASSERT_EQUAL(chain3.deleteSegmentsFrom("SegmentY"), (uint)1);
198+
CPPUNIT_ASSERT_EQUAL(chain3.deleteSegmentsFrom("SegmentX"), (uint)1);
199+
CPPUNIT_ASSERT_EQUAL(chain3.getNrOfJoints(), chain4.getNrOfJoints());
200+
CPPUNIT_ASSERT_EQUAL(chain3.getNrOfSegments(), chain4.getNrOfSegments()-6);
201+
// reset the chain, then remove similarly to before
202+
chain3 = chain4;
203+
CPPUNIT_ASSERT_EQUAL(chain3.deleteSegmentsFrom("SegmentX"), (uint)2);
204+
CPPUNIT_ASSERT_EQUAL(chain3.deleteSegmentsFrom("SegmentX"), (uint)4);
205+
CPPUNIT_ASSERT_EQUAL(chain3.getNrOfJoints(), chain4.getNrOfJoints());
206+
CPPUNIT_ASSERT_EQUAL(chain3.getNrOfSegments(), chain4.getNrOfSegments()-6);
156207
}
157208

158209
// forward declaration, see below
@@ -244,6 +295,50 @@ void KinFamTest::TreeTest()
244295
cout << "Subtree (rooted at " << subroot << "):" << endl << tree2str(subtree) << endl;
245296
CPPUNIT_ASSERT(!isSubtree(tree1.getSegment(subroot), subtree.getRootSegment()));
246297
CPPUNIT_ASSERT(isSubtree(subtree.getRootSegment(), tree1.getSegment(subroot)));
298+
299+
Tree tree3("root");
300+
tree3.addSegment(Segment("S1", Joint("J1", Joint::RotX)), "root");
301+
tree3.addSegment(Segment("S2", Joint("J2", Joint::RotX)), "root");
302+
tree3.addSegment(Segment("S3", Joint("J3", Joint::RotX)), "S2");
303+
tree3.addSegment(Segment("S4", Joint("J4", Joint::None)), "S3");
304+
tree3.addSegment(Segment("S5", Joint("J5", Joint::RotX)), "S2");
305+
tree3.addSegment(Segment("S6", Joint("J6", Joint::RotX)), "S5");
306+
tree3.addSegment(Segment("S7", Joint("J7", Joint::None)), "S5");
307+
cout << "Tree 3:" << endl << tree3 << endl;
308+
309+
Tree tree4(tree3);
310+
tree4.deleteSegmentsFrom("S1");
311+
CPPUNIT_ASSERT_EQUAL(tree4.getNrOfSegments(), (uint)6);
312+
CPPUNIT_ASSERT_EQUAL(tree4.getNrOfJoints(), (uint)4);
313+
cout << "After removing S1:" << endl << tree4 << endl;
314+
315+
tree4 = tree3;
316+
tree4.deleteSegmentsFrom("S2");
317+
CPPUNIT_ASSERT_EQUAL(tree4.getNrOfSegments(), (uint)1);
318+
CPPUNIT_ASSERT_EQUAL(tree4.getNrOfJoints(), (uint)1);
319+
cout << "After removing S2:" << endl << tree4 << endl;
320+
321+
tree4 = tree3;
322+
tree4.deleteSegmentsFrom("S3");
323+
CPPUNIT_ASSERT_EQUAL(tree4.getNrOfSegments(), (uint)5);
324+
CPPUNIT_ASSERT_EQUAL(tree4.getNrOfJoints(), (uint)4);
325+
cout << "After removing S3:" << endl << tree4 << endl;
326+
327+
tree4 = tree3;
328+
tree4.deleteSegmentsFrom("S7");
329+
CPPUNIT_ASSERT_EQUAL(tree4.getNrOfSegments(), (uint)6);
330+
CPPUNIT_ASSERT_EQUAL(tree4.getNrOfJoints(), (uint)5);
331+
cout << "After removing S7:" << endl << tree4 << endl;
332+
333+
tree4 = tree3;
334+
tree4.deleteSegmentsFrom("ABCDEF");
335+
CPPUNIT_ASSERT_EQUAL(tree4.getNrOfSegments(), tree3.getNrOfSegments());
336+
CPPUNIT_ASSERT_EQUAL(tree4.getNrOfJoints(), tree3.getNrOfJoints());
337+
338+
tree4 = tree3;
339+
tree4.deleteSegmentsFrom("root");
340+
CPPUNIT_ASSERT_EQUAL(tree4.getNrOfSegments(), tree3.getNrOfSegments());
341+
CPPUNIT_ASSERT_EQUAL(tree4.getNrOfJoints(), tree3.getNrOfJoints());
247342
}
248343

249344
//Utility to check if the set of segments in contained is a subset of container.

‎orocos_kdl/tests/solvertest.cpp‎

Lines changed: 69 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -326,6 +326,75 @@ void SolverTest::UpdateChainTest()
326326
CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= dynparam.JntToCoriolis(q_in, q_in2, q_out));
327327
CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= dynparam.JntToGravity(q_in, q_out));
328328
CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= dynparam.JntToMass(q_in, m));
329+
330+
chain2.deleteSegmentsFrom("Segment 6");
331+
332+
CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH,fksolverpos.JntToCart(q_in, T, chain2.getNrOfSegments()));
333+
CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH,fksolvervel.JntToCart(q_in3, T2, chain2.getNrOfSegments()));
334+
CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, jacsolver1.JntToJac(q_in, jac, chain2.getNrOfSegments()));
335+
CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, jacdotsolver1.JntToJacDot(q_in3, jac, chain2.getNrOfSegments()));
336+
CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, jacdotsolver1.JntToJacDot(q_in3, t, chain2.getNrOfSegments()));
337+
CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, iksolver2.CartToJnt(q_in,t,q_out));
338+
CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, iksolver_pinv_givens2.CartToJnt(q_in,t,q_out));
339+
CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, iksolver_pinv_nso.CartToJnt(q_in,t,q_out));
340+
CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, iksolver_wdls.CartToJnt(q_in,t,q_out));
341+
CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, iksolverpos.CartToJnt(q_in,T,q_out));
342+
CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, iksolverpos2.CartToJnt(q_in,T,q_out));
343+
CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, iksolverpos3.CartToJnt(q_in,T,q_out));
344+
CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, idsolver1.CartToJnt(q_in,q_in2,q_out,wrenches,q_out2));
345+
CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, idsolver2.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches,q_out2));
346+
CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, dynparam.JntToCoriolis(q_in, q_in2, q_out));
347+
CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, dynparam.JntToGravity(q_in, q_out));
348+
CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, dynparam.JntToMass(q_in, m));
349+
350+
fksolverpos.updateInternalDataStructures();
351+
fksolvervel.updateInternalDataStructures();
352+
jacsolver1.updateInternalDataStructures();
353+
jacdotsolver1.updateInternalDataStructures();
354+
iksolver2.updateInternalDataStructures();
355+
iksolver_pinv_givens2.updateInternalDataStructures();
356+
iksolver_pinv_nso.updateInternalDataStructures();
357+
iksolver_wdls.updateInternalDataStructures();
358+
iksolverpos.updateInternalDataStructures();
359+
iksolverpos2.updateInternalDataStructures();
360+
iksolverpos3.updateInternalDataStructures();
361+
idsolver1.updateInternalDataStructures();
362+
idsolver2.updateInternalDataStructures();
363+
dynparam.updateInternalDataStructures();
364+
365+
q_in.resize(chain2.getNrOfJoints());
366+
q_in2.resize(chain2.getNrOfJoints());
367+
q_in3.resize(chain2.getNrOfJoints());
368+
jac.resize(chain2.getNrOfJoints());
369+
q_out.resize(chain2.getNrOfJoints());
370+
q_out2.resize(chain2.getNrOfJoints());
371+
wrenches.resize(chain2.getNrOfJoints());
372+
m.resize(chain2.getNrOfJoints());
373+
374+
CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR,fksolverpos.JntToCart(q_in, T, chain2.getNrOfSegments()));
375+
CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR,fksolvervel.JntToCart(q_in3, T2, chain2.getNrOfSegments()));
376+
CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR,fksolverpos.JntToCart(q_in, T, chain2.getNrOfSegments()));
377+
CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR,fksolvervel.JntToCart(q_in3, T2, chain2.getNrOfSegments()));
378+
CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, jacsolver1.JntToJac(q_in, jac, chain2.getNrOfSegments()));
379+
CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, jacdotsolver1.JntToJacDot(q_in3, jac, chain2.getNrOfSegments()));
380+
CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, jacdotsolver1.JntToJacDot(q_in3, t, chain2.getNrOfSegments()));
381+
CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= iksolver2.CartToJnt(q_in,t,q_out));
382+
CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= iksolver_pinv_givens2.CartToJnt(q_in,t,q_out));
383+
CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= iksolver_pinv_nso.CartToJnt(q_in,t,q_out));
384+
CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= iksolver_wdls.CartToJnt(q_in,t,q_out));
385+
CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= iksolverpos.CartToJnt(q_in,T,q_out));
386+
CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= iksolverpos2.CartToJnt(q_in,T,q_out));
387+
CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= iksolverpos3.CartToJnt(q_in,T,q_out));
388+
CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= iksolverpos2.CartToJnt(q_in,T,q_out));
389+
CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= iksolverpos3.CartToJnt(q_in,T,q_out));
390+
CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= idsolver1.CartToJnt(q_in,q_in2,q_out,wrenches,q_out2));
391+
CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= idsolver2.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches,q_out2));
392+
CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= dynparam.JntToCoriolis(q_in, q_in2, q_out));
393+
CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= dynparam.JntToGravity(q_in, q_out));
394+
CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= dynparam.JntToMass(q_in, m));
395+
396+
chain2.addSegment(Segment("Segment 6", Joint("Joint 6", Joint::RotX),
397+
Frame(Vector(0.0,0.0,0.1))));
329398
}
330399
void SolverTest::FkPosAndJacTest()
331400
{

0 commit comments

Comments
 (0)