From 1accfe6c067afae7f39627f52536b62e97d1cba0 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Sat, 15 Apr 2017 07:15:21 -0400 Subject: [PATCH] Code format: distance (#172) --- .clang-format | 2 +- CMakeLists.txt | 2 + cmake/Findactionlib.cmake | 2 +- cmake/Findcontrol_msgs.cmake | 2 +- include/aikido/distance/DistanceMetric.hpp | 13 ++-- include/aikido/distance/RnEuclidean.hpp | 14 ++--- include/aikido/distance/SO2Angular.hpp | 26 ++++---- include/aikido/distance/SO3Angular.hpp | 23 +++---- include/aikido/distance/Weighted.hpp | 31 +++++----- include/aikido/distance/defaults.hpp | 10 ++- .../distance/detail/RnEuclidean-impl.hpp | 23 +++---- .../aikido/distance/detail/defaults-impl.hpp | 43 +++++++------ include/aikido/planner/SnapPlanner.hpp | 1 + .../planner/parabolic/ParabolicTimer.hpp | 3 +- src/distance/CMakeLists.txt | 1 + src/distance/RnEuclidean.cpp | 18 ++---- src/distance/SO2Angular.cpp | 23 ++++--- src/distance/SO3Angular.cpp | 16 ++--- src/distance/Weighted.cpp | 62 +++++++++++-------- src/distance/defaults.cpp | 13 ++-- 20 files changed, 168 insertions(+), 160 deletions(-) diff --git a/.clang-format b/.clang-format index d5d53df52e..08a5ddf0bd 100644 --- a/.clang-format +++ b/.clang-format @@ -49,7 +49,7 @@ IncludeCategories: Priority: 1 - Regex: '^(<|")aikido/' # Aikido headers Priority: 3 - - Regex: '^(<[a-z]+)' # dependency headers + - Regex: '^(<[A-z]+)' # dependency headers Priority: 2 - Regex: '^".*' # headers relative to this project Priority: 4 diff --git a/CMakeLists.txt b/CMakeLists.txt index dc5277beed..9aba8facbe 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -89,8 +89,10 @@ endfunction() # TODO: Temporarily split header list per subdirectories. file(GLOB_RECURSE headers_planner "${CMAKE_SOURCE_DIR}/include/aikido/planner/*.hpp") file(GLOB_RECURSE headers_util "${CMAKE_SOURCE_DIR}/include/aikido/util/*.hpp") +file(GLOB_RECURSE headers_distance "${CMAKE_SOURCE_DIR}/include/aikido/distance/*.hpp") format_add_sources(${headers_planner}) format_add_sources(${headers_util}) +format_add_sources(${headers_distance}) #============================================================================== # Helper functions. diff --git a/cmake/Findactionlib.cmake b/cmake/Findactionlib.cmake index 79aafc22c5..b5b7a7dafa 100644 --- a/cmake/Findactionlib.cmake +++ b/cmake/Findactionlib.cmake @@ -13,7 +13,7 @@ find_package(PkgConfig QUIET REQUIRED) # Check to see if pkgconfig is installed. set(PKG_CONFIG_USE_CMAKE_PREFIX_PATH TRUE) -pkg_check_modules(PC_actionlib actionlib QUIET REQUIRED) +pkg_check_modules(PC_actionlib actionlib QUIET) # Include directories find_path(actionlib_INCLUDE_DIRS diff --git a/cmake/Findcontrol_msgs.cmake b/cmake/Findcontrol_msgs.cmake index ad9fab691f..64ca495b5c 100644 --- a/cmake/Findcontrol_msgs.cmake +++ b/cmake/Findcontrol_msgs.cmake @@ -12,7 +12,7 @@ find_package(PkgConfig QUIET REQUIRED) # Check to see if pkgconfig is installed. set(PKG_CONFIG_USE_CMAKE_PREFIX_PATH TRUE) -pkg_check_modules(PC_control_msgs control_msgs QUIET REQUIRED) +pkg_check_modules(PC_control_msgs control_msgs QUIET) # Include directories find_path(control_msgs_INCLUDE_DIRS diff --git a/include/aikido/distance/DistanceMetric.hpp b/include/aikido/distance/DistanceMetric.hpp index 48082dafd7..ded6b61150 100644 --- a/include/aikido/distance/DistanceMetric.hpp +++ b/include/aikido/distance/DistanceMetric.hpp @@ -3,10 +3,9 @@ #include "../statespace/StateSpace.hpp" -namespace aikido -{ -namespace distance -{ +namespace aikido { +namespace distance { + /// Implements a distance metric defined on a StateSpace class DistanceMetric { @@ -29,6 +28,8 @@ class DistanceMetric }; using DistanceMetricPtr = std::shared_ptr; -} -} + +} // namespace distance +} // namespace aikido + #endif diff --git a/include/aikido/distance/RnEuclidean.hpp b/include/aikido/distance/RnEuclidean.hpp index 51954c353c..e55a30f4a6 100644 --- a/include/aikido/distance/RnEuclidean.hpp +++ b/include/aikido/distance/RnEuclidean.hpp @@ -1,13 +1,12 @@ #ifndef AIKIDO_DISTANCE_EUCLIDEANDISTANCEMETRIC_HPP_ #define AIKIDO_DISTANCE_EUCLIDEANDISTANCEMETRIC_HPP_ -#include "DistanceMetric.hpp" #include "../statespace/Rn.hpp" +#include "DistanceMetric.hpp" + +namespace aikido { +namespace distance { -namespace aikido -{ -namespace distance -{ /// Implements a Euclidean distance metric template class REuclidean : public DistanceMetric @@ -23,8 +22,9 @@ class REuclidean : public DistanceMetric /// Computes Euclidean distance between two states. /// \param _state1 The first state (type Rn::State) /// \param _state2 The second state (type Rn::State) - double distance(const statespace::StateSpace::State* _state1, - const statespace::StateSpace::State* _state2) const override; + double distance( + const statespace::StateSpace::State* _state1, + const statespace::StateSpace::State* _state2) const override; private: std::shared_ptr> mStateSpace; diff --git a/include/aikido/distance/SO2Angular.hpp b/include/aikido/distance/SO2Angular.hpp index c8566c445f..51cb74ca79 100644 --- a/include/aikido/distance/SO2Angular.hpp +++ b/include/aikido/distance/SO2Angular.hpp @@ -1,34 +1,36 @@ #ifndef AIKIDO_DISTANCE_ANGULARDISTANCEMETRIC_HPP_ #define AIKIDO_DISTANCE_ANGULARDISTANCEMETRIC_HPP_ -#include "DistanceMetric.hpp" #include "../statespace/SO2.hpp" +#include "DistanceMetric.hpp" + +namespace aikido { +namespace distance { -namespace aikido -{ -namespace distance -{ /// Computes the shortest distance between two angles in SO(2) class SO2Angular : public DistanceMetric { public: /// Constructor. /// \param _space The SO2 this distance metric operates on - explicit SO2Angular( - std::shared_ptr _space); + explicit SO2Angular(std::shared_ptr _space); // Documentation inherited statespace::StateSpacePtr getStateSpace() const override; - /// Computes shortest distance between two angles. (return value between 0 and pi) + /// Computes shortest distance between two angles. (return value between 0 and + /// pi) /// \param _state1 The first state (type: SO2::State) /// \param _state2 The second state (type: SO2::State) - double distance(const statespace::StateSpace::State* _state1, - const statespace::StateSpace::State* _state2) const override; + double distance( + const statespace::StateSpace::State* _state1, + const statespace::StateSpace::State* _state2) const override; private: std::shared_ptr mStateSpace; }; -} -} + +} // namespace distance +} // namespace aikido + #endif diff --git a/include/aikido/distance/SO3Angular.hpp b/include/aikido/distance/SO3Angular.hpp index 2f17b6d163..d6317b8f6f 100644 --- a/include/aikido/distance/SO3Angular.hpp +++ b/include/aikido/distance/SO3Angular.hpp @@ -1,21 +1,19 @@ #ifndef AIKIDO_DISTANCE_GEODESICDISTANCEMETRIC_HPP_ #define AIKIDO_DISTANCE_GEODESICDISTANCEMETRIC_HPP_ -#include "DistanceMetric.hpp" #include "../statespace/SO3.hpp" +#include "DistanceMetric.hpp" + +namespace aikido { +namespace distance { -namespace aikido -{ -namespace distance -{ /// Implements a distance metric on SO(3) class SO3Angular : public DistanceMetric { public: /// Constructor. /// \param _space The SO3 this distance metric operates on - explicit SO3Angular( - std::shared_ptr _space); + explicit SO3Angular(std::shared_ptr _space); // Documentation inherited statespace::StateSpacePtr getStateSpace() const override; @@ -23,12 +21,15 @@ class SO3Angular : public DistanceMetric /// Computes distance (in radians) between the two states /// \param _state1 The first state (type SO3::State) /// \param _state2 The second state (type SO3::State) - double distance(const statespace::StateSpace::State* _state1, - const statespace::StateSpace::State* _state2) const override; + double distance( + const statespace::StateSpace::State* _state1, + const statespace::StateSpace::State* _state2) const override; private: std::shared_ptr mStateSpace; }; -} -} + +} // namespace distance +} // namespace aikido + #endif diff --git a/include/aikido/distance/Weighted.hpp b/include/aikido/distance/Weighted.hpp index ef748c7a29..88989bbda6 100644 --- a/include/aikido/distance/Weighted.hpp +++ b/include/aikido/distance/Weighted.hpp @@ -1,13 +1,12 @@ #ifndef AIKIDO_DISTANCE_WEIGHTEDDISTANCEMETRIC_HPP_ #define AIKIDO_DISTANCE_WEIGHTEDDISTANCEMETRIC_HPP_ -#include "DistanceMetric.hpp" #include "../statespace/CartesianProduct.hpp" +#include "DistanceMetric.hpp" + +namespace aikido { +namespace distance { -namespace aikido -{ -namespace distance -{ /// Implements a distance metric on a CartesianProduct. This metric computes /// the weighted /// sum of distances on the individual components of the statespace. @@ -18,8 +17,9 @@ class Weighted : public DistanceMetric /// \param _space The state space /// \param _metrics A vector containing one element for every component of the /// CartesianProduct - Weighted(std::shared_ptr _space, - std::vector _metrics); + Weighted( + std::shared_ptr _space, + std::vector _metrics); /// Constructor. /// \param _space The state space @@ -27,9 +27,9 @@ class Weighted : public DistanceMetric /// CartesianProduct. The first element of every pair in the vector is the /// metric and the second is the weight to be applied to the metric. The /// weights must all be positive. - Weighted(std::shared_ptr _space, - std::vector> _metrics); - + Weighted( + std::shared_ptr _space, + std::vector> _metrics); // Documentation inherited statespace::StateSpacePtr getStateSpace() const override; @@ -38,13 +38,16 @@ class Weighted : public DistanceMetric /// of distances between their matching subcomponents. /// \param _state1 The first state (type CartesianProduct::State) /// \param _state2 The second state (type CartesianProduct::State) - double distance(const statespace::StateSpace::State* _state1, - const statespace::StateSpace::State* _state2) const override; + double distance( + const statespace::StateSpace::State* _state1, + const statespace::StateSpace::State* _state2) const override; private: std::shared_ptr mStateSpace; std::vector> mMetrics; }; -} -} + +} // namespace distance +} // namespace aikido + #endif diff --git a/include/aikido/distance/defaults.hpp b/include/aikido/distance/defaults.hpp index 700a729c7e..59cd329606 100644 --- a/include/aikido/distance/defaults.hpp +++ b/include/aikido/distance/defaults.hpp @@ -1,13 +1,11 @@ #ifndef AIKIDO_DISTANCE_DISTANCEMETRICDEFAULTS_HPP_ #define AIKIDO_DISTANCE_DISTANCEMETRICDEFAULTS_HPP_ -#include "DistanceMetric.hpp" #include "../statespace/StateSpace.hpp" +#include "DistanceMetric.hpp" -namespace aikido -{ -namespace distance -{ +namespace aikido { +namespace distance { /// Creates a DistanceMetric that is appropriate for the statespace of type /// Space /// \param _sspace The StateSpace the distance metric will operator on @@ -15,7 +13,7 @@ template std::unique_ptr createDistanceMetricFor( std::shared_ptr _sspace); -/// Creates a DistanceMetric that is appropriate for the statespace. +/// Creates a DistanceMetric that is appropriate for the statespace. /// \param _sspace The StateSpace the distance metric will operator on std::unique_ptr createDistanceMetric( statespace::StateSpacePtr _sspace); diff --git a/include/aikido/distance/detail/RnEuclidean-impl.hpp b/include/aikido/distance/detail/RnEuclidean-impl.hpp index 6e5945f023..e02665d098 100644 --- a/include/aikido/distance/detail/RnEuclidean-impl.hpp +++ b/include/aikido/distance/detail/RnEuclidean-impl.hpp @@ -4,30 +4,25 @@ namespace aikido { namespace distance { //============================================================================== -extern template -class REuclidean<0>; +extern template class REuclidean<0>; -extern template -class REuclidean<1>; +extern template class REuclidean<1>; -extern template -class REuclidean<2>; +extern template class REuclidean<2>; -extern template -class REuclidean<3>; +extern template class REuclidean<3>; -extern template -class REuclidean<6>; +extern template class REuclidean<6>; -extern template -class REuclidean; +extern template class REuclidean; //============================================================================= template REuclidean::REuclidean(std::shared_ptr> _space) - : mStateSpace(std::move(_space)) + : mStateSpace(std::move(_space)) { - if (mStateSpace == nullptr) { + if (mStateSpace == nullptr) + { throw std::invalid_argument("Rn is nullptr."); } } diff --git a/include/aikido/distance/detail/defaults-impl.hpp b/include/aikido/distance/detail/defaults-impl.hpp index ebc83f42ba..1cb05e767b 100644 --- a/include/aikido/distance/detail/defaults-impl.hpp +++ b/include/aikido/distance/detail/defaults-impl.hpp @@ -1,13 +1,13 @@ -#include "../Weighted.hpp" -#include "../SO2Angular.hpp" -#include "../SO3Angular.hpp" -#include "../RnEuclidean.hpp" +#include +#include "../../statespace/CartesianProduct.hpp" +#include "../../statespace/Rn.hpp" #include "../../statespace/SO2.hpp" #include "../../statespace/SO3.hpp" -#include "../../statespace/Rn.hpp" -#include "../../statespace/CartesianProduct.hpp" #include "../../util/metaprogramming.hpp" -#include +#include "../RnEuclidean.hpp" +#include "../SO2Angular.hpp" +#include "../SO3Angular.hpp" +#include "../Weighted.hpp" namespace aikido { namespace distance { @@ -18,7 +18,9 @@ using Ptr = std::unique_ptr; //============================================================================= template -struct createDistanceMetricFor_impl {}; +struct createDistanceMetricFor_impl +{ +}; //============================================================================= template <> @@ -110,22 +112,19 @@ struct createDistanceMetricFor_impl metrics.emplace_back(std::move(metric)); } - return make_unique( - std::move(_sspace), std::move(metrics)); + return make_unique(std::move(_sspace), std::move(metrics)); } }; //============================================================================= -using SupportedStateSpaces = util::type_list< - statespace::CartesianProduct, - statespace::R0, - statespace::R1, - statespace::R2, - statespace::R3, - statespace::R6, - statespace::SO2, - statespace::SO3 - >; +using SupportedStateSpaces = util::type_list; } // namespace detail @@ -137,8 +136,8 @@ std::unique_ptr createDistanceMetricFor( if (_sspace == nullptr) throw std::invalid_argument("_sspace is null."); - return detail::createDistanceMetricFor_impl - ::create(std::move(_sspace)); + return detail::createDistanceMetricFor_impl::create( + std::move(_sspace)); } } // namespace distance diff --git a/include/aikido/planner/SnapPlanner.hpp b/include/aikido/planner/SnapPlanner.hpp index d4ad30a252..9a4b15df5d 100644 --- a/include/aikido/planner/SnapPlanner.hpp +++ b/include/aikido/planner/SnapPlanner.hpp @@ -1,5 +1,6 @@ #ifndef AIKIDO_PLANNER_SNAP_PLANNER_HPP_ #define AIKIDO_PLANNER_SNAP_PLANNER_HPP_ + #include "../constraint/Testable.hpp" #include "../statespace/Interpolator.hpp" #include "../statespace/StateSpace.hpp" diff --git a/include/aikido/planner/parabolic/ParabolicTimer.hpp b/include/aikido/planner/parabolic/ParabolicTimer.hpp index 24fe7bc30f..73e9ff3dcf 100644 --- a/include/aikido/planner/parabolic/ParabolicTimer.hpp +++ b/include/aikido/planner/parabolic/ParabolicTimer.hpp @@ -1,8 +1,9 @@ #ifndef AIKIDO_PLANNER_PARABOLIC_PARABOLICTIMER_HPP_ #define AIKIDO_PLANNER_PARABOLIC_PARABOLICTIMER_HPP_ + +#include #include "../../trajectory/Interpolated.hpp" #include "../../trajectory/Spline.hpp" -#include namespace aikido { namespace planner { diff --git a/src/distance/CMakeLists.txt b/src/distance/CMakeLists.txt index c1bad8f9cc..6cf2ce6356 100644 --- a/src/distance/CMakeLists.txt +++ b/src/distance/CMakeLists.txt @@ -24,3 +24,4 @@ add_component_targets(${PROJECT_NAME} distance "${PROJECT_NAME}_distance") add_component_dependencies(${PROJECT_NAME} distance statespace) coveralls_add_sources(${sources}) +format_add_sources(${sources}) diff --git a/src/distance/RnEuclidean.cpp b/src/distance/RnEuclidean.cpp index 58c162f643..f54e842e6f 100644 --- a/src/distance/RnEuclidean.cpp +++ b/src/distance/RnEuclidean.cpp @@ -3,23 +3,17 @@ namespace aikido { namespace distance { -template -class REuclidean<0>; +template class REuclidean<0>; -template -class REuclidean<1>; +template class REuclidean<1>; -template -class REuclidean<2>; +template class REuclidean<2>; -template -class REuclidean<3>; +template class REuclidean<3>; -template -class REuclidean<6>; +template class REuclidean<6>; -template -class REuclidean; +template class REuclidean; } // namespace distance } // namespace aikido diff --git a/src/distance/SO2Angular.cpp b/src/distance/SO2Angular.cpp index 27fe1e5a64..570bee5c6b 100644 --- a/src/distance/SO2Angular.cpp +++ b/src/distance/SO2Angular.cpp @@ -4,11 +4,11 @@ namespace aikido { namespace distance { //============================================================================= -SO2Angular::SO2Angular( - std::shared_ptr _space) - : mStateSpace(std::move(_space)) +SO2Angular::SO2Angular(std::shared_ptr _space) + : mStateSpace(std::move(_space)) { - if (mStateSpace == nullptr) { + if (mStateSpace == nullptr) + { throw std::invalid_argument("SO2 is nullptr."); } } @@ -25,16 +25,15 @@ double SO2Angular::distance( const aikido::statespace::StateSpace::State* _state2) const { // Difference between angles - double diff = - mStateSpace->getAngle( - static_cast(_state1)) - - mStateSpace->getAngle( - static_cast(_state2)); + double diff = mStateSpace->getAngle( + static_cast(_state1)) + - mStateSpace->getAngle( + static_cast(_state2)); diff = std::fmod(std::fabs(diff), 2.0 * M_PI); if (diff > M_PI) - diff -= 2.0 * M_PI; + diff -= 2.0 * M_PI; return std::fabs(diff); } -} -} +} // namespace distance +} // namespace aikido diff --git a/src/distance/SO3Angular.cpp b/src/distance/SO3Angular.cpp index 2818de1d8b..ddfa106da1 100644 --- a/src/distance/SO3Angular.cpp +++ b/src/distance/SO3Angular.cpp @@ -4,11 +4,11 @@ namespace aikido { namespace distance { //============================================================================= -SO3Angular::SO3Angular( - std::shared_ptr _space) - : mStateSpace(std::move(_space)) +SO3Angular::SO3Angular(std::shared_ptr _space) + : mStateSpace(std::move(_space)) { - if (mStateSpace == nullptr) { + if (mStateSpace == nullptr) + { throw std::invalid_argument("SO3 is nullptr."); } } @@ -26,9 +26,9 @@ double SO3Angular::distance( { auto state1 = static_cast(_state1); auto state2 = static_cast(_state2); - return mStateSpace->getQuaternion(state1) - .angularDistance(mStateSpace->getQuaternion(state2)); + return mStateSpace->getQuaternion(state1).angularDistance( + mStateSpace->getQuaternion(state2)); } -} -} +} // namespace distance +} // namespace aikido diff --git a/src/distance/Weighted.cpp b/src/distance/Weighted.cpp index 8b04d036fc..8eb5c61b00 100644 --- a/src/distance/Weighted.cpp +++ b/src/distance/Weighted.cpp @@ -7,13 +7,15 @@ namespace distance { Weighted::Weighted( std::shared_ptr _space, std::vector _metrics) - : mStateSpace(std::move(_space)) + : mStateSpace(std::move(_space)) { - if (mStateSpace == nullptr) { + if (mStateSpace == nullptr) + { throw std::invalid_argument("CartesianProduct is nullptr"); } - if (mStateSpace->getNumSubspaces() != _metrics.size()) { + if (mStateSpace->getNumSubspaces() != _metrics.size()) + { std::stringstream msg; msg << "Must provide a metric for every subspace in the " "CartesianProduct. " @@ -23,14 +25,17 @@ Weighted::Weighted( } mMetrics.reserve(_metrics.size()); - for (size_t i = 0; i < mStateSpace->getNumSubspaces(); ++i) { - if (_metrics[i] == nullptr) { + for (size_t i = 0; i < mStateSpace->getNumSubspaces(); ++i) + { + if (_metrics[i] == nullptr) + { std::stringstream msg; msg << "DistanceMetric " << i << " is nullptr."; throw std::invalid_argument(msg.str()); } - if (mStateSpace->getSubspace<>(i) != _metrics[i]->getStateSpace()) { + if (mStateSpace->getSubspace<>(i) != _metrics[i]->getStateSpace()) + { std::stringstream msg; msg << "DistanceMetric " << i << " is not defined over the correct StateSpace."; @@ -44,14 +49,15 @@ Weighted::Weighted( Weighted::Weighted( std::shared_ptr _space, std::vector> _metrics) - : mStateSpace(std::move(_space)) - , mMetrics(std::move(_metrics)) + : mStateSpace(std::move(_space)), mMetrics(std::move(_metrics)) { - if (mStateSpace == nullptr) { + if (mStateSpace == nullptr) + { throw std::invalid_argument("CartesianProduct is nullptr"); } - if (mStateSpace->getNumSubspaces() != mMetrics.size()) { + if (mStateSpace->getNumSubspaces() != mMetrics.size()) + { std::stringstream msg; msg << "Must provide a metric for every subspace in the " "CartesianProduct. " @@ -60,19 +66,23 @@ Weighted::Weighted( throw std::invalid_argument(msg.str()); } - for (size_t i = 0; i < mStateSpace->getNumSubspaces(); ++i) { - if (mMetrics[i].first == nullptr) { + for (size_t i = 0; i < mStateSpace->getNumSubspaces(); ++i) + { + if (mMetrics[i].first == nullptr) + { std::stringstream msg; msg << "DistanceMetric " << i << " is nullptr."; throw std::invalid_argument(msg.str()); } - if (mStateSpace->getSubspace<>(i) != mMetrics[i].first->getStateSpace()) { + if (mStateSpace->getSubspace<>(i) != mMetrics[i].first->getStateSpace()) + { std::stringstream msg; msg << "DistanceMetric " << i << " is not defined over the correct StateSpace."; throw std::invalid_argument(msg.str()); } - if (mMetrics[i].second < 0) { + if (mMetrics[i].second < 0) + { std::stringstream msg; msg << "The weight for subspace " << i << " is negative. All weights must be positive."; @@ -92,19 +102,21 @@ double Weighted::distance( const aikido::statespace::StateSpace::State* _state1, const aikido::statespace::StateSpace::State* _state2) const { - auto state1 = - static_cast(_state1); - auto state2 = - static_cast(_state2); + auto state1 + = static_cast(_state1); + auto state2 + = static_cast(_state2); double dist = 0.0; - for (size_t i = 0; i < mMetrics.size(); ++i) { - dist += - mMetrics[i].second - * mMetrics[i].first->distance(mStateSpace->getSubState<>(state1, i), - mStateSpace->getSubState<>(state2, i)); + for (size_t i = 0; i < mMetrics.size(); ++i) + { + dist += mMetrics[i].second + * mMetrics[i].first->distance( + mStateSpace->getSubState<>(state1, i), + mStateSpace->getSubState<>(state2, i)); } return dist; } -} -} + +} // namespace distance +} // namespace aikido diff --git a/src/distance/defaults.cpp b/src/distance/defaults.cpp index 0b00981358..23485a4cf1 100644 --- a/src/distance/defaults.cpp +++ b/src/distance/defaults.cpp @@ -5,17 +5,16 @@ namespace distance { //============================================================================= std::unique_ptr createDistanceMetric( - statespace::StateSpacePtr _sspace) + statespace::StateSpacePtr _sspace) { if (_sspace == nullptr) throw std::invalid_argument("_sspace is null."); - return util::DynamicCastFactory< - detail::createDistanceMetricFor_impl, - util::DynamicCastFactory_shared_ptr, - statespace::StateSpace, - detail::SupportedStateSpaces - >::create(_sspace); + return util::DynamicCastFactory:: + create(_sspace); } } // namespace distance