CompliantContactSubsystem.h

Go to the documentation of this file.

#ifndef SimTK_SIMBODY_COMPLIANT_CONTACT_SUBSYSTEM_H_
#define SimTK_SIMBODY_COMPLIANT_CONTACT_SUBSYSTEM_H_

/* -------------------------------------------------------------------------- *
 *                      SimTK Core: SimTK Simbody(tm)                         *
 * -------------------------------------------------------------------------- *
 * This is part of the SimTK Core 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.               *
 *                                                                            *
 * Portions copyright (c) 2008-10 Stanford University and the Authors.        *
 * Authors: Peter Eastman, Michael Sherman                                    *
 * Contributors:                                                              *
 *                                                                            *
 * Permission is hereby granted, free of charge, to any person obtaining a    *
 * copy of this software and associated documentation files (the "Software"), *
 * to deal in the Software without restriction, including without limitation  *
 * the rights to use, copy, modify, merge, publish, distribute, sublicense,   *
 * and/or sell copies of the Software, and to permit persons to whom the      *
 * Software is furnished to do so, subject to the following conditions:       *
 *                                                                            *
 * The above copyright notice and this permission notice shall be included in *
 * all copies or substantial portions of the Software.                        *
 *                                                                            *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,   *
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL    *
 * THE AUTHORS, CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,    *
 * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR      *
 * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE  *
 * USE OR OTHER DEALINGS IN THE SOFTWARE.                                     *
 * -------------------------------------------------------------------------- */

#include "simbody/internal/common.h"
#include "simbody/internal/ForceSubsystem.h"
#include "simbody/internal/Contact.h"
#include "simbody/internal/ContactGeometry.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;

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

Real getDissipatedEnergy(const State& state) const;

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

void calcContactPatchDetails(const State& state, 
                             Array_<ContactPatch>& patches) 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) {}

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 clear() {m_contactId.invalidate();}
bool isValid() const {return m_contactId.isValid();}

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

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

inline std::ostream& operator<<(std::ostream& o, const ContactForce& f) {
    o << "ContactForce for ContactId " << f.m_contactId << " (ground frame):\n";
    o << "  contact point=" << f.m_contactPt << "\n";
    o << "  force on surf2 =" << f.m_forceOnSurface2 << "\n";
    o << "  pot. energy=" << f.m_potentialEnergy << "  powerLoss=" << f.m_powerLoss;
    return o << "\n";
}

//==============================================================================
//                              CONTACT DETAIL
//==============================================================================
class ContactDetail {
public:
int             m_whichSurface;         // 0=patch, 1=surface1, 2=surface2
int             m_elementId;            // meaning defined by patch type
Transform       m_patchFrame;           // X_GP
Vec2            m_patchHalfDimensions;  // x >= y >= 0
Vec3            m_deformation;          // vec to undeformed loc of material
Vec3            m_deformationRate;      // d/dt deformation, w.r.t. body frame
SpatialVec      m_slipVelocity;         // material slip rate
SpatialVec      m_forceOnSurface2InP;   // applied at OP, -force to surf1
Real            m_patchArea;            // >= 0
Real            m_peakPressure;         // > 0 in compression
Real            m_potentialEnergy;      // > 0 when due to compression
Real            m_powerLoss;            // > 0 means dissipation
};



//==============================================================================
//                               CONTACT PATCH
//==============================================================================
class SimTK_SIMBODY_EXPORT ContactPatch {
public:
ContactForce            m_resultant;
Array_<ContactDetail>   m_elements;
};



//==============================================================================
//                          CONTACT FORCE GENERATOR
//==============================================================================
class SimTK_SIMBODY_EXPORT ContactForceGenerator {
public:
class ElasticFoundation;        // for TriangleMeshContact
class HertzCircular;            // for PointContact
class HertzElliptical;          // for EllipticalPointContact

// 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()) {}

virtual ~HertzCircular() {}
virtual void calcContactForce
   (const State&            state,
    const Contact&          overlapping,
    const SpatialVec&       V_S1S2,
    ContactForce&           contactForce) const;

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




//==============================================================================
//                       ELASTIC FOUNDATION GENERATOR
//==============================================================================
class SimTK_SIMBODY_EXPORT ContactForceGenerator::ElasticFoundation 
:   public ContactForceGenerator {
public:
ElasticFoundation() 
:   ContactForceGenerator(TriangleMeshContact::classTypeId()) {}
virtual ~ElasticFoundation() {}
virtual void calcContactForce
   (const State&            state,
    const Contact&          overlapping,
    const SpatialVec&       V_S1S2,
    ContactForce&           contactForce) const;

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

private:
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 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) {}
virtual ~DoNothing() {}
virtual void calcContactForce
   (const State&            state,
    const Contact&          overlapping,
    const SpatialVec&       V_S1S2,
    ContactForce&           contactForce) const
{   SimTK_ASSERT_ALWAYS(!"implemented",
        "ContactForceGenerator::DoNothing::calcContactForce() not implemented yet."); }
virtual void calcContactPatch
   (const State&            state,
    const Contact&          overlapping,
    const SpatialVec&       V_S1S2,
    ContactPatch&           patch) const
{   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) {}
virtual ~ThrowError() {}
virtual void calcContactForce
   (const State&            state,
    const Contact&          overlapping,
    const SpatialVec&       V_S1S2,
    ContactForce&           contactForce) const
{   SimTK_ASSERT_ALWAYS(!"implemented",
        "ContactForceGenerator::ThrowError::calcContactForce() not implemented yet."); }
virtual void calcContactPatch
   (const State&            state,
    const Contact&          overlapping,
    const SpatialVec&       V_S1S2,
    ContactPatch&           patch) const
{   SimTK_ASSERT_ALWAYS(!"implemented",
        "ContactForceGenerator::ThrowError::calcContactPatch() not implemented yet."); }
};

} // namespace SimTK

#endif // SimTK_SIMBODY_COMPLIANT_CONTACT_SUBSYSTEM_H_