libKinematics.cpp
Go to the documentation of this file.
00001 /* --------------------------------------------------------------------------------
00002  #
00003  #      libKinematics.cpp
00004  #  Project : Kinematics
00005  #      author : jhaller
00006  #      24.04.2008
00007  #
00008  # --------------------------------------------------------------------------------*/
00009 
00010 
00011 #include "libKinematics.h"
00012 
00013 
00014 extern "C"{
00015 
00016 KinematicsLib* _kinematics;
00017 bool LibInstantiated = false;
00018 
00020 
00021 int kin_setType(int type) {
00022         if (LibInstantiated == true) delete _kinematics;
00023         _kinematics = new KinematicsLib(type);
00024         LibInstantiated = true;
00025         return 0;
00026 }
00027 
00028 int kin_setMDH(FloatVector* theta, FloatVector* d, FloatVector* a,
00029                 FloatVector* alpha, int typeNr) {
00030         if (LibInstantiated == true) delete _kinematics;
00031         _kinematics = new KinematicsLib();
00032         LibInstantiated = true;
00033         std::vector<double> thetaw, dw, aw, alphaw;
00034         for (int i = 0; i < theta->length; ++i) {
00035                 thetaw.push_back(theta->data[i]);
00036                 dw.push_back(d->data[i]);
00037                 aw.push_back(a->data[i]);
00038                 alphaw.push_back(alpha->data[i]);
00039         }
00040         int ok = _kinematics->setMDH(thetaw, dw, aw, alphaw, typeNr);
00041         int error = (ok < 0) ? -1 : 0;
00042         return error;
00043 }
00044 
00045 int kin_setLinkLen(FloatVector* links) {
00046         if (!LibInstantiated)
00047                 return -1;
00048         std::vector<double> fw;
00049         for (int i = 0; i < links->length; ++i) {
00050                 fw.push_back(links->data[i]);
00051         }
00052         int ok = _kinematics->setLinkLen(fw);
00053         int error = (ok < 0) ? -1 : 0;
00054         return error;
00055 }
00056 
00057 int kin_setImmob(int immobile) {
00058         if (!LibInstantiated)
00059                 return -1;
00060         int ok = _kinematics->setImmob(immobile);
00061         int error = (ok < 0) ? -1 : 0;
00062         return error;
00063 }
00064 
00065 int kin_setEPC(IntVector* epc) {
00066         if (!LibInstantiated)
00067                 return -1;
00068         std::vector<int> iw;
00069         for (int i = 0; i < epc->length; ++i) {
00070                 iw.push_back(epc->data[i]);
00071         }
00072         int ok = _kinematics->setEPC(iw);
00073         int error = (ok < 0) ? -1 : 0;
00074         return error;
00075 }
00076 
00077 int kin_setEncOff(IntVector* encOffset) {
00078         if (!LibInstantiated)
00079                 return -1;
00080         std::vector<int> iw;
00081         for (int i = 0; i < encOffset->length; ++i) {
00082                 iw.push_back(encOffset->data[i]);
00083         }
00084         int ok = _kinematics->setEncOff(iw);
00085         int error = (ok < 0) ? -1 : 0;
00086         return error;
00087 }
00088 
00089 int kin_setRotDir(IntVector* rotDir) {
00090         if (!LibInstantiated)
00091                 return -1;
00092         std::vector<int> iw;
00093         for (int i = 0; i < rotDir->length; ++i) {
00094                 iw.push_back(rotDir->data[i]);
00095         }
00096         int ok = _kinematics->setRotDir(iw);
00097         int error = (ok < 0) ? -1 : 0;
00098         return error;
00099 }
00100 
00101 int kin_setAngOff(FloatVector* angleOffset) {
00102         if (!LibInstantiated)
00103                 return -1;
00104         std::vector<double> fw;
00105         for (int i = 0; i < angleOffset->length; ++i) {
00106                 fw.push_back(angleOffset->data[i]);
00107         }
00108         int ok = _kinematics->setAngOff(fw);
00109         int error = (ok < 0) ? -1 : 0;
00110         return error;
00111 }
00112 
00113 int kin_setAngRan(FloatVector* angleRange) {
00114         if (!LibInstantiated)
00115                 return -1;
00116         // check vector size
00117         std::vector<double> fw;
00118         for (int i = 0; i < angleRange->length; ++i) {
00119                 fw.push_back(angleRange->data[i]);
00120         }
00121         int ok = _kinematics->setAngRan(fw);
00122         int error = (ok < 0) ? -1 : 0;
00123         return error;
00124 }
00125 
00126 int kin_setTcpOff(FloatVector* tcpOffset) {
00127         if (!LibInstantiated)
00128                 return -1;
00129         std::vector<double> fw;
00130         for (int i = 0; i < tcpOffset->length; ++i) {
00131                 fw.push_back(tcpOffset->data[i]);
00132         }
00133         int ok = _kinematics->setTcpOff(fw);
00134         int error = (ok < 0) ? -1 : 0;
00135         return error;
00136 }
00137 
00139 
00140 int kin_getType() {
00141         if (!LibInstantiated)
00142                 return -1;
00143         return _kinematics->getType();
00144 }
00145 
00146 int kin_getMaxDOF() {
00147         if (!LibInstantiated)
00148                 return -1;
00149         return _kinematics->getMaxDOF();
00150 }
00151 
00152 int kin_getDOF() {
00153         if (!LibInstantiated)
00154                 return -1;
00155         return _kinematics->getDOF();
00156 }
00157 
00158 int kin_getDOM() {
00159         if (!LibInstantiated)
00160                 return -1;
00161         return _kinematics->getDOM();
00162 }
00163 
00164 int kin_getMDH(FloatVector* theta, FloatVector* d,
00165                 FloatVector* a, FloatVector* alpha) {
00166         if (!LibInstantiated)
00167                 return -1;
00168         std::vector<double> thetaw, dw, aw, alphaw;
00169         int ok = _kinematics->getMDH(thetaw, dw, aw, alphaw);
00170         for (int i = 0; i < (int)thetaw.size(); ++i) {
00171                 theta->data[i] = (float)thetaw.at(i);
00172                 d->data[i] = (float)dw.at(i);
00173                 a->data[i] = (float)aw.at(i);
00174                 alpha->data[i] = (float)alphaw.at(i);
00175         }
00176         theta->length = (int)thetaw.size();
00177         d->length = (int)thetaw.size();
00178         a->length = (int)thetaw.size();
00179         alpha->length = (int)thetaw.size();
00180         int error = (ok < 0) ? -1 : 0;
00181         return error;
00182 }
00183 
00184 int kin_getImmob() {
00185         if (!LibInstantiated)
00186                 return -1;
00187         return _kinematics->getImmob();
00188 }
00189 
00190 int kin_getEPC(IntVector* epc) {
00191         if (!LibInstantiated)
00192                 return -1;
00193         std::vector<int> iw;
00194         int ok = _kinematics->getEPC(iw);
00195         for (int i = 0; i < (int)iw.size(); ++i) {
00196                 epc->data[i] = iw.at(i);
00197         }
00198         epc->length = (int)iw.size();
00199         if ((int)iw.size() == 5) {
00200                 epc->data[5] = 51200;
00201                 epc->length++;
00202         }
00203         int error = (ok < 0) ? -1 : 0;
00204         return error;
00205 }
00206 
00207 int kin_getEncOff(IntVector* encOffset) {
00208         if (!LibInstantiated)
00209                 return -1;
00210         std::vector<int> iw;
00211         int ok = _kinematics->getEncOff(iw);
00212         for (int i = 0; i < (int)iw.size(); ++i) {
00213                 encOffset->data[i] = iw.at(i);
00214         }
00215         encOffset->length = iw.size();
00216         if ((int)iw.size() == 5) {
00217                 encOffset->data[5] = 31000;
00218                 encOffset->length++;
00219         }
00220         int error = (ok < 0) ? -1 : 0;
00221         return error;
00222 }
00223 
00224 int kin_getRotDir(IntVector* rotDir) {
00225         if (!LibInstantiated)
00226                 return -1;
00227         std::vector<int> iw;
00228         int ok = _kinematics->getRotDir(iw);
00229         for (int i = 0; i < (int)iw.size(); ++i) {
00230                 rotDir->data[i] = iw.at(i);
00231         }
00232         rotDir->length = iw.size();
00233         if ((int)iw.size() == 5) {
00234                 rotDir->data[5] = 1;
00235                 rotDir->length++;
00236         }
00237         int error = (ok < 0) ? -1 : 0;
00238         return error;
00239 }
00240 
00241 int kin_getAngOff(FloatVector* angleOffset) {
00242         if (!LibInstantiated)
00243                 return -1;
00244         std::vector<double> fw;
00245         int ok = _kinematics->getAngOff(fw);
00246         for (int i = 0; i < (int)fw.size(); ++i) {
00247                 angleOffset->data[i] = (float)fw.at(i);
00248         }
00249         angleOffset->length = fw.size();
00250         if ((int)fw.size() == 5) {
00251                 angleOffset->data[5] = 0.0;
00252                 angleOffset->length++;
00253         }
00254         int error = (ok < 0) ? -1 : 0;
00255         return error;
00256 }
00257 
00258 int kin_getAngRan(FloatVector* angleRange) {
00259         if (!LibInstantiated)
00260                 return -1;
00261         std::vector<double> fw;
00262         int ok = _kinematics->getAngRan(fw);
00263         for (int i = 0; i < (int)fw.size(); ++i) {
00264                 angleRange->data[i] = (float)fw.at(i);
00265         }
00266         angleRange->length = fw.size();
00267         if ((int)fw.size() == 5) {
00268                 angleRange->data[5] = 0.0;
00269                 angleRange->length++;
00270         }
00271         int error = (ok < 0) ? -1 : 0;
00272         return error;
00273 }
00274 
00275 int kin_getTcpOff(FloatVector* tcpOffset) {
00276         if (!LibInstantiated)
00277                 return -1;
00278         std::vector<double> fw;
00279         int ok = _kinematics->getTcpOff(fw);
00280         for (int i = 0; i < (int)fw.size(); ++i) {
00281                 tcpOffset->data[i] = (float)fw.at(i);
00282         }
00283         tcpOffset->length = fw.size();
00284         int error = (ok < 0) ? -1 : 0;
00285         return error;
00286 }
00287 
00288 int kin_getVersion(IntVector* version) {
00289         int error;
00290         if (LibInstantiated) {
00291                 std::vector<int> iw;
00292                 int ok = _kinematics->getVersion(iw);
00293                 for (int i = 0; i < (int)iw.size(); ++i) {
00294                         version->data[i] = iw.at(i);
00295                 }
00296                 version->length = iw.size();
00297                 error = (ok < 0) ? -1 : 0;
00298         } else {
00299                 version->data[0] = KINLIB_VERSION_MAJOR;
00300                 version->data[1] = KINLIB_VERSION_MINOR;
00301                 version->data[2] = KINLIB_VERSION_REVISION;
00302                 version->length = 3;
00303                 error = 0;
00304         }
00305         return error;
00306 }
00307 
00309 
00310 int kin_init() {
00311         if (!LibInstantiated)
00312                 return -1;
00313         int ok = _kinematics->init();
00314         int error = (ok < 0) ? -1 : 0;
00315         return error;
00316 }
00317 
00319 
00320 int kin_clean() {
00321         if (LibInstantiated == true)
00322                 delete _kinematics;
00323         LibInstantiated = false;
00324         return 0;
00325 }
00326 
00328 
00329 int kin_K4D2mDHAng(FloatVector* angleK4D, FloatVector* angleMDH) {
00330         if (!LibInstantiated)
00331                 return -1;
00332         std::vector<double> k4d, mdh;
00333         for (int i = 0; i < angleK4D->length; ++i) {
00334                 k4d.push_back(angleK4D->data[i]);
00335         }
00336         int ok = _kinematics->K4D2mDHAng(k4d, mdh);
00337         for (int i = 0; i < (int)mdh.size(); ++i) {
00338                 angleMDH->data[i] = (float)mdh.at(i);
00339         }
00340         angleMDH->length = mdh.size();
00341         int error = (ok < 0) ? -1 : 0;
00342         return error;
00343 }
00344 
00345 int kin_mDH2K4DAng(FloatVector* angleMDH, FloatVector* angleK4D) {
00346         if (!LibInstantiated)
00347                 return -1;
00348         std::vector<double> mdh, k4d;
00349         for (int i = 0; i < angleMDH->length; ++i) {
00350                 mdh.push_back(angleMDH->data[i]);
00351         }
00352         int ok = _kinematics->mDH2K4DAng(mdh, k4d);
00353         for (int i = 0; i < (int)k4d.size(); ++i) {
00354                 angleK4D->data[i] = (float)k4d.at(i);
00355         }
00356         angleK4D->length = k4d.size();
00357         int error = (ok < 0) ? -1 : 0;
00358         return error;
00359 }
00360 
00361 int kin_enc2rad(IntVector* enc, FloatVector* angle) {
00362         if (!LibInstantiated)
00363                 return -1;
00364         std::vector<int> iw;
00365         for (int i = 0; i < enc->length; ++i) {
00366                 iw.push_back(enc->data[i]);
00367         }
00368         std::vector<double> fw;
00369         int ok = _kinematics->enc2rad(iw, fw);
00370         for (int i = 0; i < (int)fw.size(); ++i) {
00371                 angle->data[i] = (float)fw.at(i);
00372         }
00373         angle->length = fw.size();
00374         int error = (ok < 0) ? -1 : 0;
00375         return error;
00376 }
00377 
00378 int kin_rad2enc(FloatVector* angle, IntVector* enc) {
00379         if (!LibInstantiated)
00380                 return -1;
00381         std::vector<double> fw;
00382         for (int i = 0; i < angle->length; ++i) {
00383                 fw.push_back(angle->data[i]);
00384         }
00385         std::vector<int> iw;
00386         int ok = _kinematics->rad2enc(fw, iw);
00387         for (int i = 0; i < (int)iw.size(); ++i) {
00388                 enc->data[i] = iw.at(i);
00389         }
00390         enc->length = iw.size();
00391         int error = (ok < 0) ? -1 : 0;
00392         return error;
00393 }
00394 
00395 int kin_DK(FloatVector* angle, FloatVector* pose) {
00396         if (!LibInstantiated)
00397                 return -1;
00398         std::vector<double> aw, pw;
00399         for (int i = 0; i < angle->length; ++i) {
00400                 aw.push_back(angle->data[i]);
00401         }
00402         int ok = _kinematics->directKinematics(aw, pw);
00403         for (int i = 0; i < (int)pw.size(); ++i) {
00404                 pose->data[i] = (float)pw.at(i);
00405         }
00406         pose->length = pw.size();
00407         int error = (ok < 0) ? -1 : 0;
00408         return error;
00409 }
00410 
00411 int kin_IK(FloatVector* pose, FloatVector* prev, FloatVector* angle,
00412                 int maxBisection) {
00413         if (!LibInstantiated)
00414                 return -1;
00415         std::vector<double> pw;
00416         for (int i = 0; i < pose->length; ++i) {
00417                 pw.push_back(pose->data[i]);
00418         }
00419         std::vector<double> paw;
00420         for (int i = 0; i < prev->length; ++i) {
00421                 paw.push_back(prev->data[i]);
00422         }
00423         std::vector<double> aw;
00424         int ok = _kinematics->inverseKinematics(pw, paw, aw, maxBisection);
00425         for (int i = 0; i < (int)aw.size(); ++i) {
00426                 angle->data[i] = (float)aw.at(i);
00427         }
00428         angle->length = aw.size();
00429         int error = (ok < 0) ? -1 : 0;
00430         return error;
00431 }
00432 
00434 } // end extern "C"
00435 
00436 
00437 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


kni
Author(s): Neuronics AG (see AUTHORS.txt); ROS wrapper by Martin Günther
autogenerated on Tue May 28 2013 14:52:54