1 /* 2 * PURPOSE: 3 * Class representing an articulated rigid body. Stores the body's 4 * current state, allows forces and torques to be set, handles 5 * timestepping and implements Featherstone's algorithm. 6 * 7 * COPYRIGHT: 8 * Copyright (C) Stephen Thompson, <stephen (at) solarflare.org.uk>, 2011-2013 9 * Portions written By Erwin Coumans: replacing Eigen math library by Bullet LinearMath and a dedicated 6x6 matrix inverse (solveImatrix) 10 11 This software is provided 'as-is', without any express or implied warranty. 12 In no event will the authors be held liable for any damages arising from the use of this software. 13 Permission is granted to anyone to use this software for any purpose, 14 including commercial applications, and to alter it and redistribute it freely, 15 subject to the following restrictions: 16 17 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. 18 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 19 3. This notice may not be removed or altered from any source distribution. 20 21 */ 22 23 24 #include "btMultiBody.h" 25 #include "btMultiBodyLink.h" 26 #include "btMultiBodyLinkCollider.h" 27 #include "btMultiBodyJointFeedback.h" 28 #include "LinearMath/btTransformUtil.h" 29 #include "LinearMath/btSerializer.h" 30 // #define INCLUDE_GYRO_TERM 31 32 ///todo: determine if we need these options. If so, make a proper API, otherwise delete those globals 33 bool gJointFeedbackInWorldSpace = false; 34 bool gJointFeedbackInJointFrame = false; 35 36 namespace { 37 const btScalar SLEEP_EPSILON = btScalar(0.05); // this is a squared velocity (m^2 s^-2) 38 const btScalar SLEEP_TIMEOUT = btScalar(2); // in seconds 39 } 40 41 namespace { 42 void SpatialTransform(const btMatrix3x3 &rotation_matrix, // rotates vectors in 'from' frame to vectors in 'to' frame 43 const btVector3 &displacement, // vector from origin of 'from' frame to origin of 'to' frame, in 'to' coordinates 44 const btVector3 &top_in, // top part of input vector 45 const btVector3 &bottom_in, // bottom part of input vector 46 btVector3 &top_out, // top part of output vector 47 btVector3 &bottom_out) // bottom part of output vector 48 { 49 top_out = rotation_matrix * top_in; 50 bottom_out = -displacement.cross(top_out) + rotation_matrix * bottom_in; 51 } 52 53 void InverseSpatialTransform(const btMatrix3x3 &rotation_matrix, 54 const btVector3 &displacement, 55 const btVector3 &top_in, 56 const btVector3 &bottom_in, 57 btVector3 &top_out, 58 btVector3 &bottom_out) 59 { 60 top_out = rotation_matrix.transpose() * top_in; 61 bottom_out = rotation_matrix.transpose() * (bottom_in + displacement.cross(top_in)); 62 } 63 64 btScalar SpatialDotProduct(const btVector3 &a_top, 65 const btVector3 &a_bottom, 66 const btVector3 &b_top, 67 const btVector3 &b_bottom) 68 { 69 return a_bottom.dot(b_top) + a_top.dot(b_bottom); 70 } 71 72 void SpatialCrossProduct(const btVector3 &a_top, 73 const btVector3 &a_bottom, 74 const btVector3 &b_top, 75 const btVector3 &b_bottom, 76 btVector3 &top_out, 77 btVector3 &bottom_out) 78 { 79 top_out = a_top.cross(b_top); 80 bottom_out = a_bottom.cross(b_top) + a_top.cross(b_bottom); 81 } 82 } 83 84 85 // 86 // Implementation of class btMultiBody 87 // 88 89 btMultiBody::btMultiBody(int n_links, 90 btScalar mass, 91 const btVector3 &inertia, 92 bool fixedBase, 93 bool canSleep, 94 bool multiDof) 95 : 96 m_baseCollider(0), 97 m_baseName(0), 98 m_basePos(0,0,0), 99 m_baseQuat(0, 0, 0, 1), 100 m_baseMass(mass), 101 m_baseInertia(inertia), 102 103 m_fixedBase(fixedBase), 104 m_awake(true), 105 m_canSleep(canSleep), 106 m_sleepTimer(0), 107 108 m_linearDamping(0.04f), 109 m_angularDamping(0.04f), 110 m_useGyroTerm(true), 111 m_maxAppliedImpulse(1000.f), 112 m_maxCoordinateVelocity(100.f), 113 m_hasSelfCollision(true), 114 m_isMultiDof(multiDof), 115 __posUpdated(false), 116 m_dofCount(0), 117 m_posVarCnt(0), 118 m_useRK4(false), 119 m_useGlobalVelocities(false), 120 m_internalNeedsJointFeedback(false) 121 { 122 123 if(!m_isMultiDof) 124 { 125 m_vectorBuf.resize(2*n_links); 126 m_realBuf.resize(6 + 2*n_links); 127 m_posVarCnt = n_links; 128 } 129 130 m_links.resize(n_links); 131 m_matrixBuf.resize(n_links + 1); 132 133 134 m_baseForce.setValue(0, 0, 0); 135 m_baseTorque.setValue(0, 0, 0); 136 } 137 138 btMultiBody::~btMultiBody() 139 { 140 } 141 142 void btMultiBody::setupFixed(int i, 143 btScalar mass, 144 const btVector3 &inertia, 145 int parent, 146 const btQuaternion &rotParentToThis, 147 const btVector3 &parentComToThisPivotOffset, 148 const btVector3 &thisPivotToThisComOffset, 149 bool disableParentCollision) 150 { 151 btAssert(m_isMultiDof); 152 153 m_links[i].m_mass = mass; 154 m_links[i].m_inertiaLocal = inertia; 155 m_links[i].m_parent = parent; 156 m_links[i].m_zeroRotParentToThis = rotParentToThis; 157 m_links[i].m_dVector = thisPivotToThisComOffset; 158 m_links[i].m_eVector = parentComToThisPivotOffset; 159 160 m_links[i].m_jointType = btMultibodyLink::eFixed; 161 m_links[i].m_dofCount = 0; 162 m_links[i].m_posVarCount = 0; 163 164 if (disableParentCollision) 165 m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION; 166 // 167 m_links[i].updateCacheMultiDof(); 168 169 // 170 //if(m_isMultiDof) 171 // resizeInternalMultiDofBuffers(); 172 // 173 updateLinksDofOffsets(); 174 175 } 176 177 178 void btMultiBody::setupPrismatic(int i, 179 btScalar mass, 180 const btVector3 &inertia, 181 int parent, 182 const btQuaternion &rotParentToThis, 183 const btVector3 &jointAxis, 184 const btVector3 &parentComToThisPivotOffset, 185 const btVector3 &thisPivotToThisComOffset, 186 bool disableParentCollision) 187 { 188 if(m_isMultiDof) 189 { 190 m_dofCount += 1; 191 m_posVarCnt += 1; 192 } 193 194 m_links[i].m_mass = mass; 195 m_links[i].m_inertiaLocal = inertia; 196 m_links[i].m_parent = parent; 197 m_links[i].m_zeroRotParentToThis = rotParentToThis; 198 m_links[i].setAxisTop(0, 0., 0., 0.); 199 m_links[i].setAxisBottom(0, jointAxis); 200 m_links[i].m_eVector = parentComToThisPivotOffset; 201 m_links[i].m_dVector = thisPivotToThisComOffset; 202 m_links[i].m_cachedRotParentToThis = rotParentToThis; 203 204 m_links[i].m_jointType = btMultibodyLink::ePrismatic; 205 m_links[i].m_dofCount = 1; 206 m_links[i].m_posVarCount = 1; 207 m_links[i].m_jointPos[0] = 0.f; 208 m_links[i].m_jointTorque[0] = 0.f; 209 210 if (disableParentCollision) 211 m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION; 212 // 213 if(m_isMultiDof) 214 m_links[i].updateCacheMultiDof(); 215 else 216 m_links[i].updateCache(); 217 // 218 if(m_isMultiDof) 219 updateLinksDofOffsets(); 220 } 221 222 void btMultiBody::setupRevolute(int i, 223 btScalar mass, 224 const btVector3 &inertia, 225 int parent, 226 const btQuaternion &rotParentToThis, 227 const btVector3 &jointAxis, 228 const btVector3 &parentComToThisPivotOffset, 229 const btVector3 &thisPivotToThisComOffset, 230 bool disableParentCollision) 231 { 232 if(m_isMultiDof) 233 { 234 m_dofCount += 1; 235 m_posVarCnt += 1; 236 } 237 238 m_links[i].m_mass = mass; 239 m_links[i].m_inertiaLocal = inertia; 240 m_links[i].m_parent = parent; 241 m_links[i].m_zeroRotParentToThis = rotParentToThis; 242 m_links[i].setAxisTop(0, jointAxis); 243 m_links[i].setAxisBottom(0, jointAxis.cross(thisPivotToThisComOffset)); 244 m_links[i].m_dVector = thisPivotToThisComOffset; 245 m_links[i].m_eVector = parentComToThisPivotOffset; 246 247 m_links[i].m_jointType = btMultibodyLink::eRevolute; 248 m_links[i].m_dofCount = 1; 249 m_links[i].m_posVarCount = 1; 250 m_links[i].m_jointPos[0] = 0.f; 251 m_links[i].m_jointTorque[0] = 0.f; 252 253 if (disableParentCollision) 254 m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION; 255 // 256 if(m_isMultiDof) 257 m_links[i].updateCacheMultiDof(); 258 else 259 m_links[i].updateCache(); 260 // 261 if(m_isMultiDof) 262 updateLinksDofOffsets(); 263 } 264 265 266 267 void btMultiBody::setupSpherical(int i, 268 btScalar mass, 269 const btVector3 &inertia, 270 int parent, 271 const btQuaternion &rotParentToThis, 272 const btVector3 &parentComToThisPivotOffset, 273 const btVector3 &thisPivotToThisComOffset, 274 bool disableParentCollision) 275 { 276 btAssert(m_isMultiDof); 277 m_dofCount += 3; 278 m_posVarCnt += 4; 279 280 m_links[i].m_mass = mass; 281 m_links[i].m_inertiaLocal = inertia; 282 m_links[i].m_parent = parent; 283 m_links[i].m_zeroRotParentToThis = rotParentToThis; 284 m_links[i].m_dVector = thisPivotToThisComOffset; 285 m_links[i].m_eVector = parentComToThisPivotOffset; 286 287 m_links[i].m_jointType = btMultibodyLink::eSpherical; 288 m_links[i].m_dofCount = 3; 289 m_links[i].m_posVarCount = 4; 290 m_links[i].setAxisTop(0, 1.f, 0.f, 0.f); 291 m_links[i].setAxisTop(1, 0.f, 1.f, 0.f); 292 m_links[i].setAxisTop(2, 0.f, 0.f, 1.f); 293 m_links[i].setAxisBottom(0, m_links[i].getAxisTop(0).cross(thisPivotToThisComOffset)); 294 m_links[i].setAxisBottom(1, m_links[i].getAxisTop(1).cross(thisPivotToThisComOffset)); 295 m_links[i].setAxisBottom(2, m_links[i].getAxisTop(2).cross(thisPivotToThisComOffset)); 296 m_links[i].m_jointPos[0] = m_links[i].m_jointPos[1] = m_links[i].m_jointPos[2] = 0.f; m_links[i].m_jointPos[3] = 1.f; 297 m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = 0.f; 298 299 300 if (disableParentCollision) 301 m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION; 302 // 303 m_links[i].updateCacheMultiDof(); 304 // 305 updateLinksDofOffsets(); 306 } 307 308 void btMultiBody::setupPlanar(int i, 309 btScalar mass, 310 const btVector3 &inertia, 311 int parent, 312 const btQuaternion &rotParentToThis, 313 const btVector3 &rotationAxis, 314 const btVector3 &parentComToThisComOffset, 315 bool disableParentCollision) 316 { 317 btAssert(m_isMultiDof); 318 m_dofCount += 3; 319 m_posVarCnt += 3; 320 321 m_links[i].m_mass = mass; 322 m_links[i].m_inertiaLocal = inertia; 323 m_links[i].m_parent = parent; 324 m_links[i].m_zeroRotParentToThis = rotParentToThis; 325 m_links[i].m_dVector.setZero(); 326 m_links[i].m_eVector = parentComToThisComOffset; 327 328 // 329 static btVector3 vecNonParallelToRotAxis(1, 0, 0); 330 if(rotationAxis.normalized().dot(vecNonParallelToRotAxis) > 0.999) 331 vecNonParallelToRotAxis.setValue(0, 1, 0); 332 // 333 334 m_links[i].m_jointType = btMultibodyLink::ePlanar; 335 m_links[i].m_dofCount = 3; 336 m_links[i].m_posVarCount = 3; 337 btVector3 n=rotationAxis.normalized(); 338 m_links[i].setAxisTop(0, n[0],n[1],n[2]); 339 m_links[i].setAxisTop(1,0,0,0); 340 m_links[i].setAxisTop(2,0,0,0); 341 m_links[i].setAxisBottom(0,0,0,0); 342 btVector3 cr = m_links[i].getAxisTop(0).cross(vecNonParallelToRotAxis); 343 m_links[i].setAxisBottom(1,cr[0],cr[1],cr[2]); 344 cr = m_links[i].getAxisBottom(1).cross(m_links[i].getAxisTop(0)); 345 m_links[i].setAxisBottom(2,cr[0],cr[1],cr[2]); 346 m_links[i].m_jointPos[0] = m_links[i].m_jointPos[1] = m_links[i].m_jointPos[2] = 0.f; 347 m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = 0.f; 348 349 if (disableParentCollision) 350 m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION; 351 // 352 m_links[i].updateCacheMultiDof(); 353 // 354 updateLinksDofOffsets(); 355 } 356 357 void btMultiBody::finalizeMultiDof() 358 { 359 btAssert(m_isMultiDof); 360 m_deltaV.resize(0); 361 m_deltaV.resize(6 + m_dofCount); 362 m_realBuf.resize(6 + m_dofCount + m_dofCount*m_dofCount + 6 + m_dofCount); //m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices + delta-pos vector (6 base "vels" + joint "vels") 363 m_vectorBuf.resize(2 * m_dofCount); //two 3-vectors (i.e. one six-vector) for each system dof ("h" matrices) 364 365 updateLinksDofOffsets(); 366 } 367 368 int btMultiBody::getParent(int i) const 369 { 370 return m_links[i].m_parent; 371 } 372 373 btScalar btMultiBody::getLinkMass(int i) const 374 { 375 return m_links[i].m_mass; 376 } 377 378 const btVector3 & btMultiBody::getLinkInertia(int i) const 379 { 380 return m_links[i].m_inertiaLocal; 381 } 382 383 btScalar btMultiBody::getJointPos(int i) const 384 { 385 return m_links[i].m_jointPos[0]; 386 } 387 388 btScalar btMultiBody::getJointVel(int i) const 389 { 390 return m_realBuf[6 + i]; 391 } 392 393 btScalar * btMultiBody::getJointPosMultiDof(int i) 394 { 395 return &m_links[i].m_jointPos[0]; 396 } 397 398 btScalar * btMultiBody::getJointVelMultiDof(int i) 399 { 400 return &m_realBuf[6 + m_links[i].m_dofOffset]; 401 } 402 403 const btScalar * btMultiBody::getJointPosMultiDof(int i) const 404 { 405 return &m_links[i].m_jointPos[0]; 406 } 407 408 const btScalar * btMultiBody::getJointVelMultiDof(int i) const 409 { 410 return &m_realBuf[6 + m_links[i].m_dofOffset]; 411 } 412 413 414 void btMultiBody::setJointPos(int i, btScalar q) 415 { 416 m_links[i].m_jointPos[0] = q; 417 m_links[i].updateCache(); 418 } 419 420 void btMultiBody::setJointPosMultiDof(int i, btScalar *q) 421 { 422 for(int pos = 0; pos < m_links[i].m_posVarCount; ++pos) 423 m_links[i].m_jointPos[pos] = q[pos]; 424 425 m_links[i].updateCacheMultiDof(); 426 } 427 428 void btMultiBody::setJointVel(int i, btScalar qdot) 429 { 430 m_realBuf[6 + i] = qdot; 431 } 432 433 void btMultiBody::setJointVelMultiDof(int i, btScalar *qdot) 434 { 435 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) 436 m_realBuf[6 + m_links[i].m_dofOffset + dof] = qdot[dof]; 437 } 438 439 const btVector3 & btMultiBody::getRVector(int i) const 440 { 441 return m_links[i].m_cachedRVector; 442 } 443 444 const btQuaternion & btMultiBody::getParentToLocalRot(int i) const 445 { 446 return m_links[i].m_cachedRotParentToThis; 447 } 448 449 btVector3 btMultiBody::localPosToWorld(int i, const btVector3 &local_pos) const 450 { 451 btVector3 result = local_pos; 452 while (i != -1) { 453 // 'result' is in frame i. transform it to frame parent(i) 454 result += getRVector(i); 455 result = quatRotate(getParentToLocalRot(i).inverse(),result); 456 i = getParent(i); 457 } 458 459 // 'result' is now in the base frame. transform it to world frame 460 result = quatRotate(getWorldToBaseRot().inverse() ,result); 461 result += getBasePos(); 462 463 return result; 464 } 465 466 btVector3 btMultiBody::worldPosToLocal(int i, const btVector3 &world_pos) const 467 { 468 if (i == -1) { 469 // world to base 470 return quatRotate(getWorldToBaseRot(),(world_pos - getBasePos())); 471 } else { 472 // find position in parent frame, then transform to current frame 473 return quatRotate(getParentToLocalRot(i),worldPosToLocal(getParent(i), world_pos)) - getRVector(i); 474 } 475 } 476 477 btVector3 btMultiBody::localDirToWorld(int i, const btVector3 &local_dir) const 478 { 479 btVector3 result = local_dir; 480 while (i != -1) { 481 result = quatRotate(getParentToLocalRot(i).inverse() , result); 482 i = getParent(i); 483 } 484 result = quatRotate(getWorldToBaseRot().inverse() , result); 485 return result; 486 } 487 488 btVector3 btMultiBody::worldDirToLocal(int i, const btVector3 &world_dir) const 489 { 490 if (i == -1) { 491 return quatRotate(getWorldToBaseRot(), world_dir); 492 } else { 493 return quatRotate(getParentToLocalRot(i) ,worldDirToLocal(getParent(i), world_dir)); 494 } 495 } 496 497 void btMultiBody::compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const 498 { 499 int num_links = getNumLinks(); 500 // Calculates the velocities of each link (and the base) in its local frame 501 omega[0] = quatRotate(m_baseQuat ,getBaseOmega()); 502 vel[0] = quatRotate(m_baseQuat ,getBaseVel()); 503 504 for (int i = 0; i < num_links; ++i) { 505 const int parent = m_links[i].m_parent; 506 507 // transform parent vel into this frame, store in omega[i+1], vel[i+1] 508 SpatialTransform(btMatrix3x3(m_links[i].m_cachedRotParentToThis), m_links[i].m_cachedRVector, 509 omega[parent+1], vel[parent+1], 510 omega[i+1], vel[i+1]); 511 512 // now add qidot * shat_i 513 omega[i+1] += getJointVel(i) * m_links[i].getAxisTop(0); 514 vel[i+1] += getJointVel(i) * m_links[i].getAxisBottom(0); 515 } 516 } 517 518 btScalar btMultiBody::getKineticEnergy() const 519 { 520 int num_links = getNumLinks(); 521 // TODO: would be better not to allocate memory here 522 btAlignedObjectArray<btVector3> omega;omega.resize(num_links+1); 523 btAlignedObjectArray<btVector3> vel;vel.resize(num_links+1); 524 compTreeLinkVelocities(&omega[0], &vel[0]); 525 526 // we will do the factor of 0.5 at the end 527 btScalar result = m_baseMass * vel[0].dot(vel[0]); 528 result += omega[0].dot(m_baseInertia * omega[0]); 529 530 for (int i = 0; i < num_links; ++i) { 531 result += m_links[i].m_mass * vel[i+1].dot(vel[i+1]); 532 result += omega[i+1].dot(m_links[i].m_inertiaLocal * omega[i+1]); 533 } 534 535 return 0.5f * result; 536 } 537 538 btVector3 btMultiBody::getAngularMomentum() const 539 { 540 int num_links = getNumLinks(); 541 // TODO: would be better not to allocate memory here 542 btAlignedObjectArray<btVector3> omega;omega.resize(num_links+1); 543 btAlignedObjectArray<btVector3> vel;vel.resize(num_links+1); 544 btAlignedObjectArray<btQuaternion> rot_from_world;rot_from_world.resize(num_links+1); 545 compTreeLinkVelocities(&omega[0], &vel[0]); 546 547 rot_from_world[0] = m_baseQuat; 548 btVector3 result = quatRotate(rot_from_world[0].inverse() , (m_baseInertia * omega[0])); 549 550 for (int i = 0; i < num_links; ++i) { 551 rot_from_world[i+1] = m_links[i].m_cachedRotParentToThis * rot_from_world[m_links[i].m_parent+1]; 552 result += (quatRotate(rot_from_world[i+1].inverse() , (m_links[i].m_inertiaLocal * omega[i+1]))); 553 } 554 555 return result; 556 } 557 558 void btMultiBody::clearConstraintForces() 559 { 560 m_baseConstraintForce.setValue(0, 0, 0); 561 m_baseConstraintTorque.setValue(0, 0, 0); 562 563 564 for (int i = 0; i < getNumLinks(); ++i) { 565 m_links[i].m_appliedConstraintForce.setValue(0, 0, 0); 566 m_links[i].m_appliedConstraintTorque.setValue(0, 0, 0); 567 } 568 } 569 void btMultiBody::clearForcesAndTorques() 570 { 571 m_baseForce.setValue(0, 0, 0); 572 m_baseTorque.setValue(0, 0, 0); 573 574 575 for (int i = 0; i < getNumLinks(); ++i) { 576 m_links[i].m_appliedForce.setValue(0, 0, 0); 577 m_links[i].m_appliedTorque.setValue(0, 0, 0); 578 m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = m_links[i].m_jointTorque[3] = m_links[i].m_jointTorque[4] = m_links[i].m_jointTorque[5] = 0.f; 579 } 580 } 581 582 void btMultiBody::clearVelocities() 583 { 584 for (int i = 0; i < 6 + getNumLinks(); ++i) 585 { 586 m_realBuf[i] = 0.f; 587 } 588 } 589 void btMultiBody::addLinkForce(int i, const btVector3 &f) 590 { 591 m_links[i].m_appliedForce += f; 592 } 593 594 void btMultiBody::addLinkTorque(int i, const btVector3 &t) 595 { 596 m_links[i].m_appliedTorque += t; 597 } 598 599 void btMultiBody::addLinkConstraintForce(int i, const btVector3 &f) 600 { 601 m_links[i].m_appliedConstraintForce += f; 602 } 603 604 void btMultiBody::addLinkConstraintTorque(int i, const btVector3 &t) 605 { 606 m_links[i].m_appliedConstraintTorque += t; 607 } 608 609 610 611 void btMultiBody::addJointTorque(int i, btScalar Q) 612 { 613 m_links[i].m_jointTorque[0] += Q; 614 } 615 616 void btMultiBody::addJointTorqueMultiDof(int i, int dof, btScalar Q) 617 { 618 m_links[i].m_jointTorque[dof] += Q; 619 } 620 621 void btMultiBody::addJointTorqueMultiDof(int i, const btScalar *Q) 622 { 623 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) 624 m_links[i].m_jointTorque[dof] = Q[dof]; 625 } 626 627 const btVector3 & btMultiBody::getLinkForce(int i) const 628 { 629 return m_links[i].m_appliedForce; 630 } 631 632 const btVector3 & btMultiBody::getLinkTorque(int i) const 633 { 634 return m_links[i].m_appliedTorque; 635 } 636 637 btScalar btMultiBody::getJointTorque(int i) const 638 { 639 return m_links[i].m_jointTorque[0]; 640 } 641 642 btScalar * btMultiBody::getJointTorqueMultiDof(int i) 643 { 644 return &m_links[i].m_jointTorque[0]; 645 } 646 647 inline btMatrix3x3 outerProduct(const btVector3& v0, const btVector3& v1) //renamed it from vecMulVecTranspose (http://en.wikipedia.org/wiki/Outer_product); maybe it should be moved to btVector3 like dot and cross? 648 { 649 btVector3 row0 = btVector3( 650 v0.x() * v1.x(), 651 v0.x() * v1.y(), 652 v0.x() * v1.z()); 653 btVector3 row1 = btVector3( 654 v0.y() * v1.x(), 655 v0.y() * v1.y(), 656 v0.y() * v1.z()); 657 btVector3 row2 = btVector3( 658 v0.z() * v1.x(), 659 v0.z() * v1.y(), 660 v0.z() * v1.z()); 661 662 btMatrix3x3 m(row0[0],row0[1],row0[2], 663 row1[0],row1[1],row1[2], 664 row2[0],row2[1],row2[2]); 665 return m; 666 } 667 668 #define vecMulVecTranspose(v0, v1Transposed) outerProduct(v0, v1Transposed) 669 // 670 671 void btMultiBody::stepVelocitiesMultiDof(btScalar dt, 672 btAlignedObjectArray<btScalar> &scratch_r, 673 btAlignedObjectArray<btVector3> &scratch_v, 674 btAlignedObjectArray<btMatrix3x3> &scratch_m, 675 bool isConstraintPass) 676 { 677 // Implement Featherstone's algorithm to calculate joint accelerations (q_double_dot) 678 // and the base linear & angular accelerations. 679 680 // We apply damping forces in this routine as well as any external forces specified by the 681 // caller (via addBaseForce etc). 682 683 // output should point to an array of 6 + num_links reals. 684 // Format is: 3 angular accelerations (in world frame), 3 linear accelerations (in world frame), 685 // num_links joint acceleration values. 686 687 m_internalNeedsJointFeedback = false; 688 689 int num_links = getNumLinks(); 690 691 const btScalar DAMPING_K1_LINEAR = m_linearDamping; 692 const btScalar DAMPING_K2_LINEAR = m_linearDamping; 693 694 const btScalar DAMPING_K1_ANGULAR = m_angularDamping; 695 const btScalar DAMPING_K2_ANGULAR= m_angularDamping; 696 697 btVector3 base_vel = getBaseVel(); 698 btVector3 base_omega = getBaseOmega(); 699 700 // Temporary matrices/vectors -- use scratch space from caller 701 // so that we don't have to keep reallocating every frame 702 703 scratch_r.resize(2*m_dofCount + 6); //multidof? ("Y"s use it and it is used to store qdd) => 2 x m_dofCount 704 scratch_v.resize(8*num_links + 6); 705 scratch_m.resize(4*num_links + 4); 706 707 //btScalar * r_ptr = &scratch_r[0]; 708 btScalar * output = &scratch_r[m_dofCount]; // "output" holds the q_double_dot results 709 btVector3 * v_ptr = &scratch_v[0]; 710 711 // vhat_i (top = angular, bottom = linear part) 712 btSpatialMotionVector *spatVel = (btSpatialMotionVector *)v_ptr; 713 v_ptr += num_links * 2 + 2; 714 // 715 // zhat_i^A 716 btSpatialForceVector * zeroAccSpatFrc = (btSpatialForceVector *)v_ptr; 717 v_ptr += num_links * 2 + 2; 718 // 719 // chat_i (note NOT defined for the base) 720 btSpatialMotionVector * spatCoriolisAcc = (btSpatialMotionVector *)v_ptr; 721 v_ptr += num_links * 2; 722 // 723 // Ihat_i^A. 724 btSymmetricSpatialDyad * spatInertia = (btSymmetricSpatialDyad *)&scratch_m[num_links + 1]; 725 726 // Cached 3x3 rotation matrices from parent frame to this frame. 727 btMatrix3x3 * rot_from_parent = &m_matrixBuf[0]; 728 btMatrix3x3 * rot_from_world = &scratch_m[0]; 729 730 // hhat_i, ahat_i 731 // hhat is NOT stored for the base (but ahat is) 732 btSpatialForceVector * h = (btSpatialForceVector *)(m_dofCount > 0 ? &m_vectorBuf[0] : 0); 733 btSpatialMotionVector * spatAcc = (btSpatialMotionVector *)v_ptr; 734 v_ptr += num_links * 2 + 2; 735 // 736 // Y_i, invD_i 737 btScalar * invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0; 738 btScalar * Y = &scratch_r[0]; 739 // 740 //aux variables 741 static btSpatialMotionVector spatJointVel; //spatial velocity due to the joint motion (i.e. without predecessors' influence) 742 static btScalar D[36]; //"D" matrix; it's dofxdof for each body so asingle 6x6 D matrix will do 743 static btScalar invD_times_Y[6]; //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies 744 static btSpatialMotionVector result; //holds results of the SolveImatrix op; it is a spatial motion vector (accel) 745 static btScalar Y_minus_hT_a[6]; //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough 746 static btSpatialForceVector spatForceVecTemps[6]; //6 temporary spatial force vectors 747 static btSpatialTransformationMatrix fromParent; //spatial transform from parent to child 748 static btSymmetricSpatialDyad dyadTemp; //inertia matrix temp 749 static btSpatialTransformationMatrix fromWorld; 750 fromWorld.m_trnVec.setZero(); 751 ///////////////// 752 753 // ptr to the joint accel part of the output 754 btScalar * joint_accel = output + 6; 755 756 // Start of the algorithm proper. 757 758 // First 'upward' loop. 759 // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich. 760 761 rot_from_parent[0] = btMatrix3x3(m_baseQuat); //m_baseQuat assumed to be alias!? 762 763 //create the vector of spatial velocity of the base by transforming global-coor linear and angular velocities into base-local coordinates 764 spatVel[0].setVector(rot_from_parent[0] * base_omega, rot_from_parent[0] * base_vel); 765 766 if (m_fixedBase) 767 { 768 zeroAccSpatFrc[0].setZero(); 769 } 770 else 771 { 772 btVector3 baseForce = isConstraintPass? m_baseConstraintForce : m_baseForce; 773 btVector3 baseTorque = isConstraintPass? m_baseConstraintTorque : m_baseTorque; 774 //external forces 775 zeroAccSpatFrc[0].setVector(-(rot_from_parent[0] * baseTorque), -(rot_from_parent[0] * baseForce)); 776 777 //adding damping terms (only) 778 btScalar linDampMult = 1., angDampMult = 1.; 779 zeroAccSpatFrc[0].addVector(angDampMult * m_baseInertia * spatVel[0].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[0].getAngular().norm()), 780 linDampMult * m_baseMass * spatVel[0].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[0].getLinear().norm())); 781 782 // 783 //p += vhat x Ihat vhat - done in a simpler way 784 if (m_useGyroTerm) 785 zeroAccSpatFrc[0].addAngular(spatVel[0].getAngular().cross(m_baseInertia * spatVel[0].getAngular())); 786 // 787 zeroAccSpatFrc[0].addLinear(m_baseMass * spatVel[0].getAngular().cross(spatVel[0].getLinear())); 788 } 789 790 791 //init the spatial AB inertia (it has the simple form thanks to choosing local body frames origins at their COMs) 792 spatInertia[0].setMatrix( btMatrix3x3(0,0,0,0,0,0,0,0,0), 793 // 794 btMatrix3x3(m_baseMass, 0, 0, 795 0, m_baseMass, 0, 796 0, 0, m_baseMass), 797 // 798 btMatrix3x3(m_baseInertia[0], 0, 0, 799 0, m_baseInertia[1], 0, 800 0, 0, m_baseInertia[2]) 801 ); 802 803 rot_from_world[0] = rot_from_parent[0]; 804 805 // 806 for (int i = 0; i < num_links; ++i) { 807 const int parent = m_links[i].m_parent; 808 rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis); 809 rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1]; 810 811 fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector; 812 fromWorld.m_rotMat = rot_from_world[i+1]; 813 fromParent.transform(spatVel[parent+1], spatVel[i+1]); 814 815 // now set vhat_i to its true value by doing 816 // vhat_i += qidot * shat_i 817 if(!m_useGlobalVelocities) 818 { 819 spatJointVel.setZero(); 820 821 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) 822 spatJointVel += m_links[i].m_axes[dof] * getJointVelMultiDof(i)[dof]; 823 824 // remember vhat_i is really vhat_p(i) (but in current frame) at this point => we need to add velocity across the inboard joint 825 spatVel[i+1] += spatJointVel; 826 827 // 828 // vhat_i is vhat_p(i) transformed to local coors + the velocity across the i-th inboard joint 829 //spatVel[i+1] = fromParent * spatVel[parent+1] + spatJointVel; 830 831 } 832 else 833 { 834 fromWorld.transformRotationOnly(m_links[i].m_absFrameTotVelocity, spatVel[i+1]); 835 fromWorld.transformRotationOnly(m_links[i].m_absFrameLocVelocity, spatJointVel); 836 } 837 838 // we can now calculate chat_i 839 spatVel[i+1].cross(spatJointVel, spatCoriolisAcc[i]); 840 841 // calculate zhat_i^A 842 // 843 //external forces 844 btVector3 linkAppliedForce = isConstraintPass? m_links[i].m_appliedConstraintForce : m_links[i].m_appliedForce; 845 btVector3 linkAppliedTorque =isConstraintPass ? m_links[i].m_appliedConstraintTorque : m_links[i].m_appliedTorque; 846 847 zeroAccSpatFrc[i+1].setVector(-(rot_from_world[i+1] * linkAppliedTorque), -(rot_from_world[i+1] * linkAppliedForce )); 848 849 #if 0 850 { 851 852 b3Printf("stepVelocitiesMultiDof zeroAccSpatFrc[%d] linear:%f,%f,%f, angular:%f,%f,%f", 853 i+1, 854 zeroAccSpatFrc[i+1].m_topVec[0], 855 zeroAccSpatFrc[i+1].m_topVec[1], 856 zeroAccSpatFrc[i+1].m_topVec[2], 857 858 zeroAccSpatFrc[i+1].m_bottomVec[0], 859 zeroAccSpatFrc[i+1].m_bottomVec[1], 860 zeroAccSpatFrc[i+1].m_bottomVec[2]); 861 } 862 #endif 863 // 864 //adding damping terms (only) 865 btScalar linDampMult = 1., angDampMult = 1.; 866 zeroAccSpatFrc[i+1].addVector(angDampMult * m_links[i].m_inertiaLocal * spatVel[i+1].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[i+1].getAngular().norm()), 867 linDampMult * m_links[i].m_mass * spatVel[i+1].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[i+1].getLinear().norm())); 868 869 // calculate Ihat_i^A 870 //init the spatial AB inertia (it has the simple form thanks to choosing local body frames origins at their COMs) 871 spatInertia[i+1].setMatrix( btMatrix3x3(0,0,0,0,0,0,0,0,0), 872 // 873 btMatrix3x3(m_links[i].m_mass, 0, 0, 874 0, m_links[i].m_mass, 0, 875 0, 0, m_links[i].m_mass), 876 // 877 btMatrix3x3(m_links[i].m_inertiaLocal[0], 0, 0, 878 0, m_links[i].m_inertiaLocal[1], 0, 879 0, 0, m_links[i].m_inertiaLocal[2]) 880 ); 881 // 882 //p += vhat x Ihat vhat - done in a simpler way 883 if(m_useGyroTerm) 884 zeroAccSpatFrc[i+1].addAngular(spatVel[i+1].getAngular().cross(m_links[i].m_inertiaLocal * spatVel[i+1].getAngular())); 885 // 886 zeroAccSpatFrc[i+1].addLinear(m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear())); 887 //btVector3 temp = m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear()); 888 ////clamp parent's omega 889 //btScalar parOmegaMod = temp.length(); 890 //btScalar parOmegaModMax = 1000; 891 //if(parOmegaMod > parOmegaModMax) 892 // temp *= parOmegaModMax / parOmegaMod; 893 //zeroAccSpatFrc[i+1].addLinear(temp); 894 //printf("|zeroAccSpatFrc[%d]| = %.4f\n", i+1, temp.length()); 895 //temp = spatCoriolisAcc[i].getLinear(); 896 //printf("|spatCoriolisAcc[%d]| = %.4f\n", i+1, temp.length()); 897 898 899 900 //printf("w[%d] = [%.4f %.4f %.4f]\n", i, vel_top_angular[i+1].x(), vel_top_angular[i+1].y(), vel_top_angular[i+1].z()); 901 //printf("v[%d] = [%.4f %.4f %.4f]\n", i, vel_bottom_linear[i+1].x(), vel_bottom_linear[i+1].y(), vel_bottom_linear[i+1].z()); 902 //printf("c[%d] = [%.4f %.4f %.4f]\n", i, coriolis_bottom_linear[i].x(), coriolis_bottom_linear[i].y(), coriolis_bottom_linear[i].z()); 903 } 904 905 // 'Downward' loop. 906 // (part of TreeForwardDynamics in Mirtich.) 907 for (int i = num_links - 1; i >= 0; --i) 908 { 909 const int parent = m_links[i].m_parent; 910 fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector; 911 912 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) 913 { 914 btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof]; 915 // 916 hDof = spatInertia[i+1] * m_links[i].m_axes[dof]; 917 // 918 Y[m_links[i].m_dofOffset + dof] = m_links[i].m_jointTorque[dof] 919 - m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i+1]) 920 - spatCoriolisAcc[i].dot(hDof) 921 ; 922 } 923 924 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) 925 { 926 btScalar *D_row = &D[dof * m_links[i].m_dofCount]; 927 for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2) 928 { 929 btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2]; 930 D_row[dof2] = m_links[i].m_axes[dof].dot(hDof2); 931 } 932 } 933 934 btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset]; 935 switch(m_links[i].m_jointType) 936 { 937 case btMultibodyLink::ePrismatic: 938 case btMultibodyLink::eRevolute: 939 { 940 invDi[0] = 1.0f / D[0]; 941 break; 942 } 943 case btMultibodyLink::eSpherical: 944 case btMultibodyLink::ePlanar: 945 { 946 static btMatrix3x3 D3x3; D3x3.setValue(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]); 947 static btMatrix3x3 invD3x3; invD3x3 = D3x3.inverse(); 948 949 //unroll the loop? 950 for(int row = 0; row < 3; ++row) 951 { 952 for(int col = 0; col < 3; ++col) 953 { 954 invDi[row * 3 + col] = invD3x3[row][col]; 955 } 956 } 957 958 break; 959 } 960 default: 961 { 962 963 } 964 } 965 966 //determine h*D^{-1} 967 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) 968 { 969 spatForceVecTemps[dof].setZero(); 970 971 for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2) 972 { 973 btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2]; 974 // 975 spatForceVecTemps[dof] += hDof2 * invDi[dof2 * m_links[i].m_dofCount + dof]; 976 } 977 } 978 979 dyadTemp = spatInertia[i+1]; 980 981 //determine (h*D^{-1}) * h^{T} 982 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) 983 { 984 btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof]; 985 // 986 dyadTemp -= symmetricSpatialOuterProduct(hDof, spatForceVecTemps[dof]); 987 } 988 989 fromParent.transformInverse(dyadTemp, spatInertia[parent+1], btSpatialTransformationMatrix::Add); 990 991 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) 992 { 993 invD_times_Y[dof] = 0.f; 994 995 for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2) 996 { 997 invD_times_Y[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * Y[m_links[i].m_dofOffset + dof2]; 998 } 999 } 1000 1001 spatForceVecTemps[0] = zeroAccSpatFrc[i+1] + spatInertia[i+1] * spatCoriolisAcc[i]; 1002 1003 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) 1004 { 1005 btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof]; 1006 // 1007 spatForceVecTemps[0] += hDof * invD_times_Y[dof]; 1008 } 1009 1010 fromParent.transformInverse(spatForceVecTemps[0], spatForceVecTemps[1]); 1011 1012 zeroAccSpatFrc[parent+1] += spatForceVecTemps[1]; 1013 } 1014 1015 1016 // Second 'upward' loop 1017 // (part of TreeForwardDynamics in Mirtich) 1018 1019 if (m_fixedBase) 1020 { 1021 spatAcc[0].setZero(); 1022 } 1023 else 1024 { 1025 if (num_links > 0) 1026 { 1027 m_cachedInertiaTopLeft = spatInertia[0].m_topLeftMat; 1028 m_cachedInertiaTopRight = spatInertia[0].m_topRightMat; 1029 m_cachedInertiaLowerLeft = spatInertia[0].m_bottomLeftMat; 1030 m_cachedInertiaLowerRight= spatInertia[0].m_topLeftMat.transpose(); 1031 1032 } 1033 1034 solveImatrix(zeroAccSpatFrc[0], result); 1035 spatAcc[0] = -result; 1036 } 1037 1038 1039 // now do the loop over the m_links 1040 for (int i = 0; i < num_links; ++i) 1041 { 1042 // qdd = D^{-1} * (Y - h^{T}*apar) = (S^{T}*I*S)^{-1} * (tau - S^{T}*I*cor - S^{T}*zeroAccFrc - S^{T}*I*apar) 1043 // a = apar + cor + Sqdd 1044 //or 1045 // qdd = D^{-1} * (Y - h^{T}*(apar+cor)) 1046 // a = apar + Sqdd 1047 1048 const int parent = m_links[i].m_parent; 1049 fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector; 1050 1051 fromParent.transform(spatAcc[parent+1], spatAcc[i+1]); 1052 1053 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) 1054 { 1055 btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof]; 1056 // 1057 Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i+1].dot(hDof); 1058 } 1059 1060 btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset]; 1061 //D^{-1} * (Y - h^{T}*apar) 1062 mulMatrix(invDi, Y_minus_hT_a, m_links[i].m_dofCount, m_links[i].m_dofCount, m_links[i].m_dofCount, 1, &joint_accel[m_links[i].m_dofOffset]); 1063 1064 spatAcc[i+1] += spatCoriolisAcc[i]; 1065 1066 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) 1067 spatAcc[i+1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof]; 1068 1069 if (m_links[i].m_jointFeedback) 1070 { 1071 m_internalNeedsJointFeedback = true; 1072 1073 btVector3 angularBotVec = (spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_bottomVec; 1074 btVector3 linearTopVec = (spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_topVec; 1075 1076 if (gJointFeedbackInJointFrame) 1077 { 1078 //shift the reaction forces to the joint frame 1079 //linear (force) component is the same 1080 //shift the angular (torque, moment) component using the relative position, m_links[i].m_dVector 1081 angularBotVec = angularBotVec - linearTopVec.cross(m_links[i].m_dVector); 1082 } 1083 1084 1085 if (gJointFeedbackInWorldSpace) 1086 { 1087 if (isConstraintPass) 1088 { 1089 m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec += m_links[i].m_cachedWorldTransform.getBasis()*angularBotVec; 1090 m_links[i].m_jointFeedback->m_reactionForces.m_topVec += m_links[i].m_cachedWorldTransform.getBasis()*linearTopVec; 1091 } else 1092 { 1093 m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = m_links[i].m_cachedWorldTransform.getBasis()*angularBotVec; 1094 m_links[i].m_jointFeedback->m_reactionForces.m_topVec = m_links[i].m_cachedWorldTransform.getBasis()*linearTopVec; 1095 } 1096 } else 1097 { 1098 if (isConstraintPass) 1099 { 1100 m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec += angularBotVec; 1101 m_links[i].m_jointFeedback->m_reactionForces.m_topVec += linearTopVec; 1102 1103 } 1104 else 1105 { 1106 m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = angularBotVec; 1107 m_links[i].m_jointFeedback->m_reactionForces.m_topVec = linearTopVec; 1108 } 1109 } 1110 } 1111 1112 } 1113 1114 // transform base accelerations back to the world frame. 1115 btVector3 omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].getAngular(); 1116 output[0] = omegadot_out[0]; 1117 output[1] = omegadot_out[1]; 1118 output[2] = omegadot_out[2]; 1119 1120 btVector3 vdot_out = rot_from_parent[0].transpose() * (spatAcc[0].getLinear() + spatVel[0].getAngular().cross(spatVel[0].getLinear())); 1121 output[3] = vdot_out[0]; 1122 output[4] = vdot_out[1]; 1123 output[5] = vdot_out[2]; 1124 1125 ///////////////// 1126 //printf("q = ["); 1127 //printf("%.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f ", m_baseQuat.x(), m_baseQuat.y(), m_baseQuat.z(), m_baseQuat.w(), m_basePos.x(), m_basePos.y(), m_basePos.z()); 1128 //for(int link = 0; link < getNumLinks(); ++link) 1129 // for(int dof = 0; dof < m_links[link].m_dofCount; ++dof) 1130 // printf("%.6f ", m_links[link].m_jointPos[dof]); 1131 //printf("]\n"); 1132 //// 1133 //printf("qd = ["); 1134 //for(int dof = 0; dof < getNumDofs() + 6; ++dof) 1135 // printf("%.6f ", m_realBuf[dof]); 1136 //printf("]\n"); 1137 //printf("qdd = ["); 1138 //for(int dof = 0; dof < getNumDofs() + 6; ++dof) 1139 // printf("%.6f ", output[dof]); 1140 //printf("]\n"); 1141 ///////////////// 1142 1143 // Final step: add the accelerations (times dt) to the velocities. 1144 1145 if (!isConstraintPass) 1146 { 1147 if(dt > 0.) 1148 applyDeltaVeeMultiDof(output, dt); 1149 1150 } 1151 ///// 1152 //btScalar angularThres = 1; 1153 //btScalar maxAngVel = 0.; 1154 //bool scaleDown = 1.; 1155 //for(int link = 0; link < m_links.size(); ++link) 1156 //{ 1157 // if(spatVel[link+1].getAngular().length() > maxAngVel) 1158 // { 1159 // maxAngVel = spatVel[link+1].getAngular().length(); 1160 // scaleDown = angularThres / spatVel[link+1].getAngular().length(); 1161 // break; 1162 // } 1163 //} 1164 1165 //if(scaleDown != 1.) 1166 //{ 1167 // for(int link = 0; link < m_links.size(); ++link) 1168 // { 1169 // if(m_links[link].m_jointType == btMultibodyLink::eRevolute || m_links[link].m_jointType == btMultibodyLink::eSpherical) 1170 // { 1171 // for(int dof = 0; dof < m_links[link].m_dofCount; ++dof) 1172 // getJointVelMultiDof(link)[dof] *= scaleDown; 1173 // } 1174 // } 1175 //} 1176 ///// 1177 1178 ///////////////////// 1179 if(m_useGlobalVelocities) 1180 { 1181 for (int i = 0; i < num_links; ++i) 1182 { 1183 const int parent = m_links[i].m_parent; 1184 //rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis); /// <- done 1185 //rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1]; /// <- done 1186 1187 fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector; 1188 fromWorld.m_rotMat = rot_from_world[i+1]; 1189 1190 // vhat_i = i_xhat_p(i) * vhat_p(i) 1191 fromParent.transform(spatVel[parent+1], spatVel[i+1]); 1192 //nice alternative below (using operator *) but it generates temps 1193 ///////////////////////////////////////////////////////////// 1194 1195 // now set vhat_i to its true value by doing 1196 // vhat_i += qidot * shat_i 1197 spatJointVel.setZero(); 1198 1199 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) 1200 spatJointVel += m_links[i].m_axes[dof] * getJointVelMultiDof(i)[dof]; 1201 1202 // remember vhat_i is really vhat_p(i) (but in current frame) at this point => we need to add velocity across the inboard joint 1203 spatVel[i+1] += spatJointVel; 1204 1205 1206 fromWorld.transformInverseRotationOnly(spatVel[i+1], m_links[i].m_absFrameTotVelocity); 1207 fromWorld.transformInverseRotationOnly(spatJointVel, m_links[i].m_absFrameLocVelocity); 1208 } 1209 } 1210 1211 } 1212 1213 1214 void btMultiBody::stepVelocities(btScalar dt, 1215 btAlignedObjectArray<btScalar> &scratch_r, 1216 btAlignedObjectArray<btVector3> &scratch_v, 1217 btAlignedObjectArray<btMatrix3x3> &scratch_m) 1218 { 1219 // Implement Featherstone's algorithm to calculate joint accelerations (q_double_dot) 1220 // and the base linear & angular accelerations. 1221 1222 // We apply damping forces in this routine as well as any external forces specified by the 1223 // caller (via addBaseForce etc). 1224 1225 // output should point to an array of 6 + num_links reals. 1226 // Format is: 3 angular accelerations (in world frame), 3 linear accelerations (in world frame), 1227 // num_links joint acceleration values. 1228 1229 int num_links = getNumLinks(); 1230 1231 const btScalar DAMPING_K1_LINEAR = m_linearDamping; 1232 const btScalar DAMPING_K2_LINEAR = m_linearDamping; 1233 1234 const btScalar DAMPING_K1_ANGULAR = m_angularDamping; 1235 const btScalar DAMPING_K2_ANGULAR= m_angularDamping; 1236 1237 btVector3 base_vel = getBaseVel(); 1238 btVector3 base_omega = getBaseOmega(); 1239 1240 // Temporary matrices/vectors -- use scratch space from caller 1241 // so that we don't have to keep reallocating every frame 1242 1243 scratch_r.resize(2*num_links + 6); 1244 scratch_v.resize(8*num_links + 6); 1245 scratch_m.resize(4*num_links + 4); 1246 1247 btScalar * r_ptr = &scratch_r[0]; 1248 btScalar * output = &scratch_r[num_links]; // "output" holds the q_double_dot results 1249 btVector3 * v_ptr = &scratch_v[0]; 1250 1251 // vhat_i (top = angular, bottom = linear part) 1252 btVector3 * vel_top_angular = v_ptr; v_ptr += num_links + 1; 1253 btVector3 * vel_bottom_linear = v_ptr; v_ptr += num_links + 1; 1254 1255 // zhat_i^A 1256 btVector3 * f_zero_acc_top_angular = v_ptr; v_ptr += num_links + 1; 1257 btVector3 * f_zero_acc_bottom_linear = v_ptr; v_ptr += num_links + 1; 1258 1259 // chat_i (note NOT defined for the base) 1260 btVector3 * coriolis_top_angular = v_ptr; v_ptr += num_links; 1261 btVector3 * coriolis_bottom_linear = v_ptr; v_ptr += num_links; 1262 1263 // top left, top right and bottom left blocks of Ihat_i^A. 1264 // bottom right block = transpose of top left block and is not stored. 1265 // Note: the top right and bottom left blocks are always symmetric matrices, but we don't make use of this fact currently. 1266 btMatrix3x3 * inertia_top_left = &scratch_m[num_links + 1]; 1267 btMatrix3x3 * inertia_top_right = &scratch_m[2*num_links + 2]; 1268 btMatrix3x3 * inertia_bottom_left = &scratch_m[3*num_links + 3]; 1269 1270 // Cached 3x3 rotation matrices from parent frame to this frame. 1271 btMatrix3x3 * rot_from_parent = &m_matrixBuf[0]; 1272 btMatrix3x3 * rot_from_world = &scratch_m[0]; 1273 1274 // hhat_i, ahat_i 1275 // hhat is NOT stored for the base (but ahat is) 1276 btVector3 * h_top = num_links > 0 ? &m_vectorBuf[0] : 0; 1277 btVector3 * h_bottom = num_links > 0 ? &m_vectorBuf[num_links] : 0; 1278 btVector3 * accel_top = v_ptr; v_ptr += num_links + 1; 1279 btVector3 * accel_bottom = v_ptr; v_ptr += num_links + 1; 1280 1281 // Y_i, D_i 1282 btScalar * Y1 = r_ptr; r_ptr += num_links; 1283 btScalar * D = num_links > 0 ? &m_realBuf[6 + num_links] : 0; 1284 1285 // ptr to the joint accel part of the output 1286 btScalar * joint_accel = output + 6; 1287 1288 1289 // Start of the algorithm proper. 1290 1291 // First 'upward' loop. 1292 // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich. 1293 1294 rot_from_parent[0] = btMatrix3x3(m_baseQuat); 1295 1296 vel_top_angular[0] = rot_from_parent[0] * base_omega; 1297 vel_bottom_linear[0] = rot_from_parent[0] * base_vel; 1298 1299 if (m_fixedBase) { 1300 f_zero_acc_top_angular[0] = f_zero_acc_bottom_linear[0] = btVector3(0,0,0); 1301 } else { 1302 f_zero_acc_top_angular[0] = - (rot_from_parent[0] * (m_baseForce 1303 - m_baseMass*(DAMPING_K1_LINEAR+DAMPING_K2_LINEAR*base_vel.norm())*base_vel)); 1304 1305 f_zero_acc_bottom_linear[0] = 1306 - (rot_from_parent[0] * m_baseTorque); 1307 1308 if (m_useGyroTerm) 1309 f_zero_acc_bottom_linear[0]+=vel_top_angular[0].cross( m_baseInertia * vel_top_angular[0] ); 1310 1311 f_zero_acc_bottom_linear[0] += m_baseInertia * vel_top_angular[0] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[0].norm()); 1312 1313 } 1314 1315 1316 1317 inertia_top_left[0] = btMatrix3x3(0,0,0,0,0,0,0,0,0);//::Zero(); 1318 1319 1320 inertia_top_right[0].setValue(m_baseMass, 0, 0, 1321 0, m_baseMass, 0, 1322 0, 0, m_baseMass); 1323 inertia_bottom_left[0].setValue(m_baseInertia[0], 0, 0, 1324 0, m_baseInertia[1], 0, 1325 0, 0, m_baseInertia[2]); 1326 1327 rot_from_world[0] = rot_from_parent[0]; 1328 1329 for (int i = 0; i < num_links; ++i) { 1330 const int parent = m_links[i].m_parent; 1331 rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis); 1332 1333 1334 rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1]; 1335 1336 // vhat_i = i_xhat_p(i) * vhat_p(i) 1337 SpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector, 1338 vel_top_angular[parent+1], vel_bottom_linear[parent+1], 1339 vel_top_angular[i+1], vel_bottom_linear[i+1]); 1340 1341 // we can now calculate chat_i 1342 // remember vhat_i is really vhat_p(i) (but in current frame) at this point 1343 coriolis_bottom_linear[i] = vel_top_angular[i+1].cross(vel_top_angular[i+1].cross(m_links[i].m_cachedRVector)) 1344 + 2 * vel_top_angular[i+1].cross(m_links[i].getAxisBottom(0)) * getJointVel(i); 1345 if (m_links[i].m_jointType == btMultibodyLink::eRevolute) { 1346 coriolis_top_angular[i] = vel_top_angular[i+1].cross(m_links[i].getAxisTop(0)) * getJointVel(i); 1347 coriolis_bottom_linear[i] += (getJointVel(i) * getJointVel(i)) * m_links[i].getAxisTop(0).cross(m_links[i].getAxisBottom(0)); 1348 } else { 1349 coriolis_top_angular[i] = btVector3(0,0,0); 1350 } 1351 1352 // now set vhat_i to its true value by doing 1353 // vhat_i += qidot * shat_i 1354 vel_top_angular[i+1] += getJointVel(i) * m_links[i].getAxisTop(0); 1355 vel_bottom_linear[i+1] += getJointVel(i) * m_links[i].getAxisBottom(0); 1356 1357 // calculate zhat_i^A 1358 f_zero_acc_top_angular[i+1] = - (rot_from_world[i+1] * (m_links[i].m_appliedForce)); 1359 f_zero_acc_top_angular[i+1] += m_links[i].m_mass * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR*vel_bottom_linear[i+1].norm()) * vel_bottom_linear[i+1]; 1360 1361 f_zero_acc_bottom_linear[i+1] = 1362 - (rot_from_world[i+1] * m_links[i].m_appliedTorque); 1363 if (m_useGyroTerm) 1364 { 1365 f_zero_acc_bottom_linear[i+1] += vel_top_angular[i+1].cross( m_links[i].m_inertiaLocal * vel_top_angular[i+1] ); 1366 } 1367 1368 f_zero_acc_bottom_linear[i+1] += m_links[i].m_inertiaLocal * vel_top_angular[i+1] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[i+1].norm()); 1369 1370 // calculate Ihat_i^A 1371 inertia_top_left[i+1] = btMatrix3x3(0,0,0,0,0,0,0,0,0);//::Zero(); 1372 inertia_top_right[i+1].setValue(m_links[i].m_mass, 0, 0, 1373 0, m_links[i].m_mass, 0, 1374 0, 0, m_links[i].m_mass); 1375 inertia_bottom_left[i+1].setValue(m_links[i].m_inertiaLocal[0], 0, 0, 1376 0, m_links[i].m_inertiaLocal[1], 0, 1377 0, 0, m_links[i].m_inertiaLocal[2]); 1378 } 1379 1380 1381 // 'Downward' loop. 1382 // (part of TreeForwardDynamics in Mirtich.) 1383 for (int i = num_links - 1; i >= 0; --i) { 1384 1385 h_top[i] = inertia_top_left[i+1] * m_links[i].getAxisTop(0) + inertia_top_right[i+1] * m_links[i].getAxisBottom(0); 1386 h_bottom[i] = inertia_bottom_left[i+1] * m_links[i].getAxisTop(0) + inertia_top_left[i+1].transpose() * m_links[i].getAxisBottom(0); 1387 btScalar val = SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), h_top[i], h_bottom[i]); 1388 D[i] = val; 1389 //Y1 = joint torque - zero acceleration force - coriolis force 1390 Y1[i] = m_links[i].m_jointTorque[0] 1391 - SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), f_zero_acc_top_angular[i+1], f_zero_acc_bottom_linear[i+1]) 1392 - SpatialDotProduct(h_top[i], h_bottom[i], coriolis_top_angular[i], coriolis_bottom_linear[i]); 1393 1394 const int parent = m_links[i].m_parent; 1395 1396 btAssert(D[i]!=0.f); 1397 // Ip += pXi * (Ii - hi hi' / Di) * iXp 1398 const btScalar one_over_di = 1.0f / D[i]; 1399 1400 1401 1402 1403 const btMatrix3x3 TL = inertia_top_left[i+1] - vecMulVecTranspose(one_over_di * h_top[i] , h_bottom[i]); 1404 const btMatrix3x3 TR = inertia_top_right[i+1] - vecMulVecTranspose(one_over_di * h_top[i] , h_top[i]); 1405 const btMatrix3x3 BL = inertia_bottom_left[i+1]- vecMulVecTranspose(one_over_di * h_bottom[i] , h_bottom[i]); 1406 1407 1408 btMatrix3x3 r_cross; 1409 r_cross.setValue( 1410 0, -m_links[i].m_cachedRVector[2], m_links[i].m_cachedRVector[1], 1411 m_links[i].m_cachedRVector[2], 0, -m_links[i].m_cachedRVector[0], 1412 -m_links[i].m_cachedRVector[1], m_links[i].m_cachedRVector[0], 0); 1413 1414 inertia_top_left[parent+1] += rot_from_parent[i+1].transpose() * ( TL - TR * r_cross ) * rot_from_parent[i+1]; 1415 inertia_top_right[parent+1] += rot_from_parent[i+1].transpose() * TR * rot_from_parent[i+1]; 1416 inertia_bottom_left[parent+1] += rot_from_parent[i+1].transpose() * 1417 (r_cross * (TL - TR * r_cross) + BL - TL.transpose() * r_cross) * rot_from_parent[i+1]; 1418 1419 1420 // Zp += pXi * (Zi + Ii*ci + hi*Yi/Di) 1421 btVector3 in_top, in_bottom, out_top, out_bottom; 1422 const btScalar Y_over_D = Y1[i] * one_over_di; 1423 in_top = f_zero_acc_top_angular[i+1] 1424 + inertia_top_left[i+1] * coriolis_top_angular[i] 1425 + inertia_top_right[i+1] * coriolis_bottom_linear[i] 1426 + Y_over_D * h_top[i]; 1427 in_bottom = f_zero_acc_bottom_linear[i+1] 1428 + inertia_bottom_left[i+1] * coriolis_top_angular[i] 1429 + inertia_top_left[i+1].transpose() * coriolis_bottom_linear[i] 1430 + Y_over_D * h_bottom[i]; 1431 InverseSpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector, 1432 in_top, in_bottom, out_top, out_bottom); 1433 f_zero_acc_top_angular[parent+1] += out_top; 1434 f_zero_acc_bottom_linear[parent+1] += out_bottom; 1435 } 1436 1437 1438 // Second 'upward' loop 1439 // (part of TreeForwardDynamics in Mirtich) 1440 1441 if (m_fixedBase) 1442 { 1443 accel_top[0] = accel_bottom[0] = btVector3(0,0,0); 1444 } 1445 else 1446 { 1447 if (num_links > 0) 1448 { 1449 //Matrix<btScalar, 6, 6> Imatrix; 1450 //Imatrix.block<3,3>(0,0) = inertia_top_left[0]; 1451 //Imatrix.block<3,3>(3,0) = inertia_bottom_left[0]; 1452 //Imatrix.block<3,3>(0,3) = inertia_top_right[0]; 1453 //Imatrix.block<3,3>(3,3) = inertia_top_left[0].transpose(); 1454 //cached_imatrix_lu.reset(new Eigen::LU<Matrix<btScalar, 6, 6> >(Imatrix)); // TODO: Avoid memory allocation here? 1455 1456 m_cachedInertiaTopLeft = inertia_top_left[0]; 1457 m_cachedInertiaTopRight = inertia_top_right[0]; 1458 m_cachedInertiaLowerLeft = inertia_bottom_left[0]; 1459 m_cachedInertiaLowerRight= inertia_top_left[0].transpose(); 1460 1461 } 1462 btVector3 rhs_top (f_zero_acc_top_angular[0][0], f_zero_acc_top_angular[0][1], f_zero_acc_top_angular[0][2]); 1463 btVector3 rhs_bot (f_zero_acc_bottom_linear[0][0], f_zero_acc_bottom_linear[0][1], f_zero_acc_bottom_linear[0][2]); 1464 float result[6]; 1465 1466 solveImatrix(rhs_top, rhs_bot, result); 1467 // printf("result=%f,%f,%f,%f,%f,%f\n",result[0],result[0],result[0],result[0],result[0],result[0]); 1468 for (int i = 0; i < 3; ++i) { 1469 accel_top[0][i] = -result[i]; 1470 accel_bottom[0][i] = -result[i+3]; 1471 } 1472 1473 } 1474 1475 // now do the loop over the m_links 1476 for (int i = 0; i < num_links; ++i) 1477 { 1478 //acceleration of the child link = acceleration of the parent transformed into child frame + 1479 // + coriolis acceleration 1480 // + joint acceleration 1481 const int parent = m_links[i].m_parent; 1482 SpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector, 1483 accel_top[parent+1], accel_bottom[parent+1], 1484 accel_top[i+1], accel_bottom[i+1]); 1485 1486 1487 joint_accel[i] = (Y1[i] - SpatialDotProduct(h_top[i], h_bottom[i], accel_top[i+1], accel_bottom[i+1])) / D[i]; 1488 accel_top[i+1] += coriolis_top_angular[i] + joint_accel[i] * m_links[i].getAxisTop(0); 1489 accel_bottom[i+1] += coriolis_bottom_linear[i] + joint_accel[i] * m_links[i].getAxisBottom(0); 1490 } 1491 1492 // transform base accelerations back to the world frame. 1493 btVector3 omegadot_out = rot_from_parent[0].transpose() * accel_top[0]; 1494 output[0] = omegadot_out[0]; 1495 output[1] = omegadot_out[1]; 1496 output[2] = omegadot_out[2]; 1497 1498 btVector3 vdot_out = rot_from_parent[0].transpose() * accel_bottom[0]; 1499 output[3] = vdot_out[0]; 1500 output[4] = vdot_out[1]; 1501 output[5] = vdot_out[2]; 1502 1503 ///////////////// 1504 //printf("q = ["); 1505 //printf("%.2f, %.2f, %.2f, %.2f, %.2f, %.2f, %.2f", m_baseQuat.x(), m_baseQuat.y(), m_baseQuat.z(), m_baseQuat.w(), m_basePos.x(), m_basePos.y(), m_basePos.z()); 1506 //for(int link = 0; link < getNumLinks(); ++link) 1507 // printf("%.2f ", m_links[link].m_jointPos[0]); 1508 //printf("]\n"); 1509 //// 1510 //printf("qd = ["); 1511 //for(int dof = 0; dof < getNumLinks() + 6; ++dof) 1512 // printf("%.2f ", m_realBuf[dof]); 1513 //printf("]\n"); 1514 //// 1515 //printf("qdd = ["); 1516 //for(int dof = 0; dof < getNumLinks() + 6; ++dof) 1517 // printf("%.2f ", output[dof]); 1518 //printf("]\n"); 1519 ///////////////// 1520 1521 // Final step: add the accelerations (times dt) to the velocities. 1522 //todo: do we already need to update the velocities, or can we move this into the constraint solver? 1523 //the gravity (and other forces) will cause an undesired bounce/restitution effect when using this approach 1524 applyDeltaVee(output, dt); 1525 1526 1527 } 1528 1529 void btMultiBody::solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, float result[6]) const 1530 { 1531 int num_links = getNumLinks(); 1532 ///solve I * x = rhs, so the result = invI * rhs 1533 if (num_links == 0) 1534 { 1535 // in the case of 0 m_links (i.e. a plain rigid body, not a multibody) rhs * invI is easier 1536 result[0] = rhs_bot[0] / m_baseInertia[0]; 1537 result[1] = rhs_bot[1] / m_baseInertia[1]; 1538 result[2] = rhs_bot[2] / m_baseInertia[2]; 1539 result[3] = rhs_top[0] / m_baseMass; 1540 result[4] = rhs_top[1] / m_baseMass; 1541 result[5] = rhs_top[2] / m_baseMass; 1542 } else 1543 { 1544 /// Special routine for calculating the inverse of a spatial inertia matrix 1545 ///the 6x6 matrix is stored as 4 blocks of 3x3 matrices 1546 btMatrix3x3 Binv = m_cachedInertiaTopRight.inverse()*-1.f; 1547 btMatrix3x3 tmp = m_cachedInertiaLowerRight * Binv; 1548 btMatrix3x3 invIupper_right = (tmp * m_cachedInertiaTopLeft + m_cachedInertiaLowerLeft).inverse(); 1549 tmp = invIupper_right * m_cachedInertiaLowerRight; 1550 btMatrix3x3 invI_upper_left = (tmp * Binv); 1551 btMatrix3x3 invI_lower_right = (invI_upper_left).transpose(); 1552 tmp = m_cachedInertiaTopLeft * invI_upper_left; 1553 tmp[0][0]-= 1.0; 1554 tmp[1][1]-= 1.0; 1555 tmp[2][2]-= 1.0; 1556 btMatrix3x3 invI_lower_left = (Binv * tmp); 1557 1558 //multiply result = invI * rhs 1559 { 1560 btVector3 vtop = invI_upper_left*rhs_top; 1561 btVector3 tmp; 1562 tmp = invIupper_right * rhs_bot; 1563 vtop += tmp; 1564 btVector3 vbot = invI_lower_left*rhs_top; 1565 tmp = invI_lower_right * rhs_bot; 1566 vbot += tmp; 1567 result[0] = vtop[0]; 1568 result[1] = vtop[1]; 1569 result[2] = vtop[2]; 1570 result[3] = vbot[0]; 1571 result[4] = vbot[1]; 1572 result[5] = vbot[2]; 1573 } 1574 1575 } 1576 } 1577 void btMultiBody::solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionVector &result) const 1578 { 1579 int num_links = getNumLinks(); 1580 ///solve I * x = rhs, so the result = invI * rhs 1581 if (num_links == 0) 1582 { 1583 // in the case of 0 m_links (i.e. a plain rigid body, not a multibody) rhs * invI is easier 1584 result.setAngular(rhs.getAngular() / m_baseInertia); 1585 result.setLinear(rhs.getLinear() / m_baseMass); 1586 } else 1587 { 1588 /// Special routine for calculating the inverse of a spatial inertia matrix 1589 ///the 6x6 matrix is stored as 4 blocks of 3x3 matrices 1590 btMatrix3x3 Binv = m_cachedInertiaTopRight.inverse()*-1.f; 1591 btMatrix3x3 tmp = m_cachedInertiaLowerRight * Binv; 1592 btMatrix3x3 invIupper_right = (tmp * m_cachedInertiaTopLeft + m_cachedInertiaLowerLeft).inverse(); 1593 tmp = invIupper_right * m_cachedInertiaLowerRight; 1594 btMatrix3x3 invI_upper_left = (tmp * Binv); 1595 btMatrix3x3 invI_lower_right = (invI_upper_left).transpose(); 1596 tmp = m_cachedInertiaTopLeft * invI_upper_left; 1597 tmp[0][0]-= 1.0; 1598 tmp[1][1]-= 1.0; 1599 tmp[2][2]-= 1.0; 1600 btMatrix3x3 invI_lower_left = (Binv * tmp); 1601 1602 //multiply result = invI * rhs 1603 { 1604 btVector3 vtop = invI_upper_left*rhs.getLinear(); 1605 btVector3 tmp; 1606 tmp = invIupper_right * rhs.getAngular(); 1607 vtop += tmp; 1608 btVector3 vbot = invI_lower_left*rhs.getLinear(); 1609 tmp = invI_lower_right * rhs.getAngular(); 1610 vbot += tmp; 1611 result.setVector(vtop, vbot); 1612 } 1613 1614 } 1615 } 1616 1617 void btMultiBody::mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const 1618 { 1619 for (int row = 0; row < rowsA; row++) 1620 { 1621 for (int col = 0; col < colsB; col++) 1622 { 1623 pC[row * colsB + col] = 0.f; 1624 for (int inner = 0; inner < rowsB; inner++) 1625 { 1626 pC[row * colsB + col] += pA[row * colsA + inner] * pB[col + inner * colsB]; 1627 } 1628 } 1629 } 1630 } 1631 1632 void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output, 1633 btAlignedObjectArray<btScalar> &scratch_r, btAlignedObjectArray<btVector3> &scratch_v) const 1634 { 1635 // Temporary matrices/vectors -- use scratch space from caller 1636 // so that we don't have to keep reallocating every frame 1637 1638 1639 int num_links = getNumLinks(); 1640 scratch_r.resize(m_dofCount); 1641 scratch_v.resize(4*num_links + 4); 1642 1643 btScalar * r_ptr = m_dofCount ? &scratch_r[0] : 0; 1644 btVector3 * v_ptr = &scratch_v[0]; 1645 1646 // zhat_i^A (scratch space) 1647 btSpatialForceVector * zeroAccSpatFrc = (btSpatialForceVector *)v_ptr; 1648 v_ptr += num_links * 2 + 2; 1649 1650 // rot_from_parent (cached from calcAccelerations) 1651 const btMatrix3x3 * rot_from_parent = &m_matrixBuf[0]; 1652 1653 // hhat (cached), accel (scratch) 1654 // hhat is NOT stored for the base (but ahat is) 1655 const btSpatialForceVector * h = (btSpatialForceVector *)(m_dofCount > 0 ? &m_vectorBuf[0] : 0); 1656 btSpatialMotionVector * spatAcc = (btSpatialMotionVector *)v_ptr; 1657 v_ptr += num_links * 2 + 2; 1658 1659 // Y_i (scratch), invD_i (cached) 1660 const btScalar * invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0; 1661 btScalar * Y = r_ptr; 1662 //////////////// 1663 //aux variables 1664 static btScalar invD_times_Y[6]; //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies 1665 static btSpatialMotionVector result; //holds results of the SolveImatrix op; it is a spatial motion vector (accel) 1666 static btScalar Y_minus_hT_a[6]; //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough 1667 static btSpatialForceVector spatForceVecTemps[6]; //6 temporary spatial force vectors 1668 static btSpatialTransformationMatrix fromParent; 1669 ///////////////// 1670 1671 // First 'upward' loop. 1672 // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich. 1673 1674 // Fill in zero_acc 1675 // -- set to force/torque on the base, zero otherwise 1676 if (m_fixedBase) 1677 { 1678 zeroAccSpatFrc[0].setZero(); 1679 } else 1680 { 1681 //test forces 1682 fromParent.m_rotMat = rot_from_parent[0]; 1683 fromParent.transformRotationOnly(btSpatialForceVector(-force[0],-force[1],-force[2], -force[3],-force[4],-force[5]), zeroAccSpatFrc[0]); 1684 } 1685 for (int i = 0; i < num_links; ++i) 1686 { 1687 zeroAccSpatFrc[i+1].setZero(); 1688 } 1689 1690 // 'Downward' loop. 1691 // (part of TreeForwardDynamics in Mirtich.) 1692 for (int i = num_links - 1; i >= 0; --i) 1693 { 1694 const int parent = m_links[i].m_parent; 1695 fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector; 1696 1697 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) 1698 { 1699 Y[m_links[i].m_dofOffset + dof] = force[6 + m_links[i].m_dofOffset + dof] 1700 - m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i+1]) 1701 ; 1702 } 1703 1704 btVector3 in_top, in_bottom, out_top, out_bottom; 1705 const btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset]; 1706 1707 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) 1708 { 1709 invD_times_Y[dof] = 0.f; 1710 1711 for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2) 1712 { 1713 invD_times_Y[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * Y[m_links[i].m_dofOffset + dof2]; 1714 } 1715 } 1716 1717 // Zp += pXi * (Zi + hi*Yi/Di) 1718 spatForceVecTemps[0] = zeroAccSpatFrc[i+1]; 1719 1720 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) 1721 { 1722 const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof]; 1723 // 1724 spatForceVecTemps[0] += hDof * invD_times_Y[dof]; 1725 } 1726 1727 1728 fromParent.transformInverse(spatForceVecTemps[0], spatForceVecTemps[1]); 1729 1730 zeroAccSpatFrc[parent+1] += spatForceVecTemps[1]; 1731 } 1732 1733 // ptr to the joint accel part of the output 1734 btScalar * joint_accel = output + 6; 1735 1736 1737 // Second 'upward' loop 1738 // (part of TreeForwardDynamics in Mirtich) 1739 1740 if (m_fixedBase) 1741 { 1742 spatAcc[0].setZero(); 1743 } 1744 else 1745 { 1746 solveImatrix(zeroAccSpatFrc[0], result); 1747 spatAcc[0] = -result; 1748 1749 } 1750 1751 // now do the loop over the m_links 1752 for (int i = 0; i < num_links; ++i) 1753 { 1754 const int parent = m_links[i].m_parent; 1755 fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector; 1756 1757 fromParent.transform(spatAcc[parent+1], spatAcc[i+1]); 1758 1759 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) 1760 { 1761 const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof]; 1762 // 1763 Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i+1].dot(hDof); 1764 } 1765 1766 const btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset]; 1767 mulMatrix(const_cast<btScalar*>(invDi), Y_minus_hT_a, m_links[i].m_dofCount, m_links[i].m_dofCount, m_links[i].m_dofCount, 1, &joint_accel[m_links[i].m_dofOffset]); 1768 1769 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) 1770 spatAcc[i+1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof]; 1771 } 1772 1773 // transform base accelerations back to the world frame. 1774 btVector3 omegadot_out; 1775 omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].getAngular(); 1776 output[0] = omegadot_out[0]; 1777 output[1] = omegadot_out[1]; 1778 output[2] = omegadot_out[2]; 1779 1780 btVector3 vdot_out; 1781 vdot_out = rot_from_parent[0].transpose() * spatAcc[0].getLinear(); 1782 output[3] = vdot_out[0]; 1783 output[4] = vdot_out[1]; 1784 output[5] = vdot_out[2]; 1785 1786 ///////////////// 1787 //printf("delta = ["); 1788 //for(int dof = 0; dof < getNumDofs() + 6; ++dof) 1789 // printf("%.2f ", output[dof]); 1790 //printf("]\n"); 1791 ///////////////// 1792 } 1793 1794 1795 void btMultiBody::calcAccelerationDeltas(const btScalar *force, btScalar *output, 1796 btAlignedObjectArray<btScalar> &scratch_r, btAlignedObjectArray<btVector3> &scratch_v) const 1797 { 1798 // Temporary matrices/vectors -- use scratch space from caller 1799 // so that we don't have to keep reallocating every frame 1800 int num_links = getNumLinks(); 1801 scratch_r.resize(num_links); 1802 scratch_v.resize(4*num_links + 4); 1803 1804 btScalar * r_ptr = num_links == 0 ? 0 : &scratch_r[0]; 1805 btVector3 * v_ptr = &scratch_v[0]; 1806 1807 // zhat_i^A (scratch space) 1808 btVector3 * zero_acc_top_angular = v_ptr; v_ptr += num_links + 1; 1809 btVector3 * zero_acc_bottom_linear = v_ptr; v_ptr += num_links + 1; 1810 1811 // rot_from_parent (cached from calcAccelerations) 1812 const btMatrix3x3 * rot_from_parent = &m_matrixBuf[0]; 1813 1814 // hhat (cached), accel (scratch) 1815 const btVector3 * h_top = num_links > 0 ? &m_vectorBuf[0] : 0; 1816 const btVector3 * h_bottom = num_links > 0 ? &m_vectorBuf[num_links] : 0; 1817 btVector3 * accel_top = v_ptr; v_ptr += num_links + 1; 1818 btVector3 * accel_bottom = v_ptr; v_ptr += num_links + 1; 1819 1820 // Y_i (scratch), D_i (cached) 1821 btScalar * Y = r_ptr; r_ptr += num_links; 1822 const btScalar * D = num_links > 0 ? &m_realBuf[6 + num_links] : 0; 1823 1824 btAssert(num_links == 0 || r_ptr - &scratch_r[0] == scratch_r.size()); 1825 btAssert(v_ptr - &scratch_v[0] == scratch_v.size()); 1826 1827 1828 1829 // First 'upward' loop. 1830 // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich. 1831 1832 btVector3 input_force(force[3],force[4],force[5]); 1833 btVector3 input_torque(force[0],force[1],force[2]); 1834 1835 // Fill in zero_acc 1836 // -- set to force/torque on the base, zero otherwise 1837 if (m_fixedBase) 1838 { 1839 zero_acc_top_angular[0] = zero_acc_bottom_linear[0] = btVector3(0,0,0); 1840 } else 1841 { 1842 zero_acc_top_angular[0] = - (rot_from_parent[0] * input_force); 1843 zero_acc_bottom_linear[0] = - (rot_from_parent[0] * input_torque); 1844 } 1845 for (int i = 0; i < num_links; ++i) 1846 { 1847 zero_acc_top_angular[i+1] = zero_acc_bottom_linear[i+1] = btVector3(0,0,0); 1848 } 1849 1850 // 'Downward' loop. 1851 for (int i = num_links - 1; i >= 0; --i) 1852 { 1853 // btScalar sdp = -SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1]); 1854 Y[i] = - SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1]); 1855 Y[i] += force[6 + i]; // add joint torque 1856 1857 const int parent = m_links[i].m_parent; 1858 1859 // Zp += pXi * (Zi + hi*Yi/Di) 1860 btVector3 in_top, in_bottom, out_top, out_bottom; 1861 const btScalar Y_over_D = Y[i] / D[i]; 1862 in_top = zero_acc_top_angular[i+1] + Y_over_D * h_top[i]; 1863 in_bottom = zero_acc_bottom_linear[i+1] + Y_over_D * h_bottom[i]; 1864 InverseSpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector, 1865 in_top, in_bottom, out_top, out_bottom); 1866 zero_acc_top_angular[parent+1] += out_top; 1867 zero_acc_bottom_linear[parent+1] += out_bottom; 1868 } 1869 1870 // ptr to the joint accel part of the output 1871 btScalar * joint_accel = output + 6; 1872 1873 // Second 'upward' loop 1874 if (m_fixedBase) 1875 { 1876 accel_top[0] = accel_bottom[0] = btVector3(0,0,0); 1877 } else 1878 { 1879 btVector3 rhs_top (zero_acc_top_angular[0][0], zero_acc_top_angular[0][1], zero_acc_top_angular[0][2]); 1880 btVector3 rhs_bot (zero_acc_bottom_linear[0][0], zero_acc_bottom_linear[0][1], zero_acc_bottom_linear[0][2]); 1881 1882 float result[6]; 1883 solveImatrix(rhs_top,rhs_bot, result); 1884 // printf("result=%f,%f,%f,%f,%f,%f\n",result[0],result[0],result[0],result[0],result[0],result[0]); 1885 1886 for (int i = 0; i < 3; ++i) { 1887 accel_top[0][i] = -result[i]; 1888 accel_bottom[0][i] = -result[i+3]; 1889 } 1890 1891 } 1892 1893 // now do the loop over the m_links 1894 for (int i = 0; i < num_links; ++i) { 1895 const int parent = m_links[i].m_parent; 1896 SpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector, 1897 accel_top[parent+1], accel_bottom[parent+1], 1898 accel_top[i+1], accel_bottom[i+1]); 1899 btScalar Y_minus_hT_a = (Y[i] - SpatialDotProduct(h_top[i], h_bottom[i], accel_top[i+1], accel_bottom[i+1])); 1900 joint_accel[i] = Y_minus_hT_a / D[i]; 1901 accel_top[i+1] += joint_accel[i] * m_links[i].getAxisTop(0); 1902 accel_bottom[i+1] += joint_accel[i] * m_links[i].getAxisBottom(0); 1903 } 1904 1905 // transform base accelerations back to the world frame. 1906 btVector3 omegadot_out; 1907 omegadot_out = rot_from_parent[0].transpose() * accel_top[0]; 1908 output[0] = omegadot_out[0]; 1909 output[1] = omegadot_out[1]; 1910 output[2] = omegadot_out[2]; 1911 1912 btVector3 vdot_out; 1913 vdot_out = rot_from_parent[0].transpose() * accel_bottom[0]; 1914 1915 output[3] = vdot_out[0]; 1916 output[4] = vdot_out[1]; 1917 output[5] = vdot_out[2]; 1918 1919 1920 ///////////////// 1921 /* 1922 int ndof = getNumLinks() + 6; 1923 printf("test force(impulse) (%d) = [\n",ndof); 1924 1925 for (int i=0;i<ndof;i++) 1926 { 1927 printf("%.2f ", force[i]); 1928 printf("]\n"); 1929 } 1930 1931 printf("delta(%d) = [",ndof); 1932 for(int dof = 0; dof < getNumLinks() + 6; ++dof) 1933 printf("%.2f ", output[dof]); 1934 printf("]\n"); 1935 ///////////////// 1936 */ 1937 1938 //int dummy = 0; 1939 } 1940 1941 void btMultiBody::stepPositions(btScalar dt) 1942 { 1943 int num_links = getNumLinks(); 1944 // step position by adding dt * velocity 1945 btVector3 v = getBaseVel(); 1946 m_basePos += dt * v; 1947 1948 // "exponential map" method for the rotation 1949 btVector3 base_omega = getBaseOmega(); 1950 const btScalar omega_norm = base_omega.norm(); 1951 const btScalar omega_times_dt = omega_norm * dt; 1952 const btScalar SMALL_ROTATION_ANGLE = 0.02f; // Theoretically this should be ~ pow(FLT_EPSILON,0.25) which is ~ 0.0156 1953 if (fabs(omega_times_dt) < SMALL_ROTATION_ANGLE) 1954 { 1955 const btScalar xsq = omega_times_dt * omega_times_dt; // |omega|^2 * dt^2 1956 const btScalar sin_term = dt * (xsq / 48.0f - 0.5f); // -sin(0.5*dt*|omega|) / |omega| 1957 const btScalar cos_term = 1.0f - xsq / 8.0f; // cos(0.5*dt*|omega|) 1958 m_baseQuat = m_baseQuat * btQuaternion(sin_term * base_omega[0],sin_term * base_omega[1],sin_term * base_omega[2],cos_term); 1959 } else 1960 { 1961 m_baseQuat = m_baseQuat * btQuaternion(base_omega / omega_norm,-omega_times_dt); 1962 } 1963 1964 // Make sure the quaternion represents a valid rotation. 1965 // (Not strictly necessary, but helps prevent any round-off errors from building up.) 1966 m_baseQuat.normalize(); 1967 1968 // Finally we can update m_jointPos for each of the m_links 1969 for (int i = 0; i < num_links; ++i) 1970 { 1971 float jointVel = getJointVel(i); 1972 m_links[i].m_jointPos[0] += dt * jointVel; 1973 m_links[i].updateCache(); 1974 } 1975 } 1976 1977 void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd) 1978 { 1979 int num_links = getNumLinks(); 1980 // step position by adding dt * velocity 1981 //btVector3 v = getBaseVel(); 1982 //m_basePos += dt * v; 1983 // 1984 btScalar *pBasePos = (pq ? &pq[4] : m_basePos); 1985 btScalar *pBaseVel = (pqd ? &pqd[3] : &m_realBuf[3]); //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety) 1986 // 1987 pBasePos[0] += dt * pBaseVel[0]; 1988 pBasePos[1] += dt * pBaseVel[1]; 1989 pBasePos[2] += dt * pBaseVel[2]; 1990 1991 /////////////////////////////// 1992 //local functor for quaternion integration (to avoid error prone redundancy) 1993 struct 1994 { 1995 //"exponential map" based on btTransformUtil::integrateTransform(..) 1996 void operator() (const btVector3 &omega, btQuaternion &quat, bool baseBody, btScalar dt) 1997 { 1998 //baseBody => quat is alias and omega is global coor 1999 //!baseBody => quat is alibi and omega is local coor 2000 2001 btVector3 axis; 2002 btVector3 angvel; 2003 2004 if(!baseBody) 2005 angvel = quatRotate(quat, omega); //if quat is not m_baseQuat, it is alibi => ok 2006 else 2007 angvel = omega; 2008 2009 btScalar fAngle = angvel.length(); 2010 //limit the angular motion 2011 if (fAngle * dt > ANGULAR_MOTION_THRESHOLD) 2012 { 2013 fAngle = btScalar(0.5)*SIMD_HALF_PI / dt; 2014 } 2015 2016 if ( fAngle < btScalar(0.001) ) 2017 { 2018 // use Taylor's expansions of sync function 2019 axis = angvel*( btScalar(0.5)*dt-(dt*dt*dt)*(btScalar(0.020833333333))*fAngle*fAngle ); 2020 } 2021 else 2022 { 2023 // sync(fAngle) = sin(c*fAngle)/t 2024 axis = angvel*( btSin(btScalar(0.5)*fAngle*dt)/fAngle ); 2025 } 2026 2027 if(!baseBody) 2028 quat = btQuaternion(axis.x(),axis.y(),axis.z(),btCos( fAngle*dt*btScalar(0.5) )) * quat; 2029 else 2030 quat = quat * btQuaternion(-axis.x(),-axis.y(),-axis.z(),btCos( fAngle*dt*btScalar(0.5) )); 2031 //equivalent to: quat = (btQuaternion(axis.x(),axis.y(),axis.z(),btCos( fAngle*dt*btScalar(0.5) )) * quat.inverse()).inverse(); 2032 2033 quat.normalize(); 2034 } 2035 } pQuatUpdateFun; 2036 /////////////////////////////// 2037 2038 //pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt); 2039 // 2040 btScalar *pBaseQuat = pq ? pq : m_baseQuat; 2041 btScalar *pBaseOmega = pqd ? pqd : &m_realBuf[0]; //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety) 2042 // 2043 static btQuaternion baseQuat; baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]); 2044 static btVector3 baseOmega; baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]); 2045 pQuatUpdateFun(baseOmega, baseQuat, true, dt); 2046 pBaseQuat[0] = baseQuat.x(); 2047 pBaseQuat[1] = baseQuat.y(); 2048 pBaseQuat[2] = baseQuat.z(); 2049 pBaseQuat[3] = baseQuat.w(); 2050 2051 2052 //printf("pBaseOmega = %.4f %.4f %.4f\n", pBaseOmega->x(), pBaseOmega->y(), pBaseOmega->z()); 2053 //printf("pBaseVel = %.4f %.4f %.4f\n", pBaseVel->x(), pBaseVel->y(), pBaseVel->z()); 2054 //printf("baseQuat = %.4f %.4f %.4f %.4f\n", pBaseQuat->x(), pBaseQuat->y(), pBaseQuat->z(), pBaseQuat->w()); 2055 2056 if(pq) 2057 pq += 7; 2058 if(pqd) 2059 pqd += 6; 2060 2061 // Finally we can update m_jointPos for each of the m_links 2062 for (int i = 0; i < num_links; ++i) 2063 { 2064 btScalar *pJointPos = (pq ? pq : &m_links[i].m_jointPos[0]); 2065 btScalar *pJointVel = (pqd ? pqd : getJointVelMultiDof(i)); 2066 2067 switch(m_links[i].m_jointType) 2068 { 2069 case btMultibodyLink::ePrismatic: 2070 case btMultibodyLink::eRevolute: 2071 { 2072 btScalar jointVel = pJointVel[0]; 2073 pJointPos[0] += dt * jointVel; 2074 break; 2075 } 2076 case btMultibodyLink::eSpherical: 2077 { 2078 static btVector3 jointVel; jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]); 2079 static btQuaternion jointOri; jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]); 2080 pQuatUpdateFun(jointVel, jointOri, false, dt); 2081 pJointPos[0] = jointOri.x(); pJointPos[1] = jointOri.y(); pJointPos[2] = jointOri.z(); pJointPos[3] = jointOri.w(); 2082 break; 2083 } 2084 case btMultibodyLink::ePlanar: 2085 { 2086 pJointPos[0] += dt * getJointVelMultiDof(i)[0]; 2087 2088 btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2); 2089 btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), pJointPos[0]), q0_coors_qd1qd2); 2090 pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt; 2091 pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt; 2092 2093 break; 2094 } 2095 default: 2096 { 2097 } 2098 2099 } 2100 2101 m_links[i].updateCacheMultiDof(pq); 2102 2103 if(pq) 2104 pq += m_links[i].m_posVarCount; 2105 if(pqd) 2106 pqd += m_links[i].m_dofCount; 2107 } 2108 } 2109 2110 void btMultiBody::filConstraintJacobianMultiDof(int link, 2111 const btVector3 &contact_point, 2112 const btVector3 &normal_ang, 2113 const btVector3 &normal_lin, 2114 btScalar *jac, 2115 btAlignedObjectArray<btScalar> &scratch_r, 2116 btAlignedObjectArray<btVector3> &scratch_v, 2117 btAlignedObjectArray<btMatrix3x3> &scratch_m) const 2118 { 2119 // temporary space 2120 int num_links = getNumLinks(); 2121 int m_dofCount = getNumDofs(); 2122 scratch_v.resize(3*num_links + 3); //(num_links + base) offsets + (num_links + base) normals_lin + (num_links + base) normals_ang 2123 scratch_m.resize(num_links + 1); 2124 2125 btVector3 * v_ptr = &scratch_v[0]; 2126 btVector3 * p_minus_com_local = v_ptr; v_ptr += num_links + 1; 2127 btVector3 * n_local_lin = v_ptr; v_ptr += num_links + 1; 2128 btVector3 * n_local_ang = v_ptr; v_ptr += num_links + 1; 2129 btAssert(v_ptr - &scratch_v[0] == scratch_v.size()); 2130 2131 scratch_r.resize(m_dofCount); 2132 btScalar * results = m_dofCount > 0 ? &scratch_r[0] : 0; 2133 2134 btMatrix3x3 * rot_from_world = &scratch_m[0]; 2135 2136 const btVector3 p_minus_com_world = contact_point - m_basePos; 2137 const btVector3 &normal_lin_world = normal_lin; //convenience 2138 const btVector3 &normal_ang_world = normal_ang; 2139 2140 rot_from_world[0] = btMatrix3x3(m_baseQuat); 2141 2142 // omega coeffients first. 2143 btVector3 omega_coeffs_world; 2144 omega_coeffs_world = p_minus_com_world.cross(normal_lin_world); 2145 jac[0] = omega_coeffs_world[0] + normal_ang_world[0]; 2146 jac[1] = omega_coeffs_world[1] + normal_ang_world[1]; 2147 jac[2] = omega_coeffs_world[2] + normal_ang_world[2]; 2148 // then v coefficients 2149 jac[3] = normal_lin_world[0]; 2150 jac[4] = normal_lin_world[1]; 2151 jac[5] = normal_lin_world[2]; 2152 2153 //create link-local versions of p_minus_com and normal 2154 p_minus_com_local[0] = rot_from_world[0] * p_minus_com_world; 2155 n_local_lin[0] = rot_from_world[0] * normal_lin_world; 2156 n_local_ang[0] = rot_from_world[0] * normal_ang_world; 2157 2158 // Set remaining jac values to zero for now. 2159 for (int i = 6; i < 6 + m_dofCount; ++i) 2160 { 2161 jac[i] = 0; 2162 } 2163 2164 // Qdot coefficients, if necessary. 2165 if (num_links > 0 && link > -1) { 2166 2167 // TODO: speed this up -- don't calculate for m_links we don't need. 2168 // (Also, we are making 3 separate calls to this function, for the normal & the 2 friction directions, 2169 // which is resulting in repeated work being done...) 2170 2171 // calculate required normals & positions in the local frames. 2172 for (int i = 0; i < num_links; ++i) { 2173 2174 // transform to local frame 2175 const int parent = m_links[i].m_parent; 2176 const btMatrix3x3 mtx(m_links[i].m_cachedRotParentToThis); 2177 rot_from_world[i+1] = mtx * rot_from_world[parent+1]; 2178 2179 n_local_lin[i+1] = mtx * n_local_lin[parent+1]; 2180 n_local_ang[i+1] = mtx * n_local_ang[parent+1]; 2181 p_minus_com_local[i+1] = mtx * p_minus_com_local[parent+1] - m_links[i].m_cachedRVector; 2182 2183 // calculate the jacobian entry 2184 switch(m_links[i].m_jointType) 2185 { 2186 case btMultibodyLink::eRevolute: 2187 { 2188 results[m_links[i].m_dofOffset] = n_local_lin[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(0)); 2189 results[m_links[i].m_dofOffset] += n_local_ang[i+1].dot(m_links[i].getAxisTop(0)); 2190 break; 2191 } 2192 case btMultibodyLink::ePrismatic: 2193 { 2194 results[m_links[i].m_dofOffset] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(0)); 2195 break; 2196 } 2197 case btMultibodyLink::eSpherical: 2198 { 2199 results[m_links[i].m_dofOffset + 0] = n_local_lin[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(0)); 2200 results[m_links[i].m_dofOffset + 1] = n_local_lin[i+1].dot(m_links[i].getAxisTop(1).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(1)); 2201 results[m_links[i].m_dofOffset + 2] = n_local_lin[i+1].dot(m_links[i].getAxisTop(2).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(2)); 2202 2203 results[m_links[i].m_dofOffset + 0] += n_local_ang[i+1].dot(m_links[i].getAxisTop(0)); 2204 results[m_links[i].m_dofOffset + 1] += n_local_ang[i+1].dot(m_links[i].getAxisTop(1)); 2205 results[m_links[i].m_dofOffset + 2] += n_local_ang[i+1].dot(m_links[i].getAxisTop(2)); 2206 2207 break; 2208 } 2209 case btMultibodyLink::ePlanar: 2210 { 2211 results[m_links[i].m_dofOffset + 0] = n_local_lin[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i+1]));// + m_links[i].getAxisBottom(0)); 2212 results[m_links[i].m_dofOffset + 1] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(1)); 2213 results[m_links[i].m_dofOffset + 2] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(2)); 2214 2215 break; 2216 } 2217 default: 2218 { 2219 } 2220 } 2221 2222 } 2223 2224 // Now copy through to output. 2225 //printf("jac[%d] = ", link); 2226 while (link != -1) 2227 { 2228 for(int dof = 0; dof < m_links[link].m_dofCount; ++dof) 2229 { 2230 jac[6 + m_links[link].m_dofOffset + dof] = results[m_links[link].m_dofOffset + dof]; 2231 //printf("%.2f\t", jac[6 + m_links[link].m_dofOffset + dof]); 2232 } 2233 2234 link = m_links[link].m_parent; 2235 } 2236 //printf("]\n"); 2237 } 2238 } 2239 2240 void btMultiBody::fillContactJacobian(int link, 2241 const btVector3 &contact_point, 2242 const btVector3 &normal, 2243 btScalar *jac, 2244 btAlignedObjectArray<btScalar> &scratch_r, 2245 btAlignedObjectArray<btVector3> &scratch_v, 2246 btAlignedObjectArray<btMatrix3x3> &scratch_m) const 2247 { 2248 // temporary space 2249 int num_links = getNumLinks(); 2250 scratch_v.resize(2*num_links + 2); 2251 scratch_m.resize(num_links + 1); 2252 2253 btVector3 * v_ptr = &scratch_v[0]; 2254 btVector3 * p_minus_com = v_ptr; v_ptr += num_links + 1; 2255 btVector3 * n_local = v_ptr; v_ptr += num_links + 1; 2256 btAssert(v_ptr - &scratch_v[0] == scratch_v.size()); 2257 2258 scratch_r.resize(num_links); 2259 btScalar * results = num_links > 0 ? &scratch_r[0] : 0; 2260 2261 btMatrix3x3 * rot_from_world = &scratch_m[0]; 2262 2263 const btVector3 p_minus_com_world = contact_point - m_basePos; 2264 2265 rot_from_world[0] = btMatrix3x3(m_baseQuat); 2266 2267 p_minus_com[0] = rot_from_world[0] * p_minus_com_world; 2268 n_local[0] = rot_from_world[0] * normal; 2269 2270 // omega coeffients first. 2271 if (this->m_fixedBase) 2272 { 2273 for (int i=0;i<6;i++) 2274 { 2275 jac[i]=0; 2276 } 2277 } else 2278 { 2279 btVector3 omega_coeffs; 2280 2281 omega_coeffs = p_minus_com_world.cross(normal); 2282 jac[0] = omega_coeffs[0]; 2283 jac[1] = omega_coeffs[1]; 2284 jac[2] = omega_coeffs[2]; 2285 // then v coefficients 2286 jac[3] = normal[0]; 2287 jac[4] = normal[1]; 2288 jac[5] = normal[2]; 2289 } 2290 // Set remaining jac values to zero for now. 2291 for (int i = 6; i < 6 + num_links; ++i) { 2292 jac[i] = 0; 2293 } 2294 2295 // Qdot coefficients, if necessary. 2296 if (num_links > 0 && link > -1) { 2297 2298 // TODO: speed this up -- don't calculate for m_links we don't need. 2299 // (Also, we are making 3 separate calls to this function, for the normal & the 2 friction directions, 2300 // which is resulting in repeated work being done...) 2301 2302 // calculate required normals & positions in the local frames. 2303 for (int i = 0; i < num_links; ++i) { 2304 2305 // transform to local frame 2306 const int parent = m_links[i].m_parent; 2307 const btMatrix3x3 mtx(m_links[i].m_cachedRotParentToThis); 2308 rot_from_world[i+1] = mtx * rot_from_world[parent+1]; 2309 2310 n_local[i+1] = mtx * n_local[parent+1]; 2311 p_minus_com[i+1] = mtx * p_minus_com[parent+1] - m_links[i].m_cachedRVector; 2312 2313 // calculate the jacobian entry 2314 if (m_links[i].m_jointType == btMultibodyLink::eRevolute) { 2315 results[i] = n_local[i+1].dot( m_links[i].getAxisTop(0).cross(p_minus_com[i+1]) + m_links[i].getAxisBottom(0) ); 2316 } else { 2317 results[i] = n_local[i+1].dot( m_links[i].getAxisBottom(0) ); 2318 } 2319 } 2320 2321 // Now copy through to output. 2322 //printf("jac[%d] = ", link); 2323 while (link != -1) 2324 { 2325 jac[6 + link] = results[link]; 2326 //printf("%.2f\t", jac[6 + link]); 2327 link = m_links[link].m_parent; 2328 } 2329 //printf("]\n"); 2330 } 2331 } 2332 2333 void btMultiBody::wakeUp() 2334 { 2335 m_awake = true; 2336 } 2337 2338 void btMultiBody::goToSleep() 2339 { 2340 m_awake = false; 2341 } 2342 2343 void btMultiBody::checkMotionAndSleepIfRequired(btScalar timestep) 2344 { 2345 int num_links = getNumLinks(); 2346 extern bool gDisableDeactivation; 2347 if (!m_canSleep || gDisableDeactivation) 2348 { 2349 m_awake = true; 2350 m_sleepTimer = 0; 2351 return; 2352 } 2353 2354 // motion is computed as omega^2 + v^2 + (sum of squares of joint velocities) 2355 btScalar motion = 0; 2356 if(m_isMultiDof) 2357 { 2358 for (int i = 0; i < 6 + m_dofCount; ++i) 2359 motion += m_realBuf[i] * m_realBuf[i]; 2360 } 2361 else 2362 { 2363 for (int i = 0; i < 6 + num_links; ++i) 2364 motion += m_realBuf[i] * m_realBuf[i]; 2365 } 2366 2367 2368 if (motion < SLEEP_EPSILON) { 2369 m_sleepTimer += timestep; 2370 if (m_sleepTimer > SLEEP_TIMEOUT) { 2371 goToSleep(); 2372 } 2373 } else { 2374 m_sleepTimer = 0; 2375 if (!m_awake) 2376 wakeUp(); 2377 } 2378 } 2379 2380 2381 void btMultiBody::forwardKinematics(btAlignedObjectArray<btQuaternion>& world_to_local,btAlignedObjectArray<btVector3>& local_origin) 2382 { 2383 2384 int num_links = getNumLinks(); 2385 2386 // Cached 3x3 rotation matrices from parent frame to this frame. 2387 btMatrix3x3* rot_from_parent =(btMatrix3x3 *) &m_matrixBuf[0]; 2388 2389 rot_from_parent[0] = btMatrix3x3(m_baseQuat); //m_baseQuat assumed to be alias!? 2390 2391 for (int i = 0; i < num_links; ++i) 2392 { 2393 rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis); 2394 } 2395 2396 int nLinks = getNumLinks(); 2397 ///base + num m_links 2398 world_to_local.resize(nLinks+1); 2399 local_origin.resize(nLinks+1); 2400 2401 world_to_local[0] = getWorldToBaseRot(); 2402 local_origin[0] = getBasePos(); 2403 2404 for (int k=0;k<getNumLinks();k++) 2405 { 2406 const int parent = getParent(k); 2407 world_to_local[k+1] = getParentToLocalRot(k) * world_to_local[parent+1]; 2408 local_origin[k+1] = local_origin[parent+1] + (quatRotate(world_to_local[k+1].inverse() , getRVector(k))); 2409 } 2410 2411 for (int link=0;link<getNumLinks();link++) 2412 { 2413 int index = link+1; 2414 2415 btVector3 posr = local_origin[index]; 2416 btScalar quat[4]={-world_to_local[index].x(),-world_to_local[index].y(),-world_to_local[index].z(),world_to_local[index].w()}; 2417 btTransform tr; 2418 tr.setIdentity(); 2419 tr.setOrigin(posr); 2420 tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); 2421 getLink(link).m_cachedWorldTransform = tr; 2422 2423 } 2424 2425 } 2426 2427 void btMultiBody::updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQuaternion>& world_to_local,btAlignedObjectArray<btVector3>& local_origin) 2428 { 2429 world_to_local.resize(getNumLinks()+1); 2430 local_origin.resize(getNumLinks()+1); 2431 2432 world_to_local[0] = getWorldToBaseRot(); 2433 local_origin[0] = getBasePos(); 2434 2435 if (getBaseCollider()) 2436 { 2437 btVector3 posr = local_origin[0]; 2438 // float pos[4]={posr.x(),posr.y(),posr.z(),1}; 2439 btScalar quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()}; 2440 btTransform tr; 2441 tr.setIdentity(); 2442 tr.setOrigin(posr); 2443 tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); 2444 2445 getBaseCollider()->setWorldTransform(tr); 2446 2447 } 2448 2449 for (int k=0;k<getNumLinks();k++) 2450 { 2451 const int parent = getParent(k); 2452 world_to_local[k+1] = getParentToLocalRot(k) * world_to_local[parent+1]; 2453 local_origin[k+1] = local_origin[parent+1] + (quatRotate(world_to_local[k+1].inverse() , getRVector(k))); 2454 } 2455 2456 2457 for (int m=0;m<getNumLinks();m++) 2458 { 2459 btMultiBodyLinkCollider* col = getLink(m).m_collider; 2460 if (col) 2461 { 2462 int link = col->m_link; 2463 btAssert(link == m); 2464 2465 int index = link+1; 2466 2467 btVector3 posr = local_origin[index]; 2468 // float pos[4]={posr.x(),posr.y(),posr.z(),1}; 2469 btScalar quat[4]={-world_to_local[index].x(),-world_to_local[index].y(),-world_to_local[index].z(),world_to_local[index].w()}; 2470 btTransform tr; 2471 tr.setIdentity(); 2472 tr.setOrigin(posr); 2473 tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); 2474 2475 col->setWorldTransform(tr); 2476 } 2477 } 2478 } 2479 2480 int btMultiBody::calculateSerializeBufferSize() const 2481 { 2482 int sz = sizeof(btMultiBodyData); 2483 return sz; 2484 } 2485 2486 ///fills the dataBuffer and returns the struct name (and 0 on failure) 2487 const char* btMultiBody::serialize(void* dataBuffer, class btSerializer* serializer) const 2488 { 2489 btMultiBodyData* mbd = (btMultiBodyData*) dataBuffer; 2490 getBaseWorldTransform().serialize(mbd->m_baseWorldTransform); 2491 mbd->m_baseMass = this->getBaseMass(); 2492 getBaseInertia().serialize(mbd->m_baseInertia); 2493 { 2494 char* name = (char*) serializer->findNameForPointer(m_baseName); 2495 mbd->m_baseName = (char*)serializer->getUniquePointer(name); 2496 if (mbd->m_baseName) 2497 { 2498 serializer->serializeName(name); 2499 } 2500 } 2501 mbd->m_numLinks = this->getNumLinks(); 2502 if (mbd->m_numLinks) 2503 { 2504 int sz = sizeof(btMultiBodyLinkData); 2505 int numElem = mbd->m_numLinks; 2506 btChunk* chunk = serializer->allocate(sz,numElem); 2507 btMultiBodyLinkData* memPtr = (btMultiBodyLinkData*)chunk->m_oldPtr; 2508 for (int i=0;i<numElem;i++,memPtr++) 2509 { 2510 2511 memPtr->m_jointType = getLink(i).m_jointType; 2512 memPtr->m_dofCount = getLink(i).m_dofCount; 2513 memPtr->m_posVarCount = getLink(i).m_posVarCount; 2514 2515 getLink(i).m_inertiaLocal.serialize(memPtr->m_linkInertia); 2516 memPtr->m_linkMass = getLink(i).m_mass; 2517 memPtr->m_parentIndex = getLink(i).m_parent; 2518 getLink(i).m_eVector.serialize(memPtr->m_parentComToThisComOffset); 2519 getLink(i).m_dVector.serialize(memPtr->m_thisPivotToThisComOffset); 2520 getLink(i).m_zeroRotParentToThis.serialize(memPtr->m_zeroRotParentToThis); 2521 btAssert(memPtr->m_dofCount<=3); 2522 for (int dof = 0;dof<getLink(i).m_dofCount;dof++) 2523 { 2524 getLink(i).getAxisBottom(dof).serialize(memPtr->m_jointAxisBottom[dof]); 2525 getLink(i).getAxisTop(dof).serialize(memPtr->m_jointAxisTop[dof]); 2526 2527 memPtr->m_jointTorque[dof] = getLink(i).m_jointTorque[dof]; 2528 memPtr->m_jointVel[dof] = getJointVelMultiDof(i)[dof]; 2529 2530 } 2531 int numPosVar = getLink(i).m_posVarCount; 2532 for (int posvar = 0; posvar < numPosVar;posvar++) 2533 { 2534 memPtr->m_jointPos[posvar] = getLink(i).m_jointPos[posvar]; 2535 } 2536 2537 2538 { 2539 char* name = (char*) serializer->findNameForPointer(m_links[i].m_linkName); 2540 memPtr->m_linkName = (char*)serializer->getUniquePointer(name); 2541 if (memPtr->m_linkName) 2542 { 2543 serializer->serializeName(name); 2544 } 2545 } 2546 { 2547 char* name = (char*) serializer->findNameForPointer(m_links[i].m_jointName); 2548 memPtr->m_jointName = (char*)serializer->getUniquePointer(name); 2549 if (memPtr->m_jointName) 2550 { 2551 serializer->serializeName(name); 2552 } 2553 } 2554 memPtr->m_linkCollider = (btCollisionObjectData*)serializer->getUniquePointer(getLink(i).m_collider); 2555 2556 } 2557 serializer->finalizeChunk(chunk,btMultiBodyLinkDataName,BT_ARRAY_CODE,(void*) &m_links[0]); 2558 } 2559 mbd->m_links = mbd->m_numLinks? (btMultiBodyLinkData*) serializer->getUniquePointer((void*)&m_links[0]):0; 2560 2561 return btMultiBodyDataName; 2562 } 2563
