From 78a86de65ad10fb4fa70f6738d6f5fe2c42dd97a Mon Sep 17 00:00:00 2001 From: Gilwoo Lee Date: Wed, 19 Apr 2017 01:30:45 -0400 Subject: [PATCH 1/5] Removes locking from step() --- ...arrettHandKinematicSimulationPositionCommandExecutor.hpp | 4 +++- .../control/KinematicSimulationTrajectoryExecutor.hpp | 4 +++- src/control/KinematicSimulationTrajectoryExecutor.cpp | 6 ++---- 3 files changed, 8 insertions(+), 6 deletions(-) diff --git a/include/aikido/control/BarrettHandKinematicSimulationPositionCommandExecutor.hpp b/include/aikido/control/BarrettHandKinematicSimulationPositionCommandExecutor.hpp index c83c477e92..08f93f52a3 100644 --- a/include/aikido/control/BarrettHandKinematicSimulationPositionCommandExecutor.hpp +++ b/include/aikido/control/BarrettHandKinematicSimulationPositionCommandExecutor.hpp @@ -47,7 +47,9 @@ class BarrettHandKinematicSimulationPositionCommandExecutor /// \return Future which becomes available when the execution completes. std::future execute(const Eigen::VectorXd& goalPositions) override; - // Documentation inherited. + /// 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. diff --git a/include/aikido/control/KinematicSimulationTrajectoryExecutor.hpp b/include/aikido/control/KinematicSimulationTrajectoryExecutor.hpp index 945129e6d2..61e01996a9 100644 --- a/include/aikido/control/KinematicSimulationTrajectoryExecutor.hpp +++ b/include/aikido/control/KinematicSimulationTrajectoryExecutor.hpp @@ -35,7 +35,9 @@ class KinematicSimulationTrajectoryExecutor : public TrajectoryExecutor std::future execute( trajectory::TrajectoryPtr traj) override; - // Documentation inherited. + /// 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: diff --git a/src/control/KinematicSimulationTrajectoryExecutor.cpp b/src/control/KinematicSimulationTrajectoryExecutor.cpp index 2a127d9098..0fd2aafe51 100644 --- a/src/control/KinematicSimulationTrajectoryExecutor.cpp +++ b/src/control/KinematicSimulationTrajectoryExecutor.cpp @@ -106,7 +106,7 @@ void KinematicSimulationTrajectoryExecutor::step() } auto timeSinceBeginning = system_clock::now() - mExecutionStartTime; - double t = duration_cast(timeSinceBeginning).count(); + auto t = duration(timeSinceBeginning).count(); // Can't do static here because MetaSkeletonStateSpace inherits // CartesianProduct which inherits virtual StateSpace @@ -117,10 +117,7 @@ void KinematicSimulationTrajectoryExecutor::step() mTraj->evaluate(t, state); - // Lock the skeleton, set state. - std::unique_lock skeleton_lock(mSkeleton->getMutex()); space->setState(state); - skeleton_lock.unlock(); // Check if trajectory has completed. bool const is_done = (t >= mTraj->getEndTime()); @@ -129,6 +126,7 @@ void KinematicSimulationTrajectoryExecutor::step() mPromise->set_value(); mInExecution = false; } + } } From 1d734b22faafa6c595696f2336ce89ccf692baba Mon Sep 17 00:00:00 2001 From: Gilwoo Lee Date: Thu, 20 Apr 2017 14:19:42 -0400 Subject: [PATCH 2/5] Addressing Mike and JS's comments --- ...rrettHandKinematicSimulationPositionCommandExecutor.hpp | 2 ++ .../control/KinematicSimulationTrajectoryExecutor.hpp | 2 ++ include/aikido/trajectory/Trajectory.hpp | 4 ++-- src/control/KinematicSimulationTrajectoryExecutor.cpp | 7 ++++--- 4 files changed, 10 insertions(+), 5 deletions(-) diff --git a/include/aikido/control/BarrettHandKinematicSimulationPositionCommandExecutor.hpp b/include/aikido/control/BarrettHandKinematicSimulationPositionCommandExecutor.hpp index 08f93f52a3..f1f8a4196e 100644 --- a/include/aikido/control/BarrettHandKinematicSimulationPositionCommandExecutor.hpp +++ b/include/aikido/control/BarrettHandKinematicSimulationPositionCommandExecutor.hpp @@ -47,6 +47,8 @@ class BarrettHandKinematicSimulationPositionCommandExecutor /// \return Future which becomes available when the execution completes. std::future execute(const Eigen::VectorXd& goalPositions) override; + /// \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. diff --git a/include/aikido/control/KinematicSimulationTrajectoryExecutor.hpp b/include/aikido/control/KinematicSimulationTrajectoryExecutor.hpp index 61e01996a9..b071ffc4ca 100644 --- a/include/aikido/control/KinematicSimulationTrajectoryExecutor.hpp +++ b/include/aikido/control/KinematicSimulationTrajectoryExecutor.hpp @@ -35,6 +35,8 @@ class KinematicSimulationTrajectoryExecutor : public TrajectoryExecutor std::future execute( trajectory::TrajectoryPtr traj) override; + /// \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. diff --git a/include/aikido/trajectory/Trajectory.hpp b/include/aikido/trajectory/Trajectory.hpp index 4d023e01b0..b4e277173f 100644 --- a/include/aikido/trajectory/Trajectory.hpp +++ b/include/aikido/trajectory/Trajectory.hpp @@ -50,8 +50,8 @@ class Trajectory /// function is implementation-defined if \c _t is not between /// \c getStartTime() and \c getEndTime(). /// - /// \param _t time parameter - /// \param[out] _state output state of the trajectory at time \c _t + /// \param _t time parameter (in seconds) + /// \param[out] _state output state of the trajectory at time \c _t (seconds) virtual void evaluate(double _t, statespace::StateSpace::State *_state) const = 0; diff --git a/src/control/KinematicSimulationTrajectoryExecutor.cpp b/src/control/KinematicSimulationTrajectoryExecutor.cpp index 0fd2aafe51..aba4f662ef 100644 --- a/src/control/KinematicSimulationTrajectoryExecutor.cpp +++ b/src/control/KinematicSimulationTrajectoryExecutor.cpp @@ -105,8 +105,9 @@ void KinematicSimulationTrajectoryExecutor::step() mInExecution = false; } + using seconds = std::chrono::duration; auto timeSinceBeginning = system_clock::now() - mExecutionStartTime; - auto t = duration(timeSinceBeginning).count(); + auto tsec = duration_cast(timeSinceBeginning).count(); // Can't do static here because MetaSkeletonStateSpace inherits // CartesianProduct which inherits virtual StateSpace @@ -115,12 +116,12 @@ void KinematicSimulationTrajectoryExecutor::step() auto metaSkeleton = space->getMetaSkeleton(); auto state = space->createState(); - mTraj->evaluate(t, state); + mTraj->evaluate(tsec, state); space->setState(state); // 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(); From 4e84775b1e193a95c95506c3ce769165865675b9 Mon Sep 17 00:00:00 2001 From: Gilwoo Lee Date: Thu, 20 Apr 2017 14:20:27 -0400 Subject: [PATCH 3/5] Removing empty line --- src/control/KinematicSimulationTrajectoryExecutor.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/control/KinematicSimulationTrajectoryExecutor.cpp b/src/control/KinematicSimulationTrajectoryExecutor.cpp index aba4f662ef..4230bdff7a 100644 --- a/src/control/KinematicSimulationTrajectoryExecutor.cpp +++ b/src/control/KinematicSimulationTrajectoryExecutor.cpp @@ -127,7 +127,6 @@ void KinematicSimulationTrajectoryExecutor::step() mPromise->set_value(); mInExecution = false; } - } } From 38682d6da15b3d8afb864d9221bbe7b77b4796a2 Mon Sep 17 00:00:00 2001 From: Gilwoo Lee Date: Thu, 20 Apr 2017 14:41:43 -0400 Subject: [PATCH 4/5] Fixing float to double --- src/control/KinematicSimulationTrajectoryExecutor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/control/KinematicSimulationTrajectoryExecutor.cpp b/src/control/KinematicSimulationTrajectoryExecutor.cpp index 4230bdff7a..4eb0e8b557 100644 --- a/src/control/KinematicSimulationTrajectoryExecutor.cpp +++ b/src/control/KinematicSimulationTrajectoryExecutor.cpp @@ -105,7 +105,7 @@ void KinematicSimulationTrajectoryExecutor::step() mInExecution = false; } - using seconds = std::chrono::duration; + using seconds = std::chrono::duration; auto timeSinceBeginning = system_clock::now() - mExecutionStartTime; auto tsec = duration_cast(timeSinceBeginning).count(); From 81ec3c72cc7fe691f6f76a27bfe6a26e924c3d75 Mon Sep 17 00:00:00 2001 From: Gilwoo Lee Date: Sat, 22 Apr 2017 14:31:42 -0400 Subject: [PATCH 5/5] Addressing Mike's comments --- include/aikido/trajectory/Trajectory.hpp | 4 ++-- src/control/KinematicSimulationTrajectoryExecutor.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/include/aikido/trajectory/Trajectory.hpp b/include/aikido/trajectory/Trajectory.hpp index b4e277173f..4d023e01b0 100644 --- a/include/aikido/trajectory/Trajectory.hpp +++ b/include/aikido/trajectory/Trajectory.hpp @@ -50,8 +50,8 @@ class Trajectory /// function is implementation-defined if \c _t is not between /// \c getStartTime() and \c getEndTime(). /// - /// \param _t time parameter (in seconds) - /// \param[out] _state output state of the trajectory at time \c _t (seconds) + /// \param _t time parameter + /// \param[out] _state output state of the trajectory at time \c _t virtual void evaluate(double _t, statespace::StateSpace::State *_state) const = 0; diff --git a/src/control/KinematicSimulationTrajectoryExecutor.cpp b/src/control/KinematicSimulationTrajectoryExecutor.cpp index 4eb0e8b557..a49777cbf3 100644 --- a/src/control/KinematicSimulationTrajectoryExecutor.cpp +++ b/src/control/KinematicSimulationTrajectoryExecutor.cpp @@ -105,9 +105,9 @@ void KinematicSimulationTrajectoryExecutor::step() mInExecution = false; } - using seconds = std::chrono::duration; auto timeSinceBeginning = system_clock::now() - mExecutionStartTime; - auto tsec = duration_cast(timeSinceBeginning).count(); + auto tsec = duration_cast>( + timeSinceBeginning).count(); // Can't do static here because MetaSkeletonStateSpace inherits // CartesianProduct which inherits virtual StateSpace