Missing promotion rules for numbers and variables

[rdeits]

I'm trying to integrate the contact model from MomentumBasedControl.jl into my controller, and since I'm explicitly reasoning about the us, i need to extract the torques from my contact wrenches. That means doing something like @expression torque(J, wrench) where J is a Paramter{GeometricJacobian} and wrench is a Wrench{Variable}. However, torque(J(), wrench) is currently type-unstable because promote_type(Float64, Variable) is Any. Here's a non-parameterized example:

body = findbody(mechanism, "foot")
path_to_root = path(state.mechanism, body, root_body(state.mechanism))
J = geometric_jacobian(state, path_to_root)
w = Wrench(root_frame(mechanism), 
    SVector(Variable(1), Variable(2), Variable(3)),
    SVector(Variable(1), Variable(2), Variable(3)))
torque(J, w)

which returns:

2-element Array{Any,1}:
 -0.0 * x1 + -0.0 * x2 + -0.0 * x3 + -0.0 * x1 + -0.0 * x2 + -1.0 * x3 + 0.0
 -0.0 * x1 + -0.0 * x2 + -0.0 * x3 + -0.0 * x1 + -0.0 * x2 + 1.0 * x3 + 0.0 

[rdeits]

Actually, I'm not sure how to get a non-allocating torque even with this promotion fixed. Even with a concretely-typed result, torque!(result, J, w) still allocates because it makes a bunch of new affine terms internally.

What's the right thing to do in a case like this? I could implement an optimize method for torque, but only by being a type-pirate ☠️. Alternatively, I could extract the matrix representation of J and the vector representation of w and then just do a standard matrix-vector product, which will get optimized.

[tkoolen]

Yeah, this is the kind of thing that we're giving up by not basing off of Cassette. With Cassette, you could perform the optimizations directly on the code of e.g. torque!.

What's the right thing to do in a case like this? I could implement an optimize method for torque, but only by being a type-pirate ☠️. Alternatively, I could extract the matrix representation of J and the vector representation of w and then just do a standard matrix-vector product, which will get optimized.

Yep, those are the two possibilities. I'd start with transpose(angular(J)) * angular(w) + transpose(linear(J)) * linear(w) (instead of concatenating the matrices and working with one matrix-vector product), and then we'll have to see which optimizations are missing (if any) to make that fast. Unfortunately there are changes to the transpose stuff between 0.6 and 0.7; if you want it now, we may have to start with At_mul_B!(angular(J), angular(w)) + At_mul_B!(linear(J), linear(w)) instead.