Automotive TrajectoryAgents #8725

jadecastro · 2018-05-01T02:13:28Z

Toward #8529.

This change is

jadecastro · 2018-05-01T02:14:41Z

+@stonier feature review.

Review status: 0 of 4 files reviewed at latest revision, all discussions resolved.

Comments from Reviewable

stonier · 2018-05-03T06:26:43Z

automotive/trajectory_agent.h

+/// * velocity
+///   (OutputPort getter: raw_pose_output())
+///
+/// output port 1: A PoseVector containing X_WC, where C is the car frame.


Agent, !Car -> X_WA?

stonier · 2018-05-04T06:33:23Z

pending conflict fixes / CI resolution

Comments from Reviewable

jadecastro · 2018-05-04T13:07:45Z

+@sherm1 for platform review please.

Review status: 0 of 5 files reviewed at latest revision, all discussions resolved, some commit checks failed.

Comments from Reviewable

jadecastro · 2018-05-04T13:10:38Z

BTW, I’m not entirely sure what to make of the CI error. It looks like a pybind thing but not sure.
@EricCousineau-TRI do you have any thoughts on how to fix this?

Review status: 0 of 5 files reviewed at latest revision, all discussions resolved, some commit checks failed.

Comments from Reviewable

EricCousineau-TRI · 2018-05-04T14:29:04Z

I don't think it's pybind-related (whew!), but rather related to this PR: #8664

Per the fixes Jeremy submitted in #8726, I think you'd add the target to the dependency list:

error: Missing deps in //automotive's drake_cc_package_library.
note: In the automotive/BUILD.bazel rule for drake_cc_package_library, add the following lines to the deps:
        ":trajectory_agent",

jadecastro · 2018-05-04T14:52:05Z

Ah, 🤦, thanks. I should have looked at the updated BUILD file. It's clear to me now.

Review status: 0 of 5 files reviewed at latest revision, all discussions resolved, some commit checks failed.

Comments from Reviewable

sherm1 · 2018-05-05T01:21:18Z

Platform pending some BTWs.

Reviewed 1 of 4 files at r1, 2 of 3 files at r2, 2 of 2 files at r3.
Review status: all files reviewed at latest revision, all discussions resolved.

automotive/trajectory_agent.h, line 34 at r3 (raw file):

/// taking as input an AgentData structure and an AgentTrajectory.
///
/// output port 0:

BTW consider saying this is a SimpleCarState.

automotive/trajectory_agent.h, line 38 at r3 (raw file):

///   heading is 0 rad when pointed +x, pi/2 rad when pointed +y;
///   heading is defined around the +z axis, so positive-turn-left
/// * velocity

BTW is this ẋ ẏ heading_dot? (consider clarifying)

automotive/trajectory_agent.h, line 45 at r3 (raw file):

///   (OutputPort getter: pose_output())
///
/// output port 2: A FrameVelocity containing Xdot_WA, where A is the agent's

BTW this is not correct (and I see FrameVelocity's documentation is wrong too :( ). What you are outputting here is the spatial velocity of A in W, which we write V_WA. That is not the same thing as Xdot_WA, which literally means d/dt X_WA. The derivative of a vector or matrix quantity is an identically-shaped quantity whose elements are the time derivatives of the original elements. That is definitely not true here since (1) a PoseVector has 7 elements while FrameVelocity has 6, and (I hate to say this) (2) the rotational and translational elements are flipped around (!!).

automotive/trajectory_agent.h, line 47 at r3 (raw file):

/// output port 2: A FrameVelocity containing Xdot_WA, where A is the agent's
/// reference frame.
///   (OutputPort getter: velocity_output())

BTW why do you use FrameVelocity here rather than SpatialVelocity? I notice you use PoseVelocity elsewhere and that uses SpatialVelocity.

automotive/trajectory_agent.h, line 66 at r3 (raw file):

  /// @param trajectory an AgentTrajectory containing the trajectory.
  /// @param sampling_time_sec the requested sampling time (in sec) for this
  /// system.  @default 0.01.

BTW does this format correctly in doxygen?

automotive/trajectory_agent.h, line 76 at r3 (raw file):

      : TrajectoryAgent<T>(other.agent_data_, other.trajectory_) {}

  /// Accessors for the outputs, as enumerated in the class documentation.

BTW you need an @name here for grouping to work in doxygen.

Please generate the doxygen for this PR and inspect it to make sure it is formatted as you expected (and confirm here that it is).

automotive/trajectory_agent.h, line 103 at r3 (raw file):

  // FrameVelocity output.
  void CalcVelocityOutput(const systems::Context<T>& context,
                          systems::rendering::FrameVelocity<T>* velocity) const;

BTW could this be SpatialVelocity? Then you can just copy it directly out of PoseVelocity.

automotive/trajectory_agent.cc, line 40 at r3 (raw file):

void TrajectoryAgent<T>::CalcPoseOutput(const systems::Context<T>& context,
                                        PoseVector<T>* pose) const {
  const PoseVelocity values = GetValues(context);

BTW I see that PoseVelocity isn't templatized. Does that mean the derivatives produced when T=AutoDiff are wrong? If so, please document. Otherwise please add a comment (not sure if this is the right place) explaining why everything is still good.

automotive/test/trajectory_agent_test.cc, line 57 at r3 (raw file):

  const std::vector<TestCase> test_cases{
      {0., 1.},           {M_PI_2, 1.},      {-M_PI_2, 1.}, {0.125 * M_PI, 10.},
      {0.125 * M_PI, 1.}, {0.125 * M_PI, 1.}};

FYI spacing seems odd.

Comments from Reviewable

jadecastro · 2018-05-07T02:12:29Z

Comments addressed.

Review status: 3 of 5 files reviewed at latest revision, 9 unresolved discussions.

automotive/trajectory_agent.h, line 34 at r3 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW consider saying this is a SimpleCarState.

Done.

automotive/trajectory_agent.h, line 38 at r3 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW is this ẋ ẏ heading_dot? (consider clarifying)

Done.

automotive/trajectory_agent.h, line 45 at r3 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW this is not correct (and I see FrameVelocity's documentation is wrong too :( ). What you are outputting here is the spatial velocity of A in W, which we write V_WA. That is not the same thing as Xdot_WA, which literally means d/dt X_WA. The derivative of a vector or matrix quantity is an identically-shaped quantity whose elements are the time derivatives of the original elements. That is definitely not true here since (1) a PoseVector has 7 elements while FrameVelocity has 6, and (I hate to say this) (2) the rotational and translational elements are flipped around (!!).

Done.

automotive/trajectory_agent.h, line 47 at r3 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW why do you use FrameVelocity here rather than SpatialVelocity? I notice you use PoseVelocity elsewhere and that uses SpatialVelocity.

I'm using FrameVelocity simply because I want all outputs to be Vector-based, as opposed to Abstract outputs. SpatialVelocity does not specialize BasicVector, so instead I'm using its BasicVector container (FrameVelocity).

automotive/trajectory_agent.h, line 66 at r3 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW does this format correctly in doxygen?

Here's the result:

automotive/trajectory_agent.h, line 76 at r3 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW you need an @name here for grouping to work in doxygen.

Please generate the doxygen for this PR and inspect it to make sure it is formatted as you expected (and confirm here that it is).

Done. The @name keyword fixed the formatting. Thanks!

automotive/trajectory_agent.h, line 103 at r3 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW could this be SpatialVelocity? Then you can just copy it directly out of PoseVelocity.

(see above for the FrameVelocity rationale)

automotive/trajectory_agent.cc, line 40 at r3 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW I see that PoseVelocity isn't templatized. Does that mean the derivatives produced when T=AutoDiff are wrong? If so, please document. Otherwise please add a comment (not sure if this is the right place) explaining why everything is still good.

Done. (I've added a note within the class documentation to clarify.)

automotive/test/trajectory_agent_test.cc, line 57 at r3 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

FYI spacing seems odd.

Done. (I think this was clang-format's fault)

Comments from Reviewable

sherm1 · 2018-05-07T02:52:10Z

Reviewed 2 of 2 files at r4.
Review status: all files reviewed at latest revision, all discussions resolved, some commit checks pending.

automotive/trajectory_agent.h, line 41 at r4 (raw file):

///   heading is 0 rad when pointed +x, pi/2 rad when pointed +y;
///   heading is defined around the +z axis, positive-left-turn.
/// * speed: ṡ = √{ẋ² + ẏ²}

BTW ṡ -> s ? (Looks more like an acceleration if it is speed_dot.)

Comments from Reviewable

jadecastro · 2018-05-07T09:46:12Z

Review status: 4 of 5 files reviewed at latest revision, 1 unresolved discussion.

automotive/trajectory_agent.h, line 41 at r4 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW ṡ -> s ? (Looks more like an acceleration if it is speed_dot.)

Done.

Comments from Reviewable

jadecastro · 2018-05-07T11:31:11Z

@drake-jenkins-bot linux-xenial-clang-bazel-experimental-cache please

jadecastro assigned stonier May 1, 2018

stonier reviewed May 3, 2018

View reviewed changes

jadecastro force-pushed the auto-traj-agents branch 3 times, most recently from 9e71bd0 to 7517361 Compare May 4, 2018 02:00

jadecastro force-pushed the auto-traj-agents branch from 7517361 to db88b42 Compare May 4, 2018 10:55

jadecastro assigned sherm1 May 4, 2018

jadecastro force-pushed the auto-traj-agents branch from db88b42 to eac0f72 Compare May 4, 2018 14:57

jadecastro force-pushed the auto-traj-agents branch from eac0f72 to ffb73a7 Compare May 7, 2018 01:58

Add TrajectoryAgent

a48936d

jadecastro force-pushed the auto-traj-agents branch from ffb73a7 to a48936d Compare May 7, 2018 09:45

jadecastro merged commit ac1b321 into RobotLocomotion:master May 7, 2018

jadecastro deleted the auto-traj-agents branch May 7, 2018 12:37

jadecastro mentioned this pull request May 7, 2018

Trajectory- and waypoint-following agents #8529

Closed

4 tasks

stonier mentioned this pull request May 9, 2018

Add all_cars_in_dragway python demo; Remove C++ automotive_demo maliput/delphyne#396

Merged

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Automotive TrajectoryAgents #8725

Automotive TrajectoryAgents #8725

jadecastro commented May 1, 2018 •

edited by sherm1

Loading

jadecastro commented May 1, 2018

stonier May 3, 2018

stonier commented May 4, 2018 •

edited

Loading

jadecastro commented May 4, 2018

jadecastro commented May 4, 2018

EricCousineau-TRI commented May 4, 2018

jadecastro commented May 4, 2018

sherm1 commented May 5, 2018

jadecastro commented May 7, 2018

sherm1 commented May 7, 2018

jadecastro commented May 7, 2018

jadecastro commented May 7, 2018

Automotive TrajectoryAgents #8725

Automotive TrajectoryAgents #8725

Conversation

jadecastro commented May 1, 2018 • edited by sherm1 Loading

jadecastro commented May 1, 2018

stonier May 3, 2018

Choose a reason for hiding this comment

stonier commented May 4, 2018 • edited Loading

jadecastro commented May 4, 2018

jadecastro commented May 4, 2018

EricCousineau-TRI commented May 4, 2018

jadecastro commented May 4, 2018

sherm1 commented May 5, 2018

jadecastro commented May 7, 2018

sherm1 commented May 7, 2018

jadecastro commented May 7, 2018

jadecastro commented May 7, 2018

jadecastro commented May 1, 2018 •

edited by sherm1

Loading

stonier commented May 4, 2018 •

edited

Loading