Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Added support for loading the same SDF file multiple times. #2426

Merged
merged 39 commits into from
Jun 17, 2016

Conversation

liangfok
Copy link
Contributor

@liangfok liangfok commented May 23, 2016

Closes #2546.

This PR contains changes necessary to load the same SDF file multiple times.
You can run the demo by executing the following:

$ cd [drake distro]/drake/examples/Cars
$ ../../../build/bin/drake-visualizer &
$ ../../pod-build/bin/multi_car_sim_lcm

Here's what you should see in Drake Visualizer:

screen shot 2016-05-23 at 6 28 52 am

The example source code is in [drake distro]/drake/examples/cars/multi_car_sim_lcm.cc.

You can also run the following unit test:

$ cd /drake/drake/pod-build/
$ ctest -VV -R multi_car_sim_lcm

This change is Reviewable

@RussTedrake
Copy link
Contributor

Reviewed 9 of 9 files at r1.
Review status: all files reviewed at latest revision, 5 unresolved discussions.


drake/examples/Cars/CMakeLists.txt, line 38 [r1] (raw file):

  add_dependencies(multi_car_sim drake_lcmtypes lcmtype_agg_hpp)
  target_link_libraries(multi_car_sim drakeCars drakeRBSystem drakeLCMSystem)

add as a test, too?


drake/examples/Cars/multi_car_sim.cpp, line 118 [r1] (raw file):

  rigid_body_sys->penetration_stiffness = 5000.0;
  rigid_body_sys->penetration_damping = rigid_body_sys->penetration_stiffness / 10.0;
  rigid_body_sys->friction_coefficient = 10.0;  // essentially infinite friction

looks like it's already been moved there. obviously zap all of the above before your actual merge.


drake/systems/plants/RigidBodySystem.h, line 181 [r1] (raw file):

  void addRobotFromSDF(const std::string& sdf_filename,
                       const std::string* model_name_postfix,

inserting this in the middle of the arguments will unnecessarily break the api. recommend putting it at the end?


drake/systems/plants/RigidBodyTree.cpp, line 107 [r1] (raw file):

  SortTree();

  std::map<std::string, int> model_name_to_robotnum;

i think that the longer-term fix is probably to purge all occurrences of robotnum which is essentially a UID, in favor of some data interface using pointers (e.g. the RigidBody has a link to the specific model_name string (or object), which no longer has to be unique). We should figure out the best design for the c++ and then I'm happy to help connect it back through to matlab.

this is a good fix for the short term.


drake/systems/plants/RigidBodyTree.h, line 118 [r1] (raw file):

                           DrakeJoint::QUATERNION,
                       std::shared_ptr<RigidBodyFrame> weld_to_frame = nullptr);

again, recommend it as the last argument.


Comments from Reviewable

Chien-Liang Fok added 8 commits May 31, 2016 17:31
…e/multi_prius_demo

# Conflicts:
#	drake/examples/Cars/car_simulation.cc
#	drake/systems/plants/RigidBodySystem.cpp
#	drake/systems/plants/RigidBodyTreeSDF.cpp
…e/multi_prius_demo

# Conflicts:
#	drake/examples/Cars/car_simulation.cc
… to co-exist in the rigid body tree. They are differentiated by the model ID.
@liangfok
Copy link
Contributor Author

liangfok commented Jun 4, 2016

I addressed all reviewer comments and also updated this PR based on recent discussions about keeping the model name intact without adding a postfix. A model ID is still being used to differentiate between models within a rigid body tree. A future PR will be issued that removes this ID and switch to using pointers to differentiate between different models.

+@RussTedrake for feature review and +@jwnimmer-tri for platform review

Previously, liangfok (Chien-Liang Fok) wrote…

Added support for loading the same SDF file multiple times.

This is not yet ready to be merged (I quickly hacked in the new feature), but it contains changes necessary to load the same SDF file multiple times.

You can run the demo by executing the following:


$ cd [drake distro]/drake/examples/Cars

$ ../../../build/bin/drake-visualizer &

$ ../../pod-build/bin/multi_car_sim

Here's what you should see in Drake Visualizer:

screen shot 2016-05-23 at 6 28 52 am

The example source code is in [drake distro]/drake/examples/cars/multi_car_sim.cpp.


Review status: 0 of 12 files reviewed at latest revision, 5 unresolved discussions.


drake/examples/Cars/CMakeLists.txt, line 38 [r1] (raw file):

Previously, RussTedrake (Russ Tedrake) wrote…

add as a test, too?

Done. The new unit test is called "multi_car_sim_lcm". The command for running it is:
$ ctest -VV -R multi_car_sim_lcm

drake/systems/plants/RigidBodySystem.h, line 181 [r1] (raw file):

Previously, RussTedrake (Russ Tedrake) wrote…

inserting this in the middle of the arguments will unnecessarily break the api. recommend putting it at the end?

Done. I removed the `model_name_postfix` in r3. Just the model ID is used to differentiate between otherwise identically-named models.

drake/systems/plants/RigidBodyTree.h, line 118 [r1] (raw file):

Previously, RussTedrake (Russ Tedrake) wrote…

again, recommend it as the last argument.

Done. I actually removed the `model_name_postfix` in r3. Just the model ID is used to differentiate between otherwise identically-named models.

drake/examples/Cars/multi_car_sim.cpp, line 118 [r1] (raw file):

Previously, RussTedrake (Russ Tedrake) wrote…

looks like it's already been moved there. obviously zap all of the above before your actual merge.

Done. I moved it to a new method called `SetRigidBodySystemParameters()`

Comments from Reviewable

@liangfok
Copy link
Contributor Author

liangfok commented Jun 4, 2016

Two unit tests are failing: rigid_body_tree_test and load_model_test. I am able to replicate these failures locally and will debug them soon.

Previously, liangfok (Chien-Liang Fok) wrote…

I addressed all reviewer comments and also updated this PR based on recent discussions about keeping the model name intact without adding a postfix. A model ID is still being used to differentiate between models within a rigid body tree. A future PR will be issued that removes this ID and switch to using pointers to differentiate between different models.

+@RussTedrake for feature review and +@jwnimmer-tri for platform review


Review status: 0 of 12 files reviewed at latest revision, 5 unresolved discussions, some commit checks failed.


Comments from Reviewable

…tion instead of returning a nullptr when finding a non-existent link in a rigid body tree.
@liangfok
Copy link
Contributor Author

liangfok commented Jun 4, 2016

This PR passed https://drake-jenkins.csail.mit.edu/job/experimental/1946/ and is thus ready for review.

Previously, liangfok (Chien-Liang Fok) wrote…

Two unit tests are failing: rigid_body_tree_test and load_model_test. I am able to replicate these failures locally and will debug them soon.


Review status: 0 of 14 files reviewed at latest revision, 5 unresolved discussions.


Comments from Reviewable

@jwnimmer-tri
Copy link
Collaborator

I started reading this, but TBH I'm not sure this is a direction I want to support. Robot instancing would be trivial with a different RBT design; bolting it onto this design is just more difficult-to-get-right complexity. If there is no immediate need (in other words, if this PR was only for my benefit), I'd rather stack this PR after a general cleanup / redesign of the RB/RBT interfaces. Adding one more hand-crafted underdocumented public integer into the mix doesn't seem worth it, if nobody is chomping at the bit for the feature. If there is a quick need for this, let me know and I can try to find a good compromise.

Previously, liangfok (Chien-Liang Fok) wrote…

This PR passed https://drake-jenkins.csail.mit.edu/job/experimental/1946/ and is thus ready for review.


Reviewed 1 of 13 files at r3.
Review status: 1 of 14 files reviewed at latest revision, 5 unresolved discussions.


Comments from Reviewable

@sherm1
Copy link
Member

sherm1 commented Jun 6, 2016

@liangfok this still has status "do not review" -- should that be removed?

@liangfok
Copy link
Contributor Author

liangfok commented Jun 6, 2016

@jwnimmer-tri: +1 for overhauling the rigid body tree design. I'm thinking there should be a "robot" abstraction that sits between the rigid body tree and the rigid body abstractions. However, I'm not sure this is enough since how would one identify a particular robot unless there were some unique ID included with each "robot" abstraction?

@sherm1: Oops. I will remove the the "do not merge" label.

@amcastro-tri
Copy link
Contributor

:lgtm:

Previously, liangfok (Chien-Liang Fok) wrote…

This PR passed https://drake-jenkins.csail.mit.edu/job/experimental/2041/ and is thus ready for review.


Review status: all files reviewed at latest revision, 33 unresolved discussions.


drake/examples/Cars/car_simulation.h, line 61 [r9] (raw file):

Previously, liangfok (Chien-Liang Fok) wrote…

What is the intention of the DRAKECARS_EXPORT?

For Windows compatibility. This file is part of a shared library.

Also, what do you think of the design I use in PR #2542 where a more specific system (Atlas) inherits from RigidBodySystem. A similar pattern is followed in the SpringMassSystem. Maybe worth updating the cars example to follow a similar pattern as we migrate towards a System 2.0 design?

I like it but would like to have a more thorough discussion: #2550

I was not proposing to make such a change in this PR though. Just suggesting the idea for when System 2.0 arrives. Thanks for creating #2550!

drake/examples/Cars/car_simulation.h, line 76 [r10] (raw file):

 *
 * The length and width of the terrain lies in the X-Y plane of the world
 * coordinate frame abd has lengths of \p box_width. The depth of the terrain is

abd->and


Comments from Reviewable

@liangfok
Copy link
Contributor Author

I addressed all reviewer comments. PTAL.

Previously, amcastro-tri (Alejandro Castro) wrote…

:lgtm:


Review status: all files reviewed at latest revision, 33 unresolved discussions.


drake/examples/Cars/car_simulation.h, line 61 [r9] (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

I was not proposing to make such a change in this PR though. Just suggesting the idea for when System 2.0 arrives. Thanks for creating #2550!

No problem!

drake/examples/Cars/car_simulation.h, line 76 [r10] (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

abd->and

Fixed. Thanks. I also updated the text for clarity.

Comments from Reviewable

@liangfok
Copy link
Contributor Author

This PR passed https://drake-jenkins.csail.mit.edu/job/experimental/2047/ and is thus ready for review.

@sherm1
Copy link
Member

sherm1 commented Jun 14, 2016

Platform review pass done.

Previously, liangfok (Chien-Liang Fok) wrote…

This PR passed https://drake-jenkins.csail.mit.edu/job/experimental/2047/ and is thus ready for review.


Reviewed 1 of 14 files at r3, 2 of 7 files at r8, 5 of 6 files at r9, 1 of 2 files at r10, 1 of 1 files at r11.
Review status: all files reviewed at latest revision, 30 unresolved discussions.


drake/examples/Cars/car_simulation.h, line 76 [r11] (raw file):

 *
 * The Z-axis of the box matches the Z-axis of the world, i.e., positive Z
 * points up. The length and width of the terrain is \p box_size. The depth of

This still isn't completely clear. You talked about Z but were silent about X and Y. Should be explicit: length and width are aligned with X and Y and have the same dimension box_size; depth is aligned with Z.


drake/examples/Cars/car_simulation.h, line 80 [r11] (raw file):

 *
 * @param[in] rigid_body_tree The rigid body tree to which to add the terrain.
 * @param[in] box_size The length and width of the terrain.

aligned with world's x and y


drake/examples/Cars/car_simulation.h, line 81 [r11] (raw file):

 * @param[in] rigid_body_tree The rigid body tree to which to add the terrain.
 * @param[in] box_size The length and width of the terrain.
 * @param[in] box_depth The depth of the terrain. Note that regardless of how

aligned with world's z


drake/systems/plants/RigidBodySystem.cpp, line 819 [r5] (raw file):

Previously, liangfok (Chien-Liang Fok) wrote…

Perhaps one source of confusion is the current PR enables the same SDF file to be loaded multiple times. Thus, final_model_id is not equal to the number of models in the SDF file since other SDF files may have been loaded previously.

Please explain that in the code.

drake/systems/plants/RigidBodyTree.h, line 729 [r8] (raw file):

Previously, liangfok (Chien-Liang Fok) wrote…

(1) If that comment is correct why isn't this method called findBodyIndex()?

Historical reasons; this was the name of the method prior to this PR. Should I rename it to be FindBodyIndex() in this PR?

(2) Why would anyone think a body index is the ID of a model?

The only member variable within a RigidBody that hints at an "ID" is model_id. Thus, I wanted to ensure no one thought this method returned the model_id. Also, see your comment below about how there are two IDs that need to be disambiguated. I suppose changing the method name will clear up all confusion.

Yes, I think you should rename it to FindBodyIndex() in this PR since you are introducing a real "id" in this PR.

Comments from Reviewable

@liangfok
Copy link
Contributor Author

I addressed all reviewer comments. PTAL.
I will add an entry to the CHANGELOG once this PR is ready for merge into master.

Previously, sherm1 (Michael Sherman) wrote…

Platform review pass done.


Review status: 10 of 21 files reviewed at latest revision, 30 unresolved discussions.


drake/examples/Cars/car_simulation.h, line 76 [r11] (raw file):

Previously, sherm1 (Michael Sherman) wrote…

This still isn't completely clear. You talked about Z but were silent about X and Y. Should be explicit: length and width are aligned with X and Y and have the same dimension box_size; depth is aligned with Z.

Done.

drake/examples/Cars/car_simulation.h, line 80 [r11] (raw file):

Previously, sherm1 (Michael Sherman) wrote…

aligned with world's x and y

Done.

drake/examples/Cars/car_simulation.h, line 81 [r11] (raw file):

Previously, sherm1 (Michael Sherman) wrote…

aligned with world's z

Done.

drake/systems/plants/RigidBodySystem.cpp, line 819 [r5] (raw file):

Previously, sherm1 (Michael Sherman) wrote…

Please explain that in the code.

Done. Please see the new Doxygen comments for `RigidBodySystem::addRobotFromSDF()`.

drake/systems/plants/RigidBodyTree.h, line 729 [r8] (raw file):

Previously, sherm1 (Michael Sherman) wrote…

Yes, I think you should rename it to FindBodyIndex() in this PR since you are introducing a real "id" in this PR.

Done.

Comments from Reviewable

@sherm1
Copy link
Member

sherm1 commented Jun 14, 2016

Review pass done -- almost there!


Reviewed 11 of 11 files at r12.
Review status: all files reviewed at latest revision, 28 unresolved discussions.


drake/examples/Cars/car_simulation.h, line 76 [r11] (raw file):

Previously, liangfok (Chien-Liang Fok) wrote…

Done.

Looks great now, Liang -- thanks!

drake/systems/plants/RigidBodySystem.cpp, line 819 [r5] (raw file):

Previously, liangfok (Chien-Liang Fok) wrote…

Done. Please see the new Doxygen comments for RigidBodySystem::addRobotFromSDF().

That's a nice addition to the API description! But I was referring to the mysterious implementation code, not the external behavior. Please add some comments explaining what's going on with the various chasing of model lists, counting of models, and arithmetic used finally to assign model ids to models. Maybe this seems obvious to you but your code is depending on a lot of knowledge that resides only in your head at the moment.

drake/systems/plants/RigidBodySystem.h, line 190 [r12] (raw file):

   * @param[in] floating_base_type The type of floating base to use to connect
   * the models within the SDF file to the world.
   * @param[in] welt_to_frame The frame used for connecting the models in the

typo


Comments from Reviewable

@liangfok
Copy link
Contributor Author

I've addressed all reviewer comments. PTAL. There are still at least two things I need to do:

  1. Update the CHANGELOG since this PR changes some APIs.
  2. Add a unit test for RigidBodySystem that verifies the correct implementation of model_id when the same SDF is loaded multiple times and the SDF contains multiple models with at least one joint and one sensor each.

Review status: 19 of 21 files reviewed at latest revision, 27 unresolved discussions.


drake/systems/plants/RigidBodySystem.cpp, line 819 [r5] (raw file):

Previously, sherm1 (Michael Sherman) wrote…

That's a nice addition to the API description! But I was referring to the mysterious implementation code, not the external behavior. Please add some comments explaining what's going on with the various chasing of model lists, counting of models, and arithmetic used finally to assign model ids to models. Maybe this seems obvious to you but your code is depending on a lot of knowledge that resides only in your head at the moment.

Done. I added additional documentation comments to this method.

drake/systems/plants/RigidBodySystem.h, line 190 [r12] (raw file):

Previously, sherm1 (Michael Sherman) wrote…

typo

Fixed.

Comments from Reviewable

@liangfok
Copy link
Contributor Author

This PR passed https://drake-jenkins.csail.mit.edu/job/experimental/2068/ and is thus ready for review.


Review status: 19 of 21 files reviewed at latest revision, 27 unresolved discussions.


Comments from Reviewable

@liangfok
Copy link
Contributor Author

I added the unit test for RigidBodySystem and added a CHANGELOG. Thus this PR is ready for another round of review.


Review status: 14 of 26 files reviewed at latest revision, 26 unresolved discussions.


Comments from Reviewable

@liangfok
Copy link
Contributor Author

This PR passed https://drake-jenkins.csail.mit.edu/job/experimental/2094/ and is thus ready for review.


Review status: 13 of 26 files reviewed at latest revision, 26 unresolved discussions.


Comments from Reviewable

@sherm1
Copy link
Member

sherm1 commented Jun 16, 2016

Liang, did this PR pick up some unrelated commits?


Reviewed 1 of 2 files at r13, 7 of 11 files at r15.
Review status: 21 of 26 files reviewed at latest revision, 27 unresolved discussions.


drake/systems/plants/RigidBodySystem.cpp, line 819 [r5] (raw file):

Previously, liangfok (Chien-Liang Fok) wrote…

Done. I added additional documentation comments to this method.

Great explanation -- thanks!

drake/systems/plants/RigidBodySystem.cpp, line 878 [r16] (raw file):

        "ID (" +
        std::to_string(final_model_id) + ")");
  }

@liangfok, it's great that you documented this now. As often happens when one tries to explain code to someone else, I see you found several bugs in this code! I think that makes it clear that this needs some unit tests in place to make sure the model ids are being generated correctly before this PR can go in.


Comments from Reviewable

@liangfok
Copy link
Contributor Author

I just scanned through the commits associated with this PR and don't see anything out of the ordinary.


Review status: 21 of 26 files reviewed at latest revision, 27 unresolved discussions.


drake/systems/plants/RigidBodySystem.cpp, line 819 [r5] (raw file):

Previously, sherm1 (Michael Sherman) wrote…

Great explanation -- thanks!

No problem! Thank you for the reviews.

drake/systems/plants/RigidBodySystem.cpp, line 878 [r16] (raw file):

Previously, sherm1 (Michael Sherman) wrote…

@liangfok, it's great that you documented this now. As often happens when one tries to explain code to someone else, I see you found several bugs in this code! I think that makes it clear that this needs some unit tests in place to make sure the model ids are being generated correctly before this PR can go in.

Yes, in fact, I discovered the bugs after I added a unit test called `rigid_body_system_test` (see `drake/systems/plants/test/rigid_body_system/rigid_body_system_test.cc` in this PR). I believe this unit test is sufficient to verify that the model IDs are being correctly set.

Comments from Reviewable

@liangfok
Copy link
Contributor Author

This PR passed https://drake-jenkins.csail.mit.edu/job/experimental/2096/ and is thus ready for review.


Review status: 21 of 26 files reviewed at latest revision, 27 unresolved discussions.


Comments from Reviewable

@sherm1
Copy link
Member

sherm1 commented Jun 17, 2016

:lgtm: ready to merge!


Reviewed 3 of 11 files at r15, 2 of 2 files at r16.
Review status: all files reviewed at latest revision, 28 unresolved discussions.


drake/systems/plants/test/rigid_body_system/rigid_body_system_test.cc, line 120 [r16] (raw file):

  EXPECT_NE(tree->findJoint("joint_2", 2), nullptr);
  EXPECT_NE(tree->findJoint("joint_2", 3), nullptr);
}

BTW, test looks good!


Comments from Reviewable

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Projects
None yet
Development

Successfully merging this pull request may close these issues.

5 participants