// Print out details of Molecule, taking the form:
// ========
// MOLECULE
// ========
// No. of e- = nel, charge = q, singlet/doublet/triplet etc.
// Enuc = nuclear energy
// (if inertia = true then also prints the section:
// ............................
// Principal Moments of Inertia
// ............................
// Ia = ..., Ib = ..., Ic = ...
// Rotational type: ...
// ....................
// Rotational Constants
// ....................
// (the coordinate system is also transformed to inertial coords)
// =====
// ATOMS
// =====
// Atom z Mass (a.m.u) Coordinates
// ...............................................................
// C 6 12.014 (0.0, 3.53, -1.24)
// etc...
void Logger::print(Molecule& mol, bool inertia) const
{
title("Molecule");
// Print out basic details
outfile << "# electrons = " << mol.getNel() << ", ";
outfile << "charge = " << mol.getCharge() << ", ";
std::string temp;
// Get the state type
switch (mol.getMultiplicity()) {
case 1: { temp = "Singlet"; break; }
case 2: { temp = "Doublet"; break; }
case 3: { temp = "Triplet"; break; }
case 4: { temp = "Quartet"; break; }
case 5: { temp = "Quintet"; break; }
default: { temp = "Spintacular"; break; } // Are hextets even a thing?!
}
outfile << temp << "\n";
outfile << "ENUC = " << mol.getEnuc() << " Hartree\n";
// Print inertial details if needed, and rotate
// into the inertial coordinate system
if (inertia) {
// Get it
temp = mol.rType();
Vector rconsts(3);
rconsts = mol.rConsts(1); // MHz
Vector inert(3);
inert = mol.getInertia(true);
// Print it out
outfile << std::string(30, '.') << "\n";
outfile << "Principal Moments of Inertia\n";
outfile << std::string(30, '.') << "\n";
outfile << "Ia = " << std::setw(12) << inert(0);
outfile << ", Ib = " << std::setw(12) << inert(1);
outfile << ", Ic = " << std::setw(12) << inert(2) << "\n";
outfile << "Rotational type: " << temp << "\n";
outfile << std::string(29, '.') << "\n";
outfile << "Rotational Constants / GHz\n";
outfile << std::string(29, '.') << "\n";
outfile << "A = " << std::setw(12) << rconsts(0);
outfile << ", B = " << std::setw(12) << rconsts(1);
outfile << ", C = " << std::setw(12) << rconsts(2) << "\n";
}
// Finally, print out all the atoms
title("Atoms");
outfile << std::setw(10) << "Atom" << std::setw(10) << "z";
outfile << std::setw(10) << "Mass" << std::setw(10) << "#CGBFs";
outfile << std::setw(30) << "Coordinates" << "\n";
outfile << std::string(70, '.') << "\n";
for (int i = 0; i < mol.getNAtoms(); i++){
print(mol.getAtom(i));
}
}
// recursive function to walk through tree
bool addChildrenToTree(urdf::LinkPtr root, Tree& tree)
{
urdf::LinkVector children = root->child_links;
//std::cerr << "[INFO] Link " << root->name << " had " << children.size() << " children" << std::endl;
// constructs the optional inertia
RigidBodyInertia inert(0);
if (root->inertial)
inert = toKdl(root->inertial);
// constructs the kdl joint
Joint jnt = toKdl(root->parent_joint);
// construct the kdl segment
Segment sgm(root->name, jnt, toKdl(root->parent_joint->parent_to_joint_origin_transform), inert);
// add segment to tree
tree.addSegment(sgm, root->parent_joint->parent_link_name);
// recurslively add all children
for (size_t i=0; i<children.size(); i++){
if (!addChildrenToTree(children[i], tree))
return false;
}
return true;
}
// recursive function to walk through tree
bool addChildrenToTree(boost::shared_ptr<const urdf::Link> root, Tree& tree)
{
std::vector<boost::shared_ptr<urdf::Link> > children = root->child_links;
std::cout<<"Link "<<root->name.c_str()<<" had "<<(int)children.size()<<" children."<<std::endl;
// ROS_DEBUG("Link %s had %i children", root->name.c_str(), (int)children.size());
// constructs the optional inertia
RigidBodyInertia inert(0);
if (root->inertial)
inert = toKdl(root->inertial);
// constructs the kdl joint
Joint jnt = toKdl(root->parent_joint);
// construct the kdl segment
Segment sgm(root->name, jnt, toKdl(root->parent_joint->parent_to_joint_origin_transform), inert);
// add segment to tree
tree.addSegment(sgm, root->parent_joint->parent_link_name);
// recurslively add all children
for (size_t i=0; i<children.size(); i++){
if (!addChildrenToTree(children[i], tree))
return false;
}
return true;
}
bool treeFromUrdfModel(const urdf::ModelInterface& robot_model, Tree& tree, const bool consider_root_link_inertia)
{
if (consider_root_link_inertia) {
//For giving a name to the root of KDL using the robot name,
//as it is not used elsewhere in the KDL tree
std::string fake_root_name = "__kdl_import__" + robot_model.getName()+"__fake_root__";
std::string fake_root_fixed_joint_name = "__kdl_import__" + robot_model.getName()+"__fake_root_fixed_joint__";
tree = Tree(fake_root_name);
const urdf::ConstLinkPtr root = robot_model.getRoot();
// constructs the optional inertia
RigidBodyInertia inert(0);
if (root->inertial)
inert = toKdl(root->inertial);
// constructs the kdl joint
Joint jnt = Joint(fake_root_fixed_joint_name, Joint::None);
// construct the kdl segment
Segment sgm(root->name, jnt, Frame::Identity(), inert);
// add segment to tree
tree.addSegment(sgm, fake_root_name);
} else {
tree = Tree(robot_model.getRoot()->name);
// warn if root link has inertia. KDL does not support this
if (robot_model.getRoot()->inertial)
std::cerr << "The root link " << robot_model.getRoot()->name <<
" has an inertia specified in the URDF, but KDL does not support a root link with an inertia. As a workaround, you can add an extra dummy link to your URDF." << std::endl;
}
// add all children
for (size_t i=0; i<robot_model.getRoot()->child_links.size(); i++)
if (!addChildrenToTree(robot_model.getRoot()->child_links[i], tree))
return false;
return true;
}
