From 37fcbc7101fae3bee843626dd8328e2b148965e7 Mon Sep 17 00:00:00 2001 From: Duy-Nguyen Ta Date: Thu, 21 Dec 2017 11:26:22 -0500 Subject: [PATCH 01/25] Refactor RgbdRenderer (#7611) Refactor RgbdRenderer into an abstract interface and a specific VTK implementation. This is to allow different implementations of the RgbdRenderer using non-VTK graphics engines, e.g. Godot. * rgbd_renderer_test -> rgbd_renderer_vtk_test * split RgbdRendererTest fixture to a header file, so it can also be used to test godot * rgbd_renderer_test.h -> rgbd_renderer_test_util.h * Add RenderingConfig, change X_WR->X_WC * Add comment for why ASSERT is used instead of EXPECT_XXX --- systems/sensors/BUILD.bazel | 18 +- systems/sensors/rgbd_camera.cc | 41 +- systems/sensors/rgbd_renderer.cc | 506 +----------------- systems/sensors/rgbd_renderer.h | 121 +++-- systems/sensors/rgbd_renderer_vtk.cc | 487 +++++++++++++++++ systems/sensors/rgbd_renderer_vtk.h | 45 ++ systems/sensors/test/rgbd_renderer_test.cc | 344 ------------ .../sensors/test/rgbd_renderer_test_util.h | 135 +++++ .../sensors/test/rgbd_renderer_vtk_test.cc | 231 ++++++++ 9 files changed, 1014 insertions(+), 914 deletions(-) create mode 100644 systems/sensors/rgbd_renderer_vtk.cc create mode 100644 systems/sensors/rgbd_renderer_vtk.h delete mode 100644 systems/sensors/test/rgbd_renderer_test.cc create mode 100644 systems/sensors/test/rgbd_renderer_test_util.h create mode 100644 systems/sensors/test/rgbd_renderer_vtk_test.cc diff --git a/systems/sensors/BUILD.bazel b/systems/sensors/BUILD.bazel index 03ab211a7aad..fa030745ef0f 100644 --- a/systems/sensors/BUILD.bazel +++ b/systems/sensors/BUILD.bazel @@ -176,10 +176,14 @@ drake_cc_library( drake_cc_library( name = "rgbd_renderer", - srcs = ["rgbd_renderer.cc"], + srcs = [ + "rgbd_renderer.cc", + "rgbd_renderer_vtk.cc", + ], hdrs = [ "color_palette.h", "rgbd_renderer.h", + "rgbd_renderer_vtk.h", ], deps = [ ":image", @@ -440,8 +444,14 @@ drake_cc_googletest( ], ) +drake_cc_library( + name = "rgbd_renderer_test_util", + testonly = 1, + hdrs = ["test/rgbd_renderer_test_util.h"], +) + drake_cc_googletest( - name = "rgbd_renderer_test", + name = "rgbd_renderer_vtk_test", data = [ ":test_models", ], @@ -456,6 +466,7 @@ drake_cc_googletest( ], deps = [ ":rgbd_renderer", + ":rgbd_renderer_test_util", "//drake/common:find_resource", "//drake/common/test_utilities:eigen_matrix_compare", "//drake/math:geometric_transform", @@ -475,6 +486,9 @@ sh_test( data = [ ":test_models", ], + # Mitigates driver-related issues when running under `bazel test`. For more + # information, see #7004. + local = 1, # Disable under LeakSanitizer and Valgrind Memcheck due to driver-related # leaks. For more information, see #7520. tags = [ diff --git a/systems/sensors/rgbd_camera.cc b/systems/sensors/rgbd_camera.cc index 1f38d969d945..e441cbc376ba 100644 --- a/systems/sensors/rgbd_camera.cc +++ b/systems/sensors/rgbd_camera.cc @@ -11,7 +11,7 @@ #include "drake/systems/rendering/pose_vector.h" #include "drake/systems/sensors/camera_info.h" #include "drake/systems/sensors/image.h" -#include "drake/systems/sensors/rgbd_renderer.h" +#include "drake/systems/sensors/rgbd_renderer_vtk.h" namespace drake { namespace systems { @@ -60,42 +60,37 @@ void RgbdCamera::ConvertDepthImageToPointCloud(const ImageDepth32F& depth_image, RgbdCamera::RgbdCamera(const std::string& name, const RigidBodyTree& tree, const Eigen::Vector3d& position, - const Eigen::Vector3d& orientation, - double z_near, - double z_far, - double fov_y, - bool show_window) - : tree_(tree), frame_(RigidBodyFrame()), + const Eigen::Vector3d& orientation, double z_near, + double z_far, double fov_y, bool show_window) + : tree_(tree), + frame_(RigidBodyFrame()), camera_fixed_(true), color_camera_info_(kImageWidth, kImageHeight, fov_y), depth_camera_info_(kImageWidth, kImageHeight, fov_y), X_WB_initial_( Eigen::Translation3d(position[0], position[1], position[2]) * Eigen::Isometry3d(math::rpy2rotmat(orientation))), - renderer_(new RgbdRenderer( - (Eigen::Translation3d(position[0], position[1], position[2]) * - Eigen::Isometry3d(math::rpy2rotmat(orientation))) * X_BC_, - kImageWidth, kImageHeight, - z_near, z_far, - fov_y, show_window)) { + renderer_(new RgbdRendererVTK( + RenderingConfig{kImageWidth, kImageHeight, fov_y, z_near, z_far, + show_window}, + Eigen::Translation3d(position[0], position[1], position[2]) * + Eigen::Isometry3d(math::rpy2rotmat(orientation)) * X_BC_)) { Init(name); } RgbdCamera::RgbdCamera(const std::string& name, const RigidBodyTree& tree, - const RigidBodyFrame& frame, - double z_near, - double z_far, - double fov_y, - bool show_window) - : tree_(tree), frame_(frame), + const RigidBodyFrame& frame, double z_near, + double z_far, double fov_y, bool show_window) + : tree_(tree), + frame_(frame), camera_fixed_(false), color_camera_info_(kImageWidth, kImageHeight, fov_y), depth_camera_info_(kImageWidth, kImageHeight, fov_y), - renderer_(new RgbdRenderer(Eigen::Isometry3d::Identity(), - kImageWidth, kImageHeight, - z_near, z_far, - fov_y, show_window)) { + renderer_( + new RgbdRendererVTK(RenderingConfig{kImageWidth, kImageHeight, fov_y, + z_near, z_far, show_window}, + Eigen::Isometry3d::Identity())) { Init(name); } diff --git a/systems/sensors/rgbd_renderer.cc b/systems/sensors/rgbd_renderer.cc index 4d24bc1a2cca..026b8918d9f5 100644 --- a/systems/sensors/rgbd_renderer.cc +++ b/systems/sensors/rgbd_renderer.cc @@ -1,531 +1,53 @@ #include "drake/systems/sensors/rgbd_renderer.h" -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "drake/common/drake_assert.h" -#include "drake/systems/sensors/image.h" -#include "drake/systems/sensors/vtk_util.h" - -// This macro declares vtkRenderingOpenGL2_AutoInit_{Construct(), Destruct()} -// functions and the former is called via VTK_AUTOINIT_CONSTRUCT in -// ModuleInitVtkRenderingOpenGL2. -VTK_AUTOINIT_DECLARE(vtkRenderingOpenGL2) - -// TODO(kunimatsu-tri) Refactor RgbdRenderer with GeometryWorld when it's ready, -// so that other VTK dependent sensor simulators can share the world without -// duplicating it. - namespace drake { namespace systems { namespace sensors { -using vtk_util::ConvertToVtkTransform; -using vtk_util::MakeVtkPointerArray; - -namespace { - const int kNumMaxLabel = 256; -// TODO(kunimatsu-tri) Add support for the arbitrary clipping planes. -const double kClippingPlaneNear = 0.01; -const double kClippingPlaneFar = 100.; -const double kTerrainSize = 100.; - -// For Zbuffer value conversion. -const double kA = kClippingPlaneFar / (kClippingPlaneFar - kClippingPlaneNear); -const double kB = -kA * kClippingPlaneNear; - -// Register the object factories for the vtkRenderingOpenGL2 module. -struct ModuleInitVtkRenderingOpenGL2 { - ModuleInitVtkRenderingOpenGL2() { - VTK_AUTOINIT_CONSTRUCT(vtkRenderingOpenGL2) - } -}; - -std::string RemoveFileExtension(const std::string& filepath) { - const size_t last_dot = filepath.find_last_of("."); - if (last_dot == std::string::npos) { - DRAKE_ABORT_MSG("File has no extention."); - } - return filepath.substr(0, last_dot); -} - -// Updates VTK rendering related objects including vtkRenderWindow, -// vtkWindowToImageFilter and vtkImageExporter, so that VTK reflects -// vtkActors' pose update for rendering. -void PerformVTKUpdate( - const vtkNew& window, - const vtkNew& filter, - const vtkNew& exporter) { - window->Render(); - filter->Modified(); - filter->Update(); - exporter->Update(); -} - -void SetModelTransformMatrixToVtkCamera( - vtkCamera* camera, const vtkSmartPointer& X_WC) { - // vtkCamera contains a transformation as the internal state and - // ApplyTransform multiplies a given transformation on top of the internal - // transformation. Thus, resetting 'Set{Position, FocalPoint, ViewUp}' is - // needed here. - camera->SetPosition(0., 0., 0.); - camera->SetFocalPoint(0., 0., 1.); // Sets z-forward. - camera->SetViewUp(0., -1, 0.); // Sets y-down. For the detail, please refer - // to CameraInfo's document. - camera->ApplyTransform(X_WC); -} - -} // namespace - -class RgbdRenderer::Impl : private ModuleInitVtkRenderingOpenGL2 { - public: - DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Impl) - - Impl(const Eigen::Isometry3d& X_WC, int width, int height, - double z_near, double z_far, double fov_y, bool show_window); - - ~Impl() {} - - void AddFlatTerrain(); - - optional RegisterVisual( - const DrakeShapes::VisualElement& visual, int body_id); - - void UpdateVisualPose( - const Eigen::Isometry3d& X_WV, int body_id, VisualIndex visual_id) const; - - void UpdateViewpoint(const Eigen::Isometry3d& X_WR) const; - - void RenderColorImage(ImageRgba8U* color_image_out) const; - - void RenderDepthImage(ImageDepth32F* depth_image_out) const; - - void RenderLabelImage(ImageLabel16I* label_image_out) const; - - int width() const { return width_; } - - int height() const { return height_; } - - double fov_y() const { return fov_y_; } - - const ColorI& get_sky_color() const { - return color_palette_.get_sky_color(); - } - - const ColorI& get_flat_terrain_color() const { - return color_palette_.get_terrain_color(); - } - - private: - float CheckRangeAndConvertToMeters(float z_buffer_value) const; - - const int width_; - const int height_; - const double fov_y_; - const double z_near_; - const double z_far_; - const ColorPalette color_palette_; - - vtkNew terrain_actor_; - // An array of maps which take pairs of a body index in RBT and a vector of - // vtkSmartPointer to vtkActor. The each vtkActor corresponds to an visual - // element specified in SDF / URDF. The first element of this array is for - // color and depth rendering and the second is for label image rendering. - // TODO(kunimatsu-tri) Make this more straight forward for the readability. - std::array>>, 2> - id_object_maps_; - vtkNew color_depth_renderer_; - vtkNew label_renderer_; - vtkNew color_depth_render_window_; - vtkNew label_render_window_; - vtkNew color_filter_; - vtkNew depth_filter_; - vtkNew label_filter_; - vtkNew color_exporter_; - vtkNew depth_exporter_; - vtkNew label_exporter_; -}; - -void RgbdRenderer::Impl::AddFlatTerrain() { - vtkSmartPointer plane = - vtk_util::CreateSquarePlane(kTerrainSize); - vtkNew mapper; - mapper->SetInputConnection(plane->GetOutputPort()); - terrain_actor_->SetMapper(mapper.GetPointer()); - - auto color = ColorPalette::Normalize(color_palette_.get_terrain_color()); - terrain_actor_->GetProperty()->SetColor(color.r, color.g, color.b); - terrain_actor_->GetProperty()->LightingOff(); - for (auto& renderer : MakeVtkPointerArray(color_depth_renderer_, - label_renderer_)) { - renderer->AddActor(terrain_actor_.GetPointer()); - } -} - -void RgbdRenderer::Impl::UpdateVisualPose( - const Eigen::Isometry3d& X_WV, int body_id, VisualIndex visual_id) const { - vtkSmartPointer vtk_X_WV = ConvertToVtkTransform(X_WV); - // `id_object_maps_` is modified here. This is OK because 1) we are just - // copying data to the memory spaces allocated at construction time - // and 2) we are not outputting these data to outside the class. - for (auto& id_object_map : id_object_maps_) { - auto& actor = id_object_map.at(body_id).at(visual_id); - actor->SetUserTransform(vtk_X_WV); - } -} - -void RgbdRenderer::Impl::UpdateViewpoint(const Eigen::Isometry3d& X_WR) const { - vtkSmartPointer vtk_X_WR = ConvertToVtkTransform(X_WR); - - for (auto& renderer : MakeVtkPointerArray(color_depth_renderer_, - label_renderer_)) { - auto camera = renderer->GetActiveCamera(); - SetModelTransformMatrixToVtkCamera(camera, vtk_X_WR); - } -} - -void RgbdRenderer::Impl::RenderColorImage( - ImageRgba8U* color_image_out) const { - // TODO(sherm1) Should evaluate VTK cache entry. - PerformVTKUpdate(color_depth_render_window_, color_filter_, color_exporter_); - color_exporter_->Export(color_image_out->at(0, 0)); -} - -void RgbdRenderer::Impl::RenderDepthImage( - ImageDepth32F* depth_image_out) const { - // TODO(sherm1) Should evaluate VTK cache entry. - PerformVTKUpdate(color_depth_render_window_, depth_filter_, depth_exporter_); - depth_exporter_->Export(depth_image_out->at(0, 0)); - - // TODO(kunimatsu-tri) Calculate this in a vertex shader. - for (int v = 0; v < height_; ++v) { - for (int u = 0; u < width_; ++u) { - depth_image_out->at(u, v)[0] = - CheckRangeAndConvertToMeters(depth_image_out->at(u, v)[0]); - } - } -} - -void RgbdRenderer::Impl::RenderLabelImage( - ImageLabel16I* label_image_out) const { - // TODO(sherm1) Should evaluate VTK cache entry. - PerformVTKUpdate(label_render_window_, label_filter_, label_exporter_); - - ImageRgb8U image(width_, height_); - label_exporter_->Export(image.at(0, 0)); - - ColorI color; - for (int v = 0; v < height_; ++v) { - for (int u = 0; u < width_; ++u) { - color.r = image.at(u, v)[0]; - color.g = image.at(u, v)[1]; - color.b = image.at(u, v)[2]; - label_image_out->at(u, v)[0] = - static_cast(color_palette_.LookUpId(color)); - } - } -} - -RgbdRenderer::Impl::Impl(const Eigen::Isometry3d& X_WC, - int width, - int height, - double z_near, - double z_far, - double fov_y, - bool show_window) - : width_(width), height_(height), fov_y_(fov_y), - z_near_(z_near), z_far_(z_far), +RgbdRenderer::RgbdRenderer(const RenderingConfig& config, + const Eigen::Isometry3d&) + : config_(config), color_palette_(kNumMaxLabel, Label::kFlatTerrain, Label::kNoBody) { - if (!show_window) { - for (auto& window : MakeVtkPointerArray(color_depth_render_window_, - label_render_window_)) { - window->SetOffScreenRendering(1); - } - } - - const auto sky_color = - ColorPalette::Normalize(color_palette_.get_sky_color()); - const auto renderers = MakeVtkPointerArray(color_depth_renderer_, - label_renderer_); - const vtkSmartPointer vtk_X_WC = ConvertToVtkTransform(X_WC); - - for (auto& renderer : renderers) { - renderer->SetBackground(sky_color.r, sky_color.g, sky_color.b); - auto camera = renderer->GetActiveCamera(); - camera->SetViewAngle(fov_y_ * 180. / M_PI); - camera->SetClippingRange(kClippingPlaneNear, kClippingPlaneFar); - SetModelTransformMatrixToVtkCamera(camera, vtk_X_WC); - } - - color_depth_renderer_->SetUseDepthPeeling(1); - color_depth_renderer_->UseFXAAOn(); - - const auto windows = MakeVtkPointerArray( - color_depth_render_window_, label_render_window_); - for (size_t i = 0; i < windows.size(); ++i) { - windows[i]->SetSize(width_, height_); - windows[i]->AddRenderer(renderers[i].GetPointer()); - } - label_render_window_->SetMultiSamples(0); - - color_filter_->SetInput(color_depth_render_window_.GetPointer()); - color_filter_->SetInputBufferTypeToRGBA(); - depth_filter_->SetInput(color_depth_render_window_.GetPointer()); - depth_filter_->SetInputBufferTypeToZBuffer(); - label_filter_->SetInput(label_render_window_.GetPointer()); - label_filter_->SetInputBufferTypeToRGB(); - - auto exporters = MakeVtkPointerArray( - color_exporter_, depth_exporter_, label_exporter_); - - auto filters = MakeVtkPointerArray( - color_filter_, depth_filter_, label_filter_); - - for (int i = 0; i < 3; ++i) { - filters[i]->SetMagnification(1); - filters[i]->ReadFrontBufferOff(); - filters[i]->Update(); - exporters[i]->SetInputData(filters[i]->GetOutput()); - exporters[i]->ImageLowerLeftOff(); - } } -optional RgbdRenderer::Impl::RegisterVisual( - const DrakeShapes::VisualElement& visual, int body_id) { - // Initializes containers in id_object_maps_ if it's not done. - for (auto& id_object_map : id_object_maps_) { - const auto it = id_object_map.find(body_id); - if (it == id_object_map.end()) { - std::vector> vec; - id_object_map[body_id] = vec; - } - } - - vtkNew actor; - vtkNew mapper; - bool shape_matched = true; - bool texture_found = false; - const DrakeShapes::Geometry& geometry = visual.getGeometry(); - switch (visual.getShape()) { - case DrakeShapes::BOX: { - auto box = dynamic_cast(geometry); - vtkNew vtk_cube; - vtk_cube->SetXLength(box.size(0)); - vtk_cube->SetYLength(box.size(1)); - vtk_cube->SetZLength(box.size(2)); - - mapper->SetInputConnection(vtk_cube->GetOutputPort()); - break; - } - case DrakeShapes::SPHERE: { - auto sphere = dynamic_cast(geometry); - vtkNew vtk_sphere; - vtk_sphere->SetRadius(sphere.radius); - vtk_sphere->SetThetaResolution(50); - vtk_sphere->SetPhiResolution(50); - - mapper->SetInputConnection(vtk_sphere->GetOutputPort()); - break; - } - case DrakeShapes::CYLINDER: { - auto cylinder = dynamic_cast(geometry); - vtkNew vtk_cylinder; - vtk_cylinder->SetHeight(cylinder.length); - vtk_cylinder->SetRadius(cylinder.radius); - vtk_cylinder->SetResolution(50); - - // Since the cylinder in vtkCylinderSource is y-axis aligned, we need - // to rotate it to be z-axis aligned because that is what Drake uses. - vtkNew transform; - transform->RotateX(90); - vtkNew transform_filter; - transform_filter->SetInputConnection(vtk_cylinder->GetOutputPort()); - transform_filter->SetTransform(transform.GetPointer()); - transform_filter->Update(); - - mapper->SetInputConnection(transform_filter->GetOutputPort()); - break; - } - case DrakeShapes::MESH: { - const auto* mesh_filename = dynamic_cast( - geometry).resolved_filename_.c_str(); - - // TODO(kunimatsu-tri) Add support for other file formats. - vtkNew mesh_reader; - mesh_reader->SetFileName(mesh_filename); - mesh_reader->Update(); - - // TODO(kunimatsu-tri) Guessing the texture file name is bad. Instead, - // get it from somewhere like `DrakeShapes::MeshWithTexture` when it's - // implemented. - // TODO(kunimatsu-tri) Add support for other file formats. - const std::string texture_file( - RemoveFileExtension(mesh_filename) + ".png"); - std::ifstream file_exist(texture_file); - - if (file_exist) { - vtkNew texture_reader; - texture_reader->SetFileName(texture_file.c_str()); - texture_reader->Update(); - vtkNew texture; - texture->SetInputConnection(texture_reader->GetOutputPort()); - texture->InterpolateOn(); - actor->SetTexture(texture.GetPointer()); - - texture_found = true; - } - - mapper->SetInputConnection(mesh_reader->GetOutputPort()); - break; - } - case DrakeShapes::CAPSULE: { - // TODO(kunimatsu-tri) Implement this as needed. - shape_matched = false; - break; - } - default: { - shape_matched = false; - break; - } - } - - // Registers actors. - if (shape_matched) { - if (!texture_found) { - const auto color = visual.getMaterial(); - actor->GetProperty()->SetColor(color[0], color[1], color[2]); - } - - const auto& color = color_palette_.get_normalized_color(body_id); - vtkNew actor_for_label; - actor_for_label->GetProperty()->SetColor(color.r, color.g, color.b); - // This is to disable shadows and to get an object painted with a single - // color. - actor_for_label->GetProperty()->LightingOff(); - - vtkSmartPointer vtk_transform = - ConvertToVtkTransform(visual.getWorldTransform()); - - auto renderers = MakeVtkPointerArray(color_depth_renderer_, - label_renderer_); - auto actors = MakeVtkPointerArray(actor, actor_for_label); - - for (size_t i = 0; i < actors.size(); ++i) { - actors[i]->SetMapper(mapper.GetPointer()); - actors[i]->SetUserTransform(vtk_transform); - renderers[i]->AddActor(actors[i].GetPointer()); - id_object_maps_[i][body_id].push_back( - vtkSmartPointer(actors[i].GetPointer())); - } - - return optional(VisualIndex( - static_cast(id_object_maps_[0][body_id].size() - 1))); - } - - return nullopt; -} - -float RgbdRenderer::Impl::CheckRangeAndConvertToMeters( - float z_buffer_value) const { - float z; - // When the depth is either closer than `kClippingPlaneNear` or farther than - // `kClippingPlaneFar`, `z_buffer_value` becomes `1.f`. - if (z_buffer_value == 1.f) { - z = std::numeric_limits::quiet_NaN(); - } else { - z = static_cast(kB / (z_buffer_value - kA)); - - if (z > z_far_) { - z = InvalidDepth::kTooFar; - } else if (z < z_near_) { - z = InvalidDepth::kTooClose; - } - } - - return z; -} - -RgbdRenderer::RgbdRenderer(const Eigen::Isometry3d& X_WC, - int width, - int height, - double z_near, - double z_far, - double fov_y, - bool show_window) - : impl_(new RgbdRenderer::Impl(X_WC, width, height, z_near, z_far, fov_y, - show_window)) {} - RgbdRenderer::~RgbdRenderer() {} optional RgbdRenderer::RegisterVisual( const DrakeShapes::VisualElement& visual, int body_id) { - return impl_->RegisterVisual(visual, body_id); + return ImplRegisterVisual(visual, body_id); } void RgbdRenderer::AddFlatTerrain() { - impl_->AddFlatTerrain(); + ImplAddFlatTerrain(); } -void RgbdRenderer::UpdateViewpoint(const Eigen::Isometry3d& X_WR) const { - impl_->UpdateViewpoint(X_WR); +void RgbdRenderer::UpdateViewpoint(const Eigen::Isometry3d& X_WC) const { + ImplUpdateViewpoint(X_WC); } void RgbdRenderer::UpdateVisualPose( const Eigen::Isometry3d& X_WV, int body_id, VisualIndex visual_id) const { - impl_->UpdateVisualPose(X_WV, body_id, visual_id); + ImplUpdateVisualPose(X_WV, body_id, visual_id); } void RgbdRenderer::RenderColorImage(ImageRgba8U* color_image_out) const { - impl_->RenderColorImage(color_image_out); + ImplRenderColorImage(color_image_out); } void RgbdRenderer::RenderDepthImage(ImageDepth32F* depth_image_out) const { - impl_->RenderDepthImage(depth_image_out); + ImplRenderDepthImage(depth_image_out); } void RgbdRenderer::RenderLabelImage(ImageLabel16I* label_image_out) const { - impl_->RenderLabelImage(label_image_out); + ImplRenderLabelImage(label_image_out); } -int RgbdRenderer::width() const { return impl_->width(); } - -int RgbdRenderer::height() const { return impl_->height(); } - -double RgbdRenderer::fov_y() const { return impl_->fov_y(); } - -const ColorI& RgbdRenderer::get_sky_color() const { - return impl_->get_sky_color(); -} +const RenderingConfig& RgbdRenderer::config() const { return config_; } -const ColorI& RgbdRenderer::get_flat_terrain_color() const { - return impl_->get_flat_terrain_color(); +const ColorPalette& RgbdRenderer::color_palette() const { + return color_palette_; } } // namespace sensors diff --git a/systems/sensors/rgbd_renderer.h b/systems/sensors/rgbd_renderer.h index 87ac94dcf4e2..6a7af353bfaf 100644 --- a/systems/sensors/rgbd_renderer.h +++ b/systems/sensors/rgbd_renderer.h @@ -1,7 +1,6 @@ #pragma once #include -#include #include @@ -16,9 +15,33 @@ namespace drake { namespace systems { namespace sensors { -/// An RGB-D renderer that renders RGB, depth and label images using -/// VisualElement. The coordinate system of RgbdRenderer's viewpoint `R` is -/// `X-right`, `Y-down` and `Z-forward` with respect to the rendered images. +/// Common configurations of rendering systems. +struct RenderingConfig { + /// The width of the image to be rendered in pixels. + const int width; + /// The height of the image to be rendered in pixels. + const int height; + /// The renderer's camera vertical field of view in radians. + const double fov_y; + /// The minimum depth RgbdRenderer can output. Note that this is different + /// from renderer's clipping range where all the objects outside the range are + /// not rendered even in RGB image while this only affects depth image. + const double z_near; + /// The maximum depth RgbdRenderer can output. Note that this is different + /// from renderer's clipping range where all the objects outside the range are + /// not rendered even in RGB image while this only affects depth image. + const double z_far; + /// A flag for showing visible windows for RGB and label images. If this is + /// false, offscreen rendering is executed. This is useful for debugging + /// purposes. + const bool show_window; +}; + + +/// Abstract interface of RGB-D renderers, which render RGB, depth and label +/// images using VisualElement. The coordinate system of RgbdRenderer's +/// viewpoint `R` is `X-right`, `Y-down` and `Z-forward` with respect to the +/// rendered images. /// /// Output image format: /// - RGB (ImageRgba8U) : the RGB image has four channels in the following @@ -38,44 +61,23 @@ namespace sensors { /// corresponds to an object in the scene. For the pixels corresponding to /// no body, namely the sky and the flat terrain, we assign Label::kNoBody /// and Label::kFlatTerrain, respectively. -class RgbdRenderer final { +class RgbdRenderer { public: DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(RgbdRenderer) /// A constructor for %RgbdRenderer. /// - /// @param X_WR The initial pose of the renderer's viewpoint `R` at the world - /// coordinate system. The `R` can be updated by calling `UpdateViewpoint` - /// later on. - /// - /// @param width The width of the image to be rendered in pixels. - /// - /// @param height The height of the image to be rendered in pixels. - /// - /// @param z_near The minimum depth RgbdRenderer can output. Note that - /// this is different from renderer's clipping range where all the objects - /// outside the range are not rendered even in RGB image while this only - /// affects depth image. - /// - /// @param z_far The maximum depth RgbdRenderer can output. Note that - /// this is different from renderer's clipping range where all the objects - /// outside the range are not rendered even in RGB image while this only - /// affects depth image. - /// - /// @param fov_y The RgbdRenderer's vertical field of view in radians. + /// @param config Configurations of the renderer. See RenderingConfig. /// - /// @param show_window A flag for showing visible windows for RGB and label - /// images. If this is false, offscreen rendering is executed. This is - /// useful for debugging purposes. - RgbdRenderer(const Eigen::Isometry3d& X_WR, - int width, - int height, - double z_near, - double z_far, - double fov_y, - bool show_window); - - ~RgbdRenderer(); + /// @param X_WC The initial pose of the renderer's unique camera viewpoint `C` + /// in the world coordinate system. The camera pose `C` can be updated by + /// calling `UpdateViewpoint` later on. Default value: Identity. + /// TODO(thduynguyen, kunimatsu-tri): Handle multiple viewpoints, e.g. for + /// stereo depth camera? + RgbdRenderer(const RenderingConfig& config, + const Eigen::Isometry3d& X_WC = Eigen::Isometry3d::Identity()); + + virtual ~RgbdRenderer(); /// Adds a flat terrain in the rendering scene. void AddFlatTerrain(); @@ -119,11 +121,11 @@ class RgbdRenderer final { void UpdateVisualPose(const Eigen::Isometry3d& X_WV, int body_id, VisualIndex visual_id) const; - /// Updates renderer's viewpoint with given pose X_WR. + /// Updates renderer's camera viewpoint with given pose X_WC. /// - /// @param X_WR The pose of renderer's viewpoint in the world coordinate - /// system. - void UpdateViewpoint(const Eigen::Isometry3d& X_WR) const; + /// @param X_WC The pose of renderer's camera viewpoint in the world + /// coordinate system. + void UpdateViewpoint(const Eigen::Isometry3d& X_WC) const; /// Renders and outputs the rendered color image. /// @@ -140,24 +142,37 @@ class RgbdRenderer final { /// @param label_image_out The rendered label image. void RenderLabelImage(ImageLabel16I* label_image_out) const; - /// Returns the image width. - int width() const; + /// Returns the configuration object of this renderer. + const RenderingConfig& config() const; + + /// Returns the color palette of this renderer. + const ColorPalette& color_palette() const; - /// Returns the image height. - int height() const; + private: + virtual void ImplAddFlatTerrain() = 0; - /// Returns the renderer's vertical field of view. - double fov_y() const; + virtual optional ImplRegisterVisual( + const DrakeShapes::VisualElement& visual, int body_id) = 0; - /// Returns sky's color in RGB image. - const ColorI& get_sky_color() const; + virtual void ImplUpdateVisualPose(const Eigen::Isometry3d& X_WV, int body_id, + VisualIndex visual_id) const = 0; - /// Returns flat terrain's color in RGB image. - const ColorI& get_flat_terrain_color() const; + virtual void ImplUpdateViewpoint(const Eigen::Isometry3d& X_WC) const = 0; - private: - class Impl; - std::unique_ptr impl_; + virtual void ImplRenderColorImage(ImageRgba8U* color_image_out) const = 0; + + virtual void ImplRenderDepthImage(ImageDepth32F* depth_image_out) const = 0; + + virtual void ImplRenderLabelImage(ImageLabel16I* label_image_out) const = 0; + + /// The common configuration needed by all implementations of this interface. + RenderingConfig config_; + + /// The color palette for sky, terrain colors and ground truth label rendering + /// TODO(thduynguyen, SeanCurtis-TRI): This is a world's property (colors for + /// each object/segment) hence should be moved to GeometryWorld. That would + /// also answer the question whether this heavy object should be a singleton. + ColorPalette color_palette_; }; } // namespace sensors diff --git a/systems/sensors/rgbd_renderer_vtk.cc b/systems/sensors/rgbd_renderer_vtk.cc new file mode 100644 index 000000000000..05ebe42f0134 --- /dev/null +++ b/systems/sensors/rgbd_renderer_vtk.cc @@ -0,0 +1,487 @@ +#include "drake/systems/sensors/rgbd_renderer_vtk.h" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "drake/common/drake_assert.h" +#include "drake/systems/sensors/vtk_util.h" + +// This macro declares vtkRenderingOpenGL2_AutoInit_{Construct(), Destruct()} +// functions and the former is called via VTK_AUTOINIT_CONSTRUCT in +// ModuleInitVtkRenderingOpenGL2. +VTK_AUTOINIT_DECLARE(vtkRenderingOpenGL2) + +// TODO(kunimatsu-tri) Refactor RgbdRenderer with GeometryWorld when it's ready, +// so that other VTK dependent sensor simulators can share the world without +// duplicating it. + +namespace drake { +namespace systems { +namespace sensors { + +using vtk_util::ConvertToVtkTransform; +using vtk_util::MakeVtkPointerArray; + +namespace { + +// TODO(kunimatsu-tri) Add support for the arbitrary clipping planes. +const double kClippingPlaneNear = 0.01; +const double kClippingPlaneFar = 100.; +const double kTerrainSize = 100.; + +// For Zbuffer value conversion. +const double kA = kClippingPlaneFar / (kClippingPlaneFar - kClippingPlaneNear); +const double kB = -kA * kClippingPlaneNear; + +// Register the object factories for the vtkRenderingOpenGL2 module. +struct ModuleInitVtkRenderingOpenGL2 { + ModuleInitVtkRenderingOpenGL2() { + VTK_AUTOINIT_CONSTRUCT(vtkRenderingOpenGL2) + } +}; + +std::string RemoveFileExtension(const std::string& filepath) { + const size_t last_dot = filepath.find_last_of("."); + if (last_dot == std::string::npos) { + DRAKE_ABORT_MSG("File has no extention."); + } + return filepath.substr(0, last_dot); +} + +// Updates VTK rendering related objects including vtkRenderWindow, +// vtkWindowToImageFilter and vtkImageExporter, so that VTK reflects +// vtkActors' pose update for rendering. +void PerformVTKUpdate(const vtkNew& window, + const vtkNew& filter, + const vtkNew& exporter) { + window->Render(); + filter->Modified(); + filter->Update(); + exporter->Update(); +} + +void SetModelTransformMatrixToVtkCamera( + vtkCamera* camera, const vtkSmartPointer& X_WC) { + // vtkCamera contains a transformation as the internal state and + // ApplyTransform multiplies a given transformation on top of the internal + // transformation. Thus, resetting 'Set{Position, FocalPoint, ViewUp}' is + // needed here. + camera->SetPosition(0., 0., 0.); + camera->SetFocalPoint(0., 0., 1.); // Sets z-forward. + camera->SetViewUp(0., -1, 0.); // Sets y-down. For the detail, please refer + // to CameraInfo's document. + camera->ApplyTransform(X_WC); +} + +} // namespace + +class RgbdRendererVTK::Impl : private ModuleInitVtkRenderingOpenGL2 { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Impl) + + Impl(RgbdRendererVTK* parent, const Eigen::Isometry3d& X_WC); + ~Impl() {} + + void ImplAddFlatTerrain(); + + optional ImplRegisterVisual( + const DrakeShapes::VisualElement& visual, int body_id); + + void ImplUpdateVisualPose(const Eigen::Isometry3d& X_WV, int body_id, + VisualIndex visual_id) const; + + void ImplUpdateViewpoint(const Eigen::Isometry3d& X_WC) const; + + void ImplRenderColorImage(ImageRgba8U* color_image_out) const; + + void ImplRenderDepthImage(ImageDepth32F* depth_image_out) const; + + void ImplRenderLabelImage(ImageLabel16I* label_image_out) const; + + private: + float CheckRangeAndConvertToMeters(float z_buffer_value) const; + + RgbdRendererVTK* parent_ = nullptr; + + vtkNew terrain_actor_; + // An array of maps which take pairs of a body index in RBT and a vector of + // vtkSmartPointer to vtkActor. The each vtkActor corresponds to an visual + // element specified in SDF / URDF. The first element of this array is for + // color and depth rendering and the second is for label image rendering. + // TODO(kunimatsu-tri) Make this more straight forward for the readability. + std::array>>, 2> + id_object_maps_; + vtkNew color_depth_renderer_; + vtkNew label_renderer_; + vtkNew color_depth_render_window_; + vtkNew label_render_window_; + vtkNew color_filter_; + vtkNew depth_filter_; + vtkNew label_filter_; + vtkNew color_exporter_; + vtkNew depth_exporter_; + vtkNew label_exporter_; +}; + +float RgbdRendererVTK::Impl::CheckRangeAndConvertToMeters( + float z_buffer_value) const { + float z; + // When the depth is either closer than `kClippingPlaneNear` or farther than + // `kClippingPlaneFar`, `z_buffer_value` becomes `1.f`. + if (z_buffer_value == 1.f) { + z = std::numeric_limits::quiet_NaN(); + } else { + z = static_cast(kB / (z_buffer_value - kA)); + + if (z > parent_->config().z_far) { + z = InvalidDepth::kTooFar; + } else if (z < parent_->config().z_near) { + z = InvalidDepth::kTooClose; + } + } + + return z; +} + +void RgbdRendererVTK::Impl::ImplAddFlatTerrain() { + vtkSmartPointer plane = + vtk_util::CreateSquarePlane(kTerrainSize); + vtkNew mapper; + mapper->SetInputConnection(plane->GetOutputPort()); + terrain_actor_->SetMapper(mapper.GetPointer()); + + auto color = + ColorPalette::Normalize(parent_->color_palette().get_terrain_color()); + terrain_actor_->GetProperty()->SetColor(color.r, color.g, color.b); + terrain_actor_->GetProperty()->LightingOff(); + for (auto& renderer : + MakeVtkPointerArray(color_depth_renderer_, label_renderer_)) { + renderer->AddActor(terrain_actor_.GetPointer()); + } +} + +void RgbdRendererVTK::Impl::ImplUpdateVisualPose(const Eigen::Isometry3d& X_WV, + int body_id, + VisualIndex visual_id) const { + vtkSmartPointer vtk_X_WV = ConvertToVtkTransform(X_WV); + // `id_object_maps_` is modified here. This is OK because 1) we are just + // copying data to the memory spaces allocated at construction time + // and 2) we are not outputting these data to outside the class. + for (auto& id_object_map : id_object_maps_) { + auto& actor = id_object_map.at(body_id).at(visual_id); + actor->SetUserTransform(vtk_X_WV); + } +} + +void RgbdRendererVTK::Impl::ImplUpdateViewpoint( + const Eigen::Isometry3d& X_WC) const { + vtkSmartPointer vtk_X_WC = ConvertToVtkTransform(X_WC); + + for (auto& renderer : + MakeVtkPointerArray(color_depth_renderer_, label_renderer_)) { + auto camera = renderer->GetActiveCamera(); + SetModelTransformMatrixToVtkCamera(camera, vtk_X_WC); + } +} + +void RgbdRendererVTK::Impl::ImplRenderColorImage( + ImageRgba8U* color_image_out) const { + // TODO(sherm1) Should evaluate VTK cache entry. + PerformVTKUpdate(color_depth_render_window_, color_filter_, color_exporter_); + color_exporter_->Export(color_image_out->at(0, 0)); +} + +void RgbdRendererVTK::Impl::ImplRenderDepthImage( + ImageDepth32F* depth_image_out) const { + // TODO(sherm1) Should evaluate VTK cache entry. + PerformVTKUpdate(color_depth_render_window_, depth_filter_, depth_exporter_); + depth_exporter_->Export(depth_image_out->at(0, 0)); + + // TODO(kunimatsu-tri) Calculate this in a vertex shader. + for (int v = 0; v < parent_->config().height; ++v) { + for (int u = 0; u < parent_->config().width; ++u) { + depth_image_out->at(u, v)[0] = + CheckRangeAndConvertToMeters(depth_image_out->at(u, v)[0]); + } + } +} + +void RgbdRendererVTK::Impl::ImplRenderLabelImage( + ImageLabel16I* label_image_out) const { + // TODO(sherm1) Should evaluate VTK cache entry. + PerformVTKUpdate(label_render_window_, label_filter_, label_exporter_); + + ImageRgb8U image(parent_->config().width, parent_->config().height); + label_exporter_->Export(image.at(0, 0)); + + ColorI color; + for (int v = 0; v < parent_->config().height; ++v) { + for (int u = 0; u < parent_->config().width; ++u) { + color.r = image.at(u, v)[0]; + color.g = image.at(u, v)[1]; + color.b = image.at(u, v)[2]; + label_image_out->at(u, v)[0] = + static_cast(parent_->color_palette().LookUpId(color)); + } + } +} + +RgbdRendererVTK::Impl::Impl(RgbdRendererVTK* parent, + const Eigen::Isometry3d& X_WC) + : parent_(parent) { + if (!parent_->config().show_window) { + for (auto& window : MakeVtkPointerArray(color_depth_render_window_, + label_render_window_)) { + window->SetOffScreenRendering(1); + } + } + + const auto sky_color = + ColorPalette::Normalize(parent_->color_palette().get_sky_color()); + const auto renderers = + MakeVtkPointerArray(color_depth_renderer_, label_renderer_); + const vtkSmartPointer vtk_X_WC = ConvertToVtkTransform(X_WC); + + for (auto& renderer : renderers) { + renderer->SetBackground(sky_color.r, sky_color.g, sky_color.b); + auto camera = renderer->GetActiveCamera(); + camera->SetViewAngle(parent_->config().fov_y * 180. / M_PI); + camera->SetClippingRange(kClippingPlaneNear, kClippingPlaneFar); + SetModelTransformMatrixToVtkCamera(camera, vtk_X_WC); + } + + color_depth_renderer_->SetUseDepthPeeling(1); + color_depth_renderer_->UseFXAAOn(); + + const auto windows = + MakeVtkPointerArray(color_depth_render_window_, label_render_window_); + for (size_t i = 0; i < windows.size(); ++i) { + windows[i]->SetSize(parent_->config().width, parent_->config().height); + windows[i]->AddRenderer(renderers[i].GetPointer()); + } + label_render_window_->SetMultiSamples(0); + + color_filter_->SetInput(color_depth_render_window_.GetPointer()); + color_filter_->SetInputBufferTypeToRGBA(); + depth_filter_->SetInput(color_depth_render_window_.GetPointer()); + depth_filter_->SetInputBufferTypeToZBuffer(); + label_filter_->SetInput(label_render_window_.GetPointer()); + label_filter_->SetInputBufferTypeToRGB(); + + auto exporters = + MakeVtkPointerArray(color_exporter_, depth_exporter_, label_exporter_); + + auto filters = + MakeVtkPointerArray(color_filter_, depth_filter_, label_filter_); + + for (int i = 0; i < 3; ++i) { + filters[i]->SetMagnification(1); + filters[i]->ReadFrontBufferOff(); + filters[i]->Update(); + exporters[i]->SetInputData(filters[i]->GetOutput()); + exporters[i]->ImageLowerLeftOff(); + } +} + +optional RgbdRendererVTK::Impl::ImplRegisterVisual( + const DrakeShapes::VisualElement& visual, int body_id) { + // Initializes containers in id_object_maps_ if it's not done. + for (auto& id_object_map : id_object_maps_) { + const auto it = id_object_map.find(body_id); + if (it == id_object_map.end()) { + std::vector> vec; + id_object_map[body_id] = vec; + } + } + + vtkNew actor; + vtkNew mapper; + bool shape_matched = true; + bool texture_found = false; + const DrakeShapes::Geometry& geometry = visual.getGeometry(); + switch (visual.getShape()) { + case DrakeShapes::BOX: { + auto box = dynamic_cast(geometry); + vtkNew vtk_cube; + vtk_cube->SetXLength(box.size(0)); + vtk_cube->SetYLength(box.size(1)); + vtk_cube->SetZLength(box.size(2)); + + mapper->SetInputConnection(vtk_cube->GetOutputPort()); + break; + } + case DrakeShapes::SPHERE: { + auto sphere = dynamic_cast(geometry); + vtkNew vtk_sphere; + vtk_sphere->SetRadius(sphere.radius); + vtk_sphere->SetThetaResolution(50); + vtk_sphere->SetPhiResolution(50); + + mapper->SetInputConnection(vtk_sphere->GetOutputPort()); + break; + } + case DrakeShapes::CYLINDER: { + auto cylinder = dynamic_cast(geometry); + vtkNew vtk_cylinder; + vtk_cylinder->SetHeight(cylinder.length); + vtk_cylinder->SetRadius(cylinder.radius); + vtk_cylinder->SetResolution(50); + + // Since the cylinder in vtkCylinderSource is y-axis aligned, we need + // to rotate it to be z-axis aligned because that is what Drake uses. + vtkNew transform; + transform->RotateX(90); + vtkNew transform_filter; + transform_filter->SetInputConnection(vtk_cylinder->GetOutputPort()); + transform_filter->SetTransform(transform.GetPointer()); + transform_filter->Update(); + + mapper->SetInputConnection(transform_filter->GetOutputPort()); + break; + } + case DrakeShapes::MESH: { + const auto* mesh_filename = + dynamic_cast(geometry) + .resolved_filename_.c_str(); + + // TODO(kunimatsu-tri) Add support for other file formats. + vtkNew mesh_reader; + mesh_reader->SetFileName(mesh_filename); + mesh_reader->Update(); + + // TODO(kunimatsu-tri) Guessing the texture file name is bad. Instead, + // get it from somewhere like `DrakeShapes::MeshWithTexture` when it's + // implemented. + // TODO(kunimatsu-tri) Add support for other file formats. + const std::string texture_file(RemoveFileExtension(mesh_filename) + + ".png"); + std::ifstream file_exist(texture_file); + + if (file_exist) { + vtkNew texture_reader; + texture_reader->SetFileName(texture_file.c_str()); + texture_reader->Update(); + vtkNew texture; + texture->SetInputConnection(texture_reader->GetOutputPort()); + texture->InterpolateOn(); + actor->SetTexture(texture.GetPointer()); + + texture_found = true; + } + + mapper->SetInputConnection(mesh_reader->GetOutputPort()); + break; + } + case DrakeShapes::CAPSULE: { + // TODO(kunimatsu-tri) Implement this as needed. + shape_matched = false; + break; + } + default: { + shape_matched = false; + break; + } + } + + // Registers actors. + if (shape_matched) { + if (!texture_found) { + const auto color = visual.getMaterial(); + actor->GetProperty()->SetColor(color[0], color[1], color[2]); + } + + const auto& color = parent_->color_palette().get_normalized_color(body_id); + vtkNew actor_for_label; + actor_for_label->GetProperty()->SetColor(color.r, color.g, color.b); + // This is to disable shadows and to get an object painted with a single + // color. + actor_for_label->GetProperty()->LightingOff(); + + vtkSmartPointer vtk_transform = + ConvertToVtkTransform(visual.getWorldTransform()); + + auto renderers = + MakeVtkPointerArray(color_depth_renderer_, label_renderer_); + auto actors = MakeVtkPointerArray(actor, actor_for_label); + + for (size_t i = 0; i < actors.size(); ++i) { + actors[i]->SetMapper(mapper.GetPointer()); + actors[i]->SetUserTransform(vtk_transform); + renderers[i]->AddActor(actors[i].GetPointer()); + id_object_maps_[i][body_id].push_back( + vtkSmartPointer(actors[i].GetPointer())); + } + + return optional( + VisualIndex(static_cast(id_object_maps_[0][body_id].size() - 1))); + } + + return nullopt; +} + +RgbdRendererVTK::RgbdRendererVTK(const RenderingConfig& config, + const Eigen::Isometry3d& X_WC) + : RgbdRenderer(config, X_WC), + impl_(new RgbdRendererVTK::Impl(this, X_WC)) {} + +RgbdRendererVTK::~RgbdRendererVTK() {} + +optional RgbdRendererVTK::ImplRegisterVisual( + const DrakeShapes::VisualElement& visual, int body_id) { + return impl_->ImplRegisterVisual(visual, body_id); +} + +void RgbdRendererVTK::ImplAddFlatTerrain() { impl_->ImplAddFlatTerrain(); } + +void RgbdRendererVTK::ImplUpdateViewpoint(const Eigen::Isometry3d& X_WC) const { + impl_->ImplUpdateViewpoint(X_WC); +} + +void RgbdRendererVTK::ImplUpdateVisualPose(const Eigen::Isometry3d& X_WV, + int body_id, + VisualIndex visual_id) const { + impl_->ImplUpdateVisualPose(X_WV, body_id, visual_id); +} + +void RgbdRendererVTK::ImplRenderColorImage(ImageRgba8U* color_image_out) const { + impl_->ImplRenderColorImage(color_image_out); +} + +void RgbdRendererVTK::ImplRenderDepthImage( + ImageDepth32F* depth_image_out) const { + impl_->ImplRenderDepthImage(depth_image_out); +} + +void RgbdRendererVTK::ImplRenderLabelImage( + ImageLabel16I* label_image_out) const { + impl_->ImplRenderLabelImage(label_image_out); +} + +} // namespace sensors +} // namespace systems +} // namespace drake diff --git a/systems/sensors/rgbd_renderer_vtk.h b/systems/sensors/rgbd_renderer_vtk.h new file mode 100644 index 000000000000..842215f9a01d --- /dev/null +++ b/systems/sensors/rgbd_renderer_vtk.h @@ -0,0 +1,45 @@ +#pragma once + +#include + +#include "drake/systems/sensors/rgbd_renderer.h" + +namespace drake { +namespace systems { +namespace sensors { + +/// An RgbdRenderer implementation using VTK. +class RgbdRendererVTK final : public RgbdRenderer { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(RgbdRendererVTK) + + RgbdRendererVTK( + const RenderingConfig& config, + const Eigen::Isometry3d& X_WC = Eigen::Isometry3d::Identity()); + + ~RgbdRendererVTK(); + + private: + void ImplAddFlatTerrain() override; + + optional ImplRegisterVisual( + const DrakeShapes::VisualElement& visual, int body_id) override; + + void ImplUpdateVisualPose(const Eigen::Isometry3d& X_WV, + int body_id, VisualIndex visual_id) const override; + + void ImplUpdateViewpoint(const Eigen::Isometry3d& X_WC) const override; + + void ImplRenderColorImage(ImageRgba8U* color_image_out) const override; + + void ImplRenderDepthImage(ImageDepth32F* depth_image_out) const override; + + void ImplRenderLabelImage(ImageLabel16I* label_image_out) const override; + + class Impl; + std::unique_ptr impl_; +}; + +} // namespace sensors +} // namespace systems +} // namespace drake diff --git a/systems/sensors/test/rgbd_renderer_test.cc b/systems/sensors/test/rgbd_renderer_test.cc deleted file mode 100644 index 891336b9769c..000000000000 --- a/systems/sensors/test/rgbd_renderer_test.cc +++ /dev/null @@ -1,344 +0,0 @@ -#include "drake/systems/sensors/rgbd_renderer.h" - -#include -#include - -#include "drake/common/find_resource.h" -#include "drake/common/test_utilities/eigen_matrix_compare.h" -#include "drake/systems/sensors/image.h" - -namespace drake { -namespace systems { -namespace sensors { -namespace { - -using Eigen::Isometry3d; - -const int kWidth = 640; -const int kHeight = 480; -const double kZNear = 0.5; -const double kZFar = 5.; -const double kFovY = M_PI_4; -const bool kShowWindow = false; - -// The following tolerance is used due to a precision difference between Ubuntu -// Linux and Mac OSX. -const double kColorPixelTolerance = 1.001; -const double kDepthTolerance = 1.1e-4; - -// Holds `(x, y)` indices of the screen coordinate system where the ranges of -// `x` and `y` are [0, kWidth) and [0, kHeight) respectively. -struct ScreenCoord { - int x{}; - int y{}; -}; - -void CompareColor(const uint8_t* pixel, - const sensors::ColorI& color, - int alpha) { - ASSERT_NEAR(pixel[0], color.r, kColorPixelTolerance); - ASSERT_NEAR(pixel[1], color.g, kColorPixelTolerance); - ASSERT_NEAR(pixel[2], color.b, kColorPixelTolerance); - ASSERT_EQ(pixel[3], alpha); -} - -// This suite tests RgbdRenderer. -class RgbdRendererTest : public ::testing::Test { - public: - RgbdRendererTest() : - color_(kWidth, kHeight), depth_(kWidth, kHeight), label_(kWidth, kHeight), - // Looking strait down from 3m above the ground. - X_WR_(Eigen::Translation3d(0, 0, kDefaultDistance) * - Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(-M_PI_2, Eigen::Vector3d::UnitZ())) {} - - protected: - void Render() { - renderer_->RenderColorImage(&color_); - renderer_->RenderDepthImage(&depth_); - renderer_->RenderLabelImage(&label_); - } - - void VerifyUniformColor(const sensors::ColorI& pixel, int alpha) { - for (int y = 0; y < kHeight; ++y) { - for (int x = 0; x < kWidth; ++x) { - CompareColor(color_.at(x, y), pixel, alpha); - } - } - } - - void VerifyUniformLabel(int16_t label) { - for (int y = 0; y < kHeight; ++y) { - for (int x = 0; x < kWidth; ++x) { - ASSERT_EQ(label_.at(x, y)[0], label); - } - } - } - - void VerifyUniformDepth(float depth) { - for (int y = 0; y < kHeight; ++y) { - for (int x = 0; x < kWidth; ++x) { - ASSERT_NEAR(depth_.at(x, y)[0], depth, kDepthTolerance); - } - } - } - - // Verifies the chosen pixels, i.e. `kOutliers`, belong to the ground. - void VerifyOutliers() { - const auto& kTerrain = renderer_->get_flat_terrain_color(); - for (const auto& screen_coord : kOutliers) { - const int x = screen_coord.x; - const int y = screen_coord.y; - // Color - CompareColor(color_.at(x, y), kTerrain, 255u); - // Depth - ASSERT_NEAR(depth_.at(x, y)[0], kDefaultDistance, kDepthTolerance); - // Label - ASSERT_EQ(label_.at(x, y)[0], Label::kFlatTerrain); - } - } - - void SetUp() override {} - - // All tests on this class must invoke this first. - void SetUp(const Eigen::Isometry3d& X_WR, bool add_terrain = false) { - renderer_ = std::make_unique( - X_WR, kWidth, kHeight, kZNear, kZFar, kFovY, kShowWindow); - - if (add_terrain) - renderer_->AddFlatTerrain(); - } - - const sensors::ColorI kDefaultVisualColor = {179u, 179u, 179u}; - const float kDefaultDistance{3.f}; - // `kInlier` is chosen to point to a pixel representing an object in the - // test scene. - const ScreenCoord kInlier = {320, 240}; - // The outliers are chosen to point to pixels representing the ground, - // not objects in the test scene. - const std::array kOutliers{{ - {10, 10}, {10, 470}, {630, 10}, {630, 470}}}; - - ImageRgba8U color_; - ImageDepth32F depth_; - ImageLabel16I label_; - Isometry3d X_WR_; - - std::unique_ptr renderer_; -}; - -TEST_F(RgbdRendererTest, InstantiationTest) { - SetUp(Isometry3d::Identity()); - - EXPECT_EQ(renderer_->width(), kWidth); - EXPECT_EQ(renderer_->height(), kHeight); - EXPECT_EQ(renderer_->fov_y(), kFovY); -} - -TEST_F(RgbdRendererTest, NoBodyTest) { - SetUp(Isometry3d::Identity()); - Render(); - - VerifyUniformColor(renderer_->get_sky_color(), 0u); - VerifyUniformLabel(Label::kNoBody); - // Verifies depth. - for (int y = 0; y < kHeight; ++y) { - for (int x = 0; x < kWidth; ++x) { - EXPECT_TRUE(std::isnan(depth_.at(x, y)[0])); - } - } -} - -TEST_F(RgbdRendererTest, TerrainTest) { - SetUp(X_WR_, true); - - const auto& kTerrain = renderer_->get_flat_terrain_color(); - // At two different distances. - for (auto depth : std::array({{2.f, 5.f}})) { - X_WR_.translation().z() = depth; - renderer_->UpdateViewpoint(X_WR_); - Render(); - VerifyUniformColor(kTerrain, 255u); - VerifyUniformLabel(Label::kFlatTerrain); - VerifyUniformDepth(depth); - } - - // Closer than kZNear. - X_WR_.translation().z() = kZNear - 1e-5; - renderer_->UpdateViewpoint(X_WR_); - Render(); - VerifyUniformColor(kTerrain, 255u); - VerifyUniformLabel(Label::kFlatTerrain); - VerifyUniformDepth(InvalidDepth::kTooClose); - - // Farther than kZFar. - X_WR_.translation().z() = kZFar + 1e-3; - renderer_->UpdateViewpoint(X_WR_); - Render(); - VerifyUniformColor(kTerrain, 255u); - VerifyUniformLabel(Label::kFlatTerrain); - // Verifies depth. - for (int y = 0; y < kHeight; ++y) { - for (int x = 0; x < kWidth; ++x) { - ASSERT_EQ(InvalidDepth::kTooFar, depth_.at(x, y)[0]); - } - } -} - -TEST_F(RgbdRendererTest, HorizonTest) { - // Camera at the origin, pointing in a direction parallel to the ground. - Isometry3d X_WR = - Eigen::Translation3d(0, 0, 0) * - Eigen::AngleAxisd(-M_PI_2, Eigen::Vector3d::UnitX()) * - Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d::UnitY()); - SetUp(X_WR, true); - - // Returns y in [0, kHeight / 2], index of horizon location in image - // coordinate system under two assumptions: 1) the gound plane is not clipped - // by `kClippingPlaneFar`, 2) camera is located above the ground. - auto CalcHorizon = [](double z) { - const double kTerrainSize = 50.; - const double kFocalLength = kHeight * 0.5 / std::tan(0.5 * kFovY); - return 0.5 * kHeight + z / kTerrainSize * kFocalLength; - }; - - // Verifies v index of horizon at three different camera heights. - const std::array Zs{{2., 1., 0.5}}; - for (const auto& z : Zs) { - X_WR.translation().z() = z; - renderer_->UpdateViewpoint(X_WR); - Render(); - - const auto& kTerrain = renderer_->get_flat_terrain_color(); - int actual_horizon{0}; - for (int y = 0; y < kHeight; ++y) { - // Looking for the boundary between the sky and the ground. - if ((static_cast(kTerrain.r == color_.at(0, y)[0])) && - (static_cast(kTerrain.g == color_.at(0, y)[1])) && - (static_cast(kTerrain.b == color_.at(0, y)[2]))) { - actual_horizon = y; - break; - } - } - - const double expected_horizon = CalcHorizon(z); - ASSERT_NEAR(expected_horizon, actual_horizon, 1.001); - } -} - -TEST_F(RgbdRendererTest, BoxTest) { - SetUp(X_WR_, true); - - // Sets up a box. - Isometry3d X_WV = Isometry3d::Identity(); - X_WV.translation().z() = 0.5; - DrakeShapes::VisualElement visual(X_WV); - Eigen::Vector3d box_size(1, 1, 1); - visual.setGeometry(DrakeShapes::Box(box_size)); - const int kBodyID = 0; - const RgbdRenderer::VisualIndex kVisualID(0); - renderer_->RegisterVisual(visual, kBodyID); - renderer_->UpdateVisualPose(X_WV, kBodyID, kVisualID); - Render(); - - VerifyOutliers(); - - // Verifies inside the box. - const int x = kInlier.x; - const int y = kInlier.y; - // Color - CompareColor(color_.at(x, y), kDefaultVisualColor, 255u); - // Depth - ASSERT_NEAR(depth_.at(x, y)[0], 2.f, kDepthTolerance); - // Label - ASSERT_EQ(label_.at(x, y)[0], kBodyID); -} - -TEST_F(RgbdRendererTest, SphereTest) { - SetUp(X_WR_, true); - - // Sets up a sphere. - Isometry3d X_WV = Isometry3d::Identity(); - X_WV.translation().z() = 0.5; - DrakeShapes::VisualElement visual(X_WV); - visual.setGeometry(DrakeShapes::Sphere(0.5)); - const int kBodyID = 0; - const RgbdRenderer::VisualIndex kVisualID(0); - renderer_->RegisterVisual(visual, kBodyID); - renderer_->UpdateVisualPose(X_WV, kBodyID, kVisualID); - Render(); - - VerifyOutliers(); - - // Verifies inside the sphere. - const int x = kInlier.x; - const int y = kInlier.y; - // Color - CompareColor(color_.at(x, y), kDefaultVisualColor, 255u); - // Depth - ASSERT_NEAR(depth_.at(x, y)[0], 2.f, kDepthTolerance); - // Label - ASSERT_EQ(label_.at(x, y)[0], kBodyID); -} - -TEST_F(RgbdRendererTest, CylinderTest) { - SetUp(X_WR_, true); - - // Sets up a sphere. - Isometry3d X_WV = Isometry3d::Identity(); - X_WV.translation().z() = 0.6; - DrakeShapes::VisualElement visual(X_WV); - visual.setGeometry(DrakeShapes::Cylinder(0.2, 1.2)); // Radius and length. - const int kBodyID = 1; - const RgbdRenderer::VisualIndex kVisualID(0); - renderer_->RegisterVisual(visual, kBodyID); - renderer_->UpdateVisualPose(X_WV, kBodyID, kVisualID); - Render(); - - VerifyOutliers(); - - // Verifies inside the cylinder. - const int x = kInlier.x; - const int y = kInlier.y; - // Color - CompareColor(color_.at(x, y), kDefaultVisualColor, 255u); - // Depth - ASSERT_NEAR(depth_.at(x, y)[0], 1.8f, kDepthTolerance); - // Label - ASSERT_EQ(label_.at(x, y)[0], kBodyID); -} - -TEST_F(RgbdRendererTest, MeshTest) { - SetUp(X_WR_, true); - - Isometry3d X_WV = Isometry3d::Identity(); - DrakeShapes::VisualElement visual(X_WV); - auto filename = - FindResourceOrThrow("drake/systems/sensors/test/models/meshes/box.obj"); - visual.setGeometry(DrakeShapes::Mesh("", filename)); - const int kBodyID = 0; - const RgbdRenderer::VisualIndex kVisualID(0); - renderer_->RegisterVisual(visual, kBodyID); - renderer_->UpdateVisualPose(X_WV, kBodyID, kVisualID); - Render(); - - VerifyOutliers(); - - // Verifies inside the cylinder. - const int x = kInlier.x; - const int y = kInlier.y; - // Color - CompareColor(color_.at(x, y), ColorI({4u, 241u, 33u}), 255u); - // Depth - ASSERT_NEAR(depth_.at(x, y)[0], 2., kDepthTolerance); - // Label - ASSERT_EQ(label_.at(x, y)[0], kBodyID); -} - -// TODO(kunimatsu-tri) Move DepthImageToPointCloudConversionTest here from -// rgbd_camera_test. - -} // anonymous namespace -} // namespace sensors -} // namespace systems -} // namespace drake diff --git a/systems/sensors/test/rgbd_renderer_test_util.h b/systems/sensors/test/rgbd_renderer_test_util.h new file mode 100644 index 000000000000..719139e3478f --- /dev/null +++ b/systems/sensors/test/rgbd_renderer_test_util.h @@ -0,0 +1,135 @@ +#pragma once + +#include + +#include +#include + +#include "drake/common/find_resource.h" +#include "drake/common/test_utilities/eigen_matrix_compare.h" +#include "drake/systems/sensors/image.h" + +namespace drake { +namespace systems { +namespace sensors { +namespace test { + +// Holds `(x, y)` indices of the screen coordinate system where the ranges of +// `x` and `y` are [0, kWidth) and [0, kHeight) respectively. +struct ScreenCoord { + int x{}; + int y{}; +}; + +void CompareColor(const uint8_t* pixel, + const sensors::ColorI& color, + int alpha, double tolerance) { + // Use ASSERT here instead of EXPECT_NEAR to stop all subsequent testing, + // because this function could be called inside for loops to search over all + // the pixels in an image. + ASSERT_NEAR(pixel[0], color.r, tolerance); + ASSERT_NEAR(pixel[1], color.g, tolerance); + ASSERT_NEAR(pixel[2], color.b, tolerance); + ASSERT_EQ(pixel[3], alpha); +} + +// This suite tests RgbdRenderer. +template +class RgbdRendererTest : public ::testing::Test { + public: + RgbdRendererTest() : + color_(kWidth, kHeight), depth_(kWidth, kHeight), label_(kWidth, kHeight), + // Looking strait down from 3m above the ground. + X_WC_(Eigen::Translation3d(0, 0, kDefaultDistance) * + Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(-M_PI_2, Eigen::Vector3d::UnitZ())) {} + + protected: + void Render() { + renderer_->RenderColorImage(&color_); + renderer_->RenderDepthImage(&depth_); + renderer_->RenderLabelImage(&label_); + } + + void VerifyUniformColor(const sensors::ColorI& pixel, int alpha) { + for (int y = 0; y < kHeight; ++y) { + for (int x = 0; x < kWidth; ++x) { + CompareColor(color_.at(x, y), pixel, alpha, kColorPixelTolerance); + } + } + } + + void VerifyUniformLabel(int16_t label) { + for (int y = 0; y < kHeight; ++y) { + for (int x = 0; x < kWidth; ++x) { + ASSERT_EQ(label_.at(x, y)[0], label); + } + } + } + + void VerifyUniformDepth(float depth) { + for (int y = 0; y < kHeight; ++y) { + for (int x = 0; x < kWidth; ++x) { + ASSERT_NEAR(depth_.at(x, y)[0], depth, kDepthTolerance); + } + } + } + + // Verifies the chosen pixels, i.e. `kOutliers`, belong to the ground. + void VerifyOutliers() { + const auto& kTerrain = renderer_->color_palette().get_terrain_color(); + for (const auto& screen_coord : kOutliers) { + const int x = screen_coord.x; + const int y = screen_coord.y; + // Color + CompareColor(color_.at(x, y), kTerrain, 255u, kColorPixelTolerance); + // Depth + ASSERT_NEAR(depth_.at(x, y)[0], kDefaultDistance, kDepthTolerance); + // Label + ASSERT_EQ(label_.at(x, y)[0], Label::kFlatTerrain); + } + } + + // All tests on this class must invoke this first. + void Init(const Eigen::Isometry3d& X_WC, bool add_terrain = false) { + renderer_ = std::make_unique( + RenderingConfig{kWidth, kHeight, kFovY, kZNear, kZFar, kShowWindow}, + X_WC); + + if (add_terrain) renderer_->AddFlatTerrain(); + } + + const int kWidth = 640; + const int kHeight = 480; + const double kZNear = 0.5; + const double kZFar = 5.; + const double kFovY = M_PI_4; + const bool kShowWindow = false; + + // The following tolerance is used due to a precision difference between + // Ubuntu Linux and Mac OSX. + const double kColorPixelTolerance = 1.001; + const double kDepthTolerance = 1.1e-4; + + const sensors::ColorI kDefaultVisualColor = {179u, 179u, 179u}; + const float kDefaultDistance{3.f}; + // `kInlier` is chosen to point to a pixel representing an object in the + // test scene. + const ScreenCoord kInlier = {320, 240}; + // The outliers are chosen to point to pixels representing the ground, + // not objects in the test scene. + const std::array kOutliers{{ + {10, 10}, {10, 470}, {630, 10}, {630, 470}}}; + + ImageRgba8U color_; + ImageDepth32F depth_; + ImageLabel16I label_; + Eigen::Isometry3d X_WC_; + + std::unique_ptr renderer_; +}; + +} // namespace test +} // namespace sensors +} // namespace systems +} // namespace drake diff --git a/systems/sensors/test/rgbd_renderer_vtk_test.cc b/systems/sensors/test/rgbd_renderer_vtk_test.cc new file mode 100644 index 000000000000..87e8eceb2dd6 --- /dev/null +++ b/systems/sensors/test/rgbd_renderer_vtk_test.cc @@ -0,0 +1,231 @@ +#include "drake/systems/sensors/rgbd_renderer_vtk.h" + +#include "drake/systems/sensors/test/rgbd_renderer_test_util.h" + +namespace drake { +namespace systems { +namespace sensors { +namespace test { + +using RgbdRendererVTKTest = RgbdRendererTest; +using Eigen::Isometry3d; + +TEST_F(RgbdRendererVTKTest, InstantiationTest) { + Init(Isometry3d::Identity()); + + EXPECT_EQ(renderer_->config().width, kWidth); + EXPECT_EQ(renderer_->config().height, kHeight); + EXPECT_EQ(renderer_->config().fov_y, kFovY); +} + +TEST_F(RgbdRendererVTKTest, NoBodyTest) { + Init(Isometry3d::Identity()); + Render(); + + VerifyUniformColor(renderer_->color_palette().get_sky_color(), 0u); + VerifyUniformLabel(Label::kNoBody); + // Verifies depth. + for (int y = 0; y < kHeight; ++y) { + for (int x = 0; x < kWidth; ++x) { + EXPECT_TRUE(std::isnan(depth_.at(x, y)[0])); + } + } +} + +TEST_F(RgbdRendererVTKTest, TerrainTest) { + Init(X_WC_, true); + + const auto& kTerrain = renderer_->color_palette().get_terrain_color(); + // At two different distances. + for (auto depth : std::array({{2.f, 5.f}})) { + X_WC_.translation().z() = depth; + renderer_->UpdateViewpoint(X_WC_); + Render(); + VerifyUniformColor(kTerrain, 255u); + VerifyUniformLabel(Label::kFlatTerrain); + VerifyUniformDepth(depth); + } + + // Closer than kZNear. + X_WC_.translation().z() = kZNear - 1e-5; + renderer_->UpdateViewpoint(X_WC_); + Render(); + VerifyUniformColor(kTerrain, 255u); + VerifyUniformLabel(Label::kFlatTerrain); + VerifyUniformDepth(InvalidDepth::kTooClose); + + // Farther than kZFar. + X_WC_.translation().z() = kZFar + 1e-3; + renderer_->UpdateViewpoint(X_WC_); + Render(); + VerifyUniformColor(kTerrain, 255u); + VerifyUniformLabel(Label::kFlatTerrain); + // Verifies depth. + for (int y = 0; y < kHeight; ++y) { + for (int x = 0; x < kWidth; ++x) { + ASSERT_EQ(InvalidDepth::kTooFar, depth_.at(x, y)[0]); + } + } +} + +TEST_F(RgbdRendererVTKTest, HorizonTest) { + // Camera at the origin, pointing in a direction parallel to the ground. + Isometry3d X_WC = + Eigen::Translation3d(0, 0, 0) * + Eigen::AngleAxisd(-M_PI_2, Eigen::Vector3d::UnitX()) * + Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d::UnitY()); + Init(X_WC, true); + + // Returns y in [0, kHeight / 2], index of horizon location in image + // coordinate system under two assumptions: 1) the gound plane is not clipped + // by `kClippingPlaneFar`, 2) camera is located above the ground. + auto CalcHorizon = [](double z, double fov, double height) { + const double kTerrainSize = 50.; + const double kFocalLength = height * 0.5 / std::tan(0.5 * fov); + return 0.5 * height + z / kTerrainSize * kFocalLength; + }; + + // Verifies v index of horizon at three different camera heights. + const std::array Zs{{2., 1., 0.5}}; + for (const auto& z : Zs) { + X_WC.translation().z() = z; + renderer_->UpdateViewpoint(X_WC); + Render(); + + const auto& kTerrain = renderer_->color_palette().get_terrain_color(); + int actual_horizon{0}; + for (int y = 0; y < kHeight; ++y) { + // Looking for the boundary between the sky and the ground. + if ((static_cast(kTerrain.r == color_.at(0, y)[0])) && + (static_cast(kTerrain.g == color_.at(0, y)[1])) && + (static_cast(kTerrain.b == color_.at(0, y)[2]))) { + actual_horizon = y; + break; + } + } + + const double expected_horizon = CalcHorizon(z, kFovY, kHeight); + ASSERT_NEAR(expected_horizon, actual_horizon, 1.001); + } +} + +TEST_F(RgbdRendererVTKTest, BoxTest) { + Init(X_WC_, true); + + // Sets up a box. + Isometry3d X_WV = Isometry3d::Identity(); + X_WV.translation().z() = 0.5; + DrakeShapes::VisualElement visual(X_WV); + Eigen::Vector3d box_size(1, 1, 1); + visual.setGeometry(DrakeShapes::Box(box_size)); + const int kBodyID = 0; + const RgbdRenderer::VisualIndex kVisualID(0); + renderer_->RegisterVisual(visual, kBodyID); + renderer_->UpdateVisualPose(X_WV, kBodyID, kVisualID); + Render(); + + VerifyOutliers(); + + // Verifies inside the box. + const int x = kInlier.x; + const int y = kInlier.y; + // Color + CompareColor(color_.at(x, y), kDefaultVisualColor, 255u, + kColorPixelTolerance); + // Depth + ASSERT_NEAR(depth_.at(x, y)[0], 2.f, kDepthTolerance); + // Label + ASSERT_EQ(label_.at(x, y)[0], kBodyID); +} + +TEST_F(RgbdRendererVTKTest, SphereTest) { + Init(X_WC_, true); + + // Sets up a sphere. + Isometry3d X_WV = Isometry3d::Identity(); + X_WV.translation().z() = 0.5; + DrakeShapes::VisualElement visual(X_WV); + visual.setGeometry(DrakeShapes::Sphere(0.5)); + const int kBodyID = 0; + const RgbdRenderer::VisualIndex kVisualID(0); + renderer_->RegisterVisual(visual, kBodyID); + renderer_->UpdateVisualPose(X_WV, kBodyID, kVisualID); + Render(); + + VerifyOutliers(); + + // Verifies inside the sphere. + const int x = kInlier.x; + const int y = kInlier.y; + // Color + CompareColor(color_.at(x, y), kDefaultVisualColor, 255u, + kColorPixelTolerance); + // Depth + ASSERT_NEAR(depth_.at(x, y)[0], 2.f, kDepthTolerance); + // Label + ASSERT_EQ(label_.at(x, y)[0], kBodyID); +} + +TEST_F(RgbdRendererVTKTest, CylinderTest) { + Init(X_WC_, true); + + // Sets up a sphere. + Isometry3d X_WV = Isometry3d::Identity(); + X_WV.translation().z() = 0.6; + DrakeShapes::VisualElement visual(X_WV); + visual.setGeometry(DrakeShapes::Cylinder(0.2, 1.2)); // Radius and length. + const int kBodyID = 1; + const RgbdRenderer::VisualIndex kVisualID(0); + renderer_->RegisterVisual(visual, kBodyID); + renderer_->UpdateVisualPose(X_WV, kBodyID, kVisualID); + Render(); + + VerifyOutliers(); + + // Verifies inside the cylinder. + const int x = kInlier.x; + const int y = kInlier.y; + // Color + CompareColor(color_.at(x, y), kDefaultVisualColor, 255u, + kColorPixelTolerance); + // Depth + ASSERT_NEAR(depth_.at(x, y)[0], 1.8f, kDepthTolerance); + // Label + ASSERT_EQ(label_.at(x, y)[0], kBodyID); +} + +TEST_F(RgbdRendererVTKTest, MeshTest) { + Init(X_WC_, true); + + Isometry3d X_WV = Isometry3d::Identity(); + DrakeShapes::VisualElement visual(X_WV); + auto filename = + FindResourceOrThrow("drake/systems/sensors/test/models/meshes/box.obj"); + visual.setGeometry(DrakeShapes::Mesh("", filename)); + const int kBodyID = 0; + const RgbdRenderer::VisualIndex kVisualID(0); + renderer_->RegisterVisual(visual, kBodyID); + renderer_->UpdateVisualPose(X_WV, kBodyID, kVisualID); + Render(); + + VerifyOutliers(); + + // Verifies inside the cylinder. + const int x = kInlier.x; + const int y = kInlier.y; + // Color + CompareColor(color_.at(x, y), ColorI({4u, 241u, 33u}), 255u, + kColorPixelTolerance); + // Depth + ASSERT_NEAR(depth_.at(x, y)[0], 2., kDepthTolerance); + // Label + ASSERT_EQ(label_.at(x, y)[0], kBodyID); +} + +// TODO(kunimatsu-tri) Move DepthImageToPointCloudConversionTest here from +// rgbd_camera_test. + +} // namespace test +} // namespace sensors +} // namespace systems +} // namespace drake From 1bfef36f17db3d23d3bf15b6be5855611d80ea74 Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Thu, 21 Dec 2017 18:08:23 +0000 Subject: [PATCH 02/25] python binding for 'RelativeQuatConstraint' --- bindings/pydrake/solvers/ik.py | 1 + bindings/pydrake/solvers/ik_py.cc | 15 +++++++++++++++ 2 files changed, 16 insertions(+) diff --git a/bindings/pydrake/solvers/ik.py b/bindings/pydrake/solvers/ik.py index 46391498338a..d5717d56c5be 100644 --- a/bindings/pydrake/solvers/ik.py +++ b/bindings/pydrake/solvers/ik.py @@ -9,6 +9,7 @@ InverseKinPointwise, PostureConstraint, RelativePositionConstraint, + RelativeQuatConstraint, WorldEulerConstraint, WorldQuatConstraint, WorldGazeDirConstraint, diff --git a/bindings/pydrake/solvers/ik_py.cc b/bindings/pydrake/solvers/ik_py.cc index 555d41dd96f9..19211d90c4e1 100644 --- a/bindings/pydrake/solvers/ik_py.cc +++ b/bindings/pydrake/solvers/ik_py.cc @@ -59,6 +59,21 @@ PYBIND11_MODULE(_ik_py, m) { py::arg("bTbp"), py::arg("tspan") = DrakeRigidBodyConstraint::default_tspan); + py::class_( + m, "RelativeQuatConstraint") + .def(py::init*, + int, + int, + const Eigen::Vector4d&, + double, + const Eigen::Vector2d&>(), + py::arg("model"), + py::arg("bodyA_idx"), + py::arg("bodyB_idx"), + py::arg("quat_des"), + py::arg("tol"), + py::arg("tspan") = DrakeRigidBodyConstraint::default_tspan); + py::class_( m, "WorldPositionInFrameConstraint") .def(py::init*, From 13c40e5ebcb179f4bd3d8e0983a05d705ea603ef Mon Sep 17 00:00:00 2001 From: Hongkai Dai Date: Thu, 21 Dec 2017 15:05:31 -0500 Subject: [PATCH 03/25] Scs sdp solver (#7555) * SCS solver parses PositiveSemidefiniteConstraint. * Parse LinearMatrixInequalityConstraint in SCS. * lint * update the solver table. * minor fix after rebasing. * Address Andres and Sherm's reviews. --- solvers/BUILD.bazel | 1 + solvers/constraint.h | 2 +- solvers/mathematical_program.cc | 3 +- solvers/mathematical_program.h | 4 +- solvers/scs_solver.cc | 102 ++++++++++++++++++ solvers/test/mosek_solver_test.cc | 8 +- solvers/test/scs_solver_test.cc | 29 +++++ solvers/test/semidefinite_program_examples.cc | 78 +++++++------- solvers/test/semidefinite_program_examples.h | 84 ++++++++------- 9 files changed, 230 insertions(+), 81 deletions(-) diff --git a/solvers/BUILD.bazel b/solvers/BUILD.bazel index 3ba3bc5deb9a..cff7099f7d1d 100644 --- a/solvers/BUILD.bazel +++ b/solvers/BUILD.bazel @@ -987,6 +987,7 @@ drake_cc_googletest( ":mathematical_program_test_util", ":quadratic_program_examples", ":second_order_cone_program_examples", + ":semidefinite_program_examples", ], ) diff --git a/solvers/constraint.h b/solvers/constraint.h index 4ba6ab0f248d..e75a12aaac2d 100644 --- a/solvers/constraint.h +++ b/solvers/constraint.h @@ -699,7 +699,7 @@ class PositiveSemidefiniteConstraint : public Constraint { /** * Impose the matrix inequality constraint on variable x * - * F0 + x1 * F1 + ... xn * Fn is p.s.d + * F₀ + x₁ * F₁ + ... xₙ * Fₙ is p.s.d * <--> * @f[ * F_0 + x_1 F_1 + ... + x_n F_n \text{ is p.s.d} diff --git a/solvers/mathematical_program.cc b/solvers/mathematical_program.cc index 1ecd610b6f7a..bd6ec06a5ed4 100644 --- a/solvers/mathematical_program.cc +++ b/solvers/mathematical_program.cc @@ -87,7 +87,8 @@ AttributesSet kMosekCapabilities = // Scs solver capatilities. AttributesSet kScsCapabilities = (kLinearEqualityConstraint | kLinearConstraint | kLorentzConeConstraint | - kRotatedLorentzConeConstraint | kLinearCost | kQuadraticCost); + kRotatedLorentzConeConstraint | kLinearCost | kQuadraticCost | + kPositiveSemidefiniteConstraint); // Solvers for generic systems of constraints and costs. AttributesSet kGenericSolverCapabilities = diff --git a/solvers/mathematical_program.h b/solvers/mathematical_program.h index 8382501df1a9..c73259bae6c1 100644 --- a/solvers/mathematical_program.h +++ b/solvers/mathematical_program.h @@ -95,8 +95,8 @@ namespace solvers { * ♦ * ♦ * ♦ - * - * + * ♦ + * ♦ * * * diff --git a/solvers/scs_solver.cc b/solvers/scs_solver.cc index 5a489d800022..04e5f54acffa 100644 --- a/solvers/scs_solver.cc +++ b/solvers/scs_solver.cc @@ -334,6 +334,104 @@ void ParseSecondOrderConeConstraints( } } +// This function parses both PositiveSemidefinite and +// LinearMatrixInequalityConstraint. +void ParsePositiveSemidefiniteConstraint( + const MathematicalProgram& prog, + std::vector>* A_triplets, std::vector* b, + int* A_row_count, SCS_CONE* cone) { + std::vector psd_cone_length; + const double sqrt2 = std::sqrt(2); + for (const auto& psd_constraint : prog.positive_semidefinite_constraints()) { + // PositiveSemidefiniteConstraint encodes the matrix X being psd. + // We convert it to SCS form + // A * x + s = 0 + // s in positive semidefinite cone. + // where A is a diagonal matrix, with its diagonal entries being the stacked + // column vector of the lower triangular part of matrix + // ⎡ -1 -√2 -√2 ... -√2⎤ + // ⎢-√2 -1 -√2 ... -√2⎥ + // ⎢-√2 -√2 -1 ... -√2⎥ + // ⎢ ... ⎥ + // ⎣-√2 -√2 -√2 ... -1⎦ + // The √2 scaling factor in the off-diagonal entries are required by SCS, + // as it uses only the lower triangular part of the symmetric matrix, as + // explained in https://github.com/cvxgrp/scs + // x is the stacked column vector of the lower triangular part of the + // symmetric matrix X. + const int X_rows = psd_constraint.constraint()->matrix_rows(); + int x_index_count = 0; + const VectorXDecisionVariable& flat_X = psd_constraint.variables(); + DRAKE_DEMAND(flat_X.rows() == X_rows * X_rows); + b->reserve(b->size() + X_rows * (X_rows + 1) / 2); + for (int j = 0; j < X_rows; ++j) { + A_triplets->emplace_back( + *A_row_count + x_index_count, + prog.FindDecisionVariableIndex(flat_X(j * X_rows + j)), -1); + b->push_back(0); + ++x_index_count; + for (int i = j + 1; i < X_rows; ++i) { + A_triplets->emplace_back( + *A_row_count + x_index_count, + prog.FindDecisionVariableIndex(flat_X(j * X_rows + i)), -sqrt2); + b->push_back(0); + ++x_index_count; + } + } + (*A_row_count) += X_rows * (X_rows + 1) / 2; + psd_cone_length.push_back(X_rows); + } + for (const auto& lmi_constraint : + prog.linear_matrix_inequality_constraints()) { + // LinearMatrixInequalityConstraint encodes + // F₀ + x₁*F₁ + x₂*F₂ + ... + xₙFₙ is p.s.d + // We convert this to SCS form as + // A_cone * x + s = b_cone + // s in SCS positive semidefinite cone. + // where + // ⎡ F₁(0, 0) F₂(0, 0) ... Fₙ(0, 0)⎤ + // ⎢√2F₁(1, 0) √2F₂(1, 0) ... √2Fₙ(1, 0)⎥ + // A_cone = - ⎢√2F₁(2, 0) √2F₂(2, 0) ... √2Fₙ(2, 0)⎥, + // ⎢ ... ⎥ + // ⎣ F₁(m, m) F₂(m, m) ... Fₙ(m, m)⎦ + // + // ⎡ F₀(0, 0)⎤ + // ⎢√2F₀(1, 0)⎥ + // b_cone = ⎢√2F₀(2, 0)⎥, + // ⎢ ... ⎥ + // ⎣ F₀(m, m)⎦ + // and + // x = [x₁; x₂; ... ; xₙ]. + // As explained above, the off-diagonal rows are scaled by √2. Please refer + // to https://github.com/cvxgrp/scs about the scaling factor √2. + const std::vector& F = lmi_constraint.constraint()->F(); + const VectorXDecisionVariable& x = lmi_constraint.variables(); + const int F_rows = lmi_constraint.constraint()->matrix_rows(); + const std::vector x_indices = FindDecisionVariableIndices(prog, x); + int A_cone_row_count = 0; + b->reserve(b->size() + F_rows * (F_rows + 1) / 2); + for (int j = 0; j < F_rows; ++j) { + for (int i = j; i < F_rows; ++i) { + const double scale_factor = i == j ? 1 : sqrt2; + for (int k = 1; k < static_cast(F.size()); ++k) { + A_triplets->emplace_back(*A_row_count + A_cone_row_count, + x_indices[k - 1], + -scale_factor * F[k](i, j)); + } + b->push_back(scale_factor * F[0](i, j)); + ++A_cone_row_count; + } + } + *A_row_count += F_rows * (F_rows + 1) / 2; + psd_cone_length.push_back(F_rows); + } + cone->ssize = psd_cone_length.size(); + cone->s = static_cast(scs_calloc(cone->ssize, sizeof(scs_int))); + for (int i = 0; i < cone->ssize; ++i) { + cone->s[i] = psd_cone_length[i]; + } +} + std::string Scs_return_info(scs_int scs_status) { switch (scs_status) { case SCS_INFEASIBLE_INACCURATE: @@ -522,6 +620,10 @@ SolutionResult ScsSolver::Solve(MathematicalProgram& prog) const { cone->q[i] = lorentz_cone_length[i]; } + // Parse PositiveSemidefiniteConstraint and LinearMatrixInequalityConstraint. + ParsePositiveSemidefiniteConstraint(prog, &A_triplets, &b, &A_row_count, + cone); + Eigen::SparseMatrix A(A_row_count, num_x); A.setFromTriplets(A_triplets.begin(), A_triplets.end()); A.makeCompressed(); diff --git a/solvers/test/mosek_solver_test.cc b/solvers/test/mosek_solver_test.cc index f9e6adf65eea..906d20ac401b 100644 --- a/solvers/test/mosek_solver_test.cc +++ b/solvers/test/mosek_solver_test.cc @@ -75,28 +75,28 @@ INSTANTIATE_TEST_CASE_P( GTEST_TEST(TestSemidefiniteProgram, TrivialSDP) { MosekSolver mosek_solver; if (mosek_solver.available()) { - TestTrivialSDP(mosek_solver); + TestTrivialSDP(mosek_solver, 1E-8); } } GTEST_TEST(TestSemidefiniteProgram, CommonLyapunov) { MosekSolver mosek_solver; if (mosek_solver.available()) { - FindCommonLyapunov(mosek_solver); + FindCommonLyapunov(mosek_solver, 1E-8); } } GTEST_TEST(TestSemidefiniteProgram, OuterEllipsoid) { MosekSolver mosek_solver; if (mosek_solver.available()) { - FindOuterEllipsoid(mosek_solver); + FindOuterEllipsoid(mosek_solver, 1E-6); } } GTEST_TEST(TestSemidefiniteProgram, EigenvalueProblem) { MosekSolver mosek_solver; if (mosek_solver.available()) { - SolveEigenvalueProblem(mosek_solver); + SolveEigenvalueProblem(mosek_solver, 1E-7); } } } // namespace test diff --git a/solvers/test/scs_solver_test.cc b/solvers/test/scs_solver_test.cc index 973682552f27..83e4848c9937 100644 --- a/solvers/test/scs_solver_test.cc +++ b/solvers/test/scs_solver_test.cc @@ -8,6 +8,7 @@ #include "drake/solvers/test/mathematical_program_test_util.h" #include "drake/solvers/test/quadratic_program_examples.h" #include "drake/solvers/test/second_order_cone_program_examples.h" +#include "drake/solvers/test/semidefinite_program_examples.h" namespace drake { namespace solvers { @@ -203,6 +204,34 @@ GTEST_TEST(QPtest, TestUnitBallExample) { TestQPonUnitBallExample(solver); } } + +GTEST_TEST(TestSemidefiniteProgram, TrivialSDP) { + ScsSolver scs_solver; + if (scs_solver.available()) { + TestTrivialSDP(scs_solver, 2E-3); + } +} + +GTEST_TEST(TestSemidefiniteProgram, CommonLyapunov) { + ScsSolver scs_solver; + if (scs_solver.available()) { + FindCommonLyapunov(scs_solver, 5E-4); + } +} + +GTEST_TEST(TestSemidefiniteProgram, OuterEllipsoid) { + ScsSolver scs_solver; + if (scs_solver.available()) { + FindOuterEllipsoid(scs_solver, 1E-3); + } +} + +GTEST_TEST(TestSemidefiniteProgram, EigenvalueProblem) { + ScsSolver scs_solver; + if (scs_solver.available()) { + SolveEigenvalueProblem(scs_solver, 1E-3); + } +} } // namespace test } // namespace solvers } // namespace drake diff --git a/solvers/test/semidefinite_program_examples.cc b/solvers/test/semidefinite_program_examples.cc index 7c3d474010c0..747d0fbe5878 100644 --- a/solvers/test/semidefinite_program_examples.cc +++ b/solvers/test/semidefinite_program_examples.cc @@ -1,5 +1,6 @@ #include "drake/solvers/test/semidefinite_program_examples.h" +#include #include #include @@ -15,7 +16,8 @@ using Eigen::Matrix3d; using Eigen::Vector3d; using Eigen::Matrix4d; -void TestTrivialSDP(const MathematicalProgramSolverInterface& solver) { +void TestTrivialSDP(const MathematicalProgramSolverInterface& solver, + double tol) { MathematicalProgram prog; auto S = prog.NewSymmetricContinuousVariables<2>("S"); @@ -33,14 +35,16 @@ void TestTrivialSDP(const MathematicalProgramSolverInterface& solver) { auto S_value = prog.GetSolution(S); - // Choose 1E-8 since that is Mosek's default feasibility tolerance. - EXPECT_TRUE(CompareMatrices(S_value, Eigen::Matrix2d::Ones(), 1E-8)); + EXPECT_TRUE(CompareMatrices(S_value, Eigen::Matrix2d::Ones(), tol)); } -void FindCommonLyapunov(const MathematicalProgramSolverInterface& solver) { +void FindCommonLyapunov(const MathematicalProgramSolverInterface& solver, + double tol) { MathematicalProgram prog; auto P = prog.NewSymmetricContinuousVariables<3>("P"); - prog.AddPositiveSemidefiniteConstraint(P - 1E-3 * Matrix3d::Identity()); + const double psd_epsilon{1E-3}; + prog.AddPositiveSemidefiniteConstraint(P - + psd_epsilon * Matrix3d::Identity()); Eigen::Matrix3d A1; // clang-format off A1 << -1, -1, -2, @@ -48,7 +52,7 @@ void FindCommonLyapunov(const MathematicalProgramSolverInterface& solver) { 0, 0, -1; // clang-format on auto binding1 = prog.AddPositiveSemidefiniteConstraint( - -A1.transpose() * P - P * A1 - 1E-3 * Matrix3d::Identity()); + -A1.transpose() * P - P * A1 - psd_epsilon * Matrix3d::Identity()); Eigen::Matrix3d A2; // clang-format off @@ -57,7 +61,7 @@ void FindCommonLyapunov(const MathematicalProgramSolverInterface& solver) { 0, 0, -0.4; // clang-format on auto binding2 = prog.AddPositiveSemidefiniteConstraint( - -A2.transpose() * P - P * A2 - 1E-3 * Matrix3d::Identity()); + -A2.transpose() * P - P * A2 - psd_epsilon * Matrix3d::Identity()); RunSolver(&prog, solver); @@ -78,15 +82,16 @@ void FindCommonLyapunov(const MathematicalProgramSolverInterface& solver) { EXPECT_GE(eigen_solver_Q1.eigenvalues().minCoeff(), 0); Eigen::SelfAdjointEigenSolver eigen_solver_Q2(Q2_value); EXPECT_GE(eigen_solver_Q2.eigenvalues().minCoeff(), 0); - EXPECT_TRUE(CompareMatrices( - A1.transpose() * P_value + P_value * A1 + 1E-3 * Matrix3d::Identity(), - -Q1_value, 1E-8, MatrixCompareType::absolute)); - EXPECT_TRUE(CompareMatrices( - A2.transpose() * P_value + P_value * A2 + 1E-3 * Matrix3d::Identity(), - -Q2_value, 1E-8, MatrixCompareType::absolute)); + EXPECT_TRUE(CompareMatrices(A1.transpose() * P_value + P_value * A1 + + psd_epsilon * Matrix3d::Identity(), + -Q1_value, tol, MatrixCompareType::absolute)); + EXPECT_TRUE(CompareMatrices(A2.transpose() * P_value + P_value * A2 + + psd_epsilon * Matrix3d::Identity(), + -Q2_value, tol, MatrixCompareType::absolute)); } -void FindOuterEllipsoid(const MathematicalProgramSolverInterface& solver) { +void FindOuterEllipsoid(const MathematicalProgramSolverInterface& solver, + double tol) { std::array Q; std::array b; Q[0] = Matrix3d::Identity(); @@ -117,22 +122,19 @@ void FindOuterEllipsoid(const MathematicalProgramSolverInterface& solver) { prog.AddPositiveSemidefiniteConstraint(M); } - // I have to compute trace of P in the loop, instead of using P.trace(), - // since Variable cannot be cast to Expression implicitly. - symbolic::Expression P_trace{0}; - for (int i = 0; i < 3; ++i) { - P_trace += P(i, i); - } - prog.AddLinearCost(-P_trace); + prog.AddLinearCost(-P.cast().trace()); RunSolver(&prog, solver); - auto P_value = prog.GetSolution(P); - auto s_value = prog.GetSolution(s); - auto c_value = prog.GetSolution(c); + const auto P_value = prog.GetSolution(P); + const auto s_value = prog.GetSolution(s); + const auto c_value = prog.GetSolution(c); - Eigen::SelfAdjointEigenSolver es_P(P_value); - EXPECT_TRUE((es_P.eigenvalues().array() >= -1E-6).all()); + const Eigen::SelfAdjointEigenSolver es_P(P_value); + EXPECT_TRUE((es_P.eigenvalues().array() >= -tol).all()); + // The minimal eigen value of M should be 0, since the optimality happens at + // the boundary of the PSD cone. + double M_min_eigenvalue = std::numeric_limits::infinity(); for (int i = 0; i < 3; ++i) { Matrix4d M_value; // clang-format off @@ -140,11 +142,15 @@ void FindOuterEllipsoid(const MathematicalProgramSolverInterface& solver) { s_value(i) * b[i].transpose() - c_value.transpose(), 1 - s_value(i); // clang-format on Eigen::SelfAdjointEigenSolver es_M(M_value); - EXPECT_TRUE((es_M.eigenvalues().array() >= -1E-6).all()); + EXPECT_TRUE((es_M.eigenvalues().array() >= -tol).all()); + M_min_eigenvalue = + std::min(M_min_eigenvalue, es_M.eigenvalues().minCoeff()); } + EXPECT_NEAR(M_min_eigenvalue, 0, tol); } -void SolveEigenvalueProblem(const MathematicalProgramSolverInterface& solver) { +void SolveEigenvalueProblem(const MathematicalProgramSolverInterface& solver, + double tol) { MathematicalProgram prog; auto x = prog.NewContinuousVariables<2>("x"); Matrix3d F1; @@ -161,22 +167,22 @@ void SolveEigenvalueProblem(const MathematicalProgramSolverInterface& solver) { prog.AddLinearMatrixInequalityConstraint( {Matrix3d::Zero(), Matrix3d::Identity(), -F1, -F2}, {z, x}); - Vector2d x_lb(0.1, 1); - Vector2d x_ub(2, 3); + const Vector2d x_lb(0.1, 1); + const Vector2d x_ub(2, 3); prog.AddBoundingBoxConstraint(x_lb, x_ub, x); - prog.AddLinearCost(drake::Vector1d(1), z); + prog.AddLinearCost(z(0)); RunSolver(&prog, solver); const double z_value = prog.GetSolution(z(0)); - auto x_value = prog.GetSolution(x); - auto xF_sum = x_value(0) * F1 + x_value(1) * F2; + const auto x_value = prog.GetSolution(x); + const auto xF_sum = x_value(0) * F1 + x_value(1) * F2; Eigen::SelfAdjointEigenSolver eigen_solver_xF(xF_sum); - // The comparison tolerance is set to 1E-7, slightly larger than Mosek's - // default feasibility tolerance 1E-8. - EXPECT_NEAR(z_value, eigen_solver_xF.eigenvalues().maxCoeff(), 1E-7); + EXPECT_NEAR(z_value, eigen_solver_xF.eigenvalues().maxCoeff(), tol); + EXPECT_TRUE(((x_value - x_lb).array() >= -tol).all()); + EXPECT_TRUE(((x_value - x_ub).array() <= tol).all()); } } // namespace test } // namespace solvers diff --git a/solvers/test/semidefinite_program_examples.h b/solvers/test/semidefinite_program_examples.h index 61fae5253566..d2634a8ce300 100644 --- a/solvers/test/semidefinite_program_examples.h +++ b/solvers/test/semidefinite_program_examples.h @@ -12,46 +12,56 @@ namespace test { /// The analytical solution is /// S = [1 1] /// [1 1] -void TestTrivialSDP(const MathematicalProgramSolverInterface& solver); +void TestTrivialSDP(const MathematicalProgramSolverInterface& solver, + double tol); -/// Solve a semidefinite programming problem. -/// Find the common Lyapunov function for linear systems -/// xdot = Ai*x -/// The condition is -/// min 0 -/// s.t P is positive definite -/// - (Ai'*P + P*Ai) is positive definite -void FindCommonLyapunov(const MathematicalProgramSolverInterface& solver); +// Solve a semidefinite programming problem. +// Find the common Lyapunov function for linear systems +// xdot = Ai*x +// The condition is +// min 0 +// s.t P is positive definite +// - (Ai'*P + P*Ai) is positive definite +void FindCommonLyapunov(const MathematicalProgramSolverInterface& solver, + double tol); +/* + * Given some ellipsoids ℰᵢ : xᵀQᵢx + 2 bᵢᵀx ≤ 1, i = 1, 2, ..., n, find an + * ellipsoid xᵀPx + 2cᵀx ≤ 1 as an outer approximation for the union of + * ellipsoids ℰᵢ. + * + * Using s-lemma, the ellipsoid xᵀPx + 2cᵀx ≤ 1 contains the ellipsoid ℰᵢ, + * if and only if there exists a scalar sᵢ ≥ 0 such that + * + * (1 - xᵀPx - cᵀx) - sᵢ(1 - xᵀQᵢx - bᵢᵀx) ≥ 0 ∀x. + * + * This is equivalent to requiring that the matrix + * + * ⎡sᵢQᵢ - P sᵢbᵢ - c⎤ + * ⎣sᵢbᵢᵀ - cᵀ 1 - sᵢ⎦ + * + * is positive semidefinite. + * + * In order to find a tight outer approximation, we choose to maximize the + * trace of P. The optimization problem becomes + * + * min_{P, c, si} -trace(P) + * s.t ⎡sᵢQᵢ - P sᵢbᵢ - c⎤ is p.s.d + * ⎣sᵢbᵢᵀ - cᵀ 1 - sᵢ⎦ + * P is p.s.d + */ +void FindOuterEllipsoid(const MathematicalProgramSolverInterface& solver, + double tol); -/// Given some ellipsoids ℰᵢ : xᵀ*Qi*x + 2 * biᵀ * x <= 1, i = 1, 2, ..., n, -/// find an ellipsoid -/// xᵀ*P*x + 2*cᵀ*x <= 1 as an outer approximation for the union of ellipsoids -/// ℰᵢ. -/// Using s-lemma, the ellipsoid xᵀ*P*x + 2*cᵀ*x <= 1 contains the ellipsoid -/// ℰᵢ : xᵀ*Qi*x + 2*biᵀ*x <= 1, if and only if there exists a scalar si >= 0 -/// and (1 - xᵀ*P*x - cᵀ * x) - si*(1 - xᵀ*Qi*x - biᵀ * x) >= 0 ∀x. -/// Namely the matrix -/// [ si*Qi - P si*bi - c] -/// [si*biᵀ - cᵀ 1 - si] -/// is positive semidefinite. -/// In order to find a tight outer approximation, we choose to minimize the -/// trace of P. -/// The optimiation problem becomes -/// min_{P, c, si} -trace(P) -/// s.t [ si*Qi - P si*bi - c] is p.s.d -/// [si*biᵀ - cᵀ 1 - si] -/// P is p.s.d -void FindOuterEllipsoid(const MathematicalProgramSolverInterface& solver); - -/// Solve an eigen value problem through a semidefinite programming. -/// Minimize the maximum eigen value of a matrix that depends affinely on a -/// variable x -/// min z -/// s.t z * Identity - x1 * F1 - ... - xn * Fn is p.s.d -/// A * x <= b -/// C * x = d -void SolveEigenvalueProblem(const MathematicalProgramSolverInterface& solver); +// Solve an eigen value problem through a semidefinite programming. +// Minimize the maximum eigen value of a matrix that depends affinely on a +// variable x +// min z +// s.t z * Identity - x1 * F1 - ... - xn * Fn is p.s.d +// A * x <= b +// C * x = d +void SolveEigenvalueProblem(const MathematicalProgramSolverInterface& solver, + double tol); } // namespace test } // namespace solvers } // namespace drake From 39ed50ec769b598da8b892f0d1e1ad476f4ece65 Mon Sep 17 00:00:00 2001 From: Eric Cousineau Date: Thu, 21 Dec 2017 16:20:18 -0500 Subject: [PATCH 04/25] pydrake.systems: Add initial bindings and examples. --- WORKSPACE | 4 +- bindings/pydrake/BUILD.bazel | 11 +- bindings/pydrake/solvers/BUILD.bazel | 12 +- bindings/pydrake/solvers/__init__.py | 1 + bindings/pydrake/systems/BUILD.bazel | 167 +++++++++++++ bindings/pydrake/systems/__init__.py | 1 + bindings/pydrake/systems/all.py | 8 + bindings/pydrake/systems/analysis_py.cc | 26 ++ bindings/pydrake/systems/drawing.py | 40 ++++ bindings/pydrake/systems/framework_py.cc | 222 ++++++++++++++++++ bindings/pydrake/systems/primitives_py.cc | 37 +++ bindings/pydrake/systems/test/custom_test.py | 87 +++++++ bindings/pydrake/systems/test/general_test.py | 141 +++++++++++ .../pydrake/systems/test/graphviz_example.py | 25 ++ .../pydrake/systems/test/lifetime_test.py | 101 ++++++++ .../systems/test/lifetime_test_util_py.cc | 58 +++++ bindings/pydrake/systems/test/vector_test.py | 39 +++ tools/skylark/drake_py.bzl | 4 + 18 files changed, 972 insertions(+), 12 deletions(-) create mode 100644 bindings/pydrake/systems/BUILD.bazel create mode 100644 bindings/pydrake/systems/__init__.py create mode 100644 bindings/pydrake/systems/all.py create mode 100644 bindings/pydrake/systems/analysis_py.cc create mode 100644 bindings/pydrake/systems/drawing.py create mode 100644 bindings/pydrake/systems/framework_py.cc create mode 100644 bindings/pydrake/systems/primitives_py.cc create mode 100644 bindings/pydrake/systems/test/custom_test.py create mode 100644 bindings/pydrake/systems/test/general_test.py create mode 100644 bindings/pydrake/systems/test/graphviz_example.py create mode 100644 bindings/pydrake/systems/test/lifetime_test.py create mode 100644 bindings/pydrake/systems/test/lifetime_test_util_py.cc create mode 100644 bindings/pydrake/systems/test/vector_test.py diff --git a/WORKSPACE b/WORKSPACE index 93d50750d5a7..5b7464513f08 100644 --- a/WORKSPACE +++ b/WORKSPACE @@ -280,8 +280,8 @@ github_archive( github_archive( name = "pybind11", repository = "RobotLocomotion/pybind11", - commit = "ffcf754ae9e766632610975d22372a86a7b63014", - sha256 = "7cd6f4efb02bf9ae17eeb2afba68023af913e61ae76e8b4254203d0eec019525", # noqa + commit = "48999b69bde29cdf8d616d4fbd3d6ab1c561027d", + sha256 = "2ea18adfb608948cab1b5978081dc8c318ed47573ccd66f1603a37fbdbfc56da", # noqa build_file = "tools/workspace/pybind11/pybind11.BUILD.bazel", ) diff --git a/bindings/pydrake/BUILD.bazel b/bindings/pydrake/BUILD.bazel index a49cb3952971..c96951aaf4b9 100644 --- a/bindings/pydrake/BUILD.bazel +++ b/bindings/pydrake/BUILD.bazel @@ -116,13 +116,16 @@ PYBIND_LIBRARIES = adjust_labels_for_drake_hoist([ ":rbtree_py", ":symbolic_py", "//drake/bindings/pydrake/solvers", + "//drake/bindings/pydrake/systems", ]) +PY_LIBRARIES = [ + ":util_py", +] + install( name = "install", - targets = [ - ":util_py", - ], + targets = PY_LIBRARIES, py_dest = get_pybind_library_dest(), visibility = ["//visibility:public"], deps = get_drake_pybind_installs(PYBIND_LIBRARIES), @@ -131,7 +134,7 @@ install( drake_py_library( name = "pydrake", visibility = ["//visibility:public"], - deps = PYBIND_LIBRARIES, + deps = PYBIND_LIBRARIES + PY_LIBRARIES, ) # Test ODR (One Definition Rule). diff --git a/bindings/pydrake/solvers/BUILD.bazel b/bindings/pydrake/solvers/BUILD.bazel index deccf05e5ed2..ee6d54aede8d 100644 --- a/bindings/pydrake/solvers/BUILD.bazel +++ b/bindings/pydrake/solvers/BUILD.bazel @@ -81,18 +81,18 @@ PYBIND_LIBRARIES = [ ":mosek_py", ] +PY_LIBRARIES = [ + ":module_py", +] + drake_py_library( name = "solvers", - deps = PYBIND_LIBRARIES + [ - ":module_py", - ], + deps = PYBIND_LIBRARIES + PY_LIBRARIES, ) install( name = "install", - targets = [ - ":module_py", - ], + targets = PY_LIBRARIES, py_dest = get_pybind_library_dest(), deps = get_drake_pybind_installs(PYBIND_LIBRARIES), ) diff --git a/bindings/pydrake/solvers/__init__.py b/bindings/pydrake/solvers/__init__.py index e69de29bb2d1..8df4b86ebc5a 100644 --- a/bindings/pydrake/solvers/__init__.py +++ b/bindings/pydrake/solvers/__init__.py @@ -0,0 +1 @@ +# Blank Python module. diff --git a/bindings/pydrake/systems/BUILD.bazel b/bindings/pydrake/systems/BUILD.bazel new file mode 100644 index 000000000000..dbbce7c7740c --- /dev/null +++ b/bindings/pydrake/systems/BUILD.bazel @@ -0,0 +1,167 @@ +# -*- python -*- + +load("@drake//tools/install:install.bzl", "install") +load("//tools/lint:lint.bzl", "add_lint_tests") +load( + "//tools/skylark:pybind.bzl", + "drake_pybind_library", + "get_drake_pybind_installs", + "get_pybind_library_dest", +) +load( + "//tools/skylark:drake_py.bzl", + "drake_py_binary", + "drake_py_library", + "drake_py_test", +) +load("//tools/skylark:6996.bzl", "adjust_labels_for_drake_hoist") + +package(default_visibility = adjust_labels_for_drake_hoist([ + "//drake/bindings/pydrake:__subpackages__", +])) + +# @note Symbols are NOT imported directly into +# `__init__.py` to simplify dependency management, meaning that +# classes are organized by their directory structure rather than +# by C++ namespace. If you want all symbols, use `all.py`. +drake_py_library( + name = "module_py", + srcs = ["__init__.py"], + deps = [ + "//drake/bindings/pydrake:common_py", + ], +) + +drake_pybind_library( + name = "framework_py", + cc_so_name = "framework", + cc_srcs = ["framework_py.cc"], + py_deps = [ + ":module_py", + ], +) + +drake_pybind_library( + name = "primitives_py", + cc_so_name = "primitives", + cc_srcs = ["primitives_py.cc"], + py_deps = [ + ":framework_py", + ":module_py", + ], +) + +drake_pybind_library( + name = "analysis_py", + cc_so_name = "analysis", + cc_srcs = ["analysis_py.cc"], + py_deps = [ + ":framework_py", + ":module_py", + ], +) + +drake_py_library( + name = "drawing_py", + srcs = ["drawing.py"], + deps = [":module_py"], + # TODO(eric.cousineau): Expose information to allow `imports = ...` to be + # defined, rather than rely on `module_py`. +) + +drake_py_library( + name = "all_py", + deps = [ + ":analysis_py", + ":drawing_py", + ":framework_py", + ":primitives_py", + ], +) + +PYBIND_LIBRARIES = [ + ":analysis_py", + ":framework_py", + ":primitives_py", +] + +PY_LIBRARIES = [ + ":all_py", + ":drawing_py", + ":module_py", +] + +drake_py_library( + name = "systems", + deps = PYBIND_LIBRARIES + PY_LIBRARIES, +) + +install( + name = "install", + targets = PY_LIBRARIES, + py_dest = get_pybind_library_dest(), + deps = get_drake_pybind_installs(PYBIND_LIBRARIES), +) + +drake_py_test( + name = "general_test", + size = "small", + deps = [ + ":analysis_py", + ":framework_py", + ":primitives_py", + ], +) + +# TODO(eric.cousineau): Convert this to a workflow test once `pydot` is added +# to `install_prereqs.sh`. +drake_py_binary( + name = "graphviz_example", + srcs = ["test/graphviz_example.py"], + deps = [ + ":drawing_py", + ":framework_py", + ":primitives_py", + ], +) + +drake_pybind_library( + name = "lifetime_test_util", + testonly = 1, + add_install = False, + cc_so_name = "test/lifetime_test_util", + cc_srcs = ["test/lifetime_test_util_py.cc"], + py_deps = [ + ":primitives_py", + ], +) + +drake_py_test( + name = "lifetime_test", + deps = [ + ":analysis_py", + ":framework_py", + ":lifetime_test_util", + ":primitives_py", + ], +) + +drake_py_test( + name = "custom_test", + size = "small", + deps = [ + ":analysis_py", + ":framework_py", + ":primitives_py", + ], +) + +drake_py_test( + name = "vector_test", + size = "small", + deps = [ + ":framework_py", + ], +) + +add_lint_tests() diff --git a/bindings/pydrake/systems/__init__.py b/bindings/pydrake/systems/__init__.py new file mode 100644 index 000000000000..8df4b86ebc5a --- /dev/null +++ b/bindings/pydrake/systems/__init__.py @@ -0,0 +1 @@ +# Blank Python module. diff --git a/bindings/pydrake/systems/all.py b/bindings/pydrake/systems/all.py new file mode 100644 index 000000000000..6370a8a99263 --- /dev/null +++ b/bindings/pydrake/systems/all.py @@ -0,0 +1,8 @@ +from .analysis import * +from .framework import * +from .primitives import * + +try: + from .drawing import * +except ImportError: + pass diff --git a/bindings/pydrake/systems/analysis_py.cc b/bindings/pydrake/systems/analysis_py.cc new file mode 100644 index 000000000000..a38543148931 --- /dev/null +++ b/bindings/pydrake/systems/analysis_py.cc @@ -0,0 +1,26 @@ +#include + +#include "drake/systems/analysis/simulator.h" + +namespace py = pybind11; + +using std::unique_ptr; + +PYBIND11_MODULE(analysis, m) { + // NOLINTNEXTLINE(build/namespaces): Emulate placement in namespace. + using namespace drake::systems; + + auto py_iref = py::return_value_policy::reference_internal; + + m.doc() = "Bindings for the analysis portion of the Systems framework."; + + using T = double; + + py::class_>(m, "Simulator") + .def(py::init&>()) + .def(py::init&, unique_ptr>>()) + .def("Initialize", &Simulator::Initialize) + .def("StepTo", &Simulator::StepTo) + .def("get_context", &Simulator::get_context, py_iref) + .def("get_mutable_context", &Simulator::get_mutable_context, py_iref); +} diff --git a/bindings/pydrake/systems/drawing.py b/bindings/pydrake/systems/drawing.py new file mode 100644 index 000000000000..c176692a87bf --- /dev/null +++ b/bindings/pydrake/systems/drawing.py @@ -0,0 +1,40 @@ +""" +Provides general visualization utilities. This is NOT related to `rendering`. +@note This is an optional module, dependent on `pydot` and `matplotlib` being +installed. +""" + +from StringIO import StringIO + +import matplotlib.image as mpimg +import matplotlib.pyplot as plt +import pydot + + +# TODO(eric.cousineau): Move `plot_graphviz` to something more accessible to +# `call_python_client`. + + +def plot_graphviz(dot_text): + """Renders a DOT graph in matplotlib.""" + # @ref https://stackoverflow.com/a/18522941/7829525 + # Tried (reason ignored): pydotplus (`pydot` works), networkx + # (`read_dot` does not work robustly?), pygraphviz (coupled with + # `networkx`). + g = pydot.graph_from_dot_data(dot_text) + if isinstance(g, list): + # Per Ioannis's follow-up comment in the above link, in pydot >= 1.2.3 + # `graph_from_dot_data` returns a list of graphs. + # Handle this case for now. + assert len(g) == 1 + g = g[0] + s = StringIO() + g.write_png(s) + s.seek(0) + plt.axis('off') + return plt.imshow(plt.imread(s), aspect="equal") + + +def plot_system_graphviz(system): + """Renders a System's Graphviz representation in `matplotlib`. """ + return plot_graphviz(system.GetGraphvizString()) diff --git a/bindings/pydrake/systems/framework_py.cc b/bindings/pydrake/systems/framework_py.cc new file mode 100644 index 000000000000..0a84b74635e8 --- /dev/null +++ b/bindings/pydrake/systems/framework_py.cc @@ -0,0 +1,222 @@ +#include +#include +#include +#include +#include + +#include "drake/systems/framework/abstract_values.h" +#include "drake/systems/framework/basic_vector.h" +#include "drake/systems/framework/context.h" +#include "drake/systems/framework/diagram.h" +#include "drake/systems/framework/diagram_builder.h" +#include "drake/systems/framework/leaf_context.h" +#include "drake/systems/framework/leaf_system.h" +#include "drake/systems/framework/output_port_value.h" +#include "drake/systems/framework/subvector.h" +#include "drake/systems/framework/supervector.h" +#include "drake/systems/framework/system.h" + +namespace py = pybind11; + +using std::make_unique; +using std::unique_ptr; +using std::vector; + +PYBIND11_MODULE(framework, m) { + // NOLINTNEXTLINE(build/namespaces): Emulate placement in namespace. + using namespace drake; + // NOLINTNEXTLINE(build/namespaces): Emulate placement in namespace. + using namespace drake::systems; + + // Aliases for commonly used return value policies. + // `py_ref` is used when `keep_alive` is explicitly used (e.g. for extraction + // methods, like `GetMutableSubsystemState`). + auto py_ref = py::return_value_policy::reference; + // `py_iref` is used when pointers / lvalue references are returned (no need + // for `keep_alive`, as it is implicit. + auto py_iref = py::return_value_policy::reference_internal; + + m.doc() = "Bindings for the core Systems framework."; + + // TODO(eric.cousineau): At present, we only bind doubles. + // In the future, we will bind more scalar types, and enable scalar + // conversion. + using T = double; + + // TODO(eric.cousineau): Resolve `str_py` workaround. + auto str_py = py::eval("str"); + + py::setattr(m, "kAutoSize", py::cast(kAutoSize)); + + py::enum_(m, "PortDataType") + .value("kVectorValued", kVectorValued) + .value("kAbstractValued", kAbstractValued); + + class PySystem : public py::wrapper> { + public: + using Base = py::wrapper>; + using Base::Base; + // Expose protected methods for binding. + using Base::DeclareInputPort; + }; + + // TODO(eric.cousineau): Show constructor, but somehow make sure `pybind11` + // knows this is abstract? + py::class_, PySystem>(m, "System") + .def("set_name", &System::set_name) + // Topology. + .def("get_input_port", &System::get_input_port, py_iref) + .def("get_output_port", &System::get_output_port, py_iref) + .def( + "_DeclareInputPort", + [](PySystem* self, PortDataType arg1, int arg2) -> auto&& { + return self->DeclareInputPort(arg1, arg2); + }, py_iref) + // Context. + .def("CreateDefaultContext", &System::CreateDefaultContext) + .def("AllocateOutput", &System::AllocateOutput) + .def( + "EvalVectorInput", + [](const System* self, const Context& arg1, int arg2) { + return self->EvalVectorInput(arg1, arg2); + }, py_iref) + .def("CalcOutput", &System::CalcOutput) + // Sugar. + .def( + "GetGraphvizString", + [str_py](const System* self) { + // @note This is a workaround; for some reason, + // casting this using `py::str` does not work, but directly + // calling the Python function (`str_py`) does. + return str_py(self->GetGraphvizString()); + }); + + class PyLeafSystem : public py::wrapper> { + public: + using Base = py::wrapper>; + using Base::Base; + // Expose protected methods for binding. + using Base::DeclareVectorOutputPort; + }; + + // Don't use a const-rvalue as a function handle parameter, as pybind11 wants + // to copy it? + // TODO(eric.cousineau): Make a helper wrapper for this; file a bug in + // pybind11 (since these are arguments). + using CalcVectorPtrCallback = + std::function*, BasicVector*)>; + + py::class_, PyLeafSystem, System>(m, "LeafSystem") + .def(py::init<>()) + .def( + "_DeclareVectorOutputPort", + [](PyLeafSystem* self, const BasicVector& arg1, + CalcVectorPtrCallback arg2) -> auto&& { + typename LeafOutputPort::CalcVectorCallback wrapped = + [arg2](const Context& nest_arg1, BasicVector* nest_arg2) { + return arg2(&nest_arg1, nest_arg2); + }; + return self->DeclareVectorOutputPort(arg1, wrapped); + }, py_iref); + + py::class_>(m, "Context") + .def("get_num_input_ports", &Context::get_num_input_ports) + .def("FixInputPort", + py::overload_cast>>( + &Context::FixInputPort)) + .def("get_time", &Context::get_time) + .def("Clone", &Context::Clone) + .def("__copy__", &Context::Clone) + .def("get_state", &Context::get_state, py_iref) + .def("get_mutable_state", &Context::get_mutable_state, py_iref); + + py::class_, Context>(m, "LeafContext"); + + py::class_, System>(m, "Diagram") + .def("GetMutableSubsystemState", + [](Diagram* self, const System& arg1, Context* arg2) + -> auto&& { + // @note Use `auto&&` to get perfect forwarding. + // @note Compiler does not like `py::overload_cast` with this setup? + return self->GetMutableSubsystemState(arg1, arg2); + }, py_ref, py::keep_alive<0, 3>()); + + // Glue mechanisms. + py::class_>(m, "DiagramBuilder") + .def(py::init<>()) + .def( + "AddSystem", + [](DiagramBuilder* self, unique_ptr> arg1) { + return self->AddSystem(std::move(arg1)); + }) + .def("Connect", + py::overload_cast&, const InputPortDescriptor&>( + &DiagramBuilder::Connect)) + .def("ExportInput", &DiagramBuilder::ExportInput) + .def("ExportOutput", &DiagramBuilder::ExportOutput) + .def("Build", &DiagramBuilder::Build) + .def("BuildInto", &DiagramBuilder::BuildInto); + + py::class_>(m, "OutputPort"); + + py::class_>(m, "SystemOutput") + .def("get_num_ports", &SystemOutput::get_num_ports) + .def("get_vector_data", &SystemOutput::get_vector_data, py_iref); + + py::class_>(m, "InputPortDescriptor"); + + // Value types. + py::class_>(m, "VectorBase") + .def("CopyToVector", &VectorBase::CopyToVector) + .def("SetFromVector", &VectorBase::SetFromVector); + + // TODO(eric.cousineau): Make a helper function for the Eigen::Ref<> patterns. + py::class_, VectorBase>(m, "BasicVector") + .def(py::init()) + .def(py::init>()) + .def("get_value", + [](const BasicVector* self) -> Eigen::Ref> { + return self->get_value(); + }, py_iref) + .def("get_mutable_value", + [](BasicVector* self) -> Eigen::Ref> { + return self->get_mutable_value(); + }, py_iref); + + py::class_, VectorBase>(m, "Supervector"); + + py::class_, VectorBase>(m, "Subvector"); + + // TODO(eric.cousineau): Interfacing with the C++ abstract value types may be + // a tad challenging. This should be more straightforward once + // scalar-type conversion is supported, as the template-exposure mechanisms + // should be relatively similar. + py::class_(m, "AbstractValue"); + + // Parameters. + // TODO(eric.cousineau): Fill this out. + py::class_>(m, "Parameters"); + + // State. + py::class_>(m, "State") + .def(py::init<>()) + .def("get_continuous_state", + &State::get_continuous_state, py_iref) + .def("get_mutable_continuous_state", + &State::get_mutable_continuous_state, py_iref) + .def("get_discrete_state", + &State::get_discrete_state, py_iref); + + // - Constituents. + py::class_>(m, "ContinuousState") + .def(py::init<>()) + .def("get_vector", &ContinuousState::get_vector, py_iref) + .def("get_mutable_vector", + &ContinuousState::get_mutable_vector, py_iref); + + py::class_>(m, "DiscreteValues") + .def("num_groups", &DiscreteValues::num_groups) + .def("get_data", &DiscreteValues::get_data, py_iref); + + py::class_(m, "AbstractValues"); +} diff --git a/bindings/pydrake/systems/primitives_py.cc b/bindings/pydrake/systems/primitives_py.cc new file mode 100644 index 000000000000..8c2007fec94b --- /dev/null +++ b/bindings/pydrake/systems/primitives_py.cc @@ -0,0 +1,37 @@ +#include +#include + +#include "drake/systems/primitives/adder.h" +#include "drake/systems/primitives/constant_value_source.h" +#include "drake/systems/primitives/constant_vector_source.h" +#include "drake/systems/primitives/integrator.h" +#include "drake/systems/primitives/zero_order_hold.h" + +namespace py = pybind11; + +PYBIND11_MODULE(primitives, m) { + // NOLINTNEXTLINE(build/namespaces): Emulate placement in namespace. + using namespace drake; + // NOLINTNEXTLINE(build/namespaces): Emulate placement in namespace. + using namespace drake::systems; + + m.doc() = "Bindings for the primitives portion of the Systems framework."; + + using T = double; + + py::class_, LeafSystem>(m, "ConstantVectorSource") + .def(py::init>()); + + py::class_, LeafSystem>(m, "ConstantValueSource"); + + py::class_, LeafSystem>(m, "Adder") + .def(py::init()); + + py::class_, LeafSystem>(m, "Integrator") + .def(py::init()); + + py::class_, LeafSystem>(m, "ZeroOrderHold") + .def(py::init()); + + // TODO(eric.cousineau): Add more systems as needed. +} diff --git a/bindings/pydrake/systems/test/custom_test.py b/bindings/pydrake/systems/test/custom_test.py new file mode 100644 index 000000000000..8b9f2730aabf --- /dev/null +++ b/bindings/pydrake/systems/test/custom_test.py @@ -0,0 +1,87 @@ +#!/usr/bin/env python +# -*- coding: utf8 -*- + +from __future__ import print_function + +import copy +import unittest +import numpy as np + +from pydrake.systems.analysis import ( + Simulator, + ) +from pydrake.systems.framework import ( + BasicVector, + DiagramBuilder, + LeafSystem, + PortDataType, + ) +from pydrake.systems.primitives import ( + ZeroOrderHold, + ) + + +class CustomAdder(LeafSystem): + # Reimplements `Adder`. + def __init__(self, num_inputs, size): + LeafSystem.__init__(self) + for i in xrange(num_inputs): + self._DeclareInputPort(PortDataType.kVectorValued, size) + self._DeclareVectorOutputPort(BasicVector(size), self._calc_sum) + + def _calc_sum(self, context, sum_data): + # @note This will NOT work if the scalar type is AutoDiff or symbolic, + # since they are not stored densely. + sum = sum_data.get_mutable_value() + sum[:] = 0 + for i in xrange(context.get_num_input_ports()): + input_vector = self.EvalVectorInput(context, i) + sum += input_vector.get_value() + + +class TestCustom(unittest.TestCase): + def _create_system(self): + system = CustomAdder(2, 3) + return system + + def _fix_inputs(self, context): + self.assertEquals(context.get_num_input_ports(), 2) + context.FixInputPort(0, BasicVector([1, 2, 3])) + context.FixInputPort(1, BasicVector([4, 5, 6])) + + def test_execution(self): + system = self._create_system() + context = system.CreateDefaultContext() + self._fix_inputs(context) + output = system.AllocateOutput(context) + self.assertEquals(output.get_num_ports(), 1) + system.CalcOutput(context, output) + value = output.get_vector_data(0).get_value() + self.assertTrue(np.allclose([5, 7, 9], value)) + + def test_simulation(self): + builder = DiagramBuilder() + adder = builder.AddSystem(self._create_system()) + adder.set_name("custom_adder") + # Add ZOH so we can easily extract state. + zoh = builder.AddSystem(ZeroOrderHold(0.1, 3)) + zoh.set_name("zoh") + + builder.ExportInput(adder.get_input_port(0)) + builder.ExportInput(adder.get_input_port(1)) + builder.Connect(adder.get_output_port(0), zoh.get_input_port(0)) + diagram = builder.Build() + context = diagram.CreateDefaultContext() + self._fix_inputs(context) + + simulator = Simulator(diagram, context) + simulator.Initialize() + simulator.StepTo(1) + # Ensure that we have the outputs we want. + state = diagram.GetMutableSubsystemState(zoh, context) + value = state.get_discrete_state().get_data()[0].get_value() + self.assertTrue(np.allclose([5, 7, 9], value)) + + +if __name__ == '__main__': + unittest.main() diff --git a/bindings/pydrake/systems/test/general_test.py b/bindings/pydrake/systems/test/general_test.py new file mode 100644 index 000000000000..65937ddc79d3 --- /dev/null +++ b/bindings/pydrake/systems/test/general_test.py @@ -0,0 +1,141 @@ +#!/usr/bin/env python +# -*- coding: utf8 -*- + +from __future__ import print_function + +import copy + +import unittest +import numpy as np + +from pydrake.systems.analysis import ( + Simulator, + ) +from pydrake.systems.framework import ( + BasicVector, + Diagram, + DiagramBuilder, + ) +from pydrake.systems.primitives import ( + Adder, + ConstantVectorSource, + Integrator, + ) + + +class TestGeneral(unittest.TestCase): + def test_simulator_ctor(self): + # Create simple system. + system = ConstantVectorSource([1]) + + def check_output(context): + # Check number of output ports and value for a given context. + output = system.AllocateOutput(context) + self.assertEquals(output.get_num_ports(), 1) + system.CalcOutput(context, output) + value = output.get_vector_data(0).get_value() + self.assertTrue(np.allclose([1], value)) + + # Create simulator with basic constructor. + simulator = Simulator(system) + simulator.Initialize() + self.assertTrue(simulator.get_context() is + simulator.get_mutable_context()) + check_output(simulator.get_context()) + simulator.StepTo(1) + + # Create simulator specifying context. + context = system.CreateDefaultContext() + # @note `simulator` now owns `context`. + simulator = Simulator(system, context) + self.assertTrue(simulator.get_context() is context) + check_output(context) + simulator.StepTo(1) + + def test_copy(self): + # Copy a context using `copy` or `clone`. + system = ConstantVectorSource([1]) + context = system.CreateDefaultContext() + context_2 = copy.copy(context) + self.assertNotEquals(context, context_2) + context_3 = context.Clone() + self.assertNotEquals(context, context_3) + # TODO(eric.cousineau): Check more properties. + + def test_diagram_simulation(self): + # Similar to: //systems/framework:diagram_test, ExampleDiagram + size = 3 + + builder = DiagramBuilder() + adder0 = builder.AddSystem(Adder(2, size)) + adder0.set_name("adder0") + adder1 = builder.AddSystem(Adder(2, size)) + adder1.set_name("adder1") + + integrator = builder.AddSystem(Integrator(size)) + integrator.set_name("integrator") + + builder.Connect(adder0.get_output_port(0), adder1.get_input_port(0)) + builder.Connect(adder1.get_output_port(0), + integrator.get_input_port(0)) + + builder.ExportInput(adder0.get_input_port(0)) + builder.ExportInput(adder0.get_input_port(1)) + builder.ExportInput(adder1.get_input_port(1)) + builder.ExportOutput(integrator.get_output_port(0)) + + diagram = builder.Build() + # TODO(eric.cousineau): Figure out unicode handling if needed. + # See //systems/framework/test/diagram_test.cc:349 (sha: bc84e73) + # for an example name. + diagram.set_name("test_diagram") + + simulator = Simulator(diagram) + context = simulator.get_mutable_context() + + # Create and attach inputs. + # TODO(eric.cousineau): Not seeing any assertions being printed if no + # inputs are connected. Need to check this behavior. + input0 = BasicVector([0.1, 0.2, 0.3]) + context.FixInputPort(0, input0) + input1 = BasicVector([0.02, 0.03, 0.04]) + context.FixInputPort(1, input1) + input2 = BasicVector([0.003, 0.004, 0.005]) + context.FixInputPort(2, input2) + + # Initialize integrator states. + def get_mutable_continuous_state(system): + return (diagram.GetMutableSubsystemState(system, context) + .get_mutable_continuous_state()) + + integrator_xc = get_mutable_continuous_state(integrator) + integrator_xc.get_mutable_vector().SetFromVector([0, 1, 2]) + + simulator.Initialize() + + # Simulate briefly, and take full-context snapshots at intermediate + # points. + n = 6 + times = np.linspace(0, 1, n) + context_log = [] + for t in times: + simulator.StepTo(t) + # Record snapshot of *entire* context. + context_log.append(context.Clone()) + + xc_initial = np.array([0, 1, 2]) + xc_final = np.array([0.123, 1.234, 2.345]) + + for i, context_i in enumerate(context_log): + t = times[i] + self.assertEqual(context_i.get_time(), t) + xc = (context_i.get_state().get_continuous_state() + .get_vector().CopyToVector()) + xc_expected = (float(i) / (n - 1) * (xc_final - xc_initial) + + xc_initial) + print("xc[t = {}] = {}".format(t, xc)) + self.assertTrue(np.allclose(xc, xc_expected)) + + +if __name__ == '__main__': + unittest.main() diff --git a/bindings/pydrake/systems/test/graphviz_example.py b/bindings/pydrake/systems/test/graphviz_example.py new file mode 100644 index 000000000000..8e69e89501b8 --- /dev/null +++ b/bindings/pydrake/systems/test/graphviz_example.py @@ -0,0 +1,25 @@ +#!/usr/bin/env python + +import matplotlib.pyplot as plt + +from pydrake.systems.drawing import plot_system_graphviz +from pydrake.systems.framework import DiagramBuilder +from pydrake.systems.primitives import Adder + +builder = DiagramBuilder() +size = 1 +adders = [ + builder.AddSystem(Adder(1, size)), + builder.AddSystem(Adder(1, size)), +] +for i, adder in enumerate(adders): + adder.set_name("adders[{}]".format(i)) +builder.Connect(adders[0].get_output_port(0), adders[1].get_input_port(0)) +builder.ExportInput(adders[0].get_input_port(0)) +builder.ExportOutput(adders[1].get_output_port(0)) + +diagram = builder.Build() +diagram.set_name("graphviz_example") + +plot_system_graphviz(diagram) +plt.show() diff --git a/bindings/pydrake/systems/test/lifetime_test.py b/bindings/pydrake/systems/test/lifetime_test.py new file mode 100644 index 000000000000..a4c4deb0a81b --- /dev/null +++ b/bindings/pydrake/systems/test/lifetime_test.py @@ -0,0 +1,101 @@ +#!/usr/bin/env python +# -*- coding: utf8 -*- + +""" +@file +Captures limitations for the present state of the Python bindings for the +lifetime of objects, eventually lock down capabilities as they are introduced. +""" + +from __future__ import print_function + +import unittest +import numpy as np + +from pydrake.systems.analysis import ( + Simulator, + ) +from pydrake.systems.framework import ( + DiagramBuilder, + ) +from pydrake.systems.primitives import ( + Adder, + ) +from pydrake.systems.test.lifetime_test_util import ( + DeleteListenerSystem, + DeleteListenerVector, + ) + + +class Info(object): + # Tracks if an instance has been deleted. + def __init__(self): + self.deleted = False + + def record_deletion(self): + assert not self.deleted + self.deleted = True + + +class TestLifetime(unittest.TestCase): + def test_basic(self): + info = Info() + system = DeleteListenerSystem(info.record_deletion) + self.assertFalse(info.deleted) + del system + self.assertTrue(info.deleted) + + def test_ownership_diagram(self): + info = Info() + system = DeleteListenerSystem(info.record_deletion) + builder = DiagramBuilder() + # `system` is now owned by `builder`. + builder.AddSystem(system) + # `system` is now owned by `diagram`. + diagram = builder.Build() + # Delete the builder. Should still be alive. + del builder + self.assertFalse(info.deleted) + # Delete the diagram. Should be dead. + del diagram + # WARNING + self.assertTrue(info.deleted) + self.assertTrue(system is not None) + + def test_ownership_multiple_containers(self): + info = Info() + system = DeleteListenerSystem(info.record_deletion) + builder_1 = DiagramBuilder() + builder_2 = DiagramBuilder() + builder_1.AddSystem(system) + # This is tested in our fork of `pybind11`, but echoed here for when + # we decide to switch to use `shared_ptr`. + with self.assertRaises(RuntimeError): + # This should throw an error from `pybind11`, since two containers + # are trying to own a unique_ptr-held object. + builder_2.AddSystem(system) + + def test_ownership_simulator(self): + info = Info() + system = DeleteListenerSystem(info.record_deletion) + simulator = Simulator(system) + self.assertFalse(info.deleted) + del simulator + # Simulator does not own the system. + self.assertFalse(info.deleted) + self.assertTrue(system is not None) + + def test_ownership_vector(self): + system = Adder(1, 1) + context = system.CreateDefaultContext() + info = Info() + vector = DeleteListenerVector(info.record_deletion) + context.FixInputPort(0, vector) + del context + # WARNING + self.assertTrue(info.deleted) + self.assertTrue(vector is not None) + + +assert __name__ == '__main__' +unittest.main() diff --git a/bindings/pydrake/systems/test/lifetime_test_util_py.cc b/bindings/pydrake/systems/test/lifetime_test_util_py.cc new file mode 100644 index 000000000000..2040ba49c17e --- /dev/null +++ b/bindings/pydrake/systems/test/lifetime_test_util_py.cc @@ -0,0 +1,58 @@ +#include +#include + +#include "drake/systems/framework/basic_vector.h" +#include "drake/systems/primitives/constant_vector_source.h" + +namespace py = pybind11; + +using std::unique_ptr; +using drake::VectorX; +using drake::systems::BasicVector; +using drake::systems::ConstantVectorSource; + +using T = double; + +namespace { + +// Informs listener when this class is deleted. +class DeleteListenerSystem : public ConstantVectorSource { + public: + explicit DeleteListenerSystem(std::function delete_callback) + : ConstantVectorSource(VectorX::Constant(1, 0.)), + delete_callback_(delete_callback) {} + + ~DeleteListenerSystem() override { + delete_callback_(); + } + private: + std::function delete_callback_; +}; + +class DeleteListenerVector : public BasicVector { + public: + explicit DeleteListenerVector(std::function delete_callback) + : BasicVector(VectorX::Constant(1, 0.)), + delete_callback_(delete_callback) {} + + ~DeleteListenerVector() override { + delete_callback_(); + } + private: + std::function delete_callback_; +}; + +} // namespace + +PYBIND11_MODULE(lifetime_test_util, m) { + // Import dependencies. + py::module::import("pydrake.systems.framework"); + py::module::import("pydrake.systems.primitives"); + + py::class_>( + m, "DeleteListenerSystem") + .def(py::init>()); + py::class_>( + m, "DeleteListenerVector") + .def(py::init>()); +} diff --git a/bindings/pydrake/systems/test/vector_test.py b/bindings/pydrake/systems/test/vector_test.py new file mode 100644 index 000000000000..93283435c552 --- /dev/null +++ b/bindings/pydrake/systems/test/vector_test.py @@ -0,0 +1,39 @@ +#!/usr/bin/env python +# -*- coding: utf8 -*- + +from __future__ import print_function + +import copy +import unittest +import numpy as np + +from pydrake.systems.framework import ( + BasicVector, + ) + +# TODO(eric.cousineau): Add negative test cases for AutoDiffXd and Symbolic +# once they are in the bindings. + + +class TestReference(unittest.TestCase): + def test_basic_vector_double(self): + # Ensure that we can get vectors templated on double by reference. + init = [1., 2, 3] + value_data = BasicVector(init) + value = value_data.get_mutable_value() + # TODO(eric.cousineau): Determine if there is a way to extract the + # pointer referred to by the buffer (e.g. `value.data`). + value[:] += 1 + expected = [2., 3, 4] + self.assertTrue(np.allclose(value, expected)) + self.assertTrue(np.allclose(value_data.get_value(), expected)) + self.assertTrue(np.allclose(value_data.get_mutable_value(), expected)) + expected = [5., 6, 7] + value_data.SetFromVector(expected) + self.assertTrue(np.allclose(value, expected)) + self.assertTrue(np.allclose(value_data.get_value(), expected)) + self.assertTrue(np.allclose(value_data.get_mutable_value(), expected)) + + +if __name__ == '__main__': + unittest.main() diff --git a/tools/skylark/drake_py.bzl b/tools/skylark/drake_py.bzl index 213bb0da1e3a..3b31d486ada3 100644 --- a/tools/skylark/drake_py.bzl +++ b/tools/skylark/drake_py.bzl @@ -32,14 +32,18 @@ def drake_py_binary( def drake_py_test( name, + srcs = None, deps = None, data = None, **kwargs): """A wrapper to insert Drake-specific customizations.""" + if srcs == None: + srcs = ["test/%s.py" % name] deps = adjust_labels_for_drake_hoist(deps) data = adjust_labels_for_drake_hoist(data) native.py_test( name = name, + srcs = srcs, deps = deps, data = data, **kwargs) From 084ff10020ff36fbdab12149b491c467fc735a59 Mon Sep 17 00:00:00 2001 From: Rick Cory Date: Fri, 22 Dec 2017 07:34:12 -0800 Subject: [PATCH 05/25] Updated default young's modulus parameters (#7651) * Updated default young's modulus to be that of aluminum. Set box.urdf material parameters to represent a compliant box. * Adding some comments on default Young's modulus --- .../dev/box_rotation/iiwa_box_simulation.cc | 6 ++- .../dev/box_rotation/iiwa_dual_box_rot.pmd | 2 +- .../dev/box_rotation/models/box.urdf | 54 +++++++++++++++++++ 3 files changed, 60 insertions(+), 2 deletions(-) diff --git a/examples/kuka_iiwa_arm/dev/box_rotation/iiwa_box_simulation.cc b/examples/kuka_iiwa_arm/dev/box_rotation/iiwa_box_simulation.cc index 77b4ae4a7491..a54671c51891 100644 --- a/examples/kuka_iiwa_arm/dev/box_rotation/iiwa_box_simulation.cc +++ b/examples/kuka_iiwa_arm/dev/box_rotation/iiwa_box_simulation.cc @@ -43,7 +43,11 @@ DEFINE_string(urdf, "", "Name of urdf file to load"); DEFINE_double(simulation_sec, std::numeric_limits::infinity(), "Number of seconds to simulate (s)"); -DEFINE_double(youngs_modulus, 3e7, "Default material's Young's modulus (Pa)"); +// Set default Young's modulus to be that of aluminum. This demo assumes the box +// defines it's own Young's modulus in it's URDF, and does not use this default +// value. +DEFINE_double(youngs_modulus, 6.9e10, + "Default Young's modulus (Pa), set to be that of aluminum."); DEFINE_double(dissipation, 5, "Contact Dissipation (s/m)"); DEFINE_double(static_friction, 0.5, "Static Friction"); DEFINE_double(dynamic_friction, 0.2, "Dynamic Friction"); diff --git a/examples/kuka_iiwa_arm/dev/box_rotation/iiwa_dual_box_rot.pmd b/examples/kuka_iiwa_arm/dev/box_rotation/iiwa_dual_box_rot.pmd index ba0607b6b6e6..e7e83f2e664d 100644 --- a/examples/kuka_iiwa_arm/dev/box_rotation/iiwa_dual_box_rot.pmd +++ b/examples/kuka_iiwa_arm/dev/box_rotation/iiwa_dual_box_rot.pmd @@ -15,7 +15,7 @@ cmd "0.drake_visualizer" { } cmd "1.iiwa-sim" { - exec = "${DRAKE_WORKSPACE}/bazel-bin/examples/kuka_iiwa_arm/dev/box_rotation/iiwa_box_simulation --urdf examples/kuka_iiwa_arm/dev/box_rotation/models/${URDF} --contact_radius 2e-4 --youngs_modulus 3e7 --dissipation 5 --static_friction 0.5 --dynamic_friction 0.2 --v_stiction_tol 0.01 --use_visualizer=true"; + exec = "${DRAKE_WORKSPACE}/bazel-bin/examples/kuka_iiwa_arm/dev/box_rotation/iiwa_box_simulation --urdf examples/kuka_iiwa_arm/dev/box_rotation/models/${URDF} --contact_radius 2e-4 --youngs_modulus 6.9e10 --dissipation 5 --static_friction 0.5 --dynamic_friction 0.2 --v_stiction_tol 0.01 --use_visualizer=true"; host = "localhost"; } diff --git a/examples/kuka_iiwa_arm/dev/box_rotation/models/box.urdf b/examples/kuka_iiwa_arm/dev/box_rotation/models/box.urdf index 86a18f096dbc..d7322babf8d5 100644 --- a/examples/kuka_iiwa_arm/dev/box_rotation/models/box.urdf +++ b/examples/kuka_iiwa_arm/dev/box_rotation/models/box.urdf @@ -56,6 +56,12 @@ + + 3e7 + 5 + 0.5 + 0.2 + @@ -71,6 +77,12 @@ + + 3e7 + 5 + 0.5 + 0.2 + @@ -84,6 +96,12 @@ + + 3e7 + 5 + 0.5 + 0.2 + @@ -99,6 +117,12 @@ + + 3e7 + 5 + 0.5 + 0.2 + @@ -112,6 +136,12 @@ + + 3e7 + 5 + 0.5 + 0.2 + @@ -126,6 +156,12 @@ + + 3e7 + 5 + 0.5 + 0.2 + @@ -139,6 +175,12 @@ + + 3e7 + 5 + 0.5 + 0.2 + @@ -152,6 +194,12 @@ + + 3e7 + 5 + 0.5 + 0.2 + @@ -165,6 +213,12 @@ + + 3e7 + 5 + 0.5 + 0.2 + From 6ce6cd05577f3e12e0696970b3f8ff2eb0c28e46 Mon Sep 17 00:00:00 2001 From: Evan Drumwright Date: Fri, 22 Dec 2017 09:38:36 -0800 Subject: [PATCH 06/25] Fix for issue #6277. --- systems/analysis/test/simulator_test.cc | 4 +--- systems/framework/test/diagram_context_test.cc | 2 +- systems/framework/test/leaf_context_test.cc | 2 +- 3 files changed, 3 insertions(+), 5 deletions(-) diff --git a/systems/analysis/test/simulator_test.cc b/systems/analysis/test/simulator_test.cc index 06e4801265d6..a7265ef705e0 100644 --- a/systems/analysis/test/simulator_test.cc +++ b/systems/analysis/test/simulator_test.cc @@ -442,11 +442,9 @@ GTEST_TEST(SimulatorTest, MultipleWitnessesIdentical) { EXPECT_EQ(w1, w2); // Verify that they are triggering. - // NOTE: value_or(999) necessary to work around Mac OS X bug where value() - // function is declared but not defined. optional iso_time = simulator->GetCurrentWitnessTimeIsolation(); EXPECT_TRUE(iso_time); - EXPECT_LT(std::abs(w1), iso_time.value_or(999)); + EXPECT_LT(std::abs(w1), iso_time.value()); // Indicate that the method has been called. published = true; diff --git a/systems/framework/test/diagram_context_test.cc b/systems/framework/test/diagram_context_test.cc index c0c2e5136f28..1dd181fb2518 100644 --- a/systems/framework/test/diagram_context_test.cc +++ b/systems/framework/test/diagram_context_test.cc @@ -306,7 +306,7 @@ TEST_F(DiagramContextTest, Accuracy) { const double unity = 1.0; context_->set_accuracy(unity); std::unique_ptr> clone = context_->Clone(); - EXPECT_EQ(clone->get_accuracy().value_or(999), unity); + EXPECT_EQ(clone->get_accuracy().value(), unity); } } // namespace diff --git a/systems/framework/test/leaf_context_test.cc b/systems/framework/test/leaf_context_test.cc index d6a6b96713ef..77d177046854 100644 --- a/systems/framework/test/leaf_context_test.cc +++ b/systems/framework/test/leaf_context_test.cc @@ -419,7 +419,7 @@ TEST_F(LeafContextTest, Accuracy) { const double unity = 1.0; context_.set_accuracy(unity); std::unique_ptr> clone = context_.Clone(); - EXPECT_EQ(clone->get_accuracy().value_or(999), unity); + EXPECT_EQ(clone->get_accuracy().value(), unity); } } // namespace systems From 71ae136f92baa3bcd21255082c08ef70c5d72fb5 Mon Sep 17 00:00:00 2001 From: Soonho Kong Date: Thu, 21 Dec 2017 09:32:10 -0500 Subject: [PATCH 07/25] Update install_prereqs.sh for Ubuntu to use dReal-4.17.12.3 This version fixes the problems reported in https://github.com/RobotLocomotion/drake/pull/7647. --- setup/ubuntu/16.04/install_prereqs.sh | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/setup/ubuntu/16.04/install_prereqs.sh b/setup/ubuntu/16.04/install_prereqs.sh index 669d3e6eda78..61748e2517d1 100755 --- a/setup/ubuntu/16.04/install_prereqs.sh +++ b/setup/ubuntu/16.04/install_prereqs.sh @@ -178,9 +178,9 @@ dpkg_install_from_wget \ # https://github.com/dreal/dreal4/blob/master/README.md#build-debian-package for # build instructions. dpkg_install_from_wget \ - dreal 4.17.12.2 \ - https://dl.bintray.com/dreal/dreal/dreal_4.17.12.2_amd64.deb \ - 9347492e47a518ff78991e15fe9de0cff0200573091385e42940cdbf1fcf77a5 + dreal 4.17.12.3 \ + https://dl.bintray.com/dreal/dreal/dreal_4.17.12.3_amd64.deb \ + 72e878e2af14b1509b8d3a2943d7e7c824babfa755f4928cc3618e1fe85695c9 # Remove deb that we used to generate and install, but no longer need. if [ -L /usr/lib/ccache/bazel ]; then From c34b041e65d8a6218e44cdf2922a2d77982e8a4b Mon Sep 17 00:00:00 2001 From: Soonho Kong Date: Thu, 21 Dec 2017 09:33:39 -0500 Subject: [PATCH 08/25] Enable dreal_solver_test --- solvers/BUILD.bazel | 24 ++++++++++++++++-------- 1 file changed, 16 insertions(+), 8 deletions(-) diff --git a/solvers/BUILD.bazel b/solvers/BUILD.bazel index cff7099f7d1d..af3d2fb1bf82 100644 --- a/solvers/BUILD.bazel +++ b/solvers/BUILD.bazel @@ -758,14 +758,22 @@ drake_cc_googletest( ], ) -# TODO(soonho-tri): Enable this test after checking lsan/tsan/asan results. -# See https://github.com/RobotLocomotion/drake/pull/7647 -# drake_cc_googletest( -# name = "dreal_solver_test", -# deps = [ -# ":dreal_solver", -# ], -# ) +drake_cc_googletest( + name = "dreal_solver_test", + tags = [ + # For most of floating-point operations, Valgrind only supports the + # IEEE default mode (round to nearest) while dReal uses to +infinity + # and to -infinity rounding modes intensively to maintain conservative + # interval approximations. This causes some of the assertions to fail + # in the test. So we disable memcheck for this test. See + # http://valgrind.org/docs/manual/manual-core.html#manual-core.limits + # for more information. + "no_memcheck", + ], + deps = [ + ":dreal_solver", + ], +) drake_cc_googletest( name = "gurobi_solver_test", From 71a8ed2ba18bf90693b9a849752b3c4fbfc60dc7 Mon Sep 17 00:00:00 2001 From: Eric Cousineau Date: Fri, 22 Dec 2017 17:37:01 -0500 Subject: [PATCH 09/25] call_python: Test plotting. --- common/proto/BUILD.bazel | 16 +++++++---- common/proto/call_python_client.py | 8 +++--- common/proto/test/call_python_full_test.sh | 32 +++++++++++++++++----- 3 files changed, 39 insertions(+), 17 deletions(-) diff --git a/common/proto/BUILD.bazel b/common/proto/BUILD.bazel index e2b61f6a7783..8c073d161a1b 100644 --- a/common/proto/BUILD.bazel +++ b/common/proto/BUILD.bazel @@ -98,15 +98,11 @@ drake_cc_googletest( ], ) -# TODO(eric.cousineau): Remove --no_plotting once we can open `$DISPLAY` on CI. +# TODO(eric.cousineau): Add a test which will use an interactive matplotlib +# backend on CI only. sh_test( name = "call_python_full_test", srcs = ["test/call_python_full_test.sh"], - args = [ - "--no_plotting", - "$(location :call_python_test)", - "$(location :call_python_client_cli)", - ], data = [ ":call_python_client_cli", ":call_python_test", @@ -114,6 +110,14 @@ sh_test( # Certain versions of Mac don't like this script using `ps`. For this # reason, make the test run locally. local = 1, + # TODO(eric.cousineau): The `call_python_test` binary is flaky with asan / + # lsan; however, when the binary fails, it simply aborts without + # diagnostic information. When this is resolved, these tags should be + # removed. + tags = [ + "no_asan", + "no_lsan", + ], ) drake_cc_googletest( diff --git a/common/proto/call_python_client.py b/common/proto/call_python_client.py index b8f473b552dd..bf4b0104941a 100644 --- a/common/proto/call_python_client.py +++ b/common/proto/call_python_client.py @@ -430,7 +430,7 @@ def producer_loop(): self._execute_message(msg) # Spin busy for a bit, let matplotlib (or whatever) flush its # event queue. - pause(0.001) + pause(0.01) except KeyboardInterrupt: # User pressed Ctrl+C. self._done = True @@ -549,9 +549,9 @@ def main(argv): parser.add_argument( "--no_threading", action='store_true', help="Disable threaded dispatch.") - parser.add_argument("--stop_on_error", action='store_true', - help="Stop client if there is an error when " + - "executing a call.") + parser.add_argument( + "--stop_on_error", action='store_true', + help="Stop client if there is an error when executing a call.") parser.add_argument("-f", "--file", type=str, default=None) args = parser.parse_args(argv) diff --git a/common/proto/test/call_python_full_test.sh b/common/proto/test/call_python_full_test.sh index 418abe6ee9a0..506a999ab9c5 100755 --- a/common/proto/test/call_python_full_test.sh +++ b/common/proto/test/call_python_full_test.sh @@ -4,20 +4,27 @@ set -e -u # @file # @brief Tests the `call_python_client` CLI and `call_python` test together. -cc_bin_flags= -while [[ $# -gt 2 ]]; do +no_plotting= +# By default, set backend so that the test does not open windows. +export MPLBACKEND="ps" + +while [[ $# -gt 0 ]]; do case ${1} in --no_plotting) - cc_bin_flags='--gtest_filter=-TestCallPython.Plot*' + no_plotting=1 shift;; + --matplotlib_backend) + export MPLBACKEND=${2} + shift; shift;; *) echo "Bad argument: ${1}" >&2 exit 1;; esac done -cc_bin=${1} -py_client_cli=${2} +cur=$(dirname $0) +cc_bin=${cur}/call_python_test +py_client_cli=${cur}/call_python_client_cli # TODO(eric.cousineau): Use `tempfile` once we can choose which file C++ # uses. filename=$(mktemp) @@ -26,6 +33,17 @@ done_file=${filename}_done is_mac= if [[ "${OSTYPE}" == "darwin"* ]]; then is_mac=1 + # Do not test if Mac has matplotlib 2.1.0: + # @ref https://github.com/matplotlib/matplotlib/issues/9345 + mpl_ver=$(python -c "import matplotlib as mpl; print(mpl.__version__)") + if [[ ${mpl_ver} == "2.1.0" ]]; then + no_plotting=1 + fi +fi + +cc_bin_flags= +if [[ ${no_plotting} == 1 ]]; then + cc_bin_flags='--gtest_filter=-TestCallPython.Plot*' fi py-error() { @@ -35,7 +53,7 @@ py-error() { pause() { # General busy-spinning. - sleep 0.5 + sleep 0.05 } should-fail() { @@ -136,7 +154,7 @@ threading-loop() { # TODO(eric.cousineau): In script form, this generally works well (only # one interrupt needed); however, interactively we need a few more. while ps -p ${pid} > /dev/null; do - kill -INT ${pid} + kill -INT ${pid} || : pause done fi From 0456f3a449b20ad7a9c6e1e6fbf1d97a842a9990 Mon Sep 17 00:00:00 2001 From: mitiguy Date: Fri, 22 Dec 2017 21:23:24 -0800 Subject: [PATCH 10/25] Create Transform class (rotation matrix and position vector) to replace Isometry3 . --- multibody/multibody_tree/math/BUILD.bazel | 19 ++ .../multibody_tree/math/rotation_matrix.h | 32 +- .../math/test/rotation_matrix_test.cc | 23 +- .../math/test/transform_test.cc | 285 ++++++++++++++++++ multibody/multibody_tree/math/transform.cc | 9 + multibody/multibody_tree/math/transform.h | 242 +++++++++++++++ 6 files changed, 601 insertions(+), 9 deletions(-) mode change 100755 => 100644 multibody/multibody_tree/math/rotation_matrix.h create mode 100755 multibody/multibody_tree/math/test/transform_test.cc create mode 100755 multibody/multibody_tree/math/transform.cc create mode 100755 multibody/multibody_tree/math/transform.h diff --git a/multibody/multibody_tree/math/BUILD.bazel b/multibody/multibody_tree/math/BUILD.bazel index 0bda99f3d636..f7b28a91d2d0 100755 --- a/multibody/multibody_tree/math/BUILD.bazel +++ b/multibody/multibody_tree/math/BUILD.bazel @@ -75,6 +75,18 @@ drake_cc_library( ], ) +drake_cc_library( + name = "transform", + srcs = ["transform.cc"], + hdrs = ["transform.h"], + deps = [ + ":rotation_matrix", + "//drake/common", + "//drake/common:default_scalars", + "//drake/math:geometric_transform", + ], +) + drake_cc_googletest( name = "spatial_algebra_test", deps = [ @@ -90,4 +102,11 @@ drake_cc_googletest( ], ) +drake_cc_googletest( + name = "transform_test", + deps = [ + ":transform", + ], +) + add_lint_tests() diff --git a/multibody/multibody_tree/math/rotation_matrix.h b/multibody/multibody_tree/math/rotation_matrix.h old mode 100755 new mode 100644 index cc72c61378a8..5407f21ae1b0 --- a/multibody/multibody_tree/math/rotation_matrix.h +++ b/multibody/multibody_tree/math/rotation_matrix.h @@ -58,19 +58,27 @@ class RotationMatrix { SetUnchecked(R); } - /// @returns a 3x3 identity %RotationMatrix. - static RotationMatrix MakeIdentity() { return RotationMatrix(); } + /// Returns the 3x3 identity %RotationMatrix. + /// @returns the 3x3 identity %RotationMatrix. + // @internal This method's name was chosen to mimic Eigen's Identity(). + static const RotationMatrix& Identity() { + static const never_destroyed> kIdentity; + return kIdentity.access(); + } + /// Calculates R_BA = R_AB⁻¹, the inverse (transpose) of this %RotationMatrix. /// @retval R_BA = R_AB⁻¹, the inverse (transpose) of this %RotationMatrix. /// @note For a valid rotation matrix R_BA = R_AB⁻¹ = R_ABᵀ. + // @internal This method's name was chosen to mimic Eigen's inverse(). RotationMatrix inverse() const { return RotationMatrix(R_AB_.transpose()); } + /// Returns the Matrix3 underlying a %RotationMatrix. /// @returns the Matrix3 underlying a %RotationMatrix. const Matrix3& matrix() const { return R_AB_; } - /// Operator to multiply `this` rotation matrix `R_AB` by `other` rotation + /// In-place multiply of `this` rotation matrix `R_AB` by `other` rotation /// matrix `R_BC`. On return, `this` is set to equal `R_AB * R_BC`. /// @param[in] other %RotationMatrix that post-multiplies `this`. /// @returns `this` rotation matrix which has been multiplied by `other`. @@ -81,7 +89,7 @@ class RotationMatrix { return *this; } - /// Operator to multiply `this` rotation matrix `R_AB` by `other` rotation + /// Calculates `this` rotation matrix `R_AB` multiplied by `other` rotation /// matrix `R_BC`, returning the composition `R_AB * R_BC`. /// @param[in] other %RotationMatrix that post-multiplies `this`. /// @returns rotation matrix that results from `this` multiplied by `other`. @@ -91,7 +99,7 @@ class RotationMatrix { return RotationMatrix(matrix() * other.matrix(), true); } - /// Operator to multiply `this` rotation matrix R by an arbitrary Vector3. + /// Calculates `this` rotation matrix R multiplied by an arbitrary Vector3. /// @param[in] v 3x1 vector that post-multiplies `this`. /// @returns 3x1 vector that results from `R * v`. Vector3 operator*(const Vector3& v) const { @@ -154,6 +162,18 @@ class RotationMatrix { /// @returns `true` if `this` is a valid rotation matrix. bool IsValid() const { return IsValid(matrix()); } + /// Returns `true` if `this` is exactly equal to the identity matrix. + /// @returns `true` if `this` is exactly equal to the identity matrix. + bool IsExactlyIdentity() const { return matrix() == Matrix3::Identity(); } + + /// Returns true if `this` is within tolerance of the identity matrix. + /// @returns `true` if `this` is equal to the identity matrix to within the + /// threshold of get_internal_tolerance_for_orthonormality(). + bool IsIdentityToInternalTolerance() const { + return IsNearlyEqualTo(matrix(), Matrix3::Identity(), + get_internal_tolerance_for_orthonormality()); + } + /// Compares each element of `this` to the corresponding element of `other` /// to check if they are the same to within a specified `tolerance`. /// @param[in] other %RotationMatrix to compare to `this`. @@ -178,6 +198,7 @@ class RotationMatrix { /// @returns proper orthonormal matrix R that is close to M. /// @throws exception std::logic_error if M fails IsValid(M). // @internal This function is not generated for symbolic Expression. + // @internal This function's name is referenced in Doxygen documentation. template static typename std::enable_if::value, RotationMatrix>::type ProjectToRotationMatrix(const Matrix3& M) { @@ -186,6 +207,7 @@ class RotationMatrix { return RotationMatrix(M_orthonormalized, true); } + /// Returns an internal tolerance that checks rotation matrix orthonormality. /// @returns internal tolerance (small multiplier of double-precision epsilon) /// used to check whether or not a rotation matrix is orthonormal. /// @note The tolerance is chosen by developers to ensure a reasonably diff --git a/multibody/multibody_tree/math/test/rotation_matrix_test.cc b/multibody/multibody_tree/math/test/rotation_matrix_test.cc index 55d30c4fd104..b717a1fea9e7 100755 --- a/multibody/multibody_tree/math/test/rotation_matrix_test.cc +++ b/multibody/multibody_tree/math/test/rotation_matrix_test.cc @@ -64,7 +64,7 @@ GTEST_TEST(RotationMatrix, SetRotationMatrix) { // Test setting a RotationMatrix to an identity matrix. GTEST_TEST(RotationMatrix, MakeIdentityMatrix) { - RotationMatrix R = RotationMatrix::MakeIdentity(); + const RotationMatrix& R = RotationMatrix::Identity(); Matrix3d zero_matrix = Matrix3::Identity() - R.matrix(); EXPECT_TRUE((zero_matrix.array() == 0).all()); } @@ -78,9 +78,9 @@ GTEST_TEST(RotationMatrix, Inverse) { 0, cos_theta, sin_theta, 0, -sin_theta, cos_theta; RotationMatrix R(m); - RotationMatrix I = R * R.inverse(); - RotationMatrix identity_matrix(Matrix3::Identity()); - EXPECT_TRUE(I.IsNearlyEqualTo(identity_matrix, 8 * kEpsilon)); + RotationMatrix RRinv = R * R.inverse(); + const RotationMatrix& I = RotationMatrix::Identity(); + EXPECT_TRUE(RRinv.IsNearlyEqualTo(I, 8 * kEpsilon)); } @@ -171,6 +171,21 @@ GTEST_TEST(RotationMatrix, IsValid) { EXPECT_FALSE(RotationMatrix::IsValid(m, 5 * kEpsilon)); } +// Tests whether or not a RotationMatrix is an identity matrix. +GTEST_TEST(RotationMatrix, IsExactlyIdentity) { + const double cos_theta = std::cos(0.5); + const double sin_theta = std::sin(0.5); + Matrix3d m; + m << 1, 0, 0, + 0, cos_theta, sin_theta, + 0, -sin_theta, cos_theta; + + const RotationMatrix R1(m); + const RotationMatrix R2; + EXPECT_FALSE(R1.IsExactlyIdentity()); + EXPECT_TRUE(R2.IsExactlyIdentity()); +} + // Test ProjectMatrixToRotationMatrix. GTEST_TEST(RotationMatrix, ProjectToRotationMatrix) { Matrix3d m; diff --git a/multibody/multibody_tree/math/test/transform_test.cc b/multibody/multibody_tree/math/test/transform_test.cc new file mode 100755 index 000000000000..a4c6409d5008 --- /dev/null +++ b/multibody/multibody_tree/math/test/transform_test.cc @@ -0,0 +1,285 @@ +#include "drake/multibody/multibody_tree/math/transform.h" + +#include + +namespace drake { +namespace multibody { +namespace math { +namespace { + +using Eigen::Matrix3d; +using Eigen::Vector3d; + +constexpr double kEpsilon = std::numeric_limits::epsilon(); + +// Helper function to create a rotation matrix associated with a BodyXYZ +// rotation by angles q1, q2, q3. +// Note: These matrices must remain BodyXYZ matrices with the specified angles +// q1, q2, q3, as these matrices are used in conjunction with MotionGenesis +// pre-computed solutions based on these exact matrices. +RotationMatrix GetRotationMatrixA() { + const double q1 = 0.2, q2 = 0.3, q3 = 0.4; + const double c1 = std::cos(q1), c2 = std::cos(q2), c3 = std::cos(q3); + const double s1 = std::sin(q1), s2 = std::sin(q2), s3 = std::sin(q3); + Matrix3d m; + m << c2 * c3, + s3 * c1 + s1 * s2 * c3, + s1 * s3 - s2 * c1 * c3, + -s3 * c2, + c1 * c3 - s1 * s2 * s3, + s1 * c3 + s2 * s3 * c1, + s2, + -s1 * c2, + c1 * c2; + return RotationMatrix(m); +} + +// Helper function to create a rotation matrix associated with a BodyXYX +// rotation by angles r1, r2, r3. +// Note: These matrices must remain BodyXYX matrices with the specified angles +// r1, r2, r3, as these matrices are used in conjunction with MotionGenesis +// pre-computed solutions based on these exact matrices. +RotationMatrix GetRotationMatrixB() { + const double r1 = 0.5, r2 = 0.5, r3 = 0.7; + const double c1 = std::cos(r1), c2 = std::cos(r2), c3 = std::cos(r3); + const double s1 = std::sin(r1), s2 = std::sin(r2), s3 = std::sin(r3); + Matrix3d m; + m << c2, + s1 * s2, + -s2 * c1, + s2 * s3, + c1 * c3 - s1 * s3 * c2, + s1 * c3 + s3 * c1 * c2, + s2 * c3, + -s3 * c1 - s1 * c2 * c3, + c1 * c2 * c3 - s1 * s3; + return RotationMatrix(m); +} + +#ifdef DRAKE_ASSERT_IS_ARMED +// Helper function to create an invalid rotation matrix. +Matrix3d GetBadRotationMatrix() { + const double theta = 0.5; + const double cos_theta = std::cos(theta), sin_theta = std::sin(theta); + Matrix3d m; + m << 1, 9000*kEpsilon, 9000*kEpsilon, + 0, cos_theta, sin_theta, + 0, -sin_theta, cos_theta; + return m; +} +#endif + +// Helper functions to create generic position vectors. +Vector3d GetPositionVectorA() { return Vector3d(2, 3, 4); } +Vector3d GetPositionVectorB() { return Vector3d(5, 6, 7); } + +// Helper function to create generic transforms. +Transform GetTransformA() { + return Transform(GetRotationMatrixA(), GetPositionVectorA()); +} + +Transform GetTransformB() { + return Transform(GetRotationMatrixB(), GetPositionVectorB()); +} + +// Tests default constructor - should be identity transform. +GTEST_TEST(Transform, DefaultTransformIsIdentity) { + const Transform X; + EXPECT_TRUE(X.IsExactlyIdentity()); +} + +// Tests constructing a Transform from a RotationMatrix and Vector3. +GTEST_TEST(Transform, TransformConstructor) { + const RotationMatrix R1 = GetRotationMatrixB(); + const Matrix3d m = R1.matrix(); + const Vector3 p(4, 5, 6); + const Transform X(R1, p); + const Matrix3d zero_rotation = m - X.rotation().matrix(); + const Vector3d zero_position = p - X.translation(); + EXPECT_TRUE((zero_rotation.array() == 0).all()); + EXPECT_TRUE((zero_position.array() == 0).all()); + +#ifdef DRAKE_ASSERT_IS_ARMED + // Bad rotation matrix should throw exception. + // Note: Although this test seems redundant with similar tests for the + // RotationMatrix class, it is here due to the bug (mentioned below) in + // EXPECT_THROW. Contrast the use here of EXPECT_THROW (which does not have + // an extra set of parentheses around its first argument) with the use of + // EXPECT_THROW((Transform(isometryC)), std::logic_error); below. + const Matrix3d bad = GetBadRotationMatrix(); + EXPECT_THROW(Transform(RotationMatrix(bad), p), + std::logic_error); +#endif +} + +// Tests getting a 4x4 matrix from a Transform. +GTEST_TEST(Transform, Matrix44) { + const RotationMatrix R = GetRotationMatrixB(); + const Vector3 p(4, 5, 6); + const Transform X(R, p); + const Matrix4 Y = X.GetAsMatrix(); + const Matrix3d m = R.matrix(); + + EXPECT_EQ(Y(0, 0), m(0, 0)); + EXPECT_EQ(Y(0, 1), m(0, 1)); + EXPECT_EQ(Y(0, 2), m(0, 2)); + EXPECT_EQ(Y(0, 3), p(0)); + EXPECT_EQ(Y(1, 0), m(1, 0)); + EXPECT_EQ(Y(1, 1), m(1, 1)); + EXPECT_EQ(Y(1, 2), m(1, 2)); + EXPECT_EQ(Y(1, 3), p(1)); + EXPECT_EQ(Y(2, 0), m(2, 0)); + EXPECT_EQ(Y(2, 1), m(2, 1)); + EXPECT_EQ(Y(2, 2), m(2, 2)); + EXPECT_EQ(Y(2, 3), p(2)); + EXPECT_EQ(Y(3, 0), 0); + EXPECT_EQ(Y(3, 1), 0); + EXPECT_EQ(Y(3, 2), 0); + EXPECT_EQ(Y(3, 3), 1); +} + +// Tests set/get a Transform with an Isometry3. +GTEST_TEST(Transform, Isometry3) { + const RotationMatrix R = GetRotationMatrixB(); + const Matrix3d m = R.matrix(); + const Vector3 p(4, 5, 6); + Isometry3 isometryA; + isometryA.linear() = m; + isometryA.translation() = p; + + const Transform X(isometryA); + Matrix3d zero_rotation = m - X.rotation().matrix(); + Vector3d zero_position = p - X.translation(); + EXPECT_TRUE((zero_rotation.array() == 0).all()); + EXPECT_TRUE((zero_position.array() == 0).all()); + + // Tests making an Isometry3 from a Transform. + const Isometry3 isometryB = X.GetAsIsometry3(); + zero_rotation = m - isometryB.linear(); + zero_position = p - isometryB.translation(); + EXPECT_TRUE((zero_rotation.array() == 0).all()); + EXPECT_TRUE((zero_position.array() == 0).all()); + +#ifdef DRAKE_ASSERT_IS_ARMED + // Bad matrix should throw exception. + const Matrix3d bad = GetBadRotationMatrix(); + Isometry3 isometryC; + isometryC.linear() = bad; + // Note: As of December 2017, there seems to be a bug in EXPECT_THROW. + // The next line incorrectly calls the default Transform constructor. + // The default Transform constructor does not throw an exception which means + // the EXPECT_THROW fails. The fix (credit Sherm) was to add an extra set + // set of parentheses around the first argument of EXPECT_THROW. + EXPECT_THROW((Transform(isometryC)), std::logic_error); +#endif +} + +// Tests method Identity (identity rotation matrix and zero vector). +GTEST_TEST(Transform, Identity) { + const Transform& X = Transform::Identity(); + EXPECT_TRUE(X.IsExactlyIdentity()); +} + +// Tests method SetIdentity. +GTEST_TEST(Transform, SetIdentity) { + const RotationMatrix R = GetRotationMatrixA(); + const Vector3d p(2, 3, 4); + Transform X(R, p); + X.SetIdentity(); + EXPECT_TRUE(X.IsExactlyIdentity()); +} + +// Tests whether or not a Transform is an identity transform. +GTEST_TEST(Transform, IsIdentity) { + // Test whether it is an identity matrix multiple ways. + Transform X1; + EXPECT_TRUE(X1.IsExactlyIdentity()); + EXPECT_TRUE(X1.IsIdentityToEpsilon(0.0)); + EXPECT_TRUE(X1.rotation().IsExactlyIdentity()); + EXPECT_TRUE((X1.translation().array() == 0).all()); + + // Test non-identity matrix. + const RotationMatrix R = GetRotationMatrixA(); + const Vector3d p(2, 3, 4); + Transform X2(R, p); + EXPECT_FALSE(X2.IsExactlyIdentity()); + + // Change rotation matrix to identity, but leave non-zero position vector. + X2.set_rotation(RotationMatrix::Identity()); + EXPECT_FALSE(X2.IsExactlyIdentity()); + EXPECT_FALSE(X2.IsIdentityToEpsilon(3.99)); + EXPECT_TRUE(X2.IsIdentityToEpsilon(4.01)); + + // Change position vector to zero vector. + const Vector3d zero_vector(0, 0, 0); + X2.set_translation(zero_vector); + EXPECT_TRUE(X2.IsExactlyIdentity()); +} + +// Tests calculating the inverse of a Transform. +GTEST_TEST(Transform, Inverse) { + const RotationMatrix R_AB = GetRotationMatrixA(); + const Vector3d p_AoBo_A(2, 3, 4); + const Transform X(R_AB, p_AoBo_A); + const Transform I = X * X.inverse(); + const Transform X_identity = Transform::Identity(); + // As documented in IsNearlyEqualTo(), 8 * epsilon was chosen because it is + // slightly larger than the characteristic length |p_AoBo_A| = 5.4 + // Note: The square-root of the condition number for a Transform is roughly + // the magnitude of the position vector. The accuracy of the calculation for + // the inverse of a Transform drops off with the sqrt condition number. + EXPECT_TRUE(I.IsNearlyEqualTo(X_identity, 8 * kEpsilon)); +} + +// Tests Transform multiplied by another Transform +GTEST_TEST(Transform, OperatorMultiplyByTransform) { + const Transform X_BA = GetTransformA(); + const Transform X_CB = GetTransformB(); + const Transform X_CA = X_CB * X_BA; + + // Check accuracy of rotation calculations. + const RotationMatrix R_CA = X_CA.rotation(); + const RotationMatrix R_BA = GetRotationMatrixA(); + const RotationMatrix R_CB = GetRotationMatrixB(); + const RotationMatrix R_CA_expected = R_CB * R_BA; + EXPECT_TRUE(R_CA.IsNearlyEqualTo(R_CA_expected, 0)); + + // Expected position vector (from MotionGenesis). + const double x_expected = 5.761769695362743; + const double y_expected = 11.26952907288644; + const double z_expected = 6.192677089863299; + const Vector3d p_CoAo_C_expected(x_expected, y_expected, z_expected); + + // Check accuracy of translation calculations. + const Vector3d p_CoAo_C_actual = X_CA.translation(); + EXPECT_TRUE(p_CoAo_C_actual.isApprox(p_CoAo_C_expected, 32 * kEpsilon)); + + // Expected transform (with position vector from MotionGenesis). + // As documented in IsNearlyEqualTo(), 32 * epsilon was chosen because it is + // slightly larger than the characteristic length |p_CoAo_C| = 14.2 + const Transform X_CA_expected(R_CA_expected, p_CoAo_C_expected); + EXPECT_TRUE(X_CA.IsNearlyEqualTo(X_CA_expected, 32 * kEpsilon)); +} + +// Tests Transform multiplied by a position vector. +GTEST_TEST(Transform, OperatorMultiplyByPositionVector) { + const Transform X_CB = GetTransformB(); + + // Calculate position vector from Co to Q, expressed in C. + const Vector3d p_BoQ_B = GetPositionVectorA(); + const Vector3d p_CoQ_C = X_CB * p_BoQ_B; + + // Expected position vector (from MotionGenesis). + const double x_expected = 5.761769695362743; + const double y_expected = 11.26952907288644; + const double z_expected = 6.192677089863299; + const Vector3d p_CoQ_C_expected(x_expected, y_expected, z_expected); + + // Check accuracy of translation calculations. + EXPECT_TRUE(p_CoQ_C.isApprox(p_CoQ_C_expected, 32 * kEpsilon)); +} + +} // namespace +} // namespace math +} // namespace multibody +} // namespace drake diff --git a/multibody/multibody_tree/math/transform.cc b/multibody/multibody_tree/math/transform.cc new file mode 100755 index 000000000000..59d7e8093482 --- /dev/null +++ b/multibody/multibody_tree/math/transform.cc @@ -0,0 +1,9 @@ +#include "drake/multibody/multibody_tree/math/transform.h" + +#include "drake/common/default_scalars.h" + +// Explicitly instantiate on the most common scalar types. +// TODO(Mitiguy) Ensure this class handles Transform. +// To enable symbolic expressions, remove _NONSYMBOLIC in next line. +DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS( + class ::drake::multibody::Transform) diff --git a/multibody/multibody_tree/math/transform.h b/multibody/multibody_tree/math/transform.h new file mode 100755 index 000000000000..2f4c408fd7a4 --- /dev/null +++ b/multibody/multibody_tree/math/transform.h @@ -0,0 +1,242 @@ +#pragma once + +#include + +#include "drake/common/drake_assert.h" +#include "drake/common/drake_copyable.h" +#include "drake/common/eigen_types.h" +#include "drake/common/never_destroyed.h" +#include "drake/multibody/multibody_tree/math/rotation_matrix.h" + +namespace drake { +namespace multibody { + +/// This class represents a rigid transform between two frames, which can be +/// regarded in two ways. It can be regarded as a distance-preserving linear +/// operator (i.e., one that rotates and/or translates) which (for example) can +/// add one position vector to another and express the result in a particular +/// basis as `p_AoQ_A = X_AB * p_BoQ_B` (Q is any point). Alternately, a rigid +/// transform describes the pose between two frames A and B (i.e., the relative +/// orientation and position of A to B). Herein, the terms rotation/orientation +/// and translation/position are used interchangeably. +/// The class stores a RotationMatrix that relates right-handed orthogonal +/// unit vectors Ax, Ay, Az fixed in frame A to right-handed orthogonal +/// unit vectors Bx, By, Bz fixed in frame B. +/// The class also stores a position vector from Ao (the origin of frame A) to +/// Bo (the origin of frame B). The position vector is expressed in frame A. +/// The monogram notation for the transform relating frame A to B is `X_AB`. +/// The monogram notation for the rotation matrix relating A to B is `R_AB`. +/// The monogram notation for the position vector from Ao to Bo is `p_AoBo_A`. +/// See @ref multibody_quantities for monogram notation for dynamics. +/// +/// @note This class does not store the frames associated with the transform and +/// cannot enforce proper usage of this class. For example, it makes sense to +/// multiply transforms as `X_AB * X_BC`, but not `X_AB * X_CB`. +/// +/// @note This class is not a 4x4 transformation matrix -- even though its +/// operator*() methods act like 4x4 matrix multiplication. Instead, this class +/// contains a rotation matrix class as well as a 3x1 position vector. To form +/// a 4x4 matrix, use GetAsMatrix(). GetAsIsometry() is treated similarly. +/// +/// @tparam T The underlying scalar type. Must be a valid Eigen scalar. +template +class Transform { + public: + DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Transform) + + /// Constructs the %Transform that corresponds to aligning the two frames so + /// unit vectors Ax = Bx, Ay = By, Az = Bz and point Ao is coincident with Bo. + /// Hence, the constructed %Transform contains an identity RotationMatrix and + /// a zero position vector. + Transform() { SetIdentity(); } + + /// Constructs a %Transform from a rotation matrix and a position vector. + /// @param[in] R rotation matrix relating frames A and B (e.g., `R_AB`). + /// @param[in] p position vector from frame A's origin to frame B's origin, + /// expressed in frame A. In monogram notation p is denoted `p_AoBo_A`. + Transform(const RotationMatrix& R, const Vector3& p) + : R_AB_(R), p_AoBo_A_(p) {} + + /// Constructs a Transform from an Eigen Isometry3. + /// @param[in] pose Isometry3 that contains an allegedly valid rotation matrix + /// `R_AB` and also contains a position vector `p_AoBo_A` from frame A's + /// origin to frame B's origin. `p_AoBo_A` must be expressed in frame A. + /// @throws exception std::logic_error in debug builds if R_AB is not a proper + /// orthonormal 3x3 rotation matrix. + /// @note no attempt is made to orthogonalize the 3x3 rotation matrix part of + /// `pose`. As needed, use RotationMatrix::ProjectToRotationMatrix(). + explicit Transform(const Isometry3& pose) : + Transform(RotationMatrix(pose.linear()), pose.translation()) {} + + /// Returns the identity %Transform (which corresponds to coincident frames). + /// @returns the %Transform that corresponds to aligning the two frames so + /// unit vectors Ax = Bx, Ay = By, Az = Bz and point Ao is coincident with Bo. + /// Hence, the returned %Transform contains a 3x3 identity matrix and a + /// zero position vector. + // @internal This method's name was chosen to mimic Eigen's Identity(). + static const Transform& Identity() { + static const never_destroyed> kIdentity; + return kIdentity.access(); + } + + /// Returns R_AB, the rotation matrix portion of `this` transform. + /// @retval R_AB the rotation matrix portion of `this` transform. + const RotationMatrix& rotation() const { return R_AB_; } + + /// Sets the %RotationMatrix portion of `this` transform. + /// @param[in] R rotation matrix relating frames A and B (e.g., `R_AB`). + void set_rotation(const RotationMatrix& R) { R_AB_ = R; } + + /// Returns `p_AoBo_A`, the position vector portion of `this` transform. + /// @retval p_AoBo_A the position vector portion of `this` transform, i.e., + /// the position vector from Ao (frame A's origin) to Bo (frame B's origin). + const Vector3& translation() const { return p_AoBo_A_; } + + /// Sets the position vector portion of `this` transform. + /// @param[in] p position vector from Ao (frame A's origin) to Bo (frame B's + /// origin) expressed in frame A. In monogram notation p is denoted p_AoBo_A. + void set_translation(const Vector3& p) { p_AoBo_A_ = p; } + + /// Returns the 4x4 matrix associated with a %Transform. + /// @returns the 4x4 matrix associated with a %Transform, i.e., returns + /// ┌ ┐ + /// │ R_AB p_AoBo_A │ + /// │ │ + /// │ 0 1 │ + /// └ ┘ + Matrix4 GetAsMatrix() const { + Matrix4 pose; + pose.topLeftCorner(3, 3) = rotation().matrix(); + pose.topLeftCorner(3, 3) = rotation().matrix(); + pose.topRightCorner(3, 1) = translation(); + pose.row(3) = Vector4(0, 0, 0, 1); + return pose; + } + + /// Returns the Isometry associated with a %Transform. + /// @returns the Isometry associated with a %Transform. + Isometry3 GetAsIsometry3() const { + // pose.linear() returns a mutable reference to the 3x3 rotation matrix part + // of Isometry3 and pose.translation() returns a mutable reference to the + // 3x1 position vector part of the Isometry3. + Isometry3 pose; + pose.linear() = rotation().matrix(); + pose.translation() = translation(); + return pose; + } + + /// Sets `this` %Transform so it corresponds to aligning the two frames so + /// unit vectors Ax = Bx, Ay = By, Az = Bz and point Ao is coincident with Bo. + /// Hence, `this` %Transform contains a 3x3 identity matrix and a + /// zero position vector. + const Transform& SetIdentity() { + R_AB_ = RotationMatrix::Identity(); + p_AoBo_A_.setZero(); + return *this; + } + + /// Returns `true` if `this` is exactly the identity transform. + /// @returns `true` if `this` is exactly the identity transform. + /// @see IsIdentityToEpsilon(). + bool IsExactlyIdentity() const { + return rotation().IsExactlyIdentity() && (translation().array() == 0).all(); + } + + /// Returns true if `this` is within tolerance of the identity transform. + /// @returns `true` if the RotationMatrix portion of `this` satisfies + /// RotationMatrix::IsIdentityToInternalTolerance() and if the position vector + /// portion of `this` is equal to zero vector within `translation_tolerance`. + /// @param[in] translation_tolerance a non-negative number. One way to choose + /// `translation_tolerance` is to multiply a characteristic length + /// (e.g., the magnitude of a characteristic position vector) by an epsilon + /// (e.g., RotationMatrix::get_internal_tolerance_for_orthonormality()). + /// @see IsExactlyIdentity(). + bool IsIdentityToEpsilon(double translation_tolerance) const { + const T max_component = translation().template lpNorm(); + return max_component <= translation_tolerance && + rotation().IsIdentityToInternalTolerance(); + } + + /// Calculates X_BA = X_AB⁻¹, the inverse of `this` %Transform. + /// @retval X_BA = X_AB⁻¹ the inverse of `this` %Transform. + /// @note The inverse of transform X_AB is X_BA, which contains the rotation + /// matrix R_BA = R_AB⁻¹ = R_ABᵀ and the position vector `p_BoAo_B_`, + /// (position from B's origin Bo to A's origin Ao, expressed in frame B). + /// @note: The square-root of the condition number for a Transform is roughly + /// the magnitude of the position vector. The accuracy of the calculation for + /// the inverse of a Transform drops off with the sqrt condition number. + // @internal This method's name was chosen to mimic Eigen's inverse(). + Transform inverse() const { + const RotationMatrix R_BA = R_AB_.inverse(); + return Transform(R_BA, R_BA * (-p_AoBo_A_)); + } + + /// In-place multiply of `this` transform `X_AB` by `other` transform `X_BC`. + /// @param[in] other %Transform that post-multiplies `this`. + /// @returns `this` transform which has been multiplied by `other`. + /// On return, `this = X_AC`, where `X_AC = X_AB * X_BC`. + Transform& operator*=(const Transform& other) { + p_AoBo_A_ = *this * other.translation(); + R_AB_ *= other.rotation(); + return *this; + } + + /// Calculates `this` transform `X_AB` multiplied by `other` transform `X_BC`. + /// @param[in] other %Transform that post-multiplies `this`. + /// @retval X_AC = X_AB * X_BC + Transform operator*(const Transform& other) const { + const Vector3 p_AoCo_A = *this * other.translation(); + return Transform(rotation() * other.rotation(), p_AoCo_A); + } + + /// Calculates `this` transform `X_AB` multiplied by the position vector + /// 'p_BoQ_B` which is from Bo (B's origin) to an arbitrary point Q. + /// @param[in] p_BoQ_B position vector from Bo to Q, expressed in frame B. + /// @retval p_AoQ_A position vector from Ao to Q, expressed in frame A. + Vector3 operator*(const Vector3& p_BoQ_B) const { + return p_AoBo_A_ + R_AB_ * p_BoQ_B; + } + + /// Compares each element of `this` to the corresponding element of `other` + /// to check if they are the same to within a specified `tolerance`. + /// @param[in] other %Transform to compare to `this`. + /// @param[in] tolerance maximum allowable absolute difference between the + /// elements in `this` and `other`. + /// @returns `true` if `‖this.matrix() - other.matrix()‖∞ <= tolerance`. + /// @note Consider scaling tolerance with the largest of magA and magB, where + /// magA and magB denoted the magnitudes of `this` position vector and `other` + /// position vectors, respectively. + bool IsNearlyEqualTo(const Transform& other, double tolerance) const { + return GetMaximumAbsoluteDifference(other) <= tolerance; + } + + /// Computes the infinity norm of `this` - `other` (i.e., the maximum absolute + /// value of the difference between the elements of `this` and `other`). + /// @param[in] other %Transform to subtract from `this`. + /// @returns ‖`this` - `other`‖∞ + T GetMaximumAbsoluteDifference(const Transform& other) const { + const T R_difference = R_AB_.GetMaximumAbsoluteDifference(other.rotation()); + const T p_difference = GetMaximumAbsoluteTranslationDifference(other); + return R_difference > p_difference ? R_difference : p_difference; + } + + /// Returns the maximum absolute value of the difference in the position + /// vectors (translation) in `this` and `other`. In other words, returns + /// the infinity norm of the difference in the position vectors. + /// @param[in] other %Transform whose position vector is subtracted from + /// the position vector in `this`. + T GetMaximumAbsoluteTranslationDifference(const Transform& other) const { + const Vector3 p_difference = translation() - other.translation(); + return p_difference.template lpNorm(); + } + + private: + // Rotation matrix relating two frames, e.g. frame A and frame B. + RotationMatrix R_AB_; + + // Position vector from A's origin Ao to B's origin Bo, expressed in A. + Vector3 p_AoBo_A_; +}; + +} // namespace multibody +} // namespace drake From 23444420b4f8a886d89ad13f52e3e551430fa500 Mon Sep 17 00:00:00 2001 From: Jamie Snape Date: Wed, 27 Dec 2017 16:42:11 -0500 Subject: [PATCH 11/25] Use system gflags --- WORKSPACE | 10 +-- automotive/maliput/dragway/BUILD.bazel | 2 +- common/BUILD.bazel | 2 +- examples/acrobot/BUILD.bazel | 12 +-- examples/contact_model/BUILD.bazel | 6 +- examples/humanoid_controller/BUILD.bazel | 2 +- examples/kinova_jaco_arm/BUILD.bazel | 8 +- examples/kuka_iiwa_arm/BUILD.bazel | 6 +- .../kuka_iiwa_arm/controlled_kuka/BUILD.bazel | 2 +- .../dev/box_rotation/BUILD.bazel | 4 +- .../dev/monolithic_pick_and_place/BUILD.bazel | 2 +- .../dev/pick_and_place/BUILD.bazel | 6 +- examples/multibody/pendulum/BUILD.bazel | 2 +- examples/pendulum/BUILD.bazel | 8 +- examples/pr2/BUILD.bazel | 2 +- examples/quadrotor/BUILD.bazel | 4 +- examples/rod2d/BUILD.bazel | 2 +- examples/schunk_wsg/BUILD.bazel | 2 +- examples/valkyrie/BUILD.bazel | 2 +- manipulation/util/BUILD.bazel | 2 +- multibody/dev/BUILD.bazel | 2 +- setup/mac/install_prereqs.sh | 1 + setup/ubuntu/16.04/install_prereqs.sh | 1 + systems/sensors/BUILD.bazel | 4 +- tools/install/libdrake/drake.cps | 11 +-- tools/workspace/BUILD.bazel | 1 - tools/workspace/gflags/BUILD.bazel | 36 +-------- tools/workspace/gflags/gflags.bzl | 75 +++++++++++++++++++ tools/workspace/gflags/gflags.cps | 15 ---- 29 files changed, 127 insertions(+), 105 deletions(-) create mode 100644 tools/workspace/gflags/gflags.bzl delete mode 100644 tools/workspace/gflags/gflags.cps diff --git a/WORKSPACE b/WORKSPACE index 2193012041ee..0a58f36beacc 100644 --- a/WORKSPACE +++ b/WORKSPACE @@ -128,12 +128,10 @@ github_archive( build_file = "tools/workspace/gtest/gtest.BUILD.bazel", ) -# When updating the version of gflags, update tools/install/gflags/gflags.cps -github_archive( - name = "com_github_gflags_gflags", - repository = "gflags/gflags", - commit = "95ffb27c9c7496ede1409e042571054c70cb9519", - sha256 = "723c21f783c720c0403c9b44bf500d1961a08bd2635cbc117107af22d2e1643f", # noqa +load("//tools/workspace/gflags:gflags.bzl", "gflags_repository") + +gflags_repository( + name = "gflags", ) github_archive( diff --git a/automotive/maliput/dragway/BUILD.bazel b/automotive/maliput/dragway/BUILD.bazel index 9e1ea80f8bd6..7829258f0abb 100644 --- a/automotive/maliput/dragway/BUILD.bazel +++ b/automotive/maliput/dragway/BUILD.bazel @@ -42,7 +42,7 @@ drake_cc_binary( "//drake/automotive/maliput/utility", "//drake/common:essential", "//drake/common:text_logging_gflags", - "@com_github_gflags_gflags//:gflags", + "@gflags", "@spruce", ], ) diff --git a/common/BUILD.bazel b/common/BUILD.bazel index 14345191d052..74555ef79f05 100644 --- a/common/BUILD.bazel +++ b/common/BUILD.bazel @@ -325,7 +325,7 @@ drake_cc_library( }), deps = [ ":essential", - "@com_github_gflags_gflags//:gflags", + "@gflags", ], ) diff --git a/examples/acrobot/BUILD.bazel b/examples/acrobot/BUILD.bazel index d2af0efb8e65..76834f3e586f 100644 --- a/examples/acrobot/BUILD.bazel +++ b/examples/acrobot/BUILD.bazel @@ -71,7 +71,7 @@ drake_cc_binary( "//drake/multibody/parsers", "//drake/multibody/rigid_body_plant:drake_visualizer", "//drake/systems/analysis:simulator", - "@com_github_gflags_gflags//:gflags", + "@gflags", ], ) @@ -97,7 +97,7 @@ drake_cc_binary( "//drake/systems/primitives:linear_system", "//drake/systems/primitives:signal_logger", "//drake/systems/sensors:rotary_encoders", - "@com_github_gflags_gflags//:gflags", + "@gflags", ], ) @@ -117,7 +117,7 @@ drake_cc_binary( "//drake/multibody/rigid_body_plant:drake_visualizer", "//drake/systems/analysis:simulator", "//drake/systems/framework:diagram", - "@com_github_gflags_gflags//:gflags", + "@gflags", ], ) @@ -138,7 +138,7 @@ drake_cc_binary( "//drake/multibody/rigid_body_plant:drake_visualizer", "//drake/systems/analysis", "//drake/systems/controllers:linear_quadratic_regulator", - "@com_github_gflags_gflags//:gflags", + "@gflags", ], ) @@ -160,7 +160,7 @@ drake_cc_binary( "//drake/systems/controllers:linear_quadratic_regulator", "//drake/systems/primitives:trajectory_source", "//drake/systems/trajectory_optimization:direct_collocation", - "@com_github_gflags_gflags//:gflags", + "@gflags", ], ) @@ -195,7 +195,7 @@ drake_cc_binary( "//drake/multibody/rigid_body_plant:drake_visualizer", "//drake/systems/analysis", "//drake/systems/controllers:linear_quadratic_regulator", - "@com_github_gflags_gflags//:gflags", + "@gflags", ], ) diff --git a/examples/contact_model/BUILD.bazel b/examples/contact_model/BUILD.bazel index ed275c62c1ec..621959bd1acb 100644 --- a/examples/contact_model/BUILD.bazel +++ b/examples/contact_model/BUILD.bazel @@ -27,7 +27,7 @@ drake_cc_binary( "//drake/multibody/rigid_body_plant:drake_visualizer", "//drake/systems/analysis", "//drake/systems/primitives:constant_vector_source", - "@com_github_gflags_gflags//:gflags", + "@gflags", ], ) @@ -52,7 +52,7 @@ drake_cc_binary( "//drake/multibody/rigid_body_plant:contact_results_to_lcm", "//drake/multibody/rigid_body_plant:drake_visualizer", "//drake/systems/analysis", - "@com_github_gflags_gflags//:gflags", + "@gflags", ], ) @@ -76,7 +76,7 @@ drake_cc_binary( "//drake/multibody/rigid_body_plant:drake_visualizer", "//drake/systems/analysis", "//drake/systems/lcm", - "@com_github_gflags_gflags//:gflags", + "@gflags", ], ) diff --git a/examples/humanoid_controller/BUILD.bazel b/examples/humanoid_controller/BUILD.bazel index 50b380e57528..0d15a5f67724 100644 --- a/examples/humanoid_controller/BUILD.bazel +++ b/examples/humanoid_controller/BUILD.bazel @@ -102,7 +102,7 @@ drake_cc_binary( "//drake/manipulation/util:robot_state_msg_translator", "//drake/multibody:rigid_body_tree", "//drake/multibody/parsers", - "@com_github_gflags_gflags//:gflags", + "@gflags", "@lcmtypes_bot2_core", "@lcmtypes_robotlocomotion", ], diff --git a/examples/kinova_jaco_arm/BUILD.bazel b/examples/kinova_jaco_arm/BUILD.bazel index 6655eeaeecd9..3880dc571ad1 100644 --- a/examples/kinova_jaco_arm/BUILD.bazel +++ b/examples/kinova_jaco_arm/BUILD.bazel @@ -54,7 +54,7 @@ drake_cc_binary( "//drake/systems/primitives:adder", "//drake/systems/primitives:demultiplexer", "//drake/systems/primitives:multiplexer", - "@com_github_gflags_gflags//:gflags", + "@gflags", "@lcmtypes_bot2_core", "@lcmtypes_robotlocomotion", ], @@ -78,7 +78,7 @@ drake_cc_binary( "//drake/multibody/rigid_body_plant:drake_visualizer", "//drake/systems/analysis:simulator", "//drake/systems/primitives:constant_vector_source", - "@com_github_gflags_gflags//:gflags", + "@gflags", ], ) @@ -104,7 +104,7 @@ drake_cc_binary( "//drake/systems/analysis:simulator", "//drake/systems/controllers:inverse_dynamics_controller", "//drake/systems/primitives:constant_vector_source", - "@com_github_gflags_gflags//:gflags", + "@gflags", ], ) @@ -128,7 +128,7 @@ drake_cc_binary( "//drake/systems/analysis:simulator", "//drake/systems/controllers:inverse_dynamics_controller", "//drake/systems/primitives:trajectory_source", - "@com_github_gflags_gflags//:gflags", + "@gflags", ], ) diff --git a/examples/kuka_iiwa_arm/BUILD.bazel b/examples/kuka_iiwa_arm/BUILD.bazel index cdc8da69da73..e87363a9178b 100644 --- a/examples/kuka_iiwa_arm/BUILD.bazel +++ b/examples/kuka_iiwa_arm/BUILD.bazel @@ -90,7 +90,7 @@ drake_example_cc_binary( deps = [ ":iiwa_common", ":lcm_plan_interpolator", - "@com_github_gflags_gflags//:gflags", + "@gflags", ], ) @@ -111,7 +111,7 @@ drake_example_cc_binary( ":iiwa_lcm", ":oracular_state_estimator", "//drake/examples/kuka_iiwa_arm/iiwa_world:iiwa_wsg_diagram_factory", - "@com_github_gflags_gflags//:gflags", + "@gflags", ], ) @@ -129,7 +129,7 @@ drake_example_cc_binary( deps = [ ":iiwa_common", ":iiwa_lcm", - "@com_github_gflags_gflags//:gflags", + "@gflags", ], ) diff --git a/examples/kuka_iiwa_arm/controlled_kuka/BUILD.bazel b/examples/kuka_iiwa_arm/controlled_kuka/BUILD.bazel index e16480cdfbe6..511b6ae11c93 100644 --- a/examples/kuka_iiwa_arm/controlled_kuka/BUILD.bazel +++ b/examples/kuka_iiwa_arm/controlled_kuka/BUILD.bazel @@ -24,7 +24,7 @@ drake_cc_binary( "//drake/systems/analysis:simulator", "//drake/systems/controllers:inverse_dynamics_controller", "//drake/systems/primitives:trajectory_source", - "@com_github_gflags_gflags//:gflags", + "@gflags", ], ) diff --git a/examples/kuka_iiwa_arm/dev/box_rotation/BUILD.bazel b/examples/kuka_iiwa_arm/dev/box_rotation/BUILD.bazel index fe01d3cf60aa..85f6086455b3 100644 --- a/examples/kuka_iiwa_arm/dev/box_rotation/BUILD.bazel +++ b/examples/kuka_iiwa_arm/dev/box_rotation/BUILD.bazel @@ -26,7 +26,7 @@ drake_cc_binary( deps = [ "//drake/common:find_resource", "//drake/examples/kuka_iiwa_arm:iiwa_common", - "@com_github_gflags_gflags//:gflags", + "@gflags", ], ) @@ -81,7 +81,7 @@ drake_cc_binary( "//drake/systems/sensors:optitrack_encoder", "//drake/systems/sensors:optitrack_sender", "//drake/util:lcm_util", - "@com_github_gflags_gflags//:gflags", + "@gflags", "@lcmtypes_bot2_core", "@lcmtypes_robotlocomotion", ], diff --git a/examples/kuka_iiwa_arm/dev/monolithic_pick_and_place/BUILD.bazel b/examples/kuka_iiwa_arm/dev/monolithic_pick_and_place/BUILD.bazel index 2e1649644c40..847a528288bb 100644 --- a/examples/kuka_iiwa_arm/dev/monolithic_pick_and_place/BUILD.bazel +++ b/examples/kuka_iiwa_arm/dev/monolithic_pick_and_place/BUILD.bazel @@ -57,7 +57,7 @@ drake_cc_binary( "//drake/lcm", "//drake/lcmtypes:iiwa", "//drake/systems/analysis:simulator", - "@com_github_gflags_gflags//:gflags", + "@gflags", "@lcmtypes_bot2_core", ], ) diff --git a/examples/kuka_iiwa_arm/dev/pick_and_place/BUILD.bazel b/examples/kuka_iiwa_arm/dev/pick_and_place/BUILD.bazel index a65d0bd701ee..fd3740836124 100644 --- a/examples/kuka_iiwa_arm/dev/pick_and_place/BUILD.bazel +++ b/examples/kuka_iiwa_arm/dev/pick_and_place/BUILD.bazel @@ -98,7 +98,7 @@ drake_cc_library( "//drake/systems/lcm", "//drake/systems/primitives:pass_through", "//drake/util:lcm_util", - "@com_github_gflags_gflags//:gflags", + "@gflags", "@lcmtypes_bot2_core", "@lcmtypes_robotlocomotion", ], @@ -142,7 +142,7 @@ drake_cc_binary( "//drake/systems/lcm", "//drake/systems/lcm:lcm_driven_loop", "//drake/util:lcm_util", - "@com_github_gflags_gflags//:gflags", + "@gflags", "@lcmtypes_bot2_core", "@lcmtypes_robotlocomotion", ], @@ -174,7 +174,7 @@ drake_cc_binary( "//drake/multibody/rigid_body_plant:contact_results_to_lcm", "//drake/multibody/rigid_body_plant:create_load_robot_message", "//drake/systems/analysis:simulator", - "@com_github_gflags_gflags//:gflags", + "@gflags", "@lcmtypes_bot2_core", "@optitrack_driver//lcmtypes:optitrack_lcmtypes", ], diff --git a/examples/multibody/pendulum/BUILD.bazel b/examples/multibody/pendulum/BUILD.bazel index 54c00e560e30..8b060c99efbf 100644 --- a/examples/multibody/pendulum/BUILD.bazel +++ b/examples/multibody/pendulum/BUILD.bazel @@ -39,7 +39,7 @@ drake_cc_binary( "//drake/systems/framework:diagram", "//drake/systems/lcm", "//drake/systems/rendering:pose_bundle_to_draw_message", - "@com_github_gflags_gflags//:gflags", + "@gflags", ], ) diff --git a/examples/pendulum/BUILD.bazel b/examples/pendulum/BUILD.bazel index fd2af18a289a..166e504032f1 100644 --- a/examples/pendulum/BUILD.bazel +++ b/examples/pendulum/BUILD.bazel @@ -50,7 +50,7 @@ drake_cc_binary( "//drake/systems/analysis:simulator", "//drake/systems/framework:diagram", "//drake/systems/primitives:constant_vector_source", - "@com_github_gflags_gflags//:gflags", + "@gflags", ], ) @@ -70,7 +70,7 @@ drake_cc_binary( "//drake/systems/analysis:simulator", "//drake/systems/framework:diagram", "//drake/systems/framework:leaf_system", - "@com_github_gflags_gflags//:gflags", + "@gflags", ], ) @@ -92,7 +92,7 @@ drake_cc_binary( "//drake/systems/controllers:linear_quadratic_regulator", "//drake/systems/framework:diagram", "//drake/systems/framework:leaf_system", - "@com_github_gflags_gflags//:gflags", + "@gflags", ], ) @@ -115,7 +115,7 @@ drake_cc_binary( "//drake/systems/framework", "//drake/systems/primitives:trajectory_source", "//drake/systems/trajectory_optimization:direct_collocation", - "@com_github_gflags_gflags//:gflags", + "@gflags", ], ) diff --git a/examples/pr2/BUILD.bazel b/examples/pr2/BUILD.bazel index 3c6f3106b71c..a7193887b586 100644 --- a/examples/pr2/BUILD.bazel +++ b/examples/pr2/BUILD.bazel @@ -26,7 +26,7 @@ drake_cc_binary( "//drake/systems/analysis:simulator", "//drake/systems/framework:diagram_builder", "//drake/systems/primitives:constant_vector_source", - "@com_github_gflags_gflags//:gflags", + "@gflags", ], ) diff --git a/examples/quadrotor/BUILD.bazel b/examples/quadrotor/BUILD.bazel index f75e54a3ee8a..067b4fa44ce0 100644 --- a/examples/quadrotor/BUILD.bazel +++ b/examples/quadrotor/BUILD.bazel @@ -52,7 +52,7 @@ drake_cc_binary( "//drake/systems/analysis:simulator", "//drake/systems/framework:diagram", "//drake/systems/primitives:constant_vector_source", - "@com_github_gflags_gflags//:gflags", + "@gflags", ], ) @@ -77,7 +77,7 @@ drake_cc_binary( "//drake/multibody/rigid_body_plant:drake_visualizer", "//drake/systems/analysis:simulator", "//drake/systems/framework:diagram", - "@com_github_gflags_gflags//:gflags", + "@gflags", ], ) diff --git a/examples/rod2d/BUILD.bazel b/examples/rod2d/BUILD.bazel index f55c9bbe7778..0fa3fee55c15 100644 --- a/examples/rod2d/BUILD.bazel +++ b/examples/rod2d/BUILD.bazel @@ -36,7 +36,7 @@ drake_cc_binary( "//drake/systems/rendering:drake_visualizer_client", "//drake/systems/rendering:pose_aggregator", "//drake/systems/rendering:pose_bundle_to_draw_message", - "@com_github_gflags_gflags//:gflags", + "@gflags", ], ) diff --git a/examples/schunk_wsg/BUILD.bazel b/examples/schunk_wsg/BUILD.bazel index 15a9db6313fe..e7e0af968670 100644 --- a/examples/schunk_wsg/BUILD.bazel +++ b/examples/schunk_wsg/BUILD.bazel @@ -41,7 +41,7 @@ drake_cc_binary( "//drake/systems/analysis:simulator", "//drake/systems/framework", "//drake/systems/lcm", - "@com_github_gflags_gflags//:gflags", + "@gflags", ], ) diff --git a/examples/valkyrie/BUILD.bazel b/examples/valkyrie/BUILD.bazel index 7c8d8890b418..0d6a8bd3416a 100644 --- a/examples/valkyrie/BUILD.bazel +++ b/examples/valkyrie/BUILD.bazel @@ -114,7 +114,7 @@ drake_cc_library( "//drake/systems/lcm:lcmt_drake_signal_translator", "//drake/systems/primitives:constant_vector_source", "//drake/systems/primitives:pass_through", - "@com_github_gflags_gflags//:gflags", + "@gflags", ], ) diff --git a/manipulation/util/BUILD.bazel b/manipulation/util/BUILD.bazel index 95f49a18c024..e244ca22b056 100644 --- a/manipulation/util/BUILD.bazel +++ b/manipulation/util/BUILD.bazel @@ -92,7 +92,7 @@ drake_cc_binary( "//drake/lcm", "//drake/multibody/parsers", "//drake/systems/framework", - "@com_github_gflags_gflags//:gflags", + "@gflags", ], ) diff --git a/multibody/dev/BUILD.bazel b/multibody/dev/BUILD.bazel index 53b88e18e461..89c44fef0413 100644 --- a/multibody/dev/BUILD.bazel +++ b/multibody/dev/BUILD.bazel @@ -60,7 +60,7 @@ drake_cc_googletest( "//drake/systems/analysis:simulator", "//drake/systems/framework:diagram", "//drake/systems/primitives:constant_vector_source", - "@com_github_gflags_gflags//:gflags", + "@gflags", ], ) diff --git a/setup/mac/install_prereqs.sh b/setup/mac/install_prereqs.sh index d4840433cece..b094977eb4b8 100755 --- a/setup/mac/install_prereqs.sh +++ b/setup/mac/install_prereqs.sh @@ -42,6 +42,7 @@ cmake diffstat doxygen dreal/dreal/dreal +gflags glew glib graphviz diff --git a/setup/ubuntu/16.04/install_prereqs.sh b/setup/ubuntu/16.04/install_prereqs.sh index 61748e2517d1..9af7c89eb16f 100755 --- a/setup/ubuntu/16.04/install_prereqs.sh +++ b/setup/ubuntu/16.04/install_prereqs.sh @@ -72,6 +72,7 @@ libboost-all-dev libbz2-dev libexpat1-dev libfreetype6-dev +libgflags-dev libglib2.0-dev libglu1-mesa-dev libhdf5-dev diff --git a/systems/sensors/BUILD.bazel b/systems/sensors/BUILD.bazel index fa030745ef0f..f46f3e001f7c 100644 --- a/systems/sensors/BUILD.bazel +++ b/systems/sensors/BUILD.bazel @@ -249,7 +249,7 @@ drake_cc_binary( "//drake/systems/analysis:simulator", "//drake/systems/lcm", "//drake/systems/rendering:pose_stamped_t_pose_vector_translator", - "@com_github_gflags_gflags//:gflags", + "@gflags", ], ) @@ -304,7 +304,7 @@ drake_cc_binary( "//drake/common:text_logging_gflags", "//drake/lcm", "//drake/systems/analysis:simulator", - "@com_github_gflags_gflags//:gflags", + "@gflags", ], ) diff --git a/tools/install/libdrake/drake.cps b/tools/install/libdrake/drake.cps index 163bc5410d93..9be12a393f78 100644 --- a/tools/install/libdrake/drake.cps +++ b/tools/install/libdrake/drake.cps @@ -35,11 +35,6 @@ "Hints": ["@prefix@/lib/cmake/fmt"], "X-CMake-Find-Args": ["CONFIG"] }, - "gflags": { - "Version": "2.2.0", - "Hints": ["@prefix@/lib/cmake/gflags"], - "X-CMake-Find-Args": ["CONFIG"] - }, "ignition-math3": { "Version": "3.2.0", "Hints": ["@prefix@/lib/cmake/ignition-math3"], @@ -126,10 +121,8 @@ "drake-common-text-logging-gflags": { "Type": "interface", "Includes": ["@prefix@/include"], - "Requires": [ - ":drake", - "gflags:gflags" - ] + "Link-Flags": ["-lgflags"], + "Requires": [":drake"] }, "drake-lcmtypes-cpp": { "Type": "interface", diff --git a/tools/workspace/BUILD.bazel b/tools/workspace/BUILD.bazel index 12032dd7376e..1bd9868ea70b 100644 --- a/tools/workspace/BUILD.bazel +++ b/tools/workspace/BUILD.bazel @@ -40,7 +40,6 @@ _DRAKE_EXTERNAL_PACKAGE_INSTALLS = ["@%s//:install" % p for p in [ "vtk", "yaml_cpp", ]] + ["//tools/workspace/%s:install" % p for p in [ - "gflags", "jchart2d", "optitrack_driver", ]] + select({ diff --git a/tools/workspace/gflags/BUILD.bazel b/tools/workspace/gflags/BUILD.bazel index ef8a01360ff9..2e5301dc6578 100644 --- a/tools/workspace/gflags/BUILD.bazel +++ b/tools/workspace/gflags/BUILD.bazel @@ -1,38 +1,8 @@ # -*- python -*- -load( - "@drake//tools/install:install.bzl", - "cmake_config", - "install", - "install_cmake_config", -) -load("//tools/lint:lint.bzl", "add_lint_tests") - -package(default_visibility = ["//visibility:public"]) - -cmake_config( - cps_file_name = "gflags.cps", - package = "gflags", -) +# This file exists to make our directory into a Bazel package, so that our +# neighboring *.bzl file can be loaded elsewhere. -install_cmake_config(package = "gflags") # Creates rule :install_cmake_config. - -install( - name = "install", - targets = ["@com_github_gflags_gflags//:gflags"], - hdr_dest = "include", - hdr_strip_prefix = ["include"], - guess_hdrs = "PACKAGE", - guess_hdrs_exclude = [ - "config.h", - "src/mutex.h", - "src/util.h", - ], - docs = ["@com_github_gflags_gflags//:COPYING.txt"], - doc_dest = "share/doc/gflags", - allowed_externals = ["@com_github_gflags_gflags//:gflags"], - visibility = ["//tools/workspace:__pkg__"], - deps = [":install_cmake_config"], -) +load("//tools/lint:lint.bzl", "add_lint_tests") add_lint_tests() diff --git a/tools/workspace/gflags/gflags.bzl b/tools/workspace/gflags/gflags.bzl new file mode 100644 index 000000000000..b12deb58e470 --- /dev/null +++ b/tools/workspace/gflags/gflags.bzl @@ -0,0 +1,75 @@ +# -*- mode: python -*- +# vi: set ft=python : + +""" +Makes system-installed gflags headers and library available to be used as a +C/C++ dependency. On macOS, pkg-config is used to locate the gflags headers and +library. On Ubuntu Xenial, no pkg-config gflags.pc file is installed, but the +gflags headers and library are always located in /usr/include and +/usr/lib/x86_64-linux-gnu, respectively. + +Example: + WORKSPACE: + load("//tools/workspace/gflags:gflags.bzl", "gflags_repository") + gflags_repository(name = "foo") + + BUILD: + cc_library( + name = "foobar", + deps = ["@foo//:gflags"], + srcs = ["bar.cc"], + ) + +Argument: + name: A unique name for this rule. +""" + +load( + "@kythe//tools/build_rules/config:pkg_config.bzl", + "setup_pkg_config_package", +) +load("@drake//tools/workspace:os.bzl", "determine_os") + +def _impl(repository_ctx): + os_result = determine_os(repository_ctx) + if os_result.error != None: + fail(os_result.error) + + if os_result.ubuntu_release == "16.04": + repository_ctx.symlink("/usr/include/gflags", "include/gflags") + + file_content = """ +cc_library( + name = "gflags", + hdrs = [ + "include/gflags/gflags.h", + "include/gflags/gflags_completions.h", + "include/gflags/gflags_declare.h", + "include/gflags/gflags_gflags.h", + ], + includes = ["include"], + linkopts = ["-lgflags"], + visibility = ["//visibility:public"], +) +""" + + repository_ctx.file("BUILD", content = file_content, + executable = False) + else: + error = setup_pkg_config_package(repository_ctx).error + + if error != None: + fail(error) + +gflags_repository = repository_rule( + attrs = { + "modname": attr.string(default = "gflags"), + "build_file_template": attr.label( + default = Label("@kythe//tools/build_rules/config:BUILD.tpl"), + single_file = True, + allow_files = True, + ), + }, + local = True, + implementation = _impl, +) diff --git a/tools/workspace/gflags/gflags.cps b/tools/workspace/gflags/gflags.cps deleted file mode 100644 index 90c003d06214..000000000000 --- a/tools/workspace/gflags/gflags.cps +++ /dev/null @@ -1,15 +0,0 @@ -{ - "Cps-Version": "0.8.0", - "Name": "gflags", - "Description": "A C++ library that implements command line flag processing", - "License": "BSD-3-Clause", - "Version": "2.2.0", - "Default-Components": [":gflags"], - "Components": { - "gflags": { - "Type": "dylib", - "Location": "@prefix@/lib/libgflags.so", - "Includes": ["@prefix@/include"] - } - } -} From 3592fa2aa4e8decf42323d2e47879632a1a72c38 Mon Sep 17 00:00:00 2001 From: Jamie Snape Date: Thu, 28 Dec 2017 09:28:23 -0500 Subject: [PATCH 12/25] Disable call_python_full_test due to multiple CI failures --- common/proto/BUILD.bazel | 3 +++ 1 file changed, 3 insertions(+) diff --git a/common/proto/BUILD.bazel b/common/proto/BUILD.bazel index 8c073d161a1b..e8c6fbbc5e04 100644 --- a/common/proto/BUILD.bazel +++ b/common/proto/BUILD.bazel @@ -114,7 +114,10 @@ sh_test( # lsan; however, when the binary fails, it simply aborts without # diagnostic information. When this is resolved, these tags should be # removed. + # TODO(jamiesnape): Tagged "manual" because fails with latest Matplotlib + # (~2.1.1) on macOS (and possibly other platforms). tags = [ + "manual", "no_asan", "no_lsan", ], From 0215e565aa38aaa9271d7757174a1c5ec61bcd6c Mon Sep 17 00:00:00 2001 From: Michael Sherman Date: Tue, 2 Jan 2018 12:55:05 -0800 Subject: [PATCH 13/25] Move caching design document to doxygen (#7674) Move caching design document to doxygen. Fix doxygen image source location (remove spurious "/drake"). Fix old doxygen image references to use full path to image source. Add mainpage links to technical notes. --- doc/Doxyfile_CXX.in | 2 +- doc/doxygen.h | 6 + .../rigid_body_plant/contact_model_doxygen.h | 6 +- systems/framework/cache_doxygen.h | 405 +++++++++ .../framework/images/system_context_cache.png | Bin 0 -> 58102 bytes .../framework/images/system_context_cache.svg | 770 ++++++++++++++++++ 6 files changed, 1185 insertions(+), 4 deletions(-) create mode 100644 systems/framework/cache_doxygen.h create mode 100644 systems/framework/images/system_context_cache.png create mode 100644 systems/framework/images/system_context_cache.svg diff --git a/doc/Doxyfile_CXX.in b/doc/Doxyfile_CXX.in index a482a2ada26d..4e49e4d1c16f 100644 --- a/doc/Doxyfile_CXX.in +++ b/doc/Doxyfile_CXX.in @@ -133,7 +133,7 @@ EXCLUDE_SYMBOLS = EXAMPLE_PATH = EXAMPLE_PATTERNS = EXAMPLE_RECURSIVE = NO -IMAGE_PATH = "@DRAKE_WORKSPACE_ROOT@/drake" +IMAGE_PATH = "@DRAKE_WORKSPACE_ROOT@" INPUT_FILTER = FILTER_PATTERNS = FILTER_SOURCE_FILES = NO diff --git a/doc/doxygen.h b/doc/doxygen.h index 29aa14dd518c..01e9bd9a5f5a 100644 --- a/doc/doxygen.h +++ b/doc/doxygen.h @@ -54,6 +54,12 @@ out the Doxygen C++ documentation

hosted online for the master branch, but is only updated nightly.

+

Technical Notes

+These links provide notes on Drake's design and implementation. + +- @ref multibody_concepts +- @ref cache_design_notes + */ diff --git a/multibody/rigid_body_plant/contact_model_doxygen.h b/multibody/rigid_body_plant/contact_model_doxygen.h index 10d0cce587fb..69482f8992d3 100644 --- a/multibody/rigid_body_plant/contact_model_doxygen.h +++ b/multibody/rigid_body_plant/contact_model_doxygen.h @@ -112,7 +112,7 @@ forces are equal and opposite, we limit our discussion to the force `f` acting on `A` at `Ac` (such that `-f` acts on `B` at `Bc`). - @image html simple_contact.png "Figure 1: Illustration of contact between two spheres." + @image html multibody/rigid_body_plant/images/simple_contact.png "Figure 1: Illustration of contact between two spheres." The computation of the contact force is most naturally discussed in the contact frame `C` (shown in Figure 1). @@ -487,7 +487,7 @@ fₚ Figure 2: Idealized Stiction/Sliding Friction Model --> - @image html ideal_stiction.png "Figure 2: Idealized Stiction/Sliding Friction Model" + @image html multibody/rigid_body_plant/images/ideal_stiction.png "Figure 2: Idealized Stiction/Sliding Friction Model" In _idealized_ stiction, tangent force `fₜ` is equal and opposite to the pushing force `fₚ` up to the point where that force is sufficient to @@ -523,7 +523,7 @@ Figure 3: Stribeck function for stiction. --> - @image html stribeck.png "Figure 3: Stribeck function for stiction" + @image html multibody/rigid_body_plant/images/stribeck.png "Figure 3: Stribeck function for stiction" +@parblock
+ \"There are only two hard things in computer science:
+ cache invalidation, and naming things.\"
+ -- Phil Karlton +
+@endparblock + +@warning DRAFT -- to be reviewed along with caching code. The text here refers +to objects that are not yet in Drake's master branch. + +

Background

+ +Drake System objects are used to specify the computational _structure_ of a +model to be studied. The actual _values_ during computation are stored in a +separate Context object. The Context contains _source_ values (time, parameters, +states, input ports) and _computed_ values (e.g. derivatives, output ports) +that depend on some or all of the source values. We call the particular +dependencies of a computed value its _prerequisites_. The caching system +described here manages computed values so that + - they are recomputed _only if_ a prerequisite has changed, + - they are marked out of date _whenever_ a prerequisite changes, and + - _every_ access to a computed value first ensures that it is up to date. + +Accessing computed values is a critical, inner-loop activity during +simulation (many times per step) so the implementation is designed to provide +validity-checked access at minimal computational cost. Marking computations +out of date as prerequisites change is also frequent (at least once per step), +and potentially expensive, so must be efficient; the implementation goes to +great lengths to minimize the cost of that operation. + +@anchor cache_design_goals +

Design Constraints and Goals

+ +Caching is entirely about performance. Hence, other than correctness, the +performance goals listed above are the primary architectural constraints. Other +goals also influenced the design. Here are the main goals in roughly descending +order of importance. + 1. Must be correct (same result with caching on or off). + 2. Must be fast. + 3. Must preserve independence of System and Context objects (e.g., no + cross-pointers). + 4. Should provide a simple conceptual model and API for users. + 5. Should treat all value sources and dependencies in Drake uniformly. + 6. Should be backwards compatible with the existing API. + +In service of correctness and speed we need instrumentation for debugging and +performance measurement. For example, it should be possible to disable caching +to verify that the results don’t change. + +Not a goal for the initial implementation: automatic determination or +validation of dependency lists. Instead we rely on a conservative default +(depends on everything), and the ability to disable caching during testing. +Several follow-ons are possible to improve this: + - Runtime validation that computations only request sub-computations for which + they hold tickets (probably affordable only in Debug), and + - Use of symbolic expressions to calculate dependency lists automatically. + +@anchor cache_design_architecture +

Architecture

+ +This is the basic architecture Drake uses to address the above design goals. + +Every declared source and computed value is assigned a small-integer +DependencyTicket (“ticket”) by the System (unique within a subsystem). The +Context contains corresponding DependencyTracker ("tracker") objects that manage +that value’s downstream dependents and upstream prerequisites; these can be +accessed efficiently using the ticket. Cached computations are declared and +accessed through System-owned CacheEntry objects; their values are stored in +Context-owned CacheEntryValue objects whose validity with respect to +source values is rigorously tracked via their associated dependency +trackers. Computed values may in turn serve as +source values for further computations. A change to a source value invalidates +any computed values that depend on it, recursively. Any value contained in a +Context may be exported via an output port that exposes that value to downstream +Systems; inter-subsystem dependencies are tracked using the same mechanism as +intra-subsystem dependencies. + +From a user perspective: + - Cache entries of arbitrary type are allocated to hold the results of all + significant computations, including built-ins like derivatives, discrete + updates, and output ports as well as user-defined internal computations. + Prerequisites for these computations are explicitly noted at the time they + are declared; the default is dependency on all sources. + - Computation is initiated when a result is requested via an “Eval” method, if + the result is not already up to date with respect to its prerequisites, which + are recursively obtained using their Eval methods. + - Cached results are automatically invalidated when any of their prerequisites + change. + +Figure 1 below illustrates the computational structure of a Drake LeafSystem, +paired with its LeafContext. A Drake Diagram interconnects subsystems like +these by connecting output ports of subsystems to input ports of other +subsystems, and aggregates results such as derivative calculations. When +referring to individual Systems within a diagram, we use the terms +“subsystem” and “subcontext”. */ + +// Can't put the following in doxygen comments because the "-->" arrows +// terminate them! + +/* Looks something like this ... + + drake::systems::LeafSystem + ┌─────────────────────────────────────┐ + time ------>│>------. ┌───────┐ ···---│---> derivatives, + │ `--------->│cache │----------│---> updates, etc. + input --->│>---------------->│entry │ │ + ports u --->│ └───────┘ │──────┐ + ┌--->│ ┌───────┐ ···---│----------> output + | │ (Parameters)---->│cache │----------│----------> ports y + | │ (State)--------->│entry │ ···---│----------> + | │ └───────┘ │ │ + | └─────────────────────────────────────┘ │ + | │ │ + └<----------│-(Fixed input) │ + │ │ + └─────────────────────────────────────┘ + drake::systems::LeafContext +*/ + +/** @addtogroup cache_design_notes +@image html systems/framework/images/system_context_cache.png "Figure 1: Computational structure of a Drake System." + +In Figure 1 above, values are shown in gray like the Context to emphasize +that they are actually being stored in the Context. The System can declare the +structure (colored borders), but does not contain the actual values.There can +also be values in the Context of which the System is unaware, including the +Fixed Input values as shown, and additional cached computations. Computed values +depend on source values, but are shown rounded on one side to emphasize that +they then become sources to downstream computations. The arrows as drawn should +be considered “is-prerequisite-of” edges; drawn in the reverse direction they +could be labeled “depends-on” edges. + +@anchor cache_design_value_sources +

Value sources

+ +When a cache entry is allocated, a list of prerequisite value sources is +provided by listing the dependency tickets for those sources. Only +_intra_-System dependencies are permitted; _inter_-System dependencies are +expressed by listing an input port as a prerequisite. There are six kinds of +value sources within a System’s Context that can serve as prerequisites for +cached results within that Context: + 1. Time + 2. Input ports + 3. Parameters (numerical and abstract) + 4. State (including continuous, discrete, and abstract variables) + 5. Other cache entries + 6. Accuracy + +In addition, we support "fixed" input ports whose values are provided locally; +each such value is a value source, but is restricted to its corresponding +input port. Fixed values are semantically like additional Parameters. + +The Accuracy setting serves as a prerequisite for cached computations that are +computed approximately, to make sure they get recomputed if accuracy +requirements change. That is a technical detail not of interest to most users. +Its dependencies are handled similarly to time dependencies. + +Each value source has a unique DependencyTicket that is used to locate its +DependencyTracker in the Context. Further granularity is provided for individual +value sources from the above categories. For example, configuration variables q +and velocity variables v have separate tickets. The ticket is used to designate +that source as a prerequisite for a cache entry. Tickets are assigned during +System construction, whenever a component is specified that can serve as a value +source. Each value source’s DependencyTracker maintains a list of its dependents +(called “subscribers”) so that it can perform invalidation at run time, and is +registered with its prerequisites so that it can be properly invalidated. + +@anchor cache_design_output_ports +

Output ports

+ +An Output Port for a System is a “window” onto one of the value +sources within that System's Context; that is the only way in which internal +values of a System are exported to other Systems in a Diagram. That value source +may be + - a cache entry allocated specifically for the output port, or + - a pre-existing source like a state subgroup or a cached value that has other + uses, or + - an output port of a contained subsystem that has been exported. + +An output port is a subscriber to its source, and a prerequisite to the +downstream input ports that it drives. + +Every output port has an associated dependency ticket and tracker. If there is +a cache entry associated with the output port, it has its own ticket and tracker +to which the output port’s tracker subscribes. A Diagram output port that is +exported from a contained subsystem still has its own tracker and subscribes +to the source output port’s tracker. + +@anchor cache_design_input_ports +

Input ports

+ +The value source for a subsystem input port is either + - an output port of a peer subsystem, or + - an input port of its parent Diagram, or + - a locally-stored value. + +When an input port’s value comes from a locally-stored value, we call it a +fixed input port. Note that the fixed value is stored as a source value in the +Context. There is no corresponding entry in the System since providing values +for input ports is done exclusively via the Context (see Figure 1 above). + +Fixed input ports are treated identically to Parameters -- they may have +numerical or abstract value types; they may be changed with downstream +invalidation handled automatically; and their values do not change during a +time-advancing simulation. The values for fixed input ports are represented by +FreestandingInputPortValue objects, which have their own ticket and tracker to +which the corresponding input port subscribes. + +Every input port has an associated dependency ticket and tracker. The tracker +is automatically subscribed to the input port’s source’s tracker. + +@anchor cache_design_known_computations +

Known computations

+ +Certain computations are defined by the System framework so are automatically +assigned cache entries in the Context. Currently those are: + - Leaf output ports + - Time derivatives + - Discrete state updates + - Power and energy (scalars) + +Output ports that have their own Calc() methods are also automatically assigned +cache entries. Currently that applies to every leaf output port, but not to +diagram output ports. + +@anchor cache_design_declaring +

Declaring a cache entry

+ +The API for declaring a cache entry is similar to the existing output port API. +A cache entry is defined by an Allocator() method returning an AbstractValue, +and a Calculator() method that takes a const (sub)Context and an AbstractValue +object of the type returned by the Allocator, and computes the correct value +given the Context, and a list of prerequisites for the Calculator() function, +which are DependencyTickets for value sources in the same subcontext. + +In the absence of explicit prerequisites, a cache entry is implicitly presumed +to be dependent on all possible values sources, so will be invalidated whenever +accuracy, time, any input port, parameter, or state variable changes value. No +implicit dependency on other cache entries is assumed. + +A typical declaration looks like this (in the constructor for a LeafSystem): +@code{.cpp} + const CacheEntry& pe_cache_entry = + DeclareCacheEntry("potential energy", 0.0, + &MySystem::CalcPotentialEnergy, + {all_parameters_ticket(), configuration_ticket()}); +@endcode + +That is a templatized “sugar” method where the allocator has been specified to +simply copy the given default value. The usual variants are available, as for +output port declarations. The new CacheEntry object’s CacheIndex and +DependencyTicket can be obtained from the entry if needed. The signature of the +most-general method is: +@code{.cpp} + const CacheEntry& DeclareCacheEntry( + std::string description, + CacheEntry::AllocCallback alloc_function, + CacheEntry::CalcCallback calc_function, + std::vector calc_prerequisites); +@endcode + +If no dependencies are listed the default is `{all_sources_ticket()}`. A cache +entry that is truly independent of all sources must explicitly say so by +specifying `{nothing_ticket()}`. + +@anchor cache_design_implementation +

Implementation

+ +The logical cache entry object is split into two pieces to reflect the const +and mutable aspects. Given a CacheIndex, either piece may be obtained very +efficiently. The `const` part is a CacheEntry owned by the System, and consists +of: + - The Allocator() and Calculator() methods, + - a list of the Calculator’s prerequisites, and + - the assigned dependency ticket for this cache entry. + +The mutable part is a CacheEntryValue and associated DependencyTracker, both +owned by the Context. The CacheEntryValue is designed to optimize access +efficiency. It contains: + - an AbstractValue (obtained by invoking the Allocator()), and + - a flag indicating whether the value is up to date with respect to its + prerequisites, and + - a serial number that is incremented whenever the contained value changes, and + - the index to its DependencyTracker (i.e. the ticket). + +The “up to date” boolean is set (validated) when a new cache entry value is +assigned; it is reset (invalidated) by the associated DependencyTracker as a +result of a prerequisite change. For debugging and performance analysis, there +is also a flag indicating whether caching has been disabled for this entry, in +which case the value is considered in need of recomputation every time it +is accessed. + +The Eval() method for a CacheEntry operates as follows: + 1. The CacheEntryValue is obtained using an array index into the Context’s + cache. + 2. If the value is not up to date (or caching is disabled), call the + Calculator() method to bring it up to date, and set the “up to date” + boolean to true. + 3. Return a reference to the cache entry’s AbstractValue. + +The DependencyTracker is designed to optimize invalidation efficiency. +It contains: + - Pointers to the prerequisite DependencyTrackers to which it has subscribed, + - pointers to downstream subscribers that have registered as dependents of + this cache entry, + - a pointer back to the CacheEntryValue for efficient invalidation, and + - bookkeeping and statistics-gathering members. + +A prerequisite change to a particular value source works like this: + 1. A “set” method or method providing mutable access to a source is invoked + on a Context. + 2. A unique “change event” number N is assigned. + 3. The ticket associated with the source is used to find its DependencyTracker + (just an array index operation). + 4. The tracker’s notification method is invoked, providing the change event + number N. + 5. The tracker records N, and then notifies all trackers on its “subscribers” + list that change event N is occurring. + 6. A notified tracker first checks its recorded change event number. If it is + already set to N then no further action is taken. Otherwise, it records N, + invalidates the associated cache entry (if any) and recursively notifies all + trackers on its “subscribers” list. + +For efficiency, changes to multiple value sources can be grouped into the same +change event. Also, DependencyTracker subscriber lists and cache entry +references are just direct pointers (internal to the overall diagram Context) +so no lookups are required. This is very fast but makes cloning a Context more +difficult because the pointers must be fixed up to point to corresponding +entities in the clone. + +@anchor cache_design_loose_ends +

Notes & Loose Ends

+ +

No cross pointers

+It is important to emphasize that a Context never contains pointers to a +particular System. Thus the allocator and calculator functors for a cache entry +reside in the (sub)System, not in the (sub)Context. That implies that you +cannot evaluate a cache entry without both a System and a Context. It also means +you can use the same Context with different Systems, and that a Context could +be serialized then deserialized to use later, provided a compatible System +is available. + +

System vs Context

+ +Drake’s System framework provides a careful separation between the persistent +structure of a System and its ephemeral values (Context). The structure is +represented in const objects derived from SystemBase, while the values are +represented in mutable objects derived from ContextBase. Generally that means +that what are logically single objects are split into two separate classes. +Since users are intended to interact primarily with System objects, we use +the logical names for the System half of the object, and more obscure names +for the Context half that stores the runtime values. Here are some examples: + + Logical object | System-side class | Context-side class +:------------------|:--------------------|:------------------ +System | System | Context +Output port | OutputPort | (cache entry) +Input port | InputPortDescriptor | FreestandingInputPortValue +Cache entry | CacheEntry | CacheEntryValue +Dependency tracker | (ticket only) | DependencyTracker + +(The names are as they exist now; some clearly need changing!) + +The System-side objects are used for declaring the computational structure of +a System; the Context-side objects are used to maintain correct current values. +For example, the necessary allocation and calculation methods for a cache entry +are stored in the System-side CacheEntry object in the System, while the result +of executing those methods is stored in the corresponding CacheEntryValue object +in the Context. + +

Output Port cache entries

+ +Each port and each cache entry is assigned a unique DependencyTracker object. +Output ports generally also have their own cache entries for storing their +values; that results in two DependencyTrackers -- the output port +DependencyTracker lists its cache entry’s DependencyTracker as a prerequisite. +(TODO: In the initial implementation there will always be a cache entry +assigned for leaf output ports, but this design is intended to allow output +ports to forward from other sources without making unnecessary copies -- an +output port DependencyTracker is still needed in those cases even though there +won’t be an associated cache entry.) + +

Declaring dependencies for built-in computations

+ +The built-in computations like time derivatives and discrete updates are +associated with bespoke “Calc” methods which need to have known dependency +lists. Rather than add these with yet more System methods, we should consider +changing the API to define them analogously to the OutputPort and CacheEntry +declarations which accept Allocator() and Calculator() functors and a +dependency list for the Calculator(). Currently they are just defaulting to +“depends on everything”. +*/ + +} // namespace systems +} // namespace drake diff --git a/systems/framework/images/system_context_cache.png b/systems/framework/images/system_context_cache.png new file mode 100644 index 0000000000000000000000000000000000000000..f02cc5426ed66fbda72443c38feb34adf5576ca8 GIT binary patch literal 58102 zcmc$FWmr{Fx9$cMke2R{l$35zQo6evHYMGmbW3+PNO!A7;mjGMR`e7Btj$z1cEB{QA`;Efdl_xej>txuRuHO`M?9bv5ce`y`>O;cs&9&*CXPw_Rd};d7;`kO%2eB^2#SD zNkyIXehHR#6P?|DQY$UgfgKxTj-P_RrKL;wFJA0x#n11SM<$BVGrW#lj)zdUhFUBX zKgPWX#1KKGm>gBQB!Pf~K#Vfflq>Kcg2;*V|N9{K_n)b$M3G!Yx&%HQqowWtycHkQ z?S(u%JfyN)@bU50YBH**sBm*Py+p>H^u>1{z!1TXqq-!46KNQln!4N^NQNlt9X~^> zs*m!OOCdR9mKP*{wi!CDd%A@Dp4w^TC#wv)&bLRB86BLQysi$kPOD-_1+^QXQ{5rx z?Ck71jW#WoV|iG#@`yn{Ws;fIOz=@xlPCf^F^YUjV#gZ?)zk*p@!ExP(MBF>kF6mqe~&8MnAKqzQ%MBItyI1&U3D z+4too^1m3nQi^Ye>~i_%Flh2z=2cYSknsXfNLyu3r{+$>;y4%VEu z23a{cGPv#a#V_|~KUAoP1O>V5jQ*INP3d0a;pcC%U+Hw(9;RlgGgaNh-79$k0jnJD zl>B#QXkno(TtnOYH>=5Sd;$UwFRv=Cy5Hc^3~ag{FFzhGG^vHjc7m_eVQQG7IewV$1^~aS~m;Jw(l9IiDv(q`Oly#-jxoo9L_-*Iw<1F4* zep>1D>AA1BTWWO=$;*4g!cwK22u@^sb2AZX_WO4Tq*>QaT%*7w#J=mX@xV-(E+H;i zi>=HN*+o?(&a}yHssGCO;_R&KZ(@>n!@%BBo12<(No8^bqr-ZCSwGXn<8QiaW0}rL zDycJf0mw%g83rvLvwmzcGP2r!w+ab+Epa^MZ#R_`fqOi%_ln(3#yv=o>c+-@h4qn& z4}2^vG@C>BvhDj5O9`rU3VI1@gWv5)w~^}$`O!)jIs_LN*9?87+6XR4#NpGYKZApe zE~^i>=P;`y!q2iYGAi_mySuv~Xe4=xbSpmBdZmJ0&re=(Z~=3578=gte6FkKw~$X; zg9N3jY4P!OPTTS-Dic|PUeG4!#7enF74EU$<(?X3XR=gfis7wC$7Zm`ZXz^BuhrQ~ zSvl@%JKOI`Yr*)Jd0jbacXF4mfjb4Y+QsitA0%6Mh@pu|-KVvmq9JHKJv~+&X>t6j zo0U@+OqAuW|1J|TsQ)$VvAma%3?ZM^=8c=vb(}&U95u(oc}_n+Khl_9!*`&j8Tg3* zT?JWL**U^Aa0TsT!WtSb@eKYV2zp*XAhfi_O8F8M`W<)k4c5?xR`>m}d`TF?(VP!G z)IRrDFa0Scqg6ZN;%0R0P!jd6`b-$^TIg9-BUoB6YLVj{wS3A;{TVbn(x}j32_O7kaLvQd=;33#JI5^3nVtKXMYJ5T+!&!nd`D6XQ zbV-855O@@P#j=JXQ+%^A%lf_M{%bZiwl4tzb2hFHk4)P2?w5Ost_&YAkohM2=bbUi zQq{YYHBkkH5e4b{`phHHK$wyT?3df;iWSI6LY}V{>>=-re!loZE`9XQdYTdP$=NwF zJUs3rRbO8p=&Yw_XM5nfmre|238oKh4rlT=HVRsroh@VOGJ9DwkgT2teo#MrAmHs)Mtm{#+uB zV({b`%Qw*}&GO$D0W(M>SGlz7;cNh`7Y>rRu5N~_cW{2b$*EA{azk3D8C8)FA}8Wa zQ#fH~eLaVcjt*VSm6m=>K6XQTJ&k-u$@)ngr==e%GBQJ(g0HXdKoZk{y0N{z0sSfq zL7KK*Mj=}kncu@}NPU&mY5XsR3Mx9fl1VhsuS)w9al6##H6*S_T1{?#Cd*v!OiHr^ zJ|u8UwCFU@YcHDZPgkgQS*v-p2;C>CJ^oKu$^V*`#^?Lsj*N^9A23!VmmV65cxaL6 z`V=8;G57T&Rh{`zZDZrsCm!(E6w7DQHz<_W$58b}lkg{1&NpQ4%uJ(s%9Bj*gBZA|i~8 zj3_85-rnB6_lMlEA5{|>m^g1giVJH{2eWv$(97LB8zA1kwT}#6)~1rWI+&~Tdw!x- z$hx?^G^?8n4h~*wb>VkC&@8odS!p2bFau|Zn9GNzvHQM+m5|SQ2aE%-&0>go^NuwY z)z4We^2xgfxa1`(7v``>ntaAA%R3=NQrQEe6c zfEu0+C{Cqr7T?R)|DLz~13VAlms|-Lo%?^_CXn!@?EmhdwG-~WOHVwl!r%4vOOlrW z8=1nuzzC2ktSIIDMg}Kyj7rQy$HXx6vtN7VGkKt5H}?6-a(u{rg%%f+{ojxEW)d_CM?4L(bzgc$G}1*4TK5wlQIa7@n46 ziAat(Y6@rSrmQ)16?vmVx3EIMw31ZZ_X(=P?t>7eSOK-PS~*$k zrlh146%n~QTGAO4GU<(80!SkCHRYQ(Z=mdr!D6~paniq}gJ9{fNev9YZ;qexbBLhA zipzyQdYM1om3P9G!ea;Jr8c?spPKYgUmVz|dkZjdO>g{kXv^O2(`muGB1s_Q2wz3M zz^4^yD3Z1!3lb@HCX1tLXxnx8@gs_eTS7wO?*2X`BqW)U?499vFdV@zmx#oNfvl~q z5fBiBppzXfwww>9vR`7`bWpsGAX{#`r8zQF7O?^E{R_X|#Qq8GWq-HImMR-Z6%j6` z92O{&tEs)f!)d*{n3rfWk(9NZNt}_k$L|`bm@hGI`7&QJnNdkcvxJF}O0N|H5ijLv z_L|vQ+9FADJeHMTS2z_8t+nMSE@W+_UD@7vxO7G141xF8lvS#^qQ;l zqNw8Eo}S0w+_H{OM0Nkvedw4-{??C9sFn`%Ms=Vp;VKdOy}U$CA?DB*R zee1};P#(7_B_%aCH>a+yE|Z*Y(lC!2z`S2A)EY)6Cm2 zM%Y&ie>8t#$}d0Z;X(yq#MYJrgMSTEWwn8ba$IP+V7(s zthnu}{`!^s)MrmG_4l>NN0yJc3{FE(`pm!Y{8~FrK9opIy3fyi)2h3_J{~Zw4MN6) z{T_gT4p>nklUB7M4CG*;scPe2n@2%McXM)T^EfY3rI(YI1|8kf&hBV$s@x@G6X00L zC};)Bo3CHL&g6H4_h-x`$Tr;yd% zU9B?R3Uy)MhZK=qcQ3DGLFqQXLWQqH0H6n>5EvU7y~f2=e|XUz3^-^krQ{Q7tI?18 zud{dx&b}WmrEO9g!n#FFvQCnel!XR3CZ~0b&++Y=cv*{w<8gA!&8WbC(Cq* z%2B&&4U&&ACsM-N+$=hx^S8Hl_`lxSZv2V0v$OMiy0y^P*B{ChFI4XEyzKAp&fk(B z`#QP&f?1ZPZX6Rr2ZsLjryvoH@EpPLfz2fPF1KS+$j!}7f>%OrE(K&`dwaj6#Rv;2 zh=|K3G%~Va%(B5_+4u1-Q^4g;cH?u-7?$;UJ0hn!c48b2u1bWMYGNFHqM|8Vneq?c zOA)K>_lbRJ(S!wDoGgea@ILZ=iWZ}m;af~|_J4b(_~Ns*D{v!vUcP*(CM?FB3<^+z z5BUSeo7f#JQMiAgPxQ!?EmfC!rbbjk;x{0`xQv?b0O$meXBdFw)$gCDW*v0*0e)c8 zX=LQiFA3is$@cR=VP8}BrOtZe?IRPg`&hmW-t zC!rGRe3X=#v6xh4Qa z5*A|0r`x@*=s{-~*N=N`Gp^Fx8p=}t6%#17Y*87-}pk3V_PF*jlmg&U$L)st`?cF#4?(Q~eqT{hk<6_353EB{-WiGfL*{jL z0Er9^4Mi-h(a5H(uCI5meX7)LQFin%l`e$N)oy-ETa9Y^0vwza`Q&yp@M=dQc#}(IgA?Ruk*JF{P@|=ch+FI5+@u zo`P5mGiB~+8$!0Z?5Wk$Hqlqt7re}V35}}DNOOP&A{ia@8 z826^;BNZlYnwF;4+^n|dg0|)y54ZK)_)xyqz%joU^vX?}#nM9y2SePfxWh0f@9I;)l3`R>XYo)|J~l&F-7 z#S^kjmeX!@;fKfT6)rBWDK#Tq-Nn&dQ2tWq;GAA!b_0ahW|z!%1UXJ-e@ zbh%VEv*8S0b8~Y*@c~QnBA)j_ebHRwXI$W6P>nqL4A?M(5zByR~kj2Qz$hgM+SLkbiRNiWd;#g0=J_k(B zZl#k17k6uQ^=np^FW4oZ4Y4TP(`DDxONf^{C(yIB>8MFV3?FVzfiU^jOP?h7Xue@T z-M;e`3W|WkU$MS3TqbR7^vQiAil%pceLL zsn}J_Ni^`-6l@+(Ii0|r5OFoUNeNR+HLzSmg;W#M&O8t&j z(Gc_%Wh%_D%uGE%W98+CL2UzNQSa=Pj-%sQ&W8X1-%X&un#Bj9ljg0*EznbwDJ{y_J;-kV|oLl0msMqIF6E_cTAn}_OjAMdY=l}opWGa2aU z=x)c4!SVyt784PHh4{k2@6n}C6bT7wI9pgDf9&r1*gzC7kA)G8F5|?uzW#pbU+P;066DJ}xdUGm{B7 zVxiHtyyDl7AGxM~l`hjZF-P?^_iyrAsj(E44y6E&`r zv-81B)e6`!pxjl{jehm?WD2@5pE>f%`QRAU&Q{vaw>4UkaX`OF_ zr9R4i&oO!oiMBjcTAN`D+@mQI`}uhMd2FaWxT&%x9+S$pBcj!Hgh)_nZyLASt2`^^ zVBQSF|Rg-$@G!Y(bF_vdSl28n(XxE^1Xe0BCg+h*_>*A1^gn!T@TwGfjkcMk?rlNZ)r9rZ!O3vHG)Ym1dQ0o}i3c-1~EMDPD+wr;e z*T@!;7HnESOlVLENsSdhbV&s6A)0F=J^vMchDhNE_W>S_V!LqHMlncN&^O}#p&N3>KLDgo0==7HNe26xww3khxm-)A@=si zfEog2c6N3)nZ*E&h>MYvvtdzCSc++AR0kOd$F<;GQ)Ri#`V+z>L@Zyw_%{|)WgG35 z_`L3aKV1N6aAUksCY{?J4H@}9GCC`po`Pa+ze@6D%wv)LYVwuLRIT-24wvD-6Wm!3 z_2v`n2yS<`jZY?13aM7dWrdD`Wv|((=N_smhC;5OV_ihsRFJ?frKuj+Zn zxei&kCn&?EdtxG;@rmi0tO?z=()!EhgUNHC8wUrv(E085_I8~nJ1dt62orKRB zU%LAOF>!*{4nI zv*`{i;VO#MO}9u1Pdq|GK5!}kMieTiW@NNmP0*mDqkkc@Rt^F&7sH2n9rlZH4csn>aKR=J)e;Bjm(9oCxCXpLhqsa$Sd3kwI;Fw53NI`$6 z$~6Gfu$(GW^Y!iIT3B~%0YscWHD0Y_E8G~~ACPeZ+0u6NY9`oyp9FqKCVZLrFyq`ICV`;_Q#vHhBmCwi^Cfartvy$#Zk*}5JrJ<3VkYWL*$>eb`2G6ovZoie{)WBwkM3V4dt^NoQe!8KI?{ju>$pjAJ>0%%R17MTQ z%_*=^N8Aa7Enx8`$x-onN@l*YRtO59KXyEt{>lQrBpNc|-v0jmT!6P!oHyuPKrM(N z<)5DKb{b4b71C?fupey2+1fr_syW^H+~tXm&vzJ-Ejz>rJwGUM$O{Yn;#3@|AXaT+ zfOwC_jL#;r_@l`;YUCYn`v`R3)5=3{b)OCC>w66BD0mJ+a;Y3ee-W6d-#Z6dW259y zrPpdydEq^g^{Pl8{%E)K=`MtNz6kYc{o25T1Cd|-(nxC7%jkUm>3F_SW?#J`BQvw2g29R_Ub-+WEDYel>m%W2 zm;IvL*DF;TT}!li2PB!+v`=v_Xo^-?^jRH$HW{V+A@A)9dLJZweajx@o?X}aN+faA zGV(~5o3}Rm`nV^L*zkc;@lc@FXPCPWvd$5uV1YXK%YDHqp_Q($jYN z=~N|+tjX>iGx=P!>&&CT+} zo)Slb=f$@Zl=pKEEJ*O^3iBR{)7ruVI+THgiJ;y2_N}E{ziVx^hA^f^gaqOg7I+DX z-jF-AQygZ5U<6*IxZ*R7n`__C^3-b&|tDDc85*ia_ zkz;@biux}C06iQ#8tLfG7HI9V-!8JvWeADH=Qn;(m9buc3cLhLX7IJWhDlIEOxf3o zGsjXk0`XEHvX@#Qe6jv`;{L)Qhl@47t(ohl2c-n_)$?D1^?1qVXK}nMqKwu(bSW<~ zAA}$bk?dC!LX#y8R>u?Oy`Lu&HbT3aVi0#W#wtahbE>-brmD1CA5%#lh#zXeohUGLThc)=f}{fWc!I&<*4#Dw`%_ybHeMkL|7}$kOslPC89;&p*9jKA^u!F zg^M={d0`PPwNTH3NSNAgn+-1sOEJ0X3=F~Ztsx+G+@TdD8P11b%<}R)RXYzw-U>f^ ztzGO=-fKLsWE}axyQ1UoER`m4(M^{AQ3<9IN=t_ca3Kq%t|#OAR~Hi# z6G~B#h6V?$Ciec4w<-fIHVY_Jy4I;KiD^kdS9gKKe$(7~<=5b1 z+*Q}@dD+7L`KIIs`w{QQ`Gc6K-?Y(G_D?d) zwJx^e*SwZ*D3152pMuyWxbuJ=^HT_}&YW3sp7ZK6;e(7tW+%Z*!YE*>a1o+~n6ka< z^ndr)%H7+eE@^z4)De$c%bFV%q1TZUicTs(=eFCZyxieM8{e0<-_|SN5ct`OOa$jI zKECt*T(QoG5G3d8z+Z!oP_I*}tJ`=dT6BwoPKG}NoLroBwKWaZwNk$E?@XRBYd&&# zoqc3?ipWvIf`dn$aJ#w7wV7u088Bb&iyxA>QqXzQ`9R`D0=6{wUFF*0zJyz4ok4FH zzbg|8eoE_^o6Qu1#K;I1{7bX{GK5HR8PrQ`Y{a@lRDwjxG-a`nQeVAle?Cf)d>HyD zi+%5kbwxmTaV&ZA@-7-%t#Z-V9S$Dl2H4FADEQTmoBiLve+O;fpv93|P;d!A!(K8! zkW%nG%@WD|^mum}8yoxf?OQMo7i!I9fUt3@{ZAiTZ1-?7Ha7n7;q!ZPK;^s3)XD_} z1ZWhph)GG&Apa3yXqcGf+Wgejr-5s&kS&y%mIk%8?EvcX>3Xjy>gAgu#v10a2LcsOR5n4s4c%BxpnKFr>KA`}y=XNlbus*Fr% z9sx4V!ct1kTR-lxI2LxYT^_N%LqRiV)1oho8^wT!U(EP6c%v~Ak0n#px>OPh3#W2@ z*q~SQ)oruA(*)+@x9l({2%=)gW%R*~&$yY1eRxfga?WW8B_e8w zLjAE`dT@wSp)JMl%82r+@6JrTKeoH?S)%XZF_I<2a%rghYKDwpF7*x#cQ;SHeBdwy zkA!3im^u(1_Y=cxAs;X&S@c>}6&3%jC`d+S3VNxx=>S8$stVXFQ+7)Y)-ymW9LW|o zSeyrd=ASkKv{#iku|RWJ92$~K7Y+&vio$383!)=bQn<7_z!dxO;|K7xz;%afmQ~!Ch0z^bI znt03yNjC$vL*wYLO-XoBH1WxiZJb}%kEci2r3DY`WO|1z0cC+^g8+i@Q-WC*{aM-D zEtg+&+P?ELO67B>RsWHodOu}7_J;Z7euC;x&M7Q{_gr?gKT|^-#w;v%+5uLFk?eVmZqMdyib)g09&>gX6o`qz0Alv}33vxaNweNU&Z_BGXrRC*u!^F0>wtymi z48*t7X@L5`2JCV_1yT(fG0!KU)y%(8l6ez5J2$tvy`9|Ftm^98n4h1oN0UB;I_yPT(nNZ zke%g?P{dlBB1I|7!TQI-M0Mwlmby5|HEN%q+xtz=1?^sZJladUew1x9L)Is|E0$0Y z!@{e<#NuJecFO7nwG~5ht!%YV?sA>b()1P|pUV0$mzCq&7(}t7sImHwRy&DD|MPiR9Q&x>{{j>XW}!8@=NW@4eQvXZh-u5(WVT%Ksmmk8-LD?j8z6S>_= zlaO{><_6u~4%TCv@A0dj5vh+`Pp7{~EBLrJsXJZUFqm)qW7Sa08r^1GG&}qqyUa>s z6xr)BZ?Tn`*A|;{Iy?MtE@DB~xw*Yfq;L0o?u>~+hb*?aDcp-n0l?46&5hs>{G;QQ zuCB@Hzs21xEiHfmKpXhz=qv$`2J{=05eTs;NJ+u?0~6$5^kU=l|7aA5K0ZD$t3bs9 zvs>q%WA}eWh8lXjlD7F;Bi(lQ)90t-=Q3p{DS(2`3b#uudqeX~s86ksG`zHBtf3(^ z39-x0UMfYei!->Ga9~n{OpUotxg^gRD=n{Iq6WyHx~{oG&2~18XPeVui8$jV;=MO5 zocE!dR%BA+Vj^kPUzr(4pWZVt8g7_QoICE!X+>r*dg`N53d(hF(jTVL*&ZgkqOobj z{QNAsDLn6zpOHfRn* zwSg~fm$AIO%x$-b5hez96pKM8F_(=d6be;iR&;AH>jqiBy88NW(nHjV@mGmH@@n@> z&-JW^P2Ka7aVI<<93RYmZNe8CC@B9uI)f9HJec-ym_fL5`bOl59;^n2i1 z_p1$GEa&V@vw%~CVE&1Qf^UL1f4Xs8cm^e zA@Iyrkr7F=SBbC$ImXL_|j~0~-KPGRXJ2IbIf)k)mHV=N=v&fIimudmnpi;#fo%{>X#2zWWcorgMG2qEFvI46bju-WY7R$3u%fz+>Q-5g+nfHi~s)xgNeOp%r@^EJ?LTwPr`2<3?W<*b0+QvYd9 z#NOVXCjJ%}D+{e*QLK!xn?8 z+|b05z0B{o75~^Sv-0UUFn)BzPSVOePcb#D?&>9=L;&cN|fc-$$16wKQkA zWru!OeUaLK_>Zh?5PzP_$esatBh1{PRfQE?P3 z^*@DDD0>Ce)0Zz_K%)Z5XkRoKAar~PfOk1wUK$-8?d{z@+Z+HhpSsu%h!&B+K&z=a z1uvH^;DL#OvAVfgXsfFWY89C4s3<6{u0GEs-TwQTQ1p1YOjK!ubF( zo$GU3TV=CR5vouKwGIdx zJVP9&;KJ#{JpL()r9W0P$ovaZ=J$Q2V6pEaM~Q;?XQeM0uWFQb1&4cHjk#GOLj39D zlRWK~5otfY#k-$lpU6(uTXLxWsH&;w@>FwHye5j^b2+7EDA zY9MHKz&t0y$FH6{c!fnHo53^IQ|Kw-mdIi98^z7-`d=h^-nx?c#Rm@iWp7Y?JyPkN z-ov8HXe8vUr;j5#O(N%8CtbQ%g52Eh*GG>dGnF4kvQ8d&H8s7ZGD?at2ptFQUlr+2 z{j9p>e7s2%`Ak8Qg0@wkeN|L(`@6Hn_Lf9rpAoe*vn5!u>|?1297MTALNpFS9vd&h zy7u>bv&oI^(lV_h2F0L_q zXGDliGSj=LS(%fYz}Lr*&o5~V1EZ>P)=m?6=O%umaQ#(VfF-rE$7(Vw1QKcqJ%4gW zSw0sq_4m>uq^U5K=&++CD3b?(yazIe-@bhVU;+iD?AH1j)bhU^ZHJ3`)qe>tdxF~m zBQp0WsB2F?mUEcCPXt`HUj)6Ri+!%Hs{MrBy}BdtPA+Fo?l^505g9bp8+_w_QIIrP zOq9x|%GS`Qs9EbB&GV0KU0PHuW_LPeVZ~Q&NBcriwKT;LsRI@Bn6a@$SSb=P-@H8kkp7$! z>Or|lpx9A@JABt6;KQ!dm6ep9EofrbCt|g56$BTeZKs>wXj9P%E86obv$bZ0iYl0F z3y|8UUagjV+P9f$1cIHV$ITZpSxLX0?R;s@L^!ACW{a$SPyYsmgdk*(kO9>e7Vs{sv=L_Q5;Pf)Oy8KS*0@J>)eY&`O-@XSV`0`_n<@=2Ks%DqzK@`Y) zI>rvszAYmI1BgGjz0kTqiG+9ETdAT^ls!89ZTqaa3Y-0pb+|jZeYRq>zYS)}DGpWW zI7L83PzTOS@V}*q!w`8DORKyMmI7%iNIdX$vUz`g`neU2Kr2D^@_3dB`0UJTOMq{s zdOit{V6{7&b#_r?G?b>a5|NU+2l@ot@crU4*RNl1P2U!p#BCnuag$xjFg@ez{&8S+ z&TrTxU_7acUC8y%&h*p``^=}iZsUtZ-R0d0hi$uE5EdF*TofQDe-9F_G2Tim`AA#X z523DVe5nUA!})$X!=@d5U-$XL_b61LrqXg7Ll1tW*vh2|HF}p5&x2Dy3#WPBUJhYF zrB9Tod^*uJ$ek=vrGk_oer0$Srkk%A>sWZ@v44+He&k+{w$)mU?;jPo(U7_M+_;>sVtCh!lh}p!{TxMU+Ahxe zG^^{I7s$%w2Hos4X`*DcKvcah=38R=&r$*@`wKm#=a4*QEoSWuesfHalO@a|eC8kd z1A)AIJK>^J5$t&pBg;eK2pVPUtiH!dFg z!phW`N0mnR!k0wKaoSfY${#^h3@dW{ap*=c;JTb`$ zI|!EQmAJT}DmNkQUH$exe;~AO?~|vB_0+Y_Ef0!7}oYt#z>?r~*8H9sF6#PW42mZWHIW%5hxFop!tko8u8FM-e^-Sl# z3CW_>HFoom-lJ_S-|(fvEB+JO@C zexhh|a z%t{nzOBL&JT2-RP1ajK%wKb$-^DwVynI%DMc7Z^sR#d&{H%%A>wC@W|hE}hzXhllR zLM;aU17FNGp_i!QbD>OE2GwQ;iHLrtT079fh{%PfxsV|HMr{8)^(A@eo9d&WwG|@N z?sdyaj`y9Rj7dKHlTVsyZ76*G7QPak3oaT`t$_gYM5ne%gG0qTL;r%zCLk{g((t>{ zXJ#PvM!r=W{wTxV>=xs7i?qEa>R{1TV2e;-yRc6!5(rh{waShzYN$=`U-Ia~)T}M| zt1@mhfIC|&u0ah8^YG{v#F+y%wfQLASMbY-wI~A1#SG2q6!Znj3Dk3srY`0tALNB)ELPEWgVXN@V(9{GmEF>xT z0CiO{Ci*;F+*+#ozDXo)s+AcXqi)t9lo5PR5)@$F7KK3G7lyZ9{}tE!{+XoVyZO*~##?r^t)y1B zcu#l2rDo$#yHL5_bF!vCD#~!TxKhfSY#=jFpB$hqr&K;kOe)9~yNawTAoKGFwc8&} zEe39p(eM1J{rv-54Cc1fIwOOlUp7N-`L!vzF{0eb||=bKqHdRWpT%;Ax^%)<0mW|D`qk<;U8^S?5neSXh^9~f!Pq!mk~kRT)e z{YGD0TeMR#ai4Ry>wnbWuSq0LrBH5CNRTEYFrYmS9k%Mwjhui)T!_>l7(%!DDE7 zP;@?v)qOKz$x$F8N3Z(b2sYw_(KsEIzt{(QH7ZCo0X7oqEJ|97|Iy*kg~b>P+b?sZ zWf_0E&{_5bihMYG9T$#03}R^AUCib^!gN!$oTzf7#QFo+Nao7d&r zT7(dhAb0Q0M(k{fr4Pc{kNlbKA$M1Y((7-LnPk5*cefuaIw?g;YDPupuRT=WCtI&e zghsyb^Fu^`4$S}AO+g8VMixArX|y#6Ma-w5kaEu;=k;Y5<`O?TmM5DDnjY@+?*40%GUD2@2ME7xdj&vl%uR6c4?wVK;W$;wo zrH0t{k?|>;DTUyQB)knDyt%r6()+tNnkrF#U4f1`MTuQI-L|f#F`pa<%cKnnfveL0 zJVpBykQ573p%Wkr|LWCoGQ0#FJlfeww)484N};r19%X(LLI?evzFNC1HQI{Gih^Ry zzAaaE9c$x5Zmo?v%UJbns?)Q=%4ZG^j@W9oCazNpdwSdb#V+SMH}{H6_kf-fhJ0D* zj|Y2+p%gyWckdKeKIi-{CvECv)PYBO$*6tN$4ds$)alL>&r$pyj>CzeC0Ow0kE-jj zXpAjFQ~p7^-MebR{8mRHh^4Khj!r2M(2Yj*YHe+G9WFT7 z&=L{csa01J+=vI<{8862`_#Nj%3&(^?KT9X>}%z;5I6U1^J_Nt56mIfGdUI`=mg^hQtv$21l%E*uRc$$r37#R47?Q8>r zi{MV!)%EjSvHZ_F;NBxN5+Up}ckn&vrDTgCrI_U()-LK4M>Rj$)pY!6n|+obSW&)r zek4D%Yq*&xFUKs-w{iGmk@_Gc_;o zr%fcf#AE(*oTi=ljr(Va;T#*pKa-z=8!tiH{T$x)5SE2S&mmhF{oloso!PzeQLkc6Q%nMi}o3`Utw)3R{j<=K~K0k5C`lLh#1s)eRr8K~C= zfdK7jwEg<9i5$24;kMcY0fpd9i8_Y#vx6A}87?j?#QPE_B@GtB!yiz`E}Qjj|LcuA z1fno9zRAn`HQ|K-X>-Djuh%i@_ujCaq^py_KhK(70dz*dsR=x0$*o2#NS@oMH zV(r7|x)ZnJ>Btic8 z%NN5ZZ}*E5XX->?7L1nTW$?gc@{4{%bl!y};s4U$LLFji1_PO$OJtrgcVAvs1mOYO z=l$85i{bh11~pF7Tv|4D@q)QDR@$pZ!|}JaBv#X~f{_&@V_~N=F0%Kd2rmrCFE0Qw zuBj%Ac6w08ZkPwo+1sw;NjLIV4sfqwgZ3lP)W3Xz1)a7~K3nMU_}Jb$;@oCNSS|CN z6gdR)wGRRb6oJqQ`l_dwSS=h1sk!rf;=v$zm79w}h=h%FafIsk_>zkYhcr*{b<-f` zvRU?tNKb{&yb=u%{Uv#Xn7_}?x$p2M7n7q^) zC~abinAn+LqGp53Z+W5!?&1D--jxPjmB$#P7G==+#Re-!1yMK4ynALZ)i4s*Lpr4GswV?^> z$su$o35Hh{MH>ng<=hxW;pGrToBGcoFJp#4H8bWFkxiTNYdI>$wn+T#U6a!snA$5; zQz46mFs$?K!8ZgbzVd0L^Cu; z)4W6?B=7UbUlEUgRy+iY<@Ue>X(uxu>lD=OM2U^9HvADn5>4XORSKxqS1N3r#NpW+ zZ$w0Ye@F@+i1V%fM%7Ey)E1FPD=74t3_*t&8$%$Bj6w;EEpRVj4j14e)LP@}>#x<+ z%2H9=&NBC2A+W3Ws!N!7rM2=C9<+Y-xo+|GO#KhBOOfgI-9kuDQZ7^~`A|eY`66^P zFc?z$MX~Hd?&Ke}v^S*^Z)n*Z)m}M9X_hg^S^Z*@i;ct>Kx`naC!mOt;p8evKil~o z?2JV!cOURJo90~s`)&&1G}q73kH7%^w57Sc6K2*#PeQy=S=m6QCO3*t@K#rMvQQ!w z2HfIvbhsd)k@t=Jf!=n4`TRa8d(7D4cKa^xsKeCGtaEpol_w4_cfOe6Fe21nzr$ZX zq|Ramy)hHqQ*PLg-Rw<}#DYLciHHOml>;m|_CMSH^tNY_xVFxy7cGSy!IA?4K}Szv z1);X@!)U*Zn?J2R+VOGNqm5*DgPR#9gi5g?tE(R@7p@*L)+=;i`uArMATeZJRb@&o zr#kz85lUeWU+%w|Fuyc%gg#0g-{QMixaId>PlWa5un`#k8WTD+Q+<4o^C7&DuqF~i zDh!r2mO&LSeCo9H3^ta}Kz1W6^yeGS5IL|+sf1%gFHsm&S_@ex##8L92LBIPUjbA{ zux)#A2n4s_4#9)FySr;}cXtae!Cito1a}J(+=IKjyZqt4`>KBZPjyWVQith1-P?Pu z8pfg-hr*vY3EZv`rxVxLVzTB7$JZ3PElReWe>cNkU;E$Q7O64Lcm7h;QPS9)D67Be z-AZb3RA*-ptYNury52%*D<7z|;>_>`Tc%XipC4EAKe-Gc12G(khyzl@Gs2 z{=KFz7>?1|e7E>yo$&1qcW)vT0F3}ipaL}n-ZrKeJGGREn^Sm7x$dcaM$a4ZT|Peh zTWafN=;g*FdIo83Qs*w;q`{TA#1}>vMzgys^pmLg(If40kM(1jv0byW8p1NhwPI67 zb95up^jZ|zK|UZerBE==pk#)DycjZ^`fs`dTve@;sVfW_dzvo#P>n){2CX&eTi<-9 zh28k9E^gVcU*q~<(WdU_$<4~@K7+sH$%2Rx5HS()hxtA0HSKdo7bBepeUvSzWfaR# z{Zg~v{JLgKG2Cf{ikW892j-fPh&yJI?kfN9z_xd*pOEcz5kj}EcRcIjiX7x$P9u}` z32nrKRyk%;fk+a_HT>DSw{a&fP|zo*~X3vA)6w}61h3e3r=l6ty_p~e``0y?u} zq2bS0;MTn%?$3h+K(@AwuCy-~5XPp-33nLi-H+D-`GW6ns-N2Ov^dw^%ApuCG)ten ze1)tQQ-3(N@7i}p%b zaHfG1XLjTD%w|j4jHjd6cY89JXoFeazcVkD>E2f+hh{qO%~>|XP^RUwDtuEX!o4nS z=fqaia#Rnb8VXf?Crh;8FQgg2;1x<0ol^nv7GNu-le^=|ckO;D02?NiNmP9fqq2bt zIZ^Mg%_I>vi-fwW$>M^51nkKogm$Ahx^bQ9#d#Ousb(2x&BESTu1ar#r0UV~KmRYd z)3FQ|-^@L{1yR1W(zF#3R*;)o`7TfruJjj}wJ{x+&!Vy5^iQW6T8v)UznOcskfc$N z%BTG-mYPss^&$4iD0ENB->5N}&c_(}8}T>Sh(sg&Ws`VsQ#-m!A0?AJ zt=7I}>5s}f%3<1Y53$RHZQI`jn-m5a?Z&EKt1=`=u;X~r94M+8wLWmr(S;=FPch(m z1BGS6Tt~UPBg7=&YSlj+w!3>aXJTaKez02k_XieATTwK77M8L;PfMj#o9eZoSl!Mj zK)}@D{FP3$T<{Y1I0>*6%QZoocuK!MH29N;vskOW;KTAnDPIsHt=c}O#1oN%-=&!v zU&;9G7kjO7V~rWN*6(x;CVX1W1;rZlc)1CU=262QHx@%MX5Q1WY@^8ovk-F5%p05K z6Y(fy{R8R~=9&}4_VSd9fAn7X9)dPSb=uCly}5O&mO2F}d0MKmGo2K&2B%u7ytqX& z0Z#jCv52Lg0{#Cc2vP*So1J&&=a)4?`=4v?$=UraxDOWapVS6@%e`6q`78Gq2^+e{ zhO){<-oZ01Q3RC+l@uCO0W4;La6zys6>&$(;psTtI^o|SWD4Gy_-k!?tQF4tQMsYf zlg!@}Wva+1o5RRH;;H;|^UZQ}Iq#!S-G*6E%JD;tU8c{ugY z`$YIwYDMJ-wO9?YSS@T4BKc$6De|y_gsX1I_rT6a4W*L3&Jc&1S^^tiNE8ZgS^U%7 zZ?&7(((o2DRNHUsPlozKekm~3Z@Kn8sPf`F9u-20FZ0@Yb_{b!_^NbBcgrp0H>>P+ zb_=Gl;nyne(E@NH^$>+Vuv==KuVB~H()L@BCktTL`+co0w;jieW}ElxhQsLYm!j&> z`&Nt_UZMwtIV)!WzMUo&54` zLHi5p;IE*Nsh(crl9hhzddH?~o3xOdQ}E|fnxZJ7HjS`BNO&XR(o_RO=P*W!64r6} z(2iGz7}Vj|cTGMQwTJ1DJv}{L*999|Ny+xG_-t;Yv$>|Nh+HCuo7KXt@)TQ}bvvt> z@w!dcM1SNdTB&E`Kb4uqKCE;j*Fwup><`HdUBL_;PeUZ~SL$){(VtRzXW#Fy2k^f> znQ)Jt_FWdRiL@c}P=k<^)bMuox%XH$^TwHMPt%VV8y- z{8Mavf^tb6kbM^Qj?sy73r}zkPgrSDDsVAIpZ)@Mmw23f>Sy63jVEZy#3q7`MjM5Q zIS}$m_yvm2SC8X88YBMt>vWgA)f-RnqP5}#zoG2G#$j%IAnR92*S!K#7MXZY!N+60 z$De&B%mfpU=VqPmao<;cl; zgU>Pn59+pLA%54zyy{s7bt?;el8 ze~T6T1b;@0T))s4b9JFHGyD;oHm>M)dhq)rSjma!F(>xcbT^XzxiyIyM zSK_F?w*sNpoJczI?{B5w?NO7-wbTuy9EpgLxqbNuVG>-l@rbeZ!J$T{P zX0;kVpXGYo%8PD4m5Ni87W`6c5*x!HBWd?ZEb*CLrR&ooO;9wS`0BcYKJ)b-od>yd zc=qQzQkqCuXoWl+FfRQ9lekPS-PT{fPd$h*ji9fvH$-3L zBz z*U~#!Lnb9o&A@iNsY=}6%7fsYuR^K5Gbot0vJ&-;^7Z7-z_p-{G{e_3+aG<(JRVDM z1s>36Apf9UCsE<*Y@`f+1l(oe>2}c!?qIYL3bZuxz~*I=ON^@EOb@b%xt2Ojop5W*H&~Cb+1q0cmqu5&KQ8HSo^gJIKM? z$M0pM@_S|Si3gyUNXmZx;z@YE5#Q_D$uci>l>266~~6^~%hs<;aWh+l8ct{sikpjr?hrks0o9mI)jIz6~4^3DyxB0)7@184|1!#_B`; zxSbUOwL3bQeHGgFb=L8Q4JvgG=jp`zv_$fWn1DkhlTwn4DLp2tGXXZfkbK(7%-BpG z`hA^ytG;~xE_^TBXRPBX-Rs8ih4uC%nM8yo2+>R!xk>1}xDtQlvhK}mM*m2INB8jP zP#qx%uA_7av=trgL6Z{`hJ-ffgS6#|4Q=_QH2G&O@f;FS)rJ8_RygY-M}7#ire;DC zs}^TM5*@YM%JB0#2E5sC2R3f2hHHdDX;BVA111zXc>8mqSO^@y5sec!q`jVhkm1GQ)UC(g9Sspgf03K;2?%U~QMU`4`KDFi4PH{< z4*qUW+B-p9>#;Y*op5OXJ6wxr*XWc)M@M@B1e-`Lw1u!y_NFdO_?=7novchF1GET=#)uLriyu^c)zwE zf*HXhHheNPawZ*ET3V%RqazYQbe4h%Y{vR{IKr4j+qZo5!J5n0**xFCswt7MVF+Q{ znWs;kZ@?xGb4o~fePiVnA-1S(1I{ge9)7zNr(mpEa#^uVBH*QdJ#{;r>Kg5MGWBkp znyxGsC$Z)dLsQ^$@O=Dp*zGWVZKeuy_lNIW5~bwpMQtm}Hmz=}@_#f4Vxf=i?aLBv zA*3CytJ}Yf{_*M2)w&DdWj%HMWdLosuNfGY-x<`@=p^M|SGbO!H9xz-^l6^8;Fqo1 z*4MZeT`5G<`6Z)r+>IJcSeKn*$E*)Ey(U?zckue1CW$v<3CgZ?9lSrzMb+vaD-_1Ei9Pha>#XUCYu{1hG%roS{r72 zt4gYG!pb{u`E$9^I^xn4nu`&IR_o1$voq}URbKr>rHGLC_=dkBZz4fdo>{xG0#^Sl z*c%Si<6O^{vU&tBs+cDVVP4*vCvE*(Q>sno;s6d^=2R~ZZRX-IaG4t3NtxCnRkk!3 zL5P|fr8Wuf3Pey!=AMyYv^dceUKLGEe(KGT+@BX-+9RqH`cGv8Ftqfn0 zfmCGoB-U_0SOO()h#2ZRO17FRIggmdgO49ruR(R{L}Qmaws3jg7RreL6wM68*QEQt z)-IcQBIy`Exy7>|V3xwBL|k-hsYCpxt!Az(^c4S@Oh;Q>7kS8RksQd>TCzf%fIJnT zVvLM|Pa6eW@GB_T(j!ur>j%+VEJ=uK=on^>!o%=!NOe1Xr>A6xnc1lF45w5HF1IAm zeo`sREk77WME6Gx3WpgCvwz@2%!z{4-1*4!V z&>G8O!X-NtwS%-RY@GI&-R|OBv%wJUFR)zMk`kL>Mm`yILW)l_u5SZMB^~Qfj!rM$#Z6s z)*r!%0`cxrDO+Qt!p>evMa_B@+0`^gh_y4`!-fzfkVK?Fey}L#x&Lea&&qHE;4l(O zvR19D|6;GZ-24?Y_3IC|WGIy){F+z<(Hf|aF)$Z5(;!5u5YJt| z-5*gbH^aOToK@MMGS3BRiQP#Nd6BD71d7qP@h*F^&fIM)p8EZ+0HbF5Cbq(1|RVL-@OBwY0*h8yr*__FZ6M!(3Ew&N?2SRim?r}?v8c-Z+4@@%sw z9?vMTwt1H&X?{R(#&qc#bj4e6JM`Zyo_%kKIv`*;#bh|dp)92mi6WAifs^=|Up4K+ zBbfSx$asq%cYg;HKYLvU(U!kIF@P<`{ylRuFKAo$|SX|L=EL-8$57lS!2@lgVi^bt|?Cm}*M*l9A3A5J{#Zf&2iO%Ip)rW?fHtVTpF z)K$CQP_KHJrpAtoIw7L|27QJIokUTrLckQ%0w;U`*-EyvokD;R9`sP21}X0sfg-{V zm+p7SZ$&VR+%eWS6#Chr$51DP>I)tF_%?l_404Ev1PqnWZ0A8n3bp%2M*_L{EB)HZ z%I~TA&t-UhHNN;Je|2~9fBh*@BMM{zhsH+oUOhcCl0MK#ovghm><^Zs;lB&r8rsa2 zS9AOnLhKQC*%Jy_m*0+dnMwl4vBH3iRu}8`8eTAVl=Hz&k#65#+a1Isd>7@Mp6z)` zKTpv^#(wG!N_0U0)3O+%-L%E3+qZQpw)>`pGF6>#eG7lYC<|XIAPsl1c52eGJYw)& z!8Gr6!DNP#AM*>Hj-%OPH+{sNBE3FdI1wG(4>X>^`PC}Z3aLt5W?)C7UbH_fk@x>x z2k&}Oxq89#5~*)-<9CYXD_VO6?EV-AW8ec9U<8ubWA9W&;Vys>lJtq{l*NsB)$^_O z@CHuX^yirAJ}FDHOz>B-)L?KFI7Gxz*EQ?G3fC%u!DCt-6@MaXiG^=!Oil$Xf2u=_ zz2rn;;PZn*Vi7MHsN6E!EuX&IIsA z`{_dAK}!gBUs!d4)I^pV{sMmE3#%Meg#=h-xNfD*vJ;c$H4kav1JTEmy4fC3 zj3N*A_@gL*sC4Qh4g#)m65{xdALkeQ>49`B~VNBFCntLQy4qR zOOFzI&cTYR_DL#EP9XK9Avh`$c=kit@#T=Sm(AFzMDmi!8A=NQR1Y^^#D(b%+0v_0|*grBBxvYyXdmh96-itwIJe z3SWpbnRT2llOO+~Z~u5DUtN|8y#bEdOW4;xLkPaX48m5mPo%oybb0A3^9Uv17OZXe<2LLT*MH3jd{)6fdQL&97V#pXC%llmyCXFT8kvwSu zmz6wg0oRd?7Oq*Cu3Gvf3Ak{kMNvwqQNRQm?0(87f)s^Xf}1Hh84b>p+^gbkNH9~# z4DHf5kI#VLG-sJzRan|rqdK+9+WGF?3vmR8R5EjY3Nvw(qn(67NP)AeK35%-NHmEA zIqhkP-P7h)U;Ty`&=b>baIjWh1P#RV2lc^KnIF%9$ezZSOdJKh!O?5pt%2F3br43S zR*0$JK_IRqcX-1Jg$S6l| zw}W-!E6)q1Ue(?1!DxmU9OA*1KL26jcAd1j;`T7GsJ(5yH)f2$<@COt^5tnal$&cH z^)I}Y5wYUD2y2nz4RYq-gN#fZ)hI$QhSG7u81$puPeorRr$XS9u@eaQBzr1bVi9UI zj49>SY<|?z+Ony)VX#GaQ5Hy)IFK;|Bd*J5`9>XH!1nU zmnTlGAD6;Ix7dF3c>XS{tWX)+?>$hpeB}$YEz5V{ky7U(rfzuEv7wtZ&RU}P9fI;a z)lHcPeuFMX(wJYxV=a^cmVob0z)XUH}fbCkwy>!t2xghh;C=J_|!AP%P(P zX4ZyPM!pBERAI|0Hl2EVsS6~#KQ4AOx^2bAch60J4|O_rVuTSAks*d0EJreW1>^_I zO_H31BfUkkjNc(4p}|E36_BIw;P_ua(b{wohV(D)t`_^BgT(+J?s;NIlt+%FSDf-I zFse9&;)6kFzS-_nGxTBO0 zRD$bzh3j*rHb=~dc#RDGFWivpaWZHm- zha;r^!M?425GaG4eR+nQqPI*{K?9Zz7mpJhi65cAe{8;G`6I@dOly}wgOlKCiF|qA z3-P=Ao~R_%{^s)LR9@%HQ$#nH|5yLdJrx;Fgh-^?PiQV59FV~Z$9}Au4!=_o82;K9 z$Gykl_p*pE|E@RiNGK(xTv=SIuL01idKDc`O~Tq-|Cz!VxeTehqjC3kAM3uYR*Vjd zHJ1)1z6iNnGNs$k=dW7h$66g$J? zj-eFYfKemBS3jMkvlG}gN!T^Xj`{V)Kn>UM8`zI;zHb#m{lgd)I^*yP;T-yE8n7T_ zwC$Ivh~qI1BDWX!T}n6L%t;RnqXWswIueZXH!oZ84+A`a!Ae)QJ*aUEe?xcs_mMe=NCM%8ca=vg@6T z{AcY>Ayjl2dR_D~(#o$b3@2ItPHJs{*(J$l_m~5{rIbqgf`V~LP-$s#aT_~RQBlKe zRk=!F0=L%(-VsQC*bgMm6bxII=;61gM5oJwQp|#W`s_K)-&dSVWWJKdb4fgtNucHT zLk9SQ^HP{?i|utfes9;XKAs-~&crPSLaB)TI9j$_Mmo`A^H7@_sjmI~95W|`6GJI^ zI+~_hU4s)XnqPYZ9M}XLvWd)}gfFZYd1t$51Yfh8x>2U8k5_Sl9;|!YeT(&M zUJwWX_30XCoV=Mshk^oa6i^w|IiF#gONVObP4WG)p+Y?dN+jl5LorR;nEa?@=mz>H zbZHm!mF%74%Pu+@n&$V;{*^yTDT?kUpt&fD+!udWthR+bZMpV6OTF`*HZEu}1>`zU z{Yod;k({3H@|)rwsk=KRg`+TZ@A9{s_w-)nRm%9n3<9nBt^YMXuGFq^BnNd@Xu2_| zWeV#2tOfJ$-zu1hNYlx$jVWGhfCuH8$)^BM_+hK+8<8CN@q%1Ywibn1t*}IFJn3BR zkwi@cUZbn8DLS$`zf1Q_P&xx=r4d!ZZYX3F6oCIOt@rLWT37edWPRbxQ3;hll}M|V z*kRPRepcxa8$;==B`+R43+!iX7T9{NgJmqBxx1amo`Z-13Apop6(|k@y481v+D>Wa zsl6YF7p&vrw~UF9LEzvZ(0nDtUFXv#aI{PF%fao%>;1*uG6)pQzh<4)hX0|ufE1V6 z*TO&D^jGRdkbn|CF=G(DHK*xFvU{G`d#SZs(WnBb;YTy!VtJwG-u`BLA=<7516q5~@XFZz-RI)IdTmpj5Nx8a z)5m~GRiCNZo6~v`TvHPn9a5eLth!@9^MAw}u@5E=Q4w~pC=ph0Hw}!4E=&vje@-V& zSJqzghs^M&T+Fy%1Vo_~6$Mp{a~XLAkdNfUsS@E@A@H%(TQ8Don%-Tm2m7*P3SGYX zZbx5m#y;DvDR4bo$PWf`2qRxC7y-kjiA++X4tl<##cTJiA%hyj`#So1IfXu?P+E8H zzXn8ua0@MPHjt=cqi6hgM3>g7w`(nL`B1j11R-DX1YI8d@Rw3#_&coN2gSU%K!b*E zdC4alSU=9v_=IQzoE0xAtRfU=8&c7rNYyU`F}Lr|(Nw2pbb0xa*@43&MSUc?vn|!V zm(%_4pFH0Be{0JB0_)mS*ZPD5Bwoy;KYV9%8^W!~KSC9{!6m>r>UV@tRyd=gazan! zt}@jeiMB3XQ?xbyjHyOW4rGFIGUKEr{*%~iK?hQNAwZr_k~kzhq&c-$0yQP#tkm53 zDg98aDyzFTj~7GOi@Fa2^6X8i15GfM5RZu8Wz@w&|K#LvyMO|pMXT)!y12j9r|d@} z?ZKE#6_U<$%fpqArOUx=Dj){U^GrLMd02+y6~_FrhqL#;CEgR$1p<{A9~f z`}Xmn1qs#qFP1?hf zhySDi&-dJ7d~IhEKDA|HYz)ttn{Sa?Q`7BIKFi$&1Cp!SIM=1^O(3I8Qf&)C3op;E z?U5{K&t-}>-Hov9QxE>D@vzlZus;D^?YX{^AM3hBS`sJ;cuK;m+w@^ zjmKX_d;O?ciaFt@>RSh54r^@Ua za>?+(2)!5YWKm}~Rk{<`2Z}_{pj%rNR%npFR9DvhlUEVnYer#H>^>ip#mW%C9KtSb zM{rY}FAmf3-pNt{H<*Ti1KHBTXio&Jh$w>3o$gO{b{|=(JUV<>rAT(u5WtbQ6IpE) zH8Dd2>IbU2-brBLPdNuO+Jtnl~m(N)j+RdkCI_VwWLtgKlsi=RbuA^6-I zQ{sl(1Fg9Hci7fTkTXR+^OaR$P(Z8}GJi)5J57W=0!{B%eDJ8wnOAZ)A8m4$w$nQ9 zlk$EvS6H`qg^m;E7$6wLgD*Hn%~zCR^}}tBdZ88q8tKR9TW$xlK4>Z`f1Bl8KR@@) zkiGD{Pbwngl^z~uIUA=SdzylIECqNS^ji>tX``&gb_$bV8y%mCjrSE;8+ zmZ(JE0z3f3W(%4036l;0#WszF{ojXEl5g%uZFh&$(@Ab|MJvPIH;vG3o5f`-uv;7p zC(n+7x-_xt`s~V0s|nuI^SPOZ3^JJ(tPMYz!R{{Xe}ZOV${9DZF*Z)0`1E6chAR|V(BgdtF|dmdB~p1`CRtor^;t0pKDu{-X$+XYy)g; z1OU?_IM`nUAqcu>cjUf)(_^6q?CAz94Z)a6LaNMHMNE{^l$lloWXU>YoP<@*r*+Hx zydj9qiTd&?NaSC`n92g@dQw6+6bKHqfEOsa-z~Obal3#c1nK*A6l)^{(d(YI+%K8| z%{q2nAzNEdzVb&ffDC@Hxe&oB;Z7yd5KnX=KdoGQR96nYo$qJ<=?B)kDbHQ$;SSfD z79ylY$5ma;s=eYeag~o!b#QIP_w%(Ohiy%rRM(9}I>e-?8_)3Y8cPhIM}7=iJf^!3 zX+43A_mONg3*KxZa1p_jIg=9tupaX3O?q2-p$2o*lZVt+!?VVH(AeL=5%K)591XCC zCPH=F431`(Y!{o5G%$fUIb`ds|HE7HeS17h;`IW7OvhUH^+LdbU;PO1dA9V5`0g1fq&!)5<5k%@8DEBymZxEjB!g7DjXVGrYy^)df*v^XDR z_BO4_Z{KU1e@HY0p0aCxozgk~Ah5-5`;Z!Vk6@*!(y6qmx1e9SeSDI+)!S_Fa-xO8T>~7$#rW z!o!9z&taMJVd}K=^4R)8#mrsB47MC>Allzt#A&_QFR_(hwXXdu*1aEt;B{_h34%HC zz|Om>Gx$1=M1NJtxWBeu3}YQ>hW92|XLH+Q==9>z3-DH1>q6f(`re_Nb#)&9NeaN&cxy91I$&AuBEvtjOy zSDq&0vV(FxKOdD)0sfv`c5t0po;)FNWMOC|D3l=ZIg&_i#r@iJwU!m~!-uOp6^?^8 z+#Rkl%Xa8Pn<9z&-O@?IA2W!E-Tik5h_r?=WCY?^^YYIE4CjlnmX_QcS35r!oiyGK zPt4P_aZ0C^0L%9I=P=3$oXMY6%!1Ufa^=a_kJWl(!x(eVSGL#_5QE?GVDQ}bKVxlk z7^DKbbBhA4>L1|j0r+V~npV#uKveVyzs_=tztO6K>yTGQSn~P=1}Z34f4e>J0^7*G zK?G6A28u>*th9L1&r<^M#KgiI#W^1;>qgOi1583HFs}6|3KIhi6dbDF#b*Q!^lG%Z z3Fq*Qo4M1dtY}P0UiUmCyWfTVi|~2bdSuN~QL19fHoVm`CwV_+r>_3lPg=#acp7fs?km5; zg*_6#TiCo9wOqmTwWgo4<`Y!8O08oEDb*hoyrZ``%*I4zE@(u&)cAPRdEhqv;BDH| zl{-vCAbtZ_m2qS>v>KH}Wxq*>O(AL@Y%|i2t~ooZ=6bIu`eN|i&hfd>JKu1Gtlk8F zQQ33&8|E(AIg^tE2L>)L?N4!l(+%vl0*X_-$M4iNv~gsxkODIez6?0f(RVEvWy_&N zdPDBspE*wGXiw|RG8x|<`N~Ires-Ta|5+Viz2ItKGo}?b+C0ood zE14b)@i(aKMt82{y`C?T!%h5!&xan8_nLwXg%V!8Ym=GPU+ar{{T$`9+BC1thy$B@ zt8_v3Zn2`V1OSRu_b?EQ_>^E`fqG)Y20i&=)>5;&i3Z z4;=$EJX@x2b~qV{QjUG7=8#5bqOhiK1lpvh-15jvW7!_PGqo%b#|=KBIY?x$zaaEE zz8-R6z`DsFX0|q#JE>fSEO*u7DVuH6`E|PR-q+*)64ENF)Xu9wxbb{=dmheejOEvR zoK%(v_Nz9u)juJJZ|GP+n_`Xi7Z6A3rjo1xAl6ow?%!?Zv8Tg;fru% zhMzlEW|85S)lTv!{q>uiTFq|{E;2-}ey}_fIjx;uDnP1CW$NAdUzXUhtSHNJ=5cb| zyvhF;Bn^iG4X5ISP?1nTTX-%J?59AF>u&2GgM^{sXvYVxvs55|(MW%?^YKaDPK4_{ zK^X*C$Xqq`y%>xZQ(0=MrjwP!9pW^EfhaIgDqH1%@G|ueYH_yfKrf&|@{x}u((6xl z1?7`Ly`LJtyy`wz^7G=$Z9Ayh9J(beUXA8H`YEBfj2X6!Mz=G&F6sOlQ5z9@>-5MS zzr%Y%7E1sA=xF{AQG_HcBG6>SV7bwY;Ps6A;Y#{o5seHrP9j&GL8FU+&AOy*IhHSw z(}{=(^k=JWSKameM>rZW86+e)?B@^^(v2aMRNl5mU$JO#ywKNY!E%=?^Bg`9>%@g+ zgZ%~wS;Pc!$$K&xu}R1Z=+G>ec;@+f$XR~T@z4$B2VrwLl|p4kP5JG~La$&+eXmZQUJar`?L4Oka7ah!9kqU6y(3QdT5IL9Kdg zv;s0X(ZF8r{&bX=cX5z^vFcW1DtpVpiP-sqy0w*Sr@w#i`}B2^h3An>Yk7a!`YG{7~#vp&p0Sl->J@rC*$Z4@?-IEv_YC5UE7!FWo9p7;)HsFD81tG>)tncZbv#Z zfzx~iA=-@y!9womyhG!i`%mLpammmC{b?5sGn!fzWFGRGXZa$jP>da0cMYwJq4a zevKjoGD$e$%qdB}%!euCy35^a5FAk>CVq*`R5@CLyyclCY-QRSsH9=Ajuy7U=VuWSca4 zTmIg1YQ!r%D>G}MfC9bBDKC=gt&SU1nKbZFMAc*~F#irOVpa^#d)H2Bz@PeWckdK= zwN-s1kjVt>vpWXYJ&(M0$%S4vV!=1(Hhl{8&4?a5cdj@u4f6yEqSAd7e7p+|Ph}CD zo+d=><<8gat{8l6?H7GtIDOy4R*nD0lfVa!JkrGz#6E3}OglT$DW_L?IL07Bt!W&MYAUw9%I4ygnPQ@l2W5Dc1CH?-um*4Qc)&LyV+eGIE28c5 zd>qW(mj`SVH1peS9#KIZheLZYghSPeN=fou(=>K0*q*Xg{SnaF(9zO7nyPO*O2dws z_~mL?1`5eFbBX-U5OiR_C9=GqeeE12n}h_Ao{deSh=R#N00gegFE?m6hY;(2NVT9JdMjpv)LEqxT?iOR$P z-ry$Pu2J?jXJu)PM24d2=ygKKK~zXA-^+OGE5qG6qlfo;3NL5i>Xy4*U;`C^l`k4cW01e1V4r++5ym$o#RZbKT=_x!Famn9i~IN)rPJ z`$U&G<9YXrIV;tD>OZu?oVU%!V_Untl{XEQhm!Bui zb9$6Kscr5{t=jM^I;2O1h1+;haHn-eB&{sh>D%RX%-z7n34e#V3G)9u1GX~cB`w!= z!R|`3|9;HFmh_bSiSnX9*0*y4E^782NztRH>`~}$2ZR7L`p>0@S0lCrTwR-vdp~V$ zevA{m*vi&x*VWzQ@;kEy)78wRl&IHWn<-Vwy3s3VUJ0a_Fl9}d@JuC6?XWd=L*~;^9 z-gFiG^VBMIc}K%?)A88sn0#9(lS7+6&$%w% zk#BmvSdRWtu>`_tzk~- ze!_Ylo1IgN7`AIRQnMMSdN_?Ab%%`E`zK->EfF-IeW+!q2JOL%3;YwX$-#ue;CE9c zegGGNh{&B(iEu?GpdsPzUT|F;91D*%y6R%NRRw z(lkObqcuS2&#d)l#dIk8K3--cHNin1(E0<-y)As76SQ$EM&Ms}gmZ^i5EciG9DyuE z;7=n2r%99tON^cuOdYQfVyM@ySi8H8_hWrcRbCHXPoKPiqJlo9=?hfPE$`WA5EHzT zQDEDA7w_1@*J+wYDPwexO6s+B+Iw3k&n4)?WeV8svc7{pL~q= zEh;G^DZ5L=k=HTU+6a{pxWv2yp13$m>Nkcs7*r*Gp{@N!bB8=UkiBAG@~u50jTOC- zEO}IE;x|m@BGh@6Ek<+>XOF1bu(|!ZC+{IHpJLu0o6Ss72-cLaM-m~fn2>HHFGiDF zy?cY#MQBzk+v;9iF%)@q;|hwBK)uhFXRrt>Gv{&g>SG-N}2_t%6p;8>?va%XpLwc53Vh2V)A)L6tg8$OgQQ}E9gV#b{Fhk)E0vjq@c-_ zP164k!277}1P<)!tO2Wy?rJhYz(@3j4<9!t9y&UgZqQK@q)894;8XXUUH z1s+9Jqe>buIRvSNB~3~=e($eF;vG-!FGkueMNXfHS~x4F_bEsK^Q)Wa4xa$|-^8Mb z-J%WflSN4n^jpS4oo|osH%fT&909e!ANy(XxPe53RI84>_s&A@eN5KrwTjs^rOIU! zkIv`Jbp&tnp+kMCuM7X%3jo7|0TI|t2;>5acME+oSFO8zyFG1kUhk&3mx&k=upsME zLfz0pTu?$>Fdszo4o8mcg>qxiOGr&l396dA0h-ak^7)m>#&%$g$Y??g> zI;OVdR>|ZDnEm6yW=YsrdxsmU(zkZyj3?WBTYR6I8m$r`Xse{ z!G}b_xAkTwjywTmcAxA0r})OpSl>Gqy|edDZN%2=CS5)%*!$NIoE3Tg3c;RS5v{s^xo`-VlxMp!TC;IZ zpLfq2Xpd8;-}TS@EFmvBfaMo!XLo2|J&>Re)iUWAfC2a z@8^QcD}w82ep;+1APtMvfd32cGUhy+XVWq!aAXTHp9f!5xpsgg2fL;{@zQKRH5A7) zWfjfp*>*37+rebeD)!_idqxoXPuAo_$MeIPc@FP(uKWl76NN+!AeUy^?t)@Tdn%d^3l)h3m4$E}Xr& z!53-zp}fE4P?x)sAzc9iEDH+@3_m`Aw;kX!ZUvyKYmEl;ghK(!vL(w(9rzFC6#(2+ zG!hd4H01>Vc==8NAWd!na~j|o1z5FQ0KD_Z$49@H%RvB10${UAPfP^3b4l{2+$p18 zUS8DoeOLj6Yk-Xnpb7&>xSOvPfIp&s)6WCICXkYf4q2_{0d|%~fK>>k~izOiHYjB!r4J^=oV)Y(>kqy+ zASw3&@koTV1RGVsti}rJWmGM*x!@U?Wy- zd-&j|1Q>ToNVblvyFUI5z#Ip#qGRzml`0p3YBM6R)%|jU|1j7;;G)FZlI;fEMH%h# z^UQ}qglRLbDaF8%HBW{{3aWiS*}xx6nIpNqBh1Rj<>`Y5)qDNBE8NX$NqWCuD8O31 zi-O$>0UR)1d||7f%0jWe&z?tSdY+AY2j7fBWm1!PcFuk#R?ru_%leJmBI0!DOnC&( zw4JTv)*ANx0Os&9!M6d5gs%XGJOG4?!LHZhC`wdvI(uh%`u@0|!+l*KDk=)lQMP(N zG6||=H)v^Sd|>AT$h+X(z~nvNKyC z{<QfNO~ZZQP~CXLV_eZaDGp{+Y3!YApTjH* zLi)Ns4dHBq->5D@T-W+=fOsNC5BKQb#@Ab$!L`pSVu(jPBN&RR!p!q-zS#V{kFpcl z-HrBVqzMDjcs%YkFh=H>01UH?q$C_XJm9uV1m%$no^1o2k)!);POf$B+e7J&onjlt*5D}3E&w6WTuV)?UY_MJ2rxA+v_;p{l0fYQ*|qwT>exIt=C8T z8u;4U+KS)@z?yFgQ}oJ~oB%4{xqv>)BWpju5`bQBxyDdfMn=Zl+uPcjR&xG9i5g(} zMQAVHkpeKsyL?~E0F0YlwQ^===JHhlv)6rlAUfR>8YEN%NARq7RoAAgP@E|(v99pEhu4s121z`GX4*)7$LvEy+(RPEHR1$^Q0b zl^q~^12oqh{oelm^CRotjiG~xn^48~O0HD`N(?Yn z35SwU5J^DUE1YuLzKKPAC3qgWz^y?#o2bIeC>QG!d zR(|74MPbI~6|tAh{Uw!Lkrs!BxVdDuD&f>7jO z{P}1ibWbWUZ1Nmsjg5|0pTe;h4QTW(C%?jwGh?bwzS(jgOPVi1y2BUyYvD>Cf|S+K zkqn*Hwodl?-M090lb59TTDBCruOM#aADwz`ezT>>N?ziCAyWw#5$ETQOnE7Krbl4> zF$;*X?`hiEQcI(AO3jGH_%dHh(E5*i;9l$J?5H=(2=w<)R;QkAW3F-#LbN=k0$ zb3b)xuUgx*{QDIR|2OoeXb-;4!=rrvrfK={lT@vjr-{!vkxa5nJIO_v@Q;HD zAG(8G>}Y1loW7*kb(k4GSrawq&z*fTyS>gJL6GL{xKkH4kmI5Lf=#FCSV&>T+GW^% zS60%-3nb8-+jhg*nY+IcRJ^W+e)SgWua;kvPd1W3FRvre3P4ATOH13f<@@mA!@<_f zY3E%hd}CBkZWK@Ll103*{QMk7Rt`G7zFx@RNM1of^zv*MBrgA}i6*a0YEat!hUH~t zZvD|~+=F8&SnJhX8<^SV6rf@bfN4Wj3J-=WyK<%hTBYrECLmVNRx(HY%@ z+at%X07QsM{3~OuZlAS5HAP86Grv<|GdD31aOhl+!SMc$tb!jLy{?sq4&KI{fLchVXOqfm9KKB`g?K7lvZ9tijMPISH`8o zTKS##_~ICP(<`Q_uYO$Sq#f5}0miv?ag{a3dDeOFMoSvgTNB%e;gapeZtEz!i1N1I zvUsX$YN&OF-hKq=rO<~O3>v7z)o}2X)IR~N1E?MU{Q2{37w;98+YCLSCFRQo2n1th z_1vBfl3%yNw+3L8ri0C|hrcMy_rxa2=kN^J=LJKW{T_)m^4If?G;My~QmWf}K!#3i zD=u{XF>JT(kbRA>>@GV!Z+9<=DsQ(=?B#fQM*dw3UqI{G^cxeROc+=l&gp?R<6xlP zMaVvW48q+qrP>|TDzyG7Ox!KiOkp)q_*KW@@^odwp6zFu*#415V(vjkGXCH9Ogb`) zlCIn)J0tz$QpFUTOgu~aJt7gw+EW&I=;p5m=}JzIrY9Q<(BEQRlsip+4G1Uo-DIaz>rrC6t&o`%K zMOs2x@gg<;Zy>_tq@oV8!&AC&nT)_orX)ZenQlD1G9}P5ZoTcr_FP-WZuxl61mU#0D=4f zHDaVwhSTQnYFf-HY|#D^`Atrd`d_=>ykV`22EH`!&U` zTxIYr#YaA;Rt`B2XRVi-6EOaAESQpF%y!mcaD;z$j)oxM47s1zz@C@FUemyS|E;>8 zAir~Z<$@Kan@VC4d!IqH^ew~cxQ|+)>sJtn`CBX>ytHrfzW+e!P5+wECMx!Ifyawi zPiHn>ykw4MQZhw3Q{Md!R>>0Kn!vf)S}c4julY$Kf)QA@5fKFFyd=By8kBn<&eqkH|Ci7 zsp4@apFg?{=-!hRH`8J)<0YPWN*W_|e^BhfLWA{E$|luV{DQpGxV}z9+p`CK4-_^i zga;p;-+jORuYX+W;n0k3%veWFJcEz$UeS3KoAr}Z-#Oty&BFcq=G9%sj;n=VA7RTA zKVg)_C{R|tE!~YKm*5|c5y9ydmdr7E^HXhdWi*FBZ9u*+$4&MV_W;(gp%spHG<1X-b@GdIdUK$;wX@D^2%y`vyb za79H~${*K+B%J1ISw>kllC%4MVUF9!$K@*xO`^rWLS`z=hyR{K1GKf;mDJbPkq_sR z9ri7xhx6r7SBH*!@V&1SPx!`;x4kF``(u7maIl6CV_3hR)&8P_7<^NIzh-5dYUry3nT_>a z%O@sE54Yj@AQFVe+s^~3y1z3+rQaIy&Ec(l8GTSLCqwFxt)h^K*O`4cM0fZFT?pCw z9V6wtnThjSzU;Aow)flkC|~|1@PBT_;7ic&*lbJj{M$oc#F@gAzgbsO7< zvTs=A8J4u)zwJ$eXcjr8LHf3A-3#&UZ(p-&CFHN8L{zxf<|sAL;WcoCean9SP-2@; zoG9$IP3vL2f^%u6t)f6U$6?o%%x()&#P3$o$WQ)UDUmM_f# zT7w6|`&Y4|{?Fd>Z||_mCuGYVZ03S8PaCZ>*yAw0$-ZK;9-zHGrI4^0iN{jwa~W3@ z3H_#Z2VtRAz)XN;MR-6+pTxG-$>g!VOk_31dBx1=&hK(+?XIVy=tin*J)^p{@(g3E zOBN}5IZ+!S7AfLITo!LIV>C-fZjYLl%RN_qMwic#CmT61)Sz7h##BazHI%D2DTT7${z29bCl5KU* zf6owuY-qiTUn6(ZjH7$BO`;gRaPw9HYcIsTym&q=C6sOkOjIc%bV-q?zpuHT5=hX= z#TM+~^bF&4bADK2lfPNZ`6yH|;U)K4fgnbQxqGbl0FmmYvsH%oa;vE=pCW70w|RDF zE6li-rD1d2>pnq7d;#Aw=J)EP++xgeEqrB*#Gljmxn})POdJ)+nav(es1V4~v>Y_U zWlT{i9FAeyUzSgiDALM%gWVvfRnW_%@k?XK8P{o{Rt`Ot$cS8v!6u*YtNs!L@hR8O zt%mg5svFDhRpa!J60q*}{V+1jIP*&yAE+|9I@l~W{!*#PP9;EBD^+dh{+A<_CtZs| z6W=9{L1;YXCr`WghPdVna=J|qcoI6j4utlt{X(~BWayqvZO2;b(c{y!n(P>^GsA5Z zvoMWCcUR_^tVi01d{Vs2q;cyO)2_nkJ(GfWoEqQC&zhKz?Qn1H&dj8sP03EWd~q;a z9G)HRx<-HN-AAP&E!%{vNjD!6pj&@0!h3^RIrQn4y`GgeJ%R<_*W=*3yz8l=FGu03 zxa-B{3Pv?o5LK+kI@%4pWML_httYR9lIv%DQ;V-U-D-@U;o~)_#wSVHr2k`k!&zNK zD*TtKgIr)ix@?4@c4|~fXII=y;s-k~x`h%rx5aAXbBRbIJv2_rJq|stXyHm1yT|{`YdaeEW)Gsv>Z~yOP(G=wS2NCaE ztprXo2BVR+gQM5PSl}?G#g0wo@>*<}=+*3=>FM@6^l&06S{hnCH?t|xW(2Ana3Zi9 zo*k84rO1-=Gf>UYdf7y!{z1N^^Svv&{CeulGD`@OV%6 zO0ctVq#Ir6XcUo7^3QT;4`%#`G4ya|=jRDbEDd<>{Mk)=%v&U4wv^R9zsRB%yv_Cy zazKe*u#-ZQc#fC-6iNeLGq{X=e9*3{!-4IO$*xuvbZ?rTo`!~4BlmVc>6ABKoNNFv z+0oWEn#D?ydn|v|q%5)T$Y*295*hpoTJk=Hk z{Yrx?ol>N^GwE)fCjXT2v&t00>>FymOg)qgqIA42`uo+YJ<}blc5QdIrbLPhy=+R0 znhc-;kQFq0gVyrc`1oUMZ^nxClfd8lGv5MzzFs|5nVA7t4CWG?{KC@*@J=$PiM*Lv zURs)=WP$0>M8qry{M;~1h)8tXT`w=M<}CRrbiuRyx5TKiVZqIlnfRDD+Z|!>A%T6u zBFrQyz(cp8yA5lr9p~TO^lNJBknXU(|1}Pr4`1axt-bWYBju7+X|m53zmjYK%Ds8> zrZ2&QeXX6H9rTri_U5H%EscPdGi!hTne#B|tDQTb!_px_30==JnXEa%ysb%twc|A@<*9&Mb_TvFJ+2-zmX!RK> z1%=|dQU(z!7yytNG;)JBaj5Q|;IQi!Rr#Mm2R>?#Jrye}E9j{N@PSgQkdD3^U=VVV zR%68}4hztV4=`(8nq1Ao6dp@Wz>oHyLuepcK1Yw$>v$!do~ZH7i#Vqy#qpt%bLB|7 zfs4xXFIn3K6(&_h7~YH%f28XkZf|lCVhV(qv|PPn7J*vyUp0}uum-1WEDUnHniHb zFfcHnFJ5q9Aiw9Cv%Gw_47EoEwA_u3j%F+Z!HyXQ$CR# zLHy&F4Wxb2{zW!BIAT3$aMroMc$~iIuFqb|L1s9)_J(esRe2m39WO;Gd4dc6{{4G^ zus3JE@IH8uUsF@_qa1*2Xwj^vuYViYKP+qjfPIVi)s2AFInBB-(9nEqYxC<^+~1mU z`}Ph4fXPpsASN``)ronX(qP`89BLS3IikKI-tar4;yDE z>-?CH2QY6n?&(|@=2~A2IulounEt{2b~H|9G(MgMS)!b)FK9JJ|6POdjUQQ-3nMdG00n z_YA`HrqEh?=UlZc-CN^NxeHAWQK&TL6OlE{HlcuBRg^bOTaIhaH!;{(XDC zWXPbo>$|YH2(1!NXPoAWv`bNaeMgr?A%p`6(v2H8?$Xi<2@5}>jfjd;)7Gv$P3nA6 z20P_peF`*Ae!-al4JT(xp9j3!W<61zl#{H*ZEVkDR9WIHz)RmB)N)YTJmEMi-*{R6 z)hci*;+-ZEWy?A?c#OmzPMEB5geq3tZ$Le`;OhFwj%LP zFUL1KYum$zgYGrhOr9}U5#v{wweWH*{>uee`O$3Q(j@ zrG>_e7jzPxV`IEDH1n`BpWL^g9W;>WwdY=DI$AXBY;fUFEzc3|&fIYIoQ5-mK!Ee} zkIb@}^Q!Ax9dS(mk*?$9*|oJXSGf0~z1VIrT^!nMHrmcNdtX5VWm*bd*)<9^A?!LT zFmQIfi;CfZJkT<5!Jy%`7|Hwm`EzS)YiDOC+<10Qjz&$EwBH%@8-`|*(AYEgNg#~h z`Oh7PJkCHV2?Ade6KPe|VN_n2)%ee?E$c2WxJG?_J(E_kcG}TW*A1#L1`^)xnM~Qp z`iQ@$3{8p+X>L7%8v-Vs0#r{&NdFmv9D8KNn)F(_Yd~>qYMoA_iR+DqB+cnTUd(#_ zszYxY;$gW+ArH%*Qt}O~=2jDIgD}!j-Xn+9BI}M>is=S7r?wBoZU?_hsa%T_x?Uvm z*)6~kXTnwDrI3p}d~HM3P=0DYkLIiF?Y6Y^ZI176fwmWd+kd!^DT z-P*!t!EoOP7mN@R?3Z%HLPp?8jTKK3UKjCz~YkV)cH9fX!WLK zuzLJ+7F5&lj=O?V4IQ-}#J z9vtK|cW|H$RN>Wslu%6lXvBvzBG-QEA&;%7`lRaA3rB|YdPR-+vm>dU#T$H2c1cdJ z$w~T53!uvh${(r9@K#L>CcU|RFN}I6SA@-TuVVGBbV=s0IiK)2(?bC>TlJ|cpRLj0 z3X0aq$P_YyCNjz6>2p8_jyomX@5^JK`;Ck~G!*-~Mu!MWvHMh2}&sY&q zW5OGd7vxp`t?LFM$QE|$vFjqmlm;8zU1q~3Br{&AUZ*Y>hHDs3OOvWDj@>iAZ!$6j z^*cIQEN+dow{&$#E^W>)Z8HA(S@kZyT`}HdJ+pZy`E2L6kc+0c%acYSg<_VWrv9W5& zv3K3{3VL%-t`;fHa~@ku;NNQef@8Se9QVP6m9|)_H-^tHd5zoOr#LJ1<|L=z6N3WQ zSW9`6f=nU}@zSz%DGlw94``6D{d$=($G+#JRA+mTj}h3!9i&Ez(Wfkio5D1k-9t$i z60Yi!PuQW1q5c?Qu!9x6K(_E=Y| zIs@;Ld``*pa*quka9k+I5c=rd_p|Ia#r3ETlbvx~>#SK;wJa_35HfI+dCe;^L)!B+ zEtVM@Byv!`a-~7WFjEhAnSDsFRD6LaqitgO*Oteti}VLhPS?XfoXxad7&pqiHe7(4 zOm_ij%2W60xYq4}uEQw*W4V@3H)6;V6cZTKes$lhMLD%5jS@^zh1kT2L^)52N4hpt zIVp3w)~x75y0XhxxiO>KGryBEx6ko*nGEq)tj9I32a~;{Bakp{{={X5lv>BNct^*L z$s5o)cwRVPDy(SHz^X(X)>k^CNF)BZkaJ^`&wW zGO8=xH4w3BVjJ)NFlxZqc0!RVscVH3L#XQg0O9U9E!9+D2g$C^ldGwP?^h&pOUZJv z;Hutus3XAw%@5p~*%dp%|JPrSoOtnrT+!7>#gZJQgK zJl0Z}R?g0USL=cLpY0tKy7CJEGsyLM3tNUde(DS?{PPYjyaO;7>h(`Clz{a8pGzD7a0eei`v+LFp+@}? z1R4z1rUY3?h<_1~MDqdReHMr3-u}_s^gx zFi=M(6HO@HRrO@6l(2< z2X+qg`EM>QEiD;d5b+2z-Ne24=Q#h5A&H0cU4Q%U(9uwLKKt)P|FZ2Z~?5G3zzi){_dy)Ts zPMw!ge9LVn{j2Nez$ zUyZkq_)jDPaez|5VusXtL7Y4~1++oLF9IX)Yebk>l<7&vTFTb?GpoUWnicO*;VhP4 z-g<;}>FLeCOBfW2!%ylHI*!}=+z3~o-Guh~4USbl3E8jKpG+*vY#omOl zT7DzetJCl=R;ieiUTd3_o?>fDt59%{#Rr=#FI_)=JHt8`0W_WBdp#rBdLem;YCXx1 z%Lxk6F9P2&YZ5J6sJ6D~`PBp=b_o`#XnXa2UEZ5-(JJokj9vlr0R!eKoGws&dhBEs z_eE@QXKX~mJt7H30R!f(eAopLVdiVOmwNl@jWA*Pv5yH*c#pvq7Q$_89Tog9OUvx z(;17Kq86J&i)J&;_wq5O`bO8ZD(zEgQ<5YbAuh`LC#F3}OuWjNg|}N3VopMXlr~H` z9A+9MxClfM4ocGb)U#+9@!N?e$@X#14A8@c^!>c9QAi4My_?qAqOIXqxVupL*^#Qk zk*f2gL?{1)yXG^`NTq3a>q)1O&Ik7zTD9|dSw@mbeo5{+e4FPbL<|c28+ORL_aHoq zH#I>uH35pZ_Mv0%e)KyYob?JJ{4KZ0PB~MfIgk+MfS2urcXle1bz7K?dv1WuawNNIsw_bHVLlCpDHO z{9KGr_~HBOO}Pl~SnU7Y*md#DIcl;)1D@{DrF7Z$b~!y8tT(?a?;G?VCFv*FI6lf@ zTsZUieh}o()3I;k@Us2U(1hZV0|ugk`(G{7*VFfAw;)S^17G3yx8KCWk#64ae0-=y zJ%6ui^1(S#dT&DC%Bfb~>sCTmZ8*}z>rFfsc%`Wv?g1vp?cF2FKA&0HV*U&z7t@m? zJjZHB$8$=k)mhA$g55(wls1t&{PBjX|M8&a(O~K=BEz#x!Pf&v=0Y7B$fhcUH_27h zK-_j7;?Q61o6L70TyPWp$-m@D`k3y-e0fs-FZJy5?>ejGvPb0(*kcL!5!8tEd*1A= zp6MkeI^$rkniwfae@^--JV-%AST)HW?f?9S8&`l7N8_2s%4!v8u~%X?X(`c#eaAWN z2NuvIqo8mH z!VJ*i3~OvNK7Uqbi~%O4=R#{>tW~a3ngNjdVwki>EF1u~1NUhH*qGRU>ROy-WIsL6 zj!M5h1GtJB+^VRQl)hzqLy!H@Cn*AW8vfZD=8`d&E4LsNHN}>*qj-i2p;>ZU#KY%}sOUnKXfEy|*s+^o0;Q8da zHAFEefv-0Lo;)B(K%&wt*6Ui?l?f&lr%VB8T5!9*9GJ{xA+eE>oIpT+HC3g-`*dg$ zfNG{zpzgEc0zIk0jW@2RZ(soC;smJD6hsF`r2a4k-s=TGJwhyHx>K3ZVucypw2n@(qeW@Fq8P-YVo6TnM{0B>*>ng#Mr-X2`SKOi8C zjHlcF1kkXk$Vez3!QeaC9m8Zt0nnYbpS&e$SLIfe+tD8yKw`d30d*C{(PL0b%?|yT zlcQrJ>v{BNerah5)?Y)*&%xLaH$~8?(S8{_pe4Up!?Ap$Y~K3gabanw4Hn|D%)d^w z5*r&E*dz7aUtb3CW zG@T#(Kvsfpkrx*6IC&!>A<-~pO1Vqpo2|MAv_NK2%^Ix7hg&n-b4`X#AK-jeznXG{ z-30d=ueeW9Vsj@_>hzMGs@^)8D|$H`6SjRH3S7Kw&dzeUQOo2x6rx{}DjGb~)f9LU zI%rk_U`O?wryDTxe*BG9q~8Dp%;6<535lz#>tq2QE~d|&_v@h_q%wiHdJLATT!cg`pf5 zi*WX#9Y^Tqw^g$7B~P_pQ((Ui139Y+fFf}pOr4YY{{V181~D(O(BA=jotKwau!5b9 zZ2<6B*WksP39I5k9c{E*x8m3ukBpCZpeZH)Ka6LWlfAWT_*7OE?H2>(SB(dVTPhho zULK$X*3&-^XHbTGW7}2bN~+B@(Saqr59wZRs-lh!u5&z+rI}cdeP=2ywP0tv?X40L z68?oD=?=qd6w!RWA4LZQpuI?q%*JsW@*}R{l8Ol*?C$pT^puNOJWML`x{2*)QCTN} zMe6V3=5~5|3heM<&L=an>H<5huNJR~TD;sEsfbw!F#Rj-;1drWK!>?&MQ{;`*D>+% z0*!G)Urcwir!`uhap+L~^gqPO>A)PKbbCDu)PI0HmFH)G20!P#7(yfIubfrQW!fvN zsHn~u1E7b4gG0`b^(|Plxke9U#^J2nHoz3Ou>D4wX94xHo+xY6EIQfyuJqj47=#x? zR97ekzdfvVb~fGm;Fs;jFYd=X10turZEZQ}=|2GugY<&WBD~DZR`7*@-)Fm`2VAF2 zjE^5Lf2owO&KTE&%2F6JaO>0Dtk5i=1>G7&9dB(CpM_@DQpfJR=5P&$<27~n%@-HP`ei+CWl04S%8crU>1eq>vzD{R^u52R3D-f9py z=TKelhk_Cc;syCsX*q`N*9>_(AplYW(5d~!vFJ(e?%v+nfY${V9LOYYBARTT^bzLo zAuGSg2YNB=GhZVboUWZP{IwDx7dQm}hQjLSRoz5P>z}GJw;xG$bM1z_{5WD&{yxah z^}9radB&{sQzxKraewHkV@?)$rG zI|f!M;)9Po=_`J}a*u!IVzhq0y5K@X^UH{GYEZy+5oKEm>6)8?CrWT|1-iiA@oHB|?2L zl8|AWluP0L8E0gNL~4Ic0P^*%`BY=Qb>G?`TMqo!zj()1vrA>1H3Z7E#bfUWHrsQS zM0dtT9oy431v9JN0NZ;CE1Rh zlr)j&ED7$6yMbZ9V}8YsKjeP&4HFC1oV)j~PHVicMcMwW3$%oA#YI}yU;nrS?*l43 z^UdU09*l+C^T5+GS()PF?&uf!ADC4MIPa-^lH$fyQ3?tmjT4yHpIq3Yt;>!7#n@`b zZVV%KKr^C7T)#v3jOYqWZ|=eq8q6Dv8WC;SnNo5$A9UK|x$26=<_D|Gsu$Dr@MPb( zwOIa-hs2i_@^|9>KzrUb3e5JG&lLrXIl{etz^8j8}Fi4Xoh_dt%h|g@uQ-qmB{F^1#=IKY=z6G;_p?4zm&YgO*hY963FOBo3RkOK> z7%B64X??IBRQi4kUJJ>Vmm#84wU8@tC!wVAb+$_Oze^!mFMpWSW*6LXVi=sm6)COO zwtt~}motG-LNB<1D&Aq+c+8m27uq zWxF$GpdH;xv9s-2p%C038tVR@*anmFNF$B*YkESz`R>vC5~yw-=K@@ zGL+pW^)V>T4QUa3LneX5s^yb-FD_}--cyAEs;@zO{F)e3bM{?&xLtHBg-)bTE&?&A z`&G{j<&Jx6UV;7j|KE$ZTdSCeXKrBC5hwLIzF`6=&xDbUt%JRHs0gwDHiB+#^&#S! zLPr?&Ku^!>f8K91F4YFv*kVV@ZUu}u6kdTaBfg{l54->`{_o$w3-0HjK3ds({mXk^ z89e(w2GMwk7Yf!9_eq+YuU(^d|GBc#VO&Z_BnRPTNEzCk!#%Bk@u%G^;u#=C>9?Uv z@?uK>KA}X_iJIx|)g%!MV-azJP;1$99Rr@8u*2;+o8S-rxVPjJ_fpe(&E&z;C0wurFl;5a}3=bR$AZPW*n@h+n$>c7$# zW9ZrVeJ%m5hEFAA7W@GapdmB&KR?AG@=~bRK}JpO1%_DnT9t?E=5A!AH!o%$E+!x} zeSx|~se(?$)z!RV9YKMC6Afptn#mO=Ka5LF2nd6r9tZ768& zlKa$YXAQ{;!$ic)5ksI^2*QtXJ*e=fe2%=5QX0hkmrZ-qMCaz_()vtKPfoy7P|H`> z%)@B;3~LX01gTvwFwxOJ=~i~zJK(u@-7l%%!FDh%F=)t(hM=nIgOLr@I9;*VkFFxb zxH7)4L8gFaOIjIZwP~w@`N16K;o-sJCk4q~e1{E(v*pEYsz1R*XLb>Iv-+$nU7ay2 zD~stS5dp!Ss4hnC_?~52UuPi$2Q76;|9rJv%|=DsmQQTo>ohdze<&m6A|aeuAQZ$Q zAW#>oWpQq$^$=n?brl&~0~$pxd69)e>Tz+Oe(_4j+_F4ZkD>#!M#*;&tW!%lj-n!2*>(T#-@mw+Xf#Xn^Wr&x3*PZ1#g&Fs zDt(J9eYXHa^)`jNi#a1NMe=qDE{+7n-IalA_bpwnq&~>E zMACqT(47nUiDRVx$45s%CQ;|JgBeC=5vTnkGi12N$Q?TAjz>tJ&91 z-{vw^cY4J0!q(ni0tw5Ym^F0s)~!o0G>8J1MzsucLRIIt+k$Sf9J3kJzb9CSxc3ZB zY2z$olGa9y>I$ntJ>T8CKCWkLxoN8B?p6=KY`x0sd97iY>aX{{a($MRN6V;o80_(N zPB3tgG)rr1E)f0Q+1UYStgg0}#+QUuPdnoSP#nNbGv_8N5EZ<0=WCT#Vp7ruocNVp zh?;@r;~Yn$4>&& zXMS^2e`XCyLlx!()^hi~qmz@&0_~G0R+;dPNE$D%OYjmuX_wOS^H03s)Cr{$aweeJ zYbUqSR>j1p3MLS+{VDeEP^XkPVsxP|$zol_H+nLAnirI)aj9&mMA8Wh3lmCogi@Ag zLmhYu)bMolX+}O^Q*^v?&WQ%6Sw2Vg_)J=jX%1A0KPH&$D1{J#j6xk}T4)@UV6W+Ze`7%5$fI+|SkV z(&zfD*{n9fjD?@2^|SO?OIRhBdD(eN!@fNmQGTSa%BZf=5~C|HxYwY%6E;yg(}a6817+?$W36=gb^e@e5LE?SS6oR#(eq7{RA8&LW%9hQ|sl< zlE+C8kr`muPp+0(WR7isH2?CVD-yCbDQRxrxbd8S4;U3RzIoriz|zzBf^EE8<`)hP z;wugE_iIOmixtSNGX0$-if*aK!6DPo4*Kvx0tuN(N)Z_D8UN)1jGCm^EimHuogN>L zE;FtV2iG<6zkdDt{QTTSh$^533mcmeYWRd-X>KIJ`e)Mr9!%NX^%Zd1a^LvD5+eP2 z_t)cfpQPFJT~^@_qFcs?mT`2+KA3b@ghw@LBU<; zdf+`kHgSfswt>L~44AHK@h^b{kkP)IaRW#}%3D*del+69!W^4yLJC0c=64-tjm3TT`mL4qq{r#YSx?zbKgZWlg zR?4-ReF;LOdpbMw3~7K-1BMOkv_<<`5b&_gHSgbas2#?B+OB?B^Y6k*rs}pdCbIdu zC-6!;*sy$NPklG4_`i9x|2J5QEHOsAyaT`X5RtP{{OeBnJ$$|HOdO zHN0Urz~le(i*$$q!PVJag@?^3X5MRtwMn=NH`{@XUj2wb6kzba1iQ~>DV%0?$3!ZC ztu(}+j&NU2JQ9Mhh1493J`=&KZ(3i_zWaI;yVW>?;(FI24JKM(n=Gw(W23VeVrT!6 zr)TE?zKio6iuOc=E#gQL%Ns4e&on`iKCUN5{@#8QNZ)+*<>h7YlV5NGB;fNcebmB1 zAjO~rOeZAGx&oQQNF6t(&$q?Dc^#zwAZs&}-y6lt(lxX9c6GrrV4+_XNgG(%^^?eu zrOTuWlA;ZiYF@VAyu=+R$RApIHJ%(%mt=87|equJ2$G*!JmKx>LwG6Z&k{dp4t?BWkHwk`@AD|7oLe$KA)4XyL$T;py zwY9aNr^~$V-^4D}X1@@})ef zMt8ZHHI!FMw#=Q>XHl6*C+8>emOzy;#V7baFE1~b zqK%NH1D>5~{>b-#9y2pD-@mId#?;l-O>X9+u^@n-lb}d zRW3&rK(br6Zb1YTbRw(=QD7_h2+Smba4(%huP&*JVOpC7|O|s?aZv1N@?{@vk)@9mU-(IS6Nv_is0DpUM6jS zU(%UEoO2v24K=m!Mx+LLbNlv#2-)9r zLda9y>+gPk%;uvMY;UrC>%Ony-i|xc@Fi;sT`cG-$gfdOvJh0VBj^$UgaGK;-CI*L zG@OHS5s(#n0@xfpPEI%z6lHk78-Zj1@*L2DBT`c@mb#)qp#42Q1E~iXI}pSwK|mj1 zdC~l~bOF@JdFlP`){Jw~!=nO?yr;Y})yRKB7Kn>SSwm-F{(<1s0tNSaB@|{*U0q#P zRu-VMt7vFC%&}SIIR=M4d4tBxDmZ-62=^{OpV@Uln;u#>&S>N&_~+ za~a;Hxn9$Gwx$}-OX1lsya$`rdHr*=Rn{{n0MP-@ZoK{x=?{Am?pw@2C6^2U$3mcv z=4-KTzaS|ey1&tC8u7=zNvH)92B8L!*iaq9-_H+5#KXwQC?r(hynvUO=4Ug{diURSQ(ezQ1S|Nve2p zadtdEKaT>u<)XyO2i0f4eNCmkdl%)H0@#Wo+wTm43m8JmK?@nt1jZZcm!7_n`IX?!!x4xb33nFN#-9f&=_wGX+@{pHRez@s+Mufo6~`K14)D#Exi_FBhZU_P<{_Z`k=s5Q>#8%Hx{ zA?=}*JeYZp zT@`W+2go0fyvlKN684Q5H(B|Fo`HcUEGF1P@Qt(n#5^*rtgL~7fgn#NVVlC;N=QmF zfH4;p^?cwV9Rq`5t-asC3Ij-IdV1Jk7GWEI{``TFrwO?XM@G?gL1>Mhj!sG%^#JgC z2lo4qM;K5T=-Rd1Trf98;5eboFN4p|-f9*;R!~TK_=+rM2YjbQ#gE+}5DNgihf^mk zB;-C_V+R!a{N!B!n>~v^y|gEjqocWj*b7$#uiPCv2!S*HV;PVKu(TUa)(c^)LctW> zYHR)TG-4zkmR0 zKHGGErUDu+eB3vppEov3)Nla=phf4l<+K@=RiI+<`fIgo8&{_0fIfK3k6 zZ~Caw!6$=}V&h|EwRLsZsf4sk#4LG5LE2pl-SM1rLCg_3Lp&d#OcUDg#%n1 zoGJJMh%I=XtmT6E7&fbLjaDlJk}V8MT1I9YFr)#|ONfQeLnSPTzd=?FF)8UdDbGV0 zm;``Y1iUWl#qNO6Jpy)qa62RKVPylHQd-|xmz5On$u0a+;O?TTe0_0J0-akq30j2} z_Ar!{$Pa%7MmuCcWLj6+LxKe4#=rx@dZF@AoI!Ln&_G;IZyI)og1r2m>v;${FSrrU zqLk*K6cdY1*%EAL*9L)BUti1uZF{0MbE035PE_&ouW z`>Cw5(zG~(c72=u7@rX(h~cY1!i{e1BQv0u3b6g>ye?-{f5Pl!tC|Dk;TNkI&pr5t4Azu)vup%iiF6A`(9s7WnIc?VZ8)gc{Yz9j&XEFSL-ILTXr?7V>5S`b@| zpVD$EFkFs?+DAGU&MI)XTeA%cA$xG6uZFYbV#xJgCsfkP?Lchh3^HthvONsiwk}Ou z6y$+!5h`M0>*!cqs{xvC5>|ltsLFB-C5~Y${cvGasw-e!hUI{a8&qYcVCh9TxC9dD z;=gCU9D+;&7Q7;TISlf7doWBgzQ#U^+`B!OmYUjcTx$IMxhPaqFf=rr628Wk3q_s) z&LIo=1rlM4tyalK_}9Vy<`kF_+pU2&cx2{5IG|bw)#`0QEE`D9VFUuNfAtn<6jtbN z!iI$~p7({z4jfKY#TGCSP);_)qSQ|yFiS;2fr@p44dL(Wdj{i;jm$3xSITrCl@dym z0SpLiXV~E=LjmOS50~|w;itLD5Kx}1Fc%jWPxCm?0SLo{a{?mG13=Pu{H@KXi`o;A zwJ|Vni<-F-L(zA4pa2k5f|DGnSUJ!c?CwFsyxv+?7A~edKln1u;~`Ysz|MeLREuCa z6>b8T&R{*cCw<#4~>Z&;+t;evg;7?5Mxlp7rRCen6}n2{usMEknrRsjPa5 ztUw=}8k5JP6c}l1TZY;d;HFp8iGT%q?;bd6_gGm&OeqMex;Jahrsa~7k}JIlJ_Kc3NvIOdhj)E{ z6zfmbzJ{!a&E@4v=fB3jgP>sYaYvYop`_X~kVhkUiH2^Qm6)0qN~R19StV@p3We^F z+?BT@Gz8Kml$0a0I}?21D(AloAXeY}9*?NQr*RN%NgOC(m-@N9Y;rpV(qXcf!x(bF zlm(c*q%2vNYfM=shLJ6tjKjDU;KwKD5-P%pHVyll%QwIsP*YR0ynnlbj^LnD6=L?f z6*j^LQo>YRrno-e^1|o(`a(|rz~vrEuIFU7!lQqmLe| z403<%djQ!I9ugVPZ9bglbz!dme!oUqA4JkJEb%x4AP8%$*z&U+tYXn91fZYLxto679FVf!#s;B_gf82C= zDthf<3)#Mo?EborGTRoI3!iSV6gK6P|F|8}j{QKRz3fNB8XyD6j8C7KlPdUGS^GKX zOF(BLGav@c2#Aj*q=?{s>U@jLAn00l=+#qIU6`L&;Xem!81&G=;UNSr4q+9%0N$$A zNEl7gxE<9+hkWO%r>+k8ub&RMe4`%cZuz~$j0JZ|f_UtgWY?#9V0!_c|C8Qc@)3$z zTWK(&tFHi9$r5m72qkV59mvQ6g3oJX0DB(|4XvTRoYjsWkxxQ92O zXQTE}F$2>D0JOjTg4^l|45Fe6D>yrJ$9eR`^X<-D6tgKZ}vi+Nxgr@LhMrnxx8MCz}H1yCc zeEy;cE_V0SF&S&}Sp}?4_^NH5yNh6B7r5tvUIjjdj9)A0IZbW%$3tsxi0R99JEmci zmH%HQTzNdy`5#8dPpw=zI^^0xj+NvJjcXJYMVKKX8ANp1At{lgvM6FR?ql749GzWu)c@Ol}1zn|~t^FE&Ec|TBDupVV;RFTfF$Gtw&cfgv8t?GH%S`8& z6%Ux^isr(7AGc7oWTpmcO77kJ(pz>2Dy-T0c_K=tC7ANGNu(IJH(tcDU?TG~QcxIr zHsatxhCw2ce)@?RUfnatvkg~wT5yVz?j2NmYksEA6l4jzb@n?LU5f#<$vRe=!?8Cg z&TiWmMYo~89lT?U5ny(&O@6d}N6fyCn|6V{I&6%)6~)BFCI2#cW*c>53 z4lnAnK2$Q{3Po%X!gKBMW8DoRK!R|Z@|zCn>zt*7Q^J!~b=-{xTa~oguNtS%#*Q_2fqTdK>y)c{bB@9 z0uQNm%`!`;mOkx^qePcq^}-Y>D3;%il}Mzw*jjs%OSECQrEIqq!!1wJ2!0iPuDdo>BdtHH2Ivgl zZy&!Um&w!+7k1}GG#`69bbqxr3!U%~n4aCgY&iY#ulpG2#kO!21!d>s=Z6O57z$8~ zQp3=842|8v#Pekb?_S8nhDS(2;#9L^QaTmx`at5;jSstO^~1>xgQ$S+08yo;red_u zJV<3dj15sz5T_E|Ayyt`A|_^U133HnsY~)Y_lr`hta>Yu_Wk1hIA<3~hvqoB4+Ahbm?s4YtXf*(qI3}YhK3q4h zg~~r+TT`C=i<6mYYnOBmt%i85?Iw}BwLP1LJ{2Y>i|McGug?NZKuflEj7P6e9ZTF@ zT8I;kRJphmM^VmCH!v~%PqmM4WoWACvcMOmS$SR&=iqQ8zTCmh=F_4tTi;j?7vZg{@Ryom!c;2idvc zLnHA*P`*4GI}>Yp`tJv;c={}k)x&SWBTaDX^jA|W!PaPUdO|%3Xcq+ytugF0LT`3f zmcJg8ru6|QapX_M+74E8HVykol8L0&EvphEgY(|FpFYPjS^EzR5L*vEy!u5$qsjgSTS) z-j5F%Bk8c}+vLD?=jzhe{t>}iJ3G%|0I+XuEv&8W9T?bYW;PN&uEssG{A`Z`rsQSY zr=ich06Rr8nOyqkFgf*+9y&CS<&ojnv3hgyrv;Yz_m+!uxEVPrhi+wV+vvSxd!~(q z1nZ-3hL>cNtjEgOr`93#XV$Vu=2u%VB%V#yKYCp%u57XC?V#@T*t@JpAwm2T?Oi*2 z>auH=+bpJYeTyf)1$U2o%NZS8x^M91)BDf==9C=RE)odc3XhlKrW(1(L^D;{(C~`S zI4eh`(ZbLr&CfF?C|u0yqVI;TIMz}rT2V`;0}(cI(7EPnKrY2tDl{adwyQBhr8_9>)SgM)5vZv6^H?45`<&wHsUr15v}R$4p_ z;5a)wGa9&MWycq4@A&9WoH9DX4Oh{ZxtW%R6Z`>fsAlrn^(+4dPKr*UP3MVk4PvGp zuWl*my_Im==rn3Bj3$mFOaZ9=o{$B`)1R8_-r)Px@da10C$6^6?` zl7t_i55~qoLNGQqhJSVajn2Zr=$F5y`$XsQ_$uTZZc!|rbD->mPV+!Pm zu;p*NEW}ygugUJ#*GBl87G#qIG0}jfrL@t}(mGehg07pfm6X88yXFu=DaFvzFCgDU zMn-yiDs<1dRP=eXd;9z0;BaC@re${i>ryC;pcxHj7ZxcFy?BX12B4@lJty;CvtA~$ zw{^Z15ep0t7t)+B3bxTL%PT#RtmYS@7wSWi7b)l|q5E~^6|2^Id#x7uYg@ETsDQYo z4a)^vv*C4;xU#L|U-EDAXP8Hpcj{VPk6cpC%~@YP2Mc{|0nGVB$MA~z8%>W07*q2&XROFLzxQC(MJaWK6$~J{$ z>nNp9CTm}N1K^Ip3Xu#wJ%fLFcuYV*h-yo*aB8+UnHf=$H!<)t*v8y%GQ;rFAM17M zdMMn}A4l1qFzUnJAd^6!Zq$-*H?p_y`INCyYr}rt+(Ul~$hPN>1Nl~^P4fJA&sqfD z?-pMY__E@e|135h6zr4T;I$jWX;&wBr0ZBbsH;;uJ}c0Ez%UcOgHramu3 z9I2*40W}k$si}z;%+tfe!^!DpinMA&L#l#V@mtmo2j!?R`DxQ@?GC0%D+SjGU2CVW zyrH+j$JN!QB8~1>IOJaB@?)&fan2RR>(8!SOOSAi5VX<1azWsPt@4e$t=9^+R=;R) z|H{nC5nX**BW-W>i^;jg=xCv6MF?s_jI&)T(zHos{4F3ZMOt-4){DmDY!(GFi!a~D zjb3cdEk2&O`=IuRa-^tB-Q%g{^n6(3u6?GrMKu1m z_ESccj%BCh+E2TKO6?1fYlkVaUKO76N zu6gTivLi_n%w7Tx_QJxsq|vaP2|8ABCQZg#YtG-pbt=#qg + + + + + image/svg+xml + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + drake::systems::LeafSystem + drake::systems::LeafContext + Time (t) + Inputports (u) + Parameters (p) + State (x) + Values: + Source + Computed + (cache entry) + Outputports (y) + derivatives,updates, etc. + Fixed inputs + From d820d510b149f79efcd5d46d171b2dafcfcb6b74 Mon Sep 17 00:00:00 2001 From: Soonho Kong Date: Thu, 21 Dec 2017 12:59:30 -0500 Subject: [PATCH 14/25] Add symbolic support to Maliput lane --- automotive/maliput/api/lane.cc | 14 ++ automotive/maliput/api/lane.h | 22 ++++ automotive/maliput/dragway/lane.cc | 27 +++- automotive/maliput/dragway/lane.h | 9 ++ .../maliput/dragway/test/dragway_test.cc | 121 ++++++++++++++++++ common/test/autodiffxd_max_test.cc | 11 ++ 6 files changed, 199 insertions(+), 5 deletions(-) diff --git a/automotive/maliput/api/lane.cc b/automotive/maliput/api/lane.cc index b6149db40964..bf1192b32f42 100644 --- a/automotive/maliput/api/lane.cc +++ b/automotive/maliput/api/lane.cc @@ -23,6 +23,12 @@ GeoPositionT Lane::ToGeoPositionT( } template<> +GeoPositionT Lane::ToGeoPositionT( + const LanePositionT& lane_pos) const { + return DoToGeoPositionSymbolic(lane_pos); +} + +template <> LanePositionT Lane::ToLanePositionT( const GeoPositionT& geo_pos, GeoPositionT* nearest_point, @@ -64,6 +70,14 @@ LanePositionT Lane::ToLanePositionT( return result; } +template <> +LanePositionT Lane::ToLanePositionT( + const GeoPositionT& geo_pos, + GeoPositionT* nearest_point, + symbolic::Expression* distance) const { + return DoToLanePositionSymbolic(geo_pos, nearest_point, distance); +} + } // namespace api } // namespace maliput } // namespace drake diff --git a/automotive/maliput/api/lane.h b/automotive/maliput/api/lane.h index 8093b3bf9a34..7cd7493643a4 100644 --- a/automotive/maliput/api/lane.h +++ b/automotive/maliput/api/lane.h @@ -8,6 +8,7 @@ #include "drake/common/autodiff.h" #include "drake/common/drake_copyable.h" #include "drake/common/drake_optional.h" +#include "drake/common/symbolic.h" namespace drake { namespace maliput { @@ -115,6 +116,7 @@ class Lane { /// Instantiated templates for the following kinds of T's are provided: /// - double /// - drake::AutoDiffXd + /// - drake::symbolic::Expression /// /// They are already available to link against in the containing library. /// @@ -160,6 +162,7 @@ class Lane { /// Instantiated templates for the following kinds of T's are provided: /// - double /// - drake::AutoDiffXd + /// - drake::symbolic::Expression /// /// They are already available to link against in the containing library. /// @@ -300,6 +303,25 @@ class Lane { "but a Lane backend has not overridden its AutoDiffXd specialization."); } + // symbolic::Expression overload of DoToGeoPosition(). + virtual GeoPositionT DoToGeoPositionSymbolic( + const LanePositionT&) const { + throw std::runtime_error( + "DoToGeoPosition has been instantiated with symbolic::Expression " + "arguments, but a Lane backend has not overridden its " + "symbolic::Expression specialization."); + } + + // symbolic::Expression overload of DoToLanePosition(). + virtual LanePositionT DoToLanePositionSymbolic( + const GeoPositionT&, + GeoPositionT*, symbolic::Expression*) const { + throw std::runtime_error( + "DoToLanePosition has been instantiated with symbolic::Expression " + "arguments, but a Lane backend has not overridden its " + "symbolic::Expression specialization."); + } + // TODO(jadecastro): Template the entire `api::Lane` class to prevent explicit // virtual functions for each member function. ///@} diff --git a/automotive/maliput/dragway/lane.cc b/automotive/maliput/dragway/lane.cc index e3b491d60df9..7471c4b2ff94 100644 --- a/automotive/maliput/dragway/lane.cc +++ b/automotive/maliput/dragway/lane.cc @@ -102,6 +102,12 @@ api::GeoPositionT Lane::DoToGeoPositionAutoDiff( lane_pos.h()}; } +api::GeoPositionT Lane::DoToGeoPositionSymbolic( + const api::LanePositionT& lane_pos) const { + return {lane_pos.s(), lane_pos.r() + symbolic::Expression(Lane::y_offset()), + lane_pos.h()}; +} + api::Rotation Lane::DoGetOrientation( const api::LanePosition&) const { return api::Rotation(); // Default is Identity. @@ -121,6 +127,14 @@ api::LanePositionT Lane::DoToLanePositionAutoDiff( return ImplDoToLanePositionT(geo_pos, nearest_point, distance); } +api::LanePositionT Lane::DoToLanePositionSymbolic( + const api::GeoPositionT& geo_pos, + api::GeoPositionT* nearest_point, + symbolic::Expression* distance) const { + return ImplDoToLanePositionT(geo_pos, nearest_point, + distance); +} + template api::LanePositionT Lane::ImplDoToLanePositionT( const api::GeoPositionT& geo_pos, @@ -169,11 +183,14 @@ api::LanePositionT Lane::ImplDoToLanePositionT( // implementation chooses the derivatives taken from within the interior // (zero) in order to remain consistent with the derivatives of // nearest_point and the returned LanePositionT. - if (distance_unsat > T(0.)) { - *distance = distance_unsat; - } else { // ∂/∂x(distance) = 0 when distance = 0. - *distance = T(0.); - } + // + // We want to make sure that ∂/∂x(distance) = 0 when distance = 0 (not + // ∂/∂x(distance) = ∂/∂x(distance_unsat). The max function in + // common/autodiffxd.h returns the first argument when the two arguments + // have the same value. As a result, one should not change the order of the + // arguments in the max function below. + using std::max; + *distance = max(T(0.), distance_unsat); } return {closest_point.x(), diff --git a/automotive/maliput/dragway/lane.h b/automotive/maliput/dragway/lane.h index a210b2a6de2d..d1f858e71ab3 100644 --- a/automotive/maliput/dragway/lane.h +++ b/automotive/maliput/dragway/lane.h @@ -164,6 +164,10 @@ class Lane final : public api::Lane { api::Rotation DoGetOrientation(const api::LanePosition& lane_pos) const final; + api::GeoPositionT DoToGeoPositionSymbolic( + const api::LanePositionT& lane_pos) const final; + + api::LanePosition DoToLanePosition(const api::GeoPosition& geo_pos, api::GeoPosition* nearest_point, double* distance) const final; @@ -173,6 +177,11 @@ class Lane final : public api::Lane { api::GeoPositionT* nearest_point, AutoDiffXd* distance) const final; + api::LanePositionT DoToLanePositionSymbolic( + const api::GeoPositionT& geo_pos, + api::GeoPositionT* nearest_point, + symbolic::Expression* distance) const final; + template api::LanePositionT ImplDoToLanePositionT( const api::GeoPositionT& geo_pos, diff --git a/automotive/maliput/dragway/test/dragway_test.cc b/automotive/maliput/dragway/test/dragway_test.cc index d4324a9d561d..e76cad3754a9 100644 --- a/automotive/maliput/dragway/test/dragway_test.cc +++ b/automotive/maliput/dragway/test/dragway_test.cc @@ -826,6 +826,127 @@ TEST_F(MaliputDragwayLaneTest, ThrowIfLanePosHasMismatchedDerivatives) { std::runtime_error); } +TEST_F(MaliputDragwayLaneTest, ToLanePositionSymbolic1) { + // We want to show that the result from ToLanePositionSymbolic() corresponds + // with the one from ToLanePosition() when x, y, z are instantiated with the + // same values used to construct the input to ToLanePosition(). + + MakeDragway(1 /* num lanes */); + const symbolic::Variable x{"x"}; + const symbolic::Variable y{"y"}; + const symbolic::Variable z{"z"}; + const double v_x = 1.0; + const double v_y = 2.0; + const double v_z = -0.5; + symbolic::Environment env{{{x, v_x}, {y, v_y}, {z, v_z}}}; + + // Computes symbolic gp and distance. + api::GeoPositionT symbolic_gp; + symbolic::Expression symbolic_distance{0.0}; + const api::LanePositionT symbolic_lp{ + lane_->ToLanePositionT( + api::GeoPositionT{x, y, z}, &symbolic_gp, + &symbolic_distance)}; + + // Computes gp and distance (of double). + api::GeoPosition gp; + double distance{0.0}; + const api::LanePosition lp{ + lane_->ToLanePosition(api::GeoPosition{v_x, v_y, v_z}, &gp, &distance)}; + + EXPECT_EQ(symbolic_lp.s().Evaluate(env), lp.s()); + EXPECT_EQ(symbolic_lp.r().Evaluate(env), lp.r()); + EXPECT_EQ(symbolic_lp.h().Evaluate(env), lp.h()); + + EXPECT_EQ(symbolic_gp.x().Evaluate(env), gp.x()); + EXPECT_EQ(symbolic_gp.y().Evaluate(env), gp.y()); + EXPECT_EQ(symbolic_gp.z().Evaluate(env), gp.z()); + EXPECT_EQ(symbolic_distance.Evaluate(env), distance); + + // These values (1.0, 2.0, -0.5) lie outside the lane. Therefore, 1) we have a + // positive distance and 2) the symbolic_gp evaluated with those values is + // different from (1.0, 2.0, -0.5). + EXPECT_GT(distance, 0.0); + EXPECT_FALSE(symbolic_gp.x().Evaluate(env) == v_x && + symbolic_gp.y().Evaluate(env) == v_y && + symbolic_gp.z().Evaluate(env) == v_z); +} + +TEST_F(MaliputDragwayLaneTest, ToLanePositionSymbolic2) { + // We want to show that the result from ToLanePositionSymbolic() corresponds + // with the one from ToLanePosition() when x, y, z are instantiated with the + // same values used to construct the input to ToLanePosition(). + + MakeDragway(1 /* num lanes */); + const symbolic::Variable x{"x"}; + const symbolic::Variable y{"y"}; + const symbolic::Variable z{"z"}; + const double v_x = 1.0; + const double v_y = 2.0; + const double v_z = 0.0; + symbolic::Environment env{{{x, v_x}, {y, v_y}, {z, v_z}}}; + + // Computes symbolic gp and distance. + api::GeoPositionT symbolic_gp; + symbolic::Expression symbolic_distance{0.0}; + + const api::LanePositionT symbolic_lp{ + lane_->ToLanePositionT( + api::GeoPositionT{x, y, z}, &symbolic_gp, + &symbolic_distance)}; + + // Computes gp and distance (of double). + api::GeoPosition gp; + double distance{0.0}; + const api::LanePosition lp{ + lane_->ToLanePosition(api::GeoPosition{v_x, v_y, v_z}, &gp, &distance)}; + + EXPECT_EQ(symbolic_lp.s().Evaluate(env), lp.s()); + EXPECT_EQ(symbolic_lp.r().Evaluate(env), lp.r()); + EXPECT_EQ(symbolic_lp.h().Evaluate(env), lp.h()); + + EXPECT_EQ(symbolic_gp.x().Evaluate(env), gp.x()); + EXPECT_EQ(symbolic_gp.y().Evaluate(env), gp.y()); + EXPECT_EQ(symbolic_gp.z().Evaluate(env), gp.z()); + EXPECT_EQ(symbolic_distance.Evaluate(env), distance); + + // These values (1.0, 2.0, 0.0) lie on the lane. Therefore we have 1) a zero + // distance and 2) the symbolic_gp evaluated with those values is (1.0, 2.0, + // 0.0). + EXPECT_EQ(distance, 0.0); + EXPECT_TRUE(symbolic_gp.x().Evaluate(env) == v_x && + symbolic_gp.y().Evaluate(env) == v_y && + symbolic_gp.z().Evaluate(env) == v_z); +} + +TEST_F(MaliputDragwayLaneTest, ToGeoPositionSymbolic) { + // We want to show that the result from ToGeoPositionSymbolic() corresponds + // with the one from ToGeoPosition() when s, r, h are instantiated with the + // same values used to construct the input to ToGeoPosition(). + MakeDragway(1 /* num lanes */); + + const symbolic::Variable s{"s"}; + const symbolic::Variable r{"r"}; + const symbolic::Variable h{"h"}; + const double v_s = 1.0; + const double v_r = 2.0; + const double v_h = -0.5; + symbolic::Environment env{{{s, v_s}, {r, v_r}, {h, v_h}}}; + + // Computes symbolic gp. + const api::GeoPositionT symbolic_gp{ + lane_->ToGeoPositionT( + api::LanePositionT{s, r, h})}; + + // Computes gp (of double). + const api::GeoPosition gp{ + lane_->ToGeoPosition(api::LanePosition{v_s, v_r, v_h})}; + + EXPECT_EQ(symbolic_gp.x().Evaluate(env), gp.x()); + EXPECT_EQ(symbolic_gp.y().Evaluate(env), gp.y()); + EXPECT_EQ(symbolic_gp.z().Evaluate(env), gp.z()); +} + } // namespace } // namespace dragway } // namespace maliput diff --git a/common/test/autodiffxd_max_test.cc b/common/test/autodiffxd_max_test.cc index d97e26a7508b..75af8030058a 100644 --- a/common/test/autodiffxd_max_test.cc +++ b/common/test/autodiffxd_max_test.cc @@ -12,6 +12,17 @@ TEST_F(AutoDiffXdTest, Max) { CHECK_BINARY_FUNCTION_ADS_ADS(max, y, x, -0.4); } +TEST_F(AutoDiffXdTest, TieBreakingCheck) { + // Given `max(v1, v2)`, Eigen autodiff's implementation of the max function + // and our overload return the first argument `v1` when `v1 == v2` holds. In + // Drake, we rely on this implementation-detail (i.e. Maliput Dragway's + // Lane::ImplDoToLanePositionT method). This test checks if the property holds + // so that we can detect a possible change in future. + const AutoDiffXd v1{1.0, Vector1(1.)}; + const AutoDiffXd v2{1.0, Vector1(2.)}; + EXPECT_EQ(max(v1, v2).derivatives()[0], 1.0); // Returns v1, not v2. +} + } // namespace } // namespace test } // namespace drake From 8fa5246767ed6026e860c659e2aeffff81c1aee3 Mon Sep 17 00:00:00 2001 From: Jamie Snape Date: Wed, 3 Jan 2018 10:37:58 -0500 Subject: [PATCH 15/25] Declare yaml-cpp as a private dependency in drake.cps (#7676) * Declare yaml-cpp as a private dependency in drake.cps After #7453 no headers that use yaml-cpp are installed. --- tools/install/libdrake/drake.cps | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/tools/install/libdrake/drake.cps b/tools/install/libdrake/drake.cps index 9be12a393f78..0f7d0b289a6f 100644 --- a/tools/install/libdrake/drake.cps +++ b/tools/install/libdrake/drake.cps @@ -98,7 +98,8 @@ "Link-Requires": [ "fmt:fmt", "SDFormat:sdformat", - "tinyobjloader:tinyobjloader" + "tinyobjloader:tinyobjloader", + "yaml-cpp:yaml-cpp" ], "Requires": [ ":drake-lcmtypes-cpp", @@ -114,8 +115,7 @@ "protobuf:libprotobuf", "robotlocomotion-lcmtypes:robotlocomotion-lcmtypes-cpp", "spdlog:spdlog", - "stx:stx", - "yaml-cpp:yaml-cpp" + "stx:stx" ] }, "drake-common-text-logging-gflags": { From a21a1226a3e3a5a8b825b8bd9b0aab5f6577e203 Mon Sep 17 00:00:00 2001 From: Francois Budin Date: Wed, 3 Jan 2018 08:53:40 -0800 Subject: [PATCH 16/25] Speed up installation (#7683) * Speed up installation * Run `file` command only once, to detect if installed files are executables. Instead of being run once for each file to test, it is run once with the list of all the installed files, and returns the list of all installed files with their file type. The list is then processed in line by line to assess if each installed file is a binary executable or not. This reduces the installation time by about 3 to 4s (out of 17-18s) * Skip testing if a file is an executable if the extension belongs to the following list: *.h, *.py, *.obj, *.cmake, *.1, *.hpp, *.txt. Since there are more than 100 files installed for each of these extensions, it decreases the installation time significantally (by about 3 to 4s). Installation time is decreased from 16-20s to 8-10s. --- tools/install/install.py.in | 75 +++++++++++++++++++++---------------- 1 file changed, 42 insertions(+), 33 deletions(-) diff --git a/tools/install/install.py.in b/tools/install/install.py.in index 231daf342b44..d78ff0ee77f1 100644 --- a/tools/install/install.py.in +++ b/tools/install/install.py.in @@ -8,40 +8,36 @@ import re import shutil import stat import sys -from subprocess import check_output, check_call, CalledProcessError +from subprocess import check_output, check_call subdirs = set() prefix = None -libs = {} +files_to_fix_up = {} +potential_binaries = [] list_only = False # On linux, dynamic libraries may have their version number # as a suffix (e.g. my_lib.so.x.y.z). dylib_match = r"(.*\.so)(\.\d+)*$|(.*\.dylib)$" -def is_binary_executable(dst): - """Checks if given executable is an ELF or Mach-O executable - Returns True iff given executable `dst` is an ELF or Mach-O executable, or - an ELF shared object. +def find_binary_executables(): + """Finds installed files that are binary executables to fix them up later. + + Takes `potential_binaries` as input list, and updates + `files_to_fix_up` with executables that need to be fixed up. """ - # To speed up the process of identifying executables, we skip - # header files which should never be executables. - if dst.endswith(".h"): - return False # Checking file type with command `file` is the safest way to find # executables. Files without an extension are likely to be executables, but # it is not always the case. - # TODO(fbudin69500) Explore ways to speed up this process. This could be - # achieved by calling the `file` command with multiple input files at once, - # if the overhead is in `subprocess.check_output()`. - file_output = check_output(["file", dst]) + file_output = check_output(["file"] + potential_binaries) # On Linux, executables can be ELF shared objects. - executable_match = r"(.*ELF.*(executable|shared object).*)|(.*Mach-O.*executable.*)" - re_result = re.match(executable_match, file_output) - if re_result is not None: - return True - else: - return False + executable_pattern = r"(.*):.*(ELF.*executable|shared object.*|Mach-O.*executable.*)" # noqa + executable_match = re.compile(executable_pattern) + for line in file_output.splitlines(): + re_result = executable_match.match(line) + if re_result is not None: + files_to_fix_up[re_result.group(1)] = (re_result.group(1), 1) + def needs_install(src, dst): # Get canonical destination. @@ -94,22 +90,34 @@ def install(src, dst): # Check that dependency is only referenced once # in the library dictionary. If it is referenced multiple times, # we do not know which one to use, and fail fast. - if basename in libs: + if basename in files_to_fix_up: sys.stderr.write( "Multiple installation rules found for %s." % (basename)) sys.exit(1) - libs[basename] = (dst_full, installed) + files_to_fix_up[basename] = (dst_full, installed) else: # It is not a library, it may be an executable. - if is_binary_executable(dst_full): # It is an executable. - libs[dst_full] = (dst_full, installed) - else: # Neither a library nor an executable. + # Do not check files with the following extensions (over 100 files + # with each extension, and we are certain these are not executables). + if (installed and + not dst_full.endswith(".h") and + not dst_full.endswith(".py") and + not dst_full.endswith(".obj") and + not dst_full.endswith(".cmake") and + not dst_full.endswith(".1") and + not dst_full.endswith(".hpp") and + not dst_full.endswith(".txt")): + potential_binaries.append(dst_full) + else: pass def fix_rpaths(): + # Add binary executables to list of files to be fixed up: + find_binary_executables() # Only fix files that are installed now. - fix_libs = [(k, libs[k][0]) for k in libs.keys() if libs[k][1]] - for basename, dst_full in fix_libs: + fix_files = [(k, files_to_fix_up[k][0]) + for k in files_to_fix_up.keys() if files_to_fix_up[k][1]] + for basename, dst_full in fix_files: # Enable write permissions to allow modification. os.chmod(dst_full, stat.S_IRUSR | stat.S_IWUSR | stat.S_IXUSR | stat.S_IRGRP | stat.S_IXGRP | stat.S_IROTH | stat.S_IXOTH) @@ -135,10 +143,11 @@ def macos_fix_rpaths(basename, dst_full): dep_basename = os.path.basename(relative_path) # Look for the absolute path in the dictionary of fixup files to # find library paths. - if dep_basename not in libs.keys(): + if dep_basename not in files_to_fix_up.keys(): continue lib_dirname = os.path.dirname(dst_full) - diff_path = os.path.relpath(libs[dep_basename][0], lib_dirname) + diff_path = os.path.relpath(files_to_fix_up[dep_basename][0], + lib_dirname) check_call( ['install_name_tool', "-change", relative_path, @@ -170,18 +179,18 @@ def linux_fix_rpaths(dst_full): ldd_result = line.strip().split(' => ') # If library found and not in install prefix, then skip. if len(ldd_result) < 2 or \ - not (ldd_result[1] == "not found" - or ldd_result[1].startswith(prefix)): + not (ldd_result[1] == "not found" or + ldd_result[1].startswith(prefix)): continue re_result = re.match(dylib_match, ldd_result[0]) # Look for the absolute path in the dictionary of libraries using the # library name without its possible version number. soname, _, _ = re_result.groups() - if soname not in libs.keys(): + if soname not in files_to_fix_up.keys(): continue lib_dirname = os.path.dirname(dst_full) diff_path = os.path.dirname( - os.path.relpath(libs[soname][0], lib_dirname) + os.path.relpath(files_to_fix_up[soname][0], lib_dirname) ) rpath.append('$ORIGIN' + '/' + diff_path) From f36a580d4dc8a610e8d81a9a0a061e9a875d9bd4 Mon Sep 17 00:00:00 2001 From: Eric Cousineau Date: Wed, 3 Jan 2018 12:45:15 -0500 Subject: [PATCH 17/25] show_images: Support uncompressed data. --- systems/sensors/visualization/show_images.py | 23 +++++++++++++------- 1 file changed, 15 insertions(+), 8 deletions(-) diff --git a/systems/sensors/visualization/show_images.py b/systems/sensors/visualization/show_images.py index 316bf0e825cf..1a8b1ced44fd 100755 --- a/systems/sensors/visualization/show_images.py +++ b/systems/sensors/visualization/show_images.py @@ -338,7 +338,6 @@ def decode_image_t(msg, image_in=None): rli = rl.image_t w = msg.width h = msg.height - assert msg.compression_method == rli.COMPRESSION_METHOD_ZLIB pixel_desc = (msg.pixel_format, msg.channel_type) if pixel_desc == (rli.PIXEL_FORMAT_RGBA, rli.CHANNEL_TYPE_UINT8): num_channels = 4 @@ -353,14 +352,22 @@ def decode_image_t(msg, image_in=None): raise RuntimeError("Unsupported pixel type: {}".format(pixel_desc)) bytes_per_pixel = np.dtype(dtype).itemsize * num_channels assert msg.row_stride == msg.width * bytes_per_pixel - + if msg.compression_method == rli.COMPRESSION_METHOD_NOT_COMPRESSED: + data_bytes = msg.data + elif msg.compression_method == rli.COMPRESSION_METHOD_ZLIB: + # TODO(eric.cousineau): Consider using `data`s buffer, if possible. + # Can decompress() somehow use an existing buffer in Python? + data_bytes = zlib.decompress(msg.data) + else: + raise RuntimeError( + "Unsupported compression type: {}".format(msg.compression_method)) + # Cast to desired type and shape. + data = np.frombuffer(data_bytes, dtype=dtype) + data.shape = (h, w, num_channels) + # Copy data to VTK image. image = create_image_if_needed(w, h, num_channels, dtype, image_in) - data = vtk_image_to_numpy(image) - # TODO(eric.cousineau): Consider using `data`s buffer, if possible. - # Can decompress() somehow use an existing buffer in Python? - data_in = np.frombuffer(zlib.decompress(msg.data), dtype=dtype) - data_in.shape = (h, w, num_channels) - data[:] = data_in[:] + image_data = vtk_image_to_numpy(image) + image_data[:] = data[:] return image From e2379be51a42fb854cb353dea1f6717b8732b5b9 Mon Sep 17 00:00:00 2001 From: Eric Cousineau Date: Wed, 3 Jan 2018 12:50:11 -0500 Subject: [PATCH 18/25] pydrake: Rename pybind utility headers. --- bindings/pydrake/BUILD.bazel | 24 ++++++++++++------- ...iff_types_py.h => autodiff_types_pybind.h} | 0 bindings/pydrake/autodiffutils_py.cc | 2 +- bindings/pydrake/rbtree_py.cc | 2 +- bindings/pydrake/solvers/BUILD.bazel | 2 +- .../pydrake/solvers/mathematicalprogram_py.cc | 2 +- bindings/pydrake/symbolic_py.cc | 2 +- ...lic_types_py.h => symbolic_types_pybind.h} | 0 bindings/pydrake/test/odr_test_module_py.cc | 2 +- 9 files changed, 22 insertions(+), 14 deletions(-) rename bindings/pydrake/{autodiff_types_py.h => autodiff_types_pybind.h} (100%) rename bindings/pydrake/{symbolic_types_py.h => symbolic_types_pybind.h} (100%) diff --git a/bindings/pydrake/BUILD.bazel b/bindings/pydrake/BUILD.bazel index c96951aaf4b9..b9c65ffc342c 100644 --- a/bindings/pydrake/BUILD.bazel +++ b/bindings/pydrake/BUILD.bazel @@ -25,6 +25,14 @@ package(default_visibility = adjust_labels_for_drake_hoist([ # N.B. Any common headers should be shared via a HEADER ONLY library. +# Target Naming Conventions: +# `*_py` +# A Python library (can be pure Python or pybind) +# Files: `*.py`, `*_py.cc` +# `*_pybind` +# A C++ library for adding pybind-specific utilities to be consumed by C++. +# Files: `*_pybind.{h,cc}` + # This is a pure-python, standalone library. # Keep this away from `common_py` to simplify test dependencies. drake_py_library( @@ -47,8 +55,8 @@ drake_pybind_library( # TODO(eric.cousineau): Ensure that this is not installed. drake_cc_library( - name = "autodiff_types", - hdrs = ["autodiff_types_py.h"], + name = "autodiff_types_pybind", + hdrs = ["autodiff_types_pybind.h"], # NOTE: We must violate IWYU and not list "autodiff" in "deps" here because # we do not want these dependencies leaking in at install time. ) @@ -56,7 +64,7 @@ drake_cc_library( drake_pybind_library( name = "autodiffutils_py", cc_deps = [ - ":autodiff_types", + ":autodiff_types_pybind", ], cc_srcs = ["autodiffutils_py.cc"], py_deps = [ @@ -71,7 +79,7 @@ drake_pybind_library( drake_pybind_library( name = "rbtree_py", cc_deps = [ - ":autodiff_types", + ":autodiff_types_pybind", ], cc_srcs = ["rbtree_py.cc"], py_deps = [ @@ -93,14 +101,14 @@ drake_pybind_library( # TODO(eric.cousineau): Make private. drake_cc_library( - name = "symbolic_types_py_h", - hdrs = ["symbolic_types_py.h"], + name = "symbolic_types_pybind", + hdrs = ["symbolic_types_pybind.h"], ) drake_pybind_library( name = "symbolic_py", cc_deps = [ - ":symbolic_types_py_h", + ":symbolic_types_pybind", ], cc_srcs = ["symbolic_py.cc"], py_deps = [ @@ -143,7 +151,7 @@ drake_pybind_library( testonly = 1, add_install = False, cc_deps = [ - ":symbolic_types_py_h", + ":symbolic_types_pybind", ], cc_so_name = "test/odr_test_module", cc_srcs = ["test/odr_test_module_py.cc"], diff --git a/bindings/pydrake/autodiff_types_py.h b/bindings/pydrake/autodiff_types_pybind.h similarity index 100% rename from bindings/pydrake/autodiff_types_py.h rename to bindings/pydrake/autodiff_types_pybind.h diff --git a/bindings/pydrake/autodiffutils_py.cc b/bindings/pydrake/autodiffutils_py.cc index cb668b01f2db..5fc59a7efaff 100644 --- a/bindings/pydrake/autodiffutils_py.cc +++ b/bindings/pydrake/autodiffutils_py.cc @@ -2,7 +2,7 @@ #include #include -#include "drake/bindings/pydrake/autodiff_types_py.h" +#include "drake/bindings/pydrake/autodiff_types_pybind.h" namespace py = pybind11; diff --git a/bindings/pydrake/rbtree_py.cc b/bindings/pydrake/rbtree_py.cc index 5af0f73f361a..d296d091578d 100644 --- a/bindings/pydrake/rbtree_py.cc +++ b/bindings/pydrake/rbtree_py.cc @@ -4,7 +4,7 @@ #include #include -#include "drake/bindings/pydrake/autodiff_types_py.h" +#include "drake/bindings/pydrake/autodiff_types_pybind.h" #include "drake/multibody/parsers/package_map.h" #include "drake/multibody/parsers/urdf_parser.h" #include "drake/multibody/rigid_body_tree.h" diff --git a/bindings/pydrake/solvers/BUILD.bazel b/bindings/pydrake/solvers/BUILD.bazel index ee6d54aede8d..25bd67fedd56 100644 --- a/bindings/pydrake/solvers/BUILD.bazel +++ b/bindings/pydrake/solvers/BUILD.bazel @@ -43,7 +43,7 @@ drake_pybind_library( drake_pybind_library( name = "mathematicalprogram_py", cc_deps = [ - "//drake/bindings/pydrake:symbolic_types_py_h", + "//drake/bindings/pydrake:symbolic_types_pybind", ], cc_srcs = ["mathematicalprogram_py.cc"], py_deps = [ diff --git a/bindings/pydrake/solvers/mathematicalprogram_py.cc b/bindings/pydrake/solvers/mathematicalprogram_py.cc index 81789839e9f1..f0ad968d6914 100644 --- a/bindings/pydrake/solvers/mathematicalprogram_py.cc +++ b/bindings/pydrake/solvers/mathematicalprogram_py.cc @@ -5,7 +5,7 @@ #include #include -#include "drake/bindings/pydrake/symbolic_types_py.h" +#include "drake/bindings/pydrake/symbolic_types_pybind.h" #include "drake/solvers/mathematical_program.h" #include "drake/solvers/solver_type_converter.h" diff --git a/bindings/pydrake/symbolic_py.cc b/bindings/pydrake/symbolic_py.cc index b0b9a1797f69..2c5de89daafd 100644 --- a/bindings/pydrake/symbolic_py.cc +++ b/bindings/pydrake/symbolic_py.cc @@ -2,7 +2,7 @@ #include #include -#include "drake/bindings/pydrake/symbolic_types_py.h" +#include "drake/bindings/pydrake/symbolic_types_pybind.h" namespace py = pybind11; diff --git a/bindings/pydrake/symbolic_types_py.h b/bindings/pydrake/symbolic_types_pybind.h similarity index 100% rename from bindings/pydrake/symbolic_types_py.h rename to bindings/pydrake/symbolic_types_pybind.h diff --git a/bindings/pydrake/test/odr_test_module_py.cc b/bindings/pydrake/test/odr_test_module_py.cc index 714fc1ef38b5..26ecdd26f929 100644 --- a/bindings/pydrake/test/odr_test_module_py.cc +++ b/bindings/pydrake/test/odr_test_module_py.cc @@ -1,6 +1,6 @@ #include -#include "drake/bindings/pydrake/symbolic_types_py.h" +#include "drake/bindings/pydrake/symbolic_types_pybind.h" namespace py = pybind11; From 5517cfabff91362a3df4bda682b61833c542ac0a Mon Sep 17 00:00:00 2001 From: Hongkai Dai Date: Wed, 3 Jan 2018 10:28:34 -0800 Subject: [PATCH 19/25] Add WithVariable to MathematicalProgram (#7666) * Add function WithVariable to MathematicalProgram class. * WIP: add WithIndeterminates. * Add tests for WithIndeterminates. * Address Eric's comments. * Address Grant's comments. * lint. * Address Eric's comments. * address Eric's comment. --- solvers/mathematical_program.cc | 72 +++++++++- solvers/mathematical_program.h | 31 ++++- solvers/test/mathematical_program_test.cc | 152 ++++++++++++++++++++-- 3 files changed, 233 insertions(+), 22 deletions(-) diff --git a/solvers/mathematical_program.cc b/solvers/mathematical_program.cc index bd6ec06a5ed4..39f9348fdc42 100644 --- a/solvers/mathematical_program.cc +++ b/solvers/mathematical_program.cc @@ -13,6 +13,9 @@ #include #include +#include +#include + #include "drake/common/eigen_types.h" #include "drake/common/symbolic.h" #include "drake/math/matrix_util.h" @@ -149,6 +152,37 @@ MatrixXDecisionVariable MathematicalProgram::NewSymmetricContinuousVariables( return NewVariables(VarType::CONTINUOUS, rows, rows, true, names); } +void MathematicalProgram::AddDecisionVariables( + const Eigen::Ref& decision_variables) { + const int num_existing_decision_vars = num_vars(); + for (int i = 0; i < decision_variables.rows(); ++i) { + if (decision_variables(i).is_dummy()) { + throw std::runtime_error(fmt::format( + "decision_variables({}) should not be a dummy variable", i)); + } + if (decision_variable_index_.find(decision_variables(i).get_id()) != + decision_variable_index_.end()) { + throw std::runtime_error(fmt::format("{} is already a decision variable.", + decision_variables(i))); + } + if (indeterminates_index_.find(decision_variables(i).get_id()) != + indeterminates_index_.end()) { + throw std::runtime_error(fmt::format("{} is already an indeterminate.", + decision_variables(i))); + } + decision_variable_index_.insert(std::make_pair( + decision_variables(i).get_id(), num_existing_decision_vars + i)); + } + decision_variables_.conservativeResize(num_existing_decision_vars + + decision_variables.rows()); + decision_variables_.tail(decision_variables.rows()) = decision_variables; + x_values_.resize(num_existing_decision_vars + decision_variables.rows(), NAN); + x_initial_guess_.conservativeResize(num_existing_decision_vars + + decision_variables.rows()); + x_initial_guess_.tail(decision_variables.rows()) + .fill(std::numeric_limits::quiet_NaN()); +} + symbolic::Polynomial MathematicalProgram::NewFreePolynomial( const Variables& indeterminates, const int degree, const string& coeff_name) { @@ -216,6 +250,34 @@ MatrixXIndeterminate MathematicalProgram::NewIndeterminates( return NewIndeterminates(rows, cols, names); } +void MathematicalProgram::AddIndeterminates( + const Eigen::Ref& new_indeterminates) { + const int num_old_indeterminates = num_indeterminates(); + for (int i = 0; i < new_indeterminates.rows(); ++i) { + if (new_indeterminates(i).is_dummy()) { + throw std::runtime_error(fmt::format( + "new_indeterminates({}) should not be a dummy variable.", i)); + } + if (indeterminates_index_.find(new_indeterminates(i).get_id()) != + indeterminates_index_.end() || + decision_variable_index_.find(new_indeterminates(i).get_id()) != + decision_variable_index_.end()) { + throw std::runtime_error( + fmt::format("{} already exists in the optimization program.", + new_indeterminates(i))); + } + if (new_indeterminates(i).get_type() != + symbolic::Variable::Type::CONTINUOUS) { + throw std::runtime_error("indeterminate should of type CONTINUOUS.\n"); + } + indeterminates_index_.insert(std::make_pair(new_indeterminates(i).get_id(), + num_old_indeterminates + i)); + } + indeterminates_.conservativeResize(num_old_indeterminates + + new_indeterminates.rows()); + indeterminates_.tail(new_indeterminates.rows()) = new_indeterminates; +} + namespace { template @@ -379,7 +441,7 @@ Binding MathematicalProgram::AddConstraint( // TODO(eric.cousineau): This is a good assertion... But seems out of place, // possibly redundant w.r.t. the binding infrastructure. DRAKE_ASSERT(binding.constraint()->A().cols() == - static_cast(binding.GetNumElements())); + static_cast(binding.GetNumElements())); CheckBinding(binding); required_capabilities_ |= kLinearConstraint; linear_constraints_.push_back(binding); @@ -398,7 +460,7 @@ Binding MathematicalProgram::AddLinearConstraint( Binding MathematicalProgram::AddConstraint( const Binding& binding) { DRAKE_ASSERT(binding.constraint()->A().cols() == - static_cast(binding.GetNumElements())); + static_cast(binding.GetNumElements())); CheckBinding(binding); required_capabilities_ |= kLinearEqualityConstraint; linear_equality_constraints_.push_back(binding); @@ -433,7 +495,7 @@ Binding MathematicalProgram::AddConstraint( const Binding& binding) { CheckBinding(binding); DRAKE_ASSERT(binding.constraint()->num_constraints() == - binding.GetNumElements()); + binding.GetNumElements()); required_capabilities_ |= kLinearConstraint; bbox_constraints_.push_back(binding); return bbox_constraints_.back(); @@ -582,7 +644,7 @@ Binding MathematicalProgram::AddConstraint( const Binding& binding) { CheckBinding(binding); DRAKE_ASSERT(static_cast(binding.constraint()->F().size()) == - static_cast(binding.GetNumElements()) + 1); + static_cast(binding.GetNumElements()) + 1); required_capabilities_ |= kPositiveSemidefiniteConstraint; linear_matrix_inequality_constraint_.push_back(binding); return linear_matrix_inequality_constraint_.back(); @@ -662,7 +724,7 @@ SolutionResult MathematicalProgram::Solve() { nlopt_solver_->available()) { return nlopt_solver_->Solve(*this); } else if (is_satisfied(required_capabilities_, kScsCapabilities) && - scs_solver_->available()) { + scs_solver_->available()) { // Use SCS as the last resort. SCS uses ADMM method, which converges fast to // modest accuracy quite fast, but then slows down significantly if the user // wants high accuracy. diff --git a/solvers/mathematical_program.h b/solvers/mathematical_program.h index c73259bae6c1..93bbd109484d 100644 --- a/solvers/mathematical_program.h +++ b/solvers/mathematical_program.h @@ -355,8 +355,8 @@ class MathematicalProgram { * readability. */ template - MatrixDecisionVariable - NewContinuousVariables(int rows, int cols, const std::string& name) { + MatrixDecisionVariable NewContinuousVariables( + int rows, int cols, const std::string& name) { rows = Rows == Eigen::Dynamic ? rows : Rows; cols = Cols == Eigen::Dynamic ? cols : Cols; auto names = @@ -425,8 +425,8 @@ class MathematicalProgram { * readability. */ template - MatrixDecisionVariable - NewBinaryVariables(int rows, int cols, const std::string& name) { + MatrixDecisionVariable NewBinaryVariables( + int rows, int cols, const std::string& name) { rows = Rows == Eigen::Dynamic ? rows : Rows; cols = Cols == Eigen::Dynamic ? cols : Cols; auto names = @@ -515,6 +515,16 @@ class MathematicalProgram { return NewSymmetricVariables(VarType::CONTINUOUS, names); } + /** Appends new variables to the end of the existing variables. + * @param decision_variables The newly added decision_variables. + * @pre `decision_variables` should not intersect with the existing variables + * or indeterminates in the optimization program. + * @pre Each entry in `decision_variables` should not be a dummy variable. + * @throw runtime_error if the preconditions are not satisfied. + */ + void AddDecisionVariables( + const Eigen::Ref& decision_variables); + /** * Returns a free polynomial in a monomial basis over @p indeterminates of a * given @p degree. It uses @p coeff_name to make new decision variables and @@ -690,6 +700,19 @@ class MathematicalProgram { MatrixXIndeterminate NewIndeterminates(int rows, int cols, const std::string& name = "X"); + /** Adds indeterminates. + * This method appends some indeterminates to the end of the program's old + * indeterminates. + * @param new_indeterminates The indeterminates to be appended to the + * program's old indeterminates. + * @pre `new_indeterminates` should not intersect with the program's old + * indeterminates or decision variables. + * @pre Each entry in new_indeterminates should not be dummy. + * @pre Each entry in new_indeterminates should be of CONTINUOUS type. + */ + void AddIndeterminates( + const Eigen::Ref& new_indeterminates); + /** * Adds a generic cost to the optimization program. */ diff --git a/solvers/test/mathematical_program_test.cc b/solvers/test/mathematical_program_test.cc index 2e3fb0f3bd2d..cba72e550196 100644 --- a/solvers/test/mathematical_program_test.cc +++ b/solvers/test/mathematical_program_test.cc @@ -385,6 +385,78 @@ GTEST_TEST(testAddVariable, testAddBinaryVariable8) { false, MathematicalProgram::VarType::BINARY); } +GTEST_TEST(testAddDecisionVariables, AddDecisionVariables1) { + // Call AddVariable on an empty program. + MathematicalProgram prog; + const Variable x0("x0", Variable::Type::CONTINUOUS); + const Variable x1("x1", Variable::Type::CONTINUOUS); + const Variable x2("x2", Variable::Type::BINARY); + prog.AddDecisionVariables(VectorDecisionVariable<3>(x0, x1, x2)); + EXPECT_EQ(prog.num_vars(), 3); + EXPECT_EQ(prog.FindDecisionVariableIndex(x0), 0); + EXPECT_EQ(prog.FindDecisionVariableIndex(x1), 1); + EXPECT_EQ(prog.FindDecisionVariableIndex(x2), 2); + EXPECT_EQ(prog.initial_guess().rows(), 3); + EXPECT_EQ(prog.decision_variables().rows(), 3); + const VectorDecisionVariable<3> vars_expected(x0, x1, x2); + prog.SetDecisionVariableValues(Vector3::Zero()); + for (int i = 0; i < 3; ++i) { + EXPECT_EQ(prog.GetSolution(vars_expected(i)), 0); + EXPECT_TRUE(prog.decision_variables()(i).equal_to(vars_expected(i))); + } +} + +GTEST_TEST(testAddDecisionVariables, AddVariable2) { + // Call AddDecisionVariables on a program that has some existing variables. + MathematicalProgram prog; + auto y = prog.NewContinuousVariables<3>("y"); + const Variable x0("x0", Variable::Type::CONTINUOUS); + const Variable x1("x1", Variable::Type::CONTINUOUS); + const Variable x2("x2", Variable::Type::BINARY); + prog.AddDecisionVariables(VectorDecisionVariable<3>(x0, x1, x2)); + EXPECT_EQ(prog.num_vars(), 6); + EXPECT_EQ(prog.FindDecisionVariableIndex(x0), 3); + EXPECT_EQ(prog.FindDecisionVariableIndex(x1), 4); + EXPECT_EQ(prog.FindDecisionVariableIndex(x2), 5); + EXPECT_EQ(prog.initial_guess().rows(), 6); + prog.SetDecisionVariableValues(Vector6::Zero()); + VectorDecisionVariable<6> vars_expected; + vars_expected << y, x0, x1, x2; + for (int i = 0; i < 6; ++i) { + EXPECT_EQ(prog.GetSolution(vars_expected(i)), 0); + EXPECT_TRUE(prog.decision_variables()(i).equal_to(vars_expected(i))); + } +} + +GTEST_TEST(testAddDecisionVariables, AddVariable3) { + // Test the error inputs. + MathematicalProgram prog; + auto y = prog.NewContinuousVariables<3>("y"); + const Variable x0("x0", Variable::Type::CONTINUOUS); + const Variable x1("x1", Variable::Type::CONTINUOUS); + // Call AddDecisionVariables on a program that has some existing variables, + // and the + // new variables intersects with the existing variables. + EXPECT_THROW( + prog.AddDecisionVariables(VectorDecisionVariable<3>(x0, x1, y(0))), + std::runtime_error); + // The newly added variables have duplicated entries. + EXPECT_THROW(prog.AddDecisionVariables(VectorDecisionVariable<3>(x0, x1, x0)), + std::runtime_error); + // The newly added variables contain a dummy variable. + Variable dummy; + EXPECT_TRUE(dummy.is_dummy()); + EXPECT_THROW( + prog.AddDecisionVariables(VectorDecisionVariable<3>(x0, x1, dummy)), + std::runtime_error); + auto z = prog.NewIndeterminates<2>("z"); + // Call AddDecisionVariables on a program that has some indeterminates, and + // the new + // variables intersects with the indeterminates. + EXPECT_THROW(prog.AddDecisionVariables(VectorDecisionVariable<2>(x0, z(0))), + std::runtime_error); +} + GTEST_TEST(testAddIndeterminates, testAddIndeterminates1) { // Adds a dynamic-sized matrix of Indeterminates. MathematicalProgram prog; @@ -447,6 +519,61 @@ CheckGetSolution(const MathematicalProgram& prog, } } +GTEST_TEST(testAddIndeterminates, AddIndeterminates1) { + // Call AddIndeterminates on an empty program. + MathematicalProgram prog; + const Variable x0("x0", Variable::Type::CONTINUOUS); + const Variable x1("x1", Variable::Type::CONTINUOUS); + const Variable x2("x2", Variable::Type::CONTINUOUS); + prog.AddIndeterminates(VectorIndeterminate<3>(x0, x1, x2)); + const VectorIndeterminate<3> indeterminates_expected(x0, x1, x2); + EXPECT_EQ(prog.indeterminates().rows(), 3); + for (int i = 0; i < 3; ++i) { + EXPECT_TRUE(prog.indeterminates()(i).equal_to(indeterminates_expected(i))); + EXPECT_EQ(prog.FindIndeterminateIndex(indeterminates_expected(i)), i); + } +} + +GTEST_TEST(testAddIndeterminates, AddIndeterminates2) { + // Call AddIndeterminates on a program with some indeterminates. + MathematicalProgram prog; + auto y = prog.NewIndeterminates<2>("y"); + const Variable x0("x0", Variable::Type::CONTINUOUS); + const Variable x1("x1", Variable::Type::CONTINUOUS); + const Variable x2("x2", Variable::Type::CONTINUOUS); + prog.AddIndeterminates(VectorIndeterminate<3>(x0, x1, x2)); + VectorIndeterminate<5> indeterminates_expected; + indeterminates_expected << y(0), y(1), x0, x1, x2; + EXPECT_EQ(prog.indeterminates().rows(), 5); + for (int i = 0; i < 5; ++i) { + EXPECT_TRUE(prog.indeterminates()(i).equal_to(indeterminates_expected(i))); + EXPECT_EQ(prog.FindIndeterminateIndex(indeterminates_expected(i)), i); + } +} + +GTEST_TEST(testAddIndeterminates, AddIndeterminates3) { + // Call with erroneous inputs. + MathematicalProgram prog; + auto x = prog.NewContinuousVariables<2>("x"); + auto y = prog.NewIndeterminates<2>("y"); + const Variable x0("x0", Variable::Type::CONTINUOUS); + const Variable x1("x1", Variable::Type::BINARY); + // Call AddIndeterminates with a input that intersects with old + // indeterminates. + EXPECT_THROW(prog.AddIndeterminates(VectorIndeterminate<2>(x0, y(0))), + std::runtime_error); + // Call AddIndeterminates with a input that intersects with old decision + // variables. + EXPECT_THROW(prog.AddIndeterminates(VectorIndeterminate<2>(x0, x(0))), + std::runtime_error); + // Call AddIndeterminates with a input of type BINARY. + EXPECT_THROW(prog.AddIndeterminates(VectorIndeterminate<2>(x0, x1)), + std::runtime_error); + // Call AddIndeterminates with a dummy variable. + Variable dummy; + EXPECT_THROW(prog.AddIndeterminates(VectorIndeterminate<2>(x0, dummy)), + std::runtime_error); +} GTEST_TEST(testGetSolution, testSetSolution1) { // Tests setting and getting solution for // 1. A static-sized matrix of decision variables. @@ -506,8 +633,7 @@ void ExpectBadVar(MathematicalProgram* prog, int num_var, Args&&... args) { using internal::CreateBinding; auto c = make_shared(std::forward(args)...); VectorXDecisionVariable x(num_var); - for (int i = 0; i < num_var; ++i) - x(i) = Variable("bad" + std::to_string(i)); + for (int i = 0; i < num_var; ++i) x(i) = Variable("bad" + std::to_string(i)); // Use minimal call site (directly on adding Binding). // TODO(eric.cousineau): Check if there is a way to parse the error text to // ensure that we are capturing the correct error. @@ -1984,11 +2110,11 @@ TEST_F(SymbolicLorentzConeTest, TestError) { // The quadratic expression is actually affine. EXPECT_THROW(prog_.AddLorentzConeConstraint(2 * x_(0), 3 * x_(1) + 2), runtime_error); - EXPECT_THROW( - prog_.AddLorentzConeConstraint( - 2 * x_(0), x_(1) * x_(1) - (x_(1) - x_(0)) * (x_(1) + x_(0)) - - x_(0) * x_(0) + 2 * x_(1) + 3), - runtime_error); + EXPECT_THROW(prog_.AddLorentzConeConstraint( + 2 * x_(0), + x_(1) * x_(1) - (x_(1) - x_(0)) * (x_(1) + x_(0)) - + x_(0) * x_(0) + 2 * x_(1) + 3), + runtime_error); // The Hessian matrix is not positive semidefinite. EXPECT_THROW(prog_.AddLorentzConeConstraint(2 * x_(0) + 3, @@ -2012,10 +2138,11 @@ TEST_F(SymbolicLorentzConeTest, TestError) { runtime_error); // The quadratic expression is a negative constant. - EXPECT_THROW(prog_.AddLorentzConeConstraint( - 2 * x_(0) + 3, pow(x_(0), 2) - pow(x_(1), 2) - - (x_(0) + x_(1)) * (x_(0) - x_(1)) - 1), - runtime_error); + EXPECT_THROW( + prog_.AddLorentzConeConstraint(2 * x_(0) + 3, + pow(x_(0), 2) - pow(x_(1), 2) - + (x_(0) + x_(1)) * (x_(0) - x_(1)) - 1), + runtime_error); // The first expression is not actually linear. EXPECT_THROW(prog_.AddLorentzConeConstraint(2 * x_(0) * x_(1), pow(x_(0), 2)), @@ -2101,8 +2228,7 @@ GTEST_TEST(testMathematicalProgram, AddSymbolicRotatedLorentzConeConstraint5) { symbolic::Polynomial(z(1)), tol)); EXPECT_TRUE(symbolic::test::PolynomialEqual( symbolic::Polynomial(quadratic_expression), - symbolic::Polynomial(z.tail(z.rows() - 2).squaredNorm()), - tol)); + symbolic::Polynomial(z.tail(z.rows() - 2).squaredNorm()), tol)); } namespace { From f6f23c5bc539a6aaf754c27b69ef14a69ab3430f Mon Sep 17 00:00:00 2001 From: Michael Sherman Date: Wed, 3 Jan 2018 11:51:21 -0800 Subject: [PATCH 20/25] Restrict fixed input ports to use the FixInputPort() API (#7686) Restrict fixed input ports to use the FixInputPort() API. Hide SetInputPortValue() method. --- automotive/maliput_railcar.h | 2 +- automotive/simple_car.cc | 8 +-- bindings/pydrake/systems/framework_py.cc | 14 +++- .../schunk_wsg/test/schunk_wsg_lcm_test.cc | 7 +- multibody/multibody_doxygen.h | 2 +- .../test/drake_visualizer_test.cc | 5 +- .../test/frame_visualizer_test.cc | 4 +- systems/framework/context.h | 68 ++++++++++--------- systems/framework/diagram_context.h | 24 +++---- systems/framework/input_port_value.h | 24 +------ systems/framework/leaf_context.h | 15 ++-- .../framework/test/input_port_value_test.cc | 3 +- systems/framework/test/system_test.cc | 6 +- systems/lcm/test/lcm_publisher_system_test.cc | 5 +- systems/primitives/linear_system.cc | 4 +- .../direct_collocation.cc | 12 ++-- .../direct_transcription.cc | 6 +- 17 files changed, 90 insertions(+), 119 deletions(-) diff --git a/automotive/maliput_railcar.h b/automotive/maliput_railcar.h index ecbe648c41c3..abf0b790e7db 100644 --- a/automotive/maliput_railcar.h +++ b/automotive/maliput_railcar.h @@ -105,7 +105,7 @@ class MaliputRailcar : public systems::LeafSystem { /// Sets `railcar_state` to contain the default state for MaliputRailcar. static void SetDefaultState(MaliputRailcarState* railcar_state); - /// Returns a mutable pointer to the parameters in the given @p context. + /// Returns a mutable reference to the parameters in the given @p context. MaliputRailcarParams& get_mutable_parameters( systems::Context* context) const; diff --git a/automotive/simple_car.cc b/automotive/simple_car.cc index c8bbed8b7553..58845372f591 100644 --- a/automotive/simple_car.cc +++ b/automotive/simple_car.cc @@ -48,11 +48,9 @@ const DrivingCommand& get_input(const SimpleCar* simple_car, // Obtain our parameters from a context. template const SimpleCarParams& get_params(const systems::Context& context) { - const SimpleCarParams* const params = - dynamic_cast*>( - &context.get_numeric_parameter(0)); - DRAKE_DEMAND(params); - return *params; + const SimpleCarParams& params = + dynamic_cast&>(context.get_numeric_parameter(0)); + return params; } } // namespace diff --git a/bindings/pydrake/systems/framework_py.cc b/bindings/pydrake/systems/framework_py.cc index 0a84b74635e8..bfede975bf6a 100644 --- a/bindings/pydrake/systems/framework_py.cc +++ b/bindings/pydrake/systems/framework_py.cc @@ -28,6 +28,10 @@ PYBIND11_MODULE(framework, m) { // NOLINTNEXTLINE(build/namespaces): Emulate placement in namespace. using namespace drake::systems; + // TODO(eric.cousineau): pybind11 defaults to C++-like copies when dealing + // with rvalues. We should wrap this into a drake-level binding, so that we + // can default this to `reference` or `reference_internal.` + // Aliases for commonly used return value policies. // `py_ref` is used when `keep_alive` is explicitly used (e.g. for extraction // methods, like `GetMutableSubsystemState`). @@ -123,7 +127,7 @@ PYBIND11_MODULE(framework, m) { .def("get_num_input_ports", &Context::get_num_input_ports) .def("FixInputPort", py::overload_cast>>( - &Context::FixInputPort)) + &Context::FixInputPort), py_iref) .def("get_time", &Context::get_time) .def("Clone", &Context::Clone) .def("__copy__", &Context::Clone) @@ -152,11 +156,15 @@ PYBIND11_MODULE(framework, m) { .def("Connect", py::overload_cast&, const InputPortDescriptor&>( &DiagramBuilder::Connect)) - .def("ExportInput", &DiagramBuilder::ExportInput) - .def("ExportOutput", &DiagramBuilder::ExportOutput) + .def("ExportInput", &DiagramBuilder::ExportInput, py_iref) + .def("ExportOutput", &DiagramBuilder::ExportOutput, py_iref) .def("Build", &DiagramBuilder::Build) .def("BuildInto", &DiagramBuilder::BuildInto); + // TODO(eric.cousineau): Figure out how to handle template-specialized method + // signatures(e.g. GetValue()). + py::class_(m, "FreestandingInputPortValue"); + py::class_>(m, "OutputPort"); py::class_>(m, "SystemOutput") diff --git a/manipulation/schunk_wsg/test/schunk_wsg_lcm_test.cc b/manipulation/schunk_wsg/test/schunk_wsg_lcm_test.cc index e41d559e0fb4..90b7c32081db 100644 --- a/manipulation/schunk_wsg/test/schunk_wsg_lcm_test.cc +++ b/manipulation/schunk_wsg/test/schunk_wsg_lcm_test.cc @@ -26,11 +26,8 @@ GTEST_TEST(SchunkWsgLcmTest, SchunkWsgTrajectoryGeneratorTest) { lcmt_schunk_wsg_command initial_command{}; initial_command.target_position_mm = 100; initial_command.force = 40; - std::unique_ptr input_command = - std::make_unique( - std::make_unique>( - initial_command)); - context->SetInputPortValue(0, std::move(input_command)); + context->FixInputPort(0, + systems::AbstractValue::Make(initial_command)); context->FixInputPort(1, Eigen::VectorXd::Zero(1)); // Step a little bit. We should be commanding a point on the diff --git a/multibody/multibody_doxygen.h b/multibody/multibody_doxygen.h index b1af934d6e94..dd775d8806be 100644 --- a/multibody/multibody_doxygen.h +++ b/multibody/multibody_doxygen.h @@ -234,7 +234,7 @@ When we speak of the "location" or "pose" of a body, we really mean the location of the body frame origin Bo or pose of the body frame B, respectively. Body properties such as inertia and geometry are given with respect to the body frame. We denote -a body's center of mass point as @f$B_{cm}@f$ (`Bcm`); it's location on the body +a body's center of mass point as @f$B_{cm}@f$ (`Bcm`); its location on the body is specified by a position vector from Bo to Bcm. For a rigid body, any frames attached to it are fixed with respect to the body frame. For a flexible body, deformations are measured with respect to the body frame. diff --git a/multibody/rigid_body_plant/test/drake_visualizer_test.cc b/multibody/rigid_body_plant/test/drake_visualizer_test.cc index 9cd846be72e6..5a48b184d0e5 100644 --- a/multibody/rigid_body_plant/test/drake_visualizer_test.cc +++ b/multibody/rigid_body_plant/test/drake_visualizer_test.cc @@ -477,10 +477,7 @@ GTEST_TEST(DrakeVisualizerTests, BasicTest) { tree->get_num_positions() + tree->get_num_velocities(); auto input_data = make_unique>(vector_size); input_data->set_value(Eigen::VectorXd::Zero(vector_size)); - - context->SetInputPortValue( - 0, std::make_unique( - std::move(input_data))); + context->FixInputPort(0, std::move(input_data)); // Publishes the `RigidBodyTree` visualization messages. PublishLoadRobotModelMessageHelper(dut, *context); diff --git a/multibody/rigid_body_plant/test/frame_visualizer_test.cc b/multibody/rigid_body_plant/test/frame_visualizer_test.cc index 7507b3baf855..616d9675a0d9 100644 --- a/multibody/rigid_body_plant/test/frame_visualizer_test.cc +++ b/multibody/rigid_body_plant/test/frame_visualizer_test.cc @@ -46,9 +46,7 @@ GTEST_TEST(FrameVisualizerTests, TestMessageGeneration) { auto input_data = std::make_unique>(vector_size); VectorX x = VectorX::Zero(vector_size); input_data->set_value(x); - context->SetInputPortValue( - 0, std::make_unique( - std::move(input_data))); + context->FixInputPort(0, std::move(input_data)); dut.Publish(*context); KinematicsCache cache = tree.CreateKinematicsCache(); diff --git a/systems/framework/context.h b/systems/framework/context.h index 9824d8ea83ee..127f8d97772f 100644 --- a/systems/framework/context.h +++ b/systems/framework/context.h @@ -206,46 +206,40 @@ class Context { // ========================================================================= // Accessors and Mutators for Input. - /// Connects the input port at @p index to the value source @p port_value. - /// Disconnects whatever value source was previously there, and deregisters - /// it from the output port on which it depends. In some Context - /// implementations, may require a recursive search through a tree of - /// subcontexts. Asserts if @p index is out of range. - virtual void SetInputPortValue( - int index, std::unique_ptr port_value) = 0; - /// Returns the number of input ports. virtual int get_num_input_ports() const = 0; /// Connects the input port at @p index to a FreestandingInputPortValue with - /// the given vector @p value. Asserts if @p index is out of range. - void FixInputPort(int index, std::unique_ptr> value) { - SetInputPortValue( - index, std::make_unique(std::move(value))); + /// the given abstract @p value. Aborts if @p index is out of range. + /// Returns a reference to the allocated FreestandingInputPortValue. The + /// reference will remain valid until this input port's value source is + /// replaced or the %Context is destroyed. You may use that reference to + /// modify the input port's value using the appropriate + /// FreestandingInputPortValue method, which will ensure that invalidation + /// notifications are delivered. + FreestandingInputPortValue& FixInputPort( + int index, std::unique_ptr value) { + auto free_value_ptr = + std::make_unique(std::move(value)); + FreestandingInputPortValue& free_value = *free_value_ptr; + SetInputPortValue(index, std::move(free_value_ptr)); + return free_value; } /// Connects the input port at @p index to a FreestandingInputPortValue with - /// the given abstract @p value. Asserts if @p index is out of range. - void FixInputPort(int index, std::unique_ptr value) { - SetInputPortValue( - index, std::make_unique(std::move(value))); + /// the given vector @p vec. Otherwise same as above method. + FreestandingInputPortValue& FixInputPort( + int index, std::unique_ptr> vec) { + return FixInputPort( + index, std::make_unique>>(std::move(vec))); } - /// Connects the input port at @p index to a FreestandingInputPortValue with - /// the given vector @p value. Asserts if @p index is out of range. - /// Returns a reference to the allocated FreestandingInputPortValue that will - /// remain valid until this input port's value source is replaced or the - /// context is destroyed. You may use that reference to modify the input - /// port's value using the appropriate FreestandingInputPortValue method, - /// which will ensure that invalidation notifications are delivered. + /// Same as above method but starts with an Eigen vector whose contents are + /// used to initialize a BasicVector in the FreestandingInputPortValue. FreestandingInputPortValue& FixInputPort( int index, const Eigen::Ref>& data) { auto vec = std::make_unique>(data); - auto freestanding = - std::make_unique(std::move(vec)); - FreestandingInputPortValue& freestanding_ref = *freestanding; - SetInputPortValue(index, std::move(freestanding)); - return freestanding_ref; + return FixInputPort(index, std::move(vec)); } /// Evaluates and returns the value of the input port identified by @@ -489,14 +483,26 @@ class Context { /// Asserts if @p index is out of range. virtual const InputPortValue* GetInputPortValue(int index) const = 0; - /// Returns the InputPortValue at the given @p index from the given - /// @p context. Returns nullptr if the given port has never been set with - /// SetInputPortValue(). Asserts if @p index is out of range. + /// Allows derived classes to invoke the protected method on subcontexts. static const InputPortValue* GetInputPortValue(const Context& context, int index) { return context.GetInputPortValue(index); } + /// Connects the input port at @p index to the value source @p port_value. + /// Disconnects whatever value source was previously there, and de-registers + /// it from the output port on which it depends. In some Context + /// implementations, may require a recursive search through a tree of + /// subcontexts. Implementations must abort if @p index is out of range. + virtual void SetInputPortValue( + int index, std::unique_ptr port_value) = 0; + + /// Allows derived classes to invoke the protected method on subcontexts. + static void SetInputPortValue(Context* context, int index, + std::unique_ptr port_value) { + context->SetInputPortValue(index, std::move(port_value)); + } + private: // Current time and step information. StepInfo step_info_; diff --git a/systems/framework/diagram_context.h b/systems/framework/diagram_context.h index bd0a2ad2ed65..86cde8f5d4cb 100644 --- a/systems/framework/diagram_context.h +++ b/systems/framework/diagram_context.h @@ -181,7 +181,8 @@ class DiagramContext : public Context { // Construct and install the destination port. auto input_port = std::make_unique(output_port_value); - dest_context.SetInputPortValue(dest_port_index, std::move(input_port)); + Context::SetInputPortValue(&dest_context, dest_port_index, + std::move(input_port)); // Remember the graph structure. We need it in DoClone(). dependency_graph_[dest] = src; @@ -272,17 +273,6 @@ class DiagramContext : public Context { return static_cast(input_ids_.size()); } - void SetInputPortValue(int index, - std::unique_ptr port) override { - DRAKE_ASSERT(index >= 0 && index < get_num_input_ports()); - const PortIdentifier& id = input_ids_[index]; - SystemIndex system_index = id.first; - PortIndex port_index = id.second; - GetMutableSubsystemContext(system_index) - .SetInputPortValue(port_index, std::move(port)); - // TODO(david-german-tri): Set invalidation callbacks. - } - const State& get_state() const final { DRAKE_ASSERT(state_ != nullptr); return *state_; @@ -374,6 +364,16 @@ class DiagramContext : public Context { return static_cast(contexts_.size()); } + void SetInputPortValue(int index, + std::unique_ptr port) final { + DRAKE_DEMAND(index >= 0 && index < get_num_input_ports()); + const PortIdentifier& id = input_ids_[index]; + SystemIndex system_index = id.first; + PortIndex port_index = id.second; + Context::SetInputPortValue(&GetMutableSubsystemContext(system_index), + port_index, std::move(port)); + } + std::vector input_ids_; // The outputs are stored in SystemIndex order, and outputs_ is equal in diff --git a/systems/framework/input_port_value.h b/systems/framework/input_port_value.h index 0a801074ccd9..2f3a48edb111 100644 --- a/systems/framework/input_port_value.h +++ b/systems/framework/input_port_value.h @@ -119,32 +119,10 @@ class FreestandingInputPortValue : public InputPortValue { // FreestandingInputPortValue objects are neither copyable nor moveable. DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(FreestandingInputPortValue) - /// Constructs a vector-valued %FreestandingInputPortValue. - /// Takes ownership of @p vec. - /// - /// @tparam T The type of the vector data. Must be a valid Eigen scalar. - /// @tparam V The type of @p vec itself. Must implement BasicVector. - template