Automotive TrajectoryAgents (RobotLocomotion#8725)

* Add TrajectoryAgent

automotive/BUILD.bazel

@@ -53,6 +53,7 @@ drake_cc_package_library(
         ":road_path",
         ":simple_car",
         ":simple_powertrain",
+        ":trajectory_agent",
         ":trajectory_car",
         ":trivial_right_of_way_state_provider",
     ],
@@ -395,6 +396,21 @@ drake_cc_library(
     ],
 )
 
+drake_cc_library(
+    name = "trajectory_agent",
+    srcs = ["trajectory_agent.cc"],
+    hdrs = ["trajectory_agent.h"],
+    deps = [
+        ":agent_trajectory",
+        ":generated_vectors",
+        "//common:autodiff",
+        "//common:extract_double",
+        "//systems/framework:leaf_system",
+        "//systems/rendering:frame_velocity",
+        "//systems/rendering:pose_vector",
+    ],
+)
+
 drake_cc_library(
     name = "trajectory_car",
     srcs = ["trajectory_car.cc"],
@@ -737,6 +753,16 @@ drake_cc_googletest(
     ],
 )
 
+drake_cc_googletest(
+    name = "trajectory_agent_test",
+    deps = [
+        "//automotive:trajectory_agent",
+        "//common:extract_double",
+        "//systems/analysis:simulator",
+        "//systems/framework/test_utilities",
+    ],
+)
+
 drake_cc_googletest(
     name = "trajectory_car_test",
     deps = [

automotive/test/trajectory_agent_test.cc

@@ -0,0 +1,156 @@
+#include "drake/automotive/trajectory_agent.h"
+
+#include <vector>
+
+#include <gtest/gtest.h>
+
+#include "drake/common/extract_double.h"
+#include "drake/systems/analysis/simulator.h"
+#include "drake/systems/framework/test_utilities/scalar_conversion.h"
+
+namespace drake {
+namespace automotive {
+namespace {
+
+static constexpr double kTol = 1e-12;
+static constexpr double kSpeed = 10.;
+
+using systems::rendering::FrameVelocity;
+using systems::rendering::PoseVector;
+
+GTEST_TEST(TrajectoryAgentTest, Topology) {
+  const std::vector<double> times{0., 1.};
+  const std::vector<Eigen::Isometry3d> poses{Eigen::Isometry3d::Identity(),
+                                             Eigen::Isometry3d::Identity()};
+  const TrajectoryAgent<double> car(CarAgent(),
+                                    AgentTrajectory::Make(times, poses));
+
+  ASSERT_EQ(0, car.get_num_input_ports());
+  ASSERT_EQ(3, car.get_num_output_ports());
+
+  const auto& state_output = car.raw_pose_output();
+  EXPECT_EQ(systems::kVectorValued, state_output.get_data_type());
+  EXPECT_EQ(SimpleCarStateIndices::kNumCoordinates, state_output.size());
+
+  const auto& pose_output = car.pose_output();
+  EXPECT_EQ(systems::kVectorValued, pose_output.get_data_type());
+  EXPECT_EQ(PoseVector<double>::kSize, pose_output.size());
+
+  const auto& velocity_output = car.velocity_output();
+  EXPECT_EQ(systems::kVectorValued, velocity_output.get_data_type());
+  EXPECT_EQ(FrameVelocity<double>::kSize, velocity_output.size());
+
+  ASSERT_FALSE(car.HasAnyDirectFeedthrough());
+}
+
+GTEST_TEST(TrajectoryAgentTest, Outputs) {
+  using std::cos;
+  using std::sin;
+
+  struct TestCase {
+    double heading;
+    double distance;
+  };
+
+  const std::vector<TestCase> test_cases{
+      {0., 1.},             // BR
+      {M_PI_2, 1.},         // BR
+      {-M_PI_2, 1.},        // BR
+      {0.125 * M_PI, 10.},  // BR
+      {0.125 * M_PI, 1.},   // BR
+      {0.125 * M_PI, 1.},
+  };
+
+  for (const auto& it : test_cases) {
+    Eigen::Isometry3d start_pose({5., 10., 0.});
+    start_pose.rotate(
+        math::RollPitchYaw<double>(0., 0., it.heading).ToQuaternion());
+    Eigen::Isometry3d end_pose = start_pose;
+    end_pose.translation() =
+        end_pose.translation() +
+        math::RotationMatrix<double>(start_pose.rotation()).matrix() *
+            Eigen::Vector3d{it.distance, 0., 0.};
+
+    const std::vector<Eigen::Isometry3d> poses{start_pose, end_pose};
+    const std::vector<double> times{0., it.distance / kSpeed};
+    const AgentTrajectory trajectory = AgentTrajectory::Make(times, poses);
+
+    const TrajectoryAgent<double> agent(CarAgent(), trajectory);
+
+    // Check that the outputs are correct over the entirety of the trajectory.
+    systems::Simulator<double> simulator(agent);
+    systems::Context<double>& context = simulator.get_mutable_context();
+    std::unique_ptr<systems::SystemOutput<double>> outputs =
+        agent.AllocateOutput(context);
+    simulator.Initialize();
+
+    const double end_time = it.distance / kSpeed;
+    for (double time = 0.; time <= end_time; time += 0.1) {
+      simulator.StepTo(time);
+
+      const double scalar =
+          std::min(std::max(0.0, (time * kSpeed) / it.distance), 1.);
+      const double position = scalar * it.distance;
+      Eigen::Translation<double, 3> expected_translation(
+          start_pose.translation() + Eigen::Vector3d{cos(it.heading) * position,
+                                                     sin(it.heading) * position,
+                                                     0.});
+      agent.CalcOutput(context, outputs.get());
+
+      // Tests the raw pose output.
+      const auto raw_pose = dynamic_cast<const SimpleCarState<double>*>(
+          outputs->get_vector_data(agent.raw_pose_output().get_index()));
+      EXPECT_EQ(SimpleCarStateIndices::kNumCoordinates, raw_pose->size());
+
+      EXPECT_NEAR(expected_translation.x(), raw_pose->x(), kTol);
+      EXPECT_NEAR(expected_translation.y(), raw_pose->y(), kTol);
+      EXPECT_NEAR(it.heading, raw_pose->heading(), kTol);
+      EXPECT_DOUBLE_EQ(kSpeed, raw_pose->velocity());
+
+      // Tests the PoseVector output.
+      const auto pose = dynamic_cast<const PoseVector<double>*>(
+          outputs->get_vector_data(agent.pose_output().get_index()));
+      ASSERT_NE(nullptr, pose);
+      EXPECT_EQ(PoseVector<double>::kSize, pose->size());
+
+      EXPECT_NEAR(expected_translation.x(),
+                  pose->get_translation().translation().x(), kTol);
+      EXPECT_NEAR(expected_translation.y(),
+                  pose->get_translation().translation().y(), kTol);
+      EXPECT_NEAR(cos(it.heading / 2), pose->get_rotation().w(), kTol);
+      EXPECT_NEAR(sin(it.heading / 2), pose->get_rotation().z(), kTol);
+
+      // Tests the FrameVelocity output.
+      const auto velocity = dynamic_cast<const FrameVelocity<double>*>(
+          outputs->get_vector_data(agent.velocity_output().get_index()));
+
+      ASSERT_NE(nullptr, velocity);
+      EXPECT_EQ(FrameVelocity<double>::kSize, velocity->size());
+
+      EXPECT_NEAR(kSpeed * cos(it.heading),
+                  velocity->get_velocity().translational().x(), kTol);
+      EXPECT_NEAR(kSpeed * sin(it.heading),
+                  velocity->get_velocity().translational().y(), kTol);
+    }
+  }
+}
+
+GTEST_TEST(TrajectoryAgentTest, ToAutoDiff) {
+  const std::vector<double> times{0., 1.};
+  const std::vector<Eigen::Isometry3d> poses{Eigen::Isometry3d::Identity(),
+                                             Eigen::Isometry3d::Identity()};
+  const TrajectoryAgent<double> agent(CarAgent(),
+                                      AgentTrajectory::Make(times, poses));
+
+  EXPECT_TRUE(is_autodiffxd_convertible(agent, [&](const auto& autodiff_dut) {
+    auto context = autodiff_dut.CreateDefaultContext();
+    auto output = autodiff_dut.AllocateOutput(*context);
+
+    // Check that the public methods can be called without exceptions.
+    autodiff_dut.CalcOutput(*context, output.get());
+  }));
+}
+
+}  // namespace
+}  // namespace automotive
+}  // namespace drake

automotive/trajectory_agent.cc

@@ -0,0 +1,66 @@
+#include "drake/automotive/trajectory_agent.h"
+
+#include "drake/common/default_scalars.h"
+
+namespace drake {
+namespace automotive {
+
+using multibody::SpatialVelocity;
+using systems::rendering::FrameVelocity;
+using systems::rendering::PoseVector;
+
+template <typename T>
+TrajectoryAgent<T>::TrajectoryAgent(const AgentData& agent_data,
+                                    const AgentTrajectory& trajectory,
+                                    double sampling_time_sec)
+    : systems::LeafSystem<T>(
+          systems::SystemTypeTag<automotive::TrajectoryAgent>{}),
+      agent_data_(agent_data),
+      trajectory_(trajectory) {
+  this->DeclarePeriodicUnrestrictedUpdate(sampling_time_sec, 0.);
+  this->DeclareVectorOutputPort(&TrajectoryAgent::CalcStateOutput);
+  this->DeclareVectorOutputPort(&TrajectoryAgent::CalcPoseOutput);
+  this->DeclareVectorOutputPort(&TrajectoryAgent::CalcVelocityOutput);
+}
+
+template <typename T>
+void TrajectoryAgent<T>::CalcStateOutput(
+    const systems::Context<T>& context,
+    SimpleCarState<T>* output_vector) const {
+  const PoseVelocity values = GetValues(context);
+  output_vector->set_x(T{values.pose3().x()});
+  output_vector->set_y(T{values.pose3().y()});
+  output_vector->set_heading(T{values.pose3().z()});
+  output_vector->set_velocity(T{values.speed()});
+}
+
+template <typename T>
+void TrajectoryAgent<T>::CalcPoseOutput(const systems::Context<T>& context,
+                                        PoseVector<T>* pose) const {
+  const PoseVelocity values = GetValues(context);
+  pose->set_translation(Eigen::Translation<T, 3>{values.translation()});
+  const Eigen::Quaternion<double>& q =
+      math::RotationMatrix<double>(values.rotation()).ToQuaternion();
+  pose->set_rotation(Eigen::Quaternion<T>{q});
+}
+
+template <typename T>
+void TrajectoryAgent<T>::CalcVelocityOutput(const systems::Context<T>& context,
+                                            FrameVelocity<T>* velocity) const {
+  const PoseVelocity values = GetValues(context);
+  const Eigen::Vector3d& v = values.velocity().translational();
+  const Eigen::Vector3d& w = values.velocity().rotational();
+  velocity->set_velocity(SpatialVelocity<T>{Vector3<T>{w}, Vector3<T>{v}});
+}
+
+template <typename T>
+PoseVelocity TrajectoryAgent<T>::GetValues(
+    const systems::Context<T>& context) const {
+  return trajectory_.value(ExtractDoubleOrThrow(context.get_time()));
+}
+
+}  // namespace automotive
+}  // namespace drake
+
+DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS(
+    class ::drake::automotive::TrajectoryAgent)

automotive/trajectory_agent.h

@@ -0,0 +1,121 @@
+#pragma once
+
+#include <Eigen/Geometry>
+
+#include "drake/automotive/agent_trajectory.h"
+#include "drake/automotive/gen/simple_car_state.h"
+#include "drake/common/drake_copyable.h"
+#include "drake/common/extract_double.h"
+#include "drake/systems/framework/leaf_system.h"
+#include "drake/systems/rendering/frame_velocity.h"
+#include "drake/systems/rendering/pose_vector.h"
+
+namespace drake {
+namespace automotive {
+
+/// Container for static data for a given agent model.
+struct AgentData {
+  enum class AgentType { kCar, kBicycle, kPedestrian };
+  // TODO(jadecastro) Add more types as necessary.
+
+  explicit AgentData(const AgentType& t) : type(t) {}
+
+  AgentType type{AgentType::kCar};
+};
+
+// Provide an explicit specialization for the kCar agent type.
+struct CarAgent : AgentData {
+  CarAgent() : AgentData(AgentType::kCar) {}
+};
+
+/// TrajectoryAgent models an agent that follows a pre-established trajectory,
+/// taking as input an AgentData structure and an AgentTrajectory.
+///
+/// Note that, when T = AutoDiffXd, the AutoDiffXd derivatives for each element
+/// of the the outputs are empty.
+///
+/// output port 0: A SimpleCarState containing:
+/// * position: x, y, heading;
+///   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 = √{ẋ² + ẏ²}
+///   (OutputPort getter: raw_pose_output())
+///
+/// output port 1: A PoseVector containing X_WA, where A is the agent's
+/// reference frame.
+///   (OutputPort getter: pose_output())
+///
+/// output port 2: A FrameVelocity containing the spatial velocity V_WA, where A
+/// is the agent's reference frame.
+///   (OutputPort getter: velocity_output())
+///
+/// Instantiated templates for the following kinds of T's are provided:
+/// - double
+/// - drake::AutoDiffXd
+///
+/// They are already available to link against in the containing library.
+///
+/// @ingroup automotive_plants
+template <typename T>
+class TrajectoryAgent final : public systems::LeafSystem<T> {
+ public:
+  DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(TrajectoryAgent)
+
+  /// Constructs a TrajectoryAgent system that traces a given AgentTrajectory.
+  ///
+  /// @param agent_data an AgentData structure defining the agent.
+  /// @param trajectory an AgentTrajectory containing the trajectory.
+  /// @param sampling_time_sec the requested sampling time (in sec) for this
+  /// system.  @default 0.01.
+  TrajectoryAgent(const AgentData& agent_data,
+                  const AgentTrajectory& trajectory,
+                  double sampling_time_sec = 0.01);
+
+  /// Scalar-converting copy constructor.  See @ref system_scalar_conversion.
+  template <typename U>
+  explicit TrajectoryAgent(const TrajectoryAgent<U>& other)
+      : TrajectoryAgent<T>(other.agent_data_, other.trajectory_) {}
+
+  /// @name Accessors for the outputs, as enumerated in the class documentation.
+  /// @{
+  const systems::OutputPort<T>& raw_pose_output() const {
+    return this->get_output_port(0);
+  }
+  const systems::OutputPort<T>& pose_output() const {
+    return this->get_output_port(1);
+  }
+  const systems::OutputPort<T>& velocity_output() const {
+    return this->get_output_port(2);
+  }
+  /// @}
+
+ private:
+  // Converts a PoseVelocity, evaluated at the current time, into a
+  // SimpleCarState output.
+  void CalcStateOutput(const systems::Context<T>& context,
+                       SimpleCarState<T>* output_vector) const;
+
+  // Converts a PoseVelocity, evaluated at the current time, into a PoseVector
+  // output.
+  void CalcPoseOutput(const systems::Context<T>& context,
+                      systems::rendering::PoseVector<T>* pose) const;
+
+  // Converts a PoseVelocity, evaluated at the current time, into a
+  // FrameVelocity output.
+  void CalcVelocityOutput(const systems::Context<T>& context,
+                          systems::rendering::FrameVelocity<T>* velocity) const;
+
+  // Extracts the PoseVelocity value at the current time, as provided in
+  // Context.
+  PoseVelocity GetValues(const systems::Context<T>& context) const;
+
+  // Allow different specializations to access each other's private data.
+  template <typename>
+  friend class TrajectoryAgent;
+
+  const AgentData agent_data_;
+  const AgentTrajectory trajectory_;
+};
+
+}  // namespace automotive
+}  // namespace drake