Create a test to check AutoDiff and vector differentiation in a non-world frame.

[mitiguy]

Create a test to see how Drake handles vector differentiation. For example, consider a pendulum B of length L that has a simple rotation in world W. The position vector from Wo (world origin) to Bp (the distal end of B) can be written in monogram notation as either p_WoBp_B = [L, 0, 0] p_WoBp_W = [Lcos(theta), Lsin(theta), 0].

p_WoBp is a vector function of theta in world W. p_WoBp is not a vector function of theta in B.

Check to see if AutoDiff produces results which match the "expressed-in" frame to the "measured-in" frame. For example, I imagine that Dt( p_WoBp_B ) = [0, 0, 0] represents the derivative of p_WoBp in B. Dt( p_WoBp_W) = [-Lsin(theta), Lcos(theta), 0] * theta' represents the derivative of p_WoBp in W.

If all goes well, see if AutoDiff can be used to calculate Dt(Jacobian, A) * v for Kuka arm test in multibody_plant_jacobians_test.cc when the measured-in-frame is frame A (not world W).
Note: Be careful with differentiating position vectors for points that are regarded as "instantaneously" fixed on a rigid frame or rigid body.

[amcastro-tri]

I guess worth posting my email here:

I think you are right, we could use AutoDiff to compute an arbitrary acceleration. As you mentioned, you'd need to understand what AutoDiff does for you. Well, AutoDiff does not know of course about frames. You give it a function of time, and it computes the regular derivative with respect to that parameter.

Then, I went back to your book and looked up the definition of "measured in". If I got it right, then we can write, by definition:

A_MBp_M = d(V_MBp_M)/dt.

That is, the acceleration of a point P affixed to a frame B, measured and expressed in a frame M, is simply the "regular" time derivative of each of the six numerical coefficients in V_MBp_M.

That gives us what we want, no? that is, you'd write a function that computes V_MBp_M (much easier than writing accelerations). With AutoDiff, you take it's time derivative. Each of the six coefficients in this new object are the components of A_MBp_M, no?

If you want to re-express, then you can do A_MBp_E = R_EM * A_MBp_M.

The unit test would then compute Jacobian J_MBp_E (in master!) and bias ABias_MBp_E (your new PR!) and then compare the two methods: A_MBp_E_method1 = J_MBp_E * vdot + ABias_MBp_E A_MBp_E_method2 (AutoDiff)

What do you think? Am I messing up the math? This would apply to any system, including a big articulated robot in 3D.

[sherm1]

AutoDiff only does partial derivatives, so we probably ought to analyze it that way. I think the full definition of the acceleration quantity should mention that the time derivative is taken in frame M:

A_MBp_M = d_M(V_MBp_M)/dt.

Then AutoDiff will take partials one scalar at a time so can’t change frames

A_MBp_M = ∂ V_MBp_M / ∂ x ⋅ ẋ
        = AutoDiff(V_MBp_M, x) ⋅ ẋ

where x = {q,v}.

[sherm1]

Forwarding email exchange w/@amcastro-tri:

AC: But that's not what I tried to write. I literally meant A_MBp_M = d(V_MBp_M)/dt, with not frame M, because I am taking just a "regular" time derivative of each of the components of V_MBp_M, which autodiff can compute.

MS: Oh, I see – you mean you would do AutoDiff(V_MBp_M, t) rather than x! Good idea. That is automatically taking the time derivative in the M frame, since the original calculation is measured in M.

[mitiguy]

Yes, I agree. A few thoughts about tests and notation.

1. Test the hypothesis that AutoDiff can carry out the definition of vector differentiation.
2. Perhaps provide a check to that test with the "Golden rule for vector differentiation".
3. Perhaps encode that "check" as a new method used in conjunction with AutoDiff (I imagine it would be significantly more efficient to compute vector derivatives using the Golden rule).
4. There is some subtlety about differentiating position vectors for points that are regarded as "instantaneously" fixed on a rigid frame or rigid body. I think this may occur with an AutoDiff calculation of Jdot * v in a measured-in-frame that is not World. I have worked through this analytically, but I need to check the associated calculations in Drake.
5. It would be helpful for monogram notation to distinguish between an ordinary derivative with respect to time t and a partial derivative with respect to x (or in a rare case, a partial derivative with respect to t). As we converge on notation, I can update https://drake.mit.edu/doxygen_cxx/group___dt__multibody__quantities.html

[sherm1]

Oh, rats -- @amcastro-tri, Paul pointed out that the time autodifferentiation doesn't work because q and v look like constants in our code rather than q(t) and v(t). So when we AutoDiff(f(qₜ,vₜ), t) we get ∂f/∂t evaluated at q=qₜ, v=vₜ -- that is, we catch only explicit time dependencies in f. (By qₜ,vₜ I mean the current values for q and v as found in the Context.)

[amcastro-tri]

I don't understand this last comment @sherm1. We've done differentiation with respect to time over a trajectory q(t), v(t). Given arbitrary values of q and v, for this case we need to specify the time derivatives as:

1. For v, (arbitrary) derivatives vdot for each v.
2. For q, derivatives must be qdot = MapVToQdot(v).

If you do that, then you get the total derivative of f(qₜ,vₜ) above.

[sherm1]

That's true if you AutoDiff a function that includes the time dependence of q and v. But kinematic functions like f(q,v) ≜ frame.CalcSpatialVelocity() just take q and v as inputs. The partial w.r.t. time of that function is zero.

[amcastro-tri]

per f2f. When computing time derivatives, the known vdot is stored in v.derivatives(). Therefore the proposal above works and provides a simple way to implement these tests.

[sherm1]

Thanks for that explanation, Alejandro. To elaborate:

We have a templated function f<T>(t,q,v). We want to use Autodiff to obtain the total time derivative df/dt. Note that q=q(t) and v=v(t) but f does not contain those dependencies. Instantiate f<AD1> where AD1=AutoDiffScalar<1>, i.e. just a value and one derivative. Ignoring the value part, we have

f<AD1>(t.d, q.d, v.d) = 

    ∂f         ∂f         ∂f
    -- t.d  +  -- q.d  +  -- v.d
    ∂t         ∂q         ∂v

where t.d, q.d, and v.d are the initial values supplied in the derivative part of AD1. To get the total time derivative we need to supply the initial values t.d=dt/dt=1, q.d=dq/dt=q̇, and v.d=dv/dt=v̇ giving

f<AD1>({t,1}, {q,q̇}, {v,v̇}) =

   {     ∂f     ∂f       ∂f   }              df
   { f,  --  +  -- q̇  +  -- v̇ }    ≡   { f,  -- }
   {     ∂t     ∂q       ∂v   }              dt

[mitiguy]

Created PR #13593 to check AutoDiff and vector differentiation in a non-world frame (for issue #13562).

[sherm1]

FYI here is Evan's Intro to AutoDiff.

[mitiguy]

@amcastro-tri and @sherm1 Feel free to comment on the current plan below. My current plan for two PRs (possibly one being an evolution of PR #13593) is as follows:

1. In a test section of the code, specifically in multibody_plant_jacobians_test.cc, create a utility function for calculating an ordinary time-derivative using AutoDiff functionality.
2. Use the two relatively simple test fixtures in multibody_plant_jacobians_test.cc (namely the planar double pendulum test fixture and the satellite tracker (3D) test fixture) to verify CalcBiasTranslationalAcceleration() test results for a non-World measured-in-frame by differentiating the Jacobian J to calculate Jdot and multiplying by v (generalized speed). Ideally, the calculation of Jdot * v should match results from CalcBiasTranslationalAcceleration(). This results are already cross-verified by analytical results, so having a third check also serves as a check of the utility function in item 1 (above).
3. Use the utility function in item 1 (above) to differentiate translational velocity v with values of vdot = 0. Ideally, the calculation of Dt(v) evaluated when vdot = 0 should also match results from CalcBiasTranslationalAcceleration().
4. Items 1-3 will constitute the first PR.
5. The second PR will more extensively test CalcBiasTranslationalAcceleration() and CalcBiasSpatialAcceleration() for the Kuka Arm for a non-World measured-in-frame. This second PR will make use of the utility function in item #1.