bool getPosition(const TiXmlElement *root, vec3 &pos) {
bool ok1, ok2, ok3;
if (root == NULL) {
QTWARNING("The given root is not a Position Element");
return false;
}
QString rootValue = root->Value();
rootValue = rootValue.stripWhiteSpace();
if (!(rootValue == "position" || rootValue == "orientation")) {
QTWARNING("The given root is not a Position Element");
return false;
}
QString valueStr = root->GetText();
valueStr = valueStr.simplifyWhiteSpace().stripWhiteSpace();
QStringList l;
l = QStringList::split(' ', valueStr);
if (l.count() != 3) {
QTWARNING("Invalid position input");
return false;
}
double x, y, z;
x = l[0].toDouble(&ok1);
y = l[1].toDouble(&ok2);
z = l[2].toDouble(&ok3);
if (!ok1 || !ok2 || !ok3) {
QTWARNING("Invalid position input");
return false;
}
pos.set(x, y, z);
return true;
}
/*!
Reads the planning and testing parameters from the text entry boxes.
If the automatic sampling box is checked or the input filename box is
empty, this calls the planner to generate a set of candidate grasps.
Otherwise it reads the candidate grasps from a text file. The test button
is enabled, and a connection is set up so that after testing is completed
the show button will be enabled.
*/
void PlannerDlg::generateGrasps()
{
int a, b, c, d, e;
double f;
int density;
if (automaticCheckBox->isChecked()) {
density = densityFactorLine->text().toInt();
myGraspManager->get_graspPlanner()->set_parameterMode(density);
}
else {
a = num360stepsLine->text().toInt();
b = numParPlanesLine->text().toInt();
c = num180graspsLine->text().toInt();
d = numGraspRotsLine->text().toInt();
myGraspManager->get_graspPlanner()->set_parameterMode(0);
myGraspManager->get_graspPlanner()->set_planningParameters(a, b, c, d);
}
e = maxNumStepsLine->text().toInt();
f = backstepSizeLine->text().toDouble();
myGraspManager->get_graspTester()->set_testingParameters(e, f);
myGraspManager->get_graspTester()->
useQM(qmComboBox->currentItem());
myGraspManager->set_render(visualizeBox->isChecked());
if (automaticCheckBox->isChecked() || filenameLineEdit->text().isEmpty()) {
myGraspManager->generateGrasps();
QObject::connect(myGraspManager->get_graspTester(), SIGNAL(testingComplete()),
this, SLOT(enableShowButton()));
}
else {
if (filenameLineEdit->text().contains("master.txt")) {
masterFile.setName(filenameLineEdit->text());
if (!masterFile.open(QIODevice::ReadOnly)) {
QTWARNING("Could not open master grasp file");
return;
}
stream.setDevice(&masterFile);
QObject::connect(myGraspManager->get_graspTester(), SIGNAL(testingComplete()),
this, SLOT(testGrasps()));
}
else if (myGraspManager->
readCandidateGraspsFile(filenameLineEdit->text())) {
QTWARNING("Could not read grasps from file.");
QObject::connect(myGraspManager->get_graspTester(), SIGNAL(testingComplete()),
this, SLOT(enableShowButton()));
return;
}
}
TestButton->setEnabled(true);
}
int EigenGraspDlg::setWorld( World *w )
{
mWorld = w;
mHand = mWorld->getCurrentHand();
assert(mHand);
mEigenGrasps = mHand->getEigenGrasps();
if (!mEigenGrasps || mEigenGrasps->getSize() == 0) {
QTWARNING("Current hand contains no eigen grasp information");
return 0;
}
resetSlave();
adjustSliders();
handConfigurationChanged();
return 1;
}
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;
}
/*!
This calls on the grasp_manager to start the testing process. If the
results are to be saved, the filename is also provided to the grasp_tester.
*/
void PlannerDlg::testGrasps()
{
QString planFilename, quadFilename;
World *world = graspitCore->getWorld();
/*
if (TestButton->text() == "Pause") {
TestButton->setText("Continue");
myGraspManager->get_graspTester()->pauseTests();
return;
}
if (TestButton->text() == "Continue") {
TestButton->setText("Pause");
myGraspManager->get_graspTester()->continueTests();
return;
}
*/
if (!masterFile.isOpen()) {
if (!savefileLineEdit->text().isEmpty())
myGraspManager->get_graspTester()->
saveGraspsToFile(savefileLineEdit->text(), false);
//TestButton->setText("Pause");
myGraspManager->testGrasps();
}
else { // This is just a hack for a project we are working on
if (stream.atEnd()) {
masterFile.close();
return;
}
if (!savefileLineEdit->text().isEmpty())
myGraspManager->get_graspTester()->
saveGraspsToFile(savefileLineEdit->text(), true);
if (world->getNumGB() > 0) {
world->destroyElement(world->getGB(0));
}
stream >> quadFilename >> planFilename; stream.readLine();
// if (quadFilename.stripWhiteSpace().isEmpty() || planFilename.stripWhiteSpace().isEmpty())
world->importBody("GraspableBody", quadFilename);
world->getHand(0)->getGrasp()->setObject(world->getGB(0));
QFile logfile("grasplog.txt");
if (logfile.open(QIODevice::WriteOnly | QIODevice::Append)) {
QTextStream lout(&logfile);
lout << "Evaluating grasps of " <<
world->getGB(0)->getName() << endl;
logfile.close();
}
if (myGraspManager->
readCandidateGraspsFile(planFilename)) {
QTWARNING("Could not read grasps from file.");
masterFile.close();
return;
}
myGraspManager->testGrasps();
}
//TestButton->setText("Test");
}
bool getTransform(const TiXmlElement *root, transf &totalTran)
{
bool ok1, ok2, ok3;
bool ok[9];
if (root == NULL) {
QTWARNING("The given root is not a Transform Element");
return false;
}
QString rootValue = root->Value();
rootValue = rootValue.stripWhiteSpace();
if (rootValue != "transform") {
QTWARNING("The given root is not a Transform Element");
return false;
}
totalTran = transf::IDENTITY;
const TiXmlElement *child = root->FirstChildElement();
while (child != NULL) {
transf newTran;
QString valueStr = child->GetText();
valueStr = valueStr.simplifyWhiteSpace().stripWhiteSpace();
QStringList l;
l = QStringList::split(' ', valueStr);
QString defString = child->Value();
defString = defString.stripWhiteSpace();
if (defString == "translation") {
if (l.count() != 3) {
QTWARNING("Invalid translation transformation input");
return false;
}
double x, y, z;
x = l[0].toDouble(&ok1);
y = l[1].toDouble(&ok2);
z = l[2].toDouble(&ok3);
if (!ok1 || !ok2 || !ok3) {
QTWARNING("Invalid translation transformation input");
return false;
}
newTran.set(Quaternion::IDENTITY, vec3(x, y, z));
}
else if (defString == "rotation") {
if (l.count() != 2) {
QTWARNING("Invalid rotation transformation input");
return false;
}
double rotationAngle = l[0].toDouble(&ok1);
if (!ok1) {
QTWARNING("Invalid rotation transformation input");
return false;
}
rotationAngle *= M_PI / 180.0;
QString rotationAxis = l[1];
if (rotationAxis == "x") {
newTran.set(Quaternion(rotationAngle, vec3::X), vec3(0, 0, 0));
} else if (rotationAxis == "y") {
newTran.set(Quaternion(rotationAngle, vec3::Y), vec3(0, 0, 0));
} else if (rotationAxis == "z") {
newTran.set(Quaternion(rotationAngle, vec3::Z), vec3(0, 0, 0));
} else {
QTWARNING("Invalid rotation transformation input");
return false;
}
}
else if (defString == "rotationMatrix") {
if (l.count() != 9) {
QTWARNING("Invalid rotation transformation input");
return false;
}
double R[9];
for (int i = 0; i < 9; i++) {
R[i] = l[i].toDouble(&ok[i]);
if (!ok[i]) {
QTWARNING("Invalid rotation transformation input");
return false;
}
}
newTran.set(Quaternion(mat3(R).transpose()), vec3(0, 0, 0));
}
else if (defString == "fullTransform") {
QTextStream lineStream(&valueStr, QIODevice::ReadOnly);
lineStream >> newTran;
}
totalTran = newTran * totalTran;
child = child->NextSiblingElement();
}
/*! 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;
}
