From 09e3c69e05e04399e6952418437b323d0829783d Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Fri, 26 Apr 2024 16:25:57 -0500 Subject: [PATCH 1/5] Copy IFOPT source code into trajopt_ifopt --- trajopt_ifopt/CMakeLists.txt | 124 ++------ .../cmake/trajopt_ifopt-config.cmake.in | 31 -- trajopt_ifopt/ifopt/CMakeLists.txt | 44 +++ .../include/trajopt_ifopt/ifopt/bounds.h | 75 +++++ .../include/trajopt_ifopt/ifopt/composite.h | 234 +++++++++++++++ .../trajopt_ifopt/ifopt/constraint_set.h | 137 +++++++++ .../include/trajopt_ifopt/ifopt/cost_term.h | 77 +++++ .../include/trajopt_ifopt/ifopt/problem.h | 281 ++++++++++++++++++ .../include/trajopt_ifopt/ifopt/solver.h | 72 +++++ .../trajopt_ifopt/ifopt/variable_set.h | 63 ++++ trajopt_ifopt/ifopt/src/composite.cc | 195 ++++++++++++ trajopt_ifopt/ifopt/src/leaves.cc | 100 +++++++ trajopt_ifopt/ifopt/src/problem.cc | 169 +++++++++++ trajopt_ifopt/ifopt/test/composite_test.cc | 127 ++++++++ .../ifopt/test/ifopt/test_vars_constr_cost.h | 174 +++++++++++ trajopt_ifopt/ifopt/test/problem_test.cc | 188 ++++++++++++ trajopt_ifopt/trajopt/CMakeLists.txt | 122 ++++++++ .../constraints/cartesian_line_constraint.h | 4 +- .../cartesian_position_constraint.h | 4 +- .../continuous_collision_constraint.h | 6 +- .../continuous_collision_evaluators.h | 6 +- ...ontinuous_collision_numerical_constraint.h | 6 +- .../collision/discrete_collision_constraint.h | 4 +- .../collision/discrete_collision_evaluators.h | 6 +- .../discrete_collision_numerical_constraint.h | 6 +- .../collision/weighted_average_methods.h | 6 +- .../inverse_kinematics_constraint.h | 4 +- .../joint_acceleration_constraint.h | 4 +- .../constraints/joint_jerk_constraint.h | 4 +- .../constraints/joint_position_constraint.h | 4 +- .../constraints/joint_velocity_constraint.h | 4 +- .../trajopt}/costs/absolute_cost.h | 6 +- .../trajopt}/costs/squared_cost.h | 4 +- .../include/trajopt_ifopt/trajopt}/fwd.h | 6 +- .../trajopt}/utils/ifopt_utils.h | 4 +- .../trajopt}/utils/numeric_differentiation.h | 4 +- .../trajopt}/utils/trajopt_utils.h | 4 +- .../variable_sets/joint_position_variable.h | 4 +- .../constraints/cartesian_line_constraint.cpp | 8 +- .../cartesian_position_constraint.cpp | 8 +- .../continuous_collision_constraint.cpp | 8 +- .../continuous_collision_evaluators.cpp | 2 +- ...tinuous_collision_numerical_constraint.cpp | 8 +- .../discrete_collision_constraint.cpp | 10 +- .../discrete_collision_evaluators.cpp | 2 +- ...iscrete_collision_numerical_constraint.cpp | 10 +- .../collision/weighted_average_methods.cpp | 2 +- .../inverse_kinematics_constraint.cpp | 4 +- .../joint_acceleration_constraint.cpp | 4 +- .../src/constraints/joint_jerk_constraint.cpp | 4 +- .../constraints/joint_position_constraint.cpp | 4 +- .../constraints/joint_velocity_constraint.cpp | 4 +- .../{ => trajopt}/src/costs/absolute_cost.cpp | 4 +- .../{ => trajopt}/src/costs/squared_cost.cpp | 4 +- .../{ => trajopt}/src/utils/ifopt_utils.cpp | 2 +- .../src/utils/numeric_differentiation.cpp | 2 +- .../{ => trajopt}/src/utils/trajopt_utils.cpp | 4 +- .../variable_sets/joint_position_variable.cpp | 4 +- .../{ => trajopt}/test/CMakeLists.txt | 2 +- .../test/cartesian_line_constraint_unit.cpp | 6 +- .../cartesian_position_constraint_unit.cpp | 6 +- .../{ => trajopt}/test/cast_cost_unit.cpp | 10 +- .../{ => trajopt}/test/collision_unit.cpp | 6 +- .../continuous_collision_gradient_unit.cpp | 12 +- .../{ => trajopt}/test/cost_wrappers_unit.cpp | 8 +- .../test/discrete_collision_gradient_unit.cpp | 18 +- .../inverse_kinematics_constraint_unit.cpp | 6 +- .../{ => trajopt}/test/joint_terms_unit.cpp | 12 +- .../test/simple_collision_unit.cpp | 16 +- .../{ => trajopt}/test/utils_unit.cpp | 2 +- .../{ => trajopt}/test/variable_sets_unit.cpp | 2 +- trajopt_optimizers/trajopt_sqp/CMakeLists.txt | 4 +- .../callbacks/cartesian_error_plotter.h | 2 +- .../trajopt_sqp/callbacks/collision_plotter.h | 2 +- .../callbacks/joint_state_plotter.h | 2 +- .../src/callbacks/cartesian_error_plotter.cpp | 2 +- .../src/callbacks/collision_plotter.cpp | 2 +- .../src/callbacks/joint_state_plotter.cpp | 4 +- .../trajopt_sqp/src/ifopt_qp_problem.cpp | 6 +- .../trajopt_sqp/src/trajopt_qp_problem.cpp | 2 +- .../trajopt_sqp/test/CMakeLists.txt | 2 +- .../test/benchmarks/CMakeLists.txt | 2 +- .../test/benchmarks/solve_benchmarks.cpp | 20 +- .../test/cart_position_optimization_unit.cpp | 8 +- .../test/cast_cost_attached_unit.cpp | 10 +- .../test/cast_cost_octomap_unit.cpp | 10 +- .../trajopt_sqp/test/cast_cost_unit.cpp | 10 +- .../trajopt_sqp/test/cast_cost_world_unit.cpp | 10 +- .../joint_acceleration_optimization_unit.cpp | 8 +- .../test/joint_jerk_optimization_unit.cpp | 8 +- .../test/joint_position_optimization_unit.cpp | 4 +- .../test/joint_velocity_optimization_unit.cpp | 8 +- .../trajopt_sqp/test/numerical_ik_unit.cpp | 10 +- .../trajopt_sqp/test/planning_unit.cpp | 12 +- .../test/simple_collision_unit.cpp | 16 +- 95 files changed, 2313 insertions(+), 368 deletions(-) delete mode 100644 trajopt_ifopt/cmake/trajopt_ifopt-config.cmake.in create mode 100644 trajopt_ifopt/ifopt/CMakeLists.txt create mode 100644 trajopt_ifopt/ifopt/include/trajopt_ifopt/ifopt/bounds.h create mode 100644 trajopt_ifopt/ifopt/include/trajopt_ifopt/ifopt/composite.h create mode 100644 trajopt_ifopt/ifopt/include/trajopt_ifopt/ifopt/constraint_set.h create mode 100644 trajopt_ifopt/ifopt/include/trajopt_ifopt/ifopt/cost_term.h create mode 100644 trajopt_ifopt/ifopt/include/trajopt_ifopt/ifopt/problem.h create mode 100644 trajopt_ifopt/ifopt/include/trajopt_ifopt/ifopt/solver.h create mode 100644 trajopt_ifopt/ifopt/include/trajopt_ifopt/ifopt/variable_set.h create mode 100644 trajopt_ifopt/ifopt/src/composite.cc create mode 100644 trajopt_ifopt/ifopt/src/leaves.cc create mode 100644 trajopt_ifopt/ifopt/src/problem.cc create mode 100644 trajopt_ifopt/ifopt/test/composite_test.cc create mode 100644 trajopt_ifopt/ifopt/test/ifopt/test_vars_constr_cost.h create mode 100644 trajopt_ifopt/ifopt/test/problem_test.cc create mode 100644 trajopt_ifopt/trajopt/CMakeLists.txt rename trajopt_ifopt/{include/trajopt_ifopt => trajopt/include/trajopt_ifopt/trajopt}/constraints/cartesian_line_constraint.h (98%) rename trajopt_ifopt/{include/trajopt_ifopt => trajopt/include/trajopt_ifopt/trajopt}/constraints/cartesian_position_constraint.h (98%) rename trajopt_ifopt/{include/trajopt_ifopt => trajopt/include/trajopt_ifopt/trajopt}/constraints/collision/continuous_collision_constraint.h (96%) rename trajopt_ifopt/{include/trajopt_ifopt => trajopt/include/trajopt_ifopt/trajopt}/constraints/collision/continuous_collision_evaluators.h (97%) rename trajopt_ifopt/{include/trajopt_ifopt => trajopt/include/trajopt_ifopt/trajopt}/constraints/collision/continuous_collision_numerical_constraint.h (95%) rename trajopt_ifopt/{include/trajopt_ifopt => trajopt/include/trajopt_ifopt/trajopt}/constraints/collision/discrete_collision_constraint.h (97%) rename trajopt_ifopt/{include/trajopt_ifopt => trajopt/include/trajopt_ifopt/trajopt}/constraints/collision/discrete_collision_evaluators.h (96%) rename trajopt_ifopt/{include/trajopt_ifopt => trajopt/include/trajopt_ifopt/trajopt}/constraints/collision/discrete_collision_numerical_constraint.h (95%) rename trajopt_ifopt/{include/trajopt_ifopt => trajopt/include/trajopt_ifopt/trajopt}/constraints/collision/weighted_average_methods.h (88%) rename trajopt_ifopt/{include/trajopt_ifopt => trajopt/include/trajopt_ifopt/trajopt}/constraints/inverse_kinematics_constraint.h (98%) rename trajopt_ifopt/{include/trajopt_ifopt => trajopt/include/trajopt_ifopt/trajopt}/constraints/joint_acceleration_constraint.h (97%) rename trajopt_ifopt/{include/trajopt_ifopt => trajopt/include/trajopt_ifopt/trajopt}/constraints/joint_jerk_constraint.h (97%) rename trajopt_ifopt/{include/trajopt_ifopt => trajopt/include/trajopt_ifopt/trajopt}/constraints/joint_position_constraint.h (97%) rename trajopt_ifopt/{include/trajopt_ifopt => trajopt/include/trajopt_ifopt/trajopt}/constraints/joint_velocity_constraint.h (97%) rename trajopt_ifopt/{include/trajopt_ifopt => trajopt/include/trajopt_ifopt/trajopt}/costs/absolute_cost.h (95%) rename trajopt_ifopt/{include/trajopt_ifopt => trajopt/include/trajopt_ifopt/trajopt}/costs/squared_cost.h (97%) rename trajopt_ifopt/{include/trajopt_ifopt => trajopt/include/trajopt_ifopt/trajopt}/fwd.h (93%) rename trajopt_ifopt/{include/trajopt_ifopt => trajopt/include/trajopt_ifopt/trajopt}/utils/ifopt_utils.h (98%) rename trajopt_ifopt/{include/trajopt_ifopt => trajopt/include/trajopt_ifopt/trajopt}/utils/numeric_differentiation.h (97%) rename trajopt_ifopt/{include/trajopt_ifopt => trajopt/include/trajopt_ifopt/trajopt}/utils/trajopt_utils.h (95%) rename trajopt_ifopt/{include/trajopt_ifopt => trajopt/include/trajopt_ifopt/trajopt}/variable_sets/joint_position_variable.h (96%) rename trajopt_ifopt/{ => trajopt}/src/constraints/cartesian_line_constraint.cpp (98%) rename trajopt_ifopt/{ => trajopt}/src/constraints/cartesian_position_constraint.cpp (98%) rename trajopt_ifopt/{ => trajopt}/src/constraints/collision/continuous_collision_constraint.cpp (95%) rename trajopt_ifopt/{ => trajopt}/src/constraints/collision/continuous_collision_evaluators.cpp (99%) rename trajopt_ifopt/{ => trajopt}/src/constraints/collision/continuous_collision_numerical_constraint.cpp (95%) rename trajopt_ifopt/{ => trajopt}/src/constraints/collision/discrete_collision_constraint.cpp (93%) rename trajopt_ifopt/{ => trajopt}/src/constraints/collision/discrete_collision_evaluators.cpp (99%) rename trajopt_ifopt/{ => trajopt}/src/constraints/collision/discrete_collision_numerical_constraint.cpp (94%) rename trajopt_ifopt/{ => trajopt}/src/constraints/collision/weighted_average_methods.cpp (97%) rename trajopt_ifopt/{ => trajopt}/src/constraints/inverse_kinematics_constraint.cpp (97%) rename trajopt_ifopt/{ => trajopt}/src/constraints/joint_acceleration_constraint.cpp (97%) rename trajopt_ifopt/{ => trajopt}/src/constraints/joint_jerk_constraint.cpp (97%) rename trajopt_ifopt/{ => trajopt}/src/constraints/joint_position_constraint.cpp (97%) rename trajopt_ifopt/{ => trajopt}/src/constraints/joint_velocity_constraint.cpp (97%) rename trajopt_ifopt/{ => trajopt}/src/costs/absolute_cost.cpp (96%) rename trajopt_ifopt/{ => trajopt}/src/costs/squared_cost.cpp (95%) rename trajopt_ifopt/{ => trajopt}/src/utils/ifopt_utils.cpp (99%) rename trajopt_ifopt/{ => trajopt}/src/utils/numeric_differentiation.cpp (95%) rename trajopt_ifopt/{ => trajopt}/src/utils/trajopt_utils.cpp (93%) rename trajopt_ifopt/{ => trajopt}/src/variable_sets/joint_position_variable.cpp (96%) rename trajopt_ifopt/{ => trajopt}/test/CMakeLists.txt (99%) rename trajopt_ifopt/{ => trajopt}/test/cartesian_line_constraint_unit.cpp (97%) rename trajopt_ifopt/{ => trajopt}/test/cartesian_position_constraint_unit.cpp (97%) rename trajopt_ifopt/{ => trajopt}/test/cast_cost_unit.cpp (95%) rename trajopt_ifopt/{ => trajopt}/test/collision_unit.cpp (96%) rename trajopt_ifopt/{ => trajopt}/test/continuous_collision_gradient_unit.cpp (92%) rename trajopt_ifopt/{ => trajopt}/test/cost_wrappers_unit.cpp (97%) rename trajopt_ifopt/{ => trajopt}/test/discrete_collision_gradient_unit.cpp (87%) rename trajopt_ifopt/{ => trajopt}/test/inverse_kinematics_constraint_unit.cpp (97%) rename trajopt_ifopt/{ => trajopt}/test/joint_terms_unit.cpp (97%) rename trajopt_ifopt/{ => trajopt}/test/simple_collision_unit.cpp (90%) rename trajopt_ifopt/{ => trajopt}/test/utils_unit.cpp (99%) rename trajopt_ifopt/{ => trajopt}/test/variable_sets_unit.cpp (97%) diff --git a/trajopt_ifopt/CMakeLists.txt b/trajopt_ifopt/CMakeLists.txt index ce04396d..163fc160 100644 --- a/trajopt_ifopt/CMakeLists.txt +++ b/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 "$" - "$") - -# ###################################################################################################################### -# 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 - - 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() diff --git a/trajopt_ifopt/cmake/trajopt_ifopt-config.cmake.in b/trajopt_ifopt/cmake/trajopt_ifopt-config.cmake.in deleted file mode 100644 index 4f7d1800..00000000 --- a/trajopt_ifopt/cmake/trajopt_ifopt-config.cmake.in +++ /dev/null @@ -1,31 +0,0 @@ -@PACKAGE_INIT@ - -set(@PROJECT_NAME@_FOUND ON) -set_and_check(@PROJECT_NAME@_INCLUDE_DIRS "${PACKAGE_PREFIX_DIR}/include") -set_and_check(@PROJECT_NAME@_LIBRARY_DIRS "${PACKAGE_PREFIX_DIR}/lib") - -include(CMakeFindDependencyMacro) - -find_dependency(console_bridge) -find_dependency(ifopt) -find_dependency(Eigen3) -find_dependency(trajopt_common) -find_dependency(tesseract_environment) -if(${CMAKE_VERSION} VERSION_LESS "3.15.0") - find_package(Boost REQUIRED) -else() - find_dependency(Boost) -endif() - -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() - -include("${CMAKE_CURRENT_LIST_DIR}/@PROJECT_NAME@-targets.cmake") diff --git a/trajopt_ifopt/ifopt/CMakeLists.txt b/trajopt_ifopt/ifopt/CMakeLists.txt new file mode 100644 index 00000000..4fe610b6 --- /dev/null +++ b/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 $ + $) +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 $) + 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() diff --git a/trajopt_ifopt/ifopt/include/trajopt_ifopt/ifopt/bounds.h b/trajopt_ifopt/ifopt/include/trajopt_ifopt/ifopt/bounds.h new file mode 100644 index 00000000..2bbd9235 --- /dev/null +++ b/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_ */ diff --git a/trajopt_ifopt/ifopt/include/trajopt_ifopt/ifopt/composite.h b/trajopt_ifopt/ifopt/include/trajopt_ifopt/ifopt/composite.h new file mode 100644 index 00000000..d08bef47 --- /dev/null +++ b/trajopt_ifopt/ifopt/include/trajopt_ifopt/ifopt/composite.h @@ -0,0 +1,234 @@ +/****************************************************************************** +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. +******************************************************************************/ + +/** + * @file composite.h + * @brief Declares the classes Composite and Component used as variables, + * costs and constraints. + */ + +#ifndef TRAJOPT_IFOPT_IFOPT_COMPOSITE_H_ +#define TRAJOPT_IFOPT_IFOPT_COMPOSITE_H_ + +#include +#include +#include + +#include +#include + +#include "bounds.h" + +namespace ifopt +{ +/** + * @brief Interface representing either Variable, Cost or Constraint. + * + * Variables, costs and constraints can all be fit into the same interface + * (Component). For example, each has a "value", which is either the actual + * value of the variables, the constraint value g or the cost. + * This representation provides one common interface + * ("smallest common denominator") that can be contain either individual + * variables/costs/constraints or a Composite of these. This pattern + * takes care of stacking variables, ordering Jacobians and providing bounds + * for the complete problem without duplicating code. For more information on + * the composite pattern visit https://sourcemaking.com/design_patterns/composite + */ +class Component +{ +public: + using Ptr = std::shared_ptr; + + using Jacobian = Eigen::SparseMatrix; + using VectorXd = Eigen::VectorXd; + using VecBound = std::vector; + + /** + * @brief Creates a component. + * @param num_rows The number of rows of this components. + * @param name The identifier for this component. + * + * The number of rows @c num_rows can represent either + * @li number of variables in this variables set + * @li number of constraints in this constraint set + * @li 1 if this component represents a Cost. + */ + Component(int num_rows, const std::string& name); + virtual ~Component() = default; + + /** + * @brief Returns the "values" of whatever this component represents. + * + * @li For Variable this represents the actual optimization values. + * @li For Constraint this represents the constraint value g. + * @li For Cost this represents the cost value. + */ + virtual VectorXd GetValues() const = 0; + + /** + * @brief Returns the "bounds" of this component. + * + * @li For Variable these are the upper and lower variable bound. + * @li For Constraint this represents the constraint bounds. + * @li For Cost these done't exists (set to infinity). + */ + virtual VecBound GetBounds() const = 0; + + /** + * @brief Sets the optimization variables from an Eigen vector. + * + * This is only done for Variable, where these are set from the current + * values of the @ref solvers. + */ + virtual void SetVariables(const VectorXd& x) = 0; + + /** + * @brief Returns derivatives of each row w.r.t. the variables + * + * @li For Constraint this is a matrix with one row per constraint. + * @li For a Cost this is a row vector (gradient transpose). + * @li Not sensible for Variable. + */ + virtual Jacobian GetJacobian() const = 0; + + /** + * @brief Returns the number of rows of this component. + */ + int GetRows() const; + + /** + * @brief Returns the name (id) of this component. + */ + std::string GetName() const; + + /** + * @brief Prints the relevant information (name, rows, values) of this component. + * @param tolerance When to flag constraint/bound violation. + * @param index_start Of this specific variables-, constraint- or cost set. + */ + virtual void Print(double tolerance, int& index_start) const; + + /** + * @brief Sets the number of rows of this component. + * + * @attention This should correctly be done through constructor call, only + * delay this by using @c kSpecifyLater if you have good reason. + */ + void SetRows(int num_rows); + static const int kSpecifyLater = -1; + +private: + int num_rows_ = kSpecifyLater; + std::string name_; +}; + +/** + * @brief A collection of components which is treated as another Component. + * + * This class follows the Component interface as well, but doesn't actually + * do any evaluation, but only stitches together the results of the + * components it is holding. This is where multiple sets of variables, + * constraints or costs are ordered and combined. + * + * See Component and Composite Pattern for more information. + */ +class Composite : public Component +{ +public: + using Ptr = std::shared_ptr; + using ComponentVec = std::vector; + + /** + * @brief Creates a Composite holding either variables, costs or constraints. + * @param is_cost True if this class holds cost terms, false for all others. + * + * Constraints and variables append individual constraint sets and Jacobian + * rows below one another, whereas costs terms are all accumulated to a + * scalar value/a single Jacobian row. + */ + Composite(const std::string& name, bool is_cost); + virtual ~Composite() = default; + + // see Component for documentation + VectorXd GetValues() const override; + Jacobian GetJacobian() const override; + VecBound GetBounds() const override; + void SetVariables(const VectorXd& x) override; + void PrintAll() const; + + /** + * @brief Access generic component with the specified name. + * @param name The name given to the component. + * @return A generic pointer of that component. + */ + const Component::Ptr GetComponent(std::string name) const; + + /** + * @brief Access type-casted component with the specified name. + * @param name The name given to the component. + * @tparam T Type of component. + * @return A type-casted pointer possibly providing addtional functionality. + */ + template + std::shared_ptr GetComponent(const std::string& name) const; + + /** + * @brief Adds a component to this composite. + */ + void AddComponent(const Component::Ptr&); + + /** + * @brief Removes all component from this composite. + */ + void ClearComponents(); + + /** + * @brief Returns read access to the components. + */ + const ComponentVec GetComponents() const; + +private: + ComponentVec components_; + bool is_cost_; + // The number of variables for costs/constraint composites (not set for variables). + // Is initialized the first the GetJacobian() is called. + mutable size_t n_var = -1; +}; + +// implementation of template functions +template +std::shared_ptr Composite::GetComponent(const std::string& name) const +{ + Component::Ptr c = GetComponent(name); + return std::dynamic_pointer_cast(c); +} + +} // namespace ifopt + +#endif /* TRAJOPT_IFOPT_IFOPT_COMPOSITE_H_ */ diff --git a/trajopt_ifopt/ifopt/include/trajopt_ifopt/ifopt/constraint_set.h b/trajopt_ifopt/ifopt/include/trajopt_ifopt/ifopt/constraint_set.h new file mode 100644 index 00000000..364d839d --- /dev/null +++ b/trajopt_ifopt/ifopt/include/trajopt_ifopt/ifopt/constraint_set.h @@ -0,0 +1,137 @@ +/****************************************************************************** +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_CONSTRAINT_SET_H_ +#define TRAJOPT_IFOPT_IFOPT_CONSTRAINT_SET_H_ + +#include "composite.h" + +namespace ifopt +{ +/** + * @brief A container holding a set of related constraints. + * + * This container holds constraints representing a single concept, e.g. + * @c n constraints keeping a foot inside its range of motion. Each of the + * @c n rows is given by: + * lower_bound < g(x) < upper_bound + * + * These constraint sets are later then stitched together to form the overall + * problem. + * + * @ingroup ProblemFormulation + * @sa Component + */ +class ConstraintSet : public Component +{ +public: + using Ptr = std::shared_ptr; + using VariablesPtr = Composite::Ptr; + + /** + * @brief Creates constraints on the variables @c x. + * @param n_constraints The number of constraints. + * @param name What these constraints represent. + */ + ConstraintSet(int n_constraints, const std::string& name); + virtual ~ConstraintSet() = default; + + /** + * @brief Connects the constraint with the optimization variables. + * @param x A pointer to the current values of the optimization variables. + * + * The optimization variable values are necessary for calculating constraint + * violations and Jacobians. + */ + void LinkWithVariables(const VariablesPtr& x); + + /** + * @brief The matrix of derivatives for these constraints and variables. + * + * Assuming @c n constraints and @c m variables, the returned Jacobian + * has dimensions n x m. Every row represents the derivatives of a single + * constraint, whereas every column refers to a single optimization variable. + * + * This function only combines the user-defined jacobians from + * FillJacobianBlock(). + */ + Jacobian GetJacobian() const final; + + /** + * @brief Set individual Jacobians corresponding to each decision variable set. + * @param var_set Set of variables the current Jacobian block belongs to. + * @param jac_block Columns of the overall Jacobian affected by var_set. + * + * A convenience function so the user does not have to worry about the + * ordering of variable sets. All that is required is that the user knows + * the internal ordering of variables in each individual set and provides + * the Jacobian of the constraints w.r.t. this set (starting at column 0). + * GetJacobian() then inserts these columns at the correct position in the + * overall Jacobian. + * + * If the constraint doen't depend on a @c var_set, this function should + * simply do nothing. + * + * Attention: @c jac_bock is a sparse matrix, and must always have the same + * sparsity pattern. Therefore, it's better not to build a dense matrix and + * call .sparseView(), because if some entries happen to be zero for some + * iteration, that changes the sparsity pattern. A more robust way is to directly + * set as follows (which can also be set =0.0 without erros): + * jac_block.coeffRef(1, 3) = ... + */ + virtual void FillJacobianBlock(std::string var_set, Jacobian& jac_block) const = 0; + +protected: + /** + * @brief Read access to the value of the optimization variables. + * + * This must be used to formulate the constraint violation and Jacobian. + */ + const VariablesPtr GetVariables() const { return variables_; }; + +private: + VariablesPtr variables_; + + /** + * @brief Initialize quantities that depend on the optimization variables. + * @param x A pointer to the initial values of the optimization variables. + * + * Sometimes the number of constraints depends on the variable representation, + * or shorthands to specific variable sets want to be saved for quicker + * access later. This function can be overwritten for that. + */ + virtual void InitVariableDependedQuantities(const VariablesPtr& /*x_init*/){}; + + // doesn't exist for constraints, generated run-time error when used + void SetVariables(const VectorXd& /*x*/) final { assert(false); }; +}; + +} // namespace ifopt + +#endif /* TRAJOPT_IFOPT_IFOPT_CONSTRAINT_SET_H_ */ diff --git a/trajopt_ifopt/ifopt/include/trajopt_ifopt/ifopt/cost_term.h b/trajopt_ifopt/ifopt/include/trajopt_ifopt/ifopt/cost_term.h new file mode 100644 index 00000000..271b57e5 --- /dev/null +++ b/trajopt_ifopt/ifopt/include/trajopt_ifopt/ifopt/cost_term.h @@ -0,0 +1,77 @@ +/****************************************************************************** +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_COST_TERM_H_ +#define TRAJOPT_IFOPT_IFOPT_COST_TERM_H_ + +#include "constraint_set.h" + +namespace ifopt +{ +/** + * @brief A container holding a single cost term. + * + * This container builds a scalar cost term from the values of the variables. + * This can be seen as a constraint with only one row and no bounds. + * + * @ingroup ProblemFormulation + * @sa Component + */ +class CostTerm : public ConstraintSet +{ +public: + CostTerm(const std::string& name); + virtual ~CostTerm() = default; + +private: + /** + * @brief Returns the scalar cost term calculated from the @c variables. + */ + virtual double GetCost() const = 0; + +public: + /** + * @brief Wrapper function that converts double to Eigen::VectorXd. + */ + VectorXd GetValues() const final; + + /** + * @brief Returns infinite bounds (e.g. no bounds). + */ + VecBound GetBounds() const final; + + /** + * Cost term printout slightly different from variables/constraints. + */ + void Print(double tol, int& index) const final; +}; + +} // namespace ifopt + +#endif /* TRAJOPT_IFOPT_IFOPT_COST_TERM_H_ */ diff --git a/trajopt_ifopt/ifopt/include/trajopt_ifopt/ifopt/problem.h b/trajopt_ifopt/ifopt/include/trajopt_ifopt/ifopt/problem.h new file mode 100644 index 00000000..0f437780 --- /dev/null +++ b/trajopt_ifopt/ifopt/include/trajopt_ifopt/ifopt/problem.h @@ -0,0 +1,281 @@ +/****************************************************************************** +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_PROBLEM_H_ +#define TRAJOPT_IFOPT_IFOPT_PROBLEM_H_ + +#include "constraint_set.h" +#include "cost_term.h" +#include "variable_set.h" + +/** + * @brief common namespace for all elements in this library. + */ +namespace ifopt +{ +/** + * @defgroup ProblemFormulation + * @brief The elements to formulate the solver independent optimization problem. + * + * An optimization problem usually consists of multiple sets of independent + * variable- or constraint-sets. Each set represents a common concept, e.g. one + * set of variables represents spline coefficients, another footstep positions. + * Similarly, each constraint-set groups a set of similar constraints. + * + * The Nonlinear Optimization Problem to solve is defined as: + * + * find x0, x1 (variable-sets 0 & 1) + * s.t + * x0_lower <= x0 <= x0_upper (bounds on variable-set x0 \in R^2) + * + * {x0,x1} = arg min c0(x0,x1)+c1(x0,x1) (cost-terms 0 and 1) + * + * g0_lower < g0(x0,x1) < g0_upper (constraint-set 0 \in R^2) + * g1_lower < g1(x0,x1) < g0_upper (constraint-set 1 \in R^1) + * + * + * #### GetValues() + * This structure allows a user to define each of these sets independently in + * separate classes and ifopt takes care of building the overall problem from + * these sets. This is implemented by + * * stacking all variable-sets to build the overall variable vector + * * summing all cost-terms to calculate the total cost + * * stacking all constraint-sets to build the overall constraint vector. + * + * #### GetJacobian() + * Supplying derivative information greatly increases solution speed. ifopt + * allows to define the derivative of each cost-term/constraint-set with + * respect to each variable-set independently. This ensures that when the + * order of variable-sets changes in the overall vector, this derivative + * information is still valid. These "Jacobian blocks" must be supplied through + * ConstraintSet::FillJacobianBlock() and are then used to build the complete Jacobian for + * the cost and constraints. + * + * \image html ifopt.png + */ + +/** + * @brief A generic optimization problem with variables, costs and constraints. + * + * This class is responsible for holding all the information of an optimization + * problem, which includes the optimization variables, their variable bounds, + * the cost function, the constraints and their bounds and derivatives of + * all. With this information the problem can be solved by any specific solver. + * All the quantities (variables, cost, constraint) are represented + * by the same generic Component class. + * + * @ingroup ProblemFormulation + * See @ref Solvers for currently implemented solvers. + */ +class Problem +{ +public: + using VecBound = Component::VecBound; + using Jacobian = Component::Jacobian; + using VectorXd = Component::VectorXd; + + /** + * @brief Creates a optimization problem with no variables, costs or constraints. + */ + Problem(); + virtual ~Problem() = default; + + /** + * @brief Add one individual set of variables to the optimization problem. + * @param variable_set The selection of optimization variables. + * + * This function can be called multiple times, with multiple sets, e.g. + * one set that parameterizes a body trajectory, the other that resembles + * the optimal timing values. This function correctly appends the + * individual variables sets and ensures correct order of Jacobian columns. + */ + void AddVariableSet(const VariableSet::Ptr& variable_set); + + /** + * @brief Add a set of multiple constraints to the optimization problem. + * @param constraint_set This can be 1 to infinity number of constraints. + * + * This function can be called multiple times for different sets of + * constraints. It makes sure the overall constraint and Jacobian correctly + * considers all individual constraint sets. + */ + void AddConstraintSet(const ConstraintSet::Ptr& constraint_set); + + /** + * @brief Add a cost term to the optimization problem. + * @param cost_set The calculation of the cost from the variables. + * + * This function can be called multiple times if the cost function is + * composed of different cost terms. It makes sure the overall value and + * gradient is considering each individual cost. + */ + void AddCostSet(const CostTerm::Ptr& cost_set); + + /** + * @brief Updates the variables with the values of the raw pointer @c x. + */ + void SetVariables(const double* x); + + /** + * @brief The number of optimization variables. + */ + int GetNumberOfOptimizationVariables() const; + + /** + * @brief True if the optimization problem includes a cost, false if + * merely a feasibility problem is defined. + */ + bool HasCostTerms() const; + + /** + * @brief The maximum and minimum value each optimization variable + * is allowed to have. + */ + VecBound GetBoundsOnOptimizationVariables() const; + + /** + * @brief The current value of the optimization variables. + */ + VectorXd GetVariableValues() const; + + /** + * @brief The scalar cost for current optimization variables @c x. + */ + double EvaluateCostFunction(const double* x); + + /** + * @brief The column-vector of derivatives of the cost w.r.t. each variable. + * @details ipopt uses 10e-8 for their derivative check, but setting here to more precise + * https://coin-or.github.io/Ipopt/OPTIONS.html#OPT_derivative_test_perturbation + */ + VectorXd EvaluateCostFunctionGradient(const double* x, + bool use_finite_difference_approximation = false, + double epsilon = std::numeric_limits::epsilon()); + + /** + * @brief The number of individual constraints. + */ + int GetNumberOfConstraints() const; + + /** + * @brief The upper and lower bound of each individual constraint. + */ + VecBound GetBoundsOnConstraints() const; + + /** + * @brief Each constraint value g(x) for current optimization variables @c x. + */ + VectorXd EvaluateConstraints(const double* x); + + /** + * @brief Extracts those entries from constraint Jacobian that are not zero. + * @param [in] x The current values of the optimization variables. + * @param [out] values The nonzero derivatives ordered by Eigen::RowMajor. + */ + void EvalNonzerosOfJacobian(const double* x, double* values); + + /** + * @brief The sparse-matrix representation of Jacobian of the constraints. + * + * Each row corresponds to a constraint and each column to an optimizaton + * variable. + */ + Jacobian GetJacobianOfConstraints() const; + + /** + * @brief The sparse-matrix representation of Jacobian of the costs. + * + * Returns one row corresponding to the costs and each column corresponding + * to an optimizaton variable. + */ + Jacobian GetJacobianOfCosts() const; + + /** + * @brief Saves the current values of the optimization variables in x_prev. + * + * This is used to keep a history of the values for each NLP iterations. + */ + void SaveCurrent(); + + /** + * @brief Read/write access to the current optimization variables. + */ + Composite::Ptr GetOptVariables() const; + + /** + * @brief Sets the optimization variables to those at iteration iter. + */ + void SetOptVariables(int iter); + + /** + * @brief Sets the optimization variables to those of the final iteration. + */ + void SetOptVariablesFinal(); + + /** + * @brief The number of iterations it took to solve the problem. + */ + int GetIterationCount() const { return x_prev.size(); }; + + /** + * @brief Prints the variables, costs and constraints. + */ + void PrintCurrent() const; + + /** + * @brief Read access to the constraints composite + * @return A const reference to constraints_ + */ + const Composite& GetConstraints() const { return constraints_; }; + + /** + * @brief Read access to the costs composite + * @return A const reference to costs_ + */ + const Composite& GetCosts() const { return costs_; }; + + /** + * @brief Read access to the history of iterations + * @return A const reference to x_prev + */ + const std::vector& GetIterations() const { return x_prev; }; + +private: + Composite::Ptr variables_; + Composite constraints_; + Composite costs_; + + std::vector x_prev; ///< the pure variables for every iteration. + + VectorXd ConvertToEigen(const double* x) const; +}; + +} // namespace ifopt + +#endif /* TRAJOPT_IFOPT_IFOPT_PROBLEM_H_ */ diff --git a/trajopt_ifopt/ifopt/include/trajopt_ifopt/ifopt/solver.h b/trajopt_ifopt/ifopt/include/trajopt_ifopt/ifopt/solver.h new file mode 100644 index 00000000..4f2d8ece --- /dev/null +++ b/trajopt_ifopt/ifopt/include/trajopt_ifopt/ifopt/solver.h @@ -0,0 +1,72 @@ +/****************************************************************************** +Copyright (c) 2017, Alexander W. Winkler, ETH Zurich. 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 ETH ZURICH 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 ETH ZURICH 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_SOLVER_H_ +#define TRAJOPT_IFOPT_IFOPT_SOLVER_H_ + +#include + +#include + +namespace ifopt +{ +/** + * @defgroup Solvers + * @brief Interfaces to IPOPT and SNOPT to solve the optimization problem. + * + * These are included in the folders: @ref ifopt_ipopt/ and @ref ifopt_snopt/. + */ + +/** + * @brief Solver interface implemented by IPOPT and SNOPT. + * + * @ingroup Solvers + */ +class Solver +{ +public: + using Ptr = std::shared_ptr; + + virtual ~Solver() = default; + + /** @brief Uses a specific solver (IPOPT, SNOPT) to solve the NLP. + * @param [in/out] nlp The nonlinear programming problem. + */ + virtual void Solve(Problem& nlp) = 0; + + /** @brief Get the return status for the optimization. + * + * e.g. https://coin-or.github.io/Ipopt/OUTPUT.html + */ + int GetReturnStatus() const { return status_; }; + +protected: + int status_; +}; + +} /* namespace ifopt */ + +#endif /* TRAJOPT_IFOPT_IFOPT_SOLVER_H_ */ diff --git a/trajopt_ifopt/ifopt/include/trajopt_ifopt/ifopt/variable_set.h b/trajopt_ifopt/ifopt/include/trajopt_ifopt/ifopt/variable_set.h new file mode 100644 index 00000000..bbae7f69 --- /dev/null +++ b/trajopt_ifopt/ifopt/include/trajopt_ifopt/ifopt/variable_set.h @@ -0,0 +1,63 @@ +/****************************************************************************** +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_VARIABLE_SET_H_ +#define TRAJOPT_IFOPT_IFOPT_VARIABLE_SET_H_ + +#include "composite.h" + +namespace ifopt +{ +/** + * @brief A container holding a set of related optimization variables. + * + * This is a single set of variables representing a single concept, e.g + * "spline coefficients" or "step durations". + * + * @ingroup ProblemFormulation + * @sa Component + */ +class VariableSet : public Component +{ +public: + /** + * @brief Creates a set of variables representing a single concept. + * @param n_var Number of variables. + * @param name What the variables represent to (e.g. "spline coefficients"). + */ + VariableSet(int n_var, const std::string& name); + virtual ~VariableSet() = default; + + // doesn't exist for variables, generated run-time error when used. + Jacobian GetJacobian() const final { throw std::runtime_error("not implemented for variables"); }; +}; + +} // namespace ifopt + +#endif /* TRAJOPT_IFOPT_IFOPT_VARIABLE_SET_H_ */ diff --git a/trajopt_ifopt/ifopt/src/composite.cc b/trajopt_ifopt/ifopt/src/composite.cc new file mode 100644 index 00000000..4ce68750 --- /dev/null +++ b/trajopt_ifopt/ifopt/src/composite.cc @@ -0,0 +1,195 @@ +/****************************************************************************** +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. +******************************************************************************/ + +#include + +#include +#include + +namespace ifopt +{ +Component::Component(int num_rows, const std::string& name) +{ + num_rows_ = num_rows; + name_ = name; +} + +int Component::GetRows() const { return num_rows_; } + +void Component::SetRows(int num_rows) { num_rows_ = num_rows; } + +std::string Component::GetName() const { return name_; } + +void Component::Print(double tol, int& index) const +{ + // calculate squared bound violation + VectorXd x = GetValues(); + VecBound bounds = GetBounds(); + + std::vector viol_idx; + for (std::size_t i = 0; i < bounds.size(); ++i) + { + double lower = bounds.at(i).lower_; + double upper = bounds.at(i).upper_; + double val = x(i); + if (val < lower - tol || upper + tol < val) + viol_idx.push_back(i); // constraint out of bounds + } + + std::string black = "\033[0m"; + std::string red = "\033[31m"; + std::string color = viol_idx.empty() ? black : red; + + std::cout.precision(2); + std::cout << std::fixed << std::left << std::setw(30) << name_ << std::right << std::setw(4) << num_rows_ + << std::setw(9) << index << std::setfill('.') << std::setw(7) << index + num_rows_ - 1 << std::setfill(' ') + << color << std::setw(12) << viol_idx.size() << black << std::endl; + + index += num_rows_; +} + +Composite::Composite(const std::string& name, bool is_cost) : Component(0, name) { is_cost_ = is_cost; } + +void Composite::AddComponent(const Component::Ptr& c) +{ + // at this point the number of rows must be specified. + assert(c->GetRows() != kSpecifyLater); + + components_.push_back(c); + + if (is_cost_) + SetRows(1); + else + SetRows(GetRows() + c->GetRows()); +} + +void Composite::ClearComponents() +{ + components_.clear(); + SetRows(0); +} + +const Component::Ptr Composite::GetComponent(std::string name) const +{ + for (const auto& c : components_) + if (c->GetName() == name) + return c; + + assert(false); // component with name doesn't exist, abort program + return Component::Ptr(); +} + +Composite::VectorXd Composite::GetValues() const +{ + VectorXd g_all = VectorXd::Zero(GetRows()); + + int row = 0; + for (const auto& c : components_) + { + int n_rows = c->GetRows(); + VectorXd g = c->GetValues(); + g_all.middleRows(row, n_rows) += g; + + if (!is_cost_) + row += n_rows; + } + return g_all; +} + +void Composite::SetVariables(const VectorXd& x) +{ + int row = 0; + for (auto& c : components_) + { + int n_rows = c->GetRows(); + c->SetVariables(x.middleRows(row, n_rows)); + row += n_rows; + } +} + +Composite::Jacobian Composite::GetJacobian() const +{ // set number of variables only the first time this function is called, + // since number doesn't change during the optimization. Improves efficiency. + if (n_var == -1) + n_var = components_.empty() ? 0 : components_.front()->GetJacobian().cols(); + + Jacobian jacobian(GetRows(), n_var); + + if (n_var == 0) + return jacobian; + + int row = 0; + std::vector> triplet_list; + + for (const auto& c : components_) + { + const Jacobian& jac = c->GetJacobian(); + triplet_list.reserve(triplet_list.size() + jac.nonZeros()); + + for (int k = 0; k < jac.outerSize(); ++k) + for (Jacobian::InnerIterator it(jac, k); it; ++it) + triplet_list.push_back(Eigen::Triplet(row + it.row(), it.col(), it.value())); + + if (!is_cost_) + row += c->GetRows(); + } + + jacobian.setFromTriplets(triplet_list.begin(), triplet_list.end()); + return jacobian; +} + +Composite::VecBound Composite::GetBounds() const +{ + VecBound bounds_; + for (const auto& c : components_) + { + VecBound b = c->GetBounds(); + bounds_.insert(bounds_.end(), b.begin(), b.end()); + } + + return bounds_; +} + +const Composite::ComponentVec Composite::GetComponents() const { return components_; } + +void Composite::PrintAll() const +{ + int index = 0; + double tol = 0.001; ///< tolerance when printing out constraint/bound violation. + + std::cout << GetName() << ":\n"; + for (auto c : components_) + { + std::cout << " "; // indent components + c->Print(tol, index); + } + std::cout << std::endl; +} + +} // namespace ifopt diff --git a/trajopt_ifopt/ifopt/src/leaves.cc b/trajopt_ifopt/ifopt/src/leaves.cc new file mode 100644 index 00000000..4c7137be --- /dev/null +++ b/trajopt_ifopt/ifopt/src/leaves.cc @@ -0,0 +1,100 @@ +/****************************************************************************** +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. +******************************************************************************/ + +#include +#include +#include + +#include +#include + +namespace ifopt +{ +VariableSet::VariableSet(int n_var, const std::string& name) : Component(n_var, name) {} + +ConstraintSet::ConstraintSet(int row_count, const std::string& name) : Component(row_count, name) {} + +ConstraintSet::Jacobian ConstraintSet::GetJacobian() const +{ + Jacobian jacobian(GetRows(), variables_->GetRows()); + + int col = 0; + Jacobian jac; + std::vector> triplet_list; + + for (const auto& vars : variables_->GetComponents()) + { + int n = vars->GetRows(); + jac.resize(GetRows(), n); + + FillJacobianBlock(vars->GetName(), jac); + // reserve space for the new elements to reduce memory allocation + triplet_list.reserve(triplet_list.size() + jac.nonZeros()); + + // create triplets for the derivative at the correct position in the overall Jacobian + for (int k = 0; k < jac.outerSize(); ++k) + for (Jacobian::InnerIterator it(jac, k); it; ++it) + triplet_list.push_back(Eigen::Triplet(it.row(), col + it.col(), it.value())); + col += n; + } + + // transform triplet vector into sparse matrix + jacobian.setFromTriplets(triplet_list.begin(), triplet_list.end()); + return jacobian; +} + +void ConstraintSet::LinkWithVariables(const VariablesPtr& x) +{ + variables_ = x; + InitVariableDependedQuantities(x); +} + +CostTerm::CostTerm(const std::string& name) : ConstraintSet(1, name) {} + +CostTerm::VectorXd CostTerm::GetValues() const +{ + VectorXd cost(1); + cost(0) = GetCost(); + return cost; +} + +CostTerm::VecBound CostTerm::GetBounds() const { return VecBound(GetRows(), NoBound); } + +void CostTerm::Print(double tol, int& index) const +{ + // only one scalar cost value + double cost = GetValues()(0); + + std::cout.precision(2); + std::cout << std::fixed << std::left << std::setw(30) << GetName() << std::right << std::setw(4) << GetRows() + << std::setw(9) << index << std::setfill('.') << std::setw(7) << index + GetRows() - 1 << std::setfill(' ') + << std::setw(12) << cost << std::endl; +} + +} // namespace ifopt diff --git a/trajopt_ifopt/ifopt/src/problem.cc b/trajopt_ifopt/ifopt/src/problem.cc new file mode 100644 index 00000000..b082b5f4 --- /dev/null +++ b/trajopt_ifopt/ifopt/src/problem.cc @@ -0,0 +1,169 @@ +/****************************************************************************** +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. +******************************************************************************/ + +#include +#include +#include + +namespace ifopt +{ +Problem::Problem() : constraints_("constraint-sets", false), costs_("cost-terms", true) +{ + variables_ = std::make_shared("variable-sets", false); +} + +void Problem::AddVariableSet(const VariableSet::Ptr& variable_set) { variables_->AddComponent(variable_set); } + +void Problem::AddConstraintSet(const ConstraintSet::Ptr& constraint_set) +{ + constraint_set->LinkWithVariables(variables_); + constraints_.AddComponent(constraint_set); +} + +void Problem::AddCostSet(const CostTerm::Ptr& cost_set) +{ + cost_set->LinkWithVariables(variables_); + costs_.AddComponent(cost_set); +} + +int Problem::GetNumberOfOptimizationVariables() const { return variables_->GetRows(); } + +Problem::VecBound Problem::GetBoundsOnOptimizationVariables() const { return variables_->GetBounds(); } + +Problem::VectorXd Problem::GetVariableValues() const { return variables_->GetValues(); } + +void Problem::SetVariables(const double* x) { variables_->SetVariables(ConvertToEigen(x)); } + +double Problem::EvaluateCostFunction(const double* x) +{ + VectorXd g = VectorXd::Zero(1); + if (HasCostTerms()) + { + SetVariables(x); + g = costs_.GetValues(); + } + return g(0); +} + +Problem::VectorXd Problem::EvaluateCostFunctionGradient(const double* x, + bool use_finite_difference_approximation, + double epsilon) +{ + int n = GetNumberOfOptimizationVariables(); + Jacobian jac = Jacobian(1, n); + if (HasCostTerms()) + { + if (use_finite_difference_approximation) + { + double step_size = epsilon; + + // calculate forward difference by disturbing each optimization variable + double g = EvaluateCostFunction(x); + std::vector x_new(x, x + n); + for (int i = 0; i < n; ++i) + { + x_new[i] += step_size; // disturb + double g_new = EvaluateCostFunction(x_new.data()); + jac.coeffRef(0, i) = (g_new - g) / step_size; + x_new[i] = x[i]; // reset for next iteration + } + } + else + { + SetVariables(x); + jac = costs_.GetJacobian(); + } + } + + return jac.row(0).transpose(); +} + +Problem::VecBound Problem::GetBoundsOnConstraints() const { return constraints_.GetBounds(); } + +int Problem::GetNumberOfConstraints() const { return GetBoundsOnConstraints().size(); } + +Problem::VectorXd Problem::EvaluateConstraints(const double* x) +{ + SetVariables(x); + return constraints_.GetValues(); +} + +bool Problem::HasCostTerms() const { return costs_.GetRows() > 0; } + +void Problem::EvalNonzerosOfJacobian(const double* x, double* values) +{ + SetVariables(x); + Jacobian jac = GetJacobianOfConstraints(); + + jac.makeCompressed(); // so the valuePtr() is dense and accurate + std::copy(jac.valuePtr(), jac.valuePtr() + jac.nonZeros(), values); +} + +Problem::Jacobian Problem::GetJacobianOfConstraints() const { return constraints_.GetJacobian(); } + +Problem::Jacobian Problem::GetJacobianOfCosts() const { return costs_.GetJacobian(); } + +void Problem::SaveCurrent() { x_prev.push_back(variables_->GetValues()); } + +Composite::Ptr Problem::GetOptVariables() const { return variables_; } + +void Problem::SetOptVariables(int iter) { variables_->SetVariables(x_prev.at(iter)); } + +void Problem::SetOptVariablesFinal() { variables_->SetVariables(x_prev.at(GetIterationCount() - 1)); } + +void Problem::PrintCurrent() const +{ + using namespace std; + cout << "\n" + << "************************************************************\n" + << " IFOPT - Interface to Nonlinear Optimizers (v2.0)\n" + << " \u00a9 Alexander W. Winkler\n" + << " https://github.com/ethz-adrl/ifopt\n" + << "************************************************************" + << "\n" + << "Legend:\n" + << "c - number of variables, constraints or cost terms" << std::endl + << "i - indices of this set in overall problem" << std::endl + << "v - number of [violated variable- or constraint-bounds] or [cost " + "term value]" + << "\n\n" + << std::right << std::setw(33) << "" << std::setw(5) << "c " << std::setw(16) << "i " << std::setw(11) + << "v " << std::left << "\n"; + + variables_->PrintAll(); + constraints_.PrintAll(); + costs_.PrintAll(); +}; + +Problem::VectorXd Problem::ConvertToEigen(const double* x) const +{ + return Eigen::Map(x, GetNumberOfOptimizationVariables()); +} + +} // namespace ifopt diff --git a/trajopt_ifopt/ifopt/test/composite_test.cc b/trajopt_ifopt/ifopt/test/composite_test.cc new file mode 100644 index 00000000..7f8d65d8 --- /dev/null +++ b/trajopt_ifopt/ifopt/test/composite_test.cc @@ -0,0 +1,127 @@ +/****************************************************************************** +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. +******************************************************************************/ + +#include +#include + +namespace ifopt +{ +class ExComponent : public Component +{ +public: + ExComponent(int n_var, const std::string& name) : Component(n_var, name){}; + + virtual VectorXd GetValues() const { throw std::runtime_error("not implemented"); }; + virtual VecBound GetBounds() const { throw std::runtime_error("not implemented"); }; + virtual Jacobian GetJacobian() const { throw std::runtime_error("not implemented"); }; + virtual void SetVariables(const VectorXd& x){}; +}; + +} // namespace ifopt + +using namespace ifopt; + +TEST(Component, GetRows) +{ + ExComponent c(2, "ex_component"); + EXPECT_EQ(2, c.GetRows()); + + c.SetRows(4); + EXPECT_EQ(4, c.GetRows()); +} + +TEST(Component, GetName) +{ + ExComponent c(2, "ex_component"); + EXPECT_STREQ("ex_component", c.GetName().c_str()); +} + +TEST(Composite, GetRowsCost) +{ + auto c1 = std::make_shared(0, "component1"); + auto c2 = std::make_shared(1, "component2"); + auto c3 = std::make_shared(2, "component3"); + + Composite cost("cost", true); + cost.AddComponent(c1); + cost.AddComponent(c2); + cost.AddComponent(c3); + EXPECT_EQ(1, cost.GetRows()); +} + +TEST(Composite, GetRowsConstraint) +{ + auto c1 = std::make_shared(0, "component1"); + auto c2 = std::make_shared(1, "component2"); + auto c3 = std::make_shared(2, "component3"); + + Composite constraint("constraint", false); + constraint.AddComponent(c1); + constraint.AddComponent(c2); + constraint.AddComponent(c3); + EXPECT_EQ(0 + 1 + 2, constraint.GetRows()); +} + +TEST(Composite, GetComponent) +{ + auto c1 = std::make_shared(0, "component1"); + auto c2 = std::make_shared(1, "component2"); + auto c3 = std::make_shared(2, "component3"); + + Composite comp("composite", false); + comp.AddComponent(c1); + comp.AddComponent(c2); + comp.AddComponent(c3); + + auto c1_new = comp.GetComponent("component1"); + EXPECT_EQ(c1->GetRows(), c1_new->GetRows()); + + auto c2_new = comp.GetComponent("component2"); + EXPECT_EQ(c2->GetRows(), c2_new->GetRows()); + + auto c3_new = comp.GetComponent("component3"); + EXPECT_NE(c1->GetRows(), c3_new->GetRows()); +} + +TEST(Composite, ClearComponents) +{ + auto c1 = std::make_shared(0, "component1"); + auto c2 = std::make_shared(1, "component2"); + auto c3 = std::make_shared(2, "component3"); + + Composite comp("composite", false); + comp.AddComponent(c1); + comp.AddComponent(c2); + comp.AddComponent(c3); + + EXPECT_EQ(0 + 1 + 2, comp.GetRows()); + + comp.ClearComponents(); + EXPECT_EQ(0, comp.GetRows()); +} diff --git a/trajopt_ifopt/ifopt/test/ifopt/test_vars_constr_cost.h b/trajopt_ifopt/ifopt/test/ifopt/test_vars_constr_cost.h new file mode 100644 index 00000000..3aebeda3 --- /dev/null +++ b/trajopt_ifopt/ifopt/test/ifopt/test_vars_constr_cost.h @@ -0,0 +1,174 @@ +/****************************************************************************** +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. +******************************************************************************/ + +/** + * @file test_vars_constr_cost.h + * + * @brief Example to generate a solver-independent formulation for the problem, taken + * from the IPOPT cpp_example. + * + * The example problem to be solved is given as: + * + * min_x f(x) = -(x1-2)^2 + * s.t. + * 0 = x0^2 + x1 - 1 + * -1 <= x0 <= 1 + * + * In this simple example we only use one set of variables, constraints and + * cost. However, most real world problems have multiple different constraints + * and also different variable sets representing different quantities. This + * framework allows to define each set of variables or constraints absolutely + * independently from another and correctly stitches them together to form the + * final optimization problem. + * + * For a helpful graphical overview, see: + * http://docs.ros.org/api/ifopt/html/group__ProblemFormulation.html + */ + +#include +#include +#include + +namespace ifopt +{ +using Eigen::Vector2d; + +class ExVariables : public VariableSet +{ +public: + // Every variable set has a name, here "var_set1". this allows the constraints + // and costs to define values and Jacobians specifically w.r.t this variable set. + ExVariables() : ExVariables("var_set1") {} + ExVariables(const std::string& name) : VariableSet(2, name) + { + // the initial values where the NLP starts iterating from + x0_ = 3.5; + x1_ = 1.5; + } + + // Here is where you can transform the Eigen::Vector into whatever + // internal representation of your variables you have (here two doubles, but + // can also be complex classes such as splines, etc.. + void SetVariables(const VectorXd& x) override + { + x0_ = x(0); + x1_ = x(1); + } + + // Here is the reverse transformation from the internal representation to + // to the Eigen::Vector + VectorXd GetValues() const override { return Vector2d(x0_, x1_); } + + // Each variable has an upper and lower bound set here + VecBound GetBounds() const override + { + VecBound bounds(GetRows()); + bounds.at(0) = Bounds(-1.0, 1.0); + bounds.at(1) = NoBound; + return bounds; + } + +private: + double x0_, x1_; +}; + +class ExConstraint : public ConstraintSet +{ +public: + ExConstraint() : ExConstraint("constraint1") {} + + // This constraint set just contains 1 constraint, however generally + // each set can contain multiple related constraints. + ExConstraint(const std::string& name) : ConstraintSet(1, name) {} + + // The constraint value minus the constant value "1", moved to bounds. + VectorXd GetValues() const override + { + VectorXd g(GetRows()); + Vector2d x = GetVariables()->GetComponent("var_set1")->GetValues(); + g(0) = std::pow(x(0), 2) + x(1); + return g; + } + + // The only constraint in this set is an equality constraint to 1. + // Constant values should always be put into GetBounds(), not GetValues(). + // For inequality constraints (<,>), use Bounds(x, inf) or Bounds(-inf, x). + VecBound GetBounds() const override + { + VecBound b(GetRows()); + b.at(0) = Bounds(1.0, 1.0); + return b; + } + + // This function provides the first derivative of the constraints. + // In case this is too difficult to write, you can also tell the solvers to + // approximate the derivatives by finite differences and not overwrite this + // function, e.g. in ipopt.cc::use_jacobian_approximation_ = true + // Attention: see the parent class function for important information on sparsity pattern. + void FillJacobianBlock(std::string var_set, Jacobian& jac_block) const override + { + // must fill only that submatrix of the overall Jacobian that relates + // to this constraint and "var_set1". even if more constraints or variables + // classes are added, this submatrix will always start at row 0 and column 0, + // thereby being independent from the overall problem. + if (var_set == "var_set1") + { + Vector2d x = GetVariables()->GetComponent("var_set1")->GetValues(); + + jac_block.coeffRef(0, 0) = 2.0 * x(0); // derivative of first constraint w.r.t x0 + jac_block.coeffRef(0, 1) = 1.0; // derivative of first constraint w.r.t x1 + } + } +}; + +class ExCost : public CostTerm +{ +public: + ExCost() : ExCost("cost_term1") {} + ExCost(const std::string& name) : CostTerm(name) {} + + double GetCost() const override + { + Vector2d x = GetVariables()->GetComponent("var_set1")->GetValues(); + return -std::pow(x(1) - 2, 2); + } + + void FillJacobianBlock(std::string var_set, Jacobian& jac) const override + { + if (var_set == "var_set1") + { + Vector2d x = GetVariables()->GetComponent("var_set1")->GetValues(); + + jac.coeffRef(0, 0) = 0.0; // derivative of cost w.r.t x0 + jac.coeffRef(0, 1) = -2.0 * (x(1) - 2.0); // derivative of cost w.r.t x1 + } + } +}; + +} // namespace ifopt diff --git a/trajopt_ifopt/ifopt/test/problem_test.cc b/trajopt_ifopt/ifopt/test/problem_test.cc new file mode 100644 index 00000000..fd8f7b51 --- /dev/null +++ b/trajopt_ifopt/ifopt/test/problem_test.cc @@ -0,0 +1,188 @@ +/****************************************************************************** +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. +******************************************************************************/ + +#include + +#include +#include + +using namespace ifopt; + +TEST(Problem, GetNumberOfOptimizationVariables) +{ + Problem nlp; + nlp.AddVariableSet(std::make_shared("var_set0")); + nlp.AddVariableSet(std::make_shared("var_set1")); + + EXPECT_EQ(2 + 2, nlp.GetNumberOfOptimizationVariables()); +} + +TEST(Problem, GetBoundsOnOptimizationVariables) +{ + Problem nlp; + nlp.AddVariableSet(std::make_shared("var_set0")); + nlp.AddVariableSet(std::make_shared("var_set1")); + + auto bounds = nlp.GetBoundsOnOptimizationVariables(); + EXPECT_EQ(2 + 2, bounds.size()); + + // var_set0 + EXPECT_DOUBLE_EQ(-1.0, bounds.at(0).lower_); + EXPECT_DOUBLE_EQ(+1.0, bounds.at(0).upper_); + EXPECT_DOUBLE_EQ(-inf, bounds.at(1).lower_); + EXPECT_DOUBLE_EQ(+inf, bounds.at(1).upper_); + + // var_set1 + EXPECT_DOUBLE_EQ(-1.0, bounds.at(2).lower_); + EXPECT_DOUBLE_EQ(+1.0, bounds.at(2).upper_); + EXPECT_DOUBLE_EQ(-inf, bounds.at(3).lower_); + EXPECT_DOUBLE_EQ(+inf, bounds.at(3).upper_); +} + +TEST(Problem, GetVariableValues) +{ + auto var_set0 = std::make_shared("var_set0"); + var_set0->SetVariables(Eigen::Vector2d(0.1, 0.2)); + + auto var_set1 = std::make_shared("var_set1"); + var_set1->SetVariables(Eigen::Vector2d(0.3, 0.4)); + + Problem nlp; + nlp.AddVariableSet(var_set0); + nlp.AddVariableSet(var_set1); + + Eigen::VectorXd x = nlp.GetVariableValues(); + EXPECT_EQ(0.1, x(0)); + EXPECT_EQ(0.2, x(1)); + EXPECT_EQ(0.3, x(2)); + EXPECT_EQ(0.4, x(3)); +} + +TEST(Problem, GetNumberOfConstraints) +{ + Problem nlp; + nlp.AddConstraintSet(std::make_shared("constraint1")); + + // add same constraints again for testing. + // notice how the Jacobian calculation inside ExConstraint-class remains the + // same - the full Jacobian is stitched together accordingly. + nlp.AddConstraintSet(std::make_shared("constraint2")); + + EXPECT_EQ(1 + 1, nlp.GetNumberOfConstraints()); +} + +TEST(Problem, GetBoundsOnConstraints) +{ + Problem nlp; + nlp.AddConstraintSet(std::make_shared("constraint1")); + nlp.AddConstraintSet(std::make_shared("constraint2")); + + auto bounds = nlp.GetBoundsOnConstraints(); + // since it's an equality contraint, upper and lower bound are equal + EXPECT_DOUBLE_EQ(1.0, bounds.at(0).lower_); + EXPECT_DOUBLE_EQ(1.0, bounds.at(0).upper_); + EXPECT_DOUBLE_EQ(1.0, bounds.at(1).lower_); + EXPECT_DOUBLE_EQ(1.0, bounds.at(1).upper_); +} + +TEST(Problem, EvaluateConstraints) +{ + Problem nlp; + nlp.AddVariableSet(std::make_shared()); + nlp.AddConstraintSet(std::make_shared("constraint1")); + nlp.AddConstraintSet(std::make_shared("constraint2")); + + double x[2] = { 2.0, 3.0 }; + Eigen::VectorXd g = nlp.EvaluateConstraints(x); + EXPECT_DOUBLE_EQ(2 * 2.0 + 3.0, g(0)); // constant -1 moved to bounds + EXPECT_DOUBLE_EQ(2 * 2.0 + 3.0, g(1)); // constant -1 moved to bounds +} + +TEST(Problem, GetJacobianOfConstraints) +{ + Problem nlp; + nlp.AddVariableSet(std::make_shared()); + nlp.AddConstraintSet(std::make_shared("constraint1")); + nlp.AddConstraintSet(std::make_shared("constraint2")); + + double x[2] = { 2.0, 3.0 }; + nlp.SetVariables(x); + auto jac = nlp.GetJacobianOfConstraints(); + EXPECT_EQ(nlp.GetNumberOfConstraints(), jac.rows()); + EXPECT_EQ(nlp.GetNumberOfOptimizationVariables(), jac.cols()); + + EXPECT_DOUBLE_EQ(2 * x[0], jac.coeffRef(0, 0)); // constraint 1 w.r.t x0 + EXPECT_DOUBLE_EQ(1.0, jac.coeffRef(0, 1)); // constraint 1 w.r.t x1 + EXPECT_DOUBLE_EQ(2 * x[0], jac.coeffRef(1, 0)); // constraint 2 w.r.t x0 + EXPECT_DOUBLE_EQ(1.0, jac.coeffRef(1, 1)); // constraint 2 w.r.t x1 +} + +TEST(Problem, EvaluateCostFunction) +{ + Problem nlp; + nlp.AddVariableSet(std::make_shared()); + nlp.AddCostSet(std::make_shared("cost_term1")); + nlp.AddCostSet(std::make_shared("cost_term2")); + + EXPECT_TRUE(nlp.HasCostTerms()); + + double x[2] = { 2.0, 3.0 }; + EXPECT_DOUBLE_EQ(2 * (-std::pow(x[1] - 2.0, 2)), + nlp.EvaluateCostFunction(x)); // constant -1 moved to bounds +} + +TEST(Problem, HasCostTerms) +{ + Problem nlp; + EXPECT_FALSE(nlp.HasCostTerms()); + + nlp.AddVariableSet(std::make_shared()); + EXPECT_FALSE(nlp.HasCostTerms()); + + nlp.AddConstraintSet(std::make_shared()); + EXPECT_FALSE(nlp.HasCostTerms()); + + nlp.AddCostSet(std::make_shared()); + EXPECT_TRUE(nlp.HasCostTerms()); +} + +TEST(Problem, EvaluateCostFunctionGradient) +{ + Problem nlp; + nlp.AddVariableSet(std::make_shared()); + nlp.AddCostSet(std::make_shared("cost_term1")); + nlp.AddCostSet(std::make_shared("cost_term2")); + + double x[2] = { 2.0, 3.0 }; + Eigen::VectorXd grad = nlp.EvaluateCostFunctionGradient(x); + + EXPECT_EQ(nlp.GetNumberOfOptimizationVariables(), grad.rows()); + EXPECT_DOUBLE_EQ(0.0, grad(0)); // cost1+cost2 w.r.t x0 + EXPECT_DOUBLE_EQ(2 * (-2 * (x[1] - 2)), grad(1)); // cost1+cost2 w.r.t x1 +} diff --git a/trajopt_ifopt/trajopt/CMakeLists.txt b/trajopt_ifopt/trajopt/CMakeLists.txt new file mode 100644 index 00000000..01e16562 --- /dev/null +++ b/trajopt_ifopt/trajopt/CMakeLists.txt @@ -0,0 +1,122 @@ +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() + +# ###################################################################################################################### +# 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) + +add_library(${PROJECT_NAME}_trajopt ${TRAJOPT_IFOPT_SOURCE_FILES}) +target_link_libraries( + ${PROJECT_NAME}_trajopt + PUBLIC console_bridge::console_bridge + ifopt::ifopt_core + trajopt::trajopt_common + tesseract::tesseract_environment + Eigen3::Eigen + Boost::boost) +target_compile_options(${PROJECT_NAME}_trajopt PRIVATE ${TRAJOPT_COMPILE_OPTIONS_PRIVATE}) +target_compile_options(${PROJECT_NAME}_trajopt PUBLIC ${TRAJOPT_COMPILE_OPTIONS_PUBLIC}) +target_compile_definitions(${PROJECT_NAME}_trajopt PUBLIC ${TRAJOPT_COMPILE_DEFINITIONS}) +target_cxx_version(${PROJECT_NAME}_trajopt PUBLIC VERSION ${TRAJOPT_CXX_VERSION}) +target_clang_tidy(${PROJECT_NAME}_trajopt ENABLE ${TRAJOPT_ENABLE_CLANG_TIDY}) +target_include_directories(${PROJECT_NAME}_trajopt PUBLIC "$" + "$") + +# Configure Components +configure_component( + COMPONENT + trajopt + NAMESPACE + trajopt + TARGETS + ${PROJECT_NAME}_trajopt + DEPENDENCIES + trajopt_common + console_bridge + Eigen3 + ifopt + tesseract_environment + Boost) + +# 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() + +if(TRAJOPT_PACKAGE) + cpack_component( + COMPONENT + trajopt + VERSION + ${pkg_extracted_version} + DESCRIPTION + "TrajOpt Ifopt trajopt components" + # COMPONENT_DEPENDS ifopt + 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") +endif() diff --git a/trajopt_ifopt/include/trajopt_ifopt/constraints/cartesian_line_constraint.h b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/constraints/cartesian_line_constraint.h similarity index 98% rename from trajopt_ifopt/include/trajopt_ifopt/constraints/cartesian_line_constraint.h rename to trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/constraints/cartesian_line_constraint.h index 56f862da..a9d7a6a0 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/constraints/cartesian_line_constraint.h +++ b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/constraints/cartesian_line_constraint.h @@ -25,8 +25,8 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -#ifndef TRAJOPT_IFOPT_CARTESIAN_LINE_CONSTRAINT_H -#define TRAJOPT_IFOPT_CARTESIAN_LINE_CONSTRAINT_H +#ifndef TRAJOPT_IFOPT_TRAJOPT_CARTESIAN_LINE_CONSTRAINT_H +#define TRAJOPT_IFOPT_TRAJOPT_CARTESIAN_LINE_CONSTRAINT_H #include TRAJOPT_IGNORE_WARNINGS_PUSH diff --git a/trajopt_ifopt/include/trajopt_ifopt/constraints/cartesian_position_constraint.h b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/constraints/cartesian_position_constraint.h similarity index 98% rename from trajopt_ifopt/include/trajopt_ifopt/constraints/cartesian_position_constraint.h rename to trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/constraints/cartesian_position_constraint.h index 2b963813..abe2e4ca 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/constraints/cartesian_position_constraint.h +++ b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/constraints/cartesian_position_constraint.h @@ -24,8 +24,8 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -#ifndef TRAJOPT_IFOPT_CARTESIAN_POSITION_CONSTRAINT_H -#define TRAJOPT_IFOPT_CARTESIAN_POSITION_CONSTRAINT_H +#ifndef TRAJOPT_IFOPT_TRAJOPT_CARTESIAN_POSITION_CONSTRAINT_H +#define TRAJOPT_IFOPT_TRAJOPT_CARTESIAN_POSITION_CONSTRAINT_H #include TRAJOPT_IGNORE_WARNINGS_PUSH diff --git a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/continuous_collision_constraint.h b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/constraints/collision/continuous_collision_constraint.h similarity index 96% rename from trajopt_ifopt/include/trajopt_ifopt/constraints/collision/continuous_collision_constraint.h rename to trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/constraints/collision/continuous_collision_constraint.h index af91340f..6c600e85 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/continuous_collision_constraint.h +++ b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/constraints/collision/continuous_collision_constraint.h @@ -24,8 +24,8 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -#ifndef TRAJOPT_IFOPT_CONTINUOUS_COLLISION_CONSTRAINT_V2_H -#define TRAJOPT_IFOPT_CONTINUOUS_COLLISION_CONSTRAINT_V2_H +#ifndef TRAJOPT_IFOPT_TRAJOPT_CONTINUOUS_COLLISION_CONSTRAINT_V2_H +#define TRAJOPT_IFOPT_TRAJOPT_CONTINUOUS_COLLISION_CONSTRAINT_V2_H #include TRAJOPT_IGNORE_WARNINGS_PUSH @@ -118,4 +118,4 @@ class ContinuousCollisionConstraint : public ifopt::ConstraintSet }; } // namespace trajopt_ifopt -#endif // TRAJOPT_IFOPT_CONTINUOUS_COLLISION_CONSTRAINT_H +#endif // TRAJOPT_IFOPT_TRAJOPT_CONTINUOUS_COLLISION_CONSTRAINT_H diff --git a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/continuous_collision_evaluators.h b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/constraints/collision/continuous_collision_evaluators.h similarity index 97% rename from trajopt_ifopt/include/trajopt_ifopt/constraints/collision/continuous_collision_evaluators.h rename to trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/constraints/collision/continuous_collision_evaluators.h index 3908a6f7..279d90a1 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/continuous_collision_evaluators.h +++ b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/constraints/collision/continuous_collision_evaluators.h @@ -22,8 +22,8 @@ * limitations under the License. */ -#ifndef TRAJOPT_IFOPT_CONTINUOUS_COLLISION_EVALUATOR_H -#define TRAJOPT_IFOPT_CONTINUOUS_COLLISION_EVALUATOR_H +#ifndef TRAJOPT_IFOPT_TRAJOPT_CONTINUOUS_COLLISION_EVALUATOR_H +#define TRAJOPT_IFOPT_TRAJOPT_CONTINUOUS_COLLISION_EVALUATOR_H #include TRAJOPT_IGNORE_WARNINGS_PUSH @@ -204,4 +204,4 @@ class LVSDiscreteCollisionEvaluator : public ContinuousCollisionEvaluator const std::array& position_vars_fixed); }; } // namespace trajopt_ifopt -#endif // TRAJOPT_IFOPT_CONTINUOUS_COLLISION_EVALUATOR_H +#endif // TRAJOPT_IFOPT_TRAJOPT_CONTINUOUS_COLLISION_EVALUATOR_H diff --git a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/continuous_collision_numerical_constraint.h b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/constraints/collision/continuous_collision_numerical_constraint.h similarity index 95% rename from trajopt_ifopt/include/trajopt_ifopt/constraints/collision/continuous_collision_numerical_constraint.h rename to trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/constraints/collision/continuous_collision_numerical_constraint.h index bf8fc9aa..e6dbac52 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/continuous_collision_numerical_constraint.h +++ b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/constraints/collision/continuous_collision_numerical_constraint.h @@ -24,8 +24,8 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -#ifndef TRAJOPT_IFOPT_CONTINUOUS_COLLISION_NUMERICAL_CONSTRAINT_H -#define TRAJOPT_IFOPT_CONTINUOUS_COLLISION_NUMERICAL_CONSTRAINT_H +#ifndef TRAJOPT_IFOPT_TRAJOPT_CONTINUOUS_COLLISION_NUMERICAL_CONSTRAINT_H +#define TRAJOPT_IFOPT_TRAJOPT_CONTINUOUS_COLLISION_NUMERICAL_CONSTRAINT_H #include TRAJOPT_IGNORE_WARNINGS_PUSH #include @@ -116,4 +116,4 @@ class ContinuousCollisionNumericalConstraint : public ifopt::ConstraintSet std::shared_ptr collision_evaluator_; }; } // namespace trajopt_ifopt -#endif // TRAJOPT_IFOPT_CONTINUOUS_COLLISION_NUMERICAL_CONSTRAINT_H +#endif // TRAJOPT_IFOPT_TRAJOPT_CONTINUOUS_COLLISION_NUMERICAL_CONSTRAINT_H diff --git a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/discrete_collision_constraint.h b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/constraints/collision/discrete_collision_constraint.h similarity index 97% rename from trajopt_ifopt/include/trajopt_ifopt/constraints/collision/discrete_collision_constraint.h rename to trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/constraints/collision/discrete_collision_constraint.h index 13da1948..aa4a5233 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/discrete_collision_constraint.h +++ b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/constraints/collision/discrete_collision_constraint.h @@ -24,8 +24,8 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -#ifndef TRAJOPT_IFOPT_COLLISION_CONSTRAINT_V3_H -#define TRAJOPT_IFOPT_COLLISION_CONSTRAINT_V3_H +#ifndef TRAJOPT_IFOPT_TRAJOPT_COLLISION_CONSTRAINT_V3_H +#define TRAJOPT_IFOPT_TRAJOPT_COLLISION_CONSTRAINT_V3_H #include TRAJOPT_IGNORE_WARNINGS_PUSH diff --git a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/discrete_collision_evaluators.h b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/constraints/collision/discrete_collision_evaluators.h similarity index 96% rename from trajopt_ifopt/include/trajopt_ifopt/constraints/collision/discrete_collision_evaluators.h rename to trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/constraints/collision/discrete_collision_evaluators.h index a8f3fddf..51b77e27 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/discrete_collision_evaluators.h +++ b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/constraints/collision/discrete_collision_evaluators.h @@ -21,8 +21,8 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -#ifndef TRAJOPT_IFOPT_DISCRETE_COLLISION_EVALUATOR_H -#define TRAJOPT_IFOPT_DISCRETE_COLLISION_EVALUATOR_H +#ifndef TRAJOPT_IFOPT_TRAJOPT_DISCRETE_COLLISION_EVALUATOR_H +#define TRAJOPT_IFOPT_TRAJOPT_DISCRETE_COLLISION_EVALUATOR_H #include TRAJOPT_IGNORE_WARNINGS_PUSH @@ -133,4 +133,4 @@ class SingleTimestepCollisionEvaluator : public DiscreteCollisionEvaluator } // namespace trajopt_ifopt -#endif // TRAJOPT_IFOPT_DISCRETE_COLLISION_EVALUATOR_H +#endif // TRAJOPT_IFOPT_TRAJOPT_DISCRETE_COLLISION_EVALUATOR_H diff --git a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/discrete_collision_numerical_constraint.h b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/constraints/collision/discrete_collision_numerical_constraint.h similarity index 95% rename from trajopt_ifopt/include/trajopt_ifopt/constraints/collision/discrete_collision_numerical_constraint.h rename to trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/constraints/collision/discrete_collision_numerical_constraint.h index 9c18d822..96bbee2a 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/discrete_collision_numerical_constraint.h +++ b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/constraints/collision/discrete_collision_numerical_constraint.h @@ -24,8 +24,8 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -#ifndef TRAJOPT_IFOPT_DISCRETE_COLLISION_NUMERICAL_CONSTRAINT_H -#define TRAJOPT_IFOPT_DISCRETE_COLLISION_NUMERICAL_CONSTRAINT_H +#ifndef TRAJOPT_IFOPT_TRAJOPT_DISCRETE_COLLISION_NUMERICAL_CONSTRAINT_H +#define TRAJOPT_IFOPT_TRAJOPT_DISCRETE_COLLISION_NUMERICAL_CONSTRAINT_H #include TRAJOPT_IGNORE_WARNINGS_PUSH #include @@ -115,4 +115,4 @@ class DiscreteCollisionNumericalConstraint : public ifopt::ConstraintSet std::shared_ptr collision_evaluator_; }; } // namespace trajopt_ifopt -#endif // TRAJOPT_IFOPT_DISCRETE_COLLISION_NUMERICAL_CONSTRAINT_H +#endif // TRAJOPT_IFOPT_TRAJOPT_DISCRETE_COLLISION_NUMERICAL_CONSTRAINT_H diff --git a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/weighted_average_methods.h b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/constraints/collision/weighted_average_methods.h similarity index 88% rename from trajopt_ifopt/include/trajopt_ifopt/constraints/collision/weighted_average_methods.h rename to trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/constraints/collision/weighted_average_methods.h index 970d3096..3a3bb230 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/weighted_average_methods.h +++ b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/constraints/collision/weighted_average_methods.h @@ -21,8 +21,8 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -#ifndef TRAJOPT_IFOPT_COLLISION_WEIGHTED_AVERAGE_METHODS_H -#define TRAJOPT_IFOPT_COLLISION_WEIGHTED_AVERAGE_METHODS_H +#ifndef TRAJOPT_IFOPT_TRAJOPT_COLLISION_WEIGHTED_AVERAGE_METHODS_H +#define TRAJOPT_IFOPT_TRAJOPT_COLLISION_WEIGHTED_AVERAGE_METHODS_H #include TRAJOPT_IGNORE_WARNINGS_PUSH @@ -40,4 +40,4 @@ Eigen::VectorXd getWeightedAvgGradientT1(const trajopt_common::GradientResultsSe double max_error_with_buffer, Eigen::Index size); } // namespace trajopt_ifopt -#endif // TRAJOPT_IFOPT_COLLISION_WEIGHTED_AVERAGE_METHODS_H +#endif // TRAJOPT_IFOPT_TRAJOPT_COLLISION_WEIGHTED_AVERAGE_METHODS_H diff --git a/trajopt_ifopt/include/trajopt_ifopt/constraints/inverse_kinematics_constraint.h b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/constraints/inverse_kinematics_constraint.h similarity index 98% rename from trajopt_ifopt/include/trajopt_ifopt/constraints/inverse_kinematics_constraint.h rename to trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/constraints/inverse_kinematics_constraint.h index 05bb8dc8..9897a73e 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/constraints/inverse_kinematics_constraint.h +++ b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/constraints/inverse_kinematics_constraint.h @@ -23,8 +23,8 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -#ifndef TRAJOPT_IFOPT_INVERSE_KINEMATICS_CONSTRAINT_H -#define TRAJOPT_IFOPT_INVERSE_KINEMATICS_CONSTRAINT_H +#ifndef TRAJOPT_IFOPT_TRAJOPT_INVERSE_KINEMATICS_CONSTRAINT_H +#define TRAJOPT_IFOPT_TRAJOPT_INVERSE_KINEMATICS_CONSTRAINT_H #include TRAJOPT_IGNORE_WARNINGS_PUSH diff --git a/trajopt_ifopt/include/trajopt_ifopt/constraints/joint_acceleration_constraint.h b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/constraints/joint_acceleration_constraint.h similarity index 97% rename from trajopt_ifopt/include/trajopt_ifopt/constraints/joint_acceleration_constraint.h rename to trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/constraints/joint_acceleration_constraint.h index 714ea84e..d8d1d714 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/constraints/joint_acceleration_constraint.h +++ b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/constraints/joint_acceleration_constraint.h @@ -23,8 +23,8 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -#ifndef TRAJOPT_IFOPT_JOINT_JOINT_ACCELERATION_CONSTRAINT_H -#define TRAJOPT_IFOPT_JOINT_JOINT_ACCELERATION_CONSTRAINT_H +#ifndef TRAJOPT_IFOPT_TRAJOPT_JOINT_JOINT_ACCELERATION_CONSTRAINT_H +#define TRAJOPT_IFOPT_TRAJOPT_JOINT_JOINT_ACCELERATION_CONSTRAINT_H #include TRAJOPT_IGNORE_WARNINGS_PUSH diff --git a/trajopt_ifopt/include/trajopt_ifopt/constraints/joint_jerk_constraint.h b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/constraints/joint_jerk_constraint.h similarity index 97% rename from trajopt_ifopt/include/trajopt_ifopt/constraints/joint_jerk_constraint.h rename to trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/constraints/joint_jerk_constraint.h index b7df92cb..0ba4054c 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/constraints/joint_jerk_constraint.h +++ b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/constraints/joint_jerk_constraint.h @@ -23,8 +23,8 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -#ifndef TRAJOPT_IFOPT_JOINT_JOINT_JERK_CONSTRAINT_H -#define TRAJOPT_IFOPT_JOINT_JOINT_JERK_CONSTRAINT_H +#ifndef TRAJOPT_IFOPT_TRAJOPT_JOINT_JOINT_JERK_CONSTRAINT_H +#define TRAJOPT_IFOPT_TRAJOPT_JOINT_JOINT_JERK_CONSTRAINT_H #include TRAJOPT_IGNORE_WARNINGS_PUSH diff --git a/trajopt_ifopt/include/trajopt_ifopt/constraints/joint_position_constraint.h b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/constraints/joint_position_constraint.h similarity index 97% rename from trajopt_ifopt/include/trajopt_ifopt/constraints/joint_position_constraint.h rename to trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/constraints/joint_position_constraint.h index a5f16146..5eeacf7f 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/constraints/joint_position_constraint.h +++ b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/constraints/joint_position_constraint.h @@ -23,8 +23,8 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -#ifndef TRAJOPT_IFOPT_JOINT_POSITION_CONSTRAINT_H -#define TRAJOPT_IFOPT_JOINT_POSITION_CONSTRAINT_H +#ifndef TRAJOPT_IFOPT_TRAJOPT_JOINT_POSITION_CONSTRAINT_H +#define TRAJOPT_IFOPT_TRAJOPT_JOINT_POSITION_CONSTRAINT_H #include TRAJOPT_IGNORE_WARNINGS_PUSH diff --git a/trajopt_ifopt/include/trajopt_ifopt/constraints/joint_velocity_constraint.h b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/constraints/joint_velocity_constraint.h similarity index 97% rename from trajopt_ifopt/include/trajopt_ifopt/constraints/joint_velocity_constraint.h rename to trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/constraints/joint_velocity_constraint.h index 4dc3bf93..3b015ed5 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/constraints/joint_velocity_constraint.h +++ b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/constraints/joint_velocity_constraint.h @@ -23,8 +23,8 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -#ifndef TRAJOPT_IFOPT_JOINT_JOINT_VELOCITY_CONSTRAINT_H -#define TRAJOPT_IFOPT_JOINT_JOINT_VELOCITY_CONSTRAINT_H +#ifndef TRAJOPT_IFOPT_TRAJOPT_JOINT_JOINT_VELOCITY_CONSTRAINT_H +#define TRAJOPT_IFOPT_TRAJOPT_JOINT_JOINT_VELOCITY_CONSTRAINT_H #include TRAJOPT_IGNORE_WARNINGS_PUSH diff --git a/trajopt_ifopt/include/trajopt_ifopt/costs/absolute_cost.h b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/costs/absolute_cost.h similarity index 95% rename from trajopt_ifopt/include/trajopt_ifopt/costs/absolute_cost.h rename to trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/costs/absolute_cost.h index b008e4e8..7c5bda64 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/costs/absolute_cost.h +++ b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/costs/absolute_cost.h @@ -23,8 +23,8 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -#ifndef TRAJOPT_IFOPT_ABSOLUTE_COST_H -#define TRAJOPT_IFOPT_ABSOLUTE_COST_H +#ifndef TRAJOPT_IFOPT_TRAJOPT_ABSOLUTE_COST_H +#define TRAJOPT_IFOPT_TRAJOPT_ABSOLUTE_COST_H #include TRAJOPT_IGNORE_WARNINGS_PUSH @@ -106,4 +106,4 @@ class AbsoluteCost : public ifopt::CostTerm } // namespace trajopt_ifopt -#endif // TRAJOPT_IFOPT_ABSOLUTE_COST_H +#endif // TRAJOPT_IFOPT_TRAJOPT_ABSOLUTE_COST_H diff --git a/trajopt_ifopt/include/trajopt_ifopt/costs/squared_cost.h b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/costs/squared_cost.h similarity index 97% rename from trajopt_ifopt/include/trajopt_ifopt/costs/squared_cost.h rename to trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/costs/squared_cost.h index 76af0fad..ec91d302 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/costs/squared_cost.h +++ b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/costs/squared_cost.h @@ -23,8 +23,8 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -#ifndef TRAJOPT_IFOPT_SQUARED_COST_H -#define TRAJOPT_IFOPT_SQUARED_COST_H +#ifndef TRAJOPT_IFOPT_TRAJOPT_SQUARED_COST_H +#define TRAJOPT_IFOPT_TRAJOPT_SQUARED_COST_H #include TRAJOPT_IGNORE_WARNINGS_PUSH diff --git a/trajopt_ifopt/include/trajopt_ifopt/fwd.h b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/fwd.h similarity index 93% rename from trajopt_ifopt/include/trajopt_ifopt/fwd.h rename to trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/fwd.h index 5137178a..b5a1081d 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/fwd.h +++ b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/fwd.h @@ -13,8 +13,8 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -#ifndef TRAJOPT_IFOPT_FWD_H -#define TRAJOPT_IFOPT_FWD_H +#ifndef TRAJOPT_IFOPT_TRAJOPT_FWD_H +#define TRAJOPT_IFOPT_TRAJOPT_FWD_H namespace trajopt_ifopt { @@ -64,4 +64,4 @@ class DiscreteCollisionNumericalConstraint; } // namespace trajopt_ifopt -#endif // TRAJOPT_IFOPT_FWD_H +#endif // TRAJOPT_IFOPT_TRAJOPT_FWD_H diff --git a/trajopt_ifopt/include/trajopt_ifopt/utils/ifopt_utils.h b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/utils/ifopt_utils.h similarity index 98% rename from trajopt_ifopt/include/trajopt_ifopt/utils/ifopt_utils.h rename to trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/utils/ifopt_utils.h index 6c4452d5..297a4fef 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/utils/ifopt_utils.h +++ b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/utils/ifopt_utils.h @@ -23,8 +23,8 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -#ifndef TRAJOPT_IFOPT_IFOPT_UTILS_H -#define TRAJOPT_IFOPT_IFOPT_UTILS_H +#ifndef TRAJOPT_IFOPT_TRAJOPT_IFOPT_UTILS_H +#define TRAJOPT_IFOPT_TRAJOPT_IFOPT_UTILS_H #include TRAJOPT_IGNORE_WARNINGS_PUSH diff --git a/trajopt_ifopt/include/trajopt_ifopt/utils/numeric_differentiation.h b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/utils/numeric_differentiation.h similarity index 97% rename from trajopt_ifopt/include/trajopt_ifopt/utils/numeric_differentiation.h rename to trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/utils/numeric_differentiation.h index 0193628c..730e2fc2 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/utils/numeric_differentiation.h +++ b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/utils/numeric_differentiation.h @@ -23,8 +23,8 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -#ifndef TRAJOPT_IFOPT_NUMERIC_DIFFERENTIATION_H -#define TRAJOPT_IFOPT_NUMERIC_DIFFERENTIATION_H +#ifndef TRAJOPT_IFOPT_TRAJOPT_NUMERIC_DIFFERENTIATION_H +#define TRAJOPT_IFOPT_TRAJOPT_NUMERIC_DIFFERENTIATION_H #include TRAJOPT_IGNORE_WARNINGS_PUSH diff --git a/trajopt_ifopt/include/trajopt_ifopt/utils/trajopt_utils.h b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/utils/trajopt_utils.h similarity index 95% rename from trajopt_ifopt/include/trajopt_ifopt/utils/trajopt_utils.h rename to trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/utils/trajopt_utils.h index a9c2fbe4..8498c563 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/utils/trajopt_utils.h +++ b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/utils/trajopt_utils.h @@ -23,8 +23,8 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -#ifndef TRAJOPT_IFOPT_TRAJOPT_UTILS_H -#define TRAJOPT_IFOPT_TRAJOPT_UTILS_H +#ifndef TRAJOPT_IFOPT_TRAJOPT_TRAJOPT_UTILS_H +#define TRAJOPT_IFOPT_TRAJOPT_TRAJOPT_UTILS_H #include TRAJOPT_IGNORE_WARNINGS_PUSH diff --git a/trajopt_ifopt/include/trajopt_ifopt/variable_sets/joint_position_variable.h b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/joint_position_variable.h similarity index 96% rename from trajopt_ifopt/include/trajopt_ifopt/variable_sets/joint_position_variable.h rename to trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/joint_position_variable.h index 270dbfc8..f4e330e5 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/variable_sets/joint_position_variable.h +++ b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/joint_position_variable.h @@ -23,8 +23,8 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -#ifndef TRAJOPT_IFOPT_JOINT_POSITION_VARIABLE_H -#define TRAJOPT_IFOPT_JOINT_POSITION_VARIABLE_H +#ifndef TRAJOPT_IFOPT_TRAJOPT_JOINT_POSITION_VARIABLE_H +#define TRAJOPT_IFOPT_TRAJOPT_JOINT_POSITION_VARIABLE_H #include TRAJOPT_IGNORE_WARNINGS_PUSH diff --git a/trajopt_ifopt/src/constraints/cartesian_line_constraint.cpp b/trajopt_ifopt/trajopt/src/constraints/cartesian_line_constraint.cpp similarity index 98% rename from trajopt_ifopt/src/constraints/cartesian_line_constraint.cpp rename to trajopt_ifopt/trajopt/src/constraints/cartesian_line_constraint.cpp index 20123c32..a326c0f7 100644 --- a/trajopt_ifopt/src/constraints/cartesian_line_constraint.cpp +++ b/trajopt_ifopt/trajopt/src/constraints/cartesian_line_constraint.cpp @@ -26,10 +26,10 @@ * limitations under the License. */ -#include -#include -#include -#include +#include +#include +#include +#include #include TRAJOPT_IGNORE_WARNINGS_PUSH diff --git a/trajopt_ifopt/src/constraints/cartesian_position_constraint.cpp b/trajopt_ifopt/trajopt/src/constraints/cartesian_position_constraint.cpp similarity index 98% rename from trajopt_ifopt/src/constraints/cartesian_position_constraint.cpp rename to trajopt_ifopt/trajopt/src/constraints/cartesian_position_constraint.cpp index bcdfc08a..ff58d30f 100644 --- a/trajopt_ifopt/src/constraints/cartesian_position_constraint.cpp +++ b/trajopt_ifopt/trajopt/src/constraints/cartesian_position_constraint.cpp @@ -24,10 +24,10 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -#include -#include -#include -#include +#include +#include +#include +#include #include TRAJOPT_IGNORE_WARNINGS_PUSH diff --git a/trajopt_ifopt/src/constraints/collision/continuous_collision_constraint.cpp b/trajopt_ifopt/trajopt/src/constraints/collision/continuous_collision_constraint.cpp similarity index 95% rename from trajopt_ifopt/src/constraints/collision/continuous_collision_constraint.cpp rename to trajopt_ifopt/trajopt/src/constraints/collision/continuous_collision_constraint.cpp index 5ff0038f..611ea500 100644 --- a/trajopt_ifopt/src/constraints/collision/continuous_collision_constraint.cpp +++ b/trajopt_ifopt/trajopt/src/constraints/collision/continuous_collision_constraint.cpp @@ -30,10 +30,10 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include TRAJOPT_IGNORE_WARNINGS_POP -#include -#include -#include -#include +#include +#include +#include +#include namespace trajopt_ifopt { diff --git a/trajopt_ifopt/src/constraints/collision/continuous_collision_evaluators.cpp b/trajopt_ifopt/trajopt/src/constraints/collision/continuous_collision_evaluators.cpp similarity index 99% rename from trajopt_ifopt/src/constraints/collision/continuous_collision_evaluators.cpp rename to trajopt_ifopt/trajopt/src/constraints/collision/continuous_collision_evaluators.cpp index 219facf8..fb02b9c8 100644 --- a/trajopt_ifopt/src/constraints/collision/continuous_collision_evaluators.cpp +++ b/trajopt_ifopt/trajopt/src/constraints/collision/continuous_collision_evaluators.cpp @@ -22,7 +22,7 @@ * limitations under the License. */ -#include +#include #include #include diff --git a/trajopt_ifopt/src/constraints/collision/continuous_collision_numerical_constraint.cpp b/trajopt_ifopt/trajopt/src/constraints/collision/continuous_collision_numerical_constraint.cpp similarity index 95% rename from trajopt_ifopt/src/constraints/collision/continuous_collision_numerical_constraint.cpp rename to trajopt_ifopt/trajopt/src/constraints/collision/continuous_collision_numerical_constraint.cpp index acc5270a..f65b81a3 100644 --- a/trajopt_ifopt/src/constraints/collision/continuous_collision_numerical_constraint.cpp +++ b/trajopt_ifopt/trajopt/src/constraints/collision/continuous_collision_numerical_constraint.cpp @@ -30,10 +30,10 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include TRAJOPT_IGNORE_WARNINGS_POP -#include -#include -#include -#include +#include +#include +#include +#include namespace trajopt_ifopt { diff --git a/trajopt_ifopt/src/constraints/collision/discrete_collision_constraint.cpp b/trajopt_ifopt/trajopt/src/constraints/collision/discrete_collision_constraint.cpp similarity index 93% rename from trajopt_ifopt/src/constraints/collision/discrete_collision_constraint.cpp rename to trajopt_ifopt/trajopt/src/constraints/collision/discrete_collision_constraint.cpp index d8da0304..3075f0e1 100644 --- a/trajopt_ifopt/src/constraints/collision/discrete_collision_constraint.cpp +++ b/trajopt_ifopt/trajopt/src/constraints/collision/discrete_collision_constraint.cpp @@ -30,11 +30,11 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include TRAJOPT_IGNORE_WARNINGS_POP -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include namespace trajopt_ifopt { diff --git a/trajopt_ifopt/src/constraints/collision/discrete_collision_evaluators.cpp b/trajopt_ifopt/trajopt/src/constraints/collision/discrete_collision_evaluators.cpp similarity index 99% rename from trajopt_ifopt/src/constraints/collision/discrete_collision_evaluators.cpp rename to trajopt_ifopt/trajopt/src/constraints/collision/discrete_collision_evaluators.cpp index 297fc43a..e3c681ed 100644 --- a/trajopt_ifopt/src/constraints/collision/discrete_collision_evaluators.cpp +++ b/trajopt_ifopt/trajopt/src/constraints/collision/discrete_collision_evaluators.cpp @@ -22,7 +22,7 @@ * limitations under the License. */ -#include +#include #include #include diff --git a/trajopt_ifopt/src/constraints/collision/discrete_collision_numerical_constraint.cpp b/trajopt_ifopt/trajopt/src/constraints/collision/discrete_collision_numerical_constraint.cpp similarity index 94% rename from trajopt_ifopt/src/constraints/collision/discrete_collision_numerical_constraint.cpp rename to trajopt_ifopt/trajopt/src/constraints/collision/discrete_collision_numerical_constraint.cpp index 90f9d9ec..d5ae57bc 100644 --- a/trajopt_ifopt/src/constraints/collision/discrete_collision_numerical_constraint.cpp +++ b/trajopt_ifopt/trajopt/src/constraints/collision/discrete_collision_numerical_constraint.cpp @@ -30,11 +30,11 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include TRAJOPT_IGNORE_WARNINGS_POP -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include namespace trajopt_ifopt { diff --git a/trajopt_ifopt/src/constraints/collision/weighted_average_methods.cpp b/trajopt_ifopt/trajopt/src/constraints/collision/weighted_average_methods.cpp similarity index 97% rename from trajopt_ifopt/src/constraints/collision/weighted_average_methods.cpp rename to trajopt_ifopt/trajopt/src/constraints/collision/weighted_average_methods.cpp index 49754950..b3aefb16 100644 --- a/trajopt_ifopt/src/constraints/collision/weighted_average_methods.cpp +++ b/trajopt_ifopt/trajopt/src/constraints/collision/weighted_average_methods.cpp @@ -22,7 +22,7 @@ * limitations under the License. */ -#include +#include #include namespace trajopt_ifopt diff --git a/trajopt_ifopt/src/constraints/inverse_kinematics_constraint.cpp b/trajopt_ifopt/trajopt/src/constraints/inverse_kinematics_constraint.cpp similarity index 97% rename from trajopt_ifopt/src/constraints/inverse_kinematics_constraint.cpp rename to trajopt_ifopt/trajopt/src/constraints/inverse_kinematics_constraint.cpp index 5dff8e32..ddce0c9b 100644 --- a/trajopt_ifopt/src/constraints/inverse_kinematics_constraint.cpp +++ b/trajopt_ifopt/trajopt/src/constraints/inverse_kinematics_constraint.cpp @@ -23,8 +23,8 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -#include -#include +#include +#include TRAJOPT_IGNORE_WARNINGS_PUSH #include diff --git a/trajopt_ifopt/src/constraints/joint_acceleration_constraint.cpp b/trajopt_ifopt/trajopt/src/constraints/joint_acceleration_constraint.cpp similarity index 97% rename from trajopt_ifopt/src/constraints/joint_acceleration_constraint.cpp rename to trajopt_ifopt/trajopt/src/constraints/joint_acceleration_constraint.cpp index 3c64b350..cf0d0b1c 100644 --- a/trajopt_ifopt/src/constraints/joint_acceleration_constraint.cpp +++ b/trajopt_ifopt/trajopt/src/constraints/joint_acceleration_constraint.cpp @@ -23,8 +23,8 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -#include -#include +#include +#include TRAJOPT_IGNORE_WARNINGS_PUSH #include diff --git a/trajopt_ifopt/src/constraints/joint_jerk_constraint.cpp b/trajopt_ifopt/trajopt/src/constraints/joint_jerk_constraint.cpp similarity index 97% rename from trajopt_ifopt/src/constraints/joint_jerk_constraint.cpp rename to trajopt_ifopt/trajopt/src/constraints/joint_jerk_constraint.cpp index 317a3086..cabb4842 100644 --- a/trajopt_ifopt/src/constraints/joint_jerk_constraint.cpp +++ b/trajopt_ifopt/trajopt/src/constraints/joint_jerk_constraint.cpp @@ -23,8 +23,8 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -#include -#include +#include +#include TRAJOPT_IGNORE_WARNINGS_PUSH #include diff --git a/trajopt_ifopt/src/constraints/joint_position_constraint.cpp b/trajopt_ifopt/trajopt/src/constraints/joint_position_constraint.cpp similarity index 97% rename from trajopt_ifopt/src/constraints/joint_position_constraint.cpp rename to trajopt_ifopt/trajopt/src/constraints/joint_position_constraint.cpp index 81b6a3db..3b03baf0 100644 --- a/trajopt_ifopt/src/constraints/joint_position_constraint.cpp +++ b/trajopt_ifopt/trajopt/src/constraints/joint_position_constraint.cpp @@ -23,8 +23,8 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -#include -#include +#include +#include TRAJOPT_IGNORE_WARNINGS_PUSH #include diff --git a/trajopt_ifopt/src/constraints/joint_velocity_constraint.cpp b/trajopt_ifopt/trajopt/src/constraints/joint_velocity_constraint.cpp similarity index 97% rename from trajopt_ifopt/src/constraints/joint_velocity_constraint.cpp rename to trajopt_ifopt/trajopt/src/constraints/joint_velocity_constraint.cpp index 5001dd80..265ff712 100644 --- a/trajopt_ifopt/src/constraints/joint_velocity_constraint.cpp +++ b/trajopt_ifopt/trajopt/src/constraints/joint_velocity_constraint.cpp @@ -23,8 +23,8 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -#include -#include +#include +#include TRAJOPT_IGNORE_WARNINGS_PUSH #include diff --git a/trajopt_ifopt/src/costs/absolute_cost.cpp b/trajopt_ifopt/trajopt/src/costs/absolute_cost.cpp similarity index 96% rename from trajopt_ifopt/src/costs/absolute_cost.cpp rename to trajopt_ifopt/trajopt/src/costs/absolute_cost.cpp index bdcb43b0..8903fdc8 100644 --- a/trajopt_ifopt/src/costs/absolute_cost.cpp +++ b/trajopt_ifopt/trajopt/src/costs/absolute_cost.cpp @@ -23,8 +23,8 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -#include -#include +#include +#include TRAJOPT_IGNORE_WARNINGS_PUSH #include diff --git a/trajopt_ifopt/src/costs/squared_cost.cpp b/trajopt_ifopt/trajopt/src/costs/squared_cost.cpp similarity index 95% rename from trajopt_ifopt/src/costs/squared_cost.cpp rename to trajopt_ifopt/trajopt/src/costs/squared_cost.cpp index 7dc7f350..44865fd3 100644 --- a/trajopt_ifopt/src/costs/squared_cost.cpp +++ b/trajopt_ifopt/trajopt/src/costs/squared_cost.cpp @@ -23,8 +23,8 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -#include -#include +#include +#include TRAJOPT_IGNORE_WARNINGS_PUSH #include diff --git a/trajopt_ifopt/src/utils/ifopt_utils.cpp b/trajopt_ifopt/trajopt/src/utils/ifopt_utils.cpp similarity index 99% rename from trajopt_ifopt/src/utils/ifopt_utils.cpp rename to trajopt_ifopt/trajopt/src/utils/ifopt_utils.cpp index f6fb6c25..8a3ce45d 100644 --- a/trajopt_ifopt/src/utils/ifopt_utils.cpp +++ b/trajopt_ifopt/trajopt/src/utils/ifopt_utils.cpp @@ -31,7 +31,7 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include TRAJOPT_IGNORE_WARNINGS_POP -#include +#include namespace trajopt_ifopt { diff --git a/trajopt_ifopt/src/utils/numeric_differentiation.cpp b/trajopt_ifopt/trajopt/src/utils/numeric_differentiation.cpp similarity index 95% rename from trajopt_ifopt/src/utils/numeric_differentiation.cpp rename to trajopt_ifopt/trajopt/src/utils/numeric_differentiation.cpp index a50b8042..683bafb3 100644 --- a/trajopt_ifopt/src/utils/numeric_differentiation.cpp +++ b/trajopt_ifopt/trajopt/src/utils/numeric_differentiation.cpp @@ -23,7 +23,7 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -#include +#include namespace trajopt_ifopt { diff --git a/trajopt_ifopt/src/utils/trajopt_utils.cpp b/trajopt_ifopt/trajopt/src/utils/trajopt_utils.cpp similarity index 93% rename from trajopt_ifopt/src/utils/trajopt_utils.cpp rename to trajopt_ifopt/trajopt/src/utils/trajopt_utils.cpp index e0182d79..71024cab 100644 --- a/trajopt_ifopt/src/utils/trajopt_utils.cpp +++ b/trajopt_ifopt/trajopt/src/utils/trajopt_utils.cpp @@ -24,8 +24,8 @@ * limitations under the License. */ -#include -#include +#include +#include TRAJOPT_IGNORE_WARNINGS_PUSH #include diff --git a/trajopt_ifopt/src/variable_sets/joint_position_variable.cpp b/trajopt_ifopt/trajopt/src/variable_sets/joint_position_variable.cpp similarity index 96% rename from trajopt_ifopt/src/variable_sets/joint_position_variable.cpp rename to trajopt_ifopt/trajopt/src/variable_sets/joint_position_variable.cpp index ec58bb79..c867c16f 100644 --- a/trajopt_ifopt/src/variable_sets/joint_position_variable.cpp +++ b/trajopt_ifopt/trajopt/src/variable_sets/joint_position_variable.cpp @@ -29,8 +29,8 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include TRAJOPT_IGNORE_WARNINGS_POP -#include -#include +#include +#include namespace trajopt_ifopt { diff --git a/trajopt_ifopt/test/CMakeLists.txt b/trajopt_ifopt/trajopt/test/CMakeLists.txt similarity index 99% rename from trajopt_ifopt/test/CMakeLists.txt rename to trajopt_ifopt/trajopt/test/CMakeLists.txt index 744ab995..2350e91e 100644 --- a/trajopt_ifopt/test/CMakeLists.txt +++ b/trajopt_ifopt/trajopt/test/CMakeLists.txt @@ -39,7 +39,7 @@ macro(add_gtest test_name test_file) target_clang_tidy(${test_name} ENABLE ${TRAJOPT_ENABLE_CLANG_TIDY}) target_link_libraries( ${test_name} - ${PROJECT_NAME} + ${PROJECT_NAME}_trajopt tesseract::tesseract_environment ifopt::ifopt_ipopt GTest::GTest diff --git a/trajopt_ifopt/test/cartesian_line_constraint_unit.cpp b/trajopt_ifopt/trajopt/test/cartesian_line_constraint_unit.cpp similarity index 97% rename from trajopt_ifopt/test/cartesian_line_constraint_unit.cpp rename to trajopt_ifopt/trajopt/test/cartesian_line_constraint_unit.cpp index e14a0e84..636e0599 100644 --- a/trajopt_ifopt/test/cartesian_line_constraint_unit.cpp +++ b/trajopt_ifopt/trajopt/test/cartesian_line_constraint_unit.cpp @@ -11,9 +11,9 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include TRAJOPT_IGNORE_WARNINGS_POP -#include -#include -#include +#include +#include +#include using namespace trajopt_ifopt; using namespace std; diff --git a/trajopt_ifopt/test/cartesian_position_constraint_unit.cpp b/trajopt_ifopt/trajopt/test/cartesian_position_constraint_unit.cpp similarity index 97% rename from trajopt_ifopt/test/cartesian_position_constraint_unit.cpp rename to trajopt_ifopt/trajopt/test/cartesian_position_constraint_unit.cpp index 85d3692d..df3d4d80 100644 --- a/trajopt_ifopt/test/cartesian_position_constraint_unit.cpp +++ b/trajopt_ifopt/trajopt/test/cartesian_position_constraint_unit.cpp @@ -38,9 +38,9 @@ TRAJOPT_IGNORE_WARNINGS_PUSH TRAJOPT_IGNORE_WARNINGS_POP #include -#include -#include -#include +#include +#include +#include using namespace trajopt_ifopt; using namespace std; diff --git a/trajopt_ifopt/test/cast_cost_unit.cpp b/trajopt_ifopt/trajopt/test/cast_cost_unit.cpp similarity index 95% rename from trajopt_ifopt/test/cast_cost_unit.cpp rename to trajopt_ifopt/trajopt/test/cast_cost_unit.cpp index 82d4fa47..64d8b6ee 100644 --- a/trajopt_ifopt/test/cast_cost_unit.cpp +++ b/trajopt_ifopt/trajopt/test/cast_cost_unit.cpp @@ -44,11 +44,11 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include TRAJOPT_IGNORE_WARNINGS_POP -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include using namespace trajopt_ifopt; using namespace std; diff --git a/trajopt_ifopt/test/collision_unit.cpp b/trajopt_ifopt/trajopt/test/collision_unit.cpp similarity index 96% rename from trajopt_ifopt/test/collision_unit.cpp rename to trajopt_ifopt/trajopt/test/collision_unit.cpp index 3c33270e..9b3b44fb 100644 --- a/trajopt_ifopt/test/collision_unit.cpp +++ b/trajopt_ifopt/trajopt/test/collision_unit.cpp @@ -38,9 +38,9 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include TRAJOPT_IGNORE_WARNINGS_POP -#include -#include -#include +#include +#include +#include using namespace trajopt_ifopt; using namespace tesseract_environment; diff --git a/trajopt_ifopt/test/continuous_collision_gradient_unit.cpp b/trajopt_ifopt/trajopt/test/continuous_collision_gradient_unit.cpp similarity index 92% rename from trajopt_ifopt/test/continuous_collision_gradient_unit.cpp rename to trajopt_ifopt/trajopt/test/continuous_collision_gradient_unit.cpp index 240d45a8..78ca7217 100644 --- a/trajopt_ifopt/test/continuous_collision_gradient_unit.cpp +++ b/trajopt_ifopt/trajopt/test/continuous_collision_gradient_unit.cpp @@ -44,12 +44,12 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include TRAJOPT_IGNORE_WARNINGS_POP -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include using namespace trajopt_ifopt; using namespace std; diff --git a/trajopt_ifopt/test/cost_wrappers_unit.cpp b/trajopt_ifopt/trajopt/test/cost_wrappers_unit.cpp similarity index 97% rename from trajopt_ifopt/test/cost_wrappers_unit.cpp rename to trajopt_ifopt/trajopt/test/cost_wrappers_unit.cpp index 80bb038c..0c7e77eb 100644 --- a/trajopt_ifopt/test/cost_wrappers_unit.cpp +++ b/trajopt_ifopt/trajopt/test/cost_wrappers_unit.cpp @@ -32,10 +32,10 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include TRAJOPT_IGNORE_WARNINGS_POP -#include -#include -#include -#include +#include +#include +#include +#include /** * @brief The SimpleTestConstraint class diff --git a/trajopt_ifopt/test/discrete_collision_gradient_unit.cpp b/trajopt_ifopt/trajopt/test/discrete_collision_gradient_unit.cpp similarity index 87% rename from trajopt_ifopt/test/discrete_collision_gradient_unit.cpp rename to trajopt_ifopt/trajopt/test/discrete_collision_gradient_unit.cpp index d14b108b..92ec7108 100644 --- a/trajopt_ifopt/test/discrete_collision_gradient_unit.cpp +++ b/trajopt_ifopt/trajopt/test/discrete_collision_gradient_unit.cpp @@ -40,15 +40,15 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include TRAJOPT_IGNORE_WARNINGS_POP -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include using namespace trajopt_ifopt; using namespace tesseract_environment; diff --git a/trajopt_ifopt/test/inverse_kinematics_constraint_unit.cpp b/trajopt_ifopt/trajopt/test/inverse_kinematics_constraint_unit.cpp similarity index 97% rename from trajopt_ifopt/test/inverse_kinematics_constraint_unit.cpp rename to trajopt_ifopt/trajopt/test/inverse_kinematics_constraint_unit.cpp index 4a3cb088..53e11d9b 100644 --- a/trajopt_ifopt/test/inverse_kinematics_constraint_unit.cpp +++ b/trajopt_ifopt/trajopt/test/inverse_kinematics_constraint_unit.cpp @@ -38,9 +38,9 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include TRAJOPT_IGNORE_WARNINGS_POP -#include -#include -#include +#include +#include +#include using namespace trajopt_ifopt; using namespace std; diff --git a/trajopt_ifopt/test/joint_terms_unit.cpp b/trajopt_ifopt/trajopt/test/joint_terms_unit.cpp similarity index 97% rename from trajopt_ifopt/test/joint_terms_unit.cpp rename to trajopt_ifopt/trajopt/test/joint_terms_unit.cpp index 4c3a8eed..144f8a06 100644 --- a/trajopt_ifopt/test/joint_terms_unit.cpp +++ b/trajopt_ifopt/trajopt/test/joint_terms_unit.cpp @@ -31,12 +31,12 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include #include TRAJOPT_IGNORE_WARNINGS_POP -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include using namespace trajopt_ifopt; using namespace std; diff --git a/trajopt_ifopt/test/simple_collision_unit.cpp b/trajopt_ifopt/trajopt/test/simple_collision_unit.cpp similarity index 90% rename from trajopt_ifopt/test/simple_collision_unit.cpp rename to trajopt_ifopt/trajopt/test/simple_collision_unit.cpp index 6477e10d..e2d85318 100644 --- a/trajopt_ifopt/test/simple_collision_unit.cpp +++ b/trajopt_ifopt/trajopt/test/simple_collision_unit.cpp @@ -40,14 +40,14 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include TRAJOPT_IGNORE_WARNINGS_POP -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include using namespace trajopt_ifopt; using namespace tesseract_environment; diff --git a/trajopt_ifopt/test/utils_unit.cpp b/trajopt_ifopt/trajopt/test/utils_unit.cpp similarity index 99% rename from trajopt_ifopt/test/utils_unit.cpp rename to trajopt_ifopt/trajopt/test/utils_unit.cpp index 23891f40..2b96724b 100644 --- a/trajopt_ifopt/test/utils_unit.cpp +++ b/trajopt_ifopt/trajopt/test/utils_unit.cpp @@ -30,7 +30,7 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include #include TRAJOPT_IGNORE_WARNINGS_POP -#include +#include #include #include diff --git a/trajopt_ifopt/test/variable_sets_unit.cpp b/trajopt_ifopt/trajopt/test/variable_sets_unit.cpp similarity index 97% rename from trajopt_ifopt/test/variable_sets_unit.cpp rename to trajopt_ifopt/trajopt/test/variable_sets_unit.cpp index 71e86f7b..d838b8eb 100644 --- a/trajopt_ifopt/test/variable_sets_unit.cpp +++ b/trajopt_ifopt/trajopt/test/variable_sets_unit.cpp @@ -29,7 +29,7 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include #include TRAJOPT_IGNORE_WARNINGS_POP -#include +#include #include using namespace trajopt_ifopt; diff --git a/trajopt_optimizers/trajopt_sqp/CMakeLists.txt b/trajopt_optimizers/trajopt_sqp/CMakeLists.txt index 3bd7f051..d29b3274 100644 --- a/trajopt_optimizers/trajopt_sqp/CMakeLists.txt +++ b/trajopt_optimizers/trajopt_sqp/CMakeLists.txt @@ -49,7 +49,7 @@ add_library( target_link_libraries( ${PROJECT_NAME} PUBLIC console_bridge::console_bridge - trajopt::trajopt_ifopt + trajopt::trajopt_ifopt_trajopt ifopt::ifopt_core OsqpEigen::OsqpEigen) target_include_directories(${PROJECT_NAME} PUBLIC $ @@ -71,7 +71,7 @@ target_include_directories(${PROJECT_NAME}_callbacks PUBLIC $) target_link_libraries( ${PROJECT_NAME}_callbacks - PUBLIC trajopt::trajopt_ifopt + PUBLIC trajopt::trajopt_ifopt_trajopt trajopt::trajopt_common tesseract::tesseract_visualization tesseract::tesseract_common diff --git a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/callbacks/cartesian_error_plotter.h b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/callbacks/cartesian_error_plotter.h index d3424661..788d9d71 100644 --- a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/callbacks/cartesian_error_plotter.h +++ b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/callbacks/cartesian_error_plotter.h @@ -29,7 +29,7 @@ #include TRAJOPT_IGNORE_WARNINGS_PUSH #include -#include +#include #include TRAJOPT_IGNORE_WARNINGS_POP diff --git a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/callbacks/collision_plotter.h b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/callbacks/collision_plotter.h index 63ecabe8..71d1bafa 100644 --- a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/callbacks/collision_plotter.h +++ b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/callbacks/collision_plotter.h @@ -29,7 +29,7 @@ #include TRAJOPT_IGNORE_WARNINGS_PUSH #include -#include +#include #include TRAJOPT_IGNORE_WARNINGS_POP diff --git a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/callbacks/joint_state_plotter.h b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/callbacks/joint_state_plotter.h index c5b7804d..9666b913 100644 --- a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/callbacks/joint_state_plotter.h +++ b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/callbacks/joint_state_plotter.h @@ -30,7 +30,7 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include #include -#include +#include #include TRAJOPT_IGNORE_WARNINGS_POP diff --git a/trajopt_optimizers/trajopt_sqp/src/callbacks/cartesian_error_plotter.cpp b/trajopt_optimizers/trajopt_sqp/src/callbacks/cartesian_error_plotter.cpp index a19a0f6d..7ac9c24d 100644 --- a/trajopt_optimizers/trajopt_sqp/src/callbacks/cartesian_error_plotter.cpp +++ b/trajopt_optimizers/trajopt_sqp/src/callbacks/cartesian_error_plotter.cpp @@ -25,7 +25,7 @@ */ #include -#include +#include #include diff --git a/trajopt_optimizers/trajopt_sqp/src/callbacks/collision_plotter.cpp b/trajopt_optimizers/trajopt_sqp/src/callbacks/collision_plotter.cpp index 46d60138..68d7e574 100644 --- a/trajopt_optimizers/trajopt_sqp/src/callbacks/collision_plotter.cpp +++ b/trajopt_optimizers/trajopt_sqp/src/callbacks/collision_plotter.cpp @@ -26,7 +26,7 @@ #include #include -#include +#include #include diff --git a/trajopt_optimizers/trajopt_sqp/src/callbacks/joint_state_plotter.cpp b/trajopt_optimizers/trajopt_sqp/src/callbacks/joint_state_plotter.cpp index 42b5c334..0d92842b 100644 --- a/trajopt_optimizers/trajopt_sqp/src/callbacks/joint_state_plotter.cpp +++ b/trajopt_optimizers/trajopt_sqp/src/callbacks/joint_state_plotter.cpp @@ -25,8 +25,8 @@ */ #include -#include -#include +#include +#include #include #include diff --git a/trajopt_optimizers/trajopt_sqp/src/ifopt_qp_problem.cpp b/trajopt_optimizers/trajopt_sqp/src/ifopt_qp_problem.cpp index bbf03426..844ab65c 100644 --- a/trajopt_optimizers/trajopt_sqp/src/ifopt_qp_problem.cpp +++ b/trajopt_optimizers/trajopt_sqp/src/ifopt_qp_problem.cpp @@ -24,9 +24,9 @@ * limitations under the License. */ #include -#include -#include -#include +#include +#include +#include #include #include diff --git a/trajopt_optimizers/trajopt_sqp/src/trajopt_qp_problem.cpp b/trajopt_optimizers/trajopt_sqp/src/trajopt_qp_problem.cpp index cae334d8..c41aabe3 100644 --- a/trajopt_optimizers/trajopt_sqp/src/trajopt_qp_problem.cpp +++ b/trajopt_optimizers/trajopt_sqp/src/trajopt_qp_problem.cpp @@ -6,7 +6,7 @@ #include #include -#include +#include #include #include diff --git a/trajopt_optimizers/trajopt_sqp/test/CMakeLists.txt b/trajopt_optimizers/trajopt_sqp/test/CMakeLists.txt index 8ad6bdb2..7babdc14 100644 --- a/trajopt_optimizers/trajopt_sqp/test/CMakeLists.txt +++ b/trajopt_optimizers/trajopt_sqp/test/CMakeLists.txt @@ -43,7 +43,7 @@ macro(add_gtest test_name test_file) ${PROJECT_NAME} console_bridge ifopt::ifopt_ipopt - trajopt::trajopt_ifopt + trajopt::trajopt_ifopt_trajopt trajopt::trajopt_common GTest::GTest GTest::Main) diff --git a/trajopt_optimizers/trajopt_sqp/test/benchmarks/CMakeLists.txt b/trajopt_optimizers/trajopt_sqp/test/benchmarks/CMakeLists.txt index 43199fde..d6272315 100644 --- a/trajopt_optimizers/trajopt_sqp/test/benchmarks/CMakeLists.txt +++ b/trajopt_optimizers/trajopt_sqp/test/benchmarks/CMakeLists.txt @@ -10,7 +10,7 @@ macro(add_benchmark benchmark_name benchmark_file) ${benchmark_name} ${PROJECT_NAME} ifopt::ifopt_ipopt - trajopt::trajopt_ifopt + trajopt::trajopt_ifopt_trajopt trajopt::trajopt_common benchmark::benchmark) target_include_directories(${benchmark_name} PRIVATE "$") diff --git a/trajopt_optimizers/trajopt_sqp/test/benchmarks/solve_benchmarks.cpp b/trajopt_optimizers/trajopt_sqp/test/benchmarks/solve_benchmarks.cpp index 3edb51c1..28b2bad5 100644 --- a/trajopt_optimizers/trajopt_sqp/test/benchmarks/solve_benchmarks.cpp +++ b/trajopt_optimizers/trajopt_sqp/test/benchmarks/solve_benchmarks.cpp @@ -11,16 +11,16 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include #include diff --git a/trajopt_optimizers/trajopt_sqp/test/cart_position_optimization_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/cart_position_optimization_unit.cpp index 823ef320..22eaa8e8 100644 --- a/trajopt_optimizers/trajopt_sqp/test/cart_position_optimization_unit.cpp +++ b/trajopt_optimizers/trajopt_sqp/test/cart_position_optimization_unit.cpp @@ -47,10 +47,10 @@ TRAJOPT_IGNORE_WARNINGS_POP #include #include -#include -#include -#include -#include +#include +#include +#include +#include const bool DEBUG = false; diff --git a/trajopt_optimizers/trajopt_sqp/test/cast_cost_attached_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/cast_cost_attached_unit.cpp index 48221fec..8f1b0ce6 100644 --- a/trajopt_optimizers/trajopt_sqp/test/cast_cost_attached_unit.cpp +++ b/trajopt_optimizers/trajopt_sqp/test/cast_cost_attached_unit.cpp @@ -49,11 +49,11 @@ TRAJOPT_IGNORE_WARNINGS_POP #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include diff --git a/trajopt_optimizers/trajopt_sqp/test/cast_cost_octomap_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/cast_cost_octomap_unit.cpp index ac882be2..709b07f6 100644 --- a/trajopt_optimizers/trajopt_sqp/test/cast_cost_octomap_unit.cpp +++ b/trajopt_optimizers/trajopt_sqp/test/cast_cost_octomap_unit.cpp @@ -48,11 +48,11 @@ TRAJOPT_IGNORE_WARNINGS_POP #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include diff --git a/trajopt_optimizers/trajopt_sqp/test/cast_cost_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/cast_cost_unit.cpp index d33194ef..ec94a1e4 100644 --- a/trajopt_optimizers/trajopt_sqp/test/cast_cost_unit.cpp +++ b/trajopt_optimizers/trajopt_sqp/test/cast_cost_unit.cpp @@ -41,11 +41,11 @@ TRAJOPT_IGNORE_WARNINGS_POP #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include diff --git a/trajopt_optimizers/trajopt_sqp/test/cast_cost_world_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/cast_cost_world_unit.cpp index 10ec0dcb..3d668033 100644 --- a/trajopt_optimizers/trajopt_sqp/test/cast_cost_world_unit.cpp +++ b/trajopt_optimizers/trajopt_sqp/test/cast_cost_world_unit.cpp @@ -45,11 +45,11 @@ TRAJOPT_IGNORE_WARNINGS_POP #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include diff --git a/trajopt_optimizers/trajopt_sqp/test/joint_acceleration_optimization_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/joint_acceleration_optimization_unit.cpp index 18aeb24d..dd298559 100644 --- a/trajopt_optimizers/trajopt_sqp/test/joint_acceleration_optimization_unit.cpp +++ b/trajopt_optimizers/trajopt_sqp/test/joint_acceleration_optimization_unit.cpp @@ -39,10 +39,10 @@ TRAJOPT_IGNORE_WARNINGS_POP #include #include -#include -#include -#include -#include +#include +#include +#include +#include const bool DEBUG = false; diff --git a/trajopt_optimizers/trajopt_sqp/test/joint_jerk_optimization_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/joint_jerk_optimization_unit.cpp index b3e9b97c..fc2a4c39 100644 --- a/trajopt_optimizers/trajopt_sqp/test/joint_jerk_optimization_unit.cpp +++ b/trajopt_optimizers/trajopt_sqp/test/joint_jerk_optimization_unit.cpp @@ -41,10 +41,10 @@ TRAJOPT_IGNORE_WARNINGS_POP #include #include -#include -#include -#include -#include +#include +#include +#include +#include const bool DEBUG = false; diff --git a/trajopt_optimizers/trajopt_sqp/test/joint_position_optimization_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/joint_position_optimization_unit.cpp index f5af3b61..45f9e3ae 100644 --- a/trajopt_optimizers/trajopt_sqp/test/joint_position_optimization_unit.cpp +++ b/trajopt_optimizers/trajopt_sqp/test/joint_position_optimization_unit.cpp @@ -39,8 +39,8 @@ TRAJOPT_IGNORE_WARNINGS_POP #include #include -#include -#include +#include +#include const bool DEBUG = false; diff --git a/trajopt_optimizers/trajopt_sqp/test/joint_velocity_optimization_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/joint_velocity_optimization_unit.cpp index c2a2ed48..6ba46af9 100644 --- a/trajopt_optimizers/trajopt_sqp/test/joint_velocity_optimization_unit.cpp +++ b/trajopt_optimizers/trajopt_sqp/test/joint_velocity_optimization_unit.cpp @@ -39,10 +39,10 @@ TRAJOPT_IGNORE_WARNINGS_POP #include #include -#include -#include -#include -#include +#include +#include +#include +#include const bool DEBUG = false; diff --git a/trajopt_optimizers/trajopt_sqp/test/numerical_ik_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/numerical_ik_unit.cpp index 39411fb7..b01598e3 100644 --- a/trajopt_optimizers/trajopt_sqp/test/numerical_ik_unit.cpp +++ b/trajopt_optimizers/trajopt_sqp/test/numerical_ik_unit.cpp @@ -41,11 +41,11 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include TRAJOPT_IGNORE_WARNINGS_POP -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include diff --git a/trajopt_optimizers/trajopt_sqp/test/planning_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/planning_unit.cpp index 5ea90128..ae89814f 100644 --- a/trajopt_optimizers/trajopt_sqp/test/planning_unit.cpp +++ b/trajopt_optimizers/trajopt_sqp/test/planning_unit.cpp @@ -43,12 +43,12 @@ TRAJOPT_IGNORE_WARNINGS_POP #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include #include diff --git a/trajopt_optimizers/trajopt_sqp/test/simple_collision_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/simple_collision_unit.cpp index 44b31eac..adfdabd1 100644 --- a/trajopt_optimizers/trajopt_sqp/test/simple_collision_unit.cpp +++ b/trajopt_optimizers/trajopt_sqp/test/simple_collision_unit.cpp @@ -43,14 +43,14 @@ TRAJOPT_IGNORE_WARNINGS_POP #include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include #include #include From 33a6067fcf84c10e9ba27fe6b7f692a99ca47b8c Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Sat, 27 Apr 2024 18:30:20 -0500 Subject: [PATCH 2/5] Add node variabel set --- trajopt_ifopt/trajopt/CMakeLists.txt | 5 +- .../trajopt/variable_sets/node.h | 34 ++++ .../trajopt/variable_sets/nodes_observer.h | 70 ++++++++ .../trajopt/variable_sets/nodes_variables.h | 143 ++++++++++++++++ .../trajopt_ifopt/trajopt/variable_sets/var.h | 159 ++++++++++++++++++ .../trajopt/src/variable_sets/node.cpp | 35 ++++ .../src/variable_sets/nodes_observer.cpp | 42 +++++ .../src/variable_sets/nodes_variables.cpp | 79 +++++++++ .../trajopt/src/variable_sets/var.cpp | 27 +++ 9 files changed, 593 insertions(+), 1 deletion(-) create mode 100644 trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/node.h create mode 100644 trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/nodes_observer.h create mode 100644 trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/nodes_variables.h create mode 100644 trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/var.h create mode 100644 trajopt_ifopt/trajopt/src/variable_sets/node.cpp create mode 100644 trajopt_ifopt/trajopt/src/variable_sets/nodes_observer.cpp create mode 100644 trajopt_ifopt/trajopt/src/variable_sets/nodes_variables.cpp create mode 100644 trajopt_ifopt/trajopt/src/variable_sets/var.cpp diff --git a/trajopt_ifopt/trajopt/CMakeLists.txt b/trajopt_ifopt/trajopt/CMakeLists.txt index 01e16562..8f182fa3 100644 --- a/trajopt_ifopt/trajopt/CMakeLists.txt +++ b/trajopt_ifopt/trajopt/CMakeLists.txt @@ -42,7 +42,10 @@ set(TRAJOPT_IFOPT_SOURCE_FILES src/utils/ifopt_utils.cpp src/utils/numeric_differentiation.cpp src/utils/trajopt_utils.cpp - src/variable_sets/joint_position_variable.cpp) + src/variable_sets/joint_position_variable.cpp + src/variable_sets/node.cpp + src/variable_sets/nodes_variables.cpp + src/variable_sets/var.cpp) add_library(${PROJECT_NAME}_trajopt ${TRAJOPT_IFOPT_SOURCE_FILES}) target_link_libraries( diff --git a/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/node.h b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/node.h new file mode 100644 index 00000000..0a4ebec1 --- /dev/null +++ b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/node.h @@ -0,0 +1,34 @@ +#ifndef TRAJOPT_IFOPT_TRAJOPT_NODE_H +#define TRAJOPT_IFOPT_TRAJOPT_NODE_H + +#include +#include + +namespace trajopt_ifopt +{ +class Node +{ +public: + Node(std::string node_name = "Node"); + + Var addVar(const std::string& name); + + Var addVar(const std::string& name, const std::vector& child_names); + + Var getVar(const std::string& name) const; + + bool hasVar(const std::string& name) const; + + Eigen::Index size() const; + +protected: + friend class NodesVariables; + std::string name_; + std::unordered_map vars_; + Eigen::Index length_{ 0 }; + + void setVariables(const Eigen::Ref& x); +}; + +} // namespace trajopt_ifopt +#endif // TRAJOPT_IFOPT_TRAJOPT_NODE_H diff --git a/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/nodes_observer.h b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/nodes_observer.h new file mode 100644 index 00000000..3a1ba2b3 --- /dev/null +++ b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/nodes_observer.h @@ -0,0 +1,70 @@ +/****************************************************************************** +Copyright (c) 2018, 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_TRAJOPT_NODES_OBSERVER_H +#define TRAJOPT_IFOPT_TRAJOPT_NODES_OBSERVER_H + +#include + +namespace trajopt_ifopt +{ +class NodesVariables; + +/** + * @brief Base class to receive up-to-date values of the NodeVariables. + * + * This class registers with the node variables and everytime the positions or + * velocities of a node change, the subject updates this class by calling the + * UpdatePolynomials() method. + * + * Used by spline.h + * + * This class implements the observer pattern: + * https://sourcemaking.com/design_patterns/observer + */ +class NodesObserver +{ +public: + /** + * @brief Registers this observer with the subject class to receive updates. + * @param node_values The subject holding the Hermite node values. + */ + NodesObserver(std::weak_ptr node_values); + virtual ~NodesObserver() = default; + + /** + * @brief Callback method called every time the subject changes. + */ + virtual void UpdateNodes() = 0; + +protected: + std::weak_ptr node_values_; +}; +} // namespace trajopt_ifopt +#endif // TRAJOPT_IFOPT_TRAJOPT_NODES_OBSERVER_H diff --git a/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/nodes_variables.h b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/nodes_variables.h new file mode 100644 index 00000000..cca5ca10 --- /dev/null +++ b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/nodes_variables.h @@ -0,0 +1,143 @@ +/****************************************************************************** +Copyright (c) 2018, 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_TRAJOPT_NODES_VARIABLES_H +#define TRAJOPT_IFOPT_TRAJOPT_NODES_VARIABLES_H + +#include +TRAJOPT_IGNORE_WARNINGS_PUSH +#include +#include +#include +TRAJOPT_IGNORE_WARNINGS_POP + +#include + +namespace trajopt_ifopt +{ +class NodesObserver; + +class NodesVariables : public ifopt::VariableSet +{ +public: + using Ptr = std::shared_ptr; + + /** + * @brief Add node to the variable set + * @param node The node to append + */ + void AddNode(Node node); + + /** + * @brief Get node based on index + * @param opt_idx The node index + * @return The node + */ + const Node& GetNode(std::size_t opt_idx) const; + + /** + * @brief Pure optimization variables that define the nodes. + * + * Not all node position and velocities are independent or optimized over, so + * usually the number of optimization variables is less than all nodes' pos/vel. + * + * @sa GetNodeInfoAtOptIndex() + */ + VectorXd GetValues() const override; + + /** + * @brief Sets some node positions and velocity from the optimization variables. + * @param x The optimization variables. + * + * Not all node position and velocities are independent or optimized over, so + * usually the number of optimization variables is less than + * all nodes pos/vel. + * + * @sa GetNodeValuesInfo() + */ + void SetVariables(const VectorXd& x) override; + + /** + * @returns the bounds on position and velocity of each node and dimension. + */ + VecBound GetBounds() const override; + + /** + * @returns All the nodes that can be used to reconstruct the spline. + */ + const std::vector GetNodes() const; + + /** + * @brief Adds a dependent observer that gets notified when the nodes change. + * @param spline Usually a pointer to a spline which uses the node values. + */ + void AddObserver(std::shared_ptr observer); + + /** + * @returns The dimensions (x,y,z) of every node. + */ + Eigen::Index GetDim() const; + +protected: + /** + * @param n_dim The number of dimensions (x,y,..) each node has. + * @param variable_name The name of the variables in the optimization problem. + */ + NodesVariables(const std::string& variable_name); + virtual ~NodesVariables() = default; + + Eigen::VectorXd values_; + VecBound bounds_; ///< the bounds on the node values. + std::vector> nodes_; + Eigen::Index n_dim_{ -1 }; + std::vector> observers_; + + /** @brief Notifies the subscribed observers that the node values changes. */ + void UpdateObservers(); + + // /** + // * @brief Bounds a specific node variables. + // * @param node_id The ID of the node to bound. + // * @param deriv The derivative of the node to set. + // * @param dim The dimension of the node to bound. + // * @param values The values to set the bounds to. + // */ + // void AddBounds(int node_id, Dx deriv, const std::vector& dim, + // const VectorXd& values); + // /** + // * @brief Restricts a specific optimization variables. + // * @param node_info The specs of the optimization variables to restrict. + // * @param value The value to set the bounds to. + // */ + // void AddBound(const NodeValueInfo& node_info, double value); +}; + +} // namespace trajopt_ifopt + +#endif // TRAJOPT_IFOPT_TRAJOPT_NODES_VARIABLES_H diff --git a/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/var.h b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/var.h new file mode 100644 index 00000000..f5eacd26 --- /dev/null +++ b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/var.h @@ -0,0 +1,159 @@ +#ifndef TRAJOPT_IFOPT_TRAJOPT_VAR_H +#define TRAJOPT_IFOPT_TRAJOPT_VAR_H + +#include +#include +#include + +namespace trajopt_ifopt +{ +// struct VarRep +// { +// VarRep(Eigen::Index _index, std::string _name); +// VarRep(Eigen::Index _index, Eigen::Index _length, std::string _name, std::vector _child_names); + +// Eigen::Index index{-1}; +// Eigen::Index length{-1}; +// std::string name; +// std::vector child_names; +// }; + +// class Var +// { +// public: +// Var() = default; +// ~Var() = default; +// Var(std::shared_ptr var_rep); +// Var(const Var& other) = default; +// Var& operator=(const Var&) = default; +// Var(Var&&) = default; +// Var& operator=(Var&&) = default; + +// template +// T value(const Eigen::VectorXd& x) const +// { +// throw std::runtime_error("This should never be used"); +// } + +// template +// T name() const +// { +// throw std::runtime_error("This should never be used"); +// } + +// private: +// friend class Node; +// std::shared_ptr var_rep_{ nullptr }; +// }; + +// template<> +// double Var::value(const Eigen::VectorXd& x) const +// { +// assert(var_rep_->index > -1 && var_rep_->index < x.size()); +// assert(var_rep_->child_names.empty()); +// return x[var_rep_->index]; +// } + +// template<> +// Eigen::VectorXd Var::value(const Eigen::VectorXd& x) const +// { +// assert(!var_rep_->child_names.empty()); +// assert(var_rep_->index > -1 && var_rep_->index < x.size()); +// assert(var_rep_->length > -1 && (var_rep_->index + var_rep_->length) < x.size()); +// return x.segment(var_rep_->index, var_rep_->length); +// } + +// template<> +// std::string Var::name() const +// { +// assert(var_rep_->child_names.empty()); +// return var_rep_->name; +// } + +// template<> +// std::vector Var::name() const +// { +// assert(!var_rep_->child_names.empty()); +// return var_rep_->child_names; +// } + +struct VarRep +{ + VarRep(Eigen::Index _index, std::string _name); + VarRep(Eigen::Index _index, Eigen::Index _length, std::string _identifier, std::vector _names); + + Eigen::Index index{ -1 }; + Eigen::Index length{ -1 }; + std::string identifier; + std::vector names; + Eigen::VectorXd values; +}; + +class Var +{ +public: + Var() = default; + ~Var() = default; + Var(std::unique_ptr var_rep); + Var(const Var& other) = default; + Var& operator=(const Var&) = default; + Var(Var&&) = default; + Var& operator=(Var&&) = default; + + template + T value() const + { + throw std::runtime_error("This should never be used"); + } + + template + T name() const + { + throw std::runtime_error("This should never be used"); + } + + void setVariables(const Eigen::Ref& x) + { + assert(var_rep_->index > -1 && var_rep_->index < x.size()); + assert(var_rep_->length > 0 && (var_rep_->index + var_rep_->length) < x.size()); + var_rep_->values = x.segment(var_rep_->index, var_rep_->length); + } + +private: + std::shared_ptr var_rep_{ nullptr }; +}; + +template <> +inline double Var::value() const +{ + assert(var_rep_->values.size() == 1); + assert(var_rep_->index > -1); + assert(var_rep_->length == 1); + return var_rep_->values[0]; +} + +template <> +inline Eigen::VectorXd Var::value() const +{ + assert(var_rep_->index > -1); + assert(var_rep_->length > 1); + assert(var_rep_->names.size() > 1); + return var_rep_->values; +} + +template <> +inline std::string Var::name() const +{ + assert(var_rep_->names.size() == 1); + return var_rep_->names[0]; +} + +template <> +inline std::vector Var::name() const +{ + assert(var_rep_->names.size() > 1); + return var_rep_->names; +} +} // namespace trajopt_ifopt + +#endif // TRAJOPT_IFOPT_TRAJOPT_VAR_H diff --git a/trajopt_ifopt/trajopt/src/variable_sets/node.cpp b/trajopt_ifopt/trajopt/src/variable_sets/node.cpp new file mode 100644 index 00000000..404e0d50 --- /dev/null +++ b/trajopt_ifopt/trajopt/src/variable_sets/node.cpp @@ -0,0 +1,35 @@ +#include +#include + +namespace trajopt_ifopt +{ +Node::Node(std::string node_name) : name_(std::move(node_name)) {} + +Var Node::addVar(const std::string& name, const std::vector& child_names) +{ + Var var(std::make_unique(length_, child_names.size(), name, child_names)); + vars_[name] = var; + length_ += static_cast(child_names.size()); + return var; +} + +Var Node::addVar(const std::string& name) +{ + Var var(std::make_unique(length_, name)); + vars_[name] = var; + length_ += 1; + return var; +} + +Var Node::getVar(const std::string& name) const { return vars_.at(name); } + +bool Node::hasVar(const std::string& name) const { return (vars_.find(name) != vars_.end()); } + +Eigen::Index Node::size() const { return length_; } + +void Node::setVariables(const Eigen::Ref& x) +{ + for (auto& pair : vars_) + pair.second.setVariables(x); +} +} // namespace trajopt_ifopt diff --git a/trajopt_ifopt/trajopt/src/variable_sets/nodes_observer.cpp b/trajopt_ifopt/trajopt/src/variable_sets/nodes_observer.cpp new file mode 100644 index 00000000..d4238305 --- /dev/null +++ b/trajopt_ifopt/trajopt/src/variable_sets/nodes_observer.cpp @@ -0,0 +1,42 @@ +/****************************************************************************** +Copyright (c) 2018, 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. +******************************************************************************/ + +#include +#include + +namespace trajopt_ifopt +{ +NodesObserver::NodesObserver(std::weak_ptr subject) +{ + node_values_ = subject; + + // register observer to subject so this class always up-to-date + subject->AddObserver(this); +} +} // namespace trajopt_ifopt diff --git a/trajopt_ifopt/trajopt/src/variable_sets/nodes_variables.cpp b/trajopt_ifopt/trajopt/src/variable_sets/nodes_variables.cpp new file mode 100644 index 00000000..94bd4391 --- /dev/null +++ b/trajopt_ifopt/trajopt/src/variable_sets/nodes_variables.cpp @@ -0,0 +1,79 @@ +/****************************************************************************** +Copyright (c) 2018, 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. +******************************************************************************/ + +#include +#include + +namespace trajopt_ifopt +{ +NodesVariables::NodesVariables(const std::string& name) : VariableSet(kSpecifyLater, name) {} + +void NodesVariables::AddNode(Node node) +{ + Eigen::Index length = node.size(); + nodes_.emplace_back(n_dim_, std::move(node)); + n_dim_ += length; +} + +const Node& NodesVariables::GetNode(std::size_t opt_idx) const { return nodes_.at(opt_idx).second; } + +void NodesVariables::SetVariables(const VectorXd& x) +{ + for (auto& pair : nodes_) + pair.second.setVariables(x.segment(pair.first, pair.second.size())); + + values_ = x; + + UpdateObservers(); +} + +Eigen::VectorXd NodesVariables::GetValues() const { return values_; } + +void NodesVariables::UpdateObservers() +{ + for (auto& o : observers_) + o->UpdateNodes(); +} + +void NodesVariables::AddObserver(std::shared_ptr observer) { observers_.push_back(observer); } + +Eigen::Index NodesVariables::GetDim() const { return n_dim_; } + +NodesVariables::VecBound NodesVariables::GetBounds() const { return bounds_; } + +const std::vector NodesVariables::GetNodes() const +{ + std::vector nodes; + nodes.reserve(nodes_.size()); + for (const auto& pair : nodes_) + nodes.push_back(pair.second); + return nodes; +} + +} // namespace trajopt_ifopt diff --git a/trajopt_ifopt/trajopt/src/variable_sets/var.cpp b/trajopt_ifopt/trajopt/src/variable_sets/var.cpp new file mode 100644 index 00000000..28ab35f9 --- /dev/null +++ b/trajopt_ifopt/trajopt/src/variable_sets/var.cpp @@ -0,0 +1,27 @@ +#include + +namespace trajopt_ifopt +{ +// VarRep::VarRep(Eigen::Index _index, std::string _name) +// : index(_index), name(std::move(_name)) {} +// VarRep::VarRep(Eigen::Index _index, Eigen::Index _length, std::string _name, std::vector _child_names) +// : index(_index), length(_length), name(std::move(_name)), child_names(std::move(_child_names)) {} + +// Var::Var(std::shared_ptr var_rep) : var_rep_(std::move(var_rep)) {} + +VarRep::VarRep(Eigen::Index _index, std::string _name) + : index(_index), identifier(std::move(_name)), names({ identifier }), values(Eigen::VectorXd::Zero(1)) +{ +} + +VarRep::VarRep(Eigen::Index _index, Eigen::Index _length, std::string _identifier, std::vector _names) + : index(_index) + , length(_length) + , identifier(std::move(_identifier)) + , names(std::move(_names)) + , values(Eigen::VectorXd::Zero(static_cast(names.size()))) +{ +} + +Var::Var(std::unique_ptr var_rep) : var_rep_(std::move(var_rep)) {} +} // namespace trajopt_ifopt From a0991dfe50718039c189cffe0d4f3e50a8b8ceba Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Mon, 29 Apr 2024 22:16:37 -0500 Subject: [PATCH 3/5] fixup --- .../trajopt/variable_sets/node.h | 11 ++++++++ .../trajopt/variable_sets/nodes_variables.h | 6 ++--- .../trajopt_ifopt/trajopt/variable_sets/var.h | 8 ++++++ .../trajopt/src/variable_sets/node.cpp | 12 ++++++++- .../src/variable_sets/nodes_variables.cpp | 26 +++++++++---------- 5 files changed, 46 insertions(+), 17 deletions(-) diff --git a/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/node.h b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/node.h index 0a4ebec1..0ec24ae9 100644 --- a/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/node.h +++ b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/node.h @@ -3,14 +3,22 @@ #include #include +#include namespace trajopt_ifopt { +class NodesVariables; class Node { public: Node(std::string node_name = "Node"); + boost::uuids::uuid getUUID() const; + + const std::string& getName() const; + + NodesVariables* const getParent() const; + Var addVar(const std::string& name); Var addVar(const std::string& name, const std::vector& child_names); @@ -23,11 +31,14 @@ class Node protected: friend class NodesVariables; + boost::uuids::uuid uuid_{}; std::string name_; std::unordered_map vars_; Eigen::Index length_{ 0 }; + NodesVariables* parent_{ nullptr }; void setVariables(const Eigen::Ref& x); + void incrementIndex(Eigen::Index value); }; } // namespace trajopt_ifopt diff --git a/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/nodes_variables.h b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/nodes_variables.h index cca5ca10..16bb4c85 100644 --- a/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/nodes_variables.h +++ b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/nodes_variables.h @@ -91,7 +91,7 @@ class NodesVariables : public ifopt::VariableSet /** * @returns All the nodes that can be used to reconstruct the spline. */ - const std::vector GetNodes() const; + std::vector GetNodes() const; /** * @brief Adds a dependent observer that gets notified when the nodes change. @@ -114,8 +114,8 @@ class NodesVariables : public ifopt::VariableSet Eigen::VectorXd values_; VecBound bounds_; ///< the bounds on the node values. - std::vector> nodes_; - Eigen::Index n_dim_{ -1 }; + std::vector nodes_; + Eigen::Index n_dim_{ 0 }; std::vector> observers_; /** @brief Notifies the subscribed observers that the node values changes. */ diff --git a/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/var.h b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/var.h index f5eacd26..951cd6f8 100644 --- a/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/var.h +++ b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/var.h @@ -89,6 +89,10 @@ struct VarRep Eigen::VectorXd values; }; +/** + * @brief This is the class which the constraints should be storing + * @details This class contains all informtion necessary for filling the jacobian + */ class Var { public: @@ -100,6 +104,9 @@ class Var Var(Var&&) = default; Var& operator=(Var&&) = default; + Eigen::Index getIndex() const { return var_rep_->index; } + Eigen::Index getLength() const { return var_rep_->length; } + template T value() const { @@ -120,6 +127,7 @@ class Var } private: + friend class Node; std::shared_ptr var_rep_{ nullptr }; }; diff --git a/trajopt_ifopt/trajopt/src/variable_sets/node.cpp b/trajopt_ifopt/trajopt/src/variable_sets/node.cpp index 404e0d50..d0670fa0 100644 --- a/trajopt_ifopt/trajopt/src/variable_sets/node.cpp +++ b/trajopt_ifopt/trajopt/src/variable_sets/node.cpp @@ -1,10 +1,15 @@ #include +#include #include +#include namespace trajopt_ifopt { -Node::Node(std::string node_name) : name_(std::move(node_name)) {} +Node::Node(std::string node_name) : uuid_(boost::uuids::random_generator()()), name_(std::move(node_name)) {} +boost::uuids::uuid Node::getUUID() const { return uuid_; } +const std::string& Node::getName() const { return name_; } +NodesVariables* const Node::getParent() const { return parent_; } Var Node::addVar(const std::string& name, const std::vector& child_names) { Var var(std::make_unique(length_, child_names.size(), name, child_names)); @@ -32,4 +37,9 @@ void Node::setVariables(const Eigen::Ref& x) for (auto& pair : vars_) pair.second.setVariables(x); } +void Node::incrementIndex(Eigen::Index value) +{ + for (auto& pair : vars_) + pair.second.var_rep_->index += value; +} } // namespace trajopt_ifopt diff --git a/trajopt_ifopt/trajopt/src/variable_sets/nodes_variables.cpp b/trajopt_ifopt/trajopt/src/variable_sets/nodes_variables.cpp index 94bd4391..8267f42e 100644 --- a/trajopt_ifopt/trajopt/src/variable_sets/nodes_variables.cpp +++ b/trajopt_ifopt/trajopt/src/variable_sets/nodes_variables.cpp @@ -36,17 +36,24 @@ NodesVariables::NodesVariables(const std::string& name) : VariableSet(kSpecifyLa void NodesVariables::AddNode(Node node) { + auto it = + std::find_if(nodes_.begin(), nodes_.begin(), [&node](const Node& n) { return n.getUUID() == node.getUUID(); }); + if (it != nodes_.end()) + throw std::runtime_error("NodesVariables, Node must be unique!"); + + node.incrementIndex(n_dim_); + node.parent_ = this; Eigen::Index length = node.size(); - nodes_.emplace_back(n_dim_, std::move(node)); + nodes_.emplace_back(std::move(node)); n_dim_ += length; } -const Node& NodesVariables::GetNode(std::size_t opt_idx) const { return nodes_.at(opt_idx).second; } +const Node& NodesVariables::GetNode(std::size_t opt_idx) const { return nodes_.at(opt_idx); } void NodesVariables::SetVariables(const VectorXd& x) { - for (auto& pair : nodes_) - pair.second.setVariables(x.segment(pair.first, pair.second.size())); + for (auto& node : nodes_) + node.setVariables(x); values_ = x; @@ -61,19 +68,12 @@ void NodesVariables::UpdateObservers() o->UpdateNodes(); } -void NodesVariables::AddObserver(std::shared_ptr observer) { observers_.push_back(observer); } +void NodesVariables::AddObserver(std::shared_ptr observer) { observers_.push_back(std::move(observer)); } Eigen::Index NodesVariables::GetDim() const { return n_dim_; } NodesVariables::VecBound NodesVariables::GetBounds() const { return bounds_; } -const std::vector NodesVariables::GetNodes() const -{ - std::vector nodes; - nodes.reserve(nodes_.size()); - for (const auto& pair : nodes_) - nodes.push_back(pair.second); - return nodes; -} +std::vector NodesVariables::GetNodes() const { return nodes_; } } // namespace trajopt_ifopt From 3d806382a3e98f1e82da3cc09e510ccc00fd6384 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Tue, 30 Apr 2024 08:07:44 -0500 Subject: [PATCH 4/5] fixup --- .../trajopt/variable_sets/node.h | 25 ++++--- .../trajopt/variable_sets/nodes_variables.h | 8 +- .../trajopt_ifopt/trajopt/variable_sets/var.h | 75 +++++++++---------- .../trajopt/src/variable_sets/node.cpp | 21 +++--- .../src/variable_sets/nodes_variables.cpp | 25 ++++--- .../trajopt/src/variable_sets/var.cpp | 17 ++--- 6 files changed, 82 insertions(+), 89 deletions(-) diff --git a/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/node.h b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/node.h index 0ec24ae9..dda9e08a 100644 --- a/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/node.h +++ b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/node.h @@ -3,7 +3,6 @@ #include #include -#include namespace trajopt_ifopt { @@ -12,33 +11,35 @@ class Node { public: Node(std::string node_name = "Node"); - - boost::uuids::uuid getUUID() const; + Node(const Node&) = delete; + Node& operator=(const Node&) = delete; + Node(Node&&) = default; + Node& operator=(Node&&) = default; const std::string& getName() const; - NodesVariables* const getParent() const; + NodesVariables* getParent() const; - Var addVar(const std::string& name); + std::shared_ptr addVar(const std::string& name); - Var addVar(const std::string& name, const std::vector& child_names); + std::shared_ptr addVar(const std::string& name, const std::vector& child_names); - Var getVar(const std::string& name) const; + std::shared_ptr getVar(const std::string& name) const; bool hasVar(const std::string& name) const; Eigen::Index size() const; + void setVariables(const Eigen::Ref& x); + + void incrementIndex(Eigen::Index value); + protected: friend class NodesVariables; - boost::uuids::uuid uuid_{}; std::string name_; - std::unordered_map vars_; + std::unordered_map> vars_; Eigen::Index length_{ 0 }; NodesVariables* parent_{ nullptr }; - - void setVariables(const Eigen::Ref& x); - void incrementIndex(Eigen::Index value); }; } // namespace trajopt_ifopt diff --git a/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/nodes_variables.h b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/nodes_variables.h index 16bb4c85..c60334ad 100644 --- a/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/nodes_variables.h +++ b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/nodes_variables.h @@ -52,14 +52,14 @@ class NodesVariables : public ifopt::VariableSet * @brief Add node to the variable set * @param node The node to append */ - void AddNode(Node node); + void AddNode(std::unique_ptr node); /** * @brief Get node based on index * @param opt_idx The node index * @return The node */ - const Node& GetNode(std::size_t opt_idx) const; + const std::shared_ptr& GetNode(std::size_t opt_idx) const; /** * @brief Pure optimization variables that define the nodes. @@ -91,7 +91,7 @@ class NodesVariables : public ifopt::VariableSet /** * @returns All the nodes that can be used to reconstruct the spline. */ - std::vector GetNodes() const; + std::vector> GetNodes() const; /** * @brief Adds a dependent observer that gets notified when the nodes change. @@ -114,7 +114,7 @@ class NodesVariables : public ifopt::VariableSet Eigen::VectorXd values_; VecBound bounds_; ///< the bounds on the node values. - std::vector nodes_; + std::vector> nodes_; Eigen::Index n_dim_{ 0 }; std::vector> observers_; diff --git a/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/var.h b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/var.h index 951cd6f8..d0c221a9 100644 --- a/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/var.h +++ b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/var.h @@ -77,18 +77,6 @@ namespace trajopt_ifopt // return var_rep_->child_names; // } -struct VarRep -{ - VarRep(Eigen::Index _index, std::string _name); - VarRep(Eigen::Index _index, Eigen::Index _length, std::string _identifier, std::vector _names); - - Eigen::Index index{ -1 }; - Eigen::Index length{ -1 }; - std::string identifier; - std::vector names; - Eigen::VectorXd values; -}; - /** * @brief This is the class which the constraints should be storing * @details This class contains all informtion necessary for filling the jacobian @@ -98,69 +86,74 @@ class Var public: Var() = default; ~Var() = default; - Var(std::unique_ptr var_rep); + Var(Eigen::Index index, std::string name); + Var(Eigen::Index index, Eigen::Index length, std::string identifier, std::vector names); Var(const Var& other) = default; Var& operator=(const Var&) = default; Var(Var&&) = default; Var& operator=(Var&&) = default; - Eigen::Index getIndex() const { return var_rep_->index; } - Eigen::Index getLength() const { return var_rep_->length; } + Eigen::Index getIndex() const { return index_; } + Eigen::Index size() const { return length_; } - template - T value() const + void incrementIndex(Eigen::Index value) { index_ += value; } + void setVariables(const Eigen::Ref& x) { - throw std::runtime_error("This should never be used"); + assert(index_ > -1 && index_ < x.size()); + assert(length_ > 0 && (index_ + length_) < x.size()); + values_ = x.segment(index_, length_); } template - T name() const + const T& value() const { throw std::runtime_error("This should never be used"); } - void setVariables(const Eigen::Ref& x) + template + const T& name() const { - assert(var_rep_->index > -1 && var_rep_->index < x.size()); - assert(var_rep_->length > 0 && (var_rep_->index + var_rep_->length) < x.size()); - var_rep_->values = x.segment(var_rep_->index, var_rep_->length); + throw std::runtime_error("This should never be used"); } private: - friend class Node; - std::shared_ptr var_rep_{ nullptr }; + Eigen::Index index_{ -1 }; + Eigen::Index length_{ -1 }; + std::string identifier_; + std::vector names_; + Eigen::VectorXd values_; }; template <> -inline double Var::value() const +inline const double& Var::value() const { - assert(var_rep_->values.size() == 1); - assert(var_rep_->index > -1); - assert(var_rep_->length == 1); - return var_rep_->values[0]; + assert(values_.size() == 1); + assert(index_ > -1); + assert(length_ == 1); + return values_[0]; } template <> -inline Eigen::VectorXd Var::value() const +inline const Eigen::VectorXd& Var::value() const { - assert(var_rep_->index > -1); - assert(var_rep_->length > 1); - assert(var_rep_->names.size() > 1); - return var_rep_->values; + assert(index_ > -1); + assert(length_ > 1); + assert(names_.size() > 1); + return values_; } template <> -inline std::string Var::name() const +inline const std::string& Var::name() const { - assert(var_rep_->names.size() == 1); - return var_rep_->names[0]; + assert(names_.size() == 1); + return names_[0]; } template <> -inline std::vector Var::name() const +inline const std::vector& Var::name() const { - assert(var_rep_->names.size() > 1); - return var_rep_->names; + assert(names_.size() > 1); + return names_; } } // namespace trajopt_ifopt diff --git a/trajopt_ifopt/trajopt/src/variable_sets/node.cpp b/trajopt_ifopt/trajopt/src/variable_sets/node.cpp index d0670fa0..a665a6cc 100644 --- a/trajopt_ifopt/trajopt/src/variable_sets/node.cpp +++ b/trajopt_ifopt/trajopt/src/variable_sets/node.cpp @@ -1,32 +1,31 @@ #include #include #include -#include namespace trajopt_ifopt { -Node::Node(std::string node_name) : uuid_(boost::uuids::random_generator()()), name_(std::move(node_name)) {} +Node::Node(std::string node_name) : name_(std::move(node_name)) {} -boost::uuids::uuid Node::getUUID() const { return uuid_; } const std::string& Node::getName() const { return name_; } -NodesVariables* const Node::getParent() const { return parent_; } -Var Node::addVar(const std::string& name, const std::vector& child_names) +trajopt_ifopt::NodesVariables* Node::getParent() const { return parent_; } +std::shared_ptr Node::addVar(const std::string& name, + const std::vector& child_names) { - Var var(std::make_unique(length_, child_names.size(), name, child_names)); + auto var = std::make_shared(length_, child_names.size(), name, child_names); vars_[name] = var; length_ += static_cast(child_names.size()); return var; } -Var Node::addVar(const std::string& name) +std::shared_ptr Node::addVar(const std::string& name) { - Var var(std::make_unique(length_, name)); + auto var = std::make_shared(length_, name); vars_[name] = var; length_ += 1; return var; } -Var Node::getVar(const std::string& name) const { return vars_.at(name); } +std::shared_ptr Node::getVar(const std::string& name) const { return vars_.at(name); } bool Node::hasVar(const std::string& name) const { return (vars_.find(name) != vars_.end()); } @@ -35,11 +34,11 @@ Eigen::Index Node::size() const { return length_; } void Node::setVariables(const Eigen::Ref& x) { for (auto& pair : vars_) - pair.second.setVariables(x); + pair.second->setVariables(x); } void Node::incrementIndex(Eigen::Index value) { for (auto& pair : vars_) - pair.second.var_rep_->index += value; + pair.second->incrementIndex(value); } } // namespace trajopt_ifopt diff --git a/trajopt_ifopt/trajopt/src/variable_sets/nodes_variables.cpp b/trajopt_ifopt/trajopt/src/variable_sets/nodes_variables.cpp index 8267f42e..2b8b6525 100644 --- a/trajopt_ifopt/trajopt/src/variable_sets/nodes_variables.cpp +++ b/trajopt_ifopt/trajopt/src/variable_sets/nodes_variables.cpp @@ -34,26 +34,21 @@ namespace trajopt_ifopt { NodesVariables::NodesVariables(const std::string& name) : VariableSet(kSpecifyLater, name) {} -void NodesVariables::AddNode(Node node) +void NodesVariables::AddNode(std::unique_ptr node) { - auto it = - std::find_if(nodes_.begin(), nodes_.begin(), [&node](const Node& n) { return n.getUUID() == node.getUUID(); }); - if (it != nodes_.end()) - throw std::runtime_error("NodesVariables, Node must be unique!"); - - node.incrementIndex(n_dim_); - node.parent_ = this; - Eigen::Index length = node.size(); + node->incrementIndex(n_dim_); + node->parent_ = this; + Eigen::Index length = node->size(); nodes_.emplace_back(std::move(node)); n_dim_ += length; } -const Node& NodesVariables::GetNode(std::size_t opt_idx) const { return nodes_.at(opt_idx); } +const std::shared_ptr& NodesVariables::GetNode(std::size_t opt_idx) const { return nodes_.at(opt_idx); } void NodesVariables::SetVariables(const VectorXd& x) { for (auto& node : nodes_) - node.setVariables(x); + node->setVariables(x); values_ = x; @@ -74,6 +69,12 @@ Eigen::Index NodesVariables::GetDim() const { return n_dim_; } NodesVariables::VecBound NodesVariables::GetBounds() const { return bounds_; } -std::vector NodesVariables::GetNodes() const { return nodes_; } +std::vector > NodesVariables::GetNodes() const +{ + std::vector > nodes; + nodes.reserve(nodes_.size()); + std::copy(nodes_.begin(), nodes_.end(), std::back_inserter(nodes)); + return nodes; +} } // namespace trajopt_ifopt diff --git a/trajopt_ifopt/trajopt/src/variable_sets/var.cpp b/trajopt_ifopt/trajopt/src/variable_sets/var.cpp index 28ab35f9..d3a45acd 100644 --- a/trajopt_ifopt/trajopt/src/variable_sets/var.cpp +++ b/trajopt_ifopt/trajopt/src/variable_sets/var.cpp @@ -9,19 +9,18 @@ namespace trajopt_ifopt // Var::Var(std::shared_ptr var_rep) : var_rep_(std::move(var_rep)) {} -VarRep::VarRep(Eigen::Index _index, std::string _name) - : index(_index), identifier(std::move(_name)), names({ identifier }), values(Eigen::VectorXd::Zero(1)) +Var::Var(Eigen::Index index, std::string name) + : index_(index), identifier_(std::move(name)), names_({ identifier_ }), values_(Eigen::VectorXd::Zero(1)) { } -VarRep::VarRep(Eigen::Index _index, Eigen::Index _length, std::string _identifier, std::vector _names) - : index(_index) - , length(_length) - , identifier(std::move(_identifier)) - , names(std::move(_names)) - , values(Eigen::VectorXd::Zero(static_cast(names.size()))) +Var::Var(Eigen::Index index, Eigen::Index length, std::string identifier, std::vector names) + : index_(index) + , length_(length) + , identifier_(std::move(identifier)) + , names_(std::move(names)) + , values_(Eigen::VectorXd::Zero(static_cast(names.size()))) { } -Var::Var(std::unique_ptr var_rep) : var_rep_(std::move(var_rep)) {} } // namespace trajopt_ifopt From b5c4e64b46a22ab3d06fe35c0131a658d5329a6c Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Tue, 28 May 2024 17:08:15 -0500 Subject: [PATCH 5/5] fixup --- .../trajopt/variable_sets/nodes_observer.h | 5 ++--- .../trajopt_ifopt/trajopt/variable_sets/var.h | 13 ++++--------- trajopt_ifopt/trajopt/src/variable_sets/var.cpp | 11 +++++++++++ 3 files changed, 17 insertions(+), 12 deletions(-) diff --git a/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/nodes_observer.h b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/nodes_observer.h index 3a1ba2b3..c9a611a8 100644 --- a/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/nodes_observer.h +++ b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/nodes_observer.h @@ -39,9 +39,8 @@ class NodesVariables; /** * @brief Base class to receive up-to-date values of the NodeVariables. * - * This class registers with the node variables and everytime the positions or - * velocities of a node change, the subject updates this class by calling the - * UpdatePolynomials() method. + * This class registers with the node variables and everytime a node changes, + * the subject updates this class by calling the UpdatePolynomials() method. * * Used by spline.h * diff --git a/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/var.h b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/var.h index d0c221a9..9f1becf3 100644 --- a/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/var.h +++ b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/var.h @@ -93,16 +93,11 @@ class Var Var(Var&&) = default; Var& operator=(Var&&) = default; - Eigen::Index getIndex() const { return index_; } - Eigen::Index size() const { return length_; } + Eigen::Index getIndex() const; + Eigen::Index size() const; - void incrementIndex(Eigen::Index value) { index_ += value; } - void setVariables(const Eigen::Ref& x) - { - assert(index_ > -1 && index_ < x.size()); - assert(length_ > 0 && (index_ + length_) < x.size()); - values_ = x.segment(index_, length_); - } + void incrementIndex(Eigen::Index value); + void setVariables(const Eigen::Ref& x); template const T& value() const diff --git a/trajopt_ifopt/trajopt/src/variable_sets/var.cpp b/trajopt_ifopt/trajopt/src/variable_sets/var.cpp index d3a45acd..d9795d09 100644 --- a/trajopt_ifopt/trajopt/src/variable_sets/var.cpp +++ b/trajopt_ifopt/trajopt/src/variable_sets/var.cpp @@ -23,4 +23,15 @@ Var::Var(Eigen::Index index, Eigen::Index length, std::string identifier, std::v { } +Eigen::Index Var::getIndex() const { return index_; } +Eigen::Index Var::size() const { return length_; } + +void Var::incrementIndex(Eigen::Index value) { index_ += value; } +void Var::setVariables(const Eigen::Ref& x) +{ + assert(index_ > -1 && index_ < x.size()); + assert(length_ > 0 && (index_ + length_) < x.size()); + values_ = x.segment(index_, length_); +} + } // namespace trajopt_ifopt