Clear via point and obstacle indexes in Scholz2015GeometryPath::extendAddToSystem()

[nickbianco]

Fixes issue #4239

Brief summary of changes

Updates Scholz2015GeometryPath::extendAddToSystem() to clear the indexes stored for via points and obstacles. While Model::createMultibodySystem() clears the underlying Simbody system before calling addToSystem(), any member variables (i.e., via points and obstacle indexes) that are not part of the Simbody system will not be cleared. This change fixes that inconsistency.

As an additional "safety" measure, I've also updated the Simbody commit to include changes that call CableSpan::Impl::realizePosition() inside calls to CableSpan::calcOriginUnitForce(), CableSpan::calcCurveSegmentUnitForce(), etc., to ensure that the path is always valid.

Testing I've completed

Ran testScholz2015GeometryPath.cpp.

Looking for feedback on...

CHANGELOG.md (choose one)

• no need to update because...bug fix prior to main release.

---

This change is 

[nickbianco]

This is WIP, since I am not proposing this as a final solution, but rather a working fix for the sake of discussion.

@adamkewley I assume that the (probably temporary) addition of ForceConsumer::consumeBodyForces() would not be very useful for visualization? It essentially implements AbstractGeometryPath::addInEquivalentForces() in a roundabout way. Investigating a better solution that also supports visualization.

[adamkewley]

It would be incredibly not-useful and counter to the point of that API, yes.

If you just want to play around with raw vectors the old-fashioned way then I'd recommend using OpenSim::Force, rather than drilling holes into the more-specialized ForceProducer/ForceConsumer API. The way you've done it here involves allocating a potentially-large zeroed Vector temporary that is thrown away after probably only applying a few forces. The ForceProducer/ForceConsumer APIs were specifically designed to avoid requiring this (and to hold onto point-based information).

If visualization of which forces are actually applied by an OpenSim::Force is necessary then it requires doing the work you're doing downstream here: allocate a zeroed vector, pass it into an OpenSim::Force, inspect it, re-link the simbody body indices to the affected OpenSim::Bodys with a lookup. It's expensive---exactly why I designed a consumer API---but it does work.

[adamkewley]

I had a quick look at why the behavior might be different

The realizePosition might be significant in CableSpan's case, because it's used to check a m_indexDataPos cache variable, which then ends up calling const CableSpanData::Position& CableSpan::Impl::calcDataPos(State& s) const, which does a bunch of calculations for the cache variable update (enjoying logically const yet? 😉 ).

EDIT: additionally, it looks like it'll be a bit of faffing around if you want to use ForceConsumer in that part of the API, because even if you implement a sufficient callback into simbody, it can only emit body indices, which then need to go through a body-index-to-frame lookup on the caller's side in order to correctly tag it with the correct body frame for the ForceConsumer API. This is unfortunately the case with many simbody forces, and the solution is either to fit a body-index-to-body lookup at the model-level, or redesign the ForceConsumer API to accept body indices and do it itself (sub-optimal, because you'd only need one lookup but N implementations might have to replicate the effort).

[nickbianco]

If you just want to play around with raw vectors the old-fashioned way then I'd recommend using OpenSim::Force, rather than drilling holes into the more-specialized ForceProducer/ForceConsumer API.

The somewhat tricky part is that this is not an OpenSim::Force, but an AbstractGeometryPath implementation, which interacts with ForceConsumer via the pure virtual produceForces(). So, one option would be to change the AbstractGeometryPath interface, but I think the better solution is related to your next comments...

The realizePosition might be significant in CableSpan's case, because it's used to check a m_indexDataPos cache variable, which then ends up calling const CableSpanData::Position& CableSpan::Impl::calcDataPos(State& s) const, which does a bunch of calculations for the cache variable update (enjoying logically const yet? 😉 ).

"I swear bro, I won't change anything." Joking aside, I think this could be solved by adding a public realizePosition() to CableSpan. I had tried to add a Model::realizePosition() call inside produceForces(), but I now realize that will not necessarily trigger CableSpan::Impl::realizePosition() internally.

the solution is either to fit a body-index-to-body lookup at the model-level

If you look at the diff, I already had this solution in place. I just need to add a CableSpan::realizePosition() call in there, after adding it to Simbody, and (I think) we're good.

[nickbianco]

Update: the strange results I reported in #4239 may be somehow related to Python via C++ differences. Those results were generated via a Python script related to the model building project I'm working on. For debugging, I replicated the same simulation approach (initial conditions, integrator settings, etc) in C++ and loaded in the same model and, without changing anything, the model free falls as expected (no funny flipping motion). Working on trying to isolate the true root cause.

[nickbianco]

@adamkewley I found the true issue. We need to clear the via point and obstacle indices inside Scholz2015GeometryPath::extendAddToSystem() so multiple calls to said method will not add the same indices multiple times. (When Model calls addToSystem() inside createMultibodySystem(), the stored Simbody system is cleared, but the via point and obstacle indices will live on).

I'm thinking I still might submit a PR to add CableSpan::realizePosition() calls inside CableSpan::calcOriginUnitForce(), CableSpan::calcCurveSegmentUnitForce(), etc., to make sure the path cache is up-to-date, since that is what other convenience methods do (e.g., CableSpan::applyBodyForces()).

[nickbianco]

Leaving this as WIP until I update the CableSpan methods in Simbody.

[nickbianco]

This is ready for review. I've updated the Simbody commit to include changes that call CableSpan::Impl::realizePosition() inside calls to CableSpan::calcOriginUnitForce(), CableSpan::calcCurveSegmentUnitForce(), etc., to ensure that the path is always valid.

[adamkewley]

Looks good to me - good work @nickbianco for looking into the root cause, patching it upstream, etc.!

[nickbianco]

Thanks @adamkewley!