CompliantContactSubsystem.h

Go to the documentation of this file.

#ifndef SimTK_SIMBODY_COMPLIANT_CONTACT_SUBSYSTEM_H_
#define SimTK_SIMBODY_COMPLIANT_CONTACT_SUBSYSTEM_H_

/* -------------------------------------------------------------------------- *
 *                               Simbody(tm)                                  *
 * -------------------------------------------------------------------------- *
 * This is part of the SimTK biosimulation toolkit originating from           *
 * Simbios, the NIH National Center for Physics-Based Simulation of           *
 * Biological Structures at Stanford, funded under the NIH Roadmap for        *
 * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody.  *
 *                                                                            *
 * Portions copyright (c) 2008-13 Stanford University and the Authors.        *
 * Authors: Peter Eastman, Michael Sherman                                    *
 * Contributors:                                                              *
 *                                                                            *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may    *
 * not use this file except in compliance with the License. You may obtain a  *
 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0.         *
 *                                                                            *
 * Unless required by applicable law or agreed to in writing, software        *
 * distributed under the License is distributed on an "AS IS" BASIS,          *
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *
 * See the License for the specific language governing permissions and        *
 * limitations under the License.                                             *
 * -------------------------------------------------------------------------- */

#include "SimTKmath.h"
#include "simbody/internal/common.h"
#include "simbody/internal/ForceSubsystem.h"

#include <cassert>

namespace SimTK {

class MultibodySystem;
class SimbodyMatterSubsystem;
class ContactTrackerSubsystem;
class ContactForceGenerator;
class Contact;
class ContactForce;
class ContactPatch;



//==============================================================================
//                        COMPLIANT CONTACT SUBSYSTEM
//==============================================================================
class SimTK_SIMBODY_EXPORT CompliantContactSubsystem : public ForceSubsystem {
public:
CompliantContactSubsystem() {}

CompliantContactSubsystem(MultibodySystem&, 
                          const ContactTrackerSubsystem&);

Real getTransitionVelocity() const;
void setTransitionVelocity(Real vt);
Real getOOTransitionVelocity() const;

void setTrackDissipatedEnergy(bool shouldTrack);
bool getTrackDissipatedEnergy() const;

int getNumContactForces(const State& state) const;
const ContactForce& getContactForce(const State& state, int n) const;
const ContactForce& getContactForceById(const State& state, ContactId id) const;

bool calcContactPatchDetailsById(const State&   state,
                                 ContactId      id,
                                 ContactPatch&  patch) const;

Real getDissipatedEnergy(const State& state) const;

void setDissipatedEnergy(State& state, Real energy) const;


void adoptForceGenerator(ContactForceGenerator* generator);

void adoptDefaultForceGenerator(ContactForceGenerator* generator);

bool hasForceGenerator(ContactTypeId contact) const;

bool hasDefaultForceGenerator() const;

const ContactForceGenerator& 
    getContactForceGenerator(ContactTypeId contact) const; 

const ContactForceGenerator& getDefaultForceGenerator() const; 

const ContactTrackerSubsystem& getContactTrackerSubsystem() const;

const MultibodySystem& getMultibodySystem() const;
   // don't show in Doxygen docs
SimTK_PIMPL_DOWNCAST(CompliantContactSubsystem, ForceSubsystem);
//--------------------------------------------------------------------------
                                 private:
class CompliantContactSubsystemImpl& updImpl();
const CompliantContactSubsystemImpl& getImpl() const;
};



//==============================================================================
//                               CONTACT FORCE
//==============================================================================
class ContactForce {
public:
ContactForce() {} // invalid

ContactForce(ContactId id, const Vec3& contactPt,
             const SpatialVec& forceOnSurface2,
             Real potentialEnergy, Real powerLoss)
:   m_contactId(id), m_contactPt(contactPt), 
    m_forceOnSurface2(forceOnSurface2),
    m_potentialEnergy(potentialEnergy), m_powerLoss(powerLoss) {}

ContactId getContactId() const {return m_contactId;}
const Vec3& getContactPoint() const {return m_contactPt;}
const SpatialVec& getForceOnSurface2() const {return m_forceOnSurface2;}
Real getPotentialEnergy() const {return m_potentialEnergy;}
Real getPowerDissipation() const {return m_powerLoss;}

void setTo(ContactId id, const Vec3& contactPt,
           const SpatialVec& forceOnSurface2,
           Real potentialEnergy, Real powerLoss)
{   m_contactId         = id; 
    m_contactPt         = contactPt;
    m_forceOnSurface2   = forceOnSurface2;
    m_potentialEnergy   = potentialEnergy;
    m_powerLoss         = powerLoss; }

void setContactId(ContactId id) {m_contactId=id;}
void setContactPoint(const Vec3& contactPt) {m_contactPt=contactPt;}
void setForceOnSurface2(const SpatialVec& forceOnSurface2) 
{   m_forceOnSurface2=forceOnSurface2; }
void setPotentialEnergy(Real potentialEnergy) 
{   m_potentialEnergy=potentialEnergy; }
void setPowerDissipation(Real powerLoss) {m_powerLoss=powerLoss;}

void clear() {m_contactId.invalidate();}
bool isValid() const {return m_contactId.isValid();}

void changeFrameInPlace(const Transform& X_BA) {
    m_contactPt         = X_BA*m_contactPt;        // shift & reexpress in B
    m_forceOnSurface2   = X_BA.R()*m_forceOnSurface2;      // reexpress in B
}

private:
ContactId       m_contactId;            // Which Contact produced this force?
Vec3            m_contactPt;            // In some frame A
SpatialVec      m_forceOnSurface2;      // at contact pt, in A; neg. for Surf1
Real            m_potentialEnergy;      // > 0 when due to compression
Real            m_powerLoss;            // > 0 means dissipation
};

// For debugging.
inline std::ostream& operator<<(std::ostream& o, const ContactForce& f) {
    o << "ContactForce for ContactId " << f.getContactId() << " (ground frame):\n";
    o << "  contact point=" << f.getContactPoint() << "\n";
    o << "  force on surf2 =" << f.getForceOnSurface2() << "\n";
    o << "  pot. energy=" << f.getPotentialEnergy() 
      << "  powerLoss=" << f.getPowerDissipation();
    return o << "\n";
}

//==============================================================================
//                              CONTACT DETAIL
//==============================================================================
class ContactDetail {
public:
const Vec3& getContactPoint() const {return m_contactPt;}
const UnitVec3& getContactNormal() const {return m_patchNormal;}
const Vec3& getSlipVelocity() const {return m_slipVelocity;}
const Vec3& getForceOnSurface2() const {return m_forceOnSurface2;}
Real getDeformation() const {return m_deformation;}
Real getDeformationRate() const {return m_deformationRate;}
Real getPatchArea() const {return m_patchArea;}
Real getPeakPressure() const {return m_peakPressure;}
Real getPotentialEnergy() const {return m_potentialEnergy;}
Real getPowerDissipation() const {return m_powerLoss;}

    
void changeFrameInPlace(const Transform& X_BA) {
    const Rotation& R_BA = X_BA.R();
    m_contactPt       = X_BA*m_contactPt;       // shift & reexpress in B (18 flops)
    m_patchNormal     = R_BA*m_patchNormal;     // reexpress only       (3*15 flops)
    m_slipVelocity    = R_BA*m_slipVelocity;    //      "
    m_forceOnSurface2 = R_BA*m_forceOnSurface2; //      "
}

void changeFrameAndSwitchSurfacesInPlace(const Transform& X_BA) {
    const Rotation& R_BA = X_BA.R();
    m_contactPt       = X_BA*m_contactPt;       // shift & reexpress in B (18 flops)
    m_patchNormal     = R_BA*(-m_patchNormal);  // reverse & reexpress  (3*18 flops)
    m_slipVelocity    = R_BA*(-m_slipVelocity); //          "
    m_forceOnSurface2 = R_BA*(-m_forceOnSurface2); //       "
}

Vec3            m_contactPt;            // location of contact point C in A
UnitVec3        m_patchNormal;          // points outwards from body 1, exp. in A
Vec3            m_slipVelocity;         // material slip rate, perp. to normal, in A
Vec3            m_forceOnSurface2;      // applied at C, -force to surf1
Real            m_deformation;          // total normal compression (approach)
Real            m_deformationRate;      // d/dt deformation, w.r.t. A frame
Real            m_patchArea;            // >= 0
Real            m_peakPressure;         // > 0 in compression
Real            m_potentialEnergy;      // > 0 when due to compression
Real            m_powerLoss;            // > 0 means dissipation
};

// For debugging.
inline std::ostream& operator<<(std::ostream& o, const ContactDetail& d) {
    o << "ContactDetail (ground frame):\n";
    o << "  contact point=" << d.m_contactPt << "\n";
    o << "  contact normal=" << d.m_patchNormal << "\n";
    o << "  slip velocity=" << d.m_slipVelocity << "\n";
    o << "  force on surf2 =" << d.m_forceOnSurface2 << "\n";
    o << "  deformation=" << d.m_deformation 
      << "  deformation rate=" << d.m_deformationRate << "\n";
    o << "  patch area=" << d.m_patchArea 
      << "  peak pressure=" << d.m_peakPressure << "\n";
    o << "  pot. energy=" << d.m_potentialEnergy << "  powerLoss=" << d.m_powerLoss;
    return o << "\n";
}



//==============================================================================
//                               CONTACT PATCH
//==============================================================================
class SimTK_SIMBODY_EXPORT ContactPatch {
public:
void clear() {m_resultant.clear(); m_elements.clear();}
bool isValid() const {return m_resultant.isValid();}
const ContactForce& getContactForce() const {return m_resultant;}
int getNumDetails() const {return (int)m_elements.size();}
const ContactDetail& getContactDetail(int n) const {return m_elements[n];}

void changeFrameInPlace(const Transform& X_BA) {
    m_resultant.changeFrameInPlace(X_BA);
    for (unsigned i=0; i<m_elements.size(); ++i)
        m_elements[i].changeFrameInPlace(X_BA);
}

ContactForce            m_resultant;
Array_<ContactDetail>   m_elements;
};



//==============================================================================
//                          CONTACT FORCE GENERATOR
//==============================================================================
class SimTK_SIMBODY_EXPORT ContactForceGenerator {
public:
// Reasonably good physically-based compliant contact models.
class ElasticFoundation;        // for TriangleMeshContact
class HertzCircular;            // for CircularPointContact
class HertzElliptical;          // for EllipticalPointContact

// Penalty-based models enforcing non-penetration but without attempting
// to model the contacting materials physically.
class BrickHalfSpacePenalty;    // for BrickHalfSpaceContact

// These are for response to unknown ContactTypeIds.
class DoNothing;     // do nothing if called
class ThrowError;    // throw an error if called

explicit ContactForceGenerator(ContactTypeId type): m_contactType(type) {}

ContactTypeId getContactTypeId() const {return m_contactType;}

const CompliantContactSubsystem& getCompliantContactSubsystem() const
{   assert(m_compliantContactSubsys); return *m_compliantContactSubsys; }
void setCompliantContactSubsystem(const CompliantContactSubsystem* sub)
{   m_compliantContactSubsys = sub; }

virtual ~ContactForceGenerator() {}

virtual void calcContactForce
   (const State&            state,
    const Contact&          overlapping,
    const SpatialVec&       V_S1S2,  // relative surface velocity (S2 in S1)
    ContactForce&           contactForce) const = 0;

virtual void calcContactPatch
   (const State&            state,
    const Contact&          overlapping,
    const SpatialVec&       V_S1S2,  // relative surface velocity (S2 in S1)
    ContactPatch&           patch) const = 0;


//--------------------------------------------------------------------------
private:
    // This generator should be called only for Contact objects of the 
    // indicated type id.
    ContactTypeId                       m_contactType;
    // This is a reference to the owning CompliantContactSubsystem if any;
    // don't delete on destruction.
    const CompliantContactSubsystem*    m_compliantContactSubsys;
};




//==============================================================================
//                         HERTZ CIRCULAR GENERATOR
//==============================================================================

class SimTK_SIMBODY_EXPORT ContactForceGenerator::HertzCircular 
:   public ContactForceGenerator {
public:
HertzCircular() 
:   ContactForceGenerator(CircularPointContact::classTypeId()) {}

void calcContactForce
   (const State&            state,
    const Contact&          overlapping,
    const SpatialVec&       V_S1S2,
    ContactForce&           contactForce) const override;

void calcContactPatch
   (const State&            state,
    const Contact&          overlapping,
    const SpatialVec&       V_S1S2,
    ContactPatch&           patch) const override;
};



//==============================================================================
//                         HERTZ ELLIPTICAL GENERATOR
//==============================================================================

class SimTK_SIMBODY_EXPORT ContactForceGenerator::HertzElliptical 
:   public ContactForceGenerator {
public:
HertzElliptical() 
:   ContactForceGenerator(EllipticalPointContact::classTypeId()) {}

void calcContactForce
   (const State&            state,
    const Contact&          overlapping,
    const SpatialVec&       V_S1S2,
    ContactForce&           contactForce) const override;

void calcContactPatch
   (const State&            state,
    const Contact&          overlapping,
    const SpatialVec&       V_S1S2,
    ContactPatch&           patch) const override;
};




//==============================================================================
//                         BRICK HALFSPACE GENERATOR
//==============================================================================

class SimTK_SIMBODY_EXPORT ContactForceGenerator::BrickHalfSpacePenalty 
:   public ContactForceGenerator {
public:
BrickHalfSpacePenalty() 
:   ContactForceGenerator(BrickHalfSpaceContact::classTypeId()) {}

void calcContactForce
   (const State&            state,
    const Contact&          overlapping,
    const SpatialVec&       V_S1S2,
    ContactForce&           contactForce) const override;

void calcContactPatch
   (const State&            state,
    const Contact&          overlapping,
    const SpatialVec&       V_S1S2,
    ContactPatch&           patch) const override;
};



//==============================================================================
//                       ELASTIC FOUNDATION GENERATOR
//==============================================================================
class SimTK_SIMBODY_EXPORT ContactForceGenerator::ElasticFoundation 
:   public ContactForceGenerator {
public:
ElasticFoundation() 
:   ContactForceGenerator(TriangleMeshContact::classTypeId()) {}

void calcContactForce
   (const State&            state,
    const Contact&          overlapping,
    const SpatialVec&       V_S1S2,
    ContactForce&           contactForce) const override;

void calcContactPatch
   (const State&            state,
    const Contact&          overlapping,
    const SpatialVec&       V_S1S2,
    ContactPatch&           patch) const override;

private:
void calcContactForceAndDetails
   (const State&            state,
    const Contact&          overlapping,
    const SpatialVec&       V_S1S2,
    ContactForce&           contactForce,
    Array_<ContactDetail>*  contactDetails) const;

void calcWeightedPatchCentroid
   (const ContactGeometry::TriangleMesh&    mesh,
    const std::set<int>&                    insideFaces,
    Vec3&                                   weightedPatchCentroid,
    Real&                                   patchArea) const;
                       
void processOneMesh
   (const State&                            state,
    const ContactGeometry::TriangleMesh&    mesh,
    const std::set<int>&                    insideFaces,
    const Transform&                        X_MO, 
    const SpatialVec&                       V_MO,
    const ContactGeometry&                  other,
    Real                                    meshDeformationFraction, // 0..1
    Real                                    areaScaleFactor, // >= 0
    Real k, Real c, Real us, Real ud, Real uv,
    const Vec3&                 resultantPt_M, // where to apply forces
    SpatialVec&                 resultantForceOnOther_M, // at resultant pt
    Real&                       potentialEnergy,
    Real&                       powerLoss,
    Vec3&                       weightedCenterOfPressure_M, // COP
    Real&                       sumOfAllPressureMoments,    // COP weight
    Array_<ContactDetail>*      contactDetails) const;
};




//==============================================================================
//                        DO NOTHING FORCE GENERATOR
//==============================================================================
class SimTK_SIMBODY_EXPORT ContactForceGenerator::DoNothing 
:   public ContactForceGenerator {
public:
explicit DoNothing(ContactTypeId type = ContactTypeId(0)) 
:   ContactForceGenerator(type) {}

void calcContactForce
   (const State&            state,
    const Contact&          overlapping,
    const SpatialVec&       V_S1S2,
    ContactForce&           contactForce) const override
{   SimTK_ASSERT_ALWAYS(!"implemented",
        "ContactForceGenerator::DoNothing::calcContactForce() not implemented yet."); }
void calcContactPatch
   (const State&            state,
    const Contact&          overlapping,
    const SpatialVec&       V_S1S2,
    ContactPatch&           patch) const override
{   SimTK_ASSERT_ALWAYS(!"implemented",
        "ContactForceGenerator::DoNothing::calcContactPatch() not implemented yet."); }
};



//==============================================================================
//                       THROW ERROR FORCE GENERATOR
//==============================================================================
class SimTK_SIMBODY_EXPORT ContactForceGenerator::ThrowError 
:   public ContactForceGenerator {
public:
explicit ThrowError(ContactTypeId type = ContactTypeId(0)) 
:   ContactForceGenerator(type) {}

void calcContactForce
   (const State&            state,
    const Contact&          overlapping,
    const SpatialVec&       V_S1S2,
    ContactForce&           contactForce) const override
{   SimTK_ASSERT_ALWAYS(!"implemented",
        "ContactForceGenerator::ThrowError::calcContactForce() not implemented yet."); }
void calcContactPatch
   (const State&            state,
    const Contact&          overlapping,
    const SpatialVec&       V_S1S2,
    ContactPatch&           patch) const override
{   SimTK_ASSERT_ALWAYS(!"implemented",
        "ContactForceGenerator::ThrowError::calcContactPatch() not implemented yet."); }
};

} // namespace SimTK

#endif // SimTK_SIMBODY_COMPLIANT_CONTACT_SUBSYSTEM_H_