[multibody] Add URDF and SDF Parsing for Curvilinear Joints (#22196)

[mshalm]

Adds tags and joint configuration structure to URDF and SDF parsers for CurvilinearJoint, completing implementation of #22196.

Implements new drake: namespace XML tags to handle description of line segment and curve shapes. Format prioritizes human readability without knowledge of the particular API for the internal PiecewiseConstantCurvatureTrajectory -- for instance expressing the curves with a radius and clockwise/counterclockwise direction rather than a turning_rate. A sample URDF specification is provided below.

<drake:joint name="curvilinear_periodic" type="curvilinear">
  <origin xyz="3 2 1" rpy="0 0 0"/>
  <parent link="link12"/>
  <child link="link13"/>
  <drake:initial_curve_tangent xyz="0 0 1"/>
  <drake:plane_normal xyz="1 0 0"/>
  <drake:is_periodic value="true"/>
  <dynamics damping="0.1"/>
  <drake:curves>
    <drake:line_segment length="1.0"/>
    <drake:circular_arc length="3.14159" radius="0.5" direction="counterclockwise"/>
    <drake:circular_arc length="3.14159" radius="0.5" direction="clockwise"/>
  </drake:curves>
</drake:joint>

Per offline discussion, the API for periodicity of the PiecewiseConstantCurvatureTrajectory has been migrated to an explicit constructor argument bool is_periodic.

Some preexisting parsing files appear to not have been recently clang-formatted on mainline, so the line change count is significantly inflated due to whitespace changes.

---

This change is 

[amcastro-tri]

Reviewed 4 of 11 files at r1.
Reviewable status: 2 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers, missing label for release notes (waiting on @mshalm)

---

common/trajectories/piecewise_constant_curvature_trajectory.cc line 109 at r1 (raw file):

boolean<T> PiecewiseConstantCurvatureTrajectory<T>::EndpointsAreNearlyEqual(
    double tolerance) const {
  return CalcPose(0.).IsNearlyEqualTo(CalcPose(length()), tolerance);

is this implementation correct? under the hood CalcPose() is wrapping its argument if the trajectory was constructed to be periodic. Can we get into the situation where length() (due to round-off errors) is wrapped to s=0 and thus this will always return true regardless?

---

multibody/tree/curvilinear_joint.h line 256 at r1 (raw file):

  /** @returns copy of underlying curve of joint. */
  std::unique_ptr<trajectories::Trajectory<double>> get_curve_clone() const {
    return curvilinear_path_.Clone();

why a clone? and why a clone to the base trajectory class rather than to the specific trajectory type? (that is, do you really want to exploit plymorphism here?).

A quick look reveals this is used in unit tests to access the underlying trajectory data. If so, I believe what you wnat is:

const PiecewiseConstantCurvatureTrajectory<double>& get_trajectory() const { return curvilinear_path_;}

[mshalm]

All comments addressed in rev 2

Reviewable status: 2 unresolved discussions, LGTM missing from assignee amcastro-tri, needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @mshalm)

---

common/trajectories/piecewise_constant_curvature_trajectory.cc line 109 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

is this implementation correct? under the hood CalcPose() is wrapping its argument if the trajectory was constructed to be periodic. Can we get into the situation where length() (due to round-off errors) is wrapped to s=0 and thus this will always return true regardless?

Addressed via explicitly storing the end value.

---

multibody/tree/curvilinear_joint.h line 256 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

why a clone? and why a clone to the base trajectory class rather than to the specific trajectory type? (that is, do you really want to exploit plymorphism here?).

A quick look reveals this is used in unit tests to access the underlying trajectory data. If so, I believe what you wnat is:

const PiecewiseConstantCurvatureTrajectory<double>& get_trajectory() const { return curvilinear_path_;}

Agreed.

[amcastro-tri]

+@rpoyner-tri, can I ask you to feature review parsing relevant components?

Reviewable status: 2 unresolved discussions, LGTM missing from assignees amcastro-tri,rpoyner-tri(platform), commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @mshalm)

[amcastro-tri]

on the trajectory changes.

Reviewed 1 of 11 files at r1, 3 of 7 files at r2.
Reviewable status: 2 unresolved discussions, LGTM missing from assignee rpoyner-tri(platform), commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @mshalm)

---

multibody/tree/curvilinear_joint.h line 254 at r2 (raw file):

  }

  /** @returns copy of underlying curve of joint. */

nit,

Suggestion:

@returns A reference to the underlying trajectory.

---

common/trajectories/piecewise_constant_curvature_trajectory.cc line 195 at r2 (raw file):

  // Build frames for the start of each segment.
  std::vector<math::RigidTransform<T>> segment_start_poses;

nit, consider renaming to segment_break_poses for consistency

[amcastro-tri]

@drake-jenkins-bot ok to test

[rpoyner-tri]

minor bits, discussion of plans

Reviewed 4 of 11 files at r1, 7 of 7 files at r2, all commit messages.
Reviewable status: 5 unresolved discussions, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @mshalm)

---

a discussion (no related file):
Overall code so far looks good. The (meta) issues I have are with documentation and testing.

I must apologize for having some (as-yet) undocumented maintenance policies with parsing code:

1. I strive to get the parsing code to be throw free.
2. I strive to get the test coverage of parsing code as close to 100% as possible, especially Warn and Error branches.
   2a. in particular, Error branches under test (with DiagnosticPolicyTestBase) should be able to return from the top-level parse call without further mayhem (throws, UB, etc.).
3. drake: tags should get documentation in multibody/parsing/parsing_doxygen.h.

Just by code reading so far, I suspect that 2a is satisfied. Checking 1 and 2 will require a bunch more tests. 3 is also a bit of work.

With all that in mind, Here's what I propose:

• We land this PR, perhaps with some further minor fixups.
• We leave the new tags undocumented, and and treat them as experimental for now.
• We (perhaps Matthew, with help from me?) circle back in later PRs to fill in tests, and then documentation.
• Once tests and documentation are merged, we treat the parsing as supported and stable.

My hope is that plan will allow you to make some progress and do some integrations, without having to delay having some parsing support.

---

multibody/parsing/test/urdf_parser_test/joint_parsing_test.urdf line 170 at r2 (raw file):

    <parent link="link12"/>
    <child link="link13"/>
		<drake:initial_curve_tangent xyz="0 0 1"/>

Please replace literal tabs with spaces.

---

common/trajectories/piecewise_constant_curvature_trajectory.cc line 121 at r2 (raw file):

      initial_frame.col(kCurveTangentIndex),
      initial_frame.col(kPlaneNormalIndex), initial_pose.translation(),
      is_periodic_);

minor DoClone is not reached by unit tests.

[amcastro-tri]

Reviewable status: 6 unresolved discussions, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @mshalm)

---

multibody/parsing/test/sdf_parser_test/joint_parsing_test.sdf line 277 at r2 (raw file):

            <drake:length>3.14159265358979323846</drake:length>
            <drake:radius>0.5</drake:radius>
            <drake:direction>counterclockwise</drake:direction>

I'd advocate for a less verbose way to write these files. SDF/URDF are already very inneficient in the amount of <foo/> needed to get model data in.

In this case, I'd probably write <drake:curvature>-2.0</drake:curvature>, with the sign convention properly documented to spare users from the verbose <drake:direction>counterclockwise</drake:direction> for every segment in the curve.

Another thought. Would it be possible to make the spelling look like <drake:curvatures values='2.0, -2.0, 1.5, 0.3, -2.1, 0.0/>, i.e. being able to specify arbitrary length vectors in a single line? @rpoyner-tri you are the expert here.
Notice this also helps reduce verbosity by completely avoiding the additional <drake:line_segment> tag.

I'd even remove the additional nesting within drake:curves, since IMO being nested inside <drake:joint ... type="curvilinear"> should suffice to provide enough context.

Once again, I feel this is just too verbose to desribe something that simply requires specifying two vectors (curvatures+lengths)