Normalize drone orientation errors to [-pi, pi]

[freedomcondor]

Problem description:
When using drones, first set
robot.flight_system.set_target_pose(vector3(0,0,0), math.pi - 0.01)
and then set
robot.flight_system.set_target_pose(vector3(0,0,0), math.pi + 0.01)
instead of keep rotating, the drone tends to rotate back through the longer side to math.pi+0.1

I tweaked the Z component of orientation error and angular velocity error in drone pointmass model to make the drone rotates smoothly when jumping across the edge of Pi.
A small test scenario is enclosed
vns.zip

[allsey87]

This can be merged as soon as the issue on MacOS 11 is resolved. Until that point, we cannot run the minimal set of tests that were defined and I do not think we should merge untested changes into the master branch.

[allsey87]

@ilpincy the issue seem to be related to the tcl-tk dependency

[ilpincy]

The test scripts of Github Actions are broken at the VM setup phase. This code does not affect those tests.

I reviewed the code changes. The original code was completely wrong, so this changes are a welcome addition. The changes, however, only consider the Z axis despite being committed to a 3D engine. Still better than the previous version, so I'll merge them. Thanks for the contribution! :-)

[freedomcondor]

Hi, Michael,
Hi, Carlo,
Thank you for reviewing.
No hurry from my side to wait for github-MacOS 11 issue, take your time.
I considered making the same changes for X and Y axis, but I found it makes the code tediously longer. Maybe in the future we could find a more elegant way to represent orientation errors. For now, I think we are good until someone wants the drone to perform a somersault in the air.
Best
Weixu

[freedomcondor]

Hi, Carlo, Michael,

I made another commit about qtopengl for you to consider, this may should belong to another PR, but I can't create a new one with the current one existing, so I made a commit here. The check failure about MacOS still seems to be there, and I saw another check failure saying can't find qt5-default package. Is it my fault? Let me know how I can do things more properly.

This commit consists two aspects:

1. I'm currently working with drones flying in 3D space. I found it would be very useful if we can move the objects vertically or rotates along X axis. The current configuration is ctrl + scroll to rotate along Z axis. In this commit I added:
   shift + scroll to rotate along X axis
   shift + ctrl + scroll to move object vertically.

2. I also adjusted the camera movement logic. Currently, we can use mouse to move and rotate the camera as we are driving the camera in a 3D first shooting game. However, I found what I actually need more is I can fix the camera focus on a robot/a group of robots/interesting objects and rotate the camera around it. Currently, if I drag the mouse, the camera rotates on its spot, and I lose my sight of the robot. Therefore I adjusted the logic a bit, added a few functions in qtopengl_camera.cpp : when we drag the mouse, the camera rotates along the target.
   The setup is: drag with left button to rotate around the target
   drag with right button to move horizontally
   drag with middle button to move vertically
   scroll middle button to zoon in and out.
   I found this more intuitive, and very very handy. I'm not sure whether everybody will like it or not. I commit here just for you to consider.

Weixu