int
EigenGrasp::readFromXml(const TiXmlElement *element)
{
bool ok;
QString valueStr;
std::list<const TiXmlElement*> EVList = findAllXmlElements(element, "EigenValue");
if(!countXmlElements(element, "EigenValue"))
{
DBGA("WARNING: EigenValue tag missing from file defaulting EigenValue to 0.5!");
mEigenValue=0.5;
}
else
{
valueStr=(*EVList.begin())->Attribute("value");
if(valueStr.isNull()){
QTWARNING("DOF Type not found");
return 0;
}
mEigenValue=valueStr.toDouble(&ok); if (!ok) {DBGA("ERROR: EigenValue entries should only contain numbers.");return 0;}
}
EVList = findAllXmlElements(element, "DimVals");
if(!countXmlElements(element, "DimVals")) {DBGA("DimVals tag missing from file.");return 0;}
for (int i=0; i<mSize; i++) {
valueStr=(*EVList.begin())->Attribute(QString("d") + QString::number(i));
if ( valueStr.isNull() || valueStr.isEmpty() ) mVals[i]=0.0;
else {
mVals[i] = valueStr.toDouble(&ok);
if (!ok) {DBGA("ERROR: DimVals entries should only contain numbers.");return 0;}
}
}
EVList = findAllXmlElements(element, "Limits");
if(countXmlElements(element, "Limits"))
{
mPredefinedLimits=true;
valueStr=(*EVList.begin())->Attribute("min");
mMin=valueStr.toDouble(&ok); if (!ok) {DBGA("ERROR: min entries should only contain numbers.");return 0;}
if ( false ) mPredefinedLimits=false;
valueStr=(*EVList.begin())->Attribute("max");
mMax=valueStr.toDouble(&ok); if (!ok) {DBGA("ERROR: max entries should only contain numbers.");return 0;}
if ( false ) mPredefinedLimits=false;
}
return 1;
}
/*! First calls the super to load the hand like a regular one (all
kinematic chains, links etc). Then loads tendon information from
the same file.
*/
int HumanHand::loadFromXml(const TiXmlElement* root,QString rootPath)
{
if ( Hand::loadFromXml(root, rootPath) != SUCCESS)
return FAILURE;
int i,j;
QString ivdir,tendonName;
QString line;
const TiXmlElement* element;
double rad;
int nrInsPoints,chain,link;
vec3 position,orientation;
//load tendons from xml node
nrTendons=countXmlElements(root,"tendon");
if (nrTendons<0)
{
DBGA("Incorrect number of tendons");
return FAILURE;
}
std::list<const TiXmlElement*> elementList = findAllXmlElements(root,"tendon");
std::list<const TiXmlElement*> elementList2;
tendonVec.resize(nrTendons, NULL);
for (i=0; i<nrTendons; i++) {
tendonVec[i]=new Tendon(this);
}
std::list<const TiXmlElement*>::iterator p, p2;
for (p = elementList.begin(), i=0; p != elementList.end(); p++,i++) {
tendonName = (*p)->Attribute("name");
if(tendonName.isNull()){
DBGA("Tendon Name undefined: tendon"<<i);
return FAILURE;
}
nrInsPoints=countXmlElements(*p,"insertionPoint");
if (nrInsPoints<2){
DBGA("Incorrect number of Ins Points on tendon"<<i);
return FAILURE;
}
delete tendonVec[i];
tendonVec[i]=new Tendon(this);
tendonVec[i]->setName(tendonName);
elementList2 = findAllXmlElements(*p,"insertionPoint");
for (p2 = elementList2.begin(), j=0; p2!=elementList2.end(); p2++,j++) {
if(!getInt(*p2,"chain",chain)){
DBGA("Failed to read chain on tendon"<<i<<"ins point"<<j);
delete tendonVec[i];
tendonVec[i] = new Tendon(this);
return FAILURE;
}
if(!getInt(*p2,"link",link)){
DBGA("Failed to read link on tendon"<<i<<"ins point"<<j);
delete tendonVec[i];
tendonVec[i] = new Tendon(this);
return FAILURE;
}
element = findXmlElement(*p2,"position");
if(!element){
DBGA("Failed to read position on tendon"<<i<<"ins point"<<j);
delete tendonVec[i];
tendonVec[i] = new Tendon(this);
return FAILURE;
}
if(!getPosition(element,position)){
DBGA("Failed to read position tendon"<<i<<"ins point"<<j);
delete tendonVec[i];
tendonVec[i] = new Tendon(this);
return FAILURE;
}
tendonVec[i]->addInsertionPoint(chain,link,position,true);
}
}
for (i=0; i<nrTendons; i++)
{
IVRoot->addChild( tendonVec[i]->getIVRoot() );
DBGA("Tendon "<< tendonVec[i]->getName().ascii()<<" read and added with "
<<tendonVec[i]->nrInsPoints<<" insertion points");
}
//load tendon wrapper data from file
nrTendonWrappers=countXmlElements(root,"tendonWrapper");
if (nrTendons<0)
{
DBGA("Incorrect number of tendon wrappers");
return FAILURE;
}
tendonWrapperVec.resize(nrTendonWrappers, NULL);
elementList = findAllXmlElements(root,"tendonWrapper");
for (i=0; i<nrTendonWrappers; i++)
tendonWrapperVec[i]=new TendonWrapper(this,-1,-1,vec3(0,0,0),vec3(1,0,0),0);
for (p = elementList.begin(), i=0; p != elementList.end(); p++,i++){
if(!getInt(*p,"chain",chain)){
DBGA("Failed to read chain on tendon wrapper"<<i);
return FAILURE;
}
if(!getInt(*p,"link",link)){
DBGA("Failed to read link on tendon wrapper"<<i);
return FAILURE;
}
element = findXmlElement(*p,"position");
if(!element){
DBGA("Failed to read position on tendon wrapper"<<i);
return FAILURE;
}
if(!getPosition(element,position)){
DBGA("Failed to read position tendon wrapper"<<i);
return FAILURE;
}
element = findXmlElement(*p,"orientation");
if(!element){
DBGA("Failed to read orientation on tendon wrapper"<<i);
return FAILURE;
}
if(!getPosition(element,orientation)){
DBGA("Failed to read orientation tendon wrapper"<<i);
return FAILURE;
}
if(!getDouble(*p,"radius",rad)){
DBGA("Failed to read radius on tendon wrapper"<<i);
return FAILURE;
}
delete tendonWrapperVec[i];
tendonWrapperVec[i]=new TendonWrapper(this,chain,link,position,orientation,rad);
}
for (i=0; i<nrTendonWrappers; i++)
{
DBGA("TendonWrapper "<<i<<" read and added");
tendonWrapperVec[i]->createGeometry();
IVRoot->addChild( tendonWrapperVec[i]->getIVRoot() );
}
for (i=0; i<nrTendons; i++)
{
tendonVec[i]->updateGeometry();
tendonVec[i]->setRestPosition();
}
return SUCCESS;
}
/*! Reads the entire interface from a file, looking for keywords
inside the file. Loads the size of the dof space, all eg's
(which define the size of the eg space), any normalization
factors (optional) and the origin of the eg space (optional).
The size of the dof space must be loaded before any eg's,
origin, etc can be loaded; is must also match the number of
dof's in this robot.
*/
int
EigenGraspInterface::readFromFile(QString filename)
{
//open xml file at "filename"
bool ok;
QString fileType = filename.section('.',-1,-1);
QString xmlFilename;
if (fileType == "xml"){
//the file itself is XML
xmlFilename = filename;
} else {
QTWARNING("Could not open " + xmlFilename);
DBGA("Old non-xml file format is no longer supported");
return 0;
}
//load the graspit specific information in XML format or fail
TiXmlDocument doc(xmlFilename);
if(doc.LoadFile()==false){
DBGA("Failed to open EG file: " << filename.latin1());
QTWARNING("Could not open " + xmlFilename);
return 0;
}
//get the dimensions
int numDims = 0;
QString valueStr;
clear();
TiXmlElement* root = doc.RootElement();
if(root == NULL){
DBGA("The "<<filename.toStdString()<<" file must contain a root tag named EigenGrasps.");
return 0;
} else{
valueStr = root->Attribute("dimensions");
numDims = valueStr.toDouble(&ok); if (!ok) {DBGA("ERROR: Dimension should contain a number.");return 0;}
if (numDims <= 0) {
DBGA("invalid number of dimensions in EigenGrasps tag in file: "<<filename.toStdString());
return 0;
}
}
//get the list of EG's
std::list<const TiXmlElement*> elementList = findAllXmlElements(root, "EG");
int numEG = countXmlElements(root, "EG");
if (numEG < 1) {
DBGA("Number of Eigengrasps specified: " << numEG);
return 0;
}
std::list<const TiXmlElement*>::iterator p = elementList.begin();
while(p!=elementList.end()){
EigenGrasp *newGrasp = new EigenGrasp(numDims);
if (!newGrasp->readFromXml(*p++)) return 0;
newGrasp->normalize();
mGrasps.push_back(newGrasp);
}
//get the orgin and process (if none setsimpleorgin)
elementList = findAllXmlElements(root, "ORIGIN");
int numORG = countXmlElements(root, "ORIGIN");
if (!numORG) {
DBGA("No EG origin found; using automatic origin");
mOrigin = new EigenGrasp(numDims);
setSimpleOrigin();
}
else if(numORG==1)
{
mOrigin = new EigenGrasp(numDims);
if (!mOrigin->readFromXml(*(elementList.begin()))){ return 0;}
checkOrigin();
}
else
{
DBGA("Multiple Origins specified in Eigen Grasp file.");
return 0;
}
//get the norm and process (if none set to all 1's)
elementList = findAllXmlElements(root, "NORM");
int numNORM = countXmlElements(root, "NORM");
if (!numNORM) {
DBGA("No normalization data found; using factors of 1.0");
mNorm = new EigenGrasp(numDims);
mNorm->setOnes();
}
else if(numNORM==1)
{
mNorm = new EigenGrasp(numDims);
if (!mNorm->readFromXml(*(elementList.begin()))){ return 0;}
DBGA("EG Normalization data loaded from file");
}
else
{
DBGA("Multiple Normals specified in Eigen Grasp file.");
return 0;
}
eSize = mGrasps.size();
DBGA("Read " << eSize << " eigengrasps from EG file");
computeProjectionMatrices();
setMinMax();
return 1;
}
/*!
Sets up the chain given a XML DOM from a currently open robot
configuration file. It reads the number of joints and the number of links
and allocates space for those vectors, then it reads in the base transform
of the chain. Next, it reads a node for each joint and creates a
prismatic or revolute joint which is initialized with the kinematic data
in that node. \a linkDir should be the path to the directory where the link
body files are kept (usually rootPath/iv).
*/
int
KinematicChain::initChainFromXml(const TiXmlElement* root,QString &linkDir)
{
numJoints = countXmlElements(root,"joint");
if (numJoints < 1) {
DBGA("number of joints < 1");
return FAILURE;
}
numLinks = countXmlElements(root,"link");
if (numLinks < 1) {
DBGA("Number of links < 1");
return FAILURE;
}
jointVec.resize(numJoints, NULL);
linkVec.resize(numLinks, NULL);
lastJoint = new int[numLinks];
IVRoot = new SoSeparator;
IVTran = new SoTransform;
IVTran->ref();
/* read in the finger transformation */
const TiXmlElement* element = findXmlElement(root,"transform");
if(element){
if(!getTransform(element,tran)){
QTWARNING("Fail to perform transformation");
return FAILURE;
}
}
tran.toSoTransform(IVTran);
DBGA(" Creating joints");
numDOF = 0;
std::list<const TiXmlElement*> elementList = findAllXmlElements(root, "joint");
std::list<const TiXmlElement*>::iterator p;
int j;
for(p = elementList.begin(), j=0; p!=elementList.end(); p++,j++){
DBGA(" Joint " << j);
QString type = (*p)->Attribute("type");
if(type.isNull()){
QTWARNING("No Joint Type Specified");
return FAILURE;
}
if(type == "Revolute"){
jointVec[j] = new RevoluteJoint(this);
} else if(type == "Prismatic"){
jointVec[j] = new PrismaticJoint (this);
} else {
DBGA("Unknown joint type requested");
return FAILURE;
}
if (jointVec[j]->initJointFromXml(*p, firstJointNum+j) == FAILURE) {
DBGA("Failed to initialize joint");
return FAILURE;
}
}
DBGA(" Creating links");
std::vector<int> dynJointTypes;
int lastJointNum = -1;
elementList = findAllXmlElements(root, "link");
int l;
for (l=0, p=elementList.begin(); p!=elementList.end(); p++,l++){
DBGA(" Link " << l);
QString jointType=(*p)->Attribute("dynamicJointType");
if(jointType.isNull()){
QTWARNING("No Dynamic Joint Type Specified");
return FAILURE;
}
jointType = jointType.stripWhiteSpace();
if (jointType == "Revolute") {
dynJointTypes.push_back(DynJoint::REVOLUTE);
lastJointNum += 1;
} else if (jointType == "Ball") {
dynJointTypes.push_back(DynJoint::BALL);
lastJointNum += 3;
} else if (jointType == "Universal") {
dynJointTypes.push_back(DynJoint::UNIVERSAL);
lastJointNum += 2;
} else if (jointType == "Prismatic") {
dynJointTypes.push_back(DynJoint::PRISMATIC);
lastJointNum += 1;
} else if (jointType == "Fixed") {
dynJointTypes.push_back(DynJoint::FIXED);
} else {
DBGA("Unknown joint type requested");
return FAILURE;
}
QString linkFilename = (*p)->GetText();
linkFilename = linkFilename.stripWhiteSpace();
QString linkName = QString(owner->name()) + QString("_chain%1_link%2").arg(chainNum).arg(l);
linkVec[l] = new Link(owner,chainNum,l,owner->getWorld(),linkName.latin1());
if (linkVec[l]->load(linkDir + linkFilename)==FAILURE) {
delete linkVec[l]; linkVec[l] = NULL;
DBGA("Failed to load file for link " << l);
return FAILURE;
}
/*Handle collision rule settings:
*Common case is NULL - collisions are on except for adjacent links.
*Off - turn off collisions for this link globally, do not enter this body
*into the collision engine at all.
*OverlappingPair - turn off collisions between two particular links
*/
QString collisionRule=(*p)->Attribute("collisionRule");
if(!collisionRule.isNull()){
collisionRule = collisionRule.stripWhiteSpace();
if (collisionRule == "Off"){
linkVec[l]->addToIvc(true);
linkVec[l]->myWorld->toggleCollisions(false, linkVec[l],NULL);
DBGA("Collisions off for link " << l);
}else if (collisionRule == "OverlappingPair"){
/*targetChain - specifies the chain of the target link to disable
*collisions for. No attribute implies current chain.
*Base implies robot base.
*targetLink - specifies link number in target chain
*/
linkVec[l]->addToIvc();
QString targetChain=(*p)->Attribute("targetChain");
targetChain = targetChain.stripWhiteSpace();
if (targetChain == "base")
linkVec[l]->myWorld->toggleCollisions(false, linkVec[l],owner->getBase());
else{
QString targetLink = (*p)->Attribute("targetLink");
if (!targetLink.isNull()){
bool ok = TRUE;
int linkNum = targetLink.toInt(&ok);
if (!ok){
DBGA("targetLink not a valid input");
return FAILURE;
}
if(targetChain.isNull())
linkVec[l]->myWorld->toggleCollisions(false, linkVec[l],linkVec[linkNum]);
else{
int chainNum = targetChain.toInt(&ok);
if (!ok){
DBGA("targetChain not a valid input");
return FAILURE;
}
linkVec[l]->myWorld->toggleCollisions(false, linkVec[l],owner->getChain(chainNum)->linkVec[linkNum]);
}
}
}
}
else{
DBGA("Unknown Collision Rule");
return FAILURE;
}
}else{
linkVec[l]->addToIvc();
}
lastJoint[l] = lastJointNum;
if (lastJoint[l] >= numJoints) {
DBGA("Wrong last joint value: " << lastJoint[l]);
return FAILURE;
}
IVRoot->addChild(linkVec[l]->getIVRoot());
}
DBGA(" Creating dynamic joints");
if (createDynamicJoints(dynJointTypes) == FAILURE) {
DBGA("Failed to create dynamic joints");
return FAILURE;
}
jointsMoved = true;
updateLinkPoses();
owner->getWorld()->tendonChange();
owner->getIVRoot()->addChild(IVRoot);
return SUCCESS;
}
