From e46aba0455930cb37946978ed2423f9cb8b379ff Mon Sep 17 00:00:00 2001 From: Brian Hou Date: Thu, 19 Jul 2018 12:46:40 -0700 Subject: [PATCH] Fix bug with negative execution times in Executor::step. (#450) * Fix bug with negative execution times in KinematicSimulationTrajectoryExecutor. * Fix tests. * Update CHANGELOG.md. --- CHANGELOG.md | 2 +- src/control/KinematicSimulationTrajectoryExecutor.cpp | 4 +++- .../control/test_KinematicSimulationTrajectoryExecutor.cpp | 6 +++--- tests/control/test_QueuedTrajectoryExecutor.cpp | 5 ++--- 4 files changed, 9 insertions(+), 8 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index accd922712..39bdd24512 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -30,7 +30,7 @@ * Control * Fixed CollisionGroup bugs in Hand executors: [#299](https://github.com/personalrobotics/aikido/pull/299) - * Rewrote executors for faster-than-realtime simulation: [#316](https://github.com/personalrobotics/aikido/pull/316) + * Rewrote executors for faster-than-realtime simulation: [#316](https://github.com/personalrobotics/aikido/pull/316), [#450](https://github.com/personalrobotics/aikido/pull/450) * Introduced uniform and dart namespaces: [#342](https://github.com/personalrobotics/aikido/pull/342) * Removed Barrett-specific hand executors: [#380](https://github.com/personalrobotics/aikido/pull/380) diff --git a/src/control/KinematicSimulationTrajectoryExecutor.cpp b/src/control/KinematicSimulationTrajectoryExecutor.cpp index cd0d822efc..bd6ebbb635 100644 --- a/src/control/KinematicSimulationTrajectoryExecutor.cpp +++ b/src/control/KinematicSimulationTrajectoryExecutor.cpp @@ -125,8 +125,10 @@ void KinematicSimulationTrajectoryExecutor::step( const auto executionTime = std::chrono::duration(timeSinceBeginning).count(); + // executionTime may be negative if the thread calling \c step is queued + // before and dequeued after \c execute is called. if (executionTime < 0) - throw std::invalid_argument("Timepoint is before execution start time."); + return; auto state = mStateSpace->createState(); mTraj->evaluate(executionTime, state); diff --git a/tests/control/test_KinematicSimulationTrajectoryExecutor.cpp b/tests/control/test_KinematicSimulationTrajectoryExecutor.cpp index fc99ab6fc0..3536c526a4 100644 --- a/tests/control/test_KinematicSimulationTrajectoryExecutor.cpp +++ b/tests/control/test_KinematicSimulationTrajectoryExecutor.cpp @@ -210,7 +210,8 @@ TEST_F( EXPECT_DOUBLE_EQ(mSkeleton->getDof(0)->getPosition(), 1.0); } -TEST_F(KinematicSimulationTrajectoryExecutorTest, step_NegativeTimepoint_Throws) +TEST_F( + KinematicSimulationTrajectoryExecutorTest, step_NegativeTimepoint_NoThrows) { KinematicSimulationTrajectoryExecutor executor(mSkeleton); @@ -219,8 +220,7 @@ TEST_F(KinematicSimulationTrajectoryExecutorTest, step_NegativeTimepoint_Throws) auto simulationClock = std::chrono::system_clock::now(); auto future = executor.execute(mTraj); - EXPECT_THROW( - executor.step(simulationClock - stepTime), std::invalid_argument); + EXPECT_NO_THROW(executor.step(simulationClock - stepTime)); std::future_status status; do diff --git a/tests/control/test_QueuedTrajectoryExecutor.cpp b/tests/control/test_QueuedTrajectoryExecutor.cpp index 5b805be8c6..b20e3c4fee 100644 --- a/tests/control/test_QueuedTrajectoryExecutor.cpp +++ b/tests/control/test_QueuedTrajectoryExecutor.cpp @@ -191,7 +191,7 @@ TEST_F( EXPECT_EQ(f1.wait_for(waitTime), std::future_status::ready); } -TEST_F(QueuedTrajectoryExecutorTest, step_NegativeTimepoint_Throws) +TEST_F(QueuedTrajectoryExecutorTest, step_NegativeTimepoint_NoThrows) { QueuedTrajectoryExecutor executor(std::move(mExecutor)); @@ -202,8 +202,7 @@ TEST_F(QueuedTrajectoryExecutorTest, step_NegativeTimepoint_Throws) auto f2 = executor.execute(mTraj2); executor.step(simulationClock); // dequeue trajectory - EXPECT_THROW( - executor.step(simulationClock - stepTime), std::invalid_argument); + EXPECT_NO_THROW(executor.step(simulationClock - stepTime)); std::future_status status; do