Add Instantaneous and Queued TrajectoryExecutors #259

brianhou · 2017-11-19T03:11:15Z

This is a better implementation than #251, following @mkoval's suggestions on that pull request. This PR also makes various changes to the KinematicSimulation and Ros TrajectoryExecutors implementations to make them more consistent.

Before creating a pull request

Document new methods and classes
Format code with make format

Before merging a pull request

Set version target by selecting a milestone on the right side
Summarize this change in CHANGELOG.md
Add unit test(s) for this change

more similar.

This hasn't been tested yet!

This hasn't been tested yet either!

codecov · 2017-11-19T03:11:18Z

Codecov Report

Merging #259 into master will decrease coverage by 1.45%.
The diff coverage is 25.86%.

@@            Coverage Diff             @@
##           master     #259      +/-   ##
==========================================
- Coverage   70.43%   68.98%   -1.46%     
==========================================
  Files         182      186       +4     
  Lines        5399     5494      +95     
  Branches      847      861      +14     
==========================================
- Hits         3803     3790      -13     
- Misses       1076     1187     +111     
+ Partials      520      517       -3

Impacted Files	Coverage Δ
include/aikido/control/TrajectoryExecutor.hpp	`100% <ø> (ø)`	⬆️
include/aikido/trajectory/Trajectory.hpp	`100% <ø> (ø)`	⬆️
src/control/TrajectoryRunningException.cpp	`0% <0%> (ø)`
...lude/aikido/control/TrajectoryRunningException.hpp	`0% <0%> (ø)`
.../control/KinematicSimulationTrajectoryExecutor.cpp	`0% <0%> (-79.63%)`	⬇️
src/control/ros/RosTrajectoryExecutor.cpp	`0% <0%> (ø)`	⬆️
src/control/QueuedTrajectoryExecutor.cpp	`0% <0%> (ø)`
src/control/InstantaneousTrajectoryExecutor.cpp	`85.71% <85.71%> (ø)`
... and 3 more

brianhou · 2017-11-19T05:01:35Z

This almost works when I'm running some scripts in simulation. I'm running into issues while using QueuedTrajectoryExecutor to wrap KinematicSimulationTrajectoryExecutor.

The problem is that KinematicSimulationTrajectoryExecutor::execute attempts to lock the meta-skeleton that it operates on here. For some reason, it's unable to acquire the lock when it's being wrapped by QueuedTrajectoryExecutor, although it works just fine when it's not being wrapped. I'm looking for other places where I might be neglecting to unlock a locked meta-skeleton (especially in QueuedTrajectoryExecutor), but no luck yet.

The actual queuing logic seems fine; if I comment out the skeleton locking/unlocking, the wrapped KinematicSimulationTrajectoryExecutor works.

brianhou · 2017-11-19T05:07:24Z

Ack, just figured out what was happening. In libherb, we lock the entire robot meta-skeleton before calling QueuedTrajectoryExecutor::step. I'm calling the wrapped TrajectoryExecutor::execute from QueuedTrajectoryExecutor::step, which expects the robot meta-skeleton to be unlocked as it checks the dof names. This wasn't an issue before because we would call TrajectoryExecutor::execute from the demo-level program, so the robot was unlocked. I'll think about how to fix this.

This commit moves the error-checking logic out of TrajectoryExecutor::execute into a new TrajectoryExecutor::validate method. This allows the locked error-checking logic to be performed when the lock is not held (outside of TrajectoryExecutor::step), enabling this to work for both the QueuedTrajectoryExecutor as well as the unqueued TrajectoryExecutors. In addition, this introduces a TrajectoryMetadata struct, which should replace prpy's trajectory Tags.

brianhou · 2017-11-21T02:04:40Z

@mkoval When you get a chance, would you mind looking over my proposed fix for this locking issue in 06672e5?

The short version is that the DOF validation that is performed by [Instantaneous|KinematicSimulation]TrajectoryExecutor::execute requires the robot Skeleton to be locked. However, execute is called from QueuedTrajectoryExecutor::step, which is called while the robot Skeleton is locked.

Instead, I'm now performing the DOF validation in QueuedTrajectoryExecutor::execute, rather than QueuedTrajectoryExecutor::step. Then, by the time the trajectory is actually sent to the underlying executor in step, it no longer needs to be validated and doesn't need to acquire the Skeleton lock.

mkoval

This generally looks okay to me. I am not sure how useful these features will be - but they are implemented pretty well - so I don't really have any qualms about merging this after a few minor comments are addressed.

Specific questions:

In almost all cases, we should use MAGI to handle this queuing instead of TrajectoryExecutor.
The best reason to add queuing to the TrajectoryExecutor API is to support trajectory queuing and preemption in hardware drivers - which this PR doesn't support.
What is the purpose of the trajectory tag added in this PR?

mkoval · 2017-11-22T03:54:47Z

include/aikido/control/InstantaneousTrajectoryExecutor.hpp

+  /// Constructor.
+  /// \param skeleton Skeleton to execute trajectories on.
+  ///        All trajectories must have dofs only in this skeleton.
+  InstantaneousTrajectoryExecutor(::dart::dynamics::SkeletonPtr skeleton);


Mark this as explicit to disable implicit conversions from SkeletonPtr.

Nit: Could we use a MetaSkeleton instead of a Skeleton here?

DART MetaSkeletons don't come with an associated mutex, so we wouldn't be able to lock that mutex while checking whether the DOFs specified by the trajectory are in the skeleton. It seems necessary to lock the mutex while doing that. I'd otherwise be happy to use MetaSkeleton over Skeleton.

@jslee02 why don't MetaSkeletons have mutexes?

I don't think we have a good reason for not having a mutex in MetaSkeleton. Probably the mutex in Skeleton was added when MetaSkeleton wasn't yet introduced, and we just didn't add one to MetaSkeleton in introducing it?

@mxgrey Do you know why?

If we want to add a mutex to MetaSkeleton, I think we can just move the mutex in Skeleton to MetaSkeleton.

mkoval · 2017-11-22T03:55:45Z

include/aikido/control/InstantaneousTrajectoryExecutor.hpp

+  ///        should be all in skeleton passed to the constructor.
+  /// \return future<void> for trajectory execution. If trajectory terminates
+  ///        before completion, future will be set to a runtime_error.
+  /// \throws invalid_argument if traj is invalid.


Does execute() throw invalid_argument itself? Or is the future<void> set to an exception?

It currently throws the invalid_argument immediately, rather than setting the future<void> to an exception. I was thinking that we should throw the error as early as possible, but I could see it in either place. What do you think?

mkoval · 2017-11-22T03:56:51Z

include/aikido/control/QueuedTrajectoryExecutor.hpp

+public:
+  /// Constructor
+  /// \param executor Underlying TrajectoryExecutor
+  QueuedTrajectoryExecutor(std::unique_ptr<TrajectoryExecutor> executor);


Mark this as explicit.

I am also not sure if we want to use a unique_ptr here. I don't see why QueuedTrajectoryExecutor requires exclusive ownership of the TrajectoryExecutor that it delegates to.

mkoval · 2017-11-22T03:59:37Z

include/aikido/control/QueuedTrajectoryExecutor.hpp

+  void validate(trajectory::TrajectoryPtr traj) override;
+
+  /// Execute trajectory and set future upon completion. If another trajectory
+  /// is already running, queue the trajectory for later execution.


Define what happens if a trajectory earlier in the queue terminates with an error. Most likely, we should skip executing those trajectories and set their promise<>s to an exception.

Assuming that in the queue, one trajectory's endpoint is consistent with the beginning of the other, shouldn't we stop execution if one of them terminates with an error and set all the remaining promises to an exception? (This is what I do with SequenceExecutableSolution in magi)

I think this is something to just add to the simulation trajectory executors. I've created #272 to track this.

mkoval · 2017-11-22T04:02:44Z

include/aikido/control/InstantaneousTrajectoryExecutor.hpp

+private:
+  ::dart::dynamics::SkeletonPtr mSkeleton;
+
+  std::unique_ptr<std::promise<void>> mPromise;


This member variable seems to be unused.

It's used by InstantaneousTrajectoryExecutor::execute. I think it could be possible to avoid having this promise as a field in InstantaneousTrajectoryExecutor and just have it as a local variable in execute, if you think that would be better?

mkoval · 2017-11-22T04:04:21Z

src/control/KinematicSimulationTrajectoryExecutor.cpp

+    std::lock_guard<std::mutex> lock(mMutex);
+    DART_UNUSED(lock); // Suppress unused variable warning
+
+    mInProgress = false;


Set mPromise to an exception here to unblock anyone who is waiting on it.

mkoval · 2017-11-22T04:04:35Z

src/control/KinematicSimulationTrajectoryExecutor.cpp

    trajectory::TrajectoryPtr traj)
 {
  if (!traj)
    throw std::invalid_argument("Traj is null.");

-  auto space = std::dynamic_pointer_cast<MetaSkeletonStateSpace>(
+  if (traj->metadata.executorValidated)
+    return;


What's the purpose of this check? Is there any harm in doing the validation again?

This is how I'm attempting to avoid the deadlock issue I mentioned here: #259 (comment).

In libherb, Herb::step acquires the lock on the robot skeleton before calling QueuedTrajectoryExecutor::step. If we re-validate, then we'll attempt to acquire the lock again which will cause a deadlock. I think a (dangerous?) alternative is to not lock the skeleton while doing this check.

mkoval · 2017-11-22T04:07:26Z

src/control/QueuedTrajectoryExecutor.cpp

+    catch (const std::exception& e)
+    {
+      promise->set_exception(std::current_exception());
+    }


Why manually copy the result of mFuture to promise, rather than directly returning mFuture to the caller?

I think there are two types of futures: ones from a promise made by the QueuedTrajectoryExecutor to the client, and ones from a promise made by the underlying TrajectoryExecutor to the QueuedTrajectoryExecutor.

I believe this distinction is necessary because the first type must be returned immediately after the client calls QueuedTrajectoryExecutor::execute so that the client has something to wait on. However, the second type isn't created until the underlying TrajectoryExecutor::execute is called (when the trajectory is popped off the queue), so it can't be returned immediately by QueuedTrajectoryExecutor::execute.

I think this means that I have to manually copy the result of mFuture to promise, but if there's another way I'd be happy to try that too!

In this implementation, mPromiseQueue holds all the promises that have been made by the QueuedTrajectoryExecutor to the client, while mFuture is the current future that the QueuedTrajectoryExecutor is waiting on (from the underlying TrajectoryExecutor).

mkoval · 2017-11-22T04:08:14Z

src/control/ros/RosTrajectoryExecutor.cpp

  using aikido::statespace::dart::MetaSkeletonStateSpace;

  if (!traj)
    throw std::invalid_argument("Trajectory is null.");

+  if (traj->metadata.executorValidated)
+    return;


See above. Why is this necessary?

gilwoolee · 2017-11-23T17:58:31Z

include/aikido/control/TrajectoryExecutor.hpp

-  /// Execute traj and set future upon completion.
+  /// Validate the traj in preparation for execution.
+  /// \param traj Trajectory to be validated
+  virtual void validate(trajectory::TrajectoryPtr traj) = 0;


What exactly does it mean to validate a trajectory? Do we validate that it's collision free? Or that the robot's current state is in the same state as the beginning of the trajectory?

Validating currently just means that the trajectory's DOFs match the DOFs from the skeleton that is being controlled by the TrajectoryExecutor. It could do more though -- I was thinking of it as a function for checking things that can be checked before executing the trajectory (like whether the DOFs match up). That said, I only added this to try and work around the deadlock issue.

I think verifying that the current state is in the same state as the beginning of the trajectory wouldn't fall under this category; we want to check that immediately before we actually execute the trajectory. I've added #272 for that.

I think validating whether the trajectory is collision-free shouldn't be the job of the executor, since it has no idea what the world looks like while it's executing.

brianhou · 2017-11-30T03:31:39Z

Thanks for the comments, @mkoval! ❤️

In almost all cases, we should use MAGI to handle this queuing instead of TrajectoryExecutor.
The best reason to add queuing to the TrajectoryExecutor API is to support trajectory queuing and preemption in hardware drivers - which this PR doesn't support.

Agreed. I'm not sure whether these changes will be useful, but I figured since we already had a somewhat-working version of the code that I'd clean it up in case it's a good jumping-off point for something else in the future. I've added QueuedTrajectoryExecutor::abort, but it seems difficult to actually abort an in-progress trajectory so this only aborts queued trajectories that haven't begun execution.

What is the purpose of the trajectory tag added in this PR?

This stemmed out of a conversation with @siddhss5 where he mentioned that we should have additional metadata associated with our trajectories (like we did in prpy) about whether the trajectory was smooth, constrained, which planner was used, etc.

Adding this particular piece of metadata was just so that I could avoid adding an argument to TrajectoryExecutor::execute that decided whether or not the trajectory needed to be validated (again, to avoid the deadlock situation). It's not a particularly good reason 😓

mkoval · 2017-12-04T04:13:21Z

I'm not sure whether these changes will be useful, but I figured since we already had a somewhat-working version of the code that I'd clean it up in case it's a good jumping-off point for something else in the future.

I agree that this is potentially useful once we have more control over trajectory execution on real hardware in the future (cancelling, preempting, etc). 👍

I've added QueuedTrajectoryExecutor::abort(), but it seems difficult to actually abort an in-progress trajectory so this only aborts queued trajectories that haven't begun execution.

If you make abort() part of the generic TrajectoryExecutor API, then QueuedTrajectoryExecutor could call abort() on the underlying trajectory executor. This seems pretty straightforward for KinematicTrajectoryExecutor, since we could set a flag that interrupts the next iteration of the simulation loop.

We can eventually support this on RosTrajectoryExecutor by canceling the actionlib goal, once we support that on the ReWD controller side. That's probably outside the scope of this pull request. 🙃

Adding this particular piece of metadata was just so that I could avoid adding an argument to TrajectoryExecutor::execute() that decided whether or not the trajectory needed to be validated (again, to avoid the deadlock situation). It's not a particularly good reason 😓

Trajectory tags in general seem pretty useful for all the reasons that @siddhss5 gave. I'm not convinced that this is a great application of them, though. 😬 Unfortunately, I don't have a better solution to fixing the deadlock.

brianhou · 2017-12-04T06:43:41Z

Thanks! I've added abort to the TrajectoryExecutor interface, and I'll merge this PR once Travis says we're good to go.

I'm not convinced that this is a great application of [trajectory tags], though. Unfortunately, I don't have a better solution to fixing the deadlock.

The options I've thought of are either (1) splitting up the validation like this [and either having this silly trajectory tag, or just moving this boolean from a trajectory tag to an execute argument], (2) locking the skeleton in Herb::step and passing the owned lock to the executors, (3) not locking the skeleton in Herb::step, or (4) not locking the skeleton in the executors. I might revisit this later and see how annoying (2) would be.

mkoval · 2017-12-04T15:03:18Z

My original intent with the step() interface was (3). None of the step() functions should acquire locks internally. Instead, they should document their requirements (e.g. "requires the Skeleton passed to the constructor to be locked") and rely on the caller to acquire the lock. In the case of HERB, that would be Herb::step() in libherb. Would that solve the problem?

brianhou · 2017-12-04T19:21:27Z

Not really, we're actually still following your original intent. The tricky part is writing QueuedTrajectoryExecutor::step so that it doesn't need to acquire any locks (since it calls TrajectoryExecutor::execute which makes no such promises). I'm generally in favor of the architecture that we have currently.

* Make KinematicSimulationTrajectoryExecutor and RosTrajectoryExecutor more similar. * Introduce TrajectoryRunningException. * Add InstantaneousTrajectoryExecutor. This hasn't been tested yet! * Add QueuedTrajectoryExecutor. This hasn't been tested yet either! * Fix typos. * Add InstantaneousTrajectoryExecutor tests. * Add QueuedTrajectoryExecutor tests. * Format code. * Fix locking issues. This commit moves the error-checking logic out of TrajectoryExecutor::execute into a new TrajectoryExecutor::validate method. This allows the locked error-checking logic to be performed when the lock is not held (outside of TrajectoryExecutor::step), enabling this to work for both the QueuedTrajectoryExecutor as well as the unqueued TrajectoryExecutors. In addition, this introduces a TrajectoryMetadata struct, which should replace prpy's trajectory Tags. * Make TrajectoryExecutor constructors explicit. * Support aborting queued trajectories. * Address more of Mike's review comments. * Add abort to TrajectoryExecutor interface. * Fix segfault when mPromise is null. * Fix formatting issues.

brianhou added 4 commits November 18, 2017 14:47

Make KinematicSimulationTrajectoryExecutor and RosTrajectoryExecutor

926de21

more similar.

Introduce TrajectoryRunningException.

3cf6db0

Add InstantaneousTrajectoryExecutor.

4a7f971

This hasn't been tested yet!

Add QueuedTrajectoryExecutor.

e329a4e

This hasn't been tested yet either!

brianhou added this to the Aikido 0.2.0 milestone Nov 19, 2017

brianhou requested review from mkoval and gilwoolee November 19, 2017 03:11

Merge branch 'master' into enhancement/brianhou/traj-queuing

3222beb

brianhou and others added 4 commits November 19, 2017 10:17

Merge branch 'master' into enhancement/brianhou/traj-queuing

291b1b2

Fix typos.

9248c80

Add InstantaneousTrajectoryExecutor tests.

0863c43

Add QueuedTrajectoryExecutor tests.

c4421fe

brianhou force-pushed the enhancement/brianhou/traj-queuing branch from 231b8a8 to c4421fe Compare November 20, 2017 17:13

brianhou and others added 3 commits November 20, 2017 09:13

Merge branch 'master' into enhancement/brianhou/traj-queuing

b2199c1

Format code.

d6455d6

mkoval reviewed Nov 22, 2017

View reviewed changes

Merge branch 'master' into enhancement/brianhou/traj-queuing

52905c4

gilwoolee reviewed Nov 23, 2017

View reviewed changes

brianhou and others added 4 commits November 29, 2017 16:18

Merge branch 'master' into enhancement/brianhou/traj-queuing

af5663c

Make TrajectoryExecutor constructors explicit.

01f43e7

Support aborting queued trajectories.

6f96df2

Address more of Mike's review comments.

84bdb46

Merge branch 'master' into enhancement/brianhou/traj-queuing

5d9457a

brianhou added 3 commits December 3, 2017 22:34

Add abort to TrajectoryExecutor interface.

9db650e

Fix segfault when mPromise is null.

f6c2408

Fix formatting issues.

5b0aedc

brianhou mentioned this pull request Dec 4, 2017

Queue trajectories in TrajectoryExecutor #251

Closed

5 tasks

brianhou merged commit e210f95 into master Dec 4, 2017

brianhou deleted the enhancement/brianhou/traj-queuing branch December 4, 2017 19:21

mkoval mentioned this pull request Dec 12, 2017

Consider adding name field to Trajectory #284

Open

jslee02 mentioned this pull request Feb 25, 2018

const correctness #349

Merged

5 tasks

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add Instantaneous and Queued TrajectoryExecutors #259

Add Instantaneous and Queued TrajectoryExecutors #259

brianhou commented Nov 19, 2017 •

edited

Loading

codecov bot commented Nov 19, 2017 •

edited

Loading

brianhou commented Nov 19, 2017

brianhou commented Nov 19, 2017 •

edited

Loading

brianhou commented Nov 21, 2017

mkoval left a comment

mkoval Nov 22, 2017

brianhou Nov 30, 2017

jslee02 Nov 30, 2017 •

edited

Loading

mkoval Nov 22, 2017

brianhou Nov 30, 2017

mkoval Nov 22, 2017

mkoval Nov 22, 2017

gilwoolee Nov 23, 2017

brianhou Nov 30, 2017

mkoval Nov 22, 2017

brianhou Nov 30, 2017

mkoval Nov 22, 2017

mkoval Nov 22, 2017

brianhou Nov 30, 2017

mkoval Nov 22, 2017

brianhou Nov 30, 2017

mkoval Nov 22, 2017

gilwoolee Nov 23, 2017

brianhou Nov 30, 2017

brianhou commented Nov 30, 2017

mkoval commented Dec 4, 2017

brianhou commented Dec 4, 2017

mkoval commented Dec 4, 2017

brianhou commented Dec 4, 2017

Add Instantaneous and Queued TrajectoryExecutors #259

Add Instantaneous and Queued TrajectoryExecutors #259

Conversation

brianhou commented Nov 19, 2017 • edited Loading

codecov bot commented Nov 19, 2017 • edited Loading

Codecov Report

brianhou commented Nov 19, 2017

brianhou commented Nov 19, 2017 • edited Loading

brianhou commented Nov 21, 2017

mkoval left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

jslee02 Nov 30, 2017 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

brianhou commented Nov 30, 2017

mkoval commented Dec 4, 2017

brianhou commented Dec 4, 2017

mkoval commented Dec 4, 2017

brianhou commented Dec 4, 2017

brianhou commented Nov 19, 2017 •

edited

Loading

codecov bot commented Nov 19, 2017 •

edited

Loading

brianhou commented Nov 19, 2017 •

edited

Loading

jslee02 Nov 30, 2017 •

edited

Loading