void
Contact::output(const TaskInfo& taskInfo)
{
if (nonZeroIntersection(taskInfo.getSampleTimeSet(),
SampleTime::PerTimestep)) {
Log(Model, Debug) << "Contact::output(): \"" << getName()
<< "\" computing ground plane below" << endl;
getGround(taskInfo.getTime());
}
// Transform the plane equation to the local frame.
Plane lp = mMountFrame->planeFromRef(mGroundVal.plane);
// Get the intersection length.
real_type compressLength = lp.getDist(Vector3::zeros());
// Don't bother if we do not intersect the ground.
if (compressLength < 0) {
setForce(Vector6::zeros());
return;
}
// The velocity of the ground patch in the current frame.
Vector6 groundVel(mMountFrame->rotFromRef(mGroundVal.vel.getAngular()),
mMountFrame->rotFromRef(mGroundVal.vel.getLinear()));
groundVel -= mMountFrame->getRefVel();
// Now get the relative velocity of the ground wrt the contact point
Vector3 relVel = - groundVel.getLinear();
// The velocity perpandicular to the plane.
// Positive when the contact spring is compressed,
// negative when decompressed.
real_type compressVel = - lp.scalarProjectToNormal(relVel);
// The in plane velocity.
Vector3 sVel = lp.projectToPlane(relVel);
// Get the plane normal force.
real_type normForce = computeNormalForce(compressLength, compressVel);
// The normal force cannot get negative here.
normForce = max(static_cast<real_type>(0), normForce);
// Get the friction force.
Vector3 fricForce = computeFrictionForce(normForce, sVel, lp.getNormal(),
mGroundVal.friction);
// The resulting force is the sum of both.
// The minus sign is because of the direction of the surface normal.
Vector3 force = fricForce - normForce*lp.getNormal();
// We don't have an angular moment.
setForce(Vector6(Vector3::zeros(), force));
}
void
Launchbar::output(const TaskInfo& taskInfo)
{
if (nonZeroIntersection(taskInfo.getSampleTimeSet(),
SampleTime::PerTimestep)) {
Log(Model, Debug) << "Launchbar::output(): \"" << getName()
<< "\" computing ground plane below" << std::endl;
getGround(taskInfo.getTime());
}
// The launchbar angle
mAngle = computeCurrentLaunchbarAngle();
// Ok, apply the tension at the launchbar and have the holdback
if (mState != Mounted && mState != Launching) {
setForce(Vector6::zeros());
return;
}
// Query the catapult, write the result into the attached frame
// ... yes this function works through sideffects ... :-/
real_type catLen;
if (!computeCatFrame(taskInfo.getTime(), catLen)) {
setForce(Vector6::zeros());
return;
}
// The catapult direction vector
Vector3 catPos0 = mCatFrame->posToParent(Vector3::zeros());
Vector3 catDir = mCatFrame->rotToParent(Vector3::unit(0));
// The launchbar's tip position
Vector3 lbTip(cos(mAngle)*mLength, 0, -sin(mAngle)*mLength);
// The tension applied over the launchbar
// allways pulls the aircraft back to the cat ...
// project the launchbar tip to the catpult line ...
Vector3 lbTipOnCat = catPos0 + dot(lbTip - catPos0, catDir)*catDir;
Vector6 force = Vector6::zeros();
if (mState == Mounted) {
force.setLinear(mLaunchForce/5*normalize(lbTipOnCat));
} else {
force.setLinear(mLaunchForce*normalize(lbTipOnCat));
}
if (mState == Mounted) {
// the position of the holdback's deck mount
Vector3 hbDeckMount = mCatFrame->posToParent(mPosOnCat*Vector3::unit(0));
// ok, for now the holback is a stiff spring, will model that different
// when loop closure contranits are availble ...
Vector3 hbDir = mHoldbackMount - hbDeckMount;
real_type hbLen = norm(hbDir);
if (mHoldbackLength < hbLen) {
Vector3 hbForce = (2*mLaunchForce*(mHoldbackLength - hbLen)/hbLen)*hbDir;
force += forceFrom(mHoldbackMount, hbForce);
}
// Some damping force, just at the position the launchbar applies its force
force += mLaunchForce/2*mCatFrame->motionToParent(mCatFrame->getRelVel());
}
setForce(force);
}
void
Launchbar::update(const TaskInfo& taskInfo)
{
real_type unlagedAngleCommand = mUpAngle;
// Query the catapult, write the result into the attached frame
// ... yes this function works through sideffects ... :-/
real_type catLen;
if (!computeCatFrame(taskInfo.getTime(), catLen)) {
mState = Unmounted;
unlagedAngleCommand = mUpAngle;
} else {
// Ok, here we know that the catapult frame contains some sensible values
// The catapult direction vector
Vector3 catPos0 = mCatFrame->posToParent(Vector3::zeros());
Vector3 catDir = mCatFrame->rotToParent(Vector3::unit(0));
real_type angle = computeCurrentLaunchbarAngle();
// The launchbar's tip position
Vector3 lbTip(cos(angle)*mLength, 0, -sin(angle)*mLength);
// Compute the distance from the launchbar tip to the catapult
real_type dist = norm(lbTip-catPos0-dot(catDir, lbTip - catPos0)*catDir);
if (mState == Unmounted) {
bool tryMount = mTryMountPort.getRealValue();
if (tryMount) {
unlagedAngleCommand = mDownAngle;
// can only mount if we are near enough
if (dist < 0.1) {
// Now compute a reference position on the catapult line which
// will be used as the reference for the holdback
// compute the nearest point on the catapult line to the holdback
// mount
Vector3 hbNearest = catPos0
+ dot(mHoldbackMount - catPos0, catDir)*catDir;
// Find the distance backwards from that point matching
// the holdback length
real_type sqrCatx = mHoldbackLength*mHoldbackLength
- dot(hbNearest, hbNearest);
/// There is something wrong if the holdback mount is too far
if (0 <= sqrCatx) {
Vector3 hbDeckMount = hbNearest - sqrt(sqrCatx)*catDir;
// The reference position
mPosOnCat = dot(catDir, hbDeckMount - catPos0);
// if (mPosOnCat < 30)
// Ok, survived, mounted now ...
mState = Mounted;
}
}
} else
unlagedAngleCommand = mUpAngle;
} else if (mState == Mounted) {
if (mLaunchCommandPort.getRealValue())
mState = Launching;
if (dist > 1)
mState = Unmounted;
unlagedAngleCommand = mDownAngle;
} else {
// Launching
if (dist > 1)
mState = Unmounted;
Vector3 catPos1 = mCatFrame->posToParent(catLen*Vector3::unit(0));
if (dot(catPos1, catDir) < 0)
mState = Unmounted;
unlagedAngleCommand = mDownAngle;
}
}
real_type angleError = unlagedAngleCommand - mAngleCommand;
angleError = sign(angleError)*min(mAngularVelocity, 40*fabs(angleError));
/// FIXME: isPerTimestep sample times do not contain the step size ...
/// hardwire that ATM
mAngleCommand += 1/120.0*angleError;
}