diff --git a/CMakeLists.txt b/CMakeLists.txt index 513804a0..7a3da1ef 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,11 +1,29 @@ cmake_minimum_required(VERSION 3.15) project(bio_ik) -if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) - message("${PROJECT_NAME}: You did not request a specific build type: Choosing 'Release' for maximum performance") - set(CMAKE_BUILD_TYPE Release) + +include(cmake/StandardProjectSettings.cmake) + +add_library(project_options INTERFACE) +target_compile_features(project_options INTERFACE cxx_std_17) + +if(CMAKE_CXX_COMPILER_ID MATCHES ".*Clang") + option(ENABLE_BUILD_WITH_TIME_TRACE "Enable -ftime-trace to generate time tracing .json files on clang" OFF) + if(ENABLE_BUILD_WITH_TIME_TRACE) + target_compile_options(project_options INTERFACE -ftime-trace) + endif() endif() +add_library(project_warnings INTERFACE) + +# Add linker configuration +include(cmake/Linker.cmake) +configure_linker(project_options) + +# standard compiler warnings +include(cmake/CompilerWarnings.cmake) +set_project_warnings(project_warnings) + find_package(ament_cmake REQUIRED) find_package(ament_cmake_ros REQUIRED) @@ -116,6 +134,9 @@ ament_target_dependencies( target_link_libraries( ${PROJECT_NAME} + PRIVATE + project_options + project_warnings PUBLIC ${FANN_LIBRARIES} ${OpenMP_LIBS} @@ -137,6 +158,8 @@ target_include_directories(${PROJECT_NAME}_plugin PUBLIC target_link_libraries( ${PROJECT_NAME}_plugin PRIVATE + project_options + project_warnings ${PROJECT_NAME} ) diff --git a/cmake/Cache.cmake b/cmake/Cache.cmake new file mode 100644 index 00000000..c47b1d94 --- /dev/null +++ b/cmake/Cache.cmake @@ -0,0 +1,58 @@ +# Copyright 2021 PickNik Inc +# +# 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 PickNik Inc 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. + + +option(ENABLE_CACHE "Enable cache if available" ON) +if(NOT ENABLE_CACHE) + return() +endif() + +set(CACHE_OPTION + "ccache" + CACHE STRING "Compiler cache to be used") +set(CACHE_OPTION_VALUES "ccache" "sccache") +set_property(CACHE CACHE_OPTION PROPERTY STRINGS ${CACHE_OPTION_VALUES}) +list( + FIND + CACHE_OPTION_VALUES + ${CACHE_OPTION} + CACHE_OPTION_INDEX) + +if(${CACHE_OPTION_INDEX} EQUAL -1) + message( + STATUS + "Using custom compiler cache system: '${CACHE_OPTION}', explicitly supported entries are ${CACHE_OPTION_VALUES}") +endif() + +find_program(CACHE_BINARY ${CACHE_OPTION}) +if(CACHE_BINARY) + message(STATUS "${CACHE_OPTION} found and enabled") + set(CMAKE_CXX_COMPILER_LAUNCHER ${CACHE_BINARY}) +else() + message(WARNING "${CACHE_OPTION} is enabled but was not found. Not using it") +endif() diff --git a/cmake/CompilerWarnings.cmake b/cmake/CompilerWarnings.cmake new file mode 100644 index 00000000..e1307a80 --- /dev/null +++ b/cmake/CompilerWarnings.cmake @@ -0,0 +1,109 @@ +# Copyright 2021 PickNik Inc +# +# 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 PickNik Inc 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. + + +# from here: +# +# https://github.com/lefticus/cppbestpractices/blob/master/02-Use_the_Tools_Available.md + +function(set_project_warnings project_name) + option(WARNINGS_AS_ERRORS "Treat compiler warnings as errors" ON) + + set(MSVC_WARNINGS + /W4 # Baseline reasonable warnings + /w14242 # 'identifier': conversion from 'type1' to 'type1', possible loss of data + /w14254 # 'operator': conversion from 'type1:field_bits' to 'type2:field_bits', possible loss of data + /w14263 # 'function': member function does not override any base class virtual member function + /w14265 # 'classname': class has virtual functions, but destructor is not virtual instances of this class may not + # be destructed correctly + /w14287 # 'operator': unsigned/negative constant mismatch + /we4289 # nonstandard extension used: 'variable': loop control variable declared in the for-loop is used outside + # the for-loop scope + /w14296 # 'operator': expression is always 'boolean_value' + /w14311 # 'variable': pointer truncation from 'type1' to 'type2' + /w14545 # expression before comma evaluates to a function which is missing an argument list + /w14546 # function call before comma missing argument list + /w14547 # 'operator': operator before comma has no effect; expected operator with side-effect + /w14549 # 'operator': operator before comma has no effect; did you intend 'operator'? + /w14555 # expression has no effect; expected expression with side- effect + /w14619 # pragma warning: there is no warning number 'number' + /w14640 # Enable warning on thread un-safe static member initialization + /w14826 # Conversion from 'type1' to 'type_2' is sign-extended. This may cause unexpected runtime behavior. + /w14905 # wide string literal cast to 'LPSTR' + /w14906 # string literal cast to 'LPWSTR' + /w14928 # illegal copy-initialization; more than one user-defined conversion has been implicitly applied + /permissive- # standards conformance mode for MSVC compiler. + ) + + set(CLANG_WARNINGS + -Wall + -Wextra # reasonable and standard + -Wshadow # warn the user if a variable declaration shadows one from a parent context + # warn the user if a class with virtual functions has a non-virtual destructor. This helps + # catch hard to track down memory errors + -Wnon-virtual-dtor + -Wold-style-cast # warn for c-style casts + -Wcast-align # warn for potential performance problem casts + -Wunused # warn on anything being unused + -Woverloaded-virtual # warn if you overload (not override) a virtual function + -Wpedantic # warn if non-standard C++ is used + -Wconversion # warn on type conversions that may lose data + -Wsign-conversion # warn on sign conversions + -Wnull-dereference # warn if a null dereference is detected + -Wdouble-promotion # warn if float is implicit promoted to double + -Wformat=2 # warn on security issues around functions that format output (ie printf) + -Wimplicit-fallthrough # warn on statements that fallthrough without an explicit annotation + ) + + if(WARNINGS_AS_ERRORS) + set(CLANG_WARNINGS ${CLANG_WARNINGS} -Werror) + set(MSVC_WARNINGS ${MSVC_WARNINGS} /WX) + endif() + + set(GCC_WARNINGS + ${CLANG_WARNINGS} + -Wmisleading-indentation # warn if indentation implies blocks where blocks do not exist + -Wduplicated-cond # warn if if / else chain has duplicated conditions + -Wduplicated-branches # warn if if / else branches have duplicated code + -Wlogical-op # warn about logical operations being used where bitwise were probably wanted + -Wuseless-cast # warn if you perform a cast to the same type + ) + + if(MSVC) + set(PROJECT_WARNINGS ${MSVC_WARNINGS}) + elseif(CMAKE_CXX_COMPILER_ID MATCHES ".*Clang") + set(PROJECT_WARNINGS ${CLANG_WARNINGS}) + elseif(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") + set(PROJECT_WARNINGS ${GCC_WARNINGS}) + else() + message(AUTHOR_WARNING "No compiler warnings set for '${CMAKE_CXX_COMPILER_ID}' compiler.") + endif() + + target_compile_options(${project_name} INTERFACE ${PROJECT_WARNINGS}) + +endfunction() diff --git a/cmake/Doxygen.cmake b/cmake/Doxygen.cmake new file mode 100644 index 00000000..99ad8305 --- /dev/null +++ b/cmake/Doxygen.cmake @@ -0,0 +1,41 @@ +# Copyright 2021 PickNik Inc +# +# 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 PickNik Inc 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. + + +function(enable_doxygen) + option(ENABLE_DOXYGEN "Enable doxygen doc builds of source" OFF) + if(ENABLE_DOXYGEN) + set(DOXYGEN_CALLER_GRAPH YES) + set(DOXYGEN_CALL_GRAPH YES) + set(DOXYGEN_EXTRACT_ALL YES) + set(DOXYGEN_USE_MDFILE_AS_MAINPAGE "${CMAKE_SOURCE_DIR}/README.md") + find_package(Doxygen REQUIRED dot) + doxygen_add_docs(doxygen-docs ${PROJECT_SOURCE_DIR}) + + endif() +endfunction() diff --git a/cmake/Linker.cmake b/cmake/Linker.cmake new file mode 100644 index 00000000..b55a0f5e --- /dev/null +++ b/cmake/Linker.cmake @@ -0,0 +1,62 @@ +# Copyright 2021 PickNik Inc +# +# 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 PickNik Inc 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. + + +option(ENABLE_USER_LINKER "Enable a specific linker if available" OFF) + +include(CheckCXXCompilerFlag) + +set(USER_LINKER_OPTION + "lld" + CACHE STRING "Linker to be used") +set(USER_LINKER_OPTION_VALUES "lld" "gold" "bfd") +set_property(CACHE USER_LINKER_OPTION PROPERTY STRINGS ${USER_LINKER_OPTION_VALUES}) +list( + FIND + USER_LINKER_OPTION_VALUES + ${USER_LINKER_OPTION} + USER_LINKER_OPTION_INDEX) + +if(${USER_LINKER_OPTION_INDEX} EQUAL -1) + message( + STATUS + "Using custom linker: '${USER_LINKER_OPTION}', explicitly supported entries are ${USER_LINKER_OPTION_VALUES}") +endif() + +function(configure_linker project_name) + if(NOT ENABLE_USER_LINKER) + return() + endif() + + set(LINKER_FLAG "-fuse-ld=${USER_LINKER_OPTION}") + + check_cxx_compiler_flag(${LINKER_FLAG} CXX_SUPPORTS_USER_LINKER) + if(CXX_SUPPORTS_USER_LINKER) + target_compile_options(${project_name} INTERFACE ${LINKER_FLAG}) + endif() +endfunction() diff --git a/cmake/Sanitizers.cmake b/cmake/Sanitizers.cmake new file mode 100644 index 00000000..272f8ac5 --- /dev/null +++ b/cmake/Sanitizers.cmake @@ -0,0 +1,104 @@ +# Copyright 2021 PickNik Inc +# +# 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 PickNik Inc 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. + + +function(enable_sanitizers project_name) + + if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU" OR CMAKE_CXX_COMPILER_ID MATCHES ".*Clang") + option(ENABLE_COVERAGE "Enable coverage reporting for gcc/clang" OFF) + + if(ENABLE_COVERAGE) + target_compile_options(${project_name} INTERFACE --coverage -O0 -g -fno-omit-frame-pointer) + target_link_libraries(${project_name} INTERFACE --coverage) + endif() + + set(SANITIZERS "") + + option(ENABLE_SANITIZER_ADDRESS "Enable address sanitizer" OFF) + if(ENABLE_SANITIZER_ADDRESS) + list(APPEND SANITIZERS "address") + endif() + + option(ENABLE_SANITIZER_LEAK "Enable leak sanitizer" OFF) + if(ENABLE_SANITIZER_LEAK) + list(APPEND SANITIZERS "leak") + endif() + + option(ENABLE_SANITIZER_UNDEFINED_BEHAVIOR "Enable undefined behavior sanitizer" OFF) + if(ENABLE_SANITIZER_UNDEFINED_BEHAVIOR) + list(APPEND SANITIZERS "undefined") + endif() + + option(ENABLE_SANITIZER_THREAD "Enable thread sanitizer" OFF) + if(ENABLE_SANITIZER_THREAD) + if("address" IN_LIST SANITIZERS OR "leak" IN_LIST SANITIZERS) + message(WARNING "Thread sanitizer does not work with Address and Leak sanitizer enabled") + else() + list(APPEND SANITIZERS "thread") + if(CMAKE_CXX_COMPILER_ID MATCHES ".*Clang") + target_compile_options( + ${project_name} INTERFACE + -fsanitize-blacklist=${PROJECT_SOURCE_DIR}/.tsan-blacklist) + else() + message(WARNING "Thread sanitizer blacklisting only works with clang compiler") + endif() + endif() + endif() + + option(ENABLE_SANITIZER_MEMORY "Enable memory sanitizer" OFF) + if(ENABLE_SANITIZER_MEMORY AND CMAKE_CXX_COMPILER_ID MATCHES ".*Clang") + message(WARNING "Memory sanitizer requires all the code (including libc++) \ + to be MSan-instrumented otherwise it reports false positives") + if("address" IN_LIST SANITIZERS + OR "thread" IN_LIST SANITIZERS + OR "leak" IN_LIST SANITIZERS) + message(WARNING "Memory sanitizer does not work with Address, Thread and Leak sanitizer enabled") + else() + list(APPEND SANITIZERS "memory") + endif() + endif() + + list( + JOIN + SANITIZERS + "," + LIST_OF_SANITIZERS) + + endif() + + if(LIST_OF_SANITIZERS) + if(NOT + "${LIST_OF_SANITIZERS}" + STREQUAL + "") + target_compile_options(${project_name} INTERFACE -fsanitize=${LIST_OF_SANITIZERS}) + target_link_options(${project_name} INTERFACE -fsanitize=${LIST_OF_SANITIZERS}) + endif() + endif() + +endfunction() diff --git a/cmake/StandardProjectSettings.cmake b/cmake/StandardProjectSettings.cmake new file mode 100644 index 00000000..8b3fbf2e --- /dev/null +++ b/cmake/StandardProjectSettings.cmake @@ -0,0 +1,70 @@ +# Copyright 2021 PickNik Inc +# +# 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 PickNik Inc 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. + + +# Set a default build type if none was specified +if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) + message(STATUS "Setting build type to 'Debug' as none was specified.") + set(CMAKE_BUILD_TYPE + Debug + CACHE STRING "Choose the type of build." FORCE) + # Set the possible values of build type for cmake-gui, ccmake + set_property( + CACHE CMAKE_BUILD_TYPE + PROPERTY STRINGS + "Debug" + "Release" + "MinSizeRel" + "RelWithDebInfo") +endif() + +# Generate compile_commands.json to make it easier to work with clang based tools +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) + +option(ENABLE_IPO "Enable Interprocedural Optimization, aka Link Time Optimization (LTO)" OFF) + +if(ENABLE_IPO) + include(CheckIPOSupported) + check_ipo_supported( + RESULT + result + OUTPUT + output) + if(result) + set(CMAKE_INTERPROCEDURAL_OPTIMIZATION ON) + else() + message(SEND_ERROR "IPO is not supported: ${output}") + endif() +endif() +if(CMAKE_CXX_COMPILER_ID MATCHES ".*Clang") + add_compile_options(-fcolor-diagnostics) +elseif(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") + add_compile_options(-fdiagnostics-color=always) +else() + message(STATUS "No colored compiler diagnostic set for '${CMAKE_CXX_COMPILER_ID}' compiler.") +endif() diff --git a/cmake/StaticAnalyzers.cmake b/cmake/StaticAnalyzers.cmake new file mode 100644 index 00000000..b25c4445 --- /dev/null +++ b/cmake/StaticAnalyzers.cmake @@ -0,0 +1,52 @@ +# Copyright 2021 PickNik Inc +# +# 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 PickNik Inc 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. + +option(ENABLE_CLANG_TIDY "Enable static analysis with clang-tidy" OFF) +option(ENABLE_INCLUDE_WHAT_YOU_USE "Enable static analysis with include-what-you-use" OFF) + +if(ENABLE_CLANG_TIDY) + find_program(CLANGTIDY clang-tidy) + if(CLANGTIDY) + set(CMAKE_CXX_CLANG_TIDY ${CLANGTIDY} -extra-arg=-Wno-unknown-warning-option) + if(WARNINGS_AS_ERRORS) + list(APPEND CMAKE_CXX_CLANG_TIDY -warnings-as-errors=*) + endif() + else() + message(SEND_ERROR "clang-tidy requested but executable not found") + endif() +endif() + +if(ENABLE_INCLUDE_WHAT_YOU_USE) + find_program(INCLUDE_WHAT_YOU_USE include-what-you-use) + if(INCLUDE_WHAT_YOU_USE) + set(CMAKE_CXX_INCLUDE_WHAT_YOU_USE + ${INCLUDE_WHAT_YOU_USE} -Xiwyu --mapping-file=${PROJECT_SOURCE_DIR}/.iwyu_mapping.imp) + else() + message(SEND_ERROR "include-what-you-use requested but executable not found") + endif() +endif() diff --git a/include/bio_ik/frame.h b/include/bio_ik/frame.h index 8ba1771e..03d079b8 100644 --- a/include/bio_ik/frame.h +++ b/include/bio_ik/frame.h @@ -51,12 +51,13 @@ typedef tf2::Vector3 Vector3; struct alignas(32) Frame { Vector3 pos; - double __padding[4 - (sizeof(Vector3) / sizeof(double))]; + // TODO(wyattrees): is __padding needed? rename without double underscore? + std::array __padding; Quaternion rot; inline Frame() {} - inline Frame(const tf2::Vector3& pos, const tf2::Quaternion& rot) - : pos(pos) - , rot(rot) + inline Frame(const tf2::Vector3& position, const tf2::Quaternion& rotation) + : pos(position) + , rot(rotation) { } explicit inline Frame(const KDL::Frame& kdl) @@ -103,7 +104,7 @@ inline void frameToKDL(const Frame& frame, KDL::Frame& kdl_frame) template const Frame Frame::IdentityFrameTemplate::identity_frame(Vector3(0, 0, 0), Quaternion(0, 0, 0, 1)); -static std::ostream& operator<<(std::ostream& os, const Frame& f) { return os << "(" << f.pos.x() << "," << f.pos.y() << "," << f.pos.z() << ";" << f.rot.x() << "," << f.rot.y() << "," << f.rot.z() << "," << f.rot.w() << ")"; } +inline std::ostream& operator<<(std::ostream& os, const Frame& f) { return os << "(" << f.pos.x() << "," << f.pos.y() << "," << f.pos.z() << ";" << f.rot.x() << "," << f.rot.y() << "," << f.rot.z() << "," << f.rot.w() << ")"; } __attribute__((always_inline)) inline void quat_mul_vec(const tf2::Quaternion& q, const tf2::Vector3& v, tf2::Vector3& r) { diff --git a/include/bio_ik/goal.h b/include/bio_ik/goal.h index 48be3992..f5e3a7d3 100644 --- a/include/bio_ik/goal.h +++ b/include/bio_ik/goal.h @@ -67,13 +67,13 @@ class GoalContext public: GoalContext() {} inline const Frame& getLinkFrame(size_t i = 0) const { return tip_link_frames_[goal_link_indices_[i]]; } - inline const double getVariablePosition(size_t i = 0) const + inline double getVariablePosition(size_t i = 0) const { auto j = goal_variable_indices_[i]; if(j >= 0) - return active_variable_positions_[j]; + return active_variable_positions_[static_cast(j)]; else - return initial_guess_[-1 - j]; + return initial_guess_[static_cast(-1 - j)]; } inline const Frame& getProblemLinkFrame(size_t i) const { return tip_link_frames_[i]; } inline size_t getProblemLinkCount() const { return problem_tip_link_indices_.size(); } @@ -97,8 +97,8 @@ class GoalContext class Goal { protected: - bool secondary_; double weight_; + bool secondary_; public: Goal() @@ -115,7 +115,7 @@ class Goal context.setSecondary(secondary_); context.setWeight(weight_); } - virtual double evaluate(const GoalContext& context) const { return 0; } + virtual double evaluate(const GoalContext& /*context*/) const { return 0; } }; struct BioIKKinematicsQueryOptions : kinematics::KinematicsQueryOptions diff --git a/include/bio_ik/goal_types.h b/include/bio_ik/goal_types.h index 94af47a2..120e3276 100644 --- a/include/bio_ik/goal_types.h +++ b/include/bio_ik/goal_types.h @@ -131,8 +131,8 @@ class PoseGoal : public LinkGoalBase public: PoseGoal() - : rotation_scale_(0.5) - , frame_(Frame::identity()) + : frame_(Frame::identity()) + , rotation_scale_(0.5) { } PoseGoal(const std::string& link_name, const tf2::Vector3& position, const tf2::Quaternion& orientation, double weight = 1.0) @@ -214,95 +214,95 @@ class LookAtGoal : public LinkGoalBase class MaxDistanceGoal : public LinkGoalBase { - tf2::Vector3 target; - double distance; + tf2::Vector3 target_; + double distance_; public: MaxDistanceGoal() - : target(0, 0, 0) - , distance(1) + : target_(0, 0, 0) + , distance_(1) { } MaxDistanceGoal(const std::string& link_name, const tf2::Vector3& target, double distance, double weight = 1.0) : LinkGoalBase(link_name, weight) - , target(target) - , distance(distance) + , target_(target) + , distance_(distance) { } - const tf2::Vector3& getTarget() const { return target; } - void setTarget(const tf2::Vector3& t) { target = t; } - double getDistance() const { return distance; } - void setDistance(double d) { distance = d; } + const tf2::Vector3& getTarget() const { return target_; } + void setTarget(const tf2::Vector3& t) { target_ = t; } + double getDistance() const { return distance_; } + void setDistance(double d) { distance_ = d; } virtual double evaluate(const GoalContext& context) const { auto& fb = context.getLinkFrame(); - double d = fmax(0.0, fb.getPosition().distance(target) - distance); + double d = fmax(0.0, fb.getPosition().distance(target_) - distance_); return d * d; } }; class MinDistanceGoal : public LinkGoalBase { - tf2::Vector3 target; - double distance; + tf2::Vector3 target_; + double distance_; public: MinDistanceGoal() - : target(0, 0, 0) - , distance(1) + : target_(0, 0, 0) + , distance_(1) { } MinDistanceGoal(const std::string& link_name, const tf2::Vector3& target, double distance, double weight = 1.0) : LinkGoalBase(link_name, weight) - , target(target) - , distance(distance) + , target_(target) + , distance_(distance) { } - const tf2::Vector3& getTarget() const { return target; } - void setTarget(const tf2::Vector3& t) { target = t; } - double getDistance() const { return distance; } - void setDistance(double d) { distance = d; } + const tf2::Vector3& getTarget() const { return target_; } + void setTarget(const tf2::Vector3& t) { target_ = t; } + double getDistance() const { return distance_; } + void setDistance(double d) { distance_ = d; } virtual double evaluate(const GoalContext& context) const { auto& fb = context.getLinkFrame(); - double d = fmax(0.0, distance - fb.getPosition().distance(target)); + double d = fmax(0.0, distance_ - fb.getPosition().distance(target_)); return d * d; } }; class LineGoal : public LinkGoalBase { - tf2::Vector3 position; - tf2::Vector3 direction; + tf2::Vector3 position_; + tf2::Vector3 direction_; public: LineGoal() - : position(0, 0, 0) - , direction(0, 0, 0) + : position_(0, 0, 0) + , direction_(0, 0, 0) { } LineGoal(const std::string& link_name, const tf2::Vector3& position, const tf2::Vector3& direction, double weight = 1.0) : LinkGoalBase(link_name, weight) - , position(position) - , direction(direction.normalized()) + , position_(position) + , direction_(direction.normalized()) { } - const tf2::Vector3& getPosition() const { return position; } - void setPosition(const tf2::Vector3& p) { position = p; } - const tf2::Vector3& getDirection() const { return direction; } - void setDirection(const tf2::Vector3& d) { direction = d.normalized(); } + const tf2::Vector3& getPosition() const { return position_; } + void setPosition(const tf2::Vector3& p) { position_ = p; } + const tf2::Vector3& getDirection() const { return direction_; } + void setDirection(const tf2::Vector3& d) { direction_ = d.normalized(); } virtual double evaluate(const GoalContext& context) const { auto& fb = context.getLinkFrame(); - return position.distance2(fb.getPosition() - direction * direction.dot(fb.getPosition() - position)); + return position_.distance2(fb.getPosition() - direction_ * direction_.dot(fb.getPosition() - position_)); } }; #if (MOVEIT_FCL_VERSION < FCL_VERSION_CHECK(0, 6, 0)) class TouchGoal : public LinkGoalBase { - tf2::Vector3 position; - tf2::Vector3 normal; + tf2::Vector3 position_; + tf2::Vector3 normal_; struct CollisionShape { std::vector vertices; @@ -332,14 +332,14 @@ class TouchGoal : public LinkGoalBase public: TouchGoal() - : position(0, 0, 0) - , normal(0, 0, 0) + : position_(0, 0, 0) + , normal_(0, 0, 0) { } TouchGoal(const std::string& link_name, const tf2::Vector3& position, const tf2::Vector3& normal, double weight = 1.0) : LinkGoalBase(link_name, weight) - , position(position) - , normal(normal.normalized()) + , position_(position) + , normal_(normal.normalized()) { } virtual void describe(GoalContext& context) const; @@ -438,68 +438,68 @@ class MinimalDisplacementGoal : public Goal class JointVariableGoal : public Goal { - std::string variable_name; - double variable_position; + std::string variable_name_; + double variable_position_; public: JointVariableGoal() - : variable_position(0) + : variable_position_(0) { } JointVariableGoal(const std::string& variable_name, double variable_position, double weight = 1.0, bool secondary = false) - : variable_name(variable_name) - , variable_position(variable_position) + : variable_name_(variable_name) + , variable_position_(variable_position) { weight_ = weight; secondary_ = secondary; } - double getVariablePosition() const { return variable_position; } - void setVariablePosition(double p) { variable_position = p; } - const std::string& getVariableName() const { return variable_name; } - void setVariableName(const std::string& n) { variable_name = n; } + double getVariablePosition() const { return variable_position_; } + void setVariablePosition(double p) { variable_position_ = p; } + const std::string& getVariableName() const { return variable_name_; } + void setVariableName(const std::string& n) { variable_name_ = n; } virtual void describe(GoalContext& context) const { Goal::describe(context); - context.addVariable(variable_name); + context.addVariable(variable_name_); } virtual double evaluate(const GoalContext& context) const { - double d = variable_position - context.getVariablePosition(); + double d = variable_position_ - context.getVariablePosition(); return d * d; } }; class JointFunctionGoal : public Goal { - std::vector variable_names; - std::function&)> function; + std::vector variable_names_; + std::function&)> function_; public: JointFunctionGoal() {} JointFunctionGoal(const std::vector& variable_names, const std::function&)>& function, double weight = 1.0, bool secondary = false) - : variable_names(variable_names) - , function(function) + : variable_names_(variable_names) + , function_(function) { weight_ = weight; secondary_ = secondary; } - void setJointVariableNames(const std::vector& n) { variable_names = n; } - void setJointVariableFunction(const std::function&)>& f) { function = f; } + void setJointVariableNames(const std::vector& n) { variable_names_ = n; } + void setJointVariableFunction(const std::function&)>& f) { function_ = f; } virtual void describe(GoalContext& context) const { Goal::describe(context); - for(auto& variable_name : variable_names) + for(auto& variable_name : variable_names_) context.addVariable(variable_name); } virtual double evaluate(const GoalContext& context) const { auto& temp_vector = context.getTempVector(); - temp_vector.resize(variable_names.size()); - for(size_t i = 0; i < variable_names.size(); i++) + temp_vector.resize(variable_names_.size()); + for(size_t i = 0; i < variable_names_.size(); i++) temp_vector[i] = context.getVariablePosition(i); - function(temp_vector); + function_(temp_vector); double sum = 0.0; - for(size_t i = 0; i < variable_names.size(); i++) + for(size_t i = 0; i < variable_names_.size(); i++) { double d = temp_vector[i] - context.getVariablePosition(i); sum += d * d; @@ -540,132 +540,132 @@ class BalanceGoal : public Goal class LinkFunctionGoal : public LinkGoalBase { - std::function function; + std::function function_; public: LinkFunctionGoal() {} LinkFunctionGoal(const std::string& link_name, const std::function& function, double weight = 1.0) : LinkGoalBase(link_name, weight) - , function(function) + , function_(function) { } - void setLinkFunction(const std::function& f) { function = f; } - virtual double evaluate(const GoalContext& context) const { return function(context.getLinkFrame().getPosition(), context.getLinkFrame().getOrientation()); } + void setLinkFunction(const std::function& f) { function_ = f; } + virtual double evaluate(const GoalContext& context) const { return function_(context.getLinkFrame().getPosition(), context.getLinkFrame().getOrientation()); } }; class SideGoal : public LinkGoalBase { - tf2::Vector3 axis; - tf2::Vector3 direction; + tf2::Vector3 axis_; + tf2::Vector3 direction_; public: SideGoal() - : axis(0, 0, 1) - , direction(0, 0, 1) + : axis_(0, 0, 1) + , direction_(0, 0, 1) { } SideGoal(const std::string& link_name, const tf2::Vector3& axis, const tf2::Vector3& direction, double weight = 1.0) : LinkGoalBase(link_name, weight) - , axis(axis) - , direction(direction) + , axis_(axis) + , direction_(direction) { } - const tf2::Vector3& getAxis() const { return axis; } - const tf2::Vector3& getDirection() const { return direction; } - void setAxis(const tf2::Vector3& a) { axis = a.normalized(); } - void setDirection(const tf2::Vector3& d) { direction = d.normalized(); } + const tf2::Vector3& getAxis() const { return axis_; } + const tf2::Vector3& getDirection() const { return direction_; } + void setAxis(const tf2::Vector3& a) { axis_ = a.normalized(); } + void setDirection(const tf2::Vector3& d) { direction_ = d.normalized(); } virtual double evaluate(const GoalContext& context) const { auto& fb = context.getLinkFrame(); Vector3 v; - quat_mul_vec(fb.getOrientation(), axis, v); - double f = fmax(0.0, v.dot(direction)); + quat_mul_vec(fb.getOrientation(), axis_, v); + double f = fmax(0.0, v.dot(direction_)); return f * f; } }; class DirectionGoal : public LinkGoalBase { - tf2::Vector3 axis; - tf2::Vector3 direction; + tf2::Vector3 axis_; + tf2::Vector3 direction_; public: DirectionGoal() - : axis(0, 0, 1) - , direction(0, 0, 1) + : axis_(0, 0, 1) + , direction_(0, 0, 1) { } DirectionGoal(const std::string& link_name, const tf2::Vector3& axis, const tf2::Vector3& direction, double weight = 1.0) : LinkGoalBase(link_name, weight) - , axis(axis) - , direction(direction) + , axis_(axis) + , direction_(direction) { } - const tf2::Vector3& getAxis() const { return axis; } - const tf2::Vector3& getDirection() const { return direction; } - void setAxis(const tf2::Vector3& a) { axis = a.normalized(); } - void setDirection(const tf2::Vector3& d) { direction = d.normalized(); } + const tf2::Vector3& getAxis() const { return axis_; } + const tf2::Vector3& getDirection() const { return direction_; } + void setAxis(const tf2::Vector3& a) { axis_ = a.normalized(); } + void setDirection(const tf2::Vector3& d) { direction_ = d.normalized(); } virtual double evaluate(const GoalContext& context) const { auto& fb = context.getLinkFrame(); Vector3 v; - quat_mul_vec(fb.getOrientation(), axis, v); - return v.distance2(direction); + quat_mul_vec(fb.getOrientation(), axis_, v); + return v.distance2(direction_); } }; class ConeGoal : public LinkGoalBase { - tf2::Vector3 position; - double position_weight; - tf2::Vector3 axis; - tf2::Vector3 direction; - double angle; + tf2::Vector3 position_; + double position_weight_; + tf2::Vector3 axis_; + tf2::Vector3 direction_; + double angle_; public: - const tf2::Vector3& getPosition() const { return position; } - double getPositionWeight() const { return position_weight; } - const tf2::Vector3& getAxis() const { return axis; } - const tf2::Vector3& getDirection() const { return direction; } - double getAngle() const { return angle; } - void setPosition(const tf2::Vector3& p) { position = p; } - void setPositionWeight(double w) { position_weight = w; } - void setAxis(const tf2::Vector3& a) { axis = a.normalized(); } - void setDirection(const tf2::Vector3& d) { direction = d.normalized(); } - void setAngle(double a) { angle = a; } + const tf2::Vector3& getPosition() const { return position_; } + double getPositionWeight() const { return position_weight_; } + const tf2::Vector3& getAxis() const { return axis_; } + const tf2::Vector3& getDirection() const { return direction_; } + double getAngle() const { return angle_; } + void setPosition(const tf2::Vector3& p) { position_ = p; } + void setPositionWeight(double w) { position_weight_ = w; } + void setAxis(const tf2::Vector3& a) { axis_ = a.normalized(); } + void setDirection(const tf2::Vector3& d) { direction_ = d.normalized(); } + void setAngle(double a) { angle_ = a; } ConeGoal() - : position(0, 0, 0) - , position_weight(0) - , axis(0, 0, 1) - , direction(0, 0, 1) - , angle(0) + : position_(0, 0, 0) + , position_weight_(0) + , axis_(0, 0, 1) + , direction_(0, 0, 1) + , angle_(0) { } ConeGoal(const std::string& link_name, const tf2::Vector3& axis, const tf2::Vector3& direction, double angle, double weight = 1.0) : LinkGoalBase(link_name, weight) - , position(0, 0, 0) - , position_weight(0) - , axis(axis) - , direction(direction) - , angle(angle) + , position_(0, 0, 0) + , position_weight_(0) + , axis_(axis) + , direction_(direction) + , angle_(angle) { } ConeGoal(const std::string& link_name, const tf2::Vector3& position, const tf2::Vector3& axis, const tf2::Vector3& direction, double angle, double weight = 1.0) : LinkGoalBase(link_name, weight) - , position(position) - , position_weight(1) - , axis(axis) - , direction(direction) - , angle(angle) + , position_(position) + , position_weight_(1) + , axis_(axis) + , direction_(direction) + , angle_(angle) { } ConeGoal(const std::string& link_name, const tf2::Vector3& position, double position_weight, const tf2::Vector3& axis, const tf2::Vector3& direction, double angle, double weight = 1.0) : LinkGoalBase(link_name, weight) - , position(position) - , position_weight(position_weight) - , axis(axis) - , direction(direction) - , angle(angle) + , position_(position) + , position_weight_(position_weight) + , axis_(axis) + , direction_(direction) + , angle_(angle) { } virtual double evaluate(const GoalContext& context) const @@ -673,11 +673,11 @@ class ConeGoal : public LinkGoalBase double sum = 0.0; auto& fb = context.getLinkFrame(); Vector3 v; - quat_mul_vec(fb.getOrientation(), axis, v); - double d = fmax(0.0, v.angle(direction) - angle); + quat_mul_vec(fb.getOrientation(), axis_, v); + double d = fmax(0.0, v.angle(direction_) - angle_); sum += d * d; - double w = position_weight; - sum += w * w * (position - fb.getPosition()).length2(); + double w = position_weight_; + sum += w * w * (position_ - fb.getPosition()).length2(); return sum; } }; diff --git a/include/bio_ik/robot_info.h b/include/bio_ik/robot_info.h index a6139c14..12e23894 100644 --- a/include/bio_ik/robot_info.h +++ b/include/bio_ik/robot_info.h @@ -78,8 +78,8 @@ class RobotInfo bool bounded = bounds.position_bounded_; - auto* joint_model = model->getJointOfVariable(variables.size()); - if(auto* revolute = dynamic_cast(joint_model)) + auto* joint_model = model->getJointOfVariable(static_cast(variables.size())); + if(dynamic_cast(joint_model)) if(bounds.max_position_ - bounds.min_position_ >= 2 * M_PI * 0.9999) bounded = false; @@ -91,7 +91,7 @@ class RobotInfo info.span = info.max - info.min; - if(!(info.span >= 0 && info.span < FLT_MAX)) info.span = 1; + if(!(info.span >= 0 && info.span < DBL_MAX)) info.span = 1; info.max_velocity = bounds.max_velocity_; info.max_velocity_rcp = info.max_velocity > 0.0 ? 1.0 / info.max_velocity : 0.0; @@ -101,7 +101,7 @@ class RobotInfo for(size_t ivar = 0; ivar < model->getVariableCount(); ivar++) { - variable_joint_types.push_back(model->getJointOfVariable(ivar)->getType()); + variable_joint_types.push_back(model->getJointOfVariable(static_cast(ivar))->getType()); } } diff --git a/src/forward_kinematics.h b/src/forward_kinematics.h index a1401424..63b3e2c6 100644 --- a/src/forward_kinematics.h +++ b/src/forward_kinematics.h @@ -65,14 +65,14 @@ namespace bio_ik class RobotJointEvaluator { private: - std::vector joint_cache_variables; - std::vector joint_cache_frames; - std::vector link_frames; + std::vector joint_cache_variables_; + std::vector joint_cache_frames_; + std::vector link_frames_; protected: - std::vector joint_axis_list; - moveit::core::RobotModelConstPtr robot_model; - std::vector variables; + std::vector joint_axis_list_; + moveit::core::RobotModelConstPtr robot_model_; + std::vector variables_; public: inline void getJointFrame(const moveit::core::JointModel* joint_model, const double* variables, Frame& frame) @@ -88,7 +88,7 @@ class RobotJointEvaluator { case moveit::core::JointModel::REVOLUTE: { - auto axis = joint_axis_list[joint_index]; + auto axis = joint_axis_list_[joint_index]; auto v = variables[joint_model->getFirstVariableIndex()]; @@ -112,7 +112,7 @@ class RobotJointEvaluator } case moveit::core::JointModel::PRISMATIC: { - auto axis = joint_axis_list[joint_index]; + auto axis = joint_axis_list_[joint_index]; auto v = variables[joint_model->getFirstVariableIndex()]; frame = Frame(axis * v, Quaternion(0.0, 0.0, 0.0, 1.0)); break; @@ -140,43 +140,43 @@ class RobotJointEvaluator inline void getJointFrame(const moveit::core::JointModel* joint_model, const std::vector& variables, Frame& frame) { getJointFrame(joint_model, variables.data(), frame); } protected: - inline const Frame& getLinkFrame(const moveit::core::LinkModel* link_model) { return link_frames[link_model->getLinkIndex()]; } + inline const Frame& getLinkFrame(const moveit::core::LinkModel* link_model) { return link_frames_[link_model->getLinkIndex()]; } inline bool checkJointMoved(const moveit::core::JointModel* joint_model) { size_t i0 = joint_model->getFirstVariableIndex(); size_t cnt = joint_model->getVariableCount(); if(cnt == 0) return true; - if(cnt == 1) return !(variables[i0] == joint_cache_variables[i0]); + if(cnt == 1) return !(variables_[i0] == joint_cache_variables_[i0]); for(size_t i = i0; i < i0 + cnt; i++) - if(!(variables[i] == joint_cache_variables[i])) return true; + if(!(variables_[i] == joint_cache_variables_[i])) return true; return false; } inline void putJointCache(const moveit::core::JointModel* joint_model, const Frame& frame) { - joint_cache_frames[joint_model->getJointIndex()] = frame; + joint_cache_frames_[joint_model->getJointIndex()] = frame; size_t i0 = joint_model->getFirstVariableIndex(); size_t cnt = joint_model->getVariableCount(); for(size_t i = i0; i < i0 + cnt; i++) - joint_cache_variables[i] = variables[i]; + joint_cache_variables_[i] = variables_[i]; } inline const Frame& getJointFrame(const moveit::core::JointModel* joint_model) { size_t joint_index = joint_model->getJointIndex(); - if(!checkJointMoved(joint_model)) return joint_cache_frames[joint_index]; + if(!checkJointMoved(joint_model)) return joint_cache_frames_[joint_index]; - getJointFrame(joint_model, variables, joint_cache_frames[joint_index]); + getJointFrame(joint_model, variables_, joint_cache_frames_[joint_index]); size_t cnt = joint_model->getVariableCount(); if(cnt) { size_t i0 = joint_model->getFirstVariableIndex(); if(cnt == 1) - joint_cache_variables[i0] = variables[i0]; + joint_cache_variables_[i0] = variables_[i0]; else for(size_t i = i0; i < i0 + cnt; i++) - joint_cache_variables[i] = variables[i]; + joint_cache_variables_[i] = variables_[i]; } // TODO: optimize copy @@ -185,30 +185,30 @@ class RobotJointEvaluator size_t i0 = joint_model->getFirstVariableIndex(); memcpy(joint_cache_variables.data() + i0, variables.data() + i0, cnt * 8);*/ - return joint_cache_frames[joint_index]; + return joint_cache_frames_[joint_index]; } public: RobotJointEvaluator(moveit::core::RobotModelConstPtr model) - : robot_model(model) + : robot_model_(model) { - joint_cache_variables.clear(); - joint_cache_variables.resize(model->getVariableCount(), DBL_MAX); + joint_cache_variables_.clear(); + joint_cache_variables_.resize(model->getVariableCount(), DBL_MAX); - joint_cache_frames.clear(); - joint_cache_frames.resize(model->getJointModelCount()); + joint_cache_frames_.clear(); + joint_cache_frames_.resize(model->getJointModelCount()); - link_frames.clear(); + link_frames_.clear(); for(auto* link_model : model->getLinkModels()) - link_frames.push_back(Frame(link_model->getJointOriginTransform())); + link_frames_.push_back(Frame(link_model->getJointOriginTransform())); - joint_axis_list.clear(); - joint_axis_list.resize(robot_model->getJointModelCount()); - for(size_t i = 0; i < joint_axis_list.size(); i++) + joint_axis_list_.clear(); + joint_axis_list_.resize(robot_model_->getJointModelCount()); + for(size_t i = 0; i < joint_axis_list_.size(); i++) { - auto* joint_model = robot_model->getJointModel(i); - if(auto* j = dynamic_cast(joint_model)) joint_axis_list[i] = Vector3(j->getAxis().x(), j->getAxis().y(), j->getAxis().z()); - if(auto* j = dynamic_cast(joint_model)) joint_axis_list[i] = Vector3(j->getAxis().x(), j->getAxis().y(), j->getAxis().z()); + auto* joint_model = robot_model_->getJointModel(i); + if(auto* j = dynamic_cast(joint_model)) joint_axis_list_[i] = Vector3(j->getAxis().x(), j->getAxis().y(), j->getAxis().z()); + if(auto* j = dynamic_cast(joint_model)) joint_axis_list_[i] = Vector3(j->getAxis().x(), j->getAxis().y(), j->getAxis().z()); } } }; @@ -229,7 +229,7 @@ class RobotFK_Fast_Base : protected RobotJointEvaluator std::vector> variable_to_link_chain_map; inline void updateMimic(std::vector& values) { - for(auto* joint : robot_model->getMimicJointModels()) + for(auto* joint : robot_model_->getMimicJointModels()) { auto src = joint->getMimic()->getFirstVariableIndex(); auto dest = joint->getFirstVariableIndex(); @@ -255,15 +255,15 @@ class RobotFK_Fast_Base : protected RobotJointEvaluator tip_names.resize(tip_link_indices.size()); for(size_t i = 0; i < tip_link_indices.size(); i++) - tip_names[i] = robot_model->getLinkModelNames()[tip_link_indices[i]]; + tip_names[i] = robot_model_->getLinkModelNames()[tip_link_indices[i]]; tip_frames.resize(tip_names.size()); tip_links.clear(); for(const auto& n : tip_names) - tip_links.push_back(robot_model->getLinkModel(n)); + tip_links.push_back(robot_model_->getLinkModel(n)); - global_frames.resize(robot_model->getLinkModelCount()); + global_frames.resize(robot_model_->getLinkModelCount()); link_chains.clear(); link_schedule.clear(); @@ -298,7 +298,7 @@ class RobotFK_Fast_Base : protected RobotJointEvaluator } } - variable_to_link_map_2.resize(robot_model->getVariableCount()); + variable_to_link_map_2.resize(robot_model_->getVariableCount()); for(auto* link_model : link_schedule) { auto link_index = link_model->getLinkIndex(); @@ -311,7 +311,7 @@ class RobotFK_Fast_Base : protected RobotJointEvaluator } } - variable_to_link_chain_map.resize(robot_model->getVariableCount()); + variable_to_link_chain_map.resize(robot_model_->getVariableCount()); for(size_t chain_index = 0; chain_index < link_chains.size(); chain_index++) { auto& link_chain = link_chains[chain_index]; @@ -331,8 +331,8 @@ class RobotFK_Fast_Base : protected RobotJointEvaluator void applyConfiguration(const std::vector& jj0) { FNPROFILER(); - variables = jj0; - updateMimic(variables); + variables_ = jj0; + updateMimic(variables_); for(auto* link_model : link_schedule) { auto* joint_model = link_model->getParentJointModel(); @@ -381,7 +381,7 @@ class RobotFK_Fast : public RobotFK_Fast_Base for(auto& vi : active_variables) { // vars[vi] = vars0[vi]; - variables0[vi] = variables[vi]; + variables0[vi] = variables_[vi]; } // find moved links @@ -391,7 +391,7 @@ class RobotFK_Fast : public RobotFK_Fast_Base { auto variable_index = x.first; auto link_index = x.second; - if(vars[variable_index] != variables[variable_index]) links_changed[link_index] = true; + if(vars[variable_index] != variables_[variable_index]) links_changed[link_index] = true; } // update @@ -449,10 +449,10 @@ class RobotFK_Fast : public RobotFK_Fast_Base size_t vcount = joint_model->getVariableCount(); if(vcount == 1) - variables[vstart] = vars[vstart]; + variables_[vstart] = vars[vstart]; else for(size_t vi = vstart; vi < vstart + vcount; vi++) - variables[vi] = vars[vi]; + variables_[vi] = vars[vi]; if(ipos > 0) { @@ -464,10 +464,10 @@ class RobotFK_Fast : public RobotFK_Fast_Base } if(vcount == 1) - variables[vstart] = variables0[vstart]; + variables_[vstart] = variables0[vstart]; else for(size_t vi = vstart; vi < vstart + vcount; vi++) - variables[vi] = variables0[vi]; + variables_[vi] = variables0[vi]; // PROFILER_COUNTER("incremental update transforms"); } @@ -496,9 +496,9 @@ class RobotFK_Fast : public RobotFK_Fast_Base } // set new vars - // variables = vars; + // variables_ = vars; for(auto& vi : active_variables) - variables[vi] = vars[vi]; + variables_[vi] = vars[vi]; // test if(0) @@ -530,7 +530,7 @@ class RobotFK_Fast : public RobotFK_Fast_Base applyConfiguration(jj); chain_cache.clear(); chain_cache.resize(tip_frames.size()); - links_changed.resize(robot_model->getLinkModelCount()); + links_changed.resize(robot_model_->getLinkModelCount()); vars.resize(jj.size()); variables0.resize(jj.size()); use_incremental = true; @@ -538,7 +538,7 @@ class RobotFK_Fast : public RobotFK_Fast_Base inline void incrementalEnd() { use_incremental = false; - // applyConfiguration(variables); + // applyConfiguration(variables_); } inline void applyConfiguration(const std::vector& jj) { @@ -567,7 +567,7 @@ class RobotFK_Jacobian : public RobotFK_Fast { Base::initialize(tip_link_indices); auto tip_count = tip_names.size(); - joint_dependencies.resize(robot_model->getJointModelCount()); + joint_dependencies.resize(robot_model_->getJointModelCount()); for(auto& x : joint_dependencies) x.clear(); for(auto* link_model : link_schedule) @@ -585,7 +585,7 @@ class RobotFK_Jacobian : public RobotFK_Fast joint_dependencies[mimic->getJointIndex()].push_back(joint_model); } } - tip_dependencies.resize(robot_model->getJointModelCount() * tip_count); + tip_dependencies.resize(robot_model_->getJointModelCount() * tip_count); for(auto& x : tip_dependencies) x = 0; for(size_t tip_index = 0; tip_index < tip_count; tip_index++) @@ -597,136 +597,151 @@ class RobotFK_Jacobian : public RobotFK_Fast } } } - void computeJacobian(const std::vector& variable_indices, Eigen::MatrixXd& jacobian) - { - double step_size = 0.00001; - double half_step_size = step_size * 0.5; - double inv_step_size = 1.0 / step_size; - auto tip_count = tip_frames.size(); - jacobian.resize(tip_count * 6, variable_indices.size()); - for(size_t icol = 0; icol < variable_indices.size(); icol++) - { - for(size_t itip = 0; itip < tip_count; itip++) - { - jacobian(itip * 6 + 0, icol) = 0; - jacobian(itip * 6 + 1, icol) = 0; - jacobian(itip * 6 + 2, icol) = 0; - jacobian(itip * 6 + 3, icol) = 0; - jacobian(itip * 6 + 4, icol) = 0; - jacobian(itip * 6 + 5, icol) = 0; - } + void computeJacobian(const std::vector& variable_indices, + Eigen::MatrixXd& jacobian) { + double step_size = 0.00001; + double inv_step_size = 1.0 / step_size; + auto tip_count = tip_frames.size(); + jacobian.resize(static_cast(tip_count * 6), + static_cast(variable_indices.size())); + for (Eigen::Index icol = 0; + icol < static_cast(variable_indices.size()); ++icol) { + for (Eigen::Index itip = 0; itip < static_cast(tip_count); + ++itip) { + jacobian(itip * 6 + 0, icol) = 0; + jacobian(itip * 6 + 1, icol) = 0; + jacobian(itip * 6 + 2, icol) = 0; + jacobian(itip * 6 + 3, icol) = 0; + jacobian(itip * 6 + 4, icol) = 0; + jacobian(itip * 6 + 5, icol) = 0; } - for(size_t icol = 0; icol < variable_indices.size(); icol++) - { - auto ivar = variable_indices[icol]; - auto* var_joint_model = robot_model->getJointOfVariable(ivar); - if(var_joint_model->getMimic()) continue; - for(auto* joint_model : joint_dependencies[var_joint_model->getJointIndex()]) - { - double scale = 1; - for(auto* m = joint_model; m->getMimic() && m->getMimic() != joint_model; m = m->getMimic()) - { - scale *= m->getMimicFactor(); - } - auto* link_model = joint_model->getChildLinkModel(); - switch(joint_model->getType()) - { - case moveit::core::JointModel::FIXED: - { - // fixed joint: zero gradient (do nothing) + } + for (size_t icol = 0; icol < variable_indices.size(); icol++) { + auto ivar = variable_indices[icol]; + auto* var_joint_model = + robot_model_->getJointOfVariable(static_cast(ivar)); + if (var_joint_model->getMimic()) continue; + for (auto* joint_model : + joint_dependencies[var_joint_model->getJointIndex()]) { + double scale = 1; + for (auto* m = joint_model; + m->getMimic() && m->getMimic() != joint_model; m = m->getMimic()) { + scale *= m->getMimicFactor(); + } + auto* link_model = joint_model->getChildLinkModel(); + switch (joint_model->getType()) { + case moveit::core::JointModel::FIXED: { + // fixed joint: zero gradient (do nothing) + continue; + } + case moveit::core::JointModel::REVOLUTE: { + auto& link_frame = global_frames[link_model->getLinkIndex()]; + for (size_t itip = 0; itip < tip_count; itip++) { + if (!tip_dependencies[joint_model->getJointIndex() * tip_count + + itip]) continue; - } - case moveit::core::JointModel::REVOLUTE: - { - auto& link_frame = global_frames[link_model->getLinkIndex()]; - for(size_t itip = 0; itip < tip_count; itip++) - { - if(!tip_dependencies[joint_model->getJointIndex() * tip_count + itip]) continue; - auto& tip_frame = tip_frames[itip]; + auto& tip_frame = tip_frames[itip]; - auto q = link_frame.rot.inverse() * tip_frame.rot; - q = q.inverse(); + auto q = link_frame.rot.inverse() * tip_frame.rot; + q = q.inverse(); - auto rot = joint_axis_list[joint_model->getJointIndex()]; - quat_mul_vec(q, rot, rot); + auto rot = joint_axis_list_[joint_model->getJointIndex()]; + quat_mul_vec(q, rot, rot); - auto vel = link_frame.pos - tip_frame.pos; - quat_mul_vec(tip_frame.rot.inverse(), vel, vel); + auto vel = link_frame.pos - tip_frame.pos; + quat_mul_vec(tip_frame.rot.inverse(), vel, vel); - vel = vel.cross(rot); + vel = vel.cross(rot); - jacobian(itip * 6 + 0, icol) += vel.x() * scale; - jacobian(itip * 6 + 1, icol) += vel.y() * scale; - jacobian(itip * 6 + 2, icol) += vel.z() * scale; + auto eigen_itip = static_cast(itip); + auto eigen_icol = static_cast(icol); + jacobian(eigen_itip * 6 + 0, eigen_icol) += vel.x() * scale; + jacobian(eigen_itip * 6 + 1, eigen_icol) += vel.y() * scale; + jacobian(eigen_itip * 6 + 2, eigen_icol) += vel.z() * scale; - jacobian(itip * 6 + 3, icol) += rot.x() * scale; - jacobian(itip * 6 + 4, icol) += rot.y() * scale; - jacobian(itip * 6 + 5, icol) += rot.z() * scale; + jacobian(eigen_itip * 6 + 3, eigen_icol) += rot.x() * scale; + jacobian(eigen_itip * 6 + 4, eigen_icol) += rot.y() * scale; + jacobian(eigen_itip * 6 + 5, eigen_icol) += rot.z() * scale; - // LOG("a", vel.x(), vel.y(), vel.z(), rot.x(), rot.y(), rot.z()); - } - continue; + // LOG("a", vel.x(), vel.y(), vel.z(), rot.x(), rot.y(), rot.z()); } - case moveit::core::JointModel::PRISMATIC: - { - auto& link_frame = global_frames[link_model->getLinkIndex()]; - for(size_t itip = 0; itip < tip_count; itip++) - { - if(!tip_dependencies[joint_model->getJointIndex() * tip_count + itip]) continue; + continue; + } + case moveit::core::JointModel::PRISMATIC: { + auto& link_frame = global_frames[link_model->getLinkIndex()]; + for (size_t itip = 0; itip < tip_count; itip++) { + if (!tip_dependencies[joint_model->getJointIndex() * tip_count + + itip]) + continue; - auto& tip_frame = tip_frames[itip]; + auto& tip_frame = tip_frames[itip]; - auto q = link_frame.rot.inverse() * tip_frame.rot; - q = q.inverse(); + auto q = link_frame.rot.inverse() * tip_frame.rot; + q = q.inverse(); - auto axis = joint_axis_list[joint_model->getJointIndex()]; - auto v = axis; - quat_mul_vec(q, axis, v); + auto axis = joint_axis_list_[joint_model->getJointIndex()]; + auto v = axis; + quat_mul_vec(q, axis, v); - jacobian(itip * 6 + 0, icol) += v.x() * scale; - jacobian(itip * 6 + 1, icol) += v.y() * scale; - jacobian(itip * 6 + 2, icol) += v.z() * scale; + auto eigen_itip = static_cast(itip); + auto eigen_icol = static_cast(icol); + jacobian(eigen_itip * 6 + 0, eigen_icol) += v.x() * scale; + jacobian(eigen_itip * 6 + 1, eigen_icol) += v.y() * scale; + jacobian(eigen_itip * 6 + 2, eigen_icol) += v.z() * scale; - // LOG("a", v.x(), v.y(), v.z(), 0, 0, 0); - } - continue; + // LOG("a", v.x(), v.y(), v.z(), 0, 0, 0); } - default: - { - // numeric differentiation for joint types that are not yet implemented / or joint types that might be added to moveit in the future - auto ivar2 = ivar; - if(joint_model->getMimic()) ivar2 = ivar2 - var_joint_model->getFirstVariableIndex() + joint_model->getFirstVariableIndex(); - auto& link_frame_1 = global_frames[link_model->getLinkIndex()]; - auto v0 = variables[ivar2]; - // auto joint_frame_1 = getJointFrame(joint_model); - variables[ivar2] = v0 + step_size; - auto joint_frame_2 = getJointFrame(joint_model); - variables[ivar2] = v0; - Frame link_frame_2; - if(auto* parent_link_model = joint_model->getParentLinkModel()) - concat(global_frames[parent_link_model->getLinkIndex()], getLinkFrame(link_model), joint_frame_2, link_frame_2); - else - concat(getLinkFrame(link_model), joint_frame_2, link_frame_2); - for(size_t itip = 0; itip < tip_count; itip++) - { - if(!tip_dependencies[joint_model->getJointIndex() * tip_count + itip]) continue; - auto tip_frame_1 = tip_frames[itip]; - Frame tip_frame_2; - change(link_frame_2, link_frame_1, tip_frame_1, tip_frame_2); - auto twist = frameTwist(tip_frame_1, tip_frame_2); - jacobian(itip * 6 + 0, icol) += twist.vel.x() * inv_step_size * scale; - jacobian(itip * 6 + 1, icol) += twist.vel.y() * inv_step_size * scale; - jacobian(itip * 6 + 2, icol) += twist.vel.z() * inv_step_size * scale; - jacobian(itip * 6 + 3, icol) += twist.rot.x() * inv_step_size * scale; - jacobian(itip * 6 + 4, icol) += twist.rot.y() * inv_step_size * scale; - jacobian(itip * 6 + 5, icol) += twist.rot.z() * inv_step_size * scale; - } + continue; + } + default: { + // numeric differentiation for joint types that are not yet + // implemented / or joint types that might be added to moveit in the + // future + auto ivar2 = ivar; + if (joint_model->getMimic()) + ivar2 = ivar2 - var_joint_model->getFirstVariableIndex() + + joint_model->getFirstVariableIndex(); + auto& link_frame_1 = global_frames[link_model->getLinkIndex()]; + auto v0 = variables_[ivar2]; + // auto joint_frame_1 = getJointFrame(joint_model); + variables_[ivar2] = v0 + step_size; + auto joint_frame_2 = getJointFrame(joint_model); + variables_[ivar2] = v0; + Frame link_frame_2; + if (auto* parent_link_model = joint_model->getParentLinkModel()) + concat(global_frames[parent_link_model->getLinkIndex()], + getLinkFrame(link_model), joint_frame_2, link_frame_2); + else + concat(getLinkFrame(link_model), joint_frame_2, link_frame_2); + for (size_t itip = 0; itip < tip_count; itip++) { + if (!tip_dependencies[joint_model->getJointIndex() * tip_count + + itip]) continue; + auto tip_frame_1 = tip_frames[itip]; + Frame tip_frame_2; + change(link_frame_2, link_frame_1, tip_frame_1, tip_frame_2); + auto twist = frameTwist(tip_frame_1, tip_frame_2); + auto eigen_itip = static_cast(itip); + auto eigen_icol = static_cast(icol); + jacobian(eigen_itip * 6 + 0, eigen_icol) += + twist.vel.x() * inv_step_size * scale; + jacobian(eigen_itip * 6 + 1, eigen_icol) += + twist.vel.y() * inv_step_size * scale; + jacobian(eigen_itip * 6 + 2, eigen_icol) += + twist.vel.z() * inv_step_size * scale; + jacobian(eigen_itip * 6 + 3, eigen_icol) += + twist.rot.x() * inv_step_size * scale; + jacobian(eigen_itip * 6 + 4, eigen_icol) += + twist.rot.y() * inv_step_size * scale; + jacobian(eigen_itip * 6 + 5, eigen_icol) += + twist.rot.z() * inv_step_size * scale; } - } + continue; + } } } + } } }; @@ -744,7 +759,7 @@ struct RobotFK : public RobotFK_Jacobian void initializeMutationApproximator(const std::vector& variable_indices) { mutation_variable_indices = variable_indices; - mutation_initial_variable_positions = variables; + mutation_initial_variable_positions = variables_; } void computeApproximateMutation1(size_t variable_index, double variable_delta, const aligned_vector& input, aligned_vector& output) { @@ -812,11 +827,11 @@ class RobotFK_Mutator : public RobotFK_Jacobian tip_frames_aligned[i] = tip_frames[i]; // init first order approximators - { + { if(mutation_approx_frames.size() < tip_count) mutation_approx_frames.resize(tip_count); for(size_t itip = 0; itip < tip_count; itip++) - mutation_approx_frames[itip].resize(robot_model->getVariableCount()); + mutation_approx_frames[itip].resize(robot_model_->getVariableCount()); for(size_t itip = 0; itip < tip_count; itip++) for(auto ivar : variable_indices) @@ -824,46 +839,52 @@ class RobotFK_Mutator : public RobotFK_Jacobian computeJacobian(variable_indices, mutation_approx_jacobian); - for(size_t icol = 0; icol < variable_indices.size(); icol++) - { + for (size_t icol = 0; icol < variable_indices.size(); icol++) { size_t ivar = variable_indices[icol]; - for(size_t itip = 0; itip < tip_count; itip++) + for (size_t itip = 0; itip < tip_count; itip++) { { - { - Vector3 t; - t.setX(mutation_approx_jacobian(itip * 6 + 0, icol)); - t.setY(mutation_approx_jacobian(itip * 6 + 1, icol)); - t.setZ(mutation_approx_jacobian(itip * 6 + 2, icol)); - quat_mul_vec(tip_frames[itip].rot, t, t); - mutation_approx_frames[itip][ivar].pos = t; - } + Vector3 t; + auto eigen_itip = static_cast(itip); + auto eigen_icol = static_cast(icol); + t.setX(mutation_approx_jacobian(eigen_itip * 6 + 0, eigen_icol)); + t.setY(mutation_approx_jacobian(eigen_itip * 6 + 1, eigen_icol)); + t.setZ(mutation_approx_jacobian(eigen_itip * 6 + 2, eigen_icol)); + quat_mul_vec(tip_frames[itip].rot, t, t); + mutation_approx_frames[itip][ivar].pos = t; + } - { - Quaternion q; - q.setX(mutation_approx_jacobian(itip * 6 + 3, icol) * 0.5); - q.setY(mutation_approx_jacobian(itip * 6 + 4, icol) * 0.5); - q.setZ(mutation_approx_jacobian(itip * 6 + 5, icol) * 0.5); - q.setW(1.0); - quat_mul_quat(tip_frames[itip].rot, q, q); - q -= tip_frames[itip].rot; - mutation_approx_frames[itip][ivar].rot = q; - } + { + Quaternion q; + auto eigen_itip = static_cast(itip); + auto eigen_icol = static_cast(icol); + q.setX(mutation_approx_jacobian(eigen_itip * 6 + 3, eigen_icol) * + 0.5); + q.setY(mutation_approx_jacobian(eigen_itip * 6 + 4, eigen_icol) * + 0.5); + q.setZ(mutation_approx_jacobian(eigen_itip * 6 + 5, eigen_icol) * + 0.5); + q.setW(1.0); + quat_mul_quat(tip_frames[itip].rot, q, q); + q -= tip_frames[itip].rot; + mutation_approx_frames[itip][ivar].rot = q; + } } } } + // init second order approximators #if USE_QUADRATIC_EXTRAPOLATION { if(mutation_approx_quadratics.size() < tip_count) mutation_approx_quadratics.resize(tip_count); for(size_t itip = 0; itip < tip_count; itip++) - mutation_approx_quadratics[itip].resize(robot_model->getVariableCount()); + mutation_approx_quadratics[itip].resize(robot_model_->getVariableCount()); for(size_t itip = 0; itip < tip_count; itip++) for(auto ivar : variable_indices) mutation_approx_quadratics[itip][ivar].pos = Vector3(0, 0, 0); for(auto ivar : variable_indices) { - auto* var_joint_model = robot_model->getJointOfVariable(ivar); + auto* var_joint_model = robot_model_->getJointOfVariable(ivar); if(var_joint_model->getMimic()) continue; for(auto* joint_model : joint_dependencies[var_joint_model->getJointIndex()]) { @@ -884,7 +905,7 @@ class RobotFK_Mutator : public RobotFK_Jacobian auto& tip_frame = tip_frames[itip]; - auto axis = joint_axis_list[joint_model->getJointIndex()]; + auto axis = joint_axis_list_[joint_model->getJointIndex()]; quat_mul_vec(link_frame.rot, axis, axis); auto v = link_frame.pos - tip_frame.pos; @@ -909,7 +930,7 @@ class RobotFK_Mutator : public RobotFK_Jacobian if(mutation_approx_map.size() < tip_count) mutation_approx_map.resize(tip_count); for(size_t itip = 0; itip < tip_count; itip++) { - if(mutation_approx_mask[itip].size() < robot_model->getVariableCount()) mutation_approx_mask[itip].resize(robot_model->getVariableCount()); + if(mutation_approx_mask[itip].size() < robot_model_->getVariableCount()) mutation_approx_mask[itip].resize(robot_model_->getVariableCount()); mutation_approx_map[itip].clear(); for(size_t ii = 0; ii < variable_indices.size(); ii++) // for(size_t ivar : variable_indices) @@ -940,24 +961,25 @@ class RobotFK_Mutator : public RobotFK_Jacobian if(mutation_approx_mask[itip][variable_index] == 0) continue; auto& joint_delta = mutation_approx_frames[itip][variable_index]; const Frame& tip_frame = input[itip]; - const double* tip_frame_ptr = (const double*)&tip_frame; + + const double* tip_frame_ptr = reinterpret_cast(&tip_frame); __m256d p = _mm256_load_pd(tip_frame_ptr + 0); __m256d r = _mm256_load_pd(tip_frame_ptr + 4); { - auto joint_delta_ptr = (const double* __restrict__) & (joint_delta); + auto joint_delta_ptr = reinterpret_cast (&joint_delta); __m256d ff = _mm256_set1_pd(variable_delta); p = _mm256_fmadd_pd(ff, _mm256_load_pd(joint_delta_ptr + 0), p); r = _mm256_fmadd_pd(ff, _mm256_load_pd(joint_delta_ptr + 4), r); } #if USE_QUADRATIC_EXTRAPOLATION { - auto joint_delta_ptr = (const double* __restrict__) & (mutation_approx_quadratics[itip][variable_index]); + auto joint_delta_ptr = reinterpret_cast(&(mutation_approx_quadratics[itip][variable_index])) __m256d ff = _mm256_set1_pd(variable_delta * variable_delta); p = _mm256_fmadd_pd(ff, _mm256_load_pd(joint_delta_ptr + 0), p); } #endif auto& tip_mutation = output[itip]; - double* __restrict__ tip_mutation_ptr = (double*)&tip_mutation; + double* __restrict__ tip_mutation_ptr = reinterpret_cast(&tip_mutation); _mm256_store_pd(tip_mutation_ptr + 0, p); _mm256_store_pd(tip_mutation_ptr + 4, r); } @@ -973,13 +995,13 @@ class RobotFK_Mutator : public RobotFK_Jacobian if(mutation_approx_mask[itip][variable_index] == 0) continue; auto& joint_delta = mutation_approx_frames[itip][variable_index]; const Frame& tip_frame = input[itip]; - const double* tip_frame_ptr = (const double*)&tip_frame; + const double* tip_frame_ptr = reinterpret_cast(&tip_frame); __m128d pxy = _mm_load_pd(tip_frame_ptr + 0); __m128d pzw = _mm_load_sd(tip_frame_ptr + 2); __m128d rxy = _mm_load_pd(tip_frame_ptr + 4); __m128d rzw = _mm_load_pd(tip_frame_ptr + 6); { - auto joint_delta_ptr = (const double* __restrict__) & (joint_delta); + auto joint_delta_ptr = reinterpret_cast (&joint_delta); __m128d ff = _mm_set1_pd(variable_delta); pxy = _mm_add_pd(pxy, _mm_mul_pd(ff, _mm_load_pd(joint_delta_ptr + 0))); pzw = _mm_add_sd(pzw, _mm_mul_sd(ff, _mm_load_sd(joint_delta_ptr + 2))); @@ -988,14 +1010,14 @@ class RobotFK_Mutator : public RobotFK_Jacobian } #if USE_QUADRATIC_EXTRAPOLATION { - auto joint_delta_ptr = (const double* __restrict__) & (mutation_approx_quadratics[itip][variable_index]); + auto joint_delta_ptr = reinterpret_cast(&(mutation_approx_quadratics[itip][variable_index])) __m128d ff = _mm_set1_pd(variable_delta * variable_delta); pxy = _mm_add_pd(pxy, _mm_mul_pd(ff, _mm_load_pd(joint_delta_ptr + 0))); pzw = _mm_add_sd(pzw, _mm_mul_sd(ff, _mm_load_sd(joint_delta_ptr + 2))); } #endif auto& tip_mutation = output[itip]; - double* __restrict__ tip_mutation_ptr = (double*)&tip_mutation; + double* __restrict__ tip_mutation_ptr = reinterpret_cast(&tip_mutation); _mm_store_pd(tip_mutation_ptr + 0, pxy); _mm_store_sd(tip_mutation_ptr + 2, pzw); _mm_store_pd(tip_mutation_ptr + 4, rxy); @@ -1060,7 +1082,7 @@ class RobotFK_Mutator : public RobotFK_Jacobian #if FUNCTION_MULTIVERSIONING __attribute__((hot)) __attribute__((noinline)) __attribute__((target("fma", "avx"))) void computeApproximateMutations(size_t mutation_count, const double* const* mutation_values, std::vector>& tip_frame_mutations) const { - const double* p_variables = variables.data(); + const double* p_variables = variables_.data(); auto tip_count = tip_names.size(); tip_frame_mutations.resize(mutation_count); for(auto& m : tip_frame_mutations) @@ -1071,7 +1093,7 @@ class RobotFK_Mutator : public RobotFK_Jacobian const Frame& tip_frame = tip_frames_aligned[itip]; - const double* tip_frame_ptr = (const double*)&tip_frame; + const double* tip_frame_ptr = reinterpret_cast(&tip_frame); __m256d p0 = _mm256_load_pd(tip_frame_ptr + 0); __m256d r0 = _mm256_load_pd(tip_frame_ptr + 4); @@ -1086,7 +1108,7 @@ class RobotFK_Mutator : public RobotFK_Jacobian double variable_delta = mutation_values[imutation][vii] - p_variables[variable_index]; { - auto joint_delta_ptr = (const double* __restrict__) & (joint_deltas[variable_index]); + auto joint_delta_ptr = reinterpret_cast(&joint_deltas[variable_index]); __m256d ff = _mm256_set1_pd(variable_delta); p = _mm256_fmadd_pd(ff, _mm256_load_pd(joint_delta_ptr + 0), p); r = _mm256_fmadd_pd(ff, _mm256_load_pd(joint_delta_ptr + 4), r); @@ -1094,7 +1116,7 @@ class RobotFK_Mutator : public RobotFK_Jacobian #if USE_QUADRATIC_EXTRAPOLATION { - auto joint_delta_ptr = (const double* __restrict__) & (mutation_approx_quadratics[itip][variable_index]); + auto joint_delta_ptr = reinterpret_cast(&(mutation_approx_quadratics[itip][variable_index])) __m256d ff = _mm256_set1_pd(variable_delta * variable_delta); p = _mm256_fmadd_pd(ff, _mm256_load_pd(joint_delta_ptr + 0), p); } @@ -1102,7 +1124,7 @@ class RobotFK_Mutator : public RobotFK_Jacobian } auto& tip_mutation = tip_frame_mutations[imutation][itip]; - double* __restrict__ tip_mutation_ptr = (double*)&tip_mutation; + double* __restrict__ tip_mutation_ptr = reinterpret_cast(&tip_mutation); _mm256_store_pd(tip_mutation_ptr + 0, p); _mm256_store_pd(tip_mutation_ptr + 4, r); } @@ -1111,7 +1133,7 @@ class RobotFK_Mutator : public RobotFK_Jacobian __attribute__((hot)) __attribute__((noinline)) __attribute__((target("sse2"))) void computeApproximateMutations(size_t mutation_count, const double* const* mutation_values, std::vector>& tip_frame_mutations) const { - const double* p_variables = variables.data(); + const double* p_variables = variables_.data(); auto tip_count = tip_names.size(); tip_frame_mutations.resize(mutation_count); for(auto& m : tip_frame_mutations) @@ -1122,7 +1144,7 @@ class RobotFK_Mutator : public RobotFK_Jacobian const Frame& tip_frame = tip_frames_aligned[itip]; - const double* tip_frame_ptr = (const double*)&tip_frame; + const double* tip_frame_ptr = reinterpret_cast(&tip_frame); __m128d pxy0 = _mm_load_pd(tip_frame_ptr + 0); __m128d pzw0 = _mm_load_sd(tip_frame_ptr + 2); __m128d rxy0 = _mm_load_pd(tip_frame_ptr + 4); @@ -1141,7 +1163,7 @@ class RobotFK_Mutator : public RobotFK_Jacobian double variable_delta = mutation_values[imutation][vii] - p_variables[variable_index]; { - auto joint_delta_ptr = (const double* __restrict__) & (joint_deltas[variable_index]); + auto joint_delta_ptr = reinterpret_cast(&(joint_deltas[variable_index])); __m128d ff = _mm_set1_pd(variable_delta); pxy = _mm_add_pd(_mm_mul_pd(ff, _mm_load_pd(joint_delta_ptr + 0)), pxy); pzw = _mm_add_sd(_mm_mul_sd(ff, _mm_load_sd(joint_delta_ptr + 2)), pzw); @@ -1151,7 +1173,7 @@ class RobotFK_Mutator : public RobotFK_Jacobian #if USE_QUADRATIC_EXTRAPOLATION { - auto joint_delta_ptr = (const double* __restrict__) & (mutation_approx_quadratics[itip][variable_index]); + auto joint_delta_ptr = reinterpret_cast(&(mutation_approx_quadratics[itip][variable_index])) __m128d ff = _mm_set1_pd(variable_delta * variable_delta); pxy = _mm_add_pd(_mm_mul_pd(ff, _mm_load_pd(joint_delta_ptr + 0)), pxy); pzw = _mm_add_sd(_mm_mul_sd(ff, _mm_load_sd(joint_delta_ptr + 2)), pzw); @@ -1160,7 +1182,7 @@ class RobotFK_Mutator : public RobotFK_Jacobian } auto& tip_mutation = tip_frame_mutations[imutation][itip]; - double* __restrict__ tip_mutation_ptr = (double*)&tip_mutation; + double* __restrict__ tip_mutation_ptr = reinterpret_cast(&tip_mutation); _mm_store_pd(tip_mutation_ptr + 0, pxy); _mm_store_sd(tip_mutation_ptr + 2, pzw); _mm_store_pd(tip_mutation_ptr + 4, rxy); @@ -1174,7 +1196,7 @@ class RobotFK_Mutator : public RobotFK_Jacobian void computeApproximateMutations(size_t mutation_count, const double* const* mutation_values, std::vector>& tip_frame_mutations) const { - const double* p_variables = variables.data(); + const double* p_variables = variables_.data(); auto tip_count = tip_names.size(); tip_frame_mutations.resize(mutation_count); for(auto& m : tip_frame_mutations) @@ -1273,7 +1295,7 @@ class RobotFK_Mutator_2 : public RobotFK_Mutator auto tip_count = tip_names.size(); - link_approximators.resize(robot_model->getLinkModelCount() + 1); + link_approximators.resize(robot_model_->getLinkModelCount() + 1); for(auto& l : link_approximators) { l.position = Vector3(0, 0, 0); @@ -1281,7 +1303,7 @@ class RobotFK_Mutator_2 : public RobotFK_Mutator } link_approx_mask.clear(); - link_approx_mask.resize(robot_model->getLinkModelCount(), 0); + link_approx_mask.resize(robot_model_->getLinkModelCount(), 0); joint_approximators.clear(); @@ -1289,7 +1311,7 @@ class RobotFK_Mutator_2 : public RobotFK_Mutator for(size_t imut = 0; imut < variable_indices.size(); imut++) { size_t ivar = variable_indices[imut]; - auto* joint_model = robot_model->getJointOfVariable(ivar); + auto* joint_model = robot_model_->getJointOfVariable(ivar); { auto* link_model = joint_model->getChildLinkModel(); // if(link_approx_mask[link_model->getLinkIndex()]) continue; @@ -1298,7 +1320,7 @@ class RobotFK_Mutator_2 : public RobotFK_Mutator joint.mutation_index = imut; joint.variable_index = ivar; joint.link_index = link_model->getLinkIndex(); - link_approx_mask[link_model->getLinkIndex()] = 1; + link_approx_mask[joint.link_index] = 1; } for(auto* mimic_joint_model : joint_model->getMimicRequests()) { @@ -1311,7 +1333,7 @@ class RobotFK_Mutator_2 : public RobotFK_Mutator joint.mutation_index = imut; joint.variable_index = ivar; joint.link_index = link_model->getLinkIndex(); - link_approx_mask[link_model->getLinkIndex()] = 1; + link_approx_mask[joint.link_index] = 1; } } @@ -1319,7 +1341,7 @@ class RobotFK_Mutator_2 : public RobotFK_Mutator { // size_t imut = joint.mutation_index; // size_t ivar = joint.variable_index; - auto* link_model = robot_model->getLinkModel(joint.link_index); + auto* link_model = robot_model_->getLinkModel(joint.link_index); auto* joint_model = link_model->getParentJointModel(); joint.delta_position = Vector3(0, 0, 0); joint.delta_rotation = Vector3(0, 0, 0); @@ -1327,10 +1349,10 @@ class RobotFK_Mutator_2 : public RobotFK_Mutator switch(joint_model->getType()) { case moveit::core::JointModel::REVOLUTE: - quat_mul_vec(link_frame.rot, joint_axis_list[joint_model->getJointIndex()], joint.delta_rotation); + quat_mul_vec(link_frame.rot, joint_axis_list_[joint_model->getJointIndex()], joint.delta_rotation); break; case moveit::core::JointModel::PRISMATIC: - quat_mul_vec(link_frame.rot, joint_axis_list[joint_model->getJointIndex()], joint.delta_position); + quat_mul_vec(link_frame.rot, joint_axis_list_[joint_model->getJointIndex()], joint.delta_position); break; } for(auto* j = joint_model; j->getMimic(); j = j->getMimic()) @@ -1352,14 +1374,14 @@ class RobotFK_Mutator_2 : public RobotFK_Mutator joint.link_index = link_model->getLinkIndex(); joint.delta_position = Vector3(0, 0, 0); joint.delta_rotation = Vector3(0, 0, 0); - link_approx_mask[link_model->getLinkIndex()] = 1; + link_approx_mask[joint.link_index] = 1; } std::sort(joint_approximators.begin(), joint_approximators.end(), [](const JointApprox& a, const JointApprox& b) { return a.link_index < b.link_index; }); for(auto& joint : joint_approximators) { - for(auto* link_model = robot_model->getLinkModel(joint.link_index); link_model;) + for(auto* link_model = robot_model_->getLinkModel(joint.link_index); link_model;) { if(link_approx_mask[link_model->getLinkIndex()] >= 2) { @@ -1371,7 +1393,7 @@ class RobotFK_Mutator_2 : public RobotFK_Mutator link_model = link_model->getParentLinkModel(); if(link_model == 0) { - joint.parent_link_index = robot_model->getLinkModelCount(); + joint.parent_link_index = robot_model_->getLinkModelCount(); joint.link_position = global_frames[joint.link_index].pos; link_approx_mask[joint.link_index] = 2; break; @@ -1379,7 +1401,7 @@ class RobotFK_Mutator_2 : public RobotFK_Mutator } } - variable_to_approximator_index_map.resize(robot_model->getVariableCount()); + variable_to_approximator_index_map.resize(robot_model_->getVariableCount()); for(auto& l : variable_to_approximator_index_map) l.clear(); for(size_t approximator_index = 0; approximator_index < joint_approximators.size(); approximator_index++) @@ -1403,7 +1425,7 @@ class RobotFK_Mutator_2 : public RobotFK_Mutator /*if(joint.mutation_index < 0) dvar = 0; else*/ - dvar = mutation_values[imutation][joint.mutation_index] - variables[joint.variable_index]; + dvar = mutation_values[imutation][joint.mutation_index] - variables_[joint.variable_index]; Vector3 dpos = joint.delta_position * dvar + joint.link_position; @@ -1467,38 +1489,38 @@ typedef RobotFK_Mutator RobotFK; // for comparison class RobotFK_MoveIt { - moveit::core::RobotModelConstPtr robot_model; - moveit::core::RobotState robot_state; - std::vector tipFrames; - std::vector tipLinks; - std::vector jj; + moveit::core::RobotModelConstPtr robot_model_; + moveit::core::RobotState robot_state_; + std::vector tipFrames_; + std::vector tipLinks_; + std::vector jj_; public: RobotFK_MoveIt(moveit::core::RobotModelConstPtr model) - : robot_model(model) - , robot_state(model) + : robot_model_(model) + , robot_state_(model) { } void initialize(const std::vector& tip_link_indices) { - tipFrames.resize(tip_link_indices.size()); - tipLinks.resize(tip_link_indices.size()); + tipFrames_.resize(tip_link_indices.size()); + tipLinks_.resize(tip_link_indices.size()); for(size_t i = 0; i < tip_link_indices.size(); i++) - tipLinks[i] = robot_model->getLinkModel(tip_link_indices[i]); + tipLinks_[i] = robot_model_->getLinkModel(tip_link_indices[i]); } void applyConfiguration(const std::vector& jj0) { BLOCKPROFILER("RobotFK_MoveIt applyConfiguration"); - jj = jj0; - robot_model->interpolate(jj0.data(), jj0.data(), 0.5, jj.data()); // force mimic update - robot_state.setVariablePositions(jj); - robot_state.update(); - for(size_t i = 0; i < tipFrames.size(); i++) - tipFrames[i] = Frame(robot_state.getGlobalLinkTransform(tipLinks[i])); + jj_ = jj0; + robot_model_->interpolate(jj0.data(), jj0.data(), 0.5, jj_.data()); // force mimic update + robot_state_.setVariablePositions(jj_); + robot_state_.update(); + for(size_t i = 0; i < tipFrames_.size(); i++) + tipFrames_[i] = Frame(robot_state_.getGlobalLinkTransform(tipLinks_[i])); } - inline void incrementalBegin(const std::vector& jj) {} + inline void incrementalBegin(const std::vector& /*jj*/) {} inline void incrementalEnd() {} - const Frame& getTipFrame(size_t fi) const { return tipFrames[fi]; } - const std::vector& getTipFrames() const { return tipFrames; } + const Frame& getTipFrame(size_t fi) const { return tipFrames_[fi]; } + const std::vector& getTipFrames() const { return tipFrames_; } }; } diff --git a/src/goal_types.cpp b/src/goal_types.cpp index ef25b6f3..2d20d480 100644 --- a/src/goal_types.cpp +++ b/src/goal_types.cpp @@ -54,8 +54,7 @@ void TouchGoal::describe(GoalContext& context) const collision_model->collision_links.resize(robot_model->getLinkModelCount()); } link_model = robot_model->getLinkModel(this->getLinkName()); - size_t link_index = link_model->getLinkIndex(); - auto touch_goal_normal = normal.normalized(); + size_t link_index = static_cast(link_model->getLinkIndex()); // auto fbrot = fb.rot.normalized(); auto& collision_link = collision_model->collision_links[link_index]; if(!collision_link.initialized) @@ -93,9 +92,12 @@ void TouchGoal::describe(GoalContext& context) const for(size_t triangle_index = 0; triangle_index < triangles.size() / 3; triangle_index++) { polygons.push_back(3); - polygons.push_back(triangles[triangle_index * 3 + 0]); - polygons.push_back(triangles[triangle_index * 3 + 1]); - polygons.push_back(triangles[triangle_index * 3 + 2]); + polygons.push_back( + static_cast(triangles[triangle_index * 3 + 0])); + polygons.push_back( + static_cast(triangles[triangle_index * 3 + 1])); + polygons.push_back( + static_cast(triangles[triangle_index * 3 + 2])); } // planes are given in the same order as the triangles, though redundant ones will appear only once. for(const auto& plane : getPlanes()) @@ -115,25 +117,33 @@ void TouchGoal::describe(GoalContext& context) const // auto* fcl = new fcl::Convex(s.plane_normals.data(), s.plane_dis.data(), s.plane_normals.size(), s.points.data(), s.points.size(), s.polygons.data()); // workaround for fcl::Convex initialization bug - auto* fcl = (fcl::Convex*)::operator new(sizeof(fcl::Convex)); - fcl->num_points = s.points.size(); - fcl = new(fcl) fcl::Convex(s.plane_normals.data(), s.plane_dis.data(), s.plane_normals.size(), s.points.data(), s.points.size(), s.polygons.data()); + auto fcl = [&s]() -> fcl::Convex* { + auto buffer = operator new(sizeof(fcl::Convex)); + static_cast(buffer)->num_points = + static_cast(s.points.size()); + return new (buffer) fcl::Convex( + s.plane_normals.data(), s.plane_dis.data(), + static_cast(s.plane_normals.size()), s.points.data(), + static_cast(s.points.size()), s.polygons.data()); + }(); - s.geometry = decltype(s.geometry)(new collision_detection::FCLGeometry(fcl, link_model, shape_index)); + s.geometry = decltype(s.geometry)(new collision_detection::FCLGeometry(fcl, link_model, static_cast(shape_index))); s.edges.resize(s.points.size()); std::vector> edge_sets(s.points.size()); - for(size_t edge_index = 0; edge_index < fcl->num_edges; edge_index++) + for(int edge_index = 0; edge_index < fcl->num_edges; edge_index++) { auto edge = fcl->edges[edge_index]; - if(edge_sets[edge.first].find(edge.second) == edge_sets[edge.first].end()) - { - edge_sets[edge.first].insert(edge.second); - s.edges[edge.first].push_back(edge.second); + auto first_index = static_cast(edge.first); + auto second_index = static_cast(edge.second); + if (edge_sets[first_index].find(second_index) == + edge_sets[first_index].end()) { + edge_sets[first_index].insert(second_index); + s.edges[first_index].push_back(second_index); } - if(edge_sets[edge.second].find(edge.first) == edge_sets[edge.second].end()) - { - edge_sets[edge.second].insert(edge.first); - s.edges[edge.second].push_back(edge.first); + if (edge_sets[second_index].find(first_index) == + edge_sets[second_index].end()) { + edge_sets[second_index].insert(first_index); + s.edges[second_index].push_back(first_index); } } for(auto& p : s.points) @@ -141,11 +151,11 @@ void TouchGoal::describe(GoalContext& context) const } else { - s.geometry = collision_detection::createCollisionGeometry(link_model->getShapes()[shape_index], link_model, shape_index); + s.geometry = collision_detection::createCollisionGeometry( + link_model->getShapes()[shape_index], link_model, + static_cast(shape_index)); } - // LOG("b"); } - // getchar(); } } @@ -153,9 +163,9 @@ double TouchGoal::evaluate(const GoalContext& context) const { double dmin = DBL_MAX; context.getTempVector().resize(1); - auto& last_collision_vertex = context.getTempVector()[0]; + size_t last_collision_vertex = 0; auto& fb = context.getLinkFrame(); - size_t link_index = link_model->getLinkIndex(); + size_t link_index = static_cast(link_model->getLinkIndex()); auto& collision_link = collision_model->collision_links[link_index]; for(size_t shape_index = 0; shape_index < link_model->getShapes().size(); shape_index++) { @@ -166,7 +176,7 @@ double TouchGoal::evaluate(const GoalContext& context) const { auto& s = collision_link.shapes[shape_index]; double d = DBL_MAX; - auto goal_normal = normal; + auto goal_normal = normal_; quat_mul_vec(fb.rot.inverse(), goal_normal, goal_normal); quat_mul_vec(s->frame.rot.inverse(), goal_normal, goal_normal); /*{ @@ -206,7 +216,7 @@ double TouchGoal::evaluate(const GoalContext& context) const d = vertex_dot_normal; last_collision_vertex = vertex_index; } - d -= normal.dot(position - fb.pos); + d -= normal_.dot(position_ - fb.pos); // ROS_INFO("touch goal"); if(d < dmin) dmin = d; } @@ -216,7 +226,7 @@ double TouchGoal::evaluate(const GoalContext& context) const static fcl::Sphere shape1(offset); fcl::DistanceRequest request; fcl::DistanceResult result; - auto pos1 = position - normal * offset * 2; + auto pos1 = position_ - normal_ * offset * 2; auto* shape2 = collision_link.shapes[shape_index]->geometry->collision_geometry_.get(); auto frame2 = Frame(fb.pos, fb.rot.normalized()) * collision_link.shapes[shape_index]->frame; double d = fcl::distance(&shape1, fcl::Transform3f(fcl::Vec3f(pos1.x(), pos1.y(), pos1.z())), shape2, fcl::Transform3f(fcl::Quaternion3f(frame2.rot.w(), frame2.rot.x(), frame2.rot.y(), frame2.rot.z()), fcl::Vec3f(frame2.pos.x(), frame2.pos.y(), frame2.pos.z())), request, result); diff --git a/src/ik_base.h b/src/ik_base.h index 74f5de04..23c2a1e0 100644 --- a/src/ik_base.h +++ b/src/ik_base.h @@ -127,25 +127,25 @@ struct Random struct IKBase : Random { - IKParams params; - RobotFK model; - RobotInfo modelInfo; - int thread_index; - Problem problem; - std::vector null_tip_frames; - volatile int canceled; + RobotFK model_; + RobotInfo modelInfo_; + IKParams params_; + size_t thread_index_; + Problem problem_; + std::vector null_tip_frames_; + volatile int canceled_; virtual void step() = 0; virtual const std::vector& getSolution() const = 0; - virtual void setParams(const IKParams& p) {} + virtual void setParams(const IKParams& /*p*/) {} IKBase(const IKParams& p) : Random(p.random_seed) - , model(p.robot_model) - , modelInfo(p.robot_model) - , params(p) + , model_(p.robot_model) + , modelInfo_(p.robot_model) + , params_(p) { setParams(p); } @@ -153,38 +153,38 @@ struct IKBase : Random virtual void initialize(const Problem& problem) { - this->problem = problem; - this->problem.initialize2(); - model.initialize(problem.tip_link_indices); + problem_ = problem; + problem_.initialize2(); + model_.initialize(problem.tip_link_indices); // active_variables = problem.active_variables; - null_tip_frames.resize(problem.tip_link_indices.size()); + null_tip_frames_.resize(problem.tip_link_indices.size()); } - double computeSecondaryFitnessActiveVariables(const double* active_variable_positions) { return problem.computeGoalFitness(problem.secondary_goals, null_tip_frames.data(), active_variable_positions); } + double computeSecondaryFitnessActiveVariables(const double* active_variable_positions) { return problem_.computeGoalFitness(problem_.secondary_goals, null_tip_frames_.data(), active_variable_positions); } double computeSecondaryFitnessAllVariables(const std::vector& variable_positions) { return computeSecondaryFitnessActiveVariables(extractActiveVariables(variable_positions)); } - double computeFitnessActiveVariables(const std::vector& tip_frames, const double* active_variable_positions) { return problem.computeGoalFitness(problem.goals, tip_frames.data(), active_variable_positions); } + double computeFitnessActiveVariables(const std::vector& tip_frames, const double* active_variable_positions) { return problem_.computeGoalFitness(problem_.goals, tip_frames.data(), active_variable_positions); } - double computeFitnessActiveVariables(const aligned_vector& tip_frames, const double* active_variable_positions) { return problem.computeGoalFitness(problem.goals, tip_frames.data(), active_variable_positions); } + double computeFitnessActiveVariables(const aligned_vector& tip_frames, const double* active_variable_positions) { return problem_.computeGoalFitness(problem_.goals, tip_frames.data(), active_variable_positions); } double computeCombinedFitnessActiveVariables(const std::vector& tip_frames, const double* active_variable_positions) { double ret = 0.0; - ret += problem.computeGoalFitness(problem.goals, tip_frames.data(), active_variable_positions); - ret += problem.computeGoalFitness(problem.secondary_goals, null_tip_frames.data(), active_variable_positions); + ret += problem_.computeGoalFitness(problem_.goals, tip_frames.data(), active_variable_positions); + ret += problem_.computeGoalFitness(problem_.secondary_goals, null_tip_frames_.data(), active_variable_positions); return ret; } double computeCombinedFitnessActiveVariables(const aligned_vector& tip_frames, const double* active_variable_positions) { double ret = 0.0; - ret += problem.computeGoalFitness(problem.goals, tip_frames.data(), active_variable_positions); - ret += problem.computeGoalFitness(problem.secondary_goals, null_tip_frames.data(), active_variable_positions); + ret += problem_.computeGoalFitness(problem_.goals, tip_frames.data(), active_variable_positions); + ret += problem_.computeGoalFitness(problem_.secondary_goals, null_tip_frames_.data(), active_variable_positions); return ret; } - bool checkSolutionActiveVariables(const std::vector& tip_frames, const double* active_variable_positions) { return problem.checkSolutionActiveVariables(tip_frames, active_variable_positions); } + bool checkSolutionActiveVariables(const std::vector& tip_frames, const double* active_variable_positions) { return problem_.checkSolutionActiveVariables(tip_frames, active_variable_positions); } bool checkSolution(const std::vector& variable_positions, const std::vector& tips) { return checkSolutionActiveVariables(tips, extractActiveVariables(variable_positions)); } @@ -192,9 +192,9 @@ struct IKBase : Random double* extractActiveVariables(const std::vector& variable_positions) { - temp_active_variable_positions.resize(problem.active_variables.size()); + temp_active_variable_positions.resize(problem_.active_variables.size()); for(size_t i = 0; i < temp_active_variable_positions.size(); i++) - temp_active_variable_positions[i] = variable_positions[problem.active_variables[i]]; + temp_active_variable_positions[i] = variable_positions[problem_.active_variables[i]]; return temp_active_variable_positions.data(); } @@ -202,8 +202,8 @@ struct IKBase : Random double computeFitness(const std::vector& variable_positions) { - model.applyConfiguration(variable_positions); - return computeFitness(variable_positions, model.getTipFrames()); + model_.applyConfiguration(variable_positions); + return computeFitness(variable_positions, model_.getTipFrames()); } virtual size_t concurrency() const { return 1; } diff --git a/src/ik_evolution_1.cpp b/src/ik_evolution_1.cpp index ecbfa7a1..8c5b92b2 100644 --- a/src/ik_evolution_1.cpp +++ b/src/ik_evolution_1.cpp @@ -67,8 +67,8 @@ struct IKEvolution1 : IKBase for(auto* link_model = robot_model->getLinkModel(tip_name); link_model; link_model = link_model->getParentLinkModel()) { auto* joint_model = link_model->getParentJointModel(); - size_t v1 = joint_model->getFirstVariableIndex(); - size_t vn = joint_model->getVariableCount(); + const auto v1 = joint_model->getFirstVariableIndex(); + const auto vn = joint_model->getVariableCount(); for(size_t variable_index = v1; variable_index < v1 + vn; variable_index++) table[variable_index * tip_count + tip_index] = 1; } @@ -105,9 +105,9 @@ struct IKEvolution1 : IKBase { auto* joint_model = link_model->getParentJointModel(); - int vmin = joint_model->getFirstVariableIndex(); - int vmax = vmin + joint_model->getVariableCount(); - for(int vi = vmin; vi < vmax; vi++) + const auto vmin = joint_model->getFirstVariableIndex(); + const auto vmax = vmin + joint_model->getVariableCount(); + for(size_t vi = vmin; vi < vmax; vi++) chain_lengths_2[tip_index][vi] = chain_length; chain_length += Frame(link_model->getJointOriginTransform()).pos.length(); } @@ -121,7 +121,7 @@ struct IKEvolution1 : IKBase HeuristicErrorTree heuristicErrorTree; std::vector solution; std::vector population; - int populationSize, eliteCount; + size_t populationSize, eliteCount; std::vector tempPool; std::vector tempOffspring; std::vector initialGuess; @@ -148,12 +148,12 @@ struct IKEvolution1 : IKBase return v[index]; } - inline double clip(double v, size_t i) { return modelInfo.clip(v, i); } + inline double clip(double v, size_t i) { return modelInfo_.clip(v, i); } inline double getMutationStrength(size_t i, const Individual& parentA, const Individual& parentB) { double extinction = 0.5 * (parentA.extinction + parentB.extinction); - double span = modelInfo.getSpan(i); + double span = modelInfo_.getSpan(i); return span * extinction; } @@ -168,20 +168,20 @@ struct IKEvolution1 : IKBase return angular_scale;*/ } - double getHeuristicError(size_t variable_index, bool balanced) + double getHeuristicError(size_t variable_index, bool /*balanced*/) { // return 1; double heuristic_error = 0; // for(int tip_index = 0; tip_index < tipObjectives.size(); tip_index++) - for(int tip_index = 0; tip_index < problem.goals.size(); tip_index++) + for(size_t tip_index = 0; tip_index < problem_.goals.size(); tip_index++) { double influence = heuristicErrorTree.getInfluence(variable_index, tip_index); if(influence == 0) continue; // const auto& ta = tipObjectives[tip_index]; - const auto& ta = problem.goals[tip_index].frame; - const auto& tb = model.getTipFrame(tip_index); + const auto& ta = problem_.goals[tip_index].frame; + const auto& tb = model_.getTipFrame(tip_index); double length = heuristicErrorTree.getJointVariableChainLength(tip_index, variable_index); @@ -191,7 +191,7 @@ struct IKEvolution1 : IKBase // LOG_ALWAYS("b", length); - if(modelInfo.isPrismatic(variable_index)) + if(modelInfo_.isPrismatic(variable_index)) { // heuristic_error += ta.pos.distance(tb.pos) * influence; // if(length) heuristic_error += ta.rot.angle(tb.rot) * length * influence; @@ -207,7 +207,7 @@ struct IKEvolution1 : IKBase } } - if(modelInfo.isRevolute(variable_index)) + if(modelInfo_.isRevolute(variable_index)) { // if(length) heuristic_error += ta.pos.distance(tb.pos) / length * influence; // heuristic_error += ta.rot.angle(tb.rot) * influence; @@ -239,11 +239,11 @@ struct IKEvolution1 : IKBase { FNPROFILER(); // for(size_t i = 0; i < offspring.genes.size(); i++) - for(auto i : problem.active_variables) + for(auto i : problem_.active_variables) { - offspring.genes[i] = random(modelInfo.getMin(i), modelInfo.getMax(i)); + offspring.genes[i] = random(modelInfo_.getMin(i), modelInfo_.getMax(i)); - offspring.genes[i] = mix(offspring.genes[i], (modelInfo.getMin(i) + modelInfo.getMax(i)) * 0.5, random(0.0, 0.1)); + offspring.genes[i] = mix(offspring.genes[i], (modelInfo_.getMin(i) + modelInfo_.getMax(i)) * 0.5, random(0.0, 0.1)); offspring.gradients[i] = 0; } @@ -254,14 +254,14 @@ struct IKEvolution1 : IKBase { if(linear_fitness) { - model.applyConfiguration(genes); + model_.applyConfiguration(genes); double fitness_sum = 0.0; - for(size_t goal_index = 0; goal_index < problem.goals.size(); goal_index++) + for(size_t goal_index = 0; goal_index < problem_.goals.size(); goal_index++) { - const auto& ta = problem.goals[goal_index].frame; - const auto& tb = model.getTipFrame(problem.goals[goal_index].tip_index); + const auto& ta = problem_.goals[goal_index].frame; + const auto& tb = model_.getTipFrame(problem_.goals[goal_index].tip_index); - double tdist = ta.pos.distance(tb.pos) / computeAngularScale(problem.goals[goal_index].tip_index, ta); + double tdist = ta.pos.distance(tb.pos) / computeAngularScale(problem_.goals[goal_index].tip_index, ta); double rdist = ta.rot.angle(tb.rot); fitness_sum += mix(tdist, rdist, (balanced || in_final_adjustment_loop) ? 0.5 : random()); @@ -279,15 +279,15 @@ struct IKEvolution1 : IKBase FNPROFILER(); auto& genes = population[0].genes; // for(size_t i = 0; i < genes.size(); i++) - for(auto i : problem.active_variables) + for(auto i : problem_.active_variables) { double v0 = genes[i]; double fitness = computeFitness(genes, true); double heuristicError = getHeuristicError(i, true); // double heuristicError = 0.001; - genes[i] = modelInfo.clip(v0 + random(0, heuristicError), i); + genes[i] = modelInfo_.clip(v0 + random(0, heuristicError), i); double incFitness = computeFitness(genes, true); - genes[i] = modelInfo.clip(v0 - random(0, heuristicError), i); + genes[i] = modelInfo_.clip(v0 - random(0, heuristicError), i); double decFitness = computeFitness(genes, true); genes[i] = v0; if(incFitness < fitness || decFitness < fitness) @@ -306,7 +306,7 @@ struct IKEvolution1 : IKBase double max = population.back().fitness; for(size_t i = 0; i < populationSize; i++) { - double grading = (double)i / (double)(populationSize - 1); + double grading = static_cast(i) / static_cast(populationSize - 1); population[i].extinction = (population[i].fitness + min * (grading - 1)) / max; } } @@ -332,7 +332,7 @@ struct IKEvolution1 : IKBase double getMutationProbability(const Individual& parentA, const Individual& parentB) { double extinction = 0.5 * (parentA.extinction + parentB.extinction); - double inverse = 1.0 / parentA.genes.size(); + double inverse = 1.0 / static_cast(parentA.genes.size()); return extinction * (1.0 - inverse) + inverse; } @@ -342,7 +342,7 @@ struct IKEvolution1 : IKBase sort(population.begin(), population.end(), [](const Individual& a, const Individual& b) { return a.fitness < b.fitness; }); } - double bounce(double v, int i) + double bounce(double v, size_t i) { double c = clip(v, i); v = c - (v - c) * 2; @@ -392,7 +392,7 @@ struct IKEvolution1 : IKBase // model.incrementalBegin(individual.genes); - for(auto i : problem.active_variables) + for(auto i : problem_.active_variables) { double fitness = computeFitness(individual.genes, true); @@ -428,7 +428,7 @@ struct IKEvolution1 : IKBase // model.incrementalEnd(); - individual.fitness = fitness_sum / individual.genes.size(); + individual.fitness = fitness_sum / static_cast(individual.genes.size()); } IKEvolution1(const IKParams& p) @@ -444,7 +444,7 @@ struct IKEvolution1 : IKBase void init() { - initialGuess = problem.initial_guess; + initialGuess = problem_.initial_guess; solution = initialGuess; population.resize(populationSize); @@ -457,7 +457,7 @@ struct IKEvolution1 : IKBase p.fitness = computeFitness(p.genes, false); } - for(int i = 1; i < populationSize; i++) + for(size_t i = 1; i < populationSize; i++) { auto& p = population[i]; p.genes = solution; @@ -476,8 +476,8 @@ struct IKEvolution1 : IKBase std::vector tips; for(auto tip_link_index : problem.tip_link_indices) - tips.push_back(params.robot_model->getLinkModelNames()[tip_link_index]); - heuristicErrorTree = HeuristicErrorTree(params.robot_model, tips); + tips.push_back(params_.robot_model->getLinkModelNames()[tip_link_index]); + heuristicErrorTree = HeuristicErrorTree(params_.robot_model, tips); init(); } @@ -494,8 +494,8 @@ struct IKEvolution1 : IKBase const std::vector& getSolutionTipFrames() { - model.applyConfiguration(solution); - return model.getTipFrames(); + model_.applyConfiguration(solution); + return model_.getTipFrames(); } bool evolve() diff --git a/src/ik_evolution_2.cpp b/src/ik_evolution_2.cpp index 77ef8b9f..9617915e 100644 --- a/src/ik_evolution_2.cpp +++ b/src/ik_evolution_2.cpp @@ -62,14 +62,14 @@ template struct IKEvolution2 : IKBase std::vector solution; std::vector temp_joint_variables; double solution_fitness; - std::vector species; + std::vector species_; std::vector children; std::vector> phenotypes, phenotypes2, phenotypes3; std::vector child_indices; std::vector genotypes; std::vector phenotype; std::vector quaternion_genes; - aligned_vector genes_min, genes_max, genes_span; + aligned_vector genes_min_, genes_max_, genes_span_; aligned_vector gradient, temp; IKEvolution2(const IKParams& p) @@ -101,9 +101,9 @@ template struct IKEvolution2 : IKBase void genesToJointVariables(const Individual& individual, std::vector& variables) { auto& genes = individual.genes; - variables.resize(params.robot_model->getVariableCount()); - for(size_t i = 0; i < problem.active_variables.size(); i++) - variables[problem.active_variables[i]] = genes[i]; + variables.resize(params_.robot_model->getVariableCount()); + for(size_t i = 0; i < problem_.active_variables.size(); i++) + variables[problem_.active_variables[i]] = genes[i]; } const std::vector& getSolution() const { return solution; } @@ -118,8 +118,8 @@ template struct IKEvolution2 : IKBase quaternion_genes.clear(); for(size_t igene = 0; igene < problem.active_variables.size(); igene++) { - size_t ivar = problem.active_variables[igene]; - auto* joint_model = params.robot_model->getJointOfVariable(ivar); + auto ivar = problem.active_variables[igene]; + auto* joint_model = params_.robot_model->getJointOfVariable(static_cast(ivar)); if(joint_model->getFirstVariableIndex() + 3 != ivar) continue; if(joint_model->getType() != moveit::core::JointModel::FLOATING) continue; quaternion_genes.push_back(igene); @@ -138,8 +138,8 @@ template struct IKEvolution2 : IKBase size_t child_count = 16; // initialize population on current island - species.resize(2); - for(auto& s : species) + species_.resize(2); + for(auto& s : species_) { // create individuals s.individuals.resize(population_size); @@ -162,7 +162,7 @@ template struct IKEvolution2 : IKBase { // initialize populations on other islands randomly for(size_t i = 0; i < v.genes.size(); i++) - v.genes[i] = random(modelInfo.getMin(problem.active_variables[i]), modelInfo.getMax(problem.active_variables[i])); + v.genes[i] = random(modelInfo_.getMin(problem.active_variables[i]), modelInfo_.getMax(problem.active_variables[i])); } // set gradients to zero @@ -189,14 +189,14 @@ template struct IKEvolution2 : IKBase // init gene infos // if(genes_min.empty()) { - genes_min.resize(problem.active_variables.size()); - genes_max.resize(problem.active_variables.size()); - genes_span.resize(problem.active_variables.size()); + genes_min_.resize(problem.active_variables.size()); + genes_max_.resize(problem.active_variables.size()); + genes_span_.resize(problem.active_variables.size()); for(size_t i = 0; i < problem.active_variables.size(); i++) { - genes_min[i] = modelInfo.getClipMin(problem.active_variables[i]); - genes_max[i] = modelInfo.getClipMax(problem.active_variables[i]); - genes_span[i] = modelInfo.getSpan(problem.active_variables[i]); + genes_min_[i] = modelInfo_.getClipMin(problem.active_variables[i]); + genes_max_[i] = modelInfo_.getClipMax(problem.active_variables[i]); + genes_span_[i] = modelInfo_.getSpan(problem.active_variables[i]); } } @@ -237,7 +237,6 @@ template struct IKEvolution2 : IKBase */ // aligned_vector rmask; - // create offspring and mutate __attribute__((hot)) __attribute__((noinline)) //__attribute__((target_clones("avx2", "avx", "sse2", "default"))) @@ -245,16 +244,17 @@ template struct IKEvolution2 : IKBase void reproduce(const std::vector& population) { - const auto __attribute__((aligned(32)))* __restrict__ genes_span = this->genes_span.data(); - const auto __attribute__((aligned(32)))* __restrict__ genes_min = this->genes_min.data(); - const auto __attribute__((aligned(32)))* __restrict__ genes_max = this->genes_max.data(); + const auto __attribute__((aligned(32)))* __restrict__ genes_span = genes_span_.data(); + const auto __attribute__((aligned(32)))* __restrict__ genes_min = genes_min_.data(); + const auto __attribute__((aligned(32)))* __restrict__ genes_max = genes_max_.data(); auto gene_count = children[0].genes.size(); size_t s = (children.size() - population.size()) * gene_count + children.size() * 4 + 4; auto* __restrict__ rr = fast_random_gauss_n(s); - rr = (const double*)(((size_t)rr + 3) / 4 * 4); + rr = reinterpret_cast((reinterpret_cast(rr) + 3) / + 4 * 4); /*rmask.resize(s); for(auto& m : rmask) m = fast_random() < 0.1 ? 1.0 : 0.0; @@ -266,12 +266,11 @@ template struct IKEvolution2 : IKBase auto& parent = population[0]; auto& parent2 = population[1]; double fmix = (child_index % 2 == 0) * 0.2; - double gradient_factor = child_index % 3; + double gradient_factor = static_cast(child_index % 3); auto __attribute__((aligned(32)))* __restrict__ parent_genes = parent.genes.data(); auto __attribute__((aligned(32)))* __restrict__ parent_gradients = parent.gradients.data(); - auto __attribute__((aligned(32)))* __restrict__ parent2_genes = parent2.genes.data(); auto __attribute__((aligned(32)))* __restrict__ parent2_gradients = parent2.gradients.data(); auto& child = children[child_index]; @@ -279,8 +278,7 @@ template struct IKEvolution2 : IKBase auto __attribute__((aligned(32)))* __restrict__ child_genes = child.genes.data(); auto __attribute__((aligned(32)))* __restrict__ child_gradients = child.gradients.data(); -#pragma omp simd aligned(genes_span : 32), aligned(genes_min : 32), aligned(genes_max : 32), aligned(parent_genes : 32), aligned(parent_gradients : 32), aligned(parent2_genes : 32), aligned(parent2_gradients : 32), aligned(child_genes : 32), aligned(child_gradients : 32) aligned(rr : 32) -#pragma unroll +#pragma omp simd aligned(genes_span : 32), aligned(genes_min : 32), aligned(genes_max : 32), aligned(parent_genes : 32), aligned(parent_gradients : 32), aligned(parent2_gradients : 32), aligned(child_genes : 32), aligned(child_gradients : 32) aligned(rr : 32) for(size_t gene_index = 0; gene_index < gene_count; gene_index++) { // double mutation_rate = (1 << fast_random_index(16)) * (1.0 / (1 << 23)); @@ -292,8 +290,8 @@ template struct IKEvolution2 : IKBase double parent_gene = gene; gene += r * f; double parent_gradient = mix(parent_gradients[gene_index], parent2_gradients[gene_index], fmix); - double gradient = parent_gradient * gradient_factor; - gene += gradient; + double gradient_fctd = parent_gradient * gradient_factor; + gene += gradient_fctd; gene = clamp(gene, genes_min[gene_index], genes_max[gene_index]); child_genes[gene_index] = gene; child_gradients[gene_index] = mix(parent_gradient, gene - parent_gene, 0.3); @@ -319,7 +317,7 @@ template struct IKEvolution2 : IKBase for(auto quaternion_gene_index : quaternion_genes) { - auto& qpos = (*(Quaternion*)&(children[child_index].genes[quaternion_gene_index])); + auto& qpos = (*reinterpret_cast(&children[child_index].genes[quaternion_gene_index])); normalizeFast(qpos); } } @@ -328,10 +326,9 @@ template struct IKEvolution2 : IKBase void step() { FNPROFILER(); - - for(size_t ispecies = 0; ispecies < species.size(); ispecies++) + for(size_t ispecies = 0; ispecies < species_.size(); ispecies++) { - auto& species = this->species[ispecies]; + auto& species = species_[ispecies]; auto& population = species.individuals; { @@ -341,8 +338,8 @@ template struct IKEvolution2 : IKBase genesToJointVariables(species.individuals[0], temp_joint_variables); { BLOCKPROFILER("fk"); - model.applyConfiguration(temp_joint_variables); - model.initializeMutationApproximator(problem.active_variables); + model_.applyConfiguration(temp_joint_variables); + model_.initializeMutationApproximator(problem_.active_variables); } // run evolution for a few generations @@ -352,7 +349,7 @@ template struct IKEvolution2 : IKBase { // BLOCKPROFILER("evolution"); - if(canceled) break; + if(canceled_) break; // reproduction { @@ -363,7 +360,7 @@ template struct IKEvolution2 : IKBase size_t child_count = children.size(); // pre-selection by secondary objectives - if(problem.secondary_goals.size()) + if(problem_.secondary_goals.size()) { BLOCKPROFILER("pre-selection"); child_count = random_index(children.size() - population.size() - 1) + 1 + population.size(); @@ -373,7 +370,7 @@ template struct IKEvolution2 : IKBase } { BLOCKPROFILER("pre-selection sort"); - std::sort(children.begin() + population.size(), children.end(), [](const Individual& a, const Individual& b) { return a.fitness < b.fitness; }); + std::sort(children.begin() + static_cast(population.size()), children.end(), [](const Individual& a, const Individual& b) { return a.fitness < b.fitness; }); } } @@ -390,11 +387,10 @@ template struct IKEvolution2 : IKBase // genotype-phenotype mapping { BLOCKPROFILER("phenotype"); - size_t gene_count = children[0].genes.size(); genotypes.resize(child_count); for(size_t i = 0; i < child_count; i++) genotypes[i] = children[i].genes.data(); - model.computeApproximateMutations(child_count, genotypes.data(), phenotypes); + model_.computeApproximateMutations(child_count, genotypes.data(), phenotypes); } // fitness @@ -441,7 +437,7 @@ template struct IKEvolution2 : IKBase // init auto& individual = population[0]; - gradient.resize(problem.active_variables.size()); + gradient.resize(problem_.active_variables.size()); if(genotypes.empty()) genotypes.emplace_back(); phenotypes2.resize(1); phenotypes3.resize(1); @@ -454,46 +450,46 @@ template struct IKEvolution2 : IKBase // for(size_t generation = 0; generation < 32; generation++) { - if(canceled) break; + if(canceled_) break; // compute gradient temp = individual.genes; genotypes[0] = temp.data(); - model.computeApproximateMutations(1, genotypes.data(), phenotypes2); + model_.computeApproximateMutations(1, genotypes.data(), phenotypes2); double f2p = computeFitnessActiveVariables(phenotypes2[0], genotypes[0]); double fa = f2p + computeSecondaryFitnessActiveVariables(genotypes[0]); - for(size_t i = 0; i < problem.active_variables.size(); i++) + for(size_t i = 0; i < problem_.active_variables.size(); i++) { - double* pp = &(genotypes[0][i]); genotypes[0][i] = individual.genes[i] + dp; - model.computeApproximateMutation1(problem.active_variables[i], +dp, phenotypes2[0], phenotypes3[0]); - double fb = computeCombinedFitnessActiveVariables(phenotypes3[0], genotypes[0]); - genotypes[0][i] = individual.genes[i]; + model_.computeApproximateMutation1(problem_.active_variables[i], + +dp, phenotypes2[0], + phenotypes3[0]); + double fb = computeCombinedFitnessActiveVariables(phenotypes3[0], + genotypes[0]); double d = fb - fa; gradient[i] = d; } // normalize gradient double sum = dp * dp; - for(size_t i = 0; i < problem.active_variables.size(); i++) + for(size_t i = 0; i < problem_.active_variables.size(); i++) sum += fabs(gradient[i]); double f = 1.0 / sum * dp; - for(size_t i = 0; i < problem.active_variables.size(); i++) + for(size_t i = 0; i < problem_.active_variables.size(); i++) gradient[i] *= f; // sample support points for line search - for(size_t i = 0; i < problem.active_variables.size(); i++) + for(size_t i = 0; i < problem_.active_variables.size(); i++) genotypes[0][i] = individual.genes[i] - gradient[i]; - model.computeApproximateMutations(1, genotypes.data(), phenotypes3); + model_.computeApproximateMutations(1, genotypes.data(), phenotypes3); double f1 = computeCombinedFitnessActiveVariables(phenotypes3[0], genotypes[0]); double f2 = fa; - for(size_t i = 0; i < problem.active_variables.size(); i++) + for(size_t i = 0; i < problem_.active_variables.size(); i++) genotypes[0][i] = individual.genes[i] + gradient[i]; - model.computeApproximateMutations(1, genotypes.data(), phenotypes3); + model_.computeApproximateMutations(1, genotypes.data(), phenotypes3); double f3 = computeCombinedFitnessActiveVariables(phenotypes3[0], genotypes[0]); - // quadratic step size if(memetic == 'q') { @@ -518,12 +514,12 @@ template struct IKEvolution2 : IKBase // for(double f : { 1.0, 0.5, 0.25 }) { - double f = 1.0; + f = 1.0; // move by step size along gradient and compute fitness - for(size_t i = 0; i < problem.active_variables.size(); i++) - genotypes[0][i] = modelInfo.clip(individual.genes[i] + gradient[i] * step_size * f, problem.active_variables[i]); - model.computeApproximateMutations(1, genotypes.data(), phenotypes2); + for(size_t i = 0; i < problem_.active_variables.size(); i++) + genotypes[0][i] = modelInfo_.clip(individual.genes[i] + gradient[i] * step_size * f, problem_.active_variables[i]); + model_.computeApproximateMutations(1, genotypes.data(), phenotypes2); double f4p = computeFitnessActiveVariables(phenotypes2[0], genotypes[0]); // accept new position if better @@ -550,9 +546,9 @@ template struct IKEvolution2 : IKBase double step_size = f2 / cost_diff; // f / (f / j) = j // move by step size along gradient and compute fitness - for(size_t i = 0; i < problem.active_variables.size(); i++) - temp[i] = modelInfo.clip(individual.genes[i] - gradient[i] * step_size, problem.active_variables[i]); - model.computeApproximateMutations(1, genotypes.data(), phenotypes2); + for(size_t i = 0; i < problem_.active_variables.size(); i++) + temp[i] = modelInfo_.clip(individual.genes[i] - gradient[i] * step_size, problem_.active_variables[i]); + model_.computeApproximateMutations(1, genotypes.data(), phenotypes2); double f4p = computeFitnessActiveVariables(phenotypes2[0], genotypes[0]); // accept new position if better @@ -586,16 +582,16 @@ template struct IKEvolution2 : IKBase } // init starting point - optlib_vector.resize(problem.active_variables.size()); - for(size_t i = 0; i < problem.active_variables.size(); i++) + optlib_vector.resize(problem_.active_variables.size()); + for(size_t i = 0; i < problem_.active_variables.size(); i++) optlib_vector[i] = individual.genes[i]; // solve optlib_solver->minimize(*optlib_problem, optlib_vector); // get result - for(size_t i = 0; i < problem.active_variables.size(); i++) - individual.genes[i] = modelInfo.clip(optlib_vector[i], problem.active_variables[i]); + for(size_t i = 0; i < problem_.active_variables.size(); i++) + individual.genes[i] = modelInfo.clip(optlib_vector[i], problem_.active_variables[i]); } #endif } @@ -605,7 +601,7 @@ template struct IKEvolution2 : IKBase BLOCKPROFILER("species"); // compute species fitness - for(auto& species : this->species) + for(auto& species : species_) { genesToJointVariables(species.individuals[0], temp_joint_variables); double fitness = computeFitness(temp_joint_variables); @@ -614,33 +610,33 @@ template struct IKEvolution2 : IKBase } // sort species by fitness - std::sort(species.begin(), species.end(), [](const Species& a, const Species& b) { return a.fitness < b.fitness; }); + std::sort(species_.begin(), species_.end(), [](const Species& a, const Species& b) { return a.fitness < b.fitness; }); // wipeouts - for(size_t species_index = 1; species_index < species.size(); species_index++) + for(size_t species_index = 1; species_index < species_.size(); species_index++) { - if(fast_random() < 0.1 || !species[species_index].improved) + if(fast_random() < 0.1 || !species_[species_index].improved) // if(fast_random() < 0.05 || !species[species_index].improved) { { - auto& individual = species[species_index].individuals[0]; + auto& individual = species_[species_index].individuals[0]; for(size_t i = 0; i < individual.genes.size(); i++) - individual.genes[i] = random(modelInfo.getMin(problem.active_variables[i]), modelInfo.getMax(problem.active_variables[i])); + individual.genes[i] = random(modelInfo_.getMin(problem_.active_variables[i]), modelInfo_.getMax(problem_.active_variables[i])); for(auto& v : individual.gradients) v = 0; } - for(size_t i = 0; i < species[species_index].individuals.size(); i++) - species[species_index].individuals[i] = species[species_index].individuals[0]; + for(size_t i = 0; i < species_[species_index].individuals.size(); i++) + species_[species_index].individuals[i] = species_[species_index].individuals[0]; } } // update solution - if(species[0].fitness < solution_fitness) + if(species_[0].fitness < solution_fitness) { - genesToJointVariables(species[0].individuals[0], solution); - solution_fitness = species[0].fitness; + genesToJointVariables(species_[0].individuals[0], solution); + solution_fitness = species_[0].fitness; } } } diff --git a/src/ik_gradient.cpp b/src/ik_gradient.cpp index 8ccb0bed..92d3c6bf 100644 --- a/src/ik_gradient.cpp +++ b/src/ik_gradient.cpp @@ -41,11 +41,11 @@ namespace bio_ik // (mainly for testing RobotFK_Jacobian::computeJacobian) template struct IKJacobianBase : BASE { - using BASE::modelInfo; - using BASE::model; - using BASE::params; + using BASE::modelInfo_; + using BASE::model_; + using BASE::params_; using BASE::computeFitness; - using BASE::problem; + using BASE::problem_; std::vector tipObjectives; @@ -71,21 +71,21 @@ template struct IKJacobianBase : BASE { FNPROFILER(); - int tip_count = problem.tip_link_indices.size(); + int tip_count = static_cast(problem_.tip_link_indices.size()); tip_diffs.resize(tip_count * 6); - joint_diffs.resize(problem.active_variables.size()); + joint_diffs.resize(static_cast(problem_.active_variables.size())); // compute fk - model.applyConfiguration(solution); + model_.applyConfiguration(solution); double translational_scale = 1; double rotational_scale = 1; // compute goal diffs - tip_frames_temp = model.getTipFrames(); + tip_frames_temp = model_.getTipFrames(); for(int itip = 0; itip < tip_count; itip++) { - auto twist = frameTwist(tip_frames_temp[itip], tipObjectives[itip]); + auto twist = frameTwist(tip_frames_temp[static_cast(itip)], tipObjectives[static_cast(itip)]); tip_diffs(itip * 6 + 0) = twist.vel.x() * translational_scale; tip_diffs(itip * 6 + 1) = twist.vel.y() * translational_scale; tip_diffs(itip * 6 + 2) = twist.vel.z() * translational_scale; @@ -96,11 +96,10 @@ template struct IKJacobianBase : BASE // compute jacobian { - model.computeJacobian(problem.active_variables, jacobian); - int icol = 0; - for(auto ivar : problem.active_variables) + model_.computeJacobian(problem_.active_variables, jacobian); + for(Eigen::Index icol = 0; icol < static_cast(problem_.active_variables.size()); icol++) { - for(size_t itip = 0; itip < tip_count; itip++) + for(Eigen::Index itip = 0; itip < static_cast(tip_count); itip++) { jacobian(itip * 6 + 0, icol) *= translational_scale; jacobian(itip * 6 + 1, icol) *= translational_scale; @@ -109,7 +108,6 @@ template struct IKJacobianBase : BASE jacobian(itip * 6 + 4, icol) *= rotational_scale; jacobian(itip * 6 + 5, icol) *= rotational_scale; } - icol++; } } @@ -120,11 +118,11 @@ template struct IKJacobianBase : BASE // apply joint deltas and clip { int icol = 0; - for(auto ivar : problem.active_variables) + for(auto ivar : problem_.active_variables) { auto v = solution[ivar] + joint_diffs(icol); if(!std::isfinite(v)) continue; - v = modelInfo.clip(v, ivar); + v = modelInfo_.clip(v, ivar); solution[ivar] = v; icol++; } @@ -147,9 +145,9 @@ template struct IKGradientDescent : IKBase { IKBase::initialize(problem); solution = problem.initial_guess; - if(thread_index > 0) + if(thread_index_ > 0) for(auto& vi : problem.active_variables) - solution[vi] = random(modelInfo.getMin(vi), modelInfo.getMax(vi)); + solution[vi] = random(modelInfo_.getMin(vi), modelInfo_.getMax(vi)); best_solution = solution; reset = false; } @@ -162,15 +160,15 @@ template struct IKGradientDescent : IKBase if(reset) { reset = false; - for(auto& vi : problem.active_variables) - solution[vi] = random(modelInfo.getMin(vi), modelInfo.getMax(vi)); + for(auto& vi : problem_.active_variables) + solution[vi] = random(modelInfo_.getMin(vi), modelInfo_.getMax(vi)); } // compute gradient direction temp = solution; double jd = 0.0001; gradient.resize(solution.size(), 0); - for(auto ivar : problem.active_variables) + for(auto ivar : problem_.active_variables) { temp[ivar] = solution[ivar] - jd; double p1 = computeFitness(temp); @@ -185,23 +183,23 @@ template struct IKGradientDescent : IKBase // normalize gradient direction double sum = 0.0001; - for(auto ivar : problem.active_variables) + for(auto ivar : problem_.active_variables) sum += fabs(gradient[ivar]); double f = 1.0 / sum * jd; - for(auto ivar : problem.active_variables) + for(auto ivar : problem_.active_variables) gradient[ivar] *= f; // initialize line search temp = solution; - for(auto ivar : problem.active_variables) + for(auto ivar : problem_.active_variables) temp[ivar] = solution[ivar] - gradient[ivar]; double p1 = computeFitness(temp); // for(auto ivar : problem.active_variables) temp[ivar] = solution[ivar]; // double p2 = computeFitness(temp); - for(auto ivar : problem.active_variables) + for(auto ivar : problem_.active_variables) temp[ivar] = solution[ivar] + gradient[ivar]; double p3 = computeFitness(temp); @@ -217,8 +215,8 @@ template struct IKGradientDescent : IKBase // apply optimization step // (move along gradient direction by estimated step size) - for(auto ivar : problem.active_variables) - temp[ivar] = modelInfo.clip(solution[ivar] - gradient[ivar] * joint_diff, ivar); + for(auto ivar : problem_.active_variables) + temp[ivar] = modelInfo_.clip(solution[ivar] - gradient[ivar] * joint_diff, ivar); if(if_stuck == 'c') { @@ -278,9 +276,9 @@ template struct IKJacobian : IKJacobianBase { IKJacobianBase::initialize(problem); solution = problem.initial_guess; - if(thread_index > 0) + if(thread_index_ > 0) for(auto& vi : problem.active_variables) - solution[vi] = random(modelInfo.getMin(vi), modelInfo.getMax(vi)); + solution[vi] = random(modelInfo_.getMin(vi), modelInfo_.getMax(vi)); } const std::vector& getSolution() const { return solution; } void step() { optimizeJacobian(solution); } diff --git a/src/ik_parallel.h b/src/ik_parallel.h index 0441a72c..5c175add 100644 --- a/src/ik_parallel.h +++ b/src/ik_parallel.h @@ -42,21 +42,21 @@ namespace bio_ik // executes a function in parallel on pre-allocated threads class ParallelExecutor { - std::function fun; + volatile bool exit; std::vector threads; + std::function fun; boost::barrier barrier; - volatile bool exit; - double best_fitness; + double best_fitness_; public: template - ParallelExecutor(size_t thread_count, const FUN& f) + ParallelExecutor(size_t thread_count_, const FUN& f) : exit(false) - , threads(thread_count) + , threads(thread_count_) , fun(f) - , barrier(thread_count) + , barrier(static_cast(thread_count_)) { - for(size_t i = 1; i < thread_count; i++) + for(size_t i = 1; i < thread_count_; i++) { std::thread t([this, i]() { while(true) @@ -86,62 +86,63 @@ class ParallelExecutor } }; + // runs ik on multiple threads until a stop criterion is met struct IKParallel { - IKParams params; - std::vector> solvers; - std::vector> solver_solutions; - std::vector> solver_temps; - std::vector solver_success; - std::vector solver_fitness; - int thread_count; + IKParams params_; + std::vector> solvers_; + std::vector> solver_solutions_; + std::vector> solver_temps_; + std::vector solver_success_; + std::vector solver_fitness_; + size_t thread_count_; // std::vector fk; // TODO: remove - std::chrono::time_point> timeout; - bool success; - std::atomic finished; - std::atomic iteration_count; - std::vector result; - std::unique_ptr par; - Problem problem; - bool enable_counter; - double best_fitness; + std::chrono::time_point> timeout_; + bool success_; + std::atomic finished_; + std::atomic iteration_count_; + std::vector result_; + std::unique_ptr par_; + Problem problem_; + bool enable_counter_; + double best_fitness_; IKParallel(const IKParams& params) - : params(params) + : params_(params) { // solver class name - std::string name = params.solver_class_name; + std::string name = params_.solver_class_name; - enable_counter = params.enable_counter; + enable_counter_ = params_.enable_counter; // create solvers - solvers.emplace_back(IKFactory::create(name, params)); - thread_count = solvers.front()->concurrency(); - if(params.thread_count) { - thread_count = params.thread_count; + solvers_.emplace_back(IKFactory::create(name, params)); + thread_count_ = solvers_.front()->concurrency(); + if(params_.thread_count) { + thread_count_ = params_.thread_count; } - while(solvers.size() < thread_count) - solvers.emplace_back(IKFactory::clone(solvers.front().get())); - for(size_t i = 0; i < thread_count; i++) - solvers[i]->thread_index = i; + while(solvers_.size() < thread_count_) + solvers_.emplace_back(IKFactory::clone(solvers_.front().get())); + for(size_t i = 0; i < thread_count_; i++) + solvers_[i]->thread_index_ = i; - // while(fk.size() < thread_count) fk.emplace_back(params.robot_model); + // while(fk.size() < thread_count_) fk.emplace_back(params.robot_model); // init buffers - solver_solutions.resize(thread_count); - solver_temps.resize(thread_count); - solver_success.resize(thread_count); - solver_fitness.resize(thread_count); + solver_solutions_.resize(thread_count_); + solver_temps_.resize(thread_count_); + solver_success_.resize(thread_count_); + solver_fitness_.resize(thread_count_); // create parallel executor - par.reset(new ParallelExecutor(thread_count, [this](size_t i) { solverthread(i); })); + par_.reset(new ParallelExecutor(thread_count_, [this](size_t i) { solverthread(i); })); } void initialize(const Problem& problem) { - this->problem = problem; - // for(auto& f : fk) f.initialize(problem.tip_link_indices); + problem_ = problem; + // for(auto& f : fk) f.initialize(problem_.tip_link_indices); } private: @@ -153,40 +154,40 @@ struct IKParallel // initialize ik solvers { BLOCKPROFILER("ik solver init"); - solvers[i]->initialize(problem); + solvers_[i]->initialize(problem_); } // run solver iterations until solution found or timeout - for(size_t iteration = 0; (std::chrono::system_clock::now() < timeout && finished == 0) || (iteration == 0 && i == 0); iteration++) + for(size_t iteration = 0; (std::chrono::system_clock::now() < timeout_ && finished_ == 0) || (iteration == 0 && i == 0); iteration++) { - if(finished) break; + if(finished_) break; // run solver for a few steps - solvers[i]->step(); - iteration_count++; + solvers_[i]->step(); + iteration_count_++; for(int it2 = 1; it2 < 4; it2++) - if(std::chrono::system_clock::now() < timeout && finished == 0) solvers[i]->step(); + if(std::chrono::system_clock::now() < timeout_ && finished_ == 0) solvers_[i]->step(); - if(finished) break; + if(finished_) break; // get solution and check stop criterion - auto& result = solver_temps[i]; - result = solvers[i]->getSolution(); - auto& fk = solvers[i]->model; + auto& result = solver_temps_[i]; + result = solvers_[i]->getSolution(); + auto& fk = solvers_[i]->model_; fk.applyConfiguration(result); - bool success = solvers[i]->checkSolution(result, fk.getTipFrames()); - if(success) finished = 1; - solver_success[i] = success; - solver_solutions[i] = result; - solver_fitness[i] = solvers[i]->computeFitness(result, fk.getTipFrames()); + bool success = solvers_[i]->checkSolution(result, fk.getTipFrames()); + if(success) finished_ = 1; + solver_success_[i] = success; + solver_solutions_[i] = result; + solver_fitness_[i] = solvers_[i]->computeFitness(result, fk.getTipFrames()); if(success) break; } - finished = 1; + finished_ = 1; - for(auto& s : solvers) - s->canceled = true; + for(auto& s : solvers_) + s->canceled_ = true; } public: @@ -195,52 +196,52 @@ struct IKParallel BLOCKPROFILER("solve mt"); // prepare - iteration_count = 0; - result = problem.initial_guess; - timeout = problem.timeout; - success = false; - finished = 0; - for(auto& s : solver_solutions) - s = problem.initial_guess; - for(auto& s : solver_temps) - s = problem.initial_guess; - for(auto& s : solver_success) + iteration_count_ = 0; + result_ = problem_.initial_guess; + timeout_ = problem_.timeout; + success_ = false; + finished_ = 0; + for(auto& s : solver_solutions_) + s = problem_.initial_guess; + for(auto& s : solver_temps_) + s = problem_.initial_guess; + for(auto& s : solver_success_) s = 0; - for(auto& f : solver_fitness) + for(auto& f : solver_fitness_) f = DBL_MAX; - for(auto& s : solvers) - s->canceled = false; + for(auto& s : solvers_) + s->canceled_ = false; - // run solvers + // run solvers_ { BLOCKPROFILER("solve mt 2"); - par->run(); + par_->run(); } size_t best_index = 0; - best_fitness = DBL_MAX; + best_fitness_ = DBL_MAX; // if exact primary goal matches have been found ... - for(size_t i = 0; i < thread_count; i++) + for(size_t i = 0; i < thread_count_; i++) { - if(solver_success[i]) + if(solver_success_[i]) { double fitness; - if(solvers[0]->problem.secondary_goals.empty()) + if(solvers_[0]->problem_.secondary_goals.empty()) { // ... and if no secondary goals have been specified, - // select the best result according to primary goals - fitness = solver_fitness[i]; + // select the best result_ according to primary goals + fitness = solver_fitness_[i]; } else { // ... and if secondary goals have been specified, // select the result that best satisfies primary and secondary goals - fitness = solver_fitness[i] + solvers[0]->computeSecondaryFitnessAllVariables(solver_solutions[i]); + fitness = solver_fitness_[i] + solvers_[0]->computeSecondaryFitnessAllVariables(solver_solutions_[i]); } - if(fitness < best_fitness) + if(fitness < best_fitness_) { - best_fitness = fitness; + best_fitness_ = fitness; best_index = i; } } @@ -248,31 +249,31 @@ struct IKParallel // if no exact primary goal matches have been found, // select best primary goal approximation - if(best_fitness == DBL_MAX) + if(best_fitness_ == DBL_MAX) { - for(size_t i = 0; i < thread_count; i++) + for(size_t i = 0; i < thread_count_; i++) { - if(solver_fitness[i] < best_fitness) + if(solver_fitness_[i] < best_fitness_) { - best_fitness = solver_fitness[i]; + best_fitness_ = solver_fitness_[i]; best_index = i; } } } - if(enable_counter) + if(enable_counter_) { - LOG("iterations", iteration_count); + LOG("iterations", iteration_count_); } - result = solver_solutions[best_index]; - success = solver_success[best_index]; + result_ = solver_solutions_[best_index]; + success_ = solver_success_[best_index]; } - double getSolutionFitness() const { return best_fitness; } + double getSolutionFitness() const { return best_fitness_; } - bool getSuccess() const { return success; } + bool getSuccess() const { return success_; } - const std::vector& getSolution() const { return result; } + const std::vector& getSolution() const { return result_; } }; } diff --git a/src/ik_test.cpp b/src/ik_test.cpp index a7fabe92..9fdbc846 100644 --- a/src/ik_test.cpp +++ b/src/ik_test.cpp @@ -71,10 +71,10 @@ struct IKTest : IKBase IKBase::initialize(problem); fkref.initialize(problem.tip_link_indices); - model.initialize(problem.tip_link_indices); + model_.initialize(problem.tip_link_indices); fkref.applyConfiguration(problem.initial_guess); - model.applyConfiguration(problem.initial_guess); + model_.applyConfiguration(problem.initial_guess); // double diff = tipdiff(fkref.getTipFrames(), model.getTipFrames()); // LOG_VAR(diff); @@ -92,13 +92,13 @@ struct IKTest : IKBase { temp = problem.initial_guess; for(size_t ivar : problem.active_variables) - if(modelInfo.isRevolute(ivar) || modelInfo.isPrismatic(ivar)) temp[ivar] = modelInfo.clip(temp[ivar] + random(-0.1, 0.1), ivar); + if(modelInfo_.isRevolute(ivar) || modelInfo_.isPrismatic(ivar)) temp[ivar] = modelInfo_.clip(temp[ivar] + random(-0.1, 0.1), ivar); fkref.applyConfiguration(temp); auto& fa = fkref.getTipFrames(); - model.applyConfiguration(problem.initial_guess); - model.initializeMutationApproximator(problem.active_variables); + model_.applyConfiguration(problem.initial_guess); + model_.initializeMutationApproximator(problem.active_variables); std::vector> fbm; @@ -107,7 +107,7 @@ struct IKTest : IKBase mutation_values.push_back(temp[ivar]); const double* mutation_ptr = mutation_values.data(); - model.computeApproximateMutations(1, &mutation_ptr, fbm); + model_.computeApproximateMutations(1, &mutation_ptr, fbm); auto& fb = fbm[0]; @@ -130,7 +130,7 @@ struct IKTest : IKBase void step() {} - const std::vector& getSolution() const { return problem.initial_guess; } + const std::vector& getSolution() const { return problem_.initial_guess; } }; static IKFactory::Class test("test"); diff --git a/src/kinematics_plugin.cpp b/src/kinematics_plugin.cpp index 1fc28b96..39f7abe7 100644 --- a/src/kinematics_plugin.cpp +++ b/src/kinematics_plugin.cpp @@ -46,6 +46,7 @@ #include #include #include + #include #include #include @@ -95,7 +96,7 @@ bool isBioIKKinematicsQueryOptions(const void *ptr) { const BioIKKinematicsQueryOptions * toBioIKKinematicsQueryOptions(const void *ptr) { if (isBioIKKinematicsQueryOptions(ptr)) - return (const BioIKKinematicsQueryOptions *)ptr; + return static_cast(ptr); else return 0; } @@ -118,29 +119,33 @@ struct BioIKKinematicsPlugin : kinematics::KinematicsBase { BioIKKinematicsPlugin() { enable_profiler = false; } - virtual const std::vector &getJointNames() const { + const std::vector &getJointNames() const override { LOG_FNC(); return joint_names; } - virtual const std::vector &getLinkNames() const { + const std::vector &getLinkNames() const override { LOG_FNC(); return link_names; } - virtual bool getPositionFK(const std::vector &link_names, - const std::vector &joint_angles, - std::vector &poses) const { + bool getPositionFK( + const std::vector &, + const std::vector &, + std::vector &) const override { LOG_FNC(); return false; } - virtual bool getPositionIK(const geometry_msgs::msg::Pose &ik_pose, - const std::vector &ik_seed_state, - std::vector &solution, - moveit_msgs::msg::MoveItErrorCodes &error_code, - const kinematics::KinematicsQueryOptions &options = - kinematics::KinematicsQueryOptions()) const { + // Using the non-pure virtual getPositionIK and not overriding. + using kinematics::KinematicsBase::getPositionIK; + + virtual bool getPositionIK( + const geometry_msgs::msg::Pose &, + const std::vector &, + std::vector &, + moveit_msgs::msg::MoveItErrorCodes &, + const kinematics::KinematicsQueryOptions &) const { LOG_FNC(); return false; } @@ -194,9 +199,8 @@ struct BioIKKinematicsPlugin : kinematics::KinematicsBase { link_names = tip_frames_; - // bool enable_profiler; - getRosParam("profiler", enable_profiler, false); - // if(enable_profiler) Profiler::start(); + getRosParam("profiler", enable_profiler, false); + if(enable_profiler) Profiler::start(); robot_info = RobotInfo(robot_model_); @@ -204,22 +208,30 @@ struct BioIKKinematicsPlugin : kinematics::KinematicsBase { ikparams.joint_model_group = joint_model_group; // initialize parameters for IKParallel - getRosParam("mode", ikparams.solver_class_name, - std::string("bio2_memetic")); - getRosParam("counter", ikparams.enable_counter, false); - getRosParam("threads", ikparams.thread_count, 0); - getRosParam("random_seed", ikparams.random_seed, static_cast(std::random_device()())); - + getRosParam("mode", ikparams.solver_class_name, std::string("bio2_memetic")); + getRosParam("counter", ikparams.enable_counter, false); + // workaround for no raw "int" ROS parameter (only long int) + int64_t tc = 0; + getRosParam("threads", tc, 0); + ikparams.thread_count = static_cast(tc); + int64_t rs = 0; + getRosParam("random_seed", rs, std::random_device()()); + ikparams.random_seed = static_cast(rs); // initialize parameters for Problem - getRosParam("dpos", ikparams.dpos, DBL_MAX); - getRosParam("drot", ikparams.drot, DBL_MAX); - getRosParam("dtwist", ikparams.dtwist, 1e-5); + getRosParam("dpos", ikparams.dpos, DBL_MAX); + getRosParam("drot", ikparams.drot, DBL_MAX); + getRosParam("dtwist", ikparams.dtwist, 1e-5); // initialize parameters for ik_evolution_1 - getRosParam("no_wipeout", ikparams.opt_no_wipeout, false); - getRosParam("population_size", ikparams.population_size, 8); - getRosParam("elite_count", ikparams.elite_count, 4); - getRosParam("linear_fitness", ikparams.linear_fitness, false); + getRosParam("no_wipeout", ikparams.opt_no_wipeout, false); + int64_t ps = 8; + getRosParam("population_size", ps, 8); + ikparams.population_size = static_cast(ps); + int64_t ec = 4; + getRosParam("elite_count", ec, 4); + ikparams.elite_count = static_cast(ec); + // TODO(wyattrees): update to "enable_linear_fitness"? + getRosParam("linear_fitness", ikparams.linear_fitness, false); temp_state.reset(new moveit::core::RobotState(robot_model_)); @@ -289,12 +301,11 @@ struct BioIKKinematicsPlugin : kinematics::KinematicsBase { return true; } - virtual bool initialize(const rclcpp::Node::SharedPtr &node, - const moveit::core::RobotModel &robot_model, - const std::string &group_name, - const std::string &base_frame, - const std::vector &tip_frames, - double search_discretization) { + bool initialize(const rclcpp::Node::SharedPtr &node, + const moveit::core::RobotModel &robot_model, + const std::string &group_name, const std::string &base_frame, + const std::vector &tip_frames, + double search_discretization) override { LOG_FNC(); node_ = node; storeValues(robot_model, group_name, base_frame, tip_frames, @@ -302,48 +313,48 @@ struct BioIKKinematicsPlugin : kinematics::KinematicsBase { return load(group_name); } - virtual bool + bool searchPositionIK(const geometry_msgs::msg::Pose &ik_pose, const std::vector &ik_seed_state, double timeout, std::vector &solution, moveit_msgs::msg::MoveItErrorCodes &error_code, const kinematics::KinematicsQueryOptions &options = - kinematics::KinematicsQueryOptions()) const { + kinematics::KinematicsQueryOptions()) const override { LOG_FNC(); return searchPositionIK(std::vector{ik_pose}, ik_seed_state, timeout, std::vector(), solution, IKCallbackFn(), error_code, options); } - virtual bool + bool searchPositionIK(const geometry_msgs::msg::Pose &ik_pose, const std::vector &ik_seed_state, double timeout, const std::vector &consistency_limits, std::vector &solution, moveit_msgs::msg::MoveItErrorCodes &error_code, const kinematics::KinematicsQueryOptions &options = - kinematics::KinematicsQueryOptions()) const { + kinematics::KinematicsQueryOptions()) const override { LOG_FNC(); return searchPositionIK(std::vector{ik_pose}, ik_seed_state, timeout, consistency_limits, solution, IKCallbackFn(), error_code, options); } - virtual bool + bool searchPositionIK(const geometry_msgs::msg::Pose &ik_pose, const std::vector &ik_seed_state, double timeout, std::vector &solution, const IKCallbackFn &solution_callback, moveit_msgs::msg::MoveItErrorCodes &error_code, const kinematics::KinematicsQueryOptions &options = - kinematics::KinematicsQueryOptions()) const { + kinematics::KinematicsQueryOptions()) const override { LOG_FNC(); return searchPositionIK(std::vector{ik_pose}, ik_seed_state, timeout, std::vector(), solution, solution_callback, error_code, options); } - virtual bool + bool searchPositionIK(const geometry_msgs::msg::Pose &ik_pose, const std::vector &ik_seed_state, double timeout, const std::vector &consistency_limits, @@ -351,7 +362,7 @@ struct BioIKKinematicsPlugin : kinematics::KinematicsBase { const IKCallbackFn &solution_callback, moveit_msgs::msg::MoveItErrorCodes &error_code, const kinematics::KinematicsQueryOptions &options = - kinematics::KinematicsQueryOptions()) const { + kinematics::KinematicsQueryOptions()) const override { LOG_FNC(); return searchPositionIK(std::vector{ik_pose}, ik_seed_state, timeout, consistency_limits, @@ -383,17 +394,18 @@ struct BioIKKinematicsPlugin : kinematics::KinematicsBase { error_code, options, context_state); } - virtual bool + bool searchPositionIK(const std::vector &ik_poses, const std::vector &ik_seed_state, double timeout, - const std::vector &consistency_limits, + const std::vector &/*consistency_limits*/, std::vector &solution, const IKCallbackFn &solution_callback, moveit_msgs::msg::MoveItErrorCodes &error_code, const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions(), - const moveit::core::RobotState *context_state = NULL) const { - std::chrono::time_point> t0 = std::chrono::system_clock::now(); + const moveit::core::RobotState *context_state = NULL) const override { + std::chrono::time_point> t0 = std::chrono::system_clock::now(); if (enable_profiler) Profiler::start(); @@ -415,7 +427,7 @@ struct BioIKKinematicsPlugin : kinematics::KinematicsBase { // overwrite used variables with seed state solution = ik_seed_state; { - int i = 0; + size_t i = 0; for (auto &joint_name : getJointNames()) { auto *joint_model = robot_model_->getJointModel(joint_name); if (!joint_model) @@ -481,7 +493,7 @@ struct BioIKKinematicsPlugin : kinematics::KinematicsBase { if (!bio_ik_options || !bio_ik_options->replace) { for (size_t i = 0; i < tip_frames_.size(); i++) { - auto *goal = (PoseGoal *)default_goals[i].get(); + auto *goal = static_cast(default_goals[i].get()); goal->setPosition(tipFrames[i].pos); goal->setOrientation(tipFrames[i].rot); } @@ -592,15 +604,15 @@ struct BioIKKinematicsPlugin : kinematics::KinematicsBase { // return success if callback has accepted the solution return error_code.val == error_code.SUCCESS; - } else { - // return success - error_code.val = error_code.SUCCESS; - return true; } + // return success + error_code.val = error_code.SUCCESS; + return true; } - virtual bool supportsGroup(const moveit::core::JointModelGroup *jmg, - std::string *error_text_out = 0) const { + bool supportsGroup( + const moveit::core::JointModelGroup */*jmg*/, + std::string */*error_text_out*/ = 0) const override { LOG_FNC(); // LOG_VAR(jmg->getName()); return true; @@ -613,4 +625,4 @@ struct BioIKKinematicsPlugin : kinematics::KinematicsBase { #undef LOG #undef ERROR PLUGINLIB_EXPORT_CLASS(bio_ik_kinematics_plugin::BioIKKinematicsPlugin, - kinematics::KinematicsBase); + kinematics::KinematicsBase) diff --git a/src/problem.cpp b/src/problem.cpp index 5b5f25d1..29d162ce 100644 --- a/src/problem.cpp +++ b/src/problem.cpp @@ -56,47 +56,47 @@ enum class Problem::GoalType size_t Problem::addTipLink(const moveit::core::LinkModel* link_model) { - if(link_tip_indices[link_model->getLinkIndex()] < 0) + if(link_tip_indices_[link_model->getLinkIndex()] < 0) { - link_tip_indices[link_model->getLinkIndex()] = tip_link_indices.size(); + link_tip_indices_[link_model->getLinkIndex()] = static_cast(tip_link_indices.size()); tip_link_indices.push_back(link_model->getLinkIndex()); } - return link_tip_indices[link_model->getLinkIndex()]; + return static_cast(link_tip_indices_[link_model->getLinkIndex()]); } Problem::Problem() - : ros_params_initrd(false) + : ros_params_initrd_(false) { } void Problem::initialize(moveit::core::RobotModelConstPtr robot_model, const moveit::core::JointModelGroup* joint_model_group, const IKParams& params, const std::vector& goals2, const BioIKKinematicsQueryOptions* options) { - if(robot_model != this->robot_model) + if(robot_model != robot_model_) { - modelInfo = RobotInfo(robot_model); + modelInfo_ = RobotInfo(robot_model); #if (MOVEIT_FCL_VERSION < FCL_VERSION_CHECK(0, 6, 0)) - collision_links.clear(); - collision_links.resize(robot_model->getLinkModelCount()); + collision_links_.clear(); + collision_links_.resize(robot_model->getLinkModelCount()); #endif } - this->robot_model = robot_model; - this->joint_model_group = joint_model_group; - this->params = params; + robot_model_ = robot_model; + joint_model_group_ = joint_model_group; + params_ = params; - if(!ros_params_initrd) + if(!ros_params_initrd_) { - ros_params_initrd = true; - dpos = params.dpos; - drot = params.drot; - dtwist = params.dtwist; - if(dpos < 0.0 || dpos >= FLT_MAX || !std::isfinite(dpos)) dpos = DBL_MAX; - if(drot < 0.0 || drot >= FLT_MAX || !std::isfinite(drot)) drot = DBL_MAX; - if(dtwist < 0.0 || dtwist >= FLT_MAX || !std::isfinite(dtwist)) dtwist = DBL_MAX; + ros_params_initrd_ = true; + dpos_ = params.dpos; + drot_ = params.drot; + dtwist_ = params.dtwist; + if(dpos_ < 0.0 || dpos_ >= DBL_MAX || !std::isfinite(dpos_)) dpos_ = DBL_MAX; + if(drot_ < 0.0 || drot_ >= DBL_MAX || !std::isfinite(drot_)) drot_ = DBL_MAX; + if(dtwist_ < 0.0 || dtwist_ >= DBL_MAX || !std::isfinite(dtwist_)) dtwist_ = DBL_MAX; } - link_tip_indices.clear(); - link_tip_indices.resize(robot_model->getLinkModelCount(), -1); + link_tip_indices_.clear(); + link_tip_indices_.resize(robot_model->getLinkModelCount(), -1); tip_link_indices.clear(); active_variables.clear(); @@ -108,18 +108,18 @@ void Problem::initialize(moveit::core::RobotModelConstPtr robot_model, const mov { if(fixed_joint_name == joint_name) { - return (ssize_t)-1 - (ssize_t)robot_model->getVariableIndex(name); + return -1 - static_cast(robot_model->getVariableIndex(name)); } } } for(size_t i = 0; i < active_variables.size(); i++) - if(name == robot_model->getVariableNames()[active_variables[i]]) return i; + if(name == robot_model->getVariableNames()[active_variables[i]]) return static_cast(i); for(auto& n : joint_model_group->getVariableNames()) { if(n == name) { active_variables.push_back(robot_model->getVariableIndex(name)); - return active_variables.size() - 1; + return static_cast(active_variables.size()) - 1; } } ERROR("joint variable not found", name); @@ -189,38 +189,38 @@ void Problem::initialize(moveit::core::RobotModelConstPtr robot_model, const mov } // update active variables from active subtree - joint_usage.resize(robot_model->getJointModelCount()); - for(auto& u : joint_usage) + joint_usage_.resize(robot_model->getJointModelCount()); + for(auto& u : joint_usage_) u = 0; for(auto tip_index : tip_link_indices) for(auto* link_model = robot_model->getLinkModels()[tip_index]; link_model; link_model = link_model->getParentLinkModel()) - joint_usage[link_model->getParentJointModel()->getJointIndex()] = 1; + joint_usage_[link_model->getParentJointModel()->getJointIndex()] = 1; if(options) for(auto& fixed_joint_name : options->fixed_joints) - joint_usage[robot_model->getJointModel(fixed_joint_name)->getJointIndex()] = 0; + joint_usage_[robot_model->getJointModel(fixed_joint_name)->getJointIndex()] = 0; for(auto* joint_model : joint_model_group->getActiveJointModels()) - if(joint_usage[joint_model->getJointIndex()] && !joint_model->getMimic()) + if(joint_usage_[joint_model->getJointIndex()] && !joint_model->getMimic()) for(auto& n : joint_model->getVariableNames()) addActiveVariable(n); // init weights for minimal displacement goals { - minimal_displacement_factors.resize(active_variables.size()); + minimal_displacement_factors_.resize(active_variables.size()); double s = 0; for(auto ivar : active_variables) - s += modelInfo.getMaxVelocityRcp(ivar); + s += modelInfo_.getMaxVelocityRcp(ivar); if(s > 0) { for(size_t i = 0; i < active_variables.size(); i++) { auto ivar = active_variables[i]; - minimal_displacement_factors[i] = modelInfo.getMaxVelocityRcp(ivar) / s; + minimal_displacement_factors_[i] = modelInfo_.getMaxVelocityRcp(ivar) / s; } } else { for(size_t i = 0; i < active_variables.size(); i++) - minimal_displacement_factors[i] = 1.0 / active_variables.size(); + minimal_displacement_factors_[i] = 1.0 / static_cast(active_variables.size()); } } @@ -235,8 +235,8 @@ void Problem::initialize2() { g.goal_context.problem_active_variables_ = active_variables; g.goal_context.problem_tip_link_indices_ = tip_link_indices; - g.goal_context.velocity_weights_ = minimal_displacement_factors; - g.goal_context.robot_info_ = &modelInfo; + g.goal_context.velocity_weights_ = minimal_displacement_factors_; + g.goal_context.robot_info_ = &modelInfo_; } } } @@ -248,10 +248,10 @@ double Problem::computeGoalFitness(GoalInfo& goal_info, const Frame* tip_frames, return goal_info.goal->evaluate(goal_info.goal_context) * goal_info.weight_sq; } -double Problem::computeGoalFitness(std::vector& goals, const Frame* tip_frames, const double* active_variable_positions) +double Problem::computeGoalFitness(std::vector& input_goals, const Frame* tip_frames, const double* active_variable_positions) { double sum = 0.0; - for(auto& goal : goals) + for(auto& goal : input_goals) sum += computeGoalFitness(goal, tip_frames, active_variable_positions); return sum; } @@ -268,58 +268,58 @@ bool Problem::checkSolutionActiveVariables(const std::vector& tip_frames, case GoalType::Position: { - if(dpos != DBL_MAX) + if(dpos_ != DBL_MAX) { double p_dist = (fb.pos - fa.pos).length(); - if(!(p_dist <= dpos)) return false; + if(!(p_dist <= dpos_)) return false; } - if(dtwist != DBL_MAX) + if(dtwist_ != DBL_MAX) { KDL::Frame fk_kdl, ik_kdl; frameToKDL(fa, fk_kdl); frameToKDL(fb, ik_kdl); KDL::Twist kdl_diff(fk_kdl.M.Inverse() * KDL::diff(fk_kdl.p, ik_kdl.p), fk_kdl.M.Inverse() * KDL::diff(fk_kdl.M, ik_kdl.M)); - if(!KDL::Equal(kdl_diff.vel, KDL::Twist::Zero().vel, dtwist)) return false; + if(!KDL::Equal(kdl_diff.vel, KDL::Twist::Zero().vel, dtwist_)) return false; } continue; } case GoalType::Orientation: { - if(drot != DBL_MAX) + if(drot_ != DBL_MAX) { double r_dist = fb.rot.angleShortestPath(fa.rot); r_dist = r_dist * 180 / M_PI; - if(!(r_dist <= drot)) return false; + if(!(r_dist <= drot_)) return false; } - if(dtwist != DBL_MAX) + if(dtwist_ != DBL_MAX) { KDL::Frame fk_kdl, ik_kdl; frameToKDL(fa, fk_kdl); frameToKDL(fb, ik_kdl); KDL::Twist kdl_diff(fk_kdl.M.Inverse() * KDL::diff(fk_kdl.p, ik_kdl.p), fk_kdl.M.Inverse() * KDL::diff(fk_kdl.M, ik_kdl.M)); - if(!KDL::Equal(kdl_diff.rot, KDL::Twist::Zero().rot, dtwist)) return false; + if(!KDL::Equal(kdl_diff.rot, KDL::Twist::Zero().rot, dtwist_)) return false; } continue; } case GoalType::Pose: { - if(dpos != DBL_MAX || drot != DBL_MAX) + if(dpos_ != DBL_MAX || drot_ != DBL_MAX) { double p_dist = (fb.pos - fa.pos).length(); double r_dist = fb.rot.angleShortestPath(fa.rot); r_dist = r_dist * 180 / M_PI; - if(!(p_dist <= dpos)) return false; - if(!(r_dist <= drot)) return false; + if(!(p_dist <= dpos_)) return false; + if(!(r_dist <= drot_)) return false; } - if(dtwist != DBL_MAX) + if(dtwist_ != DBL_MAX) { KDL::Frame fk_kdl, ik_kdl; frameToKDL(fa, fk_kdl); frameToKDL(fb, ik_kdl); KDL::Twist kdl_diff(fk_kdl.M.Inverse() * KDL::diff(fk_kdl.p, ik_kdl.p), fk_kdl.M.Inverse() * KDL::diff(fk_kdl.M, ik_kdl.M)); - if(!KDL::Equal(kdl_diff, KDL::Twist::Zero(), dtwist)) return false; + if(!KDL::Equal(kdl_diff, KDL::Twist::Zero(), dtwist_)) return false; } continue; } @@ -327,8 +327,8 @@ bool Problem::checkSolutionActiveVariables(const std::vector& tip_frames, default: { double dmax = DBL_MAX; - dmax = std::fmin(dmax, dpos); - dmax = std::fmin(dmax, dtwist); + dmax = std::fmin(dmax, dpos_); + dmax = std::fmin(dmax, dtwist_); double d = computeGoalFitness(goal, tip_frames.data(), active_variable_positions); if(!(d < dmax * dmax)) return false; } diff --git a/src/problem.h b/src/problem.h index 86741911..679f37a0 100644 --- a/src/problem.h +++ b/src/problem.h @@ -34,17 +34,17 @@ #pragma once -#include "utils.h" #include +#include "utils.h" #include +#include #include #include #include -#include namespace bio_ik { @@ -52,16 +52,16 @@ namespace bio_ik class Problem { private: - bool ros_params_initrd; - std::vector joint_usage; - std::vector link_tip_indices; - std::vector minimal_displacement_factors; - std::vector joint_transmission_goal_temp, joint_transmission_goal_temp2; - moveit::core::RobotModelConstPtr robot_model; - const moveit::core::JointModelGroup* joint_model_group; - IKParams params; - RobotInfo modelInfo; - double dpos, drot, dtwist; + bool ros_params_initrd_; + std::vector joint_usage_; + std::vector link_tip_indices_; + std::vector minimal_displacement_factors_; + std::vector joint_transmission_goal_temp_, joint_transmission_goal_temp2_; + moveit::core::RobotModelConstPtr robot_model_; + const moveit::core::JointModelGroup* joint_model_group_; + IKParams params_; + RobotInfo modelInfo_; + double dpos_, drot_, dtwist_; #if (MOVEIT_FCL_VERSION < FCL_VERSION_CHECK(0, 6, 0)) struct CollisionShape { @@ -83,7 +83,7 @@ class Problem { } }; - std::vector collision_links; + std::vector collision_links_; #endif size_t addTipLink(const moveit::core::LinkModel* link_model); diff --git a/src/utils.h b/src/utils.h index 43b5cad6..c0ffc49c 100644 --- a/src/utils.h +++ b/src/utils.h @@ -46,11 +46,6 @@ #include #include - -//#include - -//#include -//#include #include namespace bio_ik @@ -64,8 +59,8 @@ struct IKParams // IKParallel parameters std::string solver_class_name; bool enable_counter; - int thread_count; - int random_seed; + size_t thread_count; + uint64_t random_seed; //Problem parameters double dpos; @@ -74,8 +69,8 @@ struct IKParams // ik_evolution_1 parameters bool opt_no_wipeout; - int population_size; - int elite_count; + size_t population_size; + size_t elite_count; bool linear_fitness; }; @@ -96,7 +91,7 @@ template inline void vprint(std::ostream& s, const T& a, { s << a << " "; vprint(s, aa...); -}; +} #define LOG2(...) vprint(LOG_STREAM, "ikbio ", __VA_ARGS__) @@ -327,7 +322,7 @@ __attribute__((always_inline)) inline double clamp2(double v, double lo, double return v; } -__attribute__((always_inline)) inline double smoothstep(float a, float b, float v) +__attribute__((always_inline)) inline double smoothstep(double a, double b, double v) { v = clamp((v - a) / (b - a), 0.0, 1.0); return v * v * (3.0 - 2.0 * v); @@ -342,8 +337,8 @@ __attribute__((always_inline)) inline double sign(double f) template class linear_int_distribution { - std::uniform_int_distribution base; t n; + std::uniform_int_distribution base; public: inline linear_int_distribution(t vrange) @@ -403,6 +398,7 @@ template class Factory : type(typeid(void)) { } + virtual ~ClassBase() = default; }; typedef std::set MapType; static MapType& classes() @@ -415,25 +411,28 @@ template class Factory template struct Class : ClassBase { BASE* create(ARGS... args) const { return new DERIVED(args...); } - BASE* clone(const BASE* o) const { return new DERIVED(*(const DERIVED*)o); } - Class(const std::string& name) + BASE* clone(const BASE* o) const { return new DERIVED(*dynamic_cast(o)); } + Class(const std::string& class_name) { - this->name = name; + // TODO(wyattrees): is the member name "name" important or can change to "name_"? + this->name = class_name; this->type = typeid(DERIVED); classes().insert(this); } ~Class() { classes().erase(this); } }; - static BASE* create(const std::string& name, ARGS... args) + + static BASE* create(const std::string& class_name, ARGS... args) { + // TODO(wyattrees): is the member name "name" important or can change to "name_"? for(auto* f : classes()) - if(f->name == name) return f->create(args...); - ERROR("class not found", name); + if(f->name == class_name) return f->create(args...); + ERROR("class not found", class_name); } template static DERIVED* clone(const DERIVED* o) { for(auto* f : classes()) - if(f->type == typeid(*o)) return (DERIVED*)f->clone(o); + if(f->type == typeid(*o)) return dynamic_cast(f->clone(o)); ERROR("class not found", typeid(*o).name()); } }; @@ -448,13 +447,13 @@ template struct aligned_allocator : public std::allocator typedef T& reference; typedef const T& const_reference; typedef T value_type; - T* allocate(size_t s, const void* hint = 0) + T* allocate(size_t s, const void* /*hint*/ = 0) { void* p; if(posix_memalign(&p, A, sizeof(T) * s + 64)) throw std::bad_alloc(); - return (T*)p; + return static_cast(p); } - void deallocate(T* ptr, size_t s) { free(ptr); } + void deallocate(T* ptr, size_t /*unused*/) { free(ptr); } template struct rebind { typedef aligned_allocator other;