From b1d903141efe7c0c29e88c9d55321af628093b08 Mon Sep 17 00:00:00 2001 From: Agustin Alba Chicar Date: Mon, 13 Sep 2021 11:32:08 -0300 Subject: [PATCH 01/28] Initial package configuration and compilation changes. - Commented out find_resource and find_runfiles --- .clang-format | 44 + .clang-tidy | 25 + .gitignore | 34 + LICENSE | 29 + README.md | 3 + cmake/DefaultCFlags.cmake | 19 + cmake/SanitizersConfig.cmake | 65 + maliput_drake/CMakeLists.txt | 85 + maliput_drake/include/CMakeLists.txt | 1 + maliput_drake/include/drake/CMakeLists.txt | 3 + .../include/drake/common/CMakeLists.txt | 1 + maliput_drake/include/drake/common/autodiff.h | 33 + .../include/drake/common/autodiff_overloads.h | 219 ++ .../include/drake/common/autodiffxd.h | 528 ++++ .../drake/common/autodiffxd_make_coherent.h | 30 + maliput_drake/include/drake/common/bit_cast.h | 25 + maliput_drake/include/drake/common/cond.h | 44 + .../include/drake/common/constants.h | 21 + .../drake/common/copyable_unique_ptr.h | 443 +++ .../include/drake/common/default_scalars.h | 231 ++ .../include/drake/common/double_overloads.h | 21 + .../include/drake/common/doxygen_cxx.h | 2 + .../include/drake/common/drake_assert.h | 151 ++ .../drake/common/drake_assertion_error.h | 18 + .../include/drake/common/drake_bool.h | 96 + .../include/drake/common/drake_copyable.h | 66 + .../include/drake/common/drake_deprecated.h | 65 + .../include/drake/common/drake_marker.h | 16 + .../include/drake/common/drake_path.h | 20 + .../include/drake/common/drake_throw.h | 54 + .../include/drake/common/dummy_value.h | 35 + .../drake/common/eigen_autodiff_types.h | 38 + .../include/drake/common/eigen_stl_types.h | 38 + .../include/drake/common/eigen_types.h | 465 ++++ .../include/drake/common/extract_double.h | 55 + .../include/drake/common/filesystem.h | 28 + .../drake/common/find_loaded_library.h | 14 + .../include/drake/common/find_resource.h | 120 + .../include/drake/common/find_runfiles.h | 43 + maliput_drake/include/drake/common/hash.h | 270 ++ .../include/drake/common/identifier.h | 250 ++ .../drake/common/is_approx_equal_abstol.h | 62 + .../include/drake/common/is_cloneable.h | 85 + .../drake/common/is_less_than_comparable.h | 74 + .../include/drake/common/name_value.h | 100 + .../include/drake/common/never_destroyed.h | 103 + .../include/drake/common/nice_type_name.h | 115 + .../drake/common/nice_type_name_override.h | 34 + .../include/drake/common/pointer_cast.h | 76 + .../include/drake/common/polynomial.h | 519 ++++ maliput_drake/include/drake/common/random.h | 61 + .../include/drake/common/reset_after_move.h | 112 + .../include/drake/common/reset_on_copy.h | 186 ++ .../include/drake/common/scope_exit.h | 61 + .../include/drake/common/scoped_singleton.h | 75 + .../include/drake/common/sorted_pair.h | 201 ++ maliput_drake/include/drake/common/symbolic.h | 53 + .../common/symbolic_chebyshev_basis_element.h | 156 ++ .../common/symbolic_chebyshev_polynomial.h | 134 + .../include/drake/common/symbolic_codegen.h | 336 +++ .../include/drake/common/symbolic_decompose.h | 175 ++ .../drake/common/symbolic_environment.h | 159 ++ .../drake/common/symbolic_expression.h | 1518 +++++++++++ .../drake/common/symbolic_expression_cell.h | 836 ++++++ .../common/symbolic_expression_visitor.h | 178 ++ .../include/drake/common/symbolic_formula.h | 1391 ++++++++++ .../drake/common/symbolic_formula_cell.h | 572 ++++ .../drake/common/symbolic_formula_visitor.h | 63 + .../common/symbolic_generic_polynomial.h | 561 ++++ .../include/drake/common/symbolic_ldlt.h | 45 + .../include/drake/common/symbolic_monomial.h | 174 ++ .../common/symbolic_monomial_basis_element.h | 238 ++ .../drake/common/symbolic_monomial_util.h | 249 ++ .../drake/common/symbolic_polynomial.h | 500 ++++ .../drake/common/symbolic_polynomial_basis.h | 162 ++ .../symbolic_polynomial_basis_element.h | 250 ++ .../drake/common/symbolic_rational_function.h | 250 ++ .../drake/common/symbolic_simplification.h | 58 + .../include/drake/common/symbolic_variable.h | 368 +++ .../include/drake/common/symbolic_variables.h | 185 ++ .../include/drake/common/temp_directory.h | 17 + .../include/drake/common/text_logging.h | 226 ++ .../common/trajectories/bspline_trajectory.h | 144 + .../trajectories/discrete_time_trajectory.h | 139 + .../exponential_plus_piecewise_polynomial.h | 82 + .../trajectories/piecewise_polynomial.h | 858 ++++++ .../common/trajectories/piecewise_pose.h | 121 + .../trajectories/piecewise_quaternion.h | 174 ++ .../trajectories/piecewise_trajectory.h | 80 + .../drake/common/trajectories/trajectory.h | 104 + .../include/drake/common/trig_poly.h | 470 ++++ .../include/drake/common/type_safe_index.h | 587 ++++ maliput_drake/include/drake/common/unused.h | 53 + maliput_drake/include/drake/common/value.h | 813 ++++++ .../include/drake/math/CMakeLists.txt | 1 + maliput_drake/include/drake/math/autodiff.h | 328 +++ .../include/drake/math/autodiff_gradient.h | 214 ++ .../include/drake/math/barycentric.h | 181 ++ .../include/drake/math/bspline_basis.h | 213 ++ .../drake/math/compute_numerical_gradient.h | 178 ++ .../continuous_algebraic_riccati_equation.h | 36 + .../drake/math/continuous_lyapunov_equation.h | 80 + .../drake/math/convert_time_derivative.h | 52 + .../include/drake/math/cross_product.h | 21 + .../discrete_algebraic_riccati_equation.h | 80 + .../drake/math/discrete_lyapunov_equation.h | 81 + .../include/drake/math/eigen_sparse_triplet.h | 85 + .../math/evenly_distributed_pts_on_sphere.h | 19 + .../math/fast_pose_composition_functions.h | 105 + maliput_drake/include/drake/math/gradient.h | 36 + .../include/drake/math/gradient_util.h | 295 ++ maliput_drake/include/drake/math/gray_code.h | 61 + .../include/drake/math/hopf_coordinate.h | 79 + maliput_drake/include/drake/math/jacobian.h | 171 ++ .../include/drake/math/knot_vector_type.h | 17 + .../include/drake/math/matrix_util.h | 155 ++ .../include/drake/math/normalize_vector.h | 60 + .../include/drake/math/orthonormal_basis.h | 61 + .../include/drake/math/quadratic_form.h | 144 + maliput_drake/include/drake/math/quaternion.h | 394 +++ .../include/drake/math/random_rotation.h | 75 + .../include/drake/math/rigid_transform.h | 670 +++++ .../include/drake/math/roll_pitch_yaw.h | 678 +++++ .../drake/math/rotation_conversion_gradient.h | 230 ++ .../include/drake/math/rotation_matrix.h | 1099 ++++++++ maliput_drake/include/drake/math/saturate.h | 23 + maliput_drake/include/drake/math/wrap_to.h | 33 + .../include/drake/systems/CMakeLists.txt | 1 + .../analysis/antiderivative_function.h | 214 ++ .../analysis/bogacki_shampine3_integrator.h | 80 + .../drake/systems/analysis/dense_output.h | 185 ++ .../analysis/explicit_euler_integrator.h | 97 + .../systems/analysis/hermitian_dense_output.h | 426 +++ .../analysis/implicit_euler_integrator.h | 567 ++++ .../systems/analysis/implicit_integrator.h | 567 ++++ .../systems/analysis/initial_value_problem.h | 260 ++ .../drake/systems/analysis/integrator_base.h | 1686 ++++++++++++ .../include/drake/systems/analysis/lyapunov.h | 66 + .../drake/systems/analysis/monte_carlo.h | 154 ++ .../drake/systems/analysis/radau_integrator.h | 326 +++ .../systems/analysis/region_of_attraction.h | 76 + .../analysis/runge_kutta2_integrator.h | 135 + .../analysis/runge_kutta3_integrator.h | 97 + .../analysis/runge_kutta5_integrator.h | 94 + .../systems/analysis/scalar_dense_output.h | 56 + .../analysis/scalar_initial_value_problem.h | 225 ++ .../analysis/scalar_view_dense_output.h | 81 + .../analysis/semi_explicit_euler_integrator.h | 171 ++ .../drake/systems/analysis/simulator.h | 922 +++++++ .../drake/systems/analysis/simulator_config.h | 37 + .../analysis/simulator_config_functions.h | 69 + .../drake/systems/analysis/simulator_gflags.h | 50 + .../systems/analysis/simulator_print_stats.h | 15 + .../drake/systems/analysis/simulator_status.h | 139 + .../systems/analysis/stepwise_dense_output.h | 62 + .../velocity_implicit_euler_integrator.h | 674 +++++ .../include/drake/systems/discrete_systems.h | 225 ++ .../systems/framework/abstract_value_cloner.h | 38 + .../drake/systems/framework/abstract_values.h | 74 + .../drake/systems/framework/basic_vector.h | 194 ++ .../include/drake/systems/framework/cache.h | 784 ++++++ .../drake/systems/framework/cache_doxygen.h | 673 +++++ .../drake/systems/framework/cache_entry.h | 387 +++ .../include/drake/systems/framework/context.h | 874 ++++++ .../drake/systems/framework/context_base.h | 753 ++++++ .../systems/framework/continuous_state.h | 264 ++ .../systems/framework/dependency_tracker.h | 577 ++++ .../include/drake/systems/framework/diagram.h | 549 ++++ .../drake/systems/framework/diagram_builder.h | 367 +++ .../drake/systems/framework/diagram_context.h | 199 ++ .../framework/diagram_continuous_state.h | 116 + .../framework/diagram_discrete_values.h | 134 + .../systems/framework/diagram_output_port.h | 148 + .../drake/systems/framework/diagram_state.h | 68 + .../drake/systems/framework/discrete_values.h | 271 ++ .../include/drake/systems/framework/event.h | 931 +++++++ .../systems/framework/event_collection.h | 851 ++++++ .../drake/systems/framework/event_status.h | 98 + .../framework/fixed_input_port_value.h | 198 ++ .../systems/framework/framework_common.h | 338 +++ .../drake/systems/framework/input_port.h | 205 ++ .../drake/systems/framework/input_port_base.h | 105 + .../drake/systems/framework/leaf_context.h | 90 + .../systems/framework/leaf_output_port.h | 126 + .../drake/systems/framework/leaf_system.h | 2160 +++++++++++++++ .../drake/systems/framework/model_values.h | 97 + .../drake/systems/framework/output_port.h | 253 ++ .../systems/framework/output_port_base.h | 76 + .../drake/systems/framework/parameters.h | 177 ++ .../drake/systems/framework/port_base.h | 174 ++ .../framework/scalar_conversion_traits.h | 107 + .../framework/single_output_vector_source.h | 114 + .../include/drake/systems/framework/state.h | 144 + .../drake/systems/framework/subvector.h | 77 + .../drake/systems/framework/supervector.h | 108 + .../include/drake/systems/framework/system.h | 1778 ++++++++++++ .../drake/systems/framework/system_base.h | 1423 ++++++++++ .../framework/system_compatibility_doxygen.h | 76 + .../systems/framework/system_constraint.h | 406 +++ .../drake/systems/framework/system_html.h | 23 + .../drake/systems/framework/system_output.h | 111 + .../system_scalar_conversion_doxygen.h | 319 +++ .../framework/system_scalar_converter.h | 301 +++ .../framework/system_symbolic_inspector.h | 172 ++ .../drake/systems/framework/system_type_tag.h | 37 + .../drake/systems/framework/system_visitor.h | 56 + .../drake/systems/framework/value_checker.h | 71 + .../drake/systems/framework/value_producer.h | 549 ++++ .../framework/value_to_abstract_value.h | 293 ++ .../drake/systems/framework/vector_base.h | 270 ++ .../drake/systems/framework/vector_system.h | 348 +++ .../systems/framework/witness_function.h | 236 ++ .../drake/systems/stochastic_systems.h | 85 + maliput_drake/include/drake/systems/systems.h | 126 + maliput_drake/package.xml | 25 + maliput_drake/setup.sh.in | 53 + maliput_drake/src/CMakeLists.txt | 3 + maliput_drake/src/common/CMakeLists.txt | 34 + .../src/common/add_text_logging_gflags.cc | 34 + maliput_drake/src/common/cond.cc | 4 + maliput_drake/src/common/double_overloads.cc | 4 + .../src/common/drake_assert_and_throw.cc | 87 + maliput_drake/src/common/drake_marker.cc | 11 + maliput_drake/src/common/drake_path.cc | 25 + maliput_drake/src/common/filesystem.cc | 23 + .../src/common/find_loaded_library.cc | 118 + maliput_drake/src/common/find_resource.cc | 274 ++ maliput_drake/src/common/find_runfiles.cc | 202 ++ .../src/common/find_runfiles_stub.cc | 29 + maliput_drake/src/common/hash.cc | 4 + maliput_drake/src/common/nice_type_name.cc | 127 + .../src/common/nice_type_name_override.cc | 27 + maliput_drake/src/common/pointer_cast.cc | 4 + maliput_drake/src/common/polynomial.cc | 868 ++++++ maliput_drake/src/common/random.cc | 41 + maliput_drake/src/common/resource_tool.cc | 59 + maliput_drake/src/common/sorted_pair.cc | 9 + maliput_drake/src/common/symbolic.cc | 4 + .../symbolic_chebyshev_basis_element.cc | 251 ++ .../common/symbolic_chebyshev_polynomial.cc | 193 ++ maliput_drake/src/common/symbolic_codegen.cc | 304 +++ .../src/common/symbolic_decompose.cc | 506 ++++ .../src/common/symbolic_environment.cc | 172 ++ .../src/common/symbolic_expression.cc | 1148 ++++++++ .../src/common/symbolic_expression_cell.cc | 2377 +++++++++++++++++ maliput_drake/src/common/symbolic_formula.cc | 396 +++ .../src/common/symbolic_formula_cell.cc | 896 +++++++ .../src/common/symbolic_generic_polynomial.cc | 853 ++++++ maliput_drake/src/common/symbolic_ldlt.cc | 65 + maliput_drake/src/common/symbolic_monomial.cc | 288 ++ .../common/symbolic_monomial_basis_element.cc | 296 ++ .../src/common/symbolic_monomial_util.cc | 25 + .../src/common/symbolic_polynomial.cc | 875 ++++++ .../symbolic_polynomial_basis_element.cc | 197 ++ .../src/common/symbolic_rational_function.cc | 222 ++ .../src/common/symbolic_simplification.cc | 788 ++++++ maliput_drake/src/common/symbolic_variable.cc | 134 + .../src/common/symbolic_variables.cc | 153 ++ maliput_drake/src/common/temp_directory.cc | 43 + maliput_drake/src/common/text_logging.cc | 151 ++ .../src/common/trajectories/CMakeLists.txt | 34 + .../common/trajectories/bspline_trajectory.cc | 194 ++ .../trajectories/discrete_time_trajectory.cc | 133 + .../exponential_plus_piecewise_polynomial.cc | 80 + .../trajectories/piecewise_polynomial.cc | 1232 +++++++++ .../src/common/trajectories/piecewise_pose.cc | 136 + .../trajectories/piecewise_quaternion.cc | 229 ++ .../trajectories/piecewise_trajectory.cc | 138 + .../src/common/trajectories/trajectory.cc | 85 + maliput_drake/src/common/value.cc | 60 + maliput_drake/src/math/CMakeLists.txt | 33 + maliput_drake/src/math/autodiff.cc | 4 + maliput_drake/src/math/autodiff_gradient.cc | 4 + maliput_drake/src/math/barycentric.cc | 199 ++ maliput_drake/src/math/bspline_basis.cc | 154 ++ .../continuous_algebraic_riccati_equation.cc | 82 + .../src/math/continuous_lyapunov_equation.cc | 232 ++ maliput_drake/src/math/cross_product.cc | 4 + .../discrete_algebraic_riccati_equation.cc | 472 ++++ .../src/math/discrete_lyapunov_equation.cc | 228 ++ .../src/math/eigen_sparse_triplet.cc | 4 + .../math/evenly_distributed_pts_on_sphere.cc | 25 + .../math/fast_pose_composition_functions.cc | 587 ++++ maliput_drake/src/math/gradient.cc | 4 + maliput_drake/src/math/gray_code.cc | 17 + maliput_drake/src/math/hopf_coordinate.cc | 1 + maliput_drake/src/math/jacobian.cc | 4 + maliput_drake/src/math/matrix_util.cc | 4 + maliput_drake/src/math/normalize_vector.cc | 4 + maliput_drake/src/math/orthonormal_basis.cc | 4 + maliput_drake/src/math/quadratic_form.cc | 98 + maliput_drake/src/math/quaternion.cc | 4 + maliput_drake/src/math/random_rotation.cc | 4 + maliput_drake/src/math/rigid_transform.cc | 34 + maliput_drake/src/math/roll_pitch_yaw.cc | 238 ++ .../src/math/rotation_conversion_gradient.cc | 4 + maliput_drake/src/math/rotation_matrix.cc | 286 ++ maliput_drake/src/systems/CMakeLists.txt | 2 + .../src/systems/analysis/CMakeLists.txt | 35 + .../analysis/antiderivative_function.cc | 4 + .../analysis/bogacki_shampine3_integrator.cc | 172 ++ .../src/systems/analysis/dense_output.cc | 4 + .../analysis/explicit_euler_integrator.cc | 4 + .../analysis/hermitian_dense_output.cc | 4 + .../analysis/implicit_euler_integrator.cc | 561 ++++ .../systems/analysis/implicit_integrator.cc | 457 ++++ .../systems/analysis/initial_value_problem.cc | 269 ++ .../src/systems/analysis/integrator_base.cc | 469 ++++ .../src/systems/analysis/lyapunov.cc | 122 + .../src/systems/analysis/monte_carlo.cc | 50 + .../src/systems/analysis/radau_integrator.cc | 667 +++++ .../systems/analysis/region_of_attraction.cc | 212 ++ .../analysis/runge_kutta2_integrator.cc | 4 + .../analysis/runge_kutta3_integrator.cc | 155 ++ .../analysis/runge_kutta5_integrator.cc | 250 ++ .../systems/analysis/scalar_dense_output.cc | 4 + .../analysis/scalar_initial_value_problem.cc | 4 + .../analysis/scalar_view_dense_output.cc | 4 + .../semi_explicit_euler_integrator.cc | 4 + .../src/systems/analysis/simulator.cc | 729 +++++ .../src/systems/analysis/simulator_config.cc | 1 + .../analysis/simulator_config_functions.cc | 222 ++ .../src/systems/analysis/simulator_gflags.cc | 108 + .../systems/analysis/simulator_print_stats.cc | 158 ++ .../src/systems/analysis/simulator_status.cc | 53 + .../systems/analysis/stepwise_dense_output.cc | 4 + .../velocity_implicit_euler_integrator.cc | 651 +++++ .../src/systems/framework/CMakeLists.txt | 35 + .../framework/abstract_value_cloner.cc | 27 + .../src/systems/framework/abstract_values.cc | 63 + .../src/systems/framework/basic_vector.cc | 4 + maliput_drake/src/systems/framework/cache.cc | 138 + .../src/systems/framework/cache_entry.cc | 87 + .../src/systems/framework/context.cc | 200 ++ .../src/systems/framework/context_base.cc | 336 +++ .../src/systems/framework/continuous_state.cc | 134 + .../systems/framework/dependency_tracker.cc | 285 ++ .../src/systems/framework/diagram.cc | 1629 +++++++++++ .../src/systems/framework/diagram_builder.cc | 422 +++ .../src/systems/framework/diagram_context.cc | 334 +++ .../framework/diagram_continuous_state.cc | 65 + .../framework/diagram_discrete_values.cc | 4 + .../systems/framework/diagram_output_port.cc | 4 + .../src/systems/framework/diagram_state.cc | 51 + .../src/systems/framework/discrete_values.cc | 4 + .../src/systems/framework/event_collection.cc | 29 + .../framework/fixed_input_port_value.cc | 19 + .../src/systems/framework/input_port.cc | 4 + .../src/systems/framework/input_port_base.cc | 39 + .../src/systems/framework/leaf_context.cc | 113 + .../src/systems/framework/leaf_output_port.cc | 24 + .../src/systems/framework/leaf_system.cc | 1093 ++++++++ .../src/systems/framework/model_values.cc | 37 + .../src/systems/framework/output_port.cc | 30 + .../src/systems/framework/output_port_base.cc | 19 + .../src/systems/framework/parameters.cc | 4 + .../src/systems/framework/port_base.cc | 55 + .../framework/scalar_conversion_traits.cc | 3 + .../framework/single_output_vector_source.cc | 4 + maliput_drake/src/systems/framework/state.cc | 19 + .../src/systems/framework/subvector.cc | 4 + .../src/systems/framework/supervector.cc | 4 + maliput_drake/src/systems/framework/system.cc | 1221 +++++++++ .../src/systems/framework/system_base.cc | 309 +++ .../systems/framework/system_constraint.cc | 60 + .../src/systems/framework/system_html.cc | 372 +++ .../src/systems/framework/system_output.cc | 4 + .../framework/system_scalar_converter.cc | 89 + .../framework/system_symbolic_inspector.cc | 267 ++ .../src/systems/framework/system_type_tag.cc | 3 + .../src/systems/framework/system_visitor.cc | 3 + .../src/systems/framework/value_producer.cc | 68 + .../src/systems/framework/vector_base.cc | 4 + .../src/systems/framework/vector_system.cc | 4 + .../src/systems/framework/witness_function.cc | 4 + maliput_drake/test/CMakeLists.txt | 0 maliput_drake/tools/reformat_code.sh | 24 + 377 files changed, 81976 insertions(+) create mode 100644 .clang-format create mode 100644 .clang-tidy create mode 100644 .gitignore create mode 100644 LICENSE create mode 100644 README.md create mode 100644 cmake/DefaultCFlags.cmake create mode 100644 cmake/SanitizersConfig.cmake create mode 100644 maliput_drake/CMakeLists.txt create mode 100644 maliput_drake/include/CMakeLists.txt create mode 100644 maliput_drake/include/drake/CMakeLists.txt create mode 100644 maliput_drake/include/drake/common/CMakeLists.txt create mode 100644 maliput_drake/include/drake/common/autodiff.h create mode 100644 maliput_drake/include/drake/common/autodiff_overloads.h create mode 100644 maliput_drake/include/drake/common/autodiffxd.h create mode 100644 maliput_drake/include/drake/common/autodiffxd_make_coherent.h create mode 100644 maliput_drake/include/drake/common/bit_cast.h create mode 100644 maliput_drake/include/drake/common/cond.h create mode 100644 maliput_drake/include/drake/common/constants.h create mode 100644 maliput_drake/include/drake/common/copyable_unique_ptr.h create mode 100644 maliput_drake/include/drake/common/default_scalars.h create mode 100644 maliput_drake/include/drake/common/double_overloads.h create mode 100644 maliput_drake/include/drake/common/doxygen_cxx.h create mode 100644 maliput_drake/include/drake/common/drake_assert.h create mode 100644 maliput_drake/include/drake/common/drake_assertion_error.h create mode 100644 maliput_drake/include/drake/common/drake_bool.h create mode 100644 maliput_drake/include/drake/common/drake_copyable.h create mode 100644 maliput_drake/include/drake/common/drake_deprecated.h create mode 100644 maliput_drake/include/drake/common/drake_marker.h create mode 100644 maliput_drake/include/drake/common/drake_path.h create mode 100644 maliput_drake/include/drake/common/drake_throw.h create mode 100644 maliput_drake/include/drake/common/dummy_value.h create mode 100644 maliput_drake/include/drake/common/eigen_autodiff_types.h create mode 100644 maliput_drake/include/drake/common/eigen_stl_types.h create mode 100644 maliput_drake/include/drake/common/eigen_types.h create mode 100644 maliput_drake/include/drake/common/extract_double.h create mode 100644 maliput_drake/include/drake/common/filesystem.h create mode 100644 maliput_drake/include/drake/common/find_loaded_library.h create mode 100644 maliput_drake/include/drake/common/find_resource.h create mode 100644 maliput_drake/include/drake/common/find_runfiles.h create mode 100644 maliput_drake/include/drake/common/hash.h create mode 100644 maliput_drake/include/drake/common/identifier.h create mode 100644 maliput_drake/include/drake/common/is_approx_equal_abstol.h create mode 100644 maliput_drake/include/drake/common/is_cloneable.h create mode 100644 maliput_drake/include/drake/common/is_less_than_comparable.h create mode 100644 maliput_drake/include/drake/common/name_value.h create mode 100644 maliput_drake/include/drake/common/never_destroyed.h create mode 100644 maliput_drake/include/drake/common/nice_type_name.h create mode 100644 maliput_drake/include/drake/common/nice_type_name_override.h create mode 100644 maliput_drake/include/drake/common/pointer_cast.h create mode 100644 maliput_drake/include/drake/common/polynomial.h create mode 100644 maliput_drake/include/drake/common/random.h create mode 100644 maliput_drake/include/drake/common/reset_after_move.h create mode 100644 maliput_drake/include/drake/common/reset_on_copy.h create mode 100644 maliput_drake/include/drake/common/scope_exit.h create mode 100644 maliput_drake/include/drake/common/scoped_singleton.h create mode 100644 maliput_drake/include/drake/common/sorted_pair.h create mode 100644 maliput_drake/include/drake/common/symbolic.h create mode 100644 maliput_drake/include/drake/common/symbolic_chebyshev_basis_element.h create mode 100644 maliput_drake/include/drake/common/symbolic_chebyshev_polynomial.h create mode 100644 maliput_drake/include/drake/common/symbolic_codegen.h create mode 100644 maliput_drake/include/drake/common/symbolic_decompose.h create mode 100644 maliput_drake/include/drake/common/symbolic_environment.h create mode 100644 maliput_drake/include/drake/common/symbolic_expression.h create mode 100644 maliput_drake/include/drake/common/symbolic_expression_cell.h create mode 100644 maliput_drake/include/drake/common/symbolic_expression_visitor.h create mode 100644 maliput_drake/include/drake/common/symbolic_formula.h create mode 100644 maliput_drake/include/drake/common/symbolic_formula_cell.h create mode 100644 maliput_drake/include/drake/common/symbolic_formula_visitor.h create mode 100644 maliput_drake/include/drake/common/symbolic_generic_polynomial.h create mode 100644 maliput_drake/include/drake/common/symbolic_ldlt.h create mode 100644 maliput_drake/include/drake/common/symbolic_monomial.h create mode 100644 maliput_drake/include/drake/common/symbolic_monomial_basis_element.h create mode 100644 maliput_drake/include/drake/common/symbolic_monomial_util.h create mode 100644 maliput_drake/include/drake/common/symbolic_polynomial.h create mode 100644 maliput_drake/include/drake/common/symbolic_polynomial_basis.h create mode 100644 maliput_drake/include/drake/common/symbolic_polynomial_basis_element.h create mode 100644 maliput_drake/include/drake/common/symbolic_rational_function.h create mode 100644 maliput_drake/include/drake/common/symbolic_simplification.h create mode 100644 maliput_drake/include/drake/common/symbolic_variable.h create mode 100644 maliput_drake/include/drake/common/symbolic_variables.h create mode 100644 maliput_drake/include/drake/common/temp_directory.h create mode 100644 maliput_drake/include/drake/common/text_logging.h create mode 100644 maliput_drake/include/drake/common/trajectories/bspline_trajectory.h create mode 100644 maliput_drake/include/drake/common/trajectories/discrete_time_trajectory.h create mode 100644 maliput_drake/include/drake/common/trajectories/exponential_plus_piecewise_polynomial.h create mode 100644 maliput_drake/include/drake/common/trajectories/piecewise_polynomial.h create mode 100644 maliput_drake/include/drake/common/trajectories/piecewise_pose.h create mode 100644 maliput_drake/include/drake/common/trajectories/piecewise_quaternion.h create mode 100644 maliput_drake/include/drake/common/trajectories/piecewise_trajectory.h create mode 100644 maliput_drake/include/drake/common/trajectories/trajectory.h create mode 100644 maliput_drake/include/drake/common/trig_poly.h create mode 100644 maliput_drake/include/drake/common/type_safe_index.h create mode 100644 maliput_drake/include/drake/common/unused.h create mode 100644 maliput_drake/include/drake/common/value.h create mode 100644 maliput_drake/include/drake/math/CMakeLists.txt create mode 100644 maliput_drake/include/drake/math/autodiff.h create mode 100644 maliput_drake/include/drake/math/autodiff_gradient.h create mode 100644 maliput_drake/include/drake/math/barycentric.h create mode 100644 maliput_drake/include/drake/math/bspline_basis.h create mode 100644 maliput_drake/include/drake/math/compute_numerical_gradient.h create mode 100644 maliput_drake/include/drake/math/continuous_algebraic_riccati_equation.h create mode 100644 maliput_drake/include/drake/math/continuous_lyapunov_equation.h create mode 100644 maliput_drake/include/drake/math/convert_time_derivative.h create mode 100644 maliput_drake/include/drake/math/cross_product.h create mode 100644 maliput_drake/include/drake/math/discrete_algebraic_riccati_equation.h create mode 100644 maliput_drake/include/drake/math/discrete_lyapunov_equation.h create mode 100644 maliput_drake/include/drake/math/eigen_sparse_triplet.h create mode 100644 maliput_drake/include/drake/math/evenly_distributed_pts_on_sphere.h create mode 100644 maliput_drake/include/drake/math/fast_pose_composition_functions.h create mode 100644 maliput_drake/include/drake/math/gradient.h create mode 100644 maliput_drake/include/drake/math/gradient_util.h create mode 100644 maliput_drake/include/drake/math/gray_code.h create mode 100644 maliput_drake/include/drake/math/hopf_coordinate.h create mode 100644 maliput_drake/include/drake/math/jacobian.h create mode 100644 maliput_drake/include/drake/math/knot_vector_type.h create mode 100644 maliput_drake/include/drake/math/matrix_util.h create mode 100644 maliput_drake/include/drake/math/normalize_vector.h create mode 100644 maliput_drake/include/drake/math/orthonormal_basis.h create mode 100644 maliput_drake/include/drake/math/quadratic_form.h create mode 100644 maliput_drake/include/drake/math/quaternion.h create mode 100644 maliput_drake/include/drake/math/random_rotation.h create mode 100644 maliput_drake/include/drake/math/rigid_transform.h create mode 100644 maliput_drake/include/drake/math/roll_pitch_yaw.h create mode 100644 maliput_drake/include/drake/math/rotation_conversion_gradient.h create mode 100644 maliput_drake/include/drake/math/rotation_matrix.h create mode 100644 maliput_drake/include/drake/math/saturate.h create mode 100644 maliput_drake/include/drake/math/wrap_to.h create mode 100644 maliput_drake/include/drake/systems/CMakeLists.txt create mode 100644 maliput_drake/include/drake/systems/analysis/antiderivative_function.h create mode 100644 maliput_drake/include/drake/systems/analysis/bogacki_shampine3_integrator.h create mode 100644 maliput_drake/include/drake/systems/analysis/dense_output.h create mode 100644 maliput_drake/include/drake/systems/analysis/explicit_euler_integrator.h create mode 100644 maliput_drake/include/drake/systems/analysis/hermitian_dense_output.h create mode 100644 maliput_drake/include/drake/systems/analysis/implicit_euler_integrator.h create mode 100644 maliput_drake/include/drake/systems/analysis/implicit_integrator.h create mode 100644 maliput_drake/include/drake/systems/analysis/initial_value_problem.h create mode 100644 maliput_drake/include/drake/systems/analysis/integrator_base.h create mode 100644 maliput_drake/include/drake/systems/analysis/lyapunov.h create mode 100644 maliput_drake/include/drake/systems/analysis/monte_carlo.h create mode 100644 maliput_drake/include/drake/systems/analysis/radau_integrator.h create mode 100644 maliput_drake/include/drake/systems/analysis/region_of_attraction.h create mode 100644 maliput_drake/include/drake/systems/analysis/runge_kutta2_integrator.h create mode 100644 maliput_drake/include/drake/systems/analysis/runge_kutta3_integrator.h create mode 100644 maliput_drake/include/drake/systems/analysis/runge_kutta5_integrator.h create mode 100644 maliput_drake/include/drake/systems/analysis/scalar_dense_output.h create mode 100644 maliput_drake/include/drake/systems/analysis/scalar_initial_value_problem.h create mode 100644 maliput_drake/include/drake/systems/analysis/scalar_view_dense_output.h create mode 100644 maliput_drake/include/drake/systems/analysis/semi_explicit_euler_integrator.h create mode 100644 maliput_drake/include/drake/systems/analysis/simulator.h create mode 100644 maliput_drake/include/drake/systems/analysis/simulator_config.h create mode 100644 maliput_drake/include/drake/systems/analysis/simulator_config_functions.h create mode 100644 maliput_drake/include/drake/systems/analysis/simulator_gflags.h create mode 100644 maliput_drake/include/drake/systems/analysis/simulator_print_stats.h create mode 100644 maliput_drake/include/drake/systems/analysis/simulator_status.h create mode 100644 maliput_drake/include/drake/systems/analysis/stepwise_dense_output.h create mode 100644 maliput_drake/include/drake/systems/analysis/velocity_implicit_euler_integrator.h create mode 100644 maliput_drake/include/drake/systems/discrete_systems.h create mode 100644 maliput_drake/include/drake/systems/framework/abstract_value_cloner.h create mode 100644 maliput_drake/include/drake/systems/framework/abstract_values.h create mode 100644 maliput_drake/include/drake/systems/framework/basic_vector.h create mode 100644 maliput_drake/include/drake/systems/framework/cache.h create mode 100644 maliput_drake/include/drake/systems/framework/cache_doxygen.h create mode 100644 maliput_drake/include/drake/systems/framework/cache_entry.h create mode 100644 maliput_drake/include/drake/systems/framework/context.h create mode 100644 maliput_drake/include/drake/systems/framework/context_base.h create mode 100644 maliput_drake/include/drake/systems/framework/continuous_state.h create mode 100644 maliput_drake/include/drake/systems/framework/dependency_tracker.h create mode 100644 maliput_drake/include/drake/systems/framework/diagram.h create mode 100644 maliput_drake/include/drake/systems/framework/diagram_builder.h create mode 100644 maliput_drake/include/drake/systems/framework/diagram_context.h create mode 100644 maliput_drake/include/drake/systems/framework/diagram_continuous_state.h create mode 100644 maliput_drake/include/drake/systems/framework/diagram_discrete_values.h create mode 100644 maliput_drake/include/drake/systems/framework/diagram_output_port.h create mode 100644 maliput_drake/include/drake/systems/framework/diagram_state.h create mode 100644 maliput_drake/include/drake/systems/framework/discrete_values.h create mode 100644 maliput_drake/include/drake/systems/framework/event.h create mode 100644 maliput_drake/include/drake/systems/framework/event_collection.h create mode 100644 maliput_drake/include/drake/systems/framework/event_status.h create mode 100644 maliput_drake/include/drake/systems/framework/fixed_input_port_value.h create mode 100644 maliput_drake/include/drake/systems/framework/framework_common.h create mode 100644 maliput_drake/include/drake/systems/framework/input_port.h create mode 100644 maliput_drake/include/drake/systems/framework/input_port_base.h create mode 100644 maliput_drake/include/drake/systems/framework/leaf_context.h create mode 100644 maliput_drake/include/drake/systems/framework/leaf_output_port.h create mode 100644 maliput_drake/include/drake/systems/framework/leaf_system.h create mode 100644 maliput_drake/include/drake/systems/framework/model_values.h create mode 100644 maliput_drake/include/drake/systems/framework/output_port.h create mode 100644 maliput_drake/include/drake/systems/framework/output_port_base.h create mode 100644 maliput_drake/include/drake/systems/framework/parameters.h create mode 100644 maliput_drake/include/drake/systems/framework/port_base.h create mode 100644 maliput_drake/include/drake/systems/framework/scalar_conversion_traits.h create mode 100644 maliput_drake/include/drake/systems/framework/single_output_vector_source.h create mode 100644 maliput_drake/include/drake/systems/framework/state.h create mode 100644 maliput_drake/include/drake/systems/framework/subvector.h create mode 100644 maliput_drake/include/drake/systems/framework/supervector.h create mode 100644 maliput_drake/include/drake/systems/framework/system.h create mode 100644 maliput_drake/include/drake/systems/framework/system_base.h create mode 100644 maliput_drake/include/drake/systems/framework/system_compatibility_doxygen.h create mode 100644 maliput_drake/include/drake/systems/framework/system_constraint.h create mode 100644 maliput_drake/include/drake/systems/framework/system_html.h create mode 100644 maliput_drake/include/drake/systems/framework/system_output.h create mode 100644 maliput_drake/include/drake/systems/framework/system_scalar_conversion_doxygen.h create mode 100644 maliput_drake/include/drake/systems/framework/system_scalar_converter.h create mode 100644 maliput_drake/include/drake/systems/framework/system_symbolic_inspector.h create mode 100644 maliput_drake/include/drake/systems/framework/system_type_tag.h create mode 100644 maliput_drake/include/drake/systems/framework/system_visitor.h create mode 100644 maliput_drake/include/drake/systems/framework/value_checker.h create mode 100644 maliput_drake/include/drake/systems/framework/value_producer.h create mode 100644 maliput_drake/include/drake/systems/framework/value_to_abstract_value.h create mode 100644 maliput_drake/include/drake/systems/framework/vector_base.h create mode 100644 maliput_drake/include/drake/systems/framework/vector_system.h create mode 100644 maliput_drake/include/drake/systems/framework/witness_function.h create mode 100644 maliput_drake/include/drake/systems/stochastic_systems.h create mode 100644 maliput_drake/include/drake/systems/systems.h create mode 100644 maliput_drake/package.xml create mode 100644 maliput_drake/setup.sh.in create mode 100644 maliput_drake/src/CMakeLists.txt create mode 100644 maliput_drake/src/common/CMakeLists.txt create mode 100644 maliput_drake/src/common/add_text_logging_gflags.cc create mode 100644 maliput_drake/src/common/cond.cc create mode 100644 maliput_drake/src/common/double_overloads.cc create mode 100644 maliput_drake/src/common/drake_assert_and_throw.cc create mode 100644 maliput_drake/src/common/drake_marker.cc create mode 100644 maliput_drake/src/common/drake_path.cc create mode 100644 maliput_drake/src/common/filesystem.cc create mode 100644 maliput_drake/src/common/find_loaded_library.cc create mode 100644 maliput_drake/src/common/find_resource.cc create mode 100644 maliput_drake/src/common/find_runfiles.cc create mode 100644 maliput_drake/src/common/find_runfiles_stub.cc create mode 100644 maliput_drake/src/common/hash.cc create mode 100644 maliput_drake/src/common/nice_type_name.cc create mode 100644 maliput_drake/src/common/nice_type_name_override.cc create mode 100644 maliput_drake/src/common/pointer_cast.cc create mode 100644 maliput_drake/src/common/polynomial.cc create mode 100644 maliput_drake/src/common/random.cc create mode 100644 maliput_drake/src/common/resource_tool.cc create mode 100644 maliput_drake/src/common/sorted_pair.cc create mode 100644 maliput_drake/src/common/symbolic.cc create mode 100644 maliput_drake/src/common/symbolic_chebyshev_basis_element.cc create mode 100644 maliput_drake/src/common/symbolic_chebyshev_polynomial.cc create mode 100644 maliput_drake/src/common/symbolic_codegen.cc create mode 100644 maliput_drake/src/common/symbolic_decompose.cc create mode 100644 maliput_drake/src/common/symbolic_environment.cc create mode 100644 maliput_drake/src/common/symbolic_expression.cc create mode 100644 maliput_drake/src/common/symbolic_expression_cell.cc create mode 100644 maliput_drake/src/common/symbolic_formula.cc create mode 100644 maliput_drake/src/common/symbolic_formula_cell.cc create mode 100644 maliput_drake/src/common/symbolic_generic_polynomial.cc create mode 100644 maliput_drake/src/common/symbolic_ldlt.cc create mode 100644 maliput_drake/src/common/symbolic_monomial.cc create mode 100644 maliput_drake/src/common/symbolic_monomial_basis_element.cc create mode 100644 maliput_drake/src/common/symbolic_monomial_util.cc create mode 100644 maliput_drake/src/common/symbolic_polynomial.cc create mode 100644 maliput_drake/src/common/symbolic_polynomial_basis_element.cc create mode 100644 maliput_drake/src/common/symbolic_rational_function.cc create mode 100644 maliput_drake/src/common/symbolic_simplification.cc create mode 100644 maliput_drake/src/common/symbolic_variable.cc create mode 100644 maliput_drake/src/common/symbolic_variables.cc create mode 100644 maliput_drake/src/common/temp_directory.cc create mode 100644 maliput_drake/src/common/text_logging.cc create mode 100644 maliput_drake/src/common/trajectories/CMakeLists.txt create mode 100644 maliput_drake/src/common/trajectories/bspline_trajectory.cc create mode 100644 maliput_drake/src/common/trajectories/discrete_time_trajectory.cc create mode 100644 maliput_drake/src/common/trajectories/exponential_plus_piecewise_polynomial.cc create mode 100644 maliput_drake/src/common/trajectories/piecewise_polynomial.cc create mode 100644 maliput_drake/src/common/trajectories/piecewise_pose.cc create mode 100644 maliput_drake/src/common/trajectories/piecewise_quaternion.cc create mode 100644 maliput_drake/src/common/trajectories/piecewise_trajectory.cc create mode 100644 maliput_drake/src/common/trajectories/trajectory.cc create mode 100644 maliput_drake/src/common/value.cc create mode 100644 maliput_drake/src/math/CMakeLists.txt create mode 100644 maliput_drake/src/math/autodiff.cc create mode 100644 maliput_drake/src/math/autodiff_gradient.cc create mode 100644 maliput_drake/src/math/barycentric.cc create mode 100644 maliput_drake/src/math/bspline_basis.cc create mode 100644 maliput_drake/src/math/continuous_algebraic_riccati_equation.cc create mode 100644 maliput_drake/src/math/continuous_lyapunov_equation.cc create mode 100644 maliput_drake/src/math/cross_product.cc create mode 100644 maliput_drake/src/math/discrete_algebraic_riccati_equation.cc create mode 100644 maliput_drake/src/math/discrete_lyapunov_equation.cc create mode 100644 maliput_drake/src/math/eigen_sparse_triplet.cc create mode 100644 maliput_drake/src/math/evenly_distributed_pts_on_sphere.cc create mode 100644 maliput_drake/src/math/fast_pose_composition_functions.cc create mode 100644 maliput_drake/src/math/gradient.cc create mode 100644 maliput_drake/src/math/gray_code.cc create mode 100644 maliput_drake/src/math/hopf_coordinate.cc create mode 100644 maliput_drake/src/math/jacobian.cc create mode 100644 maliput_drake/src/math/matrix_util.cc create mode 100644 maliput_drake/src/math/normalize_vector.cc create mode 100644 maliput_drake/src/math/orthonormal_basis.cc create mode 100644 maliput_drake/src/math/quadratic_form.cc create mode 100644 maliput_drake/src/math/quaternion.cc create mode 100644 maliput_drake/src/math/random_rotation.cc create mode 100644 maliput_drake/src/math/rigid_transform.cc create mode 100644 maliput_drake/src/math/roll_pitch_yaw.cc create mode 100644 maliput_drake/src/math/rotation_conversion_gradient.cc create mode 100644 maliput_drake/src/math/rotation_matrix.cc create mode 100644 maliput_drake/src/systems/CMakeLists.txt create mode 100644 maliput_drake/src/systems/analysis/CMakeLists.txt create mode 100644 maliput_drake/src/systems/analysis/antiderivative_function.cc create mode 100644 maliput_drake/src/systems/analysis/bogacki_shampine3_integrator.cc create mode 100644 maliput_drake/src/systems/analysis/dense_output.cc create mode 100644 maliput_drake/src/systems/analysis/explicit_euler_integrator.cc create mode 100644 maliput_drake/src/systems/analysis/hermitian_dense_output.cc create mode 100644 maliput_drake/src/systems/analysis/implicit_euler_integrator.cc create mode 100644 maliput_drake/src/systems/analysis/implicit_integrator.cc create mode 100644 maliput_drake/src/systems/analysis/initial_value_problem.cc create mode 100644 maliput_drake/src/systems/analysis/integrator_base.cc create mode 100644 maliput_drake/src/systems/analysis/lyapunov.cc create mode 100644 maliput_drake/src/systems/analysis/monte_carlo.cc create mode 100644 maliput_drake/src/systems/analysis/radau_integrator.cc create mode 100644 maliput_drake/src/systems/analysis/region_of_attraction.cc create mode 100644 maliput_drake/src/systems/analysis/runge_kutta2_integrator.cc create mode 100644 maliput_drake/src/systems/analysis/runge_kutta3_integrator.cc create mode 100644 maliput_drake/src/systems/analysis/runge_kutta5_integrator.cc create mode 100644 maliput_drake/src/systems/analysis/scalar_dense_output.cc create mode 100644 maliput_drake/src/systems/analysis/scalar_initial_value_problem.cc create mode 100644 maliput_drake/src/systems/analysis/scalar_view_dense_output.cc create mode 100644 maliput_drake/src/systems/analysis/semi_explicit_euler_integrator.cc create mode 100644 maliput_drake/src/systems/analysis/simulator.cc create mode 100644 maliput_drake/src/systems/analysis/simulator_config.cc create mode 100644 maliput_drake/src/systems/analysis/simulator_config_functions.cc create mode 100644 maliput_drake/src/systems/analysis/simulator_gflags.cc create mode 100644 maliput_drake/src/systems/analysis/simulator_print_stats.cc create mode 100644 maliput_drake/src/systems/analysis/simulator_status.cc create mode 100644 maliput_drake/src/systems/analysis/stepwise_dense_output.cc create mode 100644 maliput_drake/src/systems/analysis/velocity_implicit_euler_integrator.cc create mode 100644 maliput_drake/src/systems/framework/CMakeLists.txt create mode 100644 maliput_drake/src/systems/framework/abstract_value_cloner.cc create mode 100644 maliput_drake/src/systems/framework/abstract_values.cc create mode 100644 maliput_drake/src/systems/framework/basic_vector.cc create mode 100644 maliput_drake/src/systems/framework/cache.cc create mode 100644 maliput_drake/src/systems/framework/cache_entry.cc create mode 100644 maliput_drake/src/systems/framework/context.cc create mode 100644 maliput_drake/src/systems/framework/context_base.cc create mode 100644 maliput_drake/src/systems/framework/continuous_state.cc create mode 100644 maliput_drake/src/systems/framework/dependency_tracker.cc create mode 100644 maliput_drake/src/systems/framework/diagram.cc create mode 100644 maliput_drake/src/systems/framework/diagram_builder.cc create mode 100644 maliput_drake/src/systems/framework/diagram_context.cc create mode 100644 maliput_drake/src/systems/framework/diagram_continuous_state.cc create mode 100644 maliput_drake/src/systems/framework/diagram_discrete_values.cc create mode 100644 maliput_drake/src/systems/framework/diagram_output_port.cc create mode 100644 maliput_drake/src/systems/framework/diagram_state.cc create mode 100644 maliput_drake/src/systems/framework/discrete_values.cc create mode 100644 maliput_drake/src/systems/framework/event_collection.cc create mode 100644 maliput_drake/src/systems/framework/fixed_input_port_value.cc create mode 100644 maliput_drake/src/systems/framework/input_port.cc create mode 100644 maliput_drake/src/systems/framework/input_port_base.cc create mode 100644 maliput_drake/src/systems/framework/leaf_context.cc create mode 100644 maliput_drake/src/systems/framework/leaf_output_port.cc create mode 100644 maliput_drake/src/systems/framework/leaf_system.cc create mode 100644 maliput_drake/src/systems/framework/model_values.cc create mode 100644 maliput_drake/src/systems/framework/output_port.cc create mode 100644 maliput_drake/src/systems/framework/output_port_base.cc create mode 100644 maliput_drake/src/systems/framework/parameters.cc create mode 100644 maliput_drake/src/systems/framework/port_base.cc create mode 100644 maliput_drake/src/systems/framework/scalar_conversion_traits.cc create mode 100644 maliput_drake/src/systems/framework/single_output_vector_source.cc create mode 100644 maliput_drake/src/systems/framework/state.cc create mode 100644 maliput_drake/src/systems/framework/subvector.cc create mode 100644 maliput_drake/src/systems/framework/supervector.cc create mode 100644 maliput_drake/src/systems/framework/system.cc create mode 100644 maliput_drake/src/systems/framework/system_base.cc create mode 100644 maliput_drake/src/systems/framework/system_constraint.cc create mode 100644 maliput_drake/src/systems/framework/system_html.cc create mode 100644 maliput_drake/src/systems/framework/system_output.cc create mode 100644 maliput_drake/src/systems/framework/system_scalar_converter.cc create mode 100644 maliput_drake/src/systems/framework/system_symbolic_inspector.cc create mode 100644 maliput_drake/src/systems/framework/system_type_tag.cc create mode 100644 maliput_drake/src/systems/framework/system_visitor.cc create mode 100644 maliput_drake/src/systems/framework/value_producer.cc create mode 100644 maliput_drake/src/systems/framework/vector_base.cc create mode 100644 maliput_drake/src/systems/framework/vector_system.cc create mode 100644 maliput_drake/src/systems/framework/witness_function.cc create mode 100644 maliput_drake/test/CMakeLists.txt create mode 100755 maliput_drake/tools/reformat_code.sh diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..c49efb8 --- /dev/null +++ b/.clang-format @@ -0,0 +1,44 @@ +# -*- yaml -*- + +# This file determines clang-format's style settings; for details, refer to +# http://clang.llvm.org/docs/ClangFormatStyleOptions.html + +BasedOnStyle: Google + +Language: Cpp + +# Force pointers to the type for C++. +DerivePointerAlignment: false +PointerAlignment: Left + +# Specify the #include statement order. This implements the order mandated by +# the Google C++ Style Guide: related header, C headers, C++ headers, library +# headers, and finally the project headers. +# +# To obtain updated lists of system headers used in the below expressions, see: +# http://stackoverflow.com/questions/2027991/list-of-standard-header-files-in-c-and-c/2029106#2029106. +IncludeCategories: + # Spacers used by drake/tools/formatter.py. + - Regex: '^$' + Priority: 15 + - Regex: '^$' + Priority: 25 + - Regex: '^$' + Priority: 35 + - Regex: '^$' + Priority: 45 + # C system headers. + - Regex: '^[<"](aio|arpa/inet|assert|complex|cpio|ctype|curses|dirent|dlfcn|errno|fcntl|fenv|float|fmtmsg|fnmatch|ftw|glob|grp|iconv|inttypes|iso646|langinfo|libgen|limits|locale|math|monetary|mqueue|ndbm|netdb|net/if|netinet/in|netinet/tcp|nl_types|poll|pthread|pwd|regex|sched|search|semaphore|setjmp|signal|spawn|stdalign|stdarg|stdatomic|stdbool|stddef|stdint|stdio|stdlib|stdnoreturn|string|strings|stropts|sys/ipc|syslog|sys/mman|sys/msg|sys/resource|sys/select|sys/sem|sys/shm|sys/socket|sys/stat|sys/statvfs|sys/time|sys/times|sys/types|sys/uio|sys/un|sys/utsname|sys/wait|tar|term|termios|tgmath|threads|time|trace|uchar|ulimit|uncntrl|unistd|utime|utmpx|wchar|wctype|wordexp)\.h[">]$' + Priority: 20 + # C++ system headers (as of C++17). + - Regex: '^[<"](algorithm|any|array|atomic|bitset|cassert|ccomplex|cctype|cerrno|cfenv|cfloat|charconv|chrono|cinttypes|ciso646|climits|clocale|cmath|codecvt|complex|condition_variable|csetjmp|csignal|cstdalign|cstdarg|cstdbool|cstddef|cstdint|cstdio|cstdlib|cstring|ctgmath|ctime|cuchar|cwchar|cwctype|deque|exception|execution|filesystem|forward_list|fstream|functional|future|initializer_list|iomanip|ios|iosfwd|iostream|istream|iterator|limits|list|locale|map|memory|memory_resource|mutex|new|numeric|optional|ostream|queue|random|ratio|regex|scoped_allocator|set|shared_mutex|sstream|stack|stdexcept|streambuf|string|string_view|strstream|system_error|thread|tuple|type_traits|typeindex|typeinfo|unordered_map|unordered_set|utility|valarray|variant|vector)[">]$' + Priority: 30 + # Other libraries' h files (with angles). + - Regex: '^<' + Priority: 40 + # Your project's h files. + - Regex: '^"drake' + Priority: 50 + # Other libraries' h files (with quotes). + - Regex: '^"' + Priority: 40 diff --git a/.clang-tidy b/.clang-tidy new file mode 100644 index 0000000..364048a --- /dev/null +++ b/.clang-tidy @@ -0,0 +1,25 @@ +--- +Checks: > + clang-analyzer-*, + clang-diagnostic-*, + cppcoreguidelines-*, + google-*, + modernize-*, + performance-*, + readability-*, + -cppcoreguidelines-pro-bounds-array-to-pointer-decay, + -cppcoreguidelines-pro-bounds-pointer-arithmetic, + -cppcoreguidelines-pro-type-static-cast-downcast, + -modernize-use-bool-literals, + -modernize-use-transparent-functors, + -modernize-use-using, + -readability-else-after-return, + -readability-named-parameter, + +CheckOptions: + - { key: readability-identifier-naming.ClassCase, value: CamelCase } + - { key: readability-identifier-naming.NamespaceCase, value: lower_case } + - { key: readability-identifier-naming.PrivateMemberSuffix, value: '_' } + - { key: readability-identifier-naming.StructCase, value: CamelCase } + - { key: readability-identifier-naming.VariableCase, value: lower_case } +... diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..80bd0b1 --- /dev/null +++ b/.gitignore @@ -0,0 +1,34 @@ +# Prerequisites +*.d + +# Compiled Object files +*.slo +*.lo +*.o + +# Precompiled Headers +*.gch +*.pch + +# Compiled Dynamic libraries +*.so +*.dylib +*.dll + +# Fortran module files +*.mod +*.smod + +# Compiled Static libraries +*.lai +*.la +*.a +*.lib + +# Executables +*.exe +*.out +*.app + +# Python caches +__pycache__ diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..7c4f49d --- /dev/null +++ b/LICENSE @@ -0,0 +1,29 @@ +BSD 3-Clause License + +Copyright (c) 2019, Toyota Research Institute. +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/README.md b/README.md new file mode 100644 index 0000000..bccd39e --- /dev/null +++ b/README.md @@ -0,0 +1,3 @@ +# `maliput_drake` + +A clone of `drake`'s trajectory integration support for `maliput` backends. diff --git a/cmake/DefaultCFlags.cmake b/cmake/DefaultCFlags.cmake new file mode 100644 index 0000000..afe3e1a --- /dev/null +++ b/cmake/DefaultCFlags.cmake @@ -0,0 +1,19 @@ +# C++ Version +set (CMAKE_CXX_STANDARD 17) +set (CMAKE_CXX_STANDARD_REQUIRED ON) +set (CMAKE_CXX_EXTENSIONS OFF) + +# Compiler-specific C++17 activation. +if ("${CMAKE_CXX_COMPILER_ID} " MATCHES "GNU ") + execute_process( + COMMAND ${CMAKE_CXX_COMPILER} -dumpversion OUTPUT_VARIABLE GCC_VERSION) + if (NOT (GCC_VERSION VERSION_GREATER 6.9)) + message(FATAL_ERROR "${PROJECT_NAME} requires g++ 7.0 or greater.") + else () + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fdata-sections -fdiagnostics-color=always -ffunction-sections -fopenmp -fPIC -fstack-protector -fno-omit-frame-pointer -no-canonical-prefixes -O2 -std=c++17 -Wall -Wno-builtin-macro-redefined -Wno-missing-field-initializers -Wregister -Wstrict-overflow -Wno-unused-const-variable") + endif () +elseif ("${CMAKE_CXX_COMPILER_ID} " MATCHES "Clang ") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fdata-sections -fdiagnostics-color=always -ffunction-sections -fPIC -fstack-protector -fno-omit-frame-pointer -no-canonical-prefixes -O2 -std=c++17 -Wall -Wno-builtin-macro-redefined -Wno-deprecated-dynamic-exception-spec -Wno-enum-compare-switch -Wno-gnu-designator -Wno-missing-field-initializers -Wno-register -Wno-strict-overflow -Wno-unknown-warning-option -Wno-unneeded-internal-declaration -Wno-unused-const-variable -Wno-missing-braces") +else () + message(FATAL_ERROR "Your C++ compiler does not support C++17.") +endif () diff --git a/cmake/SanitizersConfig.cmake b/cmake/SanitizersConfig.cmake new file mode 100644 index 0000000..a5fcef8 --- /dev/null +++ b/cmake/SanitizersConfig.cmake @@ -0,0 +1,65 @@ +############################################################################## +# Useful Macros +############################################################################## + +# It will throw an error if more than one sanitizer is enabled. +macro(check_sanitizers_exclusivity) + set(SANITIZER_COUNT 0) + set(SANITIZER_LIST) + foreach(sanitizer IN ITEMS ${ARGN}) + if(${${sanitizer}}) + math(EXPR SANITIZER_COUNT "1 + ${SANITIZER_COUNT}") + list(APPEND SANITIZER_LIST ${sanitizer}) + endif() + endforeach() + + if(${SANITIZER_COUNT} GREATER 1) + message(FATAL_ERROR "Can only enable one of ${SANITIZER_LIST} at a time.") + endif() +endmacro() + +############################################################################## +# Sanitizers Configuration +############################################################################## + +set(SANITIZERS off) +if ("${CMAKE_CXX_COMPILER_ID} " MATCHES "Clang ") + option(ADDRESS_SANITIZER "Enable Clang Address Sanitizer" OFF) + option(THREAD_SANITIZER "Enable Clang Thread Sanitizer" OFF) + option(UNDEFINED_SANITIZER "Enable Clang Undefined Behaviour Sanitizer" OFF) + + check_sanitizers_exclusivity(ADDRESS_SANITIZER THREAD_SANITIZER UNDEFINED_SANITIZER) + + # Address Sanitizer Configuration + if (ADDRESS_SANITIZER) + message(STATUS "Address Sanitizer - enabled") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g -fsanitize=address -fno-optimize-sibling-calls -fsanitize-address-use-after-scope") + set(LDFLAGS "${LDFLAGS} -fsanitize=address -fno-optimize-sibling-calls -fsanitize-address-use-after-scope") + set(SANITIZERS on) + else() + message(STATUS "Address Sanitizer - disabled") + endif() + + # Thread Sanitizer Configuration + if (THREAD_SANITIZER) + message(STATUS "Thread Sanitizer - enabled") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g -fsanitize=thread") + set(LDFLAGS "${LDFLAGS} -fsanitize=thread") + set(SANITIZERS on) + else() + message(STATUS "Thread Sanitizer - disabled") + endif() + + # Undefined Behaviour Sanitizer Configuration + if (UNDEFINED_SANITIZER) + message(STATUS "Undefined Behaviour Sanitizer - enabled") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fsanitize=undefined") + set(LDFLAGS "${LDFLAGS} -fsanitize=undefined") + set(SANITIZERS on) + else() + message(STATUS "Undefined Behaviour Sanitizer - disabled") + endif() + +else () + message(STATUS "Compiler Id:${CMAKE_CXX_COMPILER_ID} - Sanitizers disabled.") +endif () diff --git a/maliput_drake/CMakeLists.txt b/maliput_drake/CMakeLists.txt new file mode 100644 index 0000000..a95c0c8 --- /dev/null +++ b/maliput_drake/CMakeLists.txt @@ -0,0 +1,85 @@ +############################################################################## +# Project +############################################################################## + +cmake_minimum_required(VERSION 3.5 FATAL_ERROR) +project(maliput_drake LANGUAGES C CXX VERSION 0.1.0) + +############################################################################## +# Find 3rd Party Packages +############################################################################## + +message(STATUS "\n\n====== Finding 3rd Party Packages ======\n") + +find_package(ament_cmake REQUIRED) +find_package(fmt 4.0.0 EXACT QUIET) +if(NOT ${fmt_FOUND}) + find_package(fmt 6.1.2 REQUIRED) +endif() +find_package(yaml-cpp REQUIRED) +find_package(spdlog REQUIRED) +find_package(Eigen3 3.3 REQUIRED) + +############################################################################## +# Project Configuration +############################################################################## + +message(STATUS "\n\n========= Project Configuration ========\n") + +set(BUILD_SHARED_LIBS true) + +include(${PROJECT_SOURCE_DIR}/../cmake/DefaultCFlags.cmake) +include(${PROJECT_SOURCE_DIR}/../cmake/SanitizersConfig.cmake) + +ament_environment_hooks( + "${ament_cmake_package_templates_ENVIRONMENT_HOOK_LIBRARY_PATH}" +) + +############################################################################## +# Sources +############################################################################## +add_subdirectory(include) +add_subdirectory(src) + +############################################################################## +# Tests +############################################################################## + +# if(BUILD_TESTING) +# find_package(ament_cmake_clang_format REQUIRED) +# enable_testing() +# add_subdirectory(test) +# ament_clang_format(CONFIG_FILE ${CMAKE_CURRENT_SOURCE_DIR}/../.clang-format) +# endif() + +############################################################################## +# Docs +############################################################################## + +# if(NOT DEFINED BUILD_DOCS) +# set(BUILD_DOCS 1) +# endif() + +# if(BUILD_DOCS) +# find_package(ament_cmake_doxygen REQUIRED) +# ament_doxygen_generate(doxygen_maliput +# CONFIG_OVERLAY doc/Doxyfile.overlay.in +# TEST_ON_WARNS +# ) +# endif() + +############################################################################## +# Export +############################################################################## + +install( + DIRECTORY include/ + DESTINATION include +) + +ament_export_include_directories(include) + +# ament_environment_hooks(setup.sh.in) +ament_export_dependencies(ament_cmake) +ament_export_interfaces(${PROJECT_NAME}-targets HAS_LIBRARY_TARGET) +ament_package() diff --git a/maliput_drake/include/CMakeLists.txt b/maliput_drake/include/CMakeLists.txt new file mode 100644 index 0000000..1a271b2 --- /dev/null +++ b/maliput_drake/include/CMakeLists.txt @@ -0,0 +1 @@ +add_subdirectory(drake) \ No newline at end of file diff --git a/maliput_drake/include/drake/CMakeLists.txt b/maliput_drake/include/drake/CMakeLists.txt new file mode 100644 index 0000000..9fd26d0 --- /dev/null +++ b/maliput_drake/include/drake/CMakeLists.txt @@ -0,0 +1,3 @@ +add_subdirectory(common) +add_subdirectory(math) +add_subdirectory(systems) diff --git a/maliput_drake/include/drake/common/CMakeLists.txt b/maliput_drake/include/drake/common/CMakeLists.txt new file mode 100644 index 0000000..b0f82b8 --- /dev/null +++ b/maliput_drake/include/drake/common/CMakeLists.txt @@ -0,0 +1 @@ +ament_export_include_directories(${CMAKE_CURRENT_SOURCE_DIR}) \ No newline at end of file diff --git a/maliput_drake/include/drake/common/autodiff.h b/maliput_drake/include/drake/common/autodiff.h new file mode 100644 index 0000000..6aa8322 --- /dev/null +++ b/maliput_drake/include/drake/common/autodiff.h @@ -0,0 +1,33 @@ +#pragma once +/// @file This header provides a single inclusion point for autodiff-related +/// header files in the `drake/common` directory. Users should include this +/// file. Including other individual headers such as `drake/common/autodiffxd.h` +/// will generate a compile-time warning. + +// In each header included below, it asserts that this macro +// `DRAKE_COMMON_AUTODIFF_HEADER` is defined. If the macro is not defined, it +// generates diagnostic warning messages. +#define DRAKE_COMMON_AUTODIFF_HEADER + +#include +#include + +static_assert(EIGEN_VERSION_AT_LEAST(3, 3, 4), + "Drake requires Eigen >= v3.3.4."); + +// Do not alpha-sort the following block of hard-coded #includes, which is +// protected by `clang-format on/off`. +// +// Rationale: We want to maximize the use of this header, `autodiff.h`, even +// inside of the autodiff-related files to avoid any mistakes which might not be +// detected. By centralizing the list here, we make sure that everyone will see +// the correct order which respects the inter-dependencies of the autodiff +// headers. This shields us from triggering undefined behaviors due to +// order-of-specialization-includes-changed mistakes. +// +// clang-format off +#include "drake/common/eigen_autodiff_types.h" +#include "drake/common/autodiffxd.h" +#include "drake/common/autodiff_overloads.h" +// clang-format on +#undef DRAKE_COMMON_AUTODIFF_HEADER diff --git a/maliput_drake/include/drake/common/autodiff_overloads.h b/maliput_drake/include/drake/common/autodiff_overloads.h new file mode 100644 index 0000000..65e07a0 --- /dev/null +++ b/maliput_drake/include/drake/common/autodiff_overloads.h @@ -0,0 +1,219 @@ +/// @file +/// Overloads for STL mathematical operations on AutoDiffScalar. +/// +/// Used via argument-dependent lookup (ADL). These functions appear +/// in the Eigen namespace so that ADL can automatically choose between +/// the STL version and the overloaded version to match the type of the +/// arguments. The proper use would be e.g. +/// +/// \code{.cc} +/// void mymethod() { +/// using std::isinf; +/// isinf(myval); +/// } +/// \endcode{} +/// +/// @note The if_then_else and cond functions for AutoDiffScalar are in +/// namespace drake because cond is defined in namespace drake in +/// "drake/common/cond.h" file. + +#pragma once + +#ifndef DRAKE_COMMON_AUTODIFF_HEADER +// TODO(soonho-tri): Change to #error. +#warning Do not directly include this file. Include "drake/common/autodiff.h". +#endif + +#include +#include + +#include "drake/common/cond.h" +#include "drake/common/drake_assert.h" +#include "drake/common/dummy_value.h" + +namespace Eigen { + +/// Overloads nexttoward to mimic std::nexttoward from . +template +double nexttoward(const Eigen::AutoDiffScalar& from, long double to) { + using std::nexttoward; + return nexttoward(from.value(), to); +} + +/// Overloads round to mimic std::round from . +template +double round(const Eigen::AutoDiffScalar& x) { + using std::round; + return round(x.value()); +} + +/// Overloads isinf to mimic std::isinf from . +template +bool isinf(const Eigen::AutoDiffScalar& x) { + using std::isinf; + return isinf(x.value()); +} + +/// Overloads isfinite to mimic std::isfinite from . +template +bool isfinite(const Eigen::AutoDiffScalar& x) { + using std::isfinite; + return isfinite(x.value()); +} + +/// Overloads isnan to mimic std::isnan from . +template +bool isnan(const Eigen::AutoDiffScalar& x) { + using std::isnan; + return isnan(x.value()); +} + +/// Overloads floor to mimic std::floor from . +template +double floor(const Eigen::AutoDiffScalar& x) { + using std::floor; + return floor(x.value()); +} + +/// Overloads ceil to mimic std::ceil from . +template +double ceil(const Eigen::AutoDiffScalar& x) { + using std::ceil; + return ceil(x.value()); +} + +/// Overloads copysign from . +template +Eigen::AutoDiffScalar copysign(const Eigen::AutoDiffScalar& x, + const T& y) { + using std::isnan; + if (isnan(x)) return (y >= 0) ? NAN : -NAN; + if ((x < 0 && y >= 0) || (x >= 0 && y < 0)) + return -x; + else + return x; +} + +/// Overloads copysign from . +template +double copysign(double x, const Eigen::AutoDiffScalar& y) { + using std::isnan; + if (isnan(x)) return (y >= 0) ? NAN : -NAN; + if ((x < 0 && y >= 0) || (x >= 0 && y < 0)) + return -x; + else + return x; +} + +/// Overloads pow for an AutoDiffScalar base and exponent, implementing the +/// chain rule. +template +Eigen::AutoDiffScalar< + typename internal::remove_all::type::PlainObject> +pow(const Eigen::AutoDiffScalar& base, + const Eigen::AutoDiffScalar& exponent) { + // The two AutoDiffScalars being exponentiated must have the same matrix + // type. This includes, but is not limited to, the same scalar type and + // the same dimension. + static_assert( + std::is_same_v< + typename internal::remove_all::type::PlainObject, + typename internal::remove_all::type::PlainObject>, + "The derivative types must match."); + + internal::make_coherent(base.derivatives(), exponent.derivatives()); + + const auto& x = base.value(); + const auto& xgrad = base.derivatives(); + const auto& y = exponent.value(); + const auto& ygrad = exponent.derivatives(); + + using std::pow; + using std::log; + const auto x_to_the_y = pow(x, y); + if (ygrad.isZero(std::numeric_limits::epsilon()) || + ygrad.size() == 0) { + // The derivative only depends on ∂(x^y)/∂x -- this prevents undefined + // behavior in the corner case where ∂(x^y)/∂y is infinite when x = 0, + // despite ∂y/∂v being 0. + return Eigen::MakeAutoDiffScalar(x_to_the_y, y * pow(x, y - 1) * xgrad); + } + return Eigen::MakeAutoDiffScalar( + // The value is x ^ y. + x_to_the_y, + // The multivariable chain rule states: + // df/dv_i = (∂f/∂x * dx/dv_i) + (∂f/∂y * dy/dv_i) + // ∂f/∂x is y*x^(y-1) + y * pow(x, y - 1) * xgrad + + // ∂f/∂y is (x^y)*ln(x) + x_to_the_y * log(x) * ygrad); +} + +} // namespace Eigen + +namespace drake { + +/// Returns the autodiff scalar's value() as a double. Never throws. +/// Overloads ExtractDoubleOrThrow from common/extract_double.h. +template +double ExtractDoubleOrThrow(const Eigen::AutoDiffScalar& scalar) { + return static_cast(scalar.value()); +} + +/// Returns @p matrix as an Eigen::Matrix with the same size +/// allocation as @p matrix. Calls ExtractDoubleOrThrow on each element of the +/// matrix, and therefore throws if any one of the extractions fail. +template +auto ExtractDoubleOrThrow( + const Eigen::MatrixBase, RowsAtCompileTime, ColsAtCompileTime, + Options, MaxRowsAtCompileTime, MaxColsAtCompileTime>>& matrix) { + return matrix + .unaryExpr([](const typename Eigen::AutoDiffScalar& value) { + return ExtractDoubleOrThrow(value); + }) + .eval(); +} + +/// Specializes common/dummy_value.h. +template +struct dummy_value> { + static constexpr Eigen::AutoDiffScalar get() { + constexpr double kNaN = std::numeric_limits::quiet_NaN(); + DerType derivatives; + derivatives.fill(kNaN); + return Eigen::AutoDiffScalar(kNaN, derivatives); + } +}; + +/// Provides if-then-else expression for Eigen::AutoDiffScalar type. To support +/// Eigen's generic expressions, we use casting to the plain object after +/// applying Eigen::internal::remove_all. It is based on the Eigen's +/// implementation of min/max function for AutoDiffScalar type +/// (https://bitbucket.org/eigen/eigen/src/10a1de58614569c9250df88bdfc6402024687bc6/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h?at=default&fileviewer=file-view-default#AutoDiffScalar.h-546). +template +Eigen::AutoDiffScalar< + typename Eigen::internal::remove_all::type::PlainObject> +if_then_else(bool f_cond, const Eigen::AutoDiffScalar& x, + const Eigen::AutoDiffScalar& y) { + typedef Eigen::AutoDiffScalar< + typename Eigen::internal::remove_all::type::PlainObject> + ADS1; + typedef Eigen::AutoDiffScalar< + typename Eigen::internal::remove_all::type::PlainObject> + ADS2; + static_assert(std::is_same_v, + "The derivative types must match."); + return f_cond ? ADS1(x) : ADS2(y); +} + +/// Provides special case of cond expression for Eigen::AutoDiffScalar type. +template +Eigen::AutoDiffScalar< + typename Eigen::internal::remove_all::type::PlainObject> +cond(bool f_cond, const Eigen::AutoDiffScalar& e_then, Rest... rest) { + return if_then_else(f_cond, e_then, cond(rest...)); +} + +} // namespace drake diff --git a/maliput_drake/include/drake/common/autodiffxd.h b/maliput_drake/include/drake/common/autodiffxd.h new file mode 100644 index 0000000..7c2e0ed --- /dev/null +++ b/maliput_drake/include/drake/common/autodiffxd.h @@ -0,0 +1,528 @@ +#pragma once + +// This file is a modification of Eigen-3.3.3's AutoDiffScalar.h file which is +// available at +// https://gitlab.com/libeigen/eigen/-/blob/3.3.3/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h +// +// Copyright (C) 2009 Gael Guennebaud +// Copyright (C) 2017 Drake Authors +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef DRAKE_COMMON_AUTODIFF_HEADER +// TODO(soonho-tri): Change to #error. +#warning Do not directly include this file. Include "drake/common/autodiff.h". +#endif + +#include +#include + +#include + +namespace Eigen { + +#if !defined(DRAKE_DOXYGEN_CXX) +// Explicit template specializations of Eigen::AutoDiffScalar for VectorXd. +// +// AutoDiffScalar tries to call internal::make_coherent to promote empty +// derivatives. However, it fails to do the promotion when an operand is an +// expression tree (i.e. CwiseBinaryOp). Our solution is to provide special +// overloading for VectorXd and change the return types of its operators. With +// this change, the operators evaluate terms immediately and return an +// AutoDiffScalar instead of expression trees (such as CwiseBinaryOp). +// Eigen's implementation of internal::make_coherent makes use of const_cast in +// order to promote zero sized derivatives. This however interferes badly with +// our caching system and produces unexpected behaviors. See #10971 for details. +// Therefore our implementation stops using internal::make_coherent and treats +// scalars with zero sized derivatives as constants, as it should. +// +// We also provide overloading of math functions for AutoDiffScalar +// which return AutoDiffScalar instead of an expression tree. +// +// See https://github.com/RobotLocomotion/drake/issues/6944 for more +// information. See also drake/common/autodiff_overloads.h. +// +// TODO(soonho-tri): Next time when we upgrade Eigen, please check if we still +// need these specializations. +// +// @note move-aware arithmetic +// Prior implementations of arithmetic overloads required construction of new +// objects at each operation, which induced costly heap allocations. In modern +// C++, it is possible to instead exploit move semantics to avoid allocation in +// many cases. In particular, the compiler can implicitly use moves to satisfy +// pass-by-value parameters in cases where moves are possible (move construction +// and assignment are available), and the storage in question is not needed +// afterward. This allows definitions of operators that pass and return by +// value, and only allocate when needed, as determined by the compiler. For C++ +// considerations, see Scott Meyers' _Effective Modern C++_ Item 41. See #13985 +// for more discussion of Drake considerations. +template <> +class AutoDiffScalar + : public internal::auto_diff_special_op { + public: + typedef internal::auto_diff_special_op Base; + typedef typename internal::remove_all::type DerType; + typedef typename internal::traits::Scalar Scalar; + typedef typename NumTraits::Real Real; + + using Base::operator+; + using Base::operator*; + + AutoDiffScalar() {} + + AutoDiffScalar(const Scalar& value, int nbDer, int derNumber) + : m_value(value), m_derivatives(DerType::Zero(nbDer)) { + m_derivatives.coeffRef(derNumber) = Scalar(1); + } + + // NOLINTNEXTLINE(runtime/explicit): Code from Eigen. + AutoDiffScalar(const Real& value) : m_value(value) { + if (m_derivatives.size() > 0) m_derivatives.setZero(); + } + + AutoDiffScalar(const Scalar& value, const DerType& der) + : m_value(value), m_derivatives(der) {} + + template + AutoDiffScalar( + const AutoDiffScalar& other +#ifndef EIGEN_PARSED_BY_DOXYGEN + , + typename internal::enable_if< + internal::is_same< + Scalar, typename internal::traits::type>::Scalar>::value, + void*>::type = 0 +#endif + ) + : m_value(other.value()), m_derivatives(other.derivatives()) { + } + + friend std::ostream& operator<<(std::ostream& s, const AutoDiffScalar& a) { + return s << a.value(); + } + + AutoDiffScalar(const AutoDiffScalar& other) + : m_value(other.value()), m_derivatives(other.derivatives()) {} + + // Move construction and assignment are trivial, but need to be explicitly + // requested, since we have user-declared copy and assignment operators. + AutoDiffScalar(AutoDiffScalar&&) = default; + AutoDiffScalar& operator=(AutoDiffScalar&&) = default; + + template + inline AutoDiffScalar& operator=(const AutoDiffScalar& other) { + m_value = other.value(); + m_derivatives = other.derivatives(); + return *this; + } + + inline AutoDiffScalar& operator=(const AutoDiffScalar& other) { + m_value = other.value(); + m_derivatives = other.derivatives(); + return *this; + } + + inline AutoDiffScalar& operator=(const Scalar& other) { + m_value = other; + if (m_derivatives.size() > 0) m_derivatives.setZero(); + return *this; + } + + inline const Scalar& value() const { return m_value; } + inline Scalar& value() { return m_value; } + + inline const DerType& derivatives() const { return m_derivatives; } + inline DerType& derivatives() { return m_derivatives; } + + inline bool operator<(const Scalar& other) const { return m_value < other; } + inline bool operator<=(const Scalar& other) const { return m_value <= other; } + inline bool operator>(const Scalar& other) const { return m_value > other; } + inline bool operator>=(const Scalar& other) const { return m_value >= other; } + inline bool operator==(const Scalar& other) const { return m_value == other; } + inline bool operator!=(const Scalar& other) const { return m_value != other; } + + friend inline bool operator<(const Scalar& a, const AutoDiffScalar& b) { + return a < b.value(); + } + friend inline bool operator<=(const Scalar& a, const AutoDiffScalar& b) { + return a <= b.value(); + } + friend inline bool operator>(const Scalar& a, const AutoDiffScalar& b) { + return a > b.value(); + } + friend inline bool operator>=(const Scalar& a, const AutoDiffScalar& b) { + return a >= b.value(); + } + friend inline bool operator==(const Scalar& a, const AutoDiffScalar& b) { + return a == b.value(); + } + friend inline bool operator!=(const Scalar& a, const AutoDiffScalar& b) { + return a != b.value(); + } + + template + inline bool operator<(const AutoDiffScalar& b) const { + return m_value < b.value(); + } + template + inline bool operator<=(const AutoDiffScalar& b) const { + return m_value <= b.value(); + } + template + inline bool operator>(const AutoDiffScalar& b) const { + return m_value > b.value(); + } + template + inline bool operator>=(const AutoDiffScalar& b) const { + return m_value >= b.value(); + } + template + inline bool operator==(const AutoDiffScalar& b) const { + return m_value == b.value(); + } + template + inline bool operator!=(const AutoDiffScalar& b) const { + return m_value != b.value(); + } + + // The arithmetic operators below exploit move-awareness to avoid heap + // allocations. See note `move-aware arithmetic` above. Particular details + // will be called out below, the first time they appear. + + // Using a friend operator instead of a method allows the ADS parameter to be + // used as storage when move optimizations are possible. + friend inline AutoDiffScalar operator+(AutoDiffScalar a, const Scalar& b) { + a += b; + return a; + } + + friend inline AutoDiffScalar operator+(const Scalar& a, AutoDiffScalar b) { + b += a; + return b; + } + + // Compound assignment operators contain the primitive implementations, since + // the choice of writable storage is clear. Binary operations invoke the + // compound assignments. + inline AutoDiffScalar& operator+=(const Scalar& other) { + value() += other; + return *this; + } + + // It is possible that further overloads could exploit more move-awareness + // here. However, overload ambiguities are difficult to resolve. Currently + // only the left-hand operand is available for optimizations. See #13985, + // #14039 for discussion. + template + friend inline AutoDiffScalar operator+( + AutoDiffScalar a, const AutoDiffScalar& b) { + a += b; + return a; + } + + template + inline AutoDiffScalar& operator+=(const AutoDiffScalar& other) { + const bool has_this_der = m_derivatives.size() > 0; + const bool has_both_der = has_this_der && (other.derivatives().size() > 0); + m_value += other.value(); + if (has_both_der) { + m_derivatives += other.derivatives(); + } else if (has_this_der) { + // noop + } else { + m_derivatives = other.derivatives(); + } + return *this; + } + + friend inline AutoDiffScalar operator-(AutoDiffScalar a, const Scalar& b) { + a -= b; + return a; + } + + // Scalar-on-the-left non-commutative operations must also contain primitive + // implementations. + friend inline AutoDiffScalar operator-(const Scalar& a, AutoDiffScalar b) { + b.value() = a - b.value(); + b.derivatives() *= -1; + return b; + } + + inline AutoDiffScalar& operator-=(const Scalar& other) { + m_value -= other; + return *this; + } + + template + friend inline AutoDiffScalar operator-( + AutoDiffScalar a, const AutoDiffScalar& other) { + a -= other; + return a; + } + + template + inline AutoDiffScalar& operator-=(const AutoDiffScalar& other) { + const bool has_this_der = m_derivatives.size() > 0; + const bool has_both_der = has_this_der && (other.derivatives().size() > 0); + m_value -= other.value(); + if (has_both_der) { + m_derivatives -= other.derivatives(); + } else if (has_this_der) { + // noop + } else { + m_derivatives = -other.derivatives(); + } + return *this; + } + + // Phrasing unary negation as a value-passing friend permits some move + // optimizations. + friend inline AutoDiffScalar operator-(AutoDiffScalar a) { + a.value() *= -1; + a.derivatives() *= -1; + return a; + } + + friend inline AutoDiffScalar operator*(AutoDiffScalar a, const Scalar& b) { + a *= b; + return a; + } + + friend inline AutoDiffScalar operator*(const Scalar& a, AutoDiffScalar b) { + b *= a; + return b; + } + + friend inline AutoDiffScalar operator/(AutoDiffScalar a, const Scalar& b) { + a /= b; + return a; + } + + friend inline AutoDiffScalar operator/(const Scalar& a, AutoDiffScalar b) { + b.derivatives() *= Scalar(-a) / (b.value() * b.value()); + b.value() = a / b.value(); + return b; + } + + template + friend inline AutoDiffScalar operator/( + AutoDiffScalar a, const AutoDiffScalar& b) { + a /= b; + return a; + } + + template + friend inline AutoDiffScalar operator*( + AutoDiffScalar a, const AutoDiffScalar& b) { + a *= b; + return a; + } + + inline AutoDiffScalar& operator*=(const Scalar& other) { + m_value *= other; + m_derivatives *= other; + return *this; + } + + template + inline AutoDiffScalar& operator*=(const AutoDiffScalar& other) { + const bool has_this_der = m_derivatives.size() > 0; + const bool has_both_der = has_this_der && (other.derivatives().size() > 0); + // Some of the math below may look tempting to rewrite using `*=`, but + // performance measurement and analysis show that this formulation is + // faster because it results in better expression tree optimization and + // inlining. + if (has_both_der) { + m_derivatives = m_derivatives * other.value() + + other.derivatives() * m_value; + } else if (has_this_der) { + m_derivatives = m_derivatives * other.value(); + } else { + m_derivatives = other.derivatives() * m_value; + } + m_value *= other.value(); + return *this; + } + + inline AutoDiffScalar& operator/=(const Scalar& other) { + m_value /= other; + m_derivatives *= Scalar(1) / other; + return *this; + } + + template + inline AutoDiffScalar& operator/=(const AutoDiffScalar& other) { + auto& this_der = m_derivatives; + const auto& other_der = other.derivatives(); + const bool has_this_der = m_derivatives.size() > 0; + const bool has_both_der = has_this_der && (other.derivatives().size() > 0); + const Scalar scale = Scalar(1) / (other.value() * other.value()); + if (has_both_der) { + this_der *= other.value(); + this_der -= other_der * m_value; + this_der *= scale; + } else if (has_this_der) { + this_der *= Scalar(1) / other.value(); + } else { + this_der = other_der * -m_value * scale; + } + m_value /= other.value(); + return *this; + } + + protected: + Scalar m_value; + DerType m_derivatives; +}; + +#define DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(FUNC, CODE) \ + inline AutoDiffScalar FUNC( \ + AutoDiffScalar x) { \ + EIGEN_UNUSED typedef double Scalar; \ + CODE; \ + return x; \ + } + +DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY( + abs, using std::abs; + x.derivatives() *= (x.value() < 0 ? -1 : 1); + x.value() = abs(x.value());) + +DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY( + abs2, using numext::abs2; + x.derivatives() *= (Scalar(2) * x.value()); + x.value() = abs2(x.value());) + +DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY( + sqrt, using std::sqrt; + Scalar sqrtx = sqrt(x.value()); + x.value() = sqrtx; + x.derivatives() *= (Scalar(0.5) / sqrtx);) + +DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY( + cos, using std::cos; using std::sin; + x.derivatives() *= -sin(x.value()); + x.value() = cos(x.value());) + +DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY( + sin, using std::sin; using std::cos; + x.derivatives() *= cos(x.value()); + x.value() = sin(x.value());) + +DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY( + exp, using std::exp; + x.value() = exp(x.value()); + x.derivatives() *= x.value();) + +DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY( + log, using std::log; + x.derivatives() *= Scalar(1) / x.value(); + x.value() = log(x.value());) + +DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY( + tan, using std::tan; using std::cos; + x.derivatives() *= Scalar(1) / numext::abs2(cos(x.value())); + x.value() = tan(x.value());) + +DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY( + asin, using std::sqrt; using std::asin; + x.derivatives() *= Scalar(1) / sqrt(1 - numext::abs2(x.value())); + x.value() = asin(x.value());) + +DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY( + acos, using std::sqrt; using std::acos; + x.derivatives() *= Scalar(-1) / sqrt(1 - numext::abs2(x.value())); + x.value() = acos(x.value());) + +DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY( + // TODO(rpoyner-tri): implementation seems fishy --see #14051. + atan, using std::atan; + x.derivatives() *= Scalar(1) / (1 + x.value() * x.value()); + x.value() = atan(x.value());) + +DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY( + tanh, using std::cosh; using std::tanh; + x.derivatives() *= Scalar(1) / numext::abs2(cosh(x.value())); + x.value() = tanh(x.value());) + +DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY( + sinh, using std::sinh; using std::cosh; + x.derivatives() *= cosh(x.value()); + x.value() = sinh(x.value());) + +DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY( + cosh, using std::sinh; using std::cosh; + x.derivatives() *= sinh(x.value()); + x.value() = cosh(x.value());) + +#undef DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY + +// We have this specialization here because the Eigen-3.3.3's atan2 +// implementation for AutoDiffScalar does not make a return with properly sized +// derivatives. +inline AutoDiffScalar atan2(AutoDiffScalar a, + const AutoDiffScalar& b) { + const bool has_a_der = a.derivatives().size() > 0; + const bool has_both_der = has_a_der && (b.derivatives().size() > 0); + const double squared_hypot = a.value() * a.value() + b.value() * b.value(); + if (has_both_der) { + a.derivatives() *= b.value(); + a.derivatives() -= a.value() * b.derivatives(); + } else if (has_a_der) { + a.derivatives() *= b.value(); + } else { + a.derivatives() = -a.value() * b.derivatives(); + } + a.derivatives() /= squared_hypot; + a.value() = std::atan2(a.value(), b.value()); + return a; +} + +// Right-hand pass-by-value optimizations for atan2() are blocked by code in +// Eigen; see #14039. + +inline AutoDiffScalar pow(AutoDiffScalar a, double b) { + // TODO(rpoyner-tri): implementation seems fishy --see #14052. + using std::pow; + a.derivatives() *= b * pow(a.value(), b - 1); + a.value() = pow(a.value(), b); + return a; +} + +// We have these implementations here because Eigen's implementations do not +// have consistent behavior when a == b. We enforce the following rules for that +// case: +// 1) If both a and b are ADS with non-empty derivatives, return a. +// 2) If both a and b are doubles, return a. +// 3) If one of a, b is a double, and the other is an ADS, return the ADS. +// 4) Treat ADS with empty derivatives as though they were doubles. +// Points (1) and (4) are handled here. Points (2) and (3) are already handled +// by Eigen's overloads. +// See https://gitlab.com/libeigen/eigen/-/issues/1870. +inline const AutoDiffScalar min(const AutoDiffScalar& a, + const AutoDiffScalar& b) { + // If both a and b have derivatives, then their derivative sizes must match. + DRAKE_ASSERT( + a.derivatives().size() == 0 || b.derivatives().size() == 0 || + a.derivatives().size() == b.derivatives().size()); + // The smaller of a or b wins; ties go to a iff it has any derivatives. + return ((a < b) || ((a == b) && (a.derivatives().size() != 0))) ? a : b; +} + +// NOLINTNEXTLINE(build/include_what_you_use) +inline const AutoDiffScalar max(const AutoDiffScalar& a, + const AutoDiffScalar& b) { + // If both a and b have derivatives, then their derivative sizes must match. + DRAKE_ASSERT( + a.derivatives().size() == 0 || b.derivatives().size() == 0 || + a.derivatives().size() == b.derivatives().size()); + // The larger of a or b wins; ties go to a iff it has any derivatives. + return ((a > b) || ((a == b) && (a.derivatives().size() != 0))) ? a : b; +} + +#endif + +} // namespace Eigen diff --git a/maliput_drake/include/drake/common/autodiffxd_make_coherent.h b/maliput_drake/include/drake/common/autodiffxd_make_coherent.h new file mode 100644 index 0000000..5986bfd --- /dev/null +++ b/maliput_drake/include/drake/common/autodiffxd_make_coherent.h @@ -0,0 +1,30 @@ +#pragma once + +#include "drake/common/autodiff.h" +#include "drake/common/symbolic.h" + +namespace drake { + +/// Makes the derviatives of the recipient coherent with respect to those of the +/// donor variable (see drake/common/autodiffxd.h). If the recipient's +/// derivatives are already populated with a vector of the same size as that of +/// the donor, variables pass through unchanged. An exception is thrown when +/// there are nonempty vectors of different sizes. +inline void autodiffxd_make_coherent(const AutoDiffXd& donor, + AutoDiffXd* recipient) { + DRAKE_ASSERT(recipient != nullptr); + if (recipient->derivatives().size() == 0) { + recipient->derivatives() = + Eigen::VectorXd::Zero(donor.derivatives().size()); + } else if (recipient->derivatives().size() != donor.derivatives().size()) { + throw std::runtime_error("Nonempty recipient derivatives must match the " + "size of the donor derivatives."); + } +} + +inline void autodiffxd_make_coherent(const double&, double*) {} + +inline void autodiffxd_make_coherent(const symbolic::Expression&, + symbolic::Expression*) {} + +} // namespace drake diff --git a/maliput_drake/include/drake/common/bit_cast.h b/maliput_drake/include/drake/common/bit_cast.h new file mode 100644 index 0000000..b750416 --- /dev/null +++ b/maliput_drake/include/drake/common/bit_cast.h @@ -0,0 +1,25 @@ +#pragma once + +#include +#include + +namespace drake { +namespace internal { + +/** Implements C++20 https://en.cppreference.com/w/cpp/numeric/bit_cast (but +without the overload resolution guards, which are not necessary in our case.) +(Once all of Drake's supported platforms offer std::bit_cast, we can remove +this function in lieu of the std one.) */ +template +To bit_cast(const From& from) noexcept { + static_assert(std::is_trivially_constructible_v); + To result; + static_assert(sizeof(To) == sizeof(From)); + static_assert(std::is_trivially_copyable_v); + static_assert(std::is_trivially_copyable_v); + std::memcpy(&result, &from, sizeof(result)); + return result; +} + +} // namespace internal +} // namespace drake diff --git a/maliput_drake/include/drake/common/cond.h b/maliput_drake/include/drake/common/cond.h new file mode 100644 index 0000000..16dd21e --- /dev/null +++ b/maliput_drake/include/drake/common/cond.h @@ -0,0 +1,44 @@ +#pragma once + +#include +#include + +#include "drake/common/double_overloads.h" + +namespace drake { +/** @name cond + Constructs conditional expression (similar to Lisp's cond). + + @verbatim + cond(cond_1, expr_1, + cond_2, expr_2, + ..., ..., + cond_n, expr_n, + expr_{n+1}) + @endverbatim + + The value returned by the above cond expression is @c expr_1 if @c cond_1 is + true; else if @c cond_2 is true then @c expr_2; ... ; else if @c cond_n is + true then @c expr_n. If none of the conditions are true, it returns @c + expr_{n+1}. + + @note This functions assumes that @p ScalarType provides @c operator< and the + type of @c f_cond is the type of the return type of operator<(ScalarType, + ScalarType). For example, @c symbolic::Expression can be used as a @p + ScalarType because it provides symbolic::Formula + operator<(symbolic::Expression, symbolic::Expression). + + + @{ + */ +template +ScalarType cond(const ScalarType& e) { + return e; +} +template +ScalarType cond(const decltype(ScalarType() < ScalarType()) & f_cond, + const ScalarType& e_then, Rest... rest) { + return if_then_else(f_cond, e_then, cond(rest...)); +} +///@} +} // namespace drake diff --git a/maliput_drake/include/drake/common/constants.h b/maliput_drake/include/drake/common/constants.h new file mode 100644 index 0000000..b1249d9 --- /dev/null +++ b/maliput_drake/include/drake/common/constants.h @@ -0,0 +1,21 @@ +#pragma once + +namespace drake { + +constexpr int kQuaternionSize = 4; + +constexpr int kSpaceDimension = 3; + +constexpr int kRpySize = 3; + +/// https://en.wikipedia.org/wiki/Screw_theory#Twist +constexpr int kTwistSize = 6; + +/// http://www.euclideanspace.com/maths/geometry/affine/matrix4x4/ +constexpr int kHomogeneousTransformSize = 16; + +const int kRotmatSize = kSpaceDimension * kSpaceDimension; + +enum class ToleranceType { kAbsolute, kRelative }; + +} // namespace drake diff --git a/maliput_drake/include/drake/common/copyable_unique_ptr.h b/maliput_drake/include/drake/common/copyable_unique_ptr.h new file mode 100644 index 0000000..c3908f6 --- /dev/null +++ b/maliput_drake/include/drake/common/copyable_unique_ptr.h @@ -0,0 +1,443 @@ +#pragma once + +/* Portions copyright (c) 2015 Stanford University and the Authors. + Authors: Michael Sherman + +Licensed under the Apache License, Version 2.0 (the "License"); you may +not use this file except in compliance with the License. You may obtain a +copy of the License at http://www.apache.org/licenses/LICENSE-2.0. + +(Adapted from Simbody's ClonePtr class.) + */ + +#include +#include +#include +#include + +#include "drake/common/drake_assert.h" + +namespace drake { + +// TODO(SeanCurtis-TRI): Consider extending this to add the Deleter as well. +/** A smart pointer with deep copy semantics. + + This is _similar_ to `std::unique_ptr` in that it does not permit shared + ownership of the contained object. However, unlike `std::unique_ptr`, + %copyable_unique_ptr supports copy and assignment operations, by insisting that + the contained object be "copyable". To be copyable, the class must have either + an accessible copy constructor, or it must have an accessible clone method + with signature @code + std::unique_ptr Clone() const; + @endcode + where Foo is the type of the managed object. By "accessible" we mean either + that the copy constructor or clone method is public, or + `friend copyable_unique_ptr;` appears in Foo's class declaration. + + + + Generally, the API is modeled as closely as possible on the C++ standard + `std::unique_ptr` API and %copyable_unique_ptr is interoperable with + `unique_ptr` wherever that makes sense. However, there are some differences: + + 1. It always uses a default deleter. + 2. There is no array version. + 3. To allow for future copy-on-write optimizations, there is a distinction + between writable and const access, the get() method is modified to return + only a const pointer, with get_mutable() added to return a writable pointer. + Furthermore, derefencing (operator*()) a mutable pointer will give a mutable + reference (in so far as T is not declared const), and dereferencing a + const pointer will give a const reference. + + This class is entirely inline and has no computational or space overhead except + when copying is required; it contains just a single pointer and does no + reference counting. + + __Usage__ + + In the simplest use case, the instantiation type will match the type of object + it references, e.g.: + @code + copyable_unique_ptr ptr = make_unique(...); + @endcode + In this case, as long `Foo` is deemed compatible, the behavior will be as + expected, i.e., when `ptr` copies, it will contain a reference to a new + instance of `Foo`. + + %copyable_unique_ptr can also be used with polymorphic classes -- a + %copyable_unique_ptr, instantiated on a _base_ class, references an + instance of a _derived_ class. When copying the object, we would want the copy + to likewise contain an instance of the derived class. For example: + + @code + copyable_unique_ptr cu_ptr = make_unique(); + copyable_unique_ptr other_cu_ptr = cu_ptr; // Triggers a copy. + is_dynamic_castable(cu_other_ptr.get()); // Should be true. + @endcode + + This works for well-designed polymorphic classes. + + @warning Ill-formed polymorphic classes can lead to fatal type slicing of the + referenced object, such that the new copy contains an instance of `Base` + instead of `Derived`. Some mistakes that would lead to this degenerate + behavior: + + - The `Base` class has a public copy constructor. + - The `Base` class's Clone() implementation does not invoke the `Derived` + class's implementation of a suitable virtual method. + + @warning One important difference between unique_ptr and %copyable_unique_ptr + is that a unique_ptr can be declared on a forward-declared class type. The + %copyable_unique_ptr _cannot_. The class must be fully defined so that the + %copyable_unique_ptr is able to determine if the type meets the requirements + (i.e., public copy constructible or cloneable). + + + + @tparam T The type of the contained object, which *must* be copyable as + defined above. May be an abstract or concrete type. + */ +template +class copyable_unique_ptr : public std::unique_ptr { + public: + /** @name Constructors */ + /**@{*/ + + /** Default constructor stores a `nullptr`. No heap allocation is performed. + The empty() method will return true when called on a default-constructed + %copyable_unique_ptr. */ + copyable_unique_ptr() noexcept : std::unique_ptr() {} + + /** Given a pointer to a writable heap-allocated object, take over + ownership of that object. No copying occurs. */ + explicit copyable_unique_ptr(T* ptr) noexcept : std::unique_ptr(ptr) {} + + /** Constructs a unique instance of T as a copy of the provided model value. + */ + explicit copyable_unique_ptr(const T& value) + : std::unique_ptr(CopyOrNull(&value)) {} + + /** Copy constructor is deep; the new %copyable_unique_ptr object contains a + new copy of the object in the source, created via the source object's + copy constructor or `Clone()` method. If the source container is empty this + one will be empty also. */ + copyable_unique_ptr(const copyable_unique_ptr& cu_ptr) + : std::unique_ptr(CopyOrNull(cu_ptr.get())) {} + + /** Copy constructor from a standard `unique_ptr` of _compatible_ type. The + copy is deep; the new %copyable_unique_ptr object contains a new copy of the + object in the source, created via the source object's copy constructor or + `Clone()` method. If the source container is empty this one will be empty + also. */ + template + explicit copyable_unique_ptr(const std::unique_ptr& u_ptr) + : std::unique_ptr(CopyOrNull(u_ptr.get())) {} + + /** Move constructor is very fast and leaves the source empty. Ownership + is transferred from the source to the new %copyable_unique_ptr. If the source + was empty this one will be empty also. No heap activity occurs. */ + copyable_unique_ptr(copyable_unique_ptr&& cu_ptr) noexcept + : std::unique_ptr(cu_ptr.release()) {} + + /** Move constructor from a standard `unique_ptr`. The move is very fast and + leaves the source empty. Ownership is transferred from the source to the new + %copyable_unique_ptr. If the source was empty this one will be empty also. No + heap activity occurs. */ + explicit copyable_unique_ptr(std::unique_ptr&& u_ptr) noexcept + : std::unique_ptr(u_ptr.release()) {} + + /** Move construction from a compatible standard `unique_ptr`. Type `U*` must + be implicitly convertible to type `T*`. Ownership is transferred from the + source to the new %copyable_unique_ptr. If the source was empty this one will + be empty also. No heap activity occurs. */ + template + explicit copyable_unique_ptr(std::unique_ptr&& u_ptr) noexcept + : std::unique_ptr(u_ptr.release()) {} + + /**@}*/ + + /** @name Assignment */ + /**@{*/ + + /** This form of assignment replaces the currently-held object by + the given source object and takes over ownership of the source object. The + currently-held object (if any) is deleted. */ + copyable_unique_ptr& operator=(T* ptr) noexcept { + std::unique_ptr::reset(ptr); + return *this; + } + + /** This form of assignment replaces the currently-held object by a + heap-allocated copy of the source object, created using its copy + constructor or `Clone()` method. The currently-held object (if any) is + deleted. */ + copyable_unique_ptr & operator=(const T& ref) { + std::unique_ptr::reset(CopyOrNull(&ref)); + return *this; + } + + /** Copy assignment from %copyable_unique_ptr replaces the currently-held + object by a copy of the object held in the source container, created using + the source object's copy constructor or `Clone()` method. The currently-held + object (if any) is deleted. If the source container is empty this one will be + empty also after the assignment. Nothing happens if the source and + destination are the same container. */ + copyable_unique_ptr& operator=(const copyable_unique_ptr& cu_ptr) { + return operator=(static_cast&>(cu_ptr)); + } + + /** Copy assignment from a compatible %copyable_unique_ptr replaces the + currently-held object by a copy of the object held in the source container, + created using the source object's copy constructor or `Clone()` method. The + currently-held object (if any) is deleted. If the source container is empty + this one will be empty also after the assignment. Nothing happens if the + source and destination are the same container. */ + template + copyable_unique_ptr& operator=(const copyable_unique_ptr& cu_ptr) { + return operator=(static_cast&>(cu_ptr)); + } + + /** Copy assignment from a standard `unique_ptr` replaces the + currently-held object by a copy of the object held in the source container, + created using the source object's copy constructor or `Clone()` method. The + currently-held object (if any) is deleted. If the source container is empty + this one will be empty also after the assignment. Nothing happens if the + source and destination are the same container. */ + copyable_unique_ptr& operator=(const std::unique_ptr& src) { + if (&src != this) { + // can't be same ptr unless null + DRAKE_DEMAND((get() != src.get()) || !get()); + std::unique_ptr::reset(CopyOrNull(src.get())); + } + return *this; + } + + /** Copy assignment from a compatible standard `unique_ptr` replaces the + currently-held object by a copy of the object held in the source container, + created using the source object's copy constructor or `Clone()` method. The + currently-held object (if any) is deleted. If the source container is empty + this one will be empty also after the assignment. Nothing happens if the + source and destination are the same container. */ + template + copyable_unique_ptr& operator=(const std::unique_ptr& u_ptr) { + // can't be same ptr unless null + DRAKE_DEMAND((get() != u_ptr.get()) || !get()); + std::unique_ptr::reset(CopyOrNull(u_ptr.get())); + return *this; + } + + /** Move assignment replaces the currently-held object by the source object, + leaving the source empty. The currently-held object (if any) is deleted. + The instance is _not_ copied. Nothing happens if the source and destination + are the same containers. */ + copyable_unique_ptr& operator=(copyable_unique_ptr&& cu_ptr) noexcept { + std::unique_ptr::reset(cu_ptr.release()); + return *this; + } + + /** Move assignment replaces the currently-held object by the compatible + source object, leaving the source empty. The currently-held object (if any) + is deleted. The instance is _not_ copied. Nothing happens if the source and + destination are the same containers. */ + template + copyable_unique_ptr& operator=(copyable_unique_ptr&& cu_ptr) noexcept { + std::unique_ptr::reset(cu_ptr.release()); + return *this; + } + + /** Move assignment replaces the currently-held object by the source object, + leaving the source empty. The currently-held object (if any) is deleted. + The instance is _not_ copied. Nothing happens if the source and destination + are the same containers. */ + copyable_unique_ptr& operator=(std::unique_ptr&& u_ptr) noexcept { + std::unique_ptr::reset(u_ptr.release()); + return *this; + } + + /** Move assignment replaces the currently-held object by the compatible + source object, leaving the source empty. The currently-held object (if + any) is deleted. The instance is _not_ copied. Nothing happens if the source + and destination are the same containers. */ + template + copyable_unique_ptr& operator=(std::unique_ptr&& u_ptr) noexcept { + std::unique_ptr::reset(u_ptr.release()); + return *this; + } + + /**@}*/ + + /** @name Observers */ + /**@{*/ + + /** Return true if this container is empty, which is the state the container + is in immediately after default construction and various other + operations. */ + bool empty() const noexcept { return !(*this); } + + /** Return a const pointer to the contained object if any, or `nullptr`. + Note that this is different than `%get()` for the standard smart pointers + like `std::unique_ptr` which return a writable pointer. Use get_mutable() + here for that purpose. */ + const T* get() const noexcept { return std::unique_ptr::get(); } + + // TODO(SeanCurtis-TRI): Consider adding some debug assertions about whether + // T is const or not. If so, it would be nice to give feedback that calling + // the mutable version makes no sense. + /** Return a writable pointer to the contained object if any, or `nullptr`. + Note that you need write access to this container in order to get write + access to the object it contains. + + @warning If %copyable_unique_ptr is instantiated on a const template + parameter (e.g., `copyable_unique_ptr`), then get_mutable() + returns a const pointer. */ + T* get_mutable() noexcept { return std::unique_ptr::get(); } + + // TODO(15344) We need to shore up this const correctness hole. Rather than an + // is-a relationship, we need some alternative relationship that will provide + // the same functionality but not be upcastable. One possibility is to own + // an unique_ptr and forward various APIs. Another is to implement from + // scratch. The current "is-A" relationship was intended so that the + // copyable_unique_ptr could be used where unique_ptrs are used. What would + // the impact of such a change in the relationship be to Drake and Drake + // users? + + /** Return a const reference to the contained object. Note that this is + different from `std::unique_ptr::operator*()` which would return a non-const + reference (if `T` is non-const), even if the container itself is const. For + a const %copyable_unique_ptr will always return a const reference to its + contained value. + + @warning Currently %copyable_unique_ptr is a std::unique_ptr. As such, a + const copyable_unique_ptr can be upcast to a const unique_ptr and + the parent's behavior will provide a mutable reference. This is strongly + discouraged and will break as the implementation of this class changes to + shore up this gap in the const correctness protection. + + @pre `this != nullptr` reports `true`. */ + const T& operator*() const { return *get(); } + + /** Return a writable reference to the contained object (if T is itself not + const). Note that you need write access to this container in order to get + write access to the object it contains. + + We *strongly* recommend, that, if dereferencing a %copyable_unique_ptr + without the intention of mutating the underlying value, prefer to dereference + a *const* %copyable_unique_ptr (or use *my_ptr.get()) and not a mutable + %copyable_unique_ptr. As "copy-on-write" behavior is introduced in the + future, this recommended practice will prevent unwanted copies of the + underlying value. + + If %copyable_unique_ptr is instantiated on a const template parameter (e.g., + `copyable_unique_ptr`), then operator*() must return a const + reference. + + @pre `this != nullptr` reports `true`. */ + T& operator*() { return *get_mutable(); } + + /**@}*/ + private: + // The can_copy() and can_clone() methods must be defined within the + // copyable_unique_ptr class so that they have the same method access as + // the class does. That way we can use them to determine whether + // copyable_unique_ptr can get access. That precludes using helper classes + // like drake::is_cloneable because those may have different access due to an + // explicit friend declaration giving copyable_unique_ptr access to Foo's + // private business. The static_assert below ensures that at least one of + // these must return true. + + // SFINAE magic explanation. We're combining several tricks here: + // (1) "..." as a parameter type is a last choice; an exact type match is + // preferred in overload resolution. + // (2) Use a dummy template parameter U that is always just T but defers + // instantiation so that substitution failure is not fatal. + // (3) We construct a non-evaluated copy constructor and Clone method in + // templatized methods to prevent instantiation if the needed method + // doesn't exist or isn't accessible. If instantiation is successful, + // we produce an exact-match method that trumps the "..."-using method. + // (4) Make these constexpr so they can be used in static_assert. + + // True iff type T provides a copy constructor that is accessible from + // %copyable_unique_ptr. Invoke with `can_copy(1)`; the argument is used + // to select the right method. Note that if both `can_copy()` and + // `can_clone()` return true, we will prefer the copy constructor over the + // Clone() method. + static constexpr bool can_copy(...) { return false; } + + // If this instantiates successfully it will be the preferred method called + // when an integer argument is provided. + template + static constexpr std::enable_if_t< + std::is_same_v())), U>, + bool> + can_copy(int) { + return true; + } + + // True iff type T provides a `Clone()` method with the appropriate + // signature (see class documentation) that is accessible from + // %copyable_unique_ptr. Invoke with `can_clone(1)`; the argument is used + // to select the right method. + static constexpr bool can_clone(...) { return false; } + + // If this instantiates successfully it will be the preferred method called + // when an integer argument is provide. + template + static constexpr std::enable_if_t< + std::is_same_v().Clone()), + std::unique_ptr>>, + bool> + can_clone(int) { + return true; + } + + static_assert( + can_copy(1) || can_clone(1), + "copyable_unique_ptr can only be used with a 'copyable' class T, " + "requiring either a copy constructor or a Clone method of the form " + "'unique_ptr Clone() const', accessible to copyable_unique_ptr. " + "You may need to friend copyable_unique_ptr."); + + // Selects Clone iff there is no copy constructor and the Clone method is of + // the expected form. + template + static typename std::enable_if_t + CopyOrNullHelper(const U* ptr, int) { + return ptr->Clone().release(); + } + + // Default to copy constructor if present. + template + static + U* CopyOrNullHelper(const U* ptr, ...) { + return new U(*ptr); + } + + // If src is non-null, clone it; otherwise return nullptr. + static T* CopyOrNull(const T *ptr) { + return ptr ? CopyOrNullHelper(ptr, 1) : nullptr; + } +}; + +/** Output the system-dependent representation of the pointer contained + in a copyable_unique_ptr object. This is equivalent to `os << p.get();`. + @relates copyable_unique_ptr */ +template +std::basic_ostream& operator<<( + std::basic_ostream& os, + const copyable_unique_ptr& cu_ptr) { + os << cu_ptr.get(); + return os; +} + +} // namespace drake diff --git a/maliput_drake/include/drake/common/default_scalars.h b/maliput_drake/include/drake/common/default_scalars.h new file mode 100644 index 0000000..fc65ef4 --- /dev/null +++ b/maliput_drake/include/drake/common/default_scalars.h @@ -0,0 +1,231 @@ +#pragma once + +#include "drake/common/autodiff.h" +#include "drake/common/symbolic.h" + +// N.B. `CommonScalarPack` and `NonSymbolicScalarPack` in `systems_pybind.h` +// should be kept in sync with this file. + +/// @defgroup default_scalars Default Scalars +/// @ingroup technical_notes +/// @{ +/// Similar to the Eigen library, many classes in Drake use a template argument +/// to specify the numeric scalar type to use for computation. We typically +/// name that template argument ``. For an example, see the class +/// drake::math::RigidTransform. +/// +/// Most scalar-templated classes in Drake only support a small, fixed set of +/// scalar types: +/// - `double` (always) +/// - drake::AutoDiffXd (almost always) +/// - drake::symbolic::Expression (sometimes) +/// +/// When Drake documentation refers to "default scalars", it means all three +/// of the types above. +/// +/// Alternatively, reference to "default nonsymbolic scalars" means all except +/// `drake::symbolic::Expression`. +/// +/// For code within Drake, we offer Doxygen custom commands to document the +/// `` template parameter in the conventional cases: +/// +/// - @c \@tparam_default_scalar for the default scalars. +/// - @c \@tparam_nonsymbolic_scalar for the default nonsymbolic scalars. +/// - @c \@tparam_double_only for only the `double` scalar and nothing else. +/// +/// All three commands assume that the template parameter is named `T`. When +/// possible, prefer to use these commands instead of writing out a @c \@tparam +/// line manually. + +/// @name Class template instantiation macros +/// These macros either declare or define class template instantiations for +/// Drake's supported scalar types (see @ref default_scalars), either "default +/// scalars" or "default nonsymbolic scalars". Use the `DECLARE` macros only +/// in .h files; use the `DEFINE` macros only in .cc files. +/// +/// @param SomeType the template typename to instantiate, *including* the +/// leading `class` or `struct` keyword, but *excluding* the template argument +/// for the scalar type. +/// +/// Example `my_system.h`: +/// @code +/// #include "drake/common/default_scalars.h" +/// +/// namespace sample { +/// template +/// class MySystem final : public drake::systems::LeafSystem { +/// ... +/// }; +/// } // namespace sample +/// +/// DRAKE_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( +/// class ::sample::MySystem) +/// @endcode +/// +/// Example `my_system.cc`: +/// @code +/// #include "my_system.h" +/// +/// DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( +/// class ::sample::MySystem) +/// @endcode +/// +/// See also @ref system_scalar_conversion. +/// @{ + +/// Defines template instantiations for Drake's default scalars. +/// This should only be used in .cc files, never in .h files. +#define DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( \ + SomeType) \ +template SomeType; \ +template SomeType<::drake::AutoDiffXd>; \ +template SomeType<::drake::symbolic::Expression>; + +/// Defines template instantiations for Drake's default nonsymbolic scalars. +/// This should only be used in .cc files, never in .h files. +#define \ + DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS( \ + SomeType) \ +template SomeType; \ +template SomeType<::drake::AutoDiffXd>; + +/// Declares that template instantiations exist for Drake's default scalars. +/// This should only be used in .h files, never in .cc files. +#define DRAKE_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( \ + SomeType) \ +extern template SomeType; \ +extern template SomeType<::drake::AutoDiffXd>; \ +extern template SomeType<::drake::symbolic::Expression>; + +/// Declares that template instantiations exist for Drake's default nonsymbolic +/// scalars. This should only be used in .h files, never in .cc files. +#define \ + DRAKE_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS( \ + SomeType) \ +extern template SomeType; \ +extern template SomeType<::drake::AutoDiffXd>; + +/// @} + +/// @name Function template instantiation macros +/// These macros define template function instantiations for Drake's supported +/// scalar types (see @ref default_scalars), either "default scalars" or +/// "default nonsymbolic scalars". Use the `DEFINE` macros only in .cc files. +/// +/// @param FunctionPointersTuple a parenthesized, comma-separated list of +/// functions to instantiate (provided as function pointers), *including* +/// the template argument(s) for the scalar type. The template arguments `T` +/// and `U` will be provided by the macro; the function will be instantiated +/// for all combinations of `T` and `U` over the default scalars. +/// +/// Example `example.h`: +/// @code +/// #include "drake/common/default_scalars.h" +/// +/// namespace sample { +/// +/// template +/// double Func1(const T&); +/// +/// template +/// double Func2(const T&, const U&); +/// +/// template +/// class SomeClass { +/// ... +/// template +/// SomeClass cast() const; +/// ... +/// }; +/// +/// } // namespace sample +/// @endcode +/// +/// Example `example.cc`: +/// @code +/// #include "example.h" +/// +/// namespace sample { +/// +/// template +/// double Func1(const T&) { +/// ... +/// } +/// +/// template +/// double Func2(const T&, const U&) { +/// ... +/// } +/// +/// template +/// template +/// SomeClass::SomeClass::cast() const { +/// ... +/// }; +/// +/// // N.B. Place the macro invocation inside the functions' namespace. +/// DRAKE_DEFINE_FUNCTION_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS(( +/// &Func1, +/// &Func2, +/// &SomeClass::template cast +/// )) +/// +/// } // namespace sample +/// @endcode +/// +/// @note In the case of an overloaded function, the `&FunctionName` syntax +/// is ambiguous. To resolve the ambiguity, you will need a +/// static_cast. + +// N.B. Below we use "Make_Function_Pointers" (etc.) as function names and +// static variable names, which violates our function name style guide by mixing +// inner capitalization with underscores. However, we've done this on purpose, +// to avoid conflicts with user-provided code in the same translation unit. We +// can't use a namespace because we need to allow for easy friendship in case a +// member function does not have public access. + +/// Defines template instantiations for Drake's default scalars. +/// This should only be used in .cc files, never in .h files. +#define DRAKE_DEFINE_FUNCTION_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( \ + FunctionPointersTuple) \ +template \ +constexpr auto Make_Function_Pointers() { \ + return std::make_tuple FunctionPointersTuple ; \ +} \ +template \ +constexpr auto Make_Function_Pointers_Pack2() { \ + return std::tuple_cat(Make_Function_Pointers()...); \ +} \ +template \ +constexpr auto Make_Function_Pointers_Pack1() { \ + return std::tuple_cat(Make_Function_Pointers_Pack2()...); \ +} \ +static constexpr auto Function_Femplates __attribute__((used)) = \ + Make_Function_Pointers_Pack1< \ + double, \ + ::drake::AutoDiffXd, \ + ::drake::symbolic::Expression>(); + +/// Defines template instantiations for Drake's default nonsymbolic scalars. +/// This should only be used in .cc files, never in .h files. +#define \ +DRAKE_DEFINE_FUNCTION_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS( \ + FunctionPointersTuple) \ +template \ +constexpr auto Make_Function_Pointers_Nonsym() { \ + return std::make_tuple FunctionPointersTuple ; \ +} \ +template \ +constexpr auto Make_Function_Pointers_Nonsym_Pack2() { \ + return std::tuple_cat(Make_Function_Pointers_Nonsym()...); \ +} \ +template \ +constexpr auto Make_Function_Pointers_Nonsym_Pack1() { \ + return std::tuple_cat(Make_Function_Pointers_Nonsym_Pack2()...); \ +} \ +static constexpr auto Function_Templates_Nonsym __attribute__((used)) = \ + Make_Function_Pointers_Nonsym_Pack1< \ + double, \ + ::drake::AutoDiffXd>(); + +/// @} diff --git a/maliput_drake/include/drake/common/double_overloads.h b/maliput_drake/include/drake/common/double_overloads.h new file mode 100644 index 0000000..125f113 --- /dev/null +++ b/maliput_drake/include/drake/common/double_overloads.h @@ -0,0 +1,21 @@ +/// @file +/// Provides necessary operations on double to have it as a ScalarType in drake. + +#pragma once + +namespace drake { +/// Provides if-then-else expression for double. The value returned by the +/// if-then-else expression is @p v_then if @p f_cond is @c true. Otherwise, it +/// returns @p v_else. + +/// The semantics is similar but not exactly the same as C++'s conditional +/// expression constructed by its ternary operator, @c ?:. In +/// if_then_else(f_cond, v_then, v_else), both of @p v_then and @p +/// v_else are evaluated regardless of the evaluation of @p f_cond. In contrast, +/// only one of @p v_then or @p v_else is evaluated in C++'s conditional +/// expression f_cond ? v_then : v_else. +inline double if_then_else(bool f_cond, double v_then, double v_else) { + return f_cond ? v_then : v_else; +} + +} // namespace drake diff --git a/maliput_drake/include/drake/common/doxygen_cxx.h b/maliput_drake/include/drake/common/doxygen_cxx.h new file mode 100644 index 0000000..9c70add --- /dev/null +++ b/maliput_drake/include/drake/common/doxygen_cxx.h @@ -0,0 +1,2 @@ +/// @defgroup cxx C++ support features +/// @ingroup technical_notes diff --git a/maliput_drake/include/drake/common/drake_assert.h b/maliput_drake/include/drake/common/drake_assert.h new file mode 100644 index 0000000..51fdfad --- /dev/null +++ b/maliput_drake/include/drake/common/drake_assert.h @@ -0,0 +1,151 @@ +#pragma once + +#include + +/// @file +/// Provides Drake's assertion implementation. This is intended to be used +/// both within Drake and by other software. Drake's asserts can be armed +/// and disarmed independently from the system-wide asserts. + +#ifdef DRAKE_DOXYGEN_CXX +/// @p DRAKE_ASSERT(condition) is similar to the built-in @p assert(condition) +/// from the C++ system header @p . Unless Drake's assertions are +/// disarmed by the pre-processor definitions listed below, @p DRAKE_ASSERT +/// will evaluate @p condition and iff the value is false will trigger an +/// assertion failure with a message showing at least the condition text, +/// function name, file, and line. +/// +/// By default, assertion failures will :abort() the program. However, when +/// using the pydrake python bindings, assertion failures will instead throw a +/// C++ exception that causes a python SystemExit exception. +/// +/// Assertions are enabled or disabled using the following pre-processor macros: +/// +/// - If @p DRAKE_ENABLE_ASSERTS is defined, then @p DRAKE_ASSERT is armed. +/// - If @p DRAKE_DISABLE_ASSERTS is defined, then @p DRAKE_ASSERT is disarmed. +/// - If both macros are defined, then it is a compile-time error. +/// - If neither are defined, then NDEBUG governs assertions as usual. +/// +/// This header will define exactly one of either @p DRAKE_ASSERT_IS_ARMED or +/// @p DRAKE_ASSERT_IS_DISARMED to indicate whether @p DRAKE_ASSERT is armed. +/// +/// This header will define both `constexpr bool drake::kDrakeAssertIsArmed` +/// and `constexpr bool drake::kDrakeAssertIsDisarmed` globals. +/// +/// One difference versus the standard @p assert(condition) is that the +/// @p condition within @p DRAKE_ASSERT is always syntax-checked, even if +/// Drake's assertions are disarmed. +/// +/// Treat @p DRAKE_ASSERT like a statement -- it must always be used +/// in block scope, and must always be followed by a semicolon. +#define DRAKE_ASSERT(condition) +/// Like @p DRAKE_ASSERT, except that the expression must be void-valued; this +/// allows for guarding expensive assertion-checking subroutines using the same +/// macros as stand-alone assertions. +#define DRAKE_ASSERT_VOID(expression) +/// Evaluates @p condition and iff the value is false will trigger an assertion +/// failure with a message showing at least the condition text, function name, +/// file, and line. +#define DRAKE_DEMAND(condition) +/// Silences a "no return value" compiler warning by calling a function that +/// always raises an exception or aborts (i.e., a function marked noreturn). +/// Only use this macro at a point where (1) a point in the code is truly +/// unreachable, (2) the fact that it's unreachable is knowable from only +/// reading the function itself (and not, e.g., some larger design invariant), +/// and (3) there is a compiler warning if this macro were removed. The most +/// common valid use is with a switch-case-return block where all cases are +/// accounted for but the enclosing function is supposed to return a value. Do +/// *not* use this macro as a "logic error" assertion; it should *only* be used +/// to silence false positive warnings. When in doubt, throw an exception +/// manually instead of using this macro. +#define DRAKE_UNREACHABLE() +#else // DRAKE_DOXYGEN_CXX + +// Users should NOT set these; only this header should set them. +#ifdef DRAKE_ASSERT_IS_ARMED +# error Unexpected DRAKE_ASSERT_IS_ARMED defined. +#endif +#ifdef DRAKE_ASSERT_IS_DISARMED +# error Unexpected DRAKE_ASSERT_IS_DISARMED defined. +#endif + +// Decide whether Drake assertions are enabled. +#if defined(DRAKE_ENABLE_ASSERTS) && defined(DRAKE_DISABLE_ASSERTS) +# error Conflicting assertion toggles. +#elif defined(DRAKE_ENABLE_ASSERTS) +# define DRAKE_ASSERT_IS_ARMED +#elif defined(DRAKE_DISABLE_ASSERTS) || defined(NDEBUG) +# define DRAKE_ASSERT_IS_DISARMED +#else +# define DRAKE_ASSERT_IS_ARMED +#endif + +namespace drake { +namespace internal { +// Abort the program with an error message. +[[noreturn]] void Abort(const char* condition, const char* func, + const char* file, int line); +// Report an assertion failure; will either Abort(...) or throw. +[[noreturn]] void AssertionFailed(const char* condition, const char* func, + const char* file, int line); +} // namespace internal +namespace assert { +// Allows for specialization of how to bool-convert Conditions used in +// assertions, in case they are not intrinsically convertible. See +// symbolic_formula.h for an example use. This is a public interface to +// extend; it is intended to be specialized by unusual Scalar types that +// require special handling. +template +struct ConditionTraits { + static constexpr bool is_valid = std::is_convertible_v; + static bool Evaluate(const Condition& value) { + return value; + } +}; +} // namespace assert +} // namespace drake + +#define DRAKE_UNREACHABLE() \ + ::drake::internal::Abort( \ + "Unreachable code was reached?!", __func__, __FILE__, __LINE__) + +#define DRAKE_DEMAND(condition) \ + do { \ + typedef ::drake::assert::ConditionTraits< \ + typename std::remove_cv_t> Trait; \ + static_assert(Trait::is_valid, "Condition should be bool-convertible."); \ + if (!Trait::Evaluate(condition)) { \ + ::drake::internal::AssertionFailed( \ + #condition, __func__, __FILE__, __LINE__); \ + } \ + } while (0) + +#ifdef DRAKE_ASSERT_IS_ARMED +// Assertions are enabled. +namespace drake { +constexpr bool kDrakeAssertIsArmed = true; +constexpr bool kDrakeAssertIsDisarmed = false; +} // namespace drake +# define DRAKE_ASSERT(condition) DRAKE_DEMAND(condition) +# define DRAKE_ASSERT_VOID(expression) do { \ + static_assert( \ + std::is_convertible_v, \ + "Expression should be void."); \ + expression; \ + } while (0) +#else +// Assertions are disabled, so just typecheck the expression. +namespace drake { +constexpr bool kDrakeAssertIsArmed = false; +constexpr bool kDrakeAssertIsDisarmed = true; +} // namespace drake +# define DRAKE_ASSERT(condition) static_assert( \ + ::drake::assert::ConditionTraits< \ + typename std::remove_cv_t>::is_valid, \ + "Condition should be bool-convertible."); +# define DRAKE_ASSERT_VOID(expression) static_assert( \ + std::is_convertible_v, \ + "Expression should be void.") +#endif + +#endif // DRAKE_DOXYGEN_CXX diff --git a/maliput_drake/include/drake/common/drake_assertion_error.h b/maliput_drake/include/drake/common/drake_assertion_error.h new file mode 100644 index 0000000..b428474 --- /dev/null +++ b/maliput_drake/include/drake/common/drake_assertion_error.h @@ -0,0 +1,18 @@ +#pragma once + +#include +#include + +namespace drake { +namespace internal { + +// This is what DRAKE_ASSERT and DRAKE_DEMAND throw when our assertions are +// configured to throw. +class assertion_error : public std::runtime_error { + public: + explicit assertion_error(const std::string& what_arg) + : std::runtime_error(what_arg) {} +}; + +} // namespace internal +} // namespace drake diff --git a/maliput_drake/include/drake/common/drake_bool.h b/maliput_drake/include/drake/common/drake_bool.h new file mode 100644 index 0000000..9e3d73e --- /dev/null +++ b/maliput_drake/include/drake/common/drake_bool.h @@ -0,0 +1,96 @@ +#pragma once + +#include + +#include + +namespace drake { + +/// A traits struct that describes the return type of predicates over a scalar +/// type (named `T`). For example, a predicate that evaluates `double`s will +/// return a `bool`, but a predicate that evaluates symbolic::Expression will +/// return a symbolic::Formula. By default, the return type is inferred from +/// the type's comparison operator, but scalar types are permitted to +/// specialize this template for their needs. +template +struct scalar_predicate { + /// The return type of predicates over T. + using type = decltype(T() < T()); + + /// Whether `type` is `bool`. + static constexpr bool is_bool = std::is_same_v; +}; + +/// An alias for a boolean-like value, conditioned on the scalar type `T`. +/// In many cases this will be a synonym for `bool`, e.g., when `T = double`. +/// When `T = symbolic::Expression`, this is a synonym for `symbolic::Formula`. +/// This is a convenience abbreviation for scalar_predicate::type. +template +using boolean = typename scalar_predicate::type; + +/// Checks truth for all elements in matrix @p m. This is identical to +/// `Eigen::DenseBase::all()`, except this function allows for lazy evaluation, +/// so works even when scalar_predicate<>::is_bool does not hold. An empty +/// matrix returns true. +template +typename Derived::Scalar all(const Eigen::DenseBase& m) { + using Boolish = typename Derived::Scalar; + if (m.rows() == 0 || m.cols() == 0) { + // `all` holds vacuously when there is nothing to check. + return Boolish{true}; + } + return m.redux([](const Boolish& v1, const Boolish& v2) { return v1 && v2; }); +} + +/// Checks if unary predicate @p pred holds for all elements in the matrix @p m. +/// An empty matrix returns true. +template +boolean all_of( + const Eigen::MatrixBase& m, + const std::function( + const typename Derived::Scalar&)>& pred) { + return drake::all(m.unaryExpr(pred)); +} + +/// Checks truth for at least one element in matrix @p m. This is identical to +/// `Eigen::DenseBase::any()`, except this function allows for lazy evaluation, +/// so works even when scalar_predicate<>::is_bool does not hold. An empty +/// matrix returns false. +template +typename Derived::Scalar any(const Eigen::DenseBase& m) { + using Boolish = typename Derived::Scalar; + if (m.rows() == 0 || m.cols() == 0) { + // `any` is vacuously false when there is nothing to check. + return Boolish{false}; + } + return m.redux([](const Boolish& v1, const Boolish& v2) { return v1 || v2; }); +} + +/// Checks if unary predicate @p pred holds for at least one element in the +/// matrix @p m. An empty matrix returns false. +template +boolean any_of( + const Eigen::MatrixBase& m, + const std::function( + const typename Derived::Scalar&)>& pred) { + return any(m.unaryExpr(pred)); +} + +/// Checks that no elements of @p m are true. An empty matrix returns true. +template +typename Derived::Scalar none(const Eigen::MatrixBase& m) { + using Boolish = typename Derived::Scalar; + const auto negate = [](const Boolish& v) -> Boolish { return !v; }; + return drake::all(m.unaryExpr(negate)); +} + +/// Checks if unary predicate @p pred holds for no elements in the matrix @p m. +/// An empty matrix returns true. +template +boolean none_of( + const Eigen::MatrixBase& m, + const std::function( + const typename Derived::Scalar&)>& pred) { + return none(m.unaryExpr(pred)); +} +} // namespace drake diff --git a/maliput_drake/include/drake/common/drake_copyable.h b/maliput_drake/include/drake/common/drake_copyable.h new file mode 100644 index 0000000..2fb63e9 --- /dev/null +++ b/maliput_drake/include/drake/common/drake_copyable.h @@ -0,0 +1,66 @@ +#pragma once + +// ============================================================================ +// N.B. The spelling of the macro names between doc/Doxyfile_CXX.in and this +// file must be kept in sync! +// ============================================================================ + +/** @file +Provides careful macros to selectively enable or disable the special member +functions for copy-construction, copy-assignment, move-construction, and +move-assignment. + +http://en.cppreference.com/w/cpp/language/member_functions#Special_member_functions + +When enabled via these macros, the `= default` implementation is provided. +Code that needs custom copy or move functions should not use these macros. +*/ + +/** DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN deletes the special member functions for +copy-construction, copy-assignment, move-construction, and move-assignment. +Drake's Doxygen is customized to render the deletions in detail, with +appropriate comments. Invoke this macro in the public section of the class +declaration, e.g.: +
+class Foo {
+ public:
+  DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Foo)
+
+  // ...
+};
+
+*/ +#define DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Classname) \ + Classname(const Classname&) = delete; \ + void operator=(const Classname&) = delete; \ + Classname(Classname&&) = delete; \ + void operator=(Classname&&) = delete; + +/** DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN defaults the special member +functions for copy-construction, copy-assignment, move-construction, and +move-assignment. This macro should be used only when copy-construction and +copy-assignment defaults are well-formed. Note that the defaulted move +functions could conceivably still be ill-formed, in which case they will +effectively not be declared or used -- but because the copy constructor exists +the type will still be MoveConstructible. Drake's Doxygen is customized to +render the functions in detail, with appropriate comments. Invoke this macro +in the public section of the class declaration, e.g.: +
+class Foo {
+ public:
+  DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Foo)
+
+  // ...
+};
+
+*/ +#define DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Classname) \ + Classname(const Classname&) = default; \ + Classname& operator=(const Classname&) = default; \ + Classname(Classname&&) = default; \ + Classname& operator=(Classname&&) = default; \ + /* Fails at compile-time if default-copy doesn't work. */ \ + static void DRAKE_COPYABLE_DEMAND_COPY_CAN_COMPILE() { \ + (void) static_cast(&Classname::operator=); \ + } diff --git a/maliput_drake/include/drake/common/drake_deprecated.h b/maliput_drake/include/drake/common/drake_deprecated.h new file mode 100644 index 0000000..5ce6328 --- /dev/null +++ b/maliput_drake/include/drake/common/drake_deprecated.h @@ -0,0 +1,65 @@ +#pragma once + +/** @file +Provides a portable macro for use in generating compile-time warnings for +use of code that is permitted but discouraged. */ + +#ifdef DRAKE_DOXYGEN_CXX +/** Use `DRAKE_DEPRECATED("removal_date", "message")` to discourage use of +certain APIs. It can be used on classes, typedefs, variables, non-static data +members, functions, arguments, enumerations, and template specializations. When +code refers to the deprecated item, a compile time warning will be issued +displaying the given message, preceded by "DRAKE DEPRECATED: ". The Doxygen API +reference will show that the API is deprecated, along with the message. + +This is typically used for constructs that have been replaced by something +better and it is good practice to suggest the appropriate replacement in the +deprecation message. Deprecation warnings are conventionally used to convey to +users that a feature they are depending on will be removed in a future release. + +Every deprecation notice must include a date (as YYYY-MM-DD string) where the +deprecated item is planned for removal. Future commits may change the date +(e.g., delaying the removal) but should generally always reflect the best +current expectation for removal. + +Absent any other particular need, we prefer to use a deprecation period of +three months by default, often rounded up to the next first of the month. So +for code announced as deprecated on 2018-01-22 the removal_date would nominally +be set to 2018-05-01. + +Try to keep the date string immediately after the DRAKE_DEPRECATED macro name, +even if the message itself must be wrapped to a new line: +@code + DRAKE_DEPRECATED("2038-01-19", + "foo is being replaced with a safer, const-method named bar().") + int foo(); +@endcode + +Sample uses: @code + // Attribute comes *before* declaration of a deprecated function or variable; + // no semicolon is allowed. + DRAKE_DEPRECATED("2038-01-19", "f() is slow; use g() instead.") + int f(int arg); + + // Attribute comes *after* struct, class, enum keyword. + class DRAKE_DEPRECATED("2038-01-19", "Use MyNewClass instead.") + MyClass { + }; + + // Type alias goes before the '='. + using NewType + DRAKE_DEPRECATED("2038-01-19", "Use NewType instead.") + = OldType; +@endcode +*/ +#define DRAKE_DEPRECATED(removal_date, message) + +#else // DRAKE_DOXYGEN_CXX + +#define DRAKE_DEPRECATED(removal_date, message) \ + [[deprecated( \ + "\nDRAKE DEPRECATED: " message \ + "\nThe deprecated code will be removed from Drake" \ + " on or after " removal_date ".")]] + +#endif // DRAKE_DOXYGEN_CXX diff --git a/maliput_drake/include/drake/common/drake_marker.h b/maliput_drake/include/drake/common/drake_marker.h new file mode 100644 index 0000000..3658d7a --- /dev/null +++ b/maliput_drake/include/drake/common/drake_marker.h @@ -0,0 +1,16 @@ +#pragma once + +/// @file +/// This is an internal (not installed) header. Do not use this outside of +/// `find_resource.cc`. + +namespace drake { +namespace internal { + +// Provides a concrete object to ensure that this marker library is linked. +// This returns a simple magic constant to ensure the library was loaded +// correctly. +int drake_marker_lib_check(); + +} // namespace internal +} // namespace drake diff --git a/maliput_drake/include/drake/common/drake_path.h b/maliput_drake/include/drake/common/drake_path.h new file mode 100644 index 0000000..4fec068 --- /dev/null +++ b/maliput_drake/include/drake/common/drake_path.h @@ -0,0 +1,20 @@ +#pragma once + +#include +#include + +#include "drake/common/drake_deprecated.h" + +namespace drake { + +/// (Advanced) Returns the fully-qualified path to the first folder containing +/// Drake resources as located by FindResource, or nullopt if none is found. +/// For example `${result}/examples/pendulum/Pendulum.urdf` would be the path +/// to the Pendulum example's URDF resource. +/// +/// Most users should prefer FindResource() or FindResourceOrThrow() to locate +/// Drake resources for a specific resource filename. This method only exists +/// for legacy compatibility reasons, and might eventually be removed. +std::optional MaybeGetDrakePath(); + +} // namespace drake diff --git a/maliput_drake/include/drake/common/drake_throw.h b/maliput_drake/include/drake/common/drake_throw.h new file mode 100644 index 0000000..bb4bae8 --- /dev/null +++ b/maliput_drake/include/drake/common/drake_throw.h @@ -0,0 +1,54 @@ +#pragma once + +#include + +#include "drake/common/drake_assert.h" + +/// @file +/// Provides a convenient wrapper to throw an exception when a condition is +/// unmet. This is similar to an assertion, but uses exceptions instead of +/// ::abort(), and cannot be disabled. + +namespace drake { +namespace internal { +// Throw an error message. +[[noreturn]] void Throw(const char* condition, const char* func, + const char* file, int line); + +template +constexpr void DrakeThrowUnlessWasUsedWithRawPointer() {} +template<> +[[deprecated("\nDRAKE DEPRECATED: When using DRAKE_THROW_UNLESS on a raw" +" pointer, always write out DRAKE_THROW_UNLESS(foo != nullptr), do not write" +" DRAKE_THROW_UNLESS(foo) and rely on implicit pointer-to-bool conversion." +"\nThe deprecated code will be removed from Drake on or after 2021-12-01.")]] +constexpr void DrakeThrowUnlessWasUsedWithRawPointer() {} + +} // namespace internal +} // namespace drake + +/// Evaluates @p condition and iff the value is false will throw an exception +/// with a message showing at least the condition text, function name, file, +/// and line. +/// +/// The condition must not be a pointer, where we'd implicitly rely on its +/// nullness. Instead, always write out "!= nullptr" to be precise. +/// +/// Correct: `DRAKE_THROW_UNLESS(foo != nullptr);` +/// Incorrect: `DRAKE_THROW_UNLESS(foo);` +/// +/// Because this macro is intended to provide a useful exception message to +/// users, we should err on the side of extra detail about the failure. The +/// meaning of "foo" isolated within error message text does not make it +/// clear that a null pointer is the proximate cause of the problem. +#define DRAKE_THROW_UNLESS(condition) \ + do { \ + typedef ::drake::assert::ConditionTraits< \ + typename std::remove_cv_t> Trait; \ + static_assert(Trait::is_valid, "Condition should be bool-convertible."); \ + ::drake::internal::DrakeThrowUnlessWasUsedWithRawPointer< \ + std::is_pointer_v>(); \ + if (!Trait::Evaluate(condition)) { \ + ::drake::internal::Throw(#condition, __func__, __FILE__, __LINE__); \ + } \ + } while (0) diff --git a/maliput_drake/include/drake/common/dummy_value.h b/maliput_drake/include/drake/common/dummy_value.h new file mode 100644 index 0000000..b9c616a --- /dev/null +++ b/maliput_drake/include/drake/common/dummy_value.h @@ -0,0 +1,35 @@ +#pragma once + +#include + +namespace drake { + +/// Provides a "dummy" value for a ScalarType -- a value that is unlikely to be +/// mistaken for a purposefully-computed value, useful for initializing a value +/// before the true result is available. +/// +/// Defaults to using std::numeric_limits::quiet_NaN when available; it is a +/// compile-time error to call the unspecialized dummy_value::get() when +/// quiet_NaN is unavailable. +/// +/// See autodiff_overloads.h to use this with Eigen's AutoDiffScalar. +template +struct dummy_value { + static constexpr T get() { + static_assert(std::numeric_limits::has_quiet_NaN, + "Custom scalar types should specialize this struct"); + return std::numeric_limits::quiet_NaN(); + } +}; + +template <> +struct dummy_value { + static constexpr int get() { + // D is for "Dummy". We assume as least 32 bits (per cppguide) -- if `int` + // is larger than 32 bits, this will leave some fraction of the bytes zero + // instead of 0xDD, but that's okay. + return 0xDDDDDDDD; + } +}; + +} // namespace drake diff --git a/maliput_drake/include/drake/common/eigen_autodiff_types.h b/maliput_drake/include/drake/common/eigen_autodiff_types.h new file mode 100644 index 0000000..cfe4b11 --- /dev/null +++ b/maliput_drake/include/drake/common/eigen_autodiff_types.h @@ -0,0 +1,38 @@ +#pragma once + +/// @file +/// This file contains abbreviated definitions for certain uses of +/// AutoDiffScalar that are commonly used in Drake. +/// @see also eigen_types.h + +#ifndef DRAKE_COMMON_AUTODIFF_HEADER +// TODO(soonho-tri): Change to #error. +#warning Do not directly include this file. Include "drake/common/autodiff.h". +#endif + +#include + +#include + +#include "drake/common/eigen_types.h" + +namespace drake { + +/// An autodiff variable with a dynamic number of partials. +using AutoDiffXd = Eigen::AutoDiffScalar; + +// TODO(hongkai-dai): Recursive template to get arbitrary gradient order. + +/// An autodiff variable with `num_vars` partials. +template +using AutoDiffd = Eigen::AutoDiffScalar >; + +/// A vector of `rows` autodiff variables, each with `num_vars` partials. +template +using AutoDiffVecd = Eigen::Matrix, rows, 1>; + +/// A dynamic-sized vector of autodiff variables, each with a dynamic-sized +/// vector of partials. +typedef AutoDiffVecd AutoDiffVecXd; + +} // namespace drake diff --git a/maliput_drake/include/drake/common/eigen_stl_types.h b/maliput_drake/include/drake/common/eigen_stl_types.h new file mode 100644 index 0000000..bf1c097 --- /dev/null +++ b/maliput_drake/include/drake/common/eigen_stl_types.h @@ -0,0 +1,38 @@ +#pragma once + +/// @file +/// This file contains definitions for using Eigen with the STL. +/// See http://eigen.tuxfamily.org/dox-devel/group__TopicStlContainers.html. +/// @see eigen_types.h + +#include +#include +#include +#include +#include + +#include +#include + +namespace drake { + +/// A std::map that uses Eigen::aligned_allocator so that the +/// contained types may be fixed-size Eigen values. +template +using eigen_aligned_std_map = + std::map, + Eigen::aligned_allocator>>; + +/// A std::unordered_map that uses Eigen::aligned_allocator so that the +/// contained types may be fixed-size Eigen values. +template +using eigen_aligned_std_unordered_map = + std::unordered_map, std::equal_to, + Eigen::aligned_allocator>>; + +/// A std::vector that uses Eigen::aligned_allocator so that the contained +/// types may be fixed-size Eigen values. +template +using eigen_aligned_std_vector = std::vector>; + +} // namespace drake diff --git a/maliput_drake/include/drake/common/eigen_types.h b/maliput_drake/include/drake/common/eigen_types.h new file mode 100644 index 0000000..5f9ae0b --- /dev/null +++ b/maliput_drake/include/drake/common/eigen_types.h @@ -0,0 +1,465 @@ +#pragma once + +/// @file +/// This file contains abbreviated definitions for certain specializations of +/// Eigen::Matrix that are commonly used in Drake. +/// These convenient definitions are templated on the scalar type of the Eigen +/// object. While Drake uses `` for scalar types across the entire code base +/// we decided in this file to use `` to be more consistent with the +/// usage of `` in Eigen's code base. +/// @see also eigen_autodiff_types.h + +#include + +#include + +#include "drake/common/constants.h" +#include "drake/common/drake_assert.h" +#include "drake/common/drake_copyable.h" + +namespace drake { + +/// The empty column vector (zero rows, one column), templated on scalar type. +template +using Vector0 = Eigen::Matrix; + +/// A column vector of size 1 (that is, a scalar), templated on scalar type. +template +using Vector1 = Eigen::Matrix; + +/// A column vector of size 1 of doubles. +using Vector1d = Eigen::Matrix; + +/// A column vector of size 2, templated on scalar type. +template +using Vector2 = Eigen::Matrix; + +/// A column vector of size 3, templated on scalar type. +template +using Vector3 = Eigen::Matrix; + +/// A column vector of size 4, templated on scalar type. +template +using Vector4 = Eigen::Matrix; + +/// A column vector of size 6. +template +using Vector6 = Eigen::Matrix; + +/// A column vector of size 6 of doubles. +using Vector6d = Eigen::Matrix; + +/// A column vector templated on the number of rows. +template +using Vector = Eigen::Matrix; + +/// A column vector of any size, templated on scalar type. +template +using VectorX = Eigen::Matrix; + +/// A vector of dynamic size templated on scalar type, up to a maximum of 6 +/// elements. +template +using VectorUpTo6 = Eigen::Matrix; + +/// A row vector of size 2, templated on scalar type. +template +using RowVector2 = Eigen::Matrix; + +/// A row vector of size 3, templated on scalar type. +template +using RowVector3 = Eigen::Matrix; + +/// A row vector of size 4, templated on scalar type. +template +using RowVector4 = Eigen::Matrix; + +/// A row vector of size 6. +template +using RowVector6 = Eigen::Matrix; + +/// A row vector templated on the number of columns. +template +using RowVector = Eigen::Matrix; + +/// A row vector of any size, templated on scalar type. +template +using RowVectorX = Eigen::Matrix; + + +/// A matrix of 2 rows and 2 columns, templated on scalar type. +template +using Matrix2 = Eigen::Matrix; + +/// A matrix of 3 rows and 3 columns, templated on scalar type. +template +using Matrix3 = Eigen::Matrix; + +/// A matrix of 4 rows and 4 columns, templated on scalar type. +template +using Matrix4 = Eigen::Matrix; + +/// A matrix of 6 rows and 6 columns, templated on scalar type. +template +using Matrix6 = Eigen::Matrix; + +/// A matrix of 2 rows, dynamic columns, templated on scalar type. +template +using Matrix2X = Eigen::Matrix; + +/// A matrix of 3 rows, dynamic columns, templated on scalar type. +template +using Matrix3X = Eigen::Matrix; + +/// A matrix of 4 rows, dynamic columns, templated on scalar type. +template +using Matrix4X = Eigen::Matrix; + +/// A matrix of 6 rows, dynamic columns, templated on scalar type. +template +using Matrix6X = Eigen::Matrix; + +/// A matrix of dynamic size, templated on scalar type. +template +using MatrixX = Eigen::Matrix; + +/// A matrix of dynamic size templated on scalar type, up to a maximum of 6 rows +/// and 6 columns. Rectangular matrices, with different number of rows and +/// columns, are allowed. +template +using MatrixUpTo6 = +Eigen::Matrix; + +/// A matrix of 6 rows and dynamic column size up to a maximum of 6, templated +/// on scalar type. +template +using Matrix6xUpTo6 = Eigen::Matrix; + +/// A quaternion templated on scalar type. +template +using Quaternion = Eigen::Quaternion; + +/// An AngleAxis templated on scalar type. +template +using AngleAxis = Eigen::AngleAxis; + +/// An Isometry templated on scalar type. +template +using Isometry3 = Eigen::Transform; + +/// A translation in 3D templated on scalar type. +template +using Translation3 = Eigen::Translation; + +/// A column vector consisting of one twist. +template +using TwistVector = Eigen::Matrix; + +/// A matrix with one twist per column, and dynamically many columns. +template +using TwistMatrix = Eigen::Matrix; + +/// A six-by-six matrix. +template +using SquareTwistMatrix = Eigen::Matrix; + +/// A column vector consisting of one wrench (spatial force) = `[r X f; f]`, +/// where f is a force (translational force) applied at a point `P` and `r` is +/// the position vector from a point `O` (called the "moment center") to point +/// `P`. +template +using WrenchVector = Eigen::Matrix; + +/// EigenSizeMinPreferDynamic::value gives the min between compile-time +/// sizes @p a and @p b. 0 has absolute priority, followed by 1, followed by +/// Dynamic, followed by other finite values. +/// +/// Note that this is a type-trait version of EIGEN_SIZE_MIN_PREFER_DYNAMIC +/// macro in "Eigen/Core/util/Macros.h". +template +struct EigenSizeMinPreferDynamic { + // clang-format off + static constexpr int value = (a == 0 || b == 0) ? 0 : + (a == 1 || b == 1) ? 1 : + (a == Eigen::Dynamic || b == Eigen::Dynamic) ? Eigen::Dynamic : + a <= b ? a : b; + // clang-format on +}; + +/// EigenSizeMinPreferFixed is a variant of EigenSizeMinPreferDynamic. The +/// difference is that finite values now have priority over Dynamic, so that +/// EigenSizeMinPreferFixed<3, Dynamic>::value gives 3. +/// +/// Note that this is a type-trait version of EIGEN_SIZE_MIN_PREFER_FIXED macro +/// in "Eigen/Core/util/Macros.h". +template +struct EigenSizeMinPreferFixed { + // clang-format off + static constexpr int value = (a == 0 || b == 0) ? 0 : + (a == 1 || b == 1) ? 1 : + (a == Eigen::Dynamic && b == Eigen::Dynamic) ? Eigen::Dynamic : + (a == Eigen::Dynamic) ? b : + (b == Eigen::Dynamic) ? a : + a <= b ? a : b; + // clang-format on +}; + +/// MultiplyEigenSizes gives a * b if both of a and b are fixed +/// sizes. Otherwise it gives Eigen::Dynamic. +template +struct MultiplyEigenSizes { + static constexpr int value = + (a == Eigen::Dynamic || b == Eigen::Dynamic) ? Eigen::Dynamic : a * b; +}; + +/* + * Determines if a type is derived from EigenBase<> (e.g. ArrayBase<>, + * MatrixBase<>). + */ +template +struct is_eigen_type : std::is_base_of, Derived> {}; + +/* + * Determines if an EigenBase<> has a specific scalar type. + */ +template +struct is_eigen_scalar_same + : std::bool_constant< + is_eigen_type::value && + std::is_same_v> {}; + +/* + * Determines if an EigenBase<> type is a compile-time (column) vector. + * This will not check for run-time size. + */ +template +struct is_eigen_vector + : std::bool_constant::value && + Derived::ColsAtCompileTime == 1> {}; + +/* + * Determines if an EigenBase<> type is a compile-time (column) vector of a + * scalar type. This will not check for run-time size. + */ +template +struct is_eigen_vector_of + : std::bool_constant< + is_eigen_scalar_same::value && + is_eigen_vector::value> {}; + +// TODO(eric.cousineau): A 1x1 matrix will be disqualified in this case, and +// this logic will qualify it as a vector. Address the downstream logic if this +// becomes an issue. +/* + * Determines if a EigenBase<> type is a compile-time non-column-vector matrix + * of a scalar type. This will not check for run-time size. + * @note For an EigenBase<> of the correct Scalar type, this logic is + * exclusive to is_eigen_vector_of<> such that distinct specializations are not + * ambiguous. + */ +template +struct is_eigen_nonvector_of + : std::bool_constant< + is_eigen_scalar_same::value && + !is_eigen_vector::value> {}; + +// TODO(eric.cousineau): Add alias is_eigen_matrix_of = is_eigen_scalar_same if +// appropriate. + +/// This wrapper class provides a way to write non-template functions taking raw +/// pointers to Eigen objects as parameters while limiting the number of copies, +/// similar to `Eigen::Ref`. Internally, it keeps an instance of `Eigen::Ref` +/// and provides access to it via `operator*` and `operator->`. As with ordinary +/// pointers, these operators do not perform nullptr checks in Release builds. +/// User-facing APIs should check for nullptr explicitly. +/// +/// The primary motivation of this class is to follow GSG's +/// "output arguments should be pointers" convention while taking advantage +/// of using `Eigen::Ref`. It can also be used to pass optional Eigen objects +/// since %EigenPtr, unlike `Eigen::Ref`, can be null. +/// +/// Some examples: +/// +/// @code +/// // This function is taking an Eigen::Ref of a matrix and modifies it in +/// // the body. This violates GSG's pointer convention for output parameters. +/// void foo(Eigen::Ref M) { +/// M(0, 0) = 0; +/// } +/// // At Call-site, we have: +/// foo(M); +/// foo(M.block(0, 0, 2, 2)); +/// +/// // We can rewrite the above function into the following using EigenPtr. +/// void foo(EigenPtr M) { +/// DRAKE_THROW_UNLESS(M != nullptr); // If you want a Release-build check. +/// (*M)(0, 0) = 0; +/// } +/// // Note that, call sites should be changed to: +/// foo(&M); +/// +/// // We need tmp to avoid taking the address of a temporary object such as the +/// // return value of .block(). +/// auto tmp = M.block(0, 0, 2, 2); +/// foo(&tmp); +/// @endcode +/// +/// Notice that methods taking an %EigenPtr can mutate the entries of a matrix +/// as in method `foo()` in the example code above, but cannot change its size. +/// This is because `operator*` and `operator->` return an `Eigen::Ref` +/// object and only plain matrices/arrays can be resized and not expressions. +/// This **is** the desired behavior, since resizing the block of a matrix or +/// even a more general expression should not be allowed. If you do want to be +/// able to resize a mutable matrix argument, then you must pass it as a +/// `Matrix*`, like so: +/// @code +/// void bar(Eigen::MatrixXd* M) { +/// DRAKE_THROW_UNLESS(M != nullptr); +/// // In this case this method only works with 4x3 matrices. +/// if (M->rows() != 4 && M->cols() != 3) { +/// M->resize(4, 3); +/// } +/// (*M)(0, 0) = 0; +/// } +/// @endcode +/// +/// @note This class provides a way to avoid the `const_cast` hack introduced in +/// Eigen's +/// documentation. +template +class EigenPtr { + public: + typedef Eigen::Ref RefType; + + EigenPtr() : EigenPtr(nullptr) {} + + /// Overload for `nullptr`. + // NOLINTNEXTLINE(runtime/explicit) This conversion is desirable. + EigenPtr(std::nullptr_t) {} + + /// Copy constructor results in a _reference_ to the given matrix type. + EigenPtr(const EigenPtr& other) { assign(other); } + + /// Constructs with a reference to another matrix type. + /// May be `nullptr`. + template + // NOLINTNEXTLINE(runtime/explicit) This conversion is desirable. + EigenPtr(PlainObjectTypeIn* m) { + if (m) { + m_.set_value(m); + } + } + + /// Constructs from another %EigenPtr. + template + // NOLINTNEXTLINE(runtime/explicit) This conversion is desirable. + EigenPtr(const EigenPtr& other) { + // Cannot directly construct `m_` from `other.m_`. + assign(other); + } + + /// Copy assignment results in a _reference_ to the given matrix type. + EigenPtr& operator=(const EigenPtr& other) { + // We must explicitly override this version of operator=. + // The template below will not take precedence over this one. + return assign(other); + } + + template + EigenPtr& operator=(const EigenPtr& other) { + return assign(other); + } + + /// @pre The pointer is not null (enforced in Debug builds only). + RefType& operator*() const { return get_reference(); } + + /// @pre The pointer is not null (enforced in Debug builds only). + RefType* operator->() const { return &get_reference(); } + + /// Returns whether or not this contains a valid reference. + operator bool() const { return is_valid(); } + + bool operator==(std::nullptr_t) const { return !is_valid(); } + + bool operator!=(std::nullptr_t) const { return is_valid(); } + + private: + // Simple reassignable container without requirement of heap allocation. + // This is used because `drake::optional<>` does not work with `Eigen::Ref<>` + // because `Ref` deletes the necessary `operator=` overload for + // `std::is_copy_assignable`. + class ReassignableRef { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(ReassignableRef) + ReassignableRef() {} + ~ReassignableRef() { + reset(); + } + + // Reset value to null. + void reset() { + if (has_value_) { + raw_value().~RefType(); + has_value_ = false; + } + } + + // Set value. + template + void set_value(PlainObjectTypeIn* value_in) { + if (has_value_) { + raw_value().~RefType(); + } + new (&raw_value()) RefType(*value_in); + has_value_ = true; + } + + // Access to value. + RefType& value() { + DRAKE_ASSERT(has_value()); + return raw_value(); + } + + // Indicates if it has a value. + bool has_value() const { return has_value_; } + + private: + // Unsafe access to value. + RefType& raw_value() { return reinterpret_cast(storage_); } + + bool has_value_{}; + typename std::aligned_storage::type + storage_; + }; + + // Use mutable, reassignable ref to permit pointer-like semantics (with + // ownership) on the stack. + mutable ReassignableRef m_; + + // Consolidate assignment here, so that both the copy constructor and the + // construction from another type may be used. + template + EigenPtr& assign(const EigenPtr& other) { + if (other) { + m_.set_value(&(*other)); + } else { + m_.reset(); + } + return *this; + } + + // Consolidate getting a reference here. + RefType& get_reference() const { + // Keep this tiny so it inlines. + DRAKE_ASSERT(m_.has_value()); + return m_.value(); + } + + bool is_valid() const { + return m_.has_value(); + } +}; + +} // namespace drake diff --git a/maliput_drake/include/drake/common/extract_double.h b/maliput_drake/include/drake/common/extract_double.h new file mode 100644 index 0000000..94ff375 --- /dev/null +++ b/maliput_drake/include/drake/common/extract_double.h @@ -0,0 +1,55 @@ +#pragma once + +#include + +#include "drake/common/drake_deprecated.h" +#include "drake/common/eigen_types.h" +#include "drake/common/nice_type_name.h" + +namespace drake { + +/// Converts a ScalarType value to a double, failing at runtime (not compile +/// time) if the type cannot be converted to a double. +/// +/// This function is useful for writing ScalarType-generic code that (1) can +/// reasonably discard any supplemental scalar data, e.g., the derivatives of an +/// AutoDiffScalar, and (2) is reasonable to fail at runtime if the extraction +/// fails. +/// +/// The default implementation throws an exception. ScalarTypes that can hold a +/// numeric value must overload this method to provide an appropriate +/// extraction. An overload for `double` is already provided. +/// +/// See autodiff_overloads.h to use this with Eigen's AutoDiffScalar. +/// See symbolic_expression.h to use this with symbolic::Expression. +template +DRAKE_DEPRECATED("2020-08-01", + "Provide a specific overload of ExtractDoubleOrThrow for any " + "type that really is sensible at compile time and should " + "defer failure to runtime; this version was too generic.") +typename std::enable_if_t::value, double> +ExtractDoubleOrThrow(const T&) { + throw std::runtime_error(NiceTypeName::Get() + + " cannot be converted to a double"); +} + +/// Returns @p scalar as a double. Never throws. +inline double ExtractDoubleOrThrow(double scalar) { return scalar; } + +/// Returns @p matrix as an Eigen::Matrix with the same size +/// allocation as @p matrix. Calls ExtractDoubleOrThrow on each element of the +/// matrix, and therefore throws if any one of the extractions fail. +template +typename std::enable_if_t< + std::is_same_v, + Eigen::Matrix> +ExtractDoubleOrThrow(const Eigen::MatrixBase& matrix) { + return matrix.unaryExpr([](const typename Derived::Scalar& value) { + return ExtractDoubleOrThrow(value); + }) + .eval(); +} + +} // namespace drake diff --git a/maliput_drake/include/drake/common/filesystem.h b/maliput_drake/include/drake/common/filesystem.h new file mode 100644 index 0000000..ac8cfae --- /dev/null +++ b/maliput_drake/include/drake/common/filesystem.h @@ -0,0 +1,28 @@ +#pragma once + +// Alias drake::filesystem to either std::filesystem or ghc::filesystem. +// +// The drake::filesystem support is intended ONLY for use within Drake's *.cc +// files -- it is not a public dependency of Drake; do not include this file +// from Drake header files. +// +// Note that until Apple ships a working std::filesystem implementation, we +// need to force-disable it. Similarly, GCC prior to 9 requires arcane linker +// flags, so we need to exclude it as well. +// +// Keep this if-sequence in sync with drake/common/filesystem.cc. +#if __has_include() && !( \ + defined(__APPLE__) || \ + (!defined(__clang__) && defined(__GNUC__) && (__GNUC__ < 9))) + +#include +namespace drake { namespace filesystem = std::filesystem; } + +#else + +#define GHC_FILESYSTEM_FWD +#include "ghc/filesystem.hpp" +#undef GHC_FILESYSTEM_FWD +namespace drake { namespace filesystem = ghc::filesystem; } + +#endif diff --git a/maliput_drake/include/drake/common/find_loaded_library.h b/maliput_drake/include/drake/common/find_loaded_library.h new file mode 100644 index 0000000..a200b6b --- /dev/null +++ b/maliput_drake/include/drake/common/find_loaded_library.h @@ -0,0 +1,14 @@ +#pragma once + +#include +#include + +namespace drake { + +/// This function returns the absolute path of the library with the name +/// `library_name` if that library was loaded in the current running +/// process. Otherwise it returns an empty optional. +std::optional LoadedLibraryPath(const std::string& library_name); + + +} // namespace drake diff --git a/maliput_drake/include/drake/common/find_resource.h b/maliput_drake/include/drake/common/find_resource.h new file mode 100644 index 0000000..672c315 --- /dev/null +++ b/maliput_drake/include/drake/common/find_resource.h @@ -0,0 +1,120 @@ +#pragma once + +#include +#include +#include + +#include "drake/common/drake_assert.h" +#include "drake/common/drake_copyable.h" + +namespace drake { + +/// Models the outcome of drake::FindResource. After a call to FindResource, +/// typical calling code would use get_absolute_path_or_throw(). +/// Alternatively, get_absolute_path() will return an `optional`, which +/// can be manually checked to contain a value before using the path. If the +/// resource was not found, get_error_message() will contain an error message. +/// +/// For a given FindResourceResult instance, exactly one of get_absolute_path() +/// or get_error_message() will contain a value. (Similarly, exactly one of +/// them will not contain a value.) +class FindResourceResult { + public: + DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(FindResourceResult); + + /// Returns the absolute path to the resource, iff the resource was found. + std::optional get_absolute_path() const; + + /// Either returns the get_absolute_path() iff the resource was found, + /// or else throws std::exception. + std::string get_absolute_path_or_throw() const; + + /// Returns the error message, iff the resource was not found. + /// The string will never be empty; only the optional can be empty. + std::optional get_error_message() const; + + /// Returns the resource_path asked of FindResource. + /// (This may be empty only in the make_empty() case.) + std::string get_resource_path() const; + + /// Returns a success result (the requested resource was found). + /// @pre neither string parameter is empty + /// @param resource_path the value passed to FindResource + /// @param base_path an absolute base path that precedes resource_path + static FindResourceResult make_success( + std::string resource_path, std::string absolute_path); + + /// Returns an error result (the requested resource was NOT found). + /// @pre neither string parameter is empty + /// @param resource_path the value passed to FindResource + static FindResourceResult make_error( + std::string resource_path, std::string error_message); + + /// Returns an empty error result (no requested resource). + static FindResourceResult make_empty(); + + private: + FindResourceResult() = default; + void CheckInvariants(); + + // The path as requested by the user. + std::string resource_path_; + + // The absolute path where resource_path was found, if success. + std::optional absolute_path_; + + // An error message, permitted to be present only when base_path is empty. + // + // All three of resource_path, base_path, and error_message can be empty + // (e.g., a default-constructed and/or moved-from object), which represents + // resource-not-found along with an unspecified non-empty default error + // message from get_error_message(). + std::optional error_message_; +}; + +/// Attempts to locate a Drake resource named by the given @p resource_path. +/// The @p resource_path refers to the relative path within the Drake source +/// repository, prepended with `drake/`. For example, to find the source +/// file `examples/pendulum/Pendulum.urdf`, the @p resource_path would be +/// `drake/examples/pendulum/Pendulum.urdf`. Paths that do not start with +/// `drake/` will return an error result. The @p resource_path must refer +/// to a file (not a directory). +/// +/// The search scans for the resource in the following resource roots and in +/// the following order: +/// +/// 1. In the DRAKE_RESOURCE_ROOT environment variable. +/// 2. In the Bazel runfiles for a bazel-bin/pkg/program. +/// 3. In the Drake CMake install directory. +/// +/// The first resource root from the list that exists is used to find any and +/// all Drake resources. If the resource root does not contain the resource, +/// the result is an error even (if a resource root lower on the list happens +/// to have the resource). If all three roots are unavailable, then returns an +/// error result. +FindResourceResult FindResource(const std::string& resource_path); + +/// Convenient wrapper for querying FindResource(resource_path) followed by +/// FindResourceResult::get_absolute_path_or_throw(). +std::string FindResourceOrThrow(const std::string& resource_path); + +/// The name of the environment variable that provides the first place where +/// FindResource attempts to look. The environment variable is allowed to be +/// unset or empty; in that case, FindResource will attempt to use other +/// locations without complaint. +/// +/// The value is guaranteed to be "DRAKE_RESOURCE_ROOT". (For some users, it +/// may be easier to hard-code a value than refer to this constant.) +/// +/// When the environment variable is set, resources are sought in relation to +/// it by appending the FindResource() `resource_path` to the environment +/// variable (with an intermediate `/` as appropriate). For example, if the +/// `resource_path` is `drake/examples/pendulum/Pendulum.urdf` and the +/// `DRAKE_RESOURCE_ROOT` is set to `/home/someuser/foo` then the resource will +/// be sought at `/home/someuser/foo/drake/examples/pendulum/Pendulum.urdf`. +/// +/// The intended use of this variable is to seek resources from an installed +/// copy of Drake, in case other methods have failed. +extern const char* const kDrakeResourceRootEnvironmentVariableName; + +} // namespace drake diff --git a/maliput_drake/include/drake/common/find_runfiles.h b/maliput_drake/include/drake/common/find_runfiles.h new file mode 100644 index 0000000..75ff271 --- /dev/null +++ b/maliput_drake/include/drake/common/find_runfiles.h @@ -0,0 +1,43 @@ +#pragma once + +#include + +/** @file +This file contains helpers to work with Bazel-declared runfiles -- declared +data dependencies used by C++ code. The functions in this file only succeed +when used within a Bazel build. + +All source code within Drake should use FindResource() or FindResourceOrThrow() +instead of these Runfiles routines, because FindResource will operate correctly +in pre-compiled / installed builds of Drake whereas FindRunfile will not. + +These runfiles-related helpers are intended for use by downstream Bazel +projects that use Drake as a library, so that those projects can reuse the +relatively complicated logic within these routines during source builds. +*/ + +namespace drake { + +/** (Advanced.) Returns true iff this process has Bazel runfiles available. +For both C++ and Python programs, and no matter what workspace a program +resides in (`@drake` or otherwise), this will be true when running +`bazel-bin/pkg/program` or `bazel test //pkg:program` or `bazel run +//pkg:program`. */ +// bool HasRunfiles(); + +/** (Advanced.) The return type of FindRunfile(). Exactly one of the two +strings is non-empty. */ +// struct RlocationOrError { +// /** The absolute path to the resource_path runfile. */ +// std::string abspath; +// /** The error message. */ +// std::string error; +// }; + +/** (Advanced.) Returns the absolute path to the given resource_path from Bazel +runfiles, or else an error message when not found. When HasRunfiles() is +false, returns an error. The `resource_path` looks like +`workspace/pkg/subpkg/file.ext`, e.g., "drake/common/foo.txt". */ +// RlocationOrError FindRunfile(const std::string& resource_path); + +} // namespace drake diff --git a/maliput_drake/include/drake/common/hash.h b/maliput_drake/include/drake/common/hash.h new file mode 100644 index 0000000..23d8ef0 --- /dev/null +++ b/maliput_drake/include/drake/common/hash.h @@ -0,0 +1,270 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "drake/common/drake_assert.h" +#include "drake/common/drake_throw.h" + +/// @defgroup hash_append hash_append generic hashing +/// @{ +/// @ingroup cxx +/// @brief Drake uses the hash_append pattern as described by N3980. +/// +/// For a full treatment of the hash_append pattern, refer to: +/// http://www.open-std.org/jtc1/sc22/wg21/docs/papers/2014/n3980.html +/// +///

Providing hash_append support within a class

+/// +/// Drake types may implement a `hash_append` function. +/// The function appends every hash-relevant member field into the hasher: +/// @code +/// class MyValue { +/// public: +/// ... +/// /// Implements the @ref hash_append concept. +/// template +/// friend void hash_append( +/// HashAlgorithm& hasher, const MyValue& item) noexcept { +/// using drake::hash_append; +/// hash_append(hasher, item.my_data_); +/// } +/// ... +/// private: +/// std::string my_data_; +/// }; +/// @endcode +/// +/// Checklist for reviewing a `hash_append` implementation: +/// +/// - The function cites `@ref hash_append` in its Doxygen comment. +/// - The function is marked `noexcept`. +/// +///

Using hashable types

+/// +/// Types that implement this pattern may be used in unordered collections: +/// @code +/// std::unordered_set foo; +/// @endcode +/// +/// Some Drake types may also choose to specialize `std::hash` to use +/// `DefaultHash`, so that the second template argument to `std::unordered_set` +/// can be omitted. For example, Drake's `symbolic::Expression` header says: +/// @code +/// namespace std { +/// struct hash : public drake::DefaultHash {}; +/// } // namespace std +/// @endcode +/// so that users are able to simply write: +/// @code +/// std::unordered_set foo; +/// @endcode +/// +/// @} + +namespace drake { + +/// Provides @ref hash_append for integral constants. +template +std::enable_if_t> +hash_append( + HashAlgorithm& hasher, const T& item) noexcept { + hasher(std::addressof(item), sizeof(item)); +} + +/// Provides @ref hash_append for enumerations. +template +std::enable_if_t> +hash_append( + HashAlgorithm& hasher, const T& item) noexcept { + hasher(std::addressof(item), sizeof(item)); +} + +/// Provides @ref hash_append for floating point values. +template +std::enable_if_t> +hash_append( + HashAlgorithm& hasher, const T& item) noexcept { + // Hashing a NaN makes no sense, since they cannot compare as equal. + DRAKE_ASSERT(!std::isnan(item)); + // +0.0 and -0.0 are equal, so must hash identically. + if (item == 0.0) { + const T zero{0.0}; + hasher(std::addressof(zero), sizeof(zero)); + } else { + hasher(std::addressof(item), sizeof(item)); + } +} + +/// Provides @ref hash_append for std::string. +/// (Technically, any string based on `CharT = char`.) +template +void hash_append( + HashAlgorithm& hasher, + const std::basic_string& item) noexcept { + using drake::hash_append; + hasher(item.data(), item.size()); + // All collection types must send their size, after their contents. + // See the #hash_append_vector anchor in N3980. + hash_append(hasher, item.size()); +} + +/// Provides @ref hash_append for std::pair. +template +void hash_append( + HashAlgorithm& hasher, const std::pair& item) noexcept { + using drake::hash_append; + hash_append(hasher, item.first); + hash_append(hasher, item.second); +} + +/// Provides @ref hash_append for std::optional. +/// +/// Note that `std::hash>` provides the peculiar invariant +/// that the hash of an `optional` bearing a value `v` shall evaluate to the +/// same hash as that of the value `v` itself. Hash operations implemented +/// with this `hash_append` do *not* provide that invariant. +template +void hash_append( + HashAlgorithm& hasher, const std::optional& item) noexcept { + if (item) { + hash_append(hasher, *item); + } + hash_append(hasher, item.has_value()); +}; + +/// Provides @ref hash_append for a range, as given by two iterators. +template +void hash_append_range( + // NOLINTNEXTLINE(runtime/references) Per hash_append convention. + HashAlgorithm& hasher, Iter begin, Iter end) noexcept { + using drake::hash_append; + size_t count{0}; + for (Iter iter = begin; iter != end; ++iter, ++count) { + hash_append(hasher, *iter); + } + // All collection types must send their size, after their contents. + // See the #hash_append_vector anchor in N3980. + hash_append(hasher, count); +} + +/// Provides @ref hash_append for std::map. +/// +/// Note that there is no `hash_append` overload for `std::unordered_map`, and +/// such an overload must never appear. See n3980.html#unordered for details. +template < + class HashAlgorithm, + class T1, + class T2, + class Compare, + class Allocator> +void hash_append( + HashAlgorithm& hasher, + const std::map& item) noexcept { + return hash_append_range(hasher, item.begin(), item.end()); +}; + +/// Provides @ref hash_append for std::set. +/// +/// Note that there is no `hash_append` overload for `std::unordered_set`, and +/// such an overload must never appear. See n3980.html#unordered for details. +template < + class HashAlgorithm, + class Key, + class Compare, + class Allocator> +void hash_append( + HashAlgorithm& hasher, + const std::set& item) noexcept { + return hash_append_range(hasher, item.begin(), item.end()); +}; + +/// A hashing functor, somewhat like `std::hash`. Given an item of type @p T, +/// applies @ref hash_append to it, directing the bytes to append into the +/// given @p HashAlgorithm, and then finally returning the algorithm's result. +template +struct uhash { + using result_type = typename HashAlgorithm::result_type; + + template + result_type operator()(const T& item) const noexcept { + HashAlgorithm hasher; + using drake::hash_append; + hash_append(hasher, item); + return static_cast(hasher); + } +}; + +namespace internal { +/// The FNV1a hash algorithm, used for @ref hash_append. +/// https://en.wikipedia.org/wiki/Fowler%E2%80%93Noll%E2%80%93Vo_hash_function +class FNV1aHasher { + public: + using result_type = size_t; + + /// Feeds a block of memory into this hash. + void operator()(const void* data, size_t length) noexcept { + const uint8_t* const begin = static_cast(data); + const uint8_t* const end = begin + length; + for (const uint8_t* iter = begin; iter < end; ++iter) { + hash_ = (hash_ ^ *iter) * kFnvPrime; + } + } + + /// Feeds a single byte into this hash. + constexpr void add_byte(uint8_t byte) noexcept { + hash_ = (hash_ ^ byte) * kFnvPrime; + } + + /// Returns the hash. + explicit constexpr operator size_t() noexcept { + return hash_; + } + + private: + static_assert(sizeof(result_type) == (64 / 8), "We require a 64-bit size_t"); + result_type hash_{0xcbf29ce484222325u}; + static constexpr size_t kFnvPrime = 1099511628211u; +}; +} // namespace internal + +/// The default HashAlgorithm concept implementation across Drake. This is +/// guaranteed to have a result_type of size_t to be compatible with std::hash. +using DefaultHasher = internal::FNV1aHasher; + +/// The default hashing functor, akin to std::hash. +using DefaultHash = drake::uhash; + +/// An adapter that forwards the HashAlgorithm::operator(data, length) function +/// concept into a runtime-provided std::function of the same signature. This +/// is useful for passing a concrete HashAlgorithm implementation through into +/// non-templated code, such as with an Impl or Cell pattern. +struct DelegatingHasher { + /// A std::function whose signature matches HashAlgorithm::operator(). + using Func = std::function; + + /// Create a delegating hasher that calls the given @p func. + explicit DelegatingHasher(Func func) : func_(std::move(func)) { + // In order for operator() to be noexcept, it must have a non-empty func_. + DRAKE_THROW_UNLESS(static_cast(func_)); + } + + /// Append [data, data + length) bytes into the wrapped algorithm. + void operator()(const void* data, size_t length) noexcept { + func_(data, length); + } + + private: + const Func func_; +}; + +} // namespace drake diff --git a/maliput_drake/include/drake/common/identifier.h b/maliput_drake/include/drake/common/identifier.h new file mode 100644 index 0000000..bec4196 --- /dev/null +++ b/maliput_drake/include/drake/common/identifier.h @@ -0,0 +1,250 @@ +#pragma once + +#include +#include +#include +#include + +#include "drake/common/drake_copyable.h" +#include "drake/common/drake_throw.h" +#include "drake/common/hash.h" +#include "drake/common/never_destroyed.h" + +namespace drake { + +/** + A simple identifier class. + + @note This is *purposely* a separate class from @ref TypeSafeIndex. For more + explanatation, see @ref TypeSafeIndexVsIndentifier "this section". + + This class serves as an upgrade to the standard practice of passing `int`s + around as unique identifiers (or, as in this case, `int64_t`s). In the common + practice, a method that takes identifiers to different types of objects would + have an interface like: + + @code + void foo(int64_t bar_id, int64_t thing_id); + @endcode + + It is possible for a programmer to accidentally switch the two ids in an + invocation. This mistake would still be _syntactically_ correct; it will + successfully compile but lead to inscrutable run-time errors. This identifier + class provides the same speed and efficiency of passing `int64_t`s, but + enforces unique types and limits the valid operations, providing compile-time + checking. The function would now look like: + + @code + void foo(BarId bar_id, ThingId thing_id) + @endcode + + and the compiler will catch instances where the order is reversed. + + The identifier is a _stripped down_ 64-bit int. Each uniquely declared + identifier type has the following properties: + + - The identifier's default constructor produces _invalid_ identifiers. + - Valid identifiers must be constructed via the copy constructor or through + Identifier::get_new_id(). + - The identifier is immutable. + - The identifier can only be tested for equality/inequality with other + identifiers of the _same_ type. + - Identifiers of different types are _not_ interconvertible. + - The identifier can be queried for its underlying `int64_t` value. + - The identifier can be written to an output stream; its underlying `int64_t` + value gets written. + - Identifiers are not guaranteed to possess _meaningful_ ordering. I.e., + identifiers for two objects created sequentially may not have sequential + identifier values. + - Identifiers can only be generated from the static method get_new_id(). + + While there _is_ the concept of an invalid identifier, this only exists to + facilitate use with STL containers that require default constructors. Using an + invalid identifier is generally considered to be an error. In Debug build, + attempts to compare, get the value of, or write an invalid identifier to a + stream will throw an exception. + + Functions that query for identifiers should not return invalid identifiers. We + prefer the practice of returning std::optional instead. + + It is the designed intent of this class, that ids derived from this class can + be passed and returned by value. (Drake's typical calling convention requires + passing input arguments by const reference, or by value when moved from. That + convention does not apply to this class.) + + The following alias will create a unique identifier type for class `Foo`: + @code{.cpp} + using FooId = Identifier; + @endcode + + __Examples of valid and invalid operations__ + + The %Identifier guarantees that id instances of different types can't be + compared or combined. Efforts to do so will cause a compile-time failure. + For example: + + @code + using AId = Identifier; + using BId = Identifier; + AId a1; // Compiler error; there is no + // default constructor. + AId a2 = AId::get_new_id(); // Ok. + AId a3(a2); // Ok. + AId a4 = AId::get_new_id(); // Ok. + BId b = BId::get_new_id(); // Ok. + if ( a2 == 1 ) { ... } // Compiler error. + if ( a2 == a4 ) { ... } // Ok, evaluates to false. + if ( a2 == a3 ) { ... } // Ok, evaluates to true. + if ( a2 == b ) { ... } // Compiler error. + a4 = a2; // Ok. + a3 = 7; // Compiler error. + @endcode + + @anchor TypeSafeIndexVsIndentifier + __TypeSafeIndex vs Identifier__ + + In principle, the *identifier* is related to the @ref TypeSafeIndex. In + some sense, both are "type-safe" `int`s. They differ in their semantics. We can + consider `ints`, indices, and identifiers as a list of `int` types with + _decreasing_ functionality. + + - The int, obviously, has the full range of C++ ints. + - The @ref TypeSafeIndex can be implicitly cast *to* an int, but there are a + limited number of operations _on_ the index that produce other instances + of the index (e.g., increment, in-place addition, etc.) They can be + compared with `int` and other indices of the same type. This behavior + arises from the intention of having them serve as an _index_ in an + ordered set (e.g., `std::vector`). + - The %Identifier is the most restricted. They exist solely to serve as a + unique identifier. They are immutable when created. Very few operations + exist on them (comparison for _equality_ with other identifiers of the same + type, hashing, writing to output stream). These *cannot* be used as + indices. + + Ultimately, indices _can_ serve as identifiers (within the scope of the object + they index into). Although, their mutability could make this a dangerous + practice for a public API. Identifiers are more general in that they don't + reflect an object's position in memory (hence the inability to transform to or + compare with an `int`). This decouples details of implementation from the idea + of the object. Combined with its immutability, it would serve well as a element + of a public API. + + @sa TypeSafeIndex + + @tparam Tag The name of the tag that uniquely segregates one + instantiation from another. + */ +template +class Identifier { + public: + DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Identifier) + + /** Default constructor; the result is an _invalid_ identifier. This only + exists to satisfy demands of working with various container classes. */ + Identifier() : value_(0) {} + + /** Extracts the underlying representation from the identifier. This is + considered invalid for invalid ids and is strictly enforced in Debug builds. + */ + int64_t get_value() const { + if (kDrakeAssertIsArmed) { + DRAKE_THROW_UNLESS(this->is_valid()); + } + return value_; + } + + /** Reports if the id is valid. */ + bool is_valid() const { return value_ > 0; } + + /** Compares one identifier with another of the same type for equality. This + is considered invalid for invalid ids and is strictly enforced in Debug + builds. */ + bool operator==(Identifier other) const { + return this->get_value() == other.get_value(); + } + + /** Compares one identifier with another of the same type for inequality. This + is considered invalid for invalid ids and is strictly enforced in Debug + builds. */ + bool operator!=(Identifier other) const { + return this->get_value() != other.get_value(); + } + + /** Compare two identifiers in order to define a total ordering among + identifiers. This makes identifiers compatible with data structures which + require total ordering (e.g., std::set). */ + bool operator<(Identifier other) const { + return this->get_value() < other.get_value(); + } + + /** Generates a new identifier for this id type. This new identifier will be + different from all previous identifiers created. This method does _not_ + make any guarantees about the values of ids from successive invocations. + This method is guaranteed to be thread safe. + */ + static Identifier get_new_id() { + // Note that id 0 is reserved for uninitialized variable which is created + // by the default constructor. As a result, we have an invariant that + // get_new_id() > 0. + static never_destroyed> next_index(1); + return Identifier(next_index.access()++); + } + + /** Implements the @ref hash_append concept. And invalid id will successfully + hash (in order to satisfy STL requirements), and it is up to the user to + confirm it is valid before using it as a key (or other hashing application). + */ + template + friend void hash_append(HashAlgorithm& hasher, const Identifier& i) noexcept { + using drake::hash_append; + hash_append(hasher, i.value_); + } + + /** (Internal use only) Compares this possibly-invalid Identifier with one + that is known to be valid and returns `false` if they don't match. It is an + error if `valid_id` is not actually valid, and that is strictly enforced in + Debug builds. However, it is not an error if `this` id is invalid; that + results in a `false` return. This method can be faster than testing + separately for validity and equality. */ + bool is_same_as_valid_id(Identifier valid_id) const { + return value_ == valid_id.get_value(); + } + + protected: + // Instantiates an identifier from the underlying representation type. + explicit Identifier(int64_t val) : value_(val) {} + + private: + // The underlying value. + int64_t value_{}; +}; + +/** Streaming output operator. This is considered invalid for invalid ids and + is strictly enforced in Debug builds. + @relates Identifier + */ +template +std::ostream& operator<<(std::ostream& out, const Identifier& id) { + out << id.get_value(); + return out; +} + +/** Enables use of identifiers with to_string. It requires ADL to work. So, + it should be invoked as: `to_string(id);` and should be preceded by + `using std::to_string`.*/ +template +std::string to_string(const drake::Identifier& id) { + return std::to_string(id.get_value()); +} + +} // namespace drake + +namespace std { + +/** Enables use of the identifier to serve as a key in STL containers. + @relates Identifier + */ +template +struct hash> : public drake::DefaultHash {}; + +} // namespace std diff --git a/maliput_drake/include/drake/common/is_approx_equal_abstol.h b/maliput_drake/include/drake/common/is_approx_equal_abstol.h new file mode 100644 index 0000000..9af0c45 --- /dev/null +++ b/maliput_drake/include/drake/common/is_approx_equal_abstol.h @@ -0,0 +1,62 @@ +#pragma once + +#include + +#include + +namespace drake { + +/// Returns true if and only if the two matrices are equal to within a certain +/// absolute elementwise @p tolerance. Special values (infinities, NaN, etc.) +/// do not compare as equal elements. +template +bool is_approx_equal_abstol(const Eigen::MatrixBase& m1, + const Eigen::MatrixBase& m2, + double tolerance) { + return ( + (m1.rows() == m2.rows()) && + (m1.cols() == m2.cols()) && + ((m1 - m2).template lpNorm() <= tolerance)); +} + +/// Returns true if and only if a simple greedy search reveals a permutation +/// of the columns of m2 to make the matrix equal to m1 to within a certain +/// absolute elementwise @p tolerance. E.g., there exists a P such that +/// 
+///    forall i,j,  |m1 - m2*P|_{i,j} <= tolerance
+///    where P is a permutation matrix:
+///       P(i,j)={0,1}, sum_i P(i,j)=1, sum_j P(i,j)=1.
+///
+/// Note: Returns false for matrices of different sizes. +/// Note: The current implementation is O(n^2) in the number of columns. +/// Note: In marginal cases (with similar but not identical columns) this +/// algorithm can fail to find a permutation P even if it exists because it +/// accepts the first column match (m1(i),m2(j)) and removes m2(j) from the +/// pool. It is possible that other columns of m2 would also match m1(i) but +/// that m2(j) is the only match possible for a later column of m1. +template +bool IsApproxEqualAbsTolWithPermutedColumns( + const Eigen::MatrixBase& m1, + const Eigen::MatrixBase& m2, double tolerance) { + if ((m1.cols() != m2.cols()) || (m1.rows() != m2.rows())) return false; + + std::vector available(m2.cols()); + for (int i = 0; i < m2.cols(); i++) available[i] = true; + + for (int i = 0; i < m1.cols(); i++) { + bool found_match = false; + for (int j = 0; j < m2.cols(); j++) { + if (available[j] && + is_approx_equal_abstol(m1.col(i), m2.col(j), tolerance)) { + found_match = true; + available[j] = false; + break; + } + } + if (!found_match) return false; + } + return true; +} + + +} // namespace drake diff --git a/maliput_drake/include/drake/common/is_cloneable.h b/maliput_drake/include/drake/common/is_cloneable.h new file mode 100644 index 0000000..b04096b --- /dev/null +++ b/maliput_drake/include/drake/common/is_cloneable.h @@ -0,0 +1,85 @@ +#pragma once + +#include +#include + +namespace drake { + +/** @cond */ + +namespace is_cloneable_internal { + +// Default case; assumes that a class is *not* cloneable. +template +struct is_cloneable_helper : std::false_type {}; + +// Special sauce for SFINAE. Only compiles if it can finds the method +// `unique_ptr T::Clone() const`. If this exists, the is_cloneable implicitly +// prefers this overload over the default overload. +template +struct is_cloneable_helper< + T, + typename std::enable_if_t().Clone().release()), + typename std::remove_const_t*>>> + : std::true_type {}; + +} // namespace is_cloneable_internal + +/** @endcond */ + +/** + @anchor is_cloneable_doc + Provides method for determining at run time if a class is "cloneable". + + __Usage__ + + This gets used like `type_traits` functions (e.g., `is_copy_constructible`, + `is_same`, etc.) To determine if a class is cloneable simply invoke: + + @code + bool value = drake::is_cloneable::value; + @endcode + + If `Foo` is cloneable, it will evaluate to true. It can also be used in + compile-time tests (e.g., SFINAE and `static_assert`s): + + @code + static_assert(is_cloneable::value, "This method requires its classes to " + "be cloneable."); + @endcode + + __Definition of "cloneability"__ + + To be cloneable, the class `Foo` must have a _public_ method of the form: + @code + unique_ptr Foo::Clone() const; + @endcode + Note that "friend" access for the %is_cloneable-using class is not sufficient. + The `Foo::Clone()` method must actually be public. + + + + The pointer contained in the returned `unique_ptr` must point to a + heap-allocated deep copy of the _concrete_ object. This test can confirm the + proper signature, but cannot confirm the heap-allocated deep copy. A Clone() + method that doesn't return such a copy of the _concrete_ object should be + considered a malformed function. + + @warning It is important to note, that a `Clone()` method that returns a + `unique_ptr` to a _super_ class is _not_ sufficient to be cloneable. In other + words the presence of: + @code + unique_ptr Derived::Clone() const; + @endcode + will not make the `Derived` class cloneable. + + @tparam T The class to test for cloneability. + */ +template +using is_cloneable = + is_cloneable_internal::is_cloneable_helper; + +} // namespace drake diff --git a/maliput_drake/include/drake/common/is_less_than_comparable.h b/maliput_drake/include/drake/common/is_less_than_comparable.h new file mode 100644 index 0000000..28aee2d --- /dev/null +++ b/maliput_drake/include/drake/common/is_less_than_comparable.h @@ -0,0 +1,74 @@ +#pragma once + +#include + +#include "drake/common/unused.h" + +namespace drake { + +#ifndef DRAKE_DOXYGEN_CXX + +namespace is_less_than_comparable_internal { + +// Default case; assumes that a class is *not* less-than comparable. +template +struct is_less_than_comparable_helper : std::false_type { }; + +// Special sauce for SFINAE. Only compiles if it can finds the method +// `operator<`. If this exists, the is_less_than_comparable implicitly +// prefers this overload over the default overload. +template +struct is_less_than_comparable_helper() < std::declval()), + (void)0)>> : std::true_type {}; + +} // namespace is_less_than_comparable_internal + +/** @endcond */ + +/** + @anchor is_less_than_comparable_doc + Provides method for determining at run time if a class is comparable using + the less-than operator (<). + + __Usage__ + + This gets used like `type_traits` functions (e.g., `is_copy_constructible`, + `is_same`, etc.) To determine if a class is less-than comparable simply invoke: + + @code + bool value = drake::is_less_than_comparable::value; + @endcode + + If `Foo` is less-than comparable, it will evaluate to true. It can also be used + in compile-time tests (e.g., SFINAE and `static_assert`s): + + @code + static_assert(is_less_than_comparable::value, "This method requires its " + "classes to be less-than comparable."); + @endcode + + __Definition of "less-than comparability"__ + + To be less-than comparable, the class `Foo` must have a public method of the + form: + + @code + bool Foo::operator<(const Foo&) const; + @endcode + + or a definition external to the class of the form: + + @code + bool operator<(const Foo&, const Foo&); + @endcode + + @tparam T The class to test for less-than comparability. + */ +template +using is_less_than_comparable = + is_less_than_comparable_internal::is_less_than_comparable_helper; + +} // namespace drake + +#endif diff --git a/maliput_drake/include/drake/common/name_value.h b/maliput_drake/include/drake/common/name_value.h new file mode 100644 index 0000000..f887f53 --- /dev/null +++ b/maliput_drake/include/drake/common/name_value.h @@ -0,0 +1,100 @@ +#pragma once + +#include "drake/common/drake_assert.h" +#include "drake/common/drake_copyable.h" + +/** @addtogroup technical_notes +@{ +@defgroup serialize_tips Writing a Serialize method + +Structured data sometimes provides a Serialize method to be compatible with a +variety of readers, writers, or any other code that needs to visit the data +generically. + +Here is an example of implementing a Serialize method: +@code +struct DoubleStruct { + template + void Serialize(Archive* a) { + a->Visit(DRAKE_NVP(value)); + } + + double value{0.0}; +}; +@endcode + +By convention, we place the Serialize method prior to the data members per +the +styleguide rule. Each data member has a matching `Visit` line in the +Serialize method, in the same order as the member fields appear. + +By convention, we declare all of the member fields as public, since they are +effectively so anyway (because anything that calls the Serialize method +receives a mutable pointer to them). The typical way to do this is to declare +the data as a `struct`, instead of a `class`. + +However, if +the +styleguide rule for struct vs class points towards using a `class` instead, +then we follow that advice and make it a `class`, but we explicitly label the +member fields as `public`. We also omit the trailing underscore from the field +names, so that the Serialize API presented to the caller of the class is +indifferent to whether it is phrased as a `struct` or a `class`. + +For how Serialize and Archive interact, see the drake::yaml::YamlReadArchive +class overview. + +@} +*/ + +namespace drake { + +/// A basic implementation of the Name-Value Pair concept as used in the +/// Serialize / Archive pattern. See, for example: +/// https://www.boost.org/doc/libs/release/libs/serialization/doc/wrappers.html#nvp +/// +/// NameValue stores a pointer to a const `name` and a pointer to a mutable +/// `value`. Both pointers must remain valid throughout the lifetime of an +/// object. %NameValue objects are typically short-lived, existing only for a +/// transient moment while an Archive is visiting some Serializable field. +template +class NameValue { + public: + DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(NameValue) + + /// Type of the referenced value. + typedef T value_type; + + /// (Advanced.) Constructs a %NameValue. Prefer DRAKE_NVP instead of this + /// constructor. Both pointers are aliased and must remain valid for the + /// lifetime of this object. Neither pointer can be nullptr. + NameValue(const char* name_in, T* value_in) + : name_(name_in), value_(value_in) { + DRAKE_ASSERT(name_in != nullptr); + DRAKE_ASSERT(value_in != nullptr); + } + + /// @name Accessors to the raw pointers + //@{ + const char* name() const { return name_; } + T* value() const { return value_; } + //@} + + private: + const char* const name_; + T* const value_; +}; + +/// (Advanced.) Creates a NameValue. The conventional method for calling this +/// function is the DRAKE_NVP sugar macro below. +/// +/// Both pointers are aliased for the lifetime of the return value. +template +NameValue MakeNameValue(const char* name, T* value) { + return NameValue(name, value); +} + +} // namespace drake + +/// Creates a NameValue pair for an lvalue `x`. +#define DRAKE_NVP(x) ::drake::MakeNameValue(#x, &(x)) diff --git a/maliput_drake/include/drake/common/never_destroyed.h b/maliput_drake/include/drake/common/never_destroyed.h new file mode 100644 index 0000000..024b355 --- /dev/null +++ b/maliput_drake/include/drake/common/never_destroyed.h @@ -0,0 +1,103 @@ +#pragma once + +#include +#include +#include + +#include "drake/common/drake_copyable.h" + +namespace drake { + +/// Wraps an underlying type T such that its storage is a direct member field +/// of this object (i.e., without any indirection into the heap), but *unlike* +/// most member fields T's destructor is never invoked. +/// +/// This is especially useful for function-local static variables that are not +/// trivially destructable. We shouldn't call their destructor at program exit +/// because of the "indeterminate order of ... destruction" as mentioned in +/// cppguide's +/// Static +/// and Global Variables section, but other solutions to this problem place +/// the objects on the heap through an indirection. +/// +/// Compared with other approaches, this mechanism more clearly describes the +/// intent to readers, avoids "possible leak" warnings from memory-checking +/// tools, and is probably slightly faster. +/// +/// Example uses: +/// +/// The singleton pattern: +/// @code +/// class Singleton { +/// public: +/// DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Singleton) +/// static Singleton& getInstance() { +/// static never_destroyed instance; +/// return instance.access(); +/// } +/// private: +/// friend never_destroyed; +/// Singleton() = default; +/// }; +/// @endcode +/// +/// A lookup table, created on demand the first time its needed, and then +/// reused thereafter: +/// @code +/// enum class Foo { kBar, kBaz }; +/// Foo ParseFoo(const std::string& foo_string) { +/// using Dict = std::unordered_map; +/// static const drake::never_destroyed string_to_enum{ +/// std::initializer_list{ +/// {"bar", Foo::kBar}, +/// {"baz", Foo::kBaz}, +/// } +/// }; +/// return string_to_enum.access().at(foo_string); +/// } +/// @endcode +/// +/// In cases where computing the static data is more complicated than an +/// initializer_list, you can use a temporary lambda to populate the value: +/// @code +/// const std::vector& GetConstantMagicNumbers() { +/// static const drake::never_destroyed> result{[]() { +/// std::vector prototype; +/// std::mt19937 random_generator; +/// for (int i = 0; i < 10; ++i) { +/// double new_value = random_generator(); +/// prototype.push_back(new_value); +/// } +/// return prototype; +/// }()}; +/// return result.access(); +/// } +/// @endcode +/// +/// Note in particular the `()` after the lambda. That causes it to be invoked. +// +// The above examples are repeated in the unit test; keep them in sync. +template +class never_destroyed { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(never_destroyed) + + /// Passes the constructor arguments along to T using perfect forwarding. + template + explicit never_destroyed(Args&&... args) { + // Uses "placement new" to construct a `T` in `storage_`. + new (&storage_) T(std::forward(args)...); + } + + /// Does nothing. Guaranteed! + ~never_destroyed() = default; + + /// Returns the underlying T reference. + T& access() { return *reinterpret_cast(&storage_); } + const T& access() const { return *reinterpret_cast(&storage_); } + + private: + typename std::aligned_storage::type storage_; +}; + +} // namespace drake diff --git a/maliput_drake/include/drake/common/nice_type_name.h b/maliput_drake/include/drake/common/nice_type_name.h new file mode 100644 index 0000000..9319ce5 --- /dev/null +++ b/maliput_drake/include/drake/common/nice_type_name.h @@ -0,0 +1,115 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include "drake/common/never_destroyed.h" + +namespace drake { + +/** @brief Obtains canonicalized, platform-independent, human-readable names for +arbitrarily-complicated C++ types. + +Usage: @code +// For types: +using std::pair; using std::string; +using MyVectorType = pair; +std::cout << "Type MyVectorType was: " + << drake::NiceTypeName::Get() << std::endl; +// Output: std::pair + +// For expressions: +std::unique_ptr thing; // Assume actual type is ConcreteThing. +std::cout << "Actual type of 'thing' was: " + << drake::NiceTypeName::Get(*thing) << std::endl; +// Output: ConcreteThing +@endcode + +We demangle and attempt to canonicalize the compiler-generated type names as +reported by `typeid(T).name()` so that the same string is returned by all +supported compilers and platforms. The output of NiceTypeName::Get() is +useful in error and log messages and testing. It also provides a +persistent, platform-independent identifier for types; `std::type_info` cannot +provide that. + +@warning Don't expect usable names for types that are defined in an anonymous +namespace or for function-local types. Names will still be produced but they +won't be unique, pretty, or compiler-independent. + +This class exists only to group type name-related static methods; don't try +to construct an object of this type. */ +class NiceTypeName { + public: + /** Returns a nicely demangled and canonicalized type name that is the same + on all platforms, using Canonicalize(). This is calculated on the fly so is + expensive whenever called, though very reasonable for use in error messages. + For repeated or performance-sensitive uses, see GetFromStorage(). */ + template + static std::string Get() { + return NiceTypeName::Get(typeid(T)); + } + + /** Like Get() but only computed once per process for a given type `T`. + The returned reference will not be deleted even at program termination, so + feel free to use it in error messages even in destructors that may be + invoked during program tear-down. */ + template + static const std::string& GetFromStorage() { + static const never_destroyed result(NiceTypeName::Get()); + return result.access(); + } + + /** Returns the type name of the most-derived type of an object of type T, + typically but not necessarily polymorphic. This must be calculated on the fly + so is expensive whenever called, though very reasonable for use in error + messages. For non-polymorphic types this produces the same result as would + `Get()` but for polymorphic types the results will + differ. */ + template + static std::string Get(const T& thing) { + return GetWithPossibleOverride(&thing, typeid(thing)); + } + + /** Returns the nicely demangled and canonicalized type name of `info`. This + must be calculated on the fly so is expensive whenever called, though very + reasonable for use in error messages. */ + static std::string Get(const std::type_info& info) { + return Canonicalize(Demangle(info.name())); + } + + /** Using the algorithm appropriate to the current compiler, demangles a type + name as returned by `typeid(T).name()`, with the result hopefully suitable for + meaningful display to a human. The result is compiler-dependent. + @see Canonicalize() */ + static std::string Demangle(const char* typeid_name); + + /** Given a compiler-dependent demangled type name string as returned by + Demangle(), attempts to form a canonicalized representation that will be + the same for any compiler. Unnecessary spaces and superfluous keywords like + "class" and "struct" are removed. The NiceTypeName::Get() method + uses this function to produce a human-friendly type name that is the same on + any platform. */ + static std::string Canonicalize(const std::string& demangled_name); + + /** Given a canonical type name that may include leading namespaces, attempts + to remove those namespaces. For example, + `drake::systems::MyThing` becomes `MyThing`. + If the last segment ends in `::`, the original string is returned unprocessed. + Note that this is just string processing -- a segment that looks like a + namespace textually will be treated as one, even if it is really a class. So + `drake::MyClass::Impl` will be reduced to `Impl` while + `drake::MyClass::Impl` is reduced to `MyClass::Impl`. */ + static std::string RemoveNamespaces(const std::string& canonical_name); + + private: + // No instances of this class should be created. + NiceTypeName() = delete; + + static std::string GetWithPossibleOverride( + const void* ptr, const std::type_info& info); +}; + +} // namespace drake diff --git a/maliput_drake/include/drake/common/nice_type_name_override.h b/maliput_drake/include/drake/common/nice_type_name_override.h new file mode 100644 index 0000000..ca2115e --- /dev/null +++ b/maliput_drake/include/drake/common/nice_type_name_override.h @@ -0,0 +1,34 @@ +#pragma once + +/** +@file +(Advanced) Provides the ability to override NiceTypeName::Get(T*) so that +Python objects can have human-readable names. +*/ + +#include +#include +#include + +namespace drake { +namespace internal { + +// Structure for passing a type-erased pointer with RTTI. +struct type_erased_ptr { + const void* raw{}; + const std::type_info& info; +}; + +// Callback for overriding an object's nice type name. +using NiceTypeNamePtrOverride = + std::function; + +// Sets override for nice type names. This can only ever be set once, and +// must be given a non-empty function<> object. +void SetNiceTypeNamePtrOverride(NiceTypeNamePtrOverride new_ptr_override); + +// Gets the override. If unset, it will be an empty function<> object. +const NiceTypeNamePtrOverride& GetNiceTypeNamePtrOverride(); + +} // namespace internal +} // namespace drake diff --git a/maliput_drake/include/drake/common/pointer_cast.h b/maliput_drake/include/drake/common/pointer_cast.h new file mode 100644 index 0000000..ad91adc --- /dev/null +++ b/maliput_drake/include/drake/common/pointer_cast.h @@ -0,0 +1,76 @@ +#pragma once + +#include +#include + +#include + +#include "drake/common/nice_type_name.h" + +namespace drake { + +/// Casts the object owned by the std::unique_ptr `other` from type `U` to `T`; +/// no runtime type checking is performed. +/// +/// This method is analogous to the built-in std::static_pointer_cast that +/// operates on a std::shared_ptr. +/// +/// Note that this function only supports default deleters. +template +std::unique_ptr static_pointer_cast(std::unique_ptr&& other) noexcept { + return std::unique_ptr(static_cast(other.release())); +} + +/// Casts the object owned by the std::unique_ptr `other` from type `U` to `T`; +/// if the cast fails, returns nullptr. Casting is performed using +/// `dynamic_cast` on the managed value (i.e., the result of `other.get()`). +/// On success, `other`'s managed value is transferred to the result and +/// `other` is empty; on failure, `other` will retain its original managed +/// value and the result is empty. As with `dynamic_cast`, casting nullptr to +/// anything always succeeds, so a nullptr result could indicate either that +/// the argument was nullptr or that the cast failed. +/// +/// This method is analogous to the built-in std::dynamic_pointer_cast that +/// operates on a std::shared_ptr. +/// +/// Note that this function only supports default deleters. +template +std::unique_ptr dynamic_pointer_cast(std::unique_ptr&& other) noexcept { + T* result = dynamic_cast(other.get()); + if (!result) { return nullptr; } + other.release(); + return std::unique_ptr(result); +} + +/// Casts the object owned by the std::unique_ptr `other` from type `U` to `T`; +/// if `other` is nullptr or the cast fails, throws a std::exception. +/// Casting is performed using `dynamic_cast` on the managed value (i.e., the +/// result of `other.get()`). On success, `other`'s managed value is +/// transferred to the result and `other` is empty; on failure, `other` will +/// retain its original managed value. +/// +/// @throws std::exception if the cast fails. +/// +/// Note that this function only supports default deleters. +template +std::unique_ptr dynamic_pointer_cast_or_throw(std::unique_ptr&& other) { + if (!other) { + throw std::logic_error(fmt::format( + "Cannot cast a unique_ptr<{}> containing nullptr to unique_ptr<{}>.", + NiceTypeName::Get(), + NiceTypeName::Get())); + } + T* result = dynamic_cast(other.get()); + if (!result) { + throw std::logic_error(fmt::format( + "Cannot cast a unique_ptr<{}> containing an object of type {} to " + "unique_ptr<{}>.", + NiceTypeName::Get(), + NiceTypeName::Get(*other), + NiceTypeName::Get())); + } + other.release(); + return std::unique_ptr(result); +} + +} // namespace drake diff --git a/maliput_drake/include/drake/common/polynomial.h b/maliput_drake/include/drake/common/polynomial.h new file mode 100644 index 0000000..f552f47 --- /dev/null +++ b/maliput_drake/include/drake/common/polynomial.h @@ -0,0 +1,519 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "drake/common/autodiff.h" +#include "drake/common/default_scalars.h" +#include "drake/common/drake_assert.h" +#include "drake/common/symbolic.h" + +namespace drake { +/** A scalar multi-variate polynomial, modeled after the msspoly in spotless. + * + * Polynomial represents a list of additive Monomials, each one of which is a + * product of a constant coefficient (of T, which by default is double) and any + * number of distinct Terms (variables raised to positive integer powers). + * + * Variables are identified by integer indices rather than symbolic names, but + * an automatic facility is provided to covert variable names up to four + * characters into unique integers, provided those variables are named using + * only lowercase letters and the "@#_." characters followed by a number. For + * example, valid names include "dx4" and "m_x". + * + * Monomials which have the same variables and powers may be constructed but + * will be automatically combined: (3 * a * b * a) + (1.5 * b * a**2) will be + * reduced to (4.5 * b * a**2) internally after construction. + * + * Polynomials can be added, subtracted, and multiplied. They may only be + * divided by scalars (of T) because Polynomials are not closed under division. + * + * @tparam_default_scalar + */ +template +class Polynomial { + public: + DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Polynomial); + + typedef unsigned int VarType; + /// This should be 'unsigned int' but MSVC considers a call to std::pow(..., + /// unsigned int) ambiguous because it won't cast unsigned int to int. + typedef int PowerType; + typedef typename Eigen::NumTraits::Real RealScalar; + typedef std::complex RootType; + typedef Eigen::Matrix RootsType; + + template + struct Product { + typedef decltype(static_cast(0) * static_cast(0)) type; + }; + + /// An individual variable raised to an integer power; e.g. x**2. + class Term { + public: + VarType var; + PowerType power; + + bool operator==(const Term& other) const { + return (var == other.var) && (power == other.power); + } + + /// A comparison to allow std::lexicographical_compare on this class; does + /// not reflect any sort of mathematical total order. + bool operator<(const Term& other) const { + return ((var < other.var) || + ((var == other.var) && (power < other.power))); + } + }; + + /// An additive atom of a Polynomial: The product of any number of + /// Terms and a coefficient. + class Monomial { + public: + T coefficient; + std::vector terms; // a list of N variable ids + + bool operator==(const Monomial& other) const { + return (coefficient == other.coefficient) && (terms == other.terms); + } + + /// A comparison to allow std::lexicographical_compare on this class; does + /// not reflect any sort of mathematical total order. + bool operator<(const Monomial& other) const { + return ((coefficient < other.coefficient) || + ((coefficient == other.coefficient) && (terms < other.terms))); + } + + int GetDegree() const; + int GetDegreeOf(VarType var) const; + bool HasSameExponents(const Monomial& other) const; + bool HasVariable(const VarType& var) const; + + /// Factors this by other; returns 0 iff other does not divide this. + Monomial Factor(const Monomial& divisor) const; + }; + + /// Construct the vacuous polynomial, "0". + Polynomial(void) : is_univariate_(true) {} + + /// Construct a Polynomial of a single constant. e.g. "5". + // This is required for some Eigen operations when used in a + // polynomial matrix. + // NOLINTNEXTLINE(runtime/explicit) This conversion is desirable. + Polynomial(const T& scalar); + + /// Construct a Polynomial consisting of a single Monomial, e.g. "5xy**3". + Polynomial(const T coeff, const std::vector& terms); + + /// Construct a Polynomial from a sequence of Monomials. + Polynomial(typename std::vector::const_iterator start, + typename std::vector::const_iterator finish); + + /// Constructs a polynomial consisting of a single Monomial of the variable + /// named `varname1`. + /// + /// @note: This constructor is only provided for T = double. For the other + /// cases, a user should use the constructor with two arguments below (taking + /// std::string and unsigned int). If we provided this constructor for T = + /// AutoDiffXd and T = symbolic::Expression, there would be compiler errors + /// for `Polynomial(0)` as the following candidates are ambiguous: + /// - Polynomial(const T& scalar) + /// - Polynomial(const std::string& varname, const unsigned int num = 1) + template + explicit Polynomial( + const std::enable_if_t, std::string>& varname) + : Polynomial(varname, 1) { + // TODO(soonho-tri): Consider deprecating this constructor to make the API + // consistent for different scalar types. + } + + /// Construct a polynomial consisting of a single Monomial of the variable + /// named varname + num. + Polynomial(const std::string& varname, unsigned int num); + + /// Construct a single Monomial of the given coefficient and variable. + Polynomial(const T& coeff, const VarType& v); + + /// A legacy constructor for univariate polynomials: Takes a vector + /// of coefficients for the constant, x, x**2, x**3... Monomials. + template + explicit Polynomial(Eigen::MatrixBase const& coefficients) { + VarType v = VariableNameToId("t"); + for (int i = 0; i < coefficients.size(); i++) { + Monomial m; + m.coefficient = coefficients(i); + if (i > 0) { + Term t; + t.var = v; + t.power = i; + m.terms.push_back(t); + } + monomials_.push_back(m); + } + is_univariate_ = true; + } + + /// Returns the number of unique Monomials (and thus the number of + /// coefficients) in this Polynomial. + int GetNumberOfCoefficients() const; + + /** Returns the highest degree of any Monomial in this Polynomial. + * + * The degree of a multivariate Monomial is the product of the degrees of + * each of its terms. */ + int GetDegree() const; + + /// Returns true iff this is a sum of terms of degree 1, plus a constant. + bool IsAffine() const; + + /// If the polynomial is "simple" -- e.g. just a single term with + /// coefficient 1 -- then returns that variable; otherwise returns 0. + VarType GetSimpleVariable() const; + + const std::vector& GetMonomials() const; + + Eigen::Matrix GetCoefficients() const; + + /// Returns a set of all of the variables present in this Polynomial. + std::set GetVariables() const; + + /** Evaluate a univariate Polynomial at a specific point. + * + * Evaluates a univariate Polynomial at the given x. + * @throws std::exception if this Polynomial is not univariate. + * + * @p x may be of any type supporting the ** and + operations (which can be + * different from both CoefficientsType and RealScalar). + * + * This method may also be used for efficient evaluation of the derivatives of + * the univariate polynomial, evaluated at @p x. @p derivative_order = 0 (the + * default) returns the polynomial value without differentiation. @p + * derivative_order = 1 returns the first derivative, etc. + * + * @pre derivative_order must be non-negative. + */ + template + typename Product::type EvaluateUnivariate( + const U& x, int derivative_order = 0) const { + // Note: have to remove_const because Product::type and + // even Product::type returns const AutoDiff. + typedef typename std::remove_const_t::type> + ProductType; + + if (!is_univariate_) + throw std::runtime_error( + "this method can only be used for univariate polynomials"); + + DRAKE_DEMAND(derivative_order >= 0); + ProductType value = 0; + using std::pow; + for (typename std::vector::const_iterator iter = + monomials_.begin(); + iter != monomials_.end(); iter++) { + PowerType degree = iter->terms.empty() ? 0 : iter->terms[0].power; + if (degree < derivative_order) continue; + T coefficient = iter->coefficient; + for (int i = 0; i < derivative_order; i++) { + coefficient *= degree--; + } + if (degree == 0) { + value += coefficient; + } else if (degree == 1) { + value += coefficient * x; + } else { // degree > 1. + value += coefficient * pow(static_cast(x), degree); + } + } + return value; + } + + /** Evaluate a multivariate Polynomial at a specific point. + * + * Evaluates a Polynomial with the given values for each variable. + * @throws std::exception if the Polynomial contains variables for which + * values were not provided. + * + * The provided values may be of any type which is std::is_arithmetic + * (supporting the std::pow, *, and + operations) and need not be + * CoefficientsType or RealScalar) + */ + template + typename Product::type EvaluateMultivariate( + const std::map& var_values) const { + using std::pow; + typedef typename std::remove_const_t< + typename Product::type> ProductType; + ProductType value = 0; + for (const Monomial& monomial : monomials_) { + ProductType monomial_value = monomial.coefficient; + for (const Term& term : monomial.terms) { + monomial_value *= + pow(static_cast(var_values.at(term.var)), term.power); + } + value += monomial_value; + } + return value; + } + + /** Substitute values for some but not necessarily all variables of a + * Polynomial. + * + * Analogous to EvaluateMultivariate, but: + * (1) Restricted to T, and + * (2) Need not map every variable in var_values. + * + * Returns a Polynomial in which each variable in var_values has been + * replaced with its value and constants appropriately combined. + */ + Polynomial EvaluatePartial( + const std::map& var_values) const; + + /// Replaces all instances of variable orig with replacement. + void Subs(const VarType& orig, const VarType& replacement); + + /// Replaces all instances of variable orig with replacement. + Polynomial Substitute(const VarType& orig, + const Polynomial& replacement) const; + + /** Takes the derivative of this (univariate) Polynomial. + * + * Returns a new Polynomial that is the derivative of this one in its sole + * variable. + * @throws std::exception if this Polynomial is not univariate. + * + * If derivative_order is given, takes the nth derivative of this + * Polynomial. + */ + Polynomial Derivative(int derivative_order = 1) const; + + /** Takes the integral of this (univariate, non-constant) Polynomial. + * + * Returns a new Polynomial that is the indefinite integral of this one in + * its sole variable. + * @throws std::exception if this Polynomial is not univariate, or if it has + * no variables. + * + * If integration_constant is given, adds that constant as the constant + * term (zeroth-order coefficient) of the resulting Polynomial. + */ + Polynomial Integral(const T& integration_constant = 0.0) const; + + bool operator==(const Polynomial& other) const; + + Polynomial& operator+=(const Polynomial& other); + + Polynomial& operator-=(const Polynomial& other); + + Polynomial& operator*=(const Polynomial& other); + + Polynomial& operator+=(const T& scalar); + + Polynomial& operator-=(const T& scalar); + + Polynomial& operator*=(const T& scalar); + + Polynomial& operator/=(const T& scalar); + + const Polynomial operator+(const Polynomial& other) const; + + const Polynomial operator-(const Polynomial& other) const; + + const Polynomial operator-() const; + + const Polynomial operator*(const Polynomial& other) const; + + friend const Polynomial operator+(const Polynomial& p, + const T& scalar) { + Polynomial ret = p; + ret += scalar; + return ret; + } + + friend const Polynomial operator+(const T& scalar, + const Polynomial& p) { + Polynomial ret = p; + ret += scalar; + return ret; + } + + friend const Polynomial operator-(const Polynomial& p, + const T& scalar) { + Polynomial ret = p; + ret -= scalar; + return ret; + } + + friend const Polynomial operator-(const T& scalar, + const Polynomial& p) { + Polynomial ret = -p; + ret += scalar; + return ret; + } + + friend const Polynomial operator*(const Polynomial& p, + const T& scalar) { + Polynomial ret = p; + ret *= scalar; + return ret; + } + friend const Polynomial operator*(const T& scalar, + const Polynomial& p) { + Polynomial ret = p; + ret *= scalar; + return ret; + } + + const Polynomial operator/(const T& scalar) const; + + /// A comparison to allow std::lexicographical_compare on this class; does + /// not reflect any sort of mathematical total order. + bool operator<(const Polynomial& other) const { + // Just delegate to the default vector std::lexicographical_compare. + return monomials_ < other.monomials_; + } + + /** Returns the roots of this (univariate) Polynomial. + * + * Returns the roots of a univariate Polynomial as an Eigen column vector of + * complex numbers whose components are of the RealScalar type. + * @throws std::exception of this Polynomial is not univariate. + */ + RootsType Roots() const; + + /** Checks if a Polynomial is approximately equal to this one. + * + * Checks that every coefficient of `other` is within `tol` of the + * corresponding coefficient of this Polynomial. + * + * Note: When `tol_type` is kRelative, if any monomials appear in `this` or + * `other` but not both, then the method returns false (since the comparison + * is relative to a missing zero coefficient). Use kAbsolute if you want to + * ignore non-matching monomials with coefficients less than `tol`. + */ + boolean CoefficientsAlmostEqual( + const Polynomial& other, const RealScalar& tol = 0.0, + const ToleranceType& tol_type = ToleranceType::kAbsolute) const; + + /** Constructs a Polynomial representing the symbolic expression `e`. + * Note that the ID of a variable is preserved in this translation. + * + * @throws std::exception if `e` is not polynomial-convertible. + * @pre e.is_polynomial() is true. + */ + static Polynomial FromExpression(const drake::symbolic::Expression& e); + + friend std::ostream& operator<<(std::ostream& os, const Monomial& m) { + // if (m.coefficient == 0) return os; + + bool print_star = false; + if (m.coefficient == -1) { + os << "-"; + } else if (m.coefficient != 1 || m.terms.empty()) { + os << '(' << m.coefficient << ")"; + print_star = true; + } + + for (typename std::vector::const_iterator iter = m.terms.begin(); + iter != m.terms.end(); iter++) { + if (print_star) + os << '*'; + else + print_star = true; + os << IdToVariableName(iter->var); + if (iter->power != 1) { + os << "^" << iter->power; + } + } + return os; + } + + friend std::ostream& operator<<(std::ostream& os, const Polynomial& poly) { + if (poly.monomials_.empty()) { + os << "0"; + return os; + } + + for (typename std::vector::const_iterator iter = + poly.monomials_.begin(); + iter != poly.monomials_.end(); iter++) { + os << *iter; + if (iter + 1 != poly.monomials_.end() && (iter + 1)->coefficient != -1) + os << '+'; + } + return os; + } + + //@{ + /** Variable name/ID conversion facility. */ + static bool IsValidVariableName(const std::string name); + + static VarType VariableNameToId(const std::string name, + unsigned int m = 1); + + static std::string IdToVariableName(const VarType id); + //@} + + template + friend Polynomial pow(const Polynomial& p, + typename Polynomial::PowerType n); + + private: + /// The Monomial atoms of the Polynomial. + std::vector monomials_; + + /// True iff only 0 or 1 distinct variables appear in the Polynomial. + bool is_univariate_; + + /// Sorts through Monomial list and merges any that have the same powers. + void MakeMonomialsUnique(void); +}; + +/** Provides power function for Polynomial. */ +template +Polynomial pow( + const Polynomial& base, + typename Polynomial::PowerType exponent) { + DRAKE_DEMAND(exponent >= 0); + if (exponent == 0) { + return Polynomial{1.0}; + } + const Polynomial pow_half{pow(base, exponent / 2)}; + if (exponent % 2 == 1) { + return base * pow_half * pow_half; // Odd exponent case. + } else { + return pow_half * pow_half; // Even exponent case. + } +} + +template +std::ostream& operator<<( + std::ostream& os, + const Eigen::Matrix, Rows, Cols>& poly_mat) { + for (int i = 0; i < poly_mat.rows(); i++) { + os << "[ "; + for (int j = 0; j < poly_mat.cols(); j++) { + os << poly_mat(i, j); + if (j < (poly_mat.cols() - 1)) os << " , "; + } + os << " ]" << std::endl; + } + return os; +} + +typedef Polynomial Polynomiald; + +/// A column vector of polynomials; used in several optimization classes. +typedef Eigen::Matrix VectorXPoly; +} // namespace drake + +DRAKE_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class drake::Polynomial) diff --git a/maliput_drake/include/drake/common/random.h b/maliput_drake/include/drake/common/random.h new file mode 100644 index 0000000..2b7bea1 --- /dev/null +++ b/maliput_drake/include/drake/common/random.h @@ -0,0 +1,61 @@ +#pragma once + +#include + +#include + +#include "drake/common/drake_copyable.h" +#include "drake/common/eigen_types.h" +#include "drake/common/extract_double.h" + +namespace drake { +/// Defines Drake's canonical implementation of the UniformRandomBitGenerator +/// C++ concept (as well as a few conventional extras beyond the concept, e.g., +/// seeds). This uses the 32-bit Mersenne Twister mt19937 by Matsumoto and +/// Nishimura, 1998. For more information, see +/// https://en.cppreference.com/w/cpp/numeric/random/mersenne_twister_engine +class RandomGenerator { + public: + DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(RandomGenerator) + + using result_type = std::mt19937::result_type; + + RandomGenerator() = default; + explicit RandomGenerator(result_type value) : generator_(value) {} + + static constexpr result_type min() { return std::mt19937::min(); } + static constexpr result_type max() { return std::mt19937::max(); } + result_type operator()() { return generator_(); } + + static constexpr result_type default_seed = std::mt19937::default_seed; + + private: + std::mt19937 generator_{}; +}; + +/// Drake supports explicit reasoning about a few carefully chosen random +/// distributions. +enum class RandomDistribution { + kUniform = 0, ///< Vector elements are independent and uniformly distributed + /// ∈ [0.0, 1.0). + kGaussian = 1, ///< Vector elements are independent and drawn from a + /// mean-zero, unit-variance normal (Gaussian) distribution. + kExponential = 2, ///< Vector elements are independent and drawn from an + /// exponential distribution with λ=1.0. +}; + +/** + * Calculates the density (probability density function) of the multivariate + * distribution. + * @param distribution The distribution type. + * @param x The value of the sampled vector. + * @tparam_nonsymbolic_scalar + * + * @note When instantiating this function, the user needs to explicitly pass in + * the scalar type, for example CalcProbabilityDensity(...), the + * compiler might have problem to deduce the scalar type automatically. + */ +template +T CalcProbabilityDensity(RandomDistribution distribution, + const Eigen::Ref>& x); +} // namespace drake diff --git a/maliput_drake/include/drake/common/reset_after_move.h b/maliput_drake/include/drake/common/reset_after_move.h new file mode 100644 index 0000000..c738759 --- /dev/null +++ b/maliput_drake/include/drake/common/reset_after_move.h @@ -0,0 +1,112 @@ +#pragma once + +#include +#include + +namespace drake { + +/// Type wrapper that performs value-initialization on the wrapped type, and +/// guarantees that when moving from this type that the donor object is reset +/// to its value-initialized value. +/// +/// Background: +/// +/// For performance reasons, we often like to provide overloaded move functions +/// on our types, instead of relying on the copy functions. When doing so, it +/// is more robust to rely on the compiler's `= default` implementation using +/// member-wise move, instead of writing out the operations manually. In +/// general, move functions should reset the donor object of the move to its +/// default-constructed (empty) resource state. Inductively, the member +/// fields' existing move implementations do this already, except in the case +/// of non-class (primitive) members, where the donor object's primitives will +/// not be zeroed. By wrapping primitive members fields with this type, they +/// are guaranteed to be zeroed on construction and after being moved from. +/// +/// Example: +/// +/// 
+/// class Foo {
+///  public:
+///   DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Foo)
+///   Foo() = default;
+///
+///  private:
+///   std::vector items_;
+///   reset_after_move sum_;
+/// };
+///
+/// +/// When moving from `Foo`, the donor object will reset to its default state: +/// `items_` will be empty and `sum_` will be zero. If `Foo` had not used the +/// `reset_after_move` wrapper, the `sum_` would remain intact (be copied) +/// while moving, even though `items_` was cleared. +/// +/// @tparam T must support CopyConstructible, CopyAssignable, MoveConstructible, +/// and MoveAssignable and must not throw exceptions during construction or +/// assignment. +/// @see reset_on_copy +template +class reset_after_move { + public: + /// Constructs a reset_after_move with a value-initialized wrapped value. + /// See http://en.cppreference.com/w/cpp/language/value_initialization. + reset_after_move() noexcept(std::is_nothrow_default_constructible_v) {} + + /// Constructs a reset_after_move with the given wrapped value. This is + /// an implicit conversion, so that reset_after_move behaves more like + /// the unwrapped type. + // NOLINTNEXTLINE(runtime/explicit) + reset_after_move(const T& value) noexcept( + std::is_nothrow_copy_constructible_v) + : value_(value) {} + + /// @name Implements CopyConstructible, CopyAssignable, MoveConstructible, + /// MoveAssignable. + //@{ + reset_after_move(const reset_after_move&) = default; + reset_after_move& operator=(const reset_after_move&) = default; + reset_after_move(reset_after_move&& other) noexcept( + std::is_nothrow_default_constructible_v && + std::is_nothrow_move_assignable_v) { + value_ = std::move(other.value_); + other.value_ = T{}; + } + reset_after_move& operator=(reset_after_move&& other) noexcept( + std::is_nothrow_default_constructible_v && + std::is_nothrow_move_assignable_v) { + if (this != &other) { + value_ = std::move(other.value_); + other.value_ = T{}; + } + return *this; + } + //@} + + /// @name Implicit conversion operators to make reset_after_move act + /// as the wrapped type. + //@{ + operator T&() { return value_; } + operator const T&() const { return value_; } + //@} + + /// @name Dereference operators if T is a pointer type. + /// If type T is a pointer, these exist and return the pointed-to object. + /// For non-pointer types these methods are not instantiated. + //@{ + template + std::enable_if_t, T> operator->() const { + return value_; + } + template + std::enable_if_t, + std::add_lvalue_reference_t>> + operator*() const { + return *value_; + } + //@} + + private: + T value_{}; +}; + +} // namespace drake diff --git a/maliput_drake/include/drake/common/reset_on_copy.h b/maliput_drake/include/drake/common/reset_on_copy.h new file mode 100644 index 0000000..3026a02 --- /dev/null +++ b/maliput_drake/include/drake/common/reset_on_copy.h @@ -0,0 +1,186 @@ +#pragma once + +#include +#include + +namespace drake { + +// NOTE(sherm1) to future implementers: if you decide to extend this adapter for +// use with class types, be sure to think carefully about the semantics of copy +// and move and how to explain that to users. Be aware that for class types T, +// the implementation of `T{}` (the default constructor, which may have been +// user-supplied) won't necessarily reset T's members to zero, nor even +// necessarily value-initialize T's members. Also, the "noexcept" reasoning +// below is more than we need with the std::is_scalar restriction, but is +// strictly necessary for class types if you want std::vector to choose move +// construction (content-preserving) over copy construction (resetting). +/// Type wrapper that performs value-initialization on copy construction or +/// assignment. +/// +/// Rather than copying the source supplied for copy construction or copy +/// assignment, this wrapper instead value-initializes the destination object. +/// Move assignment and construction preserve contents in the destination as +/// usual, but reset the source to its value-initialized value. +/// +/// Only types T that satisfy `std::is_scalar` are currently +/// permitted: integral and floating point types, enums, and pointers. +/// Value initialization means the initialization performed when a variable is +/// constructed with an empty initializer `{}`. For the restricted set of types +/// we support, that just means that numeric types are set to zero and pointer +/// types are set to nullptr. Also, all the methods here are noexcept due to the +/// `std::is_scalar` restriction. +/// See http://en.cppreference.com/w/cpp/language/value_initialization. +/// +/// Background: +/// +/// It is preferable to use default copy construction for classes whenever +/// possible because it avoids difficult-to-maintain enumeration of member +/// fields in bespoke copy constructors. The presence of fields that must be +/// reset to zero in the copy (counters, for example) prevents use of default +/// copy construction. Similarly, pointers that would be invalid in the copy +/// need to be set to null to avoid stale references. By wrapping those +/// problematic data members in this adapter, default copy construction can +/// continue to be used, with all data members copied properly except the +/// designated ones, which are value-initialized instead. The resetting of the +/// source on move doesn't change semantics since the condition of the source +/// after a move is generally undefined. It is instead opportunistic good +/// hygiene for early detection of bugs, taking advantage of the fact that we +/// know type T can be value-initialized. See reset_after_move for more +/// discussion. +/// +/// Example: +/// +/// 
+/// class Foo {
+///  public:
+///   DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Foo)
+///   Foo() = default;
+///
+///  private:
+///   std::vector items_;
+///   reset_on_copy use_count_;
+/// };
+///
+/// +/// When copying from `Foo`, the new object will contain a copy of `items_` +/// but `use_count_` will be zero. If `Foo` had not used the +/// `reset_on_copy` wrapper, `use_count_` would have been copied also, +/// which we're assuming is not the desired behavior here. +/// +/// @warning Even if you initialize a %reset_on_copy member to a non-zero value +/// using an initializer like `reset_on_copy some_member_{5}` it will be +/// _reset_ to zero, not _reinitialized_ to 5 when copied. +/// +/// @note Enum types T are permitted, but be aware that they will be reset to +/// zero, regardless of whether 0 is one of the specified enumeration values. +/// +/// @tparam T must satisfy `std::is_scalar`. +/// @see reset_after_move +template +class reset_on_copy { + public: + static_assert(std::is_scalar_v, + "reset_on_copy is permitted only for integral, " + "floating point, and pointer types T."); + + /// Constructs a reset_on_copy with a value-initialized wrapped value. + reset_on_copy() noexcept(std::is_nothrow_default_constructible_v) {} + + /// Constructs a %reset_on_copy with a copy of the given value. This is + /// an implicit conversion, so that %reset_on_copy behaves more like + /// the unwrapped type. + // NOLINTNEXTLINE(runtime/explicit) + reset_on_copy(const T& value) noexcept( + std::is_nothrow_copy_constructible_v) + : value_(value) {} + + /// Constructs a %reset_on_copy with the given wrapped value, by move + /// construction if possible. This is an implicit conversion, so that + /// %reset_on_copy behaves more like the unwrapped type. + // NOLINTNEXTLINE(runtime/explicit) + reset_on_copy(T&& value) noexcept( + std::is_nothrow_move_constructible_v) + : value_(std::move(value)) {} + + /// @name Implements copy/move construction and assignment. + /// These make %reset_on_copy objects CopyConstructible, CopyAssignable, + /// MoveConstructible, and MoveAssignable. + //@{ + + /// Copy constructor just value-initializes instead; the source is ignored. + reset_on_copy(const reset_on_copy&) noexcept( + std::is_nothrow_default_constructible_v) {} + + /// Copy assignment just destructs the contained value and then + /// value-initializes it, _except_ for self-assignment which does nothing. + /// The source argument is otherwise ignored. + reset_on_copy& operator=(const reset_on_copy& source) noexcept( + std::is_nothrow_destructible_v && + std::is_nothrow_default_constructible_v) { + if (this != &source) destruct_and_reset_value(); + return *this; + } + + /// Move construction uses T's move constructor, then destructs and + /// value initializes the source. + reset_on_copy(reset_on_copy&& source) noexcept( + std::is_nothrow_move_constructible_v && + std::is_nothrow_destructible_v && + std::is_nothrow_default_constructible_v) + : value_(std::move(source.value_)) { + source.destruct_and_reset_value(); + } + + /// Move assignment uses T's move assignment, then destructs and value + /// initializes the source, _except_ for self-assignment which does nothing. + /// The source argument is otherwise ignored. + reset_on_copy& operator=(reset_on_copy&& source) noexcept( + std::is_nothrow_move_assignable_v && + std::is_nothrow_destructible_v && + std::is_nothrow_default_constructible_v) { + if (this != &source) { + value_ = std::move(source); + source.destruct_and_reset_value(); + } + return *this; + } + //@} + + /// @name Implicit conversion operators to make reset_on_copy act + /// as the wrapped type. + //@{ + operator T&() noexcept { return value_; } + operator const T&() const noexcept { return value_; } + //@} + + /// @name Dereference operators if T is a pointer type. + /// If type T is a pointer, these exist and return the pointed-to object. + /// For non-pointer types these methods are not instantiated. + //@{ + template + std::enable_if_t, T> operator->() const noexcept { + return value_; + } + + template + std::enable_if_t, + std::add_lvalue_reference_t>> + operator*() const noexcept { + return *value_; + } + //@} + + private: + // Invokes T's destructor if there is one, then value-initializes. Note that + // the noexcept code above assumes exactly this implementation. Don't change + // this to `value_ = T{}` which would introduce an additional dependence on + // the behavior of T's copy assignment operator. + void destruct_and_reset_value() { + value_.~T(); // Invokes destructor if there is one. + new (&value_) T{}; // Placement new; no heap activity. + } + + T value_{}; +}; + +} // namespace drake diff --git a/maliput_drake/include/drake/common/scope_exit.h b/maliput_drake/include/drake/common/scope_exit.h new file mode 100644 index 0000000..f09f248 --- /dev/null +++ b/maliput_drake/include/drake/common/scope_exit.h @@ -0,0 +1,61 @@ +#pragma once + +#include +#include + +#include "drake/common/drake_copyable.h" +#include "drake/common/drake_throw.h" + +namespace drake { + +/// Helper class to create a scope exit guard -- an object that when destroyed +/// runs `func`. This is useful to apply RAII to third-party code that only +/// supports manual acquire and release operations. +/// +/// Example: +/// +/// @code +/// void some_function() { +/// void* foo = ::malloc(10); +/// ScopeExit guard([foo]() { +/// ::free(foo); +/// }); +/// +/// // ... +/// if (condition) { throw std::runtime_error("..."); } +/// // ... +/// } +/// @endcode +/// +/// Here, the allocation of `foo` will always be free'd no matter whether +/// `some_function` returns normally or via an exception. +class ScopeExit final { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(ScopeExit); + + /// Creates a resource that will call `func` when destroyed. Note that + /// `func()` should not throw an exception, since it will typically be + /// invoked during stack unwinding. + explicit ScopeExit(std::function func) + : func_(std::move(func)) { + DRAKE_THROW_UNLESS(func_ != nullptr); + } + + /// Invokes the `func` that was passed into the constructor, unless this has + /// been disarmed. + ~ScopeExit() { + if (func_) { + func_(); + } + } + + /// Disarms this guard, so that the destructor has no effect. + void Disarm() { + func_ = nullptr; + } + + private: + std::function func_; +}; + +} // namespace drake diff --git a/maliput_drake/include/drake/common/scoped_singleton.h b/maliput_drake/include/drake/common/scoped_singleton.h new file mode 100644 index 0000000..09bea7b --- /dev/null +++ b/maliput_drake/include/drake/common/scoped_singleton.h @@ -0,0 +1,75 @@ +#pragma once + +#include +#include + +#include "drake/common/drake_assert.h" +#include "drake/common/drake_copyable.h" +#include "drake/common/never_destroyed.h" +#include "drake/common/text_logging.h" + +namespace drake { + +/** + * Provides thread-safe, global-safe access to a shared resource. When + * all references are gone, the resource will be freed due to using a weak_ptr. + * @tparam T Class of the resource. Must be default-constructible. + * @tparam Unique Optional class, meant to make a unique specialization, such + * that you can have multiple singletons of T if necessary. + * + * @note An example application is obtaining license for multiple disjoint + * solver objects, where acquiring a license requires network communication, + * and the solver is under an interface where you cannot explicitly pass the + * license resource to the solver without violating encapsulation. + */ +template +std::shared_ptr GetScopedSingleton() { + // Confine implementation to a class. + class Singleton { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Singleton) + + Singleton() {} + + /* + * Acquire a reference to resource if no other instance exists. + * Otherwise, return a shared reference to the resource. + */ + std::shared_ptr Acquire() { + // We must never create more than one instance of a T at a time, since it + // is supposed to be a singleton. Thus, we need at least the make_shared + // to appear in a critical section. Rather than worrying about a double- + // checked locking pattern, we'll just hold the lock for the entire + // method for simplicity. + std::lock_guard lock(mutex_); + std::shared_ptr result; + + // Attempt to promote the weak_ptr to a shared_ptr. If the singleton + // already existed, this will extend its lifetime to our return value. + result = weak_ref_.lock(); + if (!result) { + // The singleton does not exist. Since we hold the exclusive lock on + // weak_ref_, we know that its safe for us to create the singleton now. + result = std::make_shared(); + + // Store the weak reference to the result, so that other callers will + // be able to use it, as long as our caller keeps it alive. + weak_ref_ = result; + } + + DRAKE_DEMAND(result.get() != nullptr); + return result; + } + + private: + // The mutex guards all access and changes to weak_ref_, but does not guard + // the use of the T instance that weak_ref_ points to. + std::mutex mutex_; + std::weak_ptr weak_ref_; + }; + // Allocate singleton as a static function local to control initialization. + static never_destroyed singleton; + return singleton.access().Acquire(); +} + +} // namespace drake diff --git a/maliput_drake/include/drake/common/sorted_pair.h b/maliput_drake/include/drake/common/sorted_pair.h new file mode 100644 index 0000000..282021e --- /dev/null +++ b/maliput_drake/include/drake/common/sorted_pair.h @@ -0,0 +1,201 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include "drake/common/drake_copyable.h" +#include "drake/common/hash.h" +#include "drake/common/is_less_than_comparable.h" + +/// @file +/// Provides drake::MakeSortedPair and drake::SortedPair for storing two +/// values of a certain type in sorted order. + +namespace drake { + +/// This class is similar to the std::pair class. However, this class uses a +/// pair of homogeneous types (std::pair can use heterogeneous types) and sorts +/// the first and second values such that the first value is less than or equal +/// to the second one). Note that the sort is a stable one. Thus the SortedPair +/// class is able to be used to generate keys (e.g., for std::map, etc.) from +/// pairs of objects. +/// +/// @tparam T A template type that provides `operator<` and supports default +/// construction. +template +struct SortedPair { + DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(SortedPair) + static_assert(is_less_than_comparable::value, "SortedPair can only be used" + "with types that can be compared using the less-than operator " + "(operator<)."); + + /// The default constructor creates `first()` and `second()` using their + /// respective default constructors. + SortedPair() = default; + + /// Rvalue reference constructor, permits constructing with std::unique_ptr + /// types, for example. + SortedPair(T&& a, T&& b) { + if (b < a) { + first_ = std::move(b); + second_ = std::move(a); + } else { + first_ = std::move(a); + second_ = std::move(b); + } + } + + /// Constructs a %SortedPair from two objects. + SortedPair(const T& a, const T& b) : first_(a), second_(b) { + if (second_ < first_) + std::swap(first_, second_); + } + + /// Type-converting copy constructor. + template + SortedPair(SortedPair&& u) : first_{std::forward(u.first())}, + second_{std::forward(u.second())} {} + + /// Resets the stored objects. + template + void set(U&& a, U&& b) { + first_ = std::forward(a); + second_ = std::forward(b); + if (second_ < first_) + std::swap(first_, second_); + } + + /// Gets the first (according to `operator<`) of the objects. + const T& first() const { return first_; } + + /// Gets the second (according to `operator<`) of the objects. + const T& second() const { return second_; } + + /// Swaps `this` and `t`. + void Swap(drake::SortedPair& t) { + std::swap(t.first_, first_); + std::swap(t.second_, second_); + } + + /// Implements the @ref hash_append concept. + template + friend void hash_append(HashAlgorithm& hasher, const SortedPair& p) noexcept { + using drake::hash_append; + hash_append(hasher, p.first_); + hash_append(hasher, p.second_); + } + + /// @name Support for using SortedPair in structured bindings. + //@{ + template + std::tuple_element_t>& get() { + if constexpr (Index == 0) return first_; + if constexpr (Index == 1) return second_; + } + + template + const std::tuple_element_t>& get() const { + if constexpr (Index == 0) return first_; + if constexpr (Index == 1) return second_; + } + //@} + + private: + T first_{}; // The first of the two objects, according to operator<. + T second_{}; // The second of the two objects, according to operator<. +}; + +/// Support writing a SortedPair to a stream (conditional on the support of +/// writing the underlying type T to a stream). +template +inline std::ostream& operator<<(std::ostream& out, const SortedPair& pair) { + out << "(" << pair.first() << ", " << pair.second() << ")"; + return out; +} + +/// Two pairs of the same type are equal iff their members are equal after +/// sorting. +template +inline bool operator==(const SortedPair& x, const SortedPair& y) { + return !(x < y) && !(y < x); +} + +/// Compares two pairs using lexicographic ordering. +template +inline bool operator<(const SortedPair& x, const SortedPair& y) { + return std::tie(x.first(), x.second()) < std::tie(y.first(), y.second()); +} + +/// Determine whether two SortedPair objects are not equal using `operator==`. +template +inline bool operator!=( + const SortedPair& x, const SortedPair& y) { + return !(x == y); +} + +/// Determines whether `x > y` using `operator<`. +template +inline bool operator>(const SortedPair& x, const SortedPair& y) { + return y < x; +} + +/// Determines whether `x <= y` using `operator<`. +template +inline bool operator<=(const SortedPair& x, const SortedPair& y) { + return !(y < x); +} + +/// Determines whether `x >= y` using `operator<`. +template +inline bool +operator>=(const SortedPair& x, const SortedPair& y) { + return !(x < y); +} + +/// @brief A convenience wrapper for creating a sorted pair from two objects. +/// @param x The first_ object. +/// @param y The second_ object. +/// @return A newly-constructed SortedPair object. +template +inline constexpr SortedPair::type> +MakeSortedPair(T&& x, T&& y) { + return SortedPair< + typename std::decay::type>(std::forward(x), std::forward(y)); +} + +} // namespace drake + +namespace std { + +/// Implements std::swap(). +template +void swap(drake::SortedPair& t, drake::SortedPair& u) { + t.Swap(u); +} + +/// Provides std::hash>. +template +struct hash> + : public drake::DefaultHash {}; +#if defined(__GLIBCXX__) +// https://gcc.gnu.org/onlinedocs/libstdc++/manual/unordered_associative.html +template +struct __is_fast_hash>> : std::false_type {}; +#endif + +/// Support using `SortedPair` in structured bindings. E.g., +/// +/// SortedPair pair(Foo(1), Foo(2)); +/// const auto& [a, b] = pair; +template +struct tuple_size> : integral_constant {}; + +template +struct tuple_element> { + using type = T; +}; + +} // namespace std diff --git a/maliput_drake/include/drake/common/symbolic.h b/maliput_drake/include/drake/common/symbolic.h new file mode 100644 index 0000000..5eb0964 --- /dev/null +++ b/maliput_drake/include/drake/common/symbolic.h @@ -0,0 +1,53 @@ +#pragma once +/// @file +/// Provides public header files of Drake's symbolic library. +/// A user of the symbolic library should only include this header file. +/// Including other individual headers such as symbolic_expression.h will +/// generate a compile-time error. +/// +/// Many symbolic types are not closed under the defined operations. For +/// example, relational operations (i.e. <) over symbolic::Expression produce +/// symbolic::Formula. Another example is addition (+) over Monomial which gives +/// Polynomial. If a user does not include the necessary set of header files, +/// he/she will get either 1) incomprehensible c++ errors or 2) undefined +/// runtime behaviors. The problem is trickier if we use symbolic objects via +/// Eigen. + +// In each header included below, it asserts that this macro +// `DRAKE_COMMON_SYMBOLIC_HEADER` is defined. If the macro is not defined, it +// generates diagnostic error messages. +#define DRAKE_COMMON_SYMBOLIC_HEADER + +// Do not alpha-sort the following block of hard-coded #includes, which is +// protected by `clang-format on/off`. +// +// Rationale: We want to maximize the use of this header, `symbolic.h`, even +// inside of the symbolic library files to avoid any mistakes which might not be +// detected. By centralizing the list here, we make sure that everyone will see +// the correct order which respects the inter-dependencies of the symbolic +// headers. This shields us from triggering undefined behaviors due to +// order-of-specialization-includes-changed mistakes. +// +// clang-format off +#include "drake/common/symbolic_variable.h" +#include "drake/common/symbolic_variables.h" +#include "drake/common/symbolic_environment.h" +#include "drake/common/symbolic_expression.h" +#include "drake/common/symbolic_expression_visitor.h" +#include "drake/common/symbolic_ldlt.h" +#include "drake/common/symbolic_monomial.h" +#include "drake/common/symbolic_monomial_util.h" +#include "drake/common/symbolic_polynomial.h" +#include "drake/common/symbolic_polynomial_basis_element.h" +#include "drake/common/symbolic_chebyshev_basis_element.h" +#include "drake/common/symbolic_monomial_basis_element.h" +#include "drake/common/symbolic_chebyshev_polynomial.h" +#include "drake/common/symbolic_polynomial_basis.h" +#include "drake/common/symbolic_generic_polynomial.h" +#include "drake/common/symbolic_rational_function.h" +#include "drake/common/symbolic_formula.h" +#include "drake/common/symbolic_formula_visitor.h" +#include "drake/common/symbolic_simplification.h" +#include "drake/common/symbolic_codegen.h" +// clang-format on +#undef DRAKE_COMMON_SYMBOLIC_HEADER diff --git a/maliput_drake/include/drake/common/symbolic_chebyshev_basis_element.h b/maliput_drake/include/drake/common/symbolic_chebyshev_basis_element.h new file mode 100644 index 0000000..ad53733 --- /dev/null +++ b/maliput_drake/include/drake/common/symbolic_chebyshev_basis_element.h @@ -0,0 +1,156 @@ +#pragma once + +#ifndef DRAKE_COMMON_SYMBOLIC_HEADER +#error Do not directly include this file. Include "drake/common/symbolic.h". +#endif + +#include +#include + +#include "drake/common/drake_copyable.h" +#include "drake/common/hash.h" +#include "drake/common/symbolic.h" + +namespace drake { +namespace symbolic { +/** + * ChebyshevBasisElement represents an element of Chebyshev polynomial basis, + * written as the product of Chebyshev polynomials, in the form + * Tₚ₀(x₀)Tₚ₁(x₁)...Tₚₙ(xₙ), where each Tₚᵢ(xᵢ) is a (univariate) Chebyshev + * polynomial of degree pᵢ. + */ +class ChebyshevBasisElement : public PolynomialBasisElement { + public: + DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(ChebyshevBasisElement) + + /** Constructs a ChebyshevBasisElement equals to 1. */ + ChebyshevBasisElement(); + + explicit ChebyshevBasisElement( + const std::map& var_to_degree_map); + + /** Constructs a Chebyshev polynomial T₁(var). */ + explicit ChebyshevBasisElement(const Variable& var); + + /** Constructs a Chebyshev polynomial Tₙ(var) where n = degree. */ + ChebyshevBasisElement(const Variable& var, int degree); + + /** Constructs a default value 1. This overload is used by Eigen when + * EIGEN_INITIALIZE_MATRICES_BY_ZERO is enabled. + */ + explicit ChebyshevBasisElement(std::nullptr_t); + + ChebyshevBasisElement(const Eigen::Ref>& vars, + const Eigen::Ref& degrees); + + ~ChebyshevBasisElement() override = default; + + /** + * Compares two ChebyshevBasisElement in lexicographic order. + */ + bool operator<(const ChebyshevBasisElement& other) const; + + /** + * Differentiates the ChebyshevBasisElement with respect to a variable. + * We use the fact that + * - If n is even dTₙ(x)/dx = 2n ∑ⱼ Tⱼ(x), j is odd and 1 <= j <= n-1 + * - If n is odd dTₙ(x)/dx = 2n ∑ⱼ Tⱼ(x) - n, j is even and 0 <= j <= n-1 + * We return `result`, a map from ChebyshevBasisElement to double, such that + * sum(result.key() * result[key]) is the differentiation of `this` w.r.t the + * variable. + * For example if n is even, dTₙ(x)Tₘ(y)/dx = 2n∑ⱼ Tⱼ(x)Tₘ(y), j is odd and + * 1 <= j <= n-1, then the returned result is {T₁(x)Tₘ(y), 2n}, {T₃(x)Tₘ(y), + * 2n}, ..., {T₂ₙ₋₁(x)Tₘ(y), 2n}. A special case is that @p var is not a + * variable in `this`, then we return an empty map. + * @param var A variable to differentiate with. + */ + [[nodiscard]] std::map Differentiate( + const Variable& var) const; + + /** + * Integrates a ChebyshevBasisElement for a variable. + * We use the fact that + * ∫ Tₙ(x)dx = 1/(2n+2)Tₙ₊₁(x) − 1/(2n−2)Tₙ₋₁(x) + * A special case is ∫ T₀(x)dx = T₁(x) + * @param var The variable to integrate. If @param var is not a variable in + * this ChebyshevBasisElement, then the integration result is *this * T₁(var). + * @retval result sum(key * result[key]) is the integration result. For + * example, ∫ T₂(x)T₃(y)dx = 1/6*T₃(x)T₃(y) − 1/2 * T₁(x)T₃(y), then the + * result is the map containing {T₃(x)T₃(y), 1/6} and {T₁(x)T₃(y), -1/2}. + */ + [[nodiscard]] std::map Integrate( + const Variable& var) const; + + /** Merges this Chebyshev basis element with another Chebyshev basis element + * @p other by merging their var_to_degree_map. After merging, the degree of + * each variable is raised to the sum of the degree in each basis element (if + * a variable does not show up in either one of the basis element, we regard + * its degree to be 0). For example, merging T₁(x)T₃(y) and T₂(x)T₄(z) gets + * T₃(x)T₃(y)T₄(z). + */ + void MergeBasisElementInPlace(const ChebyshevBasisElement& other); + + /** Partially evaluates using a given environment @p env. The evaluation + * result is of type pair. The first component + * (: double) represents the coefficient part while the second component + * represents the remaining parts of the ChebyshevBasisElement which was not + * evaluated, the product of the first and the second component is the result + * of the partial evaluation. For example, if this ChebyshevBasisElement is + * T₂(x)T₃(y)T₁(z), and @p env stores x→ 3, y→ 2, then the partial evaluation + * is T₂(3)*T₃(2)*T₁(z) = 17 * 26 * T₁(z) = 442*T₁(z), then we return the pair + * (442, T₁(z)). + */ + [[nodiscard]] std::pair EvaluatePartial( + const Environment& env) const; + + /** Implements the @ref hash_append concept. */ + template + friend void hash_append(HashAlgorithm& hasher, + const ChebyshevBasisElement& item) noexcept { + using drake::hash_append; + // We do not send total_degree_ to the hasher, because it is already fully + // represented by var_to_degree_map_ -- it is just a cached tally of the + // exponents. + hash_append(hasher, item.var_to_degree_map()); + } + + private: + [[nodiscard]] double DoEvaluate(double variable_val, + int degree) const override; + [[nodiscard]] Expression DoToExpression() const override; +}; + +/** + * Returns the product of two Chebyshev basis elements. + * Since Tₘ(x) * Tₙ(x) = 0.5 (Tₘ₊ₙ(x) + Tₘ₋ₙ(x)) if m >= n, the product of + * Chebyshev basis elements is the weighted sum of several Chebyshev basis + * elements. For example T₁(x)T₂(y) * T₃(x)T₁(y) = 0.25*(T₄(x)T₃(y) + T₂(x)T₃(y) + * + T₄(x)T₁(y) + T₂(x)T₁(y)) + * @return the result of the product, from each ChebyshevBasisElement to its + * coefficient. In the example above, it returns (T₄(x)T₃(y) -> 0.25), + * (T₂(x)T₃(y) -> 0.25), (T₄(x)T₁(y) -> 0.25) and (T₂(x)T₁(y) -> 0.25) + */ +std::map operator*( + const ChebyshevBasisElement& a, const ChebyshevBasisElement& b); + +std::ostream& operator<<(std::ostream& out, const ChebyshevBasisElement& m); +} // namespace symbolic +} // namespace drake + +namespace std { +/* Provides std::hash. */ +template <> +struct hash + : public drake::DefaultHash {}; +} // namespace std + +#if !defined(DRAKE_DOXYGEN_CXX) +namespace Eigen { +// Eigen scalar type traits for Matrix. +template <> +struct NumTraits + : GenericNumTraits { + static inline int digits10() { return 0; } +}; +} // namespace Eigen +#endif // !defined(DRAKE_DOXYGEN_CXX) diff --git a/maliput_drake/include/drake/common/symbolic_chebyshev_polynomial.h b/maliput_drake/include/drake/common/symbolic_chebyshev_polynomial.h new file mode 100644 index 0000000..5eae79b --- /dev/null +++ b/maliput_drake/include/drake/common/symbolic_chebyshev_polynomial.h @@ -0,0 +1,134 @@ +#pragma once +#ifndef DRAKE_COMMON_SYMBOLIC_HEADER +#error Do not directly include this file. Include "drake/common/symbolic.h". +#endif + +#include +#include +#include + +#include + +#include "drake/common/drake_copyable.h" +#include "drake/common/hash.h" +#include "drake/common/symbolic.h" + +namespace drake { +namespace symbolic { +/** + * Represents the Chebyshev polynomial of the first kind Tₙ(x). + * One definition of Chebyshev polynomial of the first kind is + * Tₙ(cos(θ)) = cos(nθ) + * It can also be defined recursively as + * + * T₀(x) = 1 + * T₁(x) = x + * Tₙ₊₁(x) = 2xTₙ(x) − Tₙ₋₁(x) + */ +class ChebyshevPolynomial { + public: + DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(ChebyshevPolynomial) + + /** + * Constructs a Chebyshev polynomial Tₙ(x) + * @param var The variable x + * @param degree The Chebyshev polynomial is of degree n. + * @pre degree >= 0. + */ + ChebyshevPolynomial(Variable var, int degree); + + /** Getter for the variable. */ + [[nodiscard]] const Variable& var() const { return var_; } + + /** Getter for the degree of the Chebyshev polynomial. */ + [[nodiscard]] int degree() const { return degree_; } + + /** + * Converts this Chebyshev polynomial to a polynomial with monomial basis. + */ + [[nodiscard]] Polynomial ToPolynomial() const; + + /** + * Evaluates this Chebyshev polynomial at @p var_val. + */ + [[nodiscard]] double Evaluate(double var_val) const; + + /** + * Checks if this and @p other represent the same Chebyshev polynomial. Two + * Chebyshev polynomials are equal iff their variable and degree are the same, + * or they both have degree 0. + * @note T₀(x) = T₀(y) = 1 + */ + bool operator==(const ChebyshevPolynomial& other) const; + + /** Checks if this and @p other do not represent the same Chebyshev + * polynomial. */ + bool operator!=(const ChebyshevPolynomial& other) const; + + /** + * Compare this to another Chebyshev polynomial, returns True if this is + * regarded as less than the other, otherwise returns false. + * + * If this.var() < other.var(), return True. + * If this.var() > other.var(), return False. + * If this.var() == other.var(), then return this.degree() < + * other.degree(). + * + * A special case is when this.degree() == 0 or other.degree() + * == 0. In this case the variable doesn't matter, and we return this.degree() + * < other.degree(). + */ + bool operator<(const ChebyshevPolynomial& other) const; + + /** + * Computes the differentiation of a Chebyshev polynomial + * dTₙ(x)/dx = nUₙ₋₁(x) + * where Uₙ₋₁(x) is a Chebyshev polynomial of the second kind. Uₙ₋₁(x) can + * be written as a summation of Chebyshev polynomials of the first kind + * with lower degrees. + * - If n is even dTₙ(x)/dx = 2n ∑ⱼ Tⱼ(x), j is odd and j <= n-1 + * - If n is odd dTₙ(x)/dx = 2n ∑ⱼ Tⱼ(x) - n, j is even and j <= n-1 + * - A special case is that dT₀(x)/dx = 0. + * @retval chebyshev_coeff_pairs. sum(chebyshev_coeff_pairs[j].first * + * chebyshev_coeff_pairs[j].second) is the differentiation dTₙ(x)/dx. If n is + * even, then chebyshev_coeff_pairs[j] = (T₂ⱼ₋₁(x), 2n). If n is odd, then + * chebyshev_coeff_pairs[j] = (T₂ⱼ(x), 2n) for j >= 1, and + * chebyshev_coeff_pairs[0] = (T₀(x), n). + * For the special case when degree() == 0, we return an empty vector. + */ + [[nodiscard]] std::vector> + Differentiate() const; + + /** Implements the @ref hash_append concept. */ + template + friend void hash_append(HashAlgorithm& hasher, + const ChebyshevPolynomial& item) noexcept { + if (item.degree() == 0) { + hash_append(hasher, item.degree()); + } else { + hash_append(hasher, std::make_pair(item.var().get_id(), item.degree())); + } + } + + private: + Variable var_{}; + int degree_{}; +}; + +std::ostream& operator<<(std::ostream& out, const ChebyshevPolynomial& p); + +/** + * Evaluates a Chebyshev polynomial at a given value. + * @param var_val The value of the variable. + * @param degree The degree of the Chebyshev polynomial. + */ +double EvaluateChebyshevPolynomial(double var_val, int degree); +} // namespace symbolic +} // namespace drake + +namespace std { +/* Provides std::hash. */ +template <> +struct hash : public drake::DefaultHash { +}; +} // namespace std diff --git a/maliput_drake/include/drake/common/symbolic_codegen.h b/maliput_drake/include/drake/common/symbolic_codegen.h new file mode 100644 index 0000000..327d888 --- /dev/null +++ b/maliput_drake/include/drake/common/symbolic_codegen.h @@ -0,0 +1,336 @@ +#pragma once + +#include +#include +#include +#include + +#include +#include + +#include "drake/common/drake_copyable.h" +#include "drake/common/symbolic.h" + +namespace drake { +namespace symbolic { + +/// Visitor class for code generation. +class CodeGenVisitor { + public: + using IdToIndexMap = + std::unordered_map::size_type>; + + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(CodeGenVisitor) + + /// Constructs an instance of this visitor class using the vector of + /// variables, @p parameters. This visitor will map a symbolic variable `var` + /// into `p[n]` where `n` is the index of the variable `var` in the given @p + /// parameters. + explicit CodeGenVisitor(const std::vector& parameters); + + /// Generates C expression for the expression @p e. + [[nodiscard]] std::string CodeGen(const Expression& e) const; + + private: + [[nodiscard]] std::string VisitVariable(const Expression& e) const; + [[nodiscard]] std::string VisitConstant(const Expression& e) const; + [[nodiscard]] std::string VisitAddition(const Expression& e) const; + [[nodiscard]] std::string VisitMultiplication(const Expression& e) const; + // Helper method to handle unary cases. + [[nodiscard]] std::string VisitUnary(const std::string& f, + const Expression& e) const; + // Helper method to handle binary cases. + [[nodiscard]] std::string VisitBinary(const std::string& f, + const Expression& e) const; + [[nodiscard]] std::string VisitPow(const Expression& e) const; + [[nodiscard]] std::string VisitDivision(const Expression& e) const; + [[nodiscard]] std::string VisitAbs(const Expression& e) const; + [[nodiscard]] std::string VisitLog(const Expression& e) const; + [[nodiscard]] std::string VisitExp(const Expression& e) const; + [[nodiscard]] std::string VisitSqrt(const Expression& e) const; + [[nodiscard]] std::string VisitSin(const Expression& e) const; + [[nodiscard]] std::string VisitCos(const Expression& e) const; + [[nodiscard]] std::string VisitTan(const Expression& e) const; + [[nodiscard]] std::string VisitAsin(const Expression& e) const; + [[nodiscard]] std::string VisitAcos(const Expression& e) const; + [[nodiscard]] std::string VisitAtan(const Expression& e) const; + [[nodiscard]] std::string VisitAtan2(const Expression& e) const; + [[nodiscard]] std::string VisitSinh(const Expression& e) const; + [[nodiscard]] std::string VisitCosh(const Expression& e) const; + [[nodiscard]] std::string VisitTanh(const Expression& e) const; + [[nodiscard]] std::string VisitMin(const Expression& e) const; + [[nodiscard]] std::string VisitMax(const Expression& e) const; + [[nodiscard]] std::string VisitCeil(const Expression& e) const; + [[nodiscard]] std::string VisitFloor(const Expression& e) const; + [[nodiscard]] std::string VisitIfThenElse(const Expression& e) const; + [[nodiscard]] std::string VisitUninterpretedFunction( + const Expression& e) const; + // Makes VisitExpression a friend of this class so that it can use private + // methods. + friend std::string VisitExpression(const CodeGenVisitor*, + const Expression&); + + IdToIndexMap id_to_idx_map_; +}; + +/// @defgroup codegen Code Generation +/// @ingroup technical_notes +/// @{ +/// Provides `CodeGen` functions which generate C99 code to evaluate symbolic +/// expressions and matrices. +/// +/// @note Generated code does not contain `#include` directives while it may use +/// math functions defined in `` such as `sin`, `cos`, `exp`, and `log`. +/// A user of generated code is responsible to include `` if needed to +/// compile generated code. + +/// For a given symbolic expression @p e, generates two C functions, +/// `` and `_meta`. The generated +/// `` function takes an array of doubles for parameters and +/// returns an evaluation result. `_meta` returns a nested struct +/// from which a caller can obtain the following information: +/// - `.p.size`: the size of input parameters. +/// +/// @param[in] function_name Name of the generated C function. +/// @param[in] parameters Vector of variables provide the ordering of +/// symbolic variables. +/// @param[in] e Symbolic expression to codegen. +/// +/// For example, `Codegen("f", {x, y}, 1 + sin(x) + cos(y))` generates the +/// following string. +/// +/// @code +/// double f(const double* p) { +/// return (1 + sin(p[0]) + cos(p[1])); +/// } +/// typedef struct { +/// /* p: input, vector */ +/// struct { int size; } p; +/// } f_meta_t; +/// f_meta_t f_meta() { return {{2}}; } +/// @endcode +/// +/// Note that in this example `x` and `y` are mapped to `p[0]` and `p[1]` +/// respectively because we passed `{x, y}` to `Codegen`. +std::string CodeGen(const std::string& function_name, + const std::vector& parameters, + const Expression& e); + +namespace internal { +// Generates code for the internal representation of a matrix, @p data, using +// @p function_name, @p parameters, and @p size. It outputs the generated code +// to the output stream @p os. +// +// @note This function is used to implement std::string +// drake::symbolic::CodeGen(const std::string &, const std::vector&, +// const Eigen::PlainObjectBase&). +void CodeGenDenseData(const std::string& function_name, + const std::vector& parameters, + const Expression* data, int size, std::ostream* os); + +// Generates code for the meta information and outputs to the output stream @p +// os. +// +// @note This function is used to implement std::string +// drake::symbolic::CodeGen(const std::string &, const std::vector&, +// const Eigen::PlainObjectBase&). +void CodeGenDenseMeta(const std::string& function_name, int parameter_size, + int rows, int cols, std::ostream* os); +} // namespace internal + +/// For a given symbolic dense matrix @p M, generates two C functions, +/// `` and `_meta`. The generated +/// `` takes two parameters: +/// +/// - const double* p : An array of doubles for input parameters. +/// - double* m : An array of doubles to store the evaluation result. +/// +/// `_meta()` returns a nested struct from which a caller can +/// obtain the following information: +/// - `.p.size`: the size of input parameters. +/// - `.m.rows`: the number of rows in the matrix. +/// - `.m.cols`: the number of columns in the matrix. +/// +/// Please consider the following example: +/// +/// @code +/// Eigen::Matrix M; +/// M(0, 0) = 1.0; +/// M(1, 0) = 3 + x + y; +/// M(0, 1) = 4 * y; +/// M(1, 1) = sin(x); +/// CodeGen("f", {x, y}, M); +/// @endcode +/// +/// When executed, the last line of the above example generates the following +/// code: +/// +/// @code +/// void f(const double* p, double* m) { +/// m[0] = 1.000000; +/// m[1] = (3 + p[0] + p[1]); +/// m[2] = (4 * p[1]); +/// m[3] = sin(p[0]); +/// } +/// typedef struct { +/// /* p: input, vector */ +/// struct { +/// int size; +/// } p; +/// /* m: output, matrix */ +/// struct { +/// int rows; +/// int cols; +/// } m; +/// } f_meta_t; +/// f_meta_t f_meta() { return {{2}, {2, 2}}; } +/// @endcode +/// +/// Note that in this example, the matrix `M` is stored in column-major order +/// and the `CodeGen` function respects the storage order in the generated code. +/// If `M` were stored in row-major order, `CodeGen` would return the following: +/// +/// @code +/// void f(const double* p, double* m) { +/// m[0] = 1.000000; +/// m[1] = (4 * p[1]); +/// m[2] = (3 + p[0] + p[1]); +/// m[3] = sin(p[0]); +/// } +/// @endcode +template +std::string CodeGen(const std::string& function_name, + const std::vector& parameters, + const Eigen::PlainObjectBase& M) { + static_assert(std::is_same_v, + "CodeGen should take a symbolic matrix."); + std::ostringstream oss; + internal::CodeGenDenseData(function_name, parameters, M.data(), + M.cols() * M.rows(), &oss); + internal::CodeGenDenseMeta(function_name, parameters.size(), M.rows(), + M.cols(), &oss); + return oss.str(); +} + +/// For a given symbolic column-major sparse matrix @p M, generates two C +/// functions, `` and `_meta`. The generated +/// `` is used to construct a sparse matrix of double which +/// stores the evaluation result of the symbolic matrix @p M for a given +/// double-precision floating-point assignment for the symbolic variables in @p +/// M. `` takes one input parameter `p` and three output +/// parameters (`outer_indicies`, `inner_indices`, and `values`). +/// +/// - const double* p : An array of doubles for input parameters. +/// - int* outer_indices : An array of integer to store the starting positions +/// of the inner vectors. +/// - int* inner_indices : An array of integer to store the array of inner +/// indices. +/// - double* values : An array of doubles to store the evaluated non-zero +/// elements. +/// +/// The three outputs, (`outer_indices`, `inner_indices`, `values`), represent a +/// sparse matrix in the widely-used Compressed Column Storage (CCS) scheme. For +/// more information about the CCS scheme, please read +/// https://eigen.tuxfamily.org/dox/group__TutorialSparse.html. +/// +/// `_meta()` returns a nested struct from which a caller can +/// obtain the following information: +/// - `.p.size`: the size of input parameters. +/// - `.m.rows`: the number of rows in the matrix. +/// - `.m.cols`: the number of columns in the matrix. +/// - `.m.non_zeros`: the number of non-zero elements in the matrix. +/// - `.m.outer_indices`: the length of the outer_indices. +/// - `.m.inner_indices`: the length of the inner_indices. +/// +/// @throws std::exception if @p M is not compressed. +// TODO(soonho-tri): Support row-major sparse matrices. +/// +/// Please consider the following example which generates code for a 3x6 +/// sparse matrix. +/// +/// @code +/// Eigen::SparseMatrix m(3, 6); +/// m.insert(0, 0) = x; +/// m.insert(0, 4) = z; +/// m.insert(1, 2) = y; +/// m.insert(2, 3) = y; +/// m.insert(2, 5) = y; +/// m.makeCompressed(); +/// // | x 0 0 0 z 0| +/// // | 0 0 y 0 0 0| +/// // | 0 0 0 y 0 y| +/// CodeGen("f", {x, y, z}, m); +/// @endcode +/// +/// When executed, the last line of the above example generates the following +/// code: +/// +/// @code +/// void f(const double* p, +/// int* outer_indices, +/// int* inner_indices, +/// double* values) { +/// outer_indices[0] = 0; +/// outer_indices[1] = 1; +/// outer_indices[2] = 1; +/// outer_indices[3] = 2; +/// outer_indices[4] = 3; +/// outer_indices[5] = 4; +/// outer_indices[6] = 5; +/// +/// inner_indices[0] = 0; +/// inner_indices[1] = 1; +/// inner_indices[2] = 2; +/// inner_indices[3] = 0; +/// inner_indices[4] = 2; +/// +/// values[0] = p[0]; +/// values[1] = p[1]; +/// values[2] = p[1]; +/// values[3] = p[2]; +/// values[4] = p[1]; +/// } +/// +/// typedef struct { +/// /* p: input, vector */ +/// struct { int size; } p; +/// /* m: output, matrix */ +/// struct { +/// int rows; +/// int cols; +/// int non_zeros; +/// } m; +/// } f_meta_t; +/// f_meta_t f_meta() { return {{3}, {3, 6, 5}}; } +/// @endcode +/// +/// In the following example, we show how to use the generated function to +/// evaluate the symbolic matrix and construct a sparse matrix of double using +/// `Eigen::Map`. +/// +/// @code +/// // set up param, outer_indices, inner_indices, and values. +/// f_meta_t meta = f_meta(); +/// const Eigen::Vector3d param{1 /* x */, 2 /* y */, 3 /* z */}; +/// std::vector outer_indices(meta.m.cols + 1); +/// std::vector inner_indices(meta.m.non_zeros); +/// std::vector values(meta.m.non_zeros); +/// +/// // call f to fill outer_indices, inner_indices, and values. +/// f(param.data(), outer_indices.data(), inner_indices.data(), values.data()); +/// +/// // use Eigen::Map to turn (outer_indices, inner_indices, values) into a +/// // sparse matrix. +/// Eigen::Map> map_sp( +/// meta.m.rows, meta.m.cols, meta.m.non_zeros, outer_indices.data(), +/// inner_indices.data(), values.data()); +/// const Eigen::SparseMatrix m_double{map_sp.eval()}; +/// @endcode +std::string CodeGen( + const std::string& function_name, const std::vector& parameters, + const Eigen::Ref>& + M); +/// @} End of codegen group. + +} // namespace symbolic +} // namespace drake diff --git a/maliput_drake/include/drake/common/symbolic_decompose.h b/maliput_drake/include/drake/common/symbolic_decompose.h new file mode 100644 index 0000000..50004cb --- /dev/null +++ b/maliput_drake/include/drake/common/symbolic_decompose.h @@ -0,0 +1,175 @@ +#pragma once + +#include +#include +#include +#include + +#include "drake/common/eigen_types.h" +#include "drake/common/symbolic.h" + +// TODO(soonho-tri): Migrate the functions in drake/solvers:symbolic_extract to +// this file. + +namespace drake { +namespace symbolic { + +/** Decomposes @p expressions into @p M * @p vars. + +@throws std::exception if @p expressions is not linear in @p vars. +@pre M.rows() == expressions.rows() && M.cols() == vars.rows(). */ +void DecomposeLinearExpressions( + const Eigen::Ref>& expressions, + const Eigen::Ref>& vars, + EigenPtr M); + +/** Decomposes @p expressions into @p M * @p vars + @p v. + +@throws std::exception if @p expressions is not affine in @p vars. +@pre M.rows() == expressions.rows() && M.cols() == vars.rows(). +@pre v.rows() == expressions.rows(). */ +void DecomposeAffineExpressions( + const Eigen::Ref>& expressions, + const Eigen::Ref>& vars, + EigenPtr M, EigenPtr v); + +/** Given an expression `e`, extract all variables inside `e`, append these +variables to `vars` if they are not included in `vars` yet. + +@param[in] e A symbolic expression. +@param[in,out] vars As an input, `vars` contain the variables before +extracting expression `e`. As an output, the variables in `e` that were not +included in `vars`, will be appended to the end of `vars`. +@param[in,out] map_var_to_index. map_var_to_index is of the same size as +`vars`, and map_var_to_index[vars(i).get_id()] = i. This invariance holds for +map_var_to_index both as the input and as the output. */ +void ExtractAndAppendVariablesFromExpression( + const symbolic::Expression& e, VectorX* vars, + std::unordered_map* map_var_to_index); + +/** Given an expression `e`, extracts all variables inside `e`. + +@param[in] e A symbolic expression. @retval pair pair.first is the variables in +`e`. pair.second is the mapping from the variable ID to the index in +pair.first, such that pair.second[pair.first(i).get_id()] = i */ +std::pair, std::unordered_map> +ExtractVariablesFromExpression(const symbolic::Expression& e); + +/** Given a quadratic polynomial @p poly, decomposes it into the form 0.5 * x' +* Q * x + b' * x + c + +@param[in] poly Quadratic polynomial to decompose. +@param[in] map_var_to_index maps variables in `poly.GetVariables()` to the +index in the vector `x`. +@param Q[out] The Hessian of the quadratic expression. @pre The size of Q +should be `num_variables x num_variables`. Q is a symmetric matrix. +@param b[out] The linear term of the quadratic expression. @pre The size of `b` +should be `num_variables`. +@param c[out] The constant term of the quadratic expression. */ +void DecomposeQuadraticPolynomial( + const symbolic::Polynomial& poly, + const std::unordered_map& map_var_to_index, + Eigen::MatrixXd* Q, Eigen::VectorXd* b, double* c); + +/** Given a vector of affine expressions v, decompose it to \f$ v = A vars + b +\f$ + +@param[in] v A vector of affine expressions +@param[out] A The matrix containing the linear coefficients. +@param[out] b The vector containing all the constant terms. +@param[out] vars All variables. */ +void DecomposeAffineExpressions( + const Eigen::Ref>& v, + Eigen::MatrixXd* A, Eigen::VectorXd* b, VectorX* vars); + +/** Decomposes an affine combination @p e = c0 + c1 * v1 + ... cn * vn into the +following: + + constant term : c0 + coefficient vector : [c1, ..., cn] + variable vector : [v1, ..., vn] + +Then, it extracts the coefficient and the constant term. A map from variable ID +to int, @p map_var_to_index, is used to decide a variable's index in a linear +combination. + +\pre +1. @c coeffs is a row vector of double, whose length matches with the size of + @c map_var_to_index. +2. e.is_polynomial() is true. +3. e is an affine expression. + +@tparam Derived An Eigen row vector type with Derived::Scalar == double. +@param[in] e The symbolic affine expression +@param[in] map_var_to_index A mapping from variable ID to variable index, such +that map_var_to_index[vi.get_ID()] = i. +@param[out] coeffs A row vector. coeffs(i) = ci. +@param[out] constant_term c0 in the equation above. +@return num_variable. Number of variables in the expression. 2 * x(0) + 3 has 1 +variable, 2 * x(0) + 3 * x(1) - 2 * x(0) has 1 variable. */ +template +typename std::enable_if_t, int> +DecomposeAffineExpression( + const symbolic::Expression& e, + const std::unordered_map& map_var_to_index, + const Eigen::MatrixBase& coeffs, double* constant_term) { + DRAKE_DEMAND(coeffs.rows() == 1); + DRAKE_DEMAND(coeffs.cols() == static_cast(map_var_to_index.size())); + if (!e.is_polynomial()) { + std::ostringstream oss; + oss << "Expression " << e << "is not a polynomial.\n"; + throw std::runtime_error(oss.str()); + } + const symbolic::Polynomial poly{e}; + int num_variable = 0; + for (const auto& p : poly.monomial_to_coefficient_map()) { + const auto& p_monomial = p.first; + DRAKE_ASSERT(is_constant(p.second)); + const double p_coeff = symbolic::get_constant_value(p.second); + if (p_monomial.total_degree() > 1) { + std::stringstream oss; + oss << "Expression " << e << " is non-linear."; + throw std::runtime_error(oss.str()); + } else if (p_monomial.total_degree() == 1) { + // Linear coefficient. + const auto& p_monomial_powers = p_monomial.get_powers(); + DRAKE_DEMAND(p_monomial_powers.size() == 1); + const symbolic::Variable::Id var_id = + p_monomial_powers.begin()->first.get_id(); + // TODO(eric.cousineau): Avoid using const_cast. + const_cast&>(coeffs)( + map_var_to_index.at(var_id)) = p_coeff; + if (p_coeff != 0) { + ++num_variable; + } + } else { + // Constant term. + *constant_term = p_coeff; + } + } + return num_variable; +} + +/** Given a vector of Expressions @p f and a list of @p parameters we define +all additional variables in @p f to be a vector of "non-parameter variables", +n. This method returns a factorization of @p f into an equivalent "data +matrix", W, which depends only on the non-parameter variables, and a "lumped +parameter vector", α, which depends only on @p parameters: f = +W(n)*α(parameters) + w0(n). + +@note The current implementation makes some simple attempts to minimize the +number of lumped parameters, but more simplification could be implemented +relatively easily. Optimal simplification, however, involves the complexity of +comparing two arbitrary Expressions (see Expression::EqualTo for more details). + +@throw std::exception if @p f is not decomposable in this way (cells containing +@p parameters may only be added or multiplied with cells containing +non-parameter variables). + +@returns W(n), α(parameters), and w0(n). */ +std::tuple, VectorX, VectorX> +DecomposeLumpedParameters( + const Eigen::Ref>& f, + const Eigen::Ref>& parameters); +} // namespace symbolic +} // namespace drake diff --git a/maliput_drake/include/drake/common/symbolic_environment.h b/maliput_drake/include/drake/common/symbolic_environment.h new file mode 100644 index 0000000..93a526b --- /dev/null +++ b/maliput_drake/include/drake/common/symbolic_environment.h @@ -0,0 +1,159 @@ +#pragma once + +#ifndef DRAKE_COMMON_SYMBOLIC_HEADER +#error Do not directly include this file. Include "drake/common/symbolic.h". +#endif + +#include +#include +#include +#include + +#include "drake/common/drake_copyable.h" +#include "drake/common/eigen_types.h" +#include "drake/common/random.h" +#include "drake/common/symbolic.h" + +namespace drake { +namespace symbolic { +/** Represents a symbolic environment (mapping from a variable to a value). + * + * This class is used when we evaluate symbolic expressions or formulas which + * include unquantified (free) variables. Here are examples: + * + * \code{.cpp} + * const Variable var_x{"x"}; + * const Variable var_y{"y"}; + * const Expression x{var_x}; + * const Expression y{var_x}; + * const Expression e1{x + y}; + * const Expression e2{x - y}; + * const Formula f{e1 > e2}; + * + * // env maps var_x to 2.0 and var_y to 3.0 + * const Environment env{{var_x, 2.0}, {var_y, 3.0}}; + * + * const double res1 = e1.Evaluate(env); // x + y => 2.0 + 3.0 => 5.0 + * const double res2 = e2.Evaluate(env); // x - y => 2.0 - 3.0 => -1.0 + * const bool res = f.Evaluate(env); // x + y > x - y => 5.0 >= -1.0 => True + * \endcode + * + * Note that it is not allowed to have a dummy variable in an environment. It + * throws std::exception for the attempts to create an environment with a + * dummy variable, to insert a dummy variable to an existing environment, or to + * take a reference to a value mapped to a dummy variable. See the following + * examples. + * + * \code{.cpp} + * Variable var_dummy{}; // OK to have a dummy variable + * Environment e1{var_dummy}; // throws exception + * Environment e2{{var_dummy, 1.0}}; // throws exception + * Environment e{}; + * e.insert(var_dummy, 1.0); // throws exception + * e[var_dummy] = 3.0; // throws exception + * \endcode + * + */ +class Environment { + public: + DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Environment) + + typedef Variable key_type; + typedef double mapped_type; + typedef typename std::unordered_map map; + /** std::pair */ + typedef typename map::value_type value_type; + typedef typename map::iterator iterator; + typedef typename map::const_iterator const_iterator; + + /** Default constructor. */ + Environment() = default; + + /** List constructor. Constructs an environment from a list of (Variable * + * double). + * + * @throws std::exception if @p init include a dummy variable or a NaN + * value. + */ + Environment(std::initializer_list init); + + /** List constructor. Constructs an environment from a list of + * Variable. Initializes the variables with 0.0. + * + * @throws std::exception if @p vars include a dummy variable. + */ + Environment(std::initializer_list vars); + + /** Constructs an environment from @p m (of `map` type, which is + * `std::unordered_map`). + * + * @throws std::exception if @p m include a dummy variable or a NaN value. + */ + explicit Environment(map m); + + /** Returns an iterator to the beginning. */ + iterator begin() { return map_.begin(); } + /** Returns an iterator to the end. */ + iterator end() { return map_.end(); } + /** Returns a const iterator to the beginning. */ + [[nodiscard]] const_iterator begin() const { return map_.cbegin(); } + /** Returns a const iterator to the end. */ + [[nodiscard]] const_iterator end() const { return map_.cend(); } + /** Returns a const iterator to the beginning. */ + [[nodiscard]] const_iterator cbegin() const { return map_.cbegin(); } + /** Returns a const iterator to the end. */ + [[nodiscard]] const_iterator cend() const { return map_.cend(); } + + /** Inserts a pair (@p key, @p elem). */ + void insert(const key_type& key, const mapped_type& elem); + + /** Given a matrix of symbolic variables @p keys and a matrix of values @p + * elements, inserts each pair (keys(i, j), elements(i, j)) into the + * environment. + * + * @throws std::exception if the size of @p keys is different from the size + * of @p elements. + */ + void insert(const Eigen::Ref>& keys, + const Eigen::Ref>& elements); + + /** Checks whether the container is empty. */ + [[nodiscard]] bool empty() const { return map_.empty(); } + /** Returns the number of elements. */ + [[nodiscard]] size_t size() const { return map_.size(); } + + /** Finds element with specific key. */ + iterator find(const key_type& key) { return map_.find(key); } + /** Finds element with specific key. */ + [[nodiscard]] const_iterator find(const key_type& key) const { + return map_.find(key); + } + + /** Returns the domain of this environment. */ + [[nodiscard]] Variables domain() const; + + /** Returns string representation. */ + [[nodiscard]] std::string to_string() const; + + /** Returns a reference to the value that is mapped to a key equivalent to + * @p key, performing an insertion if such key does not already exist. + */ + mapped_type& operator[](const key_type& key); + + /** As above, but returns a constref and does not perform an insertion + * (throwing a runtime error instead) if the key does not exist. */ + const mapped_type& operator[](const key_type& key) const; + + friend std::ostream& operator<<(std::ostream& os, const Environment& env); + + private: + map map_; +}; + +/** Populates the environment @p env by sampling values for the unassigned + * random variables in @p variables using @p random_generator. */ +Environment PopulateRandomVariables(Environment env, const Variables& variables, + RandomGenerator* random_generator); + +} // namespace symbolic +} // namespace drake diff --git a/maliput_drake/include/drake/common/symbolic_expression.h b/maliput_drake/include/drake/common/symbolic_expression.h new file mode 100644 index 0000000..003e1b5 --- /dev/null +++ b/maliput_drake/include/drake/common/symbolic_expression.h @@ -0,0 +1,1518 @@ +#pragma once + +#ifndef DRAKE_COMMON_SYMBOLIC_HEADER +#error Do not directly include this file. Include "drake/common/symbolic.h". +#endif + +#include // for cpplint only +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "drake/common/cond.h" +#include "drake/common/drake_assert.h" +#include "drake/common/drake_copyable.h" +#include "drake/common/drake_throw.h" +#include "drake/common/dummy_value.h" +#include "drake/common/eigen_types.h" +#include "drake/common/extract_double.h" +#include "drake/common/hash.h" +#include "drake/common/random.h" +#include "drake/common/symbolic.h" + +namespace drake { + +namespace symbolic { + +/** Kinds of symbolic expressions. */ +enum class ExpressionKind { + Constant, ///< constant (double) + Var, ///< variable + Add, ///< addition (+) + Mul, ///< multiplication (*) + Div, ///< division (/) + Log, ///< logarithms + Abs, ///< absolute value function + Exp, ///< exponentiation + Sqrt, ///< square root + Pow, ///< power function + Sin, ///< sine + Cos, ///< cosine + Tan, ///< tangent + Asin, ///< arcsine + Acos, ///< arccosine + Atan, ///< arctangent + Atan2, ///< arctangent2 (atan2(y,x) = atan(y/x)) + Sinh, ///< hyperbolic sine + Cosh, ///< hyperbolic cosine + Tanh, ///< hyperbolic tangent + Min, ///< min + Max, ///< max + Ceil, ///< ceil + Floor, ///< floor + IfThenElse, ///< if then else + NaN, ///< NaN + UninterpretedFunction, ///< Uninterpreted function + // TODO(soonho): add Integral +}; + +/** Total ordering between ExpressionKinds. */ +bool operator<(ExpressionKind k1, ExpressionKind k2); + +class ExpressionCell; // In symbolic_expression_cell.h +class ExpressionConstant; // In symbolic_expression_cell.h +class ExpressionVar; // In symbolic_expression_cell.h +class UnaryExpressionCell; // In symbolic_expression_cell.h +class BinaryExpressionCell; // In symbolic_expression_cell.h +class ExpressionAdd; // In symbolic_expression_cell.h +class ExpressionMul; // In symbolic_expression_cell.h +class ExpressionDiv; // In symbolic_expression_cell.h +class ExpressionLog; // In symbolic_expression_cell.h +class ExpressionAbs; // In symbolic_expression_cell.h +class ExpressionExp; // In symbolic_expression_cell.h +class ExpressionSqrt; // In symbolic_expression_cell.h +class ExpressionPow; // In symbolic_expression_cell.h +class ExpressionSin; // In symbolic_expression_cell.h +class ExpressionCos; // In symbolic_expression_cell.h +class ExpressionTan; // In symbolic_expression_cell.h +class ExpressionAsin; // In symbolic_expression_cell.h +class ExpressionAcos; // In symbolic_expression_cell.h +class ExpressionAtan; // In symbolic_expression_cell.h +class ExpressionAtan2; // In symbolic_expression_cell.h +class ExpressionSinh; // In symbolic_expression_cell.h +class ExpressionCosh; // In symbolic_expression_cell.h +class ExpressionTanh; // In symbolic_expression_cell.h +class ExpressionMin; // In symbolic_expression_cell.h +class ExpressionMax; // In symbolic_expression_cell.h +class ExpressionCeiling; // In symbolic_expression_cell.h +class ExpressionFloor; // In symbolic_expression_cell.h +class ExpressionIfThenElse; // In symbolic_expression_cell.h +class ExpressionUninterpretedFunction; // In symbolic_expression_cell.h +class Formula; // In symbolic_formula.h +class Expression; + +// Substitution is a map from a Variable to a symbolic expression. It is used in +// Expression::Substitute and Formula::Substitute methods as an argument. +using Substitution = std::unordered_map; + +/** Represents a symbolic form of an expression. + +Its syntax tree is as follows: + +@verbatim + E := Var | Constant | E + ... + E | E * ... * E | E / E | log(E) + | abs(E) | exp(E) | sqrt(E) | pow(E, E) | sin(E) | cos(E) | tan(E) + | asin(E) | acos(E) | atan(E) | atan2(E, E) | sinh(E) | cosh(E) | tanh(E) + | min(E, E) | max(E, E) | ceil(E) | floor(E) | if_then_else(F, E, E) + | NaN | uninterpreted_function(name, {v_1, ..., v_n}) +@endverbatim + +In the implementation, Expression is a simple wrapper including a shared pointer +to ExpressionCell class which is a super-class of different kinds of symbolic +expressions (i.e. ExpressionAdd, ExpressionMul, ExpressionLog, +ExpressionSin). Note that it includes a shared pointer, not a unique pointer, to +allow sharing sub-expressions. + +@note The sharing of sub-expressions is not yet implemented. + +@note -E is represented as -1 * E internally. + +@note A subtraction E1 - E2 is represented as E1 + (-1 * E2) internally. + +The following simple simplifications are implemented: +@verbatim + E + 0 -> E + 0 + E -> E + E - 0 -> E + E - E -> 0 + E * 1 -> E + 1 * E -> E + E * 0 -> 0 + 0 * E -> 0 + E / 1 -> E + E / E -> 1 + pow(E, 0) -> 1 + pow(E, 1) -> E + E * E -> E^2 (= pow(E, 2)) + sqrt(E * E) -> |E| (= abs(E)) + sqrt(E) * sqrt(E) -> E +@endverbatim + +Constant folding is implemented: +@verbatim + E(c1) + E(c2) -> E(c1 + c2) // c1, c2 are constants + E(c1) - E(c2) -> E(c1 - c2) + E(c1) * E(c2) -> E(c1 * c2) + E(c1) / E(c2) -> E(c1 / c2) + f(E(c)) -> E(f(c)) // c is a constant, f is a math function +@endverbatim + +For the math functions which are only defined over a restricted domain (namely, +log, sqrt, pow, asin, acos), we check the domain of argument(s), and throw +std::domain_error exception if a function is not well-defined for a given +argument(s). + +Relational operators over expressions (==, !=, <, >, <=, >=) return +symbolic::Formula instead of bool. Those operations are declared in +symbolic_formula.h file. To check structural equality between two expressions a +separate function, Expression::EqualTo, is provided. + +Regarding NaN, we have the following rules: + 1. NaN values are extremely rare during typical computations. Because they are + difficult to handle symbolically, we will round that up to "must never + occur". We allow the user to form ExpressionNaN cells in a symbolic + tree. For example, the user can initialize an Expression to NaN and then + overwrite it later. However, evaluating a tree that has NaN in its evaluated + sub-trees is an error (see rule (3) below). + 2. It's still valid for code to check `isnan` in order to fail-fast. So we + provide isnan(const Expression&) for the common case of non-NaN value + returning False. This way, code can fail-fast with double yet still compile + with Expression. + 3. If there are expressions that embed separate cases (`if_then_else`), some of + the sub-expressions may be not used in evaluation when they are in the + not-taken case (for NaN reasons or any other reason). Bad values within + those not-taken branches does not cause exceptions. + 4. The isnan check is different than if_then_else. In the latter, the + ExpressionNaN is within a dead sub-expression branch. In the former, it + appears in an evaluated trunk. That goes against rule (1) where a NaN + anywhere in a computation (other than dead code) is an error. + +symbolic::Expression can be used as a scalar type of Eigen types. +*/ +class Expression { + public: + DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Expression) + ~Expression() = default; + + /** Default constructor. It constructs Zero(). */ + Expression() { *this = Zero(); } + + /** Constructs a constant. */ + // NOLINTNEXTLINE(runtime/explicit): This conversion is desirable. + Expression(double d); + /** Constructs an expression from @p var. + * @pre @p var is neither a dummy nor a BOOLEAN variable. + */ + // NOLINTNEXTLINE(runtime/explicit): This conversion is desirable. + Expression(const Variable& var); + /** Returns expression kind. */ + [[nodiscard]] ExpressionKind get_kind() const; + /** Collects variables in expression. */ + [[nodiscard]] Variables GetVariables() const; + + /** Checks structural equality. + * + * Two expressions e1 and e2 are structurally equal when they have the same + * internal AST(abstract-syntax tree) representation. Please note that we can + * have two computationally (or extensionally) equivalent expressions which + * are not structurally equal. For example, consider: + * + * e1 = 2 * (x + y) + * e2 = 2x + 2y + * + * Obviously, we know that e1 and e2 are evaluated to the same value for all + * assignments to x and y. However, e1 and e2 are not structurally equal by + * the definition. Note that e1 is a multiplication expression + * (is_multiplication(e1) is true) while e2 is an addition expression + * (is_addition(e2) is true). + * + * One main reason we use structural equality in EqualTo is due to + * Richardson's Theorem. It states that checking ∀x. E(x) = F(x) is + * undecidable when we allow sin, asin, log, exp in E and F. Read + * https://en.wikipedia.org/wiki/Richardson%27s_theorem for details. + * + * Note that for polynomial cases, you can use Expand method and check if two + * polynomial expressions p1 and p2 are computationally equal. To do so, you + * check the following: + * + * (p1.Expand() - p2.Expand()).EqualTo(0). + */ + [[nodiscard]] bool EqualTo(const Expression& e) const; + + /** Provides lexicographical ordering between expressions. + This function is used as a compare function in map and + set via std::less. */ + [[nodiscard]] bool Less(const Expression& e) const; + + /** Checks if this symbolic expression is convertible to Polynomial. */ + [[nodiscard]] bool is_polynomial() const; + + /** Evaluates using a given environment (by default, an empty environment) and + * a random number generator. If there is a random variable in this expression + * which is unassigned in @p env, this method uses @p random_generator to + * sample a value and use the value to substitute all occurrences of the + * variable in this expression. + * + * @throws std::exception if there exists a non-random variable in this + * expression whose assignment is not provided by + * @p env. + * @throws std::exception if an unassigned random variable is detected + * while @p random_generator is `nullptr`. + * @throws std::exception if NaN is detected during evaluation. + */ + double Evaluate(const Environment& env = Environment{}, + RandomGenerator* random_generator = nullptr) const; + + /** Evaluates using an empty environment and a random number generator. It + * uses @p random_generator to sample values for the random variables in this + * expression. + * + * See the above overload for the exceptions that it might throw. + */ + double Evaluate(RandomGenerator* random_generator) const; + + /** Partially evaluates this expression using an environment @p + * env. Internally, this method promotes @p env into a substitution + * (Variable → Expression) and call Evaluate::Substitute with it. + * + * @throws std::exception if NaN is detected during evaluation. + */ + [[nodiscard]] Expression EvaluatePartial(const Environment& env) const; + + /** Returns true if this symbolic expression is already + * expanded. Expression::Expand() uses this flag to avoid calling + * ExpressionCell::Expand() on an pre-expanded expressions. + * Expression::Expand() also sets this flag before returning the result. + * + * @note This check is conservative in that `false` does not always indicate + * that the expression is not expanded. This is because exact checks can be + * costly and we want to avoid the exact check at the construction time. + */ + [[nodiscard]] bool is_expanded() const; + + /** Expands out products and positive integer powers in expression. For + * example, `(x + 1) * (x - 1)` is expanded to `x^2 - 1` and `(x + y)^2` is + * expanded to `x^2 + 2xy + y^2`. Note that Expand applies recursively to + * sub-expressions. For instance, `sin(2 * (x + y))` is expanded to `sin(2x + + * 2y)`. It also simplifies "division by constant" cases. See + * "drake/common/test/symbolic_expansion_test.cc" to find the examples. + * + * @throws std::exception if NaN is detected during expansion. + */ + [[nodiscard]] Expression Expand() const; + + /** Returns a copy of this expression replacing all occurrences of @p var + * with @p e. + * @throws std::exception if NaN is detected during substitution. + */ + [[nodiscard]] Expression Substitute(const Variable& var, + const Expression& e) const; + + /** Returns a copy of this expression replacing all occurrences of the + * variables in @p s with corresponding expressions in @p s. Note that the + * substitutions occur simultaneously. For example, (x / y).Substitute({{x, + * y}, {y, x}}) gets (y / x). + * @throws std::exception if NaN is detected during substitution. + */ + [[nodiscard]] Expression Substitute(const Substitution& s) const; + + /** Differentiates this symbolic expression with respect to the variable @p + * var. + * @throws std::exception if it is not differentiable. + */ + [[nodiscard]] Expression Differentiate(const Variable& x) const; + + /** Let `f` be this Expression, computes a row vector of derivatives, + * `[∂f/∂vars(0), ... , ∂f/∂vars(n-1)]` with respect to the variables + * @p vars. + */ + [[nodiscard]] RowVectorX Jacobian( + const Eigen::Ref>& vars) const; + + /** Returns string representation of Expression. */ + [[nodiscard]] std::string to_string() const; + + /** Returns zero. */ + static Expression Zero(); + /** Returns one. */ + static Expression One(); + /** Returns Pi, the ratio of a circle’s circumference to its diameter. */ + static Expression Pi(); + /** Return e, the base of natural logarithms. */ + static Expression E(); + /** Returns NaN (Not-a-Number). */ + static Expression NaN(); + + /** Implements the @ref hash_append concept. */ + template + friend void hash_append(HashAlgorithm& hasher, + const Expression& item) noexcept { + DelegatingHasher delegating_hasher( + [&hasher](const void* data, const size_t length) { + return hasher(data, length); + }); + item.HashAppend(&delegating_hasher); + } + + friend Expression operator+(Expression lhs, const Expression& rhs); + // NOLINTNEXTLINE(runtime/references) per C++ standard signature. + friend Expression& operator+=(Expression& lhs, const Expression& rhs); + + /** Provides prefix increment operator (i.e. ++x). */ + Expression& operator++(); + /** Provides postfix increment operator (i.e. x++). */ + Expression operator++(int); + /** Provides unary plus operator. */ + friend Expression operator+(const Expression& e); + + friend Expression operator-(Expression lhs, const Expression& rhs); + // NOLINTNEXTLINE(runtime/references) per C++ standard signature. + friend Expression& operator-=(Expression& lhs, const Expression& rhs); + + /** Provides unary minus operator. */ + friend Expression operator-(const Expression& e); + /** Provides prefix decrement operator (i.e. --x). */ + Expression& operator--(); + /** Provides postfix decrement operator (i.e. x--). */ + Expression operator--(int); + + friend Expression operator*(Expression lhs, const Expression& rhs); + // NOLINTNEXTLINE(runtime/references) per C++ standard signature. + friend Expression& operator*=(Expression& lhs, const Expression& rhs); + + friend Expression operator/(Expression lhs, const Expression& rhs); + // NOLINTNEXTLINE(runtime/references) per C++ standard signature. + friend Expression& operator/=(Expression& lhs, const Expression& rhs); + + friend Expression log(const Expression& e); + friend Expression abs(const Expression& e); + friend Expression exp(const Expression& e); + friend Expression sqrt(const Expression& e); + friend Expression pow(const Expression& e1, const Expression& e2); + friend Expression sin(const Expression& e); + friend Expression cos(const Expression& e); + friend Expression tan(const Expression& e); + friend Expression asin(const Expression& e); + friend Expression acos(const Expression& e); + friend Expression atan(const Expression& e); + friend Expression atan2(const Expression& e1, const Expression& e2); + friend Expression sinh(const Expression& e); + friend Expression cosh(const Expression& e); + friend Expression tanh(const Expression& e); + friend Expression min(const Expression& e1, const Expression& e2); + friend Expression max(const Expression& e1, const Expression& e2); + friend Expression ceil(const Expression& e); + friend Expression floor(const Expression& e); + + /** Constructs if-then-else expression. + + @verbatim + if_then_else(cond, expr_then, expr_else) + @endverbatim + + The value returned by the above if-then-else expression is @p expr_then if + @p cond is evaluated to true. Otherwise, it returns @p expr_else. + + The semantics is similar to the C++'s conditional expression constructed by + its ternary operator, @c ?:. However, there is a key difference between the + C++'s conditional expression and our @c if_then_else expression in a way the + arguments are evaluated during the construction. + + - In case of the C++'s conditional expression, cond ? expr_then : + expr_else, the then expression @c expr_then (respectively, the else + expression @c expr_else) is \b only evaluated when the conditional + expression @c cond is evaluated to \b true (respectively, when @c cond is + evaluated to \b false). + + - In case of the symbolic expression, if_then_else(cond, expr_then, + expr_else), however, \b both arguments @c expr_then and @c expr_else + are evaluated first and then passed to the @c if_then_else function. + + @note This function returns an \b expression and it is different from the + C++'s if-then-else \b statement. + + @note While it is still possible to define min, max, abs math + functions using @c if_then_else expression, it is highly \b recommended to + use the provided native definitions for them because it allows solvers to + detect specific math functions and to have a room for special + optimizations. + + @note More information about the C++'s conditional expression and ternary + operator is available at + http://en.cppreference.com/w/cpp/language/operator_other#Conditional_operator. + */ + friend Expression if_then_else(const Formula& f_cond, + const Expression& e_then, + const Expression& e_else); + friend Expression uninterpreted_function(std::string name, + std::vector arguments); + + friend std::ostream& operator<<(std::ostream& os, const Expression& e); + friend void swap(Expression& a, Expression& b) { std::swap(a.ptr_, b.ptr_); } + + friend bool is_constant(const Expression& e); + friend bool is_variable(const Expression& e); + friend bool is_addition(const Expression& e); + friend bool is_multiplication(const Expression& e); + friend bool is_division(const Expression& e); + friend bool is_log(const Expression& e); + friend bool is_abs(const Expression& e); + friend bool is_exp(const Expression& e); + friend bool is_sqrt(const Expression& e); + friend bool is_pow(const Expression& e); + friend bool is_sin(const Expression& e); + friend bool is_cos(const Expression& e); + friend bool is_tan(const Expression& e); + friend bool is_asin(const Expression& e); + friend bool is_acos(const Expression& e); + friend bool is_atan(const Expression& e); + friend bool is_atan2(const Expression& e); + friend bool is_sinh(const Expression& e); + friend bool is_cosh(const Expression& e); + friend bool is_tanh(const Expression& e); + friend bool is_min(const Expression& e); + friend bool is_max(const Expression& e); + friend bool is_ceil(const Expression& e); + friend bool is_floor(const Expression& e); + friend bool is_if_then_else(const Expression& e); + friend bool is_uninterpreted_function(const Expression& e); + + // Note that the following cast functions are only for low-level operations + // and not exposed to the user of drake/common/symbolic_expression.h + // header. These functions are declared in + // drake/common/symbolic_expression_cell.h header. + friend const ExpressionConstant& to_constant(const Expression& e); + friend const ExpressionVar& to_variable(const Expression& e); + friend const UnaryExpressionCell& to_unary(const Expression& e); + friend const BinaryExpressionCell& to_binary(const Expression& e); + friend const ExpressionAdd& to_addition(const Expression& e); + friend const ExpressionMul& to_multiplication(const Expression& e); + friend const ExpressionDiv& to_division(const Expression& e); + friend const ExpressionLog& to_log(const Expression& e); + friend const ExpressionAbs& to_abs(const Expression& e); + friend const ExpressionExp& to_exp(const Expression& e); + friend const ExpressionSqrt& to_sqrt(const Expression& e); + friend const ExpressionPow& to_pow(const Expression& e); + friend const ExpressionSin& to_sin(const Expression& e); + friend const ExpressionCos& to_cos(const Expression& e); + friend const ExpressionTan& to_tan(const Expression& e); + friend const ExpressionAsin& to_asin(const Expression& e); + friend const ExpressionAcos& to_acos(const Expression& e); + friend const ExpressionAtan& to_atan(const Expression& e); + friend const ExpressionAtan2& to_atan2(const Expression& e); + friend const ExpressionSinh& to_sinh(const Expression& e); + friend const ExpressionCosh& to_cosh(const Expression& e); + friend const ExpressionTanh& to_tanh(const Expression& e); + friend const ExpressionMin& to_min(const Expression& e); + friend const ExpressionMax& to_max(const Expression& e); + friend const ExpressionCeiling& to_ceil(const Expression& e); + friend const ExpressionFloor& to_floor(const Expression& e); + friend const ExpressionIfThenElse& to_if_then_else(const Expression& e); + friend const ExpressionUninterpretedFunction& + to_uninterpreted_function(const Expression& e); + + // Cast functions which takes a pointer to a non-const Expression. + friend ExpressionConstant& to_constant(Expression* e); + friend ExpressionVar& to_variable(Expression* e); + friend UnaryExpressionCell& to_unary(Expression* e); + friend BinaryExpressionCell& to_binary(Expression* e); + friend ExpressionAdd& to_addition(Expression* e); + friend ExpressionMul& to_multiplication(Expression* e); + friend ExpressionDiv& to_division(Expression* e); + friend ExpressionLog& to_log(Expression* e); + friend ExpressionAbs& to_abs(Expression* e); + friend ExpressionExp& to_exp(Expression* e); + friend ExpressionSqrt& to_sqrt(Expression* e); + friend ExpressionPow& to_pow(Expression* e); + friend ExpressionSin& to_sin(Expression* e); + friend ExpressionCos& to_cos(Expression* e); + friend ExpressionTan& to_tan(Expression* e); + friend ExpressionAsin& to_asin(Expression* e); + friend ExpressionAcos& to_acos(Expression* e); + friend ExpressionAtan& to_atan(Expression* e); + friend ExpressionAtan2& to_atan2(Expression* e); + friend ExpressionSinh& to_sinh(Expression* e); + friend ExpressionCosh& to_cosh(Expression* e); + friend ExpressionTanh& to_tanh(Expression* e); + friend ExpressionMin& to_min(Expression* e); + friend ExpressionMax& to_max(Expression* e); + friend ExpressionCeiling& to_ceil(Expression* e); + friend ExpressionFloor& to_floor(Expression* e); + friend ExpressionIfThenElse& to_if_then_else(Expression* e); + friend ExpressionUninterpretedFunction& + to_uninterpreted_function(Expression* e); + + friend class ExpressionAddFactory; + friend class ExpressionMulFactory; + + private: + // This is a helper function used to handle `Expression(double)` constructor. + static std::shared_ptr make_cell(double d); + + explicit Expression(std::shared_ptr ptr); + + void HashAppend(DelegatingHasher* hasher) const; + + // Returns a const reference to the owned cell. + const ExpressionCell& cell() const { + DRAKE_ASSERT(ptr_ != nullptr); + return *ptr_; + } + + // Returns a mutable reference to the owned cell. This function may only be + // called when this object is the sole owner of the cell. + ExpressionCell& mutable_cell(); + + // Note: We use "non-const" ExpressionCell type. This allows us to perform + // destructive updates on the pointed cell if the cell is not shared with + // other Expressions (that is, ptr_.use_count() == 1). However, because that + // pattern needs careful attention, our library code should never access + // ptr_ directly, it should always use cell() or mutable_cell(). + std::shared_ptr ptr_; +}; + +Expression operator+(Expression lhs, const Expression& rhs); +// NOLINTNEXTLINE(runtime/references) per C++ standard signature. +Expression& operator+=(Expression& lhs, const Expression& rhs); +Expression operator+(const Expression& e); +Expression operator-(Expression lhs, const Expression& rhs); +// NOLINTNEXTLINE(runtime/references) per C++ standard signature. +Expression& operator-=(Expression& lhs, const Expression& rhs); +Expression operator-(const Expression& e); +Expression operator*(Expression lhs, const Expression& rhs); +// NOLINTNEXTLINE(runtime/references) per C++ standard signature. +Expression& operator*=(Expression& lhs, const Expression& rhs); +Expression operator/(Expression lhs, const Expression& rhs); +// NOLINTNEXTLINE(runtime/references) per C++ standard signature. +Expression& operator/=(Expression& lhs, const Expression& rhs); + +Expression log(const Expression& e); +Expression abs(const Expression& e); +Expression exp(const Expression& e); +Expression sqrt(const Expression& e); +Expression pow(const Expression& e1, const Expression& e2); +Expression sin(const Expression& e); +Expression cos(const Expression& e); +Expression tan(const Expression& e); +Expression asin(const Expression& e); +Expression acos(const Expression& e); +Expression atan(const Expression& e); +Expression atan2(const Expression& e1, const Expression& e2); +Expression sinh(const Expression& e); +Expression cosh(const Expression& e); +Expression tanh(const Expression& e); +Expression min(const Expression& e1, const Expression& e2); +Expression max(const Expression& e1, const Expression& e2); +Expression ceil(const Expression& e); +Expression floor(const Expression& e); +Expression if_then_else(const Formula& f_cond, const Expression& e_then, + const Expression& e_else); + +/** Constructs an uninterpreted-function expression with @p name and @p + * arguments. An uninterpreted function is an opaque function that has no other + * property than its name and a list of its arguments. This is useful to + * applications where it is good enough to provide abstract information of a + * function without exposing full details. Declaring sparsity of a system is a + * typical example. + */ +Expression uninterpreted_function(std::string name, + std::vector arguments); +void swap(Expression& a, Expression& b); + +std::ostream& operator<<(std::ostream& os, const Expression& e); + +/** Checks if @p e is a constant expression. */ +bool is_constant(const Expression& e); +/** Checks if @p e is a constant expression representing @p v. */ +bool is_constant(const Expression& e, double v); +/** Checks if @p e is 0.0. */ +bool is_zero(const Expression& e); +/** Checks if @p e is 1.0. */ +bool is_one(const Expression& e); +/** Checks if @p e is -1.0. */ +bool is_neg_one(const Expression& e); +/** Checks if @p e is 2.0. */ +bool is_two(const Expression& e); +/** Checks if @p e is NaN. */ +bool is_nan(const Expression& e); +/** Checks if @p e is a variable expression. */ +bool is_variable(const Expression& e); +/** Checks if @p e is an addition expression. */ +bool is_addition(const Expression& e); +/** Checks if @p e is a multiplication expression. */ +bool is_multiplication(const Expression& e); +/** Checks if @p e is a division expression. */ +bool is_division(const Expression& e); +/** Checks if @p e is a log expression. */ +bool is_log(const Expression& e); +/** Checks if @p e is an abs expression. */ +bool is_abs(const Expression& e); +/** Checks if @p e is an exp expression. */ +bool is_exp(const Expression& e); +/** Checks if @p e is a square-root expression. */ +bool is_sqrt(const Expression& e); +/** Checks if @p e is a power-function expression. */ +bool is_pow(const Expression& e); +/** Checks if @p e is a sine expression. */ +bool is_sin(const Expression& e); +/** Checks if @p e is a cosine expression. */ +bool is_cos(const Expression& e); +/** Checks if @p e is a tangent expression. */ +bool is_tan(const Expression& e); +/** Checks if @p e is an arcsine expression. */ +bool is_asin(const Expression& e); +/** Checks if @p e is an arccosine expression. */ +bool is_acos(const Expression& e); +/** Checks if @p e is an arctangent expression. */ +bool is_atan(const Expression& e); +/** Checks if @p e is an arctangent2 expression. */ +bool is_atan2(const Expression& e); +/** Checks if @p e is a hyperbolic-sine expression. */ +bool is_sinh(const Expression& e); +/** Checks if @p e is a hyperbolic-cosine expression. */ +bool is_cosh(const Expression& e); +/** Checks if @p e is a hyperbolic-tangent expression. */ +bool is_tanh(const Expression& e); +/** Checks if @p e is a min expression. */ +bool is_min(const Expression& e); +/** Checks if @p e is a max expression. */ +bool is_max(const Expression& e); +/** Checks if @p e is a ceil expression. */ +bool is_ceil(const Expression& e); +/** Checks if @p e is a floor expression. */ +bool is_floor(const Expression& e); +/** Checks if @p e is an if-then-else expression. */ +bool is_if_then_else(const Expression& e); +/** Checks if @p e is an uninterpreted-function expression. */ +bool is_uninterpreted_function(const Expression& e); + +/** Returns the constant value of the constant expression @p e. + * \pre{@p e is a constant expression.} + */ +double get_constant_value(const Expression& e); +/** Returns the embedded variable in the variable expression @p e. + * \pre{@p e is a variable expression.} + */ +const Variable& get_variable(const Expression& e); +/** Returns the argument in the unary expression @p e. + * \pre{@p e is a unary expression.} + */ +const Expression& get_argument(const Expression& e); +/** Returns the first argument of the binary expression @p e. + * \pre{@p e is a binary expression.} + */ +const Expression& get_first_argument(const Expression& e); +/** Returns the second argument of the binary expression @p e. + * \pre{@p e is a binary expression.} + */ +const Expression& get_second_argument(const Expression& e); +/** Returns the constant part of the addition expression @p e. For instance, + * given 7 + 2 * x + 3 * y, it returns 7. + * \pre{@p e is an addition expression.} + */ +double get_constant_in_addition(const Expression& e); +/** Returns the map from an expression to its coefficient in the addition + * expression @p e. For instance, given 7 + 2 * x + 3 * y, the return value + * maps 'x' to 2 and 'y' to 3. + * \pre{@p e is an addition expression.} + */ +const std::map& get_expr_to_coeff_map_in_addition( + const Expression& e); +/** Returns the constant part of the multiplication expression @p e. For + * instance, given 7 * x^2 * y^3, it returns 7. + * \pre{@p e is a multiplication expression.} + */ +double get_constant_in_multiplication(const Expression& e); +/** Returns the map from a base expression to its exponent expression in the + * multiplication expression @p e. For instance, given 7 * x^2 * y^3 * z^x, the + * return value maps 'x' to 2, 'y' to 3, and 'z' to 'x'. + * \pre{@p e is a multiplication expression.} + */ +const std::map& +get_base_to_exponent_map_in_multiplication(const Expression& e); + +/** Returns the name of an uninterpreted-function expression @p e. + * \pre @p e is an uninterpreted-function expression. + */ +const std::string& get_uninterpreted_function_name(const Expression& e); + +/** Returns the arguments of an uninterpreted-function expression @p e. + * \pre @p e is an uninterpreted-function expression. + */ +const std::vector& get_uninterpreted_function_arguments( + const Expression& e); + +/** Returns the conditional formula in the if-then-else expression @p e. + * @pre @p e is an if-then-else expression. + */ +const Formula& get_conditional_formula(const Expression& e); + +/** Returns the 'then' expression in the if-then-else expression @p e. + * @pre @p e is an if-then-else expression. + */ +const Expression& get_then_expression(const Expression& e); + +/** Returns the 'else' expression in the if-then-else expression @p e. + * @pre @p e is an if-then-else expression. + */ +const Expression& get_else_expression(const Expression& e); + +Expression operator+(const Variable& var); +Expression operator-(const Variable& var); + +/// Returns the Taylor series expansion of `f` around `a` of order `order`. +/// +/// @param[in] f Symbolic expression to approximate using Taylor series +/// expansion. +/// @param[in] a Symbolic environment which specifies the point of +/// approximation. If a partial environment is provided, +/// the unspecified variables are treated as symbolic +/// variables (e.g. decision variable). +/// @param[in] order Positive integer which specifies the maximum order of the +/// resulting polynomial approximating `f` around `a`. +Expression TaylorExpand(const Expression& f, const Environment& a, int order); + +} // namespace symbolic +} // namespace drake + +namespace std { +/* Provides std::hash. */ +template <> +struct hash : public drake::DefaultHash {}; +#if defined(__GLIBCXX__) +// https://gcc.gnu.org/onlinedocs/libstdc++/manual/unordered_associative.html +template <> +struct __is_fast_hash> : std::false_type {}; +#endif + +/* Provides std::less. */ +template <> +struct less { + bool operator()(const drake::symbolic::Expression& lhs, + const drake::symbolic::Expression& rhs) const { + return lhs.Less(rhs); + } +}; + +/* Provides std::equal_to. */ +template <> +struct equal_to { + bool operator()(const drake::symbolic::Expression& lhs, + const drake::symbolic::Expression& rhs) const { + return lhs.EqualTo(rhs); + } +}; + +/* Provides std::numeric_limits. */ +template <> +struct numeric_limits + : public std::numeric_limits {}; + +/// Provides std::uniform_real_distribution, U(a, b), for symbolic expressions. +/// +/// When operator() is called, it returns a symbolic expression `a + (b - a) * +/// v` where v is a symbolic random variable associated with the standard +/// uniform distribution. +/// +/// @see std::normal_distribution for the internal +/// representation of this implementation. +template <> +class uniform_real_distribution { + public: + using RealType = drake::symbolic::Expression; + using result_type = RealType; + + /// Constructs a new distribution object with a minimum value @p a and a + /// maximum value @p b. + /// + /// @throws std::exception if a and b are constant expressions but a > b. + explicit uniform_real_distribution(RealType a, RealType b = 1.0) + : a_{std::move(a)}, + b_{std::move(b)}, + random_variables_{std::make_shared>()} { + if (is_constant(a_) && is_constant(b_) && + get_constant_value(a_) > get_constant_value(b_)) { + throw std::runtime_error( + "In constructing a uniform_real_distribution, we " + "detected that the minimum distribution parameter " + + a_.to_string() + + " is greater than the maximum distribution parameter " + + b_.to_string() + "."); + } + } + + /// Constructs a new distribution object with a = 0.0 and b = 1.0. + uniform_real_distribution() : uniform_real_distribution{0.0} {} + + DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(uniform_real_distribution); + + /// Resets the internal state of the distribution object. + void reset() { index_ = 0; } + + /// Generates a symbolic expression representing a random value that is + /// distributed according to the associated probability function. + result_type operator()() { + if (random_variables_->size() == index_) { + random_variables_->emplace_back( + "random_uniform_" + std::to_string(index_), + drake::symbolic::Variable::Type::RANDOM_UNIFORM); + } + const drake::symbolic::Variable& v{(*random_variables_)[index_++]}; + return a_ + (b_ - a_) * v; + } + + /// Generates a symbolic expression representing a random value that is + /// distributed according to the associated probability function. + /// + /// @note We provide this method, which takes a random generator, for + /// compatibility with the std::uniform_real_distribution::operator(). + template + result_type operator()(Generator&) { + return (*this)(); + } + + /// Returns the minimum value a. + [[nodiscard]] RealType a() const { return a_; } + /// Returns the maximum value b. + [[nodiscard]] RealType b() const { return b_; } + + /// Returns the minimum potentially generated value. + [[nodiscard]] result_type min() const { return a_; } + /// Returns the maximum potentially generated value. + [[nodiscard]] result_type max() const { return b_; } + + private: + using Variable = drake::symbolic::Variable; + + RealType a_; + RealType b_; + std::shared_ptr> random_variables_; + std::vector::size_type index_{0}; + + friend bool operator==( + const uniform_real_distribution& lhs, + const uniform_real_distribution& rhs) { + return lhs.a().EqualTo(rhs.a()) && lhs.b().EqualTo(rhs.b()) && + (lhs.index_ == rhs.index_) && + (lhs.random_variables_ == rhs.random_variables_); + } +}; + +inline bool operator!=( + const uniform_real_distribution& lhs, + const uniform_real_distribution& rhs) { + return !(lhs == rhs); +} + +inline std::ostream& operator<<( + std::ostream& os, + const uniform_real_distribution& d) { + return os << d.a() << " " << d.b(); +} + +/// Provides std::normal_distribution, N(μ, σ), for symbolic expressions. +/// +/// When operator() is called, it returns a symbolic expression `μ + σ * v` +/// where v is a symbolic random variable associated with the standard normal +/// (Gaussian) distribution. +/// +/// It keeps a shared pointer to the vector of symbolic random variables that +/// has been created for the following purposes: +/// +/// - When `reset()` is called, it rewinds `index_` to zero so that the next +/// operator (re)-uses the first symbolic random variable. +/// @code +/// random_device rd; +/// RandomGenerator g{rd()}; +/// std::normal_distribution d(0.0, 1.0); +/// +/// const Expression e1{d(g)}; +/// const Expression e2{d(g)}; +/// d.reset(); +/// const Expression e3{d(g)}; +/// +/// EXPECT_FALSE(e1.EqualTo(e2)); +/// EXPECT_TRUE(e1.EqualTo(e3)); +/// @endcode +/// +/// - When an instance of this class is copied, the original and copied +/// distributions share the vector of symbolic random variables. We want to +/// make sure that the two generate identical sequences of elements. +/// @code +/// random_device rd; +/// RandomGenerator g{rd()}; +/// +/// std::normal_distribution d1(0.0, 1.0); +/// std::normal_distribution d2(d1); +/// const Expression e1_1{d1(g)}; +/// const Expression e1_2{d1(g)}; +/// +/// const Expression e2_1{d2(g)}; +/// const Expression e2_2{d2(g)}; +/// +/// EXPECT_TRUE(e1_1.EqualTo(e2_1)); +/// EXPECT_TRUE(e1_2.EqualTo(e2_2)); +/// @endcode +template <> +class normal_distribution { + public: + using RealType = drake::symbolic::Expression; + using result_type = RealType; + + /// Constructs a new distribution object with @p mean and @p stddev. + /// + /// @throws std::exception if stddev is a non-positive constant expression. + explicit normal_distribution(RealType mean, RealType stddev = 1.0) + : mean_{std::move(mean)}, + stddev_{std::move(stddev)}, + random_variables_{std::make_shared>()} { + if (is_constant(stddev_) && get_constant_value(stddev_) <= 0) { + throw std::runtime_error( + "In constructing a normal_distribution, we " + "detected that the stddev distribution parameter " + + stddev_.to_string() + " is non-positive."); + } + } + + /// Constructs a new distribution object with mean = 0.0 and stddev = 1.0. + normal_distribution() : normal_distribution{0.0} {} + + DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(normal_distribution); + + /// Resets the internal state of the distribution object. + void reset() { index_ = 0; } + + /// Generates a symbolic expression representing a random value that is + /// distributed according to the associated probability function. + result_type operator()() { + if (random_variables_->size() == index_) { + random_variables_->emplace_back( + "random_gaussian_" + std::to_string(index_), + drake::symbolic::Variable::Type::RANDOM_GAUSSIAN); + } + const drake::symbolic::Variable& v{(*random_variables_)[index_++]}; + return mean_ + stddev_ * v; + } + + /// Generates a symbolic expression representing a random value that is + /// distributed according to the associated probability function. + /// + /// @note We provide this method, which takes a random generator, for + /// compatibility with the std::normal_distribution::operator(). + template + result_type operator()(Generator&) { + return (*this)(); + } + + /// Returns the mean μ distribution parameter. + [[nodiscard]] RealType mean() const { return mean_; } + /// Returns the deviation σ distribution parameter. + [[nodiscard]] RealType stddev() const { return stddev_; } + + /// Returns the minimum potentially generated value. + /// + /// @note In libstdc++ std::normal_distribution<> defines min() and max() to + /// return -DBL_MAX and DBL_MAX while the one in libc++ returns -INFINITY and + /// INFINITY. We follows libc++ and return -INFINITY and INFINITY. + [[nodiscard]] result_type min() const { + return -std::numeric_limits::infinity(); + } + /// Returns the maximum potentially generated value.o + [[nodiscard]] result_type max() const { + return std::numeric_limits::infinity(); + } + + private: + using Variable = drake::symbolic::Variable; + + RealType mean_; + RealType stddev_; + std::shared_ptr> random_variables_; + std::vector::size_type index_{0}; + + friend bool operator==( + const normal_distribution& lhs, + const normal_distribution& rhs) { + return lhs.mean().EqualTo(rhs.mean()) && + lhs.stddev().EqualTo(rhs.stddev()) && (lhs.index_ == rhs.index_) && + (lhs.random_variables_ == rhs.random_variables_); + } +}; + +inline bool operator!=( + const normal_distribution& lhs, + const normal_distribution& rhs) { + return !(lhs == rhs); +} + +inline std::ostream& operator<<( + std::ostream& os, + const normal_distribution& d) { + return os << d.mean() << " " << d.stddev(); +} + +/// Provides std::exponential_distribution, Exp(λ), for symbolic expressions. +/// +/// When operator() is called, it returns a symbolic expression `v / λ` where v +/// is a symbolic random variable associated with the standard exponential +/// distribution (λ = 1). +/// +/// @see std::normal_distribution for the internal +/// representation of this implementation. +template <> +class exponential_distribution { + public: + using RealType = drake::symbolic::Expression; + using result_type = RealType; + + /// Constructs a new distribution object with @p lambda. + /// + /// @throws std::exception if lambda is a non-positive constant expression. + explicit exponential_distribution(RealType lambda) + : lambda_{std::move(lambda)}, + random_variables_{std::make_shared>()} { + if (is_constant(lambda_) && get_constant_value(lambda_) <= 0) { + throw std::runtime_error( + "In constructing an exponential_distribution, we " + "detected that the lambda distribution parameter " + + lambda_.to_string() + " is non-positive."); + } + } + + /// Constructs a new distribution object with lambda = 1.0. + exponential_distribution() : exponential_distribution{1.0} {} + + DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(exponential_distribution); + + /// Resets the internal state of the distribution object. + void reset() { index_ = 0; } + + /// Generates a symbolic expression representing a random value that is + /// distributed according to the associated probability function. + result_type operator()() { + if (random_variables_->size() == index_) { + random_variables_->emplace_back( + "random_exponential_" + std::to_string(index_), + drake::symbolic::Variable::Type::RANDOM_EXPONENTIAL); + } + const drake::symbolic::Variable& v{(*random_variables_)[index_++]}; + return v / lambda_; + } + + /// Generates a symbolic expression representing a random value that is + /// distributed according to the associated probability function. + /// + /// @note We provide this method, which takes a random generator, for + /// compatibility with the std::exponential_distribution::operator(). + template + result_type operator()(Generator&) { + return (*this)(); + } + + /// Returns the lambda λ distribution parameter. + [[nodiscard]] RealType lambda() const { return lambda_; } + /// Returns the minimum potentially generated value. + [[nodiscard]] result_type min() const { return 0.0; } + + /// Returns the maximum potentially generated value. + /// @note that in libstdc++ exponential_distribution<>::max() returns DBL_MAX + /// while the one in libc++ returns INFINITY. We follows libc++ and return + /// INFINITY. + [[nodiscard]] result_type max() const { + return std::numeric_limits::infinity(); + } + + private: + using Variable = drake::symbolic::Variable; + + RealType lambda_; + std::shared_ptr> random_variables_; + std::vector::size_type index_{0}; + + friend bool operator==( + const exponential_distribution& lhs, + const exponential_distribution& rhs) { + return lhs.lambda().EqualTo(rhs.lambda()) && (lhs.index_ == rhs.index_) && + (lhs.random_variables_ == rhs.random_variables_); + } +}; + +inline bool operator!=( + const exponential_distribution& lhs, + const exponential_distribution& rhs) { + return !(lhs == rhs); +} + +inline std::ostream& operator<<( + std::ostream& os, + const exponential_distribution& d) { + return os << d.lambda(); +} + +} // namespace std + +#if !defined(DRAKE_DOXYGEN_CXX) +// Define Eigen traits needed for Matrix. +namespace Eigen { +// Eigen scalar type traits for Matrix. +template <> +struct NumTraits + : GenericNumTraits { + static inline int digits10() { return 0; } +}; + +// Informs Eigen that Variable op Variable gets Expression. +template +struct ScalarBinaryOpTraits { + enum { Defined = 1 }; + typedef drake::symbolic::Expression ReturnType; +}; + +// Informs Eigen that Variable op Expression gets Expression. +template +struct ScalarBinaryOpTraits { + enum { Defined = 1 }; + typedef drake::symbolic::Expression ReturnType; +}; + +// Informs Eigen that Expression op Variable gets Expression. +template +struct ScalarBinaryOpTraits { + enum { Defined = 1 }; + typedef drake::symbolic::Expression ReturnType; +}; + +// Informs Eigen that Variable op double gets Expression. +template +struct ScalarBinaryOpTraits { + enum { Defined = 1 }; + typedef drake::symbolic::Expression ReturnType; +}; + +// Informs Eigen that double op Variable gets Expression. +template +struct ScalarBinaryOpTraits { + enum { Defined = 1 }; + typedef drake::symbolic::Expression ReturnType; +}; + +// Informs Eigen that Expression op double gets Expression. +template +struct ScalarBinaryOpTraits { + enum { Defined = 1 }; + typedef drake::symbolic::Expression ReturnType; +}; + +// Informs Eigen that double op Expression gets Expression. +template +struct ScalarBinaryOpTraits { + enum { Defined = 1 }; + typedef drake::symbolic::Expression ReturnType; +}; + +} // namespace Eigen +#endif // !defined(DRAKE_DOXYGEN_CXX) + +namespace drake { +namespace symbolic { + +// Matrix * Matrix => Matrix +template +typename std::enable_if_t< + std::is_base_of_v, MatrixL> && + std::is_base_of_v, MatrixR> && + std::is_same_v && + std::is_same_v, + Eigen::Matrix> +operator*(const MatrixL& lhs, const MatrixR& rhs) { + return lhs.template cast() * rhs.template cast(); +} + +// Matrix * Matrix => Matrix +template +typename std::enable_if_t< + std::is_base_of_v, MatrixL> && + std::is_base_of_v, MatrixR> && + std::is_same_v && + std::is_same_v, + Eigen::Matrix> +operator*(const MatrixL& lhs, const MatrixR& rhs) { + return lhs.template cast() * rhs.template cast(); +} + +// Matrix * Matrix => Matrix +template +typename std::enable_if_t< + std::is_base_of_v, MatrixL> && + std::is_base_of_v, MatrixR> && + std::is_same_v && + std::is_same_v, + Eigen::Matrix> +operator*(const MatrixL& lhs, const MatrixR& rhs) { + return lhs * rhs.template cast(); +} + +// Matrix * Matrix => Matrix +template +typename std::enable_if_t< + std::is_base_of_v, MatrixL> && + std::is_base_of_v, MatrixR> && + std::is_same_v && + std::is_same_v, + Eigen::Matrix> +operator*(const MatrixL& lhs, const MatrixR& rhs) { + return lhs.template cast() * rhs; +} + +// Matrix * Matrix => Matrix +template +typename std::enable_if_t< + std::is_base_of_v, MatrixL> && + std::is_base_of_v, MatrixR> && + std::is_same_v && + std::is_same_v, + Eigen::Matrix> +operator*(const MatrixL& lhs, const MatrixR& rhs) { + return lhs.template cast() * rhs.template cast(); +} + +// Matrix * Matrix => Matrix +template +typename std::enable_if_t< + std::is_base_of_v, MatrixL> && + std::is_base_of_v, MatrixR> && + std::is_same_v && + std::is_same_v, + Eigen::Matrix> +operator*(const MatrixL& lhs, const MatrixR& rhs) { + return lhs.template cast() * rhs.template cast(); +} + +/// Transform * Transform => Transform +template +auto operator*(const Eigen::Transform& t1, + const Eigen::Transform& t2) { + return t1 * t2.template cast(); +} + +/// Transform * Transform => Transform +template +auto operator*( + const Eigen::Transform& t1, + const Eigen::Transform& t2) { + return t1.template cast() * t2; +} + +/// Evaluates a symbolic matrix @p m using @p env and @p random_generator. +/// +/// If there is a random variable in @p m which is unassigned in @p env, this +/// function uses @p random_generator to sample a value and use the value to +/// substitute all occurrences of the random variable in @p m. +/// +/// @returns a matrix of double whose size is the size of @p m. +/// @throws std::exception if NaN is detected during evaluation. +/// @throws std::exception if @p m includes unassigned random variables but +/// @p random_generator is `nullptr`. +/// @pydrake_mkdoc_identifier{expression} +template +std::enable_if_t< + std::is_same_v, + Eigen::Matrix> +Evaluate(const Eigen::MatrixBase& m, + const Environment& env = Environment{}, + RandomGenerator* random_generator = nullptr) { + // Note that the return type is written out explicitly to help gcc 5 (on + // ubuntu). Previously the implementation used `auto`, and placed an ` + // .eval()` at the end to prevent lazy evaluation. + if (random_generator == nullptr) { + return m.unaryExpr([&env](const Expression& e) { return e.Evaluate(env); }); + } else { + // Construct an environment by extending `env` by sampling values for the + // random variables in `m` which are unassigned in `env`. + const Environment env_with_random_variables{PopulateRandomVariables( + env, GetDistinctVariables(m), random_generator)}; + return m.unaryExpr([&env_with_random_variables](const Expression& e) { + return e.Evaluate(env_with_random_variables); + }); + } +} + +/** Evaluates @p m using a given environment (by default, an empty environment). + * + * @throws std::exception if there exists a variable in @p m whose value is + * not provided by @p env. + * @throws std::exception if NaN is detected during evaluation. + */ +Eigen::SparseMatrix Evaluate( + const Eigen::Ref>& m, + const Environment& env = Environment{}); + +/// Substitutes a symbolic matrix @p m using a given substitution @p subst. +/// +/// @returns a matrix of symbolic expressions whose size is the size of @p m. +/// @throws std::exception if NaN is detected during substitution. +template +Eigen::Matrix +Substitute(const Eigen::MatrixBase& m, const Substitution& subst) { + static_assert(std::is_same_v, + "Substitute only accepts a symbolic matrix."); + // Note that the return type is written out explicitly to help gcc 5 (on + // ubuntu). + return m.unaryExpr( + [&subst](const Expression& e) { return e.Substitute(subst); }); +} + +/// Substitutes @p var with @p e in a symbolic matrix @p m. +/// +/// @returns a matrix of symbolic expressions whose size is the size of @p m. +/// @throws std::exception if NaN is detected during substitution. +template +Eigen::Matrix +Substitute(const Eigen::MatrixBase& m, const Variable& var, + const Expression& e) { + static_assert(std::is_same_v, + "Substitute only accepts a symbolic matrix."); + // Note that the return type is written out explicitly to help gcc 5 (on + // ubuntu). + return Substitute(m, Substitution{{var, e}}); +} + +/// Constructs a vector of variables from the vector of variable expressions. +/// @throws std::exception if there is an expression in @p vec which is not a +/// variable. +VectorX GetVariableVector( + const Eigen::Ref>& expressions); + +/// Computes the Jacobian matrix J of the vector function @p f with respect to +/// @p vars. J(i,j) contains ∂f(i)/∂vars(j). +/// +/// For example, Jacobian([x * cos(y), x * sin(y), x^2], {x, y}) returns the +/// following 3x2 matrix: +/// 
+///  = |cos(y)   -x * sin(y)|
+///    |sin(y)    x * cos(y)|
+///    | 2 * x             0|
+///
+/// +/// @pre {@p vars is non-empty}. +MatrixX Jacobian(const Eigen::Ref>& f, + const std::vector& vars); + +/// Computes the Jacobian matrix J of the vector function @p f with respect to +/// @p vars. J(i,j) contains ∂f(i)/∂vars(j). +/// +/// @pre {@p vars is non-empty}. +/// @pydrake_mkdoc_identifier{expression} +MatrixX Jacobian(const Eigen::Ref>& f, + const Eigen::Ref>& vars); + +/// Checks if every element in `m` is affine in `vars`. +/// @note If `m` is an empty matrix, it returns true. +bool IsAffine(const Eigen::Ref>& m, + const Variables& vars); + +/// Checks if every element in `m` is affine. +/// @note If `m` is an empty matrix, it returns true. +bool IsAffine(const Eigen::Ref>& m); + +/// Returns the distinct variables in the matrix of expressions. +Variables GetDistinctVariables(const Eigen::Ref>& v); + +/// Checks if two Eigen::Matrix @p m1 and @p m2 are structurally +/// equal. That is, it returns true if and only if `m1(i, j)` is structurally +/// equal to `m2(i, j)` for all `i`, `j`. +template +typename std::enable_if_t< + std::is_base_of_v, DerivedA> && + std::is_base_of_v, DerivedB> && + std::is_same_v && + std::is_same_v, + bool> +CheckStructuralEquality(const DerivedA& m1, const DerivedB& m2) { + EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(DerivedA, DerivedB); + DRAKE_DEMAND(m1.rows() == m2.rows() && m1.cols() == m2.cols()); + // Note that std::equal_to calls Expression::EqualTo which checks + // structural equality between two expressions. + return m1.binaryExpr(m2, std::equal_to{}).all(); +} + +} // namespace symbolic + +/** Provides specialization of @c cond function defined in drake/common/cond.h + * file. This specialization is required to handle @c double to @c + * symbolic::Expression conversion so that we can write one such as cond(x > + * 0.0, 1.0, -1.0). + */ +template +symbolic::Expression cond(const symbolic::Formula& f_cond, double v_then, + Rest... rest) { + return if_then_else(f_cond, symbolic::Expression{v_then}, cond(rest...)); +} + +/// Specializes common/dummy_value.h. +template <> +struct dummy_value { + static symbolic::Expression get() { return symbolic::Expression::NaN(); } +}; + +/// Returns the symbolic expression's value() as a double. +/// +/// @throws std::exception if it is not possible to evaluate the symbolic +/// expression with an empty environment. +double ExtractDoubleOrThrow(const symbolic::Expression& e); + +/// Returns @p matrix as an Eigen::Matrix with the same size +/// allocation as @p matrix. Calls ExtractDoubleOrThrow on each element of the +/// matrix, and therefore throws if any one of the extractions fail. +template +typename std::enable_if_t< + std::is_same_v, + Eigen::Matrix> +ExtractDoubleOrThrow(const Eigen::MatrixBase& matrix) { + return matrix + .unaryExpr([](const typename Derived::Scalar& value) { + return ExtractDoubleOrThrow(value); + }) + .eval(); +} + +/* + * Determine if two EigenBase<> types are matrices (non-column-vectors) of + * Expressions and doubles, to then form an implicit formulas. + */ +template +struct is_eigen_nonvector_expression_double_pair + : std::bool_constant< + is_eigen_nonvector_of::value && + is_eigen_nonvector_of::value> {}; + +/* + * Determine if two EigenBase<> types are vectors of Expressions and doubles + * that could make a formula. + */ +template +struct is_eigen_vector_expression_double_pair + : std::bool_constant< + is_eigen_vector_of::value && + is_eigen_vector_of::value> {}; + +} // namespace drake diff --git a/maliput_drake/include/drake/common/symbolic_expression_cell.h b/maliput_drake/include/drake/common/symbolic_expression_cell.h new file mode 100644 index 0000000..fef9197 --- /dev/null +++ b/maliput_drake/include/drake/common/symbolic_expression_cell.h @@ -0,0 +1,836 @@ +#pragma once +/// @file +/// +/// Provides implementation-details of symbolic expressions. +/// +/// It is strongly discouraged to include and use this header file outside of +/// drake/common/symbolic_* files. To include this file, you need to define +/// `DRAKE_COMMON_SYMBOLIC_DETAIL_HEADER` before. Without it, you have +/// compile-time errors. +#ifndef DRAKE_COMMON_SYMBOLIC_DETAIL_HEADER +#warning Do not include this file unless you implement symbolic libraries. +#endif + +#include // for cpplint only +#include +#include +#include +#include +#include + +#include + +#include "drake/common/drake_copyable.h" +#include "drake/common/random.h" +#include "drake/common/symbolic.h" + +namespace drake { +namespace symbolic { + +// Checks if @p v contains an integer value. +bool is_integer(double v); + +// Checks if @p v contains a positive integer value. +bool is_positive_integer(double v); + +// Checks if @p v contains a non-negative integer value. +bool is_non_negative_integer(double v); + +/** Represents an abstract class which is the base of concrete + * symbolic-expression classes. + * + * @note It provides virtual function, ExpressionCell::Display, because + * operator<< is not allowed to be a virtual function. + */ +class ExpressionCell { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(ExpressionCell) + + /** Returns expression kind. */ + [[nodiscard]] ExpressionKind get_kind() const { return kind_; } + + /** Sends all hash-relevant bytes for this ExpressionCell type into the given + * hasher, per the @ref hash_append concept -- except for get_kind(), because + * Expression already sends that. + */ + virtual void HashAppendDetail(DelegatingHasher*) const = 0; + + /** Collects variables in expression. */ + [[nodiscard]] virtual Variables GetVariables() const = 0; + + /** Checks structural equality. */ + [[nodiscard]] virtual bool EqualTo(const ExpressionCell& c) const = 0; + + /** Provides lexicographical ordering between expressions. */ + [[nodiscard]] virtual bool Less(const ExpressionCell& c) const = 0; + + /** Checks if this symbolic expression is convertible to Polynomial. */ + [[nodiscard]] bool is_polynomial() const { return is_polynomial_; } + + /** Checks if this symbolic expression is already expanded. */ + [[nodiscard]] bool is_expanded() const { return is_expanded_; } + + /** Sets this symbolic expression as already expanded. */ + void set_expanded() { is_expanded_ = true; } + + /** Evaluates under a given environment (by default, an empty environment). + * @throws std::exception if NaN is detected during evaluation. + */ + [[nodiscard]] virtual double Evaluate(const Environment& env) const = 0; + + /** Expands out products and positive integer powers in expression. + * @throws std::exception if NaN is detected during expansion. + */ + [[nodiscard]] virtual Expression Expand() const = 0; + + /** Returns an Expression obtained by replacing all occurrences of the + * variables in @p s in the current expression cell with the corresponding + * expressions in @p s. + * @throws std::exception if NaN is detected during substitution. + */ + [[nodiscard]] virtual Expression Substitute(const Substitution& s) const = 0; + + /** Differentiates this symbolic expression with respect to the variable @p + * var. + * @throws std::exception if it is not differentiable. + */ + [[nodiscard]] virtual Expression Differentiate(const Variable& x) const = 0; + + /** Outputs string representation of expression into output stream @p os. */ + virtual std::ostream& Display(std::ostream& os) const = 0; + + protected: + /** Constructs ExpressionCell of kind @p k with @p is_poly and @p is_expanded. + */ + ExpressionCell(ExpressionKind k, bool is_poly, bool is_expanded); + /** Default destructor. */ + virtual ~ExpressionCell() = default; + + private: + const ExpressionKind kind_{}; + const bool is_polynomial_{false}; + bool is_expanded_{false}; +}; + +/** Represents the base class for unary expressions. */ +class UnaryExpressionCell : public ExpressionCell { + public: + void HashAppendDetail(DelegatingHasher*) const override; + [[nodiscard]] Variables GetVariables() const override; + [[nodiscard]] bool EqualTo(const ExpressionCell& e) const override; + [[nodiscard]] bool Less(const ExpressionCell& e) const override; + [[nodiscard]] double Evaluate(const Environment& env) const override; + /** Returns the argument. */ + [[nodiscard]] const Expression& get_argument() const { return e_; } + + protected: + /** Constructs UnaryExpressionCell of kind @p k with @p e, @p is_poly, and @p + * is_expanded. */ + UnaryExpressionCell(ExpressionKind k, Expression e, bool is_poly, + bool is_expanded); + /** Returns the evaluation result f(@p v ). */ + [[nodiscard]] virtual double DoEvaluate(double v) const = 0; + + private: + const Expression e_; +}; + +/** Represents the base class for binary expressions. + */ +class BinaryExpressionCell : public ExpressionCell { + public: + void HashAppendDetail(DelegatingHasher*) const override; + [[nodiscard]] Variables GetVariables() const override; + [[nodiscard]] bool EqualTo(const ExpressionCell& e) const override; + [[nodiscard]] bool Less(const ExpressionCell& e) const override; + [[nodiscard]] double Evaluate(const Environment& env) const override; + /** Returns the first argument. */ + [[nodiscard]] const Expression& get_first_argument() const { return e1_; } + /** Returns the second argument. */ + [[nodiscard]] const Expression& get_second_argument() const { return e2_; } + + protected: + /** Constructs BinaryExpressionCell of kind @p k with @p e1, @p e2, + * @p is_poly, and @p is_expanded. + */ + BinaryExpressionCell(ExpressionKind k, Expression e1, Expression e2, + bool is_poly, bool is_expanded); + /** Returns the evaluation result f(@p v1, @p v2 ). */ + [[nodiscard]] virtual double DoEvaluate(double v1, double v2) const = 0; + + private: + const Expression e1_; + const Expression e2_; +}; + +/** Symbolic expression representing a variable. */ +class ExpressionVar : public ExpressionCell { + public: + /** Constructs an expression from @p var. + * @pre @p var is neither a dummy nor a BOOLEAN variable. + */ + void HashAppendDetail(DelegatingHasher*) const override; + explicit ExpressionVar(Variable v); + [[nodiscard]] const Variable& get_variable() const { return var_; } + [[nodiscard]] Variables GetVariables() const override; + [[nodiscard]] bool EqualTo(const ExpressionCell& e) const override; + [[nodiscard]] bool Less(const ExpressionCell& e) const override; + [[nodiscard]] double Evaluate(const Environment& env) const override; + [[nodiscard]] Expression Expand() const override; + [[nodiscard]] Expression Substitute(const Substitution& s) const override; + [[nodiscard]] Expression Differentiate(const Variable& x) const override; + std::ostream& Display(std::ostream& os) const override; + + private: + const Variable var_; +}; + +/** Symbolic expression representing a constant. */ +class ExpressionConstant : public ExpressionCell { + public: + explicit ExpressionConstant(double v); + [[nodiscard]] double get_value() const { return v_; } + void HashAppendDetail(DelegatingHasher*) const override; + [[nodiscard]] Variables GetVariables() const override; + [[nodiscard]] bool EqualTo(const ExpressionCell& e) const override; + [[nodiscard]] bool Less(const ExpressionCell& e) const override; + [[nodiscard]] double Evaluate(const Environment& env) const override; + [[nodiscard]] Expression Expand() const override; + [[nodiscard]] Expression Substitute(const Substitution& s) const override; + [[nodiscard]] Expression Differentiate(const Variable& x) const override; + std::ostream& Display(std::ostream& os) const override; + + private: + const double v_{}; +}; + +/** Symbolic expression representing NaN (not-a-number). */ +class ExpressionNaN : public ExpressionCell { + public: + ExpressionNaN(); + void HashAppendDetail(DelegatingHasher*) const override; + [[nodiscard]] Variables GetVariables() const override; + [[nodiscard]] bool EqualTo(const ExpressionCell& e) const override; + [[nodiscard]] bool Less(const ExpressionCell& e) const override; + [[nodiscard]] double Evaluate(const Environment& env) const override; + [[nodiscard]] Expression Expand() const override; + [[nodiscard]] Expression Substitute(const Substitution& s) const override; + [[nodiscard]] Expression Differentiate(const Variable& x) const override; + std::ostream& Display(std::ostream& os) const override; +}; + +/** Symbolic expression representing an addition which is a sum of products. + * + * @f[ + * c_0 + \sum c_i * e_i + * @f] + * + * where @f$ c_i @f$ is a constant and @f$ e_i @f$ is a symbolic expression. + * + * Internally this class maintains a member variable @c constant_ to represent + * @f$ c_0 @f$ and another member variable @c expr_to_coeff_map_ to represent a + * mapping from an expression @f$ e_i @f$ to its coefficient @f$ c_i @f$ of + * double. + */ +class ExpressionAdd : public ExpressionCell { + public: + /** Constructs ExpressionAdd from @p constant_term and @p term_to_coeff_map. + */ + ExpressionAdd(double constant, + const std::map& expr_to_coeff_map); + void HashAppendDetail(DelegatingHasher*) const override; + [[nodiscard]] Variables GetVariables() const override; + [[nodiscard]] bool EqualTo(const ExpressionCell& e) const override; + [[nodiscard]] bool Less(const ExpressionCell& e) const override; + [[nodiscard]] double Evaluate(const Environment& env) const override; + [[nodiscard]] Expression Expand() const override; + [[nodiscard]] Expression Substitute(const Substitution& s) const override; + [[nodiscard]] Expression Differentiate(const Variable& x) const override; + std::ostream& Display(std::ostream& os) const override; + /** Returns the constant. */ + [[nodiscard]] double get_constant() const { return constant_; } + /** Returns map from an expression to its coefficient. */ + [[nodiscard]] const std::map& get_expr_to_coeff_map() + const { + return expr_to_coeff_map_; + } + + private: + std::ostream& DisplayTerm(std::ostream& os, bool print_plus, double coeff, + const Expression& term) const; + + const double constant_{}; + const std::map expr_to_coeff_map_; +}; + +/** Factory class to help build ExpressionAdd expressions. */ +class ExpressionAddFactory { + public: + DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(ExpressionAddFactory) + + /** Default constructor. */ + ExpressionAddFactory() = default; + + /** Constructs ExpressionAddFactory with @p constant and @p + * expr_to_coeff_map. */ + ExpressionAddFactory(double constant, + std::map expr_to_coeff_map); + + /** Constructs ExpressionAddFactory from @p add. */ + explicit ExpressionAddFactory(const ExpressionAdd& add); + + /** Adds @p e to this factory. */ + void AddExpression(const Expression& e); + /** Adds ExpressionAdd pointed by @p ptr to this factory. */ + void Add(const ExpressionAdd& add); + /** Assigns a factory from a an ExpressionAdd. */ + ExpressionAddFactory& operator=(const ExpressionAdd& ptr); + + /** Negates the expressions in factory. + * If it represents c0 + c1 * t1 + ... + * cn * tn, + * this method flips it into -c0 - c1 * t1 - ... - cn * tn. + * @returns *this. + */ + ExpressionAddFactory& Negate(); + /** Returns a symbolic expression. */ + [[nodiscard]] Expression GetExpression() const; + + private: + /* Adds a constant @p constant to this factory. + * Adding constant into an add factory representing + * + * c0 + c1 * t1 + ... + cn * tn + * + * results in (c0 + constant) + c1 * t1 + ... + cn * tn. */ + void AddConstant(double constant); + /* Adds coeff * term to this factory. + * + * Adding (coeff * term) into an add factory representing + * + * c0 + c1 * t1 + ... + cn * tn + * + * results in c0 + c1 * t1 + ... + (coeff * term) + ... + cn * tn. Note that + * it also performs simplifications to merge the coefficients of common terms. + */ + void AddTerm(double coeff, const Expression& term); + /* Adds expr_to_coeff_map to this factory. It calls AddConstant and AddTerm + * methods. */ + void AddMap(const std::map& expr_to_coeff_map); + + double constant_{0.0}; + std::map expr_to_coeff_map_; +}; + +/** Symbolic expression representing a multiplication of powers. + * + * @f[ + * c_0 \cdot \prod b_i^{e_i} + * @f] + * + * where @f$ c_0 @f$ is a constant and @f$ b_i @f$ and @f$ e_i @f$ are symbolic + * expressions. + * + * Internally this class maintains a member variable @c constant_ representing + * @f$ c_0 @f$ and another member variable @c base_to_exponent_map_ representing + * a mapping from a base, @f$ b_i @f$ to its exponentiation @f$ e_i @f$. + */ +class ExpressionMul : public ExpressionCell { + public: + /** Constructs ExpressionMul from @p constant and @p base_to_exponent_map. */ + ExpressionMul(double constant, + const std::map& base_to_exponent_map); + void HashAppendDetail(DelegatingHasher*) const override; + [[nodiscard]] Variables GetVariables() const override; + [[nodiscard]] bool EqualTo(const ExpressionCell& e) const override; + [[nodiscard]] bool Less(const ExpressionCell& e) const override; + [[nodiscard]] double Evaluate(const Environment& env) const override; + [[nodiscard]] Expression Expand() const override; + [[nodiscard]] Expression Substitute(const Substitution& s) const override; + [[nodiscard]] Expression Differentiate(const Variable& x) const override; + std::ostream& Display(std::ostream& os) const override; + /** Returns constant term. */ + [[nodiscard]] double get_constant() const { return constant_; } + /** Returns map from a term to its coefficient. */ + [[nodiscard]] const std::map& + get_base_to_exponent_map() const { + return base_to_exponent_map_; + } + + private: + std::ostream& DisplayTerm(std::ostream& os, bool print_mul, + const Expression& base, + const Expression& exponent) const; + + double constant_{}; + std::map base_to_exponent_map_; +}; + +/** Factory class to help build ExpressionMul expressions. */ +class ExpressionMulFactory { + public: + DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(ExpressionMulFactory) + + /** Default constructor. */ + ExpressionMulFactory() = default; + + /** Constructs ExpressionMulFactory with @p constant and @p + * base_to_exponent_map. */ + ExpressionMulFactory(double constant, + std::map base_to_exponent_map); + + /** Constructs ExpressionMulFactory from @p mul. */ + explicit ExpressionMulFactory(const ExpressionMul& mul); + + /** Adds @p e to this factory. */ + void AddExpression(const Expression& e); + /** Adds ExpressionMul pointed by @p ptr to this factory. */ + void Add(const ExpressionMul& ptr); + /** Assigns a factory from an ExpressionMul. */ + ExpressionMulFactory& operator=(const ExpressionMul& ptr); + /** Negates the expressions in factory. + * If it represents c0 * p1 * ... * pn, + * this method flips it into -c0 * p1 * ... * pn. + * @returns *this. + */ + ExpressionMulFactory& Negate(); + /** Returns a symbolic expression. */ + [[nodiscard]] Expression GetExpression() const; + + private: + /* Adds constant to this factory. + Adding constant into an mul factory representing + + c * b1 ^ e1 * ... * bn ^ en + + results in (constant * c) * b1 ^ e1 * ... * bn ^ en. */ + void AddConstant(double constant); + /* Adds pow(base, exponent) to this factory. + Adding pow(base, exponent) into an mul factory representing + + c * b1 ^ e1 * ... * bn ^ en + + results in c * b1 ^ e1 * ... * base^exponent * ... * bn ^ en. Note that + it also performs simplifications to merge the exponents of common bases. + */ + void AddTerm(const Expression& base, const Expression& exponent); + /* Adds base_to_exponent_map to this factory. It calls AddConstant and AddTerm + * methods. */ + void AddMap(const std::map& base_to_exponent_map); + + /* Sets to represent a zero expression. */ + void SetZero(); + + double constant_{1.0}; + std::map base_to_exponent_map_; +}; + +/** Symbolic expression representing division. */ +class ExpressionDiv : public BinaryExpressionCell { + public: + ExpressionDiv(const Expression& e1, const Expression& e2); + [[nodiscard]] Expression Expand() const override; + [[nodiscard]] Expression Substitute(const Substitution& s) const override; + [[nodiscard]] Expression Differentiate(const Variable& x) const override; + std::ostream& Display(std::ostream& os) const override; + + private: + [[nodiscard]] double DoEvaluate(double v1, double v2) const override; +}; + +/** Symbolic expression representing logarithms. */ +class ExpressionLog : public UnaryExpressionCell { + public: + explicit ExpressionLog(const Expression& e); + [[nodiscard]] Expression Expand() const override; + [[nodiscard]] Expression Substitute(const Substitution& s) const override; + [[nodiscard]] Expression Differentiate(const Variable& x) const override; + std::ostream& Display(std::ostream& os) const override; + + friend Expression log(const Expression& e); + + private: + /* Throws std::exception if v ∉ [0, +oo). */ + static void check_domain(double v); + [[nodiscard]] double DoEvaluate(double v) const override; +}; + +/** Symbolic expression representing absolute value function. */ +class ExpressionAbs : public UnaryExpressionCell { + public: + explicit ExpressionAbs(const Expression& e); + [[nodiscard]] Expression Expand() const override; + [[nodiscard]] Expression Substitute(const Substitution& s) const override; + [[nodiscard]] Expression Differentiate(const Variable& x) const override; + std::ostream& Display(std::ostream& os) const override; + + friend Expression abs(const Expression& e); + + private: + [[nodiscard]] double DoEvaluate(double v) const override; +}; + +/** Symbolic expression representing exponentiation using the base of + * natural logarithms. */ +class ExpressionExp : public UnaryExpressionCell { + public: + explicit ExpressionExp(const Expression& e); + [[nodiscard]] Expression Expand() const override; + [[nodiscard]] Expression Substitute(const Substitution& s) const override; + [[nodiscard]] Expression Differentiate(const Variable& x) const override; + std::ostream& Display(std::ostream& os) const override; + + private: + [[nodiscard]] double DoEvaluate(double v) const override; +}; + +/** Symbolic expression representing square-root. */ +class ExpressionSqrt : public UnaryExpressionCell { + public: + explicit ExpressionSqrt(const Expression& e); + [[nodiscard]] Expression Expand() const override; + [[nodiscard]] Expression Substitute(const Substitution& s) const override; + [[nodiscard]] Expression Differentiate(const Variable& x) const override; + std::ostream& Display(std::ostream& os) const override; + + friend Expression sqrt(const Expression& e); + + private: + /* Throws std::exception if v ∉ [0, +oo). */ + static void check_domain(double v); + [[nodiscard]] double DoEvaluate(double v) const override; +}; + +/** Symbolic expression representing power function. */ +class ExpressionPow : public BinaryExpressionCell { + public: + ExpressionPow(const Expression& e1, const Expression& e2); + [[nodiscard]] Expression Expand() const override; + [[nodiscard]] Expression Substitute(const Substitution& s) const override; + [[nodiscard]] Expression Differentiate(const Variable& x) const override; + std::ostream& Display(std::ostream& os) const override; + + friend Expression pow(const Expression& e1, const Expression& e2); + + private: + /* Throws std::exception if v1 is finite negative and v2 is finite + non-integer. */ + static void check_domain(double v1, double v2); + [[nodiscard]] double DoEvaluate(double v1, double v2) const override; +}; + +/** Symbolic expression representing sine function. */ +class ExpressionSin : public UnaryExpressionCell { + public: + explicit ExpressionSin(const Expression& e); + [[nodiscard]] Expression Expand() const override; + [[nodiscard]] Expression Substitute(const Substitution& s) const override; + [[nodiscard]] Expression Differentiate(const Variable& x) const override; + std::ostream& Display(std::ostream& os) const override; + + private: + [[nodiscard]] double DoEvaluate(double v) const override; +}; + +/** Symbolic expression representing cosine function. */ +class ExpressionCos : public UnaryExpressionCell { + public: + explicit ExpressionCos(const Expression& e); + [[nodiscard]] Expression Expand() const override; + [[nodiscard]] Expression Substitute(const Substitution& s) const override; + [[nodiscard]] Expression Differentiate(const Variable& x) const override; + std::ostream& Display(std::ostream& os) const override; + + private: + [[nodiscard]] double DoEvaluate(double v) const override; +}; + +/** Symbolic expression representing tangent function. */ +class ExpressionTan : public UnaryExpressionCell { + public: + explicit ExpressionTan(const Expression& e); + [[nodiscard]] Expression Expand() const override; + [[nodiscard]] Expression Substitute(const Substitution& s) const override; + [[nodiscard]] Expression Differentiate(const Variable& x) const override; + std::ostream& Display(std::ostream& os) const override; + + private: + [[nodiscard]] double DoEvaluate(double v) const override; +}; + +/** Symbolic expression representing arcsine function. */ +class ExpressionAsin : public UnaryExpressionCell { + public: + explicit ExpressionAsin(const Expression& e); + [[nodiscard]] Expression Expand() const override; + [[nodiscard]] Expression Substitute(const Substitution& s) const override; + [[nodiscard]] Expression Differentiate(const Variable& x) const override; + std::ostream& Display(std::ostream& os) const override; + + friend Expression asin(const Expression& e); + + private: + /* Throws std::exception if v ∉ [-1.0, +1.0]. */ + static void check_domain(double v); + [[nodiscard]] double DoEvaluate(double v) const override; +}; + +/** Symbolic expression representing arccosine function. */ +class ExpressionAcos : public UnaryExpressionCell { + public: + explicit ExpressionAcos(const Expression& e); + [[nodiscard]] Expression Expand() const override; + [[nodiscard]] Expression Substitute(const Substitution& s) const override; + [[nodiscard]] Expression Differentiate(const Variable& x) const override; + std::ostream& Display(std::ostream& os) const override; + + friend Expression acos(const Expression& e); + + private: + /* Throws std::exception if v ∉ [-1.0, +1.0]. */ + static void check_domain(double v); + [[nodiscard]] double DoEvaluate(double v) const override; +}; + +/** Symbolic expression representing arctangent function. */ +class ExpressionAtan : public UnaryExpressionCell { + public: + explicit ExpressionAtan(const Expression& e); + [[nodiscard]] Expression Expand() const override; + [[nodiscard]] Expression Substitute(const Substitution& s) const override; + [[nodiscard]] Expression Differentiate(const Variable& x) const override; + std::ostream& Display(std::ostream& os) const override; + + private: + [[nodiscard]] double DoEvaluate(double v) const override; +}; + +/** Symbolic expression representing atan2 function (arctangent function with + * two arguments). atan2(y, x) is defined as atan(y/x). */ +class ExpressionAtan2 : public BinaryExpressionCell { + public: + ExpressionAtan2(const Expression& e1, const Expression& e2); + [[nodiscard]] Expression Expand() const override; + [[nodiscard]] Expression Substitute(const Substitution& s) const override; + [[nodiscard]] Expression Differentiate(const Variable& x) const override; + std::ostream& Display(std::ostream& os) const override; + + private: + [[nodiscard]] double DoEvaluate(double v1, double v2) const override; +}; + +/** Symbolic expression representing hyperbolic sine function. */ +class ExpressionSinh : public UnaryExpressionCell { + public: + explicit ExpressionSinh(const Expression& e); + [[nodiscard]] Expression Expand() const override; + [[nodiscard]] Expression Substitute(const Substitution& s) const override; + [[nodiscard]] Expression Differentiate(const Variable& x) const override; + std::ostream& Display(std::ostream& os) const override; + + private: + [[nodiscard]] double DoEvaluate(double v) const override; +}; + +/** Symbolic expression representing hyperbolic cosine function. */ +class ExpressionCosh : public UnaryExpressionCell { + public: + explicit ExpressionCosh(const Expression& e); + [[nodiscard]] Expression Expand() const override; + [[nodiscard]] Expression Substitute(const Substitution& s) const override; + [[nodiscard]] Expression Differentiate(const Variable& x) const override; + std::ostream& Display(std::ostream& os) const override; + + private: + [[nodiscard]] double DoEvaluate(double v) const override; +}; + +/** Symbolic expression representing hyperbolic tangent function. */ +class ExpressionTanh : public UnaryExpressionCell { + public: + explicit ExpressionTanh(const Expression& e); + [[nodiscard]] Expression Expand() const override; + [[nodiscard]] Expression Substitute(const Substitution& s) const override; + [[nodiscard]] Expression Differentiate(const Variable& x) const override; + std::ostream& Display(std::ostream& os) const override; + + private: + [[nodiscard]] double DoEvaluate(double v) const override; +}; + +/** Symbolic expression representing min function. */ +class ExpressionMin : public BinaryExpressionCell { + public: + ExpressionMin(const Expression& e1, const Expression& e2); + [[nodiscard]] Expression Expand() const override; + [[nodiscard]] Expression Substitute(const Substitution& s) const override; + [[nodiscard]] Expression Differentiate(const Variable& x) const override; + std::ostream& Display(std::ostream& os) const override; + + private: + [[nodiscard]] double DoEvaluate(double v1, double v2) const override; +}; + +/** Symbolic expression representing max function. */ +class ExpressionMax : public BinaryExpressionCell { + public: + ExpressionMax(const Expression& e1, const Expression& e2); + [[nodiscard]] Expression Expand() const override; + [[nodiscard]] Expression Substitute(const Substitution& s) const override; + [[nodiscard]] Expression Differentiate(const Variable& x) const override; + std::ostream& Display(std::ostream& os) const override; + + private: + [[nodiscard]] double DoEvaluate(double v1, double v2) const override; +}; + +/** Symbolic expression representing ceil function. */ +class ExpressionCeiling : public UnaryExpressionCell { + public: + explicit ExpressionCeiling(const Expression& e); + [[nodiscard]] Expression Expand() const override; + [[nodiscard]] Expression Substitute(const Substitution& s) const override; + [[nodiscard]] Expression Differentiate(const Variable& x) const override; + std::ostream& Display(std::ostream& os) const override; + + private: + [[nodiscard]] double DoEvaluate(double v) const override; +}; + +/** Symbolic expression representing floor function. */ +class ExpressionFloor : public UnaryExpressionCell { + public: + explicit ExpressionFloor(const Expression& e); + [[nodiscard]] Expression Expand() const override; + [[nodiscard]] Expression Substitute(const Substitution& s) const override; + [[nodiscard]] Expression Differentiate(const Variable& x) const override; + std::ostream& Display(std::ostream& os) const override; + + private: + [[nodiscard]] double DoEvaluate(double v) const override; +}; + +/** Symbolic expression representing if-then-else expression. */ +class ExpressionIfThenElse : public ExpressionCell { + public: + /** Constructs if-then-else expression from @p f_cond, @p e_then, and @p + * e_else. */ + ExpressionIfThenElse(Formula f_cond, Expression e_then, Expression e_else); + void HashAppendDetail(DelegatingHasher*) const override; + [[nodiscard]] Variables GetVariables() const override; + [[nodiscard]] bool EqualTo(const ExpressionCell& e) const override; + [[nodiscard]] bool Less(const ExpressionCell& e) const override; + [[nodiscard]] double Evaluate(const Environment& env) const override; + [[nodiscard]] Expression Expand() const override; + [[nodiscard]] Expression Substitute(const Substitution& s) const override; + [[nodiscard]] Expression Differentiate(const Variable& x) const override; + std::ostream& Display(std::ostream& os) const override; + + /** Returns the conditional formula. */ + [[nodiscard]] const Formula& get_conditional_formula() const { + return f_cond_; + } + /** Returns the 'then' expression. */ + [[nodiscard]] const Expression& get_then_expression() const { + return e_then_; + } + /** Returns the 'else' expression. */ + [[nodiscard]] const Expression& get_else_expression() const { + return e_else_; + } + + private: + const Formula f_cond_; + const Expression e_then_; + const Expression e_else_; +}; + +/** Symbolic expression representing an uninterpreted function. */ +class ExpressionUninterpretedFunction : public ExpressionCell { + public: + /** Constructs an uninterpreted-function expression from @p name and @p + * arguments. + */ + ExpressionUninterpretedFunction(std::string name, + std::vector arguments); + void HashAppendDetail(DelegatingHasher*) const override; + [[nodiscard]] Variables GetVariables() const override; + [[nodiscard]] bool EqualTo(const ExpressionCell& e) const override; + [[nodiscard]] bool Less(const ExpressionCell& e) const override; + [[nodiscard]] double Evaluate(const Environment& env) const override; + [[nodiscard]] Expression Expand() const override; + [[nodiscard]] Expression Substitute(const Substitution& s) const override; + [[nodiscard]] Expression Differentiate(const Variable& x) const override; + std::ostream& Display(std::ostream& os) const override; + + /** Returns the name of this expression. */ + [[nodiscard]] const std::string& get_name() const { return name_; } + + /** Returns the arguments of this expression. */ + [[nodiscard]] const std::vector& get_arguments() const { + return arguments_; + } + + private: + const std::string name_; + const std::vector arguments_; +}; + +/** Checks if @p c is a constant expression. */ +bool is_constant(const ExpressionCell& c); +/** Checks if @p c is a variable expression. */ +bool is_variable(const ExpressionCell& c); +/** Checks if @p c is a unary expression. */ +bool is_unary(const ExpressionCell& c); +/** Checks if @p c is a binary expression. */ +bool is_binary(const ExpressionCell& c); +/** Checks if @p c is an addition expression. */ +bool is_addition(const ExpressionCell& c); +/** Checks if @p c is an multiplication expression. */ +bool is_multiplication(const ExpressionCell& c); +/** Checks if @p c is a division expression. */ +bool is_division(const ExpressionCell& c); +/** Checks if @p c is a log expression. */ +bool is_log(const ExpressionCell& c); +/** Checks if @p c is an absolute-value-function expression. */ +bool is_abs(const ExpressionCell& c); +/** Checks if @p c is an exp expression. */ +bool is_exp(const ExpressionCell& c); +/** Checks if @p c is a square-root expression. */ +bool is_sqrt(const ExpressionCell& c); +/** Checks if @p c is a power-function expression. */ +bool is_pow(const ExpressionCell& c); +/** Checks if @p c is a sine expression. */ +bool is_sin(const ExpressionCell& c); +/** Checks if @p c is a cosine expression. */ +bool is_cos(const ExpressionCell& c); +/** Checks if @p c is a tangent expression. */ +bool is_tan(const ExpressionCell& c); +/** Checks if @p c is an arcsine expression. */ +bool is_asin(const ExpressionCell& c); +/** Checks if @p c is an arccosine expression. */ +bool is_acos(const ExpressionCell& c); +/** Checks if @p c is an arctangent expression. */ +bool is_atan(const ExpressionCell& c); +/** Checks if @p c is a arctangent2 expression. */ +bool is_atan2(const ExpressionCell& c); +/** Checks if @p c is a hyperbolic-sine expression. */ +bool is_sinh(const ExpressionCell& c); +/** Checks if @p c is a hyperbolic-cosine expression. */ +bool is_cosh(const ExpressionCell& c); +/** Checks if @p c is a hyperbolic-tangent expression. */ +bool is_tanh(const ExpressionCell& c); +/** Checks if @p c is a min expression. */ +bool is_min(const ExpressionCell& c); +/** Checks if @p c is a max expression. */ +bool is_max(const ExpressionCell& c); +/** Checks if @p c is a ceil expression. */ +bool is_ceil(const ExpressionCell& c); +/** Checks if @p c is a floor expression. */ +bool is_floor(const ExpressionCell& c); +/** Checks if @p c is an if-then-else expression. */ +bool is_if_then_else(const ExpressionCell& c); +/** Checks if @p c is an uninterpreted-function expression. */ +bool is_uninterpreted_function(const ExpressionCell& c); + +} // namespace symbolic +} // namespace drake diff --git a/maliput_drake/include/drake/common/symbolic_expression_visitor.h b/maliput_drake/include/drake/common/symbolic_expression_visitor.h new file mode 100644 index 0000000..e1b08df --- /dev/null +++ b/maliput_drake/include/drake/common/symbolic_expression_visitor.h @@ -0,0 +1,178 @@ +#pragma once + +#ifndef DRAKE_COMMON_SYMBOLIC_HEADER +#error Do not directly include this file. Include "drake/common/symbolic.h". +#endif + +#include +#include + +#include "drake/common/drake_assert.h" +#include "drake/common/symbolic.h" + +namespace drake { +namespace symbolic { + +/// Calls visitor object @p v with a polynomial symbolic-expression @p e, and +/// arguments @p args. Visitor object is expected to implement the following +/// methods which take @p f and @p args: `VisitConstant`, `VisitVariable`, +/// `VisitAddition`, `VisitMultiplication`, `VisitDivision`, `VisitPow`. +/// +/// @throws std::exception if NaN is detected during a visit. +/// +/// See the implementation of @c DegreeVisitor class and @c Degree function in +/// drake/common/symbolic_monomial.cc as an example usage. +/// +/// @pre e.is_polynomial() is true. +template +Result VisitPolynomial(Visitor* v, const Expression& e, Args&&... args) { + DRAKE_DEMAND(e.is_polynomial()); + switch (e.get_kind()) { + case ExpressionKind::Constant: + return v->VisitConstant(e, std::forward(args)...); + + case ExpressionKind::Var: + return v->VisitVariable(e, std::forward(args)...); + + case ExpressionKind::Add: + return v->VisitAddition(e, std::forward(args)...); + + case ExpressionKind::Mul: + return v->VisitMultiplication(e, std::forward(args)...); + + case ExpressionKind::Div: + return v->VisitDivision(e, std::forward(args)...); + + case ExpressionKind::Pow: + return v->VisitPow(e, std::forward(args)...); + + case ExpressionKind::NaN: + throw std::runtime_error("NaN is detected while visiting an expression."); + + case ExpressionKind::Log: + case ExpressionKind::Abs: + case ExpressionKind::Exp: + case ExpressionKind::Sqrt: + case ExpressionKind::Sin: + case ExpressionKind::Cos: + case ExpressionKind::Tan: + case ExpressionKind::Asin: + case ExpressionKind::Acos: + case ExpressionKind::Atan: + case ExpressionKind::Atan2: + case ExpressionKind::Sinh: + case ExpressionKind::Cosh: + case ExpressionKind::Tanh: + case ExpressionKind::Min: + case ExpressionKind::Max: + case ExpressionKind::Ceil: + case ExpressionKind::Floor: + case ExpressionKind::IfThenElse: + case ExpressionKind::UninterpretedFunction: + // Unreachable because of `DRAKE_DEMAND(e.is_polynomial())` at the top. + throw std::domain_error( + "Unexpected Kind was is_polynomial in VisitPolynomial"); + } + // Unreachable because all switch cases are accounted for above. + DRAKE_UNREACHABLE(); +} + +/// Calls visitor object @p v with a symbolic-expression @p e, and arguments @p +/// args. Visitor object is expected to implement the following methods which +/// take @p f and @p args: `VisitConstant`, `VisitVariable`, `VisitAddition`, +/// `VisitMultiplication`, `VisitDivision`, `VisitLog`, `VisitAbs`, `VisitExp`, +/// `VisitSqrt`, `VisitPow`, `VisitSin`, `VisitCos`, `VisitTan`, `VisitAsin`, +/// `VisitAtan`, `VisitAtan2`, `VisitSinh`, `VisitCosh`, `VisitTanh`, +/// `VisitMin`, `VisitMax`, `VisitCeil`, `VisitFloor`, `VisitIfThenElse`, +/// `VisitUninterpretedFunction. +/// +/// @throws std::exception if NaN is detected during a visit. +template +Result VisitExpression(Visitor* v, const Expression& e, Args&&... args) { + switch (e.get_kind()) { + case ExpressionKind::Constant: + return v->VisitConstant(e, std::forward(args)...); + + case ExpressionKind::Var: + return v->VisitVariable(e, std::forward(args)...); + + case ExpressionKind::Add: + return v->VisitAddition(e, std::forward(args)...); + + case ExpressionKind::Mul: + return v->VisitMultiplication(e, std::forward(args)...); + + case ExpressionKind::Div: + return v->VisitDivision(e, std::forward(args)...); + + case ExpressionKind::Log: + return v->VisitLog(e, std::forward(args)...); + + case ExpressionKind::Abs: + return v->VisitAbs(e, std::forward(args)...); + + case ExpressionKind::Exp: + return v->VisitExp(e, std::forward(args)...); + + case ExpressionKind::Sqrt: + return v->VisitSqrt(e, std::forward(args)...); + + case ExpressionKind::Pow: + return v->VisitPow(e, std::forward(args)...); + + case ExpressionKind::Sin: + return v->VisitSin(e, std::forward(args)...); + + case ExpressionKind::Cos: + return v->VisitCos(e, std::forward(args)...); + + case ExpressionKind::Tan: + return v->VisitTan(e, std::forward(args)...); + + case ExpressionKind::Asin: + return v->VisitAsin(e, std::forward(args)...); + + case ExpressionKind::Acos: + return v->VisitAcos(e, std::forward(args)...); + + case ExpressionKind::Atan: + return v->VisitAtan(e, std::forward(args)...); + + case ExpressionKind::Atan2: + return v->VisitAtan2(e, std::forward(args)...); + + case ExpressionKind::Sinh: + return v->VisitSinh(e, std::forward(args)...); + + case ExpressionKind::Cosh: + return v->VisitCosh(e, std::forward(args)...); + + case ExpressionKind::Tanh: + return v->VisitTanh(e, std::forward(args)...); + + case ExpressionKind::Min: + return v->VisitMin(e, std::forward(args)...); + + case ExpressionKind::Max: + return v->VisitMax(e, std::forward(args)...); + + case ExpressionKind::Ceil: + return v->VisitCeil(e, std::forward(args)...); + + case ExpressionKind::Floor: + return v->VisitFloor(e, std::forward(args)...); + + case ExpressionKind::IfThenElse: + return v->VisitIfThenElse(e, std::forward(args)...); + + case ExpressionKind::NaN: + throw std::runtime_error("NaN is detected while visiting an expression."); + + case ExpressionKind::UninterpretedFunction: + return v->VisitUninterpretedFunction(e, std::forward(args)...); + } + DRAKE_UNREACHABLE(); +} + +} // namespace symbolic +} // namespace drake diff --git a/maliput_drake/include/drake/common/symbolic_formula.h b/maliput_drake/include/drake/common/symbolic_formula.h new file mode 100644 index 0000000..1113b23 --- /dev/null +++ b/maliput_drake/include/drake/common/symbolic_formula.h @@ -0,0 +1,1391 @@ +#pragma once + +#ifndef DRAKE_COMMON_SYMBOLIC_HEADER +#error Do not directly include this file. Include "drake/common/symbolic.h". +#endif + +#include +#include +#include +#include +#include +#include + +#include + +#include "drake/common/drake_assert.h" +#include "drake/common/drake_bool.h" +#include "drake/common/drake_copyable.h" +#include "drake/common/eigen_types.h" +#include "drake/common/hash.h" +#include "drake/common/random.h" +#include "drake/common/symbolic.h" + +namespace drake { +namespace symbolic { + +/** Kinds of symbolic formulas. */ +enum class FormulaKind { + False, ///< ⊥ + True, ///< ⊤ + Var, ///< Boolean Variable + Eq, ///< = + Neq, ///< != + Gt, ///< > + Geq, ///< >= + Lt, ///< < + Leq, ///< <= + And, ///< Conjunction (∧) + Or, ///< Disjunction (∨) + Not, ///< Negation (¬) + Forall, ///< Universal quantification (∀) + Isnan, ///< NaN check predicate + PositiveSemidefinite, ///< Positive semidefinite matrix +}; + +// Total ordering between FormulaKinds +bool operator<(FormulaKind k1, FormulaKind k2); + +class FormulaCell; // In drake/common/symbolic_formula_cell.h +class FormulaFalse; // In drake/common/symbolic_formula_cell.h +class FormulaTrue; // In drake/common/symbolic_formula_cell.h +class FormulaVar; // In drake/common/symbolic_formula_cell.h +class RelationalFormulaCell; // In drake/common/symbolic_formula_cell.h +class FormulaEq; // In drake/common/symbolic_formula_cell.h +class FormulaNeq; // In drake/common/symbolic_formula_cell.h +class FormulaGt; // In drake/common/symbolic_formula_cell.h +class FormulaGeq; // In drake/common/symbolic_formula_cell.h +class FormulaLt; // In drake/common/symbolic_formula_cell.h +class FormulaLeq; // In drake/common/symbolic_formula_cell.h +class NaryFormulaCell; // In drake/common/symbolic_formula_cell.h +class FormulaNot; // In drake/common/symbolic_formula_cell.h +class FormulaAnd; // In drake/common/symbolic_formula_cell.h +class FormulaOr; // In drake/common/symbolic_formula_cell.h +class FormulaForall; // In drake/common/symbolic_formula_cell.h +class FormulaIsnan; // In drake/common/symbolic_formula_cell.h +class FormulaPositiveSemidefinite; // In drake/common/symbolic_formula_cell.h + +/** Represents a symbolic form of a first-order logic formula. + +It has the following grammar: + +\verbatim + F := ⊥ | ⊤ | Var | E = E | E ≠ E | E > E | E ≥ E | E < E | E ≤ E + | E ∧ ... ∧ E | E ∨ ... ∨ E | ¬F | ∀ x₁, ..., xn. F +\endverbatim + +In the implementation, Formula is a simple wrapper including a shared +pointer to FormulaCell class which is a super-class of different kinds +of symbolic formulas (i.e. FormulaAnd, FormulaOr, FormulaEq). Note +that it includes a shared pointer, not a unique pointer, to allow +sharing sub-expressions. + +\note The sharing of sub-expressions is not yet implemented. + +The following simple simplifications are implemented: +\verbatim + E1 = E2 -> True (if E1 and E2 are structurally equal) + E1 ≠ E2 -> False (if E1 and E2 are structurally equal) + E1 > E2 -> False (if E1 and E2 are structurally equal) + E1 ≥ E2 -> True (if E1 and E2 are structurally equal) + E1 < E2 -> False (if E1 and E2 are structurally equal) + E1 ≤ E2 -> True (if E1 and E2 are structurally equal) + F1 ∧ F2 -> False (if either F1 or F2 is False) + F1 ∨ F2 -> True (if either F1 or F2 is True) + ¬(¬(F)) -> F +\endverbatim + +We flatten nested conjunctions (or disjunctions) at the construction. A +conjunction (resp. disjunction) takes a set of conjuncts (resp. disjuncts). Note +that any duplicated conjunct/disjunct is removed. For example, both of `f1 && +(f2 && f1)` and `(f1 && f2) && f1` are flattened to `f1 && f2 && f1` and +simplified into `f1 && f2`. As a result, the two are identified as the same +formula. + +\note Formula class has an explicit conversion operator to bool. It evaluates a +symbolic formula under an empty environment. If a symbolic formula includes +variables, the conversion operator throws an exception. This operator is only +intended for third-party code doing things like `(imag(SymbolicExpression(0)) +== SymbolicExpression(0)) { ... };` that we found in Eigen3 codebase. In +general, a user of this class should explicitly call `Evaluate` from within +Drake for readability. + +*/ +class Formula { + public: + DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Formula) + + /** Default constructor. Sets the value to Formula::False, to be consistent + * with value-initialized `bool`s. + */ + Formula() : Formula(False()) {} + + /** Constructs from a `bool`. This overload is also used by Eigen when + * EIGEN_INITIALIZE_MATRICES_BY_ZERO is enabled. + */ + explicit Formula(bool value) : Formula(value ? True() : False()) {} + + explicit Formula(std::shared_ptr ptr); + + /** Constructs a formula from @p var. + * @pre @p var is of BOOLEAN type and not a dummy variable. + */ + explicit Formula(const Variable& var); + + [[nodiscard]] FormulaKind get_kind() const; + /** Gets free variables (unquantified variables). */ + [[nodiscard]] Variables GetFreeVariables() const; + /** Checks structural equality. */ + [[nodiscard]] bool EqualTo(const Formula& f) const; + /** Checks lexicographical ordering between this and @p e. + * + * If the two formulas f1 and f2 have different kinds k1 and k2 respectively, + * f1.Less(f2) is equal to k1 < k2. If f1 and f2 are expressions of the same + * kind, we check the ordering between f1 and f2 by comparing their elements + * lexicographically. + * + * For example, in case of And, let f1 and f2 be + * + * f1 = f_1,1 ∧ ... ∧ f_1,n + * f2 = f_2,1 ∧ ... ∧ f_2,m + * + * f1.Less(f2) is true if there exists an index i (<= n, m) such that + * for all j < i, we have + * + * ¬(f_1_j.Less(f_2_j)) ∧ ¬(f_2_j.Less(f_1_j)) + * + * and f_1_i.Less(f_2_i) holds. + * + * This function is used as a compare function in + * std::map and std::set via + * std::less. */ + [[nodiscard]] bool Less(const Formula& f) const; + + /** Evaluates using a given environment (by default, an empty environment) and + * a random number generator. If there is a random variable in this formula + * which is unassigned in @p env, it uses @p random_generator to sample a + * value and use it to substitute all occurrences of the random variable in + * this formula. + * + * @throws std::exception if a variable `v` is needed for an evaluation + * but not provided by @p env. + * @throws std::exception if an unassigned random variable is detected + * while @p random_generator is `nullptr`. + */ + bool Evaluate(const Environment& env = Environment{}, + RandomGenerator* random_generator = nullptr) const; + + /** Evaluates using an empty environment and a random number generator. + * + * See the above overload for the exceptions that it might throw. + */ + bool Evaluate(RandomGenerator* random_generator) const; + + /** Returns a copy of this formula replacing all occurrences of @p var + * with @p e. + * @throws std::exception if NaN is detected during substitution. + */ + [[nodiscard]] Formula Substitute(const Variable& var, + const Expression& e) const; + + /** Returns a copy of this formula replacing all occurrences of the + * variables in @p s with corresponding expressions in @p s. Note that the + * substitutions occur simultaneously. For example, (x / y > + * 0).Substitute({{x, y}, {y, x}}) gets (y / x > 0). + * @throws std::exception if NaN is detected during substitution. + */ + [[nodiscard]] Formula Substitute(const Substitution& s) const; + + /** Returns string representation of Formula. */ + [[nodiscard]] std::string to_string() const; + + static Formula True(); + static Formula False(); + + /** Conversion to bool. */ + explicit operator bool() const { return Evaluate(); } + + /** Implements the @ref hash_append concept. */ + template + friend void hash_append(HashAlgorithm& hasher, const Formula& item) noexcept { + DelegatingHasher delegating_hasher( + [&hasher](const void* data, const size_t length) { + return hasher(data, length); + }); + item.HashAppend(&delegating_hasher); + } + + friend std::ostream& operator<<(std::ostream& os, const Formula& f); + friend void swap(Formula& a, Formula& b) { std::swap(a.ptr_, b.ptr_); } + + friend bool is_false(const Formula& f); + friend bool is_true(const Formula& f); + friend bool is_variable(const Formula& f); + friend bool is_equal_to(const Formula& f); + friend bool is_not_equal_to(const Formula& f); + friend bool is_greater_than(const Formula& f); + friend bool is_greater_than_or_equal_to(const Formula& f); + friend bool is_less_than(const Formula& f); + friend bool is_less_than_or_equal_to(const Formula& f); + friend bool is_relational(const Formula& f); + friend bool is_conjunction(const Formula& f); + friend bool is_disjunction(const Formula& f); + friend bool is_negation(const Formula& f); + friend bool is_forall(const Formula& f); + friend bool is_isnan(const Formula& f); + friend bool is_positive_semidefinite(const Formula& f); + + // Note that the following cast functions are only for low-level operations + // and not exposed to the user of symbolic_formula.h. These functions are + // declared in symbolic_formula_cell.h header. + friend std::shared_ptr to_false(const Formula& f); + friend std::shared_ptr to_true(const Formula& f); + friend std::shared_ptr to_variable(const Formula& f); + friend std::shared_ptr to_relational( + const Formula& f); + friend std::shared_ptr to_equal_to(const Formula& f); + friend std::shared_ptr to_not_equal_to(const Formula& f); + friend std::shared_ptr to_greater_than(const Formula& f); + friend std::shared_ptr to_greater_than_or_equal_to( + const Formula& f); + friend std::shared_ptr to_less_than(const Formula& f); + friend std::shared_ptr to_less_than_or_equal_to( + const Formula& f); + friend std::shared_ptr to_nary(const Formula& f); + friend std::shared_ptr to_conjunction(const Formula& f); + friend std::shared_ptr to_disjunction(const Formula& f); + friend std::shared_ptr to_negation(const Formula& f); + friend std::shared_ptr to_forall(const Formula& f); + friend std::shared_ptr to_isnan(const Formula& f); + friend std::shared_ptr + to_positive_semidefinite(const Formula& f); + + private: + void HashAppend(DelegatingHasher* hasher) const; + + // Note: We use "const" FormulaCell type here because a FormulaCell object can + // be shared by multiple formulas, a formula should _not_ be able to change + // the cell that it points to. + std::shared_ptr ptr_; +}; + +/** Returns a formula @p f, universally quantified by variables @p vars. */ +Formula forall(const Variables& vars, const Formula& f); + +/** Returns a conjunction of @p formulas. It performs the following + * simplification: + * + * - make_conjunction({}) returns True. + * - make_conjunction({f₁}) returns f₁. + * - If False ∈ @p formulas, it returns False. + * - If True ∈ @p formulas, it will not appear in the return value. + * - Nested conjunctions will be flattened. For example, make_conjunction({f₁, + * f₂ ∧ f₃}) returns f₁ ∧ f₂ ∧ f₃. + */ +Formula make_conjunction(const std::set& formulas); +Formula operator&&(const Formula& f1, const Formula& f2); +Formula operator&&(const Variable& v, const Formula& f); +Formula operator&&(const Formula& f, const Variable& v); +Formula operator&&(const Variable& v1, const Variable& v2); + +/** Returns a disjunction of @p formulas. It performs the following + * simplification: + * + * - make_disjunction({}) returns False. + * - make_disjunction({f₁}) returns f₁. + * - If True ∈ @p formulas, it returns True. + * - If False ∈ @p formulas, it will not appear in the return value. + * - Nested disjunctions will be flattened. For example, make_disjunction({f₁, + * f₂ ∨ f₃}) returns f₁ ∨ f₂ ∨ f₃. + */ +Formula make_disjunction(const std::set& formulas); +Formula operator||(const Formula& f1, const Formula& f2); +Formula operator||(const Variable& v, const Formula& f); +Formula operator||(const Formula& f, const Variable& v); +Formula operator||(const Variable& v1, const Variable& v2); +Formula operator!(const Formula& f); +Formula operator!(const Variable& v); +Formula operator==(const Expression& e1, const Expression& e2); +Formula operator!=(const Expression& e1, const Expression& e2); +Formula operator<(const Expression& e1, const Expression& e2); +Formula operator<=(const Expression& e1, const Expression& e2); +Formula operator>(const Expression& e1, const Expression& e2); +Formula operator>=(const Expression& e1, const Expression& e2); + +/** Returns a Formula for the predicate isnan(e) to the given expression. This + * serves as the argument-dependent lookup related to std::isnan(double). + * + * When this formula is evaluated, there are two possible outcomes: + * - Returns false if the e.Evaluate() is not NaN. + * - Throws std::exception if NaN is detected during evaluation. + * Note that the evaluation of `isnan(e)` never returns true. + */ +Formula isnan(const Expression& e); + +/** Returns a Formula determining if the given expression @p e is a + * positive or negative infinity. + * @throws std::exception if NaN is detected during evaluation. + */ +Formula isinf(const Expression& e); + +/** Returns a Formula determining if the given expression @p e has a finite + * value. + * @throws std::exception if NaN is detected during evaluation. + */ +Formula isfinite(const Expression& e); + +/** Returns a symbolic formula constraining @p m to be a positive-semidefinite + * matrix. By definition, a symmetric matrix @p m is positive-semidefinte if xᵀ + * m x ≥ 0 for all vector x ∈ ℝⁿ. + * + * @throws std::exception if @p m is not symmetric. + * + * @note This method checks if @p m is symmetric, which can be costly. If you + * want to avoid it, please consider using + * `positive_semidefinite(m.triangularView())` or + * `positive_semidefinite(m.triangularView())` instead of + * `positive_semidefinite(m)`. + */ +Formula positive_semidefinite(const Eigen::Ref>& m); + +/** Constructs and returns a symbolic positive-semidefinite formula from @p + * m. If @p mode is Eigen::Lower, it's using the lower-triangular part of @p m + * to construct a positive-semidefinite formula. If @p mode is Eigen::Upper, the + * upper-triangular part of @p m is used. It throws std::exception if @p has + * other values. See the following code snippet. + * + * @code + * Eigen::Matrix m; + * m << 1.0, 2.0, + * 3.0, 4.0; + * + * const Formula psd_l{positive_semidefinite(m, Eigen::Lower)}; + * // psd_l includes [1.0 3.0] + * // [3.0 4.0]. + * + * const Formula psd_u{positive_semidefinite(m, Eigen::Upper)}; + * // psd_u includes [1.0 2.0] + * // [2.0 4.0]. + * @endcode + */ +Formula positive_semidefinite(const MatrixX& m, + Eigen::UpLoType mode); + +/** Constructs and returns a symbolic positive-semidefinite formula from a lower + * triangular-view @p l. See the following code snippet. + * + * @code + * Eigen::Matrix m; + * m << 1.0, 2.0, + * 3.0, 4.0; + * + * Formula psd{positive_semidefinite(m.triangularView())}; + * // psd includes [1.0 3.0] + * // [3.0 4.0]. + * @endcode + */ +template +typename std::enable_if_t< + std::is_same_v::XprKind, + Eigen::MatrixXpr> && + std::is_same_v, + Formula> +positive_semidefinite(const Eigen::TriangularView& l) { + return positive_semidefinite(l, Eigen::Lower); +} + +/** Constructs and returns a symbolic positive-semidefinite formula from an + * upper triangular-view @p u. See the following code snippet. + * + * @code + * Eigen::Matrix m; + * m << 1.0, 2.0, + * 3.0, 4.0; + * + * Formula psd{positive_semidefinite(m.triangularView())}; + * // psd includes [1.0 2.0] + * // [2.0 4.0]. + * @endcode + */ +template +typename std::enable_if_t< + std::is_same_v::XprKind, + Eigen::MatrixXpr> && + std::is_same_v, + Formula> +positive_semidefinite(const Eigen::TriangularView& u) { + return positive_semidefinite(u, Eigen::Upper); +} + +std::ostream& operator<<(std::ostream& os, const Formula& f); + +/** Checks if @p f is structurally equal to False formula. */ +bool is_false(const Formula& f); +/** Checks if @p f is structurally equal to True formula. */ +bool is_true(const Formula& f); +/** Checks if @p f is a variable formula. */ +bool is_variable(const Formula& f); +/** Checks if @p f is a formula representing equality (==). */ +bool is_equal_to(const Formula& f); +/** Checks if @p f is a formula representing disequality (!=). */ +bool is_not_equal_to(const Formula& f); +/** Checks if @p f is a formula representing greater-than (>). */ +bool is_greater_than(const Formula& f); +/** Checks if @p f is a formula representing greater-than-or-equal-to (>=). */ +bool is_greater_than_or_equal_to(const Formula& f); +/** Checks if @p f is a formula representing less-than (<). */ +bool is_less_than(const Formula& f); +/** Checks if @p f is a formula representing less-than-or-equal-to (<=). */ +bool is_less_than_or_equal_to(const Formula& f); +/** Checks if @p f is a relational formula ({==, !=, >, >=, <, <=}). */ +bool is_relational(const Formula& f); +/** Checks if @p f is a conjunction (∧). */ +bool is_conjunction(const Formula& f); +/** Checks if @p f is a disjunction (∨). */ +bool is_disjunction(const Formula& f); +/** Checks if @p f is a n-ary formula ({∧, ∨}). */ +bool is_nary(const Formula& f); +/** Checks if @p f is a negation (¬). */ +bool is_negation(const Formula& f); +/** Checks if @p f is a Forall formula (∀). */ +bool is_forall(const Formula& f); +/** Checks if @p f is an isnan formula. */ +bool is_isnan(const Formula& f); +/** Checks if @p f is a positive-semidefinite formula. */ +bool is_positive_semidefinite(const Formula& f); + +/** Returns the embedded variable in the variable formula @p f. + * @pre @p f is a variable formula. + */ +const Variable& get_variable(const Formula& f); + +/** Returns the lhs-argument of a relational formula @p f. + * \pre{@p f is a relational formula.} + */ +const Expression& get_lhs_expression(const Formula& f); + +/** Returns the rhs-argument of a relational formula @p f. + * \pre{@p f is a relational formula.} + */ +const Expression& get_rhs_expression(const Formula& f); + +/** Returns the set of formulas in a n-ary formula @p f. + * \pre{@p f is a n-ary formula.} + */ +const std::set& get_operands(const Formula& f); + +/** Returns the formula in a negation formula @p f. + * \pre{@p f is a negation formula.} + */ +const Formula& get_operand(const Formula& f); + +/** Returns the quantified variables in a forall formula @p f. + * \pre{@p f is a forall formula.} + */ +const Variables& get_quantified_variables(const Formula& f); + +/** Returns the quantified formula in a forall formula @p f. + * \pre{@p f is a forall formula.} + */ +const Formula& get_quantified_formula(const Formula& f); + +/** Returns the matrix in a positive-semidefinite formula @p f. + * \pre{@p f is a positive-semidefinite formula.} + */ +const MatrixX& get_matrix_in_positive_semidefinite( + const Formula& f); + +namespace internal { +/// Provides a return type of relational operations (=, ≠, ≤, <, ≥, >) between +/// `Eigen::Array`s. +/// +/// @tparam DerivedA A derived type of Eigen::ArrayBase. +/// @tparam DerivedB A derived type of Eigen::ArrayBase. +/// @pre The type of (DerivedA::Scalar() == DerivedB::Scalar()) is symbolic +/// formula. +template < + typename DerivedA, + typename DerivedB, + typename = std::enable_if_t< + std::is_same_v::XprKind, + Eigen::ArrayXpr> && + std::is_same_v::XprKind, + Eigen::ArrayXpr> && + std::is_same_v>> +struct RelationalOpTraits { + using ReturnType = + Eigen::Array::value, + EigenSizeMinPreferFixed::value>; +}; +/// Returns @p f1 ∧ @p f2. +/// Note that this function returns a `Formula` while +/// `std::logical_and{}` returns a bool. +inline Formula logic_and(const Formula& f1, const Formula& f2) { + return f1 && f2; +} + +/// Returns @p f1 ∨ @p f2. +/// Note that this function returns a `Formula` while +/// `std::logical_or{}` returns a bool. +inline Formula logic_or(const Formula& f1, const Formula& f2) { + return f1 || f2; +} +} // namespace internal + +/// Returns an Eigen array of symbolic formulas where each element includes +/// element-wise symbolic-equality of two arrays @p m1 and @p m2. +/// +/// The following table describes the return type of @p m1 == @p m2. +/// +/// LHS \ RHS | EA | EA | EA +/// ----------------|----------------|--------------|-------------- +/// EA | EA | EA | EA +/// EA | EA | EA | EA +/// EA | EA | EA | EA +/// +/// In the table, `EA` is a short-hand of `Eigen::Array`. +/// +/// Note that this function does *not* provide operator overloading for the +/// following case. It returns `Eigen::Array` and is provided by Eigen. +/// +/// - Eigen::Array == Eigen::Array +/// +template +typename std::enable_if_t< + std::is_same_v::XprKind, + Eigen::ArrayXpr> && + std::is_same_v::XprKind, + Eigen::ArrayXpr> && + std::is_same_v, + typename internal::RelationalOpTraits::ReturnType> +operator==(const DerivedA& a1, const DerivedB& a2) { + EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(DerivedA, DerivedB); + DRAKE_DEMAND(a1.rows() == a2.rows() && a1.cols() == a2.cols()); + return a1.binaryExpr(a2, std::equal_to()); +} + +/// Returns an Eigen array of symbolic formulas where each element includes +/// element-wise comparison between an array @p a and a scalar @p v using +/// equal-to operator (==). That is, for all i and j, the (i, j)-th entry of `(a +/// == v)` has a symbolic formula `a(i, j) == v`. +/// +/// Here is an example using this operator overloading. +/// @code +/// Eigen::Array a; +/// a << Variable{"x"}, Variable{"y"}, +/// Variable{"z"}, Variable{"w"}; +/// Eigen::Array f = (a == 3.5); +/// // Here f = |(x == 3.5) (y == 3.5)| +/// // |(z == 3.5) (w == 3.5)|. +/// @endcode +template +typename std::enable_if_t< + std::is_same_v::XprKind, + Eigen::ArrayXpr> && + std::is_same_v, + Eigen::Array> +operator==(const Derived& a, const ScalarType& v) { + return a.unaryExpr( + [&v](const typename Derived::Scalar& x) { return x == v; }); +} + +/// Returns an Eigen array of symbolic formulas where each element includes +/// element-wise comparison between a scalar @p v and an array @p using equal-to +/// operator (==). That is, for all i and j, the (i, j)-th entry of `(v == a)` +/// has a symbolic formula `v == a(i, j)`. +/// +/// Here is an example using this operator overloading. +/// @code +/// Eigen::Array a; +/// a << Variable{"x"}, Variable{"y"}, +/// Variable{"z"}, Variable{"w"}; +/// Eigen::Array f = (3.5 == a); +/// // Here f = |(3.5 == x) (3.5 == y)| +/// // |(3.5 == z) (3.5 == w)|. +/// @endcode +template +typename std::enable_if_t< + std::is_same_v::XprKind, + Eigen::ArrayXpr> && + std::is_same_v, + Eigen::Array> +operator==(const ScalarType& v, const Derived& a) { + return a.unaryExpr( + [&v](const typename Derived::Scalar& x) { return v == x; }); +} + +/// Returns an Eigen array of symbolic formulas where each element includes +/// element-wise comparison of two arrays @p a1 and @p a2 using +/// less-than-or-equal operator (<=). +template +typename std::enable_if_t< + std::is_same_v::XprKind, + Eigen::ArrayXpr> && + std::is_same_v::XprKind, + Eigen::ArrayXpr> && + std::is_same_v, + typename internal::RelationalOpTraits::ReturnType> +operator<=(const DerivedA& a1, const DerivedB& a2) { + EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(DerivedA, DerivedB); + DRAKE_DEMAND(a1.rows() == a2.rows() && a1.cols() == a2.cols()); + return a1.binaryExpr(a2, std::less_equal()); +} + +/// Returns an Eigen array of symbolic formulas where each element includes +/// element-wise comparison between an array @p a and a scalar @p v using +/// less-than-or-equal operator (<=). That is, for all i and j, the (i, j)-th +/// entry of `(a <= v)` has a symbolic formula `a(i, j) <= v`. +template +typename std::enable_if_t< + std::is_same_v::XprKind, + Eigen::ArrayXpr> && + std::is_same_v, + Eigen::Array> +operator<=(const Derived& a, const ScalarType& v) { + return a.unaryExpr( + [&v](const typename Derived::Scalar& x) { return x <= v; }); +} + +/// Returns an Eigen array of symbolic formulas where each element includes +/// element-wise comparison between a scalar @p v and an array @p using +/// less-than-or-equal operator (<=). That is, for all i and j, the (i, j)-th +/// entry of `(v <= a)` has a symbolic formula `v <= a(i, j)`. +template +typename std::enable_if_t< + std::is_same_v::XprKind, + Eigen::ArrayXpr> && + std::is_same_v, + Eigen::Array> +operator<=(const ScalarType& v, const Derived& a) { + return a.unaryExpr( + [&v](const typename Derived::Scalar& x) { return v <= x; }); +} + +/// Returns an Eigen array of symbolic formulas where each element includes +/// element-wise comparison of two arrays @p a1 and @p a2 using less-than +/// operator (<). +template +typename std::enable_if_t< + std::is_same_v::XprKind, + Eigen::ArrayXpr> && + std::is_same_v::XprKind, + Eigen::ArrayXpr> && + std::is_same_v, + typename internal::RelationalOpTraits::ReturnType> +operator<(const DerivedA& a1, const DerivedB& a2) { + EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(DerivedA, DerivedB); + DRAKE_DEMAND(a1.rows() == a2.rows() && a1.cols() == a2.cols()); + return a1.binaryExpr(a2, std::less()); +} + +/// Returns an Eigen array of symbolic formulas where each element includes +/// element-wise comparison between an array @p a and a scalar @p v using +/// less-than operator (<). That is, for all i and j, the (i, j)-th +/// entry of `(a < v)` has a symbolic formula `a(i, j) < v`. +template +typename std::enable_if_t< + std::is_same_v::XprKind, + Eigen::ArrayXpr> && + std::is_same_v, + Eigen::Array> +operator<(const Derived& a, const ScalarType& v) { + return a.unaryExpr([&v](const typename Derived::Scalar& x) { return x < v; }); +} + +/// Returns an Eigen array of symbolic formulas where each element includes +/// element-wise comparison between a scalar @p v and an array @p using +/// less-than operator (<). That is, for all i and j, the (i, j)-th +/// entry of `(v < a)` has a symbolic formula `v < a(i, j)`. +template +typename std::enable_if_t< + std::is_same_v::XprKind, + Eigen::ArrayXpr> && + std::is_same_v, + Eigen::Array> +operator<(const ScalarType& v, const Derived& a) { + return a.unaryExpr([&v](const typename Derived::Scalar& x) { return v < x; }); +} + +/// Returns an Eigen array of symbolic formulas where each element includes +/// element-wise comparison of two arrays @p a1 and @p a2 using +/// greater-than-or-equal operator (>=). +template +typename std::enable_if_t< + std::is_same_v::XprKind, + Eigen::ArrayXpr> && + std::is_same_v::XprKind, + Eigen::ArrayXpr> && + std::is_same_v= + typename DerivedB::Scalar()), + Formula>, + typename internal::RelationalOpTraits::ReturnType> +operator>=(const DerivedA& a1, const DerivedB& a2) { + EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(DerivedA, DerivedB); + DRAKE_DEMAND(a1.rows() == a2.rows() && a1.cols() == a2.cols()); + return a1.binaryExpr(a2, std::greater_equal()); +} + +/// Returns an Eigen array of symbolic formulas where each element includes +/// element-wise comparison between an array @p a and a scalar @p v using +/// greater-than-or-equal operator (>=). That is, for all i and j, the (i, j)-th +/// entry of `(a >= v)` has a symbolic formula `a(i, j) >= v`. +template +typename std::enable_if_t< + std::is_same_v::XprKind, + Eigen::ArrayXpr> && + std::is_same_v= ScalarType()), + Formula>, + Eigen::Array> +operator>=(const Derived& a, const ScalarType& v) { + return a.unaryExpr( + [&v](const typename Derived::Scalar& x) { return x >= v; }); +} + +/// Returns an Eigen array of symbolic formulas where each element includes +/// element-wise comparison between a scalar @p v and an array @p using +/// less-than-or-equal operator (<=) instead of greater-than-or-equal operator +/// (>=). That is, for all i and j, the (i, j)-th entry of `(v >= a)` has a +/// symbolic formula `a(i, j) <= v`. +/// +/// Note that given `v >= a`, this methods returns the result of `a <= v`. First +/// of all, this formulation is mathematically equivalent to the original +/// formulation. We implement this method in this way to be consistent with +/// Eigen's semantics. See the definition of `EIGEN_MAKE_CWISE_COMP_R_OP` in +/// ArrayCwiseBinaryOps.h file in Eigen. +template +typename std::enable_if_t< + std::is_same_v::XprKind, + Eigen::ArrayXpr> && + std::is_same_v= typename Derived::Scalar()), + Formula>, + Eigen::Array> +operator>=(const ScalarType& v, const Derived& a) { + return a <= v; +} + +/// Returns an Eigen array of symbolic formulas where each element includes +/// element-wise comparison of two arrays @p a1 and @p a2 using greater-than +/// operator (>). +template +typename std::enable_if_t< + std::is_same_v::XprKind, + Eigen::ArrayXpr> && + std::is_same_v::XprKind, + Eigen::ArrayXpr> && + std::is_same_v + typename DerivedB::Scalar()), + Formula>, + typename internal::RelationalOpTraits::ReturnType> +operator>(const DerivedA& a1, const DerivedB& a2) { + EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(DerivedA, DerivedB); + DRAKE_DEMAND(a1.rows() == a2.rows() && a1.cols() == a2.cols()); + return a1.binaryExpr(a2, std::greater()); +} + +/// Returns an Eigen array of symbolic formulas where each element includes +/// element-wise comparison between an array @p a and a scalar @p v using +/// greater-than operator (>). That is, for all i and j, the (i, j)-th +/// entry of `(a > v)` has a symbolic formula `a(i, j) > v`. +template +typename std::enable_if_t< + std::is_same_v::XprKind, + Eigen::ArrayXpr> && + std::is_same_v ScalarType()), + Formula>, + Eigen::Array> +operator>(const Derived& a, const ScalarType& v) { + return a.unaryExpr([&v](const typename Derived::Scalar& x) { return x > v; }); +} + +/// Returns an Eigen array of symbolic formulas where each element includes +/// element-wise comparison between a scalar @p v and an array @p using +/// less-than operator (<) instead of greater-than operator (>). That is, for +/// all i and j, the (i, j)-th entry of `(v > a)` has a symbolic formula `a(i, +/// j) < v`. +/// +/// Note that given `v > a`, this methods returns the result of `a < v`. First +/// of all, this formulation is mathematically equivalent to the original +/// formulation. We implement this method in this way to be consistent with +/// Eigen's semantics. See the definition of `EIGEN_MAKE_CWISE_COMP_R_OP` in +/// ArrayCwiseBinaryOps.h file in Eigen. +template +typename std::enable_if_t< + std::is_same_v::XprKind, + Eigen::ArrayXpr> && + std::is_same_v typename Derived::Scalar()), + Formula>, + Eigen::Array> +operator>(const ScalarType& v, const Derived& a) { + return a < v; +} + +/// Returns an Eigen array of symbolic formulas where each element includes +/// element-wise comparison of two arrays @p a1 and @p a2 using not-equal +/// operator (!=). +template +typename std::enable_if_t< + std::is_same_v::XprKind, + Eigen::ArrayXpr> && + std::is_same_v::XprKind, + Eigen::ArrayXpr> && + std::is_same_v, + typename internal::RelationalOpTraits::ReturnType> +operator!=(const DerivedA& a1, const DerivedB& a2) { + EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(DerivedA, DerivedB); + DRAKE_DEMAND(a1.rows() == a2.rows() && a1.cols() == a2.cols()); + return a1.binaryExpr(a2, std::not_equal_to()); +} + +/// Returns an Eigen array of symbolic formulas where each element includes +/// element-wise comparison between an array @p a and a scalar @p v using +/// not-equal operator (!=). That is, for all i and j, the (i, j)-th +/// entry of `(a != v)` has a symbolic formula `a(i, j) != v`. +template +typename std::enable_if_t< + std::is_same_v::XprKind, + Eigen::ArrayXpr> && + std::is_same_v, + Eigen::Array> +operator!=(const Derived& a, const ScalarType& v) { + return a.unaryExpr( + [&v](const typename Derived::Scalar& x) { return x != v; }); +} + +/// Returns an Eigen array of symbolic formulas where each element includes +/// element-wise comparison between a scalar @p v and an array @p using +/// not-equal operator (!=). That is, for all i and j, the (i, j)-th +/// entry of `(v != a)` has a symbolic formula `v != a(i, j)`. +template +typename std::enable_if_t< + std::is_same_v::XprKind, + Eigen::ArrayXpr> && + std::is_same_v, + Eigen::Array> +operator!=(const ScalarType& v, const Derived& a) { + return a.unaryExpr( + [&v](const typename Derived::Scalar& x) { return v != x; }); +} + +/// Returns a symbolic formula checking if two matrices @p m1 and @p m2 are +/// equal. +/// +/// The following table describes the return type of @p m1 == @p m2. +/// +/// LHS \ RHS | EM | EM | EM +/// ----------------|----------------|--------------|------------ +/// EM | Formula | Formula | Formula +/// EM | Formula | Formula | Formula +/// EM | Formula | Formula | bool +/// +/// In the table, `EM` is a short-hand of `Eigen::Matrix`. +/// +/// Note that this function does *not* provide operator overloading for the +/// following case. It returns `bool` and is provided by Eigen. +/// +/// - Eigen::Matrix == Eigen::Matrix +/// +/// Note that this method returns a conjunctive formula which keeps its +/// conjuncts as `std::set` internally. This set is ordered by +/// `Formula::Less` and this ordering can be *different* from the one in +/// inputs. Also, any duplicated formulas are removed in construction. Please +/// check the following example. +/// +/// @code +/// // set up v1 = [y x y] and v2 = [1 2 1] +/// VectorX v1{3}; +/// VectorX v2{3}; +/// const Variable x{"x"}; +/// const Variable y{"y"}; +/// v1 << y, x, y; +/// v2 << 1, 2, 1; +/// // Here v1_eq_v2 = ((x = 2) ∧ (y = 1)) +/// const Formula v1_eq_v2{v1 == v2}; +/// const std::set conjuncts{get_operands(v1_eq_v2)}; +/// for (const Formula& f : conjuncts) { +/// std::cerr << f << std::endl; +/// } +/// // The outcome of the above loop is: +/// (x = 2) +/// (y = 1) +/// @endcode +template +typename std::enable_if_t< + std::is_same_v::XprKind, + Eigen::MatrixXpr> && + std::is_same_v::XprKind, + Eigen::MatrixXpr> && + std::is_same_v, + Formula> +operator==(const DerivedA& m1, const DerivedB& m2) { + EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(DerivedA, DerivedB); + DRAKE_DEMAND(m1.rows() == m2.rows() && m1.cols() == m2.cols()); + return m1.binaryExpr(m2, std::equal_to()).redux(internal::logic_and); +} + +/// Returns a symbolic formula representing the condition whether @p m1 and @p +/// m2 are not the same. +/// +/// The following table describes the return type of @p m1 != @p m2. +/// +/// LHS \ RHS | EM | EM | EM +/// ----------------|----------------|--------------|------------ +/// EM | Formula | Formula | Formula +/// EM | Formula | Formula | Formula +/// EM | Formula | Formula | bool +/// +/// In the table, `EM` is a short-hand of `Eigen::Matrix`. +/// +/// Note that this function does *not* provide operator overloading for the +/// following case. It returns `bool` and is provided by Eigen. +/// +/// - Eigen::Matrix != Eigen::Matrix +template +typename std::enable_if_t< + std::is_same_v::XprKind, + Eigen::MatrixXpr> && + std::is_same_v::XprKind, + Eigen::MatrixXpr> && + std::is_same_v, + Formula> +operator!=(const DerivedA& m1, const DerivedB& m2) { + EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(DerivedA, DerivedB); + DRAKE_DEMAND(m1.rows() == m2.rows() && m1.cols() == m2.cols()); + return m1.binaryExpr(m2, std::not_equal_to()).redux(internal::logic_or); +} + +/// Returns a symbolic formula representing element-wise comparison between two +/// matrices @p m1 and @p m2 using less-than (<) operator. +/// +/// The following table describes the return type of @p m1 < @p m2. +/// +/// LHS \ RHS | EM | EM | EM +/// ----------------|----------------|--------------|------------ +/// EM | Formula | Formula | Formula +/// EM | Formula | Formula | Formula +/// EM | Formula | Formula | N/A +/// +/// In the table, `EM` is a short-hand of `Eigen::Matrix`. +template +typename std::enable_if_t< + std::is_same_v::XprKind, + Eigen::MatrixXpr> && + std::is_same_v::XprKind, + Eigen::MatrixXpr> && + std::is_same_v, + Formula> +operator<(const DerivedA& m1, const DerivedB& m2) { + EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(DerivedA, DerivedB); + DRAKE_DEMAND(m1.rows() == m2.rows() && m1.cols() == m2.cols()); + return m1.binaryExpr(m2, std::less()).redux(internal::logic_and); +} + +/// Returns a symbolic formula representing element-wise comparison between two +/// matrices @p m1 and @p m2 using less-than-or-equal operator (<=). +/// +/// The following table describes the return type of @p m1 <= @p m2. +/// +/// LHS \ RHS | EM | EM | EM +/// ----------------|----------------|--------------|------------ +/// EM | Formula | Formula | Formula +/// EM | Formula | Formula | Formula +/// EM | Formula | Formula | N/A +/// +/// In the table, `EM` is a short-hand of `Eigen::Matrix`. +template +typename std::enable_if_t< + std::is_same_v::XprKind, + Eigen::MatrixXpr> && + std::is_same_v::XprKind, + Eigen::MatrixXpr> && + std::is_same_v, + Formula> +operator<=(const DerivedA& m1, const DerivedB& m2) { + EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(DerivedA, DerivedB); + DRAKE_DEMAND(m1.rows() == m2.rows() && m1.cols() == m2.cols()); + return m1.binaryExpr(m2, std::less_equal()).redux(internal::logic_and); +} + +/// Returns a symbolic formula representing element-wise comparison between two +/// matrices @p m1 and @p m2 using greater-than operator (>). +/// +/// The following table describes the return type of @p m1 > @p m2. +/// +/// LHS \ RHS | EM | EM | EM +/// ----------------|----------------|--------------|------------ +/// EM | Formula | Formula | Formula +/// EM | Formula | Formula | Formula +/// EM | Formula | Formula | N/A +/// +/// In the table, `EM` is a short-hand of `Eigen::Matrix`. +template +typename std::enable_if_t< + std::is_same_v::XprKind, + Eigen::MatrixXpr> && + std::is_same_v::XprKind, + Eigen::MatrixXpr> && + std::is_same_v + typename DerivedB::Scalar()), + Formula>, + Formula> +operator>(const DerivedA& m1, const DerivedB& m2) { + EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(DerivedA, DerivedB); + DRAKE_DEMAND(m1.rows() == m2.rows() && m1.cols() == m2.cols()); + return m1.binaryExpr(m2, std::greater()).redux(internal::logic_and); +} + +/// Returns a symbolic formula representing element-wise comparison between two +/// matrices @p m1 and @p m2 using greater-than-or-equal operator (>=). +/// +/// The following table describes the return type of @p m1 >= @p m2. +/// +/// LHS \ RHS | EM | EM | EM +/// ----------------|----------------|--------------|------------ +/// EM | Formula | Formula | Formula +/// EM | Formula | Formula | Formula +/// EM | Formula | Formula | N/A +/// +/// In the table, `EM` is a short-hand of `Eigen::Matrix`. +template +typename std::enable_if_t< + std::is_same_v::XprKind, + Eigen::MatrixXpr> && + std::is_same_v::XprKind, + Eigen::MatrixXpr> && + std::is_same_v= + typename DerivedB::Scalar()), + Formula>, + Formula> +operator>=(const DerivedA& m1, const DerivedB& m2) { + EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(DerivedA, DerivedB); + DRAKE_DEMAND(m1.rows() == m2.rows() && m1.cols() == m2.cols()); + return m1.binaryExpr(m2, std::greater_equal()) + .redux(internal::logic_and); +} + +} // namespace symbolic + +namespace assert { +/* We allow assertion-like statements to receive a Formula. Given the typical + * uses of assertions and Formulas, rather than trying to be clever and, e.g., + * capture assertion data for later use or partially-solve the Formula to find + * counterexamples, instead we've decided to ignore assertions for the purpose + * of Formula. They are syntax-checked, but always pass. */ +template <> +struct ConditionTraits { + static constexpr bool is_valid = true; + static bool Evaluate(const symbolic::Formula&) { return true; } +}; +} // namespace assert + +} // namespace drake + +namespace std { +/* Provides std::hash. */ +template <> +struct hash : public drake::DefaultHash {}; +#if defined(__GLIBCXX__) +// https://gcc.gnu.org/onlinedocs/libstdc++/manual/unordered_associative.html +template <> +struct __is_fast_hash> : std::false_type {}; +#endif + +/* Provides std::less. */ +template <> +struct less { + bool operator()(const drake::symbolic::Formula& lhs, + const drake::symbolic::Formula& rhs) const { + return lhs.Less(rhs); + } +}; + +/* Provides std::equal_to. */ +template <> +struct equal_to { + bool operator()(const drake::symbolic::Formula& lhs, + const drake::symbolic::Formula& rhs) const { + return lhs.EqualTo(rhs); + } +}; +} // namespace std + +#if !defined(DRAKE_DOXYGEN_CXX) +// Define Eigen traits needed for Matrix. +namespace Eigen { +// Eigen scalar type traits for Matrix. +template <> +struct NumTraits + : GenericNumTraits { + static inline int digits10() { return 0; } +}; + +namespace internal { + +/// Provides specialization for scalar_cmp_op to handle the case "Expr == Expr" +template <> +struct scalar_cmp_op + : binary_op_base { + typedef drake::symbolic::Formula result_type; + EIGEN_EMPTY_STRUCT_CTOR(scalar_cmp_op) + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type + operator()(const drake::symbolic::Expression& a, + const drake::symbolic::Expression& b) const { + return a == b; + } +}; + +/// Provides specialization for scalar_cmp_op to handle the case "Expr < Expr". +template <> +struct scalar_cmp_op + : binary_op_base { + typedef drake::symbolic::Formula result_type; + EIGEN_EMPTY_STRUCT_CTOR(scalar_cmp_op) + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type + operator()(const drake::symbolic::Expression& a, + const drake::symbolic::Expression& b) const { + return a < b; + } +}; + +/// Provides specialization for scalar_cmp_op to handle the case "Expr <= Expr". +template <> +struct scalar_cmp_op + : binary_op_base { + typedef drake::symbolic::Formula result_type; + EIGEN_EMPTY_STRUCT_CTOR(scalar_cmp_op) + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type + operator()(const drake::symbolic::Expression& a, + const drake::symbolic::Expression& b) const { + return a <= b; + } +}; + +/// Provides specialization for scalar_cmp_op to handle the case "Expr > Expr". +template <> +struct scalar_cmp_op + : binary_op_base { + typedef drake::symbolic::Formula result_type; + EIGEN_EMPTY_STRUCT_CTOR(scalar_cmp_op) + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type + operator()(const drake::symbolic::Expression& a, + const drake::symbolic::Expression& b) const { + return a > b; + } +}; + +/// Provides specialization for scalar_cmp_op to handle the case "Expr >= Expr". +template <> +struct scalar_cmp_op + : binary_op_base { + typedef drake::symbolic::Formula result_type; + EIGEN_EMPTY_STRUCT_CTOR(scalar_cmp_op) + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type + operator()(const drake::symbolic::Expression& a, + const drake::symbolic::Expression& b) const { + return a >= b; + } +}; + +/// Provides specialization for scalar_cmp_op to handle the case "Expr != Expr". +template <> +struct scalar_cmp_op + : binary_op_base { + typedef drake::symbolic::Formula result_type; + EIGEN_EMPTY_STRUCT_CTOR(scalar_cmp_op) + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type + operator()(const drake::symbolic::Expression& a, + const drake::symbolic::Expression& b) const { + return a != b; + } +}; + +/// Provides specialization for scalar_cmp_op to handle the case "Var == Var". +template <> +struct scalar_cmp_op + : binary_op_base { + typedef drake::symbolic::Formula result_type; + EIGEN_EMPTY_STRUCT_CTOR(scalar_cmp_op) + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type + operator()(const drake::symbolic::Variable& a, + const drake::symbolic::Variable& b) const { + return a == b; + } +}; + +/// Provides specialization for scalar_cmp_op to handle the case "Var < Var". +template <> +struct scalar_cmp_op + : binary_op_base { + typedef drake::symbolic::Formula result_type; + EIGEN_EMPTY_STRUCT_CTOR(scalar_cmp_op) + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type + operator()(const drake::symbolic::Variable& a, + const drake::symbolic::Variable& b) const { + return a < b; + } +}; + +/// Provides specialization for scalar_cmp_op to handle the case "Var <= Var". +template <> +struct scalar_cmp_op + : binary_op_base { + typedef drake::symbolic::Formula result_type; + EIGEN_EMPTY_STRUCT_CTOR(scalar_cmp_op) + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type + operator()(const drake::symbolic::Variable& a, + const drake::symbolic::Variable& b) const { + return a <= b; + } +}; + +/// Provides specialization for scalar_cmp_op to handle the case "Var > Var". +template <> +struct scalar_cmp_op + : binary_op_base { + typedef drake::symbolic::Formula result_type; + EIGEN_EMPTY_STRUCT_CTOR(scalar_cmp_op) + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type + operator()(const drake::symbolic::Variable& a, + const drake::symbolic::Variable& b) const { + return a > b; + } +}; + +/// Provides specialization for scalar_cmp_op to handle the case "Var >= Var". +template <> +struct scalar_cmp_op + : binary_op_base { + typedef drake::symbolic::Formula result_type; + EIGEN_EMPTY_STRUCT_CTOR(scalar_cmp_op) + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type + operator()(const drake::symbolic::Variable& a, + const drake::symbolic::Variable& b) const { + return a >= b; + } +}; + +/// Provides specialization for scalar_cmp_op to handle the case "Var != Var". +template <> +struct scalar_cmp_op + : binary_op_base { + typedef drake::symbolic::Formula result_type; + EIGEN_EMPTY_STRUCT_CTOR(scalar_cmp_op) + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type + operator()(const drake::symbolic::Variable& a, + const drake::symbolic::Variable& b) const { + return a != b; + } +}; + +} // namespace internal + +namespace numext { + +// Provides specializations for equal_strict and not_equal_strict with +// Expression. As of Eigen 3.4.0, these are called at least as part of +// triangular vector solve (though they could also potentially come up +// elsewhere). The default template relies on an implicit conversion to +// bool but our bool operator is explicit, so we need to specialize. +// +// Furthermore, various Eigen algorithms will use "short-circuit when zero" +// guards as an optimization to skip expensive computation if it can show +// that the end result will remain unchanged. If our Expression has any +// unbound variables during that guard, we will throw instead of skipping +// the optimizaton. Therefore, we tweak these guards to special-case the +// result when either of the operands is a literal zero, with no throwing +// even if the other operand has unbound variables. +// +// These functions were only added in Eigen 3.3.5, but the minimum +// Eigen version used by drake is 3.3.4, so a version check is needed. +#if EIGEN_VERSION_AT_LEAST(3, 3, 5) +template <> +EIGEN_STRONG_INLINE bool equal_strict( + const drake::symbolic::Expression& x, + const drake::symbolic::Expression& y) { + if (is_zero(x)) { return is_zero(y); } + if (is_zero(y)) { return is_zero(x); } + return static_cast(x == y); +} +template <> +EIGEN_STRONG_INLINE bool not_equal_strict( + const drake::symbolic::Expression& x, + const drake::symbolic::Expression& y) { + return !Eigen::numext::equal_strict(x, y); +} +#endif + +// Provides specialization of Eigen::numext::isfinite, numext::isnan, and +// numext::isinf for Expression. The default template relies on an implicit +// conversion to bool but our bool operator is explicit, so we need to +// specialize. +// As of Eigen 3.4.0, this is called at least as part of JacobiSVD (though +// it could also potentially come up elsewhere). +template <> +EIGEN_STRONG_INLINE bool isfinite(const drake::symbolic::Expression& e) { + return static_cast(drake::symbolic::isfinite(e)); +} + +template <> +EIGEN_STRONG_INLINE bool isinf(const drake::symbolic::Expression& e) { + return static_cast(drake::symbolic::isinf(e)); +} + +template <> +EIGEN_STRONG_INLINE bool isnan(const drake::symbolic::Expression& e) { + return static_cast(drake::symbolic::isnan(e)); +} +} // namespace numext +} // namespace Eigen +#endif // !defined(DRAKE_DOXYGEN_CXX) diff --git a/maliput_drake/include/drake/common/symbolic_formula_cell.h b/maliput_drake/include/drake/common/symbolic_formula_cell.h new file mode 100644 index 0000000..9854412 --- /dev/null +++ b/maliput_drake/include/drake/common/symbolic_formula_cell.h @@ -0,0 +1,572 @@ +#pragma once +/// @file +/// +/// Provides implementation-details of symbolic formulas. +/// +/// It is strongly discouraged to include and use this header file outside of +/// drake/common/symbolic_* files. To include this file, you need to define +/// `DRAKE_COMMON_SYMBOLIC_DETAIL_HEADER` before. Without it, you have +/// compile-time errors. +#ifndef DRAKE_COMMON_SYMBOLIC_DETAIL_HEADER +#error Do not include this file unless you implement symbolic libraries. +#endif + +#include +#include +#include +#include + +#include "drake/common/drake_assert.h" +#include "drake/common/hash.h" +#include "drake/common/random.h" +#include "drake/common/symbolic.h" + +namespace drake { +namespace symbolic { + +/** Represents an abstract class which is the base of concrete symbolic-formula + * classes (i.e. symbolic::FormulaAnd, symbolic::FormulaEq). + * + * \note It provides virtual function, FormulaCell::Display, + * because operator<< is not allowed to be a virtual function. + */ +class FormulaCell { + public: + /** Returns kind of formula. */ + [[nodiscard]] FormulaKind get_kind() const { return kind_; } + /** Sends all hash-relevant bytes for this FormulaCell type into the given + * hasher, per the @ref hash_append concept -- except for get_kind(), because + * Formula already sends that. + */ + virtual void HashAppendDetail(DelegatingHasher*) const = 0; + /** Returns set of free variables in formula. */ + [[nodiscard]] virtual Variables GetFreeVariables() const = 0; + /** Checks structural equality. */ + [[nodiscard]] virtual bool EqualTo(const FormulaCell& c) const = 0; + /** Checks ordering. */ + [[nodiscard]] virtual bool Less(const FormulaCell& c) const = 0; + /** Evaluates under a given environment. */ + [[nodiscard]] virtual bool Evaluate(const Environment& env) const = 0; + /** Returns a Formula obtained by replacing all occurrences of the + * variables in @p s in the current formula cell with the corresponding + * expressions in @p s. + */ + [[nodiscard]] virtual Formula Substitute(const Substitution& s) const = 0; + /** Outputs string representation of formula into output stream @p os. */ + virtual std::ostream& Display(std::ostream& os) const = 0; + + /** Default constructor (deleted). */ + FormulaCell() = delete; + + /** Move-assign (deleted). */ + FormulaCell& operator=(FormulaCell&& f) = delete; + + /** Copy-assign (deleted). */ + FormulaCell& operator=(const FormulaCell& f) = delete; + + protected: + /** Move-construct a formula from an rvalue. */ + FormulaCell(FormulaCell&& f) = default; + /** Copy-construct a formula from an lvalue. */ + FormulaCell(const FormulaCell& f) = default; + /** Construct FormulaCell of kind @p k. */ + explicit FormulaCell(FormulaKind k); + /** Default destructor. */ + virtual ~FormulaCell() = default; + + private: + const FormulaKind kind_{}; +}; + +/** Represents the base class for relational operators (==, !=, <, <=, >, >=). + */ +class RelationalFormulaCell : public FormulaCell { + public: + /** Default constructor (deleted). */ + RelationalFormulaCell() = delete; + /** Move-construct a formula from an rvalue. */ + RelationalFormulaCell(RelationalFormulaCell&& f) = default; + /** Copy-construct a formula from an lvalue. */ + RelationalFormulaCell(const RelationalFormulaCell& f) = default; + /** Move-assign (DELETED). */ + RelationalFormulaCell& operator=(RelationalFormulaCell&& f) = delete; + /** Copy-assign (DELETED). */ + RelationalFormulaCell& operator=(const RelationalFormulaCell& f) = delete; + /** Construct RelationalFormulaCell of kind @p k with @p lhs and @p rhs. */ + RelationalFormulaCell(FormulaKind k, Expression lhs, Expression rhs); + void HashAppendDetail(DelegatingHasher*) const override; + [[nodiscard]] Variables GetFreeVariables() const override; + [[nodiscard]] bool EqualTo(const FormulaCell& f) const override; + [[nodiscard]] bool Less(const FormulaCell& f) const override; + + /** Returns the expression on left-hand-side. */ + [[nodiscard]] const Expression& get_lhs_expression() const { return e_lhs_; } + /** Returns the expression on right-hand-side. */ + [[nodiscard]] const Expression& get_rhs_expression() const { return e_rhs_; } + + private: + const Expression e_lhs_; + const Expression e_rhs_; +}; + +/** Represents the base class for N-ary logic operators (∧ and ∨). + * + * @note Internally this class maintains a set of symbolic formulas to avoid + * duplicated elements (i.e. f1 ∧ ... ∧ f1). + */ +class NaryFormulaCell : public FormulaCell { + public: + /** Default constructor (deleted). */ + NaryFormulaCell() = delete; + /** Move-construct a formula from an rvalue. */ + NaryFormulaCell(NaryFormulaCell&& f) = default; + /** Copy-construct a formula from an lvalue. */ + NaryFormulaCell(const NaryFormulaCell& f) = default; + /** Move-assign (DELETED). */ + NaryFormulaCell& operator=(NaryFormulaCell&& f) = delete; + /** Copy-assign (DELETED). */ + NaryFormulaCell& operator=(const NaryFormulaCell& f) = delete; + /** Construct NaryFormulaCell of kind @p k with @p formulas. */ + NaryFormulaCell(FormulaKind k, std::set formulas); + void HashAppendDetail(DelegatingHasher*) const override; + [[nodiscard]] Variables GetFreeVariables() const override; + [[nodiscard]] bool EqualTo(const FormulaCell& f) const override; + [[nodiscard]] bool Less(const FormulaCell& f) const override; + /** Returns the formulas. */ + [[nodiscard]] const std::set& get_operands() const { + return formulas_; + } + + protected: + std::ostream& DisplayWithOp(std::ostream& os, const std::string& op) const; + + private: + const std::set formulas_; +}; + +/** Symbolic formula representing true. */ +class FormulaTrue : public FormulaCell { + public: + /** Default Constructor. */ + FormulaTrue(); + void HashAppendDetail(DelegatingHasher*) const override; + [[nodiscard]] Variables GetFreeVariables() const override; + [[nodiscard]] bool EqualTo(const FormulaCell& f) const override; + [[nodiscard]] bool Less(const FormulaCell& f) const override; + [[nodiscard]] bool Evaluate(const Environment& env) const override; + [[nodiscard]] Formula Substitute(const Substitution& s) const override; + std::ostream& Display(std::ostream& os) const override; +}; + +/** Symbolic formula representing false. */ +class FormulaFalse : public FormulaCell { + public: + /** Default Constructor. */ + FormulaFalse(); + void HashAppendDetail(DelegatingHasher*) const override; + [[nodiscard]] Variables GetFreeVariables() const override; + [[nodiscard]] bool EqualTo(const FormulaCell& f) const override; + [[nodiscard]] bool Less(const FormulaCell& f) const override; + [[nodiscard]] bool Evaluate(const Environment& env) const override; + [[nodiscard]] Formula Substitute(const Substitution& s) const override; + std::ostream& Display(std::ostream& os) const override; +}; + +/** Symbolic formula representing a Boolean variable. */ +class FormulaVar : public FormulaCell { + public: + /** Constructs a formula from @p var. + * @pre @p var is of BOOLEAN type and not a dummy variable. + */ + explicit FormulaVar(Variable v); + void HashAppendDetail(DelegatingHasher*) const override; + [[nodiscard]] Variables GetFreeVariables() const override; + [[nodiscard]] bool EqualTo(const FormulaCell& f) const override; + [[nodiscard]] bool Less(const FormulaCell& f) const override; + [[nodiscard]] bool Evaluate(const Environment& env) const override; + [[nodiscard]] Formula Substitute(const Substitution& subst) const override; + std::ostream& Display(std::ostream& os) const override; + [[nodiscard]] const Variable& get_variable() const; + + private: + const Variable var_; +}; + +/** Symbolic formula representing equality (e1 = e2). */ +class FormulaEq : public RelationalFormulaCell { + public: + /** Constructs from @p e1 and @p e2. */ + FormulaEq(const Expression& e1, const Expression& e2); + [[nodiscard]] bool Evaluate(const Environment& env) const override; + [[nodiscard]] Formula Substitute(const Substitution& s) const override; + std::ostream& Display(std::ostream& os) const override; +}; + +/** Symbolic formula representing disequality (e1 ≠ e2). */ +class FormulaNeq : public RelationalFormulaCell { + public: + /** Constructs from @p e1 and @p e2. */ + FormulaNeq(const Expression& e1, const Expression& e2); + [[nodiscard]] bool Evaluate(const Environment& env) const override; + [[nodiscard]] Formula Substitute(const Substitution& s) const override; + std::ostream& Display(std::ostream& os) const override; +}; + +/** Symbolic formula representing 'greater-than' (e1 > e2). */ +class FormulaGt : public RelationalFormulaCell { + public: + /** Constructs from @p e1 and @p e2. */ + FormulaGt(const Expression& e1, const Expression& e2); + [[nodiscard]] bool Evaluate(const Environment& env) const override; + [[nodiscard]] Formula Substitute(const Substitution& s) const override; + std::ostream& Display(std::ostream& os) const override; +}; + +/** Symbolic formula representing 'greater-than-or-equal-to' (e1 ≥ e2). */ +class FormulaGeq : public RelationalFormulaCell { + public: + /** Constructs from @p e1 and @p e2. */ + FormulaGeq(const Expression& e1, const Expression& e2); + [[nodiscard]] bool Evaluate(const Environment& env) const override; + [[nodiscard]] Formula Substitute(const Substitution& s) const override; + std::ostream& Display(std::ostream& os) const override; +}; + +/** Symbolic formula representing 'less-than' (e1 < e2). */ +class FormulaLt : public RelationalFormulaCell { + public: + /** Constructs from @p e1 and @p e2. */ + FormulaLt(const Expression& e1, const Expression& e2); + [[nodiscard]] bool Evaluate(const Environment& env) const override; + [[nodiscard]] Formula Substitute(const Substitution& s) const override; + std::ostream& Display(std::ostream& os) const override; +}; + +/** Symbolic formula representing 'less-than-or-equal-to' (e1 ≤ e2). */ +class FormulaLeq : public RelationalFormulaCell { + public: + /** Constructs from @p e1 and @p e2. */ + FormulaLeq(const Expression& e1, const Expression& e2); + [[nodiscard]] bool Evaluate(const Environment& env) const override; + [[nodiscard]] Formula Substitute(const Substitution& s) const override; + std::ostream& Display(std::ostream& os) const override; +}; + +/** Symbolic formula representing conjunctions (f1 ∧ ... ∧ fn). */ +class FormulaAnd : public NaryFormulaCell { + public: + /** Constructs from @p formulas. */ + explicit FormulaAnd(const std::set& formulas); + /** Constructs @p f1 ∧ @p f2. */ + FormulaAnd(const Formula& f1, const Formula& f2); + [[nodiscard]] bool Evaluate(const Environment& env) const override; + [[nodiscard]] Formula Substitute(const Substitution& s) const override; + std::ostream& Display(std::ostream& os) const override; +}; + +/** Symbolic formula representing disjunctions (f1 ∨ ... ∨ fn). */ +class FormulaOr : public NaryFormulaCell { + public: + /** Constructs from @p formulas. */ + explicit FormulaOr(const std::set& formulas); + /** Constructs @p f1 ∨ @p f2. */ + FormulaOr(const Formula& f1, const Formula& f2); + [[nodiscard]] bool Evaluate(const Environment& env) const override; + [[nodiscard]] Formula Substitute(const Substitution& s) const override; + std::ostream& Display(std::ostream& os) const override; +}; + +/** Symbolic formula representing negations (¬f). */ +class FormulaNot : public FormulaCell { + public: + /** Constructs from @p f. */ + explicit FormulaNot(Formula f); + void HashAppendDetail(DelegatingHasher*) const override; + [[nodiscard]] Variables GetFreeVariables() const override; + [[nodiscard]] bool EqualTo(const FormulaCell& f) const override; + [[nodiscard]] bool Less(const FormulaCell& f) const override; + [[nodiscard]] bool Evaluate(const Environment& env) const override; + [[nodiscard]] Formula Substitute(const Substitution& s) const override; + std::ostream& Display(std::ostream& os) const override; + /** Returns the operand. */ + [[nodiscard]] const Formula& get_operand() const { return f_; } + + private: + const Formula f_; +}; + +/** Symbolic formula representing universal quantifications + * (∀ x₁, ..., * xn. F). + */ +class FormulaForall : public FormulaCell { + public: + /** Constructs from @p vars and @p f. */ + FormulaForall(Variables vars, Formula f); + void HashAppendDetail(DelegatingHasher*) const override; + [[nodiscard]] Variables GetFreeVariables() const override; + [[nodiscard]] bool EqualTo(const FormulaCell& f) const override; + [[nodiscard]] bool Less(const FormulaCell& f) const override; + [[nodiscard]] bool Evaluate(const Environment& env) const override; + [[nodiscard]] Formula Substitute(const Substitution& s) const override; + std::ostream& Display(std::ostream& os) const override; + /** Returns the quantified variables. */ + [[nodiscard]] const Variables& get_quantified_variables() const { + return vars_; + } + /** Returns the quantified formula. */ + [[nodiscard]] const Formula& get_quantified_formula() const { return f_; } + + private: + const Variables vars_; // Quantified variables. + const Formula f_; // Quantified formula. +}; + +/** Symbolic formula representing isnan predicate. */ +class FormulaIsnan : public FormulaCell { + public: + explicit FormulaIsnan(Expression e); + void HashAppendDetail(DelegatingHasher*) const override; + [[nodiscard]] Variables GetFreeVariables() const override; + [[nodiscard]] bool EqualTo(const FormulaCell& f) const override; + [[nodiscard]] bool Less(const FormulaCell& f) const override; + [[nodiscard]] bool Evaluate(const Environment& env) const override; + [[nodiscard]] Formula Substitute(const Substitution& s) const override; + std::ostream& Display(std::ostream& os) const override; + + private: + const Expression e_; +}; + +/** Symbolic formula representing positive-semidefinite (PSD) constraint. */ +class FormulaPositiveSemidefinite : public FormulaCell { + public: + /** Constructs a positive-semidefinite formula from a symmetric matrix @p m. + * + * @throws std::exception if @p m is not symmetric. + * + * @note This constructor checks if @p m is symmetric, which can be costly. + */ + explicit FormulaPositiveSemidefinite( + const Eigen::Ref>& m); + + /** Constructs a symbolic positive-semidefinite formula from a + * lower triangular-view @p l. + */ + template + explicit FormulaPositiveSemidefinite( + const Eigen::TriangularView& l) + : FormulaCell{FormulaKind::PositiveSemidefinite}, + m_{BuildSymmetricMatrixFromLowerTringularView(l)} {} + + /** Constructs a symbolic positive-semidefinite formula from an + * upper triangular-view @p u. + */ + template + explicit FormulaPositiveSemidefinite( + const Eigen::TriangularView& u) + : FormulaCell{FormulaKind::PositiveSemidefinite}, + m_{BuildSymmetricMatrixFromUpperTriangularView(u)} {} + + void HashAppendDetail(DelegatingHasher*) const override; + [[nodiscard]] Variables GetFreeVariables() const override; + [[nodiscard]] bool EqualTo(const FormulaCell& f) const override; + /** Checks ordering between this PSD formula and @p f. The ordering between + * two PSD formulas `psd1` and `psd2` are determined by the ordering between + * the two matrices `m1` in `psd1` and `m2` in `psd2`. + * + * First, we compare the size of `m1` and `m2`: + * + * - If `m1` is smaller than `m2`, `psd1.Less(psd2)` is true. + * - If `m2` is smaller than `m1`, `psd1.Less(psd2)` is false. + * + * If `m1` and `m2` are of the same size, we perform element-wise comparison + * by following column-major order. See the following example: + * + * @code + * m1 << x + y, -3.14, + * -3.14, y; + * m2 << x + y, 3.14, + * 3.14, y; + * const Formula psd1{positive_semidefinite(m1)}; + * const Formula psd2{positive_semidefinite(m2)}; + * + * EXPECT_TRUE(psd1.Less(psd2)); + * @endcode + * + * @note In the code above, `psd1.Less(psd2)` holds because we have + * + * - m1 in column-major ordering : (x + y) -3.14 -3.14 y_ + * - m2 in column-major ordering : (x + y) 3.14 3.14 y_. + */ + [[nodiscard]] bool Less(const FormulaCell& f) const override; + [[nodiscard]] bool Evaluate(const Environment& env) const override; + [[nodiscard]] Formula Substitute(const Substitution& s) const override; + std::ostream& Display(std::ostream& os) const override; + /** Returns the corresponding matrix in this PSD formula. */ + [[nodiscard]] const MatrixX& get_matrix() const { + return m_; + } + + private: + // Builds a symmetric matrix from a lower triangular-view. + template + static MatrixX BuildSymmetricMatrixFromLowerTringularView( + const Eigen::TriangularView& l) { + MatrixX m(l.rows(), l.cols()); + m.triangularView() = l; + m.triangularView() = + m.transpose().triangularView(); + return m; + } + + // Builds a symmetric matrix from an upper triangular-view. + template + static MatrixX BuildSymmetricMatrixFromUpperTriangularView( + const Eigen::TriangularView& u) { + MatrixX m(u.rows(), u.cols()); + m.triangularView() = u; + m.triangularView() = + m.transpose().triangularView(); + return m; + } + + const MatrixX m_; +}; + +/** Checks if @p f is structurally equal to False formula. */ +bool is_false(const FormulaCell& f); +/** Checks if @p f is structurally equal to True formula. */ +bool is_true(const FormulaCell& f); +/** Checks if @p f is a variable formula. */ +bool is_variable(const FormulaCell& f); +/** Checks if @p f is a formula representing equality (==). */ +bool is_equal_to(const FormulaCell& f); +/** Checks if @p f is a formula representing disequality (!=). */ +bool is_not_equal_to(const FormulaCell& f); +/** Checks if @p f is a formula representing greater-than (>). */ +bool is_greater_than(const FormulaCell& f); +/** Checks if @p f is a formula representing greater-than-or-equal-to (>=). */ +bool is_greater_than_or_equal_to(const FormulaCell& f); +/** Checks if @p f is a formula representing less-than (<). */ +bool is_less_than(const FormulaCell& f); +/** Checks if @p f is a formula representing less-than-or-equal-to (<=). */ +bool is_less_than_or_equal_to(const FormulaCell& f); +/** Checks if @p f is a relational formula ({==, !=, >, >=, <, <=}). */ +bool is_relational(const FormulaCell& f); +/** Checks if @p f is a conjunction (∧). */ +bool is_conjunction(const FormulaCell& f); +/** Checks if @p f is a disjunction (∨). */ +bool is_disjunction(const FormulaCell& f); +/** Checks if @p f is a negation (¬). */ +bool is_negation(const FormulaCell& f); +/** Checks if @p f is a Forall formula (∀). */ +bool is_forall(const FormulaCell& f); +/** Checks if @p f is an isnan formula. */ +bool is_isnan(const FormulaCell& f); +/** Checks if @p f is a positive semidefinite formula. */ +bool is_positive_semidefinite(const FormulaCell& f); + +/** Casts @p f_ptr to @c shared_ptr. + * @pre @c is_false(*f_ptr) is true. + */ +std::shared_ptr to_false( + const std::shared_ptr& f_ptr); + +/** Casts @p f_ptr to @c shared_ptr. + * @pre @c is_true(*f_ptr) is true. + */ +std::shared_ptr to_true( + const std::shared_ptr& f_ptr); + +/** Casts @p f_ptr to @c shared_ptr. + * @pre @c is_variable(*f_ptr) is true. + */ +std::shared_ptr to_variable( + const std::shared_ptr& f_ptr); + +/** Casts @p f_ptr to @c shared_ptr. + * @pre @c is_relational(*f_ptr) is true. + */ +std::shared_ptr to_relational( + const std::shared_ptr& f_ptr); + +/** Casts @p f_ptr to @c shared_ptr. + * @pre @c is_equal_to(*f_ptr) is true. + */ +std::shared_ptr to_equal_to( + const std::shared_ptr& f_ptr); + +/** Casts @p f_ptr to @c shared_ptr. + * @pre @c is_not_equal_to(*f_ptr) is true. + */ +std::shared_ptr to_not_equal_to( + const std::shared_ptr& f_ptr); + +/** Casts @p f_ptr to @c shared_ptr. + * @pre @c is_greater_than(*f_ptr) is true. + */ +std::shared_ptr to_greater_than( + const std::shared_ptr& f_ptr); + +/** Casts @p f_ptr to @c shared_ptr. + * @pre @c is_greater_than_or_equal_to(*f_ptr) is true. + */ +std::shared_ptr to_greater_than_or_equal_to( + const std::shared_ptr& f_ptr); + +/** Casts @p f_ptr to @c shared_ptr. + * @pre @c is_less_than(*f_ptr) is true. + */ +std::shared_ptr to_less_than( + const std::shared_ptr& f_ptr); + +/** Casts @p f_ptr to @c shared_ptr. + * @pre @c is_less_than_or_equal_to(*f_ptr) is true. + */ +std::shared_ptr to_less_than_or_equal_to( + const std::shared_ptr& f_ptr); + +/** Casts @p f_ptr to @c shared_ptr. + * @pre @c is_conjunction(*f_ptr) is true. + */ +std::shared_ptr to_conjunction( + const std::shared_ptr& f_ptr); + +/** Casts @p f_ptr to @c shared_ptr. + * @pre @c is_disjunction(*f_ptr) is true. + */ +std::shared_ptr to_disjunction( + const std::shared_ptr& f_ptr); + +/** Casts @p f_ptr to @c shared_ptr. + * @pre @c is_nary(*f_ptr) is true. + */ +std::shared_ptr to_nary( + const std::shared_ptr& f_ptr); + +/** Casts @p f_ptr to @c shared_ptr. + * @pre @c is_negation(*f_ptr) is true. + */ +std::shared_ptr to_negation( + const std::shared_ptr& f_ptr); + +/** Casts @p f_ptr to @c shared_ptr. + * @pre @c is_forall(*f_ptr) is true. + */ +std::shared_ptr to_forall( + const std::shared_ptr& f_ptr); + +/** Casts @p f_ptr to @c shared_ptr. + * @pre @c is_isnan(*f_ptr) is true. + */ +std::shared_ptr to_isnan( + const std::shared_ptr& f_ptr); + +/** Casts @p f_ptr to @c shared_ptr. + * @pre @c is_positive_semidefinite(*f_ptr) is true. + */ +std::shared_ptr to_positive_semidefinite( + const std::shared_ptr& f_ptr); + +} // namespace symbolic +} // namespace drake diff --git a/maliput_drake/include/drake/common/symbolic_formula_visitor.h b/maliput_drake/include/drake/common/symbolic_formula_visitor.h new file mode 100644 index 0000000..8f869dd --- /dev/null +++ b/maliput_drake/include/drake/common/symbolic_formula_visitor.h @@ -0,0 +1,63 @@ +#pragma once + +#ifndef DRAKE_COMMON_SYMBOLIC_HEADER +#error Do not directly include this file. Include "drake/common/symbolic.h". +#endif + +#include + +#include "drake/common/drake_assert.h" +#include "drake/common/symbolic.h" + +namespace drake { +namespace symbolic { + +/// Calls visitor object @p v with a symbolic formula @p f, and arguments @p +/// args. Visitor object is expected to implement the following methods which +/// take @p f and @p args: `VisitFalse`, `VisitTrue`, `VisitVariable`, +/// `VisitEqualTo`, VisitNotEqualTo, VisitGreaterThan, +/// `VisitGreaterThanOrEqualTo`, `VisitLessThan`, `VisitLessThanOrEqualTo`, +/// `VisitConjunction`, `VisitDisjunction`, `VisitNegation`, `VisitForall`, +/// `VisitIsnan`, `VisitPositiveSemidefinite`. +/// +/// Check the implementation of @c NegationNormalFormConverter class in +/// drake/common/test/symbolic_formula_visitor_test.cc file to find an example. +template +Result VisitFormula(Visitor* v, const Formula& f, Args&&... args) { + switch (f.get_kind()) { + case FormulaKind::False: + return v->VisitFalse(f, std::forward(args)...); + case FormulaKind::True: + return v->VisitTrue(f, std::forward(args)...); + case FormulaKind::Var: + return v->VisitVariable(f, std::forward(args)...); + case FormulaKind::Eq: + return v->VisitEqualTo(f, std::forward(args)...); + case FormulaKind::Neq: + return v->VisitNotEqualTo(f, std::forward(args)...); + case FormulaKind::Gt: + return v->VisitGreaterThan(f, std::forward(args)...); + case FormulaKind::Geq: + return v->VisitGreaterThanOrEqualTo(f, std::forward(args)...); + case FormulaKind::Lt: + return v->VisitLessThan(f, std::forward(args)...); + case FormulaKind::Leq: + return v->VisitLessThanOrEqualTo(f, std::forward(args)...); + case FormulaKind::And: + return v->VisitConjunction(f, std::forward(args)...); + case FormulaKind::Or: + return v->VisitDisjunction(f, std::forward(args)...); + case FormulaKind::Not: + return v->VisitNegation(f, std::forward(args)...); + case FormulaKind::Forall: + return v->VisitForall(f, std::forward(args)...); + case FormulaKind::Isnan: + return v->VisitIsnan(f, std::forward(args)...); + case FormulaKind::PositiveSemidefinite: + return v->VisitPositiveSemidefinite(f, std::forward(args)...); + } + DRAKE_UNREACHABLE(); +} + +} // namespace symbolic +} // namespace drake diff --git a/maliput_drake/include/drake/common/symbolic_generic_polynomial.h b/maliput_drake/include/drake/common/symbolic_generic_polynomial.h new file mode 100644 index 0000000..b2a7495 --- /dev/null +++ b/maliput_drake/include/drake/common/symbolic_generic_polynomial.h @@ -0,0 +1,561 @@ +#pragma once + +#ifndef DRAKE_COMMON_SYMBOLIC_HEADER +#error Do not directly include this file. Include "drake/common/symbolic.h". +#endif + +#include +#include + +#include +#include + +#include "drake/common/drake_copyable.h" +#include "drake/common/symbolic.h" + +namespace drake { +namespace symbolic { +/** + * Represents symbolic generic polynomials using a given basis (for example, + * monomial basis, Chebyshev basis, etc). A generic symbolic polynomial keeps a + * mapping from a basis element of indeterminates to its coefficient in a + * symbolic expression. A generic polynomial `p` has to satisfy an invariant + * such that `p.decision_variables() ∩ p.indeterminates() = ∅`. We have + * CheckInvariant() method to check the invariant. + * For polynomials using different basis, you could refer to section 3.1.5 of + * Semidefinite Optimization and Convex Algebraic Geometry on the pros/cons of + * each basis. + * + * We provide two instantiations of this template + * - BasisElement = MonomialBasisElement + * - BasisElement = ChebyshevBasisElement + * @tparam BasisElement Must be a subclass of PolynomialBasisElement. + */ +template +class GenericPolynomial { + public: + static_assert( + std::is_base_of_v, + "BasisElement should be a derived class of PolynomialBasisElement"); + /** Type of mapping from basis element to coefficient */ + using MapType = std::map; + + /** Constructs a zero polynomial. */ + GenericPolynomial() = default; + + DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(GenericPolynomial) + + /** Constructs a default value. This overload is used by Eigen when + * EIGEN_INITIALIZE_MATRICES_BY_ZERO is enabled. + */ + explicit GenericPolynomial(std::nullptr_t) + : GenericPolynomial() {} + + /** Constructs a generic polynomial from a map, basis_element → coefficient. + * For example + * @code{cc} + * GenericPolynomial( + * {{MonomialBasisElement(x, 2), a}, {MonomialBasisElement(x, 3), a+b}}) + * @endcode + * constructs a polynomial ax²+(a+b)x³.*/ + explicit GenericPolynomial(MapType init); + + /** Constructs a generic polynomial from a single basis element @p m. + * @note that all variables in `m` are considered as indeterminates. Namely + * the constructed generic polynomial contains the map with a single key `m`, + * with the coefficient being 1. + */ + // Note that this implicit conversion is desirable to have a dot product of + // two Eigen::Vectors return a GenericPolynomial. + // NOLINTNEXTLINE(runtime/explicit) + GenericPolynomial(const BasisElement& m); + + /** Constructs a polynomial from an expression @p e. Note that all variables + * in `e` are considered as indeterminates. + * + * @throws std::exception if @p e is not a polynomial. + */ + explicit GenericPolynomial(const Expression& e); + + /** Constructs a polynomial from an expression @p e by decomposing it with + * respect to @p indeterminates. + * + * @note The indeterminates for the polynomial are @p indeterminates. Even if + * a variable in @p indeterminates does not show up in @p e, that variable is + * still registered as an indeterminate in this polynomial, as + * this->indeterminates() be the same as @p indeterminates. + * + * @throws std::exception if @p e is not a polynomial in @p + * indeterminates. + */ + GenericPolynomial(const Expression& e, Variables indeterminates); + + /** Returns the indeterminates of this generic polynomial. */ + [[nodiscard]] const Variables& indeterminates() const { + return indeterminates_; + } + + /** Returns the decision variables of this generic polynomial. */ + [[nodiscard]] const Variables& decision_variables() const { + return decision_variables_; + } + + /** Sets the indeterminates to `new_indeterminates`. + * + * Changing the indeterminates will change + * `basis_element_to_coefficient_map()`, and also potentially the degree of + * the polynomial. Here is an example. + * + * @code + * // p is a quadratic polynomial with x being the only indeterminate. + * symbolic::GenericPolynomial p(a * x * x + b * x + c, + * {x}); + * // p.basis_element_to_coefficient_map() contains {1: c, x: b, x*x:a}. + * std::cout << p.TotalDegree(); // prints 2. + * // Now set (a, b, c) to the indeterminates. p becomes a linear + * // polynomial of a, b, c. + * p.SetIndeterminates({a, b, c}); + * // p.basis_element_to_coefficient_map() now is {a: x * x, b: x, c: 1}. + * std::cout << p.TotalDegree(); // prints 1. + * @endcode + * This function can be expensive, as it potentially reconstructs the + * polynomial (using the new indeterminates) from the expression. + */ + void SetIndeterminates(const Variables& new_indeterminates); + + /** Returns the map from each basis element to its coefficient. */ + [[nodiscard]] const MapType& basis_element_to_coefficient_map() const { + return basis_element_to_coefficient_map_; + } + + /** Returns the highest degree of this generic polynomial in an indeterminate + * @p v. */ + [[nodiscard]] int Degree(const Variable& v) const; + + /** Returns the total degree of this generic polynomial. */ + [[nodiscard]] int TotalDegree() const; + + /** Returns an equivalent symbolic expression of this generic polynomial.*/ + [[nodiscard]] Expression ToExpression() const; + + /** + * Differentiates this generic polynomial with respect to the variable @p x. + * Note that a variable @p x can be either a decision variable or an + * indeterminate. + */ + [[nodiscard]] GenericPolynomial Differentiate( + const Variable& x) const; + + /** Computes the Jacobian matrix J of the generic polynomial with respect to + * @p vars. J(0,i) contains ∂f/∂vars(i). @p vars should be an Eigen column + * vector of symbolic variables. + */ + template + Eigen::Matrix, 1, Derived::RowsAtCompileTime> + Jacobian(const Eigen::MatrixBase& vars) const { + static_assert( + std::is_same_v && + (Derived::ColsAtCompileTime == 1), + "The argument of GenericPolynomial::Jacobian() should be a vector of " + "symbolic variables."); + const VectorX::Index n{vars.size()}; + Eigen::Matrix, 1, + Derived::RowsAtCompileTime> + J{n}; + for (VectorX::Index i = 0; i < n; ++i) { + J(i) = this->Differentiate(vars(i)); + } + return J; + } + + /** + * Evaluates this generic polynomial under a given environment @p env. + * + * @throws std::exception if there is a variable in this generic + * polynomial whose assignment is not provided by @p env. + */ + [[nodiscard]] double Evaluate(const Environment& env) const; + + /** Partially evaluates this generic polynomial using an environment @p env. + * + * @throws std::exception if NaN is detected during evaluation. + */ + [[nodiscard]] GenericPolynomial EvaluatePartial( + const Environment& env) const; + + /** Partially evaluates this generic polynomial by substituting @p var with @p + * c. + + * @throws std::exception if NaN is detected at any point during + * evaluation. + */ + [[nodiscard]] GenericPolynomial EvaluatePartial( + const Variable& var, double c) const; + + /** Adds @p coeff * @p m to this generic polynomial. */ + GenericPolynomial& AddProduct(const Expression& coeff, + const BasisElement& m); + + /** Removes the terms whose absolute value of the coefficients are smaller + * than or equal to @p coefficient_tol. + * For example, if the generic polynomial is 2x² + 3xy + 10⁻⁴x - 10⁻⁵, + * then after calling RemoveTermsWithSmallCoefficients(1e-3), the returned + * polynomial becomes 2x² + 3xy. + * @param coefficient_tol A positive scalar. + * @retval polynomial_cleaned A generic polynomial whose terms with small + * coefficients are removed. + */ + [[nodiscard]] GenericPolynomial + RemoveTermsWithSmallCoefficients(double coefficient_tol) const; + + GenericPolynomial& operator+=( + const GenericPolynomial& p); + GenericPolynomial& operator+=(const BasisElement& m); + GenericPolynomial& operator+=(double c); + GenericPolynomial& operator+=(const Variable& v); + + GenericPolynomial& operator-=( + const GenericPolynomial& p); + GenericPolynomial& operator-=(const BasisElement& m); + GenericPolynomial& operator-=(double c); + GenericPolynomial& operator-=(const Variable& v); + + GenericPolynomial& operator*=( + const GenericPolynomial& p); + GenericPolynomial& operator*=(const BasisElement& m); + GenericPolynomial& operator*=(double c); + GenericPolynomial& operator*=(const Variable& v); + + GenericPolynomial& operator/=(double c); + + /** Returns true if this and @p p are structurally equal. + */ + [[nodiscard]] bool EqualTo(const GenericPolynomial& p) const; + + /** Returns true if this generic polynomial and @p p are equal after expanding + * the coefficients. */ + [[nodiscard]] bool EqualToAfterExpansion( + const GenericPolynomial& p) const; + + /** Returns true if this polynomial and @p p are almost equal (the difference + * in the corresponding coefficients are all less than @p tol), after + * expanding the coefficients. + */ + bool CoefficientsAlmostEqual(const GenericPolynomial& p, + double tol) const; + + /** Returns a symbolic formula representing the condition where this + * polynomial and @p p are the same. + */ + Formula operator==(const GenericPolynomial& p) const; + + /** Returns a symbolic formula representing the condition where this + * polynomial and @p p are not the same. + */ + Formula operator!=(const GenericPolynomial& p) const; + + /** Implements the @ref hash_append concept. */ + template + friend void hash_append( + HashAlgorithm& hasher, + const GenericPolynomial& item) noexcept { + using drake::hash_append; + for (const auto& [basis_element, coeff] : + item.basis_element_to_coefficient_map_) { + hash_append(hasher, basis_element); + hash_append(hasher, coeff); + } + } + + private: + // Throws std::exception if there is a variable appeared in both of + // decision_variables() and indeterminates(). + void CheckInvariant() const; + + MapType basis_element_to_coefficient_map_; + Variables indeterminates_; + Variables decision_variables_; +}; + +/** Defines an explicit SFINAE alias for use with return types to dissuade CTAD + * from trying to instantiate an invalid GenericElement<> for operator + * overloads, (if that's actually the case). + * See discussion for more info: + * https://github.com/robotlocomotion/drake/pull/14053#pullrequestreview-488744679 + */ +template +using GenericPolynomialEnable = std::enable_if_t< + std::is_base_of_v, + GenericPolynomial>; + +template +GenericPolynomialEnable +operator-(const GenericPolynomial& p) { + return -1. * p; +} + +template +GenericPolynomialEnable +operator+(GenericPolynomial p1, + const GenericPolynomial& p2) { + return p1 += p2; +} + +template +GenericPolynomialEnable +operator+(GenericPolynomial p, const BasisElement& m) { + return p += m; +} + +template +GenericPolynomialEnable +operator+(GenericPolynomial p, double c) { + return p += c; +} + +template +GenericPolynomialEnable +operator+(const BasisElement& m, GenericPolynomial p) { + return p += m; +} + +template +GenericPolynomialEnable +operator+(const BasisElement& m1, const BasisElement& m2) { + return GenericPolynomial(m1) + m2; +} + +template +GenericPolynomialEnable +operator+(const BasisElement& m, double c) { + return GenericPolynomial(m) + c; +} + +template +GenericPolynomialEnable +operator+(double c, GenericPolynomial p) { + return p += c; +} + +template +GenericPolynomialEnable +operator+(double c, const BasisElement& m) { + return GenericPolynomial(m) + c; +} + +template +GenericPolynomialEnable +operator+(GenericPolynomial p, const Variable& v) { + return p += v; +} + +template +GenericPolynomialEnable +operator+(const Variable& v, GenericPolynomial p) { + return p += v; +} + +template +GenericPolynomialEnable +operator-(GenericPolynomial p1, + const GenericPolynomial& p2) { + return p1 -= p2; +} + +template +GenericPolynomialEnable +operator-(GenericPolynomial p, const BasisElement& m) { + return p -= m; +} + +template +GenericPolynomialEnable +operator-(GenericPolynomial p, double c) { + return p -= c; +} + +template +GenericPolynomialEnable +operator-(const BasisElement& m, GenericPolynomial p) { + return p = -1 * p + m; +} + +template +GenericPolynomialEnable +operator-(const BasisElement& m1, const BasisElement& m2) { + return GenericPolynomial(m1) - m2; +} + +template +GenericPolynomialEnable +operator-(const BasisElement& m, double c) { + return GenericPolynomial(m) - c; +} + +template +GenericPolynomialEnable +operator-(double c, GenericPolynomial p) { + return p = -p + c; +} + +template +GenericPolynomialEnable +operator-(double c, const BasisElement& m) { + return c - GenericPolynomial(m); +} + +template +GenericPolynomialEnable +operator-(GenericPolynomial p, const Variable& v) { + return p -= v; +} + +template +GenericPolynomialEnable +operator-(const Variable& v, GenericPolynomial p) { + return GenericPolynomial(v, p.indeterminates()) - p; +} + +template +GenericPolynomialEnable +operator*(GenericPolynomial p1, + const GenericPolynomial& p2) { + return p1 *= p2; +} + +template +GenericPolynomialEnable +operator*(GenericPolynomial p, const BasisElement& m) { + return p *= m; +} + +template +GenericPolynomialEnable +operator*(GenericPolynomial p, double c) { + return p *= c; +} + +template +GenericPolynomialEnable +operator*(const BasisElement& m, GenericPolynomial p) { + return p *= m; +} + +template +GenericPolynomialEnable +operator*(const BasisElement& m, double c) { + return GenericPolynomial(m) * c; +} + +template +GenericPolynomialEnable +operator*(double c, GenericPolynomial p) { + return p *= c; +} + +template +GenericPolynomialEnable +operator*(double c, const BasisElement& m) { + return GenericPolynomial(m) * c; +} + +template +GenericPolynomialEnable +operator*(GenericPolynomial p, const Variable& v) { + return p *= v; +} + +template +GenericPolynomialEnable +operator*(const Variable& v, GenericPolynomial p) { + return p *= v; +} + +/** Returns `p / v`. */ +template +GenericPolynomialEnable operator/( + GenericPolynomial p, double v) { + return p /= v; +} + +/** Returns polynomial @p raised to @p n. + * @param p The base polynomial. + * @param n The exponent of the power. @pre n>=0. + * */ +template +GenericPolynomialEnable pow( + const GenericPolynomial& p, int n) { + if (n < 0) { + throw std::runtime_error( + fmt::format("pow(): the degree should be non-negative, got {}.", n)); + } else if (n == 0) { + return GenericPolynomial(BasisElement()); + } else if (n == 1) { + return p; + } else if (n % 2 == 0) { + const GenericPolynomial half = pow(p, n / 2); + return half * half; + } else { + const GenericPolynomial half = pow(p, n / 2); + return half * half * p; + } +} + +template +std::ostream& operator<<(std::ostream& os, + const GenericPolynomial& p) { + const typename GenericPolynomial::MapType& map{ + p.basis_element_to_coefficient_map()}; + if (map.empty()) { + return os << 0; + } + auto it = map.begin(); + os << it->second << "*" << it->first; + for (++it; it != map.end(); ++it) { + os << " + " << it->second << "*" << it->first; + } + return os; +} + +extern template class GenericPolynomial; +extern template class GenericPolynomial; +} // namespace symbolic +} // namespace drake + +namespace std { +/* Provides std::hash>. */ +template +struct hash> + : public drake::DefaultHash {}; +#if defined(__GLIBCXX__) +// Inform GCC that this hash function is not so fast (i.e. for-loop inside). +// This will enforce caching of hash results. See +// https://gcc.gnu.org/onlinedocs/libstdc++/manual/unordered_associative.html +// for details. +template +struct __is_fast_hash>> + : std::false_type {}; +#endif +} // namespace std + +#if !defined(DRAKE_DOXYGEN_CXX) +namespace Eigen { + +// Defines Eigen traits needed for Matrix. +template <> +struct NumTraits< + drake::symbolic::GenericPolynomial> + : GenericNumTraits> { + static inline int digits10() { return 0; } +}; + +template <> +struct NumTraits< + drake::symbolic::GenericPolynomial> + : GenericNumTraits> { + static inline int digits10() { return 0; } +}; +} // namespace Eigen +#endif // !defined(DRAKE_DOXYGEN_CXX) diff --git a/maliput_drake/include/drake/common/symbolic_ldlt.h b/maliput_drake/include/drake/common/symbolic_ldlt.h new file mode 100644 index 0000000..65bd9ac --- /dev/null +++ b/maliput_drake/include/drake/common/symbolic_ldlt.h @@ -0,0 +1,45 @@ +#pragma once + +#ifndef DRAKE_COMMON_SYMBOLIC_HEADER +#error Do not directly include this file. Include "drake/common/symbolic.h". +#endif + +#include + +/// @file +/// Eigen::LDLT is specialized for drake::symbolic::Expression, for certain +/// matrix sizes. If the expression matrix is all constants, it returns the +/// robust decomposition (as per Eigen's algorithm). If there are unbound +/// variables, currently throws an exception (but may support this in the +/// future). + +#if !defined(DRAKE_DOXYGEN_CXX) +namespace Eigen { + +// Provide explicit (full) template specialization for LDLT::compute when the +// Scalar is symbolic::Expression. We need to list out all of the different +// matrix sizes that Drake uses symbolically, because we cannot partially +// specialize a template class template method. +#define DRAKE_DECLARE_SPECIALIZE_LDLT(SomeMatrix) \ +template <> \ +template <> \ +Eigen::LDLT& \ +Eigen::LDLT::compute>( \ + const EigenBase>&); \ +template <> \ +template <> \ +inline \ +Eigen::LDLT& \ +Eigen::LDLT::compute( \ + const EigenBase& a) { \ + const Ref ref_a(a.derived()); \ + return compute(ref_a); \ +} \ + +DRAKE_DECLARE_SPECIALIZE_LDLT(drake::MatrixX) +DRAKE_DECLARE_SPECIALIZE_LDLT(drake::MatrixUpTo6) + +#undef DRAKE_DECLARE_SPECIALIZE_LDLT + +} // namespace Eigen +#endif // !defined(DRAKE_DOXYGEN_CXX) diff --git a/maliput_drake/include/drake/common/symbolic_monomial.h b/maliput_drake/include/drake/common/symbolic_monomial.h new file mode 100644 index 0000000..876a8e7 --- /dev/null +++ b/maliput_drake/include/drake/common/symbolic_monomial.h @@ -0,0 +1,174 @@ +#pragma once + +#ifndef DRAKE_COMMON_SYMBOLIC_HEADER +#error Do not directly include this file. Include "drake/common/symbolic.h". +#endif + +#include +#include +#include +#include + +#include + +#include "drake/common/drake_copyable.h" +#include "drake/common/hash.h" +#include "drake/common/symbolic.h" + +namespace drake { + +namespace symbolic { + +/** Represents a monomial, a product of powers of variables with non-negative + * integer exponents. Note that it does not include the coefficient part of a + * monomial. + */ +class Monomial { + public: + DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Monomial) + + /** Constructs a monomial equal to 1. Namely the total degree is zero. */ + Monomial() = default; + + /** Constructs a default value. This overload is used by Eigen when + * EIGEN_INITIALIZE_MATRICES_BY_ZERO is enabled. + */ + explicit Monomial(std::nullptr_t) : Monomial() {} + + /** Constructs a Monomial from @p powers. + * @throws std::exception if `powers` includes a negative exponent. + */ + explicit Monomial(const std::map& powers); + + /** Constructs a Monomial from a vector of variables `vars` and their + * corresponding integer exponents `exponents`. + * For example, `Monomial([x, y, z], [2, 0, 1])` constructs a Monomial `x²z`. + * + * @pre The size of `vars` should be the same as the size of `exponents`. + * @throws std::exception if `exponents` includes a negative integer. + */ + Monomial(const Eigen::Ref>& vars, + const Eigen::Ref& exponents); + + /** + * Converts an expression to a monomial if the expression is written as + * ∏ᵢpow(xᵢ, kᵢ), otherwise throws a runtime error. + * @pre is_polynomial(e) should be true. + */ + explicit Monomial(const Expression& e); + + /** Constructs a Monomial from @p var. */ + explicit Monomial(const Variable& var); + + /** Constructs a Monomial from @p var and @p exponent. */ + Monomial(const Variable& var, int exponent); + + /** Returns the degree of this Monomial in a variable @p v. */ + int degree(const Variable& v) const; + + /** Returns the total degree of this Monomial. */ + int total_degree() const { return total_degree_; } + + /** Returns the set of variables in this monomial. */ + Variables GetVariables() const; + + /** Returns the internal representation of Monomial, the map from a base + * (Variable) to its exponent (int).*/ + const std::map& get_powers() const { return powers_; } + + /** Evaluates under a given environment @p env. + * + * @throws std::exception if there is a variable in this monomial + * whose assignment is not provided by @p env. + */ + double Evaluate(const Environment& env) const; + + /** Partially evaluates using a given environment @p env. The evaluation + * result is of type pair. The first component (: double) + * represents the coefficient part while the second component represents the + * remaining parts of the Monomial which was not evaluated. + * + * Example 1. Evaluate with a fully-specified environment + * (x³*y²).EvaluatePartial({{x, 2}, {y, 3}}) + * = (2³ * 3² = 8 * 9 = 72, Monomial{} = 1). + * + * Example 2. Evaluate with a partial environment + * (x³*y²).EvaluatePartial({{x, 2}}) + * = (2³ = 8, y²). + */ + std::pair EvaluatePartial(const Environment& env) const; + + /** Returns a symbolic expression representing this monomial. */ + Expression ToExpression() const; + + /** Checks if this monomial and @p m represent the same monomial. Two + * monomials are equal iff they contain the same variable raised to the same + * exponent. */ + bool operator==(const Monomial& m) const; + + /** Checks if this monomial and @p m do not represent the same monomial. */ + bool operator!=(const Monomial& m) const; + + /** Returns this monomial multiplied by @p m. */ + Monomial& operator*=(const Monomial& m); + + /** Returns this monomial raised to @p p. + * @throws std::exception if @p p is negative. + */ + Monomial& pow_in_place(int p); + + /** Implements the @ref hash_append concept. */ + template + friend void hash_append( + HashAlgorithm& hasher, const Monomial& item) noexcept { + using drake::hash_append; + // We do not send total_degree_ to the hasher, because it is already fully + // represented by powers_ -- it is just a cached tally of the exponents. + hash_append(hasher, item.powers_); + } + + private: + int total_degree_{0}; + std::map powers_; + friend std::ostream& operator<<(std::ostream& out, const Monomial& m); +}; + +std::ostream& operator<<(std::ostream& out, const Monomial& m); + +/** Returns a multiplication of two monomials, @p m1 and @p m2. */ +Monomial operator*(Monomial m1, const Monomial& m2); + +/** Returns @p m1 raised to @p p. + * @throws std::exception if @p p is negative. + */ +Monomial pow(Monomial m, int p); +} // namespace symbolic +} // namespace drake + +namespace std { +/* Provides std::hash. */ +template <> +struct hash + : public drake::DefaultHash {}; +} // namespace std + +#if !defined(DRAKE_DOXYGEN_CXX) +namespace Eigen { +// Eigen scalar type traits for Matrix. +template <> +struct NumTraits + : GenericNumTraits { + static inline int digits10() { return 0; } +}; + +namespace internal { +// Informs Eigen how to cast drake::symbolic::Monomial to +// drake::symbolic::Expression. +template <> +EIGEN_DEVICE_FUNC inline drake::symbolic::Expression cast( + const drake::symbolic::Monomial& m) { + return m.ToExpression(); +} +} // namespace internal +} // namespace Eigen +#endif // !defined(DRAKE_DOXYGEN_CXX) diff --git a/maliput_drake/include/drake/common/symbolic_monomial_basis_element.h b/maliput_drake/include/drake/common/symbolic_monomial_basis_element.h new file mode 100644 index 0000000..422986a --- /dev/null +++ b/maliput_drake/include/drake/common/symbolic_monomial_basis_element.h @@ -0,0 +1,238 @@ +#pragma once + +#ifndef DRAKE_COMMON_SYMBOLIC_HEADER +#error Do not directly include this file. Include "drake/common/symbolic.h". +#endif + +#include +#include + +#include + +#include "drake/common/drake_copyable.h" +#include "drake/common/hash.h" +#include "drake/common/symbolic.h" + +namespace drake { +namespace symbolic { +/** + * MonomialBasisElement represents a monomial, a product of powers of variables + * with non-negative integer exponents. Note that it doesn't not include the + * coefficient part of a monomial. So x, x³y, xy²z are all valid + * MonomialBasisElement instances, but 1+x or 2xy²z are not. + * TODO(hongkai.dai): deprecate Monomial class and replace Monomial class with + * MonomialBasisElement class. + * For more information regarding the motivation of this class, please see + * Drake github issue #13602 and #13803. + */ +class MonomialBasisElement : public PolynomialBasisElement { + public: + DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(MonomialBasisElement) + + /** Constructs a monomial equal to 1. Namely the toal degree is zero. */ + MonomialBasisElement(); + + /** Constructs a default value. This overload is used by Eigen when + * EIGEN_INITIALIZE_MATRICES_BY_ZERO is enabled. + */ + explicit MonomialBasisElement(std::nullptr_t) : MonomialBasisElement() {} + + /** + * Constructs a MonomialBasisElement from variable to degree map. + */ + explicit MonomialBasisElement( + const std::map& var_to_degree_map); + + /** + * Converts an expression to a monomial if the expression is written as + * ∏ᵢpow(xᵢ, kᵢ), otherwise throws a runtime error. + * @pre is_polynomial(e) should be true. + */ + explicit MonomialBasisElement(const Expression& e); + + /** Constructs a Monomial from a vector of variables `vars` and their + * corresponding integer degrees `degrees`. + * For example, `MonomialBasisElement([x, y, z], [2, 0, 1])` constructs a + * MonomialBasisElement `x²z`. + * + * @pre The size of `vars` should be the same as the size of `degrees`. + * @throws std::exception if `degrees` includes a negative integer. + */ + MonomialBasisElement(const Eigen::Ref>& vars, + const Eigen::Ref& degrees); + + /** + * Constructs a monomial basis element with only one variable, and the degree + * is 1. + */ + explicit MonomialBasisElement(const Variable& var); + + /** + * Constructs a monomial basis element with only one variable, and the degree + * of that variable is given by @p degree. + */ + MonomialBasisElement(const Variable& var, int degree); + + /** Partially evaluates using a given environment @p env. The evaluation + * result is of type pair. The first component + * (: double) represents the coefficient part while the second component + * represents the remaining parts of the MonomialBasisElement which was not + * evaluated. + * + * Example 1. Evaluate with a fully-specified environment + * (x³*y²).EvaluatePartial({{x, 2}, {y, 3}}) + * = (2³ * 3² = 8 * 9 = 72, MonomialBasisElement{} = 1). + * + * Example 2. Evaluate with a partial environment + * (x³*y²).EvaluatePartial({{x, 2}}) + * = (2³ = 8, y²). + */ + [[nodiscard]] std::pair EvaluatePartial( + const Environment& env) const; + + /** Returns this monomial raised to @p p. + * @throws std::exception if @p p is negative. + */ + MonomialBasisElement& pow_in_place(int p); + + /** + * Compares two MonomialBasisElement in lexicographic order. + */ + bool operator<(const MonomialBasisElement& other) const; + + /** + * Differentiates this MonomialBasisElement. + * Since dxⁿ/dx = nxⁿ⁻¹, we return the map from the MonomialBasisElement to + * its coefficient. So if this MonomialBasisElement is x³y², then + * differentiate with x will return (x²y² → 3) as dx³y²/dx = 3x²y² + * If @p var is not a variable in MonomialBasisElement, then returns an empty + * map. + */ + [[nodiscard]] std::map Differentiate( + const Variable& var) const; + + /** + * Integrates this MonomialBasisElement on a variable. + * Since ∫ xⁿ dx = 1 / (n+1) xⁿ⁺¹, we return the map from the + * MonomialBasisElement to its coefficient in the integration result. So if + * this MonomialBasisElement is x³y², then we return (x⁴y² → 1/4) as ∫ x³y²dx + * = 1/4 x⁴y². If @p var is not a variable in this MonomialBasisElement, for + * example ∫ x³y²dz = x³y²z, then we return (x³y²z → 1) + */ + [[nodiscard]] std::map Integrate( + const Variable& var) const; + + /** Merges this basis element with another basis element @p other by merging + * their var_to_degree_map. This is equivalent to multiplying this monomial + * basis element in place with monomial basis element @p other. + */ + void MergeBasisElementInPlace(const MonomialBasisElement& other); + + /** Implements the @ref hash_append concept. */ + template + friend void hash_append(HashAlgorithm& hasher, + const MonomialBasisElement& item) noexcept { + using drake::hash_append; + // We do not send total_degree_ to the hasher, because it is already fully + // represented by var_to_degree_map_ -- it is just a cached tally of the + // exponents. + hash_append(hasher, item.var_to_degree_map()); + } + + /** + * Converts this monomial to Chebyshev polynomial basis. For example, + * + * - For x², it returns 0.5T₂(x) + 0.5T₀(x). + * - For x²y³, it returns 1/8T₂(x)T₃(y) + 3/8T₂(x)T₁(y) + 1/8T₀(x)T₃(y) + + * 3/8T₀(x)T₁(y). + * + * We return the map from each ChebyshevBasisElement to its coefficient. + * For example, when this = x², it returns {[T₂(x)⇒0.5], [T₀(x)⇒0.5]}. + * When this = x²y³, it returns {[T₂(x)T₃(y)⇒1/8], [T₂(x)T₁(y)⇒3/8], + * [T₀(x)T₃(y)⇒1/8], [T₀(x)T₁(y)⇒3/8]}. + */ + [[nodiscard]] std::map ToChebyshevBasis() + const; + + /** + * Converts this monomial to a weighted sum of basis elements of type + * BasisElement. We return the map from each BasisElement to its coefficient. + * For example, if BasisElement=ChebyshevBasisElement, then when this = x²y³, + * it returns {[T₂(x)T₃(y)⇒1/8], [T₂(x)T₁(y)⇒3/8], [T₀(x)T₃(y)⇒1/8], + * [T₀(x)T₁(y)⇒3/8]}. + * @note Currently we only support @tparam BasisElement being + * MonomialBasisElement and ChebyshevBasisElement. + */ + template + std::map ToBasis() const { + static_assert(std::is_same_v || + std::is_same_v, + "MonomialBasisElement::ToBasis() does not support this " + "BasisElement type."); + if constexpr (std::is_same_v) { + return {{*this, 1.}}; + } + return ToChebyshevBasis(); + } + + private: + [[nodiscard]] double DoEvaluate(double variable_val, + int degree) const override; + [[nodiscard]] Expression DoToExpression() const override; +}; + +std::ostream& operator<<(std::ostream& out, const MonomialBasisElement& m); + +/** Returns a multiplication of two monomials, @p m1 and @p m2. + * @note that we return a map from the monomial product to its coefficient. This + * map has size 1, and the coefficient is also 1. We return a map instead of the + * MonomialBasisElement directly, because we want operator* to have the same + * return signature as other PolynomialBasisElement. For example, the product + * between two ChebyshevBasisElement objects is a weighted sum of + * ChebyshevBasisElement objects. + * @note we do not provide operator*= function for this class, since operator*= + * would return MonomialBasisElement, which is different from operator*. + */ +std::map operator*( + const MonomialBasisElement& m1, const MonomialBasisElement& m2); + +/** Returns @p m raised to @p p. + * @note that we return a map from the monomial power to its coefficient. This + * map has size 1, and the coefficient is also 1. We return a map instead of the + * MonomialBasisElement directly, because we want pow() to have the same + * return signature as other PolynomialBasisElement. For example, the power + * of a ChebyshevBasisElement object is a weighted sum of ChebyshevBasisElement + * objects. + * @throws std::exception if @p p is negative. + */ +std::map pow(MonomialBasisElement m, int p); +} // namespace symbolic +} // namespace drake + +namespace std { +/* Provides std::hash. */ +template <> +struct hash : public drake::DefaultHash { +}; +} // namespace std + +#if !defined(DRAKE_DOXYGEN_CXX) +namespace Eigen { +// Eigen scalar type traits for Matrix. +template <> +struct NumTraits + : GenericNumTraits { + static inline int digits10() { return 0; } +}; + +namespace internal { +// Informs Eigen how to cast drake::symbolic::MonomialBasisElement to +// drake::symbolic::Expression. +template <> +EIGEN_DEVICE_FUNC inline drake::symbolic::Expression cast( + const drake::symbolic::MonomialBasisElement& m) { + return m.ToExpression(); +} +} // namespace internal +} // namespace Eigen +#endif // !defined(DRAKE_DOXYGEN_CXX) diff --git a/maliput_drake/include/drake/common/symbolic_monomial_util.h b/maliput_drake/include/drake/common/symbolic_monomial_util.h new file mode 100644 index 0000000..2f6fc1a --- /dev/null +++ b/maliput_drake/include/drake/common/symbolic_monomial_util.h @@ -0,0 +1,249 @@ +#pragma once + +#ifndef DRAKE_COMMON_SYMBOLIC_HEADER +#error Do not directly include this file. Include "drake/common/symbolic.h". +#endif + +#include +#include +#include +#include +#include + +#include + +#include "drake/common/drake_assert.h" +#include "drake/common/hash.h" +#include "drake/common/symbolic.h" + +namespace drake { +namespace symbolic { + +/** Implements Graded reverse lexicographic order. + * + * @tparam VariableOrder VariableOrder{}(v1, v2) is true if v1 < v2. + * + * We first compare the total degree of the monomial; if there is a tie, then we + * use the lexicographical order as the tie breaker, but a monomial with higher + * order in lexicographical order is considered lower order in graded reverse + * lexicographical order. + * + * Take MonomialBasis({x, y, z}, 2) as an example, with the order x > y > z. To + * get the graded reverse lexicographical order, we take the following steps: + * + * First find all the monomials using the total degree. The monomials with + * degree 2 are {x^2, y^2, z^2, xy, xz, yz}. The monomials with degree 1 are {x, + * y, z}, and the monomials with degree 0 is {1}. To break the tie between + * monomials with the same total degree, first sort them in the reverse + * lexicographical order, namely x < y < z in the reverse lexicographical + * order. The lexicographical order compares two monomial by first comparing the + * exponent of the largest variable, if there is a tie then go forth to the + * second largest variable. Thus z^2 > zy >zx > y^2 > yx > x^2. Finally reverse + * the order as x^2 > xy > y^2 > xz > yz > z^2. + * + * There is an introduction to monomial order in + * https://en.wikipedia.org/wiki/Monomial_order, and an introduction to graded + * reverse lexicographical order in + * https://en.wikipedia.org/wiki/Monomial_order#Graded_reverse_lexicographic_order + */ +template +struct GradedReverseLexOrder { + /** Returns true if m1 > m2 under the Graded reverse lexicographic order. */ + bool operator()(const Monomial& m1, const Monomial& m2) const { + const int d1{m1.total_degree()}; + const int d2{m2.total_degree()}; + if (d1 > d2) { + return true; + } + if (d2 > d1) { + return false; + } + // d1 == d2 + if (d1 == 0) { + // Because both of them are 1. + return false; + } + const std::map& powers1{m1.get_powers()}; + const std::map& powers2{m2.get_powers()}; + std::map::const_iterator it1{powers1.cbegin()}; + std::map::const_iterator it2{powers2.cbegin()}; + while (it1 != powers1.cend() && it2 != powers2.cend()) { + const Variable& var1{it1->first}; + const Variable& var2{it2->first}; + const int exponent1{it1->second}; + const int exponent2{it2->second}; + if (variable_order_(var2, var1)) { + return true; + } else if (variable_order_(var1, var2)) { + return false; + } else { + // var1 == var2 + if (exponent1 == exponent2) { + ++it1; + ++it2; + } else { + return exponent2 > exponent1; + } + } + } + // When m1 and m2 are identical. + return false; + } + + private: + VariableOrder variable_order_; +}; + +namespace internal { +/** Generates [b * m for m in MonomialBasis(vars, degree)] and push them to + * bin. Used as a helper function to implement MonomialBasis. + * + * @tparam MonomialOrder provides a monomial ordering. + * TODO(hongkai.dai): Remove this method and use + * AddPolynomialBasisElementsOfDegreeN in symbolic_polynomial_basis.h instead + * when we deprecate Monomial class. + */ +template +void AddMonomialsOfDegreeN(const Variables& vars, int degree, const Monomial& b, + std::set* const bin) { + DRAKE_ASSERT(!vars.empty()); + if (degree == 0) { + bin->insert(b); + return; + } + const Variable& var{*vars.cbegin()}; + bin->insert(b * Monomial{var, degree}); + if (vars.size() == 1) { + return; + } + for (int i{degree - 1}; i >= 0; --i) { + AddMonomialsOfDegreeN(vars - var, degree - i, b * Monomial{var, i}, bin); + } +} + +enum class DegreeType { + kEven, ///< Even degree + kOdd, ///< Odd degree + kAny, ///< Any degree +}; + +/** Returns all monomials up to a given degree under the graded reverse + * lexicographic order. This is called by MonomialBasis functions defined below. + * + * @tparam rows Number of rows or Dynamic + * @param degree_type If degree_type is kAny, then the monomials' degrees are no + * larger than @p degree. If degree_type is kEven, then the monomial's degrees + * are even numbers no larger than @p degree. If degree_type is kOdd, then the + * monomial degrees are odd numbers no larger than @p degree. + */ +template +Eigen::Matrix ComputeMonomialBasis( + const Variables& vars, int degree, + DegreeType degree_type = DegreeType::kAny) { + DRAKE_DEMAND(!vars.empty()); + DRAKE_DEMAND(degree >= 0); + // 1. Collect monomials. + std::set>> monomials; + int start_degree = 0; + int degree_stride = 1; + switch (degree_type) { + case DegreeType::kAny: { + start_degree = 0; + degree_stride = 1; + break; + } + case DegreeType::kEven: { + start_degree = 0; + degree_stride = 2; + break; + } + case DegreeType::kOdd: { + start_degree = 1; + degree_stride = 2; + } + } + for (int i = start_degree; i <= degree; i += degree_stride) { + AddMonomialsOfDegreeN(vars, i, Monomial{}, &monomials); + } + // 2. Prepare the return value, basis. + DRAKE_DEMAND((rows == Eigen::Dynamic) || + (static_cast(rows) == monomials.size())); + Eigen::Matrix basis(monomials.size()); + size_t i{0}; + for (const auto& m : monomials) { + basis[i] = m; + i++; + } + return basis; +} +} // namespace internal + +/** Returns all monomials up to a given degree under the graded reverse + * lexicographic order. Note that graded reverse lexicographic order uses the + * total order among Variable which is based on a variable's unique ID. For + * example, for a given variable ordering x > y > z, `MonomialBasis({x, y, z}, + * 2)` returns a column vector `[x^2, xy, y^2, xz, yz, z^2, x, y, z, 1]`. + * + * @pre @p vars is a non-empty set. + * @pre @p degree is a non-negative integer. + */ +Eigen::Matrix MonomialBasis(const Variables& vars, + int degree); + +// Computes "n choose k", the number of ways, disregarding order, that k objects +// can be chosen from among n objects. It is used in the following MonomialBasis +// function. +constexpr int NChooseK(int n, int k) { + return (k == 0) ? 1 : (n * NChooseK(n - 1, k - 1)) / k; +} + +/** Returns all monomials up to a given degree under the graded reverse + * lexicographic order. + * + * @tparam n number of variables. + * @tparam degree maximum total degree of monomials to compute. + * + * @pre @p vars is a non-empty set. + * @pre vars.size() == @p n. + */ +template +Eigen::Matrix MonomialBasis( + const Variables& vars) { + static_assert(n > 0, "n should be a positive integer."); + static_assert(degree >= 0, "degree should be a non-negative integer."); + DRAKE_ASSERT(vars.size() == n); + return internal::ComputeMonomialBasis(vars, + degree); +} + +/** Returns all even degree monomials up to a given degree under the graded + * reverse lexicographic order. A monomial has an even degree if its total + * degree is even. So xy is an even degree monomial (degree 2) while x²y is not + * (degree 3). Note that graded reverse lexicographic order uses the total order + * among Variable which is based on a variable's unique ID. For example, for a + * given variable ordering x > y > z, `EvenDegreeMonomialBasis({x, y, z}, 2)` + * returns a column vector `[x², xy, y², xz, yz, z², 1]`. + * + * @pre @p vars is a non-empty set. + * @pre @p degree is a non-negative integer. + */ +Eigen::Matrix EvenDegreeMonomialBasis( + const Variables& vars, int degree); + +/** Returns all odd degree monomials up to a given degree under the graded + * reverse lexicographic order. A monomial has an odd degree if its total + * degree is odd. So x²y is an odd degree monomial (degree 3) while xy is not + * (degree 2). Note that graded reverse lexicographic order uses the total order + * among Variable which is based on a variable's unique ID. For example, for a + * given variable ordering x > y > z, `OddDegreeMonomialBasis({x, y, z}, 3)` + * returns a column vector `[x³, x²y, xy², y³, x²z, xyz, y²z, xz², yz², z³, x, + * y, z]` + * + * @pre @p vars is a non-empty set. + * @pre @p degree is a non-negative integer. + */ +Eigen::Matrix OddDegreeMonomialBasis( + const Variables& vars, int degree); + +} // namespace symbolic +} // namespace drake diff --git a/maliput_drake/include/drake/common/symbolic_polynomial.h b/maliput_drake/include/drake/common/symbolic_polynomial.h new file mode 100644 index 0000000..c67a383 --- /dev/null +++ b/maliput_drake/include/drake/common/symbolic_polynomial.h @@ -0,0 +1,500 @@ +#pragma once + +#ifndef DRAKE_COMMON_SYMBOLIC_HEADER +#error Do not directly include this file. Include "drake/common/symbolic.h". +#endif + +#include +#include +#include +#include +#include + +#include + +#include "drake/common/drake_copyable.h" +#include "drake/common/symbolic.h" + +namespace drake { +namespace symbolic { +namespace internal { +// Compares two monomials using the lexicographic order. It is used in +// symbolic::Polynomial::MapType. See +// https://en.wikipedia.org/wiki/Monomial_order for different monomial orders. +struct CompareMonomial { + bool operator()(const Monomial& m1, const Monomial& m2) const { + const auto& powers1 = m1.get_powers(); + const auto& powers2 = m2.get_powers(); + return std::lexicographical_compare( + powers1.begin(), powers1.end(), powers2.begin(), powers2.end(), + [](const std::pair& p1, + const std::pair& p2) { + const Variable& v1{p1.first}; + const int i1{p1.second}; + const Variable& v2{p2.first}; + const int i2{p2.second}; + if (v1.less(v2)) { + // m2 does not have the variable v1 explicitly, so we treat it as if + // it has (v1)⁰. That is, we need "return i1 < 0", but i1 should be + // positive, so this case always returns false. + return false; + } + if (v2.less(v1)) { + // m1 does not have the variable v2 explicitly, so we treat it as + // if it has (v2)⁰. That is, we need "return 0 < i2", but i2 should + // be positive, so it always returns true. + return true; + } + return i1 < i2; + }); + } +}; +} // namespace internal + +/// Represents symbolic polynomials. A symbolic polynomial keeps a mapping from +/// a monomial of indeterminates to its coefficient in a symbolic expression. +/// +/// A polynomial `p` has to satisfy an invariant such that +/// `p.decision_variables() ∩ p.indeterminates() = ∅`. We have CheckInvariant() +/// method to check the invariant. +/// +/// Note that arithmetic operations (+,-,*) between a Polynomial and a Variable +/// are not provided. The problem is that Variable class has no intrinsic +/// information if a variable is a decision variable or an indeterminate while +/// we need this information to perform arithmetic operations over Polynomials. +// TODO(hongkai.dai) when symbolic::GenericPolynomial is ready, we will +// deprecate symbolic::Polynomial class, and create an alias using +// symbolic::Polynomial=symbolic::GenericPolynomial; +// We will copy the unit tests in symbolic_polynomial_test.cc to +// symbolic_generic_polynomial_test.cc +class Polynomial { + public: + using MapType = std::map; + + /// Constructs a zero polynomial. + Polynomial() = default; + DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Polynomial) + + /** Constructs a default value. This overload is used by Eigen when + * EIGEN_INITIALIZE_MATRICES_BY_ZERO is enabled. + */ + explicit Polynomial(std::nullptr_t) : Polynomial() {} + + /// Constructs a polynomial from a map, Monomial → Expression. + explicit Polynomial(MapType init); + + /// Constructs a polynomial from a monomial @p m. Note that all variables + /// in `m` are considered as indeterminates. + // + // Note that this implicit conversion is desirable to have a dot product of + // two Eigen::Vectors return a Polynomial. + // NOLINTNEXTLINE(runtime/explicit) + Polynomial(const Monomial& m); + + /// Constructs a polynomial from an expression @p e. Note that all variables + /// in `e` are considered as indeterminates. + /// + /// @throws std::exception if @p e is not a polynomial. + explicit Polynomial(const Expression& e); + + /// Constructs a polynomial from an expression @p e by decomposing it with + /// respect to @p indeterminates. + /// + /// @note It collects the intersection of the variables appeared in `e` and + /// the provided @p indeterminates. + /// + /// @throws std::exception if @p e is not a polynomial in @p + /// indeterminates. + Polynomial(const Expression& e, Variables indeterminates); + + /// Returns the indeterminates of this polynomial. + const Variables& indeterminates() const; + + /// Returns the decision variables of this polynomial. + const Variables& decision_variables() const; + + /// Sets the indeterminates to `new_indeterminates`. + /// + /// Changing the indeterminates would change `monomial_to_coefficient_map()`, + /// and also potentially the degree of the polynomial. Here is an example. + /// + /// @code + /// // p is a quadratic polynomial with x being the indeterminates. + /// symbolic::Polynomial p(a * x * x + b * x + c, {x}); + /// // p.monomial_to_coefficient_map() contains {1: c, x: b, x*x:a}. + /// std::cout << p.TotalDegree(); // prints 2. + /// // Now set (a, b, c) to the indeterminates. p becomes a linear + /// // polynomial of a, b, c. + /// p.SetIndeterminates({a, b, c}); + /// // p.monomial_to_coefficient_map() now is {a: x * x, b: x, c: 1}. + /// std::cout << p.TotalDegree(); // prints 1. + /// @endcode + void SetIndeterminates(const Variables& new_indeterminates); + + /// Returns the highest degree of this polynomial in a variable @p v. + int Degree(const Variable& v) const; + + /// Returns the total degree of this polynomial. + int TotalDegree() const; + + /// Returns the mapping from a Monomial to its corresponding coefficient of + /// this polynomial. + const MapType& monomial_to_coefficient_map() const; + + /// Returns an equivalent symbolic expression of this polynomial. + Expression ToExpression() const; + + /** Differentiates this polynomial with respect to the variable @p x. Note + * that a variable @p x can be either a decision variable or an indeterminate. + */ + Polynomial Differentiate(const Variable& x) const; + + /// Computes the Jacobian matrix J of the polynomial with respect to + /// @p vars. J(0,i) contains ∂f/∂vars(i). + template + Eigen::Matrix Jacobian( + const Eigen::MatrixBase& vars) const { + static_assert(std::is_same_v && + (Derived::ColsAtCompileTime == 1), + "The argument of Polynomial::Jacobian should be a vector of " + "symbolic variables."); + const VectorX::Index n{vars.size()}; + Eigen::Matrix J{1, n}; + for (VectorX::Index i = 0; i < n; ++i) { + J(0, i) = Differentiate(vars(i)); + } + return J; + } + + /** Integrates this polynomial with respect to an indeterminate @p x. + * Integration with respect to decision variables is not supported yet. If @p + * x is not an indeterminate nor decision variable, then it will be added to + * the list of indeterminates. + * @throws std::exception if @p x is a decision variable. + */ + Polynomial Integrate(const Variable& x) const; + + /** Computes the definite integrate of this polynomial with respect to the + * indeterminate @p x over the domain [a, b]. Integration with respect to + * decision variables is not supported yet. + * @throws std::exception if @p x is a decision variable. + */ + Polynomial Integrate(const Variable& x, double a, double b) const; + + /// Evaluates this polynomial under a given environment @p env. + /// + /// @throws std::exception if there is a variable in this polynomial whose + /// assignment is not provided by @p env. + double Evaluate(const Environment& env) const; + + /// Partially evaluates this polynomial using an environment @p env. + /// + /// @throws std::exception if NaN is detected during evaluation. + Polynomial EvaluatePartial(const Environment& env) const; + + /// Partially evaluates this polynomial by substituting @p var with @p c. + /// + /// @throws std::exception if NaN is detected at any point during + /// evaluation. + Polynomial EvaluatePartial(const Variable& var, double c) const; + + /// Adds @p coeff * @p m to this polynomial. + Polynomial& AddProduct(const Expression& coeff, const Monomial& m); + + /// Removes the terms whose absolute value of the coefficients are smaller + /// than or equal to @p coefficient_tol + /// For example, if the polynomial is 2x² + 3xy + 10⁻⁴x - 10⁻⁵, + /// then after calling RemoveTermsWithSmallCoefficients(1e-3), the returned + /// polynomial becomes 2x² + 3xy. + /// @param coefficient_tol A positive scalar. + /// @retval polynomial_cleaned A polynomial whose terms with small + /// coefficients are removed. + Polynomial RemoveTermsWithSmallCoefficients(double coefficient_tol) const; + + Polynomial& operator+=(const Polynomial& p); + Polynomial& operator+=(const Monomial& m); + Polynomial& operator+=(double c); + Polynomial& operator+=(const Variable& v); + + Polynomial& operator-=(const Polynomial& p); + Polynomial& operator-=(const Monomial& m); + Polynomial& operator-=(double c); + Polynomial& operator-=(const Variable& v); + + Polynomial& operator*=(const Polynomial& p); + Polynomial& operator*=(const Monomial& m); + Polynomial& operator*=(double c); + Polynomial& operator*=(const Variable& v); + + /// Returns true if this polynomial and @p p are structurally equal. + bool EqualTo(const Polynomial& p) const; + + /// Returns true if this polynomial and @p p are equal, after expanding the + /// coefficients. + bool EqualToAfterExpansion(const Polynomial& p) const; + + /// Returns true if this polynomial and @p are almost equal (the difference + /// in the corresponding coefficients are all less than @p tol), after + /// expanding the coefficients. + bool CoefficientsAlmostEqual(const Polynomial& p, double tol) const; + + /// Returns a symbolic formula representing the condition where this + /// polynomial and @p p are the same. + Formula operator==(const Polynomial& p) const; + + /// Returns a symbolic formula representing the condition where this + /// polynomial and @p p are not the same. + Formula operator!=(const Polynomial& p) const; + + /// Implements the @ref hash_append concept. + template + friend void hash_append(HashAlgorithm& hasher, + const Polynomial& item) noexcept { + using drake::hash_append; + for (const auto& p : item.monomial_to_coefficient_map_) { + hash_append(hasher, p.first); + hash_append(hasher, p.second); + } + } + friend Polynomial operator/(Polynomial p, double v); + + private: + // Throws std::exception if there is a variable appeared in both of + // decision_variables() and indeterminates(). + void CheckInvariant() const; + + MapType monomial_to_coefficient_map_; + Variables indeterminates_; + Variables decision_variables_; +}; + +/// Unary minus operation for polynomial. +Polynomial operator-(const Polynomial& p); + +Polynomial operator+(Polynomial p1, const Polynomial& p2); +Polynomial operator+(Polynomial p, const Monomial& m); +Polynomial operator+(Polynomial p, double c); +Polynomial operator+(const Monomial& m, Polynomial p); +Polynomial operator+(const Monomial& m1, const Monomial& m2); +Polynomial operator+(const Monomial& m, double c); +Polynomial operator+(double c, Polynomial p); +Polynomial operator+(double c, const Monomial& m); +Polynomial operator+(Polynomial p, const Variable& v); +Polynomial operator+(const Variable& v, Polynomial p); + +Polynomial operator-(Polynomial p1, const Polynomial& p2); +Polynomial operator-(Polynomial p, const Monomial& m); +Polynomial operator-(Polynomial p, double c); +Polynomial operator-(const Monomial& m, Polynomial p); +Polynomial operator-(const Monomial& m1, const Monomial& m2); +Polynomial operator-(const Monomial& m, double c); +Polynomial operator-(double c, Polynomial p); +Polynomial operator-(double c, const Monomial& m); +Polynomial operator-(Polynomial p, const Variable& v); +Polynomial operator-(const Variable& v, const Polynomial& p); + +Polynomial operator*(Polynomial p1, const Polynomial& p2); +Polynomial operator*(Polynomial p, const Monomial& m); +Polynomial operator*(Polynomial p, double c); +Polynomial operator*(const Monomial& m, Polynomial p); +// Note that `Monomial * Monomial -> Monomial` is provided in +// symbolic_monomial.h file. +Polynomial operator*(const Monomial& m, double c); +Polynomial operator*(double c, Polynomial p); +Polynomial operator*(double c, const Monomial& m); +Polynomial operator*(Polynomial p, const Variable& v); +Polynomial operator*(const Variable& v, Polynomial p); + +/// Returns `p / v`. +Polynomial operator/(Polynomial p, double v); + +/// Returns polynomial @p rasied to @p n. +Polynomial pow(const Polynomial& p, int n); + +std::ostream& operator<<(std::ostream& os, const Polynomial& p); + +/// Provides the following seven operations: +/// +/// - Matrix * Matrix => Matrix +/// - Matrix * Matrix => Matrix +/// - Matrix * Matrix => Matrix +/// - Matrix * Matrix => Matrix +/// - Matrix * Matrix => Matrix +/// - Matrix * Matrix => Matrix +/// - Matrix * Matrix => Matrix +/// +/// @note that these operator overloadings are necessary even after providing +/// Eigen::ScalarBinaryOpTraits. See +/// https://stackoverflow.com/questions/41494288/mixing-scalar-types-in-eigen +/// for more information. +#if defined(DRAKE_DOXYGEN_CXX) +template +Eigen::Matrix +operator*(const MatrixL& lhs, const MatrixR& rhs); +#else +template +typename std::enable_if_t< + std::is_base_of_v, MatrixL> && + std::is_base_of_v, MatrixR> && + // {Polynomial, Monomial, double} x {Polynomial, Monomial, double} + (std::is_same_v || + std::is_same_v || + std::is_same_v) && + (std::is_same_v || + std::is_same_v || + std::is_same_v) && + // Exclude Polynomial x Polynomial case (because the other seven + // operations call this case. If we include this case here, we will have + // self-recursion). + !(std::is_same_v && + std::is_same_v) && + // Exclude double x double case. + !(std::is_same_v && + std::is_same_v), + Eigen::Matrix> +operator*(const MatrixL& lhs, const MatrixR& rhs) { + // "foo.template cast()" is redundant if foo is of Polynomial. + // However, we have checked that `-O2` compiler optimization reduces it into a + // no-op. + return lhs.template cast() * rhs.template cast(); +} +#endif + +} // namespace symbolic +} // namespace drake + +namespace std { +/* Provides std::hash. */ +template <> +struct hash : public drake::DefaultHash {}; +#if defined(__GLIBCXX__) +// Inform GCC that this hash function is not so fast (i.e. for-loop inside). +// This will enforce caching of hash results. See +// https://gcc.gnu.org/onlinedocs/libstdc++/manual/unordered_associative.html +// for details. +template <> +struct __is_fast_hash> : std::false_type {}; +#endif +} // namespace std + +#if !defined(DRAKE_DOXYGEN_CXX) +namespace Eigen { + +// Defines Eigen traits needed for Matrix. +template <> +struct NumTraits + : GenericNumTraits { + static inline int digits10() { return 0; } +}; + +// Informs Eigen that BinaryOp(LhsType, RhsType) gets ResultType. +#define DRAKE_SYMBOLIC_SCALAR_BINARY_OP_TRAITS(LhsType, RhsType, BinaryOp, \ + ResultType) \ + template <> \ + struct ScalarBinaryOpTraits> { \ + enum { Defined = 1 }; \ + typedef ResultType ReturnType; \ + }; + +// Informs Eigen that LhsType op RhsType gets ResultType +// where op ∈ {+, -, *, conj_product}. +#define DRAKE_SYMBOLIC_SCALAR_SUM_DIFF_PRODUCT_CONJ_PRODUCT_TRAITS( \ + LhsType, RhsType, ResultType) \ + DRAKE_SYMBOLIC_SCALAR_BINARY_OP_TRAITS(LhsType, RhsType, \ + internal::scalar_sum_op, ResultType) \ + DRAKE_SYMBOLIC_SCALAR_BINARY_OP_TRAITS( \ + LhsType, RhsType, internal::scalar_difference_op, ResultType) \ + DRAKE_SYMBOLIC_SCALAR_BINARY_OP_TRAITS( \ + LhsType, RhsType, internal::scalar_product_op, ResultType) \ + DRAKE_SYMBOLIC_SCALAR_BINARY_OP_TRAITS( \ + LhsType, RhsType, internal::scalar_conj_product_op, ResultType) + +// Informs Eigen that Polynomial op Monomial gets Polynomial +// where op ∈ {+, -, *, conj_product}. +DRAKE_SYMBOLIC_SCALAR_SUM_DIFF_PRODUCT_CONJ_PRODUCT_TRAITS( + drake::symbolic::Polynomial, drake::symbolic::Monomial, + drake::symbolic::Polynomial) + +// Informs Eigen that Polynomial op double gets Polynomial +// where op ∈ {+, -, *, conj_product}. +DRAKE_SYMBOLIC_SCALAR_SUM_DIFF_PRODUCT_CONJ_PRODUCT_TRAITS( + drake::symbolic::Polynomial, double, drake::symbolic::Polynomial) + +// Informs Eigen that Monomial op Polynomial gets Polynomial +// where op ∈ {+, -, *, conj_product}. +DRAKE_SYMBOLIC_SCALAR_SUM_DIFF_PRODUCT_CONJ_PRODUCT_TRAITS( + drake::symbolic::Monomial, drake::symbolic::Polynomial, + drake::symbolic::Polynomial) + +// Informs Eigen that Monomial op Monomial gets Polynomial +// where op ∈ {+, -, *, conj_product}. +// +// Note that we inform Eigen that the return type of Monomial op Monomial is +// Polynomial, not Monomial, while Monomial * Monomial gets a Monomial in our +// implementation. This discrepency is due to the implementation of Eigen's +// dot() method whose return type is scalar_product_op::ReturnType. For more +// information, check line 67 of Eigen/src/Core/Dot.h and line 767 of +// Eigen/src/Core/util/XprHelper.h. +DRAKE_SYMBOLIC_SCALAR_SUM_DIFF_PRODUCT_CONJ_PRODUCT_TRAITS( + drake::symbolic::Monomial, drake::symbolic::Monomial, + drake::symbolic::Polynomial) + +// Informs Eigen that Monomial op double gets Polynomial +// where op ∈ {+, -, *, conj_product}. +DRAKE_SYMBOLIC_SCALAR_SUM_DIFF_PRODUCT_CONJ_PRODUCT_TRAITS( + drake::symbolic::Monomial, double, drake::symbolic::Polynomial) + +// Informs Eigen that double op Polynomial gets Polynomial +// where op ∈ {+, -, *, conj_product}. +DRAKE_SYMBOLIC_SCALAR_SUM_DIFF_PRODUCT_CONJ_PRODUCT_TRAITS( + double, drake::symbolic::Polynomial, drake::symbolic::Polynomial) + +// Informs Eigen that double op Monomial gets Polynomial +// where op ∈ {+, -, *, conj_product}. +DRAKE_SYMBOLIC_SCALAR_SUM_DIFF_PRODUCT_CONJ_PRODUCT_TRAITS( + double, drake::symbolic::Monomial, drake::symbolic::Polynomial) + +#undef DRAKE_SYMBOLIC_SCALAR_SUM_DIFF_PRODUCT_CONJ_PRODUCT_TRAITS +#undef DRAKE_SYMBOLIC_SCALAR_BINARY_OP_TRAITS + +namespace internal { +// Informs Eigen how to cast drake::symbolic::Polynomial to +// drake::symbolic::Expression. +template <> +EIGEN_DEVICE_FUNC inline drake::symbolic::Expression cast( + const drake::symbolic::Polynomial& p) { + return p.ToExpression(); +} +} // namespace internal +} // namespace Eigen +#endif // !defined(DRAKE_DOXYGEN_CXX) + +namespace drake { +namespace symbolic { +/// Evaluates a matrix `m` of symbolic polynomials using `env`. +/// +/// @returns a matrix of double whose size is the size of @p m. +/// @throws std::exception if NaN is detected during evaluation. +/// @pydrake_mkdoc_identifier{polynomial} +template +std::enable_if_t< + std::is_same_v, + Eigen::Matrix> +Evaluate(const Eigen::MatrixBase& m, const Environment& env) { + return m.unaryExpr([&env](const Polynomial& p) { return p.Evaluate(env); }); +} + +/// Computes the Jacobian matrix J of the vector function @p f with respect to +/// @p vars. J(i,j) contains ∂f(i)/∂vars(j). +/// +/// @pre {@p vars is non-empty}. +/// @pydrake_mkdoc_identifier{polynomial} +MatrixX Jacobian(const Eigen::Ref>& f, + const Eigen::Ref>& vars); + +} // namespace symbolic +} // namespace drake diff --git a/maliput_drake/include/drake/common/symbolic_polynomial_basis.h b/maliput_drake/include/drake/common/symbolic_polynomial_basis.h new file mode 100644 index 0000000..39264b2 --- /dev/null +++ b/maliput_drake/include/drake/common/symbolic_polynomial_basis.h @@ -0,0 +1,162 @@ +#pragma once + +#ifndef DRAKE_COMMON_SYMBOLIC_HEADER +#error Do not directly include this file. Include "drake/common/symbolic.h". +#endif + +#include +#include +#include + +#include + +#include "drake/common/drake_assert.h" +#include "drake/common/symbolic.h" + +namespace drake { +namespace symbolic { +namespace internal { +/* Adds the set {RaisePower(b, m) for m in Basis(vars, degree)} to `bin`. + `Basis` is the polynomial basis associated with `BasisElement` and + `Basis(vars, degree)` returns all BasisElement objects with variables `vars` + and total degree = `degree`. RaisePower(m, b) is the operation that "merges" + two `BasisElement`s such that the merged `BasisElement`: + + - Is a product of all variables present in either m or b. + - The power of variable v is the sum of the degree of that variable in m and + b. If a variable is missing in a BasisElement, it has degree 0. + + As a concrete example, assume that BasisElement is actually + MonomialBasisElement with + vars = {x, y}, degree = 2, and b = x²yz³. MonomialBasis({x, y}, 2) would + produce the set of MonomialBasisElements: x², xy, y². And applying + RaisePower(b, m) to each m would produce: + + - RaisePower(x²yz³, x²) --> x⁴yz³ + - RaisePower(x²yz³, xy) --> x³y²z³ + - RaisePower(x²yz³, y²) --> x²y³z³ + + @tparam BasisElementOrder a total order of PolynomialBasisElement. An example + is BasisElementGradedReverseLexOrder. + @tparam BasisElement A derived class of PolynomialBasisElement. + @note A non-zero degree has no effect if vars is empty. If vars is empty + and degree is zero, then {RaisePower(b, m) for m in Basis(vars, degree)} = {b}. + */ +template +void AddPolynomialBasisElementsOfDegreeN( + const Variables& vars, int degree, const BasisElement& b, + std::set* const bin) { + /* Iterating through Basis(vars, degree) is tricky due to the fact that the + number of variables is determined at runtime. For example, assume the + MonomialBasisElement with vars = {x, y, z} and degree = 3, then + Basis({x, y, z}, 3) would produce the basis elements: x³, x²y, xy², xyz, y³, + y²z, yz², z³. Conceptually, this matches well to nested for loops. Each + for loop "consumes" a portion of the target degree, leaving the remaining + part to the nested loops. + + for (int p_x = degree; p_x >= 0; --p_x) { + for (int p_y = degree - p_x; p_y >= 0; --p_y) { + for (int p_z = degree - p_x - p_y; p_z >= 0; --p_z) { + Monomial m = x ** p_x * y ** p_y * z ** p_z; // Ok, not real code. + bin->insert(RaisePower(m, b)); + } + } + } + + However, if we add another variable, we have to add another nested for loop. + We can't do that dynamically. So, we use recursion to create the *effect* + of a dynamically-determined number of nested for loops. Each level of the + recursion selects the "first" variable in the set and iteratively consumes + some portion of the target degree. The recursive call is made on the + remaining variables with the remaining available degree. The consumed portion + is preserved by pre-multiplying it with the `b` value and passing on this + accumulated b to the recursive call. RaisePower(m, b) is simply m * b. We + can refactor the product: RaisePower(m, b) = RaisePower(m/xⁱ, b * xⁱ) and + this relationship is the recursive call. */ + static_assert( + std::is_base_of_v || + std::is_same_v, + "BasisElement should be a derived class of PolynomialBasisElement"); + if (degree == 0) { + bin->insert(b); + return; + } + if (vars.empty()) { + return; + } + const Variable& var{*vars.cbegin()}; + for (int var_degree = degree; var_degree >= 0; --var_degree) { + std::map new_var_to_degree_map = b.get_powers(); + auto it = new_var_to_degree_map.find(var); + if (it != new_var_to_degree_map.end()) { + it->second += var_degree; + } else { + new_var_to_degree_map.emplace_hint(it, var, var_degree); + } + const BasisElement new_b(new_var_to_degree_map); + AddPolynomialBasisElementsOfDegreeN(vars - var, degree - var_degree, new_b, + bin); + } +} +} // namespace internal + +/** + * Returns all polynomial basis elements up to a given degree under the graded + * reverse lexicographic order. + * @tparam rows Number of rows or Eigen::Dynamic. + * @tparam BasisElement A derived class of PolynomialBasisElement. + * @param vars The variables appearing in the polynomial basis. + * @param degree The highest total degree of the polynomial basis elements. + * @param degree_type If degree_type is kAny, then the polynomial basis + * elements' degrees are no larger than @p degree. If degree_type is kEven, then + * the elements' degrees are even numbers no larger than @p degree. If + * degree_type is kOdd, then the elements' degrees are odd numbers no larger + * than @p degree. + * TODO(hongkai.dai): this will replace ComputeMonomialBasis in + * symbolic_monomial_util.h + */ +template +Eigen::Matrix ComputePolynomialBasisUpToDegree( + const Variables& vars, int degree, internal::DegreeType degree_type) { + DRAKE_DEMAND(!vars.empty()); + DRAKE_DEMAND(degree >= 0); + // 1. Collect elements. + std::set, BasisElement>> + basis_elements_set; + int start_degree = 0; + int degree_stride = 1; + switch (degree_type) { + case internal::DegreeType::kAny: { + start_degree = 0; + degree_stride = 1; + break; + } + case internal::DegreeType::kEven: { + start_degree = 0; + degree_stride = 2; + break; + } + case internal::DegreeType::kOdd: { + start_degree = 1; + degree_stride = 2; + } + } + for (int i = start_degree; i <= degree; i += degree_stride) { + internal::AddPolynomialBasisElementsOfDegreeN(vars, i, BasisElement{}, + &basis_elements_set); + } + // 2. Prepare the return value, basis. + DRAKE_DEMAND((rows == Eigen::Dynamic) || + (static_cast(rows) == basis_elements_set.size())); + Eigen::Matrix basis(basis_elements_set.size()); + int i{0}; + for (const auto& m : basis_elements_set) { + basis[i] = m; + i++; + } + return basis; +} + +} // namespace symbolic +} // namespace drake diff --git a/maliput_drake/include/drake/common/symbolic_polynomial_basis_element.h b/maliput_drake/include/drake/common/symbolic_polynomial_basis_element.h new file mode 100644 index 0000000..e7d35a1 --- /dev/null +++ b/maliput_drake/include/drake/common/symbolic_polynomial_basis_element.h @@ -0,0 +1,250 @@ +#pragma once + +#ifndef DRAKE_COMMON_SYMBOLIC_HEADER +#error Do not directly include this file. Include "drake/common/symbolic.h". +#endif + +#include + +#include + +#include "drake/common/drake_copyable.h" +#include "drake/common/symbolic.h" + +namespace drake { +namespace symbolic { +/** + * Each polynomial p(x) can be written as a linear combination of its basis + * elements p(x) = ∑ᵢ cᵢ * ϕᵢ(x), where ϕᵢ(x) is the i'th element in the basis, + * cᵢ is the coefficient of that element. The most commonly used basis is + * monomials. For example in polynomial p(x) = 2x₀²x₁ + 3x₀x₁ + 2, x₀²x₁, x₀x₁ + * and 1 are all elements of monomial basis. Likewise, a polynomial can be + * written using other basis, such as Chebyshev polynomials, Legendre + * polynomials, etc. For a polynomial written with Chebyshev polynomial basis + * p(x) = 2T₂(x₀)T₁(x₁) + 3T₁(x₁) + 2T₂(x₀), T₂(x₀)T₁(x₁),T₁(x₁), and T₂(x₀) are + * all elements of Chebyshev basis. This PolynomialBasisElement class represents + * an element ϕᵢ(x) in the basis. We can think of an element of polynomial basis + * as a mapping from the variable to its degree. So for monomial basis element + * x₀²x₁, it can be thought of as a mapping {x₀ -> 2, x₁ -> 1}. For a Chebyshev + * basis element T₂(x₀)T₁(x₁), it can be thought of as a mapping {x₀ -> 2, x₁ -> + * 1}. + * + * Each of the derived class, `Derived`, should implement the following + * functions + * + * - std::map operator*(const Derived& A, const Derived&B) + * - std::map Derived::Differentiate(const Variable& var) + * const; + * - std::map Derived::Integrate(const Variable& var) const; + * - bool Derived::operator<(const Derived& other) const; + * - std::pair EvaluatePartial(const Environment& e) const; + * - void MergeBasisElementInPlace(const Derived& other) + * + * The function lexicographical_compare can be used when implementing operator<. + * The function DoEvaluatePartial can be used when implementing EvaluatePartial + */ +class PolynomialBasisElement { + public: + DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(PolynomialBasisElement) + + /** + * Constructs a polynomial basis with empty var_to_degree map. This element + * should be interpreted as 1. + */ + PolynomialBasisElement() = default; + + /** + * Constructs a polynomial basis given the variable and the degree of that + * variable. + * @throws std::exception if any of the degree is negative. + * @note we will ignore the variable with degree 0. + */ + explicit PolynomialBasisElement( + const std::map& var_to_degree_map); + + /** + * Constructs a polynomial basis, such that it contains the variable-to-degree + * map vars(i)→degrees(i). + * @throws std::exception if @p vars contains repeated variables. + * @throws std::exception if any degree is negative. + */ + PolynomialBasisElement(const Eigen::Ref>& vars, + const Eigen::Ref& degrees); + + virtual ~PolynomialBasisElement() = default; + + [[nodiscard]] const std::map& var_to_degree_map() const { + return var_to_degree_map_; + } + + /** + * Returns variable to degree map. + * TODO(hongkai.dai): this function is added because Monomial class has + * get_powers() function. We will remove this get_powers() function when + * Monomial class is deprecated. + */ + [[nodiscard]] const std::map& get_powers() const { + return var_to_degree_map_; + } + + /** Returns the total degree of a polynomial basis. This is the summation of + * the degree for each variable. */ + [[nodiscard]] int total_degree() const { return total_degree_; } + + /** Returns the degree of this PolynomialBasisElement in a variable @p v. If + * @p v is not a variable in this PolynomialBasisElement, then returns 0.*/ + [[nodiscard]] int degree(const Variable& v) const; + + [[nodiscard]] Variables GetVariables() const; + + /** Evaluates under a given environment @p env. + * + * @throws std::exception exception if there is a variable in this + * monomial whose assignment is not provided by @p env. + */ + [[nodiscard]] double Evaluate(const Environment& env) const; + + bool operator==(const PolynomialBasisElement& other) const; + + bool operator!=(const PolynomialBasisElement& other) const; + + [[nodiscard]] Expression ToExpression() const; + + protected: + /** + * Compares two PolynomialBasisElement using lexicographical order. This + * function is meant to be called by the derived class, to compare two + * polynomial basis of the same derived class. + */ + [[nodiscard]] bool lexicographical_compare( + const PolynomialBasisElement& other) const; + + [[nodiscard]] virtual bool EqualTo(const PolynomialBasisElement& other) const; + + // Partially evaluate a polynomial basis element, where @p e does not + // necessarily contain all the variables in this basis element. The + // evaluation result is coeff * new_basis_element. + void DoEvaluatePartial(const Environment& e, double* coeff, + std::map* new_basis_element) const; + + int* get_mutable_total_degree() { return &total_degree_; } + + std::map* get_mutable_var_to_degree_map() { + return &var_to_degree_map_; + } + + /** Merge this basis element with another basis element by merging their + * var_to_degree_map. After merging, the degree of each variable is raised to + * the sum of the degree in each basis element (if a variable does not show up + * in either one of the basis element, we regard its degree to be 0). + */ + void DoMergeBasisElementInPlace(const PolynomialBasisElement& other); + + private: + // This function evaluates the polynomial basis for a univariate polynomial at + // a given degree. For example, for a monomial basis, this evaluates xⁿ where + // x is the variable value and n is the degree; for a Chebyshev basis, this + // evaluats the Chebyshev polynomial Tₙ(x). + [[nodiscard]] virtual double DoEvaluate(double variable_val, + int degree) const = 0; + + [[nodiscard]] virtual Expression DoToExpression() const = 0; + + // Internally, the polynomial basis is represented as a mapping from a + // variable to its degree. + std::map var_to_degree_map_; + int total_degree_{}; +}; + +/** Implements Graded reverse lexicographic order. + * + * @tparam VariableOrder VariableOrder{}(v1, v2) is true if v1 < v2. + * @tparam BasisElement A derived class of PolynomialBasisElement. + * + * We first compare the total degree of the PolynomialBasisElement; if there is + * a tie, then we use the graded reverse lexicographical order as the tie + * breaker. + * + * Take monomials with variables {x, y, z} and total degree<=2 as an + * example, with the order x > y > z. To get the graded reverse lexicographical + * order, we take the following steps: + * + * First find all the monomials using the total degree. The monomials with + * degree 2 are {x², y², z², xy, xz, yz}. The monomials with degree 1 are {x, + * y, z}, and the monomials with degree 0 is {1}. To break the tie between + * monomials with the same total degree, first sort them in the reverse + * lexicographical order, namely x < y < z. The lexicographical order compares + * two monomials by first comparing the exponent of the largest variable, if + * there is a tie then go forth to the second largest variable. Thus z² > zy >zx + * > y² > yx > x². Finally reverse the order as x² > xy > y² > xz > yz > z² > x + * > y > z. + * + * There is an introduction to monomial order in + * https://en.wikipedia.org/wiki/Monomial_order, and an introduction to graded + * reverse lexicographical order in + * https://en.wikipedia.org/wiki/Monomial_order#Graded_reverse_lexicographic_order + */ +template +struct BasisElementGradedReverseLexOrder { + /** Returns true if m1 < m2 under the Graded reverse lexicographic order. */ + bool operator()(const BasisElement& m1, const BasisElement& m2) const { + const int d1{m1.total_degree()}; + const int d2{m2.total_degree()}; + if (d1 > d2) { + return false; + } + if (d2 > d1) { + return true; + } + // d1 == d2 + if (d1 == 0) { + // Because both of them are 1. + return false; + } + const std::map& powers1{m1.get_powers()}; + const std::map& powers2{m2.get_powers()}; + std::map::const_iterator it1{powers1.cbegin()}; + std::map::const_iterator it2{powers2.cbegin()}; + while (it1 != powers1.cend() && it2 != powers2.cend()) { + const Variable& var1{it1->first}; + const Variable& var2{it2->first}; + const int degree1{it1->second}; + const int degree2{it2->second}; + if (variable_order_(var2, var1)) { + return false; + } else if (variable_order_(var1, var2)) { + return true; + } else { + // var1 == var2 + if (degree1 == degree2) { + ++it1; + ++it2; + } else { + return degree1 > degree2; + } + } + } + // When m1 and m2 are identical. + return false; + } + + private: + VariableOrder variable_order_; +}; + +} // namespace symbolic +} // namespace drake + +#if !defined(DRAKE_DOXYGEN_CXX) +namespace Eigen { +namespace internal { +// Informs Eigen how to cast drake::symbolic::PolynomialBasisElement to +// drake::symbolic::Expression. +template <> +EIGEN_DEVICE_FUNC inline drake::symbolic::Expression cast( + const drake::symbolic::PolynomialBasisElement& m) { + return m.ToExpression(); +} +} // namespace internal +} // namespace Eigen +#endif // !defined(DRAKE_DOXYGEN_CXX) diff --git a/maliput_drake/include/drake/common/symbolic_rational_function.h b/maliput_drake/include/drake/common/symbolic_rational_function.h new file mode 100644 index 0000000..148ae1d --- /dev/null +++ b/maliput_drake/include/drake/common/symbolic_rational_function.h @@ -0,0 +1,250 @@ +#pragma once + +#ifndef DRAKE_COMMON_SYMBOLIC_HEADER +#error Do not directly include this file. Include "drake/common/symbolic.h". +#endif + +#include + +#include "drake/common/symbolic.h" + +namespace drake { +namespace symbolic { +/** + * Represents symbolic rational function. A function f(x) is a rational + * function, if f(x) = p(x) / q(x), where both p(x) and q(x) are polynomials of + * x. Note that rational functions are closed under (+, -, x, /). One + * application of rational function is in polynomial optimization, where we + * represent (or approximate) functions using rational functions, and then + * convert the constraint f(x) = h(x) (where h(x) is a polynomial) to a + * polynomial constraint p(x) - q(x) * h(x) = 0, or convert the inequality + * constraint f(x) >= h(x) as p(x) - q(x) * h(x) >= 0 if we know q(x) > 0. + * + * This class represents a special subset of the symbolic::Expression. While a + * symbolic::Expression can represent a rational function, extracting the + * numerator and denominator, generally, is quite difficult; for instance, from + * p1(x) / q1(x) + p2(x) / q2(x) + ... + pn(x) / qn(x). This class's explicit + * structure facilitates this decomposition. + */ +class RationalFunction { + public: + /** Constructs a zero rational function 0 / 1. */ + RationalFunction(); + + DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(RationalFunction) + + /** + * Constructs the rational function: numerator / denominator. + * @param numerator The numerator of the fraction. + * @param denominator The denominator of the fraction. + * @pre denominator cannot be structurally equal to 0. + * @pre None of the indeterminates in the numerator can be decision variables + * in the denominator; similarly none of the indeterminates in the denominator + * can be decision variables in the numerator. + * @throws std::exception if the precondition is not satisfied. + */ + RationalFunction(Polynomial numerator, Polynomial denominator); + + /** + * Constructs the rational function: p / 1. Note that we use 1 as the + * denominator. + * @param p The numerator of the rational function. + */ + explicit RationalFunction(const Polynomial& p); + + /** + * Constructs the rational function: c / 1. Note that we use 1 as the + * denominator. + * @param c The numerator of the rational function. + */ + explicit RationalFunction(double c); + + ~RationalFunction() = default; + + /// Getter for the numerator. + [[nodiscard]] const Polynomial& numerator() const { return numerator_; } + + /// Getter for the denominator. + [[nodiscard]] const Polynomial& denominator() const { return denominator_; } + + RationalFunction& operator+=(const RationalFunction& f); + RationalFunction& operator+=(const Polynomial& p); + RationalFunction& operator+=(double c); + + RationalFunction& operator-=(const RationalFunction& f); + RationalFunction& operator-=(const Polynomial& p); + RationalFunction& operator-=(double c); + + RationalFunction& operator*=(const RationalFunction& f); + RationalFunction& operator*=(const Polynomial& p); + RationalFunction& operator*=(double c); + + RationalFunction& operator/=(const RationalFunction& f); + RationalFunction& operator/=(const Polynomial& p); + RationalFunction& operator/=(double c); + + /** + * Unary minus operation for rational function. + * if f(x) = p(x) / q(x), then -f(x) = (-p(x)) / q(x) + */ + friend RationalFunction operator-(RationalFunction f); + + /** + * Returns true if this rational function and f are structurally equal. + */ + [[nodiscard]] bool EqualTo(const RationalFunction& f) const; + + /** + * Returns a symbolic formula representing the condition where this rational + * function and @p f are the same. + * If f1 = p1 / q1, f2 = p2 / q2, then f1 == f2 <=> p1 * q2 == p2 * q1 + */ + Formula operator==(const RationalFunction& f) const; + + /** + * Returns a symbolic formula representing the condition where this rational + * function and @p f are not the same. + */ + Formula operator!=(const RationalFunction& f) const; + + friend std::ostream& operator<<(std::ostream&, const RationalFunction& f); + + private: + // Throws std::exception if an indeterminate of the denominator (numerator, + // respectively) is a decision variable of the numerator (denominator). + void CheckIndeterminates() const; + Polynomial numerator_; + Polynomial denominator_; +}; + +RationalFunction operator+(RationalFunction f1, const RationalFunction& f2); +RationalFunction operator+(RationalFunction f, const Polynomial& p); +RationalFunction operator+(const Polynomial& p, RationalFunction f); +RationalFunction operator+(RationalFunction f, double c); +RationalFunction operator+(double c, RationalFunction f); + +RationalFunction operator-(RationalFunction f1, const RationalFunction& f2); +RationalFunction operator-(RationalFunction f, const Polynomial& p); +RationalFunction operator-(const Polynomial& p, const RationalFunction& f); +RationalFunction operator-(RationalFunction f, double c); +RationalFunction operator-(double c, RationalFunction f); + +RationalFunction operator*(RationalFunction f1, const RationalFunction& f2); +RationalFunction operator*(RationalFunction f, const Polynomial& p); +RationalFunction operator*(const Polynomial& p, RationalFunction f); +RationalFunction operator*(RationalFunction f, double c); +RationalFunction operator*(double c, RationalFunction f); + +RationalFunction operator/(RationalFunction f1, const RationalFunction& f2); +RationalFunction operator/(RationalFunction f, const Polynomial& p); +RationalFunction operator/(const Polynomial& p, const RationalFunction& f); +RationalFunction operator/(RationalFunction f, double c); +RationalFunction operator/(double c, const RationalFunction& f); + +/** + * Returns the rational function @p f raised to @p n. + * If n is positive, (f/g)ⁿ = fⁿ / gⁿ; + * If n is negative, (f/g)ⁿ = g⁻ⁿ / f⁻ⁿ; + * (f/g)⁰ = 1 / 1. + */ +RationalFunction pow(const RationalFunction& f, int n); +/** + * Provides the following operations: + * + * - Matrix * Matrix => Matrix + * - Matrix * Matrix => Matrix + * - Matrix * Matrix => Matrix + * - Matrix * Matrix => Matrix + * + * where RF is a shorthand for RationalFunction. + * + * @note that these operator overloadings are necessary even after providing + * Eigen::ScalarBinaryOpTraits. See + * https://stackoverflow.com/questions/41494288/mixing-scalar-types-in-eigen + * for more information + */ +#if defined(DRAKE_DOXYGEN_CXX) +template +Eigen::Matrix +operator*(const MatrixL& lhs, const MatrixR& rhs); +#else +template +typename std::enable_if_t< + std::is_base_of_v, MatrixL> && + std::is_base_of_v, MatrixR> && + ((std::is_same_v && + (std::is_same_v || + std::is_same_v)) || + (std::is_same_v && + (std::is_same_v || + std::is_same_v))), + Eigen::Matrix> +operator*(const MatrixL& lhs, const MatrixR& rhs) { + return lhs.template cast() * + rhs.template cast(); +} +#endif +} // namespace symbolic +} // namespace drake + +#if !defined(DRAKE_DOXYGEN_CXX) +namespace Eigen { + +// Defines Eigen traits needed for Matrix. +template <> +struct NumTraits + : GenericNumTraits { + static inline int digits10() { return 0; } +}; + +// Informs Eigen that BinaryOp(LhsType, RhsType) gets ResultType. +#define DRAKE_SYMBOLIC_SCALAR_BINARY_OP_TRAITS(LhsType, RhsType, BinaryOp, \ + ResultType) \ + template <> \ + struct ScalarBinaryOpTraits> { \ + enum { Defined = 1 }; \ + typedef ResultType ReturnType; \ + }; + +// Informs Eigen that LhsType op RhsType gets ResultType +// where op ∈ {+, -, *, /, conj_product}. +#define DRAKE_SYMBOLIC_SCALAR_SUM_DIFF_PRODUCT_CONJ_PRODUCT_TRAITS( \ + LhsType, RhsType, ResultType) \ + DRAKE_SYMBOLIC_SCALAR_BINARY_OP_TRAITS(LhsType, RhsType, \ + internal::scalar_sum_op, ResultType) \ + DRAKE_SYMBOLIC_SCALAR_BINARY_OP_TRAITS( \ + LhsType, RhsType, internal::scalar_difference_op, ResultType) \ + DRAKE_SYMBOLIC_SCALAR_BINARY_OP_TRAITS( \ + LhsType, RhsType, internal::scalar_product_op, ResultType) \ + DRAKE_SYMBOLIC_SCALAR_BINARY_OP_TRAITS( \ + LhsType, RhsType, internal::scalar_conj_product_op, ResultType) + +// Informs Eigen that RationalFunction op Polynomial gets RationalFunction +// where op ∈ {+, -, *, conj_product}. +DRAKE_SYMBOLIC_SCALAR_SUM_DIFF_PRODUCT_CONJ_PRODUCT_TRAITS( + drake::symbolic::RationalFunction, drake::symbolic::Polynomial, + drake::symbolic::RationalFunction) + +// Informs Eigen that Polynomial op RationalFunction gets RationalFunction +// where op ∈ {+, -, *, conj_product}. +DRAKE_SYMBOLIC_SCALAR_SUM_DIFF_PRODUCT_CONJ_PRODUCT_TRAITS( + drake::symbolic::Polynomial, drake::symbolic::RationalFunction, + drake::symbolic::RationalFunction) + +// Informs Eigen that double op RationalFunction gets RationalFunction +// where op ∈ {+, -, *, conj_product}. +DRAKE_SYMBOLIC_SCALAR_SUM_DIFF_PRODUCT_CONJ_PRODUCT_TRAITS( + double, drake::symbolic::RationalFunction, + drake::symbolic::RationalFunction) + +// Informs Eigen that RationalFunction op double gets RationalFunction +// where op ∈ {+, -, *, conj_product}. +DRAKE_SYMBOLIC_SCALAR_SUM_DIFF_PRODUCT_CONJ_PRODUCT_TRAITS( + drake::symbolic::RationalFunction, double, + drake::symbolic::RationalFunction) +#undef DRAKE_SYMBOLIC_SCALAR_BINARY_OP_TRAITS +#undef DRAKE_SYMBOLIC_SCALAR_SUM_DIFF_PRODUCT_CONJ_PRODUCT_TRAITS +} // namespace Eigen +#endif // !defined(DRAKE_DOXYGEN_CXX) diff --git a/maliput_drake/include/drake/common/symbolic_simplification.h b/maliput_drake/include/drake/common/symbolic_simplification.h new file mode 100644 index 0000000..ce243f2 --- /dev/null +++ b/maliput_drake/include/drake/common/symbolic_simplification.h @@ -0,0 +1,58 @@ +#pragma once + +#include +#include + +#include "drake/common/symbolic.h" + +namespace drake { +namespace symbolic { + +/// A pattern is an expression which possibly includes variables which represent +/// placeholders. It is used to construct a `RewritingRule`. +using Pattern = Expression; + +/// A `RewritingRule`, `lhs => rhs`, consists of two Patterns `lhs` and `rhs`. A +/// rewriting rule instructs a rewriter how to transform a given expression `e`. +/// First, the rewriter tries to find a match between the expression `e` and the +/// pattern `lhs`. If such a match is found, it applies the match result +/// (substitution) to `rhs`. Otherwise, the same expression `e` is returned. +class RewritingRule { + public: + /// Constructs a rewriting rule `lhs => rhs`. + RewritingRule(Pattern lhs, Pattern rhs) + : lhs_{std::move(lhs)}, rhs_{std::move(rhs)} {} + + /// Default copy constructor. + RewritingRule(const RewritingRule&) = default; + + /// Default move constructor. + RewritingRule(RewritingRule&&) = default; + + /// Deleted copy-assign operator. + RewritingRule& operator=(const RewritingRule&) = delete; + + /// Deleted move-assign operator. + RewritingRule& operator=(RewritingRule&&) = delete; + + /// Default destructor. + ~RewritingRule() = default; + + /// Returns the const reference of the LHS of the rewriting rule. + const Pattern& lhs() const { return lhs_; } + /// Returns the const reference of the RHS of the rewriting rule. + const Pattern& rhs() const { return rhs_; } + + private: + const Pattern lhs_; + const Pattern rhs_; +}; + +/// A `Rewriter` is a function from an Expression to an Expression. +using Rewriter = std::function; + +/// Constructs a rewriter based on a rewriting rule @p r. +Rewriter MakeRuleRewriter(const RewritingRule& r); + +} // namespace symbolic +} // namespace drake diff --git a/maliput_drake/include/drake/common/symbolic_variable.h b/maliput_drake/include/drake/common/symbolic_variable.h new file mode 100644 index 0000000..af83ffb --- /dev/null +++ b/maliput_drake/include/drake/common/symbolic_variable.h @@ -0,0 +1,368 @@ +#pragma once + +#ifndef DRAKE_COMMON_SYMBOLIC_HEADER +#error Do not directly include this file. Include "drake/common/symbolic.h". +#endif + +#include +#include +#include +#include +#include + +#include + +#include "drake/common/drake_copyable.h" +#include "drake/common/eigen_types.h" +#include "drake/common/hash.h" + +namespace drake { +namespace symbolic { + +/** Represents a symbolic variable. + * + * @note Expression::Evaluate and Formula::Evaluate methods take a symbolic + * environment (Variable → double) and a random number generator. When an + * expression or a formula includes random variables, `Evaluate` methods use the + * random number generator to draw a number for a random variable from the given + * distribution. Then this numeric value is used to substitute all the + * occurrences of the corresponding random variable in an expression or a + * formula. + */ +class Variable { + public: + typedef size_t Id; + + /** Supported types of symbolic variables. */ + // TODO(soonho-tri): refines the following descriptions. + enum class Type { + CONTINUOUS, ///< A CONTINUOUS variable takes a `double` value. + INTEGER, ///< An INTEGER variable takes an `int` value. + BINARY, ///< A BINARY variable takes an integer value from {0, 1}. + BOOLEAN, ///< A BOOLEAN variable takes a `bool` value. + RANDOM_UNIFORM, ///< A random variable whose value will be drawn from + ///< uniform real distributed ∈ [0,1). + RANDOM_GAUSSIAN, ///< A random variable whose value will be drawn from + ///< mean-zero, unit-variance normal. + RANDOM_EXPONENTIAL, ///< A random variable whose value will be drawn from + ///< exponential distribution with λ=1. + }; + + DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Variable) + + /** Default constructor. Constructs a dummy variable of CONTINUOUS type. This + * is needed to have Eigen::Matrix. The objects created by the + * default constructor share the same ID, zero. As a result, they all are + * identified as a single variable by equality operator (==). They all have + * the same hash value as well. + * + * It is allowed to construct a dummy variable but it should not be used to + * construct a symbolic expression. + */ + Variable() : name_{std::make_shared()} {} + + /** Constructs a default value. This overload is used by Eigen when + * EIGEN_INITIALIZE_MATRICES_BY_ZERO is enabled. + */ + explicit Variable(std::nullptr_t) : Variable() {} + + /** Constructs a variable with a string. If not specified, it has CONTINUOUS + * type by default.*/ + explicit Variable(std::string name, Type type = Type::CONTINUOUS); + + /** Checks if this is a dummy variable (ID = 0) which is created by + * the default constructor. */ + [[nodiscard]] bool is_dummy() const { return get_id() == 0; } + [[nodiscard]] Id get_id() const; + [[nodiscard]] Type get_type() const; + [[nodiscard]] std::string get_name() const; + [[nodiscard]] std::string to_string() const; + + /// Checks the equality of two variables based on their ID values. + [[nodiscard]] bool equal_to(const Variable& v) const { + return get_id() == v.get_id(); + } + + /// Compares two variables based on their ID values. + [[nodiscard]] bool less(const Variable& v) const { + return get_id() < v.get_id(); + } + + /** Implements the @ref hash_append concept. */ + template + friend void hash_append(HashAlgorithm& hasher, + const Variable& item) noexcept { + using drake::hash_append; + hash_append(hasher, item.id_); + // We do not send the type_ or name_ to the hasher, because the id_ is + // already unique across all instances, and two Variable instances with + // matching id_ will always have identical type_ and name_. + } + + friend std::ostream& operator<<(std::ostream& os, const Variable& var); + + private: + // Produces a unique ID for a variable. + static Id get_next_id(); + Id id_{}; // Unique identifier. + Type type_{Type::CONTINUOUS}; + + // Variable class has shared_ptr instead of string to be + // drake::test::IsMemcpyMovable. + // Please check https://github.com/RobotLocomotion/drake/issues/5974 + // for more information. + std::shared_ptr name_; // Name of variable. +}; + +std::ostream& operator<<(std::ostream& os, Variable::Type type); + +/// Creates a dynamically-sized Eigen matrix of symbolic variables. +/// @param rows The number of rows in the new matrix. +/// @param cols The number of cols in the new matrix. +/// @param name The common prefix for variables. +/// The (i, j)-th element will be named as `name(i, j)`. +/// @param type The type of variables in the matrix. +MatrixX MakeMatrixVariable( + int rows, int cols, const std::string& name, + Variable::Type type = Variable::Type::CONTINUOUS); + +/// Creates a dynamically-sized Eigen matrix of symbolic Boolean variables. +/// @param rows The number of rows in the new matrix. +/// @param cols The number of cols in the new matrix. +/// @param name The common prefix for variables. +/// The (i, j)-th element will be named as `name(i, j)`. +MatrixX MakeMatrixBooleanVariable(int rows, int cols, + const std::string& name); + +/// Creates a dynamically-sized Eigen matrix of symbolic binary variables. +/// @param rows The number of rows in the new matrix. +/// @param cols The number of cols in the new matrix. +/// @param name The common prefix for variables. +/// The (i, j)-th element will be named as `name(i, j)`. +MatrixX MakeMatrixBinaryVariable(int rows, int cols, + const std::string& name); + +/// Creates a dynamically-sized Eigen matrix of symbolic continuous variables. +/// @param rows The number of rows in the new matrix. +/// @param cols The number of cols in the new matrix. +/// @param name The common prefix for variables. +/// The (i, j)-th element will be named as `name(i, j)`. +MatrixX MakeMatrixContinuousVariable(int rows, int cols, + const std::string& name); + +/// Creates a dynamically-sized Eigen matrix of symbolic integer variables. +/// @param rows The number of rows in the new matrix. +/// @param cols The number of cols in the new matrix. +/// @param name The common prefix for variables. +/// The (i, j)-th element will be named as `name(i, j)`. +MatrixX MakeMatrixIntegerVariable(int rows, int cols, + const std::string& name); + +/// Creates a static-sized Eigen matrix of symbolic variables. +/// @tparam rows The number of rows in the new matrix. +/// @tparam cols The number of cols in the new matrix. +/// @param name The common prefix for variables. +/// The (i, j)-th element will be named as `name(i, j)`. +/// @param type The type of variables in the matrix. +template +Eigen::Matrix MakeMatrixVariable( + const std::string& name, Variable::Type type = Variable::Type::CONTINUOUS) { + Eigen::Matrix m; + for (int i = 0; i < rows; ++i) { + for (int j = 0; j < cols; ++j) { + m(i, j) = Variable{ + name + "(" + std::to_string(i) + ", " + std::to_string(j) + ")", + type}; + } + } + return m; +} + +/// Creates a static-sized Eigen matrix of symbolic Boolean variables. +/// @tparam rows The number of rows in the new matrix. +/// @tparam cols The number of cols in the new matrix. +/// @param name The common prefix for variables. +/// The (i, j)-th element will be named as `name(i, j)`. +template +Eigen::Matrix MakeMatrixBooleanVariable( + const std::string& name) { + return MakeMatrixVariable(name, Variable::Type::BOOLEAN); +} + +/// Creates a static-sized Eigen matrix of symbolic binary variables. +/// @tparam rows The number of rows in the new matrix. +/// @tparam cols The number of cols in the new matrix. +/// @param name The common prefix for variables. +/// The (i, j)-th element will be named as `name(i, j)`. +template +Eigen::Matrix MakeMatrixBinaryVariable( + const std::string& name) { + return MakeMatrixVariable(name, Variable::Type::BINARY); +} + +/// Creates a static-sized Eigen matrix of symbolic continuous variables. +/// @tparam rows The number of rows in the new matrix. +/// @tparam cols The number of cols in the new matrix. +/// @param name The common prefix for variables. +/// The (i, j)-th element will be named as `name(i, j)`. +template +Eigen::Matrix MakeMatrixContinuousVariable( + const std::string& name) { + return MakeMatrixVariable(name, Variable::Type::CONTINUOUS); +} + +/// Creates a static-sized Eigen matrix of symbolic integer variables. +/// @tparam rows The number of rows in the new matrix. +/// @tparam cols The number of cols in the new matrix. +/// @param name The common prefix for variables. +/// The (i, j)-th element will be named as `name(i, j)`. +template +Eigen::Matrix MakeMatrixIntegerVariable( + const std::string& name) { + return MakeMatrixVariable(name, Variable::Type::INTEGER); +} + +/// Creates a dynamically-sized Eigen vector of symbolic variables. +/// @param rows The size of vector. +/// @param name The common prefix for variables. +/// The i-th element will be named as `name(i)`. +/// @param type The type of variables in the vector. +VectorX MakeVectorVariable( + int rows, const std::string& name, + Variable::Type type = Variable::Type::CONTINUOUS); + +/// Creates a dynamically-sized Eigen vector of symbolic Boolean variables. +/// @param rows The size of vector. +/// @param name The common prefix for variables. +/// The i-th element will be named as `name(i)`. +VectorX MakeVectorBooleanVariable(int rows, const std::string& name); + +/// Creates a dynamically-sized Eigen vector of symbolic binary variables. +/// @param rows The size of vector. +/// @param name The common prefix for variables. +/// The i-th element will be named as `name(i)`. +VectorX MakeVectorBinaryVariable(int rows, const std::string& name); + +/// Creates a dynamically-sized Eigen vector of symbolic continuous variables. +/// @param rows The size of vector. +/// @param name The common prefix for variables. +/// The i-th element will be named as `name(i)`. +VectorX MakeVectorContinuousVariable(int rows, + const std::string& name); + +/// Creates a dynamically-sized Eigen vector of symbolic integer variables. +/// @param rows The size of vector. +/// @param name The common prefix for variables. +/// The i-th element will be named as `name(i)`. +VectorX MakeVectorIntegerVariable(int rows, const std::string& name); + +/// Creates a static-sized Eigen vector of symbolic variables. +/// @tparam rows The size of vector. +/// @param name The common prefix for variables. +/// The i-th element will be named as `name(i)`. +/// @param type The type of variables in the vector. +template +Eigen::Matrix MakeVectorVariable( + const std::string& name, Variable::Type type = Variable::Type::CONTINUOUS) { + Eigen::Matrix vec; + for (int i = 0; i < rows; ++i) { + vec[i] = Variable{name + "(" + std::to_string(i) + ")", type}; + } + return vec; +} + +/// Creates a static-sized Eigen vector of symbolic Boolean variables. +/// @tparam rows The size of vector. +/// @param name The common prefix for variables. +/// The i-th element will be named as `name(i)`. +template +Eigen::Matrix MakeVectorBooleanVariable( + const std::string& name) { + return MakeVectorVariable(name, Variable::Type::BOOLEAN); +} + +/// Creates a static-sized Eigen vector of symbolic binary variables. +/// @tparam rows The size of vector. +/// @param name The common prefix for variables. +/// The i-th element will be named as `name(i)`. +template +Eigen::Matrix MakeVectorBinaryVariable( + const std::string& name) { + return MakeVectorVariable(name, Variable::Type::BINARY); +} + +/// Creates a static-sized Eigen vector of symbolic continuous variables. +/// @tparam rows The size of vector. +/// @param name The common prefix for variables. +/// The i-th element will be named as `name(i)`. +template +Eigen::Matrix MakeVectorContinuousVariable( + const std::string& name) { + return MakeVectorVariable(name, Variable::Type::CONTINUOUS); +} + +/// Creates a static-sized Eigen vector of symbolic integer variables. +/// @tparam rows The size of vector. +/// @param name The common prefix for variables. +/// The i-th element will be named as `name(i)`. +template +Eigen::Matrix MakeVectorIntegerVariable( + const std::string& name) { + return MakeVectorVariable(name, Variable::Type::INTEGER); +} + +} // namespace symbolic +} // namespace drake + +namespace std { + +/* Provides std::hash. */ +template <> +struct hash : public drake::DefaultHash {}; + +/* Provides std::less. */ +template <> +struct less { + bool operator()(const drake::symbolic::Variable& lhs, + const drake::symbolic::Variable& rhs) const { + return lhs.less(rhs); + } +}; + +/* Provides std::equal_to. */ +template <> +struct equal_to { + bool operator()(const drake::symbolic::Variable& lhs, + const drake::symbolic::Variable& rhs) const { + return lhs.equal_to(rhs); + } +}; +} // namespace std + +#if !defined(DRAKE_DOXYGEN_CXX) +namespace Eigen { +// Eigen scalar type traits for Matrix. +template <> +struct NumTraits + : GenericNumTraits { + static inline int digits10() { return 0; } +}; +} // namespace Eigen +#endif // !defined(DRAKE_DOXYGEN_CXX) + +namespace drake { +namespace symbolic { +/// Checks if two Eigen::Matrix @p m1 and @p m2 are structurally +/// equal. That is, it returns true if and only if `m1(i, j)` is structurally +/// equal to `m2(i, j)` for all `i`, `j`. +template +typename std::enable_if_t::value && + is_eigen_scalar_same::value, + bool> +CheckStructuralEquality(const DerivedA& m1, const DerivedB& m2) { + EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(DerivedA, DerivedB); + DRAKE_DEMAND(m1.rows() == m2.rows() && m1.cols() == m2.cols()); + return m1.binaryExpr(m2, std::equal_to{}).all(); +} +} // namespace symbolic +} // namespace drake diff --git a/maliput_drake/include/drake/common/symbolic_variables.h b/maliput_drake/include/drake/common/symbolic_variables.h new file mode 100644 index 0000000..b7ece23 --- /dev/null +++ b/maliput_drake/include/drake/common/symbolic_variables.h @@ -0,0 +1,185 @@ +#pragma once + +#ifndef DRAKE_COMMON_SYMBOLIC_HEADER +#error Do not directly include this file. Include "drake/common/symbolic.h". +#endif + +#include +#include +#include +#include +#include +#include + +#include "drake/common/eigen_types.h" +#include "drake/common/hash.h" +#include "drake/common/symbolic.h" + +namespace drake { +namespace symbolic { + +/** Represents a set of variables. + * + * This class is based on std::set. The intent is to add things that + * we need including set-union (Variables::insert, operator+, operator+=), + * set-minus (Variables::erase, operator-, operator-=), and subset/superset + * checking functions (Variables::IsSubsetOf, Variables::IsSupersetOf, + * Variables::IsStrictSubsetOf, Variables::IsStrictSupersetOf). + */ +class Variables { + public: + DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Variables) + + typedef typename std::set::size_type size_type; + typedef typename std::set::iterator iterator; + typedef typename std::set::const_iterator const_iterator; + typedef typename std::set::reverse_iterator reverse_iterator; + typedef typename std::set::const_reverse_iterator + const_reverse_iterator; + + /** Default constructor. */ + Variables() = default; + + /** List constructor. */ + Variables(std::initializer_list init); + + /** Constructs from an Eigen vector of variables. */ + explicit Variables(const Eigen::Ref>& vec); + + /** Returns the number of elements. */ + [[nodiscard]] size_type size() const { return vars_.size(); } + + /** Checks if this set is empty or not. */ + [[nodiscard]] bool empty() const { return vars_.empty(); } + + /** Returns string representation of Variables. */ + [[nodiscard]] std::string to_string() const; + + /** Implements the @ref hash_append concept. */ + template + friend void hash_append(HashAlgorithm& hasher, + const Variables& item) noexcept { + using drake::hash_append; + hash_append(hasher, item.vars_); + } + + /** Returns an iterator to the beginning. */ + iterator begin() { return vars_.begin(); } + /** Returns an iterator to the end. */ + iterator end() { return vars_.end(); } + /** Returns an iterator to the beginning. */ + [[nodiscard]] const_iterator begin() const { return vars_.cbegin(); } + /** Returns an iterator to the end. */ + [[nodiscard]] const_iterator end() const { return vars_.cend(); } + /** Returns a const iterator to the beginning. */ + [[nodiscard]] const_iterator cbegin() const { return vars_.cbegin(); } + /** Returns a const iterator to the end. */ + [[nodiscard]] const_iterator cend() const { return vars_.cend(); } + /** Returns a reverse iterator to the beginning. */ + reverse_iterator rbegin() { return vars_.rbegin(); } + /** Returns a reverse iterator to the end. */ + reverse_iterator rend() { return vars_.rend(); } + /** Returns a reverse iterator to the beginning. */ + [[nodiscard]] const_reverse_iterator rbegin() const { + return vars_.crbegin(); + } + /** Returns a reverse iterator to the end. */ + [[nodiscard]] const_reverse_iterator rend() const { return vars_.crend(); } + /** Returns a const reverse-iterator to the beginning. */ + [[nodiscard]] const_reverse_iterator crbegin() const { + return vars_.crbegin(); + } + /** Returns a const reverse-iterator to the end. */ + [[nodiscard]] const_reverse_iterator crend() const { return vars_.crend(); } + + /** Inserts a variable @p var into a set. */ + void insert(const Variable& var) { vars_.insert(var); } + /** Inserts variables in [@p first, @p last) into a set. */ + template + void insert(InputIt first, InputIt last) { + vars_.insert(first, last); + } + /** Inserts variables in @p vars into a set. */ + void insert(const Variables& vars) { vars_.insert(vars.begin(), vars.end()); } + + /** Erases @p key from a set. Return number of erased elements (0 or 1). */ + size_type erase(const Variable& key) { return vars_.erase(key); } + + /** Erases variables in @p vars from a set. Return number of erased + elements ([0, vars.size()]). */ + size_type erase(const Variables& vars); + + /** Finds element with specific key. */ + iterator find(const Variable& key) { return vars_.find(key); } + [[nodiscard]] const_iterator find(const Variable& key) const { + return vars_.find(key); + } + + /** Return true if @p key is included in the Variables. */ + [[nodiscard]] bool include(const Variable& key) const { + return find(key) != end(); + } + + /** Return true if @p vars is a subset of the Variables. */ + [[nodiscard]] bool IsSubsetOf(const Variables& vars) const; + /** Return true if @p vars is a superset of the Variables. */ + [[nodiscard]] bool IsSupersetOf(const Variables& vars) const; + /** Return true if @p vars is a strict subset of the Variables. */ + [[nodiscard]] bool IsStrictSubsetOf(const Variables& vars) const; + /** Return true if @p vars is a strict superset of the Variables. */ + [[nodiscard]] bool IsStrictSupersetOf(const Variables& vars) const; + + friend bool operator==(const Variables& vars1, const Variables& vars2); + + friend bool operator<(const Variables& vars1, const Variables& vars2); + + friend std::ostream& operator<<(std::ostream&, const Variables& vars); + + friend Variables intersect(const Variables& vars1, const Variables& vars2); + + private: + /* Constructs from std::set. */ + explicit Variables(std::set vars); + + std::set vars_; +}; + +/** Updates @p var1 with the result of set-union(@p var1, @p var2). */ +// NOLINTNEXTLINE(runtime/references) per C++ standard signature. +Variables operator+=(Variables& vars1, const Variables& vars2); +/** Updates @p vars with the result of set-union(@p vars, { @p var }). */ +// NOLINTNEXTLINE(runtime/references) per C++ standard signature. +Variables operator+=(Variables& vars, const Variable& var); +/** Returns set-union of @p var1 and @p var2. */ +Variables operator+(Variables vars1, const Variables& vars2); +/** Returns set-union of @p vars and {@p var}. */ +Variables operator+(Variables vars, const Variable& var); +/** Returns set-union of {@p var} and @p vars. */ +Variables operator+(const Variable& var, Variables vars); + +/** Updates @p var1 with the result of set-minus(@p var1, @p var2). */ +// NOLINTNEXTLINE(runtime/references) per C++ standard signature. +Variables operator-=(Variables& vars1, const Variables& vars2); +/** Updates @p vars with the result of set-minus(@p vars, {@p var}). */ +// NOLINTNEXTLINE(runtime/references) per C++ standard signature. +Variables operator-=(Variables& vars, const Variable& var); +/** Returns set-minus(@p var1, @p vars2). */ +Variables operator-(Variables vars1, const Variables& vars2); +/** Returns set-minus(@p vars, { @p var }). */ +Variables operator-(Variables vars, const Variable& var); + +/** Returns the intersection of @p vars1 and @p vars2. + * + * This function has a time complexity of `O(N₁ + N₂)` where `N₁` and `N₂` are + * the size of @p vars1 and @p vars2 respectively. + */ +Variables intersect(const Variables& vars1, const Variables& vars2); + +} // namespace symbolic +} // namespace drake + +namespace std { +/* Provides std::hash. */ +template <> +struct hash : public drake::DefaultHash {}; +} // namespace std diff --git a/maliput_drake/include/drake/common/temp_directory.h b/maliput_drake/include/drake/common/temp_directory.h new file mode 100644 index 0000000..9488089 --- /dev/null +++ b/maliput_drake/include/drake/common/temp_directory.h @@ -0,0 +1,17 @@ +#pragma once + +#include + +namespace drake { + +/// Returns a directory location suitable for temporary files. +/// @return The value of the environment variable TEST_TMPDIR if defined or +/// otherwise ${TMPDIR:-/tmp}/robotlocomotion_drake_XXXXXX where each X is +/// replaced by a character from the portable filename character set. Any +/// trailing / will be stripped from the output. +/// @throws std::exception If the path referred to by TEST_TMPDIR or +/// ${TMPDIR:-/tmp}/robotlocomotion_drake_XXXXXX cannot be created, does not +/// exist, or is not a directory. +std::string temp_directory(); + +} // namespace drake diff --git a/maliput_drake/include/drake/common/text_logging.h b/maliput_drake/include/drake/common/text_logging.h new file mode 100644 index 0000000..874771f --- /dev/null +++ b/maliput_drake/include/drake/common/text_logging.h @@ -0,0 +1,226 @@ +#pragma once + +/** +@file +This is the entry point for all text logging within Drake. +Once you've included this file, the suggested ways you +should write log messages include: +
+  drake::log()->trace("Some trace message: {} {}", something, some_other);
+
+Similarly, it provides: +
+  drake::log()->debug(...);
+  drake::log()->info(...);
+  drake::log()->warn(...);
+  drake::log()->error(...);
+  drake::log()->critical(...);
+
+If you want to log objects that are expensive to serialize, these macros will +not be compiled if debugging is turned off (-DNDEBUG is set): +
+  DRAKE_LOGGER_TRACE("message: {}", something_conditionally_compiled);
+  DRAKE_LOGGER_DEBUG("message: {}", something_conditionally_compiled);
+
+ +The format string syntax is fmtlib; see https://fmt.dev/latest/syntax.html. +In particular, any class that overloads `operator<<` for `ostream` can be +printed without any special handling. (Note that the documentation link +provides syntax for the latest version of fmtlib; the version of fmtlib +used by Drake might be older.) +*/ + +#include + +#ifndef DRAKE_DOXYGEN_CXX +#ifdef HAVE_SPDLOG +#ifndef NDEBUG + +// When in Debug builds, before including spdlog we set the compile-time +// minimum log threshold so that spdlog defaults to enabling all log levels. +#define SPDLOG_ACTIVE_LEVEL SPDLOG_LEVEL_TRACE + +// Provide operative macros only when spdlog is available and Debug is enabled. +#define DRAKE_LOGGER_TRACE(...) \ + do { \ + /* Capture the drake::log() in a temporary, using a relatively unique */ \ + /* variable name to avoid potential variable name shadowing warnings. */ \ + ::drake::logging::logger* const drake_spdlog_macro_logger_alias = \ + ::drake::log(); \ + if (drake_spdlog_macro_logger_alias->level() <= spdlog::level::trace) { \ + SPDLOG_LOGGER_TRACE(drake_spdlog_macro_logger_alias, __VA_ARGS__); \ + } \ + } while (0) +#define DRAKE_LOGGER_DEBUG(...) \ + do { \ + /* Capture the drake::log() in a temporary, using a relatively unique */ \ + /* variable name to avoid potential variable name shadowing warnings. */ \ + ::drake::logging::logger* const drake_spdlog_macro_logger_alias = \ + ::drake::log(); \ + if (drake_spdlog_macro_logger_alias->level() <= spdlog::level::debug) { \ + SPDLOG_LOGGER_DEBUG(drake_spdlog_macro_logger_alias, __VA_ARGS__); \ + } \ + } while (0) + +#else + +// Spdlog is available, but we are doing a non-Debug build. +#define DRAKE_LOGGER_TRACE(...) +#define DRAKE_LOGGER_DEBUG(...) + +#endif + +/* clang-format off */ +#include +#include +/* clang-format on */ + +#else // HAVE_SPDLOG + +// We always want text_logging.h to provide fmt support to those who include +// it, even if spdlog is disabled. + +/* clang-format off */ +#include +#include +/* clang-format on */ + +#endif // HAVE_SPDLOG +#endif // DRAKE_DOXYGEN_CXX + +#include "drake/common/drake_copyable.h" + +namespace drake { + +#ifdef HAVE_SPDLOG +namespace logging { + +// If we have spdlog, just alias logger into our namespace. +/// The drake::logging::logger class provides text logging methods. +/// See the text_logging.h documentation for a short tutorial. +using logger = spdlog::logger; + +/// When spdlog is enabled in this build, drake::logging::sink is an alias for +/// spdlog::sinks::sink. When spdlog is disabled, it is an empty class. +using spdlog::sinks::sink; + +/// True only if spdlog is enabled in this build. +constexpr bool kHaveSpdlog = true; + +} // namespace logging + +#else // HAVE_SPDLOG +// If we don't have spdlog, we need to stub out logger. + +namespace logging { +constexpr bool kHaveSpdlog = false; + +// A stubbed-out version of `spdlog::logger`. Implements only those methods +// that we expect to use, as spdlog's API does change from time to time. +class logger { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(logger) + + logger(); + + template + void trace(const char*, const Args&...) {} + template + void debug(const char*, const Args&...) {} + template + void info(const char*, const Args&...) {} + template + void warn(const char*, const Args&...) {} + template + void error(const char*, const Args&...) {} + template + void critical(const char*, const Args&...) {} + + template void trace(const T&) {} + template void debug(const T&) {} + template void info(const T&) {} + template void warn(const T&) {} + template void error(const T&) {} + template void critical(const T&) {} +}; + +// A stubbed-out version of `spdlog::sinks::sink`. +class sink { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(sink) + + sink(); +}; + +} // namespace logging + +#define DRAKE_LOGGER_TRACE(...) +#define DRAKE_LOGGER_DEBUG(...) + +#endif // HAVE_SPDLOG + +/// Retrieve an instance of a logger to use for logging; for example: +/// 
+///   drake::log()->info("potato!")
+///
+/// +/// See the text_logging.h documentation for a short tutorial. +logging::logger* log(); + +namespace logging { + +/// (Advanced) Retrieves the default sink for all Drake logs. When spdlog is +/// enabled, the return value can be cast to spdlog::sinks::dist_sink_mt and +/// thus allows consumers of Drake to redirect Drake's text logs to locations +/// other than the default of stderr. When spdlog is disabled, the return +/// value is an empty class. +sink* get_dist_sink(); + +/// When constructed, logs a message (at "warn" severity); the destructor is +/// guaranteed to be trivial. This is useful for declaring an instance of this +/// class as a function-static global, so that a warning is logged the first +/// time the program encounters some code, but does not repeat the warning on +/// subsequent encounters within the same process. +/// +/// For example: +/// 
+/// double* SanityCheck(double* data) {
+///   if (!data) {
+///     static const logging::Warn log_once("Bad data!");
+///     return alternative_data();
+///   }
+///   return data;
+/// }
+///
+struct Warn { + template + Warn(const char* a, const Args&... b) { + drake::log()->warn(a, b...); + } +}; + +/// Invokes `drake::log()->set_level(level)`. +/// @param level Must be a string from spdlog enumerations: `trace`, `debug`, +/// `info`, `warn`, `err`, `critical`, `off`, or `unchanged` (not an enum, but +/// useful for command-line). +/// @return The string value of the previous log level. If SPDLOG is disabled, +/// then this returns an empty string. +std::string set_log_level(const std::string& level); + +/// The "unchanged" string to pass to set_log_level() so as to achieve a no-op. +extern const char* const kSetLogLevelUnchanged; + +/// An end-user help string suitable to describe the effects of set_log_level(). +extern const char* const kSetLogLevelHelpMessage; + +/// Invokes `drake::log()->set_pattern(pattern)`. +/// @param pattern Formatting for message. For more information, see: +/// https://github.com/gabime/spdlog/wiki/3.-Custom-formatting +void set_log_pattern(const std::string& pattern); + +/// An end-user help string suitable to describe the effects of +/// set_log_pattern(). +extern const char* const kSetLogPatternHelpMessage; + +} // namespace logging +} // namespace drake diff --git a/maliput_drake/include/drake/common/trajectories/bspline_trajectory.h b/maliput_drake/include/drake/common/trajectories/bspline_trajectory.h new file mode 100644 index 0000000..038d479 --- /dev/null +++ b/maliput_drake/include/drake/common/trajectories/bspline_trajectory.h @@ -0,0 +1,144 @@ +#pragma once + +#include +#include + +#include "drake/common/drake_bool.h" +#include "drake/common/drake_copyable.h" +#include "drake/common/drake_throw.h" +#include "drake/common/eigen_types.h" +#include "drake/common/name_value.h" +#include "drake/common/trajectories/trajectory.h" +#include "drake/math/bspline_basis.h" + +namespace drake { +namespace trajectories { +/** Represents a B-spline curve using a given `basis` with ordered +`control_points` such that each control point is a matrix in ℝʳᵒʷˢ ˣ ᶜᵒˡˢ. +@see math::BsplineBasis */ +template +class BsplineTrajectory final : public trajectories::Trajectory { + public: + DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(BsplineTrajectory); + + BsplineTrajectory() : BsplineTrajectory({}, {}) {} + + /** Constructs a B-spline trajectory with the given `basis` and + `control_points`. + @pre control_points.size() == basis.num_basis_functions() */ + BsplineTrajectory(math::BsplineBasis basis, + std::vector> control_points); + +#ifdef DRAKE_DOXYGEN_CXX + /** Constructs a T-valued B-spline trajectory from a double-valued `basis` and + T-valued `control_points`. + @pre control_points.size() == basis.num_basis_functions() */ + BsplineTrajectory(math::BsplineBasis basis, + std::vector> control_points); +#else + template + BsplineTrajectory(math::BsplineBasis basis, + std::vector> control_points, + typename std::enable_if_t>* = {}) + : BsplineTrajectory(math::BsplineBasis(basis), control_points) {} +#endif + + virtual ~BsplineTrajectory() = default; + + // Required methods for trajectories::Trajectory interface. + std::unique_ptr> Clone() const override; + + /** Evaluates the BsplineTrajectory at the given time t. + @param t The time at which to evaluate the %BsplineTrajectory. + @return The matrix of evaluated values. + @pre If T == symbolic::Expression, `t.is_constant()` must be true. + @warning If t does not lie in the range [start_time(), end_time()], the + trajectory will silently be evaluated at the closest + valid value of time to t. For example, `value(-1)` will return + `value(0)` for a trajectory defined over [0, 1]. */ + MatrixX value(const T& time) const override; + + Eigen::Index rows() const override { return control_points()[0].rows(); } + + Eigen::Index cols() const override { return control_points()[0].cols(); } + + T start_time() const override { return basis_.initial_parameter_value(); } + + T end_time() const override { return basis_.final_parameter_value(); } + + // Other methods + /** Returns the number of control points in this curve. */ + int num_control_points() const { return basis_.num_basis_functions(); } + + /** Returns the control points of this curve. */ + const std::vector>& control_points() const { + return control_points_; + } + + /** Returns this->value(this->start_time()) */ + MatrixX InitialValue() const; + + /** Returns this->value(this->end_time()) */ + MatrixX FinalValue() const; + + /** Returns the basis of this curve. */ + const math::BsplineBasis& basis() const { return basis_; } + + /** Adds new knots at the specified `additional_knots` without changing the + behavior of the trajectory. The basis and control points of the trajectory are + adjusted such that it produces the same value for any valid time before and + after this method is called. The resulting trajectory is guaranteed to have + the same level of continuity as the original, even if knot values are + duplicated. Note that `additional_knots` need not be sorted. + @pre start_time() <= t <= end_time() for all t in `additional_knots` */ + void InsertKnots(const std::vector& additional_knots); + + /** Returns a new BsplineTrajectory that uses the same basis as `this`, and + whose control points are the result of calling `select(point)` on each `point` + in `this->control_points()`.*/ + BsplineTrajectory CopyWithSelector( + const std::function(const MatrixX&)>& select) const; + + /** Returns a new BsplineTrajectory that uses the same basis as `this`, and + whose control points are the result of calling + `point.block(start_row, start_col, block_rows, block_cols)` on each `point` + in `this->control_points()`.*/ + BsplineTrajectory CopyBlock(int start_row, int start_col, + int block_rows, int block_cols) const; + + /** Returns a new BsplineTrajectory that uses the same basis as `this`, and + whose control points are the result of calling `point.head(n)` on each `point` + in `this->control_points()`. + @pre this->cols() == 1 + @pre control_points()[0].head(n) must be a valid operation. */ + BsplineTrajectory CopyHead(int n) const; + + boolean operator==(const BsplineTrajectory& other) const; + + /** Passes this object to an Archive; see @ref serialize_tips for background. + This method is only available when T = double. */ + template +#ifdef DRAKE_DOXYGEN_CXX + void +#else + // Restrict this method to T = double only; we must mix "Archive" into the + // conditional type for SFINAE to work, so we just check it against void. + std::enable_if_t && !std::is_void_v> +#endif + Serialize(Archive* a) { + a->Visit(MakeNameValue("basis", &basis_)); + a->Visit(MakeNameValue("control_points", &control_points_)); + DRAKE_THROW_UNLESS(CheckInvariants()); + } + + private: + std::unique_ptr> DoMakeDerivative( + int derivative_order) const override; + + bool CheckInvariants() const; + + math::BsplineBasis basis_; + std::vector> control_points_; +}; +} // namespace trajectories +} // namespace drake diff --git a/maliput_drake/include/drake/common/trajectories/discrete_time_trajectory.h b/maliput_drake/include/drake/common/trajectories/discrete_time_trajectory.h new file mode 100644 index 0000000..53b8cdb --- /dev/null +++ b/maliput_drake/include/drake/common/trajectories/discrete_time_trajectory.h @@ -0,0 +1,139 @@ +#pragma once + +#include +#include +#include + +#include + +#include "drake/common/drake_assert.h" +#include "drake/common/drake_copyable.h" +#include "drake/common/eigen_types.h" +#include "drake/common/trajectories/piecewise_polynomial.h" +#include "drake/common/trajectories/trajectory.h" + +namespace drake { +namespace trajectories { + +/** A DiscreteTimeTrajectory is a Trajectory whose value is only defined at +discrete time points. Calling `value()` at a time that is not equal to one of +those times (up to a tolerance) will throw. This trajectory does *not* have +well-defined time-derivatives. + +In some applications, it may be preferable to use +PiecewisePolynomial::ZeroOrderHold instead of a DiscreteTimeTrajectory (and +we offer a method here to easily convert). Note if the breaks are periodic, +then one can also achieve a similar result in a Diagram by using the +DiscreteTimeTrajectory in a TrajectorySource and connecting a ZeroOrderHold +system to the output port, but remember that this will add discrete state to +your diagram. + +So why not always use the zero-order hold (ZOH) trajectory? This class forces +us to be more precise in our implementations. For instance, consider the case +of a solution to a discrete-time finite-horizon linear quadratic regulator +(LQR) problem. In this case, the solution to the Riccati equation is a +DiscreteTimeTrajectory, K(t). Implementing +@verbatim +x(t) -> MatrixGain(-K(t)) -> u(t) +@verbatim +in a block diagram is perfectly correct, and if the u(t) is only connected to +the original system that it was designed for, then K(t) will only get evaluated +at the defined sample times, and all is well. But if you wire it up to a +continuous-time system, then K(t) may be evaluated at arbitrary times, and may +throw. If one wishes to use the K(t) solution on a continuous-time system, then +we can use +@verbatim +x(t) -> MatrixGain(-K(t)) -> ZOH -> u(t). +@verbatim +This is different, and *more correct* than implementing K(t) as a zero-order +hold trajectory, because in this version, both K(t) and the inputs x(t) will +only be evaluated at the discrete-time input. If `t_s` was the most recent +discrete sample time, then this means u(t) = -K(t_s)*x(t_s) instead of u(t) = +-K(t_s)*x(t). Using x(t_s) and having a true zero-order hold on u(t) is the +correct model for the discrete-time LQR result. + +@tparam_default_scalars +*/ +template +class DiscreteTimeTrajectory final : public Trajectory { + public: + // We are final, so this is okay. + DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(DiscreteTimeTrajectory) + + /** Default constructor creates the empty trajectory. */ + DiscreteTimeTrajectory() = default; + + /** Constructs a trajectory of vector @p values at the specified @p times. + @pre @p times must differ by more than @p time_comparison_tolerance and be + monotonically increasing. + @pre @p values must have times.size() columns. + @pre @p time_comparison_tolerance must be >= 0. + @throw if T=symbolic:Expression and @p times are not constants. */ + DiscreteTimeTrajectory(const Eigen::Ref>& times, + const Eigen::Ref>& values, + double time_comparison_tolerance = + std::numeric_limits::epsilon()); + + /** Constructs a trajectory of matrix @p values at the specified @p times. + @pre @p times should differ by more than @p time_comparison_tolerance and be + monotonically increasing. + @pre @p values must have times.size() elements, each with the same number of + rows and columns. + @pre @p time_comparison_tolerance must be >= 0. + @throw if T=symbolic:Expression and @p times are not constants. */ + DiscreteTimeTrajectory(const std::vector& times, + const std::vector>& values, + double time_comparison_tolerance = + std::numeric_limits::epsilon()); + + /** Converts the discrete-time trajectory using + PiecewisePolynomial::ZeroOrderHold(). */ + PiecewisePolynomial ToZeroOrderHold() const; + + /** The trajectory is only defined at finite sample times. This method + returns the tolerance used determine which time sample (if any) matches a + query time on calls to value(t). */ + double time_comparison_tolerance() const; + + /** Returns the number of discrete times where the trajectory value is + defined. */ + int num_times() const; + + /** Returns the times where the trajectory value is defined. */ + const std::vector& get_times() const; + + /** Returns a deep copy of the trajectory. */ + std::unique_ptr> Clone() const override; + + /** Returns the value of the trajectory at @p t. + @throws std::exception if t is not within tolerance of one of the sample + times. */ + MatrixX value(const T& t) const override; + + /** Returns the number of rows in the MatrixX returned by value(). + @pre num_times() > 0. */ + Eigen::Index rows() const override; + + /** Returns the number of cols in the MatrixX returned by value(). + @pre num_times() > 0. */ + Eigen::Index cols() const override; + + /** Returns the minimum value of get_times(). + @pre num_times() > 0. */ + T start_time() const override; + + /** Returns the maximum value of get_times(). + @pre num_times() > 0. */ + T end_time() const override; + + private: + std::vector times_; + std::vector> values_; + double time_comparison_tolerance_{}; +}; + +} // namespace trajectories +} // namespace drake + +DRAKE_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class drake::trajectories::DiscreteTimeTrajectory) diff --git a/maliput_drake/include/drake/common/trajectories/exponential_plus_piecewise_polynomial.h b/maliput_drake/include/drake/common/trajectories/exponential_plus_piecewise_polynomial.h new file mode 100644 index 0000000..b1ed49b --- /dev/null +++ b/maliput_drake/include/drake/common/trajectories/exponential_plus_piecewise_polynomial.h @@ -0,0 +1,82 @@ +#pragma once + +#include +#include + +#include + +#include "drake/common/drake_assert.h" +#include "drake/common/drake_copyable.h" +#include "drake/common/eigen_types.h" +#include "drake/common/trajectories/piecewise_polynomial.h" + +namespace drake { +namespace trajectories { + +/** + * y(t) = K * exp(A * (t - t_j)) * alpha.col(j) + piecewise_polynomial_part(t) + * + * @tparam_double_only + */ +template +class ExponentialPlusPiecewisePolynomial final + : public PiecewiseTrajectory { + public: + // We are final, so this is okay. + DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(ExponentialPlusPiecewisePolynomial) + + ExponentialPlusPiecewisePolynomial() = default; + + template + ExponentialPlusPiecewisePolynomial( + const Eigen::MatrixBase& K, + const Eigen::MatrixBase& A, + const Eigen::MatrixBase& alpha, + const PiecewisePolynomial& piecewise_polynomial_part) + : PiecewiseTrajectory(piecewise_polynomial_part), + K_(K), + A_(A), + alpha_(alpha), + piecewise_polynomial_part_(piecewise_polynomial_part) { + DRAKE_ASSERT(K.rows() == rows()); + DRAKE_ASSERT(K.cols() == A.rows()); + DRAKE_ASSERT(A.rows() == A.cols()); + DRAKE_ASSERT(alpha.rows() == A.cols()); + DRAKE_ASSERT(alpha.cols() == + piecewise_polynomial_part.get_number_of_segments()); + DRAKE_ASSERT(piecewise_polynomial_part.rows() == rows()); + DRAKE_ASSERT(piecewise_polynomial_part.cols() == 1); + } + + // from PiecewisePolynomial + ExponentialPlusPiecewisePolynomial( + const PiecewisePolynomial& piecewise_polynomial_part); + + ~ExponentialPlusPiecewisePolynomial() override = default; + + std::unique_ptr> Clone() const override; + + MatrixX value(const T& t) const override; + + ExponentialPlusPiecewisePolynomial derivative(int derivative_order = 1) const; + + Eigen::Index rows() const override; + + Eigen::Index cols() const override; + + void shiftRight(double offset); + + private: + std::unique_ptr> DoMakeDerivative( + int derivative_order = 1) const override { + return derivative(derivative_order).Clone(); + }; + + MatrixX K_; + MatrixX A_; + MatrixX alpha_; + PiecewisePolynomial piecewise_polynomial_part_; +}; + +} // namespace trajectories +} // namespace drake diff --git a/maliput_drake/include/drake/common/trajectories/piecewise_polynomial.h b/maliput_drake/include/drake/common/trajectories/piecewise_polynomial.h new file mode 100644 index 0000000..3acf6d0 --- /dev/null +++ b/maliput_drake/include/drake/common/trajectories/piecewise_polynomial.h @@ -0,0 +1,858 @@ +#pragma once + +#include +#include +#include + +#include + +#include "drake/common/default_scalars.h" +#include "drake/common/drake_copyable.h" +#include "drake/common/eigen_types.h" +#include "drake/common/polynomial.h" +#include "drake/common/trajectories/piecewise_trajectory.h" + +namespace drake { +namespace trajectories { + +/** + * A scalar multi-variate piecewise polynomial. + * + * %PiecewisePolynomial represents a list of contiguous segments in a scalar + * independent variable (typically corresponding to time) with Polynomials + * defined at each segment. We call the output from evaluating the + * %PiecewisePolynomial at the scalar independent variable "the output", and + * that output can be either a Eigen MatrixX (if evaluated using value()) + * or a scalar (if evaluated using scalar_value()). + * + * An example of a piecewise polynomial is a function of m segments in time, + * where a different polynomial is defined for each segment. For a specific + * example, consider the absolute value function over the interval [-1, 1]. + * We can define a %PiecewisePolynomial over this interval using breaks at + * t = { -1.0, 0.0, 1.0 }, and "samples" of abs(t). + * + * @code + * // Construct the PiecewisePolynomial. + * const std::vector breaks = { -1.0, 0.0, 1.0 }; + * std::vector samples(3); + * for (int i = 0; i < static_cast(breaks.size()); ++i) { + * samples[i].resize(1, 1); + * samples[i](0, 0) = std::abs(breaks[i]); + * } + * const auto pp = + * PiecewisePolynomial::FirstOrderHold(breaks, samples); + * const int row = 0, col = 0; + * + * // Evaluate the PiecewisePolynomial at some values. + * std::cout << pp.value(-.5)(row, col) << std::endl; // Outputs 0.5. + * std::cout << pp.value(0.0)(row, col) << std::endl; // Outputs 0.0; + * + * // Show how we can evaluate the first derivative (outputs -1.0). + * std::cout << pp.derivative(1).value(-.5)(row, col) << std::endl; + * @endcode + * + * A note on terminology. For piecewise-polynomial interpolation, we use + * `breaks` to indicate the scalar (e.g. times) which form the boundary of + * each segment. We use `samples` to indicate the function value at the + * `breaks`, e.g. `p(breaks[i]) = samples[i]`. The term `knot` should be + * reserved for the "(x,y)" coordinate, here + * `knot[i] = (breaks[i], samples[i])`, though it is used inconsistently in + * the interpolation literature (sometimes for `breaks`, sometimes for + * `samples`), so we try to mostly avoid it here. + * + * PiecewisePolynomial objects can be added, subtracted, and multiplied. + * They cannot be divided because Polynomials are not closed + * under division. + * + * @warning %PiecewisePolynomial silently clips input evaluations outside of + * the range defined by the breaks. So `pp.value(-2.0, row, col)` in the example + * above would evaluate to -1.0. See value(). + * + * @tparam_default_scalars + */ +template +class PiecewisePolynomial final : public PiecewiseTrajectory { + public: + /** + * Constructs an empty piecewise polynomial. + */ + PiecewisePolynomial() = default; + + // We are final, so this is okay. + DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(PiecewisePolynomial) + + typedef MatrixX> PolynomialMatrix; + + /** + * Single segment, constant value constructor over the interval [-∞, ∞]. + * The constructed %PiecewisePolynomial will return `constant_value` at + * every evaluated point (i.e., `value(t) = constant_value` ∀t ∈ [-∞, ∞]). + */ + template + explicit PiecewisePolynomial(const Eigen::MatrixBase& constant_value) + : PiecewiseTrajectory( + std::vector({-std::numeric_limits::infinity(), + std::numeric_limits::infinity()})) { + polynomials_.push_back(constant_value.template cast>()); + } + + /** + * @anchor polynomial_construction_methods + * @name Polynomial-based construction methods. + * Various methods for constructing a %PiecewisePolynomial using vectors + * of matrices of polynomials, one for each output dimension. Unlike the + * coefficient-based methods, the number of polynomials must equal the number + * of segments, which will be one fewer than the number of breaks. + * + * The following shows how such a %PiecewisePolynomial might be constructed + * and used: + * @code + * // Construct the PiecewisePolynomial. + * const std::vector breaks = { -1.0, 0.0, 1.0 }; + * Polynomiald t("t"); + * std::vector polynomials = { -(t*t), (t*t) }; + * const PiecewisePolynomial pp(polynomials, breaks); + * + * // Evaluate the PiecewisePolynomial at some values. + * std::cout << pp.scalar_value(-1.0) << std::endl; // Outputs -1.0 + * std::cout << pp.scalar_value(1.0) << std::endl; // Outputs 1.0 + * @endcode + * + * @anchor polynomial_warning + * WARNING: For robust floating point arithmetic, the polynomial for + * a segment will be evaluated (using value()) by first + * subtracting the break time from the evaluation time. In other words, when t + * lies in the half-open interval `[breaks[i], breaks[i+1])` then: + * @code + * value(t) == polynomials[i].eval(t - breaks[i]) + * @endcode + * meaning that constructing the polynomial like: + * @code + * const std::vector breaks = { 0.0, 1.0, 2.0 }; + * Polynomiald t("t"); + * std::vector polynomials = { (t*t), (t*t) }; + * const PiecewisePolynomial pp(polynomials, breaks); + * @endcode + * would give the following result: + * @code + * // Evaluate the PiecewisePolynomial on both sides of a break. + * const int row = 0, col = 0; + * const double eps = 0.5 * std::numeric_limits::epsilon(); + * std::cout << pp.value(1.0-eps)(row, col) << std::endl; // Outputs 1.0 + * std::cout << pp.value(1.0+eps)(row, col) << std::endl; // Outputs 1e-32 + * @endcode + * because the second polynomial will be evaluated at 1.0+eps minus the break + * time for that polynomial (1.0), i.e., t=eps. + * The intended result for the above example can be obtained by shifting the + * piecewise polynomial like so: + * @code + * const std::vector breaks = { 0.0, 1.0, 2.0 }; + * Polynomiald t("t"); + * std::vector polynomials = { (t*t), + * ((t+breaks[1])*(t+breaks[1])) }; + * const PiecewisePolynomial pp(polynomials, breaks); + * + * // Evaluate the PiecewisePolynomial on both sides of a break. + * const double eps = 0.5 * std::numeric_limits::epsilon(); + * std::cout << pp.value(1.0-eps)(row, col) << std::endl; // Outputs 1.0 + * std::cout << pp.value(1.0+eps)(row, col) << std::endl; // Outputs 1.0 + * @endcode + */ + // @{ + + /** + * Constructs a %PiecewisePolynomial using matrix-output Polynomials defined + * over each segment. + * + * @pre `polynomials.size() == breaks.size() - 1` + */ + PiecewisePolynomial(const std::vector& polynomials_matrix, + const std::vector& breaks); + + /** + * Constructs a %PiecewisePolynomial using scalar-output Polynomials defined + * over each segment. + * + * @pre `polynomials.size() == breaks.size() - 1` + */ + PiecewisePolynomial(const std::vector>& polynomials, + const std::vector& breaks); + // @} + + ~PiecewisePolynomial() override = default; + + std::unique_ptr> Clone() const override; + + /** + * @anchor coefficient_construction_methods + * @name Coefficient-based construction methods. + * Various methods for constructing a %PiecewisePolynomial using samples of + * coefficient matrices. Under the hood, %PiecewisePolynomial constructs + * interpolating Polynomial objects that pass through the sample points. These + * methods differ by the continuity constraints that they enforce at break + * points and whether each sample represents a full matrix (versions taking + * `const std::vector>&`) or a column vector (versions + * taking `const Eigen::Ref>&`). + * + * These methods will throw `std::exception` if: + * - the breaks and samples have different length, + * - the breaks are not strictly increasing, + * - the samples have inconsistent dimensions (i.e., the matrices do not all + * have identical dimensions), + * - the breaks vector has length smaller than 2. + */ + // @{ + + /** + * Constructs a piecewise constant %PiecewisePolynomial using matrix samples. + * Note that constructing a %PiecewisePolynomial requires at least two sample + * points, although in this case, the second sample point's value is ignored, + * and only its break time is used. + * + * @throws std::exception under the conditions specified under + * @ref coefficient_construction_methods. + * @pydrake_mkdoc_identifier{matrix} + */ + static PiecewisePolynomial ZeroOrderHold( + const std::vector& breaks, + const std::vector>& samples); + + /** + * Version of ZeroOrderHold(breaks, samples) that uses vector samples and + * Eigen VectorXd/MatrixX arguments. Each column of `samples` represents a + * sample point. + * + * @pre `samples.cols() == breaks.size()` + * @throws std::exception under the conditions specified under + * @ref coefficient_construction_methods. + * @pydrake_mkdoc_identifier{vector} + */ + static PiecewisePolynomial ZeroOrderHold( + const Eigen::Ref>& breaks, + const Eigen::Ref>& samples); + + /** + * Constructs a piecewise linear %PiecewisePolynomial using matrix samples. + * + * @throws std::exception under the conditions specified under + * @ref coefficient_construction_methods. + * @pydrake_mkdoc_identifier{matrix} + */ + static PiecewisePolynomial FirstOrderHold( + const std::vector& breaks, + const std::vector>& samples); + + /** + * Version of FirstOrderHold(breaks, samples) that uses vector samples and + * Eigen VectorXd / MatrixX arguments. Each column of `samples` represents + * a sample point. + * + * @pre `samples.cols() == breaks.size()` + * @throws std::exception under the conditions specified under + * @ref coefficient_construction_methods. + * @pydrake_mkdoc_identifier{vector} + */ + static PiecewisePolynomial FirstOrderHold( + const Eigen::Ref>& breaks, + const Eigen::Ref>& samples); + + // TODO(russt): This version of the method is not exposed in pydrake, but + // the version that is has limited documentation that refers back to this + // verbose version. Either add support for this in pydrake, or flip the + // documentation so that pydrake gets the verbose/stand-along version. + /** + * Constructs a third order %PiecewisePolynomial using vector samples, + * where each column of `samples` represents a sample point. First derivatives + * are chosen to be "shape preserving", i.e. if `samples` is monotonic + * within some interval, the interpolated data will also be monotonic. The + * second derivative is not guaranteed to be smooth across the entire spline. + * + * MATLAB calls this method "pchip" (short for "Piecewise Cubic Hermite + * Interpolating Polynomial"), and provides a nice description in their + * documentation. + * http://home.uchicago.edu/~sctchoi/courses/cs138/interp.pdf is also a good + * reference. + * + * If `zero_end_point_derivatives` is `false`, the first and last first + * derivative is chosen using a non-centered, shape-preserving three-point + * formulae. See equation (2.10) in the following reference for more details. + * http://www.mi.sanu.ac.rs/~gvm/radovi/mon.pdf + * If `zero_end_point_derivatives` is `true`, they are set to zeros. + * + * If `zero_end_point_derivatives` is `false`, `breaks` and `samples` must + * have at least 3 elements for the algorithm to determine the first + * derivatives. + * + * If `zero_end_point_derivatives` is `true`, `breaks` and `samples` may have + * 2 or more elements. For the 2 elements case, the result is equivalent to + * computing a cubic polynomial whose values are given by `samples`, and + * derivatives set to zero. + * + * @throws std::exception if: + * - `breaks` has length smaller than 3 and `zero_end_point_derivatives` is + * `false`, + * - `breaks` has length smaller than 2 and `zero_end_point_derivatives` is + * true. + * + * @throws std::exception under the conditions specified under + * @ref coefficient_construction_methods. + * @pydrake_mkdoc_identifier{matrix} + */ + static PiecewisePolynomial CubicShapePreserving( + const std::vector& breaks, + const std::vector>& samples, + bool zero_end_point_derivatives = false); + + /** + * Version of CubicShapePreserving(breaks, samples, + * zero_end_point_derivatives) that uses vector samples and Eigen VectorXd and + * MatrixX arguments. Each column of `samples` represents a sample point. + * + * @pre `samples.cols() == breaks.size()`. + * @throws std::exception under the conditions specified under + * @ref coefficient_construction_methods. + * @pydrake_mkdoc_identifier{vector} + */ + static PiecewisePolynomial CubicShapePreserving( + const Eigen::Ref>& breaks, + const Eigen::Ref>& samples, + bool zero_end_point_derivatives = false); + + /** + * Constructs a third order %PiecewisePolynomial using matrix samples. + * The %PiecewisePolynomial is constructed such that the interior segments + * have the same value, first and second derivatives at `breaks`. + * `sample_dot_at_start` and `sample_dot_at_end` are used for the first and + * last first derivatives. + * + * @throws std::exception if `sample_dot_at_start` or `sample_dot_at_end` + * and `samples` have inconsistent dimensions. + * @throws std::exception under the conditions specified under + * @ref coefficient_construction_methods. + * @pydrake_mkdoc_identifier{4args_matrix} + */ + static PiecewisePolynomial CubicWithContinuousSecondDerivatives( + const std::vector& breaks, + const std::vector>& samples, + const MatrixX& sample_dot_at_start, + const MatrixX& sample_dot_at_end); + + /** + * Version of CubicWithContinuousSecondDerivatives() that uses vector + * samples and Eigen VectorXd / MatrixX arguments. Each column of + * `samples` represents a sample point. + * + * @pre `samples.cols() == breaks.size()`. + * @throws std::exception under the conditions specified under + * @ref coefficient_construction_methods. + * @pydrake_mkdoc_identifier{4args_vector} + */ + static PiecewisePolynomial CubicWithContinuousSecondDerivatives( + const Eigen::Ref>& breaks, + const Eigen::Ref>& samples, + const Eigen::Ref>& sample_dot_at_start, + const Eigen::Ref>& sample_dot_at_end); + + /** + * Constructs a third order %PiecewisePolynomial using matrix samples and + * derivatives of samples (`samples_dot`); each matrix element of + * `samples_dot` represents the derivative with respect to the independent + * variable (e.g., the time derivative) of the corresponding entry in + * `samples`. Each segment is fully specified by `samples` and `sample_dot` at + * both ends. Second derivatives are not continuous. + * + * @pydrake_mkdoc_identifier{matrix} + */ + static PiecewisePolynomial CubicHermite( + const std::vector& breaks, + const std::vector>& samples, + const std::vector>& samples_dot); + + /** + * Version of CubicHermite(breaks, samples, samples_dot) that uses vector + * samples and Eigen VectorXd / MatrixX arguments. Corresponding columns of + * `samples` and `samples_dot` are used as the sample point and independent + * variable derivative, respectively. + * + * @pre `samples.cols() == samples_dot.cols() == breaks.size()`. + * @pydrake_mkdoc_identifier{vector} + */ + static PiecewisePolynomial CubicHermite( + const Eigen::Ref>& breaks, + const Eigen::Ref>& samples, + const Eigen::Ref>& samples_dot); + + /** + * Constructs a third order %PiecewisePolynomial using matrix samples. + * The %PiecewisePolynomial is constructed such that the interior segments + * have the same value, first and second derivatives at `breaks`. If + * `periodic_end_condition` is `false` (default), then the "Not-a-sample" end + * condition is used here, which means the third derivatives are + * continuous for the first two and last two segments. If + * `periodic_end_condition` is `true`, then the first and second derivatives + * between the end of the last segment and the beginning of the first + * segment will be continuous. Note that the periodic end condition does + * not require the first and last sample to be collocated, nor does it add + * an additional sample to connect the first and last segments. Only first + * and second derivative continuity is enforced. + * See https://en.wikipedia.org/wiki/Spline_interpolation and + * https://www.math.uh.edu/~jingqiu/math4364/spline.pdf + * for more about cubic splines and their end conditions. + * The MATLAB docs for methods "spline" and "csape" are also good + * references. + * + * @pre `breaks` and `samples` must have at least 3 elements. If + * `periodic_end_condition` is `true`, then for two samples, it would + * produce a straight line (use `FirstOrderHold` for this instead), and if + * `periodic_end_condition` is `false` the problem is ill-defined. + * @pydrake_mkdoc_identifier{3args_matrix} + */ + static PiecewisePolynomial CubicWithContinuousSecondDerivatives( + const std::vector& breaks, + const std::vector>& samples, + bool periodic_end_condition = false); + + /** + * Version of CubicWithContinuousSecondDerivatives(breaks, samples) that + * uses vector samples and Eigen VectorXd / MatrixX arguments. Each column + * of `samples` represents a sample point. + * + * @pre `samples.cols() == breaks.size()`. + * @pydrake_mkdoc_identifier{3args_vector} + */ + static PiecewisePolynomial CubicWithContinuousSecondDerivatives( + const Eigen::Ref>& breaks, + const Eigen::Ref>& samples, + bool periodic_end_condition = false); + + static PiecewisePolynomial Cubic( + const Eigen::Ref>& breaks, + const Eigen::Ref>& samples, + bool periodic_end_condition = false) { + return CubicWithContinuousSecondDerivatives(breaks, samples, + periodic_end_condition); + } + + /** + * Constructs a polynomial with a *single segment* of the lowest possible + * degree that passes through all of the sample points. See "polynomial + * interpolation" and/or "Lagrange polynomial" on Wikipedia for more + * information. + * @pre `times` must be monotonically increasing. + * @pre `samples.size() == times.size()`. + * @pydrake_mkdoc_identifier{matrix} + */ + static PiecewisePolynomial LagrangeInterpolatingPolynomial( + const std::vector& times, const std::vector>& samples); + + /** + * Version of LagrangeInterpolatingPolynomial(times, samples) that + * uses vector samples and Eigen VectorXd / MatrixX arguments. Each column + * of `samples` represents a sample point. + * + * @pre `samples.cols() == times.size()`. + * @pydrake_mkdoc_identifier{vector} + */ + static PiecewisePolynomial LagrangeInterpolatingPolynomial( + const Eigen::Ref>& times, + const Eigen::Ref>& samples); + + // @} + + /** + * Returns a %PiecewisePolynomial where each segment is the specified + * derivative of the corresponding segment in `this`. Any rules or limitations + * of Polynomial::derivative() also apply to this function. + * + * Derivatives evaluated at non-differentiable points return the value at the + * left hand side of the interval. + * @param derivative_order The order of the derivative, namely, if + * `derivative_order` = n, the n'th derivative of the polynomial will + * be returned. + * @warning In the event of discontinuous derivatives evaluated at breaks, + * it is not defined which polynomial (i.e., to the left or right + * of the break) will be the one that is evaluated at the break. + */ + PiecewisePolynomial derivative(int derivative_order = 1) const; + + /** + * Returns a %PiecewisePolynomial that is the indefinite integral of this one. + * Any rules or limitations of Polynomial::integral() also apply to this + * function. + * + * If `value_at_start_time` is given, it does the following only for the + * first segment: adds that constant as the constant term + * (zeroth-order coefficient) of the resulting Polynomial. + */ + PiecewisePolynomial integral(const T& value_at_start_time = 0.0) const; + + /** + * Returns a %PiecewisePolynomial that is the indefinite integral of this one. + * Any rules or limitations of Polynomial::integral() also apply to this + * function. + * + * If `value_at_start_time` is given, it does the following only for the + * first segment: adds `value_at_start_time(row,col)` as the constant term + * (zeroth-order coefficient) of the resulting Polynomial. + */ + PiecewisePolynomial integral( + const Eigen::Ref>& value_at_start_time) const; + + /** + * Returns `true` if this trajectory has no breaks/samples/polynomials. + */ + bool empty() const { return polynomials_.empty(); } + + /** + * Evaluates the trajectory at the given time without returning the entire + * matrix. Equivalent to value(t)(row, col). + * @warning See warnings in value(). + */ + T scalarValue(const T& t, Eigen::Index row = 0, Eigen::Index col = 0) const; + + /** + * Evaluates the %PiecewisePolynomial at the given time t. + * + * @param t The time at which to evaluate the %PiecewisePolynomial. + * @return The matrix of evaluated values. + * @pre If T == symbolic::Expression, `t.is_constant()` must be true. + * + * @warning If t does not lie in the range that the polynomial is defined + * over, the polynomial will silently be evaluated at the closest + * point to t. For example, `value(-1)` will return `value(0)` for a + * polynomial defined over [0, 1]. + * @warning See warning in the @ref polynomial_warning "constructor overview" + * above. + */ + MatrixX value(const T& t) const override { + const int derivative_order = 0; + return DoEvalDerivative(t, derivative_order); + } + + /** + * Gets the matrix of Polynomials corresponding to the given segment index. + * @warning `segment_index` is not checked for validity. + */ + const PolynomialMatrix& getPolynomialMatrix(int segment_index) const; + + /** + * Gets the Polynomial with the given matrix row and column index that + * corresponds to the given segment index. + * Equivalent to `getPolynomialMatrix(segment_index)(row, col)`. + * @note Calls PiecewiseTrajectory::segment_number_range_check() to + * validate `segment_index`. + */ + const Polynomial& getPolynomial(int segment_index, Eigen::Index row = 0, + Eigen::Index col = 0) const; + + /** + * Gets the degree of the Polynomial with the given matrix row and column + * index that corresponds to the given segment index. Equivalent to + * `getPolynomial(segment_index, row, col).GetDegree()`. + */ + int getSegmentPolynomialDegree(int segment_index, Eigen::Index row = 0, + Eigen::Index col = 0) const; + + /** + * Returns the row count of the output matrices. + * @throws std::exception if empty(). + */ + Eigen::Index rows() const override; + + /** + * Returns the column count of the output matrices. + * @throws std::exception if empty(). + */ + Eigen::Index cols() const override; + + /** + * Reshapes the dimensions of the Eigen::MatrixX returned by value(), + * EvalDerivative(), etc. + * + * @pre @p rows x @p cols must equal this.rows() * this.cols(). + * @see Eigen::PlainObjectBase::resize(). + */ + void Reshape(int rows, int cols); + + /** + * Extracts a trajectory representing a block of size (block_rows, block_cols) + * starting at (start_row, start_col) from the PiecewisePolynomial. + * @returns a PiecewisePolynomial such that + * ret.value(t) = this.value(t).block(i,j,p,q); + */ + PiecewisePolynomial Block(int start_row, int start_col, int block_rows, + int block_cols) const; + + /** + * Adds each Polynomial in the PolynomialMatrix of `other` to the + * corresponding Polynomial in the PolynomialMatrix of `this`, storing the + * result in `this`. If `this` corresponds to t² and `other` corresponds to + * t³, `this += other` will correspond to t³ + t². + * @throws std::exception if every element of `other.get_segment_times()` + * is not within PiecewiseTrajectory::kEpsilonTime from + * `this->get_segment_times(). + */ + PiecewisePolynomial& operator+=(const PiecewisePolynomial& other); + + /** + * Subtracts each Polynomial in the PolynomialMatrix of `other` from the + * corresponding Polynomial in the PolynomialMatrix of `this`, storing the + * result in `this`. If `this` corresponds to t² and `other` corresponds to + * t³, `this -= other` will correspond to t² - t³. + * @throws std::exception if every element of `other.get_segment_times()` + * is not within PiecewiseTrajectory::kEpsilonTime from + * `this->get_segment_times(). + */ + PiecewisePolynomial& operator-=(const PiecewisePolynomial& other); + + /** + * Multiplies each Polynomial in the PolynomialMatrix of `other` by the + * corresponding Polynomial in the PolynomialMatrix of `this` (i.e., a + * coefficient-wise multiplication), storing the result in `this`. If `this` + * corresponds to t² and `other` corresponds to t³, `this *= other` will + * correspond to t⁵. + * @throws std::exception if every element of `other.get_segment_times()` + * is not within PiecewiseTrajectory::kEpsilonTime from + * `this->get_segment_times(). + */ + PiecewisePolynomial& operator*=(const PiecewisePolynomial& other); + + PiecewisePolynomial& operator+=(const MatrixX& coeff); + + PiecewisePolynomial& operator-=(const MatrixX& coeff); + + /** + * Adds each Polynomial in the PolynomialMatrix of `other` to the + * corresponding Polynomial in the PolynomialMatrix of `this`. + * If `this` corresponds to t² and `other` corresponds to + * t³, `this + other` will correspond to t³ + t². + * @throws std::exception if every element of `other.get_segment_times()` + * is not within PiecewiseTrajectory::kEpsilonTime from + * `this->get_segment_times(). + */ + const PiecewisePolynomial operator+(const PiecewisePolynomial& other) const; + + /** + * Subtracts each Polynomial in the PolynomialMatrix of `other` from the + * corresponding Polynomial in the PolynomialMatrix of `this`. + * If `this` corresponds to t² and `other` corresponds to + * t³, `this - other` will correspond to t² - t³. + * @throws std::exception if every element of `other.get_segment_times()` + * is not within PiecewiseTrajectory::kEpsilonTime from + * `this->get_segment_times(). + */ + const PiecewisePolynomial operator-(const PiecewisePolynomial& other) const; + + /** + * Implements unary minus operator. Multiplies each Polynomial in `this` by + * -1. + */ + const PiecewisePolynomial operator-() const; + + /** + * Multiplies each Polynomial in the PolynomialMatrix of `other` by the + * corresponding Polynomial in the PolynomialMatrix of `this` (i.e., a + * coefficient-wise multiplication). If `this` corresponds to t² and `other` + * corresponds to t³, `this *= other` will correspond to t⁵. + * @throws std::exception if every element of `other.get_segment_times()` + * is not within PiecewiseTrajectory::kEpsilonTime from + * `this->get_segment_times()1. + */ + const PiecewisePolynomial operator*(const PiecewisePolynomial& other) const; + + const PiecewisePolynomial operator+(const MatrixX& coeff) const; + + const PiecewisePolynomial operator-(const MatrixX& coeff) const; + + // TODO(russt): Update return type to boolean so that callers can obtain a + // Formula when T=symbolic::Expression. + /** + * Checks whether a %PiecewisePolynomial is approximately equal to this one by + * calling Polynomial::CoefficientsAlmostEqual() on every element of every + * segment. + * + * @see Polynomial::CoefficientsAlmostEqual(). + * + */ + bool isApprox(const PiecewisePolynomial& other, double tol, + const ToleranceType& tol_type = ToleranceType::kRelative) const; + + /** + * Concatenates `other` to the end of `this`. + * + * @warning The resulting %PiecewisePolynomial will only be continuous to the + * degree that the first Polynomial of `other` is continuous with + * the last Polynomial of `this`. See warning about evaluating + * discontinuous derivatives at breaks in derivative(). + * @param other %PiecewisePolynomial instance to concatenate. + * @throws std::exception if trajectories' dimensions do not match + * each other (either rows() or cols() does + * not match between this and `other`). + * @throws std::exception if `this->end_time()` and `other->start_time()` + * are not within + * PiecewiseTrajectory::kEpsilonTime from + * each other. + */ + void ConcatenateInTime(const PiecewisePolynomial& other); + + /** + * The CubicHermite spline construction has a nice property of being + * incremental (each segment can be solved independently). Given a new sample + * and it's derivative, this method adds one segment to the end of `this` + * where the start sample and derivative are taken as the value and derivative + * at the final break of `this`. + * + * @pre `this` is not empty() + * @pre `time` > end_time() + * @pre `sample` and `sample_dot` must have size rows() x cols(). + */ + void AppendCubicHermiteSegment( + const T& time, const Eigen::Ref>& sample, + const Eigen::Ref>& sample_dot); + + /** + * Given a new sample, this method adds one segment to the end of `this` using + * a first-order hold, where the start sample is taken as the value at the + * final break of `this`. */ + void AppendFirstOrderSegment( + const T& time, const Eigen::Ref>& sample); + + /** Removes the final segment from the trajectory, reducing the number of + * segments by 1. + * @pre `this` is not empty() + */ + void RemoveFinalSegment(); + + /** + * Modifies the trajectory so that pp_after(t) = pp_before(-t). + * + * @note The new trajectory will evaluate differently at precisely the break + * points if the original trajectory was discontinuous at the break points. + * This is because the segments are defined on the half-open intervals + * [breaks(i), breaks(i+1)), and the order of the breaks have been reversed. + */ + void ReverseTime(); + + /** + * Scales the time of the trajectory by non-negative `scale` (use + * ReverseTime() if you want to also negate time). The resulting polynomial + * evaluates to pp_after(t) = pp_before(t/scale). + * + * As an example, `scale`=2 will result in a trajectory that is twice as long + * (start_time() and end_time() have both doubled). + */ + void ScaleTime(const T& scale); + + /** + * Adds `offset` to all of the breaks. `offset` need not be a non-negative + * number. The resulting polynomial will evaluate to pp_after(t) = + * pp_before(t-offset). + * + * As an example, `offset`=2 will result in the start_time() and end_time() + * being 2 seconds later. + */ + void shiftRight(const T& offset); + + /** + * Replaces the specified block of the PolynomialMatrix at the given + * segment index. + * @note Calls PiecewiseTrajectory::segment_number_range_check() to + * validate `segment_index`. + * @warning This code relies upon Eigen to verify that the replacement + * block is not too large. + */ + void setPolynomialMatrixBlock(const PolynomialMatrix& replacement, + int segment_index, Eigen::Index row_start = 0, + Eigen::Index col_start = 0); + + /** + * Returns the %PiecewisePolynomial comprising the `num_segments` segments + * starting at the specified `start_segment_index`. + * @note Calls PiecewiseTrajectory::segment_number_range_check() to + * validate `segment_index`. + */ + PiecewisePolynomial slice(int start_segment_index, int num_segments) const; + + private: + // Evaluates the %PiecwisePolynomial derivative at the given time @p t. + // Returns the nth derivative, where `n` is the value of @p derivative_order. + // + // @warning This method comes with the same caveats as value(). See value() + // @pre derivative_order must be non-negative. + MatrixX DoEvalDerivative(const T& t, int derivative_order) const override; + + std::unique_ptr> DoMakeDerivative( + int derivative_order) const override { + return derivative(derivative_order).Clone(); + } + + bool do_has_derivative() const override { return true; } + + T EvaluateSegmentAbsoluteTime(int segment_index, const T& t, Eigen::Index row, + Eigen::Index col, + int derivative_order = 0) const; + + // a PolynomialMatrix for each piece (segment). + std::vector polynomials_; + + // Computes coeffecients for a cubic spline given the value and first + // derivatives at the end points. + // Throws `std::exception` if `dt < PiecewiseTrajectory::kEpsilonTime`. + static Eigen::Matrix ComputeCubicSplineCoeffs(const T& dt, T y0, + T y1, T yd0, T yd1); + + // For a cubic spline, there are 4 unknowns for each segment Pi, namely + // the coefficients for Pi = a0 + a1 * t + a2 * t^2 + a3 * t^3. + // Let N be the size of breaks and samples, there are N-1 segments, + // and thus 4*(N-1) unknowns to fully specified a cubic spline for the given + // data. + // + // If we are also given N sample_dot (velocity), each Pi will be fully + // specified by (samples[i], sample_dot[i]) and (samples[i+1], + // sample_dot[i+1]). When sample_dot are not specified, we make the design + // choice to enforce continuity up to the second order (Yddot) for + // the interior points, i.e. Pi'(duration_i) = Pi+1'(0), and + // Pi''(duration_i) = Pi+1''(0), where ' means time derivative, and + // duration_i = breaks[i+1] - breaks[i] is the duration for the ith segment. + // + // At this point, we have 2 * (N - 1) position constraints: + // Pi(0) = samples[i], for i in [0, N - 2] + // Pi(duration_i) = samples[i+1], for i in [0, N - 2] + // N - 2 velocity constraints for the interior points: + // Pi'(duration_i) = Pi+1'(0), for i in [0, N - 3] + // N - 2 acceleration constraints for the interior points: + // Pi''(duration_i) = Pi+1''(0), for i in [0, N - 3] + // + // The first N - 1 position constraints can be used to directly eliminate the + // constant term of each segment as Pi(0) = a0_i. This reduces the number of + // unknowns to 3 * (N-1) and leaves 3 * (N-1) - 2 constraints. This function + // sets up the remaining constraints. There are still 2 constraints missing, + // which can be resolved by various end point conditions (velocity at the end + // points / "not-a-sample" / etc). These will be specified by the callers. + // + // This function performs the constraint setup for the row and column of the + // sample matrix specified by `row` & `col`. The coefficients for the + // left-hand-side of the constraint equations are prepared for loading into a + // sparse matrix by creating a vector of Triplets, `triplet_list` with each + // representing the row, column and value to add to the sparse matrix. The + // right-hand side of the constraint equations is loaded into `b`. + static int SetupCubicSplineInteriorCoeffsLinearSystem( + const std::vector& breaks, + const std::vector>& samples, int row, int col, + std::vector>* triplet_list, VectorX* b); + + // Throws std::exception if + // `breaks` and `samples` have different length, + // `breaks` is not strictly increasing, + // `samples` has inconsistent dimensions, + // `breaks` has length smaller than min_length. + static void CheckSplineGenerationInputValidityOrThrow( + const std::vector& breaks, + const std::vector>& samples, int min_length); +}; + +} // namespace trajectories +} // namespace drake + +DRAKE_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class drake::trajectories::PiecewisePolynomial) diff --git a/maliput_drake/include/drake/common/trajectories/piecewise_pose.h b/maliput_drake/include/drake/common/trajectories/piecewise_pose.h new file mode 100644 index 0000000..6ca8e2a --- /dev/null +++ b/maliput_drake/include/drake/common/trajectories/piecewise_pose.h @@ -0,0 +1,121 @@ +#pragma once + +#include +#include + +#include "drake/common/drake_copyable.h" +#include "drake/common/eigen_types.h" +#include "drake/common/trajectories/piecewise_polynomial.h" +#include "drake/common/trajectories/piecewise_quaternion.h" +#include "drake/common/trajectories/piecewise_trajectory.h" +#include "drake/math/rigid_transform.h" + +namespace drake { +namespace trajectories { + +/** + * A wrapper class that represents a pose trajectory, whose rotation part is a + * PiecewiseQuaternionSlerp and the translation part is a PiecewisePolynomial. + * + * @tparam_default_scalars + */ +template +class PiecewisePose final : public PiecewiseTrajectory { + public: + DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(PiecewisePose) + + /** + * Constructs an empty piecewise pose trajectory. + */ + PiecewisePose() {} + + /** + * Constructor. + * @param pos_traj Position trajectory. + * @param rot_traj Orientation trajectory. + */ + PiecewisePose(const PiecewisePolynomial& pos_traj, + const PiecewiseQuaternionSlerp& rot_traj); + + /** + * Constructs a PiecewisePose from given @p time and @p poses. + * A cubic polynomial with given end velocities is used to construct the + * position part. The rotational part is represented by a piecewise quaterion + * trajectory. There must be at least two elements in @p times and @p poses. + * @param times Breaks used to build the splines. + * @param poses Knots used to build the splines. + * @param start_vel Start linear velocity. + * @param end_vel End linear velocity. + */ + static PiecewisePose MakeCubicLinearWithEndLinearVelocity( + const std::vector& times, + const std::vector>& poses, + const Vector3& start_vel = Vector3::Zero(), + const Vector3& end_vel = Vector3::Zero()); + + std::unique_ptr> Clone() const override; + + Eigen::Index rows() const override { return 4; } + + Eigen::Index cols() const override { return 4; } + + /** + * Returns the interpolated pose at @p time. + */ + math::RigidTransform get_pose(const T& time) const; + + MatrixX value(const T& t) const override { + return get_pose(t).GetAsMatrix4(); + } + + /** + * Returns the interpolated velocity at @p time or zero if @p time is before + * this trajectory's start time or after its end time. + */ + Vector6 get_velocity(const T& time) const; + + /** + * Returns the interpolated acceleration at @p time or zero if @p time is + * before this trajectory's start time or after its end time. + */ + Vector6 get_acceleration(const T& time) const; + + /** + * Returns true if the position and orientation trajectories are both + * within @p tol from the other's. + */ + bool is_approx(const PiecewisePose& other, double tol) const; + + /** + * Returns the position trajectory. + */ + const PiecewisePolynomial& get_position_trajectory() const { + return position_; + } + + /** + * Returns the orientation trajectory. + */ + const PiecewiseQuaternionSlerp& get_orientation_trajectory() const { + return orientation_; + } + + private: + bool do_has_derivative() const override; + + MatrixX DoEvalDerivative(const T& t, int derivative_order) const override; + + std::unique_ptr> DoMakeDerivative( + int derivative_order) const override; + + PiecewisePolynomial position_; + PiecewisePolynomial velocity_; + PiecewisePolynomial acceleration_; + PiecewiseQuaternionSlerp orientation_; +}; + +} // namespace trajectories +} // namespace drake + +DRAKE_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class drake::trajectories::PiecewisePose) diff --git a/maliput_drake/include/drake/common/trajectories/piecewise_quaternion.h b/maliput_drake/include/drake/common/trajectories/piecewise_quaternion.h new file mode 100644 index 0000000..2fd64f5 --- /dev/null +++ b/maliput_drake/include/drake/common/trajectories/piecewise_quaternion.h @@ -0,0 +1,174 @@ +#pragma once + +#include +#include + +#include "drake/common/default_scalars.h" +#include "drake/common/drake_copyable.h" +#include "drake/common/eigen_types.h" +#include "drake/common/trajectories/piecewise_trajectory.h" +#include "drake/math/rotation_matrix.h" + +namespace drake { +namespace trajectories { + +/** + * A class representing a trajectory for quaternions that are interpolated + * using piecewise slerp (spherical linear interpolation). + * All the orientation samples are expected to be with respect to the same + * parent reference frame, i.e. q_i represents the rotation R_PBi for the + * orientation of frame B at the ith sample in a fixed parent frame P. + * The world frame is a common choice for the parent frame. + * The angular velocity and acceleration are also relative to the parent frame + * and expressed in the parent frame. + * Since there is a sign ambiguity when using quaternions to represent + * orientation, namely q and -q represent the same orientation, the internal + * quaternion representations ensure that q_n.dot(q_{n+1}) >= 0. + * Another intuitive way to think about this is that consecutive quaternions + * have the shortest geodesic distance on the unit sphere. + * + * @tparam_default_scalars + */ +template +class PiecewiseQuaternionSlerp final : public PiecewiseTrajectory { + public: + DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(PiecewiseQuaternionSlerp) + + /** + * Builds an empty PiecewiseQuaternionSlerp. + */ + PiecewiseQuaternionSlerp() = default; + + /** + * Builds a PiecewiseQuaternionSlerp. + * @throws std::exception if breaks and quaternions have different length, + * or breaks have length < 2. + */ + PiecewiseQuaternionSlerp( + const std::vector& breaks, + const std::vector>& quaternions); + + /** + * Builds a PiecewiseQuaternionSlerp. + * @throws std::exception if breaks and rot_matrices have different length, + * or breaks have length < 2. + */ + PiecewiseQuaternionSlerp( + const std::vector& breaks, + const std::vector>& rotation_matrices); + + /** + * Builds a PiecewiseQuaternionSlerp. + * @throws std::exception if breaks and rot_matrices have different length, + * or breaks have length < 2. + */ + PiecewiseQuaternionSlerp( + const std::vector& breaks, + const std::vector>& rotation_matrices); + + /** + * Builds a PiecewiseQuaternionSlerp. + * @throws std::exception if breaks and ang_axes have different length, + * or breaks have length < 2. + */ + PiecewiseQuaternionSlerp( + const std::vector& breaks, + const std::vector>& angle_axes); + + ~PiecewiseQuaternionSlerp() override = default; + + std::unique_ptr> Clone() const override; + + Eigen::Index rows() const override { return 4; } + + Eigen::Index cols() const override { return 1; } + + /** + * Interpolates orientation. + * @param time Time for interpolation. + * @return The interpolated quaternion at `time`. + */ + Quaternion orientation(const T& time) const; + + MatrixX value(const T& time) const override { + return orientation(time).matrix(); + } + + /** + * Interpolates angular velocity. + * @param time Time for interpolation. + * @return The interpolated angular velocity at `time`, + * which is constant per segment. + */ + Vector3 angular_velocity(const T& time) const; + + /** + * Interpolates angular acceleration. + * @param time Time for interpolation. + * @return The interpolated angular acceleration at `time`, + * which is always zero for slerp. + */ + Vector3 angular_acceleration(const T& time) const; + + /** + * Getter for the internal quaternion samples. + * + * @note The returned quaternions might be different from the ones used for + * construction because the internal representations are set to always be + * the "closest" w.r.t to the previous one. + * + * @return the internal sample points. + */ + const std::vector>& get_quaternion_samples() const { + return quaternions_; + } + + /** + * Returns true if all the corresponding segment times are within + * @p tol seconds, and the angle difference between the corresponding + * quaternion sample points are within @p tol (using `ExtractDoubleOrThrow`). + */ + bool is_approx(const PiecewiseQuaternionSlerp& other, + double tol) const; + + /** + * Given a new Quaternion, this method adds one segment to the end of `this`. + */ + void Append(const T& time, const Quaternion& quaternion); + + /** + * Given a new RotationMatrix, this method adds one segment to the end of + * `this`. + */ + void Append(const T& time, const math::RotationMatrix& rotation_matrix); + + /** + * Given a new AngleAxis, this method adds one segment to the end of `this`. + */ + void Append(const T& time, const AngleAxis& angle_axis); + + private: + // Initialize quaternions_ and computes angular velocity for each segment. + void Initialize( + const std::vector& breaks, + const std::vector>& quaternions); + + // Computes the interpolation time within each segment. Result is in [0, 1]. + T ComputeInterpTime(int segment_index, const T& time) const; + + bool do_has_derivative() const override; + + MatrixX DoEvalDerivative(const T& t, int derivative_order) const override; + + std::unique_ptr> DoMakeDerivative( + int derivative_order) const override; + + std::vector> quaternions_; + std::vector> angular_velocities_; +}; + +} // namespace trajectories +} // namespace drake + +DRAKE_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class drake::trajectories::PiecewiseQuaternionSlerp) diff --git a/maliput_drake/include/drake/common/trajectories/piecewise_trajectory.h b/maliput_drake/include/drake/common/trajectories/piecewise_trajectory.h new file mode 100644 index 0000000..11ca346 --- /dev/null +++ b/maliput_drake/include/drake/common/trajectories/piecewise_trajectory.h @@ -0,0 +1,80 @@ +#pragma once + +#include +#include +#include +#include + +#include + +#include "drake/common/default_scalars.h" +#include "drake/common/trajectories/trajectory.h" + +namespace drake { +namespace trajectories { + +/// Abstract class that implements the basic logic of maintaining consequent +/// segments of time (delimited by `breaks`) to implement a trajectory that +/// is represented by simpler logic in each segment or "piece". +/// +/// @tparam_default_scalars +template +class PiecewiseTrajectory : public Trajectory { + public: + /// Minimum delta quantity used for comparing time. + static constexpr double kEpsilonTime = std::numeric_limits::epsilon(); + + ~PiecewiseTrajectory() override = default; + + int get_number_of_segments() const; + + T start_time(int segment_number) const; + + T end_time(int segment_number) const; + + T duration(int segment_number) const; + + T start_time() const override; + + T end_time() const override; + + /** + * Returns true iff `t >= getStartTime() && t <= getEndTime()`. + */ + boolean is_time_in_range(const T& t) const; + + int get_segment_index(const T& t) const; + + const std::vector& get_segment_times() const; + + void segment_number_range_check(int segment_number) const; + + static std::vector RandomSegmentTimes( + // TODO(#2274) Fix this NOLINTNEXTLINE(runtime/references) + int num_segments, std::default_random_engine &generator); + + protected: + // Final subclasses are allowed to make copy/move/assign public. + DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(PiecewiseTrajectory) + PiecewiseTrajectory() = default; + + /// @p breaks increments must be greater or equal to kEpsilonTime. + explicit PiecewiseTrajectory(const std::vector& breaks); + + bool SegmentTimesEqual(const PiecewiseTrajectory& b, + double tol = kEpsilonTime) const; + + const std::vector& breaks() const { return breaks_; } + std::vector& get_mutable_breaks() { return breaks_; } + + private: + int GetSegmentIndexRecursive(const T& time, int start, int end) const; + + std::vector breaks_; +}; + +} // namespace trajectories +} // namespace drake + +DRAKE_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class drake::trajectories::PiecewiseTrajectory) diff --git a/maliput_drake/include/drake/common/trajectories/trajectory.h b/maliput_drake/include/drake/common/trajectories/trajectory.h new file mode 100644 index 0000000..f50ee4c --- /dev/null +++ b/maliput_drake/include/drake/common/trajectories/trajectory.h @@ -0,0 +1,104 @@ +#pragma once + +#include +#include + +#include + +#include "drake/common/default_scalars.h" +#include "drake/common/drake_copyable.h" +#include "drake/common/eigen_types.h" + +namespace drake { +namespace trajectories { + +/** + * A Trajectory represents a time-varying matrix, indexed by a single scalar + * time. + * + * @tparam_default_scalars + */ +template +class Trajectory { + public: + virtual ~Trajectory() = default; + + /** + * @return A deep copy of this Trajectory. + */ + virtual std::unique_ptr> Clone() const = 0; + + /** + * Evaluates the trajectory at the given time \p t. + * @param t The time at which to evaluate the trajectory. + * @return The matrix of evaluated values. + */ + virtual MatrixX value(const T& t) const = 0; + + /** + * If cols()==1, then evaluates the trajectory at each time @p t, and returns + * the results as a Matrix with the ith column corresponding to the ith time. + * Otherwise, if rows()==1, then evaluates the trajectory at each time @p t, + * and returns the results as a Matrix with the ith row corresponding to + * the ith time. + * @throws std::exception if both cols and rows are not equal to 1. + */ + MatrixX vector_values(const std::vector& t) const; + + /** + * Returns true iff the Trajectory provides and implementation for + * EvalDerivative() and MakeDerivative(). The derivative need not be + * continuous, but should return a result for all t for which value(t) returns + * a result. + */ + bool has_derivative() const; + + /** + * Evaluates the derivative of `this` at the given time @p t. + * Returns the nth derivative, where `n` is the value of @p derivative_order. + * + * @pre derivative_order must be non-negative. + */ + MatrixX EvalDerivative(const T& t, int derivative_order = 1) const; + + /** + * Takes the derivative of this Trajectory. + * @param derivative_order The number of times to take the derivative before + * returning. + * @return The nth derivative of this object. + */ + std::unique_ptr> MakeDerivative( + int derivative_order = 1) const; + + /** + * @return The number of rows in the matrix returned by value(). + */ + virtual Eigen::Index rows() const = 0; + + /** + * @return The number of columns in the matrix returned by value(). + */ + virtual Eigen::Index cols() const = 0; + + virtual T start_time() const = 0; + + virtual T end_time() const = 0; + + protected: + // Final subclasses are allowed to make copy/move/assign public. + DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Trajectory) + Trajectory() = default; + + virtual bool do_has_derivative() const; + + virtual MatrixX DoEvalDerivative(const T& t, int derivative_order) const; + + virtual std::unique_ptr> DoMakeDerivative( + int derivative_order) const; +}; + +} // namespace trajectories +} // namespace drake + +DRAKE_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class drake::trajectories::Trajectory) diff --git a/maliput_drake/include/drake/common/trig_poly.h b/maliput_drake/include/drake/common/trig_poly.h new file mode 100644 index 0000000..0b075a1 --- /dev/null +++ b/maliput_drake/include/drake/common/trig_poly.h @@ -0,0 +1,470 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include + +#include "drake/common/drake_assert.h" +#include "drake/common/drake_copyable.h" +#include "drake/common/polynomial.h" + +namespace drake { + +/** A scalar multi-variate polynomial containing sines and cosines. + * + * TrigPoly represents a Polynomial some of whose variables actually represent + * the sines or cosines of other variables. Sines and cosines of first-order + * polynomials (affine combinations of variables) are decomposed into + * polynomials of the sines and cosines of individual variables via the + * Prosthaphaeresis formulae. + * + * Any variables which will appear in the arguments to trigonometric functions + * must be declared in the "SinCosMap" (created automatically by most TrigPoly + * constructors); attempting to, e.g., use sin(x) without first creating a + * SinCosMap mapping for 'x' will result in an exception. + * + * The same variable may not appear more than once in the sin_cos_map, + * regardless of position. + * + * For example: + * \code + * Polynomial base_x("x"), s("s"), c("c"); + * TrigPoly x(base_x, s, c) // This "x" knows that s = sin(x) + * // and that c = cos(x) + * cout << sin(x) // emits "s1" + * cout << sin(x) * x // emits "x1*s1" + * cout << sin(x + x) * x // emits "x1*s1*c1 + x1*c1*s1" + * \endcode + * + * NOTE: Certain analyses may not succeed when individual Monomials contain + * both x and sin(x) or cos(x) terms. This restriction is not currently + * enforced programmatically. + * + */ +template +class TrigPoly final { + public: + DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(TrigPoly) + + typedef Polynomial PolyType; + typedef typename PolyType::VarType VarType; + struct SinCosVars { + VarType s; + VarType c; + + bool operator==(const struct SinCosVars& other) const { + return (s == other.s) && (c == other.c); + } + }; + typedef std::map SinCosMap; + + template + struct Product { + typedef decltype(static_cast(0) * static_cast(0)) type; + }; + + /// Constructs a vacuous TrigPoly. + TrigPoly() {} + + /// Constructs a constant TrigPoly. + // NOLINTNEXTLINE(runtime/explicit) This conversion is desirable. + TrigPoly(const T& scalar) : poly_(scalar) {} + + /** + * Constructs a TrigPoly on the associated Polynomial p with no associated + * trigonometric correspondences. + */ + explicit TrigPoly(const PolyType& p) : poly_(p) {} + + /** + * Constructs a TrigPoly on the associated Polynomial p, but with the + * additional information about sin and cos relations in _sin_cos_map. + */ + TrigPoly(const PolyType& p, const SinCosMap& _sin_cos_map) + : poly_(p), sin_cos_map_(_sin_cos_map) { + // The provided _sin_cos_map might have extraneous entries; clip them. + std::set vars_in_use = p.GetVariables(); + std::set vars_seen_in_map; + for (const auto& sin_cos_entry : _sin_cos_map) { + if (!(vars_in_use.count(sin_cos_entry.first) || + vars_in_use.count(sin_cos_entry.second.c) || + vars_in_use.count(sin_cos_entry.second.s))) { + sin_cos_map_.erase(sin_cos_entry.first); + } + DRAKE_ASSERT(!vars_seen_in_map.count(sin_cos_entry.first)); + DRAKE_ASSERT(!vars_seen_in_map.count(sin_cos_entry.second.s)); + DRAKE_ASSERT(!vars_seen_in_map.count(sin_cos_entry.second.c)); + vars_seen_in_map.insert(sin_cos_entry.first); + vars_seen_in_map.insert(sin_cos_entry.second.s); + vars_seen_in_map.insert(sin_cos_entry.second.c); + } + } + + /** + * Constructs a TrigPoly version of q, but with the additional information + * that the variables s and c represent the sine and cosine of q. + */ + TrigPoly(const PolyType& q, const PolyType& s, const PolyType& c) { + if ((q.GetDegree() != 1) || (s.GetDegree() != 1) || (c.GetDegree() != 1)) + throw std::runtime_error( + "q, s, and c must all be simple polynomials (in the msspoly sense)"); + + poly_ = q; + SinCosVars sc; + sc.s = s.GetSimpleVariable(); + sc.c = c.GetSimpleVariable(); + sin_cos_map_[q.GetSimpleVariable()] = sc; + } + + ~TrigPoly() = default; + + /// Returns the underlying Polynomial for this TrigPoly. + const PolyType& poly(void) const { return poly_; } + + /// Returns the SinCosMap for this TrigPoly. + const SinCosMap& sin_cos_map(void) const { return sin_cos_map_; } + + /** A version of sin that handles TrigPoly arguments through ADL. + * + * Implements sin(x) for a TrigPoly x. + * + * x must be of degree 0 or 1, and must contain only variables that have + * entries in its SinCosMap. + */ + friend TrigPoly sin(const TrigPoly& p) { + using std::abs; + using std::copysign; + using std::floor; + + const std::vector& m = p.poly_.GetMonomials(); + if (m.empty()) { + return TrigPoly(Polynomial(sin(T(0)))); + } else if (p.poly_.GetDegree() > 1) { + throw std::runtime_error( + "sin of polynomials with degree > 1 is not supported"); + } + + // Base case of recursion is one mononomial with coefficient of 1. + if (m.size() == 1 && abs(m[0].coefficient) == T(1)) { + TrigPoly ret = p; + if (m[0].terms.empty()) { // then it's a constant + ret.poly_ = Polynomial(sin(m[0].coefficient)); + } else { + typename SinCosMap::iterator iter = + ret.sin_cos_map_.find(m[0].terms[0].var); + if (iter == ret.sin_cos_map_.end()) + throw std::runtime_error( + "tried taking the sin of a variable that does not exist in my " + "sin_cos_map"); + ret.poly_.Subs(m[0].terms[0].var, iter->second.s); + } + return ret; + } + + Polynomial pa; + Polynomial pb(m.begin() + 1, m.end()); + if (abs(m[0].coefficient) == T(1)) { + pa = Polynomial(m[0].coefficient, m[0].terms); + } else if (m[0].coefficient - floor(m[0].coefficient) == + T(0)) { + // If the coefficient has integer magnitude greater than 1, recurse by + // expanding out a term with coefficient of magnitude 1. + auto unit = copysign(T(1), m[0].coefficient); + pa = Polynomial(unit, m[0].terms); + pb += Polynomial(m[0].coefficient - unit, m[0].terms); + } else { + throw std::runtime_error("Fractional coefficients not supported"); + } + + // Now handle the multi-monomial case recursively + // sin(a+b+...) = sin(a)cos(b+...) + cos(a)sin(b+...) + TrigPoly a(pa, p.sin_cos_map_); + TrigPoly b(pb, p.sin_cos_map_); + return sin(a) * cos(b) + cos(a) * sin(b); + } + + /// A version of cos that handles TrigPoly arguments through ADL. + /** + * Implements cos(x) for a TrigPoly x. + * + * x must be of degree 0 or 1, and must contain only variables that have + * entries in its SinCosMap. + */ + friend TrigPoly cos(const TrigPoly& p) { + using std::abs; + using std::copysign; + using std::floor; + + const std::vector& m = p.poly_.GetMonomials(); + if (m.empty()) { + return TrigPoly(Polynomial(cos(T(0)))); + } else if (p.poly_.GetDegree() > 1) { + throw std::runtime_error( + "cos of polynomials with degree > 1 is not supported"); + } + + // Base case of recursion is one mononomial with coefficient of 1. + if (m.size() == 1 && abs(m[0].coefficient) == T(1)) { + TrigPoly ret = p; + if (m[0].terms.empty()) { // then it's a constant + ret.poly_ = Polynomial(cos(m[0].coefficient)); + } else { + typename SinCosMap::iterator iter = + ret.sin_cos_map_.find(m[0].terms[0].var); + if (iter == ret.sin_cos_map_.end()) + throw std::runtime_error( + "tried taking the sin of a variable that does not exist in my " + "sin_cos_map"); + ret.poly_.Subs(m[0].terms[0].var, iter->second.c); + if (m[0].coefficient == T(-1)) { + ret *= -1; + } // cos(-q) => cos(q) => c (instead of -c) + } + return ret; + } + + Polynomial pa; + Polynomial pb(m.begin() + 1, m.end()); + if (abs(m[0].coefficient) == T(1)) { + pa = Polynomial(m[0].coefficient, m[0].terms); + } else if (m[0].coefficient - floor(m[0].coefficient) == + T(0)) { + // If the coefficient has integer magnitude greater than 1, recurse by + // expanding out a term with coefficient of magnitude 1. + auto unit = copysign(T(1), m[0].coefficient); + pa = Polynomial(unit, m[0].terms); + pb += Polynomial(m[0].coefficient - unit, m[0].terms); + } else { + throw std::runtime_error("Fractional coefficients not supported"); + } + + // Now handle the multi-monomial case recursively + // cos(a+b+...) = cos(a)cos(b+...) - sin(a)sin(b+...) + TrigPoly a(pa, p.sin_cos_map_); + TrigPoly b(pb, p.sin_cos_map_); + return cos(a) * cos(b) - sin(a) * sin(b); + } + + /// Returns all of the base (non-sin/cos) variables in this TrigPoly. + std::set GetVariables() const { + std::set vars = poly_.GetVariables(); + for (const auto& sin_cos_item : sin_cos_map_) { + vars.insert(sin_cos_item.first); + vars.erase(sin_cos_item.second.s); + vars.erase(sin_cos_item.second.c); + } + return vars; + } + + /** Given a value for every variable in this expression, evaluates it. + * + * By analogy with Polynomial::EvaluateMultivariate(). Values must be + * supplied for all base variables; supplying values for sin/cos variables + * is an error. + */ + template + typename Product::type EvaluateMultivariate( + const std::map& var_values) const { + std::map all_var_values = var_values; + for (const auto& sin_cos_item : sin_cos_map_) { + DRAKE_ASSERT(!var_values.count(sin_cos_item.second.s)); + DRAKE_ASSERT(!var_values.count(sin_cos_item.second.c)); + all_var_values[sin_cos_item.second.s] = + std::sin(var_values.at(sin_cos_item.first)); + all_var_values[sin_cos_item.second.c] = + std::cos(var_values.at(sin_cos_item.first)); + } + return poly_.EvaluateMultivariate(all_var_values); + } + + /** Partially evaluates this expression, returning the resulting expression. + * + * By analogy with Polynomial::evaluatePartial. Values must be supplied for + * all base variables only; supplying values for sin/cos variables is an + * error. + */ + virtual TrigPoly EvaluatePartial( + const std::map& var_values) const { + std::map var_values_with_sincos = var_values; + for (const auto& sin_cos_item : sin_cos_map_) { + DRAKE_ASSERT(!var_values.count(sin_cos_item.second.s)); + DRAKE_ASSERT(!var_values.count(sin_cos_item.second.c)); + if (!var_values.count(sin_cos_item.first)) { + continue; + } + var_values_with_sincos[sin_cos_item.second.s] = + std::sin(var_values.at(sin_cos_item.first)); + var_values_with_sincos[sin_cos_item.second.c] = + std::cos(var_values.at(sin_cos_item.first)); + } + return TrigPoly(poly_.EvaluatePartial(var_values_with_sincos), + sin_cos_map_); + } + + /// Compares two TrigPolys for equality. + /** + * Note that the question of equality of TrigPolys is a bit subtle. It is + * not immediately clear if two TrigPolys whose poly and sin_cos_map members + * differ equivalently (eg, a + b (b = cos(a)) and a + c (c = cos(a))) should + * be considered equal. + * + * For simplicity we only consider exactly equality rather than semantic + * equivalence. However that decision could reasonably revisited in the + * future. + */ + bool operator==(const TrigPoly& other) const { + return (poly_ == other.poly_) && (sin_cos_map_ == other.sin_cos_map_); + } + + TrigPoly& operator+=(const TrigPoly& other) { + poly_ += other.poly_; + sin_cos_map_.insert(other.sin_cos_map_.begin(), other.sin_cos_map_.end()); + return *this; + } + + TrigPoly& operator-=(const TrigPoly& other) { + poly_ -= other.poly_; + sin_cos_map_.insert(other.sin_cos_map_.begin(), other.sin_cos_map_.end()); + return *this; + } + + TrigPoly& operator*=(const TrigPoly& other) { + poly_ *= other.poly_; + sin_cos_map_.insert(other.sin_cos_map_.begin(), other.sin_cos_map_.end()); + return *this; + } + + TrigPoly& operator+=(const T& scalar) { + poly_ += scalar; + return *this; + } + + TrigPoly& operator-=(const T& scalar) { + poly_ -= scalar; + return *this; + } + + TrigPoly& operator*=(const T& scalar) { + poly_ *= scalar; + return *this; + } + + TrigPoly& operator/=(const T& scalar) { + poly_ /= scalar; + return *this; + } + + const TrigPoly operator+(const TrigPoly& other) const { + TrigPoly ret = *this; + ret += other; + return ret; + } + + const TrigPoly operator-(const TrigPoly& other) const { + TrigPoly ret = *this; + ret -= other; + return ret; + } + + const TrigPoly operator-() const { + TrigPoly ret = (*this) * T(-1); + return ret; + } + + const TrigPoly operator*(const TrigPoly& other) const { + TrigPoly ret = *this; + ret *= other; + return ret; + } + + friend const TrigPoly operator+(const TrigPoly& p, + const T& scalar) { + TrigPoly ret = p; + ret += scalar; + return ret; + } + + friend const TrigPoly operator+(const T& scalar, + const TrigPoly& p) { + TrigPoly ret = p; + ret += scalar; + return ret; + } + + friend const TrigPoly operator-(const TrigPoly& p, + const T& scalar) { + TrigPoly ret = p; + ret -= scalar; + return ret; + } + + friend const TrigPoly operator-(const T& scalar, + const TrigPoly& p) { + TrigPoly ret = -p; + ret += scalar; + return ret; + } + + friend const TrigPoly operator*(const TrigPoly& p, + const T& scalar) { + TrigPoly ret = p; + ret *= scalar; + return ret; + } + friend const TrigPoly operator*(const T& scalar, + const TrigPoly& p) { + TrigPoly ret = p; + ret *= scalar; + return ret; + } + + const TrigPoly operator/(const T& scalar) const { + TrigPoly ret = *this; + ret /= scalar; + return ret; + } + + friend std::ostream& operator<<(std::ostream& os, + const TrigPoly& tp) { + os << tp.poly_; + if (tp.sin_cos_map_.size()) { + os << " where "; + for (const auto& k_v_pair : tp.sin_cos_map_) { + std::string var = PolyType::IdToVariableName(k_v_pair.first); + std::string sin = PolyType::IdToVariableName(k_v_pair.second.s); + std::string cos = PolyType::IdToVariableName(k_v_pair.second.c); + os << sin << "=sin(" << var << "), " + << cos << "=cos(" << var << "), "; + } + } + return os; + } + + private: + PolyType poly_; + SinCosMap sin_cos_map_; +}; + +template +std::ostream& operator<<( + std::ostream& os, + const Eigen::Matrix, Rows, Cols>& tp_mat) { + Eigen::Matrix, Rows, Cols> poly_mat( + tp_mat.rows(), tp_mat.cols()); + for (int i = 0; i < poly_mat.size(); i++) { + poly_mat(i) = tp_mat(i).poly(); + } + os << poly_mat; + return os; +} + +typedef TrigPoly TrigPolyd; + +/// A column vector of TrigPoly; used in several optimization classes. +typedef Eigen::Matrix VectorXTrigPoly; + +} // namespace drake diff --git a/maliput_drake/include/drake/common/type_safe_index.h b/maliput_drake/include/drake/common/type_safe_index.h new file mode 100644 index 0000000..0359109 --- /dev/null +++ b/maliput_drake/include/drake/common/type_safe_index.h @@ -0,0 +1,587 @@ +#pragma once + +#include +#include +#include + +#include "drake/common/drake_assert.h" +#include "drake/common/drake_throw.h" +#include "drake/common/hash.h" +#include "drake/common/nice_type_name.h" + +namespace drake { + +/// A type-safe non-negative index class. +/// +/// @note This is *purposely* a separate class from Identifier. +/// For more information, see @ref TypeSafeIndexVsIndentifier "this section". +/// +/// This class serves as an upgrade to the standard practice of passing `int`s +/// around as indices. In the common practice, a method that takes indices into +/// multiple collections would have an interface like: +/// +/// @code +/// void foo(int bar_index, int thing_index); +/// @endcode +/// +/// It is possible for a programmer to accidentally switch the two index values +/// in an invocation. This mistake would still be _syntactically_ correct; it +/// will successfully compile but lead to inscrutable run-time errors. The +/// type-safe index provides the same speed and efficiency of passing `int`s, +/// but provides compile-time checking. The function would now look like: +/// +/// @code +/// void foo(BarIndex bar_index, ThingIndex thing_index); +/// @endcode +/// +/// and the compiler will catch instances where the order is reversed. +/// +/// The type-safe index is a _stripped down_ `int`. Each uniquely declared +/// index type has the following properties: +/// +/// - Valid index values are _explicitly_ constructed from `int` values. +/// - The index is implicitly convertible to an `int` (to serve as an index). +/// - The index supports increment, decrement, and in-place addition and +/// subtraction to support standard index-like operations. +/// - An index _cannot_ be constructed or compared to an index of another +/// type. +/// - In general, indices of different types are _not_ interconvertible. +/// - Binary integer operators (e.g., +, -, |, *, etc.) _always_ produce `int` +/// return values. One can even use operands of different index types in +/// such a binary expression. It is the _programmer's_ responsibility to +/// confirm that the resultant `int` value has meaning. +/// +/// While there _is_ the concept of an "invalid" index, this only exists to +/// support default construction _where appropriate_ (e.g., using indices in +/// STL containers). Using an invalid index in _any_ operation is considered +/// an error. In Debug build, attempts to compare, increment, decrement, etc. an +/// invalid index will throw an exception. +/// +/// A function that returns %TypeSafeIndex values which need to communicate +/// failure should _not_ use an invalid index. It should return an +/// `std::optional` instead. +/// +/// It is the designed intent of this class, that indices derived from this +/// class can be passed and returned by value. (Drake's typical calling +/// convention requires passing input arguments by const reference, or by value +/// when moved from. That convention does not apply to this class.) +/// +/// This is the recommended method to create a unique index type associated with +/// class `Foo`: +/// +/// @code +/// using FooIndex = TypeSafeIndex; +/// @endcode +/// +/// This references a non-existent, and ultimately anonymous, class `FooTag`. +/// This is sufficient to create a unique index type. It is certainly possible +/// to use an existing class (e.g., `Foo`). But this provides no functional +/// benefit. +/// +/// __Construction from integral types__ +/// +/// C++ will do +/// [implicit integer conversions](https://en.cppreference.com/w/cpp/language/implicit_conversion#Integral_conversions). +/// This allows construction of %TypeSafeIndex values with arbitrary integral +/// types. Index values must lie in the range of [0, 2³¹). The constructor will +/// validate the input value (in Debug mode). Ultimately, the caller is +/// responsible for confirming that the values provided lie in the valid range. +/// +/// __Examples of valid and invalid operations__ +/// +/// The TypeSafeIndex guarantees that index instances of different types can't +/// be compared or combined. Efforts to do so will cause a compile-time +/// failure. However, comparisons or operations on _other_ types that are +/// convertible to an int will succeed. For example: +/// @code +/// using AIndex = TypeSafeIndex; +/// using BIndex = TypeSafeIndex; +/// AIndex a(1); +/// BIndex b(1); +/// if (a == 2) { ... } // Ok. +/// size_t sz = 7; +/// if (a == sz) { ... } // Ok. +/// if (a == b) { ... } // <-- Compiler error. +/// AIndex invalid; // Creates an invalid index. +/// ++invalid; // Runtime error in Debug build. +/// @endcode +/// +/// As previously stated, the intent of this class is to seamlessly serve as an +/// index into indexed objects (e.g., vector, array, etc.). At the same time, we +/// want to avoid implicit conversions _from_ int to an index. These two design +/// constraints combined lead to a limitation in how TypeSafeIndex instances +/// can be used. Specifically, we've lost a common index pattern: +/// +/// @code +/// for (MyIndex a = 0; a < N; ++a) { ... } +/// @endcode +/// +/// This pattern no longer works because it requires implicit conversion of int +/// to TypeSafeIndex. Instead, the following pattern needs to be used: +/// +/// @code +/// for (MyIndex a(0); a < N; ++a) { ... } +/// @endcode +/// +/// __Use with Eigen__ +/// +/// At the time of this writing when using the latest Eigen 3.4 preview branch, +/// a TypeSafeIndex cannot be directly used to index into an Eigen::Matrix; the +/// developer must explicitly introduce the `int` conversion: +/// @code +/// VectorXd some_vector = ...; +/// FooIndex foo_index = ...; +/// some_vector(foo_index) = 0.0; // Fails to compile. +/// some_vector(int{foo_index}) = 0.0; // Compiles OK. +/// @endcode +/// TODO(#15354) We hope to fix this irregularity in the future. +/// +/// @sa drake::geometry::Identifier +/// +/// @tparam Tag The name of the tag associated with a class type. The class +/// need not be a defined class. +template +class TypeSafeIndex { + public: + /// @name Constructors + ///@{ + + /// Default constructor; the result is an _invalid_ index. This only + /// exists to serve applications which require a default constructor. + TypeSafeIndex() {} + + /// Construction from a non-negative `int` value. The value must lie in the + /// range of [0, 2³¹). Constructor only promises to test validity in + /// Debug build. + explicit TypeSafeIndex(int64_t index) : index_(static_cast(index)) { + // NOTE: This tests the *input* value and not the result as an invalid + // input can lead to a truncation that appears valid. + DRAKE_ASSERT_VOID( + AssertValid(index, "Explicitly constructing an invalid index.")); + } + + /// Disallow construction from another index type. + template + TypeSafeIndex(const TypeSafeIndex& idx) = delete; + + TypeSafeIndex(const TypeSafeIndex&) = default; + + TypeSafeIndex(TypeSafeIndex&& other) noexcept : index_(other.index_) { + other.index_ = kDefaultInvalid; + } + ///@} + + /// @name Assignment + ///@{ + + TypeSafeIndex& operator=(const TypeSafeIndex&) = default; + + TypeSafeIndex& operator=(TypeSafeIndex&& other) noexcept { + index_ = other.index_; + other.index_ = kDefaultInvalid; + return *this; + } + + ///@} + + /// @name Utility methods + ///@{ + + /// Implicit conversion-to-int operator. + operator int() const { + DRAKE_ASSERT_VOID(AssertValid(index_, "Converting to an int.")); + return index_; + } + + /// Reports if the index is valid--the only operation on an invalid index + /// that doesn't throw an exception in Debug builds. + bool is_valid() const { + // All other error testing, with assert armed, indirectly enforces the + // invariant that the only way to get an invalid index is via the default + // constructor. This assertion will catch any crack in that effort. + DRAKE_ASSERT((index_ >= 0) || (index_ == kDefaultInvalid)); + return index_ >= 0; + } + + ///@} + + /// @name Arithmetic operators + ///@{ + + /// Prefix increment operator. + const TypeSafeIndex& operator++() { + DRAKE_ASSERT_VOID( + AssertValid(index_, "Pre-incrementing an invalid index.")); + DRAKE_ASSERT_VOID( + AssertNoOverflow(1, "Pre-incrementing produced an invalid index.")); + ++index_; + return *this; + } + + /// Postfix increment operator. + TypeSafeIndex operator++(int) { + DRAKE_ASSERT_VOID( + AssertValid(index_, "Post-incrementing an invalid index.")); + DRAKE_ASSERT_VOID( + AssertNoOverflow(1, "Post-incrementing produced an invalid index.")); + ++index_; + return TypeSafeIndex(index_ - 1); + } + + /// Prefix decrement operator. + /// In Debug builds, this method asserts that the resulting index is + /// non-negative. + const TypeSafeIndex& operator--() { + DRAKE_ASSERT_VOID( + AssertValid(index_, "Pre-decrementing an invalid index.")); + --index_; + DRAKE_ASSERT_VOID( + AssertValid(index_, "Pre-decrementing produced an invalid index.")); + return *this; + } + + /// Postfix decrement operator. + /// In Debug builds, this method asserts that the resulting index is + /// non-negative. + TypeSafeIndex operator--(int) { + DRAKE_ASSERT_VOID( + AssertValid(index_, "Post-decrementing an invalid index.")); + --index_; + DRAKE_ASSERT_VOID(AssertValid( + index_, "Post-decrementing produced an invalid index.")); + return TypeSafeIndex(index_ + 1); + } + ///@} + + /// @name Compound assignment operators + ///@{ + + /// Addition assignment operator. + /// In Debug builds, this method asserts that the resulting index is + /// non-negative. + TypeSafeIndex& operator+=(int i) { + DRAKE_ASSERT_VOID( + AssertValid(index_, + "In-place addition with an int on an invalid index.")); + DRAKE_ASSERT_VOID(AssertNoOverflow( + i, "In-place addition with an int produced an invalid index.")); + index_ += i; + DRAKE_ASSERT_VOID(AssertValid( + index_, "In-place addition with an int produced an invalid index.")); + return *this; + } + + /// Allow addition for indices with the same tag. + TypeSafeIndex& operator+=(const TypeSafeIndex& other) { + DRAKE_ASSERT_VOID(AssertValid( + index_, "In-place addition with another index invalid LHS.")); + DRAKE_ASSERT_VOID(AssertValid( + other.index_, "In-place addition with another index invalid RHS.")); + DRAKE_ASSERT_VOID(AssertNoOverflow( + other.index_, + "In-place addition with another index produced an invalid index.")); + index_ += other.index_; + DRAKE_ASSERT_VOID(AssertValid( + index_, + "In-place addition with another index produced an invalid index.")); + return *this; + } + + /// Prevent addition for indices of different tags. + template + TypeSafeIndex& operator+=(const TypeSafeIndex& u) = delete; + + /// Subtraction assignment operator. + /// In Debug builds, this method asserts that the resulting index is + /// non-negative. + TypeSafeIndex& operator-=(int i) { + DRAKE_ASSERT_VOID(AssertValid( + index_, "In-place subtraction with an int on an invalid index.")); + DRAKE_ASSERT_VOID(AssertNoOverflow( + -i, "In-place subtraction with an int produced an invalid index.")); + index_ -= i; + DRAKE_ASSERT_VOID(AssertValid( + index_, "In-place subtraction with an int produced an invalid index.")); + return *this; + } + + /// Allow subtraction for indices with the same tag. + TypeSafeIndex& operator-=(const TypeSafeIndex& other) { + DRAKE_ASSERT_VOID(AssertValid( + index_, "In-place subtraction with another index invalid LHS.")); + DRAKE_ASSERT_VOID(AssertValid(other.index_, + "In-place subtraction with another index invalid RHS.")); + // No test for overflow; it would only be necessary if other had a negative + // index value. In that case, it would be invalid and that would be caught + // by the previous assertion. + index_ -= other.index_; + DRAKE_ASSERT_VOID(AssertValid( + index_, + "In-place subtraction with another index produced an invalid index.")); + return *this; + } + + /// Prevent subtraction for indices of different tags. + template + TypeSafeIndex& operator-=(const TypeSafeIndex& u) = delete; + + ///@} + + /// @name Exclusive comparison operators + /// + /// In order to prevent indices _of different type_ being added together or + /// compared against each other, we explicitly include indices of this type, + /// but exclude indices of all other types. This implicitly allows all + /// _other_ objects that can be converted to int types. + ///@{ + + // Note for developers: Each comparison operator has a SFINAE-based version + // for handling comparison with unsigned values (created to allow comparison + // with size_t). The explicit methods are necessary to enable comparison + // without unsigned/signed comparison warnings (which Drake considers to be an + // error). Furthermore, the SFINAE is necessary to prevent ambiguity. + // Index == int can be resolved two ways: + // + // - convert Index to int + // - promote int to size_t + // + // SFINAE prevents the latter. + + /// Allow equality test with indices of this tag. + bool operator==(const TypeSafeIndex& other) const { + DRAKE_ASSERT_VOID(AssertValid(index_, "Testing == with invalid LHS.")); + DRAKE_ASSERT_VOID( + AssertValid(other.index_, "Testing == with invalid RHS.")); + return index_ == other.index_; + } + + /// Allow equality test with unsigned integers. + template + typename std::enable_if_t< + std::is_integral_v && std::is_unsigned_v, bool> + operator==(const U& value) const { + DRAKE_ASSERT_VOID(AssertValid(index_, "Testing == with invalid index.")); + return value <= static_cast(kMaxIndex) && + index_ == static_cast(value); + } + + /// Prevent equality tests with indices of other tags. + template + bool operator==(const TypeSafeIndex& u) const = delete; + + /// Allow inequality test with indices of this tag. + bool operator!=(const TypeSafeIndex& other) const { + DRAKE_ASSERT_VOID(AssertValid(index_, "Testing != with invalid LHS.")); + DRAKE_ASSERT_VOID( + AssertValid(other.index_, "Testing != with invalid RHS.")); + return index_ != other.index_; + } + + /// Allow inequality test with unsigned integers. + template + typename std::enable_if_t< + std::is_integral_v && std::is_unsigned_v, bool> + operator!=(const U& value) const { + DRAKE_ASSERT_VOID(AssertValid(index_, "Testing != with invalid index.")); + return value > static_cast(kMaxIndex) || + index_ != static_cast(value); + } + + /// Prevent inequality test with indices of other tags. + template + bool operator!=(const TypeSafeIndex& u) const = delete; + + /// Allow less than test with indices of this tag. + bool operator<(const TypeSafeIndex& other) const { + DRAKE_ASSERT_VOID(AssertValid(index_, "Testing < with invalid LHS.")); + DRAKE_ASSERT_VOID( + AssertValid(other.index_, "Testing < with invalid RHS.")); + return index_ < other.index_; + } + + /// Allow less than test with unsigned integers. + template + typename std::enable_if_t< + std::is_integral_v && std::is_unsigned_v, bool> + operator<(const U& value) const { + DRAKE_ASSERT_VOID(AssertValid(index_, "Testing < with invalid index.")); + return value > static_cast(kMaxIndex) || + index_ < static_cast(value); + } + + /// Prevent less than test with indices of other tags. + template + bool operator<(const TypeSafeIndex& u) const = delete; + + /// Allow less than or equals test with indices of this tag. + bool operator<=(const TypeSafeIndex& other) const { + DRAKE_ASSERT_VOID(AssertValid(index_, "Testing <= with invalid LHS.")); + DRAKE_ASSERT_VOID( + AssertValid(other.index_, "Testing <= with invalid RHS.")); + return index_ <= other.index_; + } + + /// Allow less than or equals test with unsigned integers. + template + typename std::enable_if_t< + std::is_integral_v && std::is_unsigned_v, bool> + operator<=(const U& value) const { + DRAKE_ASSERT_VOID(AssertValid(index_, "Testing <= with invalid index.")); + return value > static_cast(kMaxIndex) || + index_ <= static_cast(value); + } + + /// Prevent less than or equals test with indices of other tags. + template + bool operator<=(const TypeSafeIndex& u) const = delete; + + /// Allow greater than test with indices of this tag. + bool operator>(const TypeSafeIndex& other) const { + DRAKE_ASSERT_VOID(AssertValid(index_, "Testing > with invalid LHS.")); + DRAKE_ASSERT_VOID( + AssertValid(other.index_, "Testing > with invalid RHS.")); + return index_ > other.index_; + } + + /// Allow greater than test with unsigned integers. + template + typename std::enable_if_t< + std::is_integral_v && std::is_unsigned_v, bool> + operator>(const U& value) const { + DRAKE_ASSERT_VOID(AssertValid(index_, "Testing > with invalid index.")); + return value <= static_cast(kMaxIndex) && + index_ > static_cast(value); + } + + /// Prevent greater than test with indices of other tags. + template + bool operator>(const TypeSafeIndex& u) const = delete; + + /// Allow greater than or equals test with indices of this tag. + bool operator>=(const TypeSafeIndex& other) const { + DRAKE_ASSERT_VOID(AssertValid(index_, "Testing >= with invalid LHS.")); + DRAKE_ASSERT_VOID( + AssertValid(other.index_, "Testing >= with invalid RHS.")); + return index_ >= other.index_; + } + + /// Allow greater than or equals test with unsigned integers. + template + typename std::enable_if_t< + std::is_integral_v && std::is_unsigned_v, bool> + operator>=(const U& value) const { + DRAKE_ASSERT_VOID(AssertValid(index_, "Testing >= with invalid index.")); + return value <= static_cast(kMaxIndex) && + index_ >= static_cast(value); + } + + /// Prevent greater than or equals test with indices of other tags. + template + bool operator>=(const TypeSafeIndex& u) const = delete; + + ///@} + + /// Implements the @ref hash_append concept. And invalid index will + /// successfully hash (in order to satisfy STL requirements), and it is up to + /// the user to confirm it is valid before using it as a key (or other hashing + /// application). + template + friend void hash_append(HashAlgorithm& hasher, + const TypeSafeIndex& i) noexcept { + using drake::hash_append; + hash_append(hasher, i.index_); + } + + private: + // Checks if this index lies in the valid range; throws an exception if not. + // Invocations provide a string explaining the origin of the bad value. + static void AssertValid(int64_t index, const char* source) { + if (index < 0 || index > kMaxIndex) { + throw std::runtime_error( + std::string(source) + " Type \"" + + drake::NiceTypeName::Get>() + + "\", has an invalid value; it must lie in the range [0, 2³¹ - 1]."); + } + } + + // This tests for overflow conditions based on adding the given delta into + // the current index value. + void AssertNoOverflow(int delta, const char* source) const { + if (delta > 0 && index_ > kMaxIndex - delta) { + throw std::runtime_error( + std::string(source) + " Type \"" + + drake::NiceTypeName::Get>() + + "\", has overflowed."); + } + } + + // This value helps distinguish indices that are invalid through construction + // or moves versus user manipulation. + enum { + kDefaultInvalid = -1234567 + }; + + int index_{kDefaultInvalid}; + + // The largest representable index. + // Note: The handling of comparisons of TypeSafeIndex with unsigned integral + // types with *fewer* bits relies on truncations of *this* value consisting + // of all 1s. The maximum int satisfies that requirement. If, for whatever + // reason, some *alternative* maximum index is preferred (or the underlying + // integral type of TypeSafeIndex changes), keep this requirement in mind. + // Otherwise, comparisons against smaller unsigned integral types is likely + // to fail. + static constexpr int kMaxIndex = std::numeric_limits::max(); +}; + +template +typename std::enable_if_t< + std::is_integral_v && std::is_unsigned_v, bool> +operator==(const U& value, const TypeSafeIndex& tag) { + return tag.operator==(value); +} + +template +typename std::enable_if_t< + std::is_integral_v && std::is_unsigned_v, bool> +operator!=(const U& value, const TypeSafeIndex& tag) { + return tag.operator!=(value); +} + +template +typename std::enable_if_t< + std::is_integral_v && std::is_unsigned_v, bool> +operator<(const U& value, const TypeSafeIndex& tag) { + return tag >= value; +} + +template +typename std::enable_if_t< + std::is_integral_v && std::is_unsigned_v, bool> +operator<=(const U& value, const TypeSafeIndex& tag) { + return tag > value; +} + +template +typename std::enable_if_t< + std::is_integral_v && std::is_unsigned_v, bool> +operator>(const U& value, const TypeSafeIndex& tag) { + return tag <= value; +} + +template +typename std::enable_if_t< + std::is_integral_v && std::is_unsigned_v, bool> +operator>=(const U& value, const TypeSafeIndex& tag) { + return tag < value; +} + +} // namespace drake + +namespace std { + +/// Enables use of the type-safe index to serve as a key in STL containers. +/// @relates TypeSafeIndex +template +struct hash> : public drake::DefaultHash {}; +} // namespace std diff --git a/maliput_drake/include/drake/common/unused.h b/maliput_drake/include/drake/common/unused.h new file mode 100644 index 0000000..5a28b01 --- /dev/null +++ b/maliput_drake/include/drake/common/unused.h @@ -0,0 +1,53 @@ +#pragma once + +namespace drake { + +/// Documents the argument(s) as unused, placating GCC's -Wunused-parameter +/// warning. This can be called within function bodies to mark that certain +/// parameters are unused. +/// +/// When possible, removing the unused parameter is better than placating the +/// warning. However, in some cases the parameter is part of a virtual API or +/// template concept that is used elsewhere, so we can't remove it. In those +/// cases, this function might be an appropriate work-around. +/// +/// Here's rough advice on how to fix Wunused-parameter warnings: +/// +/// (1) If the parameter can be removed entirely, prefer that as the first +/// choice. (This may not be possible if, e.g., a method must match some +/// virtual API or template concept.) +/// +/// (2) Unless the parameter name has acute value, prefer to omit the name of +/// the parameter, leaving only the type, e.g. +/// @code +/// void Print(const State& state) override { /* No state to print. */ } +/// @endcode +/// changes to +/// @code +/// void Print(const State&) override { /* No state to print. */} +/// @endcode +/// This no longer triggers the warning and further makes it clear that a +/// parameter required by the API is definitively unused in the function. +/// +/// This is an especially good solution in the context of method +/// definitions (vs declarations); the parameter name used in a definition +/// is entirely irrelevant to Doxygen and most readers. +/// +/// (3) When leaving the parameter name intact has acute value, it is +/// acceptable to keep the name and mark it `unused`. For example, when +/// the name appears as part of a virtual method's base class declaration, +/// the name is used by Doxygen to document the method, e.g., +/// @code +/// /** Sets the default State of a System. This default implementation is to +/// set all zeros. Subclasses may override to use non-zero defaults. The +/// custom defaults may be based on the given @p context, when relevant. */ +/// virtual void SetDefault(const Context& context, State* state) const { +/// unused(context); +/// state->SetZero(); +/// } +/// @endcode +/// +template +void unused(const Args& ...) {} + +} // namespace drake diff --git a/maliput_drake/include/drake/common/value.h b/maliput_drake/include/drake/common/value.h new file mode 100644 index 0000000..22ce1e8 --- /dev/null +++ b/maliput_drake/include/drake/common/value.h @@ -0,0 +1,813 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +#include "drake/common/copyable_unique_ptr.h" +#include "drake/common/drake_copyable.h" +#include "drake/common/hash.h" +#include "drake/common/is_cloneable.h" +#include "drake/common/nice_type_name.h" + +namespace drake { + +#if !defined(DRAKE_DOXYGEN_CXX) +template +class Value; + +namespace internal { + +// A traits type for Value, where use_copy is true when T's copy constructor +// and copy-assignment operator are used and false when T's Clone is used. +template +struct ValueTraitsImpl {}; +template +using ValueTraits = ValueTraitsImpl>; + +// SFINAE type for whether Value(Arg1, Args...) should be a forwarding ctor. +// In our ctor overload that copies into the storage, choose_copy == true. +template +using ValueForwardingCtorEnabled = typename std::enable_if_t< + // There must be such a constructor. + std::is_constructible_v && + // Disable this ctor when given T directly; in that case, we + // should call our Value(const T&) ctor above, not try to copy- + // construct a T(const T&). + !std::is_same_v && + !std::is_same_v && + // Only allow real ctors, not POD "constructor"s. + !std::is_fundamental_v && + // Disambiguate our copy implementation from our clone implementation. + (choose_copy == std::is_copy_constructible_v)>; + +template +using remove_cvref_t = std::remove_cv_t>; + +} // namespace internal +#endif + +/// A fully type-erased container class. An AbstractValue stores an object of +/// some type T (where T is declared during at construction time) that at +/// runtime can be passed between functions without mentioning T. Only when +/// the stored T must be accessed does the user need to mention T again. +/// +/// (Advanced.) Note that AbstractValue's getters and setters method declare +/// that "If T does not match, a std::logic_error will be thrown with a helpful +/// error message". The code that implements this check uses hashing, so in +/// the extraordinarily unlikely case of a 64-bit hash collision, the error may +/// go undetected in Release builds. (Debug builds have extra checks that will +/// trigger.) +/// +/// (Advanced.) Only Value should inherit directly from AbstractValue. +/// User-defined classes with additional features may inherit from Value. +class AbstractValue { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(AbstractValue) + + virtual ~AbstractValue(); + + /// Constructs an AbstractValue using T's default constructor, if available. + /// This is only available for T's that support default construction. +#if !defined(DRAKE_DOXYGEN_CXX) + template >> +#endif + static std::unique_ptr Make(); + + /// Returns an AbstractValue containing the given @p value. + template + static std::unique_ptr Make(const T& value); + + /// Returns the value wrapped in this AbstractValue as a const reference. + /// The reference remains valid only until this object is set or destroyed. + /// @tparam T The originally declared type of this AbstractValue, e.g., from + /// AbstractValue::Make() or Value::Value(). If T does not match, a + /// std::logic_error will be thrown with a helpful error message. + template + const T& get_value() const { return cast().get_value(); } + + /// Returns the value wrapped in this AbstractValue as mutable reference. + /// The reference remains valid only until this object is set or destroyed. + /// @tparam T The originally declared type of this AbstractValue, e.g., from + /// AbstractValue::Make() or Value::Value(). If T does not match, a + /// std::logic_error will be thrown with a helpful error message. + template + T& get_mutable_value() { return cast().get_mutable_value(); } + + /// Sets the value wrapped in this AbstractValue. + /// @tparam T The originally declared type of this AbstractValue, e.g., from + /// AbstractValue::Make() or Value::Value(). If T does not match, a + /// std::logic_error will be thrown with a helpful error message. + template + void set_value(const T& v) { cast().set_value(v); } + + /// Returns the value wrapped in this AbstractValue, if T matches the + /// originally declared type of this AbstractValue. + /// @tparam T The originally declared type of this AbstractValue, e.g., from + /// AbstractValue::Make() or Value::Value(). If T does not match, + /// returns nullptr. + template + const T* maybe_get_value() const; + + /// Returns the mutable value wrapped in this AbstractValue, if T matches the + /// originally declared type of this AbstractValue. + /// @tparam T The originally declared type of this AbstractValue, e.g., from + /// AbstractValue::Make() or Value::Value(). If T does not match, + /// returns nullptr. + template + T* maybe_get_mutable_value(); + + /// Returns a copy of this AbstractValue. + virtual std::unique_ptr Clone() const = 0; + + /// Copies the value in @p other to this value. If other is not compatible + /// with this object, a std::logic_error will be thrown with a helpful error + /// message. + virtual void SetFrom(const AbstractValue& other) = 0; + + /// Returns typeid of the contained object of type T. If T is polymorphic, + /// this returns the typeid of the most-derived type of the contained object. + virtual const std::type_info& type_info() const = 0; + + /// Returns typeid(T) for this Value object. If T is polymorphic, this + /// does NOT reflect the typeid of the most-derived type of the contained + /// object; the result is always the base type T. + virtual const std::type_info& static_type_info() const = 0; + + /// Returns a human-readable name for the underlying type T. This may be + /// slow but is useful for error messages. If T is polymorphic, this returns + /// the typeid of the most-derived type of the contained object. + std::string GetNiceTypeName() const; + + protected: +#if !defined(DRAKE_DOXYGEN_CXX) + // Use a struct argument (instead of a bare size_t) so that no code + // tries to convert a single-element numeric initializer_list to + // a `const AbstractValue&`. (This works around a bug in GCC 5.) + struct Wrap { size_t value{}; }; + explicit AbstractValue(Wrap hash) + : type_hash_(hash.value) {} +#endif + + private: + template bool is_maybe_matched() const; + template const Value& cast() const; + template Value& cast(); + template [[noreturn]] void ThrowCastError() const; + [[noreturn]] void ThrowCastError(const std::string&) const; + + // The TypeHash::value supplied by the Value constructor. + const size_t type_hash_; +}; + +/// A container class for an arbitrary type T (with some restrictions). This +/// class inherits from AbstractValue and therefore at runtime can be passed +/// between functions without mentioning T. +/// +/// Example: +/// @code +/// void print_string(const AbstractValue& arg) { +/// const std::string& message = arg.get_value(); +/// std::cerr << message; +/// } +/// void meow() { +/// const Value value("meow"); +/// print_string(value); +/// } +/// @endcode +/// +/// (Advanced.) User-defined classes with additional features may subclass +/// Value, but should take care to override Clone(). +/// +/// @tparam T Must be copy-constructible or cloneable. Must not be a pointer, +/// array, nor have const, volatile, or reference qualifiers. +template +class Value : public AbstractValue { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Value) + + static_assert( + std::is_same_v>, + "T should not have const, volatile, or reference qualifiers."); + + static_assert( + !std::is_pointer_v && !std::is_array_v, + "T cannot be a pointer or array."); + + /// Constructs a Value using T's default constructor, if available. + /// This is only available for T's that support default construction. +#if !defined(DRAKE_DOXYGEN_CXX) + template >> +#endif + Value(); + + /// Constructs a Value by copying or cloning the given value @p v. + explicit Value(const T& v); + + /// Constructs a Value by forwarding the given @p args to T's constructor, + /// if available. This is only available for non-primitive T's that are + /// constructible from @p args. +#if defined(DRAKE_DOXYGEN_CXX) + template + explicit Value(Args&&... args); +#else + // This overload is for copyable T. + template > + explicit Value(Arg1&& arg1, Args&&... args); + // This overload is for cloneable T. + template > + explicit Value(Arg1&& arg1, Args&&... args); +#endif + + /// Constructs a Value by copying or moving the given value @p v. + /// @pre v is non-null. + explicit Value(std::unique_ptr v); + + ~Value() override {} + + /// Returns a const reference to the stored value. + /// The reference remains valid only until this object is set or destroyed. + const T& get_value() const; + + /// Returns a mutable reference to the stored value. + /// The reference remains valid only until this object is set or destroyed. + T& get_mutable_value(); + + /// Replaces the stored value with a new one. + void set_value(const T& v); + + // AbstractValue overrides. + std::unique_ptr Clone() const override; + void SetFrom(const AbstractValue& other) override; + const std::type_info& type_info() const final; + const std::type_info& static_type_info() const final; + + // A using-declaration adds these methods into our class's Doxygen. + using AbstractValue::static_type_info; + using AbstractValue::GetNiceTypeName; + + private: + using Traits = internal::ValueTraits; + typename Traits::Storage value_; +}; + +#if !defined(DRAKE_DOXYGEN_CXX) +// Declare some private helper structs. +namespace internal { + +// Extracts a hash of the type `T` in a __PRETTY_FUNCTION__ templated on T. +// +// For, e.g., TypeHash the pretty_func string `pretty` looks like this: +// GCC 7.3: "... calc() [with T = int; size_t K = 16; ..." +// Clang 6.0: "... calc() [T = int]" +// +// We grab the characters for T's type (e.g., "int") and hash them using FNV1a. +// https://en.wikipedia.org/wiki/Fowler%E2%80%93Noll%E2%80%93Vo_hash_function +// +// The value of @p which_argument chooses the which pretty template argument to +// hash. (Only one argument at a time is ever hashed.) In the GCC example +// above, which_argument of 0 hashes "int" and 1 hashes "16". +// +// If T is a template type like "std::vector", we only hash "std::vector" +// here. We stop when we reach a '<' because each template argument is hashed +// separately below using parameter packs (see `TypeHasher>`). This +// avoids compiler bugs where __PRETTY_FUNCTION__ is fickle about the spelling +// of "T = std::vector" vs "T = std::vector>", varying +// it from one method to the next. Because we visit each base type in turn, we +// hash "std::vector" then "U" then "std::allocator" then "U" and so it doesn't +// matter exactly how templates end up being spelled in __PRETTY_FUNCTION__. +// +// When @p discard_nested is true, then stopping at '<' means our success-return +// value will be true; if discard_nested is false then seeing any '<' is an +// error. Thus, we can detect and fail-fast when our specializations for +// template parameters fail to match. +// +// When @p discard_cast is true, we will omit a leading cast-expression after +// the equals sign, e.g., when pretty looks like "... [with K = (MyEnum)0]". +// If we see any other open-paren than this possibly-skipped cast, then our +// success-return value will be false. Thus, we can detect and fail-fast when +// our specializations for non-type template parameters fail to match, or when +// T's like function pointer signatures appear. +// +// Note that the compiler is required to inform us at compile-time if there are +// undefined operations in the below, such as running off the end of a string. +// Therefore, so as long as this function compiles, we know that `pretty` had +// at least something that looks like "T = ..." in it. +// +// Returns true on success / false on failure. +constexpr bool hash_template_argument_from_pretty_func( + const char* pretty, int which_argument, + bool discard_nested, bool discard_cast, + FNV1aHasher* result) { + // Advance to the desired template argument. For example, if which_argument + // is 0 and pretty == "... calc() [T = int]", then advance to the typename + // after the "T = " so that the cursor `p` is pointing at the 'i' in "int". + const char* p = pretty; + for (int n = 0; n <= which_argument; ++n) { + for (; (*p != '='); ++p) {} // Advance to the '=' that we want. + ++p; // Advance to ' '. + ++p; // Advance to the typename we want. + } + + // For enums, GCC 7's pretty says "(MyEnum)0" not "MyEnum::kFoo". We'll strip + // off the useless parenthetical. + if (discard_cast && (*p == '(')) { + for (; (*p != ')'); ++p) {} // Advance to the ')'. + ++p; + } + + // Hash the characters in the typename, ending either when the typename ends + // (';' or ']') or maybe when the first template argument begins ('<'). + while ((*p != ';') && (*p != ']')) { + // Map Clang's spelling for the anonymous namespace to match GCC. Start by + // searching for the clang spelling starting at `p` ... + const char* const clang_spelling = "(anonymous namespace)"; + const char* clang_iter = clang_spelling; + const char* pretty_iter = p; + for (; *clang_iter != 0 && *pretty_iter != 0; ++clang_iter, ++pretty_iter) { + if (*clang_iter != *pretty_iter) { break; } + } + // ... and if we found the entire clang_spelling, emit gcc_spelling instead. + if (*clang_iter == 0) { + const char* const gcc_spelling = "{anonymous}"; + for (const char* c = gcc_spelling; *c; ++c) { + result->add_byte(*c); + } + p = pretty_iter; + continue; + } + // GCC distinguishes between "" and "{anonymous}", while Clang does + // not. Map "" to "{anonymous}" for consistency and to avoid + // confusion with nested types ("<>") below. + const char* const unnamed_spelling = ""; + const char* unnamed_iter = unnamed_spelling; + pretty_iter = p; + for (; *unnamed_iter != 0 && *pretty_iter != 0; + ++unnamed_iter, ++pretty_iter) { + if (*unnamed_iter != *pretty_iter) { + break; + } + } + if (*unnamed_iter == 0) { + const char* const anonymous_spelling = "{anonymous}"; + for (const char* c = anonymous_spelling; *c; ++c) { + result->add_byte(*c); + } + p = pretty_iter; + continue; + } + + // If we find a nested type ("<>"), then either we expected it (in which + // case we're done) or we didn't expect it (and something is wrong). + if (*p == '<') { + if (discard_nested) { + break; + } else { + return false; + } + } + + // If the type has parens (such as a function pointer or std::function), + // then we can't handle it. Adding support for function types involves not + // only unpacking the return and argument types, but also adding support + // for const / volatile / reference / etc.). + if (*p == '(') { + return false; + } + + result->add_byte(*p); + ++p; + } + + return true; +} + +// Akin to C++17 std::void_t<>. +template +using typehasher_void_t = void; + +// Traits type to ask whether T::NonTypeTemplateParameter exists. +template +struct TypeHasherHasNonTypeTemplateParameter { + static constexpr bool value = false; +}; +template +struct TypeHasherHasNonTypeTemplateParameter< + T, typehasher_void_t> { + static constexpr bool value = true; +}; + +// Provides a struct templated on T so that __PRETTY_FUNCTION__ will express T +// at compile time. The calc() function feeds the string representation of T +// to `result`. Returns true on success / false on failure. This base struct +// handles non-templated values (e.g., int); in a specialization down below, we +// handle template template T's. +template ::value> +struct TypeHasher { + // Returns true on success / false on failure. + static constexpr bool calc(FNV1aHasher* result) { + // With discard_nested disabled here, the hasher will fail if it sees a + // '<' in the typename. If that happens, it means that the parameter pack + // specialization(s) below did not match as expected. + const int which_argument = 0; + const bool discard_nested = false; + const bool discard_cast = false; + return hash_template_argument_from_pretty_func( + __PRETTY_FUNCTION__, which_argument, + discard_nested, discard_cast, result); + } +}; + +// Provides a struct templated on Ts... with a calc() that hashes a sequence of +// types (a template parameter pack). +template +struct ParameterPackHasher {}; +// Specializes for base case: an empty pack. +template <> +struct ParameterPackHasher<> { + static constexpr bool calc(FNV1aHasher*) { return true; } +}; +// Specializes for inductive case: recurse using first + rest. +template +struct ParameterPackHasher { + static constexpr bool calc(FNV1aHasher* result) { + bool success = TypeHasher::calc(result); + if (sizeof...(B)) { + // Add delimiter so that pair and pair are distinct. + result->add_byte(','); + success = success && ParameterPackHasher::calc(result); + } + return success; + } +}; + +// Specializes TypeHasher for template types T so that we have the typename of +// each template argument separately from T's outer type (as explained in the +// overview above). +template