From a48936d761713da141c8cdaa27332c7836b3e32e Mon Sep 17 00:00:00 2001 From: Jon Date: Mon, 30 Apr 2018 21:46:06 -0400 Subject: [PATCH] Add TrajectoryAgent --- automotive/BUILD.bazel | 26 ++++ automotive/test/trajectory_agent_test.cc | 156 ++++++++++++++++++++ automotive/trajectory_agent.cc | 66 +++++++++ automotive/trajectory_agent.h | 121 +++++++++++++++ tools/install/libdrake/build_components.bzl | 2 + 5 files changed, 371 insertions(+) create mode 100644 automotive/test/trajectory_agent_test.cc create mode 100644 automotive/trajectory_agent.cc create mode 100644 automotive/trajectory_agent.h diff --git a/automotive/BUILD.bazel b/automotive/BUILD.bazel index aa10ca67eb33..205ac8a7f425 100644 --- a/automotive/BUILD.bazel +++ b/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 = [ diff --git a/automotive/test/trajectory_agent_test.cc b/automotive/test/trajectory_agent_test.cc new file mode 100644 index 000000000000..79fdd9c516dc --- /dev/null +++ b/automotive/test/trajectory_agent_test.cc @@ -0,0 +1,156 @@ +#include "drake/automotive/trajectory_agent.h" + +#include + +#include + +#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 times{0., 1.}; + const std::vector poses{Eigen::Isometry3d::Identity(), + Eigen::Isometry3d::Identity()}; + const TrajectoryAgent 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::kSize, pose_output.size()); + + const auto& velocity_output = car.velocity_output(); + EXPECT_EQ(systems::kVectorValued, velocity_output.get_data_type()); + EXPECT_EQ(FrameVelocity::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 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(0., 0., it.heading).ToQuaternion()); + Eigen::Isometry3d end_pose = start_pose; + end_pose.translation() = + end_pose.translation() + + math::RotationMatrix(start_pose.rotation()).matrix() * + Eigen::Vector3d{it.distance, 0., 0.}; + + const std::vector poses{start_pose, end_pose}; + const std::vector times{0., it.distance / kSpeed}; + const AgentTrajectory trajectory = AgentTrajectory::Make(times, poses); + + const TrajectoryAgent agent(CarAgent(), trajectory); + + // Check that the outputs are correct over the entirety of the trajectory. + systems::Simulator simulator(agent); + systems::Context& context = simulator.get_mutable_context(); + std::unique_ptr> 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 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*>( + 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*>( + outputs->get_vector_data(agent.pose_output().get_index())); + ASSERT_NE(nullptr, pose); + EXPECT_EQ(PoseVector::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*>( + outputs->get_vector_data(agent.velocity_output().get_index())); + + ASSERT_NE(nullptr, velocity); + EXPECT_EQ(FrameVelocity::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 times{0., 1.}; + const std::vector poses{Eigen::Isometry3d::Identity(), + Eigen::Isometry3d::Identity()}; + const TrajectoryAgent 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 diff --git a/automotive/trajectory_agent.cc b/automotive/trajectory_agent.cc new file mode 100644 index 000000000000..63875156c86b --- /dev/null +++ b/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 +TrajectoryAgent::TrajectoryAgent(const AgentData& agent_data, + const AgentTrajectory& trajectory, + double sampling_time_sec) + : systems::LeafSystem( + systems::SystemTypeTag{}), + 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 +void TrajectoryAgent::CalcStateOutput( + const systems::Context& context, + SimpleCarState* 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 +void TrajectoryAgent::CalcPoseOutput(const systems::Context& context, + PoseVector* pose) const { + const PoseVelocity values = GetValues(context); + pose->set_translation(Eigen::Translation{values.translation()}); + const Eigen::Quaternion& q = + math::RotationMatrix(values.rotation()).ToQuaternion(); + pose->set_rotation(Eigen::Quaternion{q}); +} + +template +void TrajectoryAgent::CalcVelocityOutput(const systems::Context& context, + FrameVelocity* 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{Vector3{w}, Vector3{v}}); +} + +template +PoseVelocity TrajectoryAgent::GetValues( + const systems::Context& 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) diff --git a/automotive/trajectory_agent.h b/automotive/trajectory_agent.h new file mode 100644 index 000000000000..55071eec8ef7 --- /dev/null +++ b/automotive/trajectory_agent.h @@ -0,0 +1,121 @@ +#pragma once + +#include + +#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 +class TrajectoryAgent final : public systems::LeafSystem { + 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 + explicit TrajectoryAgent(const TrajectoryAgent& other) + : TrajectoryAgent(other.agent_data_, other.trajectory_) {} + + /// @name Accessors for the outputs, as enumerated in the class documentation. + /// @{ + const systems::OutputPort& raw_pose_output() const { + return this->get_output_port(0); + } + const systems::OutputPort& pose_output() const { + return this->get_output_port(1); + } + const systems::OutputPort& 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& context, + SimpleCarState* output_vector) const; + + // Converts a PoseVelocity, evaluated at the current time, into a PoseVector + // output. + void CalcPoseOutput(const systems::Context& context, + systems::rendering::PoseVector* pose) const; + + // Converts a PoseVelocity, evaluated at the current time, into a + // FrameVelocity output. + void CalcVelocityOutput(const systems::Context& context, + systems::rendering::FrameVelocity* velocity) const; + + // Extracts the PoseVelocity value at the current time, as provided in + // Context. + PoseVelocity GetValues(const systems::Context& context) const; + + // Allow different specializations to access each other's private data. + template + friend class TrajectoryAgent; + + const AgentData agent_data_; + const AgentTrajectory trajectory_; +}; + +} // namespace automotive +} // namespace drake diff --git a/tools/install/libdrake/build_components.bzl b/tools/install/libdrake/build_components.bzl index cf4a1625ad1f..ad7ab498e2d4 100644 --- a/tools/install/libdrake/build_components.bzl +++ b/tools/install/libdrake/build_components.bzl @@ -25,6 +25,7 @@ LIBDRAKE_COMPONENTS = [ "//automotive/maliput/simplerulebook:simplerulebook", "//automotive/maliput/simplerulebook:yaml_io", "//automotive/maliput/utility:utility", + "//automotive:agent_trajectory", "//automotive:automotive", "//automotive:automotive_simulator", "//automotive:bicycle_car", @@ -51,6 +52,7 @@ LIBDRAKE_COMPONENTS = [ "//automotive:road_path", "//automotive:simple_car", "//automotive:simple_powertrain", + "//automotive:trajectory_agent", "//automotive:trajectory_car", "//common/proto:call_matlab", "//common/proto:call_python",