File: calculateTransformations.c1 /* 2 * Academic License - for use in teaching, academic research, and meeting 3 * course requirements at degree granting institutions only. Not for 4 * government, commercial, or other organizational use. 5 * 6 * calculateTransformations.c 7 * 8 * Code generation for function 'calculateTransformations' 9 * 10 */ 11 12 /* Include files */ 13 #include "rt_nonfinite.h" 14 #include "calculateTransformations.h" 15 16 /* Function Declarations */ 17 static double rt_atan2d_snf(double u0, double u1); 18 19 /* Function Definitions */ 20 static double rt_atan2d_snf(double u0, double u1) 21 { 22 double y; 23 int b_u0; 24 int b_u1; 25 if (rtIsNaN(u0) || rtIsNaN(u1)) { 26 y = rtNaN; 27 } else if (rtIsInf(u0) && rtIsInf(u1)) { 28 if (u0 > 0.0) { 29 b_u0 = 1; 30 } else { 31 b_u0 = -1; 32 } 33 34 if (u1 > 0.0) { 35 b_u1 = 1; 36 } else { 37 b_u1 = -1; 38 } 39 40 y = atan2(b_u0, b_u1); 41 } else if (u1 == 0.0) { 42 if (u0 > 0.0) { 43 y = RT_PI / 2.0; 44 } else if (u0 < 0.0) { 45 y = -(RT_PI / 2.0); 46 } else { 47 y = 0.0; 48 } 49 } else { 50 y = atan2(u0, u1); 51 } 52 53 return y; 54 } 55 56 void calculateTransformations(const double q1[5], const double q2[5], const 57 double eta_marker[6], const double g_marker2front[16], double 58 controlCenterLink, double g_If[16], double R_If[9], double eta_f[6], double 59 g_Ib[16], double R_Ib[9], double eta_b[6], double q[8], double g_joints_home 60 [144], double g_moduleCM[144], double g_thrusters[112]) 61 { 62 double cphi; 63 double sphi; 64 double cth; 65 double sth; 66 double cpsi; 67 double spsi; 68 double b_cpsi[16]; 69 int k; 70 static const signed char iv0[4] = { 0, 0, 0, 1 }; 71 72 int i0; 73 int i1; 74 double g_joints[128]; 75 double g_center2back[16]; 76 int i; 77 boolean_T b_bool; 78 static const char joint_ax[8] = { 'Z', 'Y', 'Z', 'Y', 'Z', 'Y', 'Z', 'Y' }; 79 80 double cos_angle; 81 double sin_angle; 82 double g_front2back[16]; 83 static const signed char iv1[3] = { 0, 1, 0 }; 84 85 static const double joint_tr[27] = { 0.62, 0.1, 0.59, 0.1, 0.8, 0.1, 0.59, 0.1, 86 0.37, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 87 0.0, 0.0, 0.0, 0.0 }; 88 89 signed char I[9]; 90 static const signed char iv2[3] = { 0, 0, 1 }; 91 92 static const double g_ne[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 93 0.0, 1.0, 0.0, 0.37, 0.0, 0.0, 1.0 }; 94 95 static const signed char iv3[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; 96 97 static const double module_cm[27] = { 0.31, 0.05, 0.295, 0.05, 0.4, 0.05, 98 0.295, 0.05, 0.185, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; 100 101 static const char thruster_ax[7] = { 'Z', 'Y', 'Z', 'X', 'X', 'Y', 'Z' }; 102 103 static const double rotZ[9] = { 6.123233995736766E-17, 1.0, 0.0, -1.0, 104 6.123233995736766E-17, 0.0, 0.0, 0.0, 1.0 }; 105 106 static const double thruster_pos[21] = { 0.24, 0.35000000000000003, 0.24, 0.53, 107 0.53, 0.24, 0.35000000000000003, 0.0, 0.0, 0.0, 0.15, -0.15, 0.0, 0.0, 0.0, 108 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; 109 110 static const double rotY[9] = { 6.123233995736766E-17, 0.0, 1.0, 0.0, 1.0, 0.0, 111 -1.0, 0.0, 6.123233995736766E-17 }; 112 113 static const signed char iv4[7] = { 3, 3, 5, 5, 5, 7, 7 }; 114 115 double b_g_joints_home[9]; 116 double c_g_joints_home[3]; 117 118 /* [EELY] Specific code for Eely */ 119 /* Measured: Position and Euler angles for the markers, eta_marker */ 120 /* R = Rzyx(phi,theta,psi) computes the Euler angle */ 121 /* rotation matrix R in SO(3) using the zyx convention */ 122 /* */ 123 /* Author: Thor I. Fossen */ 124 /* Date: 14th June 2001 */ 125 /* Revisions: */ 126 /* ________________________________________________________________ */ 127 /* */ 128 /* MSS GNC is a Matlab toolbox for guidance, navigation and control. */ 129 /* The toolbox is part of the Marine Systems Simulator (MSS). */ 130 /* */ 131 /* Copyright (C) 2008 Thor I. Fossen and Tristan Perez */ 132 /* */ 133 /* This program is free software: you can redistribute it and/or modify */ 134 /* it under the terms of the GNU General Public License as published by */ 135 /* the Free Software Foundation, either version 3 of the License, or */ 136 /* (at your option) any later version. */ 137 /* */ 138 /* This program is distributed in the hope that it will be useful, but */ 139 /* WITHOUT ANY WARRANTY; without even the implied warranty of */ 140 /* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the */ 141 /* GNU General Public License for more details. */ 142 /* */ 143 /* You should have received a copy of the GNU General Public License */ 144 /* along with this program. If not, see <http://www.gnu.org/licenses/>. */ 145 /* */ 146 /* E-mail: contact@marinecontrol.org */ 147 /* URL: <http://www.marinecontrol.org> */ 148 cphi = cos(eta_marker[3]); 149 sphi = sin(eta_marker[3]); 150 cth = cos(eta_marker[4]); 151 sth = sin(eta_marker[4]); 152 cpsi = cos(eta_marker[5]); 153 spsi = sin(eta_marker[5]); 154 b_cpsi[0] = cpsi * cth; 155 b_cpsi[4] = -spsi * cphi + cpsi * sth * sphi; 156 b_cpsi[8] = spsi * sphi + cpsi * cphi * sth; 157 b_cpsi[1] = spsi * cth; 158 b_cpsi[5] = cpsi * cphi + sphi * sth * spsi; 159 b_cpsi[9] = -cpsi * sphi + sth * spsi * cphi; 160 b_cpsi[2] = -sth; 161 b_cpsi[6] = cth * sphi; 162 b_cpsi[10] = cth * cphi; 163 for (k = 0; k < 3; k++) { 164 b_cpsi[12 + k] = eta_marker[k]; 165 } 166 167 for (k = 0; k < 4; k++) { 168 b_cpsi[3 + (k << 2)] = iv0[k]; 169 } 170 171 for (k = 0; k < 4; k++) { 172 for (i0 = 0; i0 < 4; i0++) { 173 g_If[k + (i0 << 2)] = 0.0; 174 for (i1 = 0; i1 < 4; i1++) { 175 g_If[k + (i0 << 2)] += b_cpsi[k + (i1 << 2)] * g_marker2front[i1 + (i0 << 176 2)]; 177 } 178 } 179 } 180 181 for (k = 0; k < 3; k++) { 182 for (i0 = 0; i0 < 3; i0++) { 183 R_If[i0 + 3 * k] = g_If[i0 + (k << 2)]; 184 } 185 186 eta_f[k] = g_If[12 + k]; 187 } 188 189 eta_f[3] = rt_atan2d_snf(g_If[6], g_If[10]); 190 eta_f[4] = -asin(g_If[2]); 191 eta_f[5] = rt_atan2d_snf(g_If[1], g_If[0]); 192 193 /* Code to convert the joint angle inputs q1 and q2 from the Eely robot */ 194 /* to the definition used in this code by reorganizing the vector elements. */ 195 q[0] = q2[3] * 3.1415926535897931 / 180.0; 196 q[1] = q1[3] * 3.1415926535897931 / 180.0; 197 q[2] = q2[2] * 3.1415926535897931 / 180.0; 198 q[3] = q1[2] * 3.1415926535897931 / 180.0; 199 q[4] = q2[1] * 3.1415926535897931 / 180.0; 200 q[5] = q1[1] * 3.1415926535897931 / 180.0; 201 q[6] = q2[4] * 3.1415926535897931 / 180.0; 202 q[7] = q1[4] * 3.1415926535897931 / 180.0; 203 204 /* % TRANSFORMATIONS */ 205 /* Calculate the sequential transformations from base frame to the joint */ 206 /* frames */ 207 /* Calculates the transformation matrices from */ 208 /* the USM base frame, F_b = F_0, to the joint frames F_i (i=1..n) */ 209 /* From the tail module and forward */ 210 /* JOINT FRAMES */ 211 memset(&g_joints[0], 0, sizeof(double) << 7); 212 213 /* size = num_joint+1 to include F_e */ 214 for (i = 0; i < 8; i++) { 215 b_bool = false; 216 if (joint_ax[i] != 'Y') { 217 } else { 218 b_bool = true; 219 } 220 221 if (b_bool) { 222 cos_angle = cos(q[i]); 223 sin_angle = sin(q[i]); 224 g_front2back[0] = cos_angle; 225 g_front2back[4] = 0.0; 226 g_front2back[8] = sin_angle; 227 g_front2back[2] = -sin_angle; 228 g_front2back[6] = 0.0; 229 g_front2back[10] = cos_angle; 230 for (k = 0; k < 3; k++) { 231 g_front2back[1 + (k << 2)] = iv1[k]; 232 g_front2back[12 + k] = joint_tr[i + 9 * k]; 233 } 234 235 for (k = 0; k < 4; k++) { 236 g_front2back[3 + (k << 2)] = iv0[k]; 237 } 238 239 for (k = 0; k < 9; k++) { 240 I[k] = 0; 241 } 242 243 for (k = 0; k < 3; k++) { 244 I[k + 3 * k] = 1; 245 } 246 247 for (k = 0; k < 3; k++) { 248 for (i0 = 0; i0 < 3; i0++) { 249 g_center2back[i0 + (k << 2)] = I[i0 + 3 * k]; 250 } 251 252 g_center2back[12 + k] = joint_tr[i + 9 * k]; 253 } 254 255 for (k = 0; k < 4; k++) { 256 g_center2back[3 + (k << 2)] = iv0[k]; 257 } 258 } else { 259 b_bool = false; 260 if (joint_ax[i] != 'Z') { 261 } else { 262 b_bool = true; 263 } 264 265 if (b_bool) { 266 cos_angle = cos(q[i]); 267 sin_angle = sin(q[i]); 268 g_front2back[0] = cos_angle; 269 g_front2back[4] = -sin_angle; 270 g_front2back[8] = 0.0; 271 g_front2back[1] = sin_angle; 272 g_front2back[5] = cos_angle; 273 g_front2back[9] = 0.0; 274 for (k = 0; k < 3; k++) { 275 g_front2back[2 + (k << 2)] = iv2[k]; 276 g_front2back[12 + k] = joint_tr[i + 9 * k]; 277 } 278 279 for (k = 0; k < 4; k++) { 280 g_front2back[3 + (k << 2)] = iv0[k]; 281 } 282 283 for (k = 0; k < 9; k++) { 284 I[k] = 0; 285 } 286 287 for (k = 0; k < 3; k++) { 288 I[k + 3 * k] = 1; 289 } 290 291 for (k = 0; k < 3; k++) { 292 for (i0 = 0; i0 < 3; i0++) { 293 g_center2back[i0 + (k << 2)] = I[i0 + 3 * k]; 294 } 295 296 g_center2back[12 + k] = joint_tr[i + 9 * k]; 297 } 298 299 for (k = 0; k < 4; k++) { 300 g_center2back[3 + (k << 2)] = iv0[k]; 301 } 302 } else { 303 memset(&g_front2back[0], 0, sizeof(double) << 4); 304 for (k = 0; k < 4; k++) { 305 g_front2back[k + (k << 2)] = 1.0; 306 } 307 308 memset(&g_center2back[0], 0, sizeof(double) << 4); 309 for (k = 0; k < 4; k++) { 310 g_center2back[k + (k << 2)] = 1.0; 311 } 312 } 313 } 314 315 if (1 + i == 1) { 316 for (k = 0; k < 4; k++) { 317 for (i0 = 0; i0 < 4; i0++) { 318 g_joints[(i0 + (k << 2)) + (i << 4)] = g_front2back[i0 + (k << 2)]; 319 g_joints_home[(i0 + (k << 2)) + (i << 4)] = g_center2back[i0 + (k << 2)]; 320 } 321 } 322 } else { 323 for (k = 0; k < 4; k++) { 324 for (i0 = 0; i0 < 4; i0++) { 325 b_cpsi[k + (i0 << 2)] = 0.0; 326 for (i1 = 0; i1 < 4; i1++) { 327 b_cpsi[k + (i0 << 2)] += g_joints[(k + (i1 << 2)) + ((i - 1) << 4)] * 328 g_front2back[i1 + (i0 << 2)]; 329 } 330 } 331 } 332 333 for (k = 0; k < 4; k++) { 334 for (i0 = 0; i0 < 4; i0++) { 335 g_joints[(i0 + (k << 2)) + (i << 4)] = b_cpsi[i0 + (k << 2)]; 336 } 337 } 338 339 for (k = 0; k < 4; k++) { 340 for (i0 = 0; i0 < 4; i0++) { 341 g_joints_home[(k + (i0 << 2)) + (i << 4)] = 0.0; 342 for (i1 = 0; i1 < 4; i1++) { 343 g_joints_home[(k + (i0 << 2)) + (i << 4)] += g_joints[(k + (i1 << 2)) 344 + ((i - 1) << 4)] * g_center2back[i1 + (i0 << 2)]; 345 } 346 } 347 } 348 } 349 } 350 351 for (k = 0; k < 4; k++) { 352 for (i0 = 0; i0 < 4; i0++) { 353 g_joints_home[128 + (k + (i0 << 2))] = 0.0; 354 for (i1 = 0; i1 < 4; i1++) { 355 g_joints_home[128 + (k + (i0 << 2))] += g_joints[112 + (k + (i1 << 2))] * 356 g_ne[i1 + (i0 << 2)]; 357 } 358 } 359 } 360 361 /* Calculate transformations for the CM of each module */ 362 /* Calculating the transformations from the USM base frame, F_b = F_0, */ 363 /* to the MODULE CENTER OF MASS FRAMES, F_CM_i */ 364 /* From the tail module and forward */ 365 for (i = 0; i < 9; i++) { 366 for (k = 0; k < 3; k++) { 367 for (i0 = 0; i0 < 3; i0++) { 368 g_front2back[i0 + (k << 2)] = iv3[i0 + 3 * k]; 369 } 370 371 g_front2back[12 + k] = module_cm[i + 9 * k]; 372 } 373 374 for (k = 0; k < 4; k++) { 375 g_front2back[3 + (k << 2)] = iv0[k]; 376 } 377 378 if (1 + i == 1) { 379 for (k = 0; k < 4; k++) { 380 for (i0 = 0; i0 < 4; i0++) { 381 g_moduleCM[(i0 + (k << 2)) + (i << 4)] = g_front2back[i0 + (k << 2)]; 382 } 383 } 384 } else { 385 for (k = 0; k < 4; k++) { 386 for (i0 = 0; i0 < 4; i0++) { 387 g_moduleCM[(k + (i0 << 2)) + (i << 4)] = 0.0; 388 for (i1 = 0; i1 < 4; i1++) { 389 g_moduleCM[(k + (i0 << 2)) + (i << 4)] += g_joints[(k + (i1 << 2)) + 390 ((i - 1) << 4)] * g_front2back[i1 + (i0 << 2)]; 391 } 392 } 393 } 394 } 395 } 396 397 /* Calculate transformations for the thrusters */ 398 /* Calculating the transformations from the back frame to the thruster */ 399 /* frames */ 400 /* From the tail module and forward */ 401 /* THRUSTER FRAMES */ 402 for (i = 0; i < 7; i++) { 403 b_bool = false; 404 if (thruster_ax[i] != 'Y') { 405 } else { 406 b_bool = true; 407 } 408 409 if (b_bool) { 410 k = 0; 411 } else { 412 b_bool = false; 413 if (thruster_ax[i] != 'Z') { 414 } else { 415 b_bool = true; 416 } 417 418 if (b_bool) { 419 k = 1; 420 } else { 421 k = -1; 422 } 423 } 424 425 switch (k) { 426 case 0: 427 for (k = 0; k < 3; k++) { 428 for (i0 = 0; i0 < 3; i0++) { 429 g_front2back[i0 + (k << 2)] = rotZ[i0 + 3 * k]; 430 } 431 432 g_front2back[12 + k] = thruster_pos[i + 7 * k]; 433 } 434 435 for (k = 0; k < 4; k++) { 436 g_front2back[3 + (k << 2)] = iv0[k]; 437 } 438 break; 439 440 case 1: 441 for (k = 0; k < 3; k++) { 442 for (i0 = 0; i0 < 3; i0++) { 443 g_front2back[i0 + (k << 2)] = rotY[i0 + 3 * k]; 444 } 445 446 g_front2back[12 + k] = thruster_pos[i + 7 * k]; 447 } 448 449 for (k = 0; k < 4; k++) { 450 g_front2back[3 + (k << 2)] = iv0[k]; 451 } 452 break; 453 454 default: 455 memset(&g_front2back[0], 0, sizeof(double) << 4); 456 for (k = 0; k < 4; k++) { 457 g_front2back[k + (k << 2)] = 1.0; 458 } 459 break; 460 } 461 462 for (k = 0; k < 4; k++) { 463 for (i0 = 0; i0 < 4; i0++) { 464 g_center2back[i0 + (k << 2)] = g_joints[(i0 + (k << 2)) + ((iv4[i] - 2) << 465 4)]; 466 } 467 } 468 469 for (k = 0; k < 4; k++) { 470 for (i0 = 0; i0 < 4; i0++) { 471 g_thrusters[(k + (i0 << 2)) + (i << 4)] = 0.0; 472 for (i1 = 0; i1 < 4; i1++) { 473 g_thrusters[(k + (i0 << 2)) + (i << 4)] += g_center2back[k + (i1 << 2)] 474 * g_front2back[i1 + (i0 << 2)]; 475 } 476 } 477 } 478 } 479 480 /* Calculate the transformation from the front end to the back end */ 481 for (k = 0; k < 3; k++) { 482 for (i0 = 0; i0 < 3; i0++) { 483 b_g_joints_home[i0 + 3 * k] = -g_joints_home[128 + (k + (i0 << 2))]; 484 } 485 } 486 487 for (k = 0; k < 3; k++) { 488 c_g_joints_home[k] = 0.0; 489 for (i0 = 0; i0 < 3; i0++) { 490 g_front2back[i0 + (k << 2)] = g_joints_home[128 + (k + (i0 << 2))]; 491 c_g_joints_home[k] += b_g_joints_home[k + 3 * i0] * g_joints_home[140 + i0]; 492 } 493 494 g_front2back[12 + k] = c_g_joints_home[k]; 495 } 496 497 for (k = 0; k < 4; k++) { 498 g_front2back[3 + (k << 2)] = iv0[k]; 499 } 500 501 /* Transformation from inertial frame to base frame */ 502 if (controlCenterLink != 0.0) { 503 /* Calculate the transformation from the back end to the center module */ 504 /* (module number 5) */ 505 for (k = 0; k < 3; k++) { 506 for (i0 = 0; i0 < 3; i0++) { 507 b_g_joints_home[i0 + 3 * k] = -g_moduleCM[64 + (k + (i0 << 2))]; 508 } 509 } 510 511 for (k = 0; k < 3; k++) { 512 c_g_joints_home[k] = 0.0; 513 for (i0 = 0; i0 < 3; i0++) { 514 g_center2back[i0 + (k << 2)] = g_moduleCM[64 + (k + (i0 << 2))]; 515 c_g_joints_home[k] += b_g_joints_home[k + 3 * i0] * g_moduleCM[76 + i0]; 516 } 517 518 g_center2back[12 + k] = c_g_joints_home[k]; 519 } 520 521 for (k = 0; k < 4; k++) { 522 g_center2back[3 + (k << 2)] = iv0[k]; 523 for (i0 = 0; i0 < 4; i0++) { 524 b_cpsi[k + (i0 << 2)] = 0.0; 525 for (i1 = 0; i1 < 4; i1++) { 526 b_cpsi[k + (i0 << 2)] += g_If[k + (i1 << 2)] * g_front2back[i1 + (i0 << 527 2)]; 528 } 529 } 530 531 for (i0 = 0; i0 < 4; i0++) { 532 g_Ib[k + (i0 << 2)] = 0.0; 533 for (i1 = 0; i1 < 4; i1++) { 534 g_Ib[k + (i0 << 2)] += b_cpsi[k + (i1 << 2)] * g_moduleCM[64 + (i1 + 535 (i0 << 2))]; 536 } 537 } 538 } 539 540 for (i = 0; i < 7; i++) { 541 for (k = 0; k < 4; k++) { 542 for (i0 = 0; i0 < 4; i0++) { 543 b_cpsi[k + (i0 << 2)] = 0.0; 544 for (i1 = 0; i1 < 4; i1++) { 545 b_cpsi[k + (i0 << 2)] += g_center2back[k + (i1 << 2)] * g_thrusters 546 [(i1 + (i0 << 2)) + (i << 4)]; 547 } 548 } 549 } 550 551 for (k = 0; k < 4; k++) { 552 for (i0 = 0; i0 < 4; i0++) { 553 g_thrusters[(i0 + (k << 2)) + (i << 4)] = b_cpsi[i0 + (k << 2)]; 554 } 555 } 556 } 557 558 for (i = 0; i < 9; i++) { 559 for (k = 0; k < 4; k++) { 560 for (i0 = 0; i0 < 4; i0++) { 561 b_cpsi[k + (i0 << 2)] = 0.0; 562 for (i1 = 0; i1 < 4; i1++) { 563 b_cpsi[k + (i0 << 2)] += g_center2back[k + (i1 << 2)] * g_moduleCM 564 [(i1 + (i0 << 2)) + (i << 4)]; 565 } 566 } 567 } 568 569 for (k = 0; k < 4; k++) { 570 for (i0 = 0; i0 < 4; i0++) { 571 g_moduleCM[(i0 + (k << 2)) + (i << 4)] = b_cpsi[i0 + (k << 2)]; 572 } 573 } 574 } 575 } else { 576 for (k = 0; k < 4; k++) { 577 for (i0 = 0; i0 < 4; i0++) { 578 g_Ib[k + (i0 << 2)] = 0.0; 579 for (i1 = 0; i1 < 4; i1++) { 580 g_Ib[k + (i0 << 2)] += g_If[k + (i1 << 2)] * g_front2back[i1 + (i0 << 581 2)]; 582 } 583 } 584 } 585 } 586 587 for (k = 0; k < 3; k++) { 588 for (i0 = 0; i0 < 3; i0++) { 589 R_Ib[i0 + 3 * k] = g_Ib[i0 + (k << 2)]; 590 } 591 592 eta_b[k] = g_Ib[12 + k]; 593 } 594 595 eta_b[3] = rt_atan2d_snf(g_Ib[6], g_Ib[10]); 596 eta_b[4] = -asin(g_Ib[2]); 597 eta_b[5] = rt_atan2d_snf(g_Ib[1], g_Ib[0]); 598 } 599 600 /* End of code generation (calculateTransformations.c) */ 601 |