void RobotNode::collectAllRobotNodes( std::vector< RobotNodePtr > &storeNodes )
{
storeNodes.push_back(static_pointer_cast<RobotNode>(shared_from_this()));
std::vector< SceneObjectPtr > children = this->getChildren();
for (size_t i=0;i<children.size();i++)
{
RobotNodePtr n = dynamic_pointer_cast<RobotNode>(children[i]);
if (n)
n->collectAllRobotNodes(storeNodes);
}
}
