Fix for SchunkWsgTrajectoryGenerator failing on delta=0

[RussTedrake]

There was a particular set of initial conditions which could cause the trajectory generator to fail, because it passed times=[0, 0, 0] to the PiecewisePolynomial.FirstOrderHold, which then complained about non-increasing times.

This PR adds a test which captures the failure, and the fix.

---

This change is 

[siyuanfeng-tri]

+@siyuanfeng-tri

Reviewed 2 of 2 files at r1, all commit messages.
Reviewable status: LGTM missing from assignee siyuanfeng-tri, needs platform reviewer assigned, needs at least two assigned reviewers, missing label for release notes (waiting on @RussTedrake)

[siyuanfeng-tri]

Reviewable status: needs platform reviewer assigned, needs at least two assigned reviewers, missing label for release notes (waiting on @RussTedrake)

[jwnimmer-tri]

platform +@jwnimmer-tri +(priority: high).

Reviewed 2 of 2 files at r1, all commit messages.
Reviewable status: 2 unresolved discussions

---

manipulation/schunk_wsg/schunk_wsg_trajectory_generator.cc line 138 at r1 (raw file):

  } else if (delta < kDistanceToMaxVelocity * 2) {
    // If we can't accelerate to our maximum (and decelerate again)
    // within the target travel distance, calculate the peak velocity

BTW Possibly not worth changing, but the return is somewhat hiding inside this big block of stuff, easily missed, especially with the else if implying that the prior if will fallthrough.

Suggestion:

  const double delta = std::abs(target_position - cur_position);
  if (delta == 0) {
    // Create the constant trajectory.
    trajectory_.reset(new trajectories::PiecewisePolynomial<double>(knots[0]));
    return;
  }

  // The trajectory creation code below is, to say the best, a bit
  // primitive.  I (sam.creasey) would not be surprised if it could
  // be significantly improved.  It's also based only on the
  // configurable constants for the WSG 50, not on analysis of the
  // actual motion of the gripper.
  if (delta < kDistanceToMaxVelocity * 2) {
    // If we can't accelerate to our maximum (and decelerate again)
    // within the target travel distance, calculate the peak velocity

---

manipulation/schunk_wsg/test/schunk_wsg_trajectory_generator_test.cc line 58 at r1 (raw file):

  const double pos = 0.06;
  dut.get_desired_position_input_port().FixValue(context.get(), pos);
  dut.get_force_limit_input_port().FixValue(context.get(), 40.);

nit GSG 40.0 or 40

[RussTedrake]

Reviewable status: complete! all discussions resolved, LGTM from assignees jwnimmer-tri(platform),siyuanfeng-tri

---

manipulation/schunk_wsg/schunk_wsg_trajectory_generator.cc line 138 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

BTW Possibly not worth changing, but the return is somewhat hiding inside this big block of stuff, easily missed, especially with the else if implying that the prior if will fallthrough.

Done.

---

manipulation/schunk_wsg/test/schunk_wsg_trajectory_generator_test.cc line 58 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

nit GSG 40.0 or 40

Done.