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@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 #ifndef BT_MULTIBODY_H 25 #define BT_MULTIBODY_H 26 27 #include "LinearMath/btScalar.h" 28 #include "LinearMath/btVector3.h" 29 #include "LinearMath/btQuaternion.h" 30 #include "LinearMath/btMatrix3x3.h" 31 #include "LinearMath/btAlignedObjectArray.h" 32 33 34 #include "btMultiBodyLink.h" 35 class btMultiBodyLinkCollider; 36 37 class btMultiBody 38 { 39 public: 40 41 42 BT_DECLARE_ALIGNED_ALLOCATOR(); 43 44 // 45 // initialization 46 // 47 48 btMultiBody(int n_links, // NOT including the base 49 btScalar mass, // mass of base 50 const btVector3 &inertia, // inertia of base, in base frame; assumed diagonal 51 bool fixedBase, // whether the base is fixed (true) or can move (false) 52 bool canSleep, 53 bool multiDof = false 54 ); 55 56 57 ~btMultiBody(); 58 59 void setupFixed(int linkIndex, 60 btScalar mass, 61 const btVector3 &inertia, 62 int parent, 63 const btQuaternion &rotParentToThis, 64 const btVector3 &parentComToThisPivotOffset, 65 const btVector3 &thisPivotToThisComOffset, 66 bool disableParentCollision); 67 68 69 void setupPrismatic(int i, 70 btScalar mass, 71 const btVector3 &inertia, 72 int parent, 73 const btQuaternion &rotParentToThis, 74 const btVector3 &jointAxis, 75 const btVector3 &parentComToThisComOffset, 76 const btVector3 &thisPivotToThisComOffset, 77 bool disableParentCollision); 78 79 void setupRevolute(int linkIndex, // 0 to num_links-1 80 btScalar mass, 81 const btVector3 &inertia, 82 int parentIndex, 83 const btQuaternion &rotParentToThis, // rotate points in parent frame to this frame, when q = 0 84 const btVector3 &jointAxis, // in my frame 85 const btVector3 &parentComToThisPivotOffset, // vector from parent COM to joint axis, in PARENT frame 86 const btVector3 &thisPivotToThisComOffset, // vector from joint axis to my COM, in MY frame 87 bool disableParentCollision=false); 88 89 void setupSpherical(int linkIndex, // 0 to num_links-1 90 btScalar mass, 91 const btVector3 &inertia, 92 int parent, 93 const btQuaternion &rotParentToThis, // rotate points in parent frame to this frame, when q = 0 94 const btVector3 &parentComToThisPivotOffset, // vector from parent COM to joint axis, in PARENT frame 95 const btVector3 &thisPivotToThisComOffset, // vector from joint axis to my COM, in MY frame 96 bool disableParentCollision=false); 97 98 #ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS 99 void setupPlanar(int i, // 0 to num_links-1 100 btScalar mass, 101 const btVector3 &inertia, 102 int parent, 103 const btQuaternion &rotParentToThis, // rotate points in parent frame to this frame, when q = 0 104 const btVector3 &rotationAxis, 105 const btVector3 &parentComToThisComOffset, // vector from parent COM to this COM, in PARENT frame 106 bool disableParentCollision=false); 107 #endif 108 getLink(int index)109 const btMultibodyLink& getLink(int index) const 110 { 111 return m_links[index]; 112 } 113 getLink(int index)114 btMultibodyLink& getLink(int index) 115 { 116 return m_links[index]; 117 } 118 119 setBaseCollider(btMultiBodyLinkCollider * collider)120 void setBaseCollider(btMultiBodyLinkCollider* collider)//collider can be NULL to disable collision for the base 121 { 122 m_baseCollider = collider; 123 } getBaseCollider()124 const btMultiBodyLinkCollider* getBaseCollider() const 125 { 126 return m_baseCollider; 127 } getBaseCollider()128 btMultiBodyLinkCollider* getBaseCollider() 129 { 130 return m_baseCollider; 131 } 132 133 // 134 // get parent 135 // input: link num from 0 to num_links-1 136 // output: link num from 0 to num_links-1, OR -1 to mean the base. 137 // 138 int getParent(int link_num) const; 139 140 141 // 142 // get number of m_links, masses, moments of inertia 143 // 144 getNumLinks()145 int getNumLinks() const { return m_links.size(); } getNumDofs()146 int getNumDofs() const { return m_dofCount; } getNumPosVars()147 int getNumPosVars() const { return m_posVarCnt; } getBaseMass()148 btScalar getBaseMass() const { return m_baseMass; } getBaseInertia()149 const btVector3 & getBaseInertia() const { return m_baseInertia; } 150 btScalar getLinkMass(int i) const; 151 const btVector3 & getLinkInertia(int i) const; 152 153 154 // 155 // change mass (incomplete: can only change base mass and inertia at present) 156 // 157 setBaseMass(btScalar mass)158 void setBaseMass(btScalar mass) { m_baseMass = mass; } setBaseInertia(const btVector3 & inertia)159 void setBaseInertia(const btVector3 &inertia) { m_baseInertia = inertia; } 160 161 162 // 163 // get/set pos/vel/rot/omega for the base link 164 // 165 getBasePos()166 const btVector3 & getBasePos() const { return m_basePos; } // in world frame getBaseVel()167 const btVector3 getBaseVel() const 168 { 169 return btVector3(m_realBuf[3],m_realBuf[4],m_realBuf[5]); 170 } // in world frame getWorldToBaseRot()171 const btQuaternion & getWorldToBaseRot() const 172 { 173 return m_baseQuat; 174 } // rotates world vectors into base frame getBaseOmega()175 btVector3 getBaseOmega() const { return btVector3(m_realBuf[0],m_realBuf[1],m_realBuf[2]); } // in world frame 176 setBasePos(const btVector3 & pos)177 void setBasePos(const btVector3 &pos) 178 { 179 m_basePos = pos; 180 } 181 setBaseWorldTransform(const btTransform & tr)182 void setBaseWorldTransform(const btTransform& tr) 183 { 184 setBasePos(tr.getOrigin()); 185 setWorldToBaseRot(tr.getRotation().inverse()); 186 187 } setBaseVel(const btVector3 & vel)188 void setBaseVel(const btVector3 &vel) 189 { 190 191 m_realBuf[3]=vel[0]; m_realBuf[4]=vel[1]; m_realBuf[5]=vel[2]; 192 } setWorldToBaseRot(const btQuaternion & rot)193 void setWorldToBaseRot(const btQuaternion &rot) 194 { 195 m_baseQuat = rot; //m_baseQuat asumed to ba alias!? 196 } setBaseOmega(const btVector3 & omega)197 void setBaseOmega(const btVector3 &omega) 198 { 199 m_realBuf[0]=omega[0]; 200 m_realBuf[1]=omega[1]; 201 m_realBuf[2]=omega[2]; 202 } 203 204 205 // 206 // get/set pos/vel for child m_links (i = 0 to num_links-1) 207 // 208 209 btScalar getJointPos(int i) const; 210 btScalar getJointVel(int i) const; 211 212 btScalar * getJointVelMultiDof(int i); 213 btScalar * getJointPosMultiDof(int i); 214 215 void setJointPos(int i, btScalar q); 216 void setJointVel(int i, btScalar qdot); 217 void setJointPosMultiDof(int i, btScalar *q); 218 void setJointVelMultiDof(int i, btScalar *qdot); 219 220 // 221 // direct access to velocities as a vector of 6 + num_links elements. 222 // (omega first, then v, then joint velocities.) 223 // getVelocityVector()224 const btScalar * getVelocityVector() const 225 { 226 return &m_realBuf[0]; 227 } 228 /* btScalar * getVelocityVector() 229 { 230 return &real_buf[0]; 231 } 232 */ 233 234 // 235 // get the frames of reference (positions and orientations) of the child m_links 236 // (i = 0 to num_links-1) 237 // 238 239 const btVector3 & getRVector(int i) const; // vector from COM(parent(i)) to COM(i), in frame i's coords 240 const btQuaternion & getParentToLocalRot(int i) const; // rotates vectors in frame parent(i) to vectors in frame i. 241 242 243 // 244 // transform vectors in local frame of link i to world frame (or vice versa) 245 // 246 btVector3 localPosToWorld(int i, const btVector3 &vec) const; 247 btVector3 localDirToWorld(int i, const btVector3 &vec) const; 248 btVector3 worldPosToLocal(int i, const btVector3 &vec) const; 249 btVector3 worldDirToLocal(int i, const btVector3 &vec) const; 250 251 252 // 253 // calculate kinetic energy and angular momentum 254 // useful for debugging. 255 // 256 257 btScalar getKineticEnergy() const; 258 btVector3 getAngularMomentum() const; 259 260 261 // 262 // set external forces and torques. Note all external forces/torques are given in the WORLD frame. 263 // 264 265 void clearForcesAndTorques(); 266 void clearVelocities(); 267 addBaseForce(const btVector3 & f)268 void addBaseForce(const btVector3 &f) 269 { 270 m_baseForce += f; 271 } addBaseTorque(const btVector3 & t)272 void addBaseTorque(const btVector3 &t) { m_baseTorque += t; } 273 void addLinkForce(int i, const btVector3 &f); 274 void addLinkTorque(int i, const btVector3 &t); 275 void addJointTorque(int i, btScalar Q); 276 void addJointTorqueMultiDof(int i, int dof, btScalar Q); 277 void addJointTorqueMultiDof(int i, const btScalar *Q); 278 getBaseForce()279 const btVector3 & getBaseForce() const { return m_baseForce; } getBaseTorque()280 const btVector3 & getBaseTorque() const { return m_baseTorque; } 281 const btVector3 & getLinkForce(int i) const; 282 const btVector3 & getLinkTorque(int i) const; 283 btScalar getJointTorque(int i) const; 284 btScalar * getJointTorqueMultiDof(int i); 285 286 287 // 288 // dynamics routines. 289 // 290 291 // timestep the velocities (given the external forces/torques set using addBaseForce etc). 292 // also sets up caches for calcAccelerationDeltas. 293 // 294 // Note: the caller must provide three vectors which are used as 295 // temporary scratch space. The idea here is to reduce dynamic 296 // memory allocation: the same scratch vectors can be re-used 297 // again and again for different Multibodies, instead of each 298 // btMultiBody allocating (and then deallocating) their own 299 // individual scratch buffers. This gives a considerable speed 300 // improvement, at least on Windows (where dynamic memory 301 // allocation appears to be fairly slow). 302 // 303 void stepVelocities(btScalar dt, 304 btAlignedObjectArray<btScalar> &scratch_r, 305 btAlignedObjectArray<btVector3> &scratch_v, 306 btAlignedObjectArray<btMatrix3x3> &scratch_m); 307 308 void stepVelocitiesMultiDof(btScalar dt, 309 btAlignedObjectArray<btScalar> &scratch_r, 310 btAlignedObjectArray<btVector3> &scratch_v, 311 btAlignedObjectArray<btMatrix3x3> &scratch_m); 312 313 // calcAccelerationDeltas 314 // input: force vector (in same format as jacobian, i.e.: 315 // 3 torque values, 3 force values, num_links joint torque values) 316 // output: 3 omegadot values, 3 vdot values, num_links q_double_dot values 317 // (existing contents of output array are replaced) 318 // stepVelocities must have been called first. 319 void calcAccelerationDeltas(const btScalar *force, btScalar *output, 320 btAlignedObjectArray<btScalar> &scratch_r, 321 btAlignedObjectArray<btVector3> &scratch_v) const; 322 323 void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output, 324 btAlignedObjectArray<btScalar> &scratch_r, 325 btAlignedObjectArray<btVector3> &scratch_v) const; 326 327 // apply a delta-vee directly. used in sequential impulses code. applyDeltaVee(const btScalar * delta_vee)328 void applyDeltaVee(const btScalar * delta_vee) 329 { 330 331 for (int i = 0; i < 6 + getNumLinks(); ++i) 332 { 333 m_realBuf[i] += delta_vee[i]; 334 } 335 336 } applyDeltaVee(const btScalar * delta_vee,btScalar multiplier)337 void applyDeltaVee(const btScalar * delta_vee, btScalar multiplier) 338 { 339 btScalar sum = 0; 340 for (int i = 0; i < 6 + getNumLinks(); ++i) 341 { 342 sum += delta_vee[i]*multiplier*delta_vee[i]*multiplier; 343 } 344 btScalar l = btSqrt(sum); 345 /* 346 static btScalar maxl = -1e30f; 347 if (l>maxl) 348 { 349 maxl=l; 350 // printf("maxl=%f\n",maxl); 351 } 352 */ 353 if (l>m_maxAppliedImpulse) 354 { 355 // printf("exceeds 100: l=%f\n",maxl); 356 multiplier *= m_maxAppliedImpulse/l; 357 } 358 359 for (int i = 0; i < 6 + getNumLinks(); ++i) 360 { 361 sum += delta_vee[i]*multiplier*delta_vee[i]*multiplier; 362 m_realBuf[i] += delta_vee[i] * multiplier; 363 btClamp(m_realBuf[i],-m_maxCoordinateVelocity,m_maxCoordinateVelocity); 364 } 365 } 366 applyDeltaVeeMultiDof(const btScalar * delta_vee,btScalar multiplier)367 void applyDeltaVeeMultiDof(const btScalar * delta_vee, btScalar multiplier) 368 { 369 //for (int dof = 0; dof < 6 + getNumDofs(); ++dof) 370 // printf("%.4f ", delta_vee[dof]*multiplier); 371 //printf("\n"); 372 373 //btScalar sum = 0; 374 //for (int dof = 0; dof < 6 + getNumDofs(); ++dof) 375 //{ 376 // sum += delta_vee[dof]*multiplier*delta_vee[dof]*multiplier; 377 //} 378 //btScalar l = btSqrt(sum); 379 380 //if (l>m_maxAppliedImpulse) 381 //{ 382 // multiplier *= m_maxAppliedImpulse/l; 383 //} 384 385 for (int dof = 0; dof < 6 + getNumDofs(); ++dof) 386 { 387 m_realBuf[dof] += delta_vee[dof] * multiplier; 388 btClamp(m_realBuf[dof],-m_maxCoordinateVelocity,m_maxCoordinateVelocity); 389 } 390 } 391 392 // timestep the positions (given current velocities). 393 void stepPositions(btScalar dt); 394 void stepPositionsMultiDof(btScalar dt, btScalar *pq = 0, btScalar *pqd = 0); 395 396 397 // 398 // contacts 399 // 400 401 // This routine fills out a contact constraint jacobian for this body. 402 // the 'normal' supplied must be -n for body1 or +n for body2 of the contact. 403 // 'normal' & 'contact_point' are both given in world coordinates. 404 void fillContactJacobian(int link, 405 const btVector3 &contact_point, 406 const btVector3 &normal, 407 btScalar *jac, 408 btAlignedObjectArray<btScalar> &scratch_r, 409 btAlignedObjectArray<btVector3> &scratch_v, 410 btAlignedObjectArray<btMatrix3x3> &scratch_m) const; 411 412 //multidof version of fillContactJacobian fillContactJacobianMultiDof(int link,const btVector3 & contact_point,const btVector3 & normal,btScalar * jac,btAlignedObjectArray<btScalar> & scratch_r,btAlignedObjectArray<btVector3> & scratch_v,btAlignedObjectArray<btMatrix3x3> & scratch_m)413 void fillContactJacobianMultiDof(int link, 414 const btVector3 &contact_point, 415 const btVector3 &normal, 416 btScalar *jac, 417 btAlignedObjectArray<btScalar> &scratch_r, 418 btAlignedObjectArray<btVector3> &scratch_v, 419 btAlignedObjectArray<btMatrix3x3> &scratch_m) const { filConstraintJacobianMultiDof(link, contact_point, btVector3(0, 0, 0), normal, jac, scratch_r, scratch_v, scratch_m); } 420 421 //a more general version of fillContactJacobianMultiDof which does not assume.. 422 //.. that the constraint in question is contact or, to be more precise, constrains linear velocity only 423 void filConstraintJacobianMultiDof(int link, 424 const btVector3 &contact_point, 425 const btVector3 &normal_ang, 426 const btVector3 &normal_lin, 427 btScalar *jac, 428 btAlignedObjectArray<btScalar> &scratch_r, 429 btAlignedObjectArray<btVector3> &scratch_v, 430 btAlignedObjectArray<btMatrix3x3> &scratch_m) const; 431 432 433 // 434 // sleeping 435 // setCanSleep(bool canSleep)436 void setCanSleep(bool canSleep) 437 { 438 m_canSleep = canSleep; 439 } 440 getCanSleep()441 bool getCanSleep()const 442 { 443 return m_canSleep; 444 } 445 isAwake()446 bool isAwake() const { return m_awake; } 447 void wakeUp(); 448 void goToSleep(); 449 void checkMotionAndSleepIfRequired(btScalar timestep); 450 hasFixedBase()451 bool hasFixedBase() const 452 { 453 return m_fixedBase; 454 } 455 getCompanionId()456 int getCompanionId() const 457 { 458 return m_companionId; 459 } setCompanionId(int id)460 void setCompanionId(int id) 461 { 462 //printf("for %p setCompanionId(%d)\n",this, id); 463 m_companionId = id; 464 } 465 setNumLinks(int numLinks)466 void setNumLinks(int numLinks)//careful: when changing the number of m_links, make sure to re-initialize or update existing m_links 467 { 468 m_links.resize(numLinks); 469 } 470 getLinearDamping()471 btScalar getLinearDamping() const 472 { 473 return m_linearDamping; 474 } setLinearDamping(btScalar damp)475 void setLinearDamping( btScalar damp) 476 { 477 m_linearDamping = damp; 478 } getAngularDamping()479 btScalar getAngularDamping() const 480 { 481 return m_angularDamping; 482 } setAngularDamping(btScalar damp)483 void setAngularDamping( btScalar damp) 484 { 485 m_angularDamping = damp; 486 } 487 getUseGyroTerm()488 bool getUseGyroTerm() const 489 { 490 return m_useGyroTerm; 491 } setUseGyroTerm(bool useGyro)492 void setUseGyroTerm(bool useGyro) 493 { 494 m_useGyroTerm = useGyro; 495 } getMaxCoordinateVelocity()496 btScalar getMaxCoordinateVelocity() const 497 { 498 return m_maxCoordinateVelocity ; 499 } setMaxCoordinateVelocity(btScalar maxVel)500 void setMaxCoordinateVelocity(btScalar maxVel) 501 { 502 m_maxCoordinateVelocity = maxVel; 503 } 504 getMaxAppliedImpulse()505 btScalar getMaxAppliedImpulse() const 506 { 507 return m_maxAppliedImpulse; 508 } setMaxAppliedImpulse(btScalar maxImp)509 void setMaxAppliedImpulse(btScalar maxImp) 510 { 511 m_maxAppliedImpulse = maxImp; 512 } setHasSelfCollision(bool hasSelfCollision)513 void setHasSelfCollision(bool hasSelfCollision) 514 { 515 m_hasSelfCollision = hasSelfCollision; 516 } hasSelfCollision()517 bool hasSelfCollision() const 518 { 519 return m_hasSelfCollision; 520 } 521 isMultiDof()522 bool isMultiDof() { return m_isMultiDof; } 523 void finalizeMultiDof(); 524 useRK4Integration(bool use)525 void useRK4Integration(bool use) { m_useRK4 = use; } isUsingRK4Integration()526 bool isUsingRK4Integration() const { return m_useRK4; } useGlobalVelocities(bool use)527 void useGlobalVelocities(bool use) { m_useGlobalVelocities = use; } isUsingGlobalVelocities()528 bool isUsingGlobalVelocities() const { return m_useGlobalVelocities; } 529 isPosUpdated()530 bool isPosUpdated() const 531 { 532 return __posUpdated; 533 } setPosUpdated(bool updated)534 void setPosUpdated(bool updated) 535 { 536 __posUpdated = updated; 537 } 538 539 private: 540 btMultiBody(const btMultiBody &); // not implemented 541 void operator=(const btMultiBody &); // not implemented 542 543 void compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const; 544 545 void solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, float result[6]) const; 546 #ifdef TEST_SPATIAL_ALGEBRA_LAYER 547 void solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionVector &result) const; 548 #endif 549 updateLinksDofOffsets()550 void updateLinksDofOffsets() 551 { 552 int dofOffset = 0, cfgOffset = 0; 553 for(int bidx = 0; bidx < m_links.size(); ++bidx) 554 { 555 m_links[bidx].m_dofOffset = dofOffset; m_links[bidx].m_cfgOffset = cfgOffset; 556 dofOffset += m_links[bidx].m_dofCount; cfgOffset += m_links[bidx].m_posVarCount; 557 } 558 } 559 560 void mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const; 561 562 563 private: 564 565 btMultiBodyLinkCollider* m_baseCollider;//can be NULL 566 567 btVector3 m_basePos; // position of COM of base (world frame) 568 btQuaternion m_baseQuat; // rotates world points into base frame 569 570 btScalar m_baseMass; // mass of the base 571 btVector3 m_baseInertia; // inertia of the base (in local frame; diagonal) 572 573 btVector3 m_baseForce; // external force applied to base. World frame. 574 btVector3 m_baseTorque; // external torque applied to base. World frame. 575 576 btAlignedObjectArray<btMultibodyLink> m_links; // array of m_links, excluding the base. index from 0 to num_links-1. 577 btAlignedObjectArray<btMultiBodyLinkCollider*> m_colliders; 578 579 580 // 581 // realBuf: 582 // offset size array 583 // 0 6 + num_links v (base_omega; base_vel; joint_vels) MULTIDOF [sysdof x sysdof for D matrices (TOO MUCH!) + pos_delta which is sys-cfg sized] 584 // 6+num_links num_links D 585 // 586 // vectorBuf: 587 // offset size array 588 // 0 num_links h_top 589 // num_links num_links h_bottom 590 // 591 // matrixBuf: 592 // offset size array 593 // 0 num_links+1 rot_from_parent 594 // 595 596 btAlignedObjectArray<btScalar> m_realBuf; 597 btAlignedObjectArray<btVector3> m_vectorBuf; 598 btAlignedObjectArray<btMatrix3x3> m_matrixBuf; 599 600 //std::auto_ptr<Eigen::LU<Eigen::Matrix<btScalar, 6, 6> > > cached_imatrix_lu; 601 602 btMatrix3x3 m_cachedInertiaTopLeft; 603 btMatrix3x3 m_cachedInertiaTopRight; 604 btMatrix3x3 m_cachedInertiaLowerLeft; 605 btMatrix3x3 m_cachedInertiaLowerRight; 606 607 bool m_fixedBase; 608 609 // Sleep parameters. 610 bool m_awake; 611 bool m_canSleep; 612 btScalar m_sleepTimer; 613 614 int m_companionId; 615 btScalar m_linearDamping; 616 btScalar m_angularDamping; 617 bool m_useGyroTerm; 618 btScalar m_maxAppliedImpulse; 619 btScalar m_maxCoordinateVelocity; 620 bool m_hasSelfCollision; 621 bool m_isMultiDof; 622 bool __posUpdated; 623 int m_dofCount, m_posVarCnt; 624 bool m_useRK4, m_useGlobalVelocities; 625 }; 626 627 #endif 628