drake
drake copied to clipboard
RobotLocomotion
Add RationalForwardKinematics class.
This class computes the link poses as rational function of the stereographic projection variables.
This change is 
@RussTedrake sorry for the giant PR. I will split this PR to smaller pieces, that I will first send a PR with rational_forward_kinematics_internal, and then send a PR with the rest of the code. But I leave this PR here so that you can see why we have these functions in rational_forward_kinematics_interal.
ftr -- I just rebased master again; I don't think the previous one worked.
@amcastro-tri or @sherm1 Before we merge this, I'll need to you bless this component to live in the multibody/plant folder. Please ping back here to confirm.
That's because it's counting the dev code as nearly-zero. I need to expand it all and review it as-if if were newly added instead of moved. I'm currently assessing whether it's sensible to split the "move out of dev" into its own PR first. It depends on how coupled the two different unit tests are.
Yeah, the dev code here still needs some work.
While leaving this PR paused for now, please open a new PR with just the code move from dev into plant (including its internal test and test utilities, build file updates, etc) and assign me for platform review.
We can work to finish and merge that portion, and then circle back here.
Hmm, reviewable isn't working on this PR anymore. (I've opened a bug report about it already.)
Removing -@jwnimmer-tri for platform review per schedule (timed out).
Here's a few quick notes on the header file in the meantime.
(1) Remove the redundant drake:: and multibody:: namespaces, throughout.
(2) Fix function API comment verb "Calculate" => "Calculates" etc. Note that some of these verbs are after a dependent clause, on the second or third line of the comment. The private functions also need fixing for this.
(3) I have no idea what it means for a body index named "frame_A" to be associated with a Pose struct. The frames of a pose are usually associated with the variable name of a pose, not embedded into it. Is this struct the X_WA or something? In which case, the user needs to be careful to keep the index and pose in sync? Maybe the index is shouldn't be smooshed into a pose struct? I stopped reading before I was able to see how it gets used, but it struck me as odd the first time I came across it in the header.
(4) Re: cos_delta() function (likewise sin_delta) -- Maybe I missed it, but I didn't see delta defined in the class overview anywhere, in which case we'd need a Doxygen function comment here telling us what delta means.
Thanks Jeremy for the review. I will just reply your comments here, as reviewable doesn't work for me either
(1) Remove the redundant drake:: and multibody:: namespaces, throughout.
Done. I wrote the original code 4 years ago in anzu, hence it has the drake:: namespaces in the code.
(2) Fix function API comment verb "Calculate" => "Calculates" etc. Note that some of these verbs are after a dependent clause, on the second or third line of the comment. The private functions also need fixing for this.
Done.
(3) I have no idea what it means for a body index named "frame_A" to be associated with a Pose struct. The frames of a pose are usually associated with the variable name of a pose, not embedded into it. Is this struct the X_WA or something? In which case, the user needs to be careful to keep the index and pose in sync? Maybe the index is shouldn't be smooshed into a pose struct? I stopped reading before I was able to see how it gets used, but it struck me as odd the first time I came across it in the header.
You are right. That frame index was used in my very old implementation, it was not used in the final code. I removed this data member from Pose.
(4) Re: cos_delta() function (likewise sin_delta) -- Maybe I missed it, but I didn't see delta defined in the class overview anywhere, in which case we'd need a Doxygen function comment here telling us what delta means.
It was mentioned in the class documentation https://github.com/hongkai-dai/drake/blob/b538dbba5abcd09a4aca7b9a0bd5345b16862ef2/multibody/rational/rational_forward_kinematics.h#L32. I just added the documentation for cos_delta() and sin_delta() function to make it more clear.
Ah. I was reading Δθ in that text as a single quantity, but delta as a standalone quantity.
@jwnimmer-tri the reviewable stops working again on this PR. How did you let the reviewable author know the issue before?
Last time, I opened https://github.com/Reviewable/Reviewable/issues/974. If it's the same issue (seems like yes), then you could post a comment there again.
common/symbolic/trigonometric_polynomial.cc line 503 at r16 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
BTW Readers might have concerns about whether this is compatible with copy elision. Might be simplest to simply do:
return internal::SubstituteStereographicProjectionImpl( e_poly, sin_cos, t, t_set, one_plus_t_square, two_t, one_minus_t_square);The only advantage of this alternate spelling is that it goes to the core definition of "mandatory copy elision". Your current spelling puts it in the domain of "non-mandatory elision". That said, with high confidence, I believe we can say that our compilers will elide this. So, it's up to you.
It really only comes down to the question: Do you feel the name
e_rationalprovides value in comprehending this function?
My recollection is that NRVO is mandatory per the language. I'll check and report back.
common/symbolic/trigonometric_polynomial.cc line 503 at r16 (raw file):
Previously, jwnimmer-tri (Jeremy Nimmer) wrote…
My recollection is that NRVO is mandatory per the language. I'll check and report back.
Well, you are right that formally NRVO is not required by the standard.
However, I think we should behave as-if the compiler will always do it, and not both authors about it, even at the BTW level.
If you think the variable name makes the code less readable than that's a valid defect, but for our platforms I think we can assume NVRO without batting an eyelash. Let me know if you have evidence to the contrary (in which case we might want to update our GSG).
multibody/dev/c_iris/rational_forward_kinematics.cc line 59 at r3 (raw file):
Since this class is literally re-implementing each mobilizer's kinematics individually one by one, any bow towards "try to cover all joints types by using a generic architecture does not make any sense.
This code only needs to identify mobilizers that knows how to re-implement. For any unsupported mobilizer, it needs to throw. In case at runtime the user only ever feeds it mobilizers that it knows how to re-implement, then the code will work fine with the num_positions-like heuristics. If the user feeds it mobilizers it doesn't re-implement, then it will throw. My suggestion shows how to accomplish that.
Therefore, this question:
For at least the foreseeable future, we expect that the combination of (|q|, |v|, can_rotate, can_translate) will uniquely identify all mobilizers?
... has absolute no bearing on anything, so I'm not sure why we're talking about it. If someone want to add rational kinematics support for SomeVeryWeirdMobilizer in the future, then sure they will need to figure out an approach. That's no reason to belabor this PR.
multibody/rational/rational_forward_kinematics.cc line 275 at r19 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
nit: You're missing
DRAKE_DEMAND(revolute_mobilizer != nullptr);here.
The RationalForwardKinematics constructor already did sufficient sanity checking for bad heuristics on its captured plant_. No other member functions need to repeat those checks when operating on that plant.