Code format: control

CMakeLists.txt

@@ -93,12 +93,14 @@ file(GLOB_RECURSE headers_planner "${CMAKE_SOURCE_DIR}/include/aikido/planner/*.
 file(GLOB_RECURSE headers_util "${CMAKE_SOURCE_DIR}/include/aikido/util/*.hpp")
 file(GLOB_RECURSE headers_rviz "${CMAKE_SOURCE_DIR}/include/aikido/rviz/*.hpp")
 file(GLOB_RECURSE headers_distance "${CMAKE_SOURCE_DIR}/include/aikido/distance/*.hpp")
+file(GLOB_RECURSE headers_distance "${CMAKE_SOURCE_DIR}/include/aikido/control/*.hpp")
 format_add_sources(${headers_statespace})
 format_add_sources(${headers_perception})
 format_add_sources(${headers_planner})
 format_add_sources(${headers_util})
 format_add_sources(${headers_rviz})
 format_add_sources(${headers_distance})
+format_add_sources(${headers_control})
 
 #==============================================================================
 # Helper functions.

include/aikido/constraint/CartesianProductTestable.hpp

@@ -33,7 +33,7 @@ class CartesianProductTestable : public Testable
   std::vector<TestablePtr> mConstraints;
 };
 
-}  // namespace constraint
-}  // namespace aikido
+} // namespace constraint
+} // namespace aikido
 
 #endif  // define AIKIDO_CONSTRAINT_SAMPLEABLESUBSPACE_HPP_

include/aikido/constraint/Differentiable.hpp

@@ -65,7 +65,7 @@ class Differentiable
 
 using DifferentiablePtr = std::shared_ptr<Differentiable>;
 
-} // constraint
-} // aikido
+} // namespace constraint
+} // namespace aikido
 
 #endif

include/aikido/constraint/DifferentiableIntersection.hpp

@@ -49,7 +49,7 @@ class DifferentiableIntersection : public Differentiable
 };
 
 
-} // constraint
-} // aikido
+} // namespace constraint
+} // namespace aikido
 
 #endif

include/aikido/constraint/DifferentiableSubspace.hpp

@@ -51,7 +51,7 @@ class DifferentiableSubspace : public Differentiable
   size_t mIndex;
 };
 
-} // constraint
-} // aikido
+} // namespace constraint
+} // namespace aikido
 
 #endif // AIKIDO_CONSTRAINT_DIFFERENTIABLESUBSPACE_HPP_

include/aikido/constraint/FrameDifferentiable.hpp

@@ -72,7 +72,7 @@ class FrameDifferentiable: public Differentiable
 };
 
 
-} // constraint
-} // aikido
+} // namespace constraint
+} // namespace aikido
 
 #endif

include/aikido/constraint/FramePairDifferentiable.hpp

@@ -67,7 +67,7 @@ class FramePairDifferentiable: public Differentiable
 };
 
 
-} // constraint
-} // aikido
+} // namespace constraint
+} // namespace aikido
 
 #endif

include/aikido/constraint/NewtonsMethodProjectable.hpp

@@ -46,8 +46,7 @@ class NewtonsMethodProjectable : public Projectable
   bool contains(const statespace::StateSpace::State* _s) const;
 };
 
-
-} // constraint
-} // aikido
+} // namespace constraint
+} // namespace aikido
 
 #endif

include/aikido/constraint/NonColliding.hpp

@@ -75,6 +75,7 @@ class NonColliding : public Testable
 
 using NonCollidingPtr = std::shared_ptr<NonColliding>;
 
-}  // constraint
-}  // aikido
+} // namespace constraint
+} // namespace aikido
+
 #endif  // AIKIDO_CONSTRAINT_NONCOLLIDING_HPP_

include/aikido/constraint/Projectable.hpp

@@ -32,7 +32,7 @@ class Projectable
 
 using ProjectablePtr = std::shared_ptr<Projectable>;
 
-} // constraint
-} // aikido
+} // namespace constraint
+} // namespace aikido
 
 #endif

include/aikido/constraint/RejectionSampleable.hpp

@@ -46,7 +46,7 @@ class RejectionSampleable : public Sampleable
   int mMaxTrialPerSample;
 };
 
-} // constraint
-} // aikido
+} // namespace constraint
+} // namespace aikido
 
 #endif 

include/aikido/constraint/TSR.hpp

@@ -145,7 +145,7 @@ class TSR : public Sampleable,
 
 using TSRPtr = std::shared_ptr<TSR>;
 
-}  // namespace constraint
-}  // namespace aikido
+} // namespace constraint
+} // namespace aikido
 
 #endif  // AIKIDO_CONSTRAINT_TSR_HPP_

include/aikido/constraint/Testable.hpp

@@ -23,7 +23,7 @@ class Testable
 
 using TestablePtr = std::shared_ptr<Testable>;
 
-}  // constraint
-}  // aikido
+} // namespace constraint
+} // namespace aikido
 
 #endif  // AIKIDO_CONSTRAINT_TESTABLE_HPP_

include/aikido/constraint/TestableIntersection.hpp

@@ -43,7 +43,7 @@ class TestableIntersection : public Testable
 
 using TestableIntersectionPtr = std::shared_ptr<TestableIntersection>;
 
-}  // constraint
-}  // aikido
+} // namespace constraint
+} // namespace aikido
 
 #endif  // AIKIDO_CONSTRAINT_TESTABLEINTERSECTION_HPP_

include/aikido/constraint/uniform/detail/RnBoxConstraint-impl.hpp

@@ -86,7 +86,7 @@ bool RnBoxConstraintSampleGenerator<N>::sample(
 {
   Vectord value(mDistributions.size());
 
-  for (size_t i = 0; i < static_cast<std::size_t>(value.size()); ++i)
+  for (auto i = 0; i < value.size(); ++i)
     value[i] = mDistributions[i](*mRng);
 
   mSpace->setValue(static_cast<typename statespace::R<N>::State*>(_state), value);
@@ -184,7 +184,7 @@ bool RBoxConstraint<N>::isSatisfied(
   const auto value = mSpace->getValue(
     static_cast<const typename statespace::R<N>::State*>(state));
 
-  for (size_t i = 0; i < static_cast<std::size_t>(value.size()); ++i)
+  for (auto i = 0; i < value.size(); ++i)
   {
     if (value[i] < mLowerLimits[i] || value[i] > mUpperLimits[i])
       return false;
@@ -201,7 +201,7 @@ bool RBoxConstraint<N>::project(
   Vectord value = mSpace->getValue(
     static_cast<const typename statespace::R<N>::State*>(_s));
 
-  for (size_t i = 0; i < static_cast<std::size_t>(value.size()); ++i)
+  for (auto i = 0; i < value.size(); ++i)
   {
     if (value[i] < mLowerLimits[i])
       value[i] = mLowerLimits[i];
@@ -250,7 +250,7 @@ void RBoxConstraint<N>::getJacobian(
   const size_t dimension = mSpace->getDimension();
   _out = Eigen::MatrixXd::Zero(dimension, dimension);
 
-  for (size_t i = 0; i < static_cast<std::size_t>(_out.rows()); ++i)
+  for (auto i = 0; i < _out.rows(); ++i)
   {
     if (stateValue[i] < mLowerLimits[i])
       _out(i, i) = -1.;

include/aikido/control/BarrettFingerKinematicSimulationPositionCommandExecutor.hpp

@@ -1,46 +1,52 @@
 #ifndef AIKIDO_CONTROL_BARRETFINGERKINEMATICSIMULATIONPOSITIONCOMMANDEXECUTOR_HPP_
 #define AIKIDO_CONTROL_BARRETFINGERKINEMATICSIMULATIONPOSITIONCOMMANDEXECUTOR_HPP_
-#include <aikido/control/PositionCommandExecutor.hpp>
+
+#include <chrono>
+#include <condition_variable>
+#include <future>
+#include <mutex>
 #include <dart/collision/CollisionDetector.hpp>
-#include <dart/collision/CollisionOption.hpp>
-#include <dart/collision/CollisionGroup.hpp>
 #include <dart/collision/CollisionFilter.hpp>
+#include <dart/collision/CollisionGroup.hpp>
+#include <dart/collision/CollisionOption.hpp>
 #include <dart/dynamics/dynamics.hpp>
-#include <future>
-#include <mutex>
-#include <condition_variable>
-#include <chrono>
+#include <aikido/control/PositionCommandExecutor.hpp>
 
 namespace aikido {
 namespace control {
 
 /// This executor mimics the behavior of BarretFinger.
 /// It moves a finger to a desired point; it may stop early if
 /// joint limit is reached or collision is detected.
-/// Only the proximal joint is actuated; the distal joint moves with mimic ratio.
+/// Only the proximal joint is actuated; the distal joint moves with mimic
+/// ratio.
 /// When collision is detected on the distal link, the finger stops.
 /// When collision is detected on the proximal link, the distal link moves
 /// until it reaches joint limit or until distal collision is detected.
 class BarrettFingerKinematicSimulationPositionCommandExecutor
-: public PositionCommandExecutor
+    : public PositionCommandExecutor
 {
 public:
   /// Constructor.
   /// \param[in] finger Finger to be controlled by this Executor.
   /// \param[in] proximal Index of proximal dof
   /// \param[in] distal Index of distal dof
-  /// \param[in] collisionDetector CollisionDetector to check collision with fingers.
+  /// \param[in] collisionDetector CollisionDetector to check collision with
+  /// fingers.
   ///        If nullptr, default to FCLCollisionDetector.
   /// \param[in] collideWith CollisionGroup to check collision with fingers.
   ///        If nullptr, default to empty CollisionGroup.
-  /// \param[in] collisionOptions Default is (enableContact=false, binaryCheck=true,
+  /// \param[in] collisionOptions Default is (enableContact=false,
+  /// binaryCheck=true,
   ///        maxNumContacts = 1.)
   ///        See dart/collison/Option.h for more information
   BarrettFingerKinematicSimulationPositionCommandExecutor(
-    ::dart::dynamics::ChainPtr finger, size_t proximal, size_t distal,
-    ::dart::collision::CollisionDetectorPtr collisionDetector = nullptr,
-    ::dart::collision::CollisionGroupPtr collideWith = nullptr,
-    ::dart::collision::CollisionOption collisionOptions
+      ::dart::dynamics::ChainPtr finger,
+      size_t proximal,
+      size_t distal,
+      ::dart::collision::CollisionDetectorPtr collisionDetector = nullptr,
+      ::dart::collision::CollisionGroupPtr collideWith = nullptr,
+      ::dart::collision::CollisionOption collisionOptions
       = ::dart::collision::CollisionOption(false, 1));
 
   /// Open/close fingers to goal configuration.
@@ -55,7 +61,10 @@ class BarrettFingerKinematicSimulationPositionCommandExecutor
   /// the proximal joint. The joint movements follow
   /// this ratio only when both joints are moving.
   /// \return mimic ratio.
-  constexpr static double getMimicRatio() { return kMimicRatio; }
+  constexpr static double getMimicRatio()
+  {
+    return kMimicRatio;
+  }
 
   /// Moves the joints of the finger by dofVelocity*timeSincePreviousCall
   /// until execute's goalPosition by primary dof or collision is detected.
@@ -75,7 +84,7 @@ class BarrettFingerKinematicSimulationPositionCommandExecutor
   constexpr static double kMimicRatio = 0.333;
   // TODO: read velocity limit from herb_description
   constexpr static double kProximalSpeed = 2.0;
-  constexpr static double kDistalSpeed = kProximalSpeed*kMimicRatio;
+  constexpr static double kDistalSpeed = kProximalSpeed * kMimicRatio;
 
   /// If (current dof - goalPosition) execution terminates.
   constexpr static double kTolerance = 1e-3;
@@ -116,14 +125,12 @@ class BarrettFingerKinematicSimulationPositionCommandExecutor
 
   /// Helper method for step() to set variables for terminating an execution.
   void terminate();
-
 };
 
 using BarrettFingerKinematicSimulationPositionCommandExecutorPtr
-  = std::shared_ptr<BarrettFingerKinematicSimulationPositionCommandExecutor>;
-
+    = std::shared_ptr<BarrettFingerKinematicSimulationPositionCommandExecutor>;
 
-} // control
-} // aikido
+} // namespace control
+} // namespace aikido
 
 #endif

include/aikido/control/BarrettFingerKinematicSimulationSpreadCommandExecutor.hpp

@@ -1,15 +1,16 @@
 #ifndef AIKIDO_CONTROL_BARRETFINGERKINEMATICSIMULATIONSPREADCOMMANDEXECUTOR_HPP_
 #define AIKIDO_CONTROL_BARRETFINGERKINEMATICSIMULATIONSPREADCOMMANDEXECUTOR_HPP_
-#include <aikido/control/PositionCommandExecutor.hpp>
+
+#include <chrono>
+#include <condition_variable>
+#include <future>
+#include <mutex>
 #include <dart/collision/CollisionDetector.hpp>
-#include <dart/collision/CollisionOption.hpp>
-#include <dart/collision/CollisionGroup.hpp>
 #include <dart/collision/CollisionFilter.hpp>
+#include <dart/collision/CollisionGroup.hpp>
+#include <dart/collision/CollisionOption.hpp>
 #include <dart/dynamics/dynamics.hpp>
-#include <future>
-#include <mutex>
-#include <condition_variable>
-#include <chrono>
+#include <aikido/control/PositionCommandExecutor.hpp>
 
 namespace aikido {
 namespace control {
@@ -18,23 +19,26 @@ namespace control {
 /// It moves two finger spreads simultaneously to certain goal value;
 /// it will stop prematurely if joint limit is reached or collision is detected.
 class BarrettFingerKinematicSimulationSpreadCommandExecutor
-: public PositionCommandExecutor
+    : public PositionCommandExecutor
 {
 public:
   /// Constructor.
   /// \param[in] fingers 2 fingers to be controlled by this Executor.
   /// \param[in] spread Index of spread dof
-  /// \param[in] collisionDetector CollisionDetector to check collision with fingers.
+  /// \param[in] collisionDetector CollisionDetector to check collision with
+  /// fingers.
   ///        If nullptr, default to FCLCollisionDetector.
   /// \param[in] collideWith CollisionGroup to check collision with fingers.
   ///        If nullptr, default to empty CollisionGroup
-  /// \param[in] collisionOptions Default is (enableContact=false, binaryCheck=true,
+  /// \param[in] collisionOptions Default is (enableContact=false,
+  /// binaryCheck=true,
   ///        maxNumContacts = 1.)
   BarrettFingerKinematicSimulationSpreadCommandExecutor(
-    std::array<::dart::dynamics::ChainPtr, 2> fingers, size_t spread,
-    ::dart::collision::CollisionDetectorPtr collisionDetector = nullptr,
-    ::dart::collision::CollisionGroupPtr collideWith = nullptr,
-    ::dart::collision::CollisionOption collisionOptions
+      std::array<::dart::dynamics::ChainPtr, 2> fingers,
+      size_t spread,
+      ::dart::collision::CollisionDetectorPtr collisionDetector = nullptr,
+      ::dart::collision::CollisionGroupPtr collideWith = nullptr,
+      ::dart::collision::CollisionOption collisionOptions
       = ::dart::collision::CollisionOption(false, 1));
 
   /// Move the spread joint by goalPosition until goalPosition or
@@ -94,10 +98,10 @@ class BarrettFingerKinematicSimulationSpreadCommandExecutor
   double mGoalPosition;
 };
 
-using BarrettFingerKinematicSimulationSpreadCommandExecutorPtr 
-  = std::shared_ptr<BarrettFingerKinematicSimulationSpreadCommandExecutor>;
+using BarrettFingerKinematicSimulationSpreadCommandExecutorPtr
+    = std::shared_ptr<BarrettFingerKinematicSimulationSpreadCommandExecutor>;
 
-} // control
-} // aikido
+} // namespace control
+} // namespace aikido
 
 #endif