I'm interested in writing a tree ID solver based on the current chain ID

solver. The first issues I can think of is the lack of a q, q_dot, and

q_dotdot, and that the ChainIdSolver uses a JntArray, which won't work

for a tree. Given that these are only used for calculation of other

pieces of information, a map<std::string, Eigen::Vector3d> could work

but since they're only used in a few places, the time it takes to

retrieve them from the map may overshadow the time it takes to use them.

The next is the frames, twists, and wrenches of the tree, which can be

in a map<std::string, etc> and just copied locally and written back when

they're no longer needed.

Before I spend a decent amount of time on this, I'd like to get others'

feedback about how they would go about it.

Some root->leaf->root functions:

int TreeIdSolver_RNE::CartToJnt(const Tree &tree)

{

// Calculate things such as v, a, f, for the root segment

SegmentMap::const_iterator iter;

for (iter = tree.getRootSegment().children.begin(); iter !=

tree.getRootSegment().children.end(); iter++)

{

RecursiveSolver(iter);

}

// Calculate torque for root segment

}

int TreeIdSolver_RNE::RecursiveSolver(const

SegmentMap::const_iterator& current)

{

// Calculate things such as X, S, v, a, f, then continue

SegmentMap::const_iterator children;

for (children = current->children.begin(); children !=

current->children.end(); children++)

{

RecursiveSolver(children);

}

// Calculate torque for the segment

}

## Tree ID solver and Tree/TreeElement class extension

On Mon, 18 Feb 2013, Rohan Smith wrote:

> I'm interested in writing a tree ID solver based on the current chain ID

> solver.

You are not the only one :-)

> The first issues I can think of is the lack of a q, q_dot, and

> q_dotdot, and that the ChainIdSolver uses a JntArray, which won't work

> for a tree.

Indeed, trees require some "standardization" of how to serialize the joints

in the array, and the community has not yet come up with an widely accepted

suggestion...

> Given that these are only used for calculation of other

> pieces of information, a map<std::string, Eigen::Vector3d> could work

> but since they're only used in a few places, the time it takes to

> retrieve them from the map may overshadow the time it takes to use them.

I agree. I am in favour of 'serializing' at configuration time, and then

using the serialized version efficiently at runtime.

> The next is the frames, twists, and wrenches of the tree, which can be

> in a map<std::string, etc> and just copied locally and written back when

> they're no longer needed.

>

> Before I spend a decent amount of time on this, I'd like to get others'

> feedback about how they would go about it.

>

> Some root->leaf->root functions:

> int TreeIdSolver_RNE::CartToJnt(const Tree &tree)

> {

> // Calculate things such as v, a, f, for the root segment

> SegmentMap::const_iterator iter;

> for (iter = tree.getRootSegment().children.begin(); iter !=

> tree.getRootSegment().children.end(); iter++)

> {

> RecursiveSolver(iter);

> }

> // Calculate torque for root segment

> }

>

> int TreeIdSolver_RNE::RecursiveSolver(const

> SegmentMap::const_iterator& current)

> {

> // Calculate things such as X, S, v, a, f, then continue

> SegmentMap::const_iterator children;

> for (children = current->children.begin(); children !=

> current->children.end(); children++)

> {

> RecursiveSolver(children);

> }

> // Calculate torque for the segment

> }

Azamat is working on similar "tree sweeps" iterators, for, among other

things, efficient dynamics. I think he will be able to release a draft in a

couple of weeks.

The approach we are following is _not_ to make a "class library API",

because that explodes under the number of variations that have to be

supported in robotics. (Think about the different strategies one can apply

for: joint limit avoidance, redundancy resolution, external force

application, posture control,...). Hence, we think about _modelling_ a

particular variation, and _generate_ the code for a _component_ that offers

the particular data flow interface.

> Cheers,

> Rohan Smith

Herman