int getWorldFrameLoop(const UndirectedTree & undirected_tree,
const KDL::CoDyCo::GeneralizedJntPositions &q,
const Traversal & traversal,
const int distal_link_index,
Frame & frame_world_link)
{
LinkMap::const_iterator distal_it = undirected_tree.getLink(distal_link_index);
LinkMap::const_iterator proximal_it = traversal.getBaseLink();
Frame currentFrame;
Frame resultFrame = Frame::Identity();
for(LinkMap::const_iterator link=distal_it; link != proximal_it; link = traversal.getParentLink(link) ) {
LinkMap::const_iterator parent_link = traversal.getParentLink(link);
assert( parent_link != undirected_tree.getInvalidLinkIterator() );
double joint_position;
if( link->getAdjacentJoint(parent_link)->getJoint().getType() != Joint::None ) {
joint_position = q.jnt_pos((link->getAdjacentJoint(parent_link))->getDOFIndex());
} else {
joint_position =0;
}
currentFrame = link->pose(parent_link,
joint_position);
resultFrame = currentFrame*resultFrame;
}
frame_world_link = q.base_pos*resultFrame;
return 0;
}
void NistXmlDocument::annotateSentence(uint sentno, const std::string &annot) {
typedef Arabica::DOM::Traversal::DocumentTraversal<std::string> Traversal;
typedef Arabica::DOM::Comment<std::string> Comment;
Traversal dt = outnode_.getOwnerDocument().createDocumentTraversal();
SegNodeFilter filter;
Traversal::TreeWalkerT it = dt.createTreeWalker(outnode_,
static_cast<unsigned long>(Arabica::DOM::Traversal::SHOW_TEXT),
filter, true);
for(uint i = 0; i < sentno; i++)
assert(it.nextNode() != 0);
Traversal::NodeT n = it.nextNode(); // the filter finds the next node inside the <seg> element
assert(n != 0);
n = n.getParentNode(); // get the <seg>
Comment comm = n.getOwnerDocument().createComment(" SEG " + annot + " ");
Traversal::NodeT p = n.getPreviousSibling();
Traversal::NodeT txt;
if(p != 0 && p.getNodeType() == Arabica::DOM::Node<std::string>::TEXT_NODE) {
txt = p;
p = p.getPreviousSibling();
}
if(p != 0 && p.getNodeType() == Arabica::DOM::Node<std::string>::COMMENT_NODE &&
boost::starts_with(p.getNodeValue(), " SEG "))
p.getParentNode().replaceChild(comm, p);
else {
n.getParentNode().insertBefore(comm, n);
if(txt != 0)
n.getParentNode().insertBefore(txt.cloneNode(false), n);
}
}
int getFramesLoop(const UndirectedTree & undirected_tree,
const KDL::JntArray &q,
const Traversal & traversal,
std::vector<Frame> & X_base,
KDL::Frame world2base)
{
for(int i=0; i < (int)traversal.getNrOfVisitedLinks(); i++) {
double joint_pos;
LinkMap::const_iterator link_it = traversal.getOrderedLink(i);
int link_nmbr = link_it->getLinkIndex();
if( i == 0 ) {
assert( traversal.getParentLink(link_nmbr) == undirected_tree.getInvalidLinkIterator() );
X_base[link_nmbr] = world2base;
} else {
LinkMap::const_iterator parent_it = traversal.getParentLink(link_it);
int parent_nmbr = parent_it->getLinkIndex();
if( link_it->getAdjacentJoint(parent_it)->getJoint().getType() != Joint::None ) {
int dof_nr = link_it->getAdjacentJoint(parent_it)->getDOFIndex();
joint_pos = q(dof_nr);
} else {
joint_pos = 0.0;
}
KDL::Frame X_parent_son = link_it->pose(parent_it,joint_pos);
X_base[link_nmbr] = X_base[parent_nmbr]*X_parent_son;
}
}
return 0;
}
int
main ( int argc, char* argv[] )
{
// Build the abstract syntax tree
SgProject* project = frontend(argc,argv);
ROSE_ASSERT (project != NULL);
// Build the inherited attribute
InheritedAttribute inheritedAttribute;
// Define the traversal
Traversal myTraversal;
// Call the traversal starting at the project (root) node of the AST
myTraversal.traverseInputFiles(project,inheritedAttribute);
// Demonstrate the the transformation will pass the AST tests.
AstTests::runAllTests (project);
// Output an optional graph of the AST (just the tree, when active)
generateDOT ( *project );
// Output an optional graph of the AST (the whole graph, of bounded complexity, when active)
const int MAX_NUMBER_OF_IR_NODES_TO_GRAPH_FOR_WHOLE_GRAPH = 10000;
generateAstGraph(project,MAX_NUMBER_OF_IR_NODES_TO_GRAPH_FOR_WHOLE_GRAPH,"");
return backend (project); // only backend error code is reported
}
void checkComputeTraversal(const Model & model)
{
Traversal traversal;
bool ok = model.computeFullTreeTraversal(traversal);
ASSERT_EQUAL_DOUBLE(ok,true);
ASSERT_EQUAL_DOUBLE(traversal.getNrOfVisitedLinks(),model.getNrOfLinks());
}
bool Model::computeFullTreeTraversal(Traversal & traversal, const LinkIndex traversalBase) const
{
if( traversalBase < 0 || traversalBase >= (LinkIndex)this->getNrOfLinks() )
{
reportError("Model","computeFullTreeTraversal","requested traversalBase is out of bounds");
return false;
}
// Resetting the traversal for populating it
traversal.reset(*this);
// A link is considered visit when all its child (given the traversalBase)
// have been added to the traversal
std::deque<stackEl> linkToVisit;
// We add as first link the requested traversalBase
addBaseLinkToTraversal(*this,traversal,traversalBase,linkToVisit);
// while there is some link still to visit
while( linkToVisit.size() > 0 )
{
assert(linkToVisit.size() <= this->getNrOfLinks());
// DPS : we use linkToVisit as a stack
LinkConstPtr visitedLink = linkToVisit.back().link;
LinkConstPtr visitedLinkParent = linkToVisit.back().parent;
LinkIndex visitedLinkIndex = visitedLink->getIndex();
linkToVisit.pop_back();
for(unsigned int neigh_i=0; neigh_i < this->getNrOfNeighbors(visitedLinkIndex); neigh_i++ )
{
// add to the stack all the neighbors, except for parent link
// (if the visited link is the base one, add all the neighbors)
// the visited link is already in the Traversal, so we can use it
// to check for its parent
Neighbor neighb = this->getNeighbor(visitedLinkIndex,neigh_i);
if( visitedLinkParent == 0 || neighb.neighborLink != visitedLinkParent->getIndex() )
{
addLinkToTraversal(*this,traversal,neighb.neighborLink,
neighb.neighborJoint,visitedLink->getIndex(),linkToVisit);
}
}
}
// At this point the traversal should contain all the links
// of the model
assert(traversal.getNrOfVisitedLinks() == this->getNrOfLinks());
return true;
}
void getFloatingBaseJacobianLoop(const UndirectedTree & undirected_tree,
const GeneralizedJntPositions &q,
const Traversal & traversal,
const int link_index,
Jacobian & jac)
{
Frame T_total = Frame::Identity(); //The transformation between link_index frame and current_link frame
assert(link_index < (int)undirected_tree.getNrOfLinks());
KDL::CoDyCo::LinkMap::const_iterator current_link;
current_link = undirected_tree.getLink(link_index);
//All the columns not modified are zero
SetToZero(jac);
KDL::CoDyCo::LinkMap::const_iterator parent_link=traversal.getParentLink(current_link);
while(current_link != traversal.getBaseLink()) {
double joint_pos = 0.0;
if( current_link->getAdjacentJoint(parent_link)->getNrOfDOFs() == 1 ) {
KDL::Twist jac_col;
int dof_index = current_link->getAdjacentJoint(parent_link)->getDOFIndex();
joint_pos = q.jnt_pos(dof_index);
KDL::Twist S_current_parent = parent_link->S(current_link,joint_pos);
jac_col = T_total*S_current_parent;
assert(6+dof_index < (int)jac.columns());
assert( dof_index < (int)undirected_tree.getNrOfDOFs() );
jac.setColumn(6+dof_index,jac_col);
}
KDL::Frame X_current_parent = parent_link->pose(current_link,joint_pos);
T_total = T_total*X_current_parent;
current_link = parent_link;
parent_link = traversal.getParentLink(current_link);
}
//Setting the floating part of the Jacobian
T_total = T_total*KDL::Frame(q.base_pos.M.Inverse());
jac.data.block(0,0,6,6) = TwistTransformationMatrix(T_total);
jac.changeBase(T_total.M.Inverse());
}
// size_t getLocalScopeNum ( SgFunctionDefinition* func_def, const SgScopeStatement* target)
size_t
getLocalScopeNum (const SgFunctionDefinition* func_def, const SgScopeStatement* target)
{
#if SKIP_BLOCK_NUMBER_CACHING
// DQ (10/4/2006): This takes too long and stalls the compilation of
// some large codes (plum hall e.g. cvs06a/conform/ch7_22.c). It is
// rewritten below to use a cache mechanisk link to a cache invalidation
// mechanism.
// Preorder traversal to count the number of basic blocks preceeding 'target'
class Traversal : public AstSimpleProcessing
{
public:
Traversal (const SgScopeStatement* target)
: target_ (target), count_ (0), found_ (false)
{}
void visit (SgNode* node)
{
if (!found_)
{
const SgScopeStatement* stmt = isSgScopeStatement (node);
if (stmt)
{
count_++;
found_ = (stmt == target_);
}
}
}
size_t count (void) const { return found_ ? count_ : 0; }
private:
const SgScopeStatement* target_; // Target scope statement
size_t count_; // found_ ? number of target : number of last block seen
bool found_; // true <==> target_ has been found
};
Traversal counter (target);
counter.traverse (const_cast<SgFunctionDefinition *> (func_def), preorder);
return counter.count ();
#else
// DQ (10/6/2006): Implemented caching of computed lables for scopes in
// functions to avoid quadratic behavior of previous implementation. The model
// for this is the same a s what will be done to support caching of mangled names.
// printf ("getLocalScopeNum calling func_def->get_scope_number(target)! \n");
return func_def->get_scope_number(target);
#endif
}
int
main ( int argc, char* argv[] )
{
// Build the abstract syntax tree
SgProject* project = frontend(argc,argv);
ROSE_ASSERT (project != NULL);
// Build the inherited attribute
InheritedAttribute inheritedAttribute = false;
// Define the traversal
Traversal myTraversal;
// Call the traversal starting at the project (root) node of the AST
myTraversal.traverseInputFiles(project,inheritedAttribute);
// This program only does analysis, so it need not call the backend to generate code.
return 0;
}
int
main ( int argc, char* argv[] )
{
// Initialize and check compatibility. See rose::initialize
ROSE_INITIALIZE;
SgProject* project = frontend(argc,argv);
ROSE_ASSERT (project != NULL);
// Build the inherited attribute
InheritedAttribute inheritedAttribute;
Traversal myTraversal;
// Call the traversal starting at the sageProject node of the AST
myTraversal.traverseInputFiles(project,inheritedAttribute);
return 0;
}
void addBaseLinkToTraversal(const Model & model, Traversal & traversal,
LinkIndex linkToAdd, std::deque<stackEl> & linkToVisit)
{
traversal.addTraversalBase(model.getLink(linkToAdd));
stackEl el;
el.link = model.getLink(linkToAdd);
el.parent = 0;
linkToVisit.push_back(el);
}
int main(int argc, char *argv[])
{
int arr1[] = { 1, 2, 4, 5, 3, 6 };
int len1 = sizeof(arr1) / sizeof(arr1[0]);
vector<int> pre(arr1, arr1+len1);
int arr2[] = { 4, 2, 5, 1, 6, 3 };
int len2 = sizeof(arr2) / sizeof(arr2[0]);
vector<int> in(arr2, arr2+len2);
Solution s;
print(pre);
print(in);
TreeNode *tree = s.buildTree(pre, in);
cout << tree->val << endl;
cout << tree->right << endl;
Traversal tra;
print(tra.preorder(tree));
print(tra.inorder(tree));
return 0;
}
void check(Traversal &t) {
Ans got(t.event(), t.vertex(), t.edge());
if (!isGood_ || current_>=ans_.size() || got!=ans_[current_]) {
if (isGood_) {
for (size_t i=0; i<current_; ++i) {
std::cout <<" correct: " <<std::setw(3) <<i <<toString(ans_[i]) <<"\n";
}
}
if (current_ >= ans_.size()) {
std::cout <<" FAILED: past end of answer\n";
std::cout <<" got: " <<toString(got) <<"\n";
} else if (got!=ans_[current_]) {
std::cout <<" FAILED: " <<std::setw(3) <<current_ <<toString(ans_[current_]) <<"\n";
std::cout <<" got: " <<toString(got) <<"\n";
} else {
std::cout <<" correct: " <<std::setw(3) <<current_ <<toString(ans_[current_]) <<"\n";
}
isGood_ = false;
}
++current_;
}
int main ( int argc, char* argv[] ) {
SgProject* project = frontend(argc,argv);
ROSE_ASSERT (project != NULL);
// Call function to declare function to be called to recode use of all functions in the AST
beforeWrite.buildDeclaration(project);
beforeRead.buildDeclaration(project);
// Build the inherited attribute
InheritedAttribute inheritedAttribute;
Traversal myTraversal;
// Call the traversal starting at the sageProject node of the AST
myTraversal.traverseInputFiles(project,inheritedAttribute);
// Generate Code and compile it with backend (vendor) compiler to generate object code
// or executable (as specified on commandline using vendor compiler's command line).
// Returns error code form compilation using vendor's compiler.
return backend(project);
}
void testAST( SgProject* project )
{
class Traversal : public SgSimpleProcessing
{
public:
Traversal() {}
void visit ( SgNode* n )
{
SgLocatedNode* locatedNode = isSgLocatedNode(n);
if (locatedNode != NULL)
{
AttachedPreprocessingInfoType* comments = locatedNode->getAttachedPreprocessingInfo();
if (comments != NULL)
{
printf ("Found attached comments (at %p of type: %s): \n",locatedNode,locatedNode->sage_class_name());
AttachedPreprocessingInfoType::iterator i;
for (i = comments->begin(); i != comments->end(); i++)
{
ROSE_ASSERT ( (*i) != NULL );
printf (" Attached Comment (relativePosition=%s): %s\n",
((*i)->getRelativePosition() == PreprocessingInfo::before) ? "before" : "after",
(*i)->getString().c_str());
#if 1
// This does not appear to be a valid object when read in from an AST file.
printf ("Comment/Directive getNumberOfLines = %d getColumnNumberOfEndOfString = %d \n",(*i)->getNumberOfLines(),(*i)->getColumnNumberOfEndOfString());
#endif
#if 1
// This does not appear to be a valid object when read in from an AST file.
(*i)->get_file_info()->display("comment/directive location");
#endif
}
}
}
}
};
Traversal counter;
counter.traverse(project,preorder);
}
void getRelativeJacobianLoop(const UndirectedTree & undirected_tree,
const KDL::JntArray &q,
const Traversal & traversal,
const int link_index,
Jacobian & jac)
{
Frame T_total = Frame::Identity(); //The transformation between link_index frame and current_link frame
KDL::CoDyCo::LinkMap::const_iterator current_link;
current_link = undirected_tree.getLink(link_index);
//All the columns not modified are zero
SetToZero(jac);
KDL::CoDyCo::LinkMap::const_iterator parent_link=traversal.getParentLink(current_link);
while(current_link != traversal.getBaseLink()) {
double joint_pos = 0.0;
if( current_link->getAdjacentJoint(parent_link)->getNrOfDOFs() == 1 ) {
KDL::Twist jac_col;
int dof_index = current_link->getAdjacentJoint(parent_link)->getDOFIndex();
joint_pos = q(dof_index);
KDL::Twist S_current_parent = parent_link->S(current_link,joint_pos);
jac_col = T_total*S_current_parent;
jac.setColumn(dof_index,jac_col);
}
KDL::Frame X_current_parent = parent_link->pose(current_link,joint_pos);
T_total = T_total*X_current_parent;
current_link = parent_link;
parent_link = traversal.getParentLink(current_link);
}
}
void NistXmlDocument::setTranslation(const PlainTextDocument &doc) {
typedef Arabica::DOM::Traversal::DocumentTraversal<std::string> Traversal;
Traversal dt = outnode_.getOwnerDocument().createDocumentTraversal();
SegNodeFilter filter;
Traversal::TreeWalkerT it = dt.createTreeWalker(outnode_,
static_cast<unsigned long>(Arabica::DOM::Traversal::SHOW_TEXT),
filter, true);
uint i = 0;
for(;;) {
Traversal::NodeT n = it.nextNode();
if(n == 0)
break;
std::ostringstream os;
std::copy(doc.sentence_begin(i), doc.sentence_end(i), std::ostream_iterator<Word>(os, " "));
i++;
std::string str = os.str();
str.erase(str.end() - 1);
n.setNodeValue(str);
}
}
int main (int argc, char* argv[])
{
// We retrieve the traversal kind from the command line
TraversalKind traversalKind = getTraversalKind (argc, argv);
//! [snippet1_traversal]
const char* seqs[] =
{
"CGCTACAGCAGCTAGTTCATCATTGTTTATCAATGATAAAATATAATAAGCTAAAAGGAAACTATAAATA",
"CGCTACAGCAGCTAGTTCATCATTGTTTATCGATGATAAAATATAATAAGCTAAAAGGAAACTATAAATA"
// SNP HERE at pos 31 x
};
// We create a fake bank with a SNP
IBank* bank = new BankStrings (seqs, ARRAY_SIZE(seqs));
// We load the graph
Graph graph = Graph::create (bank, "-abundance-min 1 -kmer-size 15 -verbose 0");
// We create a Terminator object
BranchingTerminator terminator (graph);
// We create a Traversal instance according to the chosen traversal kind
Traversal* traversal = Traversal::create (traversalKind, graph, terminator);
LOCAL (traversal);
// We create a node from the start of the first sequence
Node node = graph.buildNode (seqs[0]);
Path path;
int len = traversal->traverse (node, DIR_OUTCOMING, path);
// We dump the length, the starting node and the path
cout << "length=" << len << " " << graph.toString (node) << path << endl;
//! [snippet1_traversal]
return EXIT_SUCCESS;
}
PlainTextDocument NistXmlDocument::asPlainTextDocument() const {
std::vector<std::vector<Word> > txt;
typedef Arabica::DOM::Traversal::DocumentTraversal<std::string> Traversal;
Traversal dt = topnode_.getOwnerDocument().createDocumentTraversal();
SegNodeFilter filter;
Traversal::TreeWalkerT it = dt.createTreeWalker(topnode_,
static_cast<unsigned long>(Arabica::DOM::Traversal::SHOW_TEXT),
filter, true);
for(;;) {
Traversal::NodeT n = it.nextNode();
if(n == 0)
break;
std::string seg = n.getNodeValue();
boost::trim(seg);
txt.push_back(std::vector<Word>());
boost::split(txt.back(), seg, boost::is_any_of(" "));
}
return PlainTextDocument(txt);
}
boost::shared_ptr<const MMAXDocument> NistXmlDocument::asMMAXDocument() const {
typedef Arabica::DOM::Traversal::DocumentTraversal<std::string> Traversal;
Traversal dt = topnode_.getOwnerDocument().createDocumentTraversal();
SegNodeFilter filter;
Traversal::TreeWalkerT it = dt.createTreeWalker(topnode_,
static_cast<unsigned long>(Arabica::DOM::Traversal::SHOW_TEXT),
filter, true);
boost::shared_ptr<MMAXDocument> mmax = boost::make_shared<MMAXDocument>();
for(;;) {
Traversal::NodeT n = it.nextNode();
if(n == 0)
break;
std::string seg = n.getNodeValue();
boost::trim(seg);
std::vector<Word> snt;
boost::split(snt, seg, boost::is_any_of(" "));
mmax->addSentence(snt.begin(), snt.end());
}
return mmax;
}
void addLinkToTraversal(const Model & model, Traversal & traversal,
LinkIndex linkToAdd, JointIndex parentJointToAdd, LinkIndex parentLinkToAdd,
std::deque<stackEl> & linkToVisit)
{
traversal.addTraversalElement(model.getLink(linkToAdd),
model.getJoint(parentJointToAdd),
model.getLink(parentLinkToAdd));
stackEl el;
el.link = model.getLink(linkToAdd);
el.parent = model.getLink(parentLinkToAdd);
linkToVisit.push_back(el);
}
bool FreeFloatingJacobianUsingLinkPos(const Model& model,
const Traversal& traversal,
const JointPosDoubleArray& jointPositions,
const LinkPositions& world_H_links,
const LinkIndex jacobianLinkIndex,
const Transform& jacobFrame_X_world,
const Transform& baseFrame_X_jacobBaseFrame,
MatrixDynSize& jacobian)
{
// We zero the jacobian
jacobian.zero();
// Compute base part
const Transform & world_H_base = world_H_links(traversal.getBaseLink()->getIndex());
toEigen(jacobian).block(0,0,6,6) = toEigen((jacobFrame_X_world*world_H_base*baseFrame_X_jacobBaseFrame).asAdjointTransform());
// Compute joint part
// We iterate from the link up in the traveral until we reach the base
LinkIndex visitedLinkIdx = jacobianLinkIndex;
while (visitedLinkIdx != traversal.getBaseLink()->getIndex())
{
LinkIndex parentLinkIdx = traversal.getParentLinkFromLinkIndex(visitedLinkIdx)->getIndex();
IJointConstPtr joint = traversal.getParentJointFromLinkIndex(visitedLinkIdx);
size_t dofOffset = joint->getDOFsOffset();
for(int i=0; i < joint->getNrOfDOFs(); i++)
{
toEigen(jacobian).block(0,6+dofOffset+i,6,1) =
toEigen(jacobFrame_X_world*(world_H_links(visitedLinkIdx)*joint->getMotionSubspaceVector(i,visitedLinkIdx,parentLinkIdx)));
}
visitedLinkIdx = parentLinkIdx;
}
return true;
}
void dynamicsRegressorFixedBaseLoop(const UndirectedTree & ,
const KDL::JntArray &q,
const Traversal & traversal,
const std::vector<Frame>& X_b,
const std::vector<Twist>& v,
const std::vector<Twist>& a,
Eigen::MatrixXd & dynamics_regressor)
{
dynamics_regressor.setZero();
Eigen::Matrix<double, 6, 10> netWrenchRegressor_i;
for(int l =(int)traversal.getNrOfVisitedLinks()-1; l >= 0; l-- ) {
LinkMap::const_iterator link = traversal.getOrderedLink(l);
int i = link->getLinkIndex();
//Each link affects the dynamics of the joints from itself to the base
netWrenchRegressor_i = netWrenchRegressor(v[i],a[i]);
//dynamics_regressor.block(0,(int)(10*i),6,10) = WrenchTransformationMatrix(X_b[i])*netWrenchRegressor_i;
LinkMap::const_iterator child_link = link;
LinkMap::const_iterator parent_link=traversal.getParentLink(link);
while( child_link != traversal.getOrderedLink(0) ) {
if( child_link->getAdjacentJoint(parent_link)->getNrOfDOFs() == 1 ) {
int dof_index = child_link->getAdjacentJoint(parent_link)->getDOFIndex();
int child_index = child_link->getLinkIndex();
Frame X_j_i = X_b[child_index].Inverse()*X_b[i];
dynamics_regressor.block(dof_index,10*i,1,10) =
toEigen(parent_link->S(child_link,q(dof_index))).transpose()*WrenchTransformationMatrix(X_j_i)*netWrenchRegressor_i;
}
child_link = parent_link;
parent_link = traversal.getParentLink(child_link);
}
}
}
void dynamicsRegressorLoop(const UndirectedTree & ,
const KDL::JntArray &q,
const Traversal & traversal,
const std::vector<Frame>& X_b,
const std::vector<Twist>& v,
const std::vector<Twist>& a,
Eigen::MatrixXd & dynamics_regressor)
{
dynamics_regressor.setZero();
Eigen::Matrix<double, 6, 10> netWrenchRegressor_i;
// Store the base_world translational transform in world orientation
KDL::Frame world_base_X_world_world = KDL::Frame(-(X_b[traversal.getBaseLink()->getLinkIndex()].p));
for(int l =(int)traversal.getNrOfVisitedLinks()-1; l >= 0; l-- ) {
LinkMap::const_iterator link = traversal.getOrderedLink(l);
int i = link->getLinkIndex();
//Each link affects the dynamics of the joints from itself to the base
netWrenchRegressor_i = netWrenchRegressor(v[i],a[i]);
//Base dynamics
// The base dynamics is expressed with the orientation of the world but
// with respect to the base origin
dynamics_regressor.block(0,(int)(10*i),6,10) = WrenchTransformationMatrix(world_base_X_world_world*X_b[i])*netWrenchRegressor_i;
//dynamics_regressor.block(0,(int)(10*i),6,10) = WrenchTransformationMatrix(X_b[i])*netWrenchRegressor_i;
LinkMap::const_iterator child_link = link;
LinkMap::const_iterator parent_link=traversal.getParentLink(link);
while( child_link != traversal.getOrderedLink(0) ) {
if( child_link->getAdjacentJoint(parent_link)->getNrOfDOFs() == 1 ) {
#ifndef NDEBUG
//std::cerr << "Calculating regressor columns for link " << link->getName() << " and joint " << child_link->getAdjacentJoint(parent_link)->getName() << std::endl;
#endif
int dof_index = child_link->getAdjacentJoint(parent_link)->getDOFIndex();
int child_index = child_link->getLinkIndex();
Frame X_j_i = X_b[child_index].Inverse()*X_b[i];
dynamics_regressor.block(6+dof_index,10*i,1,10) =
toEigen(parent_link->S(child_link,q(dof_index))).transpose()*WrenchTransformationMatrix(X_j_i)*netWrenchRegressor_i;
}
child_link = parent_link;
#ifndef NDEBUG
//std::cout << "Getting parent link of link of index " << child_link->getName() << " " << child_link->getLinkIndex() << std::endl;
//std::cout << "Current base " << traversal.order[0]->getName() << " " << traversal.order[0]->getLinkIndex() << std::endl;
#endif
parent_link = traversal.getParentLink(child_link);
}
}
}
/*
* Update the floodfill map based on the current recorded maze
* Input: Traversal - trav
* Return: None
*/
void Navigation::updateFloodfill(Traversal trav)
{
bool foundFlag;
int temp;
for(int i=0; i<CELL_MAX-1; i++)
{
foundFlag = false;
for(int j=0; j<CELL_MAX; j++) // y-coordinates
{
for( int k=0; k<CELL_MAX; k++) // x-coordinates
{
if(floodfill[k][j]==i)
{
foundFlag = true;
temp = trav.getTraversalVal(k, j);
// Check North
if(j > 0)
if((floodfill[j-1][k]==CELL_MAX)&&((temp&0x01) != 0x01))
floodfill[j-1][k] = floodfill[j][k]+1;
// Check East
if(k < BOARD_MAX-1)
if((floodfill[j][k+1]==CELL_MAX) && ((temp&0x02) != 0x02))
floodfill[j][k+1] = floodfill[j][k]+1;
// Check South
if(j < BOARD_MAX-1)
if((floodfill[j+1][k]==CELL_MAX) && ((temp&0x04) != 0x04))
floodfill[j+1][k] = floodfill[j][k]+1;
// Check West
if(k > 0)
if((floodfill[j][k-1]==CELL_MAX)&&((temp&0x08) != 0x08))
floodfill[j][k-1] = floodfill[j][k]+1;
}
}
}
if(!foundFlag){ break; }
}
}
int crba_momentum_jacobian_loop(const UndirectedTree & undirected_tree,
const Traversal & traversal,
const JntArray & q,
std::vector<RigidBodyInertia> & Ic,
MomentumJacobian & H,
RigidBodyInertia & InertiaCOM
)
{
#ifndef NDEBUG
if( undirected_tree.getNrOfLinks() != traversal.getNrOfVisitedLinks() ||
undirected_tree.getNrOfDOFs() != q.rows() ||
Ic.size() != undirected_tree.getNrOfLinks() ||
H.columns() != (undirected_tree.getNrOfDOFs() + 6) )
{
std::cerr << "crba_momentum_jacobian_loop: input data error" << std::endl;
return -1;
}
#endif
double q_;
Wrench F = Wrench::Zero();
//Sweep from root to leaf
for(int i=0; i<(int)traversal.getNrOfVisitedLinks(); i++)
{
LinkMap::const_iterator link_it = traversal.getOrderedLink(i);
int link_index = link_it->getLinkIndex();
//Collect RigidBodyInertia
Ic[link_index]=link_it->getInertia();
}
for(int i=(int)traversal.getNrOfVisitedLinks()-1; i >= 1; i-- ) {
int dof_id;
LinkMap::const_iterator link_it = traversal.getOrderedLink(i);
int link_index = link_it->getLinkIndex();
LinkMap::const_iterator parent_it = traversal.getParentLink(link_index);
int parent_index = parent_it->getLinkIndex();
if( link_it->getAdjacentJoint(parent_it)->getNrOfDOFs() == 1 ) {
dof_id = link_it->getAdjacentJoint(parent_it)->getDOFIndex();
q_ = q(dof_id);
} else {
q_ = 0.0;
dof_id = -1;
}
Ic[parent_index] = Ic[parent_index]+link_it->pose(parent_it,q_)*Ic[link_index];
if( link_it->getAdjacentJoint(parent_it)->getNrOfDOFs() == 1 ) {
KDL::Twist S_link_parent = parent_it->S(link_it,q_);
F = Ic[link_index]*S_link_parent;
if( traversal.getParentLink(link_it) != undirected_tree.getInvalidLinkIterator() ) {
double q__;
int dof_id_;
LinkMap::const_iterator predecessor_it = traversal.getParentLink(link_it);
LinkMap::const_iterator successor_it = link_it;
while(true) {
if( predecessor_it->getAdjacentJoint(successor_it)->getNrOfDOFs() == 1 ) {
q__ = q( predecessor_it->getAdjacentJoint(successor_it)->getDOFIndex());
} else {
q__ = 0.0;
}
F = successor_it->pose(predecessor_it,q__)*F;
successor_it = predecessor_it;
predecessor_it = traversal.getParentLink(predecessor_it);
if( predecessor_it == undirected_tree.getInvalidLinkIterator() ) {
break;
}
if( predecessor_it->getAdjacentJoint(successor_it)->getNrOfDOFs() == 1 ) {
dof_id_ = predecessor_it->getAdjacentJoint(successor_it)->getDOFIndex();
q__ = q(dof_id_);
} else {
q__ = 0.0;
dof_id_ = -1;
}
}
if( dof_id >= 0 ) {
H.data.block(0,6+dof_id,6,1) = toEigen(F);
}
//The first 6x6 submatrix of the Momentum Jacobian are simply the spatial inertia
//of all the structure expressed in the base reference frame
H.data.block(0,0,6,6) = toEigen(Ic[traversal.getBaseLink()->getLinkIndex()]);
}
}
}
//We have then to translate the reference point of the obtained jacobian to the com
//The Ic[traversal.order[0]->getLink(index)] contain the spatial inertial of all the tree
//expressed in link coordite frames
Vector com = Ic[traversal.getBaseLink()->getLinkIndex()].getCOG();
H.changeRefPoint(com);
InertiaCOM = Frame(com)*Ic[traversal.getBaseLink()->getLinkIndex()];
return 0;
}
int crba_fixed_base_loop(const UndirectedTree & undirected_tree, const Traversal & traversal, const JntArray & q, std::vector<RigidBodyInertia> & Ic, JntSpaceInertiaMatrix & H) {
double q_;
Wrench F;
//Sweep from root to leaf
for(int i=0; i<(int)traversal.getNrOfVisitedLinks(); i++)
{
LinkMap::const_iterator link_it = traversal.getOrderedLink(i);
int link_index = link_it->getLinkIndex();
//Collect RigidBodyInertia
Ic[link_index] = link_it->getInertia();
}
for(int i=(int)traversal.getNrOfVisitedLinks()-1; i >= 1; i-- ) {
int dof_id;
LinkMap::const_iterator link_it = traversal.getOrderedLink(i);
int link_index = link_it->getLinkIndex();
LinkMap::const_iterator parent_it = traversal.getParentLink(link_index);
int parent_index = parent_it->getLinkIndex();
if( link_it->getAdjacentJoint(parent_it)->getNrOfDOFs() == 1 ) {
dof_id = link_it->getAdjacentJoint(parent_it)->getDOFIndex();
q_ = q(dof_id);
} else {
q_ = 0.0;
dof_id = -1;
}
Ic[parent_index] = Ic[parent_index]+link_it->pose(parent_it,q_)*Ic[link_index];
if( link_it->getAdjacentJoint(parent_it)->getNrOfDOFs() == 1 ) {
KDL::Twist S_link_parent = parent_it->S(link_it,q_);
F = Ic[link_index]*S_link_parent;
H(dof_id,dof_id) = dot(S_link_parent,F);
{
assert(parent_it != undirected_tree.getInvalidLinkIterator());
double q__;
int dof_id_;
LinkMap::const_iterator predecessor_it = traversal.getParentLink(link_it);
LinkMap::const_iterator successor_it = link_it;
while( true ) {
if( predecessor_it->getAdjacentJoint(successor_it)->getNrOfDOFs() == 1 ) {
q__ = q( predecessor_it->getAdjacentJoint(successor_it)->getDOFIndex());
} else {
q__ = 0.0;
}
F = successor_it->pose(predecessor_it,q__)*F;
successor_it = predecessor_it;
predecessor_it = traversal.getParentLink(predecessor_it);
if( predecessor_it == undirected_tree.getInvalidLinkIterator() ) {
break;
}
if( predecessor_it->getAdjacentJoint(successor_it)->getNrOfDOFs() == 1 ) {
dof_id_ = predecessor_it->getAdjacentJoint(successor_it)->getDOFIndex();
q__ = q(dof_id_);
} else {
q__ = 0.0;
dof_id_ = -1;
}
Twist S_successor_predecessor = predecessor_it->S(successor_it,q__);
if( dof_id_ >= 0 ) {
H(dof_id_,dof_id) = dot(S_successor_predecessor,F);
H(dof_id,dof_id_) = H(dof_id_,dof_id);
}
}
}
}
}
return 0;
}
int crba_floating_base_loop(const UndirectedTree & undirected_tree,
const Traversal & traversal,
const GeneralizedJntPositions & q,
std::vector<RigidBodyInertia> & Ic,
FloatingJntSpaceInertiaMatrix & H) {
Wrench F = Wrench::Zero();
Wrench buffer_F = F;
//Sweep from root to leaf
for(int i=0; i<(int)traversal.getNrOfVisitedLinks(); i++)
{
LinkMap::const_iterator link_it = traversal.getOrderedLink(i);
int link_index = link_it->getLinkIndex();
//Collect RigidBodyInertia
Ic[link_index]=link_it->getInertia();
}
for(int i=(int)traversal.getNrOfVisitedLinks()-1; i >= 1; i-- ) {
double row_dof_position;
int row_dof_id;
LinkMap::const_iterator link_it = traversal.getOrderedLink(i);
int link_index = link_it->getLinkIndex();
LinkMap::const_iterator parent_it = traversal.getParentLink(link_index);
int parent_index = parent_it->getLinkIndex();
if( link_it->getAdjacentJoint(parent_it)->getNrOfDOFs() == 1 ) {
row_dof_id = link_it->getAdjacentJoint(parent_it)->getDOFIndex();
row_dof_position = q.jnt_pos(row_dof_id);
} else {
row_dof_position = 0.0;
row_dof_id = -1;
}
RigidBodyInertia buf;
buf = Ic[parent_index]+link_it->pose(parent_it,row_dof_position)*Ic[link_index];
Ic[parent_index] = buf;
if( link_it->getAdjacentJoint(parent_it)->getNrOfDOFs() == 1 ) {
KDL::Twist S_link_parent = parent_it->S(link_it,row_dof_position);
F = Ic[link_index]*S_link_parent;
H(6+row_dof_id,6+row_dof_id) = dot(S_link_parent,F);
if( traversal.getParentLink(link_it) != undirected_tree.getInvalidLinkIterator() ) {
double column_dof_position;
int column_dof_id;
LinkMap::const_iterator predecessor_it = traversal.getParentLink(link_it);
LinkMap::const_iterator successor_it = link_it;
while(true) {
if( predecessor_it->getAdjacentJoint(successor_it)->getNrOfDOFs() == 1 ) {
column_dof_position = q.jnt_pos( predecessor_it->getAdjacentJoint(successor_it)->getDOFIndex());
} else {
column_dof_position = 0.0;
}
#ifndef NDEBUG
//std::cout << "F migrated from frame " << successor_it->getLinkIndex() << " to frame " << successor_it->getLinkIndex() << std::endl;
#endif
buffer_F = successor_it->pose(predecessor_it,column_dof_position)*F;
F = buffer_F;
successor_it = predecessor_it;
predecessor_it = traversal.getParentLink(predecessor_it);
if( predecessor_it == undirected_tree.getInvalidLinkIterator() ) {
break;
}
if( predecessor_it->getAdjacentJoint(successor_it)->getNrOfDOFs() == 1 ) {
column_dof_id = predecessor_it->getAdjacentJoint(successor_it)->getDOFIndex();
column_dof_position = q.jnt_pos(column_dof_id);
} else {
column_dof_position = 0.0;
column_dof_id = -1;
}
Twist S_successor_predecessor = predecessor_it->S(successor_it,column_dof_position);
if( column_dof_id >= 0 ) {
H(6+column_dof_id,6+row_dof_id) = dot(S_successor_predecessor,F);
H(6+row_dof_id,6+column_dof_id) = H(6+column_dof_id,6+row_dof_id);
}
}
if( row_dof_id >= 0 ) {
buffer_F = q.base_pos.M*F;
F = buffer_F;
H.data.block(0,6+row_dof_id,6,1) = toEigen(F);
H.data.block(6+row_dof_id,0,1,6) = toEigen(F).transpose();
}
}
}
}
//The first 6x6 submatrix of the FlotingBase Inertia Matrix are simply the spatial inertia
//of all the structure expressed in the base reference frame
H.data.block(0,0,6,6) = toEigen(q.base_pos.M*Ic[traversal.getBaseLink()->getLinkIndex()]);
return 0;
}
