AbsoluteAngles sensor is not flexible enough

[casella]

Consider the attached MWE: TestAngleSensor.mo.txt

The body is connected to the fixed reference by a flange rotating around the z axis, while the rotation of the body is measured by an AbsoluteAngles sensor with sequence = {3, 1, 2}. The start position is with all angles equal to zero, so I'm keeping AbsoluteAngles.guessAngle1 to the zero default value.

As I apply a step torque around the z axis, the body starts spinning around it. I would expect absoluteAngles.angles[1] to follow revolute.angle and absoluteAngles.angles[2] to remain zero all the time. This is what I get instead:

As soon as absoluteAngles.angles[1] reaches 90° (not the upper limit of 180° declared in the documentation of the sensor), it jumps to -90°, while absoluteAngles.angles[2] and absoluteAngles.angles[3] jump to -180°. Technically speaking, this is still a valid representation of rotation of the body at that point in time, but I think you agree this is not really nice. If you look at what happens to absoluteAngles.angles[3] after that point, it's even worse, since the solver cannot decide between -180° and +180°.

This is really not nice at all. I am just rotating the body around the z axis, which should I get weird nonzero angles around the other two axes? That makes no sense.

The answer of course is that the way the sensor was built is to solve the implicit equations governing the sensor from a fixed guess value. In particular, it is clear that as soon as the body rotation around the z axis goes above 90°, given the initial guess rotation of 0° the found solution will be the one closest to zero, i.e. a negative angle with magnitude less than 90° - the other two angles will have to undergo abrupt +/-180° rotations accordingly.

Now, imagine that this sensor is used on-board a drone model, which is just rotating around the yaw axis to scan the horizon. These sensor readings will wreak havoc on any sensible autopilot. IT may be OK if the yaw angle jumps from +180 to -180, after all the compass reading only spans 360° and the autopilot needs to be able to cope with that. But getting a pitch angle of 180° instead of 0° all of a sudden while scanning the horizon in perfectly horizontal configuration would be quite weird. It can for sure be handled, but it's not a nice model of the output of a proper orientation sensing system.

I am aware that in general the rotation is a SO(3) group and there are always singular configurations that cannot be properly described with three angles. However, as far as I understand, in this case I'm never getting anywhere near that; the singularity would require at least two angles to be 90°. There's nothing wrong in a simple, ever-increasing yaw rotation.

This problem could be easily solved if the sensor model actually contained the forward kinematic transformation equations and used the implicit Newton solver of the tool to solve the inverse problem for the required angles. One would still need a guess value for the initial angle, but then the solver would latch onto that solution and keep sticking to that throughout the entire transient, without jumping around in weird ways. With angles changing continuously over time. Of course if one really wanted the reading to be limited in the -180°_+180° or in the 0°_360°, those could be easily computed with the rem() operator, as a post-processing option.

@tobolar, @MartinOtter what do you think?

Keeping @arun3688 in the loop.

[tobolar]

The thing is that the AbsoluteAngles sensor calculates the finite rotations while time series of infinitesimal rotations is expected. I have no experience if the suggested forward kinematic transformation would fit to all potential use cases.
A possible remedy is to use UnwrapAngle from #3914, which is sticking for some unresolved reasons.

[tobolar]

A chance is to calculate infinitesimal rotations from the angular velocity vector w which already is a part of the orientation object R. I will check this.

[MartinOtter]

Find attached two different solutions:
NewAngleSensor.mo.txt

• NewAngleSensor.AbsoluteAngles is based on the Sketch of @tobolar by integrating the angular velocity. The advantage is that a small linear equation system is solved all the time. The drawback is that there will be a drift after some time, due to the integration of the angular velocity. For an offline simulation this is probably not so critical if a variable-step solver with a higher tolerance is used, but is probably not acceptable for a real-time application with fixed-step solvers.

• NewAngleSensor.AbsoluteAngles2 is based on the sketch of @casella by formulating a nonlinear equation system (of order 3) and solving it all the time. The advantage is that there will be no drift. The drawback is that solving nonlinear equation systems in a real-time system is not so good.

A better solution is probably to model the sensor as clocked system (as it is anyway in the real implementation) and then remember the value of the angles at the previous clock tick (previous(angles)). Compute the infinite (countable) number of solutions of the angles and then pick the one which is closest to previous(angles). Maybe someone can figure out the details. I have most likely not enough time in the next days.

[HansOlsson]

Even if the drift is normally not critical for variable-step solver it is yet another thing that people will have to consider, which makes it problematic.

For the non-linear system it depends on how complicated it is to solve it; I will investigate.

[casella]

Thanks for you comments! A few comments on my side:

• the drift issue can be a very serious deal when integrating over long time horizons, particularly with low-order fixed time step, I'm not sure I would recommend using it
• nonlinear loops are not ideal for real time, but on the other hand a 3x3 system that can converge quickly because the angles do not change much between steps is not such a big deal.
• unwrapAngle is not enough: as demonstrated by my example, the problem is not the 2pi ambiguity of angles on one axis, but rather the fact that there are always two equivalent representations of the same orientation, differing by pi on all axes, and if you keep a fixed initialization it is inevitable that at some point you converge to the wrong one.

IMO this is not urgent, it's good if we experiment with different strategies so we have one (o more) robust solutions for MSL 4.2.0

[casella]

One more option: maybe we could use the integration of the angular velocity to provide a good initial guess to the current method, which will then converge to the right solution, rather than using a fixed initial guess as it is now. This method would be insensitive to the drift until it reaches 90°, which is probably never going to happen with reasonably long simulations with reasonably good integration accuracy.

In fact, we could even (optionally) check when the drift exceeds, say, 0.2 or 0.5 radians and do a reinit of the states in that case.

What do you think?