Main Page   Namespace List   Class Hierarchy   Compound List   File List   Namespace Members   Compound Members  

SIG_DynaMechsLink.cpp

00001 #include "SIGEL_Simulation/SIG_DynaMechsLink.h"
00002 
00003 #include "SIGEL_Tools/SIG_TypeConverter.h"
00004 #include "SIGEL_Tools/SIG_IO.h"
00005 
00006 #include <dm.h>
00007 #include <dmContactModel.hpp>
00008 #include <dmMDHLink.hpp>
00009 
00010 #include <cmath>
00011 
00012 using namespace SIGEL_Tools;
00013 
00014 namespace SIGEL_Simulation
00015 {
00016 
00017   SIG_DynaMechsLink::SIG_DynaMechsLink( int dynaMechsLinkNumber,
00018                                         SIGEL_Robot::SIG_Link const *link,
00019                                         dmRigidBody *dynaMechsLink,
00020                                         double screwD,
00021                                         double screwTheta )
00022     : dynaMechsLinkNumber( dynaMechsLinkNumber ),
00023       link( link ),
00024       dynaMechsLink( dynaMechsLink ),
00025       transformation( 4, 4 ),
00026       screwD( screwD ),
00027       screwTheta( screwTheta ),
00028       screwLink( 0 )
00029   {
00030     for (int j=1; j<=3; j++)
00031       transformation( 4, j ) = 0;
00032 
00033     transformation( 4, 4 ) = 1;
00034 
00035     if ((screwD != 0 ) || (screwTheta != 0))
00036       screwLink = new dmZScrewTxLink( screwD, screwTheta );
00037 
00038     SIGEL_Robot::SIG_Geometry *geometry = const_cast< SIGEL_Robot::SIG_Geometry* >(link->getGeometry());
00039 
00040     SIGEL_Robot::SIG_Material const *material = link->getMaterial();
00041 
00042     double density = material->getDensity();
00043 
00044     SIGEL_Robot::SIG_Mirtich inertiaCalculator( geometry, link->getName() );
00045 
00046     DL_Scalar mass;
00047     DL_vector centerOfMass;
00048     DL_matrix inertiaTensor;
00049 
00050     inertiaCalculator.computePhysics( density,
00051                                       mass,
00052                                       centerOfMass,
00053                                       inertiaTensor );
00054 
00055 #ifdef SIG_DEBUG
00056     SIGEL_Tools::SIG_IO::cerr << "Density: " << density << "\n";
00057     SIGEL_Tools::SIG_IO::cerr << "Mass: " << mass << "\n";
00058     SIGEL_Tools::SIG_IO::cerr << "Center of mass:";
00059     for (int i=0; i<3; i++)
00060       SIGEL_Tools::SIG_IO::cerr << " " << centerOfMass.get( i );
00061     SIGEL_Tools::SIG_IO::cerr << "\n";
00062     SIGEL_Tools::SIG_IO::cerr << "Inertia tensor:\n";
00063     for (int i=0; i<3; i++)
00064       {
00065         for (int j=0; j<3; j++)
00066           SIGEL_Tools::SIG_IO::cerr << inertiaTensor.get( i, j ) << " ";
00067             SIGEL_Tools::SIG_IO::cerr << "\n";
00068       };
00069 #endif
00070 
00071     CartesianTensor dynaMechsInertiaTensor;
00072     CartesianVector dynaMechsCenterOfMass;
00073 
00074     SIG_TypeConverter::toRotationMatrix( inertiaTensor, dynaMechsInertiaTensor );
00075     SIG_TypeConverter::toCartesianVector( centerOfMass, dynaMechsCenterOfMass );
00076 
00077     dynaMechsLink->setInertiaParameters( mass,
00078                                          dynaMechsInertiaTensor,
00079                                          dynaMechsCenterOfMass );
00080 
00081     dmContactModel *contactModel = new dmContactModel();
00082 
00083     QVector< DL_vector > vertices = geometry->getVertices();
00084 
00085     int noOfContactPoints = vertices.size();
00086 
00087     CartesianVector contactPoints[ noOfContactPoints ];
00088 
00089     for (int i=0; i<vertices.size(); i++)
00090       {
00091         DL_vector &vertex = *(vertices[i]);
00092 
00093         SIG_TypeConverter::toCartesianVector( vertex, contactPoints[i] );
00094       };
00095 
00096     contactModel->setContactPoints( noOfContactPoints,
00097                                     contactPoints );
00098 
00099     dynaMechsLink->addForce( contactModel );
00100   };
00101 
00102   void SIG_DynaMechsLink::forwardKinematics( SIG_DynaMechsLink *caller )
00103   {
00104     if (!caller)
00105       {
00106         QArray< double > q( 7 );
00107 
00108         QArray< double > qd( 6 );
00109 
00110         dynaMechsLink->getState( q.data(), qd.data() );
00111 
00112         transformation.SubMatrix( 1, 3, 4, 4 ) = SIG_TypeConverter::toColumnVector( q.data() + 4 );
00113 
00114         normalizeQuat( q.data() );
00115 
00116         RotationMatrix R;
00117 
00118         buildRotMat( q.data(), R );
00119 
00120         transformation.SubMatrix( 1, 3, 1, 3 ) = SIG_TypeConverter::toMatrix( R );
00121       }
00122     else
00123       {
00124         dmMDHLink *dynaMechsMDHLink = static_cast< dmMDHLink* >(dynaMechsLink);
00125 
00126         double a;
00127         double alpha;
00128         double d;
00129         double theta;
00130 
00131         dynaMechsMDHLink->getMDHParameters( &a,
00132                                             &alpha,
00133                                             &d,
00134                                             &theta );
00135 
00136         transformation = caller->transformation;
00137 
00138         if (screwLink)
00139           transformation *=   buildTranslationMatrix( 0, 0, screwD )
00140                             * buildZRotationMatrix( screwTheta );
00141 
00142         transformation *=   buildXRotationMatrix( alpha )
00143                           * buildTranslationMatrix( a, 0, d )
00144                           * buildZRotationMatrix( theta );
00145 
00146 #ifdef SIG_DEBUG
00147         SIGEL_Tools::SIG_IO::cerr << "theta: "
00148                                   << theta
00149                                   << ", d: "
00150                                   << d
00151                                   << "\n";
00152 
00153         SIGEL_Tools::SIG_IO::cerr << "New Transformation:\n";
00154         for (int i=1; i<=4; i++)
00155           {
00156             for (int j=1; j<=4; j++)
00157               SIGEL_Tools::SIG_IO::cerr << transformation( i, j ) << " ";
00158             SIGEL_Tools::SIG_IO::cerr << "\n";
00159           };
00160 #endif
00161 
00162       };
00163 
00164     SIG_DynaMechsLink *actSuccessor = successors.first();
00165 
00166     while (actSuccessor)
00167       {
00168         actSuccessor->forwardKinematics( this );
00169 
00170         actSuccessor = successors.next();
00171       };
00172   };
00173 
00174   NEWMAT::Matrix SIG_DynaMechsLink::buildXRotationMatrix( double angle )
00175   {
00176     NEWMAT::Matrix transformation(4, 4);
00177 
00178     double sinAngle = std::sin( angle );
00179     double cosAngle = std::cos( angle );
00180 
00181     transformation << 1 << 0 << 0 << 0
00182                    << 0 << cosAngle << - sinAngle << 0
00183                    << 0 << sinAngle << cosAngle << 0
00184                    << 0 << 0 << 0 << 1;
00185 
00186 #ifdef SIG_DEBUG
00187     SIGEL_Tools::SIG_IO::cerr << "X-Angle: " << angle << "\n"
00188                               << "Matrix:\n";
00189     for (int i=1; i<=4; i++)
00190       {
00191         for (int j=1; j<=4; j++)
00192           SIGEL_Tools::SIG_IO::cerr << transformation( i, j ) << " ";
00193         SIGEL_Tools::SIG_IO::cerr << "\n";
00194       };
00195 #endif
00196 
00197     return transformation;
00198   };
00199 
00200   NEWMAT::Matrix SIG_DynaMechsLink::buildZRotationMatrix( double angle )
00201   {
00202     NEWMAT::Matrix transformation(4, 4);
00203 
00204     double sinAngle = std::sin( angle );
00205     double cosAngle = std::cos( angle );
00206 
00207     transformation << cosAngle << - sinAngle << 0 << 0
00208                    << sinAngle << cosAngle << 0 << 0
00209                    << 0 << 0 << 1 << 0
00210                    << 0 << 0 << 0 << 1;
00211 
00212 #ifdef SIG_DEBUG
00213     SIGEL_Tools::SIG_IO::cerr << "Z-Angle: " << angle << "\n"
00214                               << "Matrix:\n";
00215     for (int i=1; i<=4; i++)
00216       {
00217         for (int j=1; j<=4; j++)
00218           SIGEL_Tools::SIG_IO::cerr << transformation( i, j ) << " ";
00219         SIGEL_Tools::SIG_IO::cerr << "\n";
00220       };
00221 #endif
00222 
00223     return transformation;
00224   };
00225 
00226   NEWMAT::Matrix SIG_DynaMechsLink::buildTranslationMatrix( double x,
00227                                                             double y,
00228                                                             double z )
00229   {
00230     NEWMAT::Matrix transformation(4, 4);
00231 
00232     transformation << 1 << 0 << 0 << x
00233                    << 0 << 1 << 0 << y
00234                    << 0 << 0 << 1 << z
00235                    << 0 << 0 << 0 << 1;
00236 
00237 #ifdef SIG_DEBUG
00238     SIGEL_Tools::SIG_IO::cerr << "Move: " << x << " " << y << " " << z << "\n"
00239                               << "Matrix:\n";
00240     for (int i=1; i<=4; i++)
00241       {
00242         for (int j=1; j<=4; j++)
00243           SIGEL_Tools::SIG_IO::cerr << transformation( i, j ) << " ";
00244         SIGEL_Tools::SIG_IO::cerr << "\n";
00245       };
00246 #endif
00247 
00248     return transformation;
00249   };
00250 }

Generated at Mon Sep 3 01:32:19 2001 for PG 368 - SIGEL by doxygen1.2.3 written by Dimitri van Heesch, © 1997-2000