Drake: RotationMatrix::ToQuaternion isn't compatible with symbolic::Expression

[david-german-tri]

rotmat2quat contains comparisons, and thus isn't friendly to symbolic::Expression. I think this limitation was introduced in #3604, which of course fixed some other problems.

The same problem exists in Eigen's built-in conversion, see https://eigen.tuxfamily.org/dox-devel/Quaternion_8h_source.html at line 767.

[jwnimmer-tri]

Ooh, another bit of code ripe for cond; I am excited!

[soonho-tri]

@jwnimmer-tri , how's this?

https://github.com/soonho-tri/drake/commit/388140786a8278f787a93b98cf53a29280b3d3ad

[jwnimmer-tri]

I think the "identify the largest trace or diagonal" selection conditions should probably not be repeated.

There should be a way to write it case-wise by using Vector4<Scalar> as the resulting type of the cond and assigning it directly onto q, so roughly:
```c++
Scalar tr = M.trace();
Vector4 q = cond(
(tr >= M(0, 0) && tr >= M(1, 1) && tr >= M(2, 2)),
Vector4(
1 + tr,
M(2, 1) - M(1, 2),
M(0, 2) - M(2, 0),
M(1, 0) - M(0, 1)),
(M(0, 0) >= M(1, 1) && M(0, 0) >= M(2, 2)),
...
);
Scalar scale = q.norm();
...
}

[soonho-tri]

Great, that looks better!

[jwnimmer-tri]

For fun we can also just return q.normalized(); I think, instead of writing it out with operator/=.

[soonho-tri]

I'll open a PR tomorrow morning.

BTW, how about the issues inside Eigen? Do you have a suggestion?

[jwnimmer-tri]

You mean the quaternionbase_assign_impl<Other,3,3>? I see no way for assigning Matrix3 into Quaternion to work using that function body. If we want to try to make that work, we would have to find a way to overload or specialize it somehow.

[sherm1]

@soonho-tri, please don't make rotmat2quat<double> any slower than it is currently; it is expensive enough already. Are you proposing to specialize it for symbolic::Expression or to rewrite it for all scalar types?

[jwnimmer-tri]

The solution I outlined would be a rewrite for all scalars, but should end up identical when compiled; cond is just sugar for if-else chains.

[david-german-tri]

Do we really want to bake a conditional into the symbolic representation of every rotation-matrix-to-quaternion conversion? As far as I can tell, the if-then-else is a numerical improvement for floating-point applications, but it's not physically meaningful: there are symbolic formulations of rotmat2quat that don't have it, such as the pre-#3604 one. I'm no expert, though.

I see no way for assigning Matrix3 into Quaternion to work using that function body. If we want to try to make that work, we would have to find a way to overload or specialize it somehow.

I agree. Using a Drake library to do the conversion, and then assigning the Eigen::Quaternion<T> from a Vector<T, 4, 1> seems like an adequate workaround.

[jwnimmer-tri]

Pre-3604, it looks like it was just a single basic block, so it already would have supported symbolic form.

I also don't understand where this issue is coming from in the first place, so its difficult to speak to the larger picture.

[david-german-tri]

Pre-3604, it looks like it was just a single basic block, so it already would have supported symbolic form.

Yeah, exactly. And if we assume uses of symbolic::Expression are infinite-precision [1], the pre-#3604 implementation is just as correct, and doesn't introduce an if-then-else.

[1] This may be a bad assumption. I have no idea what SMT solvers actually do.

I also don't understand where this issue is coming from in the first place, so its difficult to speak to the larger picture.

I'm just trying to implement a PoseVector, as discussed on Slack, that uses {R, p} as its internal representation but can produce an Eigen::Isometry3 on demand. It will appear as a System input and output.

[soonho-tri]

... if we assume uses of symbolic::Expression are infinite-precision [1] ...
[1] This may be a bad assumption. I have no idea what SMT solvers actually do.

There are some SMT solvers handling up to decidable-theories such as LRA(Linear Real Arithmetic) and they are using GMP to have required precision.

There are other SMT solvers whose goal is to handle undecidable theories such as (Non-linear Real Arithmetic) using approximation techniques. They are using numerical methods with finite precision (usually double). FYI, dReal is in this category.

[sherm1]

Pre #3604 this was implemented with a switch instead of if-then-else but otherwise the algorithm looked structurally similar -- does the switch make it better for symbolic?

The currently-implemented algorithm is the end result of an extended argument in the Aerospace literature following many incorrect algorithms. The conversion is extremely sensitive to noise in the rotation matrix so you would need truly infinite precision to do it without conditionals. Please be careful!

[jwnimmer-tri]

We definitely will leave the current arithmetic and case-analysis intact in the double case. The only question is if the new formulation performs extra throwaway computation of temporaries. David and Soonho will post more details.

[david-german-tri]

Pre #3604 this was implemented with a switch instead of if-then-else but otherwise the algorithm looked structurally similar -- does the switch make it better for symbolic?

You are correct; I misread the pre-#3604 version as the naive symbolic answer from Wikipedia.

David and Soonho will post more details.

I think the various threads on the topic can be condensed into a few candidate strategies, which I'll summarize below with pros and cons.

1. Don't convert symbolic rotation matrices to symbolic quaternions.
   a. Don't template systems that input or output PoseVector on symbolic::Expression.
   b. Don't provide a way to set PoseVector from a transformation matrix.
   c. Implement PoseVector as a transformation matrix (16-vector) instead of {R, p} (7-vector).

I reject (a) because widespread support for symbolic::Expression will make everyone's life better; auto-sparsity is one example. I'm suspicious of (b), because it seems like a pretty onerous constraint on System authors. I can live with (c), with some distaste.

1. Encode the floating-point-friendly conditional using cond.

As @sherm1 notes above, this is only acceptable if it's as fast and correct for T = double as the status quo on master. It also has downsides for T = symbolic: you get an irreducible if-then-else baked into your symbolic expression for every rotmat2quat invocation.

1. Implement the naive Wikipedia algorithm for symbolic::Expression using SFINAE.

This is guaranteed harmless to the status quo for double. By the same token, it undermines the one-logic-many-scalar-types ethos. It has upsides for T = symbolic: we get an expression we can manipulate algebraically. It also has downsides: floating-point evaluation of the symbolic expression will have all the same numerical issues as that algorithm. (Note that this downside would not apply to infinite-precision evaluation, as with GMP.)

1. Rethink our entire symbolic strategy. Instead of implementing an Eigen scalar type, implement a symbolic library in which matrices, vectors, rotations, and so on are primitives. Then there is no such thing as a rotation-matrix-to-quaternion conversion.

This is an obviously huge, pie-in-the-sky project for which I have no concrete design, and will not elaborate further.

[david-german-tri]

@soonho-tri and I have run some benchmarks on option 2. It seems to be about 10% faster than the status quo on clang, where it gets inlined, and 30% slower on gcc, where it doesn't. In my opinion, that's close enough that we should move forward with it, while keeping option 3 in our back pocket.

[jwnimmer-tri]

Not sure exactly who was working on this, but I think its @soonho-tri.

[soonho-tri]

@soonho-tri and I have run some benchmarks on option 2. It seems to be about 10% faster than the status quo on clang, where it gets inlined, and 30% slower on gcc, where it doesn't. In my opinion, that's close enough that we should move forward with it, while keeping option 3 in our back pocket.

@sherm1 , could you check this comment and let us know if you like the option 2?

[sherm1]

could you check this comment and let us know if you like the option 2?

My main worry about cond() is that (as I understand it) it is semantically different than if/else or ?: in that it evaluates both expressions regardless of the condition. That makes it a poor substitute in general since the expressions may have side effects or one of them may be un-evaluatable (divide by zero, violate precondition, etc.). Is it possible to engineer this so that it preserves the original behavior when instantiated with a numerical scalar like double or AutoDiffScalar?

[soonho-tri]

@sherm1 , thanks for the answer. We will keep the existing version for double and AutoDiffScalar. I'll add a specialized implementation for symbolic::Expression.

[david-german-tri]

To clarify, that's option (3) above.

[jwnimmer-tri]

Sherm is correct that all sub-expressions are evaluated in a cond. But we've proved that doing so is no slower, and none of them throw exceptions, so what's the actual problem?

[sherm1]

In one of two experiments the run time was 30% slower - I'm not sure that's a proof of anything good!

My main worry though is that replacing if/else with cond() here will lead to the impression that this is the right solution to making conditional code symbolic-friendly, leading to copy/paste repetition elsewhere. It would be if it were a semantic match but it is not. Short-circuit evaluation is a basic feature of C-derived languages since forever and I don't think we should give it up even if we can get away with it opportunistically. Isn't there a better solution?

[soonho-tri]

Short-circuit evaluation is a basic feature of C-derived languages since forever and I don't think we should give it up even if we can get away with it opportunistically. Isn't there a better solution?

I wish I know a nice answer for this. I was thinking about a solution using macros to form closures which will be evaluated later on demand. But I didn't spend time on implementing this idea.

[EricCousineau-TRI]

After tinkering some with Python bindings, and seeing the usage crop up again in #8282, it would be convenient to have PoseVector<T>::set_isometry, and possibly the associated overloaded constructors.

Can I ask if we ended up implementing a specialized symbolic variant of rotmat2quat in master? Or is this TODO possibly unblocked by another function that we have?
https://github.com/RobotLocomotion/drake/blob/d449ccdda3d3e94d7d0f2489857a676b71afa007/systems/rendering/pose_vector.h#L26

I see that multibody is using rotmat2quat, so I guess there's nothing else atm:
https://github.com/RobotLocomotion/drake/blob/d449ccdda3d3e94d7d0f2489857a676b71afa007/multibody/multibody_tree/quaternion_floating_mobilizer.cc#L88-L102

I'd be fine with taking Soonho's code and adding that in as a specialization (to preserve performance) at some point over the next couple of weeks.

@mitiguy Can I ask if it would conflict with your plans at all if I added the symbolic variant of rotmat2quat to rotation_matrix_deprecated?

[mitiguy]

Eric - yes feel free to add it to rotation_matrix_deprecated
after PR #8275 is in master (next few days).
I read the back-and-forth on this topic.
Perhaps, I can optimize it for numerics/symbolics later.

[mitiguy]

@EricCousineau-TRI
PR #8275 is now in master.
Feel free to add your code to rotation_matrix_deprecated.

[sherm1]

I believe rotmat2quat() has been completely removed in favor of RotationMatrix::ToQuaternion() so the issue as stated here is moot. But ... RotationMatrix::ToQuaternion() is still not usable with symbolic expressions.

@mitiguy and @soonho-tri can we fix that? Should we?

[soonho-tri]

I can open a PR adding a specialized RotationMatrix<symbolic::Expression>::ToQuaternion(). Please let me know if this is what you want.

[sherm1]

Thanks, Soonho. I don't think there is a pressing use case so we should probably wait until @mitiguy gets back from vacation to discuss -- I'm not sure this is the only method with conditionals in that family of classes. This could also be a good intro project for our new employee (Damrong Guoy, starting next week) to get him some exposure to symbolics in Drake.

[SeanCurtis-TRI]

Now that sounds like a baptism by fire.

[sherm1]

Yeah, maybe not the _first_ project!

[soonho-tri]

I can open a PR adding a specialized RotationMatrix::ToQuaternion(). Please let me know if this is what you want.

@mitiguy , could you check me if you'd like this or not?

[jwnimmer-tri]

My recommendation would be to resolve this issue by adding a new unit test case named SymbolicToQuaternion, similarly to how I added the SymbolicProjectionTest case recently.

(1) The SymbolicToQuaternion test should verify that, when T = Expression but the input to the method has no free variables, the code uses the current highly-precise if-else implementation. (That is, we should have test inputs that cover all of the branches in the current implementation.)

Rationale: I think its important that even with T = Expression, if terms are all literals then we need to produce a precise literal result. It's likely that the result will be multiplied by other expressions, so we should not allow imprecise literals to propogate just because the code used T = Expression.

(2) The SymbolicToQuaternion test should also verify that when there _is_ a free variable in the input, we expect to receive an exception -- but with a comment disclaimer that in the future we may want to produce an Expression result instead. Just locking in the current behavior so that we know when it changes.

Rationale: We don't need symbolic evaluation here yet -- and we don't have a specific application that closes the loop to make sure we choose a good answer. We do need T = Expression to _compile_, but we don't need it to handle free variables. Documenting this fact will help future rework if we choose to do it.

(3) While we're at it anyway, we could add unit tests for all four quaternion-related methods of RotationMatrix. Currently, only 1 of the 4 have a unit test :cry:.

Once that is all done, I think we can close this.

[jwnimmer-tri]

@siyuanfeng-tri Has found a use case (as part of our domain randomization work) where we'd like the symbolic form of RotationMatrix::ToQuaternion, and where we still want numerical stability when we substitute and evaluate. So, I am going to PR option (2) above.