Simbody
3.5
|
00001 #ifndef SimTK_SIMBODY_CONDITIONAL_CONSTRAINT_H_ 00002 #define SimTK_SIMBODY_CONDITIONAL_CONSTRAINT_H_ 00003 00004 /* -------------------------------------------------------------------------- * 00005 * Simbody(tm) * 00006 * -------------------------------------------------------------------------- * 00007 * This is part of the SimTK biosimulation toolkit originating from * 00008 * Simbios, the NIH National Center for Physics-Based Simulation of * 00009 * Biological Structures at Stanford, funded under the NIH Roadmap for * 00010 * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody. * 00011 * * 00012 * Portions copyright (c) 2014 Stanford University and the Authors. * 00013 * Authors: Michael Sherman * 00014 * Contributors: * 00015 * * 00016 * Licensed under the Apache License, Version 2.0 (the "License"); you may * 00017 * not use this file except in compliance with the License. You may obtain a * 00018 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * 00019 * * 00020 * Unless required by applicable law or agreed to in writing, software * 00021 * distributed under the License is distributed on an "AS IS" BASIS, * 00022 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * 00023 * See the License for the specific language governing permissions and * 00024 * limitations under the License. * 00025 * -------------------------------------------------------------------------- */ 00026 00027 #include "SimTKmath.h" 00028 #include "simbody/internal/common.h" 00029 #include "simbody/internal/Constraint.h" 00030 #include "simbody/internal/Constraint_BuiltIns.h" 00031 #include "simbody/internal/MobilizedBody.h" 00032 00033 namespace SimTK { 00034 00038 class SimTK_SIMBODY_EXPORT ConditionalConstraint { 00039 public: 00040 00047 static Real calcEffectiveCOR(Real minCOR, Real captureSpeed, Real minCORSpeed, 00048 Real impactSpeed) 00049 { 00050 assert(0 <= minCOR && minCOR <= 1); 00051 assert(0 <= captureSpeed && captureSpeed <= minCORSpeed); 00052 assert(impactSpeed >= 0); 00053 00054 if (impactSpeed <= captureSpeed) return 0; 00055 if (impactSpeed >= minCORSpeed) return minCOR; 00056 // captureSpeed < impactSpeed < minCORSpeed 00057 const Real slope = (1-minCOR) / minCORSpeed; 00058 return 1 - slope*impactSpeed; 00059 } 00060 00066 static Real calcEffectiveCOF(Real mu_s, Real mu_d, Real mu_v, 00067 Real transitionSpeed, Real slipSpeed) 00068 { 00069 assert(mu_s>=0 && mu_d>=0 && mu_v>=0); 00070 assert(mu_s>=mu_d); 00071 assert(transitionSpeed>=0 && slipSpeed>=0); 00072 const Real viscous = mu_v*slipSpeed; // typically zero 00073 return viscous + (slipSpeed <= transitionSpeed ? mu_s : mu_d); 00074 } 00075 00076 }; 00077 00078 class UnilateralContact; // normal + friction 00079 class UnilateralSpeedConstraint; 00080 class BoundedSpeedConstraint; 00081 00082 class ConstraintLimitedFriction; 00083 class StateLimitedFriction; 00084 00085 //============================================================================== 00086 // UNILATERAL CONTACT CONSTRAINT 00087 //============================================================================== 00120 class SimTK_SIMBODY_EXPORT UnilateralContact { 00121 public: 00125 explicit UnilateralContact(int sign=1) : m_sign((Real)sign) 00126 { assert(sign==1 || sign==-1); } 00127 00128 virtual ~UnilateralContact() {} 00129 00131 Real getSignConvention() const {return m_sign;} 00132 00135 virtual bool disable(State& state) const = 0; 00136 00139 virtual bool enable(State& state) const = 0; 00140 00142 virtual bool isEnabled(const State& state) const = 0; 00143 00146 virtual Vec3 whereToDisplay(const State& state) const = 0; 00147 00155 virtual Real calcEffectiveCOR(const State& state, 00156 Real defaultCaptureSpeed, 00157 Real defaultMinCORSpeed, 00158 Real impactSpeed) const = 0; 00159 00163 virtual Real getPerr(const State& state) const = 0; 00166 virtual Real getVerr(const State& state) const = 0; 00169 virtual Real getAerr(const State& state) const = 0; 00170 00175 virtual bool isProximal(const State& state, Real ptol) const 00176 { return m_sign*getPerr(state) <= ptol; } 00177 00182 virtual MultiplierIndex 00183 getContactMultiplierIndex(const State& state) const = 0; 00184 00187 virtual bool hasFriction(const State& state) const {return false;} 00188 00196 virtual Real calcEffectiveCOF(const State& state, 00197 Real defaultTransitionSpeed, 00198 Real slipSpeed) const 00199 { return NaN; } 00200 00201 virtual Vec2 getSlipVelocity(const State& state) const 00202 { return Vec2(NaN); } 00203 00207 virtual void 00208 getFrictionMultiplierIndices(const State& state, 00209 MultiplierIndex& ix_x, 00210 MultiplierIndex& ix_y) const 00211 { ix_x.invalidate(); ix_y.invalidate(); } 00212 00215 virtual Vec3 getPositionInfo(const State& state) const 00216 { return Vec3(NaN); } 00219 virtual void setInstanceParameter(State& state, const Vec3& pos) const {} 00220 00221 00222 void setMyIndex(UnilateralContactIndex cx) {m_myIx = cx;} 00223 UnilateralContactIndex getMyIndex() const {return m_myIx;} 00224 private: 00225 Real m_sign; // 1 or -1 00226 UnilateralContactIndex m_myIx; 00227 }; 00228 00229 //============================================================================== 00230 // UNILATERAL SPEED CONSTRAINT 00231 //============================================================================== 00238 class SimTK_SIMBODY_EXPORT UnilateralSpeedConstraint { 00239 public: 00240 UnilateralSpeedConstraint() {} 00241 virtual ~UnilateralSpeedConstraint() {} 00242 00243 private: 00244 }; 00245 00246 //============================================================================== 00247 // BOUNDED SPEED CONSTRAINT 00248 //============================================================================== 00272 class SimTK_SIMBODY_EXPORT BoundedSpeedConstraint { 00273 public: 00274 BoundedSpeedConstraint() {} 00275 virtual ~BoundedSpeedConstraint() {} 00276 00281 virtual Vec2 calcEffectiveBounds(const State& state) const = 0; 00282 private: 00283 }; 00284 00285 //============================================================================== 00286 // STATE LIMITED FRICTION 00287 //============================================================================== 00289 class SimTK_SIMBODY_EXPORT StateLimitedFriction { 00290 public: 00291 StateLimitedFriction() {} 00292 virtual ~StateLimitedFriction() {} 00293 00296 virtual bool disable(State& state) const = 0; 00297 00300 virtual bool enable(State& state) const = 0; 00301 00304 virtual Real getNormalForceMagnitude(const State& state) const = 0; 00305 00306 virtual Real calcEffectiveCOF(const State& state, 00307 Real defaultTransitionSpeed, 00308 Real slipSpeed) const = 0; 00309 00310 virtual Real getSlipSpeed(const State& state) const = 0; 00311 00314 virtual Vec3 getPositionInfo(const State& state) const 00315 { return Vec3(NaN); } 00318 virtual void setInstanceParameter(State& state, const Vec3& pos) const {} 00319 00320 void setMyIndex(StateLimitedFrictionIndex fx) {m_myIx = fx;} 00321 StateLimitedFrictionIndex getMyIndex() const {return m_myIx;} 00322 private: 00323 StateLimitedFrictionIndex m_myIx; 00324 }; 00325 00326 00327 //============================================================================== 00328 // HARD STOP UPPER 00329 //============================================================================== 00342 class SimTK_SIMBODY_EXPORT HardStopUpper : public UnilateralContact { 00343 public: 00344 HardStopUpper(MobilizedBody& mobod, MobilizerQIndex whichQ, 00345 Real defaultUpperLimit, Real minCOR); 00346 00347 bool disable(State& state) const OVERRIDE_11 00348 { if (m_upper.isDisabled(state)) return false; 00349 else {m_upper.disable(state); return true;} } 00350 bool enable(State& state) const OVERRIDE_11 00351 { if (!m_upper.isDisabled(state)) return false; 00352 else {m_upper.enable(state); return true;} } 00353 bool isEnabled(const State& state) const OVERRIDE_11 00354 { return !m_upper.isDisabled(state); } 00355 00356 // Returns the contact point in the Ground frame. 00357 Vec3 whereToDisplay(const State& state) const OVERRIDE_11; 00358 00359 // Currently have to fake the perr because the constraint might be 00360 // disabled in which case it won't calculate perr. Also, we want 00361 // negative to mean violated so may need to adjust the sign. 00362 Real getPerr(const State& state) const OVERRIDE_11; 00363 Real getVerr(const State& state) const OVERRIDE_11; 00364 Real getAerr(const State& state) const OVERRIDE_11; 00365 00366 Real calcEffectiveCOR(const State& state, 00367 Real defaultCaptureSpeed, 00368 Real defaultMinCORSpeed, 00369 Real impactSpeed) const OVERRIDE_11 00370 { 00371 return ConditionalConstraint::calcEffectiveCOR 00372 (m_minCOR, defaultCaptureSpeed, defaultMinCORSpeed, 00373 impactSpeed); 00374 } 00375 00376 MultiplierIndex getContactMultiplierIndex(const State& s) const OVERRIDE_11; 00377 00378 private: 00379 MobilizedBody m_mobod; 00380 Real m_defaultUpperLimit; 00381 Real m_minCOR; 00382 Constraint::ConstantCoordinate m_upper; 00383 }; 00384 00385 //============================================================================== 00386 // HARD STOP LOWER 00387 //============================================================================== 00400 class SimTK_SIMBODY_EXPORT HardStopLower : public UnilateralContact { 00401 public: 00402 HardStopLower(MobilizedBody& mobod, MobilizerQIndex whichQ, 00403 Real defaultLowerLimit, Real minCOR); 00404 00405 bool disable(State& state) const OVERRIDE_11 00406 { if (m_lower.isDisabled(state)) return false; 00407 else {m_lower.disable(state); return true;} } 00408 bool enable(State& state) const OVERRIDE_11 00409 { if (!m_lower.isDisabled(state)) return false; 00410 else {m_lower.enable(state); return true;} } 00411 bool isEnabled(const State& state) const OVERRIDE_11 00412 { return !m_lower.isDisabled(state); } 00413 00414 // Returns the contact point in the Ground frame. 00415 Vec3 whereToDisplay(const State& state) const OVERRIDE_11; 00416 00417 // Currently have to fake the perr because the constraint might be 00418 // disabled in which case it won't calculate perr. Also, we want 00419 // negative to mean violated so may need to adjust the sign. 00420 Real getPerr(const State& state) const OVERRIDE_11; 00421 Real getVerr(const State& state) const OVERRIDE_11; 00422 Real getAerr(const State& state) const OVERRIDE_11; 00423 00424 Real calcEffectiveCOR(const State& state, 00425 Real defaultCaptureSpeed, 00426 Real defaultMinCORSpeed, 00427 Real impactSpeed) const OVERRIDE_11 00428 { 00429 return ConditionalConstraint::calcEffectiveCOR 00430 (m_minCOR, defaultCaptureSpeed, defaultMinCORSpeed, 00431 impactSpeed); 00432 } 00433 00434 MultiplierIndex getContactMultiplierIndex(const State& s) const OVERRIDE_11; 00435 private: 00436 MobilizedBody m_mobod; 00437 Real m_defaultLowerLimit; 00438 Real m_minCOR; 00439 Constraint::ConstantCoordinate m_lower; 00440 }; 00441 00442 00443 00444 //============================================================================== 00445 // ROPE 00446 //============================================================================== 00459 class SimTK_SIMBODY_EXPORT Rope : public UnilateralContact { 00460 public: 00461 Rope(MobilizedBody& mobod1, const Vec3& point1, 00462 MobilizedBody& mobod2, const Vec3& point2, 00463 Real defaultLengthLimit, Real minCOR); 00464 00465 bool disable(State& state) const OVERRIDE_11 00466 { if (m_rod.isDisabled(state)) return false; 00467 else {m_rod.disable(state); return true;} } 00468 bool enable(State& state) const OVERRIDE_11 00469 { if (!m_rod.isDisabled(state)) return false; 00470 else {m_rod.enable(state); return true;} } 00471 bool isEnabled(const State& state) const OVERRIDE_11 00472 { return !m_rod.isDisabled(state); } 00473 00474 // Returns half-way location in the Ground frame. 00475 Vec3 whereToDisplay(const State& state) const OVERRIDE_11; 00476 00477 // Currently have to fake the perr because the constraint might be 00478 // disabled in which case it won't calculate perr. 00479 Real getPerr(const State& state) const OVERRIDE_11; 00480 Real getVerr(const State& state) const OVERRIDE_11; 00481 Real getAerr(const State& state) const OVERRIDE_11; 00482 00483 Real calcEffectiveCOR(const State& state, 00484 Real defaultCaptureSpeed, 00485 Real defaultMinCORSpeed, 00486 Real impactSpeed) const OVERRIDE_11 00487 { 00488 return ConditionalConstraint::calcEffectiveCOR 00489 (m_minCOR, defaultCaptureSpeed, defaultMinCORSpeed, 00490 impactSpeed); 00491 } 00492 00493 MultiplierIndex getContactMultiplierIndex(const State& s) const OVERRIDE_11; 00494 private: 00495 Real m_minCOR; 00496 Constraint::Rod m_rod; 00497 }; 00498 00499 //============================================================================== 00500 // POINT PLANE FRICTIONLESS CONTACT 00501 //============================================================================== 00506 class SimTK_SIMBODY_EXPORT PointPlaneFrictionlessContact 00507 : public UnilateralContact { 00508 public: 00509 PointPlaneFrictionlessContact( 00510 MobilizedBody& planeBodyB, const UnitVec3& normal_B, Real height, 00511 MobilizedBody& followerBodyF, const Vec3& point_F, Real minCOR); 00512 00513 bool disable(State& state) const OVERRIDE_11 { 00514 if (m_ptInPlane.isDisabled(state)) return false; 00515 m_ptInPlane.disable(state); 00516 return true; 00517 } 00518 00519 bool enable(State& state) const OVERRIDE_11 { 00520 if (!m_ptInPlane.isDisabled(state)) return false; 00521 m_ptInPlane.enable(state); 00522 return true; 00523 } 00524 00525 bool isEnabled(const State& state) const OVERRIDE_11 { 00526 return !m_ptInPlane.isDisabled(state); 00527 } 00528 00529 // Returns the contact point in the Ground frame. 00530 Vec3 whereToDisplay(const State& state) const OVERRIDE_11; 00531 00532 // Currently have to fake the perr because the constraint might be 00533 // disabled in which case it won't calculate perr. 00534 Real getPerr(const State& state) const OVERRIDE_11; 00535 00536 // We won't need to look at these except for proximal constraints which 00537 // will already have been enabled, so no need to fake. 00538 Real getVerr(const State& state) const OVERRIDE_11 00539 { return m_ptInPlane.getVelocityError(state); } 00540 Real getAerr(const State& state) const OVERRIDE_11 00541 { return m_ptInPlane.getAccelerationError(state); } 00542 00543 00544 Real calcEffectiveCOR(const State& state, 00545 Real defaultCaptureSpeed, 00546 Real defaultMinCORSpeed, 00547 Real impactSpeed) const OVERRIDE_11 00548 { 00549 return ConditionalConstraint::calcEffectiveCOR 00550 (m_minCOR, defaultCaptureSpeed, defaultMinCORSpeed, 00551 impactSpeed); 00552 } 00553 00554 MultiplierIndex getContactMultiplierIndex(const State& s) const OVERRIDE_11; 00555 00556 private: 00557 MobilizedBody m_planeBody; // body P 00558 const Rotation m_frame; // z is normal; expressed in P 00559 const Real m_height; 00560 00561 MobilizedBody m_follower; // body F 00562 const Vec3 m_point; // measured & expressed in F 00563 00564 Real m_minCOR; 00565 00566 Constraint::PointInPlane m_ptInPlane; 00567 }; 00568 00569 00570 //============================================================================== 00571 // POINT PLANE CONTACT 00572 //============================================================================== 00578 class SimTK_SIMBODY_EXPORT PointPlaneContact : public UnilateralContact { 00579 public: 00580 PointPlaneContact( 00581 MobilizedBody& planeBodyB, const UnitVec3& normal_B, Real height, 00582 MobilizedBody& followerBodyF, const Vec3& point_F, 00583 Real minCOR, Real mu_s, Real mu_d, Real mu_v); 00584 00585 bool disable(State& state) const OVERRIDE_11 { 00586 if (m_ptInPlane.isDisabled(state)) return false; 00587 m_ptInPlane.disable(state); 00588 return true; 00589 } 00590 00591 bool enable(State& state) const OVERRIDE_11 { 00592 if (!m_ptInPlane.isDisabled(state)) return false; 00593 m_ptInPlane.enable(state); 00594 return true; 00595 } 00596 00597 bool isEnabled(const State& state) const OVERRIDE_11 { 00598 return !m_ptInPlane.isDisabled(state); 00599 } 00600 00601 // Returns the contact point in the Ground frame. 00602 Vec3 whereToDisplay(const State& state) const OVERRIDE_11; 00603 00604 // Currently have to fake the perr because the constraint might be 00605 // disabled in which case it won't calculate perr. 00606 Real getPerr(const State& state) const OVERRIDE_11; 00607 00608 // We won't need to look at these except for proximal constraints which 00609 // will already have been enabled, so no need to fake. 00610 Real getVerr(const State& state) const OVERRIDE_11 00611 { return m_ptInPlane.getVelocityErrors(state)[2]; } 00612 Real getAerr(const State& state) const OVERRIDE_11 00613 { return m_ptInPlane.getAccelerationErrors(state)[2]; } 00614 00615 00616 Real calcEffectiveCOR(const State& state, 00617 Real defaultCaptureSpeed, 00618 Real defaultMinCORSpeed, 00619 Real impactSpeed) const OVERRIDE_11 00620 { 00621 return ConditionalConstraint::calcEffectiveCOR 00622 (m_minCOR, defaultCaptureSpeed, defaultMinCORSpeed, 00623 impactSpeed); 00624 } 00625 00626 bool hasFriction(const State& state) const OVERRIDE_11 00627 { return true; } 00628 00629 Vec2 getSlipVelocity(const State& state) const OVERRIDE_11 { 00630 const Vec3 v = m_ptInPlane.getVelocityErrors(state); 00631 return Vec2(v[0], v[1]); 00632 } 00633 00634 Real calcEffectiveCOF(const State& state, 00635 Real defaultTransitionSpeed, 00636 Real slipSpeed) const OVERRIDE_11 00637 { 00638 return ConditionalConstraint::calcEffectiveCOF 00639 (m_mu_s, m_mu_d, m_mu_v, defaultTransitionSpeed, slipSpeed); 00640 } 00641 00642 MultiplierIndex getContactMultiplierIndex(const State& s) const OVERRIDE_11; 00643 00644 void getFrictionMultiplierIndices(const State& s, 00645 MultiplierIndex& ix_x, 00646 MultiplierIndex& ix_y) const OVERRIDE_11; 00647 00648 private: 00649 MobilizedBody m_planeBody; // body P 00650 const Rotation m_frame; // z is normal; expressed in P 00651 const Real m_height; 00652 00653 MobilizedBody m_follower; // body F 00654 const Vec3 m_point; // measured & expressed in F 00655 00656 Real m_minCOR; 00657 Real m_mu_s, m_mu_d, m_mu_v; 00658 00659 Constraint::PointOnPlaneContact m_ptInPlane; 00660 }; 00661 00662 //============================================================================== 00663 // SPHERE PLANE CONTACT 00664 //============================================================================== 00670 class SimTK_SIMBODY_EXPORT SpherePlaneContact : public UnilateralContact { 00671 public: 00672 SpherePlaneContact( 00673 MobilizedBody& planeBodyB, const UnitVec3& normal_B, Real height, 00674 MobilizedBody& followerBodyF, const Vec3& point_F, Real radius, 00675 Real minCOR, Real mu_s, Real mu_d, Real mu_v); 00676 00677 bool disable(State& state) const OVERRIDE_11 { 00678 if (m_sphereOnPlane.isDisabled(state)) return false; 00679 m_sphereOnPlane.disable(state); 00680 return true; 00681 } 00682 00683 bool enable(State& state) const OVERRIDE_11 { 00684 if (!m_sphereOnPlane.isDisabled(state)) return false; 00685 m_sphereOnPlane.enable(state); 00686 return true; 00687 } 00688 00689 bool isEnabled(const State& state) const OVERRIDE_11 { 00690 return !m_sphereOnPlane.isDisabled(state); 00691 } 00692 00693 // Returns the contact point in the Ground frame. 00694 Vec3 whereToDisplay(const State& state) const OVERRIDE_11; 00695 00696 // Currently have to fake the perr because the constraint might be 00697 // disabled in which case it won't calculate perr. 00698 Real getPerr(const State& state) const OVERRIDE_11; 00699 00700 // We won't need to look at these except for proximal constraints which 00701 // will already have been enabled, so no need to fake. 00702 Real getVerr(const State& state) const OVERRIDE_11 00703 { return m_sphereOnPlane.getVelocityErrors(state)[2]; } 00704 Real getAerr(const State& state) const OVERRIDE_11 00705 { return m_sphereOnPlane.getAccelerationErrors(state)[2]; } 00706 00707 00708 Real calcEffectiveCOR(const State& state, 00709 Real defaultCaptureSpeed, 00710 Real defaultMinCORSpeed, 00711 Real impactSpeed) const OVERRIDE_11 00712 { 00713 return ConditionalConstraint::calcEffectiveCOR 00714 (m_minCOR, defaultCaptureSpeed, defaultMinCORSpeed, 00715 impactSpeed); 00716 } 00717 00718 bool hasFriction(const State& state) const OVERRIDE_11 00719 { return true; } 00720 00721 Vec2 getSlipVelocity(const State& state) const OVERRIDE_11 { 00722 const Vec3 v = m_sphereOnPlane.getVelocityErrors(state); 00723 return Vec2(v[0], v[1]); 00724 } 00725 00726 Real calcEffectiveCOF(const State& state, 00727 Real defaultTransitionSpeed, 00728 Real slipSpeed) const OVERRIDE_11 00729 { 00730 return ConditionalConstraint::calcEffectiveCOF 00731 (m_mu_s, m_mu_d, m_mu_v, defaultTransitionSpeed, slipSpeed); 00732 } 00733 00734 MultiplierIndex getContactMultiplierIndex(const State& s) const OVERRIDE_11; 00735 00736 void getFrictionMultiplierIndices(const State& s, 00737 MultiplierIndex& ix_x, 00738 MultiplierIndex& ix_y) const OVERRIDE_11; 00739 00740 private: 00741 Real m_minCOR; 00742 Real m_mu_s, m_mu_d, m_mu_v; 00743 00744 Constraint::SphereOnPlaneContact m_sphereOnPlane; 00745 }; 00746 00747 //============================================================================== 00748 // SPHERE SPHERE CONTACT 00749 //============================================================================== 00756 class SimTK_SIMBODY_EXPORT SphereSphereContact : public UnilateralContact { 00757 public: 00758 SphereSphereContact( 00759 MobilizedBody& mobod_F, 00760 const Vec3& defaultCenterOnF, 00761 Real defaultRadiusOnF, 00762 MobilizedBody& mobod_B, 00763 const Vec3& defaultCenterOnB, 00764 Real defaultRadiusOnB, 00765 Real minCOR, Real mu_s, Real mu_d, Real mu_v); 00766 00767 bool disable(State& state) const OVERRIDE_11 { 00768 if (m_sphereOnSphere.isDisabled(state)) return false; 00769 m_sphereOnSphere.disable(state); 00770 return true; 00771 } 00772 00773 bool enable(State& state) const OVERRIDE_11 { 00774 if (!m_sphereOnSphere.isDisabled(state)) return false; 00775 m_sphereOnSphere.enable(state); 00776 return true; 00777 } 00778 00779 bool isEnabled(const State& state) const OVERRIDE_11 { 00780 return !m_sphereOnSphere.isDisabled(state); 00781 } 00782 00783 // Returns the contact point in the Ground frame. 00784 Vec3 whereToDisplay(const State& state) const OVERRIDE_11; 00785 00786 // Currently have to fake the perr because the constraint might be 00787 // disabled in which case it won't calculate perr. 00788 Real getPerr(const State& state) const OVERRIDE_11; 00789 00790 // We won't need to look at these except for proximal constraints which 00791 // will already have been enabled, so no need to fake. 00792 Real getVerr(const State& state) const OVERRIDE_11 00793 { return m_sphereOnSphere.getVelocityErrors(state)[2]; } 00794 Real getAerr(const State& state) const OVERRIDE_11 00795 { return m_sphereOnSphere.getAccelerationErrors(state)[2]; } 00796 00797 00798 Real calcEffectiveCOR(const State& state, 00799 Real defaultCaptureSpeed, 00800 Real defaultMinCORSpeed, 00801 Real impactSpeed) const OVERRIDE_11 00802 { 00803 return ConditionalConstraint::calcEffectiveCOR 00804 (m_minCOR, defaultCaptureSpeed, defaultMinCORSpeed, 00805 impactSpeed); 00806 } 00807 00808 bool hasFriction(const State& state) const OVERRIDE_11 00809 { return true; } 00810 00811 Vec2 getSlipVelocity(const State& state) const OVERRIDE_11 { 00812 const Vec3 v = m_sphereOnSphere.getVelocityErrors(state); 00813 return Vec2(v[0], v[1]); 00814 } 00815 00816 Real calcEffectiveCOF(const State& state, 00817 Real defaultTransitionSpeed, 00818 Real slipSpeed) const OVERRIDE_11 00819 { 00820 return ConditionalConstraint::calcEffectiveCOF 00821 (m_mu_s, m_mu_d, m_mu_v, defaultTransitionSpeed, slipSpeed); 00822 } 00823 00824 MultiplierIndex getContactMultiplierIndex(const State& s) const OVERRIDE_11; 00825 00826 void getFrictionMultiplierIndices(const State& s, 00827 MultiplierIndex& ix_x, 00828 MultiplierIndex& ix_y) const OVERRIDE_11; 00829 00830 private: 00831 Real m_minCOR; 00832 Real m_mu_s, m_mu_d, m_mu_v; 00833 00834 Constraint::SphereOnSphereContact m_sphereOnSphere; 00835 }; 00836 00837 00838 //============================================================================== 00839 // EDGE EDGE CONTACT 00840 //============================================================================== 00852 class SimTK_SIMBODY_EXPORT EdgeEdgeContact : public UnilateralContact { 00853 public: 00854 EdgeEdgeContact( 00855 MobilizedBody& mobod_F, 00856 const Transform& defaultEdgeFrameF, 00857 Real defaultHalfLengthF, 00858 MobilizedBody& mobod_B, 00859 const Transform& defaultEdgeFrameB, 00860 Real defaultHalfLengthB, 00861 Real minCOR, Real mu_s, Real mu_d, Real mu_v); 00862 00863 bool disable(State& state) const OVERRIDE_11 { 00864 if (m_lineOnLine.isDisabled(state)) return false; 00865 m_lineOnLine.disable(state); 00866 return true; 00867 } 00868 00869 bool enable(State& state) const OVERRIDE_11 { 00870 if (!m_lineOnLine.isDisabled(state)) return false; 00871 m_lineOnLine.enable(state); 00872 return true; 00873 } 00874 00875 bool isEnabled(const State& state) const OVERRIDE_11 { 00876 return !m_lineOnLine.isDisabled(state); 00877 } 00878 00879 // Returns the contact point in the Ground frame. 00880 Vec3 whereToDisplay(const State& state) const OVERRIDE_11; 00881 00882 // Currently have to fake the perr because the constraint might be 00883 // disabled in which case it won't calculate perr. 00884 Real getPerr(const State& state) const OVERRIDE_11; 00885 00886 // We won't need to look at these except for proximal constraints which 00887 // will already have been enabled, so no need to fake. 00888 Real getVerr(const State& state) const OVERRIDE_11 00889 { return m_lineOnLine.getVelocityErrors(state)[2]; } 00890 Real getAerr(const State& state) const OVERRIDE_11 00891 { return m_lineOnLine.getAccelerationErrors(state)[2]; } 00892 00893 00894 Real calcEffectiveCOR(const State& state, 00895 Real defaultCaptureSpeed, 00896 Real defaultMinCORSpeed, 00897 Real impactSpeed) const OVERRIDE_11 00898 { 00899 return ConditionalConstraint::calcEffectiveCOR 00900 (m_minCOR, defaultCaptureSpeed, defaultMinCORSpeed, 00901 impactSpeed); 00902 } 00903 00904 bool hasFriction(const State& state) const OVERRIDE_11 00905 { return true; } 00906 00907 Vec2 getSlipVelocity(const State& state) const OVERRIDE_11 { 00908 const Vec3 v = m_lineOnLine.getVelocityErrors(state); 00909 return Vec2(v[0], v[1]); 00910 } 00911 00912 Real calcEffectiveCOF(const State& state, 00913 Real defaultTransitionSpeed, 00914 Real slipSpeed) const OVERRIDE_11 00915 { 00916 return ConditionalConstraint::calcEffectiveCOF 00917 (m_mu_s, m_mu_d, m_mu_v, defaultTransitionSpeed, slipSpeed); 00918 } 00919 00920 MultiplierIndex getContactMultiplierIndex(const State& s) const OVERRIDE_11; 00921 00922 void getFrictionMultiplierIndices(const State& s, 00923 MultiplierIndex& ix_x, 00924 MultiplierIndex& ix_y) const OVERRIDE_11; 00925 00926 private: 00927 Real m_minCOR; 00928 Real m_mu_s, m_mu_d, m_mu_v; 00929 00930 Constraint::LineOnLineContact m_lineOnLine; 00931 }; 00932 00933 } // namespace SimTK 00934 00935 #endif // SimTK_SIMBODY_CONDITIONAL_CONSTRAINT_H_