0
\$\begingroup\$

I have a theodolite-like device that is trying to track a point such that it's "Forward" is always facing that point.

Here is a GIF of it working as of now.

As expected, when the value of the Red Angle is near 90 or 270 it may approach a gimbal-lock situation.

How would I be able to calculate a position for the currently unused Blue axis to prevent the Red/Green axis's from aligning that way?

Main Loop:

    public void Main(string argument, UpdateType updateSource)
    {
        float pitch, yaw;
        GetOrientationTo(target, origin, out pitch, out yaw);

        UpdateSteeringVectors();
        // Update yaw to the Green rotor
        UpdateRotor(r_green_inner, yaw);
        // Update pitch to the Red rotor
        UpdateRotor(r_red_middle, pitch);

        float green = GetRotorAngle(r_green_inner);
        if (green == 0) green = 360;
        float red = GetRotorAngle(r_red_middle);
        if (red == 0) red = 360;
        float blue = GetRotorAngle(r_blue_outer);
        if (blue == 0) blue = 360;

        debugText = "Green Angle: " + green.ToString() + "\n";
        debugText += "Red Angle: " + red.ToString() + "\n";
        debugText += "Blue Angle: " + blue.ToString() + "\n";
        debug.WritePublicText(debugText);
        debugText = "";
    }

Tracking function:

    void GetOrientationTo(Vector3 Target, IMyTerminalBlock Origin, out float Pitch, out float Yaw)
    {

        // Origin point
        Vector3 OV = Origin.GetPosition();
        // Thrusters are two blocks long so we need to make it shorter
        OV = Vector3.Subtract(OV, Origin.WorldMatrix.Forward * 1.25);
        // Target point
        Vector3 TV = Vector3.Subtract(OV, Target);
        // Get reference directional vectors
        Vector3 FV = OV + Origin.WorldMatrix.Forward;
        Vector3 UV = OV + Origin.WorldMatrix.Up;
        Vector3 RV = OV + Origin.WorldMatrix.Right;

        double TVOV = (OV - TV).Length();  // Get magnitudes of vectors

        double TVFV = (FV - TV).Length();
        double TVUV = (UV - TV).Length();
        double TVRV = (RV - TV).Length();

        double OVFV = (FV - OV).Length();
        double OVUV = (UV - OV).Length();
        double OVRV = (RV - OV).Length();

        double ThetaP = Math.Acos((TVUV * TVUV - OVUV * OVUV - TVOV * TVOV) / (-2 * OVUV * TVOV));  //Use law of cosines to determine angles.
        double ThetaY = Math.Acos((TVRV * TVRV - OVRV * OVRV - TVOV * TVOV) / (-2 * OVRV * TVOV));

        double RPitch = 90 - (ThetaP * 180 / Math.PI);  //Convert from radians to degrees.
        double RYaw = 90 - (ThetaY * 180 / Math.PI);

        if (TVOV < TVFV) RPitch = 180 - RPitch;  //Normalize angles to -180 to 180 degrees.
        if (RPitch > 180) RPitch = -1 * (360 - RPitch);

        if (TVOV < TVFV) RYaw = 180 - RYaw;
        if (RYaw > 180) RYaw = -1 * (360 - RYaw);

        Pitch = (float)RPitch;
        Yaw = (float)RYaw;
    }
\$\endgroup\$
1
\$\begingroup\$

My first thought is that we could solve this by gradient descent.

Assuming we did a good job aligning our pointer to the target & minimizing gimbal lock last frame, then this frame we should be very close to a good solution for this frame too.

So, we form a scoring function that prioritizes pointing at the target first, and avoiding problematic alignment between our axes second, eg...

Score = 10 * Dot(TargetDirection, PointerDirection) 
      - Dot(MiddleGimbalAxis, PointerDirection)^2

Then we take gradient of that scoring function as we vary each of the three angles, giving us an incremental change vector we can add to our angles to home-in on a local optimum.

As I was working through this function though, I spotted a simpler method:

The gimbal lock case occurs when we're trying to point parallel to the middle gimbal's rotation axis. If we keep this axis far from the target direction (perpendicular, if we can), then we stay well clear of the danger zone.

So, we can solve the problem like so:

  1. Project the target direction into the plane of rotation of the outer gimbal.

  2. Rotate the outer gimbal so its arm stays perpendicular to this planar direction.

    (Note that there are two possible perpendicular orientations - pick whichever one is closer, to avoid unnecessary spinning when the direction crosses the pole)

  3. Transform the target direction into the outer gimbal's local coordinate space

  4. Solve the remaining two gimbal axes the same as you did before, with this new transformed target.

As long as your outer gimbal can turn as fast as your target direction does, this will keep the inner transformed target away from the poles of your inner two-gimbal system so it remains well-behaved.

Example animation showing 3-gimbal system adapting to follow a moving target

There's still a singularity in this scheme: when the target is directly below / directly overhead, the outer gimbal's rotation is no longer well-defined, and you may see it hesitate or spin here. But in this orientation the inner two gimbals have fully orthogonal degrees of freedom, so we can continue tracking the target without a lock or lurch.

\$\endgroup\$
2
  • \$\begingroup\$ Great practical solutions! I figured that 'nudging' the Green Axis around similar to your gradient decent was the easiest solution to use at first. Is the final solution still not solved mathematically, as in the Red and Blue are experiencing the effects of the Green Axis, but they do not predict it? \$\endgroup\$
    – falordphil
    Aug 10 '18 at 20:40
  • \$\begingroup\$ In the animated solution I solve the three sequentially. First I rotate the outer gimbal to point perpendicular to the target. Then I rotate the middle gimbal based on the outer's rotation, then rotate the inner gimbal based on the outer two. I'm not doing any prediction of future movement, but with a reasonably fast turning speed moving reactively seems to be enough. \$\endgroup\$
    – DMGregory
    Aug 10 '18 at 20:45

You must log in to answer this question.

Not the answer you're looking for? Browse other questions tagged .