[trajectories] Add PiecewiseConstantCurvatureTrajectory #22214

mshalm · 2024-11-19T19:31:42Z

Adding a trajectory type modeling concatenation of linear segments and circular arcs within a plane, to be used in curvilinear joints (#22196).

The trajectory is intended to eventually model the transform between inboard and outboard frames $X_{FM}$ for the curvilinear joint, and it's design was driven by that intended use case.

This change is

Adding a trajectory type modeling concatenation of linear segments and circular arcs within a plane, to be used in curvilinear joints (RobotLocomotion#22196).

rpoyner-tri

Good day, @mshalm . I’m assigning +@amcastro-tri as the most relevant team member to assist with this pull request. If you already had a different team member in mind, please let us know. To @amcastro-tri , feel free to delegate in case there is someone else more suitable.

Reviewable status: LGTM missing from assignee amcastro-tri, needs platform reviewer assigned, needs at least two assigned reviewers, missing label for release notes (waiting on @mshalm)

mshalm · 2024-11-19T22:45:54Z

Good day, @mshalm . I’m assigning +@amcastro-tri as the most relevant team member to assist with this pull request. If you already had a different team member in mind, please let us know. To @amcastro-tri , feel free to delegate in case there is someone else more suitable.

Reviewable status: LGTM missing from assignee amcastro-tri, needs platform reviewer assigned, needs at least two assigned reviewers, missing label for release notes (waiting on @mshalm)

Happy for Alejandro to review! @sherm1 and @joemasterjohn-TRI may also be good reviewers.

rpoyner-tri

+(release notes: feature)

Reviewable status: LGTM missing from assignee amcastro-tri, needs platform reviewer assigned, needs at least two assigned reviewers

sherm1 · 2024-11-25T19:08:57Z

@amcastro-tri, Matt asked me about review progress on this PR. Rico assigned you -- feel free to reassign if you don't have time.

amcastro-tri

Sorry for the delay. I already started with this.

I did a first pass. Mostly just header and docs. Difficult to understand what exactly this class does without diving into the cc files (i'll do that next). I provided some ideas based on my guess of what this class does.

I'll now reverse engineer a bit to confirm.

Reviewable status: 20 unresolved discussions, LGTM missing from assignee amcastro-tri, needs platform reviewer assigned, needs at least two assigned reviewers

common/trajectories/piecewise_constant_curvature_trajectory.h line 18 at r1 (raw file):
This docs are very minimal and don't give me the information I need to understand what this class does without reverse engineering the code.

I'd suggest something like:

This class represents a constant-curvature trajectory defined on a plane.

This class defines positions r(s) parameterized by arch length s. Trajectory r(s) is composed of N segments of constant curvature:
r(s) = rᵢ(s), ∀ s ∈ [sᵢ, s₊₁)
where rᵢ(s) is a segment of constant curvature and points sᵢ, with i=0 to N, are segment breaks.

At all points, we can define the tangent vector t̂(s) = dr(s)/ds, which is a vector of unit length given the arch length parameterization.

Similarly, we can define the normal to the curve from dt(s)/ds = k(s)⋅n̂(s), where k(s) is the curvature and n̂(s) the normal to the curve.

Finally, the binormal vector is b̂ = t̂×n̂.

This class defines a piecewise constant-curvature trajectory where each segment has the same bonormal vector b̂. That is, the entire trajectory lives in a plane perpendicular to b̂.

This notation will help you document constructors more easily since you will be able to refer to this.

Things that are still missing, and obvsiouly I cannot guess:

Is continuity at breaks sᵢ guaranteed? what about the continuity of the tangent vector t(s)?

Suggested reading for notation

common/trajectories/piecewise_constant_curvature_trajectory.h line 17 at r1 (raw file):

/**
 * Represents a piecewise constant-curvature trajectory within a plane embdedded

minor,

Suggestion:

embedded

common/trajectories/piecewise_constant_curvature_trajectory.h line 21 at r1 (raw file):

 *
 * This class defines a trajectory composed of segments with constant curvature:
 * circular arcs and line segments. It provides methods to calcualte poses,

Suggestion:

calculate

common/trajectories/piecewise_constant_curvature_trajectory.h line 25 at r1 (raw file):

 * traveled.
 *
 * The curve is path-length paramaterized; as such the linear velocity is always

Suggestion:

parameterized

common/trajectories/piecewise_constant_curvature_trajectory.h line 26 at r1 (raw file):

 *
 * The curve is path-length paramaterized; as such the linear velocity is always
 * a unit vector.

This sounds strange in that it implies velocity has no units. Most likely not what you meant.

I guess what you want to say is that if r(s) is the position vector at length s, then dr/ds is a unit vector. But there is no time involved here and thererfore it is wrong to call dr/ds a "velocity". At this point I'm guessing. I'll come back once I read the rest.

Code quote:

 * The curve is path-length paramaterized; as such the linear velocity is always
 * a unit vector.

common/trajectories/piecewise_constant_curvature_trajectory.h line 39 at r1 (raw file):

   * Constructs an empty piecewise constant curvature trajectory.
   */
  PiecewiseConstantCurvatureTrajectory() {}

Suggestion:

PiecewiseConstantCurvatureTrajectory() = default;

common/trajectories/piecewise_constant_curvature_trajectory.h line 48 at r1 (raw file):

   */
  template <typename ToScalar>
  PiecewiseConstantCurvatureTrajectory<ToScalar> CloneToScalar() const {

I haven't seen this pattern of CloneToScalar() in common/trajectories. Wondering why you'd introduce this. Is there a plan or use for it?

common/trajectories/piecewise_constant_curvature_trajectory.h line 67 at r1 (raw file):

   * Constructs a piecewise constant curvature trajectory.
   *
   * @param segment_breaks A vector of distance break points between segments.

Can you say more about this?

Thus far, reading class docs and this constructor docs I cannot figure out the semantics of this class. That means we need more docs.
I'm sure I'll reverse engineer the code, but most users won't.

In particular for segement_breaks, is this the distance to the "origin" of the path? or is this the length of each path? What is the size? the number of segments? or number of segments plus one?

With the notation I introduced above, is this sᵢ for i=0 to N?

Code quote:

A vector of distance break points between segments.

common/trajectories/piecewise_constant_curvature_trajectory.h line 69 at r1 (raw file):

   * @param segment_breaks A vector of distance break points between segments.
   * @param segment_curvatures A vector of curvatures for each segment.
   * @param curve_tangent_axis The tangent of the curve at path length 0.

If I understand correctly, per the notation I provide above, this is t(0)?

common/trajectories/piecewise_constant_curvature_trajectory.h line 73 at r1 (raw file):

   * lies.
   */
  PiecewiseConstantCurvatureTrajectory(const std::vector<T>& segment_breaks,

if I am corret these are sᵢ , then is there a pre-condition on these being provided in accending order?

common/trajectories/piecewise_constant_curvature_trajectory.h line 74 at r1 (raw file):

   */
  PiecewiseConstantCurvatureTrajectory(const std::vector<T>& segment_breaks,
                                       const std::vector<T>& segment_curvatures,

what is the sign convention for the curvatures? If following the typical Frenet-Serret formulas, these are meant to be positive?

common/trajectories/piecewise_constant_curvature_trajectory.h line 76 at r1 (raw file):

                                       const std::vector<T>& segment_curvatures,
                                       const Vector3<T>& curve_tangent_axis,
                                       const Vector3<T>& plane_normal_axis);

nit, consider binormal for an immediate connection to the kinematics of curves.
Also, expand the docs locally here to state that the plane in which the curve is defined is peperndicular to the binormal.

common/trajectories/piecewise_constant_curvature_trajectory.h line 76 at r1 (raw file):

                                       const std::vector<T>& segment_curvatures,
                                       const Vector3<T>& curve_tangent_axis,
                                       const Vector3<T>& plane_normal_axis);

For both of these axis, state a frame in which things are expressed and use monogram notation. It could be a generic frame W, i.e. tangent_W and binormal_W.

common/trajectories/piecewise_constant_curvature_trajectory.h line 81 at r1 (raw file):

   * @return The number of rows in the trajectory's output.
   */
  Eigen::Index rows() const override { return 4; }

For someone who is not familiar with the Trajectory hierarchy, what are these?
I believe these define the return size of value()? why 4x4? I thought it'd return the position vector r(s)?

common/trajectories/piecewise_constant_curvature_trajectory.h line 94 at r1 (raw file):

   * @return The interpolated pose as a RigidTransform.
   */
  math::RigidTransform<T> GetPose(const T& distance) const;

nit, in Drake we reserve GetFoo() to retrieve values often already computed.

The term of art in Drake for computation is Calc. Therefore use CalcPose() instead.

common/trajectories/piecewise_constant_curvature_trajectory.h line 94 at r1 (raw file):

   * @return The interpolated pose as a RigidTransform.
   */
  math::RigidTransform<T> GetPose(const T& distance) const;

I'd also fall back to the math here. If tangent and binormal vectors were provide in W, then this method returns the pose X_WF of the Frenet-Serret frame F.

common/trajectories/piecewise_constant_curvature_trajectory.h line 97 at r1 (raw file):

  /**
   * Returns the interpolated pose at the given distance along the trajectory as

interpolated? why not the actual exact pose?

Code quote:

interpolated

common/trajectories/piecewise_constant_curvature_trajectory.h line 103 at r1 (raw file):

   * @return The 4x4 homogeneous transform matrix representing the pose.
   */
  MatrixX<T> value(const T& t) const override {

I was surprised to see that this is what we want. Wouldn't it make more sense to have value() return r(s) and then use CalcPose() to retrieve poses?
Just double-checking with you.

common/trajectories/piecewise_constant_curvature_trajectory.h line 115 at r1 (raw file):

  /**
   * Returns the interpolated spatial velocity at the given distance along the

Is this really "spatial velocity"? I don't see anything in these docs that imply units of time.

common/trajectories/piecewise_constant_curvature_trajectory.h line 121 at r1 (raw file):

   * @return The interpolated spatial velocity.
   */
  multibody::SpatialVelocity<T> GetVelocity(const T& distance) const;

minor, CalcVelocity().

amcastro-tri

A second pass, still working. So far so good, it just needs to be documented better.

Reviewed 1 of 4 files at r1.
Reviewable status: 42 unresolved discussions, LGTM missing from assignee amcastro-tri, needs platform reviewer assigned, needs at least two assigned reviewers

common/trajectories/piecewise_constant_curvature_trajectory.h line 164 at r1 (raw file):
These docs left me having to reverse engineer your math.
I'd say you pick a notation, write the math, and then use monogram notation consistent with that math.

For instnce, if we call the Frenet-Serret axes TNB (as per usual in many calculus books), then you could define (in the class docs) vectors Tᵢ, Nᵢ, Bᵢ at the breakes. Similary, we can use F for the renet-Serret frame, and Fᵢ the Frenet-Serret frame at the i-th break.

Here, since things are local to the original of this segment, you could documents this as:

Computes the pose X_F0F(t) of the Frenet-Serret frame F at time t, relative to frame Frenet-Serret frame F0 at t =0.

NOTE: I will start using t insted of s because I just realize Trajectory methods take time, not arch length.

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

    const std::vector<T>& segment_curvatures,
    const Vector3<T>& curve_tangent_axis, const Vector3<T>& plane_normal_axis)
    : PiecewiseTrajectory<T>(segment_breaks),

looking at this construction, I realize now that segement_breaks are times? that's how it is for PiecewiseTrajectory at least.

I guess now I understand what you meant with unit velocity. Maybe state it as "We assume that the speed ṡ=1 all along the trajectory" somewhere in the constructor docs.

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

    : PiecewiseTrajectory<T>(segment_breaks),
      base_frame_(MakeBaseFrame(curve_tangent_axis, plane_normal_axis)),
      segment_curvatures_(segment_curvatures),

should we check these are all positive? is that a requirement?

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

math::RigidTransform<T>
PiecewiseConstantCurvatureTrajectory<T>::CalcPoseInLocalFrame(
    const T& curvature, const T& length) {

DRAKE_DEMAND(length >= 0)?
DRAKE_DEMAND(curvature >= 0)?

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

  Vector3<T> translation = Vector3<T>::Zero();
  // Calculate rotation angle
  T theta = length * curvature;

nit,

Suggestion:

 const T theta = length * curvature;

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

  T theta = length * curvature;
  math::RotationMatrix<T> rotation =
      math::RotationMatrix<T>::MakeZRotation(theta);

if we use notation proposed in the header, you can write R_F0F

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

    // Case 2: non-zero curvature (circular arc)
    // Calculate position on the circular arc, which
    // is about the centerpoint 1/curvature * normal_axis with

fyi, I didn't find these notes very useful.
I do realize however that really we need a schematic to explain them. Would you have a reference? that'd make thins easier for you.

If not, simply state these are the coordinates along x-y-z axis aligned with tangent-normal-binormal respectively. Since the curve lives in a plane defined by the binormal, the binormal coordinate is zero. The rest is probably easy to dervie once you understand the frame convention.

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

  const bool right_hand =
      (kPlaneNormalDimension + 1) % 3 == kCurveTangentDimension;
  DRAKE_DEMAND(right_hand);

Remove this. This will always be true given the constant values defined in the header.
I don't think this check is really chekcing anything. Also, it costs you, when all of this could be done at compiel time (don't do it though, I don't think there is a good reason. Just remove)

Code quote:

  // NOTE: assumes right hand rule for tangent \times normal = plane
  const bool right_hand =
      (kPlaneNormalDimension + 1) % 3 == kCurveTangentDimension;
  DRAKE_DEMAND(right_hand);

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

      (kPlaneNormalDimension + 1) % 3 == kCurveTangentDimension;
  DRAKE_DEMAND(right_hand);
  const Vector3<T> normal_axis = plane_normal_axis.cross(curve_tangent_axis);

confusing to see "normal" in two names. I think we should use the term "binormal" for plane_normal_axis and "normal" for the curve's actual normal.

I'd also use monogram notation so that everything falls into place, i.e. tangent_W, binormal_W, normal_W, which the leads to rotation matrix R_WF (if we use F for the Frenet-Serret frame).

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

  const Vector3<T> normal_axis = plane_normal_axis.cross(curve_tangent_axis);

  DRAKE_DEMAND(curve_tangent_axis.norm() >= kEpsilon);

I'd check this once in the constructor, and document this requirement publicly to the user.

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

      curve_tangent_axis.normalized();
  base_frame_matrix.col(kPlaneNormalDimension) = plane_normal_axis.normalized();
  return math::RotationMatrix<T>(base_frame_matrix);

you might want to use RotationMatrix::MakeFromOrthonormalColumns(), which has additional checks.

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

    const std::vector<T>& segment_curvatures,
    const std::vector<T>& segment_breaks) {
  size_t num_breaks = segment_breaks.size();

this leads me to believe that segment_breaks[0] = 0.0 always. Is that true? if so, it should be documented publicly somewhere.

Code quote:

 size_t num_breaks = segment_breaks.size();

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

    const std::vector<T>& segment_breaks) {
  size_t num_breaks = segment_breaks.size();
  size_t num_segments = num_breaks - 1;

I suggest having num_segments() and num_breaks() as class functions. They can return int as needed.

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

  const VectorX<T> segment_rotation_angles =
      segment_lengths.cwiseProduct(curvatures_eigen);
  VectorX<T> segment_cumulative_rotations = VectorX<T>::Zero(num_breaks);

This assumes the continuity of t̂(s) (or T(s) if you prefer that notation, which is probably best for monogram). Please add this property to the class documentation.

Code quote:

segment_cumulative_rotations

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

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

consider,

Suggestion:

 segment_local_poses;

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

  // build frames for the start of each segment.
  std::vector<math::RigidTransform<T>> segment_local_frames;
  segment_local_frames.reserve(num_segments);

Suggestion:

 segment_local_frames.reserve(num_breaks);

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

  segment_local_frames.push_back(math::RigidTransform<T>(base_frame));

  for (size_t i = 0; i < num_segments; i++) {

nit,

Suggestion:

for (int i = 0; i < num_segments(); i++) {

common/trajectories/piecewise_constant_curvature_trajectory.h line 162 at r1 (raw file):

 private:
  /**
   * Calculates the relative transform between the start of a constant-curavtue

typo

common/trajectories/piecewise_constant_curvature_trajectory.h line 167 at r1 (raw file):

   * @param curvature The curvature of the segment.
   * @param length The length of travel in the segment.
   * @return The pose relative to length 0 along the curve.

nit,

Suggestion:

 The pose X_F0F in frame F0

common/trajectories/piecewise_constant_curvature_trajectory.h line 185 at r1 (raw file):

  /**
   * Builds the curve from the given curvatures and segment breaks.

This seems like you pushed an oudated comment?

Code quote:

Builds the curve from the given curvatures and segment breaks.

common/trajectories/piecewise_constant_curvature_trajectory.h line 193 at r1 (raw file):

   * @return The local frames of each segment.
   */
  static std::vector<math::RigidTransform<T>> MakeSegmentLocalFrames(

without looking at the code, my guess is that this makes poses (not frames) X_WFi, the pose of the Frenet-Serret Frame Fi at the i-th break. Is that correct. If so, simply write that.

Also, document the size of the returned vector. num_segments+1?

common/trajectories/piecewise_constant_curvature_trajectory.h line 198 at r1 (raw file):

      const std::vector<T>& segment_breaks);

  math::RotationMatrix<T> base_frame_;

Can you define the meaning of base_frame_ with math here? is this the Frenet-Serret frame at s=0?

common/trajectories/piecewise_constant_curvature_trajectory.h line 203 at r1 (raw file):

  static inline constexpr size_t kCurveTangentDimension = 0;
  static inline constexpr size_t kCurveNormalDimension = 1;

consider kTangentIndex, kNormalIndex and kBinormalIndex.

amcastro-tri

Done feature reviewing the class. Only the tests are missing.
I'll wait until your replies @mshalm to make sure my understanding of this class is correct.

Reviewed 1 of 4 files at r1.
Reviewable status: 53 unresolved discussions, LGTM missing from assignee amcastro-tri, needs platform reviewer assigned, needs at least two assigned reviewers

common/trajectories/piecewise_constant_curvature_trajectory.h line 198 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

Can you define the meaning of base_frame_ with math here? is this the Frenet-Serret frame at s=0?

or in retrospect, I'd remove. After all you have that base_frame_ = segment_local_frame_[0], so no need for this extra member.

common/trajectories/piecewise_constant_curvature_trajectory.h line 200 at r1 (raw file):
nit,

Once you pick your notation, consider documenting something akin of:

Pose X_WFi of the i-th Frenet-Serret Frame Fi in the global frame W.

Suggestion:

 segment_local_pose_;

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

    const T& distance) const {
  int segment_index = this->get_segment_index(distance);
  math::RigidTransform<T> X_SiT =

Suggestion:

  const math::RigidTransform<T> X_SiT =

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

    const T& distance) const {
  int segment_index = this->get_segment_index(distance);
  math::RigidTransform<T> X_SiT =

once you pick a notation for the entire class, update this monogram notation.

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

  math::RigidTransform<T> X_SiT =
      CalcPoseInLocalFrame(segment_curvatures_[segment_index],
                           distance - this->start_time(segment_index));

This is confusing, even if at this stage I reversed engineer everything I need.

Make the change distance -> time so that this line reads okay.

The docs should state this trajectory assumes ds/dt = 1, i.e. unit speed all along the trajectory.

Code quote:

 distance - this->start_time(segment_index))

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

template <typename T>
multibody::SpatialVelocity<T>
PiecewiseConstantCurvatureTrajectory<T>::GetVelocity(const T& distance) const {

nit, distance -> time here and elsewhere.

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

  multibody::SpatialVelocity<T> spatial_velocity;
  // Anuglar velocity is about plane normal axis. The curvature is defined

typo

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

  // Anuglar velocity is about plane normal axis. The curvature is defined
  // as the ratio of the angular velocity to the linear velocity, and the
  // latter is fixed to the unit vector along the curve tangent.

not really, we don't need "time" to define the kinematics of a curve.

This essentially boils down to two ingredients, the Darboux vector and the fact you are assuming ds/dt=1. So using the chain rule, and since there is no angular component along tangent T, the angular velocity is ω = κ⋅B, with B the binormal vector.

Code quote:

  // Anuglar velocity is about plane normal axis. The curvature is defined
  // as the ratio of the angular velocity to the linear velocity, and the
  // latter is fixed to the unit vector along the curve tangent.

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

  // As the trajectory has piecewise constant curvature, it has piecewise
  // constant angular velocity and thus zero angular acceleration almost
  // everywhere. The linear acceleration is simply the centripetal

with "almost verywhere" I imagine you mean the discontinuities at the breaks?

If so, I'd say, "Since velocity is piecewise constant, the acceleration is zero in each segment, with infinite acceleration at the breaks"

Code quote:

  // constant angular velocity and thus zero angular acceleration almost
  // everywhere. The linear acceleration is simply the centripetal

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

  // constant angular velocity and thus zero angular acceleration almost
  // everywhere. The linear acceleration is simply the centripetal
  // accleration, which points along the curve normal axis.

typo

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

  spatial_acceleration.rotational() = Vector3<T>::Zero();
  spatial_acceleration.translational() =
      R_BT.col(kCurveNormalDimension) * curvature;

the centripetal acceleration is κ⋅ṡ² and we have ṡ² = 1. Add that to document your formula

common/trajectories/piecewise_constant_curvature_trajectory.h line 134 at r1 (raw file):

  /**
   * Checks if the trajectory is nearly periodic within the given tolerance.
   *

nit, probably worth stating that we use RigidTransform::IsNearlyEqualTo() as the metric of "nearly equal poses".

mshalm

Reviewable status: 51 unresolved discussions, LGTM missing from assignee amcastro-tri, needs platform reviewer assigned, needs at least two assigned reviewers

a discussion (no related file):
I have replied to your discussion on the header first, let's resolve that before I rev the PR.

I see a couple overarching issues:

There may be some missing context that this will eventually be used to implement a Joint -- please briefly look at my issue (Curvilinear Joints #22196) to get some context if you haven't already.
A number of design decisions mirror PiecewisePose<T> for instance the naming of GetXX methods and the representation as a 4x4 matrix. If you would like me to explicitly break with this structure, please let me know and I'll follow updated conventions as you find appropriate, e.g. renaming to CalcXX.
The "curvature" here is not Frenet–Serret curvature exactly in that it can be negative and represents right-hand-rule turning rate about the plane normal axis. I suggest to have a substantial discussion relating to the TNB frame in the class doc in monogram notation, similar to your suggested class doc; and to rename curvatures to something else to avoid confusion.
There is confusion about discussing time units. Trajectory<T> and all inheriting classes assume the independent variable is time. I am not sure what the right naming strategy is here overall. My preference is again to pattern match but happy to field a reasonable high-level directive here.

common/trajectories/piecewise_constant_curvature_trajectory.h line 18 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

This docs are very minimal and don't give me the information I need to understand what this class does without reverse engineering the code.

I'd suggest something like:

This class represents a constant-curvature trajectory defined on a plane.

This class defines positions r(s) parameterized by arch length s. Trajectory r(s) is composed of N segments of constant curvature:
r(s) = rᵢ(s), ∀ s ∈ [sᵢ, s₊₁)
where rᵢ(s) is a segment of constant curvature and points sᵢ, with i=0 to N, are segment breaks.

At all points, we can define the tangent vector t̂(s) = dr(s)/ds, which is a vector of unit length given the arch length parameterization.

Similarly, we can define the normal to the curve from dt(s)/ds = k(s)⋅n̂(s), where k(s) is the curvature and n̂(s) the normal to the curve.

Finally, the binormal vector is b̂ = t̂×n̂.

This class defines a piecewise constant-curvature trajectory where each segment has the same bonormal vector b̂. That is, the entire trajectory lives in a plane perpendicular to b̂.

This notation will help you document constructors more easily since you will be able to refer to this.

Things that are still missing, and obvsiouly I cannot guess:

Is continuity at breaks sᵢ guaranteed? what about the continuity of the tangent vector t(s)?

Suggested reading for notation

Great suggestion. One caveat -- the trajectory here is not in fact the TNB frame.

In Frenet-Serret analysis of curves, the curvature is always non-negative. When the supplied curvature is negative, the frame is actually offset from the TNB frame by a 180 turn about the tangent axis. I will consider changing the names of some variable to reflect this, perhaps replacing the name curvature with turning_rate and noting in the doc the difference from TNB frame an that curvature = abs(turning_rate)

common/trajectories/piecewise_constant_curvature_trajectory.h line 26 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

This sounds strange in that it implies velocity has no units. Most likely not what you meant.

I guess what you want to say is that if r(s) is the position vector at length s, then dr/ds is a unit vector. But there is no time involved here and thererfore it is wrong to call dr/ds a "velocity". At this point I'm guessing. I'll come back once I read the rest.

Part of the naming convention confusion is that Trajectory has a number of methods named as if the independent variable is time -- start_time for instance. Open to discussion about how to resolve discussion of time here.

common/trajectories/piecewise_constant_curvature_trajectory.h line 48 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

I haven't seen this pattern of CloneToScalar() in common/trajectories. Wondering why you'd introduce this. Is there a plan or use for it?

Yes -- per #22196 , this will be use to implement a Mobilizer and Joint which require CloneToScalar() functionality.

common/trajectories/piecewise_constant_curvature_trajectory.h line 67 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

Can you say more about this?

Thus far, reading class docs and this constructor docs I cannot figure out the semantics of this class. That means we need more docs.
I'm sure I'll reverse engineer the code, but most users won't.

In particular for segement_breaks, is this the distance to the "origin" of the path? or is this the length of each path? What is the size? the number of segments? or number of segments plus one?

With the notation I introduced above, is this sᵢ for i=0 to N?

This is the segment break points common to all PiecewiseTrajectory classes. I can point users to the parent class.

common/trajectories/piecewise_constant_curvature_trajectory.h line 69 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

If I understand correctly, per the notation I provide above, this is t(0)?

Correct

common/trajectories/piecewise_constant_curvature_trajectory.h line 73 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

if I am corret these are sᵢ , then is there a pre-condition on these being provided in accending order?

Yes, again by inheritance with PiecewiseTrajectory

common/trajectories/piecewise_constant_curvature_trajectory.h line 74 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

what is the sign convention for the curvatures? If following the typical Frenet-Serret formulas, these are meant to be positive?

Right hand rule -- per above comment I can clarify this in the class doc

common/trajectories/piecewise_constant_curvature_trajectory.h line 76 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

nit, consider binormal for an immediate connection to the kinematics of curves.
Also, expand the docs locally here to state that the plane in which the curve is defined is peperndicular to the binormal.

Per above comment it's not necessarily the binormal vector; could be the reflection of the binormal vector

common/trajectories/piecewise_constant_curvature_trajectory.h line 76 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

For both of these axis, state a frame in which things are expressed and use monogram notation. It could be a generic frame W, i.e. tangent_W and binormal_W.

Agreed to put in monogram notation, with explanation for the monogram in the docs

common/trajectories/piecewise_constant_curvature_trajectory.h line 81 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

For someone who is not familiar with the Trajectory hierarchy, what are these?
I believe these define the return size of value()? why 4x4? I thought it'd return the position vector r(s)?

Pattern matching PiecewisePose -- 4x4 comes from the Matrix representation of the spatial transform

common/trajectories/piecewise_constant_curvature_trajectory.h line 94 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

I'd also fall back to the math here. If tangent and binormal vectors were provide in W, then this method returns the pose X_WF of the Frenet-Serret frame F.

Per above comments, it's not necessarily the TNB frame

common/trajectories/piecewise_constant_curvature_trajectory.h line 94 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

nit, in Drake we reserve GetFoo() to retrieve values often already computed.

The term of art in Drake for computation is Calc. Therefore use CalcPose() instead.

I was pattern matching PiecewisePose but happy to do this if you insist

common/trajectories/piecewise_constant_curvature_trajectory.h line 97 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

interpolated? why not the actual exact pose?

Agreed that "interpolated" should be removed

common/trajectories/piecewise_constant_curvature_trajectory.h line 103 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

I was surprised to see that this is what we want. Wouldn't it make more sense to have value() return r(s) and then use CalcPose() to retrieve poses?
Just double-checking with you.

Again pattern matching PiecewisePose here and happy to do something else if you feel strongly

common/trajectories/piecewise_constant_curvature_trajectory.h line 115 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

Is this really "spatial velocity"? I don't see anything in these docs that imply units of time.

See previous comments on Trajectory implying time

common/trajectories/piecewise_constant_curvature_trajectory.h line 121 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

minor, CalcVelocity().

See previous comments on pattern matching PiecewisePose

common/trajectories/piecewise_constant_curvature_trajectory.h line 164 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

These docs left me having to reverse engineer your math.
I'd say you pick a notation, write the math, and then use monogram notation consistent with that math.

For instnce, if we call the Frenet-Serret axes TNB (as per usual in many calculus books), then you could define (in the class docs) vectors Tᵢ, Nᵢ, Bᵢ at the breakes. Similary, we can use F for the renet-Serret frame, and Fᵢ the Frenet-Serret frame at the i-th break.

Here, since things are local to the original of this segment, you could documents this as:

Computes the pose X_F0F(t) of the Frenet-Serret frame F at time t, relative to frame Frenet-Serret frame F0 at t =0.

NOTE: I will start using t insted of s because I just realize Trajectory methods take time, not arch length.

See previous comments on TNB frame, I will resolve the doc here based on our discussion about that.

common/trajectories/piecewise_constant_curvature_trajectory.h line 167 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

nit,

See previous comments on TNB frame, I will resolve the doc here based on our discussion about that.

common/trajectories/piecewise_constant_curvature_trajectory.h line 185 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

This seems like you pushed an oudated comment?

Fixed.

common/trajectories/piecewise_constant_curvature_trajectory.h line 193 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

without looking at the code, my guess is that this makes poses (not frames) X_WFi, the pose of the Frenet-Serret Frame Fi at the i-th break. Is that correct. If so, simply write that.

Also, document the size of the returned vector. num_segments+1?

Correct that this should be poses. See other TNB discussion

common/trajectories/piecewise_constant_curvature_trajectory.h line 198 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

or in retrospect, I'd remove. After all you have that base_frame_ = segment_local_frame_[0], so no need for this extra member.

Removed.

common/trajectories/piecewise_constant_curvature_trajectory.h line 203 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

consider kTangentIndex, kNormalIndex and kBinormalIndex.

See previous TNB frame discussion

common/trajectories/piecewise_constant_curvature_trajectory.h line 21 at r1 (raw file):

 *
 * This class defines a trajectory composed of segments with constant curvature:
 * circular arcs and line segments. It provides methods to calcualte poses,

Done.

common/trajectories/piecewise_constant_curvature_trajectory.h line 25 at r1 (raw file):

 * traveled.
 *
 * The curve is path-length paramaterized; as such the linear velocity is always

Done.

amcastro-tri

Reviewable status: 50 unresolved discussions, LGTM missing from assignee amcastro-tri, needs platform reviewer assigned, needs at least two assigned reviewers

a discussion (no related file):
Thanks @mshalm.

There may be some missing context

Thanks, I was aware of this as you brought it up during a Drake devs meeting. I love the schematics in the issue! very clear.

A number of design decisions mirror PiecewisePose<T> for instance the naming of GetXX

ah... that's unfortunate. I wonder if anyone ever consider even filing an issue for this. IMO PiecewisePose does not follow our naming conventions. Similarly I see PiecewisePolynomial has getPolynomial() which doesn't even follow our style guidelines. Therefore I'd go with CalcFoo() and break the pattern. We can also leave it as is and ask the platform reviewer when we assign one. Some pieces of Drake are from before our naming conventions.

The "curvature" here is not Frenet–Serret curvature exactly in that it can be negative and represents right-hand-rule turning

Ah, I see. In retrospect that only makes sense, otherwise curves would only wrap in one direction if the binormal vector was constant.
But to my point, there was no way I could've guessed without a careful scrutiny of the source, something a user shouldn't need to do. So yes, I agree with you in that careful notation and docs in the class are in order. I am not strong about using TNB terminology all along, but that's just my go-to model of curves in my head.

There is confusion about discussing time units.

I see two paths depending on what you need for your project. If what you need is a trajectory function of time, then I'd go with Trajectory<T>. If what you need is a path function of arch length, then maybe there is no good reason to inherit from Trajectory<T> in the first place? It doesn't even seem like you are gaining much from it right now.

common/trajectories/piecewise_constant_curvature_trajectory.h line 18 at r1 (raw file):

Previously, mshalm (Mathew Halm) wrote…

Great suggestion. One caveat -- the trajectory here is not in fact the TNB frame.

In Frenet-Serret analysis of curves, the curvature is always non-negative. When the supplied curvature is negative, the frame is actually offset from the TNB frame by a 180 turn about the tangent axis. I will consider changing the names of some variable to reflect this, perhaps replacing the name curvature with turning_rate and noting in the doc the difference from TNB frame an that curvature = abs(turning_rate)

That makes total sense. Plese make sure this shows in your documentation.
I'd suggest you choose a good notation for your use case (I only suggested one here based on my first guess of what this class does). When choosing notation I'd make the following suggestions which usually help:

Since we write docs in ASCII, notation that can be written with unicode (highly recommend unicodeit tends to work best.
You might be inclined to write Latex in Doxygen. While that renders well in the web version, most of us read these docs straight from the source. Usually we only use Latex for very complex math only for which Unicode is far from being the right tool.
Since you have frames involved, notation that allows to use simple monogram symobls also eases things.

common/trajectories/piecewise_constant_curvature_trajectory.h line 21 at r1 (raw file):

Previously, mshalm (Mathew Halm) wrote…

Done.

did you push?

common/trajectories/piecewise_constant_curvature_trajectory.h line 25 at r1 (raw file):

Previously, mshalm (Mathew Halm) wrote…

Done.

did you push? probably in other places as well since this is already the second one.

common/trajectories/piecewise_constant_curvature_trajectory.h line 26 at r1 (raw file):

Previously, mshalm (Mathew Halm) wrote…

Part of the naming convention confusion is that Trajectory has a number of methods named as if the independent variable is time -- start_time for instance. Open to discussion about how to resolve discussion of time here.

see discussion in the thread above.

common/trajectories/piecewise_constant_curvature_trajectory.h line 48 at r1 (raw file):

Previously, mshalm (Mathew Halm) wrote…

Yes -- per #22196 , this will be use to implement a Mobilizer and Joint which require CloneToScalar() functionality.

I see, thanks.
A historic note. CloneToScalar() are my doing in Drake, long before lots of the scalar conversion machinery was in place.
I honesly like how systems do this in terms of a scalar conversion constructor (nothing but a ctor templated on sclar type. So pretty much CloneToScalar with a different name). You might consider that more modern strategy instead.

common/trajectories/piecewise_constant_curvature_trajectory.h line 67 at r1 (raw file):

Previously, mshalm (Mathew Halm) wrote…

This is the segment break points common to all PiecewiseTrajectory classes. I can point users to the parent class.

yes, that'd do. If the docs can be stated simply in a few sentences though, it helps to provide locally.

Also what I like to do is to place most of the preliminaries in the class docs. Then you can simply refer to those docs to write more succint function docs.

common/trajectories/piecewise_constant_curvature_trajectory.h line 69 at r1 (raw file):

Previously, mshalm (Mathew Halm) wrote…

Correct

it might be worth choosing a notation in the calss docs and using that notation here for consistency and clarity.

common/trajectories/piecewise_constant_curvature_trajectory.h line 73 at r1 (raw file):

Previously, mshalm (Mathew Halm) wrote…

Yes, again by inheritance with PiecewiseTrajectory

Saying so would only take a line, and it would definitely not hurt referencing the specific doc in Trajectory.
Also, does this throw if that pre-condition is not met? that should show in these docs.

common/trajectories/piecewise_constant_curvature_trajectory.h line 74 at r1 (raw file):

Previously, mshalm (Mathew Halm) wrote…

Right hand rule -- per above comment I can clarify this in the class doc

yes, please provide all math and notation needed. Most users won't be able to guess it right.

common/trajectories/piecewise_constant_curvature_trajectory.h line 76 at r1 (raw file):

Previously, mshalm (Mathew Halm) wrote…

Per above comment it's not necessarily the binormal vector; could be the reflection of the binormal vector

I see that now, thanks. Maybe we can use the following notation?:

curve_tangent: for the tangent to the curve, same as Frenet's
curve_normal: for the normal to the curve, same as Frenet's
plane_normal: for the normal defining the curve's plane, not necessarily equal to the binormal.

Notice I removed _axis from the name to make it shorter and also because Vector3 already tells me this is a 3D vector. You could add it back, but it'd give you a longer name.

common/trajectories/piecewise_constant_curvature_trajectory.h line 76 at r1 (raw file):

Previously, mshalm (Mathew Halm) wrote…

Agreed to put in monogram notation, with explanation for the monogram in the docs

great. Feel free to use W, N,E or whatever name for the reference frame. I used W for "world" obvioulsy, but no need.

common/trajectories/piecewise_constant_curvature_trajectory.h line 81 at r1 (raw file):

Previously, mshalm (Mathew Halm) wrote…

Pattern matching PiecewisePose -- 4x4 comes from the Matrix representation of the spatial transform

oh, I see. You don't need to pattern match right?. It seems a bit useless in this case to have two methods doing the same.
I'd expect a class with name PieceWiseConstantCurvatureTrajectory to return r(s) upon calling value(). After all, you already have GetPose()

common/trajectories/piecewise_constant_curvature_trajectory.h line 94 at r1 (raw file):

Previously, mshalm (Mathew Halm) wrote…

I was pattern matching PiecewisePose but happy to do this if you insist

we can see what the platform reviewer says. IMO things in common/trajectories are a bit outdated in terms of our current desing patterns.

common/trajectories/piecewise_constant_curvature_trajectory.h line 94 at r1 (raw file):

Previously, mshalm (Mathew Halm) wrote…

Per above comments, it's not necessarily the TNB frame

right. The point stands. Define W, define W, document this returns X_WF

common/trajectories/piecewise_constant_curvature_trajectory.h line 103 at r1 (raw file):

Previously, mshalm (Mathew Halm) wrote…

Again pattern matching PiecewisePose here and happy to do something else if you feel strongly

not strong. But our rule of thumb as Drake devleopers is to break a pattern if we see something is out of place. Otherwise we keep spreading bad habits through the codebase.
In this case, this is not horribly, but I figured I'd ask in case we could do better for your application. After all, you are designing this class from the ground up and you have freedom to do it as you please.

common/trajectories/piecewise_constant_curvature_trajectory.h line 115 at r1 (raw file):

Previously, mshalm (Mathew Halm) wrote…

See previous comments on Trajectory implying time

Right. If we use Trajectory then SpatialVelocity is okay, documenting that we assumed ds/dt=1.
Otherwise, if the path length parameterization is what you mean, this more like the Darboux vector.

common/trajectories/piecewise_constant_curvature_trajectory.h line 121 at r1 (raw file):

Previously, mshalm (Mathew Halm) wrote…

See previous comments on pattern matching PiecewisePose

Same response. Pobably here CalcSpatialVelocity() is more appropriate.

mshalm

Reviewable status: 43 unresolved discussions, LGTM missing from assignee amcastro-tri, needs platform reviewer assigned, needs at least two assigned reviewers

a discussion (no related file):

Previously, amcastro-tri (Alejandro Castro) wrote…

Thanks @mshalm.

There may be some missing context

Thanks, I was aware of this as you brought it up during a Drake devs meeting. I love the schematics in the issue! very clear.

A number of design decisions mirror PiecewisePose<T> for instance the naming of GetXX

ah... that's unfortunate. I wonder if anyone ever consider even filing an issue for this. IMO PiecewisePose does not follow our naming conventions. Similarly I see PiecewisePolynomial has getPolynomial() which doesn't even follow our style guidelines. Therefore I'd go with CalcFoo() and break the pattern. We can also leave it as is and ask the platform reviewer when we assign one. Some pieces of Drake are from before our naming conventions.

The "curvature" here is not Frenet–Serret curvature exactly in that it can be negative and represents right-hand-rule turning

Ah, I see. In retrospect that only makes sense, otherwise curves would only wrap in one direction if the binormal vector was constant.
But to my point, there was no way I could've guessed without a careful scrutiny of the source, something a user shouldn't need to do. So yes, I agree with you in that careful notation and docs in the class are in order. I am not strong about using TNB terminology all along, but that's just my go-to model of curves in my head.

There is confusion about discussing time units.

I see two paths depending on what you need for your project. If what you need is a trajectory function of time, then I'd go with Trajectory<T>. If what you need is a path function of arch length, then maybe there is no good reason to inherit from Trajectory<T> in the first place? It doesn't even seem like you are gaining much from it right now.

Thanks again for the detailed comments. I have addressed them all in an upcoming revision which I will publish shortly.

common/trajectories/piecewise_constant_curvature_trajectory.h line 18 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

That makes total sense. Plese make sure this shows in your documentation.
I'd suggest you choose a good notation for your use case (I only suggested one here based on my first guess of what this class does). When choosing notation I'd make the following suggestions which usually help:

Since we write docs in ASCII, notation that can be written with unicode (highly recommend unicodeit tends to work best.

You might be inclined to write Latex in Doxygen. While that renders well in the web version, most of us read these docs straight from the source. Usually we only use Latex for very complex math only for which Unicode is far from being the right tool.

Since you have frames involved, notation that allows to use simple monogram symobls also eases things.

Majorly updated the class doc per your suggestions.

common/trajectories/piecewise_constant_curvature_trajectory.h line 21 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

did you push?

Not before, but I have done so now

common/trajectories/piecewise_constant_curvature_trajectory.h line 25 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

did you push? probably in other places as well since this is already the second one.

Done.

common/trajectories/piecewise_constant_curvature_trajectory.h line 26 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

see discussion in the thread above.

Resolved per offline discusison on time vs distance.

common/trajectories/piecewise_constant_curvature_trajectory.h line 48 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

I see, thanks.
A historic note. CloneToScalar() are my doing in Drake, long before lots of the scalar conversion machinery was in place.
I honesly like how systems do this in terms of a scalar conversion constructor (nothing but a ctor templated on sclar type. So pretty much CloneToScalar with a different name). You might consider that more modern strategy instead.

Done.

common/trajectories/piecewise_constant_curvature_trajectory.h line 67 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

yes, that'd do. If the docs can be stated simply in a few sentences though, it helps to provide locally.

Also what I like to do is to place most of the preliminaries in the class docs. Then you can simply refer to those docs to write more succint function docs.

Done.

common/trajectories/piecewise_constant_curvature_trajectory.h line 69 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

it might be worth choosing a notation in the calss docs and using that notation here for consistency and clarity.

Done -- I have chosen to keep the argument in human-readable format and reference the corresponding monogram in the doc.

common/trajectories/piecewise_constant_curvature_trajectory.h line 73 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

Saying so would only take a line, and it would definitely not hurt referencing the specific doc in Trajectory.
Also, does this throw if that pre-condition is not met? that should show in these docs.

Done.

common/trajectories/piecewise_constant_curvature_trajectory.h line 74 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

yes, please provide all math and notation needed. Most users won't be able to guess it right.

Done.

common/trajectories/piecewise_constant_curvature_trajectory.h line 76 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

I see that now, thanks. Maybe we can use the following notation?:

curve_tangent: for the tangent to the curve, same as Frenet's

curve_normal: for the normal to the curve, same as Frenet's

plane_normal: for the normal defining the curve's plane, not necessarily equal to the binormal.

Notice I removed _axis from the name to make it shorter and also because Vector3 already tells me this is a 3D vector. You could add it back, but it'd give you a longer name.

Agreed these are reasonable names and _axis is unnecessary

common/trajectories/piecewise_constant_curvature_trajectory.h line 76 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

great. Feel free to use W, N,E or whatever name for the reference frame. I used W for "world" obvioulsy, but no need.

Done, using A as the reference frame to avoid confusion when eventually embedding into MultibodyPlants.

common/trajectories/piecewise_constant_curvature_trajectory.h line 81 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

oh, I see. You don't need to pattern match right?. It seems a bit useless in this case to have two methods doing the same.
I'd expect a class with name PieceWiseConstantCurvatureTrajectory to return r(s) upon calling value(). After all, you already have GetPose()

Agreed -- rows, cols, and value correspondingly updated.

common/trajectories/piecewise_constant_curvature_trajectory.h line 94 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

we can see what the platform reviewer says. IMO things in common/trajectories are a bit outdated in terms of our current desing patterns.

Per high level discussion, I am no longer pattern matching and conforming to best practices.

common/trajectories/piecewise_constant_curvature_trajectory.h line 94 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

right. The point stands. Define W, define W, document this returns X_WF

Done

common/trajectories/piecewise_constant_curvature_trajectory.h line 103 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

not strong. But our rule of thumb as Drake devleopers is to break a pattern if we see something is out of place. Otherwise we keep spreading bad habits through the codebase.
In this case, this is not horribly, but I figured I'd ask in case we could do better for your application. After all, you are designing this class from the ground up and you have freedom to do it as you please.

Agreed to stop pattern matching and follow updated style guides.

common/trajectories/piecewise_constant_curvature_trajectory.h line 115 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

Right. If we use Trajectory then SpatialVelocity is okay, documenting that we assumed ds/dt=1.
Otherwise, if the path length parameterization is what you mean, this more like the Darboux vector.

Done -- documented as a trajectory with ds/dt = 1

common/trajectories/piecewise_constant_curvature_trajectory.h line 164 at r1 (raw file):

Previously, mshalm (Mathew Halm) wrote…

See previous comments on TNB frame, I will resolve the doc here based on our discussion about that.

Done

common/trajectories/piecewise_constant_curvature_trajectory.h line 203 at r1 (raw file):

Previously, mshalm (Mathew Halm) wrote…

See previous TNB frame discussion

Have changed the suffix to the suggested Index after some consideration.

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

Previously, amcastro-tri (Alejandro Castro) wrote…

looking at this construction, I realize now that segement_breaks are times? that's how it is for PiecewiseTrajectory at least.

I guess now I understand what you meant with unit velocity. Maybe state it as "We assume that the speed ṡ=1 all along the trajectory" somewhere in the constructor docs.

Clarified in class docs.

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

Previously, amcastro-tri (Alejandro Castro) wrote…

should we check these are all positive? is that a requirement?

Clarified that these can be negative in class docs

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

Previously, amcastro-tri (Alejandro Castro) wrote…

once you pick a notation for the entire class, update this monogram notation.

Done.

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

Previously, amcastro-tri (Alejandro Castro) wrote…

This is confusing, even if at this stage I reversed engineer everything I need.

Make the change distance -> time so that this line reads okay.

The docs should state this trajectory assumes ds/dt = 1, i.e. unit speed all along the trajectory.

Done.

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

Previously, amcastro-tri (Alejandro Castro) wrote…

not really, we don't need "time" to define the kinematics of a curve.

This essentially boils down to two ingredients, the Darboux vector and the fact you are assuming ds/dt=1. So using the chain rule, and since there is no angular component along tangent T, the angular velocity is ω = κ⋅B, with B the binormal vector.

Discussed in the class doc, with reference to the class doc added to this function.

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

Previously, amcastro-tri (Alejandro Castro) wrote…

with "almost verywhere" I imagine you mean the discontinuities at the breaks?

If so, I'd say, "Since velocity is piecewise constant, the acceleration is zero in each segment, with infinite acceleration at the breaks"

Clarified this discussion, including that we can defining the acceleration to be right continuous at the breaks by convention.

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

Previously, amcastro-tri (Alejandro Castro) wrote…

the centripetal acceleration is κ⋅ṡ² and we have ṡ² = 1. Add that to document your formula

Curve calculus clarified.

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

Previously, amcastro-tri (Alejandro Castro) wrote…

DRAKE_DEMAND(length >= 0)?
DRAKE_DEMAND(curvature >= 0)?

Per updated class doc, we do use this function for negative curvature (renamed to turning_rate) and length (when extrapolating before t_0).

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

Previously, amcastro-tri (Alejandro Castro) wrote…

if we use notation proposed in the header, you can write R_F0F

Done.

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

Previously, amcastro-tri (Alejandro Castro) wrote…

fyi, I didn't find these notes very useful.
I do realize however that really we need a schematic to explain them. Would you have a reference? that'd make thins easier for you.

If not, simply state these are the coordinates along x-y-z axis aligned with tangent-normal-binormal respectively. Since the curve lives in a plane defined by the binormal, the binormal coordinate is zero. The rest is probably easy to dervie once you understand the frame convention.

attempted to draw a schematic in the comments

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

Previously, amcastro-tri (Alejandro Castro) wrote…

Remove this. This will always be true given the constant values defined in the header.
I don't think this check is really chekcing anything. Also, it costs you, when all of this could be done at compiel time (don't do it though, I don't think there is a good reason. Just remove)

Done.

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

Previously, amcastro-tri (Alejandro Castro) wrote…

confusing to see "normal" in two names. I think we should use the term "binormal" for plane_normal_axis and "normal" for the curve's actual normal.

I'd also use monogram notation so that everything falls into place, i.e. tangent_W, binormal_W, normal_W, which the leads to rotation matrix R_WF (if we use F for the Frenet-Serret frame).

See previous discussion about naming conventions

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

Previously, amcastro-tri (Alejandro Castro) wrote…

I'd check this once in the constructor, and document this requirement publicly to the user.

I choose to just specify that MakeFromOrthonormalColumns may fail due to the complexity of asserting this for Expression scalar types.

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

Previously, amcastro-tri (Alejandro Castro) wrote…

you might want to use RotationMatrix::MakeFromOrthonormalColumns(), which has additional checks.

Done.

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

Previously, amcastro-tri (Alejandro Castro) wrote…

this leads me to believe that segment_breaks[0] = 0.0 always. Is that true? if so, it should be documented publicly somewhere.

It does not need to be 0, but my comments incorrectly suggested that it might need to be zero. I have now clarified this in various comments.

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

Previously, amcastro-tri (Alejandro Castro) wrote…

I suggest having num_segments() and num_breaks() as class functions. They can return int as needed.

That would not be useful here as it's a static function. Otherwise, num_segments and num_breaks are more appropriate methods for PiecewiseTrajectory which I will consider out of scope for this PR for now.

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

Previously, amcastro-tri (Alejandro Castro) wrote…

This assumes the continuity of t̂(s) (or T(s) if you prefer that notation, which is probably best for monogram). Please add this property to the class documentation.

Variable no longer needed

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

Previously, amcastro-tri (Alejandro Castro) wrote…

consider,

changed to segment_start_poses

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

Previously, amcastro-tri (Alejandro Castro) wrote…

nit,

per previous comments the function is static. Also std::vector length is unsigned so int causes a compilation error.

common/trajectories/piecewise_constant_curvature_trajectory.h line 39 at r1 (raw file):

   * Constructs an empty piecewise constant curvature trajectory.
   */
  PiecewiseConstantCurvatureTrajectory() {}

Done.

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

    const T& distance) const {
  int segment_index = this->get_segment_index(distance);
  math::RigidTransform<T> X_SiT =

Done.

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

  // build frames for the start of each segment.
  std::vector<math::RigidTransform<T>> segment_local_frames;
  segment_local_frames.reserve(num_segments);

Variable changed to be length num_segments as the last element wasn't needed

amcastro-tri

The new docs look beautiful @mshalm!

I added revised comments in the new header docs.

Per our Slack conversations. I am sorry I asked you (suggested?) to use time t consistently. Probably, and that depends on what you want to write later on for your new joint, the best thing to do here is to paremeterize by arch length rather than time.
I blieve that'll simplify documentation and avoid confusions later on (e.g. propagating terms that go with ṡ² later on, that could be missed if a user of the class thinks that's already taken into account.
If all docs are consistent and time does not show, there should be no confusion. Otherwise docs need to include awckward dislaimers about this point.

Reviewable status: 56 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) (waiting on @mshalm)

common/trajectories/piecewise_constant_curvature_trajectory.h line 68 at r2 (raw file):
similarly, no need to specify "expressed-in" frame, since the physics (well, kinematics in this case) is frame invariant.
We do need however to specify in what reference frame the derivative is taken.

That is, we write (taken Fx as an example):

ᴬdFx/dt = κᵢ⋅ŷ

See our docs on time derivatives and frames here.

Note: I personaly like how it reads without (t), but feel free to add it back.

Code quote:

 dx̂_F_A(t)/dt =

common/trajectories/piecewise_constant_curvature_trajectory.h line 22 at r2 (raw file):

 *
 * This class tracks X_AF(t), a pose F relative to a reference frame A as a
 * funciton of time t [s], with position r(t) = p_AoFo_A and R(t) = R_AF. The

typo.

Proof read the whole thing. There might be more.
Also, highly recommended to setup you editor to use spell check.

Code quote:

funciton

common/trajectories/piecewise_constant_curvature_trajectory.h line 22 at r2 (raw file):

 *
 * This class tracks X_AF(t), a pose F relative to a reference frame A as a
 * funciton of time t [s], with position r(t) = p_AoFo_A and R(t) = R_AF. The

nit,

Suggestion:

 with position r(t) = p_AoFo_A and orientation R(t) = R_AF. The

common/trajectories/piecewise_constant_curvature_trajectory.h line 23 at r2 (raw file):

 * This class tracks X_AF(t), a pose F relative to a reference frame A as a
 * funciton of time t [s], with position r(t) = p_AoFo_A and R(t) = R_AF. The
 * initial position r(t₀) is assumed to be equal to the zero vector. r(t) is the

just double checking with you. Is this requirement really needed?
And if you do have a requirement on the position at t0, do you have a similar requirement for the orientation?
IMO it'd be strange if one does and the other doesn't, but whicheve the choice, it should be documented.

Code quote:

initial position r(t₀) is assumed to be equal to the zero vector

common/trajectories/piecewise_constant_curvature_trajectory.h line 24 at r2 (raw file):

 * funciton of time t [s], with position r(t) = p_AoFo_A and R(t) = R_AF. The
 * initial position r(t₀) is assumed to be equal to the zero vector. r(t) is the
 * nominal value of the trajectory, and the full pose and its derivatives are

nit, r(t) was defined above, no need to redefine.

Suggestion:

 * initial position r(t₀) is assumed to be equal to the zero vector. 
 * The full pose and its derivatives are

common/trajectories/piecewise_constant_curvature_trajectory.h line 29 at r2 (raw file):

 *
 * The trajectory is arclength-parameterized; this means velocity along the
 * curve tangent is always 1 [m/s], and t̂(t) = dr(t)/dt is the curve's tangent

That's not entirely true.
This property is always true whenever ds/dt=1, regardless of how under the hood you parameterized the curve.

So I see two options:

You write a Trajectory<T> for piecewise archs with ds/dt=1 or,
You can always document this class as being an exception to the Trajectory<T> hierarchy (maybe with @warning in Doxygen) in that the parameter is arch length, not time.

Whether 1 or 2 is best, depends on your application and how you wrote the mobilizer code. Either one could work.
It'd seem you were looking to have

Code quote:

 * The trajectory is arclength-parameterized; this means velocity along the
 * curve tangent is always 1 [m/s], and t̂(t) = dr(t)/dt is the curve's tangent

common/trajectories/piecewise_constant_curvature_trajectory.h line 30 at r2 (raw file):

 * The trajectory is arclength-parameterized; this means velocity along the
 * curve tangent is always 1 [m/s], and t̂(t) = dr(t)/dt is the curve's tangent
 * unit vector (in the Frenet-Serrat sence).

typo

Suggestion:

Frenet−Serret

common/trajectories/piecewise_constant_curvature_trajectory.h line 34 at r2 (raw file):

 * The plane has a normal axis p̂. The curvatures are defined piecewise
 * leveraging the "segment times" concept from PiecewiseTrajectory. The segment
 * break times t₀ [s] < t₁ [s] < ... < tₙ [s] define the finite list of

I don't think you want to carry units all over here

Suggestion:

t₀ < t₁ < ... < tₙ

common/trajectories/piecewise_constant_curvature_trajectory.h line 35 at r2 (raw file):

 * leveraging the "segment times" concept from PiecewiseTrajectory. The segment
 * break times t₀ [s] < t₁ [s] < ... < tₙ [s] define the finite list of
 * points at which the curvature instantly changes. Like all PiecewiseTrajectory

Though I understand simply because I reviewed this, this text reads to me as if curvature wass defined at each break.
IMO easier to say that curvature is constant within each segment t ∈ [tᵢ, tᵢ₊₁)

Suggestion:

segments within which the curvature is constant.

common/trajectories/piecewise_constant_curvature_trajectory.h line 36 at r2 (raw file):

 * break times t₀ [s] < t₁ [s] < ... < tₙ [s] define the finite list of
 * points at which the curvature instantly changes. Like all PiecewiseTrajectory
 * types, tᵢ - tᵢ₋₁ must be at least PiecewiseTrajectory::kEpsilonTime. On

I'd remove this sentence. Here in the class focus on the math.
Then in the constructor you can specify the conditions for times to be within PiecewiseTrajectory::kEpsilonTime or the like. Otherwise this breaks the flow of your exposition.

Code quote:

 * points at which the curvature instantly changes. Like all PiecewiseTrajectory
 * types, tᵢ - tᵢ₋₁ must be at least PiecewiseTrajectory::kEpsilonTime. On

common/trajectories/piecewise_constant_curvature_trajectory.h line 37 at r2 (raw file):

 * points at which the curvature instantly changes. Like all PiecewiseTrajectory
 * types, tᵢ - tᵢ₋₁ must be at least PiecewiseTrajectory::kEpsilonTime. On
 * "segment" i, defined as [tᵢ₋₁, tᵢ], the curvature is parameterized by a

is this correct? per implementation I would've thought that the i-th segment actually is t ∈ [tᵢ, tᵢ₊₁).
That is, when we index segment_turning_rates_, don't we index from 0 to N-1 in the implemenation? just double checking with you. Or are you saying we discard segment_turning_rates_[0]?

That is, curvature with index i, κᵢ, i defined for segment i, which per the implementation seems to be t ∈ [tᵢ, tᵢ₊₁).

Code quote:

segment" i, defined as [tᵢ₋₁, tᵢ]

common/trajectories/piecewise_constant_curvature_trajectory.h line 38 at r2 (raw file):

 * types, tᵢ - tᵢ₋₁ must be at least PiecewiseTrajectory::kEpsilonTime. On
 * "segment" i, defined as [tᵢ₋₁, tᵢ], the curvature is parameterized by a
 * constant "turning rate" kᵢ [1/m], setting the angular velocity of F in W

used A at the top

Suggestion:

F in A

common/trajectories/piecewise_constant_curvature_trajectory.h line 46 at r2 (raw file):

 *   * kᵢ = 0 corresponds to a straight line segment.
 *   * kᵢ < 0 corresponds to a clockwise turn along a circular arc of radius
 * -1/kᵢ [m].

odd indenting

common/trajectories/piecewise_constant_curvature_trajectory.h line 58 at r2 (raw file):

 * continuously extrapolated with constant curvatures k_1 and k_N respectively.
 *
 * The orientation of the curve R(t) = [x̂_F_A(t), ŷ_F_A(t), ẑ_F_A(t)] rotates

The notation we adopted in Drake would be
Fx_A, Fy_A, Fz_A.

Also, usually we tend to avoid Unicode plus monogram together, since it doesn't render well. That is, we use Unicode as a cheap ASCII replacement for Latex, to write math usually in frame independent notation (invariant). And then we do use monogram using simple ASCII. Exceptions exist whenever convenient or some difficult concept cannot be expressed in this way. But for the most part we stick to this.

common/trajectories/piecewise_constant_curvature_trajectory.h line 64 at r2 (raw file):

 *   * x̂_F_A(t) = dr(t)/dt = t̂ is the curve tangent at time t
 *   * ẑ_F_A(t) = ẑ_F_A(0) = p̂, the plane normal. ẑ = sign(kᵢ) * b̂.
 *   * ŷ_F_A(t) = ẑ_F_A x x̂_F_A = sign(k) * n̂.

you can use frame independent notation here, no need to specify the "expressed-in" frame.
Same for all axes.

Suggestion:

 Fy(t)

common/trajectories/piecewise_constant_curvature_trajectory.h line 66 at r2 (raw file):

 *   * ŷ_F_A(t) = ẑ_F_A x x̂_F_A = sign(k) * n̂.
 *
 * From the Frenet-Serret formulas, we know:

nit, it'd be nice to have a reference here, probably the simple Wikipedia link is enough for our purposes.

Code quote:

he Frenet-Serret formulas

common/trajectories/piecewise_constant_curvature_trajectory.h line 66 at r2 (raw file):

 *   * ŷ_F_A(t) = ẑ_F_A x x̂_F_A = sign(k) * n̂.
 *
 * From the Frenet-Serret formulas, we know:

nit,

Suggestion:

For constant curvature paths on a plane, the Frenet–Serret formulas simplify to:

…length

mshalm

Addressed everything in rev 3

Per discussion I have moved to arclength parameterization with a @warning.
I have allowed for specification of a start position.
Typos and minor documentation issue have also been fixed.

Reviewable status: 51 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) (waiting on @mshalm)

common/trajectories/piecewise_constant_curvature_trajectory.h line 23 at r2 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

just double checking with you. Is this requirement really needed?
And if you do have a requirement on the position at t0, do you have a similar requirement for the orientation?
IMO it'd be strange if one does and the other doesn't, but whicheve the choice, it should be documented.

Not a necessary assumption -- I will remove it.

common/trajectories/piecewise_constant_curvature_trajectory.h line 24 at r2 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

nit, r(t) was defined above, no need to redefine.

I am not redefining, I am clarifying that it's r, not X_AF, available through value(). I have said this more directly in the updated doc.

common/trajectories/piecewise_constant_curvature_trajectory.h line 29 at r2 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

That's not entirely true.
This property is always true whenever ds/dt=1, regardless of how under the hood you parameterized the curve.

So I see two options:

You write a Trajectory<T> for piecewise archs with ds/dt=1 or,

You can always document this class as being an exception to the Trajectory<T> hierarchy (maybe with @warning in Doxygen) in that the parameter is arch length, not time.

Whether 1 or 2 is best, depends on your application and how you wrote the mobilizer code. Either one could work.
It'd seem you were looking to have

Agreed to do arclength parameterization explicitly and add a warning.

common/trajectories/piecewise_constant_curvature_trajectory.h line 34 at r2 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

I don't think you want to carry units all over here

Done.

common/trajectories/piecewise_constant_curvature_trajectory.h line 35 at r2 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

Though I understand simply because I reviewed this, this text reads to me as if curvature wass defined at each break.
IMO easier to say that curvature is constant within each segment t ∈ [tᵢ, tᵢ₊₁)

Done.

common/trajectories/piecewise_constant_curvature_trajectory.h line 36 at r2 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

I'd remove this sentence. Here in the class focus on the math.
Then in the constructor you can specify the conditions for times to be within PiecewiseTrajectory::kEpsilonTime or the like. Otherwise this breaks the flow of your exposition.

Done.

common/trajectories/piecewise_constant_curvature_trajectory.h line 37 at r2 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

is this correct? per implementation I would've thought that the i-th segment actually is t ∈ [tᵢ, tᵢ₊₁).
That is, when we index segment_turning_rates_, don't we index from 0 to N-1 in the implemenation? just double checking with you. Or are you saying we discard segment_turning_rates_[0]?

That is, curvature with index i, κᵢ, i defined for segment i, which per the implementation seems to be t ∈ [tᵢ, tᵢ₊₁).

Done.

common/trajectories/piecewise_constant_curvature_trajectory.h line 38 at r2 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

used A at the top

Done.

common/trajectories/piecewise_constant_curvature_trajectory.h line 46 at r2 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

odd indenting

Fixed.

common/trajectories/piecewise_constant_curvature_trajectory.h line 58 at r2 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

The notation we adopted in Drake would be
Fx_A, Fy_A, Fz_A.

Also, usually we tend to avoid Unicode plus monogram together, since it doesn't render well. That is, we use Unicode as a cheap ASCII replacement for Latex, to write math usually in frame independent notation (invariant). And then we do use monogram using simple ASCII. Exceptions exist whenever convenient or some difficult concept cannot be expressed in this way. But for the most part we stick to this.

Done.

common/trajectories/piecewise_constant_curvature_trajectory.h line 64 at r2 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

you can use frame independent notation here, no need to specify the "expressed-in" frame.
Same for all axes.

Done.

common/trajectories/piecewise_constant_curvature_trajectory.h line 68 at r2 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

similarly, no need to specify "expressed-in" frame, since the physics (well, kinematics in this case) is frame invariant.
We do need however to specify in what reference frame the derivative is taken.

That is, we write (taken Fx as an example):

ᴬdFx/dt = κᵢ⋅ŷ

See our docs on time derivatives and frames here.

Note: I personaly like how it reads without (t), but feel free to add it back.

Done.

amcastro-tri

Reviewable status: 3 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) (waiting on @mshalm)

a discussion (no related file):
Thre are a few things to iron out. I can do a push myself if that helps.
But I wanted to double check my understanding with you first, since I haven't seen your joint code.

Methods CalcSpatialVelocity() and CalcSpatialAcceleration() are a bit of a stretch, given there is no time involved here and without consider terms that go with ṡ, it'd be very easy for a user to get the math wrong.

This is what I suggest. I think it'd be clean, it'd make your joint code clear, and I believe accomplishes what you want much easier.

I'd write down the relationship between the trajectory parameterized by arclength and its derivatives. That is:

v = ᴬdr(t)/dt = ṡ(t)⋅t̂(t)
a = ᴬdv(t)/dt = d/dt(ṡ(t)⋅t̂(t)) = s̈⋅t̂ + ṡ²⋅dt̂/ds = s̈⋅t̂ + ṡ²⋅κ⋅n̂

From that math, it is clear all a user needs is t̂(s), and dt̂/ds(s). That alone allows to write the needed kinematics given s, ṡ and s̈.
Therefore, all this class would need to provide are methods like CalcTangent(), CalcTangentDerivative() (which would return dt̂/ds).

What do you think about this? I can push the changes along some minor edits if you agree.

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

Previously, mshalm (Mathew Halm) wrote…

It does not need to be 0, but my comments incorrectly suggested that it might need to be zero. I have now clarified this in various comments.

fyi, the constructor throws now if s == 0. I believe that's the right thing to do, but it'd seem to contradict this comment. Double checking with you.

common/trajectories/piecewise_constant_curvature_trajectory.h line 23 at r3 (raw file):

 * This class tracks X_AF(s), a pose F relative to a reference frame A as a
 * function of arclength s [m], with position r(s) = p_AoFo_A(s) and orientation
 * R(s) = R_AF(s). The initial position r(s₀) is provided at construction. r(s)

I wonder if we should say r(s=0) instead?

Code quote:

 r(s₀)

amcastro-tri

Reviewable status: 3 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) (waiting on @mshalm)

a discussion (no related file):

Previously, amcastro-tri (Alejandro Castro) wrote…

Thre are a few things to iron out. I can do a push myself if that helps.
But I wanted to double check my understanding with you first, since I haven't seen your joint code.

Methods CalcSpatialVelocity() and CalcSpatialAcceleration() are a bit of a stretch, given there is no time involved here and without consider terms that go with ṡ, it'd be very easy for a user to get the math wrong.

This is what I suggest. I think it'd be clean, it'd make your joint code clear, and I believe accomplishes what you want much easier.

I'd write down the relationship between the trajectory parameterized by arclength and its derivatives. That is:

v = ᴬdr(t)/dt = ṡ(t)⋅t̂(t)
a = ᴬdv(t)/dt = d/dt(ṡ(t)⋅t̂(t)) = s̈⋅t̂ + ṡ²⋅dt̂/ds = s̈⋅t̂ + ṡ²⋅κ⋅n̂

From that math, it is clear all a user needs is t̂(s), and dt̂/ds(s). That alone allows to write the needed kinematics given s, ṡ and s̈.
Therefore, all this class would need to provide are methods like CalcTangent(), CalcTangentDerivative() (which would return dt̂/ds).

What do you think about this? I can push the changes along some minor edits if you agree.

Note, dt̂/ds is the curvature vector. So maybe a better name for the API above would be CalcTangentAndCurvatureVectors(), which would compute both quantities at the same time taking advantage of a single segment lookup for both quantities.

amcastro-tri · 2024-12-04T20:16:42Z

@drake-jenkins-bot ok to test

mshalm

Reviewable status: 3 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)

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

Previously, amcastro-tri (Alejandro Castro) wrote…

fyi, the constructor throws now if s == 0. I believe that's the right thing to do, but it'd seem to contradict this comment. Double checking with you.

Yep -- this was from Rev 1, and in rev 3 upon our discussion I made the change to enforce segment_breaks[0] = 0.0 as part of making arclength the independent variable

…tory to time

mshalm

Reviewable status: 3 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)

a discussion (no related file):

Previously, amcastro-tri (Alejandro Castro) wrote…

Note, dt̂/ds is the curvature vector. So maybe a better name for the API above would be CalcTangentAndCurvatureVectors(), which would compute both quantities at the same time taking advantage of a single segment lookup for both quantities.

Per slack discussion, the derivatives are now with respect to time, and ṡ and s̈ are passed as arguments so that the functions can do chain rule. The motivation for doing so was to keep the curve calculus centralized within the class.

common/trajectories/piecewise_constant_curvature_trajectory.h line 23 at r3 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

I wonder if we should say r(s=0) instead?

I think it is fine either way, both are correct. To me either read as "start of the curve"

amcastro-tri

Reviewable status: 11 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) (waiting on @mshalm)

a discussion (no related file):
Since your branch is called "master" I was not able to push here directly without causing a local nigthmare.
So I opened a PR against your branch.
You'll either have to merge it or cherry-pick the single commit on top of your branch and the push here.

common/trajectories/piecewise_constant_curvature_trajectory.h line 31 at r5 (raw file):

 *
 * The trajectory is arclength-parameterized; thus t̂_A(s) = dr(s)/ds is the
 * curve's tangent unit vector (in the Frenet–Serret sense).

fyi, I don't think we mark this as a defect in general. But given Doxygen provides a few options to document code, over the years we started preferring this version without the asterisks at the beginning of the line. The reasons is that by using this format you gain two additional characters in you 80 characters long line, and it renders as nice anyways. I did this everywhere else.

common/trajectories/piecewise_constant_curvature_trajectory.h line 104 at r5 (raw file):

   * initial_curve_tangent as Fx_A(s₀); and plane_normal as the Fy_A(s); and
   * Fy(0) set by right-hand rule. Thus if the axis are not unit-norm and
   * orthogonal, construction of R_AF may fail; see MakeBaseFrame for details.

changed name. But most importantly, you cannot reference private methods in public docs.

Code quote:

MakeBaseFrame

common/trajectories/piecewise_constant_curvature_trajectory.h line 104 at r5 (raw file):

   * initial_curve_tangent as Fx_A(s₀); and plane_normal as the Fy_A(s); and
   * Fy(0) set by right-hand rule. Thus if the axis are not unit-norm and
   * orthogonal, construction of R_AF may fail; see MakeBaseFrame for details.

none of this is actually checked in release builds by RotationMatrix.
I fixed the code to throw accordingly.

Code quote:

   * Fy(0) set by right-hand rule. Thus if the axis are not unit-norm and
   * orthogonal, construction of R_AF may fail; see MakeBaseFrame for details.

common/trajectories/piecewise_constant_curvature_trajectory.h line 115 at r5 (raw file):

   * the parent frame, p_AoFo_A(s₀).
   *
   * @throws std::invalid_argument if the number of turning rates does not

Read style guide on exceptions

common/trajectories/piecewise_constant_curvature_trajectory.h line 166 at r5 (raw file):

   *
   * @param s The query arclength in meters.
   * @return The corresponding pose X_AF(s) as a RigidTransform.

here and in other places. Generally we do not state the type returned as part of the documentaiton. C++ is verbose enough that this information is conveyed by the type. We usually prefer use docs to document things which would otherwise not be possibly to convey from the language.

Code quote:

 as a RigidTransform.

common/trajectories/piecewise_constant_curvature_trajectory.h line 207 at r5 (raw file):

   * as approached from the right -- i.e. the acceleration is continuous on each
   * segment domain [sᵢ, sᵢ₊₁) (see implementation of
   * PiecewiseTrajectory::get_segment_index for convention on the segment index

it is not a good practice to document source code publicly. It breaks encapsulation (yes we do this in developers notes in the source code when appropriate)

Code quote:

   * segment domain [sᵢ, sᵢ₊₁) (see implementation of
   * PiecewiseTrajectory::get_segment_index for convention on the segment index

common/trajectories/piecewise_constant_curvature_trajectory.h line 236 at r5 (raw file):

   * @return The base pose as a RigidTransform.
   */
  const math::RigidTransform<T>& get_base_pose() const {

Two comments I wanted to run by you:

This will be ub whenever we use the default (empty trajectory) constructor. Do we need that constructor at all? or do we need this method at all?
A nit. If needed, consider get_initial_pose() or get_start_pose() since the word "base" doesn't show in public docs.

Code quote:

>& get_base_pose() c

common/trajectories/piecewise_constant_curvature_trajectory.h line 245 at r5 (raw file):

   * @return The plane normal axis, p̂_A.
   */
  const Eigen::Ref<const Vector3<T>> get_plane_normal() const {

ditto, ub when using default ctor.

Also, probably a @note or @warning stating that this might not be normalized.

common/trajectories/piecewise_constant_curvature_trajectory.h line 262 at r5 (raw file):

   */
  template <typename U>
  static std::vector<T> CloneSegmentDataFromScalar(

minor, consider something along the lines of ScalarConvertStdVector

amcastro-tri

Reviewable status: 11 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) (waiting on @mshalm)

a discussion (no related file):

Previously, amcastro-tri (Alejandro Castro) wrote…

Since your branch is called "master" I was not able to push here directly without causing a local nigthmare.
So I opened a PR against your branch.
You'll either have to merge it or cherry-pick the single commit on top of your branch and the push here.

btw, the last few comments I placed here are only for your reference to track changes in the PR I pushed. No action required.

Alejandro's minor edits

mshalm

Reviewable status: 11 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)

a discussion (no related file):

Previously, amcastro-tri (Alejandro Castro) wrote…

btw, the last few comments I placed here are only for your reference to track changes in the PR I pushed. No action required.

Finished some comments that weren't completed and made a couple more changes:

All references to "base pose" changed to "initial pose"
Turning rate symbol is now rho to differentiate from tau for torsion
get_initial_pose is now private and has a warning for lack of initialization. It is used for syntactic sugar internally in the class, and isn't strictly necessary outside of it.
get_plane_normal removed as it's not necessary for the joint.
CloneSegmentDataFromScalar renamed per suggestion

amcastro-tri

Reviewed 1 of 4 files at r1, 3 of 3 files at r6.
Reviewable status: 10 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 @mshalm)

a discussion (no related file):

Previously, mshalm (Mathew Halm) wrote…

Finished some comments that weren't completed and made a couple more changes:

All references to "base pose" changed to "initial pose"

Turning rate symbol is now rho to differentiate from tau for torsion

get_initial_pose is now private and has a warning for lack of initialization. It is used for syntactic sugar internally in the class, and isn't strictly necessary outside of it.

get_plane_normal removed as it's not necessary for the joint.

CloneSegmentDataFromScalar renamed per suggestion

This looks very good Mathew!
Only a few comments on the unit tests. The tests go beyond what we typically recommend, I marked where appropriate.
I'd at least get rid of the large usage of auto and reduce the excesive number of samples.
I'll leave other caveats like the use of randomly gnerated values to platform. I personally avoid random values generation in unit tests. It is best to judisiously pick a good representative set (small if possible) that tickles your implementation in the right places to provide appropriate coverage. I guess random values generation is okay if the seed is explicit? but I'll let plaform comment on that since I am not aware of that being a defect per se.

Thank you for this great piece of code!

FYI, I was thinking last night, if you do need get_initial_pose() and/or get_plane_normal(), I think it'd be okay to document with a precondition (@pre in Doxygen) that the trajectory must not be empty.
As an STL example, consider std::vector::back(), which it is documented to have undefined behavior if empty.

common/trajectories/test/piecewise_constant_curvature_test.cc line 56 at r6 (raw file):

  double r = 2;
  double kappa = 1 / r;
  int num_segments = 10000;

there is no reason to test a trajectory with 10k segments.
put it another way, where do you draw the limit? should we suse 3 segments, 10k segments, 1 billion segments? when is it enough.
The answer is, you pick the smallest set that excersices every code path in your code.
If I look at your code there are only two code paths, straight segments and curved segments.
So in principle, you'd be just fine with picking two values, say three if you want to play with positive and negative turning rates. There is no even need for random values, but precisely picked values designed to tickled your cde precisely where needed (rather than hoping an arbitrary seed does that for you).

Since you have this, probably a better compromise is to at least lower this number to something more reasonable, say 5 or 10 (no need to burn CI resources for this)

common/trajectories/test/piecewise_constant_curvature_test.cc line 76 at r6 (raw file):

    math::RigidTransformd expected_pose = math::RigidTransformd(
        math::RotationMatrixd::MakeZRotation(l * kappa),
        Vector3d(r * sin(l * kappa), r * (1 - cos(l * kappa)), 0));

a word of warning with this kind of test: you are running the risk of copy pasting your implementation to write your test. That is, if you made a mistake in your mathematical derivation, you'll make the same mistake in your code and in its test (this happned to me before :-( )

If nothing better, we can leave it as is. But keep that in mind for future unit tests.

common/trajectories/test/piecewise_constant_curvature_test.cc line 79 at r6 (raw file):

    multibody::SpatialVelocity<double> expected_velocity(
        s_dot * kappa * Vector3d::UnitZ(),
        s_dot * Vector3d(cos(l * kappa), sin(l * kappa), 0));

as a follow up to avoid copy/pasting possibly erroneous math. A cool way to get time derivatives is with AutoDiffXd!

common/trajectories/test/piecewise_constant_curvature_test.cc line 102 at r6 (raw file):

GTEST_TEST(TestPiecewiseConstantCurvatureTrajectory, TestRandomizedTrajectory) {
  std::default_random_engine generator(123);
  int num_segments = 10000;

ditto on arbitrarily ginormous number of segments.

common/trajectories/test/piecewise_constant_curvature_test.cc line 124 at r6 (raw file):

        cumulative_angles[i - 1] + Vector1d(segment_angles[i - 1]);
  }
  auto initial_quat = GenerateRandomQuaternion<double>(&generator);

ditto here, no need for something random. An arbitrary non-zero choice would tickle your code just fine. Say Quaternion<double>(1.0, 2.0, 3.0, 4.0) for instance.

Code quote:

 GenerateRandomQuaternion<double>(&generator);

common/trajectories/test/piecewise_constant_curvature_test.cc line 131 at r6 (raw file):

  auto angle_spline =
      PiecewisePolynomial<double>::FirstOrderHold(breaks, cumulative_angles);
  PiecewiseConstantCurvatureTrajectory<double> curve_spline(

minor,

spline? maybe just curve or trajectory?

ditto for angle_spline. Maybe angle_trajectory?

Code quote:

curve_spline(

common/trajectories/test/piecewise_constant_curvature_test.cc line 137 at r6 (raw file):

  // Check dense interpolated quaternions.
  for (double l = breaks.front(); l < breaks.back(); l += 0.01) {
    auto curve_pose = curve_spline.CalcPose(l);

fyi, per Drake's style guide, "use type deduction only to make the code clearer or safer, and do not use it merely to avoid the inconvenience of writing an explicit type.".

common/trajectories/test/piecewise_constant_curvature_test.cc line 152 at r6 (raw file):

    auto relative_rotation_expected = drake::math::RotationMatrixd(
        AngleAxis<double>(angle_spline.value(l)(0, 0), initial_plane_normal));
    auto curve_rotation_expected =

nit,

more occurrences of auto. At this scopy, I already forgot what the previous types where.

common/trajectories/test/piecewise_constant_curvature_test.cc line 185 at r6 (raw file):

  double r = 2;
  double kappa = 1 / r;
  int num_segments = 10000;

do we need 10k samples to test periodicity? a simple circle or a box with rounded corners would do, no?

common/trajectories/piecewise_constant_curvature_trajectory.h line 98 at r6 (raw file):

   the parent frame, p_AoFo_A(s₀).

   @throws std::invalid_argument if the number of turning rates does not match

minor, per Drake's style guide

Suggestion:

std::exception

amcastro-tri

Reviewable status: 10 unresolved discussions, LGTM missing from assignee ggould-tri(platform), commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @mshalm)

a discussion (no related file):

Previously, amcastro-tri (Alejandro Castro) wrote…

This looks very good Mathew!
Only a few comments on the unit tests. The tests go beyond what we typically recommend, I marked where appropriate.
I'd at least get rid of the large usage of auto and reduce the excesive number of samples.
I'll leave other caveats like the use of randomly gnerated values to platform. I personally avoid random values generation in unit tests. It is best to judisiously pick a good representative set (small if possible) that tickles your implementation in the right places to provide appropriate coverage. I guess random values generation is okay if the seed is explicit? but I'll let plaform comment on that since I am not aware of that being a defect per se.

Thank you for this great piece of code!

FYI, I was thinking last night, if you do need get_initial_pose() and/or get_plane_normal(), I think it'd be okay to document with a precondition (@pre in Doxygen) that the trajectory must not be empty.
As an STL example, consider std::vector::back(), which it is documented to have undefined behavior if empty.

+@ggould-tri for platform review please

ggould-tri

; minor formatting and naming notes. Thank you!

Reviewed 1 of 4 files at r1, 3 of 3 files at r6, all commit messages.
Reviewable status: 18 unresolved discussions, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @mshalm)

common/trajectories/BUILD.bazel line 304 at r6 (raw file):

drake_cc_googletest(
    name = "piecewise_constant_curvature_test",

minor: Our tests generally mirror the name of the header they are testing, replacing .h with _test.cc. So the test file and this rule should be renamed.

Suggestion:

    name = "piecewise_constant_curvature_trajectory_test",

common/trajectories/test/piecewise_constant_curvature_test.cc line 25 at r6 (raw file):

                                          std::default_random_engine* generator,
                                          double max_angle = (10 * M_PI)) {
  int size = breaks.size() - 1;

BTW: Here and throughout the test, you might consider using const for some of these locals -- you have a mix of constant and non-constant temporaries, and it's easy to make a subtle mistake.

This is not required by our conventions, and is at your discretion.

common/trajectories/test/piecewise_constant_curvature_test.cc line 35 at r6 (raw file):

      turning_rates[i] = T(0);
    } else {
      // pick a turning rate so the angle change on the segment is in (-pi, pi)

typo Complete sentences in comments should be capitalized and punctuated.

Suggestion:

      // Pick a turning rate so the angle change on the segment is in (-pi, pi).

common/trajectories/piecewise_constant_curvature_trajectory.cc line 93 at r6 (raw file):

  // The spatial acceleration is the time derivative of the spatial velocity.
  // We compute the acceleration by applying the chain rule to the formulas
  // in CalcSpatialVelocity.

minor: Generally we put comments describing a variable before the variable declaration.

Suggestion:

  // The spatial acceleration is the time derivative of the spatial velocity.
  // We compute the acceleration by applying the chain rule to the formulas
  // in CalcSpatialVelocity.
  multibody::SpatialAcceleration<T> spatial_acceleration;

common/trajectories/piecewise_constant_curvature_trajectory.cc line 123 at r6 (raw file):

    const T& rho_i, const T& ds) {
  Vector3<T> p_FioFo_Fi = Vector3<T>::Zero();
  // Calculate rotation angle

typo Complete sentences in comments should be capitalized and punctuated.

Suggestion:

  // Calculate rotation angle.

common/trajectories/piecewise_constant_curvature_trajectory.cc line 182 at r6 (raw file):

  const size_t num_segments = num_breaks - 1;

  // calculate angular change and length of each segment.

typo Complete sentences in comments should be capitalized and punctuated.

Suggestion:

  // Calculate angular change and length of each segment.

common/trajectories/piecewise_constant_curvature_trajectory.cc line 188 at r6 (raw file):

      breaks_eigen.tail(num_segments) - breaks_eigen.head(num_segments);

  // build frames for the start of each segment.

typo Complete sentences in comments should be capitalized and punctuated.

Suggestion:

  // Build frames for the start of each segment.

common/trajectories/piecewise_constant_curvature_trajectory.h line 62 at r6 (raw file):

 curve.

  @tparam_default_scalar */

BTW: This is an excellent comment; thank you.

[trajectories] Add PiecewiseConstantCuravtureTrajectory

94f1d9b

Adding a trajectory type modeling concatenation of linear segments and circular arcs within a plane, to be used in curvilinear joints (RobotLocomotion#22196).

rpoyner-tri assigned amcastro-tri Nov 19, 2024

rpoyner-tri reviewed Nov 19, 2024

View reviewed changes

rpoyner-tri added the release notes: feature This pull request contains a new feature label Nov 19, 2024

rpoyner-tri reviewed Nov 19, 2024

View reviewed changes

amcastro-tri reviewed Nov 26, 2024

View reviewed changes

mshalm commented Nov 26, 2024

View reviewed changes

amcastro-tri reviewed Nov 27, 2024

View reviewed changes

mshalm commented Nov 29, 2024

View reviewed changes

address comments on PR rev 1

6762785

amcastro-tri reviewed Dec 3, 2024

View reviewed changes

fine tune documentation; switch independent variable semantics to arc…

2568578

…length

mshalm commented Dec 4, 2024

View reviewed changes

mshalm changed the title ~~[trajectories] Add PiecewiseConstantCuravtureTrajectory~~ [trajectories] Add PiecewiseConstantCurvatureTrajectory Dec 4, 2024

amcastro-tri reviewed Dec 4, 2024

View reviewed changes

mshalm commented Dec 4, 2024

View reviewed changes

change variable of differentiation on PiecewiseContantCurvatureTrajec…

73a14c0

…tory to time

mshalm commented Dec 5, 2024

View reviewed changes

typo fix

9c110da

amcastro-tri reviewed Dec 9, 2024

View reviewed changes

amcastro-tri and others added 3 commits December 9, 2024 17:47

Alejandro's minor edits

d7e8046

Merge pull request #1 from amcastro-tri/alejandros_minor_edits

12e00f2

Alejandro's minor edits

Merge branch 'RobotLocomotion:master' into master

2b4c5a8

tweak documentation and variable names

1cb96bf

mshalm commented Dec 9, 2024

View reviewed changes

amcastro-tri reviewed Dec 10, 2024

View reviewed changes

amcastro-tri assigned ggould-tri Dec 10, 2024

amcastro-tri reviewed Dec 10, 2024

View reviewed changes

ggould-tri reviewed Dec 10, 2024

View reviewed changes

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

[trajectories] Add PiecewiseConstantCurvatureTrajectory #22214

[trajectories] Add PiecewiseConstantCurvatureTrajectory #22214

mshalm commented Nov 19, 2024 •

edited by sherm1

Loading

rpoyner-tri left a comment

mshalm commented Nov 19, 2024

rpoyner-tri left a comment

sherm1 commented Nov 25, 2024

amcastro-tri left a comment

amcastro-tri left a comment

amcastro-tri left a comment

mshalm left a comment

amcastro-tri left a comment

mshalm left a comment

amcastro-tri left a comment

mshalm left a comment

amcastro-tri left a comment

amcastro-tri left a comment

amcastro-tri commented Dec 4, 2024

mshalm left a comment

mshalm left a comment

amcastro-tri left a comment

amcastro-tri left a comment

mshalm left a comment

amcastro-tri left a comment

amcastro-tri left a comment

ggould-tri left a comment

[trajectories] Add PiecewiseConstantCurvatureTrajectory #22214

Are you sure you want to change the base?

[trajectories] Add PiecewiseConstantCurvatureTrajectory #22214

Conversation

mshalm commented Nov 19, 2024 • edited by sherm1 Loading

rpoyner-tri left a comment

Choose a reason for hiding this comment

mshalm commented Nov 19, 2024

rpoyner-tri left a comment

Choose a reason for hiding this comment

sherm1 commented Nov 25, 2024

amcastro-tri left a comment

Choose a reason for hiding this comment

amcastro-tri left a comment

Choose a reason for hiding this comment

amcastro-tri left a comment

Choose a reason for hiding this comment

mshalm left a comment

Choose a reason for hiding this comment

amcastro-tri left a comment

Choose a reason for hiding this comment

mshalm left a comment

Choose a reason for hiding this comment

amcastro-tri left a comment

Choose a reason for hiding this comment

mshalm left a comment

Choose a reason for hiding this comment

amcastro-tri left a comment

Choose a reason for hiding this comment

amcastro-tri left a comment

Choose a reason for hiding this comment

amcastro-tri commented Dec 4, 2024

mshalm left a comment

Choose a reason for hiding this comment

mshalm left a comment

Choose a reason for hiding this comment

amcastro-tri left a comment

Choose a reason for hiding this comment

amcastro-tri left a comment

Choose a reason for hiding this comment

mshalm left a comment

Choose a reason for hiding this comment

amcastro-tri left a comment

Choose a reason for hiding this comment

amcastro-tri left a comment

Choose a reason for hiding this comment

ggould-tri left a comment

Choose a reason for hiding this comment

mshalm commented Nov 19, 2024 •

edited by sherm1

Loading