Open mitiguy opened 4 years ago
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.
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}.
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.
Yes, I agree. A few thoughts about tests and notation.
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.)
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:
If you do that, then you get the total derivative of f(qₜ,vₜ) above.
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.
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.
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
Created PR #13593 to check AutoDiff and vector differentiation in a non-world frame (for issue #13562).
FYI here is Evan's Intro to AutoDiff.
@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:
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.