ContactSpringDamper< T > Class Template Reference

#include <ContactSpringDamper.h>

Inheritance diagram for ContactSpringDamper< T >:

Collaboration diagram for ContactSpringDamper< T >:

Public Member Functions
	ContactSpringDamper (FloatingBaseModel< T > *model)
virtual	~ContactSpringDamper ()
virtual void	UpdateExternalForces (T K, T D, T dt)
virtual void	UpdateQdot (FBModelState< T > &state)
Public Member Functions inherited from ContactConstraint< T >
	ContactConstraint (FloatingBaseModel< T > *model)
virtual	~ContactConstraint ()
void	AddCollision (Collision< T > *collision)
const vectorAligned< Vec3< T > > &	getContactPosList ()
const Vec3< T > &	getGCForce (size_t idx)

Protected Member Functions
void	_groundContactWithOffset (T K, T D)
Protected Member Functions inherited from ContactConstraint< T >
void	_groundContactWithOffset (T K, T D)
size_t	_CheckContact ()

Protected Attributes
size_t	_nGC
vectorAligned< Vec2< T > >	deflectionRate
vectorAligned< Vec2< T > >	_tangentialDeflections
Protected Attributes inherited from ContactConstraint< T >
vectorAligned< Vec2< T > >	deflectionRate
vectorAligned< Vec2< T > >	_tangentialDeflections
size_t	_nContact
size_t	_nCollision
FloatingBaseModel< T > *	_model
std::vector< Collision< T > * >	_collision_list
std::vector< size_t >	_idx_list
std::vector< T >	_cp_resti_list
std::vector< T >	_cp_mu_list
std::vector< T >	_cp_penetration_list
vectorAligned< Vec3< T > >	_cp_force_list
vectorAligned< Vec3< T > >	_cp_local_force_list
vectorAligned< Vec3< T > >	_cp_pos_list
vectorAligned< Mat3< T > >	_cp_frame_list

Detailed Description

template<typename T>
class ContactSpringDamper< T >

Definition at line 12 of file ContactSpringDamper.h.

Constructor & Destructor Documentation

template<typename T >

ContactSpringDamper< T >::ContactSpringDamper ( FloatingBaseModel< T > *  model )

inline

Definition at line 14 of file ContactSpringDamper.h.

References ContactSpringDamper< T >::_nGC, ContactSpringDamper< T >::_tangentialDeflections, and ContactSpringDamper< T >::deflectionRate.

      : ContactConstraint<T>(model) {
    _nGC = CC::_model->_nGroundContact;
    for (size_t i(0); i < _nGC; ++i) {
      _tangentialDeflections.push_back(Vec2<T>::Zero());
    }
    deflectionRate.resize(_nGC);
  }

template<typename T >

virtual ContactSpringDamper< T >::~ContactSpringDamper ( )

inlinevirtual

Definition at line 23 of file ContactSpringDamper.h.

References ContactSpringDamper< T >::UpdateExternalForces().

{}

Here is the call graph for this function:

Member Function Documentation

template<typename T >

void ContactSpringDamper< T >::_groundContactWithOffset ( T  K, T  D  )

protected

Parameters

K	Ground stiffness
D	Ground damping

subfunction computing contact forces

Definition at line 42 of file ContactSpringDamper.cpp.

                                                              {
  for (size_t i = 0; i < CC::_nContact; i++) {
    Vec3<T> v =
        CC::_cp_frame_list[i].transpose() *
        CC::_model->_vGC[CC::_idx_list[i]];  // velocity in plane coordinates
    T z = CC::_cp_penetration_list[i];       // the penetration into the ground
    T zd = v[2];                             // the penetration velocity
    T zr = std::sqrt(std::max(T(0), -z));  // sqrt penetration into the ground,
                                           // or zero if we aren't penetrating
    T normalForce =
        zr *
        (-K * z - D * zd);  // normal force is spring-damper * sqrt(penetration)

    // set output force to zero for now.
    CC::_cp_local_force_list[i][0] = 0;
    CC::_cp_local_force_list[i][1] = 0;
    CC::_cp_local_force_list[i][2] = 0;

    if (normalForce > 0) {
      CC::_cp_local_force_list[i][2] =
          normalForce;  // set the normal force. This is in the plane's
                        // coordinates for now

      // first, assume sticking
      // this means the tangential deformation happens at the speed of the foot.
      deflectionRate[CC::_idx_list[i]] = v.template topLeftCorner<2, 1>();
      Vec2<T> tangentialSpringForce =
          K * zr *
          _tangentialDeflections[CC::_idx_list[i]];  // tangential force due to
                                                     // "spring"
      Vec2<T> tangentialForce =
          -tangentialSpringForce -
          D * zr * deflectionRate[CC::_idx_list[i]];  // add damping to get
                                                      // total tangential

      // check for slipping:
      T slipForce = CC::_cp_mu_list[i] *
                    normalForce;  // maximum force magnitude without slipping
      T tangentialForceMagnitude =
          tangentialForce
              .norm();  // actual force magnitude if we assume sticking
      T r = tangentialForceMagnitude / slipForce;  // ratio of force/max_force

      if (r > 1) {
        // we are slipping.
        tangentialForce =
            tangentialForce / r;  // adjust tangential force to avoid slipping
        deflectionRate[CC::_idx_list[i]] =
            -(tangentialForce + tangentialSpringForce) / (D * zr);
      }
      // set forces
      CC::_cp_local_force_list[i][0] = tangentialForce[0];
      CC::_cp_local_force_list[i][1] = tangentialForce[1];
    }
    // move back into robot frame
    CC::_cp_force_list[CC::_idx_list[i]] =
        CC::_cp_frame_list[i] * CC::_cp_local_force_list[i];
  }
}

template<typename T >

void ContactSpringDamper< T >::UpdateExternalForces ( T  K, T  D, T  dt  )

virtual

Run the ground contact model for a single collision plane on a list of ground contact points The ground is allowed to deform in the tangential direction, but not the normal direction. The ground also "remembers" its deformation between separate contact events. (however it does spring back pretty quickly)

Parameters

K	Ground stiffness
D	Ground damping
dt	Timestep (used for deflection)

compute contact forces coinciding to the contact and update external force in a dynamics model.

Implements ContactConstraint< T >.

Definition at line 13 of file ContactSpringDamper.cpp.

References spatial::forceToSpatialForce().

                                                                {
  CC::_nContact = CC::_CheckContact();
  for (size_t i(0); i < _nGC; ++i) {
    // first assume there's no contact, so the ground "springs back"
    deflectionRate[i] = (-K / D) * _tangentialDeflections[i];
  }
  if (CC::_nContact > 0) {
    _groundContactWithOffset(K, D);
    for (size_t i(0); i < CC::_nContact; ++i) {
      CC::_model->_externalForces.at(CC::_model->_gcParent[CC::_idx_list[i]]) +=
          forceToSpatialForce(CC::_cp_force_list[CC::_idx_list[i]],
                              CC::_cp_pos_list[i]);
    }
    static int count(0);
    ++count;
    if (count > 5) {
      // exit(0);
    }
  }

  for (size_t i(0); i < _nGC; ++i) {
    _tangentialDeflections[i] += dt * deflectionRate[i];
  }
}

Here is the call graph for this function:

Here is the caller graph for this function:

template<typename T >

virtual void ContactSpringDamper< T >::UpdateQdot ( FBModelState< T > &  state )

inlinevirtual

Used for impulse based contact constraint method

Parameters

state

: full state of a floating system full state

Returns

state : update velocity

Implements ContactConstraint< T >.

Definition at line 36 of file ContactSpringDamper.h.

                                                  {
    (void)state; /* Do nothing */
  }

Member Data Documentation

template<typename T >

size_t ContactSpringDamper< T >::_nGC

protected

Definition at line 41 of file ContactSpringDamper.h.

template<typename T >

vectorAligned<Vec2<T> > ContactSpringDamper< T >::_tangentialDeflections

protected

Definition at line 50 of file ContactSpringDamper.h.

template<typename T >

vectorAligned<Vec2<T> > ContactSpringDamper< T >::deflectionRate

protected

Definition at line 49 of file ContactSpringDamper.h.

---

The documentation for this class was generated from the following files:

• ContactSpringDamper.h
• ContactSpringDamper.cpp