Suppose you’ve gotten a drone the form of a dice with rotors on every face, it is fully unable to manage its roll-pitch-yaw, however it will possibly speed up alongside any of its 3D orthogonal vectors (see picture beneath). Nevertheless, there’s additionally a gravitational power that accelerates the drone down within the -Z axis of the world body.
Given the quaternion representing the rotation of the drone, how would you assemble a command such that it eliminates the acceleration brought on by gravity? This command is utilized alongside the drone’s orthogonal vectors within the kind [x,y,z]. For the sake of this query as an example the vector that fully opposes gravity is given as [0,0,1] on this planet body.
Simply to make clear what I imply by “command”, if the drone was fully aligned with the world axis the command wanted to counteract gravity would simply be [0,0,1]. If the drone was rotated ahead such that it is +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 picture above) the command wanted could be [-1,0,0].
For my method, which clearly is not working, I attempted fixing this by rotating the vector [0,0,1] by the quaternion with rotquat(vec(0,0,1,0),Q) within the following lua code:
vec = operate(x,y,z,w) return {x=x,y=y,z=z,w=w} finish
operate 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)
finish
operate rotquat(V,Q)
return mquat(mquat(Q,V),vec(-Q.x,-Q.y,-Q.z,Q.w))
finish
This type’ve works for the primary few seconds, the instructions are utilized completely alongside the world +z-axis. Nevertheless, because the drone begins to naturally rotate the instructions slowly shift off the +z-axis and it zips off within the flawed route.
Any concepts on what I ought to do to repair this?