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 }