graspit: /opt/ros/diamondback/stacks/graspit_simulator/graspit/graspit

00001 //######################################################################
00002 //
00003 // GraspIt!
00004 // Copyright (C) 2002-2009  Columbia University in the City of New York.
00005 // All rights reserved.
00006 //
00007 // GraspIt! is free software: you can redistribute it and/or modify
00008 // it under the terms of the GNU General Public License as published by
00009 // the Free Software Foundation, either version 3 of the License, or
00010 // (at your option) any later version.
00011 //
00012 // GraspIt! is distributed in the hope that it will be useful,
00013 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00014 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00015 // GNU General Public License for more details.
00016 //
00017 // You should have received a copy of the GNU General Public License
00018 // along with GraspIt!.  If not, see <http://www.gnu.org/licenses/>.
00019 //
00020 // Author(s): Andrew T. Miller and Matei T. Ciocarlie
00021 //
00022 // $Id: joint.cpp,v 1.31 2009/08/19 23:17:07 cmatei Exp $
00023 //
00024 //######################################################################
00025 
00030 /* standard C includes */
00031 #include <stdio.h>
00032 #include <stdlib.h>
00033 #include <math.h>
00034 #include <iostream>
00035 
00036 #include <Inventor/SoDB.h>
00037 #include <Inventor/SoInput.h>
00038 #include <Inventor/actions/SoGetBoundingBoxAction.h>
00039 #include <Inventor/actions/SoGetMatrixAction.h>
00040 #include <Inventor/actions/SoSearchAction.h>
00041 #include <Inventor/nodes/SoSeparator.h>
00042 #include <Inventor/nodes/SoCoordinate3.h>
00043 #include <Inventor/nodes/SoTransform.h>
00044 #include <Inventor/draggers/SoRotateDiscDragger.h>
00045 
00046 #include "joint.h"
00047 #include "robot.h"
00048 #include "mytools.h"
00049 #include "body.h"
00050 #include "dynJoint.h"
00051 #include "math/matrix.h"
00052 #include "tinyxml.h"
00053 
00054 #ifdef USE_DMALLOC
00055 #include "dmalloc.h"
00056 #endif
00057 
00058 //#define GRASPITDBG
00059 #include "debug.h"
00060 
00062 DHTransform::DHTransform(double thval,double dval,double aval,double alval) :
00063 theta(thval),d(dval),a(aval),alpha(alval)
00064 {
00065         transf tr3,tr4;
00066 
00067         dtrans[0] = 0.0;
00068         dtrans[1] = 0.0;
00069         dtrans[2] = d;
00070 
00071         atrans[0] = a;
00072         atrans[1] = 0.0;
00073         atrans[2] = 0.0;
00074 
00075         tr1 = rotate_transf(theta,vec3(0,0,1));
00076         tr2 = translate_transf(dtrans);
00077         tr3 = translate_transf(atrans);
00078         tr4 = rotate_transf(alpha,vec3(1,0,0));
00079         tr4TimesTr3 = tr4 * tr3;
00080 
00081         tran = tr4TimesTr3 * tr2 * tr1;
00082 }
00083 
00088 void DHTransform::setD(double q)
00089 {
00090         d = q;
00091         dtrans[2] = d;
00092         tr2 = translate_transf(dtrans);
00093 
00094         tran = tr4TimesTr3 * tr2 * tr1;
00095 }
00096 
00101 void DHTransform::setTheta(double q)
00102 {
00103         theta = q;
00104         tr1 = rotate_transf(theta,vec3(0,0,1));
00105 
00106         tran = tr4TimesTr3 * tr2 * tr1;
00107 }
00108 
00113 Joint::~Joint()
00114 {
00115         IVTran->unref(); delete DH;
00116 }
00117 
00118 void
00119 Joint::cloneFrom(const Joint *original)
00120 {
00121         DOFnum  = original->DOFnum;
00122         jointNum = original->jointNum;
00123         minVal = original->minVal;
00124         maxVal = original->maxVal;
00125         mCouplingRatio = original->mCouplingRatio;
00126         c = original->c;
00127         f1 = original->f1;
00128         f0 = original->f0;
00129         mK = original->mK;
00130         mRestVal = original->mRestVal;
00131         worldAxis = original->worldAxis;
00132         DH = new DHTransform(original->DH);
00133         DH->getTran().toSoTransform(IVTran);
00134 }
00135 
00136 int
00137 Joint::getChainNum() const
00138 {
00139         return owner->getNum();
00140 }
00141 
00142 void
00143 Joint::applyPassiveInternalWrenches()
00144 {
00145         double f = getFriction();
00146         DBGP("Friction coeffs: " << f0 << " " << f1);
00147         DBGP("Friction force: " << f);
00148         if (f != 0.0) applyInternalWrench(f);
00149 
00150         f = getSpringForce();
00151         DBGP("Spring force: " << f);
00152         applyInternalWrench(-f);
00153 }
00154 
00156 double
00157 Joint::getSpringForce() const 
00158 {
00159         return mK * getDisplacement();
00160 }
00161 
00168 Matrix 
00169 Joint::jacobian(const Joint *joint, const transf &jointTran, 
00170                                 const transf &toTarget, bool worldCoords)
00171 {
00172         transf T;
00173         if (worldCoords) {
00174                 // the translation from joint coordinate system to world coordinate system
00175                 T = transf(Quaternion::IDENTITY, toTarget.translation()) * jointTran.inverse();
00176         } else {
00177                 T = toTarget * jointTran.inverse();     
00178         }
00179         double M[36];
00180         T.jacobian(M);
00181         Matrix fullJ(M,6,6,true);
00182         Matrix J(Matrix::ZEROES<Matrix>(6,1));
00183         if (joint->getType() == REVOLUTE) {
00184                 //keep rotation against z
00185                 J.copySubBlock(0, 0, 6, 1, fullJ, 0, 5);
00186         } else if (joint->getType() == PRISMATIC) {
00187                 //keep translation along z
00188                 J.copySubBlock(0, 0, 6, 1, fullJ, 0, 2);
00189         } else {
00190                 assert(0);
00191         }
00192         return J;
00193 }
00194 
00199 int
00200 PrismaticJoint::initJointFromXml(const TiXmlElement* root, int jnum)
00201 {
00202         char dStr[40],num[40],*tmp;
00203         QString dQStr;
00204         double theta,d,a,alpha;
00205         jointNum = jnum;
00206         const TiXmlElement* element = findXmlElement(root,"d");
00207         if(element){
00208                 dQStr = element->GetText();
00209                 dQStr = dQStr.stripWhiteSpace();
00210                 strcpy(dStr,dQStr.toStdString().c_str());
00211         } else {
00212                 return FAILURE;
00213         }
00214         if(!getDouble(root,"theta", theta)) return FAILURE;
00215         if(!getDouble(root,"a", a)) return FAILURE;
00216         if(!getDouble(root,"alpha", alpha)) return FAILURE;
00217         if(!getDouble(root,"minValue", minVal)) return FAILURE;
00218         if(!getDouble(root,"maxValue", maxVal)) return FAILURE;
00219         if (!getDouble(root,"viscousFriction", f1)) f1 = 0.0;
00220         if (!getDouble(root,"CoulombFriction", f0)) f0 = 0.0;
00221         if (!getDouble(root,"springStiffness", mK)) mK = 0.0;
00222         if (!getDouble(root,"restValue", mRestVal)) mRestVal = 0.0;
00223 
00224         DBGP("thStr: " << theta << " d: " << dStr << " a: " << a << " alpha: " 
00225                 << alpha << " minVal: " << minVal << " maxVal: " << maxVal << " f1: " 
00226                 << f1 << " f0:" << f0 << " mK: " << mK << " mRestVal: " << mRestVal);
00227 
00228         //convert to graspit force units which for now seem to be the
00229         //rather strange N * 1.0e6
00230         mK *= 1.0e6; 
00231 
00232         theta *= M_PI/180.0;
00233         alpha *= M_PI/180.0;
00234 
00235         d = 0.0;
00236         tmp = dStr+1;
00237         sscanf(tmp,"%[0-9]",num);
00238         DOFnum = atoi(num);
00239         tmp += strlen(num);
00240         if (DOFnum > owner->getOwner()->getNumDOF()) {
00241                 pr_error("DOF number is out of range\n");
00242                 return FAILURE;
00243         }
00244         if (*tmp=='*') {
00245                 tmp++;
00246                 sscanf(tmp,"%[0-9.-]",num);
00247                 tmp += strlen(num);
00248                 mCouplingRatio = atof(num);
00249         }
00250         if (*tmp=='+') {
00251                 tmp++;
00252                 sscanf(tmp,"%lf",&c);
00253         }
00254 
00255         DH = new DHTransform(theta,d+c,a,alpha);  
00256         DH->getTran().toSoTransform(IVTran);
00257 
00258         return SUCCESS;
00259 }
00260 
00265 int
00266 PrismaticJoint::setVal(double q)
00267 {
00268         DH->setD(q+c);
00269         DH->getTran().toSoTransform(IVTran);
00270         return SUCCESS;  
00271 }
00272 
00277 void
00278 PrismaticJoint::applyInternalWrench(double magnitude)
00279 {
00280         dynJoint->getPrevLink()->addForce(-magnitude * worldAxis);
00281         dynJoint->getNextLink()->addForce(magnitude * worldAxis);
00282 }
00283 
00289 int
00290 RevoluteJoint::initJointFromXml(const TiXmlElement* root, int jnum)
00291 {
00292 
00293         QString thQStr;
00294         char thStr[40],num[40],*tmp;
00295         double theta,d,a,alpha;
00296         jointNum = jnum;
00297         const TiXmlElement* element = findXmlElement(root,"theta");
00298         if(element){
00299                 thQStr = element->GetText();
00300                 thQStr = thQStr.stripWhiteSpace();
00301                 strcpy(thStr,thQStr.toStdString().c_str());
00302         }
00303         else
00304                 return FAILURE;
00305         if(!getDouble(root,"d", d)) return FAILURE;
00306         if(!getDouble(root,"a", a)) return FAILURE;
00307         if(!getDouble(root,"alpha", alpha)) return FAILURE;
00308         if(!getDouble(root,"minValue", minVal)) return FAILURE;
00309         if(!getDouble(root,"maxValue", maxVal)) return FAILURE;
00310         if(!getDouble(root,"viscousFriction", f1)) f1 = 0.0;
00311         if(!getDouble(root,"CoulombFriction", f0)) f0 = 0.0;
00312         if(!getDouble(root,"springStiffness", mK)) mK = 0.0;
00313         if(!getDouble(root,"restValue", mRestVal)) mRestVal = 0.0;
00314 
00315         DBGP("thStr: " << thStr << " d: " << d << " a: " << a << " alpha: " 
00316                 << alpha << " minVal: " << minVal << " maxVal: " << maxVal << " f1: " 
00317                 << f1 << " f0:" << f0 << " mK: " << mK << " mRestVal: " << mRestVal);
00318 
00319         if (mK < 0) {
00320                 DBGA("Negative spring stiffness");
00321                 return FAILURE;
00322         }
00323 
00324         //convert to graspit units which for now seem to be the
00325         //rather strange Nmm * 1.0e6
00326         mK *= 1.0e6; 
00327 
00328         alpha *= M_PI/180.0;
00329         minVal *= M_PI/180.0;
00330         maxVal *= M_PI/180.0;
00331 
00332         theta = 0.0;
00333         tmp = thStr+1;
00334         sscanf(tmp,"%[0-9]",num);
00335         DOFnum = atoi(num);
00336         tmp += strlen(num);
00337 
00338         if (DOFnum > owner->getOwner()->getNumDOF()) {
00339                 pr_error("DOF number is out of range\n");
00340                 return FAILURE;
00341         }
00342 
00343         if (*tmp=='*') {
00344                 tmp++;
00345                 sscanf(tmp,"%[0-9.-]",num);
00346                 tmp += strlen(num);
00347                 mCouplingRatio = atof(num);
00348         }
00349         if (*tmp=='+') {
00350                 tmp++;
00351                 sscanf(tmp,"%lf",&c);
00352                 c *= M_PI/180.0;
00353         }
00354 
00355         DH = new DHTransform(theta+c,d,a,alpha);  
00356         DH->getTran().toSoTransform(IVTran);
00357         return SUCCESS;
00358 }
00359 
00364 int
00365 RevoluteJoint::setVal(double q)
00366 {
00367         DH->setTheta(q+c);
00368         DH->getTran().toSoTransform(IVTran);
00369         return SUCCESS;  
00370 }
00371 
00376 void
00377 RevoluteJoint::applyInternalWrench(double magnitude)
00378 {
00379         dynJoint->getPrevLink()->addTorque(-magnitude * worldAxis);
00380         dynJoint->getNextLink()->addTorque(magnitude * worldAxis);
00381 }
/opt/ros/diamondback/stacks/graspit_simulator/graspit/graspit_source/src/joint.cpp
