/**
* 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;
}
// get the path as PointForceDirections directions
// CAUTION: the return points are heap allocated; you must delete them yourself!
// (TODO: that is really lame)
void GeometryPath::
getPointForceDirections(const SimTK::State& s,
OpenSim::Array<PointForceDirection*> *rPFDs) const
{
int i;
PathPoint* start;
PathPoint* end;
const OpenSim::PhysicalFrame* startBody;
const OpenSim::PhysicalFrame* endBody;
const Array<PathPoint*>& currentPath = getCurrentPath(s);
int np = currentPath.getSize();
rPFDs->ensureCapacity(np);
for (i = 0; i < np; i++) {
PointForceDirection *pfd =
new PointForceDirection(currentPath[i]->getLocation(),
currentPath[i]->getBody(), Vec3(0));
rPFDs->append(pfd);
}
for (i = 0; i < np-1; i++) {
start = currentPath[i];
end = currentPath[i+1];
startBody = &start->getBody();
endBody = &end->getBody();
if (startBody != endBody)
{
Vec3 posStart, posEnd;
Vec3 direction(0);
// Find the positions of start and end in the inertial frame.
//engine.getPosition(s, start->getBody(), start->getLocation(), posStart);
posStart = start->getLocationInGround(s);
//engine.getPosition(s, end->getBody(), end->getLocation(), posEnd);
posEnd = end->getLocationInGround(s);
// Form a vector from start to end, in the inertial frame.
direction = (posEnd - posStart);
// 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 (direction.norm() < SimTK::SignificantReal){
direction = direction*SimTK::NaN;
}
else{
direction = direction.normalize();
}
// Get resultant direction at each point
rPFDs->get(i)->addToDirection(direction);
rPFDs->get(i+1)->addToDirection(-direction);
}
}
}
/* 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);
}
}
}
}