Add World::State #265

gilwoolee · 2017-11-23T08:43:03Z

This PR addresses #261 . Based on discussion on #263, this PR creates WorldState struct and provides equals method to compare with other WorldStates.

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

codecov · 2017-11-23T09:15:03Z

Codecov Report

Merging #265 into master will decrease coverage by 0.07%.
The diff coverage is 55.55%.

@@            Coverage Diff             @@
##           master     #265      +/-   ##
==========================================
- Coverage   70.63%   70.56%   -0.08%     
==========================================
  Files         186      188       +2     
  Lines        5510     5537      +27     
  Branches      863      869       +6     
==========================================
+ Hits         3892     3907      +15     
- Misses       1089     1097       +8     
- Partials      529      533       +4

Impacted Files	Coverage Δ
src/planner/WorldStateSaver.cpp	`0% <0%> (ø)`
include/aikido/planner/World.hpp	`100% <100%> (ø)`
src/planner/World.cpp	`76.74% <70%> (-2.05%)`	⬇️

mxgrey · 2017-11-23T19:02:24Z

Just to clarify some things about the semantics in DART:

Skeleton::Configuration follows the classical concept of a robot configuration, which is basically a glorified vector that describes only the relevant components of the robot state using as few parameters as possible. This is usually all that's needed for kinematic planning.

Skeleton::State contains a perfect and complete description of the state that the robot is in, making it suitable for perfect recreations of fully dynamic simulations. The Skeleton::State class is extensible, so if users add their own "Aspects" to the Skeleton or any of its BodyNodes or Joints, then the state information of those Aspects will also be captured by the Skeleton::State. The most concrete example of this can be seen in SoftBodyNode: The deformation of the soft body is captured in a Skeleton::State, but it is not captured in a Skeleton::Configuration.

Since the World::State implementation here is building off of Skeleton::Configuration, you might want to consider naming it World::Configuration. That would leave you with an opportunity to create a World::State later on which could build off of Skeleton::State and capture more than what this current implementation is able to.

gilwoolee · 2017-11-23T23:56:10Z

Thanks for the comments @mxgrey .

Indeed, what you described is precisely why I chose to use Skeleton::Configuration instead of Skeleton::State. Skeleton::Configuration has everything we need from Skeleton for kinematic planning. I initially thought of calling ours as World::Configuration, but the reason why I decided to go with World::State is because, although the current version only contains skeleton configurations, World::State should contain everything we need for kinematic planning in aikido, which might include a set of collision groups(dart::collision::CollisionGroup) or collision constraints(aikido::constraint::CollisionFree), or joint limits, etc.

In fact, now might be a good time to discuss whether to put in collision groups or anything else, to make World::State contain everything we need for making planning calls. Any thoughts @mkoval @jslee02 @brianhou @mxgrey ?

Additionally, I think I'll add WorldStateSaver, just like aikido::statespace::dart::MetaSkeletonStateSpaceSaver, but it seems like once we have the former we might not need the latter too much. In general, shouldn't planners take World to restore after planning as well as MetaSkeletonStateSpace to plan with? Restoring World should include restoring MetaSksletonState .

mxgrey · 2017-11-26T05:46:37Z

You may want to consider putting information like collision groups and constraints into a World::Properties data structure, assuming those are parameters that describe the scenario and don't change over time. That way, if you're recording or comparing states for a scenario, you can be a little leaner in terms of memory usage and the reduce the number of operations that are performed in a comparison.

Note that the Skeleton::Properties class records time-independent information like joint limits.

… into WorldState

gilwoolee · 2017-12-01T06:47:22Z

@brianhou @mkoval Could you review this?

Conflicts: tests/statespace/dart/test_MetaSkeletonStateSpaceSaver.cpp

brianhou

Looks mostly fine to me!

brianhou · 2017-12-03T21:26:23Z

include/aikido/planner/World.hpp

 class World
 {
 public:
+  // Encapsulates the state of the World.
+  struct State


Should State also have some notion of the Skeletons it contains? Maybe the Skeleton names?

One option is to index configurations by Skeleton name, i.e. switch from an std::vector to an std::unordered_map.

brianhou · 2017-12-03T21:27:57Z

include/aikido/planner/World.hpp

+  {
+    std::vector<dart::dynamics::Skeleton::Configuration> configurations;
+
+    /// Returns true if two world states are the same.


Is there any reason to use equals rather than operator==?

I don't see any reason why we couldn't use operator == here. If we define that, then we should also remember to define operator !=. 🙃

Also, this likely should be flagged as const.

brianhou · 2017-12-03T21:28:55Z

src/planner/World.cpp

+{
+  if (state.configurations.size() != mSkeletons.size())
+    throw std::invalid_argument(
+        "World::State and this World does not have the same number of "


Nit: does -> do

brianhou · 2017-12-03T21:31:14Z

src/planner/WorldStateSaver.cpp

+
+WorldStateSaver::WorldStateSaver(
+    WorldPtr world)
+  : mWorld{world}


Would it make sense to initialize mWorldState here instead?

WorldState does not keep a pointer to a specific world, but when WorldStateSaver is destroyed, we typically want to restore the state of a World to a previous state. So I think we need to keep a pointer to the world?

brianhou · 2017-12-03T21:32:19Z

src/planner/WorldStateSaver.cpp

+namespace aikido {
+namespace planner {
+
+WorldStateSaver::WorldStateSaver(


Nit: can we also add basic tests for WorldStateSaver?

brianhou · 2017-12-03T21:33:39Z

tests/planner/test_World.cpp

+
+  EXPECT_FALSE(mWorld->getState().equals(otherWorld->getState()));
+  EXPECT_FALSE(otherWorld->getState().equals(mWorld->getState()));
+}


Nit: can we add a couple tests for setState to verify that it throws errors?

mkoval

This generally looks okay to me.

I don't think this pull request really solves the tricky question of what it means for two Worlds to be equivalent. The current logic would treat two entirely different worlds as equivalent if they happen to contain the same number of Skeletons with the same dimension Configurations.

I also am not convinced that WorldStateSaver is a good idea. When we were writing Magi we discovered that it is generally safer, more efficient, and easier to implement more granular state saving. I.e. only save the state over the parts of the environment that you modify in an ExecutableSolution. I am fine with keeping this functionality for now, but generally think we should avoid using it in routine operation.

These are hard problems and I don't have a great solution. Maybe @psigen or @siddhss5 have better ideas.

mkoval · 2017-12-04T03:33:24Z

include/aikido/planner/World.hpp

 class World
 {
 public:
+  // Encapsulates the state of the World.
+  struct State


One option is to index configurations by Skeleton name, i.e. switch from an std::vector to an std::unordered_map.

mkoval · 2017-12-04T03:33:49Z

include/aikido/planner/World.hpp

+  {
+    std::vector<dart::dynamics::Skeleton::Configuration> configurations;
+
+    /// Returns true if two world states are the same.


I don't see any reason why we couldn't use operator == here. If we define that, then we should also remember to define operator !=. 🙃

Also, this likely should be flagged as const.

mkoval · 2017-12-04T03:35:56Z

include/aikido/planner/WorldStateSaver.hpp

+  /// WorldStateSaver is destructed.
+  ///
+  /// \param _space WorldState to save/restore
+  explicit WorldStateSaver(WorldPtr world);


We should be able to flag this argument as const.

Also, I don't see a strong reason to make this accept a smart pointer rather than a reference or raw pointer.

mkoval · 2017-12-04T03:36:27Z

include/aikido/planner/WorldStateSaver.hpp

+
+  virtual ~WorldStateSaver();
+
+  // WorldStateSaver is uncopyable, must use std::move


I don't see any harm in making this class copyable. What's the concern here?

mkoval · 2017-12-04T03:40:00Z

src/planner/World.cpp

+  }
+
+  return true;
+}


I think this whole function could be replaced with:

return configurations == other.configurations;

Something is also a bit funny about this. Shouldn't we verify that the two Configurations contain the same Skeletons somehow?

We're not checking whether the skeletons are the same because the skeletons may be cloned ones. Shouldn't we want to be able to say that the two cloned worlds are in the same state if they have the same configurations?

mkoval · 2017-12-04T03:52:39Z

src/planner/World.cpp

+  for (size_t idx = 0; idx < state.configurations.size(); ++idx)
+  {
+    std::lock_guard<std::mutex> lock(mSkeletons[idx]->getMutex());
+    mSkeletons[idx]->setConfiguration(state.configurations[idx]);


Just a thought, outside the scope of this pull request: At some point we may want to abandon using per-Skeleton locks and instead having a single lock for the entire World. This wasn't an option before because we didn't have a World object. 😉

… into WorldState

gilwoolee · 2017-12-06T02:59:37Z

@brianhou could you give it a (hopefully) final pass? :)

gilwoolee · 2017-12-06T03:06:27Z

@mkoval Thanks for the feedback! I agree that saving/restoring a smaller subset of the world which gets modified during planning is a better idea, which we're not currently doing in magi. I think it'd still be useful to be able to take a snapshot of the world's current state, for example, in serializing/deserializing, etc. We might want to support selective state saving/restoring in the near future, so I'll leave it to #276.

gilwoolee · 2017-12-06T03:08:29Z

There also seems to be a bit of detailed comparison that needs to be done in order to say whether two skeletons have the same "state space", which is necessary when we are doing == operation or setState operation. I am leaving that for later and just comparing skeleton names for now.

mkoval · 2017-12-06T04:26:21Z

I think comparing Skeleton names is fine for now. I agree that comparing StateSpaces is a giant can of worms - and I don't have a good solution in mind. Let's revisit that issue once we have some solid demos running in Aikido on HERB. Then we'll have some more context for the discussion.

brianhou · 2017-12-06T19:44:10Z

src/planner/World.cpp

+{
+  World::State state;
+
+  for (auto skeleton : mSkeletons)


I think this can be const auto& skeleton?

brianhou · 2017-12-06T19:49:14Z

src/planner/World.cpp

+          "Skeleton " + name + " does not exist in state.");
+  }
+
+  for (const auto& skeleton : mSkeletons)


Could this loop be merged into the previous one?

I didn't want to merge these two because I don't want this to throw an error after modifying some of the skeletons.

brianhou · 2017-12-06T19:52:15Z

src/planner/World.cpp

+  for (const auto& skeleton : mSkeletons)
+  {
+    std::lock_guard<std::mutex> lock(skeleton->getMutex());
+    skeleton->setConfiguration(state.configurations.at(skeleton->getName()));


Can we use state.configurations[skeleton->getName()] instead of at? Since we've already checked that the keys should all exist, at seems unnecessary.

Actually, maybe we could merge the two loops and then just use at instead of explicitly calling find and then indexing? @jslee02 is it inefficient to catch the (possible) out_of_range exception from at and then trigger our own more helpful exception?

Hmm. Can't do [] because [] is a non-const operator and I'm taking in const State&.
https://stackoverflow.com/questions/15660838/error-no-match-for-operator-in-near-match

I'll keep it as is.

* Add World.equalConfiguration method. * Code format * Add WorldState * Add WorldStateSaver * Move MetaSkeletonStateSpaceSaver-impl to cpp file * Add tests for MetaSkeletonStateSpaceSaver * Remove unused header from test_MetaSkeletonStateSpaceSaver * Address Mike and Brian's comments * Modify format, docstring * Address Brian's comments

gilwoolee added 4 commits November 20, 2017 15:50

Add World.equalConfiguration method.

2194b4c

Code format

f6f64bd

Add WorldState

80f43cb

Merge branch 'master' into WorldState

b4a085b

gilwoolee mentioned this pull request Nov 23, 2017

World equal configuration #263

Closed

5 tasks

gilwoolee requested review from jslee02, mkoval and brianhou November 23, 2017 08:44

gilwoolee added 3 commits November 29, 2017 14:20

Merge branch 'master' into WorldState

c6054e6

Add WorldStateSaver

cb9206f

Merge branch 'WorldState' of https://github.com/personalrobotics/aikido…

e12dfd6

… into WorldState

gilwoolee added 5 commits December 1, 2017 18:55

Move MetaSkeletonStateSpaceSaver-impl to cpp file

710298a

Add tests for MetaSkeletonStateSpaceSaver

8398e21

Remove unused header from test_MetaSkeletonStateSpaceSaver

d200af2

Merge branch 'bugfix/MetaSkeletonStateSpaceSaver' into WorldState

9839d03

Merge branch 'master' into WorldState

30d1b80

Conflicts: tests/statespace/dart/test_MetaSkeletonStateSpaceSaver.cpp

brianhou suggested changes Dec 3, 2017

View reviewed changes

mkoval approved these changes Dec 4, 2017

View reviewed changes

gilwoolee added 5 commits December 5, 2017 12:39

Merge branch 'master' into WorldState

5d71f21

Merge branch 'master' into WorldState

705fdc7

Merge branch 'WorldState' of https://github.com/personalrobotics/aikido…

5a144ef

… into WorldState

Address Mike and Brian's comments

db874ae

Modify format, docstring

2471fe5

brianhou suggested changes Dec 6, 2017

View reviewed changes

Address Brian's comments

0a68474

brianhou approved these changes Dec 7, 2017

View reviewed changes

brianhou merged commit 257ccdb into master Dec 7, 2017

brianhou deleted the WorldState branch December 7, 2017 06:26

yskim041 added this to the Aikido 0.2.0 milestone Dec 7, 2017

brianhou mentioned this pull request Dec 8, 2017

Need to be able to compare two Worlds' states. #261

Closed

jslee02 mentioned this pull request Jan 8, 2018

Packaging AIKIDO 0.2.0 #294

Merged

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add World::State #265

Add World::State #265

gilwoolee commented Nov 23, 2017 •

edited by jslee02

Loading

codecov bot commented Nov 23, 2017 •

edited

Loading

mxgrey commented Nov 23, 2017

gilwoolee commented Nov 23, 2017

mxgrey commented Nov 26, 2017

gilwoolee commented Dec 1, 2017

brianhou left a comment

brianhou Dec 3, 2017

mkoval Dec 4, 2017

brianhou Dec 3, 2017

mkoval Dec 4, 2017

brianhou Dec 3, 2017

brianhou Dec 3, 2017

gilwoolee Dec 5, 2017

brianhou Dec 3, 2017

brianhou Dec 3, 2017

mkoval left a comment

mkoval Dec 4, 2017

mkoval Dec 4, 2017

mkoval Dec 4, 2017

mkoval Dec 4, 2017

mkoval Dec 4, 2017

gilwoolee Dec 6, 2017 •

edited

Loading

mkoval Dec 4, 2017

gilwoolee commented Dec 6, 2017

gilwoolee commented Dec 6, 2017

gilwoolee commented Dec 6, 2017

mkoval commented Dec 6, 2017

brianhou Dec 6, 2017

brianhou Dec 6, 2017

gilwoolee Dec 6, 2017

brianhou Dec 6, 2017

brianhou Dec 6, 2017

gilwoolee Dec 7, 2017


		virtual ~WorldStateSaver();

		// WorldStateSaver is uncopyable, must use std::move

Add World::State #265

Add World::State #265

Conversation

gilwoolee commented Nov 23, 2017 • edited by jslee02 Loading

codecov bot commented Nov 23, 2017 • edited Loading

Codecov Report

mxgrey commented Nov 23, 2017

gilwoolee commented Nov 23, 2017

mxgrey commented Nov 26, 2017

gilwoolee commented Dec 1, 2017

brianhou left a 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

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

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

gilwoolee Dec 6, 2017 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

gilwoolee commented Dec 6, 2017

gilwoolee commented Dec 6, 2017

gilwoolee commented Dec 6, 2017

mkoval commented Dec 6, 2017

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

gilwoolee commented Nov 23, 2017 •

edited by jslee02

Loading

codecov bot commented Nov 23, 2017 •

edited

Loading

gilwoolee Dec 6, 2017 •

edited

Loading