/*! 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;
}
/*! Initializes a prismatic joint from an XML DOM read from the robot
configuration file. Returns FAILURE if it could not read all the
necessary values from the provided XML, otherwise it returns SUCESS.
*/
int
PrismaticJoint::initJointFromXml(const TiXmlElement* root, int jnum, int DOFOffset)
{
char dStr[40],num[40],*tmp;
QString dQStr;
double theta,d,a,alpha;
jointNum = jnum;
const TiXmlElement* element = findXmlElement(root,"d");
if(element){
dQStr = element->GetText();
dQStr = dQStr.stripWhiteSpace();
strcpy(dStr,dQStr.toStdString().c_str());
} else {
return FAILURE;
}
if(!getDouble(root,"theta", theta)){
return FAILURE;
}
if(!getDouble(root,"a", a)){
return FAILURE;
}
if(!getDouble(root,"alpha", alpha)){
return FAILURE;
}
if(!getDouble(root,"minValue", minVal)){
return FAILURE;
}
if(!getDouble(root,"maxValue", maxVal)){
return FAILURE;
}
getDouble(root,"viscousFriction", f1);
getDouble(root,"CoulombFriction", f0);
getDouble(root,"springStiffness", mK);
getDouble(root,"restValue", mRestVal);
DBGP("thStr: " << theta << " d: " << dStr << " a: " << a << " alpha: "
<< alpha << " minVal: " << minVal << " maxVal: " << maxVal << " f1: "
<< f1 << " f0:" << f0 << " mK: " << mK << " mRestVal: " << mRestVal);
//convert to graspit force units which for now seem to be the
//rather strange N * 1.0e6
mK *= 1.0e6;
theta *= M_PI/180.0;
alpha *= M_PI/180.0;
d = 0.0;
tmp = dStr+1;
sscanf(tmp,"%[0-9]",num);
DOFnum = atoi(num);
tmp += strlen(num);
if (DOFnum > owner->getOwner()->getNumDOF()) {
pr_error("DOF number is out of range\n");
return FAILURE;
}
if (*tmp=='*') {
tmp++;
sscanf(tmp,"%[0-9.-]",num);
tmp += strlen(num);
mCouplingRatio = atof(num);
}
if (*tmp=='+') {
tmp++;
sscanf(tmp,"%lf",&c);
}
DH = new DHTransform(theta,d+c,a,alpha);
DH->getTran().toSoTransform(IVTran);
return SUCCESS;
}
int SnakeJoint::initMaster(const TiXmlElement *root, int jnum){
QString thQStr;
char thStr[40],num[40],*tmp;
double theta,d,a,alpha;
jointNum = jnum;
const TiXmlElement* element = findXmlElement(root,"theta");
if(element){
thQStr = element->GetText();
thQStr = thQStr.stripWhiteSpace();
strcpy(thStr,thQStr.toStdString().c_str());
}
else
return FAILURE;
if(!getDouble(root,"d", d)){
return FAILURE;
}
if(!getDouble(root,"a", a)){
return FAILURE;
}
if(!getDouble(root,"alpha", alpha)){
return FAILURE;
}
if(!getDouble(root,"minValue", minVal)){
return FAILURE;
}
if(!getDouble(root,"maxValue", maxVal)){
return FAILURE;
}
if(!getDouble(root,"snakeLength", lengthSnake)){
return FAILURE;
}
if(!getInt(root,"snakeLinks", numSnakeLinks)){
return FAILURE;
}
getDouble(root,"viscousFriction", f1);
getDouble(root,"CoulombFriction", f0);
getDouble(root,"springStiffness", mK);
getDouble(root,"restValue", mRestVal);
DBGA("thStr: " << thStr << " d: " << d << " a: " << a << " alpha: "
<< alpha << " minVal: " << minVal << " maxVal: " << maxVal << " f1: "
<< f1 << " f0:" << f0 << " mK: " << mK << " mRestVal: " << mRestVal);
if (mK < 0) {
DBGA("Negative spring stiffness");
return FAILURE;
} else if (mK>0) {
if (mRestVal < minVal || mRestVal > maxVal) {
DBGA("Joint spring rest value is not within legal range");
return FAILURE;
}
}
//convert to graspit units which for now seem to be the
//rather strange Nmm * 1.0e6
mK *= 1.0e6;
alpha *= M_PI/180.0;
minVal *= M_PI/180.0;
maxVal *= M_PI/180.0;
theta = 0.0;
tmp = thStr+1;
sscanf(tmp,"%[0-9]",num);
DOFnum = atoi(num);
tmp += strlen(num);
if (DOFnum > owner->getOwner()->getNumDOF()) {
pr_error("DOF number is out of range\n");
return FAILURE;
}
if (*tmp=='*') {
tmp++;
sscanf(tmp,"%[0-9.-]",num);
tmp += strlen(num);
mCouplingRatio = atof(num);
}
if (*tmp=='+') {
tmp++;
sscanf(tmp,"%lf",&c);
c *= M_PI/180.0;
}
DH = new DHTransform(theta+c,d,a,alpha);
DH->getTran().toSoTransform(IVTran);
return SUCCESS;
}
/*!
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;
}
