Copy IFOPT code locally to allow customization and add nodes variable set

trajopt_ifopt/CMakeLists.txt

@@ -9,124 +9,42 @@ if(WIN32)
   set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
 endif()
 
-find_package(trajopt_common)
-find_package(console_bridge REQUIRED)
-find_package(Eigen3 REQUIRED)
-find_package(ifopt REQUIRED)
-find_package(tesseract_environment REQUIRED)
-find_package(ros_industrial_cmake_boilerplate REQUIRED)
-find_package(Boost REQUIRED)
-
-if(NOT TARGET console_bridge::console_bridge)
-  add_library(console_bridge::console_bridge INTERFACE IMPORTED)
-  set_target_properties(console_bridge::console_bridge PROPERTIES INTERFACE_INCLUDE_DIRECTORIES
-                                                                  ${console_bridge_INCLUDE_DIRS})
-  set_target_properties(console_bridge::console_bridge PROPERTIES INTERFACE_LINK_LIBRARIES ${console_bridge_LIBRARIES})
-else()
-  get_target_property(CHECK_INCLUDE_DIRECTORIES console_bridge::console_bridge INTERFACE_INCLUDE_DIRECTORIES)
-  if(NOT ${CHECK_INCLUDE_DIRECTORIES})
-    set_target_properties(console_bridge::console_bridge PROPERTIES INTERFACE_INCLUDE_DIRECTORIES
-                                                                    ${console_bridge_INCLUDE_DIRS})
-  endif()
-endif()
-
 # Load variable for clang tidy args, compiler options and cxx version
+find_package(trajopt_common REQUIRED)
 trajopt_variables()
 
 # ######################################################################################################################
 # Build ##
 # ######################################################################################################################
 
-set(TRAJOPT_IFOPT_SOURCE_FILES
-    src/costs/absolute_cost.cpp
-    src/costs/squared_cost.cpp
-    src/constraints/cartesian_position_constraint.cpp
-    src/constraints/inverse_kinematics_constraint.cpp
-    src/constraints/joint_acceleration_constraint.cpp
-    src/constraints/joint_jerk_constraint.cpp
-    src/constraints/joint_position_constraint.cpp
-    src/constraints/joint_velocity_constraint.cpp
-    src/constraints/cartesian_line_constraint.cpp
-    src/constraints/collision/discrete_collision_evaluators.cpp
-    src/constraints/collision/continuous_collision_evaluators.cpp
-    src/constraints/collision/weighted_average_methods.cpp
-    src/constraints/collision/discrete_collision_constraint.cpp
-    src/constraints/collision/discrete_collision_numerical_constraint.cpp
-    src/constraints/collision/continuous_collision_constraint.cpp
-    src/constraints/collision/continuous_collision_numerical_constraint.cpp
-    src/utils/ifopt_utils.cpp
-    src/utils/numeric_differentiation.cpp
-    src/utils/trajopt_utils.cpp
-    src/variable_sets/joint_position_variable.cpp)
+set(SUPPORTED_COMPONENTS ifopt)
+add_subdirectory(ifopt)
 
-add_library(${PROJECT_NAME} ${TRAJOPT_IFOPT_SOURCE_FILES})
-target_link_libraries(
-  ${PROJECT_NAME}
-  PUBLIC console_bridge::console_bridge
-         ifopt::ifopt_core
-         trajopt::trajopt_common
-         tesseract::tesseract_environment
-         Eigen3::Eigen
-         Boost::boost)
-target_compile_options(${PROJECT_NAME} PRIVATE ${TRAJOPT_COMPILE_OPTIONS_PRIVATE})
-target_compile_options(${PROJECT_NAME} PUBLIC ${TRAJOPT_COMPILE_OPTIONS_PUBLIC})
-target_compile_definitions(${PROJECT_NAME} PUBLIC ${TRAJOPT_COMPILE_DEFINITIONS})
-target_cxx_version(${PROJECT_NAME} PUBLIC VERSION ${TRAJOPT_CXX_VERSION})
-target_clang_tidy(${PROJECT_NAME} ENABLE ${TRAJOPT_ENABLE_CLANG_TIDY})
-target_include_directories(${PROJECT_NAME} PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
-                                                  "$<INSTALL_INTERFACE:include>")
-
-# ######################################################################################################################
-# Install ##
-# ######################################################################################################################
+# Simple
+list(APPEND SUPPORTED_COMPONENTS trajopt)
+add_subdirectory(trajopt)
 
-configure_package(NAMESPACE trajopt TARGETS ${PROJECT_NAME})
-
-# Mark cpp header files for installation
-install(
-  DIRECTORY include/${PROJECT_NAME}
-  DESTINATION include
-  FILES_MATCHING
-  PATTERN "*.h"
-  PATTERN "*.hpp"
-  PATTERN ".svn" EXCLUDE)
-
-# ######################################################################################################################
-# Testing ##
-# ######################################################################################################################
-
-if(TRAJOPT_ENABLE_TESTING)
-  enable_testing()
-  add_custom_target(run_tests)
-  add_subdirectory(test)
-endif()
+# Package configuration
+configure_package(
+  COMPONENT
+  ifopt
+  SUPPORTED_COMPONENTS
+  ${SUPPORTED_COMPONENTS})
 
 if(TRAJOPT_PACKAGE)
-  cpack(
+  cpack_component_package(
     VERSION
     ${pkg_extracted_version}
-    MAINTAINER
-    <https://github.com/tesseract-robotics/trajopt>
-    VENDOR
-    "ROS-Industrial"
+    MAINTAINER_NAME
+    ${pkg_extracted_maintainer_name}
+    MAINTAINER_EMAIL
+    ${pkg_extracted_maintainer_email}
     DESCRIPTION
     ${pkg_extracted_description}
+    LICENSE_FILE
+    ${CMAKE_CURRENT_LIST_DIR}/../LICENSE
     README_FILE
     ${CMAKE_CURRENT_LIST_DIR}/../README.md
-    PACKAGE_PREFIX
-    ${TRAJOPT_PACKAGE_PREFIX}
-    LINUX_DEPENDS
-    "libeigen3-dev"
-    "libboost-dev"
-    "libconsole-bridge-dev"
-    "ifopt"
-    "${TRAJOPT_PACKAGE_PREFIX}trajopt-utils"
-    "${TESSERACT_PACKAGE_PREFIX}tesseract-environment"
-    WINDOWS_DEPENDS
-    "Eigen3"
-    "boost"
-    "console-bridge"
-    "ifopt"
-    "${TRAJOPT_PACKAGE_PREFIX}trajopt-utils"
-    "${TESSERACT_PACKAGE_PREFIX}tesseract-environment")
+    COMPONENT_DEPENDS
+    ${SUPPORTED_COMPONENTS})
 endif()

trajopt_ifopt/ifopt/CMakeLists.txt

@@ -0,0 +1,44 @@
+find_package(Eigen3 REQUIRED)
+
+# ######################################################################################################################
+# Build ##
+# ######################################################################################################################
+add_library(${PROJECT_NAME}_ifopt src/problem.cc src/composite.cc src/leaves.cc)
+target_include_directories(${PROJECT_NAME}_ifopt PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+                                                        $<INSTALL_INTERFACE:include>)
+target_link_libraries(${PROJECT_NAME}_ifopt PUBLIC Eigen3::Eigen)
+# better specify features you need, not c++ version
+target_compile_features(${PROJECT_NAME}_ifopt PUBLIC cxx_range_for cxx_auto_type)
+
+# Configure Components
+configure_component(
+  COMPONENT
+  ifopt
+  NAMESPACE
+  trajopt
+  TARGETS
+  ${PROJECT_NAME}_ifopt
+  DEPENDENCIES
+  Eigen3)
+
+# Mark cpp header files for installation
+install(
+  DIRECTORY include/${PROJECT_NAME}
+  DESTINATION include
+  FILES_MATCHING
+  PATTERN "*.h"
+  PATTERN "*.hpp"
+  PATTERN ".svn" EXCLUDE)
+
+# ######################################################################################################################
+# Testing ##
+# ######################################################################################################################
+if(TRAJOPT_ENABLE_TESTING)
+  enable_testing()
+  find_package(GTest QUIET)
+
+  add_executable(${PROJECT_NAME}_ifopt_test test/composite_test.cc test/problem_test.cc)
+  target_include_directories(${PROJECT_NAME}_ifopt_test PRIVATE $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/test>)
+  target_link_libraries(${PROJECT_NAME}_ifopt_test PRIVATE ${PROJECT_NAME}_ifopt GTest::GTest GTest::Main)
+  add_test(${PROJECT_NAME}_ifopt_test ${PROJECT_NAME}_ifopt_test)
+endif()

trajopt_ifopt/ifopt/include/trajopt_ifopt/ifopt/bounds.h

@@ -0,0 +1,75 @@
+/******************************************************************************
+Copyright (c) 2017, Alexander W Winkler. All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+******************************************************************************/
+
+#ifndef TRAJOPT_IFOPT_IFOPT_BOUNDS_H_
+#define TRAJOPT_IFOPT_IFOPT_BOUNDS_H_
+
+namespace ifopt
+{
+/**
+ * @brief Upper and lower bound for optimization variables and constraints.
+ */
+struct Bounds
+{
+  /**
+   * @brief Creates a bound between @a lower and @a upper.
+   */
+  Bounds(double lower = 0.0, double upper = 0.0)
+  {
+    lower_ = lower;
+    upper_ = upper;
+  }
+
+  double lower_;
+  double upper_;
+
+  void operator+=(double scalar)
+  {
+    lower_ += scalar;
+    upper_ += scalar;
+  }
+
+  void operator-=(double scalar)
+  {
+    lower_ -= scalar;
+    upper_ -= scalar;
+  }
+};
+
+// settings this as signals infinity for IPOPT/SNOPT solvers
+static const double inf = 1.0e20;
+
+static const Bounds NoBound = Bounds(-inf, +inf);
+static const Bounds BoundZero = Bounds(0.0, 0.0);
+static const Bounds BoundGreaterZero = Bounds(0.0, +inf);
+static const Bounds BoundSmallerZero = Bounds(-inf, 0.0);
+
+}  // namespace ifopt
+
+#endif /* TRAJOPT_IFOPT_IFOPT_BOUNDS_H_ */