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