Skip to content

Commit

Permalink
Merge branch 'silence_checked_array_iterator_warning' into moco_fatrop
Browse files Browse the repository at this point in the history
  • Loading branch information
@nickbianco
nickbianco committed Sep 17, 2024
2 parents 8526ebb + 597b30e commit 93d0060
Show file tree
Hide file tree
Showing 2 changed files with 69 additions and 91 deletions.
148 changes: 59 additions & 89 deletions OpenSim/Simulation/Model/GeometryPath.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -129,6 +129,24 @@ static void PopulatePathElementLookup(
}
}

// Returns a normalized direction vector from `a` to `b` in ground, or a vector containing `NaN`s
// if `a` and `b` are at the same location.
//
// Two points can be conincient due to infeasible wrapping of the path. E.g. when the origin or
// insertion enters the wrapping surface. This is a temporary fix, since the wrap algorithm
// should return NaN for the points and/or throw an Exception- aseth
static SimTK::Vec3 directionBetweenPointsInGroundOrNaNIfCoincident(const SimTK::State& s, const OpenSim::AbstractPathPoint& a, const OpenSim::AbstractPathPoint& b)
{
Vec3 dir = b.getLocationInGround(s) - a.getLocationInGround(s);
if (dir.norm() < SimTK::SignificantReal) {
dir = dir*SimTK::NaN;
}
else {
dir = dir.normalize();
}
return dir;
}

//=============================================================================
// CONSTRUCTOR(S) AND DESTRUCTOR
//=============================================================================
Expand Down Expand Up @@ -386,103 +404,55 @@ void GeometryPath::produceForces(const SimTK::State& s,
double tension,
ForceConsumer& forceConsumer) const
{
AbstractPathPoint* start = NULL;
AbstractPathPoint* end = NULL;
const SimTK::MobilizedBody* bo = NULL;
const SimTK::MobilizedBody* bf = NULL;
const Array<AbstractPathPoint*>& currentPath = getCurrentPath(s);
int np = currentPath.getSize();

const SimTK::SimbodyMatterSubsystem& matter =
getModel().getMatterSubsystem();

// start point, end point, direction, and force vectors in ground
Vec3 po(0), pf(0), dir(0), force(0);
// partial velocity of point in body expressed in ground
Vec3 dPodq_G(0), dPfdq_G(0);

// gen force (torque) due to moving point under tension
double fo, ff;

for (int i = 0; i < np-1; ++i) {
start = currentPath[i];
end = currentPath[i+1];

bo = &start->getParentFrame().getMobilizedBody();
bf = &end->getParentFrame().getMobilizedBody();

if (bo != bf) {
// Find the positions of start and end in the inertial frame.
po = start->getLocationInGround(s);
pf = end->getLocationInGround(s);

// Form a vector from start to end, in the inertial frame.
dir = (pf - po);

// Check that the two points are not coincident.
// This can happen due to infeasible wrapping of the path,
// when the origin or insertion enters the wrapping surface.
// This is a temporary fix, since the wrap algorithm should
// return NaN for the points and/or throw an Exception- aseth
if (dir.norm() < SimTK::SignificantReal){
dir = dir*SimTK::NaN;
}
else{
dir = dir.normalize();
}
// Retains the body index from the previous iteration of the main loop
// and the previous direction between the previous point and the current
// one (if applicable). This is used to ensure that only one point force
// is produced per path point (#3903, #3891).
MobilizedBodyIndex previousBodyIndex = MobilizedBodyIndex::Invalid();
SimTK::Vec3 previousDirection(0.0);

force = tension*dir;

const MovingPathPoint* mppo =
dynamic_cast<MovingPathPoint *>(start);
const Array<AbstractPathPoint*>& currentPath = getCurrentPath(s);
for (int i = 0; i < currentPath.getSize(); ++i) {

// do the same for the end point of this segment of the path
const MovingPathPoint* mppf =
dynamic_cast<MovingPathPoint *>(end);
const AbstractPathPoint& currentPoint = *currentPath[i];
const MobilizedBodyIndex currentBodyIndex = currentPoint.getParentFrame().getMobilizedBodyIndex();

// add in the tension point forces to body forces
if (mppo) {// moving path point location is a function of the state
forceConsumer.consumePointForce(s, start->getParentFrame(), mppo->getLocation(s), force);
}
else {
forceConsumer.consumePointForce(s, start->getParentFrame(), start->getLocation(s), force);
}
SimTK::Vec3 force(0.0);
if (previousBodyIndex.isValid() && currentBodyIndex != previousBodyIndex) {
force -= tension * previousDirection;
}

if (mppf) {// moving path point location is a function of the state
forceConsumer.consumePointForce(s, end->getParentFrame(), mppf->getLocation(s), -force);
const AbstractPathPoint* nextPoint = i < currentPath.getSize()-1 ? currentPath[i+1] : nullptr;
if (nextPoint && nextPoint->getParentFrame().getMobilizedBodyIndex() != currentBodyIndex) {
const SimTK::Vec3 currentDirection = directionBetweenPointsInGroundOrNaNIfCoincident(s, currentPoint, *nextPoint);
const SimTK::Vec3 currentToNextForce = tension * currentDirection;
force += currentToNextForce;

// Additionally, account for the work done due to the movement of a `MovingPathPoint`
// relative to the body it is on.
if (const auto* movingCurrentPoint = dynamic_cast<const MovingPathPoint*>(&currentPoint)) {
const Vec3 dPodq_G = currentPoint.getParentFrame().expressVectorInGround(s, currentPoint.getdPointdQ(s));
const double fo = ~dPodq_G*currentToNextForce;
forceConsumer.consumeGeneralizedForce(s, movingCurrentPoint->getXCoordinate(), fo);
}
else {
// transform of the frame of the point to the base mobilized body
forceConsumer.consumePointForce(s, end->getParentFrame(), end->getLocation(s), -force);
if (const auto* nextMovingPoint = dynamic_cast<const MovingPathPoint*>(nextPoint)) {
const Vec3 dPfdq_G = nextPoint->getParentFrame().expressVectorInGround(s, nextPoint->getdPointdQ(s));
const double ff = ~dPfdq_G*(-currentToNextForce);
forceConsumer.consumeGeneralizedForce(s, nextMovingPoint->getXCoordinate(), ff);
}

// Now account for the work being done by virtue of the moving
// path point motion relative to the body it is on
if(mppo){
// torque (genforce) contribution due to relative movement
// of a via point w.r.t. the body it is connected to.
dPodq_G = bo->expressVectorInGroundFrame(s, start->getdPointdQ(s));
fo = ~dPodq_G*force;

// get the mobilized body the coordinate is couple to.
const SimTK::MobilizedBody& mpbod =
matter.getMobilizedBody(mppo->getXCoordinate().getBodyIndex());

// apply the generalized (mobility) force to the coordinate's body
forceConsumer.consumeGeneralizedForce(s, mppo->getXCoordinate(), fo);
}

if(mppf){
dPfdq_G = bf->expressVectorInGroundFrame(s, end->getdPointdQ(s));
ff = ~dPfdq_G*(-force);

// get the mobilized body the coordinate is couple to.
const SimTK::MobilizedBody& mpbod =
matter.getMobilizedBody(mppf->getXCoordinate().getBodyIndex());
previousBodyIndex = currentBodyIndex;
previousDirection = currentDirection;
}
else {
previousBodyIndex = MobilizedBodyIndex::Invalid();
previousDirection = SimTK::Vec3(0.0f);
}

forceConsumer.consumeGeneralizedForce(s, mppf->getXCoordinate(), ff);
}
}
// If applicable, produce the force
if (force != SimTK::Vec3(0.0)) {
forceConsumer.consumePointForce(s, currentPoint.getParentFrame(), currentPoint.getLocation(s), force);
}
}
}

Expand Down
12 changes: 10 additions & 2 deletions dependencies/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -105,7 +105,6 @@ function(AddDependency)
list(APPEND CMAKE_ARGS
-DCMAKE_BUILD_TYPE:STRING=${CMAKE_BUILD_TYPE})
endif()
list(APPEND CMAKE_ARGS ${DEP_CMAKE_ARGS})

# Forward cmake arguments to dependencies.
list(APPEND CMAKE_ARGS
Expand Down Expand Up @@ -140,6 +139,9 @@ function(AddDependency)
list(APPEND CMAKE_ARGS -DSWIG_EXECUTABLE:FILEPATH=${SWIG_EXECUTABLE})
endif()

# Append the dependency-specific CMake arguments.
list(APPEND CMAKE_ARGS ${DEP_CMAKE_ARGS})

if(DEP_GIT_URL)
ExternalProject_Add(${DEP_NAME}
DEPENDS ${DEP_DEPENDS}
Expand Down Expand Up @@ -193,6 +195,11 @@ AddDependency(NAME docopt
GIT_URL https://github.com/docopt/docopt.cpp.git
GIT_TAG 3dd23e3280f213bacefdf5fcb04857bf52e90917)

set(SPDLOG_CXX_FLAGS "${CMAKE_CXX_FLAGS}")
if(MSVC)
# `spdlog` transitively uses a deprecated `stdext::checked_array_iterator`
set(SPDLOG_CXX_FLAGS "${SPDLOG_CXX_FLAGS} /D_SILENCE_STDEXT_ARR_ITERS_DEPRECATION_WARNING")
endif()

AddDependency(NAME spdlog
DEFAULT ON
Expand All @@ -201,7 +208,8 @@ AddDependency(NAME spdlog
CMAKE_ARGS -DSPDLOG_BUILD_BENCH:BOOL=OFF
-DSPDLOG_BUILD_TESTS:BOOL=OFF
-DSPDLOG_BUILD_EXAMPLE:BOOL=OFF
-DCMAKE_POSITION_INDEPENDENT_CODE:BOOL=ON)
-DCMAKE_POSITION_INDEPENDENT_CODE:BOOL=ON
-DCMAKE_CXX_FLAGS:STRING=${SPDLOG_CXX_FLAGS})

AddDependency(NAME catch2
DEFAULT ON
Expand Down

0 comments on commit 93d0060

Please sign in to comment.