  This paper presents a kinodynamic motion planner that is able to produce
energy efficient motions by taking the full robot dynamics into account, and
making use of gravity, inertia, and momentum to reduce the effort. Given a
specific goal state for the robot, we use factor graphs and numerical
optimization to solve for an optimal trajectory, which meets not only the
requirements of collision avoidance, but also all kinematic and dynamic
constraints, such as velocity, acceleration and torque limits. By exploiting
the sparsity in factor graphs, we can solve a kinodynamic motion planning
problem efficiently, on par with existing optimal control methods, and use
incremental elimination techniques to achieve an order of magnitude faster
replanning.
