Skip to content

Commit

Permalink
Merge pull request #3903 from opensim-org/refactor_GeometryPathProduc…
Browse files Browse the repository at this point in the history 
…eForces

Make `GeometryPath::produceForces` pre-add its point forces
  • Loading branch information
@adamkewley
adamkewley authored Sep 12, 2024
2 parents 7c2c9de + 830af43 commit bd17674
Showing 1 changed file with 59 additions and 89 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

0 comments on commit bd17674

Please sign in to comment.