/**
* Calculate the wrapping of one path segment over one wrap object.
*
* @param aPoint1 The first patth point
* @param aPoint2 The second path point
* @param aPathWrap An object holding the parameters for this path/wrap-object pairing
* @param aWrapResult The result of the wrapping (tangent points, etc.)
* @return The status, as a WrapAction enum
*/
int WrapObject::wrapPathSegment(const SimTK::State& s, PathPoint& aPoint1, PathPoint& aPoint2,
const PathWrap& aPathWrap, WrapResult& aWrapResult) const
{
int return_code = noWrap;
bool p_flag;
Vec3 pt1(0.0);
Vec3 pt2(0.0);
// Convert the path points from the frames of the bodies they are attached
// to to the frame of the wrap object's body
_model->getSimbodyEngine().transformPosition(s, aPoint1.getBody(), aPoint1.getLocation(), getBody(), pt1);
_model->getSimbodyEngine().transformPosition(s, aPoint2.getBody(), aPoint2.getLocation(), getBody(), pt2);
// Convert the path points from the frame of the wrap object's body
// into the frame of the wrap object
pt1 = _pose.shiftBaseStationToFrame(pt1);
pt2 = _pose.shiftBaseStationToFrame(pt2);
return_code = wrapLine(s, pt1, pt2, aPathWrap, aWrapResult, p_flag);
if (p_flag == true && return_code > 0) {
// Convert the tangent points from the frame of the wrap object to the
// frame of the wrap object's body
aWrapResult.r1 = _pose.shiftFrameStationToBase(aWrapResult.r1);
aWrapResult.r2 = _pose.shiftFrameStationToBase(aWrapResult.r2);
// Convert the surface points (between the tangent points) from the frame of
// the wrap object to the frame of the wrap object's body
for (int i = 0; i < aWrapResult.wrap_pts.getSize(); i++)
aWrapResult.wrap_pts.get(i) = _pose.shiftFrameStationToBase(aWrapResult.wrap_pts.get(i));
}
return return_code;
}
/*
* Add a new path point, with default location, to the path.
*
* @param aIndex The position in the pathPointSet to put the new point in.
* @param aBody The body to attach the point to.
* @return Pointer to the newly created path point.
*/
PathPoint* GeometryPath::
addPathPoint(const SimTK::State& s, int aIndex, PhysicalFrame& aBody)
{
PathPoint* newPoint = new PathPoint();
newPoint->setBody(aBody);
Vec3& location = newPoint->getLocation();
placeNewPathPoint(s, location, aIndex, aBody);
upd_PathPointSet().insert(aIndex, newPoint);
// Rename the path points starting at this new one.
namePathPoints(aIndex);
// Update start point and end point in the wrap instances so that they
// refer to the same path points they did before the new point
// was added. These indices are 1-based.
aIndex++;
for (int i=0; i<get_PathWrapSet().getSize(); i++) {
int startPoint = get_PathWrapSet().get(i).getStartPoint();
int endPoint = get_PathWrapSet().get(i).getEndPoint();
if (startPoint != -1 && aIndex <= startPoint)
get_PathWrapSet().get(i).setStartPoint(s,startPoint + 1);
if (endPoint != -1 && aIndex <= endPoint)
get_PathWrapSet().get(i).setEndPoint(s,endPoint + 1);
}
return newPoint;
}
/* add in the equivalent spatial forces on bodies for an applied tension
along the GeometryPath to a set of bodyForces */
void GeometryPath::addInEquivalentForces(const SimTK::State& s,
const double& tension,
SimTK::Vector_<SimTK::SpatialVec>& bodyForces,
SimTK::Vector& mobilityForces) const
{
PathPoint* start = NULL;
PathPoint* end = NULL;
const SimTK::MobilizedBody* bo = NULL;
const SimTK::MobilizedBody* bf = NULL;
const Array<PathPoint*>& 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->getBody().getMobilizedBody();
bf = &end->getBody().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();
}
force = tension*dir;
const MovingPathPoint* mppo =
dynamic_cast<MovingPathPoint *>(start);
// do the same for the end point of this segment of the path
const MovingPathPoint* mppf =
dynamic_cast<MovingPathPoint *>(end);
// add in the tension point forces to body forces
if (mppo) {// moving path point location is a function of the state
// transform of the frame of the point to the base mobilized body
auto X_BF = mppo->getParentFrame().findTransformInBaseFrame();
bo->applyForceToBodyPoint(s, X_BF*mppo->getLocation(s), force,
bodyForces);
}
else {
// transform of the frame of the point to the base mobilized body
auto X_BF = start->getParentFrame().findTransformInBaseFrame();
bo->applyForceToBodyPoint(s, X_BF*start->getLocation(), force,
bodyForces);
}
if (mppf) {// moving path point location is a function of the state
// transform of the frame of the point to the base mobilized body
auto X_BF = mppf->getParentFrame().findTransformInBaseFrame();
bf->applyForceToBodyPoint(s, X_BF*mppf->getLocation(s), -force,
bodyForces);
}
else {
// transform of the frame of the point to the base mobilized body
auto X_BF = end->getParentFrame().findTransformInBaseFrame();
bf->applyForceToBodyPoint(s, X_BF*end->getLocation(), -force,
bodyForces);
}
// 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
mpbod.applyOneMobilityForce(s,
mppo->getXCoordinate().getMobilizerQIndex(),
fo, mobilityForces);
}
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());
mpbod.applyOneMobilityForce(s,
mppf->getXCoordinate().getMobilizerQIndex(),
ff, mobilityForces);
}
}
}
}