void HuntCrossleyContactRep::processContact
(const Real& R,
const Real& k1, const Real& c1, const Transform& X_GB1, const SpatialVec& V_GB1,
const Real& k2, const Real& c2, const Transform& X_GB2, const SpatialVec& V_GB2,
const Vec3& undefContactPt1_G, const Vec3& undefContactPt2_G,
const UnitVec3& contactNormal_G, // points from body2 to body1
Real& pe, SpatialVec& force1, SpatialVec& force2) const
{
const Real x = ~(undefContactPt2_G-undefContactPt1_G) * contactNormal_G; // 8 flops
// Body 1 must be "above" body 2, meaning its undeformed contact point must be "below"
// body 2's so that they are interpenetrating.
assert(x > 0);
// Calculate the fraction of total squish x which will be undergone by body 1; body 2's
// squish fraction will be 1-squish1.
const Real squish1 = k2/(k1+k2); // ~11 flops counting divide as ~10
const Real squish2 = 1 - squish1; // 1 flop
// Now we can find the real contact point, which is a little up the normal from pt1
const Vec3 contactPt_G = undefContactPt1_G + (squish1*x)*contactNormal_G; // 7 flops
// Find the body stations coincident with the contact point so that we can calculate
// their velocities.
const Vec3 contactPt1_G = contactPt_G - X_GB1.p(); // 3 flops
const Vec3 contactPt2_G = contactPt_G - X_GB2.p(); // 3 flops
const Vec3 vContactPt1_G = V_GB1[1] + V_GB1[0] % contactPt1_G; // 12 flops
const Vec3 vContactPt2_G = V_GB2[1] + V_GB2[0] % contactPt2_G; // 12 flops
const Real v = ~(vContactPt2_G-vContactPt1_G)*contactNormal_G; // dx/dt (8 flops)
const Real k=k1*squish1; // = k2*squish2 1 flop
const Real c=c1*squish1 + c2*squish2; // 3 flops
const Real fH = Real(4./3.) * k * x * std::sqrt(R*k*x); // ~35 flops
const Real f = fH * (1 + Real(1.5)*c*v); // 4 flops
// If the resulting force is negative, the multibody system is "yanking"
// the objects apart so fast that the material can't undeform fast enough
// to keep up. That means it can't apply any force and that the stored
// potential energy will now be wasted. (I suppose it would be dissipated
// internally as the body's surface oscillated around its undeformed shape.)
if (f > 0) { // 1 flop
pe += Real(2./5.) * fH * x; // 3 flops
const Vec3 fvec = f * contactNormal_G; // points towards body1 (3 flops)
force1 += SpatialVec( contactPt1_G % fvec, fvec); // 15 flops
force2 -= SpatialVec( contactPt2_G % fvec, fvec); // 15 flops
}
}
void multiplyBySystemJacobian(
const SBTreePositionCache& pc,
const Real* v,
SpatialVec* Jv) const
{
Jv[0] = SpatialVec(Vec3(0));
}
SpatialVec Force::DiscreteForces::
getOneBodyForce(const State& state, const MobilizedBody& mobod) const {
const Vector_<SpatialVec>& bodyForces = getImpl().getAllBodyForces(state);
if (bodyForces.size() == 0) {return SpatialVec(Vec3(0),Vec3(0));}
return bodyForces[mobod.getMobilizedBodyIndex()];
}
void Force::TwoPointConstantForceImpl::calcForce(const State& state, Vector_<SpatialVec>& bodyForces, Vector_<Vec3>& particleForces, Vector& mobilityForces) const {
const Transform& X_GB1 = matter.getMobilizedBody(body1).getBodyTransform(state);
const Transform& X_GB2 = matter.getMobilizedBody(body2).getBodyTransform(state);
const Vec3 s1_G = X_GB1.R() * station1;
const Vec3 s2_G = X_GB2.R() * station2;
const Vec3 p1_G = X_GB1.p() + s1_G; // station measured from ground origin
const Vec3 p2_G = X_GB2.p() + s2_G;
const Vec3 r_G = p2_G - p1_G; // vector from point1 to point2
const Real x = r_G.norm(); // distance between the points
const UnitVec3 d(r_G/x, true);
const Vec3 f2_G = force * d;
bodyForces[body1] -= SpatialVec(s1_G % f2_G, f2_G);
bodyForces[body2] += SpatialVec(s2_G % f2_G, f2_G);
}
//------------------------- ENSURE FORCE CACHE VALID ---------------------------
// This will also calculate potential energy since we can do it on the cheap
// simultaneously with the force. Note that if the strength of gravity was set
// to zero then we already zeroed out the forces and pe during realizeInstance()
// so all we have to do in that case is mark the cache valid now. Also, any
// immune bodies have their force set to zero in realizeInstance() so we don't
// have to do it again here.
void Force::GravityImpl::
ensureForceCacheValid(const State& state) const {
if (isForceCacheValid(state)) return;
SimTK_STAGECHECK_GE_ALWAYS(state.getSystemStage(), Stage::Position,
"Force::GravityImpl::ensureForceCacheValid()");
const Parameters& p = getParameters(state);
if (p.g == 0) { // no gravity
markForceCacheValid(state); // zeroes must have been precalculated
return;
}
// Gravity is non-zero and gravity forces are not up to date, so this counts
// as an evaluation.
++numEvaluations;
const Vec3 gravity = p.g * p.d;
const Real zeroPEOffset = p.g * p.z;
ForceCache& fc = updForceCache(state);
fc.pe = 0;
const int nb = matter.getNumBodies();
// Skip Ground since we know it is immune.
for (MobilizedBodyIndex mbx(1); mbx < nb; ++mbx) {
if (p.mobodIsImmune[mbx])
continue; // don't apply gravity to this body; F already zero
const MobilizedBody& mobod = matter.getMobilizedBody(mbx);
const MassProperties& mprops = mobod.getBodyMassProperties(state);
const Transform& X_GB = mobod.getBodyTransform(state);
Real m = mprops.getMass();
const Vec3& p_CB = mprops.getMassCenter(); // in B
const Vec3 p_CB_G = X_GB.R()*p_CB; // exp. in G; 15 flops
const Vec3 p_G_CB = X_GB.p() + p_CB_G; // meas. in G; 3 flops
const Vec3 F_CB_G = m*gravity; // force at mass center; 3 flops
fc.F_GB[mbx] = SpatialVec(p_CB_G % F_CB_G, F_CB_G); // body frc; 9 flops
// odd signs here because height is in -gravity direction.
fc.pe -= m*(~gravity*p_G_CB + zeroPEOffset); // 8 flops
}
const int np = matter.getNumParticles();
if (np) {
const Vector& m = matter.getAllParticleMasses(state);
const Vector_<Vec3>& p_GP = matter.getAllParticleLocations(state);
for (ParticleIndex px(0); px < np; ++px) {
fc.f_GP[px] = m[px] * gravity; // 3 flops
fc.pe -= m[px]*(~gravity*p_GP[px] + zeroPEOffset); // 8 flops
}
}
markForceCacheValid(state);
}
void Force::TwoPointLinearSpringImpl::calcForce(const State& state, Vector_<SpatialVec>& bodyForces, Vector_<Vec3>& particleForces, Vector& mobilityForces) const {
const Transform& X_GB1 = matter.getMobilizedBody(body1).getBodyTransform(state);
const Transform& X_GB2 = matter.getMobilizedBody(body2).getBodyTransform(state);
const Vec3 s1_G = X_GB1.R() * station1;
const Vec3 s2_G = X_GB2.R() * station2;
const Vec3 p1_G = X_GB1.p() + s1_G; // station measured from ground origin
const Vec3 p2_G = X_GB2.p() + s2_G;
const Vec3 r_G = p2_G - p1_G; // vector from point1 to point2
const Real d = r_G.norm(); // distance between the points
const Real stretch = d - x0; // + -> tension, - -> compression
const Real frcScalar = k*stretch; // k(x-x0)
const Vec3 f1_G = (frcScalar/d) * r_G;
bodyForces[body1] += SpatialVec(s1_G % f1_G, f1_G);
bodyForces[body2] -= SpatialVec(s2_G % f1_G, f1_G);
}
/**
* An implementation of the MomentArmSolver
*
*/
MomentArmSolver::MomentArmSolver(const Model &model) : Solver(model)
{
setAuthors("Ajay Seth");
_stateCopy = model.getWorkingState();
// Get the body forces equivalent of the point forces of the path
_bodyForces = Vector_<SpatialVec>(getModel().getNumBodies(), SpatialVec(0));
// get the right size coupling vector
_coupling = _stateCopy.getU();
}
void Force::TwoPointLinearDamperImpl::calcForce(const State& state, Vector_<SpatialVec>& bodyForces, Vector_<Vec3>& particleForces, Vector& mobilityForces) const {
const Transform& X_GB1 = matter.getMobilizedBody(body1).getBodyTransform(state);
const Transform& X_GB2 = matter.getMobilizedBody(body2).getBodyTransform(state);
const Vec3 s1_G = X_GB1.R() * station1;
const Vec3 s2_G = X_GB2.R() * station2;
const Vec3 p1_G = X_GB1.p() + s1_G; // station measured from ground origin
const Vec3 p2_G = X_GB2.p() + s2_G;
const Vec3 v1_G = matter.getMobilizedBody(body1).findStationVelocityInGround(state, station1);
const Vec3 v2_G = matter.getMobilizedBody(body2).findStationVelocityInGround(state, station2);
const Vec3 vRel = v2_G - v1_G; // relative velocity
const UnitVec3 d(p2_G - p1_G); // direction from point1 to point2
const Real frc = damping*dot(vRel, d); // c*v
const Vec3 f1_G = frc*d;
bodyForces[body1] += SpatialVec(s1_G % f1_G, f1_G);
bodyForces[body2] -= SpatialVec(s2_G % f1_G, f1_G);
}
//----------------------------- REALIZE TOPOLOGY -------------------------------
// Allocate the state variables and cache entries. The force cache is a lazy-
// evaluation entry - although it can be calculated any time after
// Stage::Position, it won't be unless someone asks for it. And if it is
// evaluated early, it should not be re-evaluated when used as a force during
// Stage::Dynamics (via the calcForce() call).
//
// In addition, the force cache has a dependency on the parameter values that
// are stored in a discrete state variable. Changes to that variable
// automatically invalidate Dynamics stage, but must be carefully managed also
// to invalidate the force cache here since it is only Position-dependent.
void Force::GravityImpl::
realizeTopology(State& s) const {
GravityImpl* mThis = const_cast<GravityImpl*>(this);
const int nb=matter.getNumBodies(), np=matter.getNumParticles();
// In case more mobilized bodies were added after this Gravity element
// was constructed, make room for the rest now. Earlier default immunity
// settings are preserved.
if (defMobodIsImmune.size() != nb)
mThis->defMobodIsImmune.resize(nb, false);
// Allocate a discrete state variable to hold parameters; see above comment.
const Parameters p(defDirection,defMagnitude,defZeroHeight,
defMobodIsImmune); // initial value
mThis->parametersIx = getForceSubsystem()
.allocateDiscreteVariable(s, Stage::Dynamics, new Value<Parameters>(p));
// Don't allocate force cache space yet since we would have to copy it.
// Caution -- dependence on Parameters requires manual invalidation.
mThis->forceCacheIx = getForceSubsystem().allocateLazyCacheEntry(s,
Stage::Position, new Value<ForceCache>());
// Now allocate the appropriate amount of space, and set to zero now
// any forces that we know will end up zero so we don't have to calculate
// them at run time. Precalculated zeroes must be provided for any
// immune elements, or for all elements if g==0, and this must be kept
// up to date if there are runtime changes to the parameters.
ForceCache& fc = updForceCache(s);
if (defMagnitude == 0) {
fc.allocate(nb, np, true); // initially zero since no gravity
} else {
fc.allocate(nb, np, false); // initially NaN except for Ground
for (MobilizedBodyIndex mbx(1); mbx < nb; ++mbx)
if (defMobodIsImmune[mbx])
fc.F_GB[mbx] = SpatialVec(Vec3(0),Vec3(0));
// This doesn't mean the ForceCache is valid yet.
}
}
void Force::UniformGravityImpl::calcForce(const State& state, Vector_<SpatialVec>& bodyForces, Vector_<Vec3>& particleForces, Vector& mobilityForces) const {
const int nBodies = matter.getNumBodies();
const int nParticles = matter.getNumParticles();
if (nParticles) {
const Vector& m = matter.getAllParticleMasses(state);
const Vector_<Vec3>& loc_G = matter.getAllParticleLocations(state);
for (int i=0; i < nParticles; ++i) {
particleForces[i] += g * m[i];
}
}
// no need to apply gravity to Ground!
for (MobilizedBodyIndex i(1); i < nBodies; ++i) {
const MassProperties& mprops = matter.getMobilizedBody(i).getBodyMassProperties(state);
const Real& m = mprops.getMass();
const Vec3& com_B = mprops.getMassCenter();
const Transform& X_GB = matter.getMobilizedBody(i).getBodyTransform(state);
const Vec3 com_B_G = X_GB.R()*com_B;
const Vec3 frc_G = m*g;
bodyForces[i] += SpatialVec(com_B_G % frc_G, frc_G);
}
}
int HuntCrossleyContactRep::realizeSubsystemDynamicsImpl(const State& s) const
{
const Parameters& p = getParameters(s);
if (!p.enabled) return 0;
const MultibodySystem& mbs = getMultibodySystem(); // my owner
const SimbodyMatterSubsystem& matter = mbs.getMatterSubsystem();
Real& pe = Value<Real>::updDowncast(s.updCacheEntry(getMySubsystemIndex(), energyCacheIndex)).upd();
pe = 0;
// Get access to system-global cache entries.
Vector_<SpatialVec>& rigidBodyForces = mbs.updRigidBodyForces(s, Stage::Dynamics);
for (int s1=0; s1 < (int)p.spheres.size(); ++s1) {
const SphereParameters& sphere1 = p.spheres[s1];
const Transform& X_GB1 = matter.getMobilizedBody(sphere1.body).getBodyTransform(s);
const SpatialVec& V_GB1 = matter.getMobilizedBody(sphere1.body).getBodyVelocity(s); // in G
const Real r1 = sphere1.radius;
const Vec3 center1_G = X_GB1*sphere1.center;
for (int s2=s1+1; s2 < (int)p.spheres.size(); ++s2) {
const SphereParameters& sphere2 = p.spheres[s2];
if (sphere2.body == sphere1.body) continue;
const Transform& X_GB2 = matter.getMobilizedBody(sphere2.body).getBodyTransform(s);
const SpatialVec& V_GB2 = matter.getMobilizedBody(sphere2.body).getBodyVelocity(s);
const Real r2 = sphere2.radius;
const Vec3 center2_G = X_GB2*sphere2.center; // 18 flops
const Vec3 c2c1_G = center1_G - center2_G; // points at sphere1 (3 flops)
const Real dsq = c2c1_G.normSqr(); // 5 flops
if (dsq >= (r1+r2)*(r1+r2)) continue; // 4 flops
// There is a collision
const Real d = std::sqrt(dsq); // distance between the centers (~30 flops)
const UnitVec3 normal_G(c2c1_G/d, true); // direction from c2 center to c1 center (~12 flops)
processContact((r1*r2)/(r1+r2), // relative curvature, ~12 flops
sphere1.stiffness, sphere1.dissipation, X_GB1, V_GB1,
sphere2.stiffness, sphere2.dissipation, X_GB2, V_GB2,
center1_G - r1 * normal_G, // undeformed contact point for sphere1 (6 flops)
center2_G + r2 * normal_G, // undeformed contact point for sphere2 (6 flops)
normal_G,
pe, rigidBodyForces[sphere1.body], rigidBodyForces[sphere2.body]);
}
// half spaces
for (int h=0; h < (int)p.halfSpaces.size(); ++h) {
const HalfspaceParameters& halfSpace = p.halfSpaces[h];
if (halfSpace.body == sphere1.body) continue;
// Quick escape in the common case where the half space is on ground (7 flops)
if (halfSpace.body==0) {
const UnitVec3& normal_G = halfSpace.normal;
const Real hc1_G = ~center1_G*normal_G; // ht of center over ground
if (hc1_G - halfSpace.height < r1) {
// Collision of sphere1 with a half space on ground.
processContact(r1, // relative curvature is just r1
sphere1.stiffness, sphere1.dissipation, X_GB1, V_GB1,
halfSpace.stiffness, halfSpace.dissipation, Transform(), SpatialVec(Vec3(0)),
center1_G - (r1*normal_G), // undeformed contact point for sphere (6 flops)
halfSpace.height*normal_G, // " for halfSpace (3 flops)
normal_G, pe, rigidBodyForces[sphere1.body], rigidBodyForces[halfSpace.body]);
}
continue;
}
// Half space is not on ground.
const Transform& X_GB2 = matter.getMobilizedBody(halfSpace.body).getBodyTransform(s);
const SpatialVec& V_GB2 = matter.getMobilizedBody(halfSpace.body).getBodyVelocity(s);
const UnitVec3 normal_G = X_GB2.R()*halfSpace.normal; // 15 flops
// Find the heights of the half space surface and sphere center measured
// along the contact normal from the ground origin. Then we can get the
// height of the sphere center over the half space surface.
const Real h_G = ~X_GB2.p()*normal_G + halfSpace.height; // 6 flops
const Real hc1_G = ~center1_G*normal_G; // 5 flops
const Real d = hc1_G - h_G; // 1 flop
if (d >= r1) continue; // 1 flop
// There is a collision
processContact(r1, // relative curvature is just r1
sphere1.stiffness, sphere1.dissipation, X_GB1, V_GB1,
halfSpace.stiffness, halfSpace.dissipation, X_GB2, V_GB2,
center1_G - r1 * normal_G, // undeformed contact point for sphere (6 flops)
center1_G - d * normal_G, // undeformed contact point for halfSpace (6 flops)
normal_G,
pe, rigidBodyForces[sphere1.body], rigidBodyForces[halfSpace.body]);
}
}
return 0;
}
int realizeSubsystemDynamicsImpl(const State& s) const override {
const MultibodySystem& mbs = getMultibodySystem();
const SimbodyMatterSubsystem& matter = mbs.getMatterSubsystem();
// Get access to the discrete state variable that records whether each
// force element is enabled.
const Array_<bool>& forceEnabled = Value< Array_<bool> >::downcast
(getDiscreteVariable(s, forceEnabledIndex));
const Array_<Force*>& enabledNonParallelForces = Value<Array_<Force*>>::
downcast(getCacheEntry(s, enabledNonParallelForcesIndex));
const Array_<Force*>& enabledParallelForces = Value<Array_<Force*>>::
downcast(getCacheEntry(s, enabledParallelForcesIndex));
// Get access to System-global force cache arrays.
Vector_<SpatialVec>& rigidBodyForces =
mbs.updRigidBodyForces(s, Stage::Dynamics);
Vector_<Vec3>& particleForces =
mbs.updParticleForces (s, Stage::Dynamics);
Vector& mobilityForces =
mbs.updMobilityForces (s, Stage::Dynamics);
// Short circuit if we're not doing any caching here. Note that we're
// checking whether the *index* is valid (i.e. does the cache entry
// exist?), not the contents.
if (!cachedForcesAreValidCacheIndex.isValid()) {
// Call calcForce() on all Forces, in parallel.
calcForcesTask->initializeAll(s,
enabledNonParallelForces, enabledParallelForces,
rigidBodyForces, particleForces, mobilityForces);
calcForcesExecutor->execute(calcForcesTask.updRef(),
enabledParallelForces.size() + NumNonParallelThreads);
// Allow forces to do their own realization, but wait until all
// forces have executed calcForce(). TODO: not sure if that is
// necessary (sherm 20130716).
for (int i = 0; i < (int)forces.size(); ++i)
if (forceEnabled[i])
forces[i]->getImpl().realizeDynamics(s);
return 0;
}
// OK, we're doing some caching. This is a little messier.
// Get access to subsystem force cache entries.
bool& cachedForcesAreValid = Value<bool>::updDowncast
(updCacheEntry(s, cachedForcesAreValidCacheIndex));
Vector_<SpatialVec>&
rigidBodyForceCache = Value<Vector_<SpatialVec> >::updDowncast
(updCacheEntry(s, rigidBodyForceCacheIndex));
Vector_<Vec3>&
particleForceCache = Value<Vector_<Vec3> >::updDowncast
(updCacheEntry(s, particleForceCacheIndex));
Vector&
mobilityForceCache = Value<Vector>::updDowncast
(updCacheEntry(s, mobilityForceCacheIndex));
if (!cachedForcesAreValid) {
// We need to calculate the velocity independent forces.
rigidBodyForceCache.resize(matter.getNumBodies());
rigidBodyForceCache = SpatialVec(Vec3(0), Vec3(0));
particleForceCache.resize(matter.getNumParticles());
particleForceCache = Vec3(0);
mobilityForceCache.resize(matter.getNumMobilities());
mobilityForceCache = 0;
// Run through all the forces, accumulating directly into the
// force arrays or indirectly into the cache as appropriate.
calcForcesTask->initializeCachedAndNonCached(s,
enabledNonParallelForces, enabledParallelForces,
rigidBodyForces, particleForces, mobilityForces,
rigidBodyForceCache, particleForceCache,
mobilityForceCache);
calcForcesExecutor->execute(calcForcesTask.updRef(),
enabledParallelForces.size() + NumNonParallelThreads);
cachedForcesAreValid = true;
} else {
// Cache already valid; just need to do the non-cached ones (the
// ones for which dependsOnlyOnPositions is false).
calcForcesTask->initializeNonCached(s,
enabledNonParallelForces, enabledParallelForces,
rigidBodyForces, particleForces, mobilityForces);
calcForcesExecutor->execute(calcForcesTask.updRef(),
enabledParallelForces.size() + NumNonParallelThreads);
}
// Accumulate the values from the cache into the global arrays.
rigidBodyForces += rigidBodyForceCache;
particleForces += particleForceCache;
mobilityForces += mobilityForceCache;
// Allow forces to do their own Dynamics-stage realization. Note that
// this *follows* all the calcForce() calls.
for (int i = 0; i < (int) forces.size(); ++i)
if (forceEnabled[i]) forces[i]->getImpl().realizeDynamics(s);
return 0;
}
void setToNaN()
{ F_GB.setToNaN(); F_GB[0]=SpatialVec(Vec3(0)); // Ground
f_GP.setToNaN(); pe=NaN;}
void Force::ConstantForceImpl::calcForce(const State& state, Vector_<SpatialVec>& bodyForces, Vector_<Vec3>& particleForces, Vector& mobilityForces) const {
const Transform& X_GB = matter.getMobilizedBody(body).getBodyTransform(state);
const Vec3 station_G = X_GB.R() * station;
bodyForces[body] += SpatialVec(station_G % force, force);
}
