On 08/22/2012 11:56 AM, WouterBancken wrote:
>
> http://bugs.orocos.org/show_bug.cgi?id=981
>
> Summary: Inconsistancy in getChain in tree.cpp
> Product: KDL
> Version: unspecified
> Platform: All
> OS/Version: All
> Status: NEW
> Severity: normal
> Priority: P3
> Component: Kinematic chains
> AssignedTo: orocos-dev [..] ...
> ReportedBy: wouter [dot] bancken [..] ...
> CC: orocos-dev [..] ...
> Estimated Hours: 0.0
>
>
> There is an inconsistancy in the getChain method in tree.cpp.
>
> When you request a chain that passes the root, the chain_root will be included
> in the new chain and this chain_root segment will be renamed to the root_name
> of the original tree.
>
> When you request a chain that does not pass the root, the chain_root will not
> be included in the new chain.
>
> Example:
>
> Segment segment1("Segment 1", Joint("Joint 1", Joint::None));
> Segment segment2("Segment 2", Joint("Joint 2",
> Joint::RotX),Frame(Vector(0.1,0.2,0.3)));
> Segment segment3("Segment 3", Joint("Joint 3",
> Joint::TransZ),Frame(Rotation::RotX(1.57)));
> Segment segment4("Segment 4", Joint("Joint 4",
> Joint::RotX),Frame(Vector(0.1,0.2,0.3)));
> Segment segment5("Segment 5", Joint("Joint 5",
> Joint::RotX),Frame(Vector(0.1,0.2,0.3)));
> Segment segment6("Segment 6", Joint("Joint 6",
> Joint::RotX),Frame(Vector(0.1,0.2,0.3)));
> Segment segment7("Segment 7", Joint("Joint 7",
> Joint::RotX),Frame(Vector(0.1,0.2,0.3)));
> Segment segment8("Segment 8", Joint("Joint 8",
> Joint::RotZ),Frame(Vector(0.0,0.0,0.0)));
> Segment segment9("Segment 9", Joint("Joint 9",
> Joint::RotX),Frame(Vector(0.0,0.0,0.9)));
> Segment segment10("Segment 10", Joint("Joint 10",
> Joint::RotX),Frame(Vector(0.0,0.0,1.2)));
> Segment segment11("Segment 11", Joint("Joint 11",
> Joint::RotZ),Frame(Vector(0.0,0.0,1.5)));
> Segment segment12("Segment 12", Joint("Joint 12",
> Joint::RotX),Frame(Vector(0.0,0.0,0.0)));
> Segment segment13("Segment 13", Joint("Joint 13",
> Joint::RotZ),Frame(Vector(0.0,0.0,0.4)));
>
> Tree tree1;
> tree1.addSegment(segment1,"root");
> tree1.addSegment(segment2,"root");
> tree1.addSegment(segment3,"Segment 1");
> tree1.addSegment(segment4,"Segment 3");
> tree1.addSegment(segment5,"Segment 2");
> tree1.addSegment(segment6,"Segment 2");
> tree1.addSegment(segment7,"Segment 6");
> tree1.addSegment(segment8,"Segment 6");
> tree1.addSegment(segment9,"Segment 8");
> tree1.addSegment(segment10,"Segment 9");
> tree1.addSegment(segment11,"Segment 10");
> tree1.addSegment(segment12,"Segment 11");
> tree1.addSegment(segment13,"Segment 12");
>
> Chain extract_chain1;
> tree1.getChain("Segment 2", "Segment 4", extract_chain1);
> Chain extract_chain2;
> tree1.getChain("Segment 9", "Segment 12", extract_chain2);
>
> In this example extract_chain1 will consist of: root (= segment 2) - segment 1
> - segment 3 - segment 4
>
> and extract_chain2 will consist of: segment 10 - segment 11 - segment 12
>
> (Patch with proposed solution will follow.)
>

here is an example of the problem with output:

CODE
-----

Tree tree1;
Segment segment1("Segment 1", Joint("Joint 1", Joint::None),Frame(Vector(0.1,0.2,0.3)));
Segment segment2("Segment 2", Joint("Joint 2", Joint::RotX),Frame(Vector(0.4,0.5,0.6)));
Segment segment3("Segment 3", Joint("Joint 3", Joint::TransZ),Frame(Rotation::RotX(1.57)));
Segment segment4("Segment 4", Joint("Joint 4", Joint::RotX),Frame(Vector(0.7,0.8,0.9)));
Segment segment5("Segment 5", Joint("Joint 5", Joint::RotX),Frame(Vector(0.11,0.12,0.13)));
Segment segment6("Segment 6", Joint("Joint 6", Joint::RotX),Frame(Vector(0.14,0.15,0.16)));
Segment segment7("Segment 7", Joint("Joint 7", Joint::RotX),Frame(Vector(0.17,0.18,0.19)));

CPPUNIT_ASSERT(tree1.addSegment(segment1,"root"));
CPPUNIT_ASSERT(tree1.addSegment(segment2,"root"));
CPPUNIT_ASSERT(tree1.addSegment(segment3,"Segment 1"));
CPPUNIT_ASSERT(tree1.addSegment(segment4,"Segment 3"));

Tree tree2;
CPPUNIT_ASSERT(tree2.addSegment(segment5,"root"));
CPPUNIT_ASSERT(tree2.addSegment(segment6,"root"));
CPPUNIT_ASSERT(tree2.addSegment(segment7,"Segment 6"));

Chain chain1;
chain1.addSegment(Segment("Segment 8", Joint("Joint 8", Joint::RotZ),
Frame(Vector(0.0,0.0,0.0))));
chain1.addSegment(Segment("Segment 9", Joint("Joint 9", Joint::RotX),
Frame(Vector(0.0,0.0,0.9))));
chain1.addSegment(Segment("Segment 10", Joint("Joint 10", Joint::RotX),
Frame(Vector(0.0,0.0,1.2))));
chain1.addSegment(Segment("Segment 11", Joint("Joint 11", Joint::RotZ),
Frame(Vector(0.0,0.0,1.5))));
chain1.addSegment(Segment("Segment 12", Joint("Joint 12", Joint::RotX),
Frame(Vector(0.0,0.0,0.0))));
chain1.addSegment(Segment("Segment 13", Joint("Joint 13", Joint::RotZ),
Frame(Vector(0.0,0.0,0.4))));

CPPUNIT_ASSERT(tree2.addChain(chain1, "Segment 6"));

CPPUNIT_ASSERT(tree1.addTree(tree2, "Segment 2"));
tree1.addSegment(Segment("Segment 14", Joint("Joint 14", Joint::RotZ),Frame(Vector(0.20,0.21,0.22))),"Segment 5");

=>case you get a chain without going trough the tree root
Chain extract_chain3;
CPPUNIT_ASSERT(tree1.getChain("Segment 1", "Segment 4", extract_chain3));
cout << "1 to 4" << endl << extract_chain3 << endl << endl << endl;

Chain extract_chain4;
CPPUNIT_ASSERT(tree1.getChain("Segment 4", "Segment 1", extract_chain4));
cout << "4 to 1" << endl << extract_chain4 << endl << endl << endl;

=>case you get a chain with going trough the tree root
Chain extract_chain5;
CPPUNIT_ASSERT(tree1.getChain("Segment 6", "Segment 4", extract_chain5));
cout << "6 naar 4" << endl << extract_chain5 << endl << endl << endl;

=>case you get a chain going trough a branching point
Chain extract_chain6;
CPPUNIT_ASSERT(tree1.getChain("Segment 8", "Segment 14", extract_chain6));
cout << "8 naar 14" << endl << extract_chain6 << endl << endl << endl;

OUTPUT
-------

1 to 4
-------
[Segment 3:[Joint 3:[TransZ, axis: [ 0, 0, 1], origin[ 0, 0, 0]],
tip:
[[ 1, 0, 0;
0, 0.000796327, -1;
0, 1, 0.000796327]
[ 0, 0, 0]]]
Segment 4:[Joint 4:[RotX, axis: [ 1, 0, 0], origin[ 0, 0, 0]],
tip:
[[ 1, 0, 0;
0, 1, 0;
0, 0, 1]
[ 0.7, 0.8, 0.9]]]
]
=> for this case you don't get the first segment back, but names are ok (no shift)!

4 to 1
--------
[Segment 3:[Joint 4:[RotAxis, axis: [ -1, -0, -0], origin[ -0.7, -0.8, -0.9]],
tip:
[[ 1, 0, 0;
0, 1, 0;
0, 0, 1]
[ -0.7, -0.8, -0.9]]]
Segment 1:[Joint 3:[TransAxis, axis: [ -0, -1,-0.000796327], origin[ 0, 0, 0]],
tip:
[[ 1, 0, 0;
0, 0.000796327, 1;
0, -1, 0.000796327]
[ 0, 0, 0]]]
]
=>also for the inverse case, you don't get the 4th segment back, although you asked for getChain("Segment 4", "Segment 1", extract_chain4))

6 naar 4
---------
[Segment 2:[Joint 6:[RotAxis, axis: [ -1, -0, -0], origin[ -0.14, -0.15, -0.16]],
tip:
[[ 1, 0, 0;
0, 1, 0;
0, 0, 1]
[ -0.14, -0.15, -0.16]]]

=> how to interpret this segment? the original 6th segment has a RotX joint at its segment_root and its tip is at [ 0.14, 0.15, 0.16]
When going in the other direction, you invert root and tip:
so the location of the joint as well as the tip are with respect to the new segment_root, which is the old tip
correct?
=>then why is this called segment 2, while it is segment 6?
=> so you DO get the 6th segment back, in contrast to the first two cases, where you didn't pass trough the root

root:[Joint 2:[RotAxis, axis: [ -1, -0, -0], origin[ -0.4, -0.5, -0.6]],
tip:
[[ 1, 0, 0;
0, 1, 0;
0, 0, 1]
[ -0.4, -0.5, -0.6]]]
=> this is the actual segment 2

Segment 1:[Joint 1:[None, axis: [ 0, 0, 0], origin[ 0, 0, 0]],
tip:
[[ 1, 0, 0;
0, 1, 0;
0, 0, 1]
[ 0.1, 0.2, 0.3]]]
=> this is a mix of root and segment 1, we loose a segment with respect to before (the empty root, this could be intentional)

Segment 3:[Joint 3:[TransZ, axis: [ 0, 0, 1], origin[ 0, 0, 0]],
tip:
[[ 1, 0, 0;
0, 0.000796327, -1;
0, 1, 0.000796327]
[ 0, 0, 0]]]
=> this is correctly segment 3

Segment 4:[Joint 4:[RotX, axis: [ 1, 0, 0], origin[ 0, 0, 0]],
tip:
[[ 1, 0, 0;
0, 1, 0;
0, 0, 1]
[ 0.7, 0.8, 0.9]]]
]
=> this is correctly segment 4

8 to 14
---------
[Segment 6:[Joint 8:[RotAxis, axis: [ -0, -0, -1], origin[ 0, 0, 0]],
tip:
[[ 1, 0, 0;
0, 1, 0;
0, 0, 1]
[ 0, 0, 0]]]
=>the name is again shifted, this is the actual segment 8

Segment 2:[Joint 6:[RotAxis, axis: [ -1, -0, -0], origin[ -0.14, -0.15, -0.16]],
tip:
[[ 1, 0, 0;
0, 1, 0;
0, 0, 1]
[ -0.14, -0.15, -0.16]]]
=> the name is again shifted, this is the actual sefment 6, segment 2 is not even part of the chain we asked for

Segment 5:[Joint 5:[RotX, axis: [ 1, 0, 0], origin[ 0, 0, 0]],
tip:
[[ 1, 0, 0;
0, 1, 0;
0, 0, 1]
[ 0.11, 0.12, 0.13]]]
Segment 14:[Joint 14:[RotZ, axis: [ 0, 0, 1], origin[ 0, 0, 0]],
tip:
[[ 1, 0, 0;
0, 1, 0;
0, 0, 1]
[ 0.2, 0.21, 0.22]]]
]

=>here you get the first segment also back (similar to case when going trough root)

conclusion:
*non consistent return chain (with or without given start segment)
*non consistent names of segments within the returned chain

nick