Removes locking from step() (#182)

* Removes locking from step()

* Addressing Mike and JS's comments

* Removing empty line

* Fixing float to double

* Addressing Mike's comments

include/aikido/control/BarrettHandKinematicSimulationPositionCommandExecutor.hpp

@@ -47,7 +47,11 @@ class BarrettHandKinematicSimulationPositionCommandExecutor
   /// \return Future which becomes available when the execution completes.
   std::future<void> execute(const Eigen::VectorXd& goalPositions) override;
 
-  // Documentation inherited.
+  /// \copydoc PositionCommandExecutor::step()
+  ///
+  /// 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.
   void step() override;
 
   /// Resets CollisionGroup to check collision with fingers.

include/aikido/control/KinematicSimulationTrajectoryExecutor.hpp

@@ -35,7 +35,11 @@ class KinematicSimulationTrajectoryExecutor : public TrajectoryExecutor
   std::future<void> execute(
     trajectory::TrajectoryPtr traj) override;
 
-  // Documentation inherited.
+  /// \copydoc PositionCommandExecutor::step()
+  ///
+  /// 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.
   void step() override;
 
 private:

src/control/KinematicSimulationTrajectoryExecutor.cpp

@@ -106,7 +106,8 @@ void KinematicSimulationTrajectoryExecutor::step()
   }
 
   auto timeSinceBeginning = system_clock::now() - mExecutionStartTime;
-  double t = duration_cast<seconds>(timeSinceBeginning).count();
+  auto tsec = duration_cast<std::chrono::duration<double>>(
+    timeSinceBeginning).count();
 
   // Can't do static here because MetaSkeletonStateSpace inherits
   // CartesianProduct which inherits virtual StateSpace
@@ -115,15 +116,12 @@ void KinematicSimulationTrajectoryExecutor::step()
   auto metaSkeleton = space->getMetaSkeleton();
   auto state = space->createState();
 
-  mTraj->evaluate(t, state);
+  mTraj->evaluate(tsec, state);
 
-  // Lock the skeleton, set state.
-  std::unique_lock<std::mutex> skeleton_lock(mSkeleton->getMutex());
   space->setState(state);
-  skeleton_lock.unlock();
 
   // Check if trajectory has completed.
-  bool const is_done = (t >= mTraj->getEndTime());
+  bool const is_done = (tsec >= mTraj->getEndTime());
   if (is_done) {
     mTraj.reset();
     mPromise->set_value();