00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00030
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
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 computeTran();
00066 }
00067
00068 void DHTransform::computeTran()
00069 {
00070 transf tr3,tr4;
00071
00072 dtrans[0] = 0.0;
00073 dtrans[1] = 0.0;
00074 dtrans[2] = d;
00075
00076 atrans[0] = a;
00077 atrans[1] = 0.0;
00078 atrans[2] = 0.0;
00079
00080 tr1 = rotate_transf(theta,vec3(0,0,1));
00081 tr2 = translate_transf(dtrans);
00082 tr3 = translate_transf(atrans);
00083 tr4 = rotate_transf(alpha,vec3(1,0,0));
00084 tr4TimesTr3 = tr4 * tr3;
00085
00086 tran = tr4TimesTr3 * tr2 * tr1;
00087 }
00088
00093 void DHTransform::setD(double q)
00094 {
00095 d = q;
00096 dtrans[2] = d;
00097 tr2 = translate_transf(dtrans);
00098
00099 tran = tr4TimesTr3 * tr2 * tr1;
00100 }
00101
00106 void DHTransform::setTheta(double q)
00107 {
00108 theta = q;
00109 tr1 = rotate_transf(theta,vec3(0,0,1));
00110
00111 tran = tr4TimesTr3 * tr2 * tr1;
00112 }
00113
00118 Joint::~Joint()
00119 {
00120 IVTran->unref(); delete DH;
00121 }
00122
00123 void
00124 Joint::cloneFrom(const Joint *original)
00125 {
00126 DOFnum = original->DOFnum;
00127 jointNum = original->jointNum;
00128 minVal = original->minVal;
00129 maxVal = original->maxVal;
00130 mCouplingRatio = original->mCouplingRatio;
00131 c = original->c;
00132 f1 = original->f1;
00133 f0 = original->f0;
00134 mK = original->mK;
00135 mRestVal = original->mRestVal;
00136 worldAxis = original->worldAxis;
00137 DH = new DHTransform(original->DH);
00138 DH->getTran().toSoTransform(IVTran);
00139 }
00140
00141 int
00142 Joint::getChainNum() const
00143 {
00144 return owner->getNum();
00145 }
00146
00147 void
00148 Joint::applyPassiveInternalWrenches()
00149 {
00150 double f = getFriction();
00151 DBGP("Friction coeffs: " << f0 << " " << f1);
00152 DBGP("Friction force: " << f);
00153 if (f != 0.0) applyInternalWrench(f);
00154
00155 f = getSpringForce();
00156 DBGP("Spring force: " << f);
00157 applyInternalWrench(-f);
00158 }
00159
00161 double
00162 Joint::getSpringForce() const
00163 {
00164 return mK * getDisplacement();
00165 }
00166
00173 Matrix
00174 Joint::jacobian(const Joint *joint, const transf &jointTran,
00175 const transf &toTarget, bool worldCoords)
00176 {
00177 transf T;
00178 if (worldCoords) {
00179
00180 T = transf(Quaternion::IDENTITY, toTarget.translation()) * jointTran.inverse();
00181 } else {
00182 T = toTarget * jointTran.inverse();
00183 }
00184 double M[36];
00185 T.jacobian(M);
00186 Matrix fullJ(M,6,6,true);
00187 Matrix J(Matrix::ZEROES<Matrix>(6,1));
00188 if (joint->getType() == REVOLUTE) {
00189
00190 J.copySubBlock(0, 0, 6, 1, fullJ, 0, 5);
00191 } else if (joint->getType() == PRISMATIC) {
00192
00193 J.copySubBlock(0, 0, 6, 1, fullJ, 0, 2);
00194 } else {
00195 assert(0);
00196 }
00197 return J;
00198 }
00199
00204 int
00205 PrismaticJoint::initJointFromXml(const TiXmlElement* root, int jnum)
00206 {
00207 char dStr[40],num[40],*tmp;
00208 QString dQStr;
00209 double theta,d,a,alpha;
00210 jointNum = jnum;
00211 const TiXmlElement* element = findXmlElement(root,"d");
00212 if(element){
00213 dQStr = element->GetText();
00214 dQStr = dQStr.stripWhiteSpace();
00215 strcpy(dStr,dQStr.toStdString().c_str());
00216 } else {
00217 return FAILURE;
00218 }
00219 if(!getDouble(root,"theta", theta)) return FAILURE;
00220 if(!getDouble(root,"a", a)) return FAILURE;
00221 if(!getDouble(root,"alpha", alpha)) return FAILURE;
00222 if(!getDouble(root,"minValue", minVal)) return FAILURE;
00223 if(!getDouble(root,"maxValue", maxVal)) return FAILURE;
00224 if (!getDouble(root,"viscousFriction", f1)) f1 = 0.0;
00225 if (!getDouble(root,"CoulombFriction", f0)) f0 = 0.0;
00226 if (!getDouble(root,"springStiffness", mK)) mK = 0.0;
00227 if (!getDouble(root,"restValue", mRestVal)) mRestVal = 0.0;
00228
00229 DBGP("thStr: " << theta << " d: " << dStr << " a: " << a << " alpha: "
00230 << alpha << " minVal: " << minVal << " maxVal: " << maxVal << " f1: "
00231 << f1 << " f0:" << f0 << " mK: " << mK << " mRestVal: " << mRestVal);
00232
00233
00234
00235 mK *= 1.0e6;
00236
00237 theta *= M_PI/180.0;
00238 alpha *= M_PI/180.0;
00239
00240 d = 0.0;
00241 tmp = dStr+1;
00242 sscanf(tmp,"%[0-9]",num);
00243 DOFnum = atoi(num);
00244 tmp += strlen(num);
00245 if (DOFnum > owner->getOwner()->getNumDOF()) {
00246 pr_error("DOF number is out of range\n");
00247 return FAILURE;
00248 }
00249 if (*tmp=='*') {
00250 tmp++;
00251 sscanf(tmp,"%[0-9.-]",num);
00252 tmp += strlen(num);
00253 mCouplingRatio = atof(num);
00254 }
00255 if (*tmp=='+') {
00256 tmp++;
00257 sscanf(tmp,"%lf",&c);
00258 }
00259
00260 DH = new DHTransform(theta,d+c,a,alpha);
00261 DH->getTran().toSoTransform(IVTran);
00262
00263 return SUCCESS;
00264 }
00265
00270 int
00271 PrismaticJoint::setVal(double q)
00272 {
00273 DH->setD(q+c);
00274 DH->getTran().toSoTransform(IVTran);
00275 return SUCCESS;
00276 }
00277
00282 void
00283 PrismaticJoint::applyInternalWrench(double magnitude)
00284 {
00285 dynJoint->getPrevLink()->addForce(-magnitude * worldAxis);
00286 dynJoint->getNextLink()->addForce(magnitude * worldAxis);
00287 }
00288
00294 int
00295 RevoluteJoint::initJointFromXml(const TiXmlElement* root, int jnum)
00296 {
00297
00298 QString thQStr;
00299 char thStr[40],num[40],*tmp;
00300 double theta,d,a,alpha;
00301 jointNum = jnum;
00302 const TiXmlElement* element = findXmlElement(root,"theta");
00303 if(element){
00304 thQStr = element->GetText();
00305 thQStr = thQStr.stripWhiteSpace();
00306 strcpy(thStr,thQStr.toStdString().c_str());
00307 }
00308 else
00309 return FAILURE;
00310 if(!getDouble(root,"d", d)) return FAILURE;
00311 if(!getDouble(root,"a", a)) return FAILURE;
00312 if(!getDouble(root,"alpha", alpha)) return FAILURE;
00313 if(!getDouble(root,"minValue", minVal)) return FAILURE;
00314 if(!getDouble(root,"maxValue", maxVal)) return FAILURE;
00315 if(!getDouble(root,"viscousFriction", f1)) f1 = 0.0;
00316 if(!getDouble(root,"CoulombFriction", f0)) f0 = 0.0;
00317 if(!getDouble(root,"springStiffness", mK)) mK = 0.0;
00318 if(!getDouble(root,"restValue", mRestVal)) mRestVal = 0.0;
00319
00320 DBGP("thStr: " << thStr << " d: " << d << " a: " << a << " alpha: "
00321 << alpha << " minVal: " << minVal << " maxVal: " << maxVal << " f1: "
00322 << f1 << " f0:" << f0 << " mK: " << mK << " mRestVal: " << mRestVal);
00323
00324 if (mK < 0) {
00325 DBGA("Negative spring stiffness");
00326 return FAILURE;
00327 }
00328
00329
00330
00331 mK *= 1.0e6;
00332
00333 alpha *= M_PI/180.0;
00334 minVal *= M_PI/180.0;
00335 maxVal *= M_PI/180.0;
00336
00337 theta = 0.0;
00338 tmp = thStr+1;
00339 sscanf(tmp,"%[0-9]",num);
00340 DOFnum = atoi(num);
00341 tmp += strlen(num);
00342
00343 if (DOFnum > owner->getOwner()->getNumDOF()) {
00344 pr_error("DOF number is out of range\n");
00345 return FAILURE;
00346 }
00347
00348 if (*tmp=='*') {
00349 tmp++;
00350 sscanf(tmp,"%[0-9.-]",num);
00351 tmp += strlen(num);
00352 mCouplingRatio = atof(num);
00353 }
00354 if (*tmp=='+') {
00355 tmp++;
00356 sscanf(tmp,"%lf",&c);
00357 c *= M_PI/180.0;
00358 }
00359
00360 DH = new DHTransform(theta+c,d,a,alpha);
00361 DH->getTran().toSoTransform(IVTran);
00362 return SUCCESS;
00363 }
00364
00369 int
00370 RevoluteJoint::setVal(double q)
00371 {
00372 DH->setTheta(q+c);
00373 DH->getTran().toSoTransform(IVTran);
00374 return SUCCESS;
00375 }
00376
00381 void
00382 RevoluteJoint::applyInternalWrench(double magnitude)
00383 {
00384 dynJoint->getPrevLink()->addTorque(-magnitude * worldAxis);
00385 dynJoint->getNextLink()->addTorque(magnitude * worldAxis);
00386 }
