MultibodyPlant vs Pinocchio Performance

[mposa]

This is an issue to track Drake::MBP vs alternatives (currently Pinocchio). This example is on a Cassie URDF. Some comments on the plant:

• Does not include loop closure
• Does not include contact
• Fixed base model (pelvis)

Test code is https://github.com/DAIRLab/dairlib/blob/pinocchio/examples/Cassie/test/benchmark_dynamics.cc

To replicate:

• Clone dairlib
• Checkout pinocchio branch of dairlib
• Install Pinocchio (https://github.com/stack-of-tasks/pinocchio). Install directions at https://stack-of-tasks.github.io/pinocchio/download.html. You will not need to configure environment variables
• Build and run examples/Cassie:benchmark_dynamics. For non-Penn users, recommend

bazel run --define WITH_SNOPT=OFF examples/Cassie:benchmark_dynamics 

Status, July 18, 2020

As run on Michael's laptop

(multibody_plant) 100x inertia autodiff calculations took 182 miliseconds. 1820 microseconds per.
(multibody_plant) 10000x inverse dynamics calculations took 125 miliseconds. 12 microseconds per.
(multibody_plant) 100xautodiff inverse dynamics calculations took 240 miliseconds. 2400 microseconds per.
(multibody_plant) 10000x forward dynamics calculations took 304 miliseconds. 30 microseconds per.
(multibody_plant) 100xautodiff forward dynamics calculations took 348 miliseconds. 3480 microseconds per.
(pinocchio) 10000x rnea inverse dynamics calculations took 20 miliseconds. 2 microseconds per.
(pinocchio) 10000x rnea inverse dynamics derivatives calculations took 61 miliseconds. 6 microseconds per.
(pinocchio) 10000x aba forward dynamics calculations took 32 miliseconds. 3 microseconds per.
(pinocchio) 10000x aba forward dynamics derivative calculations took 120 miliseconds. 12 microseconds per.
vdot_mbp, vdot_pin, diff

Summary (Drake, Pinocchio)

For numerical derivatives, approximating by double x 42 (counting fixed-base coordinates nq=nv=16 and nu=10 )

• Forward dynamics: (30, 3)
• Inverse dynamics: (12, 2)
• Forward dynamics derivatives , Drake-Autodiff: (3480, 12)
• Forward dynamics derivatives , Drake-Numerical: (1260, 12)
• Inverse dynamics derivatives , Drake-Autodiff: (2400, 6)
• Inverse dynamics derivatives , Drake-Numerical: (504, 6)

[sherm1]

I'm making some progress on this -- found some inadvertent inner-loop heap allocations in MBP. Not sure yet how much those affect performance but they can't be good. Will fix shortly.

[sherm1]

FYI @mposa there appear to be some bugs in the test cases for autodiff of inverse & forward dynamics. Try setting the iteration counts to 1 in the test and run a Debug configuration (so that all the Eigen asserts are enabled). Looks like inconsistent use if drake::math::initializeAutoDiff() producing different numbers of derivatives for (e.g.) x and u in forward dynamics. Possible fixes mentioned here.

[mposa]

Yes, you're right. I caught that when I added the comparison between gradient matrices, but did not bother fixing the for loops. The bug probably makes AutoDiff actually look better than it really is.

[yminchen]

A few benchmarks related to the center of mass evaluations:

All units below are [seconds per 100,000 calls].

Without resetting drake context every loop:

r_com time (Pinocchio):0.0406533
r_com time (Drake):0.0244223
v_com time (Pinocchio):0.0578227
v_com time (Drake):0.0902847
J_com time (Pinocchio):0.0692747
J_com time (Drake):0.602594

When resetting drake context in every loop:

r_com time (Pinocchio):0.0988584
r_com time (Drake):0.369517
v_com time (Pinocchio):0.137124
v_com time (Drake):0.711533
J_com time (Pinocchio):0.148367
J_com time (Drake):1.55255

[sherm1]

Thanks @yminchen! I see that the Pinocchio times changed in the second set also -- is there an equivalent of resetting the Context in Pinocchio? Also, can you say what you did specifically to "reset" the Context in Drake?

[sherm1]

cc @amcastro-tri

[yminchen]

@sherm1 Thanks for the question! I should've mentioned this in the previous post. The benchmark was done not directly on Pinocchio but PinocchioPlant (we derived this class from Drake's MultibodyPlant by overriding center of mass related methods). This is why there is a difference between with and without resetting context for Pinocchio. The extra runtime is from calling plant.SetPositions(context.get(), q), for example.