MultibodyPlant vs Pinocchio Performance

mposa commented 3 years ago

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 commented 3 years ago

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 commented 3 years ago

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 commented 3 years ago

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 commented 2 years ago

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 commented 2 years ago

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 commented 2 years ago

cc @amcastro-tri

yminchen commented 2 years ago

@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.

DAIRLab / dairlib