Adding an abstract BarrettHandPositionCommandExecutor class

include/aikido/control/BarrettFingerPositionCommandExecutor.hpp → include/aikido/control/BarrettFingerKinematicSimulationPositionCommandExecutor.hpp

@@ -1,5 +1,5 @@
-#ifndef AIKIDO_CONTROL_BARRETFINGERPOSITIONCOMMANDEXECUTOR_HPP_
-#define AIKIDO_CONTROL_BARRETFINGERPOSITIONCOMMANDEXECUTOR_HPP_
+#ifndef AIKIDO_CONTROL_BARRETFINGERKINEMATICSIMULATIONPOSITIONCOMMANDEXECUTOR_HPP_
+#define AIKIDO_CONTROL_BARRETFINGERKINEMATICSIMULATIONPOSITIONCOMMANDEXECUTOR_HPP_
 #include <dart/collision/CollisionDetector.hpp>
 #include <dart/collision/CollisionOption.hpp>
 #include <dart/collision/CollisionGroup.hpp>
@@ -8,67 +8,74 @@
 #include <future>
 #include <mutex>
 #include <condition_variable>
+#include <chrono>
 
 namespace aikido {
 namespace control {
 
-/// This executor mimics the behavior of BarretFinger. 
-/// It moves a finger to a desired point; it may stop early if 
+/// 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.
 /// 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 BarrettFingerPositionCommandExecutor
+class BarrettFingerKinematicSimulationPositionCommandExecutor
 {
 public:
   /// Constructor.
-  /// \param _finger Finger to be controlled by this Executor.
-  /// \param _proximal Index of proximal dof
-  /// \param _distal Index of distal dof
-  /// \param _collisionDetector CollisionDetector for detecting self collision
-  ///        and collision with objects. 
-  /// \param _collisionOptions Default is (enableContact=false, binaryCheck=true,
-  ///        maxNumContacts = 1.) 
-  ///        See dart/collison/Option.h for more information 
-  BarrettFingerPositionCommandExecutor(
-    ::dart::dynamics::ChainPtr _finger, int _proximal, int _distal,
-    ::dart::collision::CollisionDetectorPtr _collisionDetector,
-    ::dart::collision::CollisionOption _collisionOptions
+  /// \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.
+  ///        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,
+  ///        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::collision::CollisionOption(false, 1));
 
   /// Sets variables to move the finger joints.
-  /// proximal dof moves to _goalPosition, joint limit, or until collision.
+  /// proximal dof moves to goalPosition, joint limit, or until collision.
   /// Step method should be called multiple times until future returns.
-  /// Distal dof follows with mimic ratio. 
-  /// \param _goalPosition Desired angle of proximal joint.
-  /// \param _collideWith CollisionGroup to check collision with fingers.
+  /// Distal dof follows with mimic ratio.
+  /// \param goalPosition Desired angle of proximal joint.
   /// \return future Becomes available when the execution completes.
-  std::future<void> execute(double _goalPosition,
-    ::dart::collision::CollisionGroupPtr _collideWith);
+  std::future<void> execute(double goalPosition);
 
-  /// Returns mimic ratio, i.e. how much the distal joint moves relative to 
+  /// Returns mimic ratio, i.e. how much the distal joint moves relative to
   /// the proximal joint. The joint movements follow
   /// this ratio only when both joints are moving.
   /// \return mimic ratio.
-  static double getMimicRatio();
+  constexpr static double getMimicRatio() { return kMimicRatio; }
 
-  /// Moves the joints of the finger by dofVelocity*_timeSIncePreviousCall
+  /// Moves the joints of the finger by dofVelocity*timeSincePreviousCall
   /// until execute's goalPosition by primary dof or collision is detected.
-  /// If proximal link is in collision, distal link moves until 
+  /// If proximal link is in collision, distal link moves until
   /// mimicRatio*goalPosition. If distal link is in collision, execution stops.
   /// If multiple threads are accessing this function or skeleton associated
   /// with this executor, it is necessary to lock the skeleton before
   /// calling this method.
-  /// \param _timeSincePreviousCall Time since previous call. 
-  void step(double _timeSincePreviousCall);
+  /// \param[in] timeSincePreviousCall Time interval to take.
+  void step(const std::chrono::milliseconds& timeSincePreviousCall);
+
+  /// Resets CollisionGroup to check collision with fingers.
+  /// \param _collideWith CollisionGroup to check collision with fingers.
+  /// \return false if fails to change collideWith (during execution).
+  bool setCollideWith(::dart::collision::CollisionGroupPtr collideWith);
 
-private: 
-  constexpr static double kMimicRatio = 0.333; 
-  constexpr static double kProximalVelocity = 0.01;
-  constexpr static double kDistalVelocity = kProximalVelocity*kMimicRatio;
+private:
+  constexpr static double kMimicRatio = 0.333;
+  constexpr static double kProximalSpeed = 1;
+  constexpr static double kDistalSpeed = kProximalSpeed*kMimicRatio;
 
-  /// If (current dof - goalPosition) execution terminates. 
+  /// If (current dof - goalPosition) execution terminates.
   constexpr static double kTolerance = 1e-3;
 
   ::dart::dynamics::ChainPtr mFinger;
@@ -81,34 +88,34 @@ class BarrettFingerPositionCommandExecutor
   std::pair<double, double> mProximalLimits, mDistalLimits;
 
   ::dart::collision::CollisionDetectorPtr mCollisionDetector;
+  ::dart::collision::CollisionGroupPtr mCollideWith;
   ::dart::collision::CollisionOption mCollisionOptions;
 
   ::dart::collision::CollisionGroupPtr mProximalCollisionGroup;
   ::dart::collision::CollisionGroupPtr mDistalCollisionGroup;
 
   std::unique_ptr<std::promise<void>> mPromise;
 
+  /// Flag for indicating execution of a command.
+  bool mInExecution;
+
   /// Control access to mPromise, mInExecution, mGoalPosition, mDistalOnly,
-  /// mCollideWith 
+  /// mCollideWith
   std::mutex mMutex;
 
-  /// Flag for indicating execution of a command. 
-  bool mInExecution;
-
   /// Desired end-position of proximal dof.
   double mProximalGoalPosition;
 
   /// Indicator that only distal finger is to be moved.
   bool mDistalOnly;
 
-  ::dart::collision::CollisionGroupPtr mCollideWith;
-
   /// Helper method for step() to set variables for terminating an execution.
   void terminate();
 
 };
 
-using BarrettFingerPositionCommandExecutorPtr = std::shared_ptr<BarrettFingerPositionCommandExecutor>;
+using BarrettFingerKinematicSimulationPositionCommandExecutorPtr
+  = std::shared_ptr<BarrettFingerKinematicSimulationPositionCommandExecutor>;
 
 
 } // control

include/aikido/control/BarrettFingerKinematicSimulationSpreadCommandExecutor.hpp

@@ -0,0 +1,96 @@
+#ifndef AIKIDO_CONTROL_BARRETFINGERKINEMATICSIMULATIONSPREADCOMMANDEXECUTOR_HPP_
+#define AIKIDO_CONTROL_BARRETFINGERKINEMATICSIMULATIONSPREADCOMMANDEXECUTOR_HPP_
+#include <dart/collision/CollisionDetector.hpp>
+#include <dart/collision/CollisionOption.hpp>
+#include <dart/collision/CollisionGroup.hpp>
+#include <dart/collision/CollisionFilter.hpp>
+#include <dart/dynamics/dynamics.hpp>
+#include <future>
+#include <mutex>
+#include <condition_variable>
+#include <chrono>
+
+namespace aikido {
+namespace control {
+
+/// This executor mimics the spread behavior of BarretFinger.
+/// 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:
+  /// Constructor.
+  /// \param[in] fingers 2 fingers to be controlled by this Executor.
+  /// \param[in] spread Index of spread dof
+  /// \param[in] collideWith CollisionGroup to check collision with fingers.
+  /// \param[in] collisionOptions Default is (enableContact=false, binaryCheck=true,
+  ///        maxNumContacts = 1.)
+  BarrettFingerKinematicSimulationSpreadCommandExecutor(
+    std::array<::dart::dynamics::ChainPtr, 2> fingers, size_t spread,
+    ::dart::collision::CollisionGroupPtr collideWith,
+    ::dart::collision::CollisionOption collisionOptions
+      = ::dart::collision::CollisionOption(false, 1));
+
+  /// Sets variables to move the spread joint by goalPosition,
+  /// joint limit has reached, or until collision is detected.
+  /// Must call step function after this for actual execution.
+  /// \param goalPosition Desired angle of spread joint.
+  /// \return future which becomes available when the execution completes.
+  std::future<void> execute(double goalPosition);
+
+  /// Moves the joint of the finger by fixed speed*timeSincePreviousCall
+  /// until execute's goalPosition by spread dof or collision is detected.
+  /// If multiple threads are accessing this function or the skeleton associated
+  /// with this executor, it is necessary to lock the skeleton before
+  /// calling this method.
+  /// \param[in] timeSincePreviousCall Time interval to take.
+  void step(const std::chrono::milliseconds& timeSincePreviousCall);
+
+
+  /// Resets CollisionGroup to check collision.
+  /// \param collideWith CollisionGroup to check collision with fingers.
+  /// \return false if fails to change collideWith (during execution).
+  bool setCollideWith(::dart::collision::CollisionGroupPtr collideWith);
+
+private:
+  constexpr static double kDofSpeed = 1;
+
+  /// If (current dof - goalPosition) execution terminates.
+  constexpr static double kTolerance = 1e-3;
+
+  /// All fingers dof values should be the same (approx. within this tolerance).
+  constexpr static double kDofTolerance = 1e-3;
+
+  constexpr static int kNumFingers = 2;
+
+  std::array<::dart::dynamics::ChainPtr, 2> mFingers;
+
+  std::vector<::dart::dynamics::DegreeOfFreedom*> mSpreadDofs;
+
+  std::pair<double, double> mDofLimits;
+
+  ::dart::collision::CollisionDetectorPtr mCollisionDetector;
+  ::dart::collision::CollisionGroupPtr mCollideWith;
+  ::dart::collision::CollisionOption mCollisionOptions;
+
+  ::dart::collision::CollisionGroupPtr mSpreadCollisionGroup;
+
+  std::unique_ptr<std::promise<void>> mPromise;
+
+  /// Controls access to mPromise, mInExecution, mGoalPosition, mCollideWith
+  std::mutex mMutex;
+
+  /// True if a command is in execution
+  bool mInExecution;
+
+  /// Desired end spread value
+  double mGoalPosition;
+};
+
+using BarrettFingerKinematicSimulationSpreadCommandExecutorPtr 
+  = std::shared_ptr<BarrettFingerKinematicSimulationSpreadCommandExecutor>;
+
+} // control
+} // aikido
+
+#endif