diff --git a/.clang-format b/.clang-format
index e48b967216..2598667dd5 100644
--- a/.clang-format
+++ b/.clang-format
@@ -45,6 +45,7 @@ ExperimentalAutoDetectBinPacking: false
 ForEachMacros:   [ foreach, Q_FOREACH, BOOST_FOREACH ]
 # sort headers by main include file (implicit priority 0),
 # then project and private includes, then system headers
+IncludeBlocks: Regroup
 IncludeCategories:
   - Regex:     '^<([a-z|_]+)>' # standard library headers
     Priority:  1
diff --git a/include/aikido/common.hpp b/include/aikido/common.hpp
index 182178d694..6441b559a1 100644
--- a/include/aikido/common.hpp
+++ b/include/aikido/common.hpp
@@ -1,10 +1,10 @@
-#include "common/ExecutorMultiplexer.hpp"
-#include "common/ExecutorThread.hpp"
-#include "common/PseudoInverse.hpp"
-#include "common/RNG.hpp"
-#include "common/Spline.hpp"
-#include "common/StepSequence.hpp"
-#include "common/VanDerCorput.hpp"
-#include "common/metaprogramming.hpp"
-#include "common/stream.hpp"
-#include "common/string.hpp"
+#include "aikido/common/ExecutorMultiplexer.hpp"
+#include "aikido/common/ExecutorThread.hpp"
+#include "aikido/common/PseudoInverse.hpp"
+#include "aikido/common/RNG.hpp"
+#include "aikido/common/Spline.hpp"
+#include "aikido/common/StepSequence.hpp"
+#include "aikido/common/VanDerCorput.hpp"
+#include "aikido/common/metaprogramming.hpp"
+#include "aikido/common/stream.hpp"
+#include "aikido/common/string.hpp"
diff --git a/include/aikido/common/ExecutorThread.hpp b/include/aikido/common/ExecutorThread.hpp
index 6d9bd8e44d..67ff73d397 100644
--- a/include/aikido/common/ExecutorThread.hpp
+++ b/include/aikido/common/ExecutorThread.hpp
@@ -66,6 +66,6 @@ class ExecutorThread final
 } // namespace common
 } // namespace aikido
 
-#include <aikido/common/detail/ExecutorThread-impl.hpp>
+#include "aikido/common/detail/ExecutorThread-impl.hpp"
 
 #endif // AIKIDO_COMMON_EXECUTORTHREAD_HPP_
diff --git a/include/aikido/common/RNG.hpp b/include/aikido/common/RNG.hpp
index bee08b01c5..8348023cf1 100644
--- a/include/aikido/common/RNG.hpp
+++ b/include/aikido/common/RNG.hpp
@@ -6,7 +6,9 @@
 #include <cstdint>
 #include <memory>
 #include <random>
+
 #include <Eigen/Geometry>
+
 #include "aikido/common/pointers.hpp"
 
 namespace aikido {
diff --git a/include/aikido/common/Spline.hpp b/include/aikido/common/Spline.hpp
index 03017013c7..d3704e24ac 100644
--- a/include/aikido/common/Spline.hpp
+++ b/include/aikido/common/Spline.hpp
@@ -4,6 +4,7 @@
 #include <cstddef>
 #include <memory>
 #include <vector>
+
 #include <Eigen/Core>
 #include <Eigen/QR>
 #include <Eigen/Sparse>
diff --git a/include/aikido/common/StepSequence.hpp b/include/aikido/common/StepSequence.hpp
index d78dd8731c..1c1379a301 100644
--- a/include/aikido/common/StepSequence.hpp
+++ b/include/aikido/common/StepSequence.hpp
@@ -4,6 +4,7 @@
 #include <cassert>
 #include <limits>
 #include <tuple>
+
 #include <boost/iterator/iterator_facade.hpp>
 
 namespace aikido {
diff --git a/include/aikido/common/VanDerCorput.hpp b/include/aikido/common/VanDerCorput.hpp
index 52d54f4c94..792ed9ac87 100644
--- a/include/aikido/common/VanDerCorput.hpp
+++ b/include/aikido/common/VanDerCorput.hpp
@@ -3,6 +3,7 @@
 
 #include <limits>
 #include <utility>
+
 #include <boost/iterator/iterator_facade.hpp>
 
 namespace aikido {
diff --git a/include/aikido/common/detail/ExecutorThread-impl.hpp b/include/aikido/common/detail/ExecutorThread-impl.hpp
index a35ba95f68..d38e13b8e1 100644
--- a/include/aikido/common/detail/ExecutorThread-impl.hpp
+++ b/include/aikido/common/detail/ExecutorThread-impl.hpp
@@ -1,4 +1,4 @@
-#include <aikido/common/ExecutorThread.hpp>
+#include "aikido/common/ExecutorThread.hpp"
 
 namespace aikido {
 namespace common {
diff --git a/include/aikido/common/detail/algorithm-impl.hpp b/include/aikido/common/detail/algorithm-impl.hpp
index b275d194a2..9a9aae8e0d 100644
--- a/include/aikido/common/detail/algorithm-impl.hpp
+++ b/include/aikido/common/detail/algorithm-impl.hpp
@@ -1,11 +1,11 @@
 #ifndef AIKIDO_COMMON_DETAIL_ALGORITHM_IMPL_HPP_
 #define AIKIDO_COMMON_DETAIL_ALGORITHM_IMPL_HPP_
 
-#include "aikido/common/algorithm.hpp"
-
 #include <cassert>
 #include <functional>
 
+#include "aikido/common/algorithm.hpp"
+
 namespace aikido {
 namespace common {
 
diff --git a/include/aikido/common/detail/pair-impl.hpp b/include/aikido/common/detail/pair-impl.hpp
index a7aab0c133..d36e38cab2 100644
--- a/include/aikido/common/detail/pair-impl.hpp
+++ b/include/aikido/common/detail/pair-impl.hpp
@@ -1,10 +1,10 @@
 #ifndef AIKIDO_COMMON_DETAIL_PAIR_IMPL_HPP_
 #define AIKIDO_COMMON_DETAIL_PAIR_IMPL_HPP_
 
-#include <aikido/common/pair.hpp>
-
 #include <boost/functional/hash.hpp>
 
+#include "aikido/common/pair.hpp"
+
 namespace aikido {
 namespace common {
 
diff --git a/include/aikido/common/stream.hpp b/include/aikido/common/stream.hpp
index 778d5d25d2..36155c9ff0 100644
--- a/include/aikido/common/stream.hpp
+++ b/include/aikido/common/stream.hpp
@@ -2,6 +2,7 @@
 #define AIKIDO_COMMON_STREAM_HPP_
 
 #include <iostream>
+
 #include <dart/collision/collision.hpp>
 
 namespace aikido {
diff --git a/include/aikido/constraint.hpp b/include/aikido/constraint.hpp
index 7c95a6e0e7..d36ac69d3e 100644
--- a/include/aikido/constraint.hpp
+++ b/include/aikido/constraint.hpp
@@ -1,30 +1,30 @@
-#include "constraint/CartesianProductProjectable.hpp"
-#include "constraint/CartesianProductSampleable.hpp"
-#include "constraint/CartesianProductTestable.hpp"
-#include "constraint/CyclicSampleable.hpp"
-#include "constraint/Differentiable.hpp"
-#include "constraint/DifferentiableIntersection.hpp"
-#include "constraint/DifferentiableSubspace.hpp"
-#include "constraint/FiniteSampleable.hpp"
-#include "constraint/NewtonsMethodProjectable.hpp"
-#include "constraint/Projectable.hpp"
-#include "constraint/RejectionSampleable.hpp"
-#include "constraint/Sampleable.hpp"
-#include "constraint/Satisfied.hpp"
-#include "constraint/Testable.hpp"
-#include "constraint/TestableIntersection.hpp"
-#include "constraint/dart/CollisionFree.hpp"
-#include "constraint/dart/FrameDifferentiable.hpp"
-#include "constraint/dart/FramePairDifferentiable.hpp"
-#include "constraint/dart/FrameTestable.hpp"
-#include "constraint/dart/InverseKinematicsSampleable.hpp"
-#include "constraint/dart/JointStateSpaceHelpers.hpp"
-#include "constraint/dart/TSR.hpp"
-#include "constraint/uniform/RnBoxConstraint.hpp"
-#include "constraint/uniform/RnConstantSampler.hpp"
-#include "constraint/uniform/SE2BoxConstraint.hpp"
-#include "constraint/uniform/SO2UniformSampler.hpp"
-#include "constraint/uniform/SO3UniformSampler.hpp"
+#include "aikido/constraint/CartesianProductProjectable.hpp"
+#include "aikido/constraint/CartesianProductSampleable.hpp"
+#include "aikido/constraint/CartesianProductTestable.hpp"
+#include "aikido/constraint/CyclicSampleable.hpp"
+#include "aikido/constraint/Differentiable.hpp"
+#include "aikido/constraint/DifferentiableIntersection.hpp"
+#include "aikido/constraint/DifferentiableSubspace.hpp"
+#include "aikido/constraint/FiniteSampleable.hpp"
+#include "aikido/constraint/NewtonsMethodProjectable.hpp"
+#include "aikido/constraint/Projectable.hpp"
+#include "aikido/constraint/RejectionSampleable.hpp"
+#include "aikido/constraint/Sampleable.hpp"
+#include "aikido/constraint/Satisfied.hpp"
+#include "aikido/constraint/Testable.hpp"
+#include "aikido/constraint/TestableIntersection.hpp"
+#include "aikido/constraint/dart/CollisionFree.hpp"
+#include "aikido/constraint/dart/FrameDifferentiable.hpp"
+#include "aikido/constraint/dart/FramePairDifferentiable.hpp"
+#include "aikido/constraint/dart/FrameTestable.hpp"
+#include "aikido/constraint/dart/InverseKinematicsSampleable.hpp"
+#include "aikido/constraint/dart/JointStateSpaceHelpers.hpp"
+#include "aikido/constraint/dart/TSR.hpp"
+#include "aikido/constraint/uniform/RnBoxConstraint.hpp"
+#include "aikido/constraint/uniform/RnConstantSampler.hpp"
+#include "aikido/constraint/uniform/SE2BoxConstraint.hpp"
+#include "aikido/constraint/uniform/SO2UniformSampler.hpp"
+#include "aikido/constraint/uniform/SO3UniformSampler.hpp"
 
 namespace aikido {
 namespace constraint {
diff --git a/include/aikido/constraint/CartesianProductProjectable.hpp b/include/aikido/constraint/CartesianProductProjectable.hpp
index 381d58a3da..658d99e353 100644
--- a/include/aikido/constraint/CartesianProductProjectable.hpp
+++ b/include/aikido/constraint/CartesianProductProjectable.hpp
@@ -1,8 +1,8 @@
 #ifndef AIKIDO_CONSTRAINT_CARTESIANPRODUCTPROJECTABLE_HPP_
 #define AIKIDO_CONSTRAINT_CARTESIANPRODUCTPROJECTABLE_HPP_
 
-#include "../statespace/CartesianProduct.hpp"
-#include "Projectable.hpp"
+#include "aikido/constraint/Projectable.hpp"
+#include "aikido/statespace/CartesianProduct.hpp"
 
 namespace aikido {
 namespace constraint {
diff --git a/include/aikido/constraint/CartesianProductSampleable.hpp b/include/aikido/constraint/CartesianProductSampleable.hpp
index 37a9fc09ed..9735112f42 100644
--- a/include/aikido/constraint/CartesianProductSampleable.hpp
+++ b/include/aikido/constraint/CartesianProductSampleable.hpp
@@ -2,8 +2,9 @@
 #define AIKIDO_CONSTRAINT_CARTESIANPRODUCTSAMPLEABLE_HPP_
 
 #include <vector>
-#include "../statespace/CartesianProduct.hpp"
-#include "Sampleable.hpp"
+
+#include "aikido/constraint/Sampleable.hpp"
+#include "aikido/statespace/CartesianProduct.hpp"
 
 namespace aikido {
 namespace constraint {
diff --git a/include/aikido/constraint/CartesianProductTestable.hpp b/include/aikido/constraint/CartesianProductTestable.hpp
index 432292e2d1..c75b3b3830 100644
--- a/include/aikido/constraint/CartesianProductTestable.hpp
+++ b/include/aikido/constraint/CartesianProductTestable.hpp
@@ -2,8 +2,9 @@
 #define AIKIDO_CONSTRAINT_TESTABLESUBSPACE_HPP_
 
 #include <vector>
-#include "../statespace/CartesianProduct.hpp"
-#include "Testable.hpp"
+
+#include "aikido/constraint/Testable.hpp"
+#include "aikido/statespace/CartesianProduct.hpp"
 
 namespace aikido {
 namespace constraint {
diff --git a/include/aikido/constraint/CyclicSampleable.hpp b/include/aikido/constraint/CyclicSampleable.hpp
index bd76ba67aa..8778a29357 100644
--- a/include/aikido/constraint/CyclicSampleable.hpp
+++ b/include/aikido/constraint/CyclicSampleable.hpp
@@ -1,7 +1,7 @@
 #ifndef AIKIDO_CONSTRAINT_CYCLICSAMPLEABLE_HPP_
 #define AIKIDO_CONSTRAINT_CYCLICSAMPLEABLE_HPP_
 
-#include "Sampleable.hpp"
+#include "aikido/constraint/Sampleable.hpp"
 
 namespace aikido {
 namespace constraint {
diff --git a/include/aikido/constraint/DefaultTestableOutcome.hpp b/include/aikido/constraint/DefaultTestableOutcome.hpp
index e019d8d25a..f5e9f76040 100644
--- a/include/aikido/constraint/DefaultTestableOutcome.hpp
+++ b/include/aikido/constraint/DefaultTestableOutcome.hpp
@@ -1,7 +1,7 @@
 #ifndef AIKIDO_CONSTRAINT_DEFAULTTESTABLEOUTCOME_HPP_
 #define AIKIDO_CONSTRAINT_DEFAULTTESTABLEOUTCOME_HPP_
 
-#include "TestableOutcome.hpp"
+#include "aikido/constraint/TestableOutcome.hpp"
 
 namespace aikido {
 namespace constraint {
diff --git a/include/aikido/constraint/Differentiable.hpp b/include/aikido/constraint/Differentiable.hpp
index afdbfe24f7..4d24db6f05 100644
--- a/include/aikido/constraint/Differentiable.hpp
+++ b/include/aikido/constraint/Differentiable.hpp
@@ -2,9 +2,11 @@
 #define AIKIDO_CONSTRAINT_DIFFERENTIABLE_HPP_
 
 #include <memory>
+
 #include <Eigen/Dense>
+
 #include "aikido/common/pointers.hpp"
-#include "../statespace/StateSpace.hpp"
+#include "aikido/statespace/StateSpace.hpp"
 
 namespace aikido {
 namespace constraint {
diff --git a/include/aikido/constraint/DifferentiableIntersection.hpp b/include/aikido/constraint/DifferentiableIntersection.hpp
index 25274b5f87..108a1acb3d 100644
--- a/include/aikido/constraint/DifferentiableIntersection.hpp
+++ b/include/aikido/constraint/DifferentiableIntersection.hpp
@@ -2,9 +2,11 @@
 #define AIKIDO_CONSTRAINT_DIFFERENTIABLEINTERSECTION_HPP_
 
 #include <memory>
+
 #include <Eigen/Dense>
-#include "../statespace/StateSpace.hpp"
-#include "Differentiable.hpp"
+
+#include "aikido/constraint/Differentiable.hpp"
+#include "aikido/statespace/StateSpace.hpp"
 
 namespace aikido {
 namespace constraint {
diff --git a/include/aikido/constraint/DifferentiableSubspace.hpp b/include/aikido/constraint/DifferentiableSubspace.hpp
index 157850fd3e..706459470b 100644
--- a/include/aikido/constraint/DifferentiableSubspace.hpp
+++ b/include/aikido/constraint/DifferentiableSubspace.hpp
@@ -1,8 +1,8 @@
 #ifndef AIKIDO_CONSTRAINT_DIFFERENTIABLESUBSPACE_HPP_
 #define AIKIDO_CONSTRAINT_DIFFERENTIABLESUBSPACE_HPP_
 
-#include "../statespace/CartesianProduct.hpp"
-#include "Differentiable.hpp"
+#include "aikido/constraint/Differentiable.hpp"
+#include "aikido/statespace/CartesianProduct.hpp"
 
 namespace aikido {
 namespace constraint {
diff --git a/include/aikido/constraint/FiniteSampleable.hpp b/include/aikido/constraint/FiniteSampleable.hpp
index ce8bd537ee..222d18aa3a 100644
--- a/include/aikido/constraint/FiniteSampleable.hpp
+++ b/include/aikido/constraint/FiniteSampleable.hpp
@@ -1,7 +1,7 @@
 #ifndef AIKIDO_CONSTRAINT_FINITESAMPLEABLE_HPP_
 #define AIKIDO_CONSTRAINT_FINITESAMPLEABLE_HPP_
 
-#include "Sampleable.hpp"
+#include "aikido/constraint/Sampleable.hpp"
 
 namespace aikido {
 namespace constraint {
diff --git a/include/aikido/constraint/NewtonsMethodProjectable.hpp b/include/aikido/constraint/NewtonsMethodProjectable.hpp
index 659bb2f80f..7b479bf2cc 100644
--- a/include/aikido/constraint/NewtonsMethodProjectable.hpp
+++ b/include/aikido/constraint/NewtonsMethodProjectable.hpp
@@ -2,8 +2,9 @@
 #define AIKIDO_CONSTRAINT_NEWTONSMETHODPROJECTABLE_HPP_
 
 #include <Eigen/Dense>
-#include "Differentiable.hpp"
-#include "Projectable.hpp"
+
+#include "aikido/constraint/Differentiable.hpp"
+#include "aikido/constraint/Projectable.hpp"
 
 namespace aikido {
 namespace constraint {
diff --git a/include/aikido/constraint/Projectable.hpp b/include/aikido/constraint/Projectable.hpp
index 7d4d83efa4..21df02456a 100644
--- a/include/aikido/constraint/Projectable.hpp
+++ b/include/aikido/constraint/Projectable.hpp
@@ -2,7 +2,8 @@
 #define AIKIDO_CONSTRAINT_PROJECTABLE_H
 
 #include <Eigen/Dense>
-#include "../statespace/StateSpace.hpp"
+
+#include "aikido/statespace/StateSpace.hpp"
 
 namespace aikido {
 namespace constraint {
diff --git a/include/aikido/constraint/RejectionSampleable.hpp b/include/aikido/constraint/RejectionSampleable.hpp
index 37e71e0b59..0d1e1387b6 100644
--- a/include/aikido/constraint/RejectionSampleable.hpp
+++ b/include/aikido/constraint/RejectionSampleable.hpp
@@ -1,9 +1,9 @@
 #ifndef AIKIDO_CONSTRAINT_REJECTIONSAMPLEABLE_HPP_
 #define AIKIDO_CONSTRAINT_REJECTIONSAMPLEABLE_HPP_
 
-#include "../statespace/StateSpace.hpp"
-#include "Sampleable.hpp"
-#include "Testable.hpp"
+#include "aikido/constraint/Sampleable.hpp"
+#include "aikido/constraint/Testable.hpp"
+#include "aikido/statespace/StateSpace.hpp"
 
 namespace aikido {
 namespace constraint {
diff --git a/include/aikido/constraint/Sampleable.hpp b/include/aikido/constraint/Sampleable.hpp
index a9af8ea3a2..a3350d4b63 100644
--- a/include/aikido/constraint/Sampleable.hpp
+++ b/include/aikido/constraint/Sampleable.hpp
@@ -3,11 +3,12 @@
 
 #include <limits>
 #include <memory>
+
 #include <boost/optional.hpp>
 
+#include "aikido/common/RNG.hpp"
 #include "aikido/common/pointers.hpp"
-#include "../common/RNG.hpp"
-#include "../statespace/StateSpace.hpp"
+#include "aikido/statespace/StateSpace.hpp"
 
 namespace aikido {
 namespace constraint {
diff --git a/include/aikido/constraint/Satisfied.hpp b/include/aikido/constraint/Satisfied.hpp
index 10bb81157a..916f5ac977 100644
--- a/include/aikido/constraint/Satisfied.hpp
+++ b/include/aikido/constraint/Satisfied.hpp
@@ -1,9 +1,9 @@
 #ifndef AIKIDO_CONSTRAINT_SATISFIED_HPP_
 #define AIKIDO_CONSTRAINT_SATISFIED_HPP_
-#include "Differentiable.hpp"
-#include "Projectable.hpp"
-#include "Sampleable.hpp"
-#include "Testable.hpp"
+#include "aikido/constraint/Differentiable.hpp"
+#include "aikido/constraint/Projectable.hpp"
+#include "aikido/constraint/Sampleable.hpp"
+#include "aikido/constraint/Testable.hpp"
 
 namespace aikido {
 namespace constraint {
diff --git a/include/aikido/constraint/Testable.hpp b/include/aikido/constraint/Testable.hpp
index 6d9076893e..eccdd6b985 100644
--- a/include/aikido/constraint/Testable.hpp
+++ b/include/aikido/constraint/Testable.hpp
@@ -2,9 +2,10 @@
 #define AIKIDO_CONSTRAINT_TESTABLE_HPP_
 
 #include <memory>
+
 #include "aikido/common/pointers.hpp"
-#include "../statespace/StateSpace.hpp"
-#include "DefaultTestableOutcome.hpp"
+#include "aikido/constraint/DefaultTestableOutcome.hpp"
+#include "aikido/statespace/StateSpace.hpp"
 
 namespace aikido {
 namespace constraint {
diff --git a/include/aikido/constraint/TestableIntersection.hpp b/include/aikido/constraint/TestableIntersection.hpp
index fc3b7bf8ac..87275592fa 100644
--- a/include/aikido/constraint/TestableIntersection.hpp
+++ b/include/aikido/constraint/TestableIntersection.hpp
@@ -3,7 +3,8 @@
 
 #include <memory>
 #include <vector>
-#include "Testable.hpp"
+
+#include "aikido/constraint/Testable.hpp"
 
 namespace aikido {
 namespace constraint {
diff --git a/include/aikido/constraint/dart/CollisionFree.hpp b/include/aikido/constraint/dart/CollisionFree.hpp
index 833c628377..a66ceeba3e 100644
--- a/include/aikido/constraint/dart/CollisionFree.hpp
+++ b/include/aikido/constraint/dart/CollisionFree.hpp
@@ -4,10 +4,12 @@
 #include <memory>
 #include <tuple>
 #include <vector>
+
 #include <dart/collision/CollisionDetector.hpp>
 #include <dart/collision/CollisionFilter.hpp>
 #include <dart/collision/CollisionGroup.hpp>
 #include <dart/collision/CollisionOption.hpp>
+
 #include "aikido/common/pointers.hpp"
 #include "aikido/constraint/Testable.hpp"
 #include "aikido/constraint/dart/CollisionFreeOutcome.hpp"
diff --git a/include/aikido/constraint/dart/CollisionFreeOutcome.hpp b/include/aikido/constraint/dart/CollisionFreeOutcome.hpp
index a4c99a2940..93166479f8 100644
--- a/include/aikido/constraint/dart/CollisionFreeOutcome.hpp
+++ b/include/aikido/constraint/dart/CollisionFreeOutcome.hpp
@@ -2,11 +2,13 @@
 #define AIKIDO_CONSTRAINT_DART_COLLISIONFREEOUTCOME_HPP_
 
 #include <vector>
+
 #include <dart/collision/CollisionObject.hpp>
 #include <dart/collision/Contact.hpp>
 #include <dart/dynamics/BodyNode.hpp>
 #include <dart/dynamics/ShapeFrame.hpp>
 #include <dart/dynamics/ShapeNode.hpp>
+
 #include "aikido/constraint/TestableOutcome.hpp"
 
 namespace aikido {
diff --git a/include/aikido/constraint/dart/FrameDifferentiable.hpp b/include/aikido/constraint/dart/FrameDifferentiable.hpp
index 088e05a923..ed59dd4fab 100644
--- a/include/aikido/constraint/dart/FrameDifferentiable.hpp
+++ b/include/aikido/constraint/dart/FrameDifferentiable.hpp
@@ -1,12 +1,12 @@
 #ifndef AIKIDO_CONSTRAINT_DART_FRAMEDIFFERENTIABLE_HPP_
 #define AIKIDO_CONSTRAINT_DART_FRAMEDIFFERENTIABLE_HPP_
 
+#include <Eigen/Dense>
 #include <dart/dynamics/dynamics.hpp>
+
 #include "aikido/constraint/Differentiable.hpp"
 #include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp"
 
-#include <Eigen/Dense>
-
 namespace aikido {
 namespace constraint {
 namespace dart {
diff --git a/include/aikido/constraint/dart/FramePairDifferentiable.hpp b/include/aikido/constraint/dart/FramePairDifferentiable.hpp
index 24eb56ee71..6e38787fa6 100644
--- a/include/aikido/constraint/dart/FramePairDifferentiable.hpp
+++ b/include/aikido/constraint/dart/FramePairDifferentiable.hpp
@@ -3,6 +3,7 @@
 
 #include <Eigen/Dense>
 #include <dart/dynamics/dynamics.hpp>
+
 #include "aikido/constraint/Differentiable.hpp"
 #include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp"
 
diff --git a/include/aikido/constraint/dart/FrameTestable.hpp b/include/aikido/constraint/dart/FrameTestable.hpp
index 7e0976b9ab..54a2fd7c3e 100644
--- a/include/aikido/constraint/dart/FrameTestable.hpp
+++ b/include/aikido/constraint/dart/FrameTestable.hpp
@@ -2,6 +2,7 @@
 #define AIKIDO_CONSTRAINT_DART_FRAMETESTABLE_HPP_
 
 #include <dart/dynamics/dynamics.hpp>
+
 #include "aikido/constraint/Testable.hpp"
 #include "aikido/statespace/SE3.hpp"
 #include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp"
diff --git a/include/aikido/constraint/dart/InverseKinematicsSampleable.hpp b/include/aikido/constraint/dart/InverseKinematicsSampleable.hpp
index 4016877de9..7cb428dcf8 100644
--- a/include/aikido/constraint/dart/InverseKinematicsSampleable.hpp
+++ b/include/aikido/constraint/dart/InverseKinematicsSampleable.hpp
@@ -2,6 +2,7 @@
 #define AIKIDO_CONSTRAINT_DART_INVERSEKINEMATICSSAMPLEABLE_HPP_
 
 #include <dart/dynamics/dynamics.hpp>
+
 #include "aikido/constraint/Sampleable.hpp"
 #include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp"
 
diff --git a/include/aikido/constraint/dart/TSR.hpp b/include/aikido/constraint/dart/TSR.hpp
index e46cabdadc..b7f18c57ad 100644
--- a/include/aikido/constraint/dart/TSR.hpp
+++ b/include/aikido/constraint/dart/TSR.hpp
@@ -3,6 +3,7 @@
 
 #include <Eigen/Dense>
 #include <dart/math/MathTypes.hpp>
+
 #include "aikido/constraint/Differentiable.hpp"
 #include "aikido/constraint/Projectable.hpp"
 #include "aikido/constraint/Sampleable.hpp"
diff --git a/include/aikido/constraint/dart/detail/JointStateSpaceHelpers-impl.hpp b/include/aikido/constraint/dart/detail/JointStateSpaceHelpers-impl.hpp
index 5792561c63..320808dd54 100644
--- a/include/aikido/constraint/dart/detail/JointStateSpaceHelpers-impl.hpp
+++ b/include/aikido/constraint/dart/detail/JointStateSpaceHelpers-impl.hpp
@@ -1,4 +1,5 @@
 #include <sstream>
+
 #include "aikido/common/memory.hpp"
 #include "aikido/common/metaprogramming.hpp"
 #include "aikido/constraint/Satisfied.hpp"
diff --git a/include/aikido/constraint/uniform/detail/RnBoxConstraint-impl.hpp b/include/aikido/constraint/uniform/detail/RnBoxConstraint-impl.hpp
index 2f77d938a3..1edf879c82 100644
--- a/include/aikido/constraint/uniform/detail/RnBoxConstraint-impl.hpp
+++ b/include/aikido/constraint/uniform/detail/RnBoxConstraint-impl.hpp
@@ -1,4 +1,5 @@
 #include <stdexcept>
+
 #include "aikido/constraint/uniform/RnBoxConstraint.hpp"
 
 namespace aikido {
diff --git a/include/aikido/constraint/uniform/detail/RnConstantSampler-impl.hpp b/include/aikido/constraint/uniform/detail/RnConstantSampler-impl.hpp
index f392214fdb..236ef04a42 100644
--- a/include/aikido/constraint/uniform/detail/RnConstantSampler-impl.hpp
+++ b/include/aikido/constraint/uniform/detail/RnConstantSampler-impl.hpp
@@ -1,5 +1,7 @@
 #include <stdexcept>
+
 #include <dart/dart.hpp>
+
 #include "aikido/common/memory.hpp"
 #include "aikido/constraint/uniform/RnConstantSampler.hpp"
 
diff --git a/include/aikido/control.hpp b/include/aikido/control.hpp
index a95b1a4c73..60ff166066 100644
--- a/include/aikido/control.hpp
+++ b/include/aikido/control.hpp
@@ -1,13 +1,13 @@
-#include "control/InstantaneousTrajectoryExecutor.hpp"
-#include "control/KinematicSimulationTrajectoryExecutor.hpp"
-#include "control/PositionCommandExecutor.hpp"
-#include "control/QueuedTrajectoryExecutor.hpp"
-#include "control/TrajectoryExecutor.hpp"
-#include "control/TrajectoryResult.hpp"
-#include "control/TrajectoryRunningException.hpp"
-#include "control/ros/Conversions.hpp"
-#include "control/ros/RosJointStateClient.hpp"
-#include "control/ros/RosPositionCommandExecutor.hpp"
-#include "control/ros/RosTrajectoryExecutionException.hpp"
-#include "control/ros/RosTrajectoryExecutor.hpp"
-#include "control/ros/util.hpp"
+#include "aikido/control/InstantaneousTrajectoryExecutor.hpp"
+#include "aikido/control/KinematicSimulationTrajectoryExecutor.hpp"
+#include "aikido/control/PositionCommandExecutor.hpp"
+#include "aikido/control/QueuedTrajectoryExecutor.hpp"
+#include "aikido/control/TrajectoryExecutor.hpp"
+#include "aikido/control/TrajectoryResult.hpp"
+#include "aikido/control/TrajectoryRunningException.hpp"
+#include "aikido/control/ros/Conversions.hpp"
+#include "aikido/control/ros/RosJointStateClient.hpp"
+#include "aikido/control/ros/RosPositionCommandExecutor.hpp"
+#include "aikido/control/ros/RosTrajectoryExecutionException.hpp"
+#include "aikido/control/ros/RosTrajectoryExecutor.hpp"
+#include "aikido/control/ros/util.hpp"
diff --git a/include/aikido/control/InstantaneousTrajectoryExecutor.hpp b/include/aikido/control/InstantaneousTrajectoryExecutor.hpp
index d48e32de9a..22a3b462dc 100644
--- a/include/aikido/control/InstantaneousTrajectoryExecutor.hpp
+++ b/include/aikido/control/InstantaneousTrajectoryExecutor.hpp
@@ -3,7 +3,9 @@
 
 #include <future>
 #include <mutex>
+
 #include <dart/dynamics/Skeleton.hpp>
+
 #include "aikido/control/TrajectoryExecutor.hpp"
 #include "aikido/trajectory/Trajectory.hpp"
 
diff --git a/include/aikido/control/KinematicSimulationTrajectoryExecutor.hpp b/include/aikido/control/KinematicSimulationTrajectoryExecutor.hpp
index 875909f87c..44de8970b4 100644
--- a/include/aikido/control/KinematicSimulationTrajectoryExecutor.hpp
+++ b/include/aikido/control/KinematicSimulationTrajectoryExecutor.hpp
@@ -3,7 +3,9 @@
 
 #include <future>
 #include <mutex>
+
 #include <dart/dynamics/Skeleton.hpp>
+
 #include "aikido/control/TrajectoryExecutor.hpp"
 #include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp"
 #include "aikido/trajectory/Trajectory.hpp"
diff --git a/include/aikido/control/PositionCommandExecutor.hpp b/include/aikido/control/PositionCommandExecutor.hpp
index 1dfa514830..5b761aeabe 100644
--- a/include/aikido/control/PositionCommandExecutor.hpp
+++ b/include/aikido/control/PositionCommandExecutor.hpp
@@ -3,7 +3,9 @@
 
 #include <chrono>
 #include <future>
+
 #include <Eigen/Dense>
+
 #include "aikido/common/pointers.hpp"
 
 namespace aikido {
diff --git a/include/aikido/control/QueuedTrajectoryExecutor.hpp b/include/aikido/control/QueuedTrajectoryExecutor.hpp
index 1a3247e0f6..999deb968a 100644
--- a/include/aikido/control/QueuedTrajectoryExecutor.hpp
+++ b/include/aikido/control/QueuedTrajectoryExecutor.hpp
@@ -2,7 +2,9 @@
 #define AIKIDO_CONTROL_QUEUEDTRAJECTORYEXECUTOR_HPP_
 
 #include <queue>
+
 #include <dart/dart.hpp>
+
 #include "aikido/control/TrajectoryExecutor.hpp"
 
 namespace aikido {
diff --git a/include/aikido/control/TrajectoryExecutor.hpp b/include/aikido/control/TrajectoryExecutor.hpp
index 20c3f2a30b..4ecb3d3231 100644
--- a/include/aikido/control/TrajectoryExecutor.hpp
+++ b/include/aikido/control/TrajectoryExecutor.hpp
@@ -4,6 +4,7 @@
 #include <chrono>
 #include <future>
 #include <set>
+
 #include "aikido/common/pointers.hpp"
 #include "aikido/trajectory/Trajectory.hpp"
 
diff --git a/include/aikido/control/TrajectoryResult.hpp b/include/aikido/control/TrajectoryResult.hpp
index 2b7d705ae7..9eb1821236 100644
--- a/include/aikido/control/TrajectoryResult.hpp
+++ b/include/aikido/control/TrajectoryResult.hpp
@@ -2,6 +2,7 @@
 #define AIKIDO_CONTROL_TRAJECTORYRESULT_HPP_
 
 #include <memory>
+
 #include "aikido/common/pointers.hpp"
 
 namespace aikido {
diff --git a/include/aikido/control/ros/Conversions.hpp b/include/aikido/control/ros/Conversions.hpp
index cb8b83acc7..50d034cb0c 100644
--- a/include/aikido/control/ros/Conversions.hpp
+++ b/include/aikido/control/ros/Conversions.hpp
@@ -2,8 +2,10 @@
 #define AIKIDO_CONTROL_ROS_CONVERSIONS_HPP_
 
 #include <memory>
+
 #include <sensor_msgs/JointState.h>
 #include <trajectory_msgs/JointTrajectory.h>
+
 #include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp"
 #include "aikido/trajectory/Spline.hpp"
 
diff --git a/include/aikido/control/ros/RosJointStateClient.hpp b/include/aikido/control/ros/RosJointStateClient.hpp
index c114d68d53..ea2e237aed 100644
--- a/include/aikido/control/ros/RosJointStateClient.hpp
+++ b/include/aikido/control/ros/RosJointStateClient.hpp
@@ -2,6 +2,7 @@
 #define AIKIDO_CONTROL_ROS_ROSJOINTSTATECLIENT_HPP_
 
 #include <string>
+
 #include <boost/circular_buffer.hpp>
 #include <dart/dynamics/dynamics.hpp>
 #include <ros/callback_queue.h>
diff --git a/include/aikido/control/ros/RosPositionCommandExecutor.hpp b/include/aikido/control/ros/RosPositionCommandExecutor.hpp
index 00db987207..8d05ea2884 100644
--- a/include/aikido/control/ros/RosPositionCommandExecutor.hpp
+++ b/include/aikido/control/ros/RosPositionCommandExecutor.hpp
@@ -4,12 +4,14 @@
 #include <chrono>
 #include <future>
 #include <mutex>
+
 #include <Eigen/Dense>
 #include <actionlib/client/action_client.h>
 #include <pr_control_msgs/SetPositionAction.h>
 #include <ros/callback_queue.h>
 #include <ros/ros.h>
 #include <sensor_msgs/JointState.h>
+
 #include "aikido/control/PositionCommandExecutor.hpp"
 
 namespace aikido {
diff --git a/include/aikido/control/ros/RosTrajectoryExecutionException.hpp b/include/aikido/control/ros/RosTrajectoryExecutionException.hpp
index 1c4b856fe4..e422ed0f84 100644
--- a/include/aikido/control/ros/RosTrajectoryExecutionException.hpp
+++ b/include/aikido/control/ros/RosTrajectoryExecutionException.hpp
@@ -2,6 +2,7 @@
 #define AIKIDO_CONTROL_ROS_ROSTRAJECTORYEXECUTIONEXCEPTION_HPP_
 
 #include <exception>
+
 #include <actionlib/client/action_client.h>
 #include <control_msgs/FollowJointTrajectoryAction.h>
 
diff --git a/include/aikido/control/ros/RosTrajectoryExecutor.hpp b/include/aikido/control/ros/RosTrajectoryExecutor.hpp
index cf6b852812..44e1c03537 100644
--- a/include/aikido/control/ros/RosTrajectoryExecutor.hpp
+++ b/include/aikido/control/ros/RosTrajectoryExecutor.hpp
@@ -4,10 +4,12 @@
 #include <chrono>
 #include <future>
 #include <mutex>
+
 #include <actionlib/client/action_client.h>
 #include <control_msgs/FollowJointTrajectoryAction.h>
 #include <ros/callback_queue.h>
 #include <ros/ros.h>
+
 #include "aikido/control/TrajectoryExecutor.hpp"
 #include "aikido/trajectory/Trajectory.hpp"
 
diff --git a/include/aikido/control/ros/detail/util-impl.hpp b/include/aikido/control/ros/detail/util-impl.hpp
index fe3de00190..fea7620c73 100644
--- a/include/aikido/control/ros/detail/util-impl.hpp
+++ b/include/aikido/control/ros/detail/util-impl.hpp
@@ -1,5 +1,6 @@
 #include <chrono>
 #include <mutex>
+
 #include <actionlib/client/action_client.h>
 #include <ros/callback_queue.h>
 #include <ros/ros.h>
diff --git a/include/aikido/control/ros/util.hpp b/include/aikido/control/ros/util.hpp
index ad81d88845..bbd183cbe5 100644
--- a/include/aikido/control/ros/util.hpp
+++ b/include/aikido/control/ros/util.hpp
@@ -2,6 +2,7 @@
 #define AIKIDO_CONTROL_ROS_COMMON_HPP_
 
 #include <chrono>
+
 #include <actionlib/client/action_client.h>
 #include <ros/callback_queue.h>
 #include <ros/ros.h>
diff --git a/include/aikido/distance.hpp b/include/aikido/distance.hpp
index 6008c58e87..8fe604a354 100644
--- a/include/aikido/distance.hpp
+++ b/include/aikido/distance.hpp
@@ -1,9 +1,9 @@
-#include "distance/CartesianProductWeighted.hpp"
-#include "distance/DistanceMetric.hpp"
-#include "distance/RnEuclidean.hpp"
-#include "distance/SE2.hpp"
-#include "distance/SE2Weighted.hpp"
-#include "distance/SO2Angular.hpp"
-#include "distance/SO3Angular.hpp"
-#include "distance/Weighted.hpp"
-#include "distance/defaults.hpp"
+#include "aikido/distance/CartesianProductWeighted.hpp"
+#include "aikido/distance/DistanceMetric.hpp"
+#include "aikido/distance/RnEuclidean.hpp"
+#include "aikido/distance/SE2.hpp"
+#include "aikido/distance/SE2Weighted.hpp"
+#include "aikido/distance/SO2Angular.hpp"
+#include "aikido/distance/SO3Angular.hpp"
+#include "aikido/distance/Weighted.hpp"
+#include "aikido/distance/defaults.hpp"
diff --git a/include/aikido/distance/CartesianProductWeighted.hpp b/include/aikido/distance/CartesianProductWeighted.hpp
index 8c196c70a0..fc9c88f453 100644
--- a/include/aikido/distance/CartesianProductWeighted.hpp
+++ b/include/aikido/distance/CartesianProductWeighted.hpp
@@ -1,8 +1,8 @@
 #ifndef AIKIDO_DISTANCE_CARTESIANPRODUCTWEIGHTED_HPP_
 #define AIKIDO_DISTANCE_CARTESIANPRODUCTWEIGHTED_HPP_
 
-#include "../statespace/CartesianProduct.hpp"
-#include "DistanceMetric.hpp"
+#include "aikido/distance/DistanceMetric.hpp"
+#include "aikido/statespace/CartesianProduct.hpp"
 
 namespace aikido {
 namespace distance {
diff --git a/include/aikido/distance/ConfigurationRanker.hpp b/include/aikido/distance/ConfigurationRanker.hpp
index 25c6f3dfb4..2f832b284f 100644
--- a/include/aikido/distance/ConfigurationRanker.hpp
+++ b/include/aikido/distance/ConfigurationRanker.hpp
@@ -1,12 +1,12 @@
 #ifndef AIKIDO_DISTANCE_CONFIGURATIONRANKER_HPP_
 #define AIKIDO_DISTANCE_CONFIGURATIONRANKER_HPP_
 
+#include <dart/dynamics/dynamics.hpp>
+
 #include "aikido/distance/DistanceMetric.hpp"
 #include "aikido/distance/defaults.hpp"
 #include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp"
 
-#include <dart/dynamics/dynamics.hpp>
-
 namespace aikido {
 namespace distance {
 
diff --git a/include/aikido/distance/DistanceMetric.hpp b/include/aikido/distance/DistanceMetric.hpp
index 7b21f4cb79..27fa8c5ef5 100644
--- a/include/aikido/distance/DistanceMetric.hpp
+++ b/include/aikido/distance/DistanceMetric.hpp
@@ -2,7 +2,7 @@
 #define AIKIDO_DISTANCE_DISTANCEMETRIC_HPP_
 
 #include "aikido/common/pointers.hpp"
-#include "../statespace/StateSpace.hpp"
+#include "aikido/statespace/StateSpace.hpp"
 
 namespace aikido {
 namespace distance {
diff --git a/include/aikido/distance/RnEuclidean.hpp b/include/aikido/distance/RnEuclidean.hpp
index 96b733cdfb..692b2fbeb9 100644
--- a/include/aikido/distance/RnEuclidean.hpp
+++ b/include/aikido/distance/RnEuclidean.hpp
@@ -1,8 +1,8 @@
 #ifndef AIKIDO_DISTANCE_EUCLIDEANDISTANCEMETRIC_HPP_
 #define AIKIDO_DISTANCE_EUCLIDEANDISTANCEMETRIC_HPP_
 
-#include "../statespace/Rn.hpp"
-#include "DistanceMetric.hpp"
+#include "aikido/distance/DistanceMetric.hpp"
+#include "aikido/statespace/Rn.hpp"
 
 namespace aikido {
 namespace distance {
diff --git a/include/aikido/distance/SE2.hpp b/include/aikido/distance/SE2.hpp
index 5662b12f39..6e6c56a8ae 100644
--- a/include/aikido/distance/SE2.hpp
+++ b/include/aikido/distance/SE2.hpp
@@ -1,8 +1,8 @@
 #ifndef AIKIDO_DISTANCE_SE2DISTANCEMETRIC_HPP_
 #define AIKIDO_DISTANCE_SE2DISTANCEMETRIC_HPP_
 
-#include "../statespace/SE2.hpp"
-#include "DistanceMetric.hpp"
+#include "aikido/distance/DistanceMetric.hpp"
+#include "aikido/statespace/SE2.hpp"
 
 namespace aikido {
 namespace distance {
diff --git a/include/aikido/distance/SE2Weighted.hpp b/include/aikido/distance/SE2Weighted.hpp
index 889c0b1cab..0804bd6be2 100644
--- a/include/aikido/distance/SE2Weighted.hpp
+++ b/include/aikido/distance/SE2Weighted.hpp
@@ -1,8 +1,8 @@
 #ifndef AIKIDO_DISTANCE_SE2WEIGHTED_HPP_
 #define AIKIDO_DISTANCE_SE2WEIGHTED_HPP_
 
-#include "../statespace/SE2.hpp"
-#include "DistanceMetric.hpp"
+#include "aikido/distance/DistanceMetric.hpp"
+#include "aikido/statespace/SE2.hpp"
 
 namespace aikido {
 namespace distance {
diff --git a/include/aikido/distance/SO2Angular.hpp b/include/aikido/distance/SO2Angular.hpp
index 0d035dcb54..932735f6bc 100644
--- a/include/aikido/distance/SO2Angular.hpp
+++ b/include/aikido/distance/SO2Angular.hpp
@@ -1,8 +1,8 @@
 #ifndef AIKIDO_DISTANCE_ANGULARDISTANCEMETRIC_HPP_
 #define AIKIDO_DISTANCE_ANGULARDISTANCEMETRIC_HPP_
 
-#include "../statespace/SO2.hpp"
-#include "DistanceMetric.hpp"
+#include "aikido/distance/DistanceMetric.hpp"
+#include "aikido/statespace/SO2.hpp"
 
 namespace aikido {
 namespace distance {
diff --git a/include/aikido/distance/SO3Angular.hpp b/include/aikido/distance/SO3Angular.hpp
index 125c0f8e25..f27da48d37 100644
--- a/include/aikido/distance/SO3Angular.hpp
+++ b/include/aikido/distance/SO3Angular.hpp
@@ -1,8 +1,8 @@
 #ifndef AIKIDO_DISTANCE_GEODESICDISTANCEMETRIC_HPP_
 #define AIKIDO_DISTANCE_GEODESICDISTANCEMETRIC_HPP_
 
-#include "../statespace/SO3.hpp"
-#include "DistanceMetric.hpp"
+#include "aikido/distance/DistanceMetric.hpp"
+#include "aikido/statespace/SO3.hpp"
 
 namespace aikido {
 namespace distance {
diff --git a/include/aikido/distance/Weighted.hpp b/include/aikido/distance/Weighted.hpp
index 8ecf798d47..4e50d9029f 100644
--- a/include/aikido/distance/Weighted.hpp
+++ b/include/aikido/distance/Weighted.hpp
@@ -1,7 +1,7 @@
 #ifndef AIKIDO_DISTANCE_WEIGHTEDDISTANCEMETRIC_HPP_
 #define AIKIDO_DISTANCE_WEIGHTEDDISTANCEMETRIC_HPP_
 
-#include "CartesianProductWeighted.hpp"
+#include "aikido/distance/CartesianProductWeighted.hpp"
 
 #warning Weighted is deprecated. Please use CartesianProductWeighted instead by including "CartesianProductWeighted".
 
diff --git a/include/aikido/distance/defaults.hpp b/include/aikido/distance/defaults.hpp
index b8e649f611..36fe728596 100644
--- a/include/aikido/distance/defaults.hpp
+++ b/include/aikido/distance/defaults.hpp
@@ -1,8 +1,8 @@
 #ifndef AIKIDO_DISTANCE_DISTANCEMETRICDEFAULTS_HPP_
 #define AIKIDO_DISTANCE_DISTANCEMETRICDEFAULTS_HPP_
 
-#include "../statespace/StateSpace.hpp"
-#include "DistanceMetric.hpp"
+#include "aikido/distance/DistanceMetric.hpp"
+#include "aikido/statespace/StateSpace.hpp"
 
 namespace aikido {
 namespace distance {
diff --git a/include/aikido/distance/detail/RnEuclidean-impl.hpp b/include/aikido/distance/detail/RnEuclidean-impl.hpp
index 886d7e6561..a74356c950 100644
--- a/include/aikido/distance/detail/RnEuclidean-impl.hpp
+++ b/include/aikido/distance/detail/RnEuclidean-impl.hpp
@@ -1,4 +1,4 @@
-#include <aikido/distance/RnEuclidean.hpp>
+#include "aikido/distance/RnEuclidean.hpp"
 
 namespace aikido {
 namespace distance {
diff --git a/include/aikido/distance/detail/defaults-impl.hpp b/include/aikido/distance/detail/defaults-impl.hpp
index 0819b61064..dd2ea59253 100644
--- a/include/aikido/distance/detail/defaults-impl.hpp
+++ b/include/aikido/distance/detail/defaults-impl.hpp
@@ -1,16 +1,15 @@
 #include "aikido/common/memory.hpp"
-
-#include "../../common/metaprogramming.hpp"
-#include "../../statespace/CartesianProduct.hpp"
-#include "../../statespace/Rn.hpp"
-#include "../../statespace/SE2.hpp"
-#include "../../statespace/SO2.hpp"
-#include "../../statespace/SO3.hpp"
-#include "../CartesianProductWeighted.hpp"
-#include "../RnEuclidean.hpp"
-#include "../SE2Weighted.hpp"
-#include "../SO2Angular.hpp"
-#include "../SO3Angular.hpp"
+#include "aikido/common/metaprogramming.hpp"
+#include "aikido/distance/CartesianProductWeighted.hpp"
+#include "aikido/distance/RnEuclidean.hpp"
+#include "aikido/distance/SE2Weighted.hpp"
+#include "aikido/distance/SO2Angular.hpp"
+#include "aikido/distance/SO3Angular.hpp"
+#include "aikido/statespace/CartesianProduct.hpp"
+#include "aikido/statespace/Rn.hpp"
+#include "aikido/statespace/SE2.hpp"
+#include "aikido/statespace/SO2.hpp"
+#include "aikido/statespace/SO3.hpp"
 
 namespace aikido {
 namespace distance {
diff --git a/include/aikido/io.hpp b/include/aikido/io.hpp
index e9dac9a7e3..9b8ab11602 100644
--- a/include/aikido/io.hpp
+++ b/include/aikido/io.hpp
@@ -1,5 +1,5 @@
-#include "io/CatkinResourceRetriever.hpp"
-#include "io/KinBodyParser.hpp"
-#include "io/trajectory.hpp"
-#include "io/util.hpp"
-#include "io/yaml.hpp"
+#include "aikido/io/CatkinResourceRetriever.hpp"
+#include "aikido/io/KinBodyParser.hpp"
+#include "aikido/io/trajectory.hpp"
+#include "aikido/io/util.hpp"
+#include "aikido/io/yaml.hpp"
diff --git a/include/aikido/io/CatkinResourceRetriever.hpp b/include/aikido/io/CatkinResourceRetriever.hpp
index b45503659e..f9500441fd 100644
--- a/include/aikido/io/CatkinResourceRetriever.hpp
+++ b/include/aikido/io/CatkinResourceRetriever.hpp
@@ -4,6 +4,7 @@
 #include <string>
 #include <unordered_map>
 #include <vector>
+
 #include <dart/common/ResourceRetriever.hpp>
 
 namespace aikido {
diff --git a/include/aikido/io/detail/yaml_extension.hpp b/include/aikido/io/detail/yaml_extension.hpp
index 2fce6cc9ee..0ca1c9c1ee 100644
--- a/include/aikido/io/detail/yaml_extension.hpp
+++ b/include/aikido/io/detail/yaml_extension.hpp
@@ -3,8 +3,10 @@
 
 #include <sstream>
 #include <unordered_map>
+
 #include <Eigen/Dense>
 #include <yaml-cpp/yaml.h>
+
 #include "aikido/common/memory.hpp"
 
 namespace aikido {
diff --git a/include/aikido/perception.hpp b/include/aikido/perception.hpp
index 1eed8f2888..ac41c84361 100644
--- a/include/aikido/perception.hpp
+++ b/include/aikido/perception.hpp
@@ -1,6 +1,6 @@
-#include "perception/AssetDatabase.hpp"
-#include "perception/DetectedObject.hpp"
-#include "perception/PerceptionModule.hpp"
-#include "perception/PoseEstimatorModule.hpp"
-#include "perception/VoxelGridModule.hpp"
-#include "perception/shape_conversions.hpp"
+#include "aikido/perception/AssetDatabase.hpp"
+#include "aikido/perception/DetectedObject.hpp"
+#include "aikido/perception/PerceptionModule.hpp"
+#include "aikido/perception/PoseEstimatorModule.hpp"
+#include "aikido/perception/VoxelGridModule.hpp"
+#include "aikido/perception/shape_conversions.hpp"
diff --git a/include/aikido/perception/AssetDatabase.hpp b/include/aikido/perception/AssetDatabase.hpp
index 5b57587c80..5198342a8f 100644
--- a/include/aikido/perception/AssetDatabase.hpp
+++ b/include/aikido/perception/AssetDatabase.hpp
@@ -2,8 +2,10 @@
 #define AIKIDO_PERCEPTION_ASSET_DATABASE_HPP_
 
 #include <stdexcept>
+
 #include <dart/common/LocalResourceRetriever.hpp>
 #include <dart/dart.hpp>
+
 #include "aikido/io/CatkinResourceRetriever.hpp"
 #include "aikido/io/yaml.hpp"
 
diff --git a/include/aikido/perception/DetectedObject.hpp b/include/aikido/perception/DetectedObject.hpp
index ab48a8cd75..faaa0f8d0e 100644
--- a/include/aikido/perception/DetectedObject.hpp
+++ b/include/aikido/perception/DetectedObject.hpp
@@ -2,7 +2,9 @@
 #define AIKIDO_PERCEPTION_DETECTEDOBJECT_HPP_
 
 #include <string>
+
 #include <dart/dart.hpp>
+
 #include "aikido/io/yaml.hpp"
 
 namespace aikido {
diff --git a/include/aikido/perception/PerceptionModule.hpp b/include/aikido/perception/PerceptionModule.hpp
index 0570017358..a720ef6ab9 100644
--- a/include/aikido/perception/PerceptionModule.hpp
+++ b/include/aikido/perception/PerceptionModule.hpp
@@ -2,6 +2,7 @@
 #define AIKIDO_PERCEPTION_PERCEPTIONMODULE_HPP_
 
 #include <ros/ros.h>
+
 #include "aikido/perception/DetectedObject.hpp"
 #include "aikido/planner/World.hpp"
 
diff --git a/include/aikido/perception/PoseEstimatorModule.hpp b/include/aikido/perception/PoseEstimatorModule.hpp
index 3768ab1dcb..64fbd7c12f 100644
--- a/include/aikido/perception/PoseEstimatorModule.hpp
+++ b/include/aikido/perception/PoseEstimatorModule.hpp
@@ -2,8 +2,10 @@
 #define AIKIDO_PERCEPTION_POSEESTIMATORMODULE_HPP_
 
 #include <string>
+
 #include <dart/dart.hpp>
 #include <tf/transform_listener.h>
+
 #include "aikido/io/CatkinResourceRetriever.hpp"
 #include "aikido/io/yaml.hpp"
 #include "aikido/perception/AssetDatabase.hpp"
diff --git a/include/aikido/planner.hpp b/include/aikido/planner.hpp
index 662d4881fb..5ff14ce2f2 100644
--- a/include/aikido/planner.hpp
+++ b/include/aikido/planner.hpp
@@ -1,16 +1,16 @@
-#include "planner/PlanningResult.hpp"
-#include "planner/SnapPlanner.hpp"
-#include "planner/TrajectoryPostProcessor.hpp"
-#include "planner/World.hpp"
-#include "planner/ompl/BackwardCompatibility.hpp"
-#include "planner/ompl/CRRT.hpp"
-#include "planner/ompl/CRRTConnect.hpp"
-#include "planner/ompl/GeometricStateSpace.hpp"
-#include "planner/ompl/GoalRegion.hpp"
-#include "planner/ompl/MotionValidator.hpp"
-#include "planner/ompl/Planner.hpp"
-#include "planner/ompl/StateSampler.hpp"
-#include "planner/ompl/StateValidityChecker.hpp"
-#include "planner/ompl/dart.hpp"
-#include "planner/parabolic/ParabolicSmoother.hpp"
-#include "planner/parabolic/ParabolicTimer.hpp"
+#include "aikido/planner/PlanningResult.hpp"
+#include "aikido/planner/SnapPlanner.hpp"
+#include "aikido/planner/TrajectoryPostProcessor.hpp"
+#include "aikido/planner/World.hpp"
+#include "aikido/planner/ompl/BackwardCompatibility.hpp"
+#include "aikido/planner/ompl/CRRT.hpp"
+#include "aikido/planner/ompl/CRRTConnect.hpp"
+#include "aikido/planner/ompl/GeometricStateSpace.hpp"
+#include "aikido/planner/ompl/GoalRegion.hpp"
+#include "aikido/planner/ompl/MotionValidator.hpp"
+#include "aikido/planner/ompl/Planner.hpp"
+#include "aikido/planner/ompl/StateSampler.hpp"
+#include "aikido/planner/ompl/StateValidityChecker.hpp"
+#include "aikido/planner/ompl/dart.hpp"
+#include "aikido/planner/parabolic/ParabolicSmoother.hpp"
+#include "aikido/planner/parabolic/ParabolicTimer.hpp"
diff --git a/include/aikido/planner/ConfigurationToConfigurations.hpp b/include/aikido/planner/ConfigurationToConfigurations.hpp
index 5ab52bf23e..2ffdc1939a 100644
--- a/include/aikido/planner/ConfigurationToConfigurations.hpp
+++ b/include/aikido/planner/ConfigurationToConfigurations.hpp
@@ -2,6 +2,7 @@
 #define AIKIDO_PLANNER_CONFIGURATIONTOCONFIGURATIONS_HPP_
 
 #include <unordered_set>
+
 #include "aikido/constraint/Testable.hpp"
 #include "aikido/planner/Problem.hpp"
 #include "aikido/statespace/Interpolator.hpp"
diff --git a/include/aikido/planner/Problem.hpp b/include/aikido/planner/Problem.hpp
index 841b0ba7d1..d95305e55d 100644
--- a/include/aikido/planner/Problem.hpp
+++ b/include/aikido/planner/Problem.hpp
@@ -2,6 +2,7 @@
 #define AIKIDO_PLANNER_PROBLEM_HPP_
 
 #include <string>
+
 #include "aikido/constraint/Testable.hpp"
 #include "aikido/statespace/StateSpace.hpp"
 
diff --git a/include/aikido/planner/SnapPlanner.hpp b/include/aikido/planner/SnapPlanner.hpp
index b2eb7079b8..b5885ffe6b 100644
--- a/include/aikido/planner/SnapPlanner.hpp
+++ b/include/aikido/planner/SnapPlanner.hpp
@@ -2,6 +2,7 @@
 #define AIKIDO_PLANNER_SNAPPLANNER_HPP_
 
 #include <memory>
+
 #include "aikido/common/deprecated.hpp"
 #include "aikido/constraint/Testable.hpp"
 #include "aikido/planner/PlanningResult.hpp"
diff --git a/include/aikido/planner/World.hpp b/include/aikido/planner/World.hpp
index 6014847946..4c816bd6ae 100644
--- a/include/aikido/planner/World.hpp
+++ b/include/aikido/planner/World.hpp
@@ -3,7 +3,9 @@
 
 #include <string>
 #include <unordered_map>
+
 #include <dart/dart.hpp>
+
 #include "aikido/common/pointers.hpp"
 
 namespace aikido {
diff --git a/include/aikido/planner/dart/ConfigurationToConfiguration.hpp b/include/aikido/planner/dart/ConfigurationToConfiguration.hpp
index 9025dd164d..a71602969d 100644
--- a/include/aikido/planner/dart/ConfigurationToConfiguration.hpp
+++ b/include/aikido/planner/dart/ConfigurationToConfiguration.hpp
@@ -2,6 +2,7 @@
 #define AIKIDO_PLANNER_DART_CONFIGURATIONTOCONFIGURATION_HPP_
 
 #include <dart/dart.hpp>
+
 #include "aikido/constraint/Testable.hpp"
 #include "aikido/planner/Problem.hpp"
 #include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp"
diff --git a/include/aikido/planner/dart/ConfigurationToEndEffectorOffset.hpp b/include/aikido/planner/dart/ConfigurationToEndEffectorOffset.hpp
index 2687f2fe5b..cc730e3bc5 100644
--- a/include/aikido/planner/dart/ConfigurationToEndEffectorOffset.hpp
+++ b/include/aikido/planner/dart/ConfigurationToEndEffectorOffset.hpp
@@ -3,6 +3,7 @@
 
 #include <boost/optional.hpp>
 #include <dart/dart.hpp>
+
 #include "aikido/constraint/Testable.hpp"
 #include "aikido/planner/Problem.hpp"
 #include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp"
diff --git a/include/aikido/planner/dart/ConfigurationToEndEffectorPose.hpp b/include/aikido/planner/dart/ConfigurationToEndEffectorPose.hpp
index 945bd025bb..0c9cf61edb 100644
--- a/include/aikido/planner/dart/ConfigurationToEndEffectorPose.hpp
+++ b/include/aikido/planner/dart/ConfigurationToEndEffectorPose.hpp
@@ -2,6 +2,7 @@
 #define AIKIDO_PLANNER_DART_CONFIGURATIONTOENDEFFECTORPOSE_HPP_
 
 #include <dart/dart.hpp>
+
 #include "aikido/planner/Problem.hpp"
 #include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp"
 #include "aikido/trajectory/Interpolated.hpp"
diff --git a/include/aikido/planner/dart/ConfigurationToTSR.hpp b/include/aikido/planner/dart/ConfigurationToTSR.hpp
index 48973806fd..8dcc6d9655 100644
--- a/include/aikido/planner/dart/ConfigurationToTSR.hpp
+++ b/include/aikido/planner/dart/ConfigurationToTSR.hpp
@@ -2,6 +2,7 @@
 #define AIKIDO_PLANNER_DART_CONFIGURATIONTOTSR_HPP_
 
 #include <dart/dart.hpp>
+
 #include "aikido/constraint/dart/TSR.hpp"
 #include "aikido/planner/Problem.hpp"
 #include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp"
diff --git a/include/aikido/planner/dart/detail/PlannerAdapter-impl.hpp b/include/aikido/planner/dart/detail/PlannerAdapter-impl.hpp
index b182f25f71..da2f10eebe 100644
--- a/include/aikido/planner/dart/detail/PlannerAdapter-impl.hpp
+++ b/include/aikido/planner/dart/detail/PlannerAdapter-impl.hpp
@@ -1,4 +1,5 @@
 #include <type_traits>
+
 #include "aikido/planner/dart/PlannerAdapter.hpp"
 #include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp"
 
diff --git a/include/aikido/planner/detail/SingleProblemPlanner-impl.hpp b/include/aikido/planner/detail/SingleProblemPlanner-impl.hpp
index d46107a026..47e1725f78 100644
--- a/include/aikido/planner/detail/SingleProblemPlanner-impl.hpp
+++ b/include/aikido/planner/detail/SingleProblemPlanner-impl.hpp
@@ -1,10 +1,10 @@
 #ifndef AIKIDO_PLANNER_DETAIL_SINGLEPROBLEMPLANNER_IMPL_HPP_
 #define AIKIDO_PLANNER_DETAIL_SINGLEPROBLEMPLANNER_IMPL_HPP_
 
-#include "aikido/planner/SingleProblemPlanner.hpp"
-
 #include <utility>
 
+#include "aikido/planner/SingleProblemPlanner.hpp"
+
 namespace aikido {
 namespace planner {
 
diff --git a/include/aikido/planner/kunzretimer/KunzRetimer.hpp b/include/aikido/planner/kunzretimer/KunzRetimer.hpp
index be0e92f475..379f5a80fc 100644
--- a/include/aikido/planner/kunzretimer/KunzRetimer.hpp
+++ b/include/aikido/planner/kunzretimer/KunzRetimer.hpp
@@ -2,6 +2,7 @@
 #define AIKIDO_PLANNER_KUNZRETIMER_KUNZRETIMER_HPP_
 
 #include <Eigen/Dense>
+
 #include "aikido/planner/TrajectoryPostProcessor.hpp"
 #include "aikido/trajectory/Interpolated.hpp"
 #include "aikido/trajectory/Spline.hpp"
diff --git a/include/aikido/planner/ompl/CRRT.hpp b/include/aikido/planner/ompl/CRRT.hpp
index d43034d6b7..c387cff7d2 100644
--- a/include/aikido/planner/ompl/CRRT.hpp
+++ b/include/aikido/planner/ompl/CRRT.hpp
@@ -5,8 +5,8 @@
 #include <ompl/datastructures/NearestNeighbors.h>
 #include <ompl/geometric/planners/PlannerIncludes.h>
 
-#include "../../constraint/Projectable.hpp"
-#include "../../planner/ompl/BackwardCompatibility.hpp"
+#include "aikido/constraint/Projectable.hpp"
+#include "aikido/planner/ompl/BackwardCompatibility.hpp"
 
 namespace aikido {
 namespace planner {
diff --git a/include/aikido/planner/ompl/CRRTConnect.hpp b/include/aikido/planner/ompl/CRRTConnect.hpp
index 76c1bb3e92..52de8ae102 100644
--- a/include/aikido/planner/ompl/CRRTConnect.hpp
+++ b/include/aikido/planner/ompl/CRRTConnect.hpp
@@ -3,8 +3,9 @@
 
 #include <ompl/datastructures/NearestNeighbors.h>
 #include <ompl/geometric/planners/PlannerIncludes.h>
-#include <aikido/planner/ompl/CRRT.hpp>
-#include "../../constraint/Projectable.hpp"
+
+#include "aikido/constraint/Projectable.hpp"
+#include "aikido/planner/ompl/CRRT.hpp"
 
 namespace aikido {
 namespace planner {
diff --git a/include/aikido/planner/ompl/GeometricStateSpace.hpp b/include/aikido/planner/ompl/GeometricStateSpace.hpp
index 3e8c4f9648..eda2011be0 100644
--- a/include/aikido/planner/ompl/GeometricStateSpace.hpp
+++ b/include/aikido/planner/ompl/GeometricStateSpace.hpp
@@ -2,6 +2,7 @@
 #define AIKIDO_OMPL_AIKIDOGEOMETRICSTATESPACE_HPP_
 
 #include <ompl/base/StateSpace.h>
+
 #include "aikido/constraint/Projectable.hpp"
 #include "aikido/constraint/Sampleable.hpp"
 #include "aikido/constraint/Testable.hpp"
diff --git a/include/aikido/planner/ompl/GoalRegion.hpp b/include/aikido/planner/ompl/GoalRegion.hpp
index 5a5b934ab8..b5a3722c60 100644
--- a/include/aikido/planner/ompl/GoalRegion.hpp
+++ b/include/aikido/planner/ompl/GoalRegion.hpp
@@ -2,8 +2,9 @@
 #define AIKIDO_OMPL_GOALREGION_HPP_
 
 #include <ompl/base/goals/GoalSampleableRegion.h>
-#include "../../constraint/Sampleable.hpp"
-#include "../../constraint/Testable.hpp"
+
+#include "aikido/constraint/Sampleable.hpp"
+#include "aikido/constraint/Testable.hpp"
 
 namespace aikido {
 namespace planner {
diff --git a/include/aikido/planner/ompl/OMPLConfigurationToConfigurationPlanner.hpp b/include/aikido/planner/ompl/OMPLConfigurationToConfigurationPlanner.hpp
index 16040207f3..431cd5b801 100644
--- a/include/aikido/planner/ompl/OMPLConfigurationToConfigurationPlanner.hpp
+++ b/include/aikido/planner/ompl/OMPLConfigurationToConfigurationPlanner.hpp
@@ -7,6 +7,7 @@
 #include <ompl/base/SpaceInformation.h>
 #include <ompl/base/goals/GoalRegion.h>
 #include <ompl/geometric/PathSimplifier.h>
+
 #include "aikido/common/RNG.hpp"
 #include "aikido/constraint/Projectable.hpp"
 #include "aikido/constraint/Sampleable.hpp"
diff --git a/include/aikido/planner/ompl/Planner.hpp b/include/aikido/planner/ompl/Planner.hpp
index ace6d01fc4..f27e64bda2 100644
--- a/include/aikido/planner/ompl/Planner.hpp
+++ b/include/aikido/planner/ompl/Planner.hpp
@@ -4,24 +4,23 @@
 #include <chrono>
 #include <utility> // std::pair
 
-#include "../../constraint/Projectable.hpp"
-#include "../../constraint/Sampleable.hpp"
-#include "../../constraint/Testable.hpp"
-#include "../../distance/DistanceMetric.hpp"
-#include "../../planner/ompl/BackwardCompatibility.hpp"
-#include "../../planner/ompl/GeometricStateSpace.hpp"
-#include "../../statespace/Interpolator.hpp"
-#include "../../statespace/StateSpace.hpp"
-#include "../../trajectory/Interpolated.hpp"
-
 #include <ompl/base/Planner.h>
 #include <ompl/base/ProblemDefinition.h>
 #include <ompl/base/ScopedState.h>
 #include <ompl/base/SpaceInformation.h>
 #include <ompl/base/goals/GoalRegion.h>
-
 #include <ompl/geometric/PathSimplifier.h>
 
+#include "aikido/constraint/Projectable.hpp"
+#include "aikido/constraint/Sampleable.hpp"
+#include "aikido/constraint/Testable.hpp"
+#include "aikido/distance/DistanceMetric.hpp"
+#include "aikido/planner/ompl/BackwardCompatibility.hpp"
+#include "aikido/planner/ompl/GeometricStateSpace.hpp"
+#include "aikido/statespace/Interpolator.hpp"
+#include "aikido/statespace/StateSpace.hpp"
+#include "aikido/trajectory/Interpolated.hpp"
+
 namespace aikido {
 namespace planner {
 namespace ompl {
diff --git a/include/aikido/planner/ompl/StateSampler.hpp b/include/aikido/planner/ompl/StateSampler.hpp
index d1924e50bb..074adc3ffe 100644
--- a/include/aikido/planner/ompl/StateSampler.hpp
+++ b/include/aikido/planner/ompl/StateSampler.hpp
@@ -2,7 +2,8 @@
 #define AIKIDO_OMPL_AIKIDOSTATESAMPLER_HPP_
 
 #include <ompl/base/StateSampler.h>
-#include "../../constraint/Sampleable.hpp"
+
+#include "aikido/constraint/Sampleable.hpp"
 
 namespace aikido {
 namespace planner {
diff --git a/include/aikido/planner/ompl/StateValidityChecker.hpp b/include/aikido/planner/ompl/StateValidityChecker.hpp
index 1830da83ef..959c9e512e 100644
--- a/include/aikido/planner/ompl/StateValidityChecker.hpp
+++ b/include/aikido/planner/ompl/StateValidityChecker.hpp
@@ -3,8 +3,9 @@
 
 #include <ompl/base/SpaceInformation.h>
 #include <ompl/base/StateValidityChecker.h>
-#include "../../constraint/Testable.hpp"
-#include "../../statespace/StateSpace.hpp"
+
+#include "aikido/constraint/Testable.hpp"
+#include "aikido/statespace/StateSpace.hpp"
 
 namespace aikido {
 namespace planner {
diff --git a/include/aikido/planner/ompl/dart.hpp b/include/aikido/planner/ompl/dart.hpp
index 0ecdcba242..bd492d29d6 100644
--- a/include/aikido/planner/ompl/dart.hpp
+++ b/include/aikido/planner/ompl/dart.hpp
@@ -2,8 +2,9 @@
 #define AIKIDO_PLANNER_OMPL_DART_HPP_
 
 #include <ompl/base/SpaceInformation.h>
-#include <aikido/constraint/Testable.hpp>
-#include <aikido/statespace/dart/MetaSkeletonStateSpace.hpp>
+
+#include "aikido/constraint/Testable.hpp"
+#include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp"
 
 namespace aikido {
 namespace planner {
diff --git a/include/aikido/planner/ompl/detail/OMPLConfigurationToConfigurationPlanner-impl.hpp b/include/aikido/planner/ompl/detail/OMPLConfigurationToConfigurationPlanner-impl.hpp
index 57f86d128c..f85c3e62f7 100644
--- a/include/aikido/planner/ompl/detail/OMPLConfigurationToConfigurationPlanner-impl.hpp
+++ b/include/aikido/planner/ompl/detail/OMPLConfigurationToConfigurationPlanner-impl.hpp
@@ -2,6 +2,7 @@
 #define AIKIDO_PLANNER_OMPL_DETAIL_OMPLCONFIGURATIONTOCONFIGURATION_IMPL_HPP_
 
 #include <utility>
+
 #include "aikido/constraint/TestableIntersection.hpp"
 #include "aikido/constraint/dart/FrameDifferentiable.hpp"
 #include "aikido/constraint/dart/FrameTestable.hpp"
diff --git a/include/aikido/planner/ompl/detail/Planner-impl.hpp b/include/aikido/planner/ompl/detail/Planner-impl.hpp
index c43bf72670..a0f9b2b194 100644
--- a/include/aikido/planner/ompl/detail/Planner-impl.hpp
+++ b/include/aikido/planner/ompl/detail/Planner-impl.hpp
@@ -1,9 +1,10 @@
 #include <ompl/geometric/PathGeometric.h>
-#include "../../../planner/ompl/BackwardCompatibility.hpp"
-#include "../../../trajectory/Interpolated.hpp"
-#include "../GeometricStateSpace.hpp"
-#include "../GoalRegion.hpp"
-#include "../StateValidityChecker.hpp"
+
+#include "aikido/planner/ompl/BackwardCompatibility.hpp"
+#include "aikido/planner/ompl/GeometricStateSpace.hpp"
+#include "aikido/planner/ompl/GoalRegion.hpp"
+#include "aikido/planner/ompl/StateValidityChecker.hpp"
+#include "aikido/trajectory/Interpolated.hpp"
 
 namespace aikido {
 namespace planner {
diff --git a/include/aikido/planner/parabolic/ParabolicSmoother.hpp b/include/aikido/planner/parabolic/ParabolicSmoother.hpp
index b8d08ee90b..02c69aca8a 100644
--- a/include/aikido/planner/parabolic/ParabolicSmoother.hpp
+++ b/include/aikido/planner/parabolic/ParabolicSmoother.hpp
@@ -2,6 +2,7 @@
 #define AIKIDO_PLANNER_PARABOLIC_PARABOLICSMOOTHER_HPP_
 
 #include <Eigen/Dense>
+
 #include "aikido/planner/TrajectoryPostProcessor.hpp"
 #include "aikido/trajectory/Interpolated.hpp"
 #include "aikido/trajectory/Spline.hpp"
diff --git a/include/aikido/planner/parabolic/ParabolicTimer.hpp b/include/aikido/planner/parabolic/ParabolicTimer.hpp
index 32bbeb334c..ec92a3c390 100644
--- a/include/aikido/planner/parabolic/ParabolicTimer.hpp
+++ b/include/aikido/planner/parabolic/ParabolicTimer.hpp
@@ -2,6 +2,7 @@
 #define AIKIDO_PLANNER_PARABOLIC_PARABOLICTIMER_HPP_
 
 #include <Eigen/Dense>
+
 #include "aikido/planner/TrajectoryPostProcessor.hpp"
 #include "aikido/trajectory/Interpolated.hpp"
 #include "aikido/trajectory/Spline.hpp"
diff --git a/include/aikido/planner/vectorfield/BodyNodePoseVectorField.hpp b/include/aikido/planner/vectorfield/BodyNodePoseVectorField.hpp
index 5b948d654c..2c110da5d3 100644
--- a/include/aikido/planner/vectorfield/BodyNodePoseVectorField.hpp
+++ b/include/aikido/planner/vectorfield/BodyNodePoseVectorField.hpp
@@ -2,8 +2,9 @@
 #define AIKIDO_PLANNER_VECTORFIELD_BODYNODEPOSEVECTORFIELD_HPP_
 
 #include <dart/dynamics/BodyNode.hpp>
-#include <aikido/planner/vectorfield/VectorField.hpp>
-#include <aikido/statespace/dart/MetaSkeletonStateSpace.hpp>
+
+#include "aikido/planner/vectorfield/VectorField.hpp"
+#include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp"
 
 namespace aikido {
 namespace planner {
diff --git a/include/aikido/planner/vectorfield/MoveEndEffectorOffsetVectorField.hpp b/include/aikido/planner/vectorfield/MoveEndEffectorOffsetVectorField.hpp
index c1e5dc5e03..4972acca80 100644
--- a/include/aikido/planner/vectorfield/MoveEndEffectorOffsetVectorField.hpp
+++ b/include/aikido/planner/vectorfield/MoveEndEffectorOffsetVectorField.hpp
@@ -1,7 +1,7 @@
 #ifndef AIKIDO_PLANNER_VECTORFIELD_MOVEENDEFFECTOROFFSETVECTORFIELD_HPP_
 #define AIKIDO_PLANNER_VECTORFIELD_MOVEENDEFFECTOROFFSETVECTORFIELD_HPP_
 
-#include <aikido/planner/vectorfield/BodyNodePoseVectorField.hpp>
+#include "aikido/planner/vectorfield/BodyNodePoseVectorField.hpp"
 
 namespace aikido {
 namespace planner {
diff --git a/include/aikido/planner/vectorfield/MoveEndEffectorPoseVectorField.hpp b/include/aikido/planner/vectorfield/MoveEndEffectorPoseVectorField.hpp
index be63793503..e1952bee2b 100644
--- a/include/aikido/planner/vectorfield/MoveEndEffectorPoseVectorField.hpp
+++ b/include/aikido/planner/vectorfield/MoveEndEffectorPoseVectorField.hpp
@@ -1,7 +1,7 @@
 #ifndef AIKIDO_PLANNER_VECTORFIELD_MOVEENDEFFECTORPOSEVECTORFIELD_HPP_
 #define AIKIDO_PLANNER_VECTORFIELD_MOVEENDEFFECTORPOSEVECTORFIELD_HPP_
 
-#include <aikido/planner/vectorfield/BodyNodePoseVectorField.hpp>
+#include "aikido/planner/vectorfield/BodyNodePoseVectorField.hpp"
 
 namespace aikido {
 namespace planner {
diff --git a/include/aikido/planner/vectorfield/VectorField.hpp b/include/aikido/planner/vectorfield/VectorField.hpp
index 6162f50014..ec3801b121 100644
--- a/include/aikido/planner/vectorfield/VectorField.hpp
+++ b/include/aikido/planner/vectorfield/VectorField.hpp
@@ -1,10 +1,10 @@
 #ifndef AIKIDO_PLANNER_VECTORFIELD_VECTORFIELD_HPP_
 #define AIKIDO_PLANNER_VECTORFIELD_VECTORFIELD_HPP_
 
-#include <aikido/constraint/Testable.hpp>
-#include <aikido/planner/vectorfield/VectorFieldPlannerStatus.hpp>
-#include <aikido/statespace/StateSpace.hpp>
-#include <aikido/trajectory/Trajectory.hpp>
+#include "aikido/constraint/Testable.hpp"
+#include "aikido/planner/vectorfield/VectorFieldPlannerStatus.hpp"
+#include "aikido/statespace/StateSpace.hpp"
+#include "aikido/trajectory/Trajectory.hpp"
 
 namespace aikido {
 namespace planner {
diff --git a/include/aikido/planner/vectorfield/VectorFieldPlanner.hpp b/include/aikido/planner/vectorfield/VectorFieldPlanner.hpp
index 7e16e67812..fae18351f5 100644
--- a/include/aikido/planner/vectorfield/VectorFieldPlanner.hpp
+++ b/include/aikido/planner/vectorfield/VectorFieldPlanner.hpp
@@ -1,11 +1,11 @@
 #ifndef AIKIDO_PLANNER_VECTORFIELD_VECTORFIELDPLANNER_HPP_
 #define AIKIDO_PLANNER_VECTORFIELD_VECTORFIELDPLANNER_HPP_
 
-#include <aikido/constraint/Testable.hpp>
-#include <aikido/planner/Planner.hpp>
-#include <aikido/planner/vectorfield/VectorField.hpp>
-#include <aikido/statespace/dart/MetaSkeletonStateSpace.hpp>
-#include <aikido/trajectory/Interpolated.hpp>
+#include "aikido/constraint/Testable.hpp"
+#include "aikido/planner/Planner.hpp"
+#include "aikido/planner/vectorfield/VectorField.hpp"
+#include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp"
+#include "aikido/trajectory/Interpolated.hpp"
 
 namespace aikido {
 namespace planner {
diff --git a/include/aikido/planner/vectorfield/VectorFieldUtil.hpp b/include/aikido/planner/vectorfield/VectorFieldUtil.hpp
index 526d8d78d2..f4269c6486 100644
--- a/include/aikido/planner/vectorfield/VectorFieldUtil.hpp
+++ b/include/aikido/planner/vectorfield/VectorFieldUtil.hpp
@@ -2,10 +2,11 @@
 #define AIKIDO_PLANNER_VECTORFIELD_VECTORFIELDUTIL_HPP_
 
 #include <dart/dynamics/BodyNode.hpp>
-#include <aikido/common/Spline.hpp>
-#include <aikido/statespace/dart/MetaSkeletonStateSpace.hpp>
-#include <aikido/trajectory/Interpolated.hpp>
-#include <aikido/trajectory/Spline.hpp>
+
+#include "aikido/common/Spline.hpp"
+#include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp"
+#include "aikido/trajectory/Interpolated.hpp"
+#include "aikido/trajectory/Spline.hpp"
 
 namespace aikido {
 namespace planner {
diff --git a/include/aikido/robot.hpp b/include/aikido/robot.hpp
index 8b2b8329f3..196d6e22fd 100644
--- a/include/aikido/robot.hpp
+++ b/include/aikido/robot.hpp
@@ -1,7 +1,7 @@
-#include "robot/ConcreteManipulator.hpp"
-#include "robot/ConcreteRobot.hpp"
-#include "robot/GrabMetadata.hpp"
-#include "robot/Hand.hpp"
-#include "robot/Manipulator.hpp"
-#include "robot/Robot.hpp"
-#include "robot/util.hpp"
+#include "aikido/robot/ConcreteManipulator.hpp"
+#include "aikido/robot/ConcreteRobot.hpp"
+#include "aikido/robot/GrabMetadata.hpp"
+#include "aikido/robot/Hand.hpp"
+#include "aikido/robot/Manipulator.hpp"
+#include "aikido/robot/Robot.hpp"
+#include "aikido/robot/util.hpp"
diff --git a/include/aikido/robot/ConcreteRobot.hpp b/include/aikido/robot/ConcreteRobot.hpp
index 1d63e21659..ea6e50ab03 100644
--- a/include/aikido/robot/ConcreteRobot.hpp
+++ b/include/aikido/robot/ConcreteRobot.hpp
@@ -4,8 +4,10 @@
 #include <chrono>
 #include <string>
 #include <unordered_map>
+
 #include <Eigen/Core>
 #include <dart/dart.hpp>
+
 #include "aikido/common/ExecutorThread.hpp"
 #include "aikido/common/RNG.hpp"
 #include "aikido/constraint/dart/CollisionFree.hpp"
diff --git a/include/aikido/robot/Hand.hpp b/include/aikido/robot/Hand.hpp
index c4506b66df..32eed77b76 100644
--- a/include/aikido/robot/Hand.hpp
+++ b/include/aikido/robot/Hand.hpp
@@ -4,7 +4,9 @@
 #include <set>
 #include <string>
 #include <unordered_map>
+
 #include <dart/dart.hpp>
+
 #include "aikido/control/PositionCommandExecutor.hpp"
 #include "aikido/io/yaml.hpp"
 #include "aikido/robot/GrabMetadata.hpp"
diff --git a/include/aikido/robot/Robot.hpp b/include/aikido/robot/Robot.hpp
index 58ee093fd9..c97a08b2c9 100644
--- a/include/aikido/robot/Robot.hpp
+++ b/include/aikido/robot/Robot.hpp
@@ -3,8 +3,10 @@
 
 #include <chrono>
 #include <unordered_map>
+
 #include <Eigen/Core>
 #include <dart/dynamics/dynamics.hpp>
+
 #include "aikido/common/RNG.hpp"
 #include "aikido/constraint/dart/CollisionFree.hpp"
 #include "aikido/constraint/dart/TSR.hpp"
diff --git a/include/aikido/robot/util.hpp b/include/aikido/robot/util.hpp
index 8c31735e84..3634fe4dee 100644
--- a/include/aikido/robot/util.hpp
+++ b/include/aikido/robot/util.hpp
@@ -3,6 +3,7 @@
 
 #include <dart/dart.hpp>
 #include <dart/dynamics/dynamics.hpp>
+
 #include "aikido/common/ExecutorThread.hpp"
 #include "aikido/common/RNG.hpp"
 #include "aikido/constraint/Testable.hpp"
diff --git a/include/aikido/rviz.hpp b/include/aikido/rviz.hpp
index 2cf2abef2e..dc7ed9c5e1 100644
--- a/include/aikido/rviz.hpp
+++ b/include/aikido/rviz.hpp
@@ -1,10 +1,10 @@
-#include "rviz/BodyNodeMarker.hpp"
-#include "rviz/FrameMarker.hpp"
-#include "rviz/InteractiveMarkerViewer.hpp"
-#include "rviz/ResourceServer.hpp"
-#include "rviz/ShapeFrameMarker.hpp"
-#include "rviz/SkeletonMarker.hpp"
-#include "rviz/TSRMarker.hpp"
-#include "rviz/WorldInteractiveMarkerViewer.hpp"
-#include "rviz/pointers.hpp"
-#include "rviz/shape_conversions.hpp"
+#include "aikido/rviz/BodyNodeMarker.hpp"
+#include "aikido/rviz/FrameMarker.hpp"
+#include "aikido/rviz/InteractiveMarkerViewer.hpp"
+#include "aikido/rviz/ResourceServer.hpp"
+#include "aikido/rviz/ShapeFrameMarker.hpp"
+#include "aikido/rviz/SkeletonMarker.hpp"
+#include "aikido/rviz/TSRMarker.hpp"
+#include "aikido/rviz/WorldInteractiveMarkerViewer.hpp"
+#include "aikido/rviz/pointers.hpp"
+#include "aikido/rviz/shape_conversions.hpp"
diff --git a/include/aikido/rviz/BodyNodeMarker.hpp b/include/aikido/rviz/BodyNodeMarker.hpp
index 67b75bcfad..9bc8691f41 100644
--- a/include/aikido/rviz/BodyNodeMarker.hpp
+++ b/include/aikido/rviz/BodyNodeMarker.hpp
@@ -2,12 +2,14 @@
 #define AIKIDO_RVIZ_BODYNODEMARKER_HPP_
 
 #include <unordered_map>
+
 #include <dart/dynamics/dynamics.hpp>
 #include <interactive_markers/interactive_marker_server.h>
 #include <visualization_msgs/InteractiveMarker.h>
+
 #include "aikido/common/pointers.hpp"
-#include "ResourceServer.hpp"
-#include "ShapeFrameMarker.hpp"
+#include "aikido/rviz/ResourceServer.hpp"
+#include "aikido/rviz/ShapeFrameMarker.hpp"
 
 namespace aikido {
 namespace rviz {
diff --git a/include/aikido/rviz/FrameMarker.hpp b/include/aikido/rviz/FrameMarker.hpp
index 57c7bb791c..c52dd501f3 100644
--- a/include/aikido/rviz/FrameMarker.hpp
+++ b/include/aikido/rviz/FrameMarker.hpp
@@ -2,8 +2,10 @@
 #define AIKIDO_RVIZ_FRAMEMARKER_HPP_
 
 #include <memory>
+
 #include <interactive_markers/interactive_marker_server.h>
 #include <visualization_msgs/InteractiveMarker.h>
+
 #include "aikido/common/pointers.hpp"
 
 namespace dart {
diff --git a/include/aikido/rviz/InteractiveMarkerViewer.hpp b/include/aikido/rviz/InteractiveMarkerViewer.hpp
index ea23ddf521..81b10da00d 100644
--- a/include/aikido/rviz/InteractiveMarkerViewer.hpp
+++ b/include/aikido/rviz/InteractiveMarkerViewer.hpp
@@ -11,10 +11,10 @@
 #include <interactive_markers/interactive_marker_server.h>
 #include <ros/ros.h>
 
-#include <aikido/constraint/dart/TSR.hpp>
-#include <aikido/rviz/TSRMarker.hpp>
-#include <aikido/rviz/pointers.hpp>
-#include <aikido/trajectory/Trajectory.hpp>
+#include "aikido/constraint/dart/TSR.hpp"
+#include "aikido/rviz/TSRMarker.hpp"
+#include "aikido/rviz/pointers.hpp"
+#include "aikido/trajectory/Trajectory.hpp"
 
 namespace aikido {
 namespace rviz {
diff --git a/include/aikido/rviz/ResourceServer.hpp b/include/aikido/rviz/ResourceServer.hpp
index 956cbb1d75..38edb40998 100644
--- a/include/aikido/rviz/ResourceServer.hpp
+++ b/include/aikido/rviz/ResourceServer.hpp
@@ -3,6 +3,7 @@
 
 #include <mutex>
 #include <unordered_map>
+
 #include <dart/dynamics/dynamics.hpp>
 #include <microhttpd.h>
 
diff --git a/include/aikido/rviz/ShapeFrameMarker.hpp b/include/aikido/rviz/ShapeFrameMarker.hpp
index 18beab64b0..acbfb7b0ef 100644
--- a/include/aikido/rviz/ShapeFrameMarker.hpp
+++ b/include/aikido/rviz/ShapeFrameMarker.hpp
@@ -5,8 +5,9 @@
 #include <dart/dynamics/dynamics.hpp>
 #include <interactive_markers/interactive_marker_server.h>
 #include <visualization_msgs/InteractiveMarker.h>
+
 #include "aikido/common/pointers.hpp"
-#include "ResourceServer.hpp"
+#include "aikido/rviz/ResourceServer.hpp"
 
 namespace aikido {
 namespace rviz {
diff --git a/include/aikido/rviz/SkeletonMarker.hpp b/include/aikido/rviz/SkeletonMarker.hpp
index e256358025..938baa2d10 100644
--- a/include/aikido/rviz/SkeletonMarker.hpp
+++ b/include/aikido/rviz/SkeletonMarker.hpp
@@ -2,12 +2,14 @@
 #define AIKIDO_RVIZ_SKELETONMARKER_HPP_
 
 #include <unordered_map>
+
 #include <dart/dynamics/dynamics.hpp>
 #include <interactive_markers/interactive_marker_server.h>
 #include <visualization_msgs/InteractiveMarker.h>
+
 #include "aikido/common/pointers.hpp"
-#include "BodyNodeMarker.hpp"
-#include "ResourceServer.hpp"
+#include "aikido/rviz/BodyNodeMarker.hpp"
+#include "aikido/rviz/ResourceServer.hpp"
 
 namespace aikido {
 namespace rviz {
diff --git a/include/aikido/rviz/TSRMarker.hpp b/include/aikido/rviz/TSRMarker.hpp
index 63e20b399f..ddf34fb016 100644
--- a/include/aikido/rviz/TSRMarker.hpp
+++ b/include/aikido/rviz/TSRMarker.hpp
@@ -4,6 +4,7 @@
 #include <memory>
 
 #include <dart/dynamics/SimpleFrame.hpp>
+
 #include "aikido/common/pointers.hpp"
 
 namespace aikido {
diff --git a/include/aikido/rviz/TrajectoryMarker.hpp b/include/aikido/rviz/TrajectoryMarker.hpp
index 46cbdd1c5e..aab31529df 100644
--- a/include/aikido/rviz/TrajectoryMarker.hpp
+++ b/include/aikido/rviz/TrajectoryMarker.hpp
@@ -4,6 +4,7 @@
 #include <dart/dynamics/Frame.hpp>
 #include <interactive_markers/interactive_marker_server.h>
 #include <visualization_msgs/InteractiveMarker.h>
+
 #include "aikido/common/pointers.hpp"
 #include "aikido/trajectory/Trajectory.hpp"
 
diff --git a/include/aikido/rviz/WorldInteractiveMarkerViewer.hpp b/include/aikido/rviz/WorldInteractiveMarkerViewer.hpp
index 2872d80068..b269f938b2 100644
--- a/include/aikido/rviz/WorldInteractiveMarkerViewer.hpp
+++ b/include/aikido/rviz/WorldInteractiveMarkerViewer.hpp
@@ -2,9 +2,10 @@
 #define AIKIDO_RVIZ_WORLDINTERACTIVEMARKERVIEWER_HPP_
 
 #include <map>
-#include <aikido/planner/World.hpp>
-#include <aikido/rviz/InteractiveMarkerViewer.hpp>
-#include <aikido/rviz/pointers.hpp>
+
+#include "aikido/planner/World.hpp"
+#include "aikido/rviz/InteractiveMarkerViewer.hpp"
+#include "aikido/rviz/pointers.hpp"
 
 namespace aikido {
 namespace rviz {
diff --git a/include/aikido/rviz/shape_conversions.hpp b/include/aikido/rviz/shape_conversions.hpp
index c71bc6430b..d7a50de9b5 100644
--- a/include/aikido/rviz/shape_conversions.hpp
+++ b/include/aikido/rviz/shape_conversions.hpp
@@ -6,7 +6,8 @@
 #include <geometry_msgs/Vector3.h>
 #include <std_msgs/ColorRGBA.h>
 #include <visualization_msgs/Marker.h>
-#include "ResourceServer.hpp"
+
+#include "aikido/rviz/ResourceServer.hpp"
 
 struct aiMesh;
 
diff --git a/include/aikido/statespace.hpp b/include/aikido/statespace.hpp
index 2068fc85ef..5b496e59d9 100644
--- a/include/aikido/statespace.hpp
+++ b/include/aikido/statespace.hpp
@@ -1,21 +1,21 @@
-#include "statespace/CartesianProduct.hpp"
-#include "statespace/GeodesicInterpolator.hpp"
-#include "statespace/Interpolator.hpp"
-#include "statespace/Rn.hpp"
-#include "statespace/SE2.hpp"
-#include "statespace/SE3.hpp"
-#include "statespace/SO2.hpp"
-#include "statespace/SO3.hpp"
-#include "statespace/ScopedState.hpp"
-#include "statespace/StateHandle.hpp"
-#include "statespace/StateSpace.hpp"
-#include "statespace/dart/JointStateSpace.hpp"
-#include "statespace/dart/JointStateSpaceHelpers.hpp"
-#include "statespace/dart/MetaSkeletonStateSaver.hpp"
-#include "statespace/dart/MetaSkeletonStateSpace.hpp"
-#include "statespace/dart/RnJoint.hpp"
-#include "statespace/dart/SE2Joint.hpp"
-#include "statespace/dart/SE3Joint.hpp"
-#include "statespace/dart/SO2Joint.hpp"
-#include "statespace/dart/SO3Joint.hpp"
-#include "statespace/dart/WeldJoint.hpp"
+#include "aikido/statespace/CartesianProduct.hpp"
+#include "aikido/statespace/GeodesicInterpolator.hpp"
+#include "aikido/statespace/Interpolator.hpp"
+#include "aikido/statespace/Rn.hpp"
+#include "aikido/statespace/SE2.hpp"
+#include "aikido/statespace/SE3.hpp"
+#include "aikido/statespace/SO2.hpp"
+#include "aikido/statespace/SO3.hpp"
+#include "aikido/statespace/ScopedState.hpp"
+#include "aikido/statespace/StateHandle.hpp"
+#include "aikido/statespace/StateSpace.hpp"
+#include "aikido/statespace/dart/JointStateSpace.hpp"
+#include "aikido/statespace/dart/JointStateSpaceHelpers.hpp"
+#include "aikido/statespace/dart/MetaSkeletonStateSaver.hpp"
+#include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp"
+#include "aikido/statespace/dart/RnJoint.hpp"
+#include "aikido/statespace/dart/SE2Joint.hpp"
+#include "aikido/statespace/dart/SE3Joint.hpp"
+#include "aikido/statespace/dart/SO2Joint.hpp"
+#include "aikido/statespace/dart/SO3Joint.hpp"
+#include "aikido/statespace/dart/WeldJoint.hpp"
diff --git a/include/aikido/statespace/CartesianProduct.hpp b/include/aikido/statespace/CartesianProduct.hpp
index 3172fbf940..7899f4e813 100644
--- a/include/aikido/statespace/CartesianProduct.hpp
+++ b/include/aikido/statespace/CartesianProduct.hpp
@@ -1,8 +1,9 @@
 #ifndef AIKIDO_STATESPACE_COMPOUNDSTATESPACE_HPP_
 #define AIKIDO_STATESPACE_COMPOUNDSTATESPACE_HPP_
 #include <vector>
-#include "ScopedState.hpp"
-#include "StateSpace.hpp"
+
+#include "aikido/statespace/ScopedState.hpp"
+#include "aikido/statespace/StateSpace.hpp"
 
 namespace aikido {
 namespace statespace {
diff --git a/include/aikido/statespace/GeodesicInterpolator.hpp b/include/aikido/statespace/GeodesicInterpolator.hpp
index 133e754bb4..d1f60f7c12 100644
--- a/include/aikido/statespace/GeodesicInterpolator.hpp
+++ b/include/aikido/statespace/GeodesicInterpolator.hpp
@@ -1,7 +1,7 @@
 #ifndef AIKIDO_STATESPACE_GEODESICINTERPOLATOR_HPP_
 #define AIKIDO_STATESPACE_GEODESICINTERPOLATOR_HPP_
-#include "Interpolator.hpp"
-#include "StateSpace.hpp"
+#include "aikido/statespace/Interpolator.hpp"
+#include "aikido/statespace/StateSpace.hpp"
 
 namespace aikido {
 namespace statespace {
diff --git a/include/aikido/statespace/Interpolator.hpp b/include/aikido/statespace/Interpolator.hpp
index 3ca3fe9f3c..430f41069a 100644
--- a/include/aikido/statespace/Interpolator.hpp
+++ b/include/aikido/statespace/Interpolator.hpp
@@ -2,8 +2,9 @@
 #define AIKIDO_STATESPACE_INTERPOLATOR_HPP_
 
 #include <memory>
+
 #include "aikido/common/pointers.hpp"
-#include "../statespace/StateSpace.hpp"
+#include "aikido/statespace/StateSpace.hpp"
 
 namespace aikido {
 namespace statespace {
diff --git a/include/aikido/statespace/Rn.hpp b/include/aikido/statespace/Rn.hpp
index bfb6a5f8fa..af8dafeb31 100644
--- a/include/aikido/statespace/Rn.hpp
+++ b/include/aikido/statespace/Rn.hpp
@@ -1,8 +1,9 @@
 #ifndef AIKIDO_STATESPACE_REALVECTORSTATESPACE_HPP_
 #define AIKIDO_STATESPACE_REALVECTORSTATESPACE_HPP_
 #include <Eigen/Core>
-#include "ScopedState.hpp"
-#include "StateSpace.hpp"
+
+#include "aikido/statespace/ScopedState.hpp"
+#include "aikido/statespace/StateSpace.hpp"
 
 namespace aikido {
 namespace statespace {
diff --git a/include/aikido/statespace/SE2.hpp b/include/aikido/statespace/SE2.hpp
index ae5644cc9e..ca1c14abb3 100644
--- a/include/aikido/statespace/SE2.hpp
+++ b/include/aikido/statespace/SE2.hpp
@@ -1,8 +1,9 @@
 #ifndef AIKIDO_STATESPACE_SE2STATESPACE_HPP_
 #define AIKIDO_STATESPACE_SE2STATESPACE_HPP_
 #include <Eigen/Geometry>
-#include "ScopedState.hpp"
-#include "StateSpace.hpp"
+
+#include "aikido/statespace/ScopedState.hpp"
+#include "aikido/statespace/StateSpace.hpp"
 
 namespace aikido {
 namespace statespace {
diff --git a/include/aikido/statespace/SE3.hpp b/include/aikido/statespace/SE3.hpp
index d445601848..77ae59c554 100644
--- a/include/aikido/statespace/SE3.hpp
+++ b/include/aikido/statespace/SE3.hpp
@@ -1,8 +1,9 @@
 #ifndef AIKIDO_STATESPACE_SE3STATESPACE_HPP_
 #define AIKIDO_STATESPACE_SE3STATESPACE_HPP_
 #include <Eigen/Geometry>
-#include "ScopedState.hpp"
-#include "StateSpace.hpp"
+
+#include "aikido/statespace/ScopedState.hpp"
+#include "aikido/statespace/StateSpace.hpp"
 
 namespace aikido {
 namespace statespace {
diff --git a/include/aikido/statespace/SO2.hpp b/include/aikido/statespace/SO2.hpp
index bdf396652d..73c44ef637 100644
--- a/include/aikido/statespace/SO2.hpp
+++ b/include/aikido/statespace/SO2.hpp
@@ -2,8 +2,9 @@
 #define AIKIDO_STATESPACE_SO2STATESPACE_HPP_
 #include <Eigen/Core>
 #include <Eigen/Geometry>
-#include "ScopedState.hpp"
-#include "StateSpace.hpp"
+
+#include "aikido/statespace/ScopedState.hpp"
+#include "aikido/statespace/StateSpace.hpp"
 
 namespace aikido {
 namespace statespace {
diff --git a/include/aikido/statespace/SO3.hpp b/include/aikido/statespace/SO3.hpp
index 4c3490830a..ed89772958 100644
--- a/include/aikido/statespace/SO3.hpp
+++ b/include/aikido/statespace/SO3.hpp
@@ -1,8 +1,9 @@
 #ifndef AIKIDO_STATESPACE_SO3STATESPACE_HPP_
 #define AIKIDO_STATESPACE_SO3STATESPACE_HPP_
 #include <Eigen/Geometry>
-#include "ScopedState.hpp"
-#include "StateSpace.hpp"
+
+#include "aikido/statespace/ScopedState.hpp"
+#include "aikido/statespace/StateSpace.hpp"
 
 namespace aikido {
 namespace statespace {
diff --git a/include/aikido/statespace/ScopedState.hpp b/include/aikido/statespace/ScopedState.hpp
index 39b8d1512e..83f9940c51 100644
--- a/include/aikido/statespace/ScopedState.hpp
+++ b/include/aikido/statespace/ScopedState.hpp
@@ -1,7 +1,8 @@
 #ifndef AIKIDO_STATESPACE_SCOPEDSTATE_HPP_
 #define AIKIDO_STATESPACE_SCOPEDSTATE_HPP_
 #include <memory>
-#include "StateHandle.hpp"
+
+#include "aikido/statespace/StateHandle.hpp"
 
 namespace aikido {
 namespace statespace {
diff --git a/include/aikido/statespace/StateSpace.hpp b/include/aikido/statespace/StateSpace.hpp
index f8c0b5024d..e4357b5127 100644
--- a/include/aikido/statespace/StateSpace.hpp
+++ b/include/aikido/statespace/StateSpace.hpp
@@ -2,7 +2,9 @@
 #define AIKIDO_STATESPACE_STATESPACE_HPP_
 
 #include <memory>
+
 #include <Eigen/Dense>
+
 #include "aikido/common/RNG.hpp"
 #include "aikido/common/pointers.hpp"
 #include "aikido/statespace/ScopedState.hpp"
diff --git a/include/aikido/statespace/dart/JointStateSpace.hpp b/include/aikido/statespace/dart/JointStateSpace.hpp
index 3cdb9c9059..22b7400af8 100644
--- a/include/aikido/statespace/dart/JointStateSpace.hpp
+++ b/include/aikido/statespace/dart/JointStateSpace.hpp
@@ -2,6 +2,7 @@
 #define AIKIDO_STATESPACE_DART_JOINTSTATESPACE_HPP_
 
 #include <dart/dynamics/dynamics.hpp>
+
 #include "aikido/statespace/StateSpace.hpp"
 
 namespace aikido {
diff --git a/include/aikido/statespace/dart/JointStateSpaceHelpers.hpp b/include/aikido/statespace/dart/JointStateSpaceHelpers.hpp
index 5b9a58fc80..4220a54644 100644
--- a/include/aikido/statespace/dart/JointStateSpaceHelpers.hpp
+++ b/include/aikido/statespace/dart/JointStateSpaceHelpers.hpp
@@ -2,7 +2,9 @@
 #define AIKIDO_STATESPACE_DART_JOINTSTATESPACEHELPERS_HPP_
 
 #include <memory>
+
 #include <dart/dynamics/dynamics.hpp>
+
 #include "aikido/statespace/dart/JointStateSpace.hpp"
 
 namespace aikido {
diff --git a/include/aikido/statespace/dart/MetaSkeletonStateSpace.hpp b/include/aikido/statespace/dart/MetaSkeletonStateSpace.hpp
index 67cbf2226d..1ef78bea9d 100644
--- a/include/aikido/statespace/dart/MetaSkeletonStateSpace.hpp
+++ b/include/aikido/statespace/dart/MetaSkeletonStateSpace.hpp
@@ -2,7 +2,9 @@
 #define AIKIDO_STATESPACE_DART_METASKELETONSTATESPACE_HPP_
 
 #include <unordered_map>
+
 #include <dart/dynamics/dynamics.hpp>
+
 #include "aikido/common/pair.hpp"
 #include "aikido/common/pointers.hpp"
 #include "aikido/statespace/CartesianProduct.hpp"
diff --git a/include/aikido/statespace/dart/SE2Joint.hpp b/include/aikido/statespace/dart/SE2Joint.hpp
index 0b69911d47..101738eb82 100644
--- a/include/aikido/statespace/dart/SE2Joint.hpp
+++ b/include/aikido/statespace/dart/SE2Joint.hpp
@@ -1,7 +1,8 @@
 #ifndef AIKIDO_STATESPACE_DART_SE2JOINTSTATESPACE_HPP_
 #define AIKIDO_STATESPACE_DART_SE2JOINTSTATESPACE_HPP_
-#include "../SE2.hpp"
-#include "JointStateSpace.hpp"
+
+#include "aikido/statespace/SE2.hpp"
+#include "aikido/statespace/dart/JointStateSpace.hpp"
 
 namespace aikido {
 namespace statespace {
diff --git a/include/aikido/statespace/dart/detail/JointStateSpaceHelpers-impl.hpp b/include/aikido/statespace/dart/detail/JointStateSpaceHelpers-impl.hpp
index 82f2c252a5..9d19d2889b 100644
--- a/include/aikido/statespace/dart/detail/JointStateSpaceHelpers-impl.hpp
+++ b/include/aikido/statespace/dart/detail/JointStateSpaceHelpers-impl.hpp
@@ -1,12 +1,13 @@
 #include <memory>
+
 #include "aikido/common/memory.hpp"
-#include "../../../common/metaprogramming.hpp"
-#include "../RnJoint.hpp"
-#include "../SE2Joint.hpp"
-#include "../SE3Joint.hpp"
-#include "../SO2Joint.hpp"
-#include "../SO3Joint.hpp"
-#include "../WeldJoint.hpp"
+#include "aikido/common/metaprogramming.hpp"
+#include "aikido/statespace/dart/RnJoint.hpp"
+#include "aikido/statespace/dart/SE2Joint.hpp"
+#include "aikido/statespace/dart/SE3Joint.hpp"
+#include "aikido/statespace/dart/SO2Joint.hpp"
+#include "aikido/statespace/dart/SO3Joint.hpp"
+#include "aikido/statespace/dart/WeldJoint.hpp"
 
 namespace aikido {
 namespace statespace {
diff --git a/include/aikido/statespace/dart/detail/RnJoint-impl.hpp b/include/aikido/statespace/dart/detail/RnJoint-impl.hpp
index 394b60b1f6..fe9e206445 100644
--- a/include/aikido/statespace/dart/detail/RnJoint-impl.hpp
+++ b/include/aikido/statespace/dart/detail/RnJoint-impl.hpp
@@ -1,4 +1,4 @@
-#include <aikido/statespace/dart/RnJoint.hpp>
+#include "aikido/statespace/dart/RnJoint.hpp"
 
 namespace aikido {
 namespace statespace {
diff --git a/include/aikido/statespace/detail/CartesianProduct-impl.hpp b/include/aikido/statespace/detail/CartesianProduct-impl.hpp
index 1af7a73b41..4d4905c3da 100644
--- a/include/aikido/statespace/detail/CartesianProduct-impl.hpp
+++ b/include/aikido/statespace/detail/CartesianProduct-impl.hpp
@@ -1,7 +1,7 @@
-#include "aikido/statespace/CartesianProduct.hpp"
-
 #include <sstream>
 
+#include "aikido/statespace/CartesianProduct.hpp"
+
 namespace aikido {
 namespace statespace {
 
diff --git a/include/aikido/statespace/detail/Rn-impl.hpp b/include/aikido/statespace/detail/Rn-impl.hpp
index 2c259d11a3..936bd76ca9 100644
--- a/include/aikido/statespace/detail/Rn-impl.hpp
+++ b/include/aikido/statespace/detail/Rn-impl.hpp
@@ -1,7 +1,7 @@
-#include "aikido/statespace/Rn.hpp"
-
 #include <type_traits>
 
+#include "aikido/statespace/Rn.hpp"
+
 namespace aikido {
 namespace statespace {
 
diff --git a/include/aikido/trajectory.hpp b/include/aikido/trajectory.hpp
index 9724bbfa46..95742c819c 100644
--- a/include/aikido/trajectory.hpp
+++ b/include/aikido/trajectory.hpp
@@ -1,3 +1,3 @@
-#include "trajectory/Interpolated.hpp"
-#include "trajectory/Spline.hpp"
-#include "trajectory/Trajectory.hpp"
+#include "aikido/trajectory/Interpolated.hpp"
+#include "aikido/trajectory/Spline.hpp"
+#include "aikido/trajectory/Trajectory.hpp"
diff --git a/include/aikido/trajectory/Interpolated.hpp b/include/aikido/trajectory/Interpolated.hpp
index 367a071aad..e22eab1271 100644
--- a/include/aikido/trajectory/Interpolated.hpp
+++ b/include/aikido/trajectory/Interpolated.hpp
@@ -2,8 +2,8 @@
 #define AIKIDO_TRAJECTORY_PIECEWISELINEAR_TRAJECTORY_HPP_
 
 #include "aikido/common/pointers.hpp"
-#include "../statespace/GeodesicInterpolator.hpp"
-#include "Trajectory.hpp"
+#include "aikido/statespace/GeodesicInterpolator.hpp"
+#include "aikido/trajectory/Trajectory.hpp"
 
 namespace aikido {
 namespace trajectory {
diff --git a/include/aikido/trajectory/Spline.hpp b/include/aikido/trajectory/Spline.hpp
index bd9eaa90ca..ae0556dde2 100644
--- a/include/aikido/trajectory/Spline.hpp
+++ b/include/aikido/trajectory/Spline.hpp
@@ -2,7 +2,7 @@
 #define AIKIDO_TRAJECTORY_SPLINETRAJECTORY2_HPP_
 
 #include "aikido/common/pointers.hpp"
-#include "Trajectory.hpp"
+#include "aikido/trajectory/Trajectory.hpp"
 
 namespace aikido {
 namespace trajectory {
diff --git a/include/aikido/trajectory/Trajectory.hpp b/include/aikido/trajectory/Trajectory.hpp
index 6611461adf..183682be91 100644
--- a/include/aikido/trajectory/Trajectory.hpp
+++ b/include/aikido/trajectory/Trajectory.hpp
@@ -2,9 +2,10 @@
 #define AIKIDO_TRAJECTORY_TRAJECTORY_HPP_
 
 #include <Eigen/Core>
+
 #include "aikido/common/pointers.hpp"
-#include <aikido/trajectory/TrajectoryMetadata.hpp>
-#include "../statespace/StateSpace.hpp"
+#include "aikido/statespace/StateSpace.hpp"
+#include "aikido/trajectory/TrajectoryMetadata.hpp"
 
 namespace aikido {
 namespace trajectory {
diff --git a/src/common/ExecutorMultiplexer.cpp b/src/common/ExecutorMultiplexer.cpp
index 28534fa81a..17a2b86f34 100644
--- a/src/common/ExecutorMultiplexer.cpp
+++ b/src/common/ExecutorMultiplexer.cpp
@@ -1,4 +1,4 @@
-#include <aikido/common/ExecutorMultiplexer.hpp>
+#include "aikido/common/ExecutorMultiplexer.hpp"
 
 #include <dart/dart.hpp>
 
diff --git a/src/common/ExecutorThread.cpp b/src/common/ExecutorThread.cpp
index 59f66c47e8..c07d4258bf 100644
--- a/src/common/ExecutorThread.cpp
+++ b/src/common/ExecutorThread.cpp
@@ -1,4 +1,4 @@
-#include <aikido/common/ExecutorThread.hpp>
+#include "aikido/common/ExecutorThread.hpp"
 
 #include <iostream>
 
diff --git a/src/common/PseudoInverse.cpp b/src/common/PseudoInverse.cpp
index 1f33f164db..37f6910335 100644
--- a/src/common/PseudoInverse.cpp
+++ b/src/common/PseudoInverse.cpp
@@ -1,8 +1,9 @@
-#include <aikido/common/PseudoInverse.hpp>
+#include "aikido/common/PseudoInverse.hpp"
 
 #include <iostream>
 #include <limits>
 #include <memory>
+
 #include <Eigen/Dense>
 
 namespace aikido {
diff --git a/src/common/RNG.cpp b/src/common/RNG.cpp
index ad5d2e7f67..8691ece151 100644
--- a/src/common/RNG.cpp
+++ b/src/common/RNG.cpp
@@ -1,4 +1,4 @@
-#include <aikido/common/RNG.hpp>
+#include "aikido/common/RNG.hpp"
 
 namespace aikido {
 namespace common {
diff --git a/src/common/StepSequence.cpp b/src/common/StepSequence.cpp
index 7dfe5254e4..e8d99b2680 100644
--- a/src/common/StepSequence.cpp
+++ b/src/common/StepSequence.cpp
@@ -1,4 +1,4 @@
-#include <aikido/common/StepSequence.hpp>
+#include "aikido/common/StepSequence.hpp"
 
 #include <cmath>
 #include <iostream>
diff --git a/src/common/VanDerCorput.cpp b/src/common/VanDerCorput.cpp
index 715aec4887..f5a40e2f64 100644
--- a/src/common/VanDerCorput.cpp
+++ b/src/common/VanDerCorput.cpp
@@ -1,4 +1,4 @@
-#include <aikido/common/VanDerCorput.hpp>
+#include "aikido/common/VanDerCorput.hpp"
 
 #include <algorithm>
 #include <cassert>
diff --git a/src/common/stream.cpp b/src/common/stream.cpp
index b060aa153d..cf133f5fff 100644
--- a/src/common/stream.cpp
+++ b/src/common/stream.cpp
@@ -1,4 +1,4 @@
-#include <aikido/common/stream.hpp>
+#include "aikido/common/stream.hpp"
 
 namespace aikido {
 namespace common {
diff --git a/src/common/string.cpp b/src/common/string.cpp
index ad40576cad..e74ceafd04 100644
--- a/src/common/string.cpp
+++ b/src/common/string.cpp
@@ -1,4 +1,4 @@
-#include <aikido/common/string.hpp>
+#include "aikido/common/string.hpp"
 
 namespace aikido {
 namespace common {
diff --git a/src/constraint/CartesianProductProjectable.cpp b/src/constraint/CartesianProductProjectable.cpp
index 7338bee5cd..c7b9c44042 100644
--- a/src/constraint/CartesianProductProjectable.cpp
+++ b/src/constraint/CartesianProductProjectable.cpp
@@ -1,5 +1,6 @@
+#include "aikido/constraint/CartesianProductProjectable.hpp"
+
 #include <sstream>
-#include <aikido/constraint/CartesianProductProjectable.hpp>
 
 namespace aikido {
 namespace constraint {
diff --git a/src/constraint/CartesianProductSampleable.cpp b/src/constraint/CartesianProductSampleable.cpp
index f5119904bf..2bf1958eb7 100644
--- a/src/constraint/CartesianProductSampleable.cpp
+++ b/src/constraint/CartesianProductSampleable.cpp
@@ -1,5 +1,6 @@
+#include "aikido/constraint/CartesianProductSampleable.hpp"
+
 #include "aikido/common/memory.hpp"
-#include <aikido/constraint/CartesianProductSampleable.hpp>
 
 namespace aikido {
 namespace constraint {
diff --git a/src/constraint/CartesianProductTestable.cpp b/src/constraint/CartesianProductTestable.cpp
index f554984ba0..7e91f70c2b 100644
--- a/src/constraint/CartesianProductTestable.cpp
+++ b/src/constraint/CartesianProductTestable.cpp
@@ -1,5 +1,6 @@
+#include "aikido/constraint/CartesianProductTestable.hpp"
+
 #include "aikido/common/memory.hpp"
-#include <aikido/constraint/CartesianProductTestable.hpp>
 
 namespace aikido {
 namespace constraint {
diff --git a/src/constraint/CyclicSampleable.cpp b/src/constraint/CyclicSampleable.cpp
index 5336debd26..336a5e087e 100644
--- a/src/constraint/CyclicSampleable.cpp
+++ b/src/constraint/CyclicSampleable.cpp
@@ -1,4 +1,4 @@
-#include <aikido/constraint/CyclicSampleable.hpp>
+#include "aikido/constraint/CyclicSampleable.hpp"
 
 #include "aikido/common/memory.hpp"
 
diff --git a/src/constraint/DefaultTestableOutcome.cpp b/src/constraint/DefaultTestableOutcome.cpp
index e7973b06cb..a59b3a6123 100644
--- a/src/constraint/DefaultTestableOutcome.cpp
+++ b/src/constraint/DefaultTestableOutcome.cpp
@@ -1,4 +1,4 @@
-#include <aikido/constraint/DefaultTestableOutcome.hpp>
+#include "aikido/constraint/DefaultTestableOutcome.hpp"
 
 namespace aikido {
 namespace constraint {
diff --git a/src/constraint/Differentiable.cpp b/src/constraint/Differentiable.cpp
index 28212676ed..9089cac069 100644
--- a/src/constraint/Differentiable.cpp
+++ b/src/constraint/Differentiable.cpp
@@ -1,4 +1,4 @@
-#include <aikido/constraint/Differentiable.hpp>
+#include "aikido/constraint/Differentiable.hpp"
 
 namespace aikido {
 namespace constraint {
diff --git a/src/constraint/DifferentiableIntersection.cpp b/src/constraint/DifferentiableIntersection.cpp
index 60db36d55f..57469ae56f 100644
--- a/src/constraint/DifferentiableIntersection.cpp
+++ b/src/constraint/DifferentiableIntersection.cpp
@@ -1,4 +1,4 @@
-#include <aikido/constraint/DifferentiableIntersection.hpp>
+#include "aikido/constraint/DifferentiableIntersection.hpp"
 
 namespace aikido {
 namespace constraint {
diff --git a/src/constraint/DifferentiableSubspace.cpp b/src/constraint/DifferentiableSubspace.cpp
index f569ab36ee..d0de3fb5ca 100644
--- a/src/constraint/DifferentiableSubspace.cpp
+++ b/src/constraint/DifferentiableSubspace.cpp
@@ -1,5 +1,6 @@
+#include "aikido/constraint/DifferentiableSubspace.hpp"
+
 #include <sstream>
-#include <aikido/constraint/DifferentiableSubspace.hpp>
 
 namespace aikido {
 namespace constraint {
diff --git a/src/constraint/FiniteSampleable.cpp b/src/constraint/FiniteSampleable.cpp
index 8d6ca8f38b..a35e8d90b8 100644
--- a/src/constraint/FiniteSampleable.cpp
+++ b/src/constraint/FiniteSampleable.cpp
@@ -1,4 +1,4 @@
-#include <aikido/constraint/FiniteSampleable.hpp>
+#include "aikido/constraint/FiniteSampleable.hpp"
 
 #include "aikido/common/memory.hpp"
 
diff --git a/src/constraint/NewtonsMethodProjectable.cpp b/src/constraint/NewtonsMethodProjectable.cpp
index ed88658225..f268ac51b6 100644
--- a/src/constraint/NewtonsMethodProjectable.cpp
+++ b/src/constraint/NewtonsMethodProjectable.cpp
@@ -1,10 +1,12 @@
-#include <aikido/constraint/NewtonsMethodProjectable.hpp>
+#include "aikido/constraint/NewtonsMethodProjectable.hpp"
 
 #include <cmath>
 #include <iostream>
 #include <limits>
+
 #include <dart/math/Geometry.hpp>
-#include <aikido/common/PseudoInverse.hpp>
+
+#include "aikido/common/PseudoInverse.hpp"
 
 namespace aikido {
 namespace constraint {
diff --git a/src/constraint/Projectable.cpp b/src/constraint/Projectable.cpp
index 8cac5ae151..cdeeb71f0e 100644
--- a/src/constraint/Projectable.cpp
+++ b/src/constraint/Projectable.cpp
@@ -1,4 +1,4 @@
-#include <aikido/constraint/Projectable.hpp>
+#include "aikido/constraint/Projectable.hpp"
 
 namespace aikido {
 namespace constraint {
diff --git a/src/constraint/RejectionSampleable.cpp b/src/constraint/RejectionSampleable.cpp
index d7d4815006..67d0196509 100644
--- a/src/constraint/RejectionSampleable.cpp
+++ b/src/constraint/RejectionSampleable.cpp
@@ -1,5 +1,6 @@
+#include "aikido/constraint/RejectionSampleable.hpp"
+
 #include "aikido/common/memory.hpp"
-#include <aikido/constraint/RejectionSampleable.hpp>
 
 namespace aikido {
 namespace constraint {
diff --git a/src/constraint/Sampleable.cpp b/src/constraint/Sampleable.cpp
index 49577dfbbf..0a70207216 100644
--- a/src/constraint/Sampleable.cpp
+++ b/src/constraint/Sampleable.cpp
@@ -1,4 +1,4 @@
-#include <aikido/constraint/Sampleable.hpp>
+#include "aikido/constraint/Sampleable.hpp"
 
 namespace aikido {
 namespace constraint {
diff --git a/src/constraint/Satisfied.cpp b/src/constraint/Satisfied.cpp
index 7384c8628e..394fb284c6 100644
--- a/src/constraint/Satisfied.cpp
+++ b/src/constraint/Satisfied.cpp
@@ -1,4 +1,4 @@
-#include <aikido/constraint/Satisfied.hpp>
+#include "aikido/constraint/Satisfied.hpp"
 
 namespace aikido {
 namespace constraint {
diff --git a/src/constraint/SequentialSampleable.cpp b/src/constraint/SequentialSampleable.cpp
index 5970ffac62..716ec9011a 100644
--- a/src/constraint/SequentialSampleable.cpp
+++ b/src/constraint/SequentialSampleable.cpp
@@ -1,6 +1,7 @@
 #include "aikido/constraint/SequentialSampleable.hpp"
 
 #include <dart/common/Console.hpp>
+
 #include "aikido/common/memory.hpp"
 
 namespace aikido {
diff --git a/src/constraint/TestableIntersection.cpp b/src/constraint/TestableIntersection.cpp
index 60282b14d0..3a1f967862 100644
--- a/src/constraint/TestableIntersection.cpp
+++ b/src/constraint/TestableIntersection.cpp
@@ -1,4 +1,4 @@
-#include <aikido/constraint/TestableIntersection.hpp>
+#include "aikido/constraint/TestableIntersection.hpp"
 
 #include <stdexcept>
 
diff --git a/src/constraint/dart/CollisionFreeOutcome.cpp b/src/constraint/dart/CollisionFreeOutcome.cpp
index 178178c3da..b1bab692ee 100644
--- a/src/constraint/dart/CollisionFreeOutcome.cpp
+++ b/src/constraint/dart/CollisionFreeOutcome.cpp
@@ -1,4 +1,5 @@
 #include "aikido/constraint/dart/CollisionFreeOutcome.hpp"
+
 #include <sstream>
 
 namespace aikido {
diff --git a/src/constraint/dart/FrameDifferentiable.cpp b/src/constraint/dart/FrameDifferentiable.cpp
index 4fa7d00efe..71a69c126b 100644
--- a/src/constraint/dart/FrameDifferentiable.cpp
+++ b/src/constraint/dart/FrameDifferentiable.cpp
@@ -1,4 +1,5 @@
 #include "aikido/constraint/dart/FrameDifferentiable.hpp"
+
 #include "aikido/statespace/SE3.hpp"
 
 namespace aikido {
diff --git a/src/constraint/dart/FramePairDifferentiable.cpp b/src/constraint/dart/FramePairDifferentiable.cpp
index 84c0238ec8..81612023dd 100644
--- a/src/constraint/dart/FramePairDifferentiable.cpp
+++ b/src/constraint/dart/FramePairDifferentiable.cpp
@@ -1,4 +1,5 @@
 #include "aikido/constraint/dart/FramePairDifferentiable.hpp"
+
 #include "aikido/statespace/SE3.hpp"
 
 namespace aikido {
diff --git a/src/constraint/dart/InverseKinematicsSampleable.cpp b/src/constraint/dart/InverseKinematicsSampleable.cpp
index a3c19637b5..7feac83432 100644
--- a/src/constraint/dart/InverseKinematicsSampleable.cpp
+++ b/src/constraint/dart/InverseKinematicsSampleable.cpp
@@ -1,4 +1,5 @@
 #include "aikido/constraint/dart/InverseKinematicsSampleable.hpp"
+
 #include "aikido/statespace/SE3.hpp"
 
 namespace aikido {
diff --git a/src/constraint/dart/JointStateSpaceHelpers.cpp b/src/constraint/dart/JointStateSpaceHelpers.cpp
index c3e601060a..34bdf15ba7 100644
--- a/src/constraint/dart/JointStateSpaceHelpers.cpp
+++ b/src/constraint/dart/JointStateSpaceHelpers.cpp
@@ -1,4 +1,5 @@
 #include "aikido/constraint/dart/JointStateSpaceHelpers.hpp"
+
 #include "aikido/common/memory.hpp"
 #include "aikido/constraint/CartesianProductProjectable.hpp"
 #include "aikido/constraint/CartesianProductSampleable.hpp"
diff --git a/src/constraint/dart/TSR.cpp b/src/constraint/dart/TSR.cpp
index 72bf495a79..c6c31a6066 100644
--- a/src/constraint/dart/TSR.cpp
+++ b/src/constraint/dart/TSR.cpp
@@ -1,11 +1,14 @@
 #include "aikido/constraint/dart/TSR.hpp"
+
 #include <cmath>
 #include <random>
 #include <stdexcept>
 #include <vector>
+
 #include <boost/format.hpp>
 #include <dart/common/Console.hpp>
 #include <dart/math/Geometry.hpp>
+
 #include "aikido/common/memory.hpp"
 
 using aikido::statespace::SE3;
diff --git a/src/constraint/uniform/SE2BoxConstraint.cpp b/src/constraint/uniform/SE2BoxConstraint.cpp
index 8cf29fe177..bd1eff0afe 100644
--- a/src/constraint/uniform/SE2BoxConstraint.cpp
+++ b/src/constraint/uniform/SE2BoxConstraint.cpp
@@ -1,5 +1,7 @@
 #include "aikido/constraint/uniform/SE2BoxConstraint.hpp"
+
 #include <stdexcept>
+
 #include "aikido/constraint/uniform/SO2UniformSampler.hpp"
 #include "aikido/statespace/SO2.hpp"
 
diff --git a/src/constraint/uniform/SO2UniformSampler.cpp b/src/constraint/uniform/SO2UniformSampler.cpp
index f24f4c6c3d..be999f5ec7 100644
--- a/src/constraint/uniform/SO2UniformSampler.cpp
+++ b/src/constraint/uniform/SO2UniformSampler.cpp
@@ -1,4 +1,5 @@
 #include "aikido/constraint/uniform/SO2UniformSampler.hpp"
+
 #include <cmath>
 
 namespace aikido {
diff --git a/src/constraint/uniform/SO3UniformSampler.cpp b/src/constraint/uniform/SO3UniformSampler.cpp
index e9a5888d22..2951338c59 100644
--- a/src/constraint/uniform/SO3UniformSampler.cpp
+++ b/src/constraint/uniform/SO3UniformSampler.cpp
@@ -1,4 +1,5 @@
 #include "aikido/constraint/uniform/SO3UniformSampler.hpp"
+
 #include <cmath>
 
 namespace aikido {
diff --git a/src/control/InstantaneousTrajectoryExecutor.cpp b/src/control/InstantaneousTrajectoryExecutor.cpp
index db87095094..10c45a50ef 100644
--- a/src/control/InstantaneousTrajectoryExecutor.cpp
+++ b/src/control/InstantaneousTrajectoryExecutor.cpp
@@ -1,4 +1,5 @@
 #include "aikido/control/InstantaneousTrajectoryExecutor.hpp"
+
 #include "aikido/control/TrajectoryRunningException.hpp"
 #include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp"
 
diff --git a/src/control/KinematicSimulationTrajectoryExecutor.cpp b/src/control/KinematicSimulationTrajectoryExecutor.cpp
index acd24c604e..a4357c305b 100644
--- a/src/control/KinematicSimulationTrajectoryExecutor.cpp
+++ b/src/control/KinematicSimulationTrajectoryExecutor.cpp
@@ -1,5 +1,7 @@
 #include "aikido/control/KinematicSimulationTrajectoryExecutor.hpp"
+
 #include <dart/common/Console.hpp>
+
 #include "aikido/common/memory.hpp"
 #include "aikido/control/TrajectoryRunningException.hpp"
 
diff --git a/src/control/QueuedTrajectoryExecutor.cpp b/src/control/QueuedTrajectoryExecutor.cpp
index cbc71fb984..c07c541c36 100644
--- a/src/control/QueuedTrajectoryExecutor.cpp
+++ b/src/control/QueuedTrajectoryExecutor.cpp
@@ -1,4 +1,5 @@
 #include "aikido/control/QueuedTrajectoryExecutor.hpp"
+
 #include <chrono>
 
 namespace aikido {
diff --git a/src/control/ros/Conversions.cpp b/src/control/ros/Conversions.cpp
index 467b76bea2..314a856cbc 100644
--- a/src/control/ros/Conversions.cpp
+++ b/src/control/ros/Conversions.cpp
@@ -2,7 +2,9 @@
 
 #include <sstream>
 #include <unordered_set>
+
 #include <dart/dynamics/Joint.hpp>
+
 #include "aikido/common/Spline.hpp"
 #include "aikido/common/StepSequence.hpp"
 #include "aikido/control/ros/Conversions.hpp"
@@ -518,8 +520,8 @@ trajectory_msgs::JointTrajectory toRosJointTrajectory(
     const aikido::trajectory::ConstTrajectoryPtr& trajectory, double timestep)
 {
   using statespace::dart::MetaSkeletonStateSpace;
-  using statespace::dart::SO2Joint;
   using statespace::dart::R1Joint;
+  using statespace::dart::SO2Joint;
 
   if (!trajectory)
     throw std::invalid_argument("Trajectory is null.");
diff --git a/src/control/ros/RosJointStateClient.cpp b/src/control/ros/RosJointStateClient.cpp
index 0e9bf9e9de..068bfde0a3 100644
--- a/src/control/ros/RosJointStateClient.cpp
+++ b/src/control/ros/RosJointStateClient.cpp
@@ -1,4 +1,4 @@
-#include <aikido/control/ros/RosJointStateClient.hpp>
+#include "aikido/control/ros/RosJointStateClient.hpp"
 
 namespace aikido {
 namespace control {
diff --git a/src/control/ros/RosPositionCommandExecutor.cpp b/src/control/ros/RosPositionCommandExecutor.cpp
index a97f6c707d..5571ad64ed 100644
--- a/src/control/ros/RosPositionCommandExecutor.cpp
+++ b/src/control/ros/RosPositionCommandExecutor.cpp
@@ -1,4 +1,5 @@
 #include "aikido/control/ros/RosPositionCommandExecutor.hpp"
+
 #include "aikido/control/ros/Conversions.hpp"
 #include "aikido/control/ros/util.hpp"
 
diff --git a/src/control/ros/RosTrajectoryExecutor.cpp b/src/control/ros/RosTrajectoryExecutor.cpp
index 9080c074d0..4e2ce2dda0 100644
--- a/src/control/ros/RosTrajectoryExecutor.cpp
+++ b/src/control/ros/RosTrajectoryExecutor.cpp
@@ -1,5 +1,7 @@
 #include "aikido/control/ros/RosTrajectoryExecutor.hpp"
+
 #include <dart/common/Console.hpp>
+
 #include "aikido/control/TrajectoryRunningException.hpp"
 #include "aikido/control/ros/Conversions.hpp"
 #include "aikido/control/ros/RosTrajectoryExecutionException.hpp"
diff --git a/src/distance/CartesianProductWeighted.cpp b/src/distance/CartesianProductWeighted.cpp
index f9e9be4269..b622a2e1c3 100644
--- a/src/distance/CartesianProductWeighted.cpp
+++ b/src/distance/CartesianProductWeighted.cpp
@@ -1,4 +1,4 @@
-#include <aikido/distance/CartesianProductWeighted.hpp>
+#include "aikido/distance/CartesianProductWeighted.hpp"
 
 namespace aikido {
 namespace distance {
diff --git a/src/distance/ConfigurationRanker.cpp b/src/distance/ConfigurationRanker.cpp
index 014e17c951..44ff0a02d4 100644
--- a/src/distance/ConfigurationRanker.cpp
+++ b/src/distance/ConfigurationRanker.cpp
@@ -1,7 +1,7 @@
-#include "aikido/common/memory.hpp"
-
 #include "aikido/distance/ConfigurationRanker.hpp"
 
+#include "aikido/common/memory.hpp"
+
 namespace aikido {
 namespace distance {
 
diff --git a/src/distance/RnEuclidean.cpp b/src/distance/RnEuclidean.cpp
index f54e842e6f..1b881f0adf 100644
--- a/src/distance/RnEuclidean.cpp
+++ b/src/distance/RnEuclidean.cpp
@@ -1,4 +1,4 @@
-#include <aikido/distance/RnEuclidean.hpp>
+#include "aikido/distance/RnEuclidean.hpp"
 
 namespace aikido {
 namespace distance {
diff --git a/src/distance/SE2.cpp b/src/distance/SE2.cpp
index e2c47eb735..b7b67ac87c 100644
--- a/src/distance/SE2.cpp
+++ b/src/distance/SE2.cpp
@@ -1,5 +1,6 @@
+#include "aikido/distance/SE2.hpp"
+
 #include <boost/math/constants/constants.hpp>
-#include <aikido/distance/SE2.hpp>
 
 namespace aikido {
 namespace distance {
diff --git a/src/distance/SE2Weighted.cpp b/src/distance/SE2Weighted.cpp
index dcbd9dca02..012d9369c4 100644
--- a/src/distance/SE2Weighted.cpp
+++ b/src/distance/SE2Weighted.cpp
@@ -1,5 +1,6 @@
+#include "aikido/distance/SE2Weighted.hpp"
+
 #include <boost/math/constants/constants.hpp>
-#include <aikido/distance/SE2Weighted.hpp>
 
 namespace aikido {
 namespace distance {
diff --git a/src/distance/SO2Angular.cpp b/src/distance/SO2Angular.cpp
index a0704a48ef..fd7b3b8a3e 100644
--- a/src/distance/SO2Angular.cpp
+++ b/src/distance/SO2Angular.cpp
@@ -1,4 +1,4 @@
-#include <aikido/distance/SO2Angular.hpp>
+#include "aikido/distance/SO2Angular.hpp"
 
 namespace aikido {
 namespace distance {
diff --git a/src/distance/SO3Angular.cpp b/src/distance/SO3Angular.cpp
index 366687e19c..164ac2e12e 100644
--- a/src/distance/SO3Angular.cpp
+++ b/src/distance/SO3Angular.cpp
@@ -1,4 +1,4 @@
-#include <aikido/distance/SO3Angular.hpp>
+#include "aikido/distance/SO3Angular.hpp"
 
 namespace aikido {
 namespace distance {
diff --git a/src/distance/defaults.cpp b/src/distance/defaults.cpp
index 7822614601..cf65e8470c 100644
--- a/src/distance/defaults.cpp
+++ b/src/distance/defaults.cpp
@@ -1,4 +1,4 @@
-#include <aikido/distance/defaults.hpp>
+#include "aikido/distance/defaults.hpp"
 
 namespace aikido {
 namespace distance {
diff --git a/src/io/CatkinResourceRetriever.cpp b/src/io/CatkinResourceRetriever.cpp
index 4dae652099..f6ed055b0c 100644
--- a/src/io/CatkinResourceRetriever.cpp
+++ b/src/io/CatkinResourceRetriever.cpp
@@ -2,6 +2,7 @@
 
 #include <fstream>
 #include <iostream>
+
 #include <boost/algorithm/string.hpp>
 #include <boost/filesystem.hpp>
 #include <dart/common/Console.hpp>
diff --git a/src/io/KinBodyParser.cpp b/src/io/KinBodyParser.cpp
index df269614e8..763c558ea6 100644
--- a/src/io/KinBodyParser.cpp
+++ b/src/io/KinBodyParser.cpp
@@ -8,7 +8,6 @@
 #include <vector>
 
 #include <Eigen/Dense>
-
 #include <dart/utils/utils.hpp>
 
 #include "aikido/common/string.hpp"
diff --git a/src/io/trajectory.cpp b/src/io/trajectory.cpp
index 77bff93228..468855cc2f 100644
--- a/src/io/trajectory.cpp
+++ b/src/io/trajectory.cpp
@@ -3,7 +3,9 @@
 #include <algorithm>
 #include <fstream>
 #include <iostream>
+
 #include <boost/program_options.hpp>
+
 #include "aikido/common/Spline.hpp"
 #include "aikido/io/detail/yaml_extension.hpp"
 #include "aikido/io/yaml.hpp"
@@ -12,8 +14,8 @@
 
 using aikido::statespace::ConstStateSpacePtr;
 using aikido::statespace::StateSpacePtr;
-using aikido::statespace::dart::MetaSkeletonStateSpace;
 using aikido::statespace::dart::ConstMetaSkeletonStateSpacePtr;
+using aikido::statespace::dart::MetaSkeletonStateSpace;
 using aikido::trajectory::Spline;
 using aikido::trajectory::UniqueSplinePtr;
 
diff --git a/src/io/util.cpp b/src/io/util.cpp
index f9f2463e27..4d99fc2bad 100644
--- a/src/io/util.cpp
+++ b/src/io/util.cpp
@@ -1,6 +1,7 @@
 #include "aikido/io/util.hpp"
 
 #include <dart/utils/urdf/DartLoader.hpp>
+
 #include "aikido/io/CatkinResourceRetriever.hpp"
 
 namespace aikido {
diff --git a/src/perception/PoseEstimatorModule.cpp b/src/perception/PoseEstimatorModule.cpp
index 5919e2ffad..9ca9f63458 100644
--- a/src/perception/PoseEstimatorModule.cpp
+++ b/src/perception/PoseEstimatorModule.cpp
@@ -5,6 +5,7 @@
 #include <ros/topic.h>
 #include <visualization_msgs/Marker.h>
 #include <visualization_msgs/MarkerArray.h>
+
 #include "aikido/perception/shape_conversions.hpp"
 
 namespace aikido {
diff --git a/src/perception/shape_conversions.cpp b/src/perception/shape_conversions.cpp
index def5257e7c..d1b3e4d01e 100644
--- a/src/perception/shape_conversions.cpp
+++ b/src/perception/shape_conversions.cpp
@@ -1,7 +1,7 @@
-#include <aikido/perception/shape_conversions.hpp>
-
 #include <dart/common/Console.hpp>
 
+#include "aikido/perception/shape_conversions.hpp"
+
 namespace aikido {
 namespace perception {
 
diff --git a/src/planner/World.cpp b/src/planner/World.cpp
index 7f778ea7b6..83fb5d3c4a 100644
--- a/src/planner/World.cpp
+++ b/src/planner/World.cpp
@@ -1,5 +1,6 @@
+#include "aikido/planner/World.hpp"
+
 #include "aikido/common/memory.hpp"
-#include <aikido/planner/World.hpp>
 
 namespace aikido {
 namespace planner {
diff --git a/src/planner/dart/ConfigurationToConfiguration_to_ConfigurationToConfiguration.cpp b/src/planner/dart/ConfigurationToConfiguration_to_ConfigurationToConfiguration.cpp
index f9238e05a9..6e49e3ac59 100644
--- a/src/planner/dart/ConfigurationToConfiguration_to_ConfigurationToConfiguration.cpp
+++ b/src/planner/dart/ConfigurationToConfiguration_to_ConfigurationToConfiguration.cpp
@@ -1,6 +1,7 @@
 #include "aikido/planner/dart/ConfigurationToConfiguration_to_ConfigurationToConfiguration.hpp"
 
 #include <dart/dynamics/dynamics.hpp>
+
 #include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp"
 
 using aikido::statespace::dart::MetaSkeletonStateSpace;
diff --git a/src/planner/dart/ConfigurationToConfiguration_to_ConfigurationToTSR.cpp b/src/planner/dart/ConfigurationToConfiguration_to_ConfigurationToTSR.cpp
index 8e271fe733..984c6e6575 100644
--- a/src/planner/dart/ConfigurationToConfiguration_to_ConfigurationToTSR.cpp
+++ b/src/planner/dart/ConfigurationToConfiguration_to_ConfigurationToTSR.cpp
@@ -1,6 +1,7 @@
 #include "aikido/planner/dart/ConfigurationToConfiguration_to_ConfigurationToTSR.hpp"
 
 #include <dart/dynamics/dynamics.hpp>
+
 #include "aikido/common/RNG.hpp"
 #include "aikido/constraint/dart/InverseKinematicsSampleable.hpp"
 #include "aikido/constraint/dart/JointStateSpaceHelpers.hpp"
diff --git a/src/planner/ompl/CRRT.cpp b/src/planner/ompl/CRRT.cpp
index a98dbcc91e..6f52c91319 100644
--- a/src/planner/ompl/CRRT.cpp
+++ b/src/planner/ompl/CRRT.cpp
@@ -1,8 +1,11 @@
+#include "aikido/planner/ompl/CRRT.hpp"
+
 #include <limits>
+
 #include <ompl/base/goals/GoalSampleableRegion.h>
 #include <ompl/tools/config/SelfConfig.h>
-#include <aikido/planner/ompl/CRRT.hpp>
-#include <aikido/planner/ompl/GeometricStateSpace.hpp>
+
+#include "aikido/planner/ompl/GeometricStateSpace.hpp"
 
 namespace aikido {
 namespace planner {
diff --git a/src/planner/ompl/CRRTConnect.cpp b/src/planner/ompl/CRRTConnect.cpp
index c3e1508437..6ec79ef0bc 100644
--- a/src/planner/ompl/CRRTConnect.cpp
+++ b/src/planner/ompl/CRRTConnect.cpp
@@ -1,8 +1,10 @@
+#include "aikido/planner/ompl/CRRTConnect.hpp"
+
 #include <ompl/base/goals/GoalSampleableRegion.h>
 #include <ompl/tools/config/SelfConfig.h>
-#include <aikido/planner/ompl/BackwardCompatibility.hpp>
-#include <aikido/planner/ompl/CRRTConnect.hpp>
-#include <aikido/planner/ompl/GeometricStateSpace.hpp>
+
+#include "aikido/planner/ompl/BackwardCompatibility.hpp"
+#include "aikido/planner/ompl/GeometricStateSpace.hpp"
 
 namespace aikido {
 namespace planner {
diff --git a/src/planner/ompl/GeometricStateSpace.cpp b/src/planner/ompl/GeometricStateSpace.cpp
index 2c88ef9a4c..ed53e8ab2e 100644
--- a/src/planner/ompl/GeometricStateSpace.cpp
+++ b/src/planner/ompl/GeometricStateSpace.cpp
@@ -1,8 +1,9 @@
+#include "aikido/planner/ompl/GeometricStateSpace.hpp"
+
 #include "aikido/common/memory.hpp"
-#include <aikido/constraint/Sampleable.hpp>
-#include <aikido/planner/ompl/BackwardCompatibility.hpp>
-#include <aikido/planner/ompl/GeometricStateSpace.hpp>
-#include <aikido/planner/ompl/StateSampler.hpp>
+#include "aikido/constraint/Sampleable.hpp"
+#include "aikido/planner/ompl/BackwardCompatibility.hpp"
+#include "aikido/planner/ompl/StateSampler.hpp"
 
 namespace aikido {
 namespace planner {
diff --git a/src/planner/ompl/GoalRegion.cpp b/src/planner/ompl/GoalRegion.cpp
index aac3836611..135a708426 100644
--- a/src/planner/ompl/GoalRegion.cpp
+++ b/src/planner/ompl/GoalRegion.cpp
@@ -1,5 +1,6 @@
-#include <aikido/planner/ompl/GeometricStateSpace.hpp>
-#include <aikido/planner/ompl/GoalRegion.hpp>
+#include "aikido/planner/ompl/GoalRegion.hpp"
+
+#include "aikido/planner/ompl/GeometricStateSpace.hpp"
 
 namespace aikido {
 namespace planner {
diff --git a/src/planner/ompl/MotionValidator.cpp b/src/planner/ompl/MotionValidator.cpp
index 2e83fff74e..725d1dfb16 100644
--- a/src/planner/ompl/MotionValidator.cpp
+++ b/src/planner/ompl/MotionValidator.cpp
@@ -1,8 +1,9 @@
-#include <aikido/planner/ompl/MotionValidator.hpp>
+#include "aikido/planner/ompl/MotionValidator.hpp"
 
 #include <ompl/base/SpaceInformation.h>
-#include <aikido/common/StepSequence.hpp>
-#include <aikido/common/VanDerCorput.hpp>
+
+#include "aikido/common/StepSequence.hpp"
+#include "aikido/common/VanDerCorput.hpp"
 
 namespace aikido {
 namespace planner {
diff --git a/src/planner/ompl/Planner.cpp b/src/planner/ompl/Planner.cpp
index 0af5e60e2c..537e57f392 100644
--- a/src/planner/ompl/Planner.cpp
+++ b/src/planner/ompl/Planner.cpp
@@ -1,13 +1,14 @@
-#include <aikido/common/memory.hpp>
-#include <aikido/constraint/TestableIntersection.hpp>
-#include <aikido/planner/ompl/CRRT.hpp>
-#include <aikido/planner/ompl/CRRTConnect.hpp>
-#include <aikido/planner/ompl/GeometricStateSpace.hpp>
-#include <aikido/planner/ompl/MotionValidator.hpp>
-#include <aikido/planner/ompl/Planner.hpp>
+#include "aikido/planner/ompl/Planner.hpp"
 
 #include <dart/dart.hpp>
 
+#include "aikido/common/memory.hpp"
+#include "aikido/constraint/TestableIntersection.hpp"
+#include "aikido/planner/ompl/CRRT.hpp"
+#include "aikido/planner/ompl/CRRTConnect.hpp"
+#include "aikido/planner/ompl/GeometricStateSpace.hpp"
+#include "aikido/planner/ompl/MotionValidator.hpp"
+
 namespace aikido {
 namespace planner {
 namespace ompl {
diff --git a/src/planner/ompl/StateSampler.cpp b/src/planner/ompl/StateSampler.cpp
index 466681c1dd..68433cb8f1 100644
--- a/src/planner/ompl/StateSampler.cpp
+++ b/src/planner/ompl/StateSampler.cpp
@@ -1,5 +1,6 @@
-#include <aikido/planner/ompl/GeometricStateSpace.hpp>
-#include <aikido/planner/ompl/StateSampler.hpp>
+#include "aikido/planner/ompl/StateSampler.hpp"
+
+#include "aikido/planner/ompl/GeometricStateSpace.hpp"
 
 namespace aikido {
 namespace planner {
diff --git a/src/planner/ompl/StateValidityChecker.cpp b/src/planner/ompl/StateValidityChecker.cpp
index cdcce6b281..7c24d61b83 100644
--- a/src/planner/ompl/StateValidityChecker.cpp
+++ b/src/planner/ompl/StateValidityChecker.cpp
@@ -1,5 +1,6 @@
-#include <aikido/planner/ompl/GeometricStateSpace.hpp>
-#include <aikido/planner/ompl/StateValidityChecker.hpp>
+#include "aikido/planner/ompl/StateValidityChecker.hpp"
+
+#include "aikido/planner/ompl/GeometricStateSpace.hpp"
 
 namespace aikido {
 namespace planner {
diff --git a/src/planner/ompl/dart.cpp b/src/planner/ompl/dart.cpp
index 981643aadb..a554013ce6 100644
--- a/src/planner/ompl/dart.cpp
+++ b/src/planner/ompl/dart.cpp
@@ -1,9 +1,10 @@
-#include <aikido/constraint/dart/JointStateSpaceHelpers.hpp>
-#include <aikido/distance/defaults.hpp>
-#include <aikido/planner/ompl/Planner.hpp>
-#include <aikido/planner/ompl/dart.hpp>
-#include <aikido/statespace/GeodesicInterpolator.hpp>
-#include <aikido/statespace/dart/MetaSkeletonStateSpace.hpp>
+#include "aikido/planner/ompl/dart.hpp"
+
+#include "aikido/constraint/dart/JointStateSpaceHelpers.hpp"
+#include "aikido/distance/defaults.hpp"
+#include "aikido/planner/ompl/Planner.hpp"
+#include "aikido/statespace/GeodesicInterpolator.hpp"
+#include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp"
 
 namespace aikido {
 namespace planner {
diff --git a/src/planner/parabolic/HauserParabolicSmootherHelpers.cpp b/src/planner/parabolic/HauserParabolicSmootherHelpers.cpp
index 5a0839125d..674371a95c 100644
--- a/src/planner/parabolic/HauserParabolicSmootherHelpers.cpp
+++ b/src/planner/parabolic/HauserParabolicSmootherHelpers.cpp
@@ -1,7 +1,10 @@
 #include "HauserParabolicSmootherHelpers.hpp"
+
 #include <chrono>
 #include <cmath>
-#include <aikido/common/VanDerCorput.hpp>
+
+#include "aikido/common/VanDerCorput.hpp"
+
 #include "Config.h"
 #include "HauserMath.h"
 #include "ParabolicUtil.hpp"
diff --git a/src/planner/parabolic/ParabolicSmoother.cpp b/src/planner/parabolic/ParabolicSmoother.cpp
index eed9d0572c..0f1551cf6a 100644
--- a/src/planner/parabolic/ParabolicSmoother.cpp
+++ b/src/planner/parabolic/ParabolicSmoother.cpp
@@ -2,9 +2,11 @@
 
 #include <cassert>
 #include <set>
+
 #include "aikido/common/Spline.hpp"
 #include "aikido/common/memory.hpp"
 #include "aikido/planner/parabolic/ParabolicTimer.hpp"
+
 #include "DynamicPath.h"
 #include "HauserParabolicSmootherHelpers.hpp"
 #include "ParabolicUtil.hpp"
diff --git a/src/planner/parabolic/ParabolicTimer.cpp b/src/planner/parabolic/ParabolicTimer.cpp
index 8e6684e5ef..2c27f60c14 100644
--- a/src/planner/parabolic/ParabolicTimer.cpp
+++ b/src/planner/parabolic/ParabolicTimer.cpp
@@ -2,9 +2,11 @@
 
 #include <cassert>
 #include <set>
+
 #include "aikido/common/Spline.hpp"
 #include "aikido/common/memory.hpp"
 #include "aikido/trajectory/Interpolated.hpp"
+
 #include "DynamicPath.h"
 #include "ParabolicUtil.hpp"
 
diff --git a/src/planner/parabolic/ParabolicUtil.cpp b/src/planner/parabolic/ParabolicUtil.cpp
index 55e619a555..352e57056d 100644
--- a/src/planner/parabolic/ParabolicUtil.cpp
+++ b/src/planner/parabolic/ParabolicUtil.cpp
@@ -4,6 +4,7 @@
 #include <set>
 
 #include <dart/dart.hpp>
+
 #include "aikido/common/Spline.hpp"
 #include "aikido/common/memory.hpp"
 #include "aikido/statespace/GeodesicInterpolator.hpp"
diff --git a/src/planner/parabolic/ParabolicUtil.hpp b/src/planner/parabolic/ParabolicUtil.hpp
index ecd6e56c41..9ecbc973dc 100644
--- a/src/planner/parabolic/ParabolicUtil.hpp
+++ b/src/planner/parabolic/ParabolicUtil.hpp
@@ -3,9 +3,9 @@
 
 #include <memory>
 #include <Eigen/Dense>
-#include <aikido/statespace/StateSpace.hpp>
-#include <aikido/trajectory/Interpolated.hpp>
-#include <aikido/trajectory/Spline.hpp>
+#include "aikido/statespace/StateSpace.hpp"
+#include "aikido/trajectory/Interpolated.hpp"
+#include "aikido/trajectory/Spline.hpp"
 #include "DynamicPath.h"
 
 namespace aikido {
diff --git a/src/planner/vectorfield/BodyNodePoseVectorField.cpp b/src/planner/vectorfield/BodyNodePoseVectorField.cpp
index 7804d4b5c0..9fc1d162ef 100644
--- a/src/planner/vectorfield/BodyNodePoseVectorField.cpp
+++ b/src/planner/vectorfield/BodyNodePoseVectorField.cpp
@@ -1,6 +1,8 @@
-#include <aikido/common/StepSequence.hpp>
-#include <aikido/planner/vectorfield/BodyNodePoseVectorField.hpp>
-#include <aikido/planner/vectorfield/VectorFieldUtil.hpp>
+#include "aikido/planner/vectorfield/BodyNodePoseVectorField.hpp"
+
+#include "aikido/common/StepSequence.hpp"
+#include "aikido/planner/vectorfield/VectorFieldUtil.hpp"
+
 #include "detail/VectorFieldPlannerExceptions.hpp"
 
 namespace aikido {
diff --git a/src/planner/vectorfield/MoveEndEffectorOffsetVectorField.cpp b/src/planner/vectorfield/MoveEndEffectorOffsetVectorField.cpp
index 8da1a77f10..25b370d85c 100644
--- a/src/planner/vectorfield/MoveEndEffectorOffsetVectorField.cpp
+++ b/src/planner/vectorfield/MoveEndEffectorOffsetVectorField.cpp
@@ -1,9 +1,12 @@
+#include "aikido/planner/vectorfield/MoveEndEffectorOffsetVectorField.hpp"
+
 #include <dart/dynamics/BodyNode.hpp>
 #include <dart/math/MathTypes.hpp>
 #include <dart/optimizer/Function.hpp>
 #include <dart/optimizer/Problem.hpp>
-#include <aikido/planner/vectorfield/MoveEndEffectorOffsetVectorField.hpp>
-#include <aikido/planner/vectorfield/VectorFieldUtil.hpp>
+
+#include "aikido/planner/vectorfield/VectorFieldUtil.hpp"
+
 #include "detail/VectorFieldPlannerExceptions.hpp"
 
 namespace aikido {
diff --git a/src/planner/vectorfield/MoveEndEffectorPoseVectorField.cpp b/src/planner/vectorfield/MoveEndEffectorPoseVectorField.cpp
index 51a08d142e..38b1072e45 100644
--- a/src/planner/vectorfield/MoveEndEffectorPoseVectorField.cpp
+++ b/src/planner/vectorfield/MoveEndEffectorPoseVectorField.cpp
@@ -1,9 +1,12 @@
+#include "aikido/planner/vectorfield/MoveEndEffectorPoseVectorField.hpp"
+
 #include <dart/dynamics/BodyNode.hpp>
 #include <dart/math/MathTypes.hpp>
 #include <dart/optimizer/Function.hpp>
 #include <dart/optimizer/Problem.hpp>
-#include <aikido/planner/vectorfield/MoveEndEffectorPoseVectorField.hpp>
-#include <aikido/planner/vectorfield/VectorFieldUtil.hpp>
+
+#include "aikido/planner/vectorfield/VectorFieldUtil.hpp"
+
 #include "detail/VectorFieldPlannerExceptions.hpp"
 
 namespace aikido {
diff --git a/src/planner/vectorfield/VectorField.cpp b/src/planner/vectorfield/VectorField.cpp
index 8e82fb39ba..92a636257a 100644
--- a/src/planner/vectorfield/VectorField.cpp
+++ b/src/planner/vectorfield/VectorField.cpp
@@ -1,4 +1,4 @@
-#include <aikido/planner/vectorfield/VectorField.hpp>
+#include "aikido/planner/vectorfield/VectorField.hpp"
 
 namespace aikido {
 namespace planner {
diff --git a/src/planner/vectorfield/VectorFieldPlanner.cpp b/src/planner/vectorfield/VectorFieldPlanner.cpp
index 35b1ee2e66..4af691ddb3 100644
--- a/src/planner/vectorfield/VectorFieldPlanner.cpp
+++ b/src/planner/vectorfield/VectorFieldPlanner.cpp
@@ -9,6 +9,7 @@
 #include "aikido/planner/vectorfield/VectorFieldUtil.hpp"
 #include "aikido/statespace/dart/MetaSkeletonStateSaver.hpp"
 #include "aikido/trajectory/Spline.hpp"
+
 #include "detail/VectorFieldIntegrator.hpp"
 #include "detail/VectorFieldPlannerExceptions.hpp"
 
diff --git a/src/planner/vectorfield/VectorFieldUtil.cpp b/src/planner/vectorfield/VectorFieldUtil.cpp
index 8bdedadac7..9c25f9e5f9 100644
--- a/src/planner/vectorfield/VectorFieldUtil.cpp
+++ b/src/planner/vectorfield/VectorFieldUtil.cpp
@@ -1,9 +1,11 @@
+#include "aikido/planner/vectorfield/VectorFieldUtil.hpp"
+
 #include <Eigen/Geometry>
 #include <dart/optimizer/Solver.hpp>
 #include <dart/optimizer/nlopt/NloptSolver.hpp>
-#include <aikido/common/algorithm.hpp>
-#include <aikido/planner/vectorfield/VectorFieldUtil.hpp>
-#include <aikido/trajectory/Spline.hpp>
+
+#include "aikido/common/algorithm.hpp"
+#include "aikido/trajectory/Spline.hpp"
 
 namespace aikido {
 namespace planner {
diff --git a/src/planner/vectorfield/detail/VectorFieldIntegrator.cpp b/src/planner/vectorfield/detail/VectorFieldIntegrator.cpp
index 935812bb25..51cfd9630e 100644
--- a/src/planner/vectorfield/detail/VectorFieldIntegrator.cpp
+++ b/src/planner/vectorfield/detail/VectorFieldIntegrator.cpp
@@ -1,10 +1,13 @@
 #include "VectorFieldIntegrator.hpp"
+
 #include <exception>
 #include <string>
-#include <aikido/common/Spline.hpp>
-#include <aikido/common/memory.hpp>
-#include <aikido/planner/vectorfield/VectorFieldUtil.hpp>
-#include <aikido/statespace/GeodesicInterpolator.hpp>
+
+#include "aikido/common/Spline.hpp"
+#include "aikido/common/memory.hpp"
+#include "aikido/planner/vectorfield/VectorFieldUtil.hpp"
+#include "aikido/statespace/GeodesicInterpolator.hpp"
+
 #include "VectorFieldPlannerExceptions.hpp"
 
 namespace aikido {
diff --git a/src/planner/vectorfield/detail/VectorFieldIntegrator.hpp b/src/planner/vectorfield/detail/VectorFieldIntegrator.hpp
index 46ba1a562a..09139b4050 100644
--- a/src/planner/vectorfield/detail/VectorFieldIntegrator.hpp
+++ b/src/planner/vectorfield/detail/VectorFieldIntegrator.hpp
@@ -2,12 +2,12 @@
 #define AIKIDO_PLANNER_VECTORFIELD_VECTORFIELDINTEGRATOR_HPP_
 
 #include <dart/common/Timer.hpp>
-#include <aikido/constraint/Testable.hpp>
-#include <aikido/planner/vectorfield/VectorField.hpp>
-#include <aikido/planner/vectorfield/VectorFieldPlannerStatus.hpp>
-#include <aikido/statespace/dart/MetaSkeletonStateSpace.hpp>
-#include <aikido/trajectory/Spline.hpp>
-#include <aikido/trajectory/Interpolated.hpp>
+#include "aikido/constraint/Testable.hpp"
+#include "aikido/planner/vectorfield/VectorField.hpp"
+#include "aikido/planner/vectorfield/VectorFieldPlannerStatus.hpp"
+#include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp"
+#include "aikido/trajectory/Spline.hpp"
+#include "aikido/trajectory/Interpolated.hpp"
 
 namespace aikido {
 namespace planner {
diff --git a/src/robot/ConcreteManipulator.cpp b/src/robot/ConcreteManipulator.cpp
index 3e790e4655..ab94266f94 100644
--- a/src/robot/ConcreteManipulator.cpp
+++ b/src/robot/ConcreteManipulator.cpp
@@ -1,4 +1,5 @@
 #include "aikido/robot/ConcreteManipulator.hpp"
+
 #include "aikido/planner/dart/util.hpp"
 #include "aikido/robot/util.hpp"
 
diff --git a/src/robot/ConcreteRobot.cpp b/src/robot/ConcreteRobot.cpp
index 5cbc3de998..7a8b4ce28b 100644
--- a/src/robot/ConcreteRobot.cpp
+++ b/src/robot/ConcreteRobot.cpp
@@ -1,4 +1,5 @@
 #include "aikido/robot/ConcreteRobot.hpp"
+
 #include "aikido/constraint/TestableIntersection.hpp"
 #include "aikido/planner/kunzretimer/KunzRetimer.hpp"
 #include "aikido/robot/util.hpp"
diff --git a/src/robot/util.cpp b/src/robot/util.cpp
index 53d43d8a68..a76e3187f9 100644
--- a/src/robot/util.cpp
+++ b/src/robot/util.cpp
@@ -1,9 +1,11 @@
 #include "aikido/robot/util.hpp"
 
 #include <algorithm>
+
 #include <dart/common/Console.hpp>
 #include <dart/common/Timer.hpp>
 #include <ompl/geometric/planners/rrt/RRTConnect.h>
+
 #include "aikido/common/RNG.hpp"
 #include "aikido/common/memory.hpp"
 #include "aikido/constraint/CyclicSampleable.hpp"
@@ -20,6 +22,7 @@
 #include "aikido/planner/PlanningResult.hpp"
 #include "aikido/planner/SnapConfigurationToConfigurationPlanner.hpp"
 #include "aikido/planner/ompl/CRRTConnect.hpp"
+#include "aikido/planner/ompl/OMPLConfigurationToConfigurationPlanner.hpp"
 #include "aikido/planner/ompl/Planner.hpp"
 #include "aikido/planner/parabolic/ParabolicSmoother.hpp"
 #include "aikido/planner/parabolic/ParabolicTimer.hpp"
@@ -29,8 +32,6 @@
 #include "aikido/statespace/dart/MetaSkeletonStateSaver.hpp"
 #include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp"
 
-#include "aikido/planner/ompl/OMPLConfigurationToConfigurationPlanner.hpp"
-
 namespace aikido {
 namespace robot {
 namespace util {
diff --git a/src/rviz/BodyNodeMarker.cpp b/src/rviz/BodyNodeMarker.cpp
index 91f386171f..9fb3aa8e4e 100644
--- a/src/rviz/BodyNodeMarker.cpp
+++ b/src/rviz/BodyNodeMarker.cpp
@@ -1,8 +1,8 @@
-#include <aikido/rviz/BodyNodeMarker.hpp>
-
 #include <sstream>
 #include <unordered_set>
-#include <aikido/rviz/shape_conversions.hpp>
+
+#include "aikido/rviz/BodyNodeMarker.hpp"
+#include "aikido/rviz/shape_conversions.hpp"
 
 using dart::dynamics::BodyNode;
 using dart::dynamics::BodyNodePtr;
diff --git a/src/rviz/FrameMarker.cpp b/src/rviz/FrameMarker.cpp
index 925165248c..ae1a1c5903 100644
--- a/src/rviz/FrameMarker.cpp
+++ b/src/rviz/FrameMarker.cpp
@@ -1,7 +1,7 @@
-#include <aikido/rviz/FrameMarker.hpp>
-
 #include <boost/format.hpp>
-#include <aikido/rviz/shape_conversions.hpp>
+
+#include "aikido/rviz/FrameMarker.hpp"
+#include "aikido/rviz/shape_conversions.hpp"
 
 using boost::format;
 using boost::str;
diff --git a/src/rviz/InteractiveMarkerViewer.cpp b/src/rviz/InteractiveMarkerViewer.cpp
index 0afa118f01..fbeebec070 100644
--- a/src/rviz/InteractiveMarkerViewer.cpp
+++ b/src/rviz/InteractiveMarkerViewer.cpp
@@ -1,10 +1,10 @@
-#include <aikido/rviz/InteractiveMarkerViewer.hpp>
-
 #include <dart/dart.hpp>
+
 #include "aikido/common/memory.hpp"
-#include <aikido/rviz/FrameMarker.hpp>
-#include <aikido/rviz/SkeletonMarker.hpp>
-#include <aikido/rviz/TrajectoryMarker.hpp>
+#include "aikido/rviz/FrameMarker.hpp"
+#include "aikido/rviz/InteractiveMarkerViewer.hpp"
+#include "aikido/rviz/SkeletonMarker.hpp"
+#include "aikido/rviz/TrajectoryMarker.hpp"
 
 using dart::dynamics::Skeleton;
 using dart::dynamics::SkeletonPtr;
diff --git a/src/rviz/ResourceServer.cpp b/src/rviz/ResourceServer.cpp
index 047552db87..cf4d86e581 100644
--- a/src/rviz/ResourceServer.cpp
+++ b/src/rviz/ResourceServer.cpp
@@ -1,6 +1,5 @@
-#include <aikido/rviz/ResourceServer.hpp>
-
 #include <fstream>
+
 #include <assimp/cexport.h>
 #include <assimp/version.h>
 #include <boost/filesystem.hpp>
@@ -8,6 +7,8 @@
 #include <ros/network.h>
 #include <sys/socket.h>
 
+#include "aikido/rviz/ResourceServer.hpp"
+
 namespace aikido {
 namespace rviz {
 
diff --git a/src/rviz/ShapeFrameMarker.cpp b/src/rviz/ShapeFrameMarker.cpp
index 04d78288fe..dcbd5fd5cd 100644
--- a/src/rviz/ShapeFrameMarker.cpp
+++ b/src/rviz/ShapeFrameMarker.cpp
@@ -1,10 +1,10 @@
-#include <aikido/rviz/ShapeFrameMarker.hpp>
-
 #include <dart/dynamics/dynamics.hpp>
 #include <interactive_markers/interactive_marker_server.h>
 #include <visualization_msgs/InteractiveMarker.h>
-#include <aikido/rviz/ResourceServer.hpp>
-#include <aikido/rviz/shape_conversions.hpp>
+
+#include "aikido/rviz/ResourceServer.hpp"
+#include "aikido/rviz/ShapeFrameMarker.hpp"
+#include "aikido/rviz/shape_conversions.hpp"
 
 using dart::dynamics::CollisionAspect;
 using dart::dynamics::ConstShapePtr;
diff --git a/src/rviz/SkeletonMarker.cpp b/src/rviz/SkeletonMarker.cpp
index a3bb00acf1..cfdbe5557e 100644
--- a/src/rviz/SkeletonMarker.cpp
+++ b/src/rviz/SkeletonMarker.cpp
@@ -1,4 +1,4 @@
-#include <aikido/rviz/SkeletonMarker.hpp>
+#include "aikido/rviz/SkeletonMarker.hpp"
 
 using aikido::rviz::BodyNodeMarker;
 using aikido::rviz::BodyNodeMarkerPtr;
diff --git a/src/rviz/TSRMarker.cpp b/src/rviz/TSRMarker.cpp
index 7fcfa012a4..f32dfb8ff0 100644
--- a/src/rviz/TSRMarker.cpp
+++ b/src/rviz/TSRMarker.cpp
@@ -1,4 +1,4 @@
-#include <aikido/rviz/TSRMarker.hpp>
+#include "aikido/rviz/TSRMarker.hpp"
 
 using dart::dynamics::SimpleFrame;
 
diff --git a/src/rviz/WorldInteractiveMarkerViewer.cpp b/src/rviz/WorldInteractiveMarkerViewer.cpp
index 266337d212..eeacca2559 100644
--- a/src/rviz/WorldInteractiveMarkerViewer.cpp
+++ b/src/rviz/WorldInteractiveMarkerViewer.cpp
@@ -1,9 +1,10 @@
 #include <dart/dart.hpp>
-#include <aikido/rviz/FrameMarker.hpp>
-#include <aikido/rviz/InteractiveMarkerViewer.hpp>
-#include <aikido/rviz/SkeletonMarker.hpp>
-#include <aikido/rviz/TrajectoryMarker.hpp>
-#include <aikido/rviz/WorldInteractiveMarkerViewer.hpp>
+
+#include "aikido/rviz/FrameMarker.hpp"
+#include "aikido/rviz/InteractiveMarkerViewer.hpp"
+#include "aikido/rviz/SkeletonMarker.hpp"
+#include "aikido/rviz/TrajectoryMarker.hpp"
+#include "aikido/rviz/WorldInteractiveMarkerViewer.hpp"
 
 using aikido::rviz::InteractiveMarkerViewer;
 
diff --git a/src/rviz/python.cpp b/src/rviz/python.cpp
index 5f9560a14d..07294c2eeb 100644
--- a/src/rviz/python.cpp
+++ b/src/rviz/python.cpp
@@ -1,7 +1,7 @@
 #include <ros/ros.h>
 #include <boost/python.hpp>
-#include <aikido/rviz/InteractiveMarkerViewer.hpp>
-#include <aikido/rviz/SkeletonMarker.hpp>
+#include "aikido/rviz/InteractiveMarkerViewer.hpp"
+#include "aikido/rviz/SkeletonMarker.hpp"
 
 using aikido::rviz::InteractiveMarkerViewer;
 
diff --git a/src/rviz/shape_conversions.cpp b/src/rviz/shape_conversions.cpp
index 1fc2fcd2a6..c517274e2f 100644
--- a/src/rviz/shape_conversions.cpp
+++ b/src/rviz/shape_conversions.cpp
@@ -1,10 +1,10 @@
-#include <aikido/rviz/shape_conversions.hpp>
-
 #include <Eigen/StdVector>
 #include <assimp/cexport.h>
 #include <boost/filesystem.hpp>
 #include <dart/dynamics/dynamics.hpp>
-#include <aikido/rviz/ResourceServer.hpp>
+
+#include "aikido/rviz/ResourceServer.hpp"
+#include "aikido/rviz/shape_conversions.hpp"
 
 using dart::dynamics::BoxShape;
 using dart::dynamics::CylinderShape;
diff --git a/src/statespace/CartesianProduct.cpp b/src/statespace/CartesianProduct.cpp
index 8eb0615c50..b275718633 100644
--- a/src/statespace/CartesianProduct.cpp
+++ b/src/statespace/CartesianProduct.cpp
@@ -1,5 +1,6 @@
+#include "aikido/statespace/CartesianProduct.hpp"
+
 #include <iostream>
-#include <aikido/statespace/CartesianProduct.hpp>
 
 namespace aikido {
 namespace statespace {
diff --git a/src/statespace/GeodesicInterpolator.cpp b/src/statespace/GeodesicInterpolator.cpp
index c24da64fd9..0324058758 100644
--- a/src/statespace/GeodesicInterpolator.cpp
+++ b/src/statespace/GeodesicInterpolator.cpp
@@ -1,4 +1,4 @@
-#include <aikido/statespace/GeodesicInterpolator.hpp>
+#include "aikido/statespace/GeodesicInterpolator.hpp"
 
 namespace aikido {
 namespace statespace {
diff --git a/src/statespace/Rn.cpp b/src/statespace/Rn.cpp
index 807a516f3c..48ebfc7ccb 100644
--- a/src/statespace/Rn.cpp
+++ b/src/statespace/Rn.cpp
@@ -1,4 +1,4 @@
-#include <aikido/statespace/Rn.hpp>
+#include "aikido/statespace/Rn.hpp"
 
 namespace aikido {
 namespace statespace {
diff --git a/src/statespace/SE2.cpp b/src/statespace/SE2.cpp
index 732eb7feb8..8341043eaf 100644
--- a/src/statespace/SE2.cpp
+++ b/src/statespace/SE2.cpp
@@ -1,5 +1,6 @@
+#include "aikido/statespace/SE2.hpp"
+
 #include <Eigen/Geometry>
-#include <aikido/statespace/SE2.hpp>
 
 namespace aikido {
 namespace statespace {
diff --git a/src/statespace/SE3.cpp b/src/statespace/SE3.cpp
index 6934cfb11a..5bba1ab585 100644
--- a/src/statespace/SE3.cpp
+++ b/src/statespace/SE3.cpp
@@ -1,5 +1,6 @@
+#include "aikido/statespace/SE3.hpp"
+
 #include <dart/math/Geometry.hpp>
-#include <aikido/statespace/SE3.hpp>
 
 namespace aikido {
 namespace statespace {
diff --git a/src/statespace/SO3.cpp b/src/statespace/SO3.cpp
index c0b22e9afc..c548bcbbe0 100644
--- a/src/statespace/SO3.cpp
+++ b/src/statespace/SO3.cpp
@@ -1,6 +1,8 @@
+#include "aikido/statespace/SO3.hpp"
+
 #include <iostream>
+
 #include <dart/math/Geometry.hpp>
-#include <aikido/statespace/SO3.hpp>
 
 namespace aikido {
 namespace statespace {
diff --git a/src/statespace/dart/JointStateSpaceHelpers.cpp b/src/statespace/dart/JointStateSpaceHelpers.cpp
index dd2f70e8e7..23429b7d10 100644
--- a/src/statespace/dart/JointStateSpaceHelpers.cpp
+++ b/src/statespace/dart/JointStateSpaceHelpers.cpp
@@ -1,4 +1,4 @@
-#include <aikido/statespace/dart/JointStateSpaceHelpers.hpp>
+#include "aikido/statespace/dart/JointStateSpaceHelpers.hpp"
 
 namespace aikido {
 namespace statespace {
diff --git a/src/statespace/dart/MetaSkeletonStateSaver.cpp b/src/statespace/dart/MetaSkeletonStateSaver.cpp
index 1546f63ad6..bfa9334435 100644
--- a/src/statespace/dart/MetaSkeletonStateSaver.cpp
+++ b/src/statespace/dart/MetaSkeletonStateSaver.cpp
@@ -1,4 +1,5 @@
 #include "aikido/statespace/dart/MetaSkeletonStateSaver.hpp"
+
 #include <iostream>
 
 namespace aikido {
diff --git a/src/statespace/dart/MetaSkeletonStateSpace.cpp b/src/statespace/dart/MetaSkeletonStateSpace.cpp
index 00ea0bfa9d..d63b441473 100644
--- a/src/statespace/dart/MetaSkeletonStateSpace.cpp
+++ b/src/statespace/dart/MetaSkeletonStateSpace.cpp
@@ -1,7 +1,10 @@
 #include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp"
+
 #include <cassert>
 #include <sstream>
+
 #include <dart/common/Console.hpp>
+
 #include "aikido/common/memory.hpp"
 #include "aikido/statespace/dart/JointStateSpaceHelpers.hpp"
 
diff --git a/src/statespace/dart/SE2Joint.cpp b/src/statespace/dart/SE2Joint.cpp
index 2c6480e12c..f9a0b97164 100644
--- a/src/statespace/dart/SE2Joint.cpp
+++ b/src/statespace/dart/SE2Joint.cpp
@@ -1,4 +1,4 @@
-#include <aikido/statespace/dart/SE2Joint.hpp>
+#include "aikido/statespace/dart/SE2Joint.hpp"
 
 namespace aikido {
 namespace statespace {
diff --git a/src/trajectory/BSpline.cpp b/src/trajectory/BSpline.cpp
index 379b9415e7..73d5fea746 100644
--- a/src/trajectory/BSpline.cpp
+++ b/src/trajectory/BSpline.cpp
@@ -1,7 +1,9 @@
 #include "aikido/trajectory/BSpline.hpp"
 
 #include <sstream>
+
 #include <Eigen/Core>
+
 #include "aikido/common/StepSequence.hpp"
 #include "aikido/common/memory.hpp"
 
diff --git a/src/trajectory/Interpolated.cpp b/src/trajectory/Interpolated.cpp
index eb1141905c..63515aa118 100644
--- a/src/trajectory/Interpolated.cpp
+++ b/src/trajectory/Interpolated.cpp
@@ -1,4 +1,4 @@
-#include <aikido/trajectory/Interpolated.hpp>
+#include "aikido/trajectory/Interpolated.hpp"
 
 using aikido::statespace::GeodesicInterpolator;
 
diff --git a/src/trajectory/Spline.cpp b/src/trajectory/Spline.cpp
index 96a9424aff..775a357632 100644
--- a/src/trajectory/Spline.cpp
+++ b/src/trajectory/Spline.cpp
@@ -1,6 +1,6 @@
-#include <aikido/trajectory/Spline.hpp>
+#include "aikido/trajectory/Spline.hpp"
 
-#include <aikido/common/Spline.hpp>
+#include "aikido/common/Spline.hpp"
 
 namespace aikido {
 namespace trajectory {
diff --git a/src/trajectory/util.cpp b/src/trajectory/util.cpp
index a1b4c1766e..4d03c41636 100644
--- a/src/trajectory/util.cpp
+++ b/src/trajectory/util.cpp
@@ -3,7 +3,9 @@
 #include <algorithm>
 #include <fstream>
 #include <iostream>
+
 #include <boost/program_options.hpp>
+
 #include "aikido/common/Spline.hpp"
 #include "aikido/common/StepSequence.hpp"
 #include "aikido/common/memory.hpp"
diff --git a/tests/common/test_Executor.cpp b/tests/common/test_Executor.cpp
index 2a740dce15..8432a3ca91 100644
--- a/tests/common/test_Executor.cpp
+++ b/tests/common/test_Executor.cpp
@@ -1,4 +1,5 @@
 #include <gtest/gtest.h>
+
 #include <aikido/common/ExecutorMultiplexer.hpp>
 #include <aikido/common/ExecutorThread.hpp>
 
diff --git a/tests/common/test_PseudoInverse.cpp b/tests/common/test_PseudoInverse.cpp
index db7b9fad8a..8a984d2846 100644
--- a/tests/common/test_PseudoInverse.cpp
+++ b/tests/common/test_PseudoInverse.cpp
@@ -1,5 +1,6 @@
 #include <Eigen/Dense>
 #include <gtest/gtest.h>
+
 #include <aikido/common/PseudoInverse.hpp>
 
 using namespace aikido::common;
diff --git a/tests/common/test_Spline.cpp b/tests/common/test_Spline.cpp
index d0fd80c369..a39c9207f9 100644
--- a/tests/common/test_Spline.cpp
+++ b/tests/common/test_Spline.cpp
@@ -1,6 +1,9 @@
 #include <cstdlib>
+
 #include <gtest/gtest.h>
+
 #include <aikido/common/Spline.hpp>
+
 #include "eigen_tests.hpp"
 
 using namespace aikido::tests;
diff --git a/tests/common/test_SplineProblem.cpp b/tests/common/test_SplineProblem.cpp
index a4021a9006..c5bdbe4773 100644
--- a/tests/common/test_SplineProblem.cpp
+++ b/tests/common/test_SplineProblem.cpp
@@ -1,6 +1,9 @@
 #include <cstdlib>
+
 #include <gtest/gtest.h>
+
 #include <aikido/common/Spline.hpp>
+
 #include "eigen_tests.hpp"
 
 using namespace aikido::tests;
diff --git a/tests/common/test_StepSequence.cpp b/tests/common/test_StepSequence.cpp
index 8c116a91c7..f74f27ab9b 100644
--- a/tests/common/test_StepSequence.cpp
+++ b/tests/common/test_StepSequence.cpp
@@ -1,6 +1,7 @@
 #include <gtest/gtest.h>
-#include "aikido/common/memory.hpp"
+
 #include <aikido/common/StepSequence.hpp>
+#include <aikido/common/memory.hpp>
 
 using aikido::common::StepSequence;
 
diff --git a/tests/common/test_VanDerCorput.cpp b/tests/common/test_VanDerCorput.cpp
index b6bdc6116d..791c640d3c 100644
--- a/tests/common/test_VanDerCorput.cpp
+++ b/tests/common/test_VanDerCorput.cpp
@@ -1,5 +1,6 @@
 #include <dart/common/StlHelpers.hpp>
 #include <gtest/gtest.h>
+
 #include <aikido/common/VanDerCorput.hpp>
 
 using aikido::common::VanDerCorput;
diff --git a/tests/common/test_string.cpp b/tests/common/test_string.cpp
index b4e28223db..f8e83a54f3 100644
--- a/tests/common/test_string.cpp
+++ b/tests/common/test_string.cpp
@@ -1,4 +1,5 @@
 #include <gtest/gtest.h>
+
 #include <aikido/common/string.hpp>
 
 using namespace aikido;
diff --git a/tests/constraint/MockConstraints.hpp b/tests/constraint/MockConstraints.hpp
index 046c77f4b7..a13e234c5a 100644
--- a/tests/constraint/MockConstraints.hpp
+++ b/tests/constraint/MockConstraints.hpp
@@ -2,6 +2,7 @@
 #define AIKIDO_TESTS_CONSTRAINT_MOCK_CONSTRAINTS_HPP_
 
 #include <memory>
+
 #include <aikido/constraint/Testable.hpp>
 
 using aikido::constraint::DefaultTestableOutcome;
diff --git a/tests/constraint/PolynomialConstraint-impl.hpp b/tests/constraint/PolynomialConstraint-impl.hpp
index 69fe0b93ae..1797860cb4 100644
--- a/tests/constraint/PolynomialConstraint-impl.hpp
+++ b/tests/constraint/PolynomialConstraint-impl.hpp
@@ -1,4 +1,5 @@
 #include <memory>
+
 #include "PolynomialConstraint.hpp"
 
 //==============================================================================
diff --git a/tests/constraint/SampleGeneratorCoverage.hpp b/tests/constraint/SampleGeneratorCoverage.hpp
index 6942db0a7c..3794f9299f 100644
--- a/tests/constraint/SampleGeneratorCoverage.hpp
+++ b/tests/constraint/SampleGeneratorCoverage.hpp
@@ -1,7 +1,9 @@
 #ifndef AIKIDO_TESTS_CONSTRAINT_SAMPLEGENERATORCOVERAGE_H_
 #define AIKIDO_TESTS_CONSTRAINT_SAMPLEGENERATORCOVERAGE_H_
 #include <vector>
+
 #include <gtest/gtest.h>
+
 #include <aikido/constraint/Sampleable.hpp>
 #include <aikido/distance/DistanceMetric.hpp>
 
diff --git a/tests/constraint/test_CartesianProductProjectable.cpp b/tests/constraint/test_CartesianProductProjectable.cpp
index a0694af669..d35fa54446 100644
--- a/tests/constraint/test_CartesianProductProjectable.cpp
+++ b/tests/constraint/test_CartesianProductProjectable.cpp
@@ -1,9 +1,11 @@
 #include <dart/common/Memory.hpp>
 #include <gtest/gtest.h>
+
 #include <aikido/constraint/CartesianProductProjectable.hpp>
 #include <aikido/constraint/Projectable.hpp>
 #include <aikido/constraint/uniform/RnBoxConstraint.hpp>
 #include <aikido/statespace/Rn.hpp>
+
 #include "../eigen_tests.hpp"
 
 using aikido::constraint::CartesianProductProjectable;
diff --git a/tests/constraint/test_CartesianProductSampleable.cpp b/tests/constraint/test_CartesianProductSampleable.cpp
index 24b612a5e7..fdf9b2e88b 100644
--- a/tests/constraint/test_CartesianProductSampleable.cpp
+++ b/tests/constraint/test_CartesianProductSampleable.cpp
@@ -1,4 +1,5 @@
 #include <gtest/gtest.h>
+
 #include <aikido/common/RNG.hpp>
 #include <aikido/constraint/CartesianProductSampleable.hpp>
 #include <aikido/constraint/Sampleable.hpp>
@@ -6,6 +7,7 @@
 #include <aikido/constraint/uniform/SO2UniformSampler.hpp>
 #include <aikido/statespace/Rn.hpp>
 #include <aikido/statespace/SO2.hpp>
+
 #include "../eigen_tests.hpp"
 
 using aikido::common::RNG;
diff --git a/tests/constraint/test_CartesianProductTestable.cpp b/tests/constraint/test_CartesianProductTestable.cpp
index 9b0152a257..f807a3a5ba 100644
--- a/tests/constraint/test_CartesianProductTestable.cpp
+++ b/tests/constraint/test_CartesianProductTestable.cpp
@@ -1,10 +1,12 @@
 #include <gtest/gtest.h>
+
 #include <aikido/constraint/CartesianProductTestable.hpp>
 #include <aikido/constraint/Satisfied.hpp>
 #include <aikido/constraint/Testable.hpp>
 #include <aikido/constraint/uniform/RnBoxConstraint.hpp>
 #include <aikido/statespace/Rn.hpp>
 #include <aikido/statespace/SO2.hpp>
+
 #include "../eigen_tests.hpp"
 
 using aikido::constraint::CartesianProductTestable;
diff --git a/tests/constraint/test_CollisionFree.cpp b/tests/constraint/test_CollisionFree.cpp
index 2bfd4b9b15..282110bc0d 100644
--- a/tests/constraint/test_CollisionFree.cpp
+++ b/tests/constraint/test_CollisionFree.cpp
@@ -1,5 +1,6 @@
 #include <dart/dart.hpp>
 #include <gtest/gtest.h>
+
 #include <aikido/constraint/dart/CollisionFree.hpp>
 #include <aikido/statespace/Rn.hpp>
 #include <aikido/statespace/SE3.hpp>
diff --git a/tests/constraint/test_CyclicSampleable.cpp b/tests/constraint/test_CyclicSampleable.cpp
index 0e2be83bc9..7d989e8705 100644
--- a/tests/constraint/test_CyclicSampleable.cpp
+++ b/tests/constraint/test_CyclicSampleable.cpp
@@ -1,5 +1,6 @@
 #include <dart/common/Memory.hpp>
 #include <gtest/gtest.h>
+
 #include <aikido/common/memory.hpp>
 #include <aikido/constraint/CyclicSampleable.hpp>
 #include <aikido/constraint/FiniteSampleable.hpp>
diff --git a/tests/constraint/test_DartHelpers.cpp b/tests/constraint/test_DartHelpers.cpp
index 5c4f149326..6f1c283dec 100644
--- a/tests/constraint/test_DartHelpers.cpp
+++ b/tests/constraint/test_DartHelpers.cpp
@@ -1,6 +1,7 @@
 #include <dart/dynamics/dynamics.hpp>
 #include <gtest/gtest.h>
-#include "aikido/common/memory.hpp"
+
+#include <aikido/common/memory.hpp>
 #include <aikido/constraint/Satisfied.hpp>
 #include <aikido/constraint/dart/JointStateSpaceHelpers.hpp>
 #include <aikido/statespace/dart/RnJoint.hpp>
diff --git a/tests/constraint/test_Differentiable.cpp b/tests/constraint/test_Differentiable.cpp
index c3034d8a4c..a8c915ae49 100644
--- a/tests/constraint/test_Differentiable.cpp
+++ b/tests/constraint/test_Differentiable.cpp
@@ -1,7 +1,8 @@
 #include <gtest/gtest.h>
-#include "../eigen_tests.hpp"
 
 #include <aikido/constraint/Differentiable.hpp>
+
+#include "../eigen_tests.hpp"
 #include "PolynomialConstraint.hpp"
 
 using aikido::constraint::DifferentiablePtr;
diff --git a/tests/constraint/test_DifferentiableIntersection.cpp b/tests/constraint/test_DifferentiableIntersection.cpp
index c881c5c44f..366739a3e7 100644
--- a/tests/constraint/test_DifferentiableIntersection.cpp
+++ b/tests/constraint/test_DifferentiableIntersection.cpp
@@ -1,9 +1,12 @@
 #include <memory>
+
 #include <Eigen/Dense>
 #include <gtest/gtest.h>
+
 #include <aikido/constraint/DifferentiableIntersection.hpp>
 #include <aikido/constraint/dart/TSR.hpp>
 #include <aikido/statespace/Rn.hpp>
+
 #include "PolynomialConstraint.hpp"
 
 using aikido::constraint::DifferentiableIntersection;
diff --git a/tests/constraint/test_DifferentiableSubspace.cpp b/tests/constraint/test_DifferentiableSubspace.cpp
index 1f9929d9c4..edae094744 100644
--- a/tests/constraint/test_DifferentiableSubspace.cpp
+++ b/tests/constraint/test_DifferentiableSubspace.cpp
@@ -1,8 +1,10 @@
 #include <gtest/gtest.h>
+
 #include <aikido/constraint/DifferentiableSubspace.hpp>
 #include <aikido/constraint/Satisfied.hpp>
 #include <aikido/statespace/Rn.hpp>
 #include <aikido/statespace/SO2.hpp>
+
 #include "../eigen_tests.hpp"
 #include "PolynomialConstraint.hpp"
 
diff --git a/tests/constraint/test_FiniteSampleable.cpp b/tests/constraint/test_FiniteSampleable.cpp
index 679d214d84..7131161a1e 100644
--- a/tests/constraint/test_FiniteSampleable.cpp
+++ b/tests/constraint/test_FiniteSampleable.cpp
@@ -1,10 +1,11 @@
 #include <Eigen/StdVector>
+#include <gtest/gtest.h>
+
 #include <aikido/constraint/FiniteSampleable.hpp>
 #include <aikido/statespace/Rn.hpp>
 #include <aikido/statespace/StateSpace.hpp>
-#include "../eigen_tests.hpp"
 
-#include <gtest/gtest.h>
+#include "../eigen_tests.hpp"
 
 using aikido::constraint::FiniteSampleable;
 using aikido::constraint::SampleGenerator;
diff --git a/tests/constraint/test_FrameDifferentiable.cpp b/tests/constraint/test_FrameDifferentiable.cpp
index 9a1fc7ad59..02b9ddce66 100644
--- a/tests/constraint/test_FrameDifferentiable.cpp
+++ b/tests/constraint/test_FrameDifferentiable.cpp
@@ -1,3 +1,6 @@
+#include <Eigen/Dense>
+#include <gtest/gtest.h>
+
 #include <aikido/common/RNG.hpp>
 #include <aikido/constraint/Satisfied.hpp>
 #include <aikido/constraint/dart/FrameDifferentiable.hpp>
@@ -7,9 +10,6 @@
 #include <aikido/statespace/SO2.hpp>
 #include <aikido/statespace/dart/MetaSkeletonStateSpace.hpp>
 
-#include <Eigen/Dense>
-#include <gtest/gtest.h>
-
 using aikido::common::RNG;
 using aikido::common::RNGWrapper;
 using aikido::constraint::dart::FrameDifferentiable;
diff --git a/tests/constraint/test_FramePairDifferentiable.cpp b/tests/constraint/test_FramePairDifferentiable.cpp
index fa9342817e..08cea9c848 100644
--- a/tests/constraint/test_FramePairDifferentiable.cpp
+++ b/tests/constraint/test_FramePairDifferentiable.cpp
@@ -1,3 +1,6 @@
+#include <Eigen/Dense>
+#include <gtest/gtest.h>
+
 #include <aikido/common/RNG.hpp>
 #include <aikido/constraint/Satisfied.hpp>
 #include <aikido/constraint/dart/FramePairDifferentiable.hpp>
@@ -7,9 +10,6 @@
 #include <aikido/statespace/SO2.hpp>
 #include <aikido/statespace/dart/MetaSkeletonStateSpace.hpp>
 
-#include <Eigen/Dense>
-#include <gtest/gtest.h>
-
 using aikido::common::RNG;
 using aikido::common::RNGWrapper;
 using aikido::constraint::dart::FramePairDifferentiable;
diff --git a/tests/constraint/test_FrameTestable.cpp b/tests/constraint/test_FrameTestable.cpp
index ba9b974a6f..e79b904b56 100644
--- a/tests/constraint/test_FrameTestable.cpp
+++ b/tests/constraint/test_FrameTestable.cpp
@@ -1,7 +1,9 @@
 #include <gtest/gtest.h>
+
 #include <aikido/constraint/dart/FrameTestable.hpp>
 #include <aikido/statespace/SE3.hpp>
 #include <aikido/statespace/SO2.hpp>
+
 #include "MockConstraints.hpp"
 
 using aikido::constraint::DefaultTestableOutcome;
diff --git a/tests/constraint/test_InverseKinematicsSampleable.cpp b/tests/constraint/test_InverseKinematicsSampleable.cpp
index 21facc7024..e6d170c724 100644
--- a/tests/constraint/test_InverseKinematicsSampleable.cpp
+++ b/tests/constraint/test_InverseKinematicsSampleable.cpp
@@ -1,6 +1,8 @@
 #include <random>
+
 #include <Eigen/Dense>
 #include <gtest/gtest.h>
+
 #include <aikido/common/RNG.hpp>
 #include <aikido/constraint/CyclicSampleable.hpp>
 #include <aikido/constraint/FiniteSampleable.hpp>
@@ -10,6 +12,7 @@
 #include <aikido/statespace/SE3.hpp>
 #include <aikido/statespace/SO2.hpp>
 #include <aikido/statespace/StateSpace.hpp>
+
 #include "MockConstraints.hpp"
 
 using aikido::common::RNG;
diff --git a/tests/constraint/test_NewtonsMethodProjectable.cpp b/tests/constraint/test_NewtonsMethodProjectable.cpp
index 9517456868..5098737e9e 100644
--- a/tests/constraint/test_NewtonsMethodProjectable.cpp
+++ b/tests/constraint/test_NewtonsMethodProjectable.cpp
@@ -1,12 +1,12 @@
+#include <Eigen/Dense>
+#include <gtest/gtest.h>
+
 #include <aikido/constraint/NewtonsMethodProjectable.hpp>
 #include <aikido/constraint/Satisfied.hpp>
 #include <aikido/constraint/dart/TSR.hpp>
-#include "PolynomialConstraint.hpp"
-
 #include <aikido/statespace/Rn.hpp>
 
-#include <Eigen/Dense>
-#include <gtest/gtest.h>
+#include "PolynomialConstraint.hpp"
 
 using aikido::constraint::NewtonsMethodProjectable;
 using aikido::constraint::Satisfied;
diff --git a/tests/constraint/test_PolynomialConstraint.cpp b/tests/constraint/test_PolynomialConstraint.cpp
index 8cb8fdcdf4..b4c3fd57f4 100644
--- a/tests/constraint/test_PolynomialConstraint.cpp
+++ b/tests/constraint/test_PolynomialConstraint.cpp
@@ -1,6 +1,8 @@
 #include <Eigen/Dense>
 #include <gtest/gtest.h>
+
 #include <aikido/common/RNG.hpp>
+
 #include "PolynomialConstraint.hpp"
 
 using aikido::statespace::R1;
diff --git a/tests/constraint/test_Projectable.cpp b/tests/constraint/test_Projectable.cpp
index 994b58f407..8ffb00727a 100644
--- a/tests/constraint/test_Projectable.cpp
+++ b/tests/constraint/test_Projectable.cpp
@@ -1,9 +1,11 @@
 #include <dart/common/Console.hpp>
 #include <gtest/gtest.h>
+
 #include <aikido/constraint/CartesianProductProjectable.hpp>
 #include <aikido/constraint/Projectable.hpp>
 #include <aikido/constraint/uniform/RnBoxConstraint.hpp>
 #include <aikido/statespace/Rn.hpp>
+
 #include "../eigen_tests.hpp"
 
 using aikido::constraint::CartesianProductProjectable;
diff --git a/tests/constraint/test_RejectionSampleable.cpp b/tests/constraint/test_RejectionSampleable.cpp
index 23d094b9eb..ef30c7fa19 100644
--- a/tests/constraint/test_RejectionSampleable.cpp
+++ b/tests/constraint/test_RejectionSampleable.cpp
@@ -1,5 +1,6 @@
 #include <Eigen/Dense>
 #include <gtest/gtest.h>
+
 #include <aikido/constraint/FiniteSampleable.hpp>
 #include <aikido/constraint/RejectionSampleable.hpp>
 #include <aikido/statespace/Rn.hpp>
diff --git a/tests/constraint/test_RnBoxConstraint.cpp b/tests/constraint/test_RnBoxConstraint.cpp
index 4140e3889b..ae6ed21530 100644
--- a/tests/constraint/test_RnBoxConstraint.cpp
+++ b/tests/constraint/test_RnBoxConstraint.cpp
@@ -1,8 +1,10 @@
 #include <dart/common/Memory.hpp>
 #include <gtest/gtest.h>
+
 #include <aikido/common/memory.hpp>
 #include <aikido/constraint/uniform/RnBoxConstraint.hpp>
 #include <aikido/distance/RnEuclidean.hpp>
+
 #include "SampleGeneratorCoverage.hpp"
 
 using aikido::common::RNG;
diff --git a/tests/constraint/test_RnConstantSampler.cpp b/tests/constraint/test_RnConstantSampler.cpp
index 772b52a713..cfe1c5f9a4 100644
--- a/tests/constraint/test_RnConstantSampler.cpp
+++ b/tests/constraint/test_RnConstantSampler.cpp
@@ -1,6 +1,8 @@
 #include <dart/dart.hpp>
 #include <gtest/gtest.h>
+
 #include <aikido/constraint/uniform/RnConstantSampler.hpp>
+
 #include "eigen_tests.hpp"
 
 using namespace aikido;
diff --git a/tests/constraint/test_SE2BoxConstraint.cpp b/tests/constraint/test_SE2BoxConstraint.cpp
index 7e1213bfa6..901aa0437e 100644
--- a/tests/constraint/test_SE2BoxConstraint.cpp
+++ b/tests/constraint/test_SE2BoxConstraint.cpp
@@ -1,8 +1,10 @@
 #include <dart/common/Memory.hpp>
 #include <gtest/gtest.h>
+
 #include <aikido/common/memory.hpp>
 #include <aikido/constraint/uniform/SE2BoxConstraint.hpp>
 #include <aikido/distance/SE2Weighted.hpp>
+
 #include "SampleGeneratorCoverage.hpp"
 
 using aikido::common::RNG;
diff --git a/tests/constraint/test_SO2UniformSampler.cpp b/tests/constraint/test_SO2UniformSampler.cpp
index 680e3bfbc4..d45146f8ef 100644
--- a/tests/constraint/test_SO2UniformSampler.cpp
+++ b/tests/constraint/test_SO2UniformSampler.cpp
@@ -1,7 +1,9 @@
 #include <gtest/gtest.h>
-#include "aikido/common/memory.hpp"
+
+#include <aikido/common/memory.hpp>
 #include <aikido/constraint/uniform/SO2UniformSampler.hpp>
 #include <aikido/distance/SO2Angular.hpp>
+
 #include "SampleGeneratorCoverage.hpp"
 
 using aikido::common::RNG;
diff --git a/tests/constraint/test_SO3UniformSampler.cpp b/tests/constraint/test_SO3UniformSampler.cpp
index 651148a6fe..ead5696f38 100644
--- a/tests/constraint/test_SO3UniformSampler.cpp
+++ b/tests/constraint/test_SO3UniformSampler.cpp
@@ -1,7 +1,9 @@
 #include <gtest/gtest.h>
-#include "aikido/common/memory.hpp"
+
+#include <aikido/common/memory.hpp>
 #include <aikido/constraint/uniform/SO3UniformSampler.hpp>
 #include <aikido/distance/SO3Angular.hpp>
+
 #include "SampleGeneratorCoverage.hpp"
 
 using aikido::common::RNG;
diff --git a/tests/constraint/test_Satisfied.cpp b/tests/constraint/test_Satisfied.cpp
index fb286e0bf1..6af4c0d830 100644
--- a/tests/constraint/test_Satisfied.cpp
+++ b/tests/constraint/test_Satisfied.cpp
@@ -1,4 +1,5 @@
 #include <gtest/gtest.h>
+
 #include <aikido/constraint/Satisfied.hpp>
 #include <aikido/statespace/Rn.hpp>
 
diff --git a/tests/constraint/test_SequentialSampleable.cpp b/tests/constraint/test_SequentialSampleable.cpp
index e1830cc68d..6fadb87881 100644
--- a/tests/constraint/test_SequentialSampleable.cpp
+++ b/tests/constraint/test_SequentialSampleable.cpp
@@ -1,13 +1,13 @@
-#include <aikido/constraint/SequentialSampleable.hpp>
-
 #include <Eigen/StdVector>
+#include <gtest/gtest.h>
+
 #include <aikido/constraint/CyclicSampleable.hpp>
 #include <aikido/constraint/FiniteSampleable.hpp>
+#include <aikido/constraint/SequentialSampleable.hpp>
 #include <aikido/statespace/Rn.hpp>
 #include <aikido/statespace/StateSpace.hpp>
-#include "../eigen_tests.hpp"
 
-#include <gtest/gtest.h>
+#include "../eigen_tests.hpp"
 
 using aikido::constraint::ConstSampleablePtr;
 using aikido::constraint::CyclicSampleable;
diff --git a/tests/constraint/test_TSR.cpp b/tests/constraint/test_TSR.cpp
index 2f6d633460..5b69b5ac07 100644
--- a/tests/constraint/test_TSR.cpp
+++ b/tests/constraint/test_TSR.cpp
@@ -2,8 +2,9 @@
 #include <Eigen/Geometry>
 #include <dart/math/Geometry.hpp>
 #include <gtest/gtest.h>
-#include "aikido/common/memory.hpp"
+
 #include <aikido/common/RNG.hpp>
+#include <aikido/common/memory.hpp>
 #include <aikido/constraint/Differentiable.hpp>
 #include <aikido/constraint/dart/TSR.hpp>
 #include <aikido/statespace/SE3.hpp>
diff --git a/tests/constraint/test_TestableIntersection.cpp b/tests/constraint/test_TestableIntersection.cpp
index 9457cdedeb..c4d54601b1 100644
--- a/tests/constraint/test_TestableIntersection.cpp
+++ b/tests/constraint/test_TestableIntersection.cpp
@@ -1,8 +1,11 @@
 #include <stdexcept>
+
 #include <gtest/gtest.h>
+
 #include <aikido/constraint/TestableIntersection.hpp>
 #include <aikido/statespace/Rn.hpp>
 #include <aikido/statespace/SO2.hpp>
+
 #include "MockConstraints.hpp"
 
 using aikido::constraint::Testable;
diff --git a/tests/control/ros/test_Conversions.cpp b/tests/control/ros/test_Conversions.cpp
index 72e4bf818e..fbbef5c1aa 100644
--- a/tests/control/ros/test_Conversions.cpp
+++ b/tests/control/ros/test_Conversions.cpp
@@ -1,12 +1,14 @@
 #include <dart/dart.hpp>
 #include <gtest/gtest.h>
+
 #include <aikido/control/ros/Conversions.hpp>
+
 #include "eigen_tests.hpp"
 
-using dart::dynamics::SkeletonPtr;
 using aikido::control::ros::toSplineJointTrajectory;
 using aikido::statespace::dart::MetaSkeletonStateSpace;
 using aikido::tests::make_vector;
+using dart::dynamics::SkeletonPtr;
 
 static const double kTolerance{1e-6};
 
diff --git a/tests/control/ros/test_RosTrajectoryConversions.cpp b/tests/control/ros/test_RosTrajectoryConversions.cpp
index ded5cf3da4..bd0a7c04ca 100644
--- a/tests/control/ros/test_RosTrajectoryConversions.cpp
+++ b/tests/control/ros/test_RosTrajectoryConversions.cpp
@@ -1,20 +1,22 @@
 #include <dart/dart.hpp>
 #include <gtest/gtest.h>
+
 #include <aikido/control/ros/Conversions.hpp>
 #include <aikido/statespace/Rn.hpp>
 #include <aikido/trajectory/Spline.hpp>
+
 #include "eigen_tests.hpp"
 
-using dart::dynamics::SkeletonPtr;
-using dart::dynamics::Skeleton;
-using dart::dynamics::BodyNode;
-using dart::dynamics::RevoluteJoint;
-using dart::dynamics::BallJoint;
-using aikido::trajectory::Spline;
 using aikido::control::ros::toRosJointTrajectory;
-using aikido::statespace::dart::MetaSkeletonStateSpacePtr;
 using aikido::statespace::dart::MetaSkeletonStateSpace;
+using aikido::statespace::dart::MetaSkeletonStateSpacePtr;
 using aikido::tests::make_vector;
+using aikido::trajectory::Spline;
+using dart::dynamics::BallJoint;
+using dart::dynamics::BodyNode;
+using dart::dynamics::RevoluteJoint;
+using dart::dynamics::Skeleton;
+using dart::dynamics::SkeletonPtr;
 
 static const double kTolerance{1e-6};
 
diff --git a/tests/control/test_InstantaneousTrajectoryExecutor.cpp b/tests/control/test_InstantaneousTrajectoryExecutor.cpp
index 4d4e3054eb..4cfe3c6926 100644
--- a/tests/control/test_InstantaneousTrajectoryExecutor.cpp
+++ b/tests/control/test_InstantaneousTrajectoryExecutor.cpp
@@ -1,6 +1,8 @@
 #include <chrono>
+
 #include <dart/dart.hpp>
 #include <gtest/gtest.h>
+
 #include <aikido/control/InstantaneousTrajectoryExecutor.hpp>
 #include <aikido/statespace/GeodesicInterpolator.hpp>
 #include <aikido/statespace/SO2.hpp>
diff --git a/tests/control/test_KinematicSimulationTrajectoryExecutor.cpp b/tests/control/test_KinematicSimulationTrajectoryExecutor.cpp
index 2eeaf01f1b..1dfb64aa9c 100644
--- a/tests/control/test_KinematicSimulationTrajectoryExecutor.cpp
+++ b/tests/control/test_KinematicSimulationTrajectoryExecutor.cpp
@@ -1,5 +1,7 @@
 #include <chrono>
+
 #include <gtest/gtest.h>
+
 #include <aikido/control/KinematicSimulationTrajectoryExecutor.hpp>
 #include <aikido/statespace/GeodesicInterpolator.hpp>
 #include <aikido/statespace/SO2.hpp>
diff --git a/tests/control/test_QueuedTrajectoryExecutor.cpp b/tests/control/test_QueuedTrajectoryExecutor.cpp
index 9d1bc422fd..ecd0a435a8 100644
--- a/tests/control/test_QueuedTrajectoryExecutor.cpp
+++ b/tests/control/test_QueuedTrajectoryExecutor.cpp
@@ -1,5 +1,7 @@
 #include <chrono>
+
 #include <gtest/gtest.h>
+
 #include <aikido/control/KinematicSimulationTrajectoryExecutor.hpp>
 #include <aikido/control/QueuedTrajectoryExecutor.hpp>
 #include <aikido/control/TrajectoryExecutor.hpp>
diff --git a/tests/distance/test_CartesianProductWeighted.cpp b/tests/distance/test_CartesianProductWeighted.cpp
index ed4fdcc1f1..6e25c71e4a 100644
--- a/tests/distance/test_CartesianProductWeighted.cpp
+++ b/tests/distance/test_CartesianProductWeighted.cpp
@@ -1,11 +1,11 @@
+#include <gtest/gtest.h>
+
 #include <aikido/distance/CartesianProductWeighted.hpp>
 #include <aikido/distance/RnEuclidean.hpp>
 #include <aikido/distance/SO2Angular.hpp>
 #include <aikido/distance/SO3Angular.hpp>
 #include <aikido/statespace/CartesianProduct.hpp>
 
-#include <gtest/gtest.h>
-
 using namespace aikido::distance;
 using namespace aikido::statespace;
 
diff --git a/tests/distance/test_DistanceMetricDefaults.cpp b/tests/distance/test_DistanceMetricDefaults.cpp
index cd7b406395..179b66fa82 100644
--- a/tests/distance/test_DistanceMetricDefaults.cpp
+++ b/tests/distance/test_DistanceMetricDefaults.cpp
@@ -1,3 +1,5 @@
+#include <gtest/gtest.h>
+
 #include <aikido/distance/CartesianProductWeighted.hpp>
 #include <aikido/distance/RnEuclidean.hpp>
 #include <aikido/distance/SO2Angular.hpp>
@@ -5,8 +7,6 @@
 #include <aikido/distance/defaults.hpp>
 #include <aikido/statespace/CartesianProduct.hpp>
 
-#include <gtest/gtest.h>
-
 using namespace aikido::distance;
 using namespace aikido::statespace;
 
diff --git a/tests/distance/test_JointAvoidanceConfigurationRanker.cpp b/tests/distance/test_JointAvoidanceConfigurationRanker.cpp
index 934718c0de..ee6b6addd8 100644
--- a/tests/distance/test_JointAvoidanceConfigurationRanker.cpp
+++ b/tests/distance/test_JointAvoidanceConfigurationRanker.cpp
@@ -1,11 +1,13 @@
-#include "aikido/distance/JointAvoidanceConfigurationRanker.hpp"
-
 #include <random>
+
 #include <Eigen/Dense>
 #include <gtest/gtest.h>
+
 #include <aikido/common/RNG.hpp>
+#include <aikido/distance/JointAvoidanceConfigurationRanker.hpp>
 #include <aikido/statespace/Rn.hpp>
 #include <aikido/statespace/StateSpace.hpp>
+
 #include "eigen_tests.hpp"
 
 using aikido::distance::JointAvoidanceConfigurationRanker;
diff --git a/tests/distance/test_NominalConfigurationRanker.cpp b/tests/distance/test_NominalConfigurationRanker.cpp
index 409c31826d..bd86771bdb 100644
--- a/tests/distance/test_NominalConfigurationRanker.cpp
+++ b/tests/distance/test_NominalConfigurationRanker.cpp
@@ -1,10 +1,12 @@
-#include "aikido/distance/NominalConfigurationRanker.hpp"
-
 #include <random>
+
 #include <Eigen/Dense>
 #include <gtest/gtest.h>
+
 #include <aikido/common/RNG.hpp>
+#include <aikido/distance/NominalConfigurationRanker.hpp>
 #include <aikido/statespace/StateSpace.hpp>
+
 #include "eigen_tests.hpp"
 
 static constexpr double EPS = 1e-6;
diff --git a/tests/distance/test_RnEuclidean.cpp b/tests/distance/test_RnEuclidean.cpp
index 36d463880e..e82ad71587 100644
--- a/tests/distance/test_RnEuclidean.cpp
+++ b/tests/distance/test_RnEuclidean.cpp
@@ -1,8 +1,8 @@
+#include <gtest/gtest.h>
+
 #include <aikido/distance/RnEuclidean.hpp>
 #include <aikido/statespace/Rn.hpp>
 
-#include <gtest/gtest.h>
-
 using namespace aikido::distance;
 using namespace aikido::statespace;
 
diff --git a/tests/distance/test_SE2Weighted.cpp b/tests/distance/test_SE2Weighted.cpp
index 01373d9156..cff9b50c1f 100644
--- a/tests/distance/test_SE2Weighted.cpp
+++ b/tests/distance/test_SE2Weighted.cpp
@@ -1,8 +1,8 @@
+#include <gtest/gtest.h>
+
 #include <aikido/distance/SE2Weighted.hpp>
 #include <aikido/statespace/SE2.hpp>
 
-#include <gtest/gtest.h>
-
 using namespace aikido::distance;
 using namespace aikido::statespace;
 
diff --git a/tests/distance/test_SO2Angular.cpp b/tests/distance/test_SO2Angular.cpp
index cb74775c8e..9a36d8a01a 100644
--- a/tests/distance/test_SO2Angular.cpp
+++ b/tests/distance/test_SO2Angular.cpp
@@ -1,8 +1,8 @@
+#include <gtest/gtest.h>
+
 #include <aikido/distance/SO2Angular.hpp>
 #include <aikido/statespace/SO2.hpp>
 
-#include <gtest/gtest.h>
-
 using namespace aikido::distance;
 using namespace aikido::statespace;
 
diff --git a/tests/distance/test_SO3Angular.cpp b/tests/distance/test_SO3Angular.cpp
index e81167937a..a7ada64f68 100644
--- a/tests/distance/test_SO3Angular.cpp
+++ b/tests/distance/test_SO3Angular.cpp
@@ -1,8 +1,8 @@
+#include <gtest/gtest.h>
+
 #include <aikido/distance/SO3Angular.hpp>
 #include <aikido/statespace/SO3.hpp>
 
-#include <gtest/gtest.h>
-
 using namespace aikido::distance;
 using namespace aikido::statespace;
 
diff --git a/tests/io/test_CatkinResourceRetriever.cpp b/tests/io/test_CatkinResourceRetriever.cpp
index 0e5385b90e..e64527bafa 100644
--- a/tests/io/test_CatkinResourceRetriever.cpp
+++ b/tests/io/test_CatkinResourceRetriever.cpp
@@ -1,5 +1,7 @@
 #include <cstdlib>
+
 #include <gtest/gtest.h>
+
 #include <aikido/io/CatkinResourceRetriever.hpp>
 
 #define STR_EXPAND(tok) #tok
diff --git a/tests/io/test_KinBodyParser.cpp b/tests/io/test_KinBodyParser.cpp
index bed8c859bb..103ff8ed7b 100644
--- a/tests/io/test_KinBodyParser.cpp
+++ b/tests/io/test_KinBodyParser.cpp
@@ -1,6 +1,8 @@
 #include <dart/utils/utils.hpp>
 #include <gtest/gtest.h>
+
 #include <aikido/io/KinBodyParser.hpp>
+
 #include "eigen_tests.hpp"
 
 #define STR_EXPAND(tok) #tok
diff --git a/tests/io/test_loadSkeletonFromURDF.cpp b/tests/io/test_loadSkeletonFromURDF.cpp
index 533bb3889e..63efef8184 100644
--- a/tests/io/test_loadSkeletonFromURDF.cpp
+++ b/tests/io/test_loadSkeletonFromURDF.cpp
@@ -1,6 +1,8 @@
 #include <dart/utils/utils.hpp>
 #include <gtest/gtest.h>
+
 #include <aikido/io/util.hpp>
+
 #include "eigen_tests.hpp"
 
 #define STR_EXPAND(tok) #tok
diff --git a/tests/io/test_trajectory_io.cpp b/tests/io/test_trajectory_io.cpp
index 9b2bfa6631..8b6429ca94 100644
--- a/tests/io/test_trajectory_io.cpp
+++ b/tests/io/test_trajectory_io.cpp
@@ -1,30 +1,31 @@
-#include "aikido/io/trajectory.hpp"
-
 #include <tuple>
+
 #include <dart/dart.hpp>
 #include <gtest/gtest.h>
+
 #include <aikido/constraint/Satisfied.hpp>
 #include <aikido/constraint/Testable.hpp>
+#include <aikido/io/trajectory.hpp>
 #include <aikido/io/yaml.hpp>
 #include <aikido/planner/ConfigurationToConfiguration.hpp>
 #include <aikido/planner/SnapConfigurationToConfigurationPlanner.hpp>
 #include <aikido/statespace/GeodesicInterpolator.hpp>
 #include <aikido/statespace/SO2.hpp>
 #include <aikido/statespace/dart/MetaSkeletonStateSpace.hpp>
-
 #include <aikido/trajectory/Interpolated.hpp>
 #include <aikido/trajectory/Spline.hpp>
 #include <aikido/trajectory/util.hpp>
+
 #include "../constraint/MockConstraints.hpp"
 
-using std::make_shared;
 using aikido::io::loadSplineTrajectory;
 using aikido::io::saveTrajectory;
-using aikido::trajectory::Interpolated;
 using aikido::planner::ConfigurationToConfiguration;
 using aikido::planner::SnapConfigurationToConfigurationPlanner;
 using aikido::statespace::ConstStateSpacePtr;
 using aikido::trajectory::convertToSpline;
+using aikido::trajectory::Interpolated;
+using std::make_shared;
 
 //==============================================================================
 class SaveLoadTrajectoryTest : public ::testing::Test
diff --git a/tests/io/test_yaml_extension.cpp b/tests/io/test_yaml_extension.cpp
index 1e62f9e426..13468bb35c 100644
--- a/tests/io/test_yaml_extension.cpp
+++ b/tests/io/test_yaml_extension.cpp
@@ -1,4 +1,5 @@
 #include <gtest/gtest.h>
+
 #include <aikido/io/yaml.hpp>
 
 //==============================================================================
diff --git a/tests/planner/kunzretimer/test_KunzRetimer.cpp b/tests/planner/kunzretimer/test_KunzRetimer.cpp
index 3888cbd043..aeb8acd098 100644
--- a/tests/planner/kunzretimer/test_KunzRetimer.cpp
+++ b/tests/planner/kunzretimer/test_KunzRetimer.cpp
@@ -1,4 +1,5 @@
 #include <gtest/gtest.h>
+
 #include <aikido/planner/kunzretimer/KunzRetimer.hpp>
 #include <aikido/planner/parabolic/ParabolicTimer.hpp>
 #include <aikido/statespace/CartesianProduct.hpp>
@@ -6,6 +7,7 @@
 #include <aikido/statespace/Rn.hpp>
 #include <aikido/statespace/SO2.hpp>
 #include <aikido/trajectory/util.hpp>
+
 #include "eigen_tests.hpp"
 
 using aikido::planner::kunzretimer::computeKunzTiming;
diff --git a/tests/planner/kunzretimer/test_KunzRetimerPostProcessor.cpp b/tests/planner/kunzretimer/test_KunzRetimerPostProcessor.cpp
index a69c86486c..481fabd377 100644
--- a/tests/planner/kunzretimer/test_KunzRetimerPostProcessor.cpp
+++ b/tests/planner/kunzretimer/test_KunzRetimerPostProcessor.cpp
@@ -1,4 +1,5 @@
 #include <gtest/gtest.h>
+
 #include <aikido/common/RNG.hpp>
 #include <aikido/planner/kunzretimer/KunzRetimer.hpp>
 #include <aikido/planner/parabolic/ParabolicTimer.hpp>
@@ -6,6 +7,7 @@
 #include <aikido/statespace/GeodesicInterpolator.hpp>
 #include <aikido/statespace/Rn.hpp>
 #include <aikido/trajectory/util.hpp>
+
 #include "eigen_tests.hpp"
 
 using aikido::planner::kunzretimer::KunzRetimer;
diff --git a/tests/planner/ompl/OMPLTestHelpers.hpp b/tests/planner/ompl/OMPLTestHelpers.hpp
index 83b9423709..3bc643847c 100644
--- a/tests/planner/ompl/OMPLTestHelpers.hpp
+++ b/tests/planner/ompl/OMPLTestHelpers.hpp
@@ -4,6 +4,7 @@
 #include <dart/dart.hpp>
 #include <gtest/gtest.h>
 #include <ompl/base/State.h>
+
 #include <aikido/common/RNG.hpp>
 #include <aikido/constraint.hpp>
 #include <aikido/distance/defaults.hpp>
diff --git a/tests/planner/ompl/test_GeometricStateSpace.cpp b/tests/planner/ompl/test_GeometricStateSpace.cpp
index 151ea5ff8e..d5b8dfffce 100644
--- a/tests/planner/ompl/test_GeometricStateSpace.cpp
+++ b/tests/planner/ompl/test_GeometricStateSpace.cpp
@@ -1,4 +1,5 @@
 #include <aikido/planner/ompl/GeometricStateSpace.hpp>
+
 #include "OMPLTestHelpers.hpp"
 
 using aikido::planner::ompl::GeometricStateSpace;
diff --git a/tests/planner/ompl/test_GoalRegion.cpp b/tests/planner/ompl/test_GoalRegion.cpp
index de517ed478..7cc529fc90 100644
--- a/tests/planner/ompl/test_GoalRegion.cpp
+++ b/tests/planner/ompl/test_GoalRegion.cpp
@@ -1,6 +1,8 @@
 #include <ompl/base/spaces/SO2StateSpace.h>
+
 #include <aikido/planner/ompl/GoalRegion.hpp>
 #include <aikido/planner/ompl/Planner.hpp>
+
 #include "../../constraint/MockConstraints.hpp"
 #include "OMPLTestHelpers.hpp"
 
diff --git a/tests/planner/ompl/test_MotionValidator.cpp b/tests/planner/ompl/test_MotionValidator.cpp
index 9676a15668..b37b0cc592 100644
--- a/tests/planner/ompl/test_MotionValidator.cpp
+++ b/tests/planner/ompl/test_MotionValidator.cpp
@@ -1,9 +1,11 @@
 #include <boost/make_shared.hpp>
 #include <gtest/gtest.h>
+
 #include <aikido/planner/ompl/GeometricStateSpace.hpp>
 #include <aikido/planner/ompl/MotionValidator.hpp>
 #include <aikido/planner/ompl/StateValidityChecker.hpp>
 #include <aikido/planner/ompl/dart.hpp>
+
 #include "../../constraint/MockConstraints.hpp"
 #include "OMPLTestHelpers.hpp"
 
diff --git a/tests/planner/ompl/test_OMPLConfigurationToConfigurationPlanner.cpp b/tests/planner/ompl/test_OMPLConfigurationToConfigurationPlanner.cpp
index 98954521cc..e5f98e1001 100644
--- a/tests/planner/ompl/test_OMPLConfigurationToConfigurationPlanner.cpp
+++ b/tests/planner/ompl/test_OMPLConfigurationToConfigurationPlanner.cpp
@@ -1,4 +1,5 @@
 #include <ompl/geometric/planners/rrt/RRTConnect.h>
+
 #include <aikido/common/StepSequence.hpp>
 #include <aikido/constraint.hpp>
 #include <aikido/planner/ConfigurationToConfiguration.hpp>
@@ -7,6 +8,7 @@
 #include <aikido/planner/ompl/Planner.hpp>
 #include <aikido/statespace/SO2.hpp>
 #include <aikido/trajectory/Interpolated.hpp>
+
 #include "../../constraint/MockConstraints.hpp"
 #include "OMPLTestHelpers.hpp"
 
diff --git a/tests/planner/ompl/test_OMPLPlanner.cpp b/tests/planner/ompl/test_OMPLPlanner.cpp
index 7272300679..d720757115 100644
--- a/tests/planner/ompl/test_OMPLPlanner.cpp
+++ b/tests/planner/ompl/test_OMPLPlanner.cpp
@@ -1,10 +1,12 @@
 #include <ompl/geometric/planners/rrt/RRTConnect.h>
+
 #include <aikido/common/StepSequence.hpp>
 #include <aikido/constraint.hpp>
 #include <aikido/planner/ompl/CRRT.hpp>
 #include <aikido/planner/ompl/CRRTConnect.hpp>
 #include <aikido/planner/ompl/MotionValidator.hpp>
 #include <aikido/planner/ompl/Planner.hpp>
+
 #include "../../constraint/MockConstraints.hpp"
 #include "OMPLTestHelpers.hpp"
 
diff --git a/tests/planner/ompl/test_OMPLSimplifier.cpp b/tests/planner/ompl/test_OMPLSimplifier.cpp
index 5747df7682..bff5ec35a2 100644
--- a/tests/planner/ompl/test_OMPLSimplifier.cpp
+++ b/tests/planner/ompl/test_OMPLSimplifier.cpp
@@ -1,8 +1,10 @@
 #include <dart/dart.hpp>
+
 #include <aikido/common/StepSequence.hpp>
 #include <aikido/planner/ompl/MotionValidator.hpp>
 #include <aikido/planner/ompl/Planner.hpp>
 #include <aikido/trajectory/Interpolated.hpp>
+
 #include "../../constraint/MockConstraints.hpp"
 #include "OMPLTestHelpers.hpp"
 #include "eigen_tests.hpp"
diff --git a/tests/planner/ompl/test_StateSampler.cpp b/tests/planner/ompl/test_StateSampler.cpp
index 48bf00f6b3..491cd4fecd 100644
--- a/tests/planner/ompl/test_StateSampler.cpp
+++ b/tests/planner/ompl/test_StateSampler.cpp
@@ -1,6 +1,8 @@
 #include <boost/make_shared.hpp>
 #include <ompl/base/spaces/SO2StateSpace.h>
+
 #include <aikido/planner/ompl/StateSampler.hpp>
+
 #include "OMPLTestHelpers.hpp"
 
 using aikido::planner::ompl::GeometricStateSpace;
diff --git a/tests/planner/ompl/test_TrajectoryConversions.cpp b/tests/planner/ompl/test_TrajectoryConversions.cpp
index 5ad4bfb4ca..36295ca009 100644
--- a/tests/planner/ompl/test_TrajectoryConversions.cpp
+++ b/tests/planner/ompl/test_TrajectoryConversions.cpp
@@ -1,6 +1,7 @@
 #include <aikido/common/StepSequence.hpp>
 #include <aikido/planner/ompl/MotionValidator.hpp>
 #include <aikido/planner/ompl/Planner.hpp>
+
 #include "../../constraint/MockConstraints.hpp"
 #include "OMPLTestHelpers.hpp"
 #include "eigen_tests.hpp"
diff --git a/tests/planner/ompl/test_ValidityChecker.cpp b/tests/planner/ompl/test_ValidityChecker.cpp
index bc4efa5213..a601d4a4e6 100644
--- a/tests/planner/ompl/test_ValidityChecker.cpp
+++ b/tests/planner/ompl/test_ValidityChecker.cpp
@@ -1,5 +1,6 @@
 #include <aikido/planner/ompl/Planner.hpp>
 #include <aikido/planner/ompl/StateValidityChecker.hpp>
+
 #include "../../constraint/MockConstraints.hpp"
 #include "OMPLTestHelpers.hpp"
 
diff --git a/tests/planner/parabolic/test_ParabolicSmoother.cpp b/tests/planner/parabolic/test_ParabolicSmoother.cpp
index 3a6fc13a17..6d92b70db7 100644
--- a/tests/planner/parabolic/test_ParabolicSmoother.cpp
+++ b/tests/planner/parabolic/test_ParabolicSmoother.cpp
@@ -1,4 +1,5 @@
 #include <gtest/gtest.h>
+
 #include <aikido/common/StepSequence.hpp>
 #include <aikido/constraint/Satisfied.hpp>
 #include <aikido/planner/parabolic/ParabolicSmoother.hpp>
@@ -7,6 +8,7 @@
 #include <aikido/statespace/GeodesicInterpolator.hpp>
 #include <aikido/statespace/Rn.hpp>
 #include <aikido/trajectory/util.hpp>
+
 #include "eigen_tests.hpp"
 
 using aikido::constraint::Satisfied;
diff --git a/tests/planner/parabolic/test_ParabolicTimer.cpp b/tests/planner/parabolic/test_ParabolicTimer.cpp
index 96044095a6..5e6a534fe3 100644
--- a/tests/planner/parabolic/test_ParabolicTimer.cpp
+++ b/tests/planner/parabolic/test_ParabolicTimer.cpp
@@ -1,4 +1,5 @@
 #include <gtest/gtest.h>
+
 #include <aikido/planner/parabolic/ParabolicTimer.hpp>
 #include <aikido/statespace/CartesianProduct.hpp>
 #include <aikido/statespace/GeodesicInterpolator.hpp>
diff --git a/tests/planner/parabolic/test_SmoothPostProcessor.cpp b/tests/planner/parabolic/test_SmoothPostProcessor.cpp
index a04f0965cd..cf9e24860a 100644
--- a/tests/planner/parabolic/test_SmoothPostProcessor.cpp
+++ b/tests/planner/parabolic/test_SmoothPostProcessor.cpp
@@ -1,10 +1,12 @@
 #include <gtest/gtest.h>
+
 #include <aikido/common/RNG.hpp>
 #include <aikido/constraint/Satisfied.hpp>
 #include <aikido/planner/parabolic/ParabolicSmoother.hpp>
 #include <aikido/statespace/CartesianProduct.hpp>
 #include <aikido/statespace/GeodesicInterpolator.hpp>
 #include <aikido/statespace/Rn.hpp>
+
 #include "eigen_tests.hpp"
 
 using aikido::constraint::Satisfied;
diff --git a/tests/planner/parabolic/test_TimePostProcessor.cpp b/tests/planner/parabolic/test_TimePostProcessor.cpp
index 77f02815ee..562b9874e2 100644
--- a/tests/planner/parabolic/test_TimePostProcessor.cpp
+++ b/tests/planner/parabolic/test_TimePostProcessor.cpp
@@ -1,10 +1,12 @@
 #include <gtest/gtest.h>
+
 #include <aikido/common/RNG.hpp>
 #include <aikido/planner/parabolic/ParabolicTimer.hpp>
 #include <aikido/statespace/CartesianProduct.hpp>
 #include <aikido/statespace/GeodesicInterpolator.hpp>
 #include <aikido/statespace/Rn.hpp>
 #include <aikido/trajectory/util.hpp>
+
 #include "eigen_tests.hpp"
 
 using aikido::planner::parabolic::ParabolicTimer;
diff --git a/tests/planner/test_DartPlanners.cpp b/tests/planner/test_DartPlanners.cpp
index d67848b6e0..6e1d99cf1d 100644
--- a/tests/planner/test_DartPlanners.cpp
+++ b/tests/planner/test_DartPlanners.cpp
@@ -1,6 +1,8 @@
 #include <tuple>
+
 #include <dart/dart.hpp>
 #include <gtest/gtest.h>
+
 #include <aikido/common/RNG.hpp>
 #include <aikido/constraint/Testable.hpp>
 #include <aikido/distance/defaults.hpp>
@@ -12,6 +14,7 @@
 #include <aikido/statespace/SO2.hpp>
 #include <aikido/statespace/dart/MetaSkeletonStateSpace.hpp>
 #include <aikido/trajectory/Interpolated.hpp>
+
 #include "../constraint/MockConstraints.hpp"
 
 using aikido::planner::ConfigurationToConfiguration;
diff --git a/tests/planner/test_SnapPlanner.cpp b/tests/planner/test_SnapPlanner.cpp
index 9b63b999f4..8a3e6b0fc2 100644
--- a/tests/planner/test_SnapPlanner.cpp
+++ b/tests/planner/test_SnapPlanner.cpp
@@ -1,6 +1,8 @@
 #include <tuple>
+
 #include <dart/dart.hpp>
 #include <gtest/gtest.h>
+
 #include <aikido/constraint/Testable.hpp>
 #include <aikido/distance/defaults.hpp>
 #include <aikido/planner/ConfigurationToConfiguration.hpp>
@@ -9,6 +11,7 @@
 #include <aikido/statespace/SO2.hpp>
 #include <aikido/statespace/dart/MetaSkeletonStateSpace.hpp>
 #include <aikido/trajectory/Interpolated.hpp>
+
 #include "../constraint/MockConstraints.hpp"
 
 using aikido::planner::ConfigurationToConfiguration;
diff --git a/tests/planner/test_World.cpp b/tests/planner/test_World.cpp
index f6f44ac28c..95f5b61a5c 100644
--- a/tests/planner/test_World.cpp
+++ b/tests/planner/test_World.cpp
@@ -1,6 +1,7 @@
 #include <dart/dart.hpp>
 #include <dart/dynamics/dynamics.hpp>
 #include <gtest/gtest.h>
+
 #include <aikido/planner/World.hpp>
 
 using std::make_shared;
diff --git a/tests/planner/vectorfield/test_VectorFieldPlanner.cpp b/tests/planner/vectorfield/test_VectorFieldPlanner.cpp
index 92552b2235..9bf4d9a6c9 100644
--- a/tests/planner/vectorfield/test_VectorFieldPlanner.cpp
+++ b/tests/planner/vectorfield/test_VectorFieldPlanner.cpp
@@ -1,9 +1,11 @@
-#include <gtest/gtest.h>
 #include <tuple>
+
 #include <dart/dart.hpp>
-#include "aikido/planner/dart/ConfigurationToEndEffectorOffset.hpp"
+#include <gtest/gtest.h>
+
 #include <aikido/constraint/Testable.hpp>
 #include <aikido/distance/defaults.hpp>
+#include <aikido/planner/dart/ConfigurationToEndEffectorOffset.hpp>
 #include <aikido/planner/vectorfield/MoveEndEffectorOffsetVectorField.hpp>
 #include <aikido/planner/vectorfield/VectorFieldConfigurationToEndEffectorOffsetPlanner.hpp>
 #include <aikido/planner/vectorfield/VectorFieldPlanner.hpp>
@@ -11,6 +13,7 @@
 #include <aikido/statespace/GeodesicInterpolator.hpp>
 #include <aikido/statespace/SO2.hpp>
 #include <aikido/statespace/dart/MetaSkeletonStateSpace.hpp>
+
 #include "../../constraint/MockConstraints.hpp"
 
 using std::make_shared;
diff --git a/tests/statespace/dart/test_DartJointStateSpaces.cpp b/tests/statespace/dart/test_DartJointStateSpaces.cpp
index 23d1d32862..ba3c051449 100644
--- a/tests/statespace/dart/test_DartJointStateSpaces.cpp
+++ b/tests/statespace/dart/test_DartJointStateSpaces.cpp
@@ -1,4 +1,5 @@
 #include <gtest/gtest.h>
+
 #include <aikido/statespace/dart/WeldJoint.hpp>
 
 using namespace aikido;
diff --git a/tests/statespace/dart/test_MetaSkeletonStateSaver.cpp b/tests/statespace/dart/test_MetaSkeletonStateSaver.cpp
index 6260e0d057..8a584a92ae 100644
--- a/tests/statespace/dart/test_MetaSkeletonStateSaver.cpp
+++ b/tests/statespace/dart/test_MetaSkeletonStateSaver.cpp
@@ -1,5 +1,6 @@
 #include <dart/dynamics/dynamics.hpp>
 #include <gtest/gtest.h>
+
 #include <aikido/statespace/dart/MetaSkeletonStateSaver.hpp>
 
 using aikido::statespace::dart::MetaSkeletonStateSaver;
diff --git a/tests/statespace/dart/test_MetaSkeletonStateSpace.cpp b/tests/statespace/dart/test_MetaSkeletonStateSpace.cpp
index 34a7d4b567..15e13d56b1 100644
--- a/tests/statespace/dart/test_MetaSkeletonStateSpace.cpp
+++ b/tests/statespace/dart/test_MetaSkeletonStateSpace.cpp
@@ -1,5 +1,6 @@
 #include <dart/dynamics/dynamics.hpp>
 #include <gtest/gtest.h>
+
 #include <aikido/statespace/CartesianProduct.hpp>
 #include <aikido/statespace/Rn.hpp>
 #include <aikido/statespace/SE2.hpp>
diff --git a/tests/statespace/test_CartesianProduct.cpp b/tests/statespace/test_CartesianProduct.cpp
index 100b3990ec..ee621eae7e 100644
--- a/tests/statespace/test_CartesianProduct.cpp
+++ b/tests/statespace/test_CartesianProduct.cpp
@@ -1,5 +1,6 @@
 #include <dart/math/Helpers.hpp>
 #include <gtest/gtest.h>
+
 #include <aikido/statespace/CartesianProduct.hpp>
 #include <aikido/statespace/Rn.hpp>
 #include <aikido/statespace/SE2.hpp>
diff --git a/tests/statespace/test_Rn.cpp b/tests/statespace/test_Rn.cpp
index 6889e72ade..fcaff75c54 100644
--- a/tests/statespace/test_Rn.cpp
+++ b/tests/statespace/test_Rn.cpp
@@ -1,4 +1,5 @@
 #include <gtest/gtest.h>
+
 #include <aikido/statespace/Rn.hpp>
 
 using aikido::statespace::R3;
diff --git a/tests/statespace/test_SE2.cpp b/tests/statespace/test_SE2.cpp
index ba89bb57cb..f4b3d76c0c 100644
--- a/tests/statespace/test_SE2.cpp
+++ b/tests/statespace/test_SE2.cpp
@@ -1,5 +1,6 @@
 #include <dart/math/Helpers.hpp>
 #include <gtest/gtest.h>
+
 #include <aikido/statespace/SE2.hpp>
 
 using aikido::statespace::SE2;
diff --git a/tests/statespace/test_SE3.cpp b/tests/statespace/test_SE3.cpp
index 2ea79cecac..15144403c6 100644
--- a/tests/statespace/test_SE3.cpp
+++ b/tests/statespace/test_SE3.cpp
@@ -1,5 +1,6 @@
 #include <dart/math/Helpers.hpp>
 #include <gtest/gtest.h>
+
 #include <aikido/statespace/SE3.hpp>
 
 using aikido::statespace::SE3;
diff --git a/tests/statespace/test_SO2.cpp b/tests/statespace/test_SO2.cpp
index 6d9a95b399..e262cb178b 100644
--- a/tests/statespace/test_SO2.cpp
+++ b/tests/statespace/test_SO2.cpp
@@ -1,6 +1,8 @@
 #include <dart/math/Helpers.hpp>
 #include <gtest/gtest.h>
+
 #include <aikido/statespace/SO2.hpp>
+
 #include "eigen_tests.hpp"
 
 using aikido::statespace::SO2;
diff --git a/tests/statespace/test_SO3.cpp b/tests/statespace/test_SO3.cpp
index a3fbd5e281..944125665c 100644
--- a/tests/statespace/test_SO3.cpp
+++ b/tests/statespace/test_SO3.cpp
@@ -1,5 +1,6 @@
 #include <dart/math/Helpers.hpp>
 #include <gtest/gtest.h>
+
 #include <aikido/statespace/SO3.hpp>
 
 using aikido::statespace::SO3;
diff --git a/tests/trajectory/test_BSpline.cpp b/tests/trajectory/test_BSpline.cpp
index 9748a7793a..7d9969d07b 100644
--- a/tests/trajectory/test_BSpline.cpp
+++ b/tests/trajectory/test_BSpline.cpp
@@ -1,4 +1,5 @@
 #include <gtest/gtest.h>
+
 #include <aikido/statespace/Rn.hpp>
 #include <aikido/trajectory/BSpline.hpp>
 
diff --git a/tests/trajectory/test_Interpolated.cpp b/tests/trajectory/test_Interpolated.cpp
index b16279791f..e88ed7abeb 100644
--- a/tests/trajectory/test_Interpolated.cpp
+++ b/tests/trajectory/test_Interpolated.cpp
@@ -1,4 +1,5 @@
 #include <gtest/gtest.h>
+
 #include <aikido/statespace/Rn.hpp>
 #include <aikido/trajectory/Interpolated.hpp>
 #include <aikido/trajectory/util.hpp>
diff --git a/tests/trajectory/test_SplineTrajectory.cpp b/tests/trajectory/test_SplineTrajectory.cpp
index df4cd4a9e2..c009ea7728 100644
--- a/tests/trajectory/test_SplineTrajectory.cpp
+++ b/tests/trajectory/test_SplineTrajectory.cpp
@@ -1,4 +1,5 @@
 #include <gtest/gtest.h>
+
 #include <aikido/statespace/CartesianProduct.hpp>
 #include <aikido/statespace/Rn.hpp>
 #include <aikido/trajectory/Spline.hpp>
diff --git a/tests/trajectory/test_util.cpp b/tests/trajectory/test_util.cpp
index 8d8ce3f7e7..09e83602e4 100644
--- a/tests/trajectory/test_util.cpp
+++ b/tests/trajectory/test_util.cpp
@@ -1,12 +1,13 @@
 #include <tuple>
+
 #include <dart/dart.hpp>
 #include <gtest/gtest.h>
+
 #include <aikido/statespace/CartesianProduct.hpp>
 #include <aikido/statespace/GeodesicInterpolator.hpp>
 #include <aikido/statespace/Rn.hpp>
 #include <aikido/statespace/SO2.hpp>
 #include <aikido/statespace/dart/MetaSkeletonStateSpace.hpp>
-
 #include <aikido/trajectory/Interpolated.hpp>
 #include <aikido/trajectory/util.hpp>