kernel/DH_Link.h

Go to the documentation of this file.
00001 #ifndef DH_LINK_H
00002 #define DH_LINK_H
00003 
00004 #include <iostream>
00005 #include <string>
00006 #include <vector>
00007 #include <stdlib.h>
00008 #include <stdio.h>
00009 
00010 #include "Operations.h"
00011 #include "Joint.h"
00012 #include "DH_Chain.h"
00013 
00014 template <class DataType, int dim>
00015 class DH_Chain;
00016 
00017 template <class DataType, int dim>
00018 class SceneBase;
00019 
00020 template <class DataType, int dim>
00021 class Scene;
00022 
00026 template <class DataType, int dim>
00027 class DH_Link {
00028 public:
00029   typedef Joint<DataType,dim> JointType;
00030   friend class Joint<DataType,dim>;
00031   friend class DH_Chain<DataType,dim>;
00032   friend class SceneBase<DataType,dim>;
00033   friend class Scene<DataType,dim>;
00034   enum arrangmentType { intersect, parallel, coincident, skew }; 
00036   // Constructors
00037   DH_Link() {
00038     setDefault();
00039   }
00040 
00041   DH_Link(JointType& a, JointType& b) {
00042     setDefault();
00043     parent = &a;
00044     child = &b;
00045     calcDenavitHartenbergParam();
00046     calcDenavitHartenbergMat();
00047   }
00048 
00049   DH_Link(JointType& a, JointType& b, DataType r, DataType al, DataType d, DataType th) {
00050     setDefault();
00051     parent = &a;
00052     child = &b;
00053 #ifdef LINK_DEBUG_ON
00054     ( mklog() << " Link type " << parent->type << " set between coordinate frames:" << stringID(parent->CFIdTag())
00055               << " to " << stringID(child->CFIdTag()) << "\n" ).flush();
00056 #endif
00057     radius = r;
00058     alpha = al;
00059     depth = d;
00060     theta = th;
00061     calcDenavitHartenbergMat();
00062   }
00063 
00064   // Destructor
00065   ~DH_Link() {}
00066 
00067   void setDefault() {
00068     id = -1;
00069     origin(0) = 1;  origin(1) = 0.;  origin(2) = 0.;
00070     xaxis(0) = 1;  xaxis(1) = 0.;  xaxis(2) = 0.;
00071     yaxis(0) = 0;  yaxis(1) = 1.;  yaxis(2) = 0.;
00072     zaxis(0) = 0;  zaxis(1) = 0.;  zaxis(2) = 1.;
00073     l0i(0) = 0;  l0i(1) = 0.;  l0i(2) = 1.;
00074 
00075     radius = 0.;
00076     alpha = 0.;
00077     depth = 0.;
00078     theta = 0.;
00079     q_pre = 0.;
00080     q_dot_pre = 0.;
00081     DHMat.resize(4,4);
00082     DHMat.assign(imatrix (4,4));
00083     Jv.resize(3,0.);
00084     Jw.resize(3,0.);
00085   }
00086 
00087   virtual int getID() { return id; }
00088 
00089   virtual void setDHMat( MType val ) { DHMat = val; }
00090   virtual MType GetDHMat() { return DHMat; }
00091 
00092   void calcDenavitHartenbergParam() {
00093     PType tmp(0.), ntmp(0.);
00094     int tog=0;
00095     dist3D_Line_to_Line( child->origin, (child->origin+child->zaxis), parent->origin, (parent->origin+parent->zaxis),
00096                          l0i, origin, xaxis, radius, arrangement);
00097 #ifdef LINK_DEBUG_ON
00098     ( mklog() << " parent->zaxis " << parent->zaxis << " parent->xaxis " << parent->xaxis
00099               << " parent->origin " << parent->origin << " child->origin " << child->origin
00100               << " child->zaxis " << child->zaxis << " child->xaxis " << child->xaxis << std::endl
00101               << " l0i " << l0i << " origin " << origin << " xaxis " << xaxis
00102               << " arrangement " << arrangement << std::endl ).flush();
00103 #endif
00104     switch (arrangement) {
00105     case intersect: {
00106       xaxis = cross3D(parent->zaxis,child->zaxis);
00107       if (norm_2(xaxis) < TOL ) {
00108         xaxis = child->zaxis;
00109       }
00110       child->xaxis = xaxis;
00111       break;
00112     }
00113     case parallel: {
00114       child->xaxis = xaxis;
00115 
00116       break;
00117     }
00118     case coincident: {
00119       xaxis = parent->xaxis;
00120       child->xaxis = xaxis;
00121       l0i = child->origin;
00122       break;
00123     }
00124     case skew: {
00125       child->xaxis = xaxis;
00126       break;
00127     }
00128     }
00129 
00130     alpha = angle3D2_axis(parent->zaxis,child->zaxis,xaxis);
00131     tmp = l0i-parent->origin;
00132     tmp = project(tmp,parent->zaxis);
00133     depth = norm_2(tmp);
00134     theta = angle3D2_axis(parent->xaxis,xaxis,parent->zaxis);
00135 
00136 #ifdef LINK_DEBUG_ON
00137     ( mklog()  << " l0i " << l0i << " parent->origin " << parent->origin
00138                << " depth " << depth ).flush();
00139 #endif
00140     if (arrangement == parallel || arrangement == coincident) {
00141       if (arrangement == parallel ) {
00142         tmp = child->origin - parent->origin;
00143         tmp = project(tmp,parent->zaxis);
00144         depth = norm_2(tmp);
00145         ntmp = (child->origin - parent->origin) - tmp;
00146         xaxis = normalize(ntmp);
00147 #ifdef LINK_DEBUG_ON
00148         ( mklog() << "\n -=-=- xaxis " << xaxis << std::endl ).flush();
00149 #endif
00150         xaxis(0) = ntmp(1);   xaxis(1) = ntmp(0);
00151         theta = angle3D3_axis(parent->xaxis,xaxis,parent->zaxis,tog);
00152       }
00153       if (octant2(tmp,parent->zaxis) == 0 ) {
00154         depth = DataType(-1.)*depth;
00155       }
00156     }
00157 #ifdef LINK_DEBUG_ON
00158     ( mklog() << "~~~~~~~~~~~~~~~~~~~~~\n"
00159               << " radius " << radius << " alpha " << alpha << " depth " << depth
00160               << " theta " << theta << std::endl ).flush();
00161 #endif
00162   }
00163 
00164   void calcDenavitHartenbergMat() {
00165 #ifdef LINK_DEBUG_ON
00166     ( mklog() << "calcDenavitHartenbergMat() parent->type = " << parent->type << std::endl ).flush();
00167 #endif
00168     if (parent->type == JointType::revolute ) {
00169 #ifdef LINK_DEBUG_ON
00170       ( mklog() << "revolute link q_pre = " << q_pre << std::endl ).flush();
00171 #endif
00172       DHMat = DenavitHartenbergMat(radius, alpha, depth, (theta+q_pre));
00173     }
00174     else if (parent->type == JointType::prismatic ) {
00175 #ifdef LINK_DEBUG_ON
00176       ( mklog() << "prismatic link q_pre = " << q_pre << std::endl ).flush();
00177 #endif
00178       DHMat = DenavitHartenbergMat(radius, alpha, (depth+q_pre), theta);
00179     }
00180     else if (parent->type == JointType::sphericalWrist ) {
00181 #ifdef LINK_DEBUG_ON
00182       ( mklog() << "spherical wrist link q_pre = " << q_pre << std::endl
00183                 << " radius " << radius << " alpha " << alpha << " depth " << depth
00184                 << " theta " << theta << std::endl ).flush();
00185 #endif
00186       DHMat = DenavitHartenbergMat_spericalWrist(radius, alpha, depth, theta); //z,X,Z,offset
00187     }
00188 #ifdef LINK_DEBUG_ON
00189     ( mklog() << " DHMat " << DHMat << std::endl
00190               << "~~~~~~~~~~~~~~~~~~~~~\n" ).flush();
00191 #endif
00192   }
00193 
00194   void update() {
00195       ( mklog() << "link update\n" ).flush();
00196     calcDenavitHartenbergParam();
00197     calcDenavitHartenbergMat();
00198   }
00199 
00200   void actuate( DataType val) {
00201     q_pre = val;
00202     q_dot_pre = 0.;
00203 #ifdef LINK_DEBUG_ON
00204     ( mklog() << "ACTUATING q_pre =" << q_pre << " q_dot_pre = " << q_dot_pre << std::endl ).flush();
00205 #endif
00206     calcDenavitHartenbergMat();
00207 
00208   }
00209 
00210   void setJacobian() {
00211     switch ( parent->type) {
00212     case JointType::revolute: {
00213       Jv = parent->zaxis;
00214       Jw = q_dot_pre*parent->zaxis;
00215       break;
00216     }
00217     case JointType::prismatic: {
00218       Jv = parent->zaxis;
00219       Jw(0) = 0.;   Jw(1) = 0.;   Jw(2) = 0.;
00220       break;
00221     }
00222     case JointType::sphericalWrist: {
00223       break;
00224     }
00225     }
00226 
00227   }
00228 
00229   void actuate( DataType pval, DataType vval) {
00230     q_pre = pval;
00231     calcDenavitHartenbergMat();
00232     q_dot_pre = vval;
00233     setJacobian();
00234   }
00235 
00236   void actuate( DataType xangle, DataType yangle, DataType zangle, DataType offset ) {
00237     if (parent->type == JointType::sphericalWrist) {
00238       radius = xangle;
00239       alpha = yangle;
00240       depth = zangle;
00241       theta = offset;
00242       q_pre = 0.;
00243       calcDenavitHartenbergMat();
00244     }
00245     else {
00246       ( mklog() << "WARNING: Parent Joint is type " << parent->type
00247                 << " not a Sperical wrist and cannot be actuated like one.\n" ).flush();
00248     }
00249   }
00250 
00251   void print(Scene<DataType,dim>& thisScene,  int level) {
00252     ( mklog() << std::string(level, '\t') << "Link " << id << std::endl
00253               << std::string(level, '\t') << "\t parent joint id " << parent->id << " " ).flush();
00254     if ( checkTag(parent->CFIdTag()) == 0 ) {
00255       ( mklog() << thisScene.GetPart(parent->CFIdTag())->Name() ).flush();
00256     }
00257     else {
00258       ( mklog() << "Assembly " << thisScene.GetAssembly(parent->CFIdTag())->Name() ).flush();
00259     }
00260     ( mklog() << "\t type " << parent->type
00261               << "\t::\t child joint id " << child->id << " " ).flush();
00262     if ( checkTag(child->CFIdTag()) == 0 ) {
00263       ( mklog() << thisScene.GetPart(child->CFIdTag())->Name() ).flush();
00264     }
00265     else {
00266       ( mklog() << "Assembly " << thisScene.GetAssembly(child->CFIdTag())->Name() ).flush();
00267     }
00268     ( mklog() << "\t type " << child->type << std::endl
00269               << std::string(level, '\t') << "\t radius " << radius << "\t alpha " << alpha
00270               << "\t depth " << depth << "\t theta " << theta << "\t q_pre " << q_pre
00271               << " q_dot_pre " << q_dot_pre << std::endl
00272               << std::string(level, '\t') << "\t DHMat " << DHMat << std::endl ).flush();
00273   }
00274 
00275   void setId(int v) { id = v; }
00276   void setQ_pre(DataType v) { q_pre = v; }
00277   void setQ_dot_pre(DataType v) { q_dot_pre = v; }
00278 
00279   virtual void Restart(std::ifstream& ifs, int& pos, double& t, double& dt) {
00280     ifs.seekg(pos);
00281 
00282 #ifdef CHECKRESTART_DEBUG_ON
00283     ( mklog() << " - Restarting Link " << id << std::endl ).flush();
00284 #endif
00285     ifs.read((char*)&origin(0),sizeof(DataType));
00286     ifs.read((char*)&origin(1),sizeof(DataType));
00287     ifs.read((char*)&origin(2),sizeof(DataType));
00288     ifs.read((char*)&xaxis(0),sizeof(DataType));
00289     ifs.read((char*)&xaxis(1),sizeof(DataType));
00290     ifs.read((char*)&xaxis(2),sizeof(DataType));
00291     ifs.read((char*)&yaxis(0),sizeof(DataType));
00292     ifs.read((char*)&yaxis(1),sizeof(DataType));
00293     ifs.read((char*)&yaxis(2),sizeof(DataType));
00294     ifs.read((char*)&zaxis(0),sizeof(DataType));
00295     ifs.read((char*)&zaxis(1),sizeof(DataType));
00296     ifs.read((char*)&zaxis(2),sizeof(DataType));
00297     ifs.read((char*)&l0i(0),sizeof(DataType));
00298     ifs.read((char*)&l0i(1),sizeof(DataType));
00299     ifs.read((char*)&l0i(2),sizeof(DataType));
00300 
00301     ifs.read((char*)&radius,sizeof(DataType));
00302     ifs.read((char*)&alpha,sizeof(DataType));
00303     ifs.read((char*)&depth,sizeof(DataType));
00304     ifs.read((char*)&theta,sizeof(DataType));
00305     ifs.read((char*)&arrangement,sizeof(int));
00306 
00307     ifs.read((char*)&DHMat(0,0),sizeof(DataType));
00308     ifs.read((char*)&DHMat(0,1),sizeof(DataType));
00309     ifs.read((char*)&DHMat(0,2),sizeof(DataType));
00310     ifs.read((char*)&DHMat(0,3),sizeof(DataType));
00311     ifs.read((char*)&DHMat(1,0),sizeof(DataType));
00312     ifs.read((char*)&DHMat(1,1),sizeof(DataType));
00313     ifs.read((char*)&DHMat(1,2),sizeof(DataType));
00314     ifs.read((char*)&DHMat(1,3),sizeof(DataType));
00315     ifs.read((char*)&DHMat(2,0),sizeof(DataType));
00316     ifs.read((char*)&DHMat(2,1),sizeof(DataType));
00317     ifs.read((char*)&DHMat(2,2),sizeof(DataType));
00318     ifs.read((char*)&DHMat(2,3),sizeof(DataType));
00319     ifs.read((char*)&DHMat(3,0),sizeof(DataType));
00320     ifs.read((char*)&DHMat(3,1),sizeof(DataType));
00321     ifs.read((char*)&DHMat(3,2),sizeof(DataType));
00322     ifs.read((char*)&DHMat(3,3),sizeof(DataType));
00323 
00324     ifs.read((char*)&Jv(0),sizeof(DataType));
00325     ifs.read((char*)&Jv(1),sizeof(DataType));
00326     ifs.read((char*)&Jv(2),sizeof(DataType));
00327     ifs.read((char*)&Jw(0),sizeof(DataType));
00328     ifs.read((char*)&Jw(1),sizeof(DataType));
00329     ifs.read((char*)&Jw(2),sizeof(DataType));
00330 #ifdef CHECKRESTART_DEBUG_ON
00331     ( mklog() << "    origin " << origin << " radius " << radius
00332               << " alpha " << alpha << " depth " << depth
00333               << " theta " << theta << " arrangement " << arrangement << std::endl ).flush();
00334 #endif
00335     pos = ifs.tellg();
00336     parent->Restart(ifs,pos,t,dt);
00337     pos = ifs.tellg();
00338     child->Restart(ifs,pos,t,dt);
00339     pos = ifs.tellg();
00340   }
00341 
00342   virtual void Checkpointing(std::ofstream& ofs) {
00343 #ifdef CHECKRESTART_DEBUG_ON
00344     ( mklog() << "\nCHECKPOINTING Link " << id << std::endl ).flush();
00345 #endif
00346     ofs.write((char*)&origin(0),sizeof(DataType));
00347     ofs.write((char*)&origin(1),sizeof(DataType));
00348     ofs.write((char*)&origin(2),sizeof(DataType));
00349     ofs.write((char*)&xaxis(0),sizeof(DataType));
00350     ofs.write((char*)&xaxis(1),sizeof(DataType));
00351     ofs.write((char*)&xaxis(2),sizeof(DataType));
00352     ofs.write((char*)&yaxis(0),sizeof(DataType));
00353     ofs.write((char*)&yaxis(1),sizeof(DataType));
00354     ofs.write((char*)&yaxis(2),sizeof(DataType));
00355     ofs.write((char*)&zaxis(0),sizeof(DataType));
00356     ofs.write((char*)&zaxis(1),sizeof(DataType));
00357     ofs.write((char*)&zaxis(2),sizeof(DataType));
00358     ofs.write((char*)&l0i(0),sizeof(DataType));
00359     ofs.write((char*)&l0i(1),sizeof(DataType));
00360     ofs.write((char*)&l0i(2),sizeof(DataType));
00361 
00362     ofs.write((char*)&radius,sizeof(DataType));
00363     ofs.write((char*)&alpha,sizeof(DataType));
00364     ofs.write((char*)&depth,sizeof(DataType));
00365     ofs.write((char*)&theta,sizeof(DataType));
00366     ofs.write((char*)&arrangement,sizeof(int));
00367 
00368     ofs.write((char*)&DHMat(0,0),sizeof(DataType));
00369     ofs.write((char*)&DHMat(0,1),sizeof(DataType));
00370     ofs.write((char*)&DHMat(0,2),sizeof(DataType));
00371     ofs.write((char*)&DHMat(0,3),sizeof(DataType));
00372     ofs.write((char*)&DHMat(1,0),sizeof(DataType));
00373     ofs.write((char*)&DHMat(1,1),sizeof(DataType));
00374     ofs.write((char*)&DHMat(1,2),sizeof(DataType));
00375     ofs.write((char*)&DHMat(1,3),sizeof(DataType));
00376     ofs.write((char*)&DHMat(2,0),sizeof(DataType));
00377     ofs.write((char*)&DHMat(2,1),sizeof(DataType));
00378     ofs.write((char*)&DHMat(2,2),sizeof(DataType));
00379     ofs.write((char*)&DHMat(2,3),sizeof(DataType));
00380     ofs.write((char*)&DHMat(3,0),sizeof(DataType));
00381     ofs.write((char*)&DHMat(3,1),sizeof(DataType));
00382     ofs.write((char*)&DHMat(3,2),sizeof(DataType));
00383     ofs.write((char*)&DHMat(3,3),sizeof(DataType));
00384 
00385     ofs.write((char*)&Jv(0),sizeof(DataType));
00386     ofs.write((char*)&Jv(1),sizeof(DataType));
00387     ofs.write((char*)&Jv(2),sizeof(DataType));
00388     ofs.write((char*)&Jw(0),sizeof(DataType));
00389     ofs.write((char*)&Jw(1),sizeof(DataType));
00390     ofs.write((char*)&Jw(2),sizeof(DataType));
00391 #ifdef CHECKRESTART_DEBUG_ON
00392     ( mklog() << "    origin " << origin << " radius " << radius
00393               << " alpha " << alpha << " depth " << depth
00394               << " theta " << theta << " arrangement " << arrangement << std::endl ).flush();
00395 #endif
00396     parent->Checkpointing(ofs);
00397     child->Checkpointing(ofs);
00398 
00399   }
00400 
00401   virtual void setChild(JointType * newChild) {
00402     child = newChild;
00403   }
00404   virtual JointType * getChild() {
00405     return child;
00406   }
00407   virtual DataType getTheta() { return theta; }
00408 
00409 
00410 protected:
00411   int id; 
00412   PType origin, xaxis, yaxis, zaxis, l0i;
00413   DataType radius_ref, alpha_ref, depth_ref, theta_ref, q_pre_ref,  q_dot_pre_ref;
00414   DataType radius, alpha, depth, theta, q_pre, q_dot_pre;
00415   JointType * parent, * child;
00416   int arrangement;
00417   MType DHMat;
00418 
00419   //Jacobian elements
00420   BPType Jv, Jw;
00421 };
00422 
00423 #endif // DH_LINK_H
Generated on Thu Jun 30 02:18:58 2016 for Motion - rigid body dynamics solver by 1.6.3