Simbody  3.5
CompliantContactSubsystem.h
Go to the documentation of this file.
00001 #ifndef SimTK_SIMBODY_COMPLIANT_CONTACT_SUBSYSTEM_H_
00002 #define SimTK_SIMBODY_COMPLIANT_CONTACT_SUBSYSTEM_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) 2008-13 Stanford University and the Authors.        *
00013  * Authors: Peter Eastman, 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/ForceSubsystem.h"
00030 
00031 #include <cassert>
00032 
00033 namespace SimTK {
00034 
00035 class MultibodySystem;
00036 class SimbodyMatterSubsystem;
00037 class ContactTrackerSubsystem;
00038 class ContactForceGenerator;
00039 class Contact;
00040 class ContactForce;
00041 class ContactPatch;
00042 
00043 
00044 
00045 //==============================================================================
00046 //                        COMPLIANT CONTACT SUBSYSTEM
00047 //==============================================================================
00053 class SimTK_SIMBODY_EXPORT CompliantContactSubsystem : public ForceSubsystem {
00054 public:
00056 CompliantContactSubsystem() {}
00057 
00063 CompliantContactSubsystem(MultibodySystem&, 
00064                           const ContactTrackerSubsystem&);
00065 
00067 Real getTransitionVelocity() const;
00069 void setTransitionVelocity(Real vt);
00071 Real getOOTransitionVelocity() const;
00072 
00081 void setTrackDissipatedEnergy(bool shouldTrack);
00086 bool getTrackDissipatedEnergy() const;
00087 
00092 int getNumContactForces(const State& state) const;
00099 const ContactForce& getContactForce(const State& state, int n) const;
00107 const ContactForce& getContactForceById(const State& state, ContactId id) const;
00108 
00136 bool calcContactPatchDetailsById(const State&   state,
00137                                  ContactId      id,
00138                                  ContactPatch&  patch) const;
00139 
00170 Real getDissipatedEnergy(const State& state) const;
00171 
00192 void setDissipatedEnergy(State& state, Real energy) const;
00193 
00194 
00199 void adoptForceGenerator(ContactForceGenerator* generator);
00200 
00205 void adoptDefaultForceGenerator(ContactForceGenerator* generator);
00206 
00209 bool hasForceGenerator(ContactTypeId contact) const;
00210 
00214 bool hasDefaultForceGenerator() const;
00215 
00219 const ContactForceGenerator& 
00220     getContactForceGenerator(ContactTypeId contact) const; 
00221 
00224 const ContactForceGenerator& getDefaultForceGenerator() const; 
00225 
00230 const ContactTrackerSubsystem& getContactTrackerSubsystem() const;
00231 
00236 const MultibodySystem& getMultibodySystem() const;
00237    // don't show in Doxygen docs
00239 SimTK_PIMPL_DOWNCAST(CompliantContactSubsystem, ForceSubsystem);
00242 //--------------------------------------------------------------------------
00243                                  private:
00244 class CompliantContactSubsystemImpl& updImpl();
00245 const CompliantContactSubsystemImpl& getImpl() const;
00246 };
00247 
00248 
00249 
00250 //==============================================================================
00251 //                               CONTACT FORCE
00252 //==============================================================================
00301 class ContactForce {
00302 public:
00304 ContactForce() {} // invalid
00305 
00310 ContactForce(ContactId id, const Vec3& contactPt,
00311              const SpatialVec& forceOnSurface2,
00312              Real potentialEnergy, Real powerLoss)
00313 :   m_contactId(id), m_contactPt(contactPt), 
00314     m_forceOnSurface2(forceOnSurface2),
00315     m_potentialEnergy(potentialEnergy), m_powerLoss(powerLoss) {}
00316 
00318 ContactId getContactId() const {return m_contactId;}
00322 const Vec3& getContactPoint() const {return m_contactPt;}
00326 const SpatialVec& getForceOnSurface2() const {return m_forceOnSurface2;}
00329 Real getPotentialEnergy() const {return m_potentialEnergy;}
00333 Real getPowerDissipation() const {return m_powerLoss;}
00334 
00340 void setTo(ContactId id, const Vec3& contactPt,
00341            const SpatialVec& forceOnSurface2,
00342            Real potentialEnergy, Real powerLoss)
00343 {   m_contactId         = id; 
00344     m_contactPt         = contactPt;
00345     m_forceOnSurface2   = forceOnSurface2;
00346     m_potentialEnergy   = potentialEnergy;
00347     m_powerLoss         = powerLoss; }
00348 
00350 void setContactId(ContactId id) {m_contactId=id;}
00352 void setContactPoint(const Vec3& contactPt) {m_contactPt=contactPt;}
00355 void setForceOnSurface2(const SpatialVec& forceOnSurface2) 
00356 {   m_forceOnSurface2=forceOnSurface2; }
00358 void setPotentialEnergy(Real potentialEnergy) 
00359 {   m_potentialEnergy=potentialEnergy; }
00361 void setPowerDissipation(Real powerLoss) {m_powerLoss=powerLoss;}
00362 
00365 void clear() {m_contactId.invalidate();}
00367 bool isValid() const {return m_contactId.isValid();}
00368 
00373 void changeFrameInPlace(const Transform& X_BA) {
00374     m_contactPt         = X_BA*m_contactPt;        // shift & reexpress in B
00375     m_forceOnSurface2   = X_BA.R()*m_forceOnSurface2;      // reexpress in B
00376 }
00377 
00378 private:
00379 ContactId       m_contactId;            // Which Contact produced this force?
00380 Vec3            m_contactPt;            // In some frame A
00381 SpatialVec      m_forceOnSurface2;      // at contact pt, in A; neg. for Surf1
00382 Real            m_potentialEnergy;      // > 0 when due to compression
00383 Real            m_powerLoss;            // > 0 means dissipation
00384 };
00385 
00386 // For debugging.
00387 inline std::ostream& operator<<(std::ostream& o, const ContactForce& f) {
00388     o << "ContactForce for ContactId " << f.getContactId() << " (ground frame):\n";
00389     o << "  contact point=" << f.getContactPoint() << "\n";
00390     o << "  force on surf2 =" << f.getForceOnSurface2() << "\n";
00391     o << "  pot. energy=" << f.getPotentialEnergy() 
00392       << "  powerLoss=" << f.getPowerDissipation();
00393     return o << "\n";
00394 }
00395 
00396 //==============================================================================
00397 //                              CONTACT DETAIL
00398 //==============================================================================
00464 class ContactDetail {
00465 public:
00468 const Vec3& getContactPoint() const {return m_contactPt;}
00472 const UnitVec3& getContactNormal() const {return m_patchNormal;}
00476 const Vec3& getSlipVelocity() const {return m_slipVelocity;}
00479 const Vec3& getForceOnSurface2() const {return m_forceOnSurface2;}
00485 Real getDeformation() const {return m_deformation;}
00491 Real getDeformationRate() const {return m_deformationRate;}
00493 Real getPatchArea() const {return m_patchArea;}
00497 Real getPeakPressure() const {return m_peakPressure;}
00500 Real getPotentialEnergy() const {return m_potentialEnergy;}
00504 Real getPowerDissipation() const {return m_powerLoss;}
00505 
00506     
00510 void changeFrameInPlace(const Transform& X_BA) {
00511     const Rotation& R_BA = X_BA.R();
00512     m_contactPt       = X_BA*m_contactPt;       // shift & reexpress in B (18 flops)
00513     m_patchNormal     = R_BA*m_patchNormal;     // reexpress only       (3*15 flops)
00514     m_slipVelocity    = R_BA*m_slipVelocity;    //      "
00515     m_forceOnSurface2 = R_BA*m_forceOnSurface2; //      "
00516 }
00517 
00521 void changeFrameAndSwitchSurfacesInPlace(const Transform& X_BA) {
00522     const Rotation& R_BA = X_BA.R();
00523     m_contactPt       = X_BA*m_contactPt;       // shift & reexpress in B (18 flops)
00524     m_patchNormal     = R_BA*(-m_patchNormal);  // reverse & reexpress  (3*18 flops)
00525     m_slipVelocity    = R_BA*(-m_slipVelocity); //          "
00526     m_forceOnSurface2 = R_BA*(-m_forceOnSurface2); //       "
00527 }
00528 
00529 Vec3            m_contactPt;            // location of contact point C in A
00530 UnitVec3        m_patchNormal;          // points outwards from body 1, exp. in A
00531 Vec3            m_slipVelocity;         // material slip rate, perp. to normal, in A
00532 Vec3            m_forceOnSurface2;      // applied at C, -force to surf1
00533 Real            m_deformation;          // total normal compression (approach)
00534 Real            m_deformationRate;      // d/dt deformation, w.r.t. A frame
00535 Real            m_patchArea;            // >= 0
00536 Real            m_peakPressure;         // > 0 in compression
00537 Real            m_potentialEnergy;      // > 0 when due to compression
00538 Real            m_powerLoss;            // > 0 means dissipation
00539 };
00540 
00541 // For debugging.
00542 inline std::ostream& operator<<(std::ostream& o, const ContactDetail& d) {
00543     o << "ContactDetail (ground frame):\n";
00544     o << "  contact point=" << d.m_contactPt << "\n";
00545     o << "  contact normal=" << d.m_patchNormal << "\n";
00546     o << "  slip velocity=" << d.m_slipVelocity << "\n";
00547     o << "  force on surf2 =" << d.m_forceOnSurface2 << "\n";
00548     o << "  deformation=" << d.m_deformation 
00549       << "  deformation rate=" << d.m_deformationRate << "\n";
00550     o << "  patch area=" << d.m_patchArea 
00551       << "  peak pressure=" << d.m_peakPressure << "\n";
00552     o << "  pot. energy=" << d.m_potentialEnergy << "  powerLoss=" << d.m_powerLoss;
00553     return o << "\n";
00554 }
00555 
00556 
00557 
00558 //==============================================================================
00559 //                               CONTACT PATCH
00560 //==============================================================================
00581 class SimTK_SIMBODY_EXPORT ContactPatch {
00582 public:
00583 void clear() {m_resultant.clear(); m_elements.clear();}
00584 bool isValid() const {return m_resultant.isValid();}
00585 const ContactForce& getContactForce() const {return m_resultant;}
00586 int getNumDetails() const {return (int)m_elements.size();}
00587 const ContactDetail& getContactDetail(int n) const {return m_elements[n];}
00588 
00592 void changeFrameInPlace(const Transform& X_BA) {
00593     m_resultant.changeFrameInPlace(X_BA);
00594     for (unsigned i=0; i<m_elements.size(); ++i)
00595         m_elements[i].changeFrameInPlace(X_BA);
00596 }
00597 
00598 ContactForce            m_resultant;
00599 Array_<ContactDetail>   m_elements;
00600 };
00601 
00602 
00603 
00604 //==============================================================================
00605 //                          CONTACT FORCE GENERATOR
00606 //==============================================================================
00615 class SimTK_SIMBODY_EXPORT ContactForceGenerator {
00616 public:
00617 // Reasonably good physically-based compliant contact models.
00618 class ElasticFoundation;        // for TriangleMeshContact
00619 class HertzCircular;            // for CircularPointContact
00620 class HertzElliptical;          // for EllipticalPointContact
00621 
00622 // Penalty-based models enforcing non-penetration but without attempting
00623 // to model the contacting materials physically.
00624 class BrickHalfSpacePenalty;    // for BrickHalfSpaceContact
00625 
00626 // These are for response to unknown ContactTypeIds.
00627 class DoNothing;     // do nothing if called
00628 class ThrowError;    // throw an error if called
00629 
00631 explicit ContactForceGenerator(ContactTypeId type): m_contactType(type) {}
00632 
00636 ContactTypeId getContactTypeId() const {return m_contactType;}
00637 
00638 const CompliantContactSubsystem& getCompliantContactSubsystem() const
00639 {   assert(m_compliantContactSubsys); return *m_compliantContactSubsys; }
00640 void setCompliantContactSubsystem(const CompliantContactSubsystem* sub)
00641 {   m_compliantContactSubsys = sub; }
00642 
00644 virtual ~ContactForceGenerator() {}
00645 
00656 virtual void calcContactForce
00657    (const State&            state,
00658     const Contact&          overlapping,
00659     const SpatialVec&       V_S1S2,  // relative surface velocity (S2 in S1)
00660     ContactForce&           contactForce) const = 0;
00661 
00668 virtual void calcContactPatch
00669    (const State&            state,
00670     const Contact&          overlapping,
00671     const SpatialVec&       V_S1S2,  // relative surface velocity (S2 in S1)
00672     ContactPatch&           patch) const = 0;
00673 
00674 
00675 //--------------------------------------------------------------------------
00676 private:
00677     // This generator should be called only for Contact objects of the 
00678     // indicated type id.
00679     ContactTypeId                       m_contactType;
00680     // This is a reference to the owning CompliantContactSubsystem if any;
00681     // don't delete on destruction.
00682     const CompliantContactSubsystem*    m_compliantContactSubsys;
00683 };
00684 
00685 
00686 
00687 
00688 //==============================================================================
00689 //                         HERTZ CIRCULAR GENERATOR
00690 //==============================================================================
00691 
00697 class SimTK_SIMBODY_EXPORT ContactForceGenerator::HertzCircular 
00698 :   public ContactForceGenerator {
00699 public:
00700 HertzCircular() 
00701 :   ContactForceGenerator(CircularPointContact::classTypeId()) {}
00702 
00703 void calcContactForce
00704    (const State&            state,
00705     const Contact&          overlapping,
00706     const SpatialVec&       V_S1S2,
00707     ContactForce&           contactForce) const OVERRIDE_11;
00708 
00709 void calcContactPatch
00710    (const State&            state,
00711     const Contact&          overlapping,
00712     const SpatialVec&       V_S1S2,
00713     ContactPatch&           patch) const OVERRIDE_11;
00714 };
00715 
00716 
00717 
00718 //==============================================================================
00719 //                         HERTZ ELLIPTICAL GENERATOR
00720 //==============================================================================
00721 
00727 class SimTK_SIMBODY_EXPORT ContactForceGenerator::HertzElliptical 
00728 :   public ContactForceGenerator {
00729 public:
00730 HertzElliptical() 
00731 :   ContactForceGenerator(EllipticalPointContact::classTypeId()) {}
00732 
00733 void calcContactForce
00734    (const State&            state,
00735     const Contact&          overlapping,
00736     const SpatialVec&       V_S1S2,
00737     ContactForce&           contactForce) const OVERRIDE_11;
00738 
00739 void calcContactPatch
00740    (const State&            state,
00741     const Contact&          overlapping,
00742     const SpatialVec&       V_S1S2,
00743     ContactPatch&           patch) const OVERRIDE_11;
00744 };
00745 
00746 
00747 
00748 
00749 //==============================================================================
00750 //                         BRICK HALFSPACE GENERATOR
00751 //==============================================================================
00752 
00755 class SimTK_SIMBODY_EXPORT ContactForceGenerator::BrickHalfSpacePenalty 
00756 :   public ContactForceGenerator {
00757 public:
00758 BrickHalfSpacePenalty() 
00759 :   ContactForceGenerator(BrickHalfSpaceContact::classTypeId()) {}
00760 
00761 void calcContactForce
00762    (const State&            state,
00763     const Contact&          overlapping,
00764     const SpatialVec&       V_S1S2,
00765     ContactForce&           contactForce) const OVERRIDE_11;
00766 
00767 void calcContactPatch
00768    (const State&            state,
00769     const Contact&          overlapping,
00770     const SpatialVec&       V_S1S2,
00771     ContactPatch&           patch) const OVERRIDE_11;
00772 };
00773 
00774 
00775 
00776 //==============================================================================
00777 //                       ELASTIC FOUNDATION GENERATOR
00778 //==============================================================================
00782 class SimTK_SIMBODY_EXPORT ContactForceGenerator::ElasticFoundation 
00783 :   public ContactForceGenerator {
00784 public:
00785 ElasticFoundation() 
00786 :   ContactForceGenerator(TriangleMeshContact::classTypeId()) {}
00787 
00788 void calcContactForce
00789    (const State&            state,
00790     const Contact&          overlapping,
00791     const SpatialVec&       V_S1S2,
00792     ContactForce&           contactForce) const OVERRIDE_11;
00793 
00794 void calcContactPatch
00795    (const State&            state,
00796     const Contact&          overlapping,
00797     const SpatialVec&       V_S1S2,
00798     ContactPatch&           patch) const OVERRIDE_11;
00799 
00800 private:
00801 void calcContactForceAndDetails
00802    (const State&            state,
00803     const Contact&          overlapping,
00804     const SpatialVec&       V_S1S2,
00805     ContactForce&           contactForce,
00806     Array_<ContactDetail>*  contactDetails) const;
00807 
00808 void calcWeightedPatchCentroid
00809    (const ContactGeometry::TriangleMesh&    mesh,
00810     const std::set<int>&                    insideFaces,
00811     Vec3&                                   weightedPatchCentroid,
00812     Real&                                   patchArea) const;
00813                        
00814 void processOneMesh
00815    (const State&                            state,
00816     const ContactGeometry::TriangleMesh&    mesh,
00817     const std::set<int>&                    insideFaces,
00818     const Transform&                        X_MO, 
00819     const SpatialVec&                       V_MO,
00820     const ContactGeometry&                  other,
00821     Real                                    meshDeformationFraction, // 0..1
00822     Real                                    areaScaleFactor, // >= 0
00823     Real k, Real c, Real us, Real ud, Real uv,
00824     const Vec3&                 resultantPt_M, // where to apply forces
00825     SpatialVec&                 resultantForceOnOther_M, // at resultant pt
00826     Real&                       potentialEnergy,
00827     Real&                       powerLoss,
00828     Vec3&                       weightedCenterOfPressure_M, // COP
00829     Real&                       sumOfAllPressureMoments,    // COP weight
00830     Array_<ContactDetail>*      contactDetails) const;
00831 };
00832 
00833 
00834 
00835 
00836 //==============================================================================
00837 //                        DO NOTHING FORCE GENERATOR
00838 //==============================================================================
00842 class SimTK_SIMBODY_EXPORT ContactForceGenerator::DoNothing 
00843 :   public ContactForceGenerator {
00844 public:
00845 explicit DoNothing(ContactTypeId type = ContactTypeId(0)) 
00846 :   ContactForceGenerator(type) {}
00847 
00848 void calcContactForce
00849    (const State&            state,
00850     const Contact&          overlapping,
00851     const SpatialVec&       V_S1S2,
00852     ContactForce&           contactForce) const OVERRIDE_11
00853 {   SimTK_ASSERT_ALWAYS(!"implemented",
00854         "ContactForceGenerator::DoNothing::calcContactForce() not implemented yet."); }
00855 void calcContactPatch
00856    (const State&            state,
00857     const Contact&          overlapping,
00858     const SpatialVec&       V_S1S2,
00859     ContactPatch&           patch) const OVERRIDE_11
00860 {   SimTK_ASSERT_ALWAYS(!"implemented",
00861         "ContactForceGenerator::DoNothing::calcContactPatch() not implemented yet."); }
00862 };
00863 
00864 
00865 
00866 //==============================================================================
00867 //                       THROW ERROR FORCE GENERATOR
00868 //==============================================================================
00873 class SimTK_SIMBODY_EXPORT ContactForceGenerator::ThrowError 
00874 :   public ContactForceGenerator {
00875 public:
00876 explicit ThrowError(ContactTypeId type = ContactTypeId(0)) 
00877 :   ContactForceGenerator(type) {}
00878 
00879 void calcContactForce
00880    (const State&            state,
00881     const Contact&          overlapping,
00882     const SpatialVec&       V_S1S2,
00883     ContactForce&           contactForce) const OVERRIDE_11
00884 {   SimTK_ASSERT_ALWAYS(!"implemented",
00885         "ContactForceGenerator::ThrowError::calcContactForce() not implemented yet."); }
00886 void calcContactPatch
00887    (const State&            state,
00888     const Contact&          overlapping,
00889     const SpatialVec&       V_S1S2,
00890     ContactPatch&           patch) const OVERRIDE_11
00891 {   SimTK_ASSERT_ALWAYS(!"implemented",
00892         "ContactForceGenerator::ThrowError::calcContactPatch() not implemented yet."); }
00893 };
00894 
00895 } // namespace SimTK
00896 
00897 #endif // SimTK_SIMBODY_COMPLIANT_CONTACT_SUBSYSTEM_H_
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines