Rotating a command relative to the body of a drone

Question

Suppose you have a drone the shape of a cube with rotors on each face, it's completely unable to control its roll-pitch-yaw, but it can accelerate along any of its 3D orthogonal vectors (see image below). However, there's also a gravitational force that accelerates the drone down in the -Z axis of the world frame.

Given the quaternion representing the rotation of the drone, how would you construct a command such that it eliminates the acceleration caused by gravity? This command is applied along the drone's orthogonal vectors in the form [x,y,z]. For the sake of this question let's say the vector that completely opposes gravity is given as [0,0,1] in the world frame.

Just to clarify what I mean by "command", if the drone was completely aligned with the world axis the command needed to counteract gravity would just be [0,0,1]. If the drone was rotated forward such that it's +Z-axis is aligned with the world's +X-axis and the drone's +X-axis is aligned with the world's -Z-axis (see image above) the command needed would be [-1,0,0].

For my approach, which obviously isn't working, I tried solving this by rotating the vector [0,0,1] by the quaternion with rotquat(vec(0,0,1,0),Q) in the following lua code:

vec = function(x,y,z,w) return {x=x,y=y,z=z,w=w} end

function mquat(Q,R)
    return vec(
    R.w*Q.x+R.x*Q.w-R.y*Q.z+R.z*Q.y,
    R.w*Q.y+R.x*Q.z+R.y*Q.w-R.z*Q.x,
    R.w*Q.z-R.x*Q.y+R.y*Q.x+R.z*Q.w,
    R.w*Q.w-R.x*Q.x-R.y*Q.y-R.z*Q.z)
end

function rotquat(V,Q)
  return mquat(mquat(Q,V),vec(-Q.x,-Q.y,-Q.z,Q.w))
end

This kind've works for the first few seconds, the commands are applied perfectly along the world +z-axis. However, as the drone begins to naturally rotate the commands slowly shift off the +z-axis and it zips off in the wrong direction.

Any ideas on what I should do to fix this?

Answer 1

I've adapted some of my code that helps me make objects face in a certain direction. If I didn't make any errors translating it, it should work.

var currentUp = drone.Up; // the current up vector for your drone
var desiredUp = ... // The up vector that you would like to have

var dot = Vector3.Dot(currentUp, desiredUp);
if (Math.Abs(dot + 1.0f) < 0.000001f)
{
    // vector a and b point exactly in the opposite direction, 
    // so it is a 180 degrees turn around the x-axis (or z-axis if you prefer)
    return new Quaternion(new Vector3(-1, 0, 0), PI);
}
if (Math.Abs(dot - 1.0f) < 0.000001f)
{
    // vector a and b point exactly in the same direction
    // so we return the identity quaternion
    return Quaternion.Identity;
}

// The cross product gives us a vector perpendicular to the two vectors
var axis = Vector3.Normalize(Vector3.Cross(currentForward, desiredForward));

// The dot product gives us the distance we need to rotate over that axis
var angle = Math.Acos(dot);

var rotation = Quaternion.CreateFromAxisAngle(rotAxis, rotAngle);

drone.Quaternion = rotation * drone.Quaternion;

In some cases this rotation leads to unwanted rotations over other axes. (For example a reorientation that you as a pilot would solve by yawing includes an unnecessary roll. If you want to you can use a different method to create a constrained rotation. Basically it rotates without going over the specified axis.

var currentUp = drone.Up; // the current up vector for your drone
var desiredUp = ... // The up vector that you would like to have
var locked = ... // A vector that should stay roughly the same during the rotation


var dot = Vector3.Dot(Vector3.Normalize(target - this.Position), locked);

if (Math.Abs(dot) < 0.99f)
{
    var matrix = Matrix4x4.CreateLookAt(drone.Position, target, locked);
    if (Matrix4x4.Invert(matrix, out var inverted))
    {
        var quaternion = Quaternion.CreateFromRotationMatrix(inverted);
        drone.Quaternion = quaternion;
    }
}

Note that this might mean that the drone will not always be able to right itself if you're not clever in picking the locked axis.

Hope this helps, a word of caution, all this code was adapted from my original code that does this for the forward vector instead of up vector. Quaternions ara black magic, so I might have made a mistake.