Growing IRIS-ZO Regions Along a Parametrized Subspace #22558
Conversation
…ically, the C-IRIS algebraic parametrization.)
There was a problem hiding this comment.
Reviewable status: 1 unresolved discussion, needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)
planning/iris/iris_zo.cc line 245 at r1 (raw file):
std::vector<Eigen::VectorXd> ambient_particles(N_k); std::transform(particles.begin(), particles.begin() + N_k, ambient_particles.begin(), options.parametrization);
FWIW I think the way to fix this would be:
- Changes the signature
bool CheckConfigCollisionFree( const Eigen::VectorXd& q, std::optional<int> context_number = std::nullopt) const
to
bool CheckConfigCollisionFree( const Eigen::Ref<const Eigen::VectorXd>& q, std::optional<int> context_number = std::nullopt) const
and similarly forCheckConfigsCollisionFree - Change
std::vector<Eigen::VectorXd> ambient_particles(N_k)tostd::vector<Eigen::Map<Eigen::VectorXd>> ambient_particles(N_k)and then you emplace backparticles.col(i).data()
Code quote:
// TODO(wernerpe, cohnt): Remove this copy operation.
std::vector<Eigen::VectorXd> ambient_particles(N_k);
std::transform(particles.begin(), particles.begin() + N_k,
ambient_particles.begin(), options.parametrization);
There was a problem hiding this comment.
Reviewable status: 1 unresolved discussion, needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)
planning/iris/iris_zo.cc line 245 at r1 (raw file):
Previously, AlexandreAmice (Alexandre Amice) wrote…
FWIW I think the way to fix this would be:
- Changes the signature
bool CheckConfigCollisionFree( const Eigen::VectorXd& q, std::optional<int> context_number = std::nullopt) const
to
bool CheckConfigCollisionFree( const Eigen::Ref<const Eigen::VectorXd>& q, std::optional<int> context_number = std::nullopt) const
and similarly forCheckConfigsCollisionFree- Change
std::vector<Eigen::VectorXd> ambient_particles(N_k)tostd::vector<Eigen::Map<Eigen::VectorXd>> ambient_particles(N_k)and then you emplace backparticles.col(i).data()
Just to clarify, we're still applying options.parametrization to each particle before dropping it into ambient_particles, so I'm not sure that using Eigen::Map will save us from that. But I definitely like the idea of changing CollisionChecker (or adding a method) to operate on Eigen::Ref and Eigen::Map.
|
+@sadraddini assigning myself for feature review |
There was a problem hiding this comment.
Thanks for doing this! Left some comments. Can't wait to use it!
Reviewable status: 9 unresolved discussions, LGTM missing from assignee sadraddini, needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)
planning/iris/iris_zo.h line 128 at r1 (raw file):
int mixing_steps{50}; /** Whether or not the parametrization function is thread-safe. */
nit Is this something the user can control?! Maybe needs more explanation.
planning/iris/iris_zo.h line 136 at r1 (raw file):
std::optional<int> parametrization_dimension{std::nullopt}; /** A function describing a parametrized subspace of the full configuration
nit Need some more explanation that this function should be a map from R^m -> R^n, where n is the dimension of the plant.
planning/iris/test/iris_zo_test.cc line 241 at r1 (raw file):
int next_point_index = 0; // Order vertices in counterclockwise order. Thanks claude.ai for the help.
btw you may not want the second sentence to stay :D
planning/iris/test/iris_zo_test.cc line 244 at r1 (raw file):
Eigen::Vector2d centroid = vregion.vertices().rowwise().mean(); Eigen::Matrix2Xd centered = vregion.vertices().colwise() - centroid; Eigen::VectorXd angles = centered.row(1).array().binaryExpr(
nit Eigen::VectorXd --> VectorXd as using Eigen::VectorXd is already declared. Ditto for Vector2d.
planning/iris/test/iris_zo_test.cc line 283 at r1 (raw file):
"Test point", math::RigidTransform(Eigen::Vector3d( ambient_query_point[0], ambient_query_point[1], 0)));
btw I suggest you post screenshots in the PR description.
planning/iris/iris_zo.cc line 104 at r1 (raw file):
double previous_volume = 0; const int ambient_dimension = checker.GetZeroConfiguration().size();
btw can be replaced by checker.plant().num_positions() - don't need to read the zero vector.
planning/iris/iris_zo.cc line 106 at r1 (raw file):
const int ambient_dimension = checker.GetZeroConfiguration().size(); const int parametrized_dimension = options.parametrization_dimension.value_or(ambient_dimension);
Ideally parametrization_dimension should be deduced from the parametrization function. I worry that a user can use a parametrization from R^m to R^n, and declare parametrization_dimension other than m. Need a unit test on what happens in this case. I guess there is no easy-to-follow error message.
Suggestion: Make parametrization function a class that takes in input and output dimensions in its ctor, and does the appropriate checks inside its transformation function.
Right now there is nothing stopping the user from declaring a function that produces inconsistent dimensions.
planning/iris/iris_zo.cc line 244 at r1 (raw file):
// TODO(wernerpe, cohnt): Remove this copy operation. std::vector<Eigen::VectorXd> ambient_particles(N_k); std::transform(particles.begin(), particles.begin() + N_k,
btw This method can be parallelized if parametrization is an expensive function.
There was a problem hiding this comment.
For reference, here are the images of the two new test cases.
Tangent parametrization for the double pendulum:
Linear subspace for the convex configuration space:
Reviewable status: 6 unresolved discussions, LGTM missing from assignee sadraddini, needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)
planning/iris/iris_zo.h line 128 at r1 (raw file):
Previously, sadraddini (Sadra Sadraddini) wrote…
nit Is this something the user can control?! Maybe needs more explanation.
Because the user can provide pretty much any callable object, it's possible they could pass in a function that's not threadsafe. By default, a lot of the code in IRIS-ZO is parallel, so if the user is going to give something not threadsafe, we need to know.
I've updated the documentation a bit more to explain -- does this make it clearer?
planning/iris/iris_zo.h line 136 at r1 (raw file):
Previously, sadraddini (Sadra Sadraddini) wrote…
nit Need some more explanation that this function should be a map from
R^m -> R^n, wherenis the dimension of the plant.
Done.
planning/iris/iris_zo.cc line 104 at r1 (raw file):
Previously, sadraddini (Sadra Sadraddini) wrote…
btw can be replaced by
checker.plant().num_positions()- don't need to read the zero vector.
Done.
planning/iris/iris_zo.cc line 106 at r1 (raw file):
Previously, sadraddini (Sadra Sadraddini) wrote…
Ideally
parametrization_dimensionshould be deduced from theparametrizationfunction. I worry that a user can use a parametrization from R^m to R^n, and declareparametrization_dimensionother thanm. Need a unit test on what happens in this case. I guess there is no easy-to-follow error message.Suggestion: Make parametrization function a class that takes in input and output dimensions in its ctor, and does the appropriate checks inside its transformation function.
Right now there is nothing stopping the user from declaring a function that produces inconsistent dimensions.
In general, it's really hard to prevent a determined user from shooting themselves in the foot when they're providing a std::function. I'm trying to imitate the approach done with FunctionHandleTrajectory, where we do some basic testing, but ultimately put the responsibility on the user to make sure the function actually does what it they think it does.
With FunctionHandleTrajectory, we at least knew a given point we could query to get the dimension (since it has to be defined for all time). I've done the same for the center of the user-provided ellipsoid, and added a test case for this.
I've also gone through and added DRAKE_ASSERT statements anytime the parametrization is called, to make sure the output is the correct shape. That way, at least in debug mode, we catch the errors. But I don't think it will be performant to check every time (hence why I'm only activating the check in debug mode).
planning/iris/iris_zo.cc line 244 at r1 (raw file):
Previously, sadraddini (Sadra Sadraddini) wrote…
btw This method can be parallelized if
parametrizationis an expensive function.
Dropped in a TODO if this becomes needed in the future. But for now, most of the functions I'm considering are pretty fast.
planning/iris/test/iris_zo_test.cc line 241 at r1 (raw file):
Previously, sadraddini (Sadra Sadraddini) wrote…
btw you may not want the second sentence to stay :D
I don't mind giving credit to the AI that helped me -- I definitely don't know how to vectorize things nicely in Eigen 😄
But I'll remove it.
planning/iris/test/iris_zo_test.cc line 244 at r1 (raw file):
Previously, sadraddini (Sadra Sadraddini) wrote…
nit
Eigen::VectorXd --> VectorXdasusing Eigen::VectorXdis already declared. Ditto forVector2d.
Done.
planning/iris/test/iris_zo_test.cc line 283 at r1 (raw file):
Previously, sadraddini (Sadra Sadraddini) wrote…
btw I suggest you post screenshots in the PR description.
Sure thing.
cohnt
force-pushed
the
iris-zo-subspace
branch
from
f67bbc6 to
cb8e78d
Compare
There was a problem hiding this comment.
Needs a platform reviewer.
Reviewable status: 2 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)
planning/iris/iris_zo.h line 128 at r1 (raw file):
Previously, cohnt (Thomas Cohn) wrote…
Because the user can provide pretty much any callable object, it's possible they could pass in a function that's not threadsafe. By default, a lot of the code in IRIS-ZO is parallel, so if the user is going to give something not threadsafe, we need to know.
I've updated the documentation a bit more to explain -- does this make it clearer?
Maybe call this bool parallelism {true} then? Thread-safety is not a thing many users know about, but parallelism is more tangible. Of course it would be their responsibility to ensure their transformation function is thread safe if they want parallelism.
There was a problem hiding this comment.
Left a small comment on changing the name of parallelism flag.
Reviewable status: 2 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)
There was a problem hiding this comment.
+@RussTedrake for platform review, if you think you'll have the time this weekend? Otherwise, I'll bump it to the on-call platform reviewer on Monday.
Reviewable status: 2 unresolved discussions, LGTM missing from assignee RussTedrake(platform), commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)
planning/iris/iris_zo.h line 128 at r1 (raw file):
Previously, sadraddini (Sadra Sadraddini) wrote…
Maybe call this
bool parallelism {true}then? Thread-safety is not a thing many users know about, but parallelism is more tangible. Of course it would be their responsibility to ensure their transformation function is thread safe if they want parallelism.
So the user already has to specify a Parallelism object, but my concern was that a user might set the parametrization without thinking about the parallel aspects. So I added a second option, that, while technically redundant, served as a warning for the user to watch out.
But we could also just include in the documentation for the parametrization option that the user should set parallelism to none. Or we could make it a requirement that the user-provided parametrization be threadsafe. (IRIS-NP2 won't have that requirement.)
I'm fine with any of these three options. We can also hold off to see what the platform reviewer thinks.
|
+@ggould-tri for platform review per schedule, please. |
There was a problem hiding this comment.
Reviewed 3 of 3 files at r2, all commit messages.
Reviewable status: 7 unresolved discussions, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)
planning/iris/iris_zo.h line 128 at r1 (raw file):
Previously, cohnt (Thomas Cohn) wrote…
So the user already has to specify a
Parallelismobject, but my concern was that a user might set the parametrization without thinking about the parallel aspects. So I added a second option, that, while technically redundant, served as a warning for the user to watch out.But we could also just include in the documentation for the parametrization option that the user should set parallelism to none. Or we could make it a requirement that the user-provided parametrization be threadsafe. (IRIS-NP2 won't have that requirement.)
I'm fine with any of these three options. We can also hold off to see what the platform reviewer thinks.
Pros:
I like demonstrative and explicit names; this will read especially well in any future python bindings. While it is impossible to force a user to think, this provides strong encouragement in the right direction.
Cons:
Because this parameter has a default, in fact the user never needs to think; they are defaulted into the unsafe case.
To my thinking: If we can put this in with default false (a breaking change to this interface), this change is extremely desirable.
However if the reason for the default true is compelling, this does not add any value over the Parallelism object and we should instead document around this issue with a doxygen admonition like @warning.
a discussion (no related file):
A note on spelling conventions:
Although Wikipedia accepts many spellings,
drake appears to strongly prefer parameterization.
ggould@b-12:~/git/drake (master)$ git grep -i parametriz | wc -l
56
ggould@b-12:~/git/drake (master)$ git grep -i parameteriz | wc -l
237
This appears to be the first arrival of the this term in the planning directory; this is a consistency regression.
planning/iris/iris_zo.h line 96 at r2 (raw file):
/** Number of threads to use when updating the particles. If the user requests * more threads than the CollisionChecker supports, that number of threads * will be used instead. */
minor: This also should link to the new semantics.
Suggestion:
* will be used instead. However, see also `parametrization_is_threadsafe`. */planning/iris/iris_zo.h line 128 at r2 (raw file):
int mixing_steps{50}; /** Whether or not the parametrization function is thread-safe. If the user
minor: IMO we should be a little bit more specific and formal about the scope here.
Suggestion:
`parametrization()`planning/iris/iris_zo.h line 141 at r2 (raw file):
* space, along which to grow the region. The function should be a map R^m to * R^n, where n is the dimension of the plant configuration space, determined * via `checker.plant().num_positions()`. The default value is just the
Suggestion:
* via `checker.plant().num_positions()` and m is `parameterization_dimension` if specified. The default value is just theplanning/iris/iris_zo.cc line 122 at r2 (raw file):
"returned a point with the wrong dimension (its size was " "{}) when called on the center of the starting ellipsoid.", ambient_dimension, computed_ambient_dimension));
nit: the center of the starting ellipsoid is not especially useful debugging information; consider:
Suggestion:
"{}) when called on {}.",
ambient_dimension, computed_ambient_dimension, starting_ellipsoid_center));
There was a problem hiding this comment.
Reviewable status: 3 unresolved discussions, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)
a discussion (no related file):
Previously, ggould-tri wrote…
A note on spelling conventions:
Although Wikipedia accepts many spellings,
drake appears to strongly prefer
parameterization.ggould@b-12:~/git/drake (master)$ git grep -i parametriz | wc -l 56 ggould@b-12:~/git/drake (master)$ git grep -i parameteriz | wc -l 237This appears to be the first arrival of the this term in the
planningdirectory; this is a consistency regression.
Updated to stick with drake's conventions (even if I disagree 😉)
planning/iris/iris_zo.h line 128 at r1 (raw file):
Previously, ggould-tri wrote…
Pros:
I like demonstrative and explicit names; this will read especially well in any future python bindings. While it is impossible to force a user to think, this provides strong encouragement in the right direction.
Cons:
Because this parameter has a default, in fact the user never needs to think; they are defaulted into the unsafe case.To my thinking: If we can put this in with default
false(a breaking change to this interface), this change is extremely desirable.
However if the reason for the defaulttrueis compelling, this does not add any value over theParallelismobject and we should instead document around this issue with a doxygen admonition like@warning.
The reason to keep the default true is that this code is frequently used with no parametrization. Instead of having tons of switching logic throughout the function, it's easier to just make the default parametrization the identity function, which can definitely be parallel. (And indeed, the speed from parallelism is essential to why this algorithm is effective.) I've added a warning to both this field and the parametrization field.
I wouldn't worry too much about python bindings. Since functions made in python can never be called in parallel by C++ code (I think), we would never allow a user to define a python function and specify whether or not its threadsafe. (I'm currently thinking about having factory methods that construct a couple common parametrizations that we need -- mimic joints, the C-IRIS "tangent configuration space", maybe others.)
planning/iris/iris_zo.h line 96 at r2 (raw file):
Previously, ggould-tri wrote…
minor: This also should link to the new semantics.
Done
planning/iris/iris_zo.h line 128 at r2 (raw file):
Previously, ggould-tri wrote…
minor: IMO we should be a little bit more specific and formal about the scope here.
Done.
planning/iris/iris_zo.cc line 122 at r2 (raw file):
Previously, ggould-tri wrote…
nit:
the center of the starting ellipsoidis not especially useful debugging information; consider:
Done.
(Ended up being a bit wonky due to eigen formatting)
planning/iris/iris_zo.h line 141 at r2 (raw file):
* space, along which to grow the region. The function should be a map R^m to * R^n, where n is the dimension of the plant configuration space, determined * via `checker.plant().num_positions()`. The default value is just the
Done.
There was a problem hiding this comment.
Reviewable status: 2 unresolved discussions, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)
planning/iris/iris_zo.h line 128 at r1 (raw file):
I would prefer the following
1.) Make parametrization, parametrization_dimension, and parametrization_is_threadsafe private members.
2.) Make the parametrization setter take in std::function<Eigen::VectorXd(const Eigen::VectorXd&)> param_function, int param_dimension, bool is_thread_safe) so that the user needs to specify and think of all of these together.
Unfortunately, this is a struct so I know its I know it's against the style guide to have private members
structsshould be used for passive objects that carry data, and may have associated constants. All fields must be public. The struct must not have invariants that imply relationships between different fields, since direct user access to those fields may break those invariants. Constructors, destructors, and helper methods may be present; however, these methods must not require or enforce any invariants.
Can we change this to a class maybe?
There was a problem hiding this comment.
Reviewed 3 of 3 files at r3, all commit messages.
Reviewable status: labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)
There was a problem hiding this comment.
Reviewable status: labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)
planning/iris/iris_zo.h line 128 at r1 (raw file):
Previously, AlexandreAmice (Alexandre Amice) wrote…
I would prefer the following
1.) Makeparametrization,parametrization_dimension, andparametrization_is_threadsafeprivate members.
2.) Make the parametrization setter take instd::function<Eigen::VectorXd(const Eigen::VectorXd&)> param_function, int param_dimension, bool is_thread_safe)so that the user needs to specify and think of all of these together.Unfortunately, this is a
structso I know its I know it's against the style guide to have private members
structsshould be used for passive objects that carry data, and may have associated constants. All fields must be public. The struct must not have invariants that imply relationships between different fields, since direct user access to those fields may break those invariants. Constructors, destructors, and helper methods may be present; however, these methods must not require or enforce any invariants.Can we change this to a class maybe?
I'm guessing making it into a class requires changes elsewhere as well? We could have a helper method that simultaneously sets all three, and "strongly encourage" users to use that, rather than setting them each manually?
|
@drake-jenkins-bot linux-jammy-gcc-bazel-experimental-everything-release please. |
|
Another possibility is we could make IrisZoOptions a class, but set all the members (besides the ones we want to control) to public? I don't see any rules that breaks from a quick skim of the style guide. But I don't have a sense of how much existing code would have to change. @ggould-tri @AlexandreAmice if you think it's not worth messing with it further, then I'm okay keeping it as is. |
There was a problem hiding this comment.
Public class members are allowed in the style guide (they used to be prohibited, but that changed). So that would be fine, and would not break any rules, because struct { ... } is equivalent to class { public: ...}. I don't think any existing code would change. Be aware that if you have any private members, you need to use one of the move/copy/assign macros (probably the DEFAULT one).
I think this code is acceptable as-is, but it's worth prototyping the class change and seeing if you like the way it looks and feels in the tests.
Reviewable status: labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)
There was a problem hiding this comment.
My take on it is that there is an invariant in the members of IrisZoOptions that needs to be maintained between parameterization, parameterization_dimension, and parameterization_is_thread_safe.
If I have a plant of dimension 3, and I set the parameterization to a function that outputs a point s in dimension 4, but forget to set options.parameterization_dimension I would segfault (noted below). If I set it to dimension 2 I would get unitialized values in the matrix. BTW maybe you can forgo parameterization_dimension completely by just querying the output size of the zero's vector passed through the parameterization.
If I switch the parameterization to one that is unsafe without setting the parameterization_is_thread_safe option (which I may not have noticed needs to be set), I could get mysterious behavior from the race conditions.
Reviewable status: 1 unresolved discussion, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)
planning/iris/iris_zo.cc line 260 at r3 (raw file):
// it's an expensive operation. std::vector<Eigen::VectorXd> ambient_particles(N_k); std::transform(particles.begin(), particles.begin() + N_k,
Seg fault would happen here if options.parameterized_dimension > dimension of the output of options.parameterization
There was a problem hiding this comment.
I will plan to reimplement it as a class, where all three parameters are private members that can only be set at the same time.
Reviewable status: 1 unresolved discussion, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits)
There was a problem hiding this comment.
Alright, here's what I've got. All three members are now private. They each have getters, but they can only be set all-at-once.
Towards @AlexandreAmice's concerns, note that we can't guarantee the parameterization will be well-defined at any given point, so we can't just query to see what the dimension is. But we do have checks in place to handle dimension mismatches:
- We check that the user-supplied parameterization dimension (i.e. input to the function) matches the dimension of the input ellipsoid and domain.
- We compute the ambient dimension by applying the user-supplied parameterization to the center of the input ellipsoid, and check that it matches the dimension of the CollisionChecker's plant.
- Every call of the parameteriztaion function has a
DRAKE_ASSERTchecking output size. This obviously is an inefficient check in release builds, but at least in debug builds, we will catch any such problem.
@sadraddini, mind taking another look to make sure you're happy with this interface? @ggould-tri, how does this look to you?
Dismissed @AlexandreAmice from a discussion.
Reviewable status: labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)
There was a problem hiding this comment.
Reviewable status: 1 unresolved discussion, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)
planning/iris/iris_zo.h line 144 at r5 (raw file):
* via `checker.plant().num_positions()` and m is `parameterization_dimension` * if specified. The user must provide `parameterization`, which is the * function f, and the output of f must be `parameterization_is_threadsafe`,
This makes it sounds like f(s) == parameterization_is_threadsafe. Likely you just want to say, "parameterization_is_threadsafeshould be set to true only if it is safe to callparameterization concurrently". "parameterization_dimension should be set to the expected size of the input of f"
Code quote:
the output of f must be `parameterization_is_threadsafe`,
cohnt
force-pushed
the
iris-zo-subspace
branch
from
64d2bfa to
3b28655
Compare
There was a problem hiding this comment.
Reviewable status: 1 unresolved discussion, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)
planning/iris/iris_zo.h line 144 at r5 (raw file):
Previously, AlexandreAmice (Alexandre Amice) wrote…
This makes it sounds like
f(s) == parameterization_is_threadsafe. Likely you just want to say, "parameterization_is_threadsafeshould be set to true only if it is safe to callparameterizationconcurrently". "parameterization_dimensionshould be set to the expected size of the input off"
Done.
There was a problem hiding this comment.
Fine by me; let's get this in and see if it survives contact with user code :-)
(and apologies for the slow review; I fell into a paperwork tarpit due to expense report nonsense)
Reviewed 2 of 3 files at r4, 1 of 1 files at r6, all commit messages.
Reviewable status: labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)
There was a problem hiding this comment.
Towards @AlexandreAmice's point: I also agree that there is nothing stopping the user to declare a parameterization that can great harm. A way to prevent this could have been only using linear parameterizations, where the matrix and dimensions are known at ocnstruct. But @cohnt understandably wants this to be generic. And I guess there is no easy way in C++ to enforce a function is, for instance, analytic.
I left some comments on the new class.
Reviewable status: 5 unresolved discussions, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)
planning/iris/test/iris_zo_test.cc line 299 at r6 (raw file):
IrisZoFromUrdf(double_pendulum_urdf, starting_ellipsoid, options, &domain), ".*wrong dimension.*");
BTW I noticed there is no unit test covering the threadsafe part.
planning/iris/iris_zo.h line 155 at r6 (raw file):
} const ParameterizationFunction& get_parameterization() const {
Needs doxygen comments.
planning/iris/iris_zo.h line 159 at r6 (raw file):
} bool get_parameterization_is_threadsafe() const {
ditto for docs.
planning/iris/iris_zo.h line 163 at r6 (raw file):
} std::optional<int> get_parameterization_dimension() const {
Returning an optional is weird. Again, The dimension cab be deduced from a parameterization. For instance, starting from dim = 1, I can try and catch errors on parameterization_(Eigen::VectorXd::Ones(dim) until it does not show an error - that is the dimension. The downside is that you can still define a special parameterization_ that still throws with vectors of 1s, for instance (maybe force to use noexcept functions?)
Alternatively, I would like to change the variable name to user_defined_dimension_ or user_hinted_dimension_ to explicitly state that this dimension is hinted by the user, and change error messages to ones that reflect that it is not matching the function output. Same name pattern can be for user_hinted_is_threadsafe_.
Then this method can be get_user_defined_dimension_or_throw(). You may also need bool user_defined_dimension_exists() as an helper.
ditto for docs.
planning/iris/iris_zo.h line 170 at r6 (raw file):
bool parameterization_is_threadsafe_{true}; /* By default, this is not specified, and the full dimension of the
This private doxygen comments would not appear in docs. Don't put them here. See comment above.
…_MOVE_AND_ASSIGN.
|
@drake-jenkins-bot linux-jammy-clang-bazel-experimental-valgrind-memcheck please. |
There was a problem hiding this comment.
FYI @ggould-tri I added DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN -- you mentioned we needed to add it but I had forgotten to do so earlier.
Reviewable status: 5 unresolved discussions, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)
planning/iris/iris_zo.h line 155 at r6 (raw file):
Previously, sadraddini (Sadra Sadraddini) wrote…
Needs doxygen comments.
Done.
planning/iris/iris_zo.h line 159 at r6 (raw file):
Previously, sadraddini (Sadra Sadraddini) wrote…
ditto for docs.
Done.
planning/iris/iris_zo.h line 163 at r6 (raw file):
Previously, sadraddini (Sadra Sadraddini) wrote…
Returning an optional is weird. Again, The dimension cab be deduced from a parameterization. For instance, starting from dim = 1, I can
tryandcatcherrors onparameterization_(Eigen::VectorXd::Ones(dim)until it does not show an error - that is the dimension. The downside is that you can still define a specialparameterization_that still throws with vectors of 1s, for instance (maybe force to usenoexceptfunctions?)Alternatively, I would like to change the variable name to
user_defined_dimension_oruser_hinted_dimension_to explicitly state that this dimension is hinted by the user, and change error messages to ones that reflect that it is not matching the function output. Same name pattern can be foruser_hinted_is_threadsafe_.Then this method can be
get_user_defined_dimension_or_throw(). You may also needbool user_defined_dimension_exists()as an helper.ditto for docs.
Catching exceptions is forbidden by the style guide, and Eigen doesn't throw an error in release builds for passing in wrong-size vectors (it usually segfaults). There's really no way to infer the input dimension -- we need the user to specify it. Similarly, we can't determine whether or not a function is threadsafe just by looking at it.
As for why it's optional, we just need a default value to indicate "use the c-space dimension as the input dimension", so that a user who doesn't want to touch the parameterization machinery doesn't have to think about it. The alternative is to make all three parameters optional, and add a bunch of switching logic inside IrisZo, but I tried prototyping this solution and it ended up being much uglier than just making the identity function the default parameterization.
planning/iris/iris_zo.h line 170 at r6 (raw file):
Previously, sadraddini (Sadra Sadraddini) wrote…
This private doxygen comments would not appear in docs. Don't put them here. See comment above.
Included the information in these private comments in the public docs for the getters.
planning/iris/test/iris_zo_test.cc line 299 at r6 (raw file):
Previously, sadraddini (Sadra Sadraddini) wrote…
BTW I noticed there is no unit test covering the threadsafe part.
We can't test for thread-safety within a unit test. I added checks to see that it's actually setting the values in the class, if that's what you meant?
There was a problem hiding this comment.
Reviewable status: 4 unresolved discussions, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)
planning/iris/iris_zo.h line 159 at r6 (raw file):
Previously, cohnt (Thomas Cohn) wrote…
Done.
nit per my earlier suggestion, this should change to Returns whether or not the parameterization is declared to be threadsafe by the user . I also suggested some variable changes such that thread safe and dimension are emphasized to be only hinted by the user, and they may not be correct in practice - the responsibility of which is on the user.
planning/iris/iris_zo.h line 163 at r6 (raw file):
Previously, cohnt (Thomas Cohn) wrote…
Catching exceptions is forbidden by the style guide, and Eigen doesn't throw an error in release builds for passing in wrong-size vectors (it usually segfaults). There's really no way to infer the input dimension -- we need the user to specify it. Similarly, we can't determine whether or not a function is threadsafe just by looking at it.
As for why it's optional, we just need a default value to indicate "use the c-space dimension as the input dimension", so that a user who doesn't want to touch the parameterization machinery doesn't have to think about it. The alternative is to make all three parameters optional, and add a bunch of switching logic inside IrisZo, but I tried prototyping this solution and it ended up being much uglier than just making the identity function the default parameterization.
On using exceptions: I see and agree. It's unfortunate!
nit
Returns the input dimension for the parameterization function, or
* std::nullopt if it has not been set.
it is misleading. The function returns a std::optional object, and the user has to use value() or to get it. That's why, for simplicity, I suggested changing it to int get_user_hinted_dimension_or_throw() or something of that ilk. I personally don't like optional-returning getters! That's just me - didn't find anything about that in the style guide.
planning/iris/test/iris_zo_test.cc line 299 at r6 (raw file):
Previously, cohnt (Thomas Cohn) wrote…
We can't test for thread-safety within a unit test. I added checks to see that it's actually setting the values in the class, if that's what you meant?
Yes, just test if the value is actually set - a boring test. Can you test if you actually use parallelism?
planning/iris/iris_zo.h line 173 at r7 (raw file):
} /** Returns the input dimension for the parameterization function, or
ditto for emphasizing that this is only what the user has given.
There was a problem hiding this comment.
Good catch!
Reviewable status: 4 unresolved discussions, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)
There was a problem hiding this comment.
Reviewed all commit messages.
Reviewable status: 4 unresolved discussions, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)
planning/iris/iris_zo.h line 163 at r6 (raw file):
Previously, sadraddini (Sadra Sadraddini) wrote…
On using exceptions: I see and agree. It's unfortunate!
nit
Returns the input dimension for the parameterization function, or * std::nullopt if it has not been set.it is misleading. The function returns a std::optional object, and the user has to use
value()or to get it. That's why, for simplicity, I suggested changing it toint get_user_hinted_dimension_or_throw()or something of that ilk. I personally don't like optional-returning getters! That's just me - didn't find anything about that in the style guide.
I disagree that these names should be changed to include the word hint. If these values get set incorrectly, then the IrisZo will not work correctly. The user must supply these values.
I agree that I dislike the optional getter, but since this is a struct that is effectively just acting as **kwargs then users shouldn't really be accessing its members once set and the only code which will be accessing these values is IrisZo itself.
planning/iris/test/iris_zo_test.cc line 299 at r6 (raw file):
Previously, sadraddini (Sadra Sadraddini) wrote…
Yes, just test if the value is actually set - a boring test. Can you test if you actually use parallelism?
The threadsafe aspect is covered by the fact that IrisZoTests has tests which use multiple threads already. The only thing one might really hope to test here is if parameterization_is_threadsafe is false, then we confirm that only one thread is used.
I don't see a good way to test this short of using a non-threadsafe parameterization and hoping that one of the leak-santizers catches it but that is not very robust.
There was a problem hiding this comment.
Since the remainder of Sadra's comments have all been nits, I'm going to merge this PR once the test cases pass. (They need to run again since I did update the documentation.) Thanks all for the help!
Dismissed @sadraddini from a discussion.
Reviewable status: 2 unresolved discussions, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)
planning/iris/iris_zo.h line 159 at r6 (raw file):
Previously, sadraddini (Sadra Sadraddini) wrote…
nit per my earlier suggestion, this should change to
Returns whether or not the parameterization is declared to be threadsafe by the user. I also suggested some variable changes such thatthread safeanddimensionare emphasized to be only hinted by the user, and they may not be correct in practice - the responsibility of which is on the user.
Updated the comment, but per Alex's reasoning, I'm not going to change the variable names.
planning/iris/iris_zo.h line 163 at r6 (raw file):
Previously, AlexandreAmice (Alexandre Amice) wrote…
I disagree that these names should be changed to include the word
hint. If these values get set incorrectly, then theIrisZowill not work correctly. The user must supply these values.I agree that I dislike the optional getter, but since this is a struct that is effectively just acting as
**kwargsthen users shouldn't really be accessing its members once set and the only code which will be accessing these values isIrisZoitself.
Agree with Alex.
planning/iris/iris_zo.h line 173 at r7 (raw file):
Previously, sadraddini (Sadra Sadraddini) wrote…
ditto for emphasizing that this is only what the user has given.
Done.
There was a problem hiding this comment.
I'm satisfied with current implementation.
Reviewable status: labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)
planning/iris/test/iris_zo_test.cc line 299 at r6 (raw file):
Previously, AlexandreAmice (Alexandre Amice) wrote…
The threadsafe aspect is covered by the fact that
IrisZoTestshas tests which use multiple threads already. The only thing one might really hope to test here is ifparameterization_is_threadsafeisfalse, then we confirm that only one thread is used.I don't see a good way to test this short of using a non-threadsafe parameterization and hoping that one of the
leak-santizerscatches it but that is not very robust.
Yeah that is too much. The current form is OK.
There was a problem hiding this comment.
Reviewed all commit messages.
Dismissed @sadraddini from a discussion.
Reviewable status: labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)
cohnt
added
status: squashing now
Ordinarily, IRIS-ZO grows collision free regions in the robot's configuration space$\mathcal C$ . This allows the user to specify a function $f:\mathcal Q\to\mathcal C$ , and grow the region in $\mathcal Q$ instead. This has the following use cases:
@wernerpe: I've currently implemented this where everything uses the parametrization machinery, and the default is to just use the identity function. I think this only adds a small overhead, since we occasionally do an extra copy, and it's a lot more readable than having switching logic in a ton of places. But I can go through and do the switching logic if you think that'll make a speed difference. We can also always go back and do that later if you notice it causing a problem.
This change is