From 91c809c964b021e958b72c6f5e726551e255aa36 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Mon, 2 Oct 2023 13:41:22 -0500 Subject: [PATCH] Move TrajOptIfopt collision gradient types and utils to trajopt_common package --- trajopt_common/CMakeLists.txt | 12 ++- .../cmake/trajopt_common-config.cmake.in | 4 + .../include/trajopt_common}/cache.h | 10 +-- .../include/trajopt_common}/collision_types.h | 14 ++-- .../include/trajopt_common}/collision_utils.h | 12 +-- trajopt_common/package.xml | 2 + .../src}/collision_types.cpp | 6 +- .../src}/collision_utils.cpp | 7 +- trajopt_ifopt/CMakeLists.txt | 2 - .../continuous_collision_evaluators.h | 84 ++++++++++--------- .../collision/discrete_collision_evaluators.h | 32 +++---- .../collision/weighted_average_methods.h | 6 +- .../include/trajopt_ifopt/trajopt_ifopt.h | 2 - .../continuous_collision_constraint.cpp | 14 ++-- .../continuous_collision_evaluators.cpp | 71 ++++++++-------- ...tinuous_collision_numerical_constraint.cpp | 16 ++-- .../discrete_collision_constraint.cpp | 12 +-- .../discrete_collision_evaluators.cpp | 37 ++++---- ...iscrete_collision_numerical_constraint.cpp | 12 +-- .../collision/weighted_average_methods.cpp | 4 +- trajopt_ifopt/test/cast_cost_unit.cpp | 4 +- trajopt_ifopt/test/collision_unit.cpp | 4 +- .../continuous_collision_gradient_unit.cpp | 4 +- .../test/discrete_collision_gradient_unit.cpp | 4 +- trajopt_ifopt/test/simple_collision_unit.cpp | 4 +- .../test/cast_cost_attached_unit.cpp | 8 +- .../test/cast_cost_octomap_unit.cpp | 4 +- .../trajopt_sqp/test/cast_cost_unit.cpp | 4 +- .../trajopt_sqp/test/cast_cost_world_unit.cpp | 4 +- .../trajopt_sqp/test/planning_unit.cpp | 4 +- .../test/simple_collision_unit.cpp | 18 ++-- 31 files changed, 222 insertions(+), 199 deletions(-) rename {trajopt_ifopt/include/trajopt_ifopt => trajopt_common/include/trajopt_common}/cache.h (85%) rename {trajopt_ifopt/include/trajopt_ifopt/constraints/collision => trajopt_common/include/trajopt_common}/collision_types.h (96%) rename {trajopt_ifopt/include/trajopt_ifopt/constraints/collision => trajopt_common/include/trajopt_common}/collision_utils.h (94%) rename {trajopt_ifopt/src/constraints/collision => trajopt_common/src}/collision_types.cpp (98%) rename {trajopt_ifopt/src/constraints/collision => trajopt_common/src}/collision_utils.cpp (98%) diff --git a/trajopt_common/CMakeLists.txt b/trajopt_common/CMakeLists.txt index 4c058afd..caf931e9 100644 --- a/trajopt_common/CMakeLists.txt +++ b/trajopt_common/CMakeLists.txt @@ -15,6 +15,8 @@ find_package(Eigen3 REQUIRED) find_package(Boost COMPONENTS program_options REQUIRED) find_package(ros_industrial_cmake_boilerplate REQUIRED) find_package(tesseract_common REQUIRED) +find_package(tesseract_collision COMPONENTS core REQUIRED) +find_package(tesseract_kinematics COMPONENTS core REQUIRED) # Load variable for clang tidy args, compiler options and cxx version trajopt_variables() @@ -22,12 +24,20 @@ trajopt_variables() set(UTILS_SOURCE_FILES src/stl_to_string.cpp src/clock.cpp + src/collision_types.cpp + src/collision_utils.cpp src/config.cpp src/logging.cpp src/utils.cpp) add_library(${PROJECT_NAME} ${UTILS_SOURCE_FILES}) -target_link_libraries(${PROJECT_NAME} PUBLIC Eigen3::Eigen Boost::program_options tesseract::tesseract_common) +target_link_libraries( + ${PROJECT_NAME} + PUBLIC Eigen3::Eigen + Boost::program_options + tesseract::tesseract_common + tesseract::tesseract_collision_core + tesseract::tesseract_kinematics_core) 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} diff --git a/trajopt_common/cmake/trajopt_common-config.cmake.in b/trajopt_common/cmake/trajopt_common-config.cmake.in index 0277eb7a..8c53f908 100644 --- a/trajopt_common/cmake/trajopt_common-config.cmake.in +++ b/trajopt_common/cmake/trajopt_common-config.cmake.in @@ -8,8 +8,12 @@ include(CMakeFindDependencyMacro) find_dependency(Eigen3) if(${CMAKE_VERSION} VERSION_LESS "3.15.0") find_package(Boost COMPONENTS program_options REQUIRED) + find_package(tesseract_collision COMPONENTS core REQUIRED) + find_package(tesseract_kinematics COMPONENTS core REQUIRED) else() find_dependency(Boost COMPONENTS program_options) + find_dependency(tesseract_collision COMPONENTS core) + find_dependency(tesseract_kinematics COMPONENTS core) endif() find_dependency(tesseract_common) diff --git a/trajopt_ifopt/include/trajopt_ifopt/cache.h b/trajopt_common/include/trajopt_common/cache.h similarity index 85% rename from trajopt_ifopt/include/trajopt_ifopt/cache.h rename to trajopt_common/include/trajopt_common/cache.h index 4a4fefac..644170c1 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/cache.h +++ b/trajopt_common/include/trajopt_common/cache.h @@ -1,5 +1,5 @@ -#ifndef TRAJOPT_IFOPT_CACHE_H -#define TRAJOPT_IFOPT_CACHE_H +#ifndef TRAJOPT_COMMON_CACHE_H +#define TRAJOPT_COMMON_CACHE_H #include TRAJOPT_IGNORE_WARNINGS_PUSH @@ -9,7 +9,7 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include TRAJOPT_IGNORE_WARNINGS_POP -namespace trajopt_ifopt +namespace trajopt_common { template class Cache @@ -41,5 +41,5 @@ class Cache std::vector valbuf_; // circular buffer }; -} // namespace trajopt_ifopt -#endif // TRAJOPT_IFOPT_CACHE_H +} // namespace trajopt_common +#endif // TRAJOPT_COMMON_CACHE_H diff --git a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/collision_types.h b/trajopt_common/include/trajopt_common/collision_types.h similarity index 96% rename from trajopt_ifopt/include/trajopt_ifopt/constraints/collision/collision_types.h rename to trajopt_common/include/trajopt_common/collision_types.h index 149bff8c..e67c818c 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/collision_types.h +++ b/trajopt_common/include/trajopt_common/collision_types.h @@ -21,8 +21,8 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -#ifndef TRAJOPT_IFOPT_COLLISION_TYPES_H -#define TRAJOPT_IFOPT_COLLISION_TYPES_H +#ifndef TRAJOPT_COMMON_COLLISION_TYPES_H +#define TRAJOPT_COMMON_COLLISION_TYPES_H #include TRAJOPT_IGNORE_WARNINGS_PUSH @@ -30,13 +30,11 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include #include #include -#include -#include TRAJOPT_IGNORE_WARNINGS_POP -#include +#include -namespace trajopt_ifopt +namespace trajopt_common { using GetStateFn = std::function& joint_values)>; @@ -266,5 +264,5 @@ struct CollisionCacheData using CollisionCache = Cache; -} // namespace trajopt_ifopt -#endif // TRAJOPT_IFOPT_COLLISION_TYPES_H +} // namespace trajopt_common +#endif // TRAJOPT_COMMON_COLLISION_TYPES_H diff --git a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/collision_utils.h b/trajopt_common/include/trajopt_common/collision_utils.h similarity index 94% rename from trajopt_ifopt/include/trajopt_ifopt/constraints/collision/collision_utils.h rename to trajopt_common/include/trajopt_common/collision_utils.h index 28e5bba4..9af20305 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/collision_utils.h +++ b/trajopt_common/include/trajopt_common/collision_utils.h @@ -21,8 +21,8 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -#ifndef TRAJOPT_IFOPT_COLLISION_UTILS_H -#define TRAJOPT_IFOPT_COLLISION_UTILS_H +#ifndef TRAJOPT_COMMON_COLLISION_UTILS_H +#define TRAJOPT_COMMON_COLLISION_UTILS_H #include TRAJOPT_IGNORE_WARNINGS_PUSH @@ -31,9 +31,9 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include TRAJOPT_IGNORE_WARNINGS_POP -#include +#include -namespace trajopt_ifopt +namespace trajopt_common { std::size_t getHash(const TrajOptCollisionConfig& collision_config, const Eigen::Ref& dof_vals); std::size_t getHash(const TrajOptCollisionConfig& collision_config, @@ -94,5 +94,5 @@ void debugPrintInfo(const tesseract_collision::ContactResult& res, const Eigen::VectorXd& dof_vals, bool header = false); -} // namespace trajopt_ifopt -#endif // TRAJOPT_IFOPT_COLLISION_UTILS_H +} // namespace trajopt_common +#endif // TRAJOPT_COMMON_COLLISION_UTILS_H diff --git a/trajopt_common/package.xml b/trajopt_common/package.xml index 0c1a4222..218f5dcf 100644 --- a/trajopt_common/package.xml +++ b/trajopt_common/package.xml @@ -18,6 +18,8 @@ libboost-program-options tesseract_common + tesseract_collision + tesseract_kinematics cmake diff --git a/trajopt_ifopt/src/constraints/collision/collision_types.cpp b/trajopt_common/src/collision_types.cpp similarity index 98% rename from trajopt_ifopt/src/constraints/collision/collision_types.cpp rename to trajopt_common/src/collision_types.cpp index 0140fc53..00b6a3cd 100644 --- a/trajopt_ifopt/src/constraints/collision/collision_types.cpp +++ b/trajopt_common/src/collision_types.cpp @@ -22,9 +22,9 @@ * limitations under the License. */ -#include +#include -namespace trajopt_ifopt +namespace trajopt_common { CollisionCoeffData::CollisionCoeffData(double default_collision_coeff) : default_collision_coeff_(default_collision_coeff) @@ -260,4 +260,4 @@ double GradientResultsSet::getMaxErrorWithBufferT1() const return e; } -} // namespace trajopt_ifopt +} // namespace trajopt_common diff --git a/trajopt_ifopt/src/constraints/collision/collision_utils.cpp b/trajopt_common/src/collision_utils.cpp similarity index 98% rename from trajopt_ifopt/src/constraints/collision/collision_utils.cpp rename to trajopt_common/src/collision_utils.cpp index 2442e341..c4c7265a 100644 --- a/trajopt_ifopt/src/constraints/collision/collision_utils.cpp +++ b/trajopt_common/src/collision_utils.cpp @@ -26,13 +26,12 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include #include -#include #include TRAJOPT_IGNORE_WARNINGS_POP -#include +#include -namespace trajopt_ifopt +namespace trajopt_common { std::size_t getHash(const TrajOptCollisionConfig& collision_config, const Eigen::Ref& dof_vals) { @@ -318,4 +317,4 @@ void debugPrintInfo(const tesseract_collision::ContactResult& res, std::printf("\n"); } -} // namespace trajopt_ifopt +} // namespace trajopt_common diff --git a/trajopt_ifopt/CMakeLists.txt b/trajopt_ifopt/CMakeLists.txt index 0ed354c1..ce04396d 100644 --- a/trajopt_ifopt/CMakeLists.txt +++ b/trajopt_ifopt/CMakeLists.txt @@ -46,9 +46,7 @@ set(TRAJOPT_IFOPT_SOURCE_FILES src/constraints/joint_jerk_constraint.cpp src/constraints/joint_position_constraint.cpp src/constraints/joint_velocity_constraint.cpp - src/constraints/collision/collision_types.cpp src/constraints/cartesian_line_constraint.cpp - src/constraints/collision/collision_utils.cpp src/constraints/collision/discrete_collision_evaluators.cpp src/constraints/collision/continuous_collision_evaluators.cpp src/constraints/collision/weighted_average_methods.cpp diff --git a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/continuous_collision_evaluators.h b/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/continuous_collision_evaluators.h index 193b69f1..31f5aa66 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/continuous_collision_evaluators.h +++ b/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/continuous_collision_evaluators.h @@ -35,7 +35,7 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include TRAJOPT_IGNORE_WARNINGS_POP -#include +#include #include namespace trajopt_ifopt @@ -66,10 +66,11 @@ class ContinuousCollisionEvaluator * @return Collision cache data. If a cache does not exist for the provided joint values it evaluates and stores the * data. */ - virtual CollisionCacheData::ConstPtr CalcCollisionData(const Eigen::Ref& dof_vals0, - const Eigen::Ref& dof_vals1, - const std::array& position_vars_fixed, - std::size_t bounds_size) = 0; + virtual trajopt_common::CollisionCacheData::ConstPtr + CalcCollisionData(const Eigen::Ref& dof_vals0, + const Eigen::Ref& dof_vals1, + const std::array& position_vars_fixed, + std::size_t bounds_size) = 0; /** * @brief Extracts the gradient information based on the contact results for the transition between dof_vals0 and @@ -80,15 +81,16 @@ class ContinuousCollisionEvaluator * data * @return The gradient results */ - virtual GradientResults CalcGradientData(const Eigen::Ref& dof_vals0, - const Eigen::Ref& dof_vals1, - const tesseract_collision::ContactResult& contact_results) = 0; + virtual trajopt_common::GradientResults + CalcGradientData(const Eigen::Ref& dof_vals0, + const Eigen::Ref& dof_vals1, + const tesseract_collision::ContactResult& contact_results) = 0; /** * @brief Get the safety margin information. * @return Safety margin information */ - virtual const TrajOptCollisionConfig& GetCollisionConfig() const = 0; + virtual const trajopt_common::TrajOptCollisionConfig& GetCollisionConfig() const = 0; }; /** @@ -103,37 +105,39 @@ class LVSContinuousCollisionEvaluator : public ContinuousCollisionEvaluator using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - LVSContinuousCollisionEvaluator(std::shared_ptr collision_cache, + LVSContinuousCollisionEvaluator(std::shared_ptr collision_cache, tesseract_kinematics::JointGroup::ConstPtr manip, tesseract_environment::Environment::ConstPtr env, - TrajOptCollisionConfig::ConstPtr collision_config, + trajopt_common::TrajOptCollisionConfig::ConstPtr collision_config, bool dynamic_environment = false); - CollisionCacheData::ConstPtr CalcCollisionData(const Eigen::Ref& dof_vals0, - const Eigen::Ref& dof_vals1, - const std::array& position_vars_fixed, - std::size_t bounds_size) override final; + trajopt_common::CollisionCacheData::ConstPtr CalcCollisionData(const Eigen::Ref& dof_vals0, + const Eigen::Ref& dof_vals1, + const std::array& position_vars_fixed, + std::size_t bounds_size) override final; - GradientResults CalcGradientData(const Eigen::Ref& dof_vals0, - const Eigen::Ref& dof_vals1, - const tesseract_collision::ContactResult& contact_results) override final; + trajopt_common::GradientResults + CalcGradientData(const Eigen::Ref& dof_vals0, + const Eigen::Ref& dof_vals1, + const tesseract_collision::ContactResult& contact_results) override final; - const TrajOptCollisionConfig& GetCollisionConfig() const override final; + const trajopt_common::TrajOptCollisionConfig& GetCollisionConfig() const override final; private: - std::shared_ptr collision_cache_; + std::shared_ptr collision_cache_; tesseract_kinematics::JointGroup::ConstPtr manip_; tesseract_environment::Environment::ConstPtr env_; - TrajOptCollisionConfig::ConstPtr collision_config_; + trajopt_common::TrajOptCollisionConfig::ConstPtr collision_config_; std::vector env_active_link_names_; std::vector manip_active_link_names_; std::vector diff_active_link_names_; - GetStateFn get_state_fn_; + trajopt_common::GetStateFn get_state_fn_; bool dynamic_environment_; tesseract_collision::ContinuousContactManager::Ptr contact_manager_; - CollisionCacheData::ConstPtr CalcCollisionsCacheDataHelper(const Eigen::Ref& dof_vals0, - const Eigen::Ref& dof_vals1); + trajopt_common::CollisionCacheData::ConstPtr + CalcCollisionsCacheDataHelper(const Eigen::Ref& dof_vals0, + const Eigen::Ref& dof_vals1); void CalcCollisionsHelper(const Eigen::Ref& dof_vals0, const Eigen::Ref& dof_vals1, @@ -152,37 +156,39 @@ class LVSDiscreteCollisionEvaluator : public ContinuousCollisionEvaluator using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - LVSDiscreteCollisionEvaluator(std::shared_ptr collision_cache, + LVSDiscreteCollisionEvaluator(std::shared_ptr collision_cache, tesseract_kinematics::JointGroup::ConstPtr manip, tesseract_environment::Environment::ConstPtr env, - TrajOptCollisionConfig::ConstPtr collision_config, + trajopt_common::TrajOptCollisionConfig::ConstPtr collision_config, bool dynamic_environment = false); - CollisionCacheData::ConstPtr CalcCollisionData(const Eigen::Ref& dof_vals0, - const Eigen::Ref& dof_vals1, - const std::array& position_vars_fixed, - std::size_t bounds_size) override final; + trajopt_common::CollisionCacheData::ConstPtr CalcCollisionData(const Eigen::Ref& dof_vals0, + const Eigen::Ref& dof_vals1, + const std::array& position_vars_fixed, + std::size_t bounds_size) override final; - GradientResults CalcGradientData(const Eigen::Ref& dof_vals0, - const Eigen::Ref& dof_vals1, - const tesseract_collision::ContactResult& contact_results) override final; + trajopt_common::GradientResults + CalcGradientData(const Eigen::Ref& dof_vals0, + const Eigen::Ref& dof_vals1, + const tesseract_collision::ContactResult& contact_results) override final; - const TrajOptCollisionConfig& GetCollisionConfig() const override final; + const trajopt_common::TrajOptCollisionConfig& GetCollisionConfig() const override final; private: - std::shared_ptr collision_cache_; + std::shared_ptr collision_cache_; tesseract_kinematics::JointGroup::ConstPtr manip_; tesseract_environment::Environment::ConstPtr env_; - TrajOptCollisionConfig::ConstPtr collision_config_; + trajopt_common::TrajOptCollisionConfig::ConstPtr collision_config_; std::vector env_active_link_names_; std::vector manip_active_link_names_; std::vector diff_active_link_names_; - GetStateFn get_state_fn_; + trajopt_common::GetStateFn get_state_fn_; bool dynamic_environment_; tesseract_collision::DiscreteContactManager::Ptr contact_manager_; - CollisionCacheData::ConstPtr CalcCollisionsCacheDataHelper(const Eigen::Ref& dof_vals0, - const Eigen::Ref& dof_vals1); + trajopt_common::CollisionCacheData::ConstPtr + CalcCollisionsCacheDataHelper(const Eigen::Ref& dof_vals0, + const Eigen::Ref& dof_vals1); void CalcCollisionsHelper(const Eigen::Ref& dof_vals0, const Eigen::Ref& dof_vals1, diff --git a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/discrete_collision_evaluators.h b/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/discrete_collision_evaluators.h index 5b804361..b355efef 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/discrete_collision_evaluators.h +++ b/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/discrete_collision_evaluators.h @@ -34,7 +34,7 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include TRAJOPT_IGNORE_WARNINGS_POP -#include +#include #include namespace trajopt_ifopt @@ -64,14 +64,14 @@ class DiscreteCollisionEvaluator * @return Collision cache data. If a cache does not exist for the provided joint values it evaluates and stores the * data. */ - virtual CollisionCacheData::ConstPtr CalcCollisions(const Eigen::Ref& dof_vals, - std::size_t bounds_size) = 0; + virtual trajopt_common::CollisionCacheData::ConstPtr CalcCollisions(const Eigen::Ref& dof_vals, + std::size_t bounds_size) = 0; /** * @brief Get the safety margin information. * @return Safety margin information */ - virtual const TrajOptCollisionConfig& GetCollisionConfig() const = 0; + virtual const trajopt_common::TrajOptCollisionConfig& GetCollisionConfig() const = 0; /** * @brief Extracts the gradient information based on the contact results @@ -80,8 +80,8 @@ class DiscreteCollisionEvaluator * @param isTimestep1 Indicates if this is the second timestep when computing gradient for continuous collision * @return The gradient results */ - virtual GradientResults GetGradient(const Eigen::VectorXd& dofvals, - const tesseract_collision::ContactResult& contact_result) = 0; + virtual trajopt_common::GradientResults GetGradient(const Eigen::VectorXd& dofvals, + const tesseract_collision::ContactResult& contact_result) = 0; }; /** @@ -96,29 +96,29 @@ class SingleTimestepCollisionEvaluator : public DiscreteCollisionEvaluator using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - SingleTimestepCollisionEvaluator(std::shared_ptr collision_cache, + SingleTimestepCollisionEvaluator(std::shared_ptr collision_cache, tesseract_kinematics::JointGroup::ConstPtr manip, tesseract_environment::Environment::ConstPtr env, - TrajOptCollisionConfig::ConstPtr collision_config, + trajopt_common::TrajOptCollisionConfig::ConstPtr collision_config, bool dynamic_environment = false); - CollisionCacheData::ConstPtr CalcCollisions(const Eigen::Ref& dof_vals, - std::size_t bounds_size) override final; + trajopt_common::CollisionCacheData::ConstPtr CalcCollisions(const Eigen::Ref& dof_vals, + std::size_t bounds_size) override final; - GradientResults GetGradient(const Eigen::VectorXd& dofvals, - const tesseract_collision::ContactResult& contact_result) override; + trajopt_common::GradientResults GetGradient(const Eigen::VectorXd& dofvals, + const tesseract_collision::ContactResult& contact_result) override; - const TrajOptCollisionConfig& GetCollisionConfig() const override; + const trajopt_common::TrajOptCollisionConfig& GetCollisionConfig() const override; private: - std::shared_ptr collision_cache_; + std::shared_ptr collision_cache_; tesseract_kinematics::JointGroup::ConstPtr manip_; tesseract_environment::Environment::ConstPtr env_; - TrajOptCollisionConfig::ConstPtr collision_config_; + trajopt_common::TrajOptCollisionConfig::ConstPtr collision_config_; std::vector env_active_link_names_; std::vector manip_active_link_names_; std::vector diff_active_link_names_; - GetStateFn get_state_fn_; + trajopt_common::GetStateFn get_state_fn_; bool dynamic_environment_; tesseract_collision::DiscreteContactManager::Ptr contact_manager_; diff --git a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/weighted_average_methods.h b/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/weighted_average_methods.h index 49637e3c..62a0fd54 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/weighted_average_methods.h +++ b/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/weighted_average_methods.h @@ -29,14 +29,14 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include TRAJOPT_IGNORE_WARNINGS_POP -#include +#include namespace trajopt_ifopt { -Eigen::VectorXd getWeightedAvgGradientT0(const GradientResultsSet& grad_results_set, +Eigen::VectorXd getWeightedAvgGradientT0(const trajopt_common::GradientResultsSet& grad_results_set, double max_error_with_buffer, Eigen::Index size); -Eigen::VectorXd getWeightedAvgGradientT1(const GradientResultsSet& grad_results_set, +Eigen::VectorXd getWeightedAvgGradientT1(const trajopt_common::GradientResultsSet& grad_results_set, double max_error_with_buffer, Eigen::Index size); } // namespace trajopt_ifopt diff --git a/trajopt_ifopt/include/trajopt_ifopt/trajopt_ifopt.h b/trajopt_ifopt/include/trajopt_ifopt/trajopt_ifopt.h index 24b6b7da..535e1d28 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/trajopt_ifopt.h +++ b/trajopt_ifopt/include/trajopt_ifopt/trajopt_ifopt.h @@ -28,8 +28,6 @@ #include #include #include -#include -#include #include #include diff --git a/trajopt_ifopt/src/constraints/collision/continuous_collision_constraint.cpp b/trajopt_ifopt/src/constraints/collision/continuous_collision_constraint.cpp index 0c59026e..9a4b1957 100644 --- a/trajopt_ifopt/src/constraints/collision/continuous_collision_constraint.cpp +++ b/trajopt_ifopt/src/constraints/collision/continuous_collision_constraint.cpp @@ -87,7 +87,7 @@ Eigen::VectorXd ContinuousCollisionConstraint::GetValues() const double margin_buffer = collision_evaluator_->GetCollisionConfig().collision_margin_buffer; Eigen::VectorXd values = Eigen::VectorXd::Constant(static_cast(bounds_.size()), -margin_buffer); - CollisionCacheData::ConstPtr collision_data = + auto collision_data = collision_evaluator_->CalcCollisionData(joint_vals0, joint_vals1, position_vars_fixed_, bounds_.size()); if (collision_data->gradient_results_sets.empty()) @@ -98,7 +98,7 @@ Eigen::VectorXd ContinuousCollisionConstraint::GetValues() const { for (std::size_t i = 0; i < cnt; ++i) { - const GradientResultsSet& r = collision_data->gradient_results_sets[i]; + const trajopt_common::GradientResultsSet& r = collision_data->gradient_results_sets[i]; values(static_cast(i)) = r.coeff * r.getMaxError(); } } @@ -106,7 +106,7 @@ Eigen::VectorXd ContinuousCollisionConstraint::GetValues() const { for (std::size_t i = 0; i < cnt; ++i) { - const GradientResultsSet& r = collision_data->gradient_results_sets[i]; + const trajopt_common::GradientResultsSet& r = collision_data->gradient_results_sets[i]; if (r.max_error[0].has_error[0] || r.max_error[1].has_error[0]) values(static_cast(i)) = r.coeff * r.getMaxErrorT0(); } @@ -115,7 +115,7 @@ Eigen::VectorXd ContinuousCollisionConstraint::GetValues() const { for (std::size_t i = 0; i < cnt; ++i) { - const GradientResultsSet& r = collision_data->gradient_results_sets[i]; + const trajopt_common::GradientResultsSet& r = collision_data->gradient_results_sets[i]; if (r.max_error[0].has_error[1] || r.max_error[1].has_error[1]) values(static_cast(i)) = r.coeff * r.getMaxErrorT1(); } @@ -141,7 +141,7 @@ void ContinuousCollisionConstraint::FillJacobianBlock(std::string var_set, Jacob Eigen::VectorXd joint_vals0 = this->GetVariables()->GetComponent(position_vars_[0]->GetName())->GetValues(); Eigen::VectorXd joint_vals1 = this->GetVariables()->GetComponent(position_vars_[1]->GetName())->GetValues(); - CollisionCacheData::ConstPtr collision_data = + auto collision_data = collision_evaluator_->CalcCollisionData(joint_vals0, joint_vals1, position_vars_fixed_, bounds_.size()); if (collision_data->gradient_results_sets.empty()) @@ -153,7 +153,7 @@ void ContinuousCollisionConstraint::FillJacobianBlock(std::string var_set, Jacob { for (std::size_t i = 0; i < cnt; ++i) { - const GradientResultsSet& r = collision_data->gradient_results_sets[i]; + const trajopt_common::GradientResultsSet& r = collision_data->gradient_results_sets[i]; if (r.max_error[0].has_error[0] || r.max_error[1].has_error[0]) { double max_error_with_buffer = r.getMaxErrorWithBufferT0(); @@ -172,7 +172,7 @@ void ContinuousCollisionConstraint::FillJacobianBlock(std::string var_set, Jacob { for (std::size_t i = 0; i < cnt; ++i) { - const GradientResultsSet& r = collision_data->gradient_results_sets[i]; + const trajopt_common::GradientResultsSet& r = collision_data->gradient_results_sets[i]; if (r.max_error[0].has_error[1] || r.max_error[1].has_error[1]) { double max_error_with_buffer = r.getMaxErrorWithBufferT1(); diff --git a/trajopt_ifopt/src/constraints/collision/continuous_collision_evaluators.cpp b/trajopt_ifopt/src/constraints/collision/continuous_collision_evaluators.cpp index c6b090ab..5dbda07f 100644 --- a/trajopt_ifopt/src/constraints/collision/continuous_collision_evaluators.cpp +++ b/trajopt_ifopt/src/constraints/collision/continuous_collision_evaluators.cpp @@ -23,15 +23,15 @@ */ #include -#include +#include namespace trajopt_ifopt { LVSContinuousCollisionEvaluator::LVSContinuousCollisionEvaluator( - std::shared_ptr collision_cache, + std::shared_ptr collision_cache, tesseract_kinematics::JointGroup::ConstPtr manip, tesseract_environment::Environment::ConstPtr env, - trajopt_ifopt::TrajOptCollisionConfig::ConstPtr collision_config, + trajopt_common::TrajOptCollisionConfig::ConstPtr collision_config, bool dynamic_environment) : collision_cache_(std::move(collision_cache)) , manip_(std::move(manip)) @@ -39,9 +39,8 @@ LVSContinuousCollisionEvaluator::LVSContinuousCollisionEvaluator( , collision_config_(std::move(collision_config)) , dynamic_environment_(dynamic_environment) { - if (collision_config_->type != tesseract_collision::CollisionEvaluatorType::CONTINUOUS && - collision_config_->type != tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS) - throw std::runtime_error("LVSContinuousCollisionEvaluator, type must be CONTINUOUS or LVS_CONTINUOUS"); + assert(collision_config_->type == tesseract_collision::CollisionEvaluatorType::CONTINUOUS || + collision_config_->type == tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS); manip_active_link_names_ = manip_->getActiveLinkNames(); @@ -78,13 +77,13 @@ LVSContinuousCollisionEvaluator::LVSContinuousCollisionEvaluator( collision_config_->collision_margin_buffer); } -CollisionCacheData::ConstPtr +trajopt_common::CollisionCacheData::ConstPtr LVSContinuousCollisionEvaluator::CalcCollisionData(const Eigen::Ref& dof_vals0, const Eigen::Ref& dof_vals1, const std::array& position_vars_fixed, std::size_t bounds_size) { - size_t key = getHash(*collision_config_, dof_vals0, dof_vals1); + size_t key = trajopt_common::getHash(*collision_config_, dof_vals0, dof_vals1); auto* it = collision_cache_->get(key); if (it != nullptr) { @@ -92,24 +91,24 @@ LVSContinuousCollisionEvaluator::CalcCollisionData(const Eigen::Ref(); + auto data = std::make_shared(); CalcCollisionsHelper(dof_vals0, dof_vals1, data->contact_results_map); for (const auto& pair : data->contact_results_map) { - using ShapeGrsType = std::map, GradientResultsSet>; + using ShapeGrsType = std::map, trajopt_common::GradientResultsSet>; ShapeGrsType shape_grs; const double coeff = collision_config_->collision_coeff_data.getPairCollisionCoeff(pair.first.first, pair.first.second); for (const tesseract_collision::ContactResult& dist_result : pair.second) { - const std::size_t shape_hash0 = cantorHash(dist_result.shape_id[0], dist_result.subshape_id[0]); - const std::size_t shape_hash1 = cantorHash(dist_result.shape_id[1], dist_result.subshape_id[1]); + const std::size_t shape_hash0 = trajopt_common::cantorHash(dist_result.shape_id[0], dist_result.subshape_id[0]); + const std::size_t shape_hash1 = trajopt_common::cantorHash(dist_result.shape_id[1], dist_result.subshape_id[1]); auto shape_key = std::make_pair(shape_hash0, shape_hash1); auto it = shape_grs.find(shape_key); if (it == shape_grs.end()) { - GradientResultsSet grs; + trajopt_common::GradientResultsSet grs; grs.key = pair.first; grs.shape_key = shape_key; grs.coeff = coeff; @@ -140,7 +139,7 @@ LVSContinuousCollisionEvaluator::CalcCollisionData(const Eigen::Refgradient_results_sets.begin(), data->gradient_results_sets.end(), - [](const GradientResultsSet& a, const GradientResultsSet& b) { + [](const trajopt_common::GradientResultsSet& a, const trajopt_common::GradientResultsSet& b) { return a.getMaxErrorWithBuffer() > b.getMaxErrorWithBuffer(); }); } @@ -148,7 +147,7 @@ LVSContinuousCollisionEvaluator::CalcCollisionData(const Eigen::Refgradient_results_sets.begin(), data->gradient_results_sets.end(), - [](const GradientResultsSet& a, const GradientResultsSet& b) { + [](const trajopt_common::GradientResultsSet& a, const trajopt_common::GradientResultsSet& b) { return a.getMaxErrorWithBufferT0() > b.getMaxErrorWithBufferT0(); }); } @@ -156,7 +155,7 @@ LVSContinuousCollisionEvaluator::CalcCollisionData(const Eigen::Refgradient_results_sets.begin(), data->gradient_results_sets.end(), - [](const GradientResultsSet& a, const GradientResultsSet& b) { + [](const trajopt_common::GradientResultsSet& a, const trajopt_common::GradientResultsSet& b) { return a.getMaxErrorWithBufferT1() > b.getMaxErrorWithBufferT1(); }); } @@ -200,7 +199,7 @@ void LVSContinuousCollisionEvaluator::CalcCollisionsHelper(const Eigen::Refcollision_coeff_data.getPairCollisionCoeff(pair.first.first, pair.first.second); const Eigen::Vector3d data = { dist, collision_config_->collision_margin_buffer, coeff }; - removeInvalidContactResults(pair.second, data); /** @todo Should this be removed? levi */ + trajopt_common::removeInvalidContactResults(pair.second, data); /** @todo Should this be removed? levi */ }; if (collision_config_->type == tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS && @@ -248,7 +247,7 @@ void LVSContinuousCollisionEvaluator::CalcCollisionsHelper(const Eigen::Ref& dof_vals0, const Eigen::Ref& dof_vals1, const tesseract_collision::ContactResult& contact_results) @@ -257,10 +256,11 @@ LVSContinuousCollisionEvaluator::CalcGradientData(const Eigen::Refcontact_manager_config.margin_data.getPairCollisionMargin( contact_results.link_names[0], contact_results.link_names[1]); - return getGradient(dof_vals0, dof_vals1, contact_results, margin, collision_config_->collision_margin_buffer, manip_); + return trajopt_common::getGradient( + dof_vals0, dof_vals1, contact_results, margin, collision_config_->collision_margin_buffer, manip_); } -const trajopt_ifopt::TrajOptCollisionConfig& LVSContinuousCollisionEvaluator::GetCollisionConfig() const +const trajopt_common::TrajOptCollisionConfig& LVSContinuousCollisionEvaluator::GetCollisionConfig() const { return *collision_config_; } @@ -268,10 +268,10 @@ const trajopt_ifopt::TrajOptCollisionConfig& LVSContinuousCollisionEvaluator::Ge ////////////////////////////////////////// LVSDiscreteCollisionEvaluator::LVSDiscreteCollisionEvaluator( - std::shared_ptr collision_cache, + std::shared_ptr collision_cache, tesseract_kinematics::JointGroup::ConstPtr manip, tesseract_environment::Environment::ConstPtr env, - trajopt_ifopt::TrajOptCollisionConfig::ConstPtr collision_config, + trajopt_common::TrajOptCollisionConfig::ConstPtr collision_config, bool dynamic_environment) : collision_cache_(std::move(collision_cache)) , manip_(std::move(manip)) @@ -314,7 +314,7 @@ LVSDiscreteCollisionEvaluator::LVSDiscreteCollisionEvaluator( collision_config_->collision_margin_buffer); } -CollisionCacheData::ConstPtr +trajopt_common::CollisionCacheData::ConstPtr LVSDiscreteCollisionEvaluator::CalcCollisionData(const Eigen::Ref& dof_vals0, const Eigen::Ref& dof_vals1, const std::array& position_vars_fixed, @@ -328,23 +328,23 @@ LVSDiscreteCollisionEvaluator::CalcCollisionData(const Eigen::Ref(); + auto data = std::make_shared(); CalcCollisionsHelper(dof_vals0, dof_vals1, data->contact_results_map); for (const auto& pair : data->contact_results_map) { - using ShapeGrsType = std::map, GradientResultsSet>; + using ShapeGrsType = std::map, trajopt_common::GradientResultsSet>; ShapeGrsType shape_grs; const double coeff = collision_config_->collision_coeff_data.getPairCollisionCoeff(pair.first.first, pair.first.second); for (const tesseract_collision::ContactResult& dist_result : pair.second) { - const std::size_t shape_hash0 = cantorHash(dist_result.shape_id[0], dist_result.subshape_id[0]); - const std::size_t shape_hash1 = cantorHash(dist_result.shape_id[1], dist_result.subshape_id[1]); + const std::size_t shape_hash0 = trajopt_common::cantorHash(dist_result.shape_id[0], dist_result.subshape_id[0]); + const std::size_t shape_hash1 = trajopt_common::cantorHash(dist_result.shape_id[1], dist_result.subshape_id[1]); auto shape_key = std::make_pair(shape_hash0, shape_hash1); auto it = shape_grs.find(shape_key); if (it == shape_grs.end()) { - GradientResultsSet grs; + trajopt_common::GradientResultsSet grs; grs.key = pair.first; grs.shape_key = shape_key; grs.coeff = coeff; @@ -375,7 +375,7 @@ LVSDiscreteCollisionEvaluator::CalcCollisionData(const Eigen::Refgradient_results_sets.begin(), data->gradient_results_sets.end(), - [](const GradientResultsSet& a, const GradientResultsSet& b) { + [](const trajopt_common::GradientResultsSet& a, const trajopt_common::GradientResultsSet& b) { return a.getMaxErrorWithBuffer() > b.getMaxErrorWithBuffer(); }); } @@ -383,7 +383,7 @@ LVSDiscreteCollisionEvaluator::CalcCollisionData(const Eigen::Refgradient_results_sets.begin(), data->gradient_results_sets.end(), - [](const GradientResultsSet& a, const GradientResultsSet& b) { + [](const trajopt_common::GradientResultsSet& a, const trajopt_common::GradientResultsSet& b) { return a.getMaxErrorWithBufferT0() > b.getMaxErrorWithBufferT0(); }); } @@ -391,7 +391,7 @@ LVSDiscreteCollisionEvaluator::CalcCollisionData(const Eigen::Refgradient_results_sets.begin(), data->gradient_results_sets.end(), - [](const GradientResultsSet& a, const GradientResultsSet& b) { + [](const trajopt_common::GradientResultsSet& a, const trajopt_common::GradientResultsSet& b) { return a.getMaxErrorWithBufferT1() > b.getMaxErrorWithBufferT1(); }); } @@ -432,7 +432,7 @@ void LVSDiscreteCollisionEvaluator::CalcCollisionsHelper(const Eigen::Refcollision_margin_buffer, coeff }; // Don't include contacts at the fixed state - removeInvalidContactResults(pair.second, data); + trajopt_common::removeInvalidContactResults(pair.second, data); }; // The first step is to see if the distance between two states is larger than the longest valid segment. If larger @@ -474,7 +474,7 @@ void LVSDiscreteCollisionEvaluator::CalcCollisionsHelper(const Eigen::Ref& dof_vals0, const Eigen::Ref& dof_vals1, const tesseract_collision::ContactResult& contact_results) @@ -483,10 +483,11 @@ LVSDiscreteCollisionEvaluator::CalcGradientData(const Eigen::Refcontact_manager_config.margin_data.getPairCollisionMargin( contact_results.link_names[0], contact_results.link_names[1]); - return getGradient(dof_vals0, dof_vals1, contact_results, margin, collision_config_->collision_margin_buffer, manip_); + return trajopt_common::getGradient( + dof_vals0, dof_vals1, contact_results, margin, collision_config_->collision_margin_buffer, manip_); } -const trajopt_ifopt::TrajOptCollisionConfig& LVSDiscreteCollisionEvaluator::GetCollisionConfig() const +const trajopt_common::TrajOptCollisionConfig& LVSDiscreteCollisionEvaluator::GetCollisionConfig() const { return *collision_config_; } diff --git a/trajopt_ifopt/src/constraints/collision/continuous_collision_numerical_constraint.cpp b/trajopt_ifopt/src/constraints/collision/continuous_collision_numerical_constraint.cpp index c0ff46dd..8a25f054 100644 --- a/trajopt_ifopt/src/constraints/collision/continuous_collision_numerical_constraint.cpp +++ b/trajopt_ifopt/src/constraints/collision/continuous_collision_numerical_constraint.cpp @@ -88,7 +88,7 @@ Eigen::VectorXd ContinuousCollisionNumericalConstraint::GetValues() const double margin_buffer = collision_evaluator_->GetCollisionConfig().collision_margin_buffer; Eigen::VectorXd values = Eigen::VectorXd::Constant(static_cast(bounds_.size()), -margin_buffer); - CollisionCacheData::ConstPtr collision_data = + auto collision_data = collision_evaluator_->CalcCollisionData(joint_vals0, joint_vals1, position_vars_fixed_, bounds_.size()); if (collision_data->gradient_results_sets.empty()) @@ -100,7 +100,7 @@ Eigen::VectorXd ContinuousCollisionNumericalConstraint::GetValues() const { for (std::size_t i = 0; i < cnt; ++i) { - const GradientResultsSet& r = collision_data->gradient_results_sets[i]; + const trajopt_common::GradientResultsSet& r = collision_data->gradient_results_sets[i]; values(static_cast(i)) = r.coeff * r.getMaxError(); } } @@ -108,7 +108,7 @@ Eigen::VectorXd ContinuousCollisionNumericalConstraint::GetValues() const { for (std::size_t i = 0; i < cnt; ++i) { - const GradientResultsSet& r = collision_data->gradient_results_sets[i]; + const trajopt_common::GradientResultsSet& r = collision_data->gradient_results_sets[i]; values(static_cast(i)) = r.coeff * r.getMaxErrorT0(); } } @@ -116,7 +116,7 @@ Eigen::VectorXd ContinuousCollisionNumericalConstraint::GetValues() const { for (std::size_t i = 0; i < cnt; ++i) { - const GradientResultsSet& r = collision_data->gradient_results_sets[i]; + const trajopt_common::GradientResultsSet& r = collision_data->gradient_results_sets[i]; values(static_cast(i)) = r.coeff * r.getMaxErrorT1(); } } @@ -143,7 +143,7 @@ void ContinuousCollisionNumericalConstraint::FillJacobianBlock(std::string var_s Eigen::VectorXd joint_vals0 = this->GetVariables()->GetComponent(position_vars_[0]->GetName())->GetValues(); Eigen::VectorXd joint_vals1 = this->GetVariables()->GetComponent(position_vars_[1]->GetName())->GetValues(); - CollisionCacheData::ConstPtr collision_data = + auto collision_data = collision_evaluator_->CalcCollisionData(joint_vals0, joint_vals1, position_vars_fixed_, bounds_.size()); if (collision_data->gradient_results_sets.empty()) return; @@ -155,13 +155,13 @@ void ContinuousCollisionNumericalConstraint::FillJacobianBlock(std::string var_s for (int j = 0; j < n_dof_; j++) { jv(j) = joint_vals0(j) + delta; - CollisionCacheData::ConstPtr collision_data_delta = + auto collision_data_delta = collision_evaluator_->CalcCollisionData(jv, joint_vals1, position_vars_fixed_, bounds_.size()); for (int i = 0; i < static_cast(cnt); ++i) { - const GradientResultsSet& baseline = collision_data->gradient_results_sets[static_cast(i)]; - auto fn = [&baseline](const GradientResultsSet& cr) { + const auto& baseline = collision_data->gradient_results_sets[static_cast(i)]; + auto fn = [&baseline](const trajopt_common::GradientResultsSet& cr) { return (cr.key == baseline.key && cr.shape_key == baseline.shape_key); }; auto it = std::find_if( diff --git a/trajopt_ifopt/src/constraints/collision/discrete_collision_constraint.cpp b/trajopt_ifopt/src/constraints/collision/discrete_collision_constraint.cpp index d7a173bf..b8c95e61 100644 --- a/trajopt_ifopt/src/constraints/collision/discrete_collision_constraint.cpp +++ b/trajopt_ifopt/src/constraints/collision/discrete_collision_constraint.cpp @@ -31,8 +31,8 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include TRAJOPT_IGNORE_WARNINGS_POP +#include #include -#include #include #include @@ -93,7 +93,8 @@ void DiscreteCollisionConstraint::FillJacobianBlock(std::string var_set, Jacobia Eigen::VectorXd DiscreteCollisionConstraint::CalcValues(const Eigen::Ref& joint_vals) const { // Check the collisions - CollisionCacheData::ConstPtr collision_data = collision_evaluator_->CalcCollisions(joint_vals, bounds_.size()); + trajopt_common::CollisionCacheData::ConstPtr collision_data = + collision_evaluator_->CalcCollisions(joint_vals, bounds_.size()); double margin_buffer = collision_evaluator_->GetCollisionConfig().collision_margin_buffer; Eigen::VectorXd values = Eigen::VectorXd::Constant(static_cast(bounds_.size()), -margin_buffer); @@ -103,7 +104,7 @@ Eigen::VectorXd DiscreteCollisionConstraint::CalcValues(const Eigen::Refgradient_results_sets.size()); for (std::size_t i = 0; i < cnt; ++i) { - const GradientResultsSet& r = collision_data->gradient_results_sets[i]; + const trajopt_common::GradientResultsSet& r = collision_data->gradient_results_sets[i]; values(static_cast(i)) = r.coeff * r.getMaxErrorT0(); } @@ -123,7 +124,8 @@ void DiscreteCollisionConstraint::CalcJacobianBlock(const Eigen::RefCalcCollisions(joint_vals, bounds_.size()); + trajopt_common::CollisionCacheData::ConstPtr collision_data = + collision_evaluator_->CalcCollisions(joint_vals, bounds_.size()); if (collision_data->gradient_results_sets.empty()) return; @@ -131,7 +133,7 @@ void DiscreteCollisionConstraint::CalcJacobianBlock(const Eigen::Refgradient_results_sets.size()); for (std::size_t i = 0; i < cnt; ++i) { - const GradientResultsSet& r = collision_data->gradient_results_sets[i]; + const trajopt_common::GradientResultsSet& r = collision_data->gradient_results_sets[i]; Eigen::VectorXd grad_vec = getWeightedAvgGradientT0(r, r.getMaxErrorWithBufferT0(), position_var_->GetRows()); // Collision is 1 x n_dof diff --git a/trajopt_ifopt/src/constraints/collision/discrete_collision_evaluators.cpp b/trajopt_ifopt/src/constraints/collision/discrete_collision_evaluators.cpp index bf991464..e4d0d5d5 100644 --- a/trajopt_ifopt/src/constraints/collision/discrete_collision_evaluators.cpp +++ b/trajopt_ifopt/src/constraints/collision/discrete_collision_evaluators.cpp @@ -23,15 +23,16 @@ */ #include -#include +#include namespace trajopt_ifopt { -SingleTimestepCollisionEvaluator::SingleTimestepCollisionEvaluator(std::shared_ptr collision_cache, - tesseract_kinematics::JointGroup::ConstPtr manip, - tesseract_environment::Environment::ConstPtr env, - TrajOptCollisionConfig::ConstPtr collision_config, - bool dynamic_environment) +SingleTimestepCollisionEvaluator::SingleTimestepCollisionEvaluator( + std::shared_ptr collision_cache, + tesseract_kinematics::JointGroup::ConstPtr manip, + tesseract_environment::Environment::ConstPtr env, + trajopt_common::TrajOptCollisionConfig::ConstPtr collision_config, + bool dynamic_environment) : collision_cache_(std::move(collision_cache)) , manip_(std::move(manip)) , env_(std::move(env)) @@ -73,7 +74,7 @@ SingleTimestepCollisionEvaluator::SingleTimestepCollisionEvaluator(std::shared_p collision_config_->collision_margin_buffer); } -CollisionCacheData::ConstPtr +trajopt_common::CollisionCacheData::ConstPtr SingleTimestepCollisionEvaluator::CalcCollisions(const Eigen::Ref& dof_vals, std::size_t bounds_size) { @@ -86,24 +87,24 @@ SingleTimestepCollisionEvaluator::CalcCollisions(const Eigen::Ref(); + auto data = std::make_shared(); CalcCollisionsHelper(dof_vals, data->contact_results_map); for (const auto& pair : data->contact_results_map) { - using ShapeGrsType = std::map, GradientResultsSet>; + using ShapeGrsType = std::map, trajopt_common::GradientResultsSet>; ShapeGrsType shape_grs; const double coeff = collision_config_->collision_coeff_data.getPairCollisionCoeff(pair.first.first, pair.first.second); for (const tesseract_collision::ContactResult& dist_result : pair.second) { - const std::size_t shape_hash0 = cantorHash(dist_result.shape_id[0], dist_result.subshape_id[0]); - const std::size_t shape_hash1 = cantorHash(dist_result.shape_id[1], dist_result.subshape_id[1]); + const std::size_t shape_hash0 = trajopt_common::cantorHash(dist_result.shape_id[0], dist_result.subshape_id[0]); + const std::size_t shape_hash1 = trajopt_common::cantorHash(dist_result.shape_id[1], dist_result.subshape_id[1]); auto shape_key = std::make_pair(shape_hash0, shape_hash1); auto it = shape_grs.find(shape_key); if (it == shape_grs.end()) { - GradientResultsSet grs; + trajopt_common::GradientResultsSet grs; grs.key = pair.first; grs.shape_key = shape_key; grs.coeff = coeff; @@ -131,7 +132,7 @@ SingleTimestepCollisionEvaluator::CalcCollisions(const Eigen::Refgradient_results_sets.begin(), data->gradient_results_sets.end(), - [](const GradientResultsSet& a, const GradientResultsSet& b) { + [](const trajopt_common::GradientResultsSet& a, const trajopt_common::GradientResultsSet& b) { return a.max_error[0].error > b.max_error[0].error; }); } @@ -180,17 +181,19 @@ void SingleTimestepCollisionEvaluator::CalcCollisionsHelper(const Eigen::Refcontact_manager_config.margin_data.getPairCollisionMargin( contact_result.link_names[0], contact_result.link_names[1]); - return getGradient(dofvals, contact_result, margin, collision_config_->collision_margin_buffer, manip_); + return trajopt_common::getGradient( + dofvals, contact_result, margin, collision_config_->collision_margin_buffer, manip_); } -const TrajOptCollisionConfig& SingleTimestepCollisionEvaluator::GetCollisionConfig() const +const trajopt_common::TrajOptCollisionConfig& SingleTimestepCollisionEvaluator::GetCollisionConfig() const { return *collision_config_; } diff --git a/trajopt_ifopt/src/constraints/collision/discrete_collision_numerical_constraint.cpp b/trajopt_ifopt/src/constraints/collision/discrete_collision_numerical_constraint.cpp index 29a2c56b..d821ecd5 100644 --- a/trajopt_ifopt/src/constraints/collision/discrete_collision_numerical_constraint.cpp +++ b/trajopt_ifopt/src/constraints/collision/discrete_collision_numerical_constraint.cpp @@ -32,9 +32,9 @@ TRAJOPT_IGNORE_WARNINGS_PUSH TRAJOPT_IGNORE_WARNINGS_POP #include -#include #include #include +#include namespace trajopt_ifopt { @@ -95,7 +95,7 @@ Eigen::VectorXd DiscreteCollisionNumericalConstraint::CalcValues(const Eigen::Ref& joint_vals) const { // Check the collisions - CollisionCacheData::ConstPtr collision_data = collision_evaluator_->CalcCollisions(joint_vals, bounds_.size()); + auto collision_data = collision_evaluator_->CalcCollisions(joint_vals, bounds_.size()); double margin_buffer = collision_evaluator_->GetCollisionConfig().collision_margin_buffer; Eigen::VectorXd values = Eigen::VectorXd::Constant(static_cast(bounds_.size()), -margin_buffer); @@ -125,7 +125,7 @@ void DiscreteCollisionNumericalConstraint::CalcJacobianBlock(const Eigen::RefCalcCollisions(joint_vals, bounds_.size()); + auto collision_data = collision_evaluator_->CalcCollisions(joint_vals, bounds_.size()); if (collision_data->gradient_results_sets.empty()) return; @@ -138,11 +138,11 @@ void DiscreteCollisionNumericalConstraint::CalcJacobianBlock(const Eigen::RefCalcCollisions(jv, bounds_.size()); + auto collision_data_delta = collision_evaluator_->CalcCollisions(jv, bounds_.size()); for (int i = 0; i < static_cast(cnt); ++i) { - const GradientResultsSet& baseline = collision_data->gradient_results_sets[static_cast(i)]; - auto fn = [&baseline](const GradientResultsSet& cr) { + const auto& baseline = collision_data->gradient_results_sets[static_cast(i)]; + auto fn = [&baseline](const trajopt_common::GradientResultsSet& cr) { return (cr.key == baseline.key && cr.shape_key == baseline.shape_key); }; diff --git a/trajopt_ifopt/src/constraints/collision/weighted_average_methods.cpp b/trajopt_ifopt/src/constraints/collision/weighted_average_methods.cpp index 019a311b..c37240a1 100644 --- a/trajopt_ifopt/src/constraints/collision/weighted_average_methods.cpp +++ b/trajopt_ifopt/src/constraints/collision/weighted_average_methods.cpp @@ -26,7 +26,7 @@ namespace trajopt_ifopt { -Eigen::VectorXd getWeightedAvgGradientT0(const GradientResultsSet& grad_results_set, +Eigen::VectorXd getWeightedAvgGradientT0(const trajopt_common::GradientResultsSet& grad_results_set, double max_error_with_buffer, Eigen::Index size) { @@ -64,7 +64,7 @@ Eigen::VectorXd getWeightedAvgGradientT0(const GradientResultsSet& grad_results_ return (1.0 / total_weight) * grad_results_set.coeff * grad_vec; } -Eigen::VectorXd getWeightedAvgGradientT1(const GradientResultsSet& grad_results_set, +Eigen::VectorXd getWeightedAvgGradientT1(const trajopt_common::GradientResultsSet& grad_results_set, double max_error_with_buffer, Eigen::Index size) { diff --git a/trajopt_ifopt/test/cast_cost_unit.cpp b/trajopt_ifopt/test/cast_cost_unit.cpp index df2b5bcd..a2bc83a9 100644 --- a/trajopt_ifopt/test/cast_cost_unit.cpp +++ b/trajopt_ifopt/test/cast_cost_unit.cpp @@ -127,7 +127,7 @@ TEST_F(CastTest, boxes) // NOLINT // Step 3: Setup collision double margin_coeff = 1; double margin = 0.02; - auto trajopt_collision_config = std::make_shared(margin, margin_coeff); + auto trajopt_collision_config = std::make_shared(margin, margin_coeff); trajopt_collision_config->type = tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS; trajopt_collision_config->collision_margin_buffer = 0.05; @@ -146,7 +146,7 @@ TEST_F(CastTest, boxes) // NOLINT nlp.AddConstraintSet(cnt); } - auto collision_cache = std::make_shared(100); + auto collision_cache = std::make_shared(100); std::array position_vars_fixed{ true, false }; for (std::size_t i = 1; i < (vars.size()); ++i) { diff --git a/trajopt_ifopt/test/collision_unit.cpp b/trajopt_ifopt/test/collision_unit.cpp index 28b1cd11..5563ea46 100644 --- a/trajopt_ifopt/test/collision_unit.cpp +++ b/trajopt_ifopt/test/collision_unit.cpp @@ -64,8 +64,8 @@ class CollisionUnit : public testing::TestWithParam // Set up collision evaluator tesseract_kinematics::JointGroup::ConstPtr kin = env->getJointGroup("manipulator"); - auto config = std::make_shared(0.1, 1); - auto collision_cache = std::make_shared(100); + auto config = std::make_shared(0.1, 1); + auto collision_cache = std::make_shared(100); collision_evaluator = std::make_shared(collision_cache, kin, env, config); diff --git a/trajopt_ifopt/test/continuous_collision_gradient_unit.cpp b/trajopt_ifopt/test/continuous_collision_gradient_unit.cpp index f59a6d33..a399dd6c 100644 --- a/trajopt_ifopt/test/continuous_collision_gradient_unit.cpp +++ b/trajopt_ifopt/test/continuous_collision_gradient_unit.cpp @@ -127,11 +127,11 @@ void runContinuousGradientTest(const Environment::Ptr& env, double coeff) // Step 3: Setup collision double margin_coeff = coeff; double margin = 0.02; - auto trajopt_collision_config = std::make_shared(margin, margin_coeff); + auto trajopt_collision_config = std::make_shared(margin, margin_coeff); trajopt_collision_config->type = tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS; trajopt_collision_config->collision_margin_buffer = 0.05; - auto collision_cache = std::make_shared(100); + auto collision_cache = std::make_shared(100); for (std::size_t i = 1; i < vars.size(); ++i) { auto collision_evaluator = std::make_shared( diff --git a/trajopt_ifopt/test/discrete_collision_gradient_unit.cpp b/trajopt_ifopt/test/discrete_collision_gradient_unit.cpp index bd6c8e5a..4e3c6c08 100644 --- a/trajopt_ifopt/test/discrete_collision_gradient_unit.cpp +++ b/trajopt_ifopt/test/discrete_collision_gradient_unit.cpp @@ -106,10 +106,10 @@ void runDiscreteGradientTest(const Environment::Ptr& env, double coeff) // Step 3: Setup collision double margin_coeff = coeff; double margin = 0.2; - auto trajopt_collision_config = std::make_shared(margin, margin_coeff); + auto trajopt_collision_config = std::make_shared(margin, margin_coeff); trajopt_collision_config->collision_margin_buffer = 0.0; // 0.05 - auto collision_cache = std::make_shared(100); + auto collision_cache = std::make_shared(100); auto collision_evaluator = std::make_shared( collision_cache, manip, env, trajopt_collision_config); auto cnt = std::make_shared(collision_evaluator, vars[0], 3); diff --git a/trajopt_ifopt/test/simple_collision_unit.cpp b/trajopt_ifopt/test/simple_collision_unit.cpp index 41ccf5db..0e78d892 100644 --- a/trajopt_ifopt/test/simple_collision_unit.cpp +++ b/trajopt_ifopt/test/simple_collision_unit.cpp @@ -106,10 +106,10 @@ TEST_F(SimpleCollisionTest, spheres) // NOLINT // Step 3: Setup collision double margin_coeff = 10; double margin = 0.2; - auto trajopt_collision_config = std::make_shared(margin, margin_coeff); + auto trajopt_collision_config = std::make_shared(margin, margin_coeff); trajopt_collision_config->collision_margin_buffer = 0.05; - auto collision_cache = std::make_shared(100); + auto collision_cache = std::make_shared(100); trajopt_ifopt::DiscreteCollisionEvaluator::Ptr collision_evaluator = std::make_shared( collision_cache, manip, env, trajopt_collision_config); 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 fcfeb0ee..4b372f82 100644 --- a/trajopt_optimizers/trajopt_sqp/test/cast_cost_attached_unit.cpp +++ b/trajopt_optimizers/trajopt_sqp/test/cast_cost_attached_unit.cpp @@ -166,7 +166,7 @@ void runCastAttachedLinkWithGeomTest(const trajopt_sqp::QPProblem::Ptr& qp_probl // Step 3: Setup collision double margin_coeff = 20; double margin = 0.3; - auto trajopt_collision_config = std::make_shared(margin, margin_coeff); + auto trajopt_collision_config = std::make_shared(margin, margin_coeff); trajopt_collision_config->type = tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS; trajopt_collision_config->collision_margin_buffer = 0.05; @@ -185,7 +185,7 @@ void runCastAttachedLinkWithGeomTest(const trajopt_sqp::QPProblem::Ptr& qp_probl qp_problem->addConstraintSet(cnt); } - auto collision_cache = std::make_shared(100); + auto collision_cache = std::make_shared(100); for (std::size_t i = 1; i < (vars.size() - 1); ++i) { auto collision_evaluator = std::make_shared( @@ -286,7 +286,7 @@ void runCastAttachedLinkWithoutGeomTest(const trajopt_sqp::QPProblem::Ptr& qp_pr // Step 3: Setup collision double margin_coeff = 10; double margin = 0.02; - auto trajopt_collision_config = std::make_shared(margin, margin_coeff); + auto trajopt_collision_config = std::make_shared(margin, margin_coeff); trajopt_collision_config->type = tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS; trajopt_collision_config->collision_margin_buffer = 0.01; trajopt_collision_config->longest_valid_segment_length = 0.05; @@ -306,7 +306,7 @@ void runCastAttachedLinkWithoutGeomTest(const trajopt_sqp::QPProblem::Ptr& qp_pr qp_problem->addConstraintSet(cnt); } - auto collision_cache = std::make_shared(100); + auto collision_cache = std::make_shared(100); std::array position_vars_fixed{ true, false }; for (std::size_t i = 1; i < vars.size(); ++i) { 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 4983e9f3..d67f49fd 100644 --- a/trajopt_optimizers/trajopt_sqp/test/cast_cost_octomap_unit.cpp +++ b/trajopt_optimizers/trajopt_sqp/test/cast_cost_octomap_unit.cpp @@ -159,7 +159,7 @@ void runCastOctomapTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const Env // Step 3: Setup collision double margin_coeff = 10; double margin = 0.02; - auto trajopt_collision_config = std::make_shared(margin, margin_coeff); + auto trajopt_collision_config = std::make_shared(margin, margin_coeff); trajopt_collision_config->type = tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS; trajopt_collision_config->collision_margin_buffer = 0.05; @@ -178,7 +178,7 @@ void runCastOctomapTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const Env qp_problem->addConstraintSet(cnt); } - auto collision_cache = std::make_shared(100); + auto collision_cache = std::make_shared(100); std::array position_vars_fixed{ true, false }; for (std::size_t i = 1; i < vars.size(); ++i) { diff --git a/trajopt_optimizers/trajopt_sqp/test/cast_cost_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/cast_cost_unit.cpp index 8d207c88..af8236a8 100644 --- a/trajopt_optimizers/trajopt_sqp/test/cast_cost_unit.cpp +++ b/trajopt_optimizers/trajopt_sqp/test/cast_cost_unit.cpp @@ -114,7 +114,7 @@ void runCastTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const Environmen // Step 3: Setup collision double margin_coeff = 10; double margin = 0.02; - auto trajopt_collision_config = std::make_shared(margin, margin_coeff); + auto trajopt_collision_config = std::make_shared(margin, margin_coeff); trajopt_collision_config->type = tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS; trajopt_collision_config->collision_margin_buffer = 0.05; @@ -133,7 +133,7 @@ void runCastTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const Environmen qp_problem->addConstraintSet(cnt); } - auto collision_cache = std::make_shared(100); + auto collision_cache = std::make_shared(100); std::array position_vars_fixed{ true, false }; for (std::size_t i = 1; i < vars.size(); ++i) { 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 ca3d5166..1db3b50d 100644 --- a/trajopt_optimizers/trajopt_sqp/test/cast_cost_world_unit.cpp +++ b/trajopt_optimizers/trajopt_sqp/test/cast_cost_world_unit.cpp @@ -143,7 +143,7 @@ void runCastWorldTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const Envir // Step 3: Setup collision double margin_coeff = 10; double margin = 0.02; - auto trajopt_collision_config = std::make_shared(margin, margin_coeff); + auto trajopt_collision_config = std::make_shared(margin, margin_coeff); trajopt_collision_config->type = tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS; trajopt_collision_config->collision_margin_buffer = 0.05; @@ -162,7 +162,7 @@ void runCastWorldTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const Envir qp_problem->addConstraintSet(cnt); } - auto collision_cache = std::make_shared(100); + auto collision_cache = std::make_shared(100); std::array position_vars_fixed{ true, false }; for (std::size_t i = 1; i < vars.size(); ++i) { diff --git a/trajopt_optimizers/trajopt_sqp/test/planning_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/planning_unit.cpp index cceb3e07..81ff1600 100644 --- a/trajopt_optimizers/trajopt_sqp/test/planning_unit.cpp +++ b/trajopt_optimizers/trajopt_sqp/test/planning_unit.cpp @@ -119,7 +119,7 @@ void runPlanningTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const Enviro double margin_coeff = 20; double margin = 0.025; - auto trajopt_collision_config = std::make_shared(margin, margin_coeff); + auto trajopt_collision_config = std::make_shared(margin, margin_coeff); trajopt_collision_config->type = tesseract_collision::CollisionEvaluatorType::CONTINUOUS; trajopt_collision_config->collision_margin_buffer = 0.01; @@ -145,7 +145,7 @@ void runPlanningTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const Enviro qp_problem->addConstraintSet(cnt); } - auto collision_cache = std::make_shared(100); + auto collision_cache = std::make_shared(100); std::array position_vars_fixed{ false, false }; for (std::size_t i = 1; i < (vars.size() - 1); ++i) { diff --git a/trajopt_optimizers/trajopt_sqp/test/simple_collision_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/simple_collision_unit.cpp index a836f96d..37f55c89 100644 --- a/trajopt_optimizers/trajopt_sqp/test/simple_collision_unit.cpp +++ b/trajopt_optimizers/trajopt_sqp/test/simple_collision_unit.cpp @@ -103,7 +103,8 @@ class SimpleCollisionConstraintIfopt : public ifopt::ConstraintSet Eigen::VectorXd err = Eigen::VectorXd::Zero(3); // Check the collisions - CollisionCacheData::ConstPtr cdata = collision_evaluator_->CalcCollisions(joint_vals, bounds_.size()); + trajopt_common::CollisionCacheData::ConstPtr cdata = + collision_evaluator_->CalcCollisions(joint_vals, bounds_.size()); if (cdata->contact_results_map.empty()) return err; @@ -137,16 +138,17 @@ class SimpleCollisionConstraintIfopt : public ifopt::ConstraintSet jac_block.reserve(n_dof_ * 3); // Calculate collisions - CollisionCacheData::ConstPtr cdata = collision_evaluator_->CalcCollisions(joint_vals, bounds_.size()); + trajopt_common::CollisionCacheData::ConstPtr cdata = + collision_evaluator_->CalcCollisions(joint_vals, bounds_.size()); // Get gradients for all contacts /** @todo Use the cdata gradient results */ - std::vector grad_results; + std::vector grad_results; for (const auto& pair : cdata->contact_results_map) { for (const auto& dist_result : pair.second) { - trajopt_ifopt::GradientResults result = collision_evaluator_->GetGradient(joint_vals, dist_result); + trajopt_common::GradientResults result = collision_evaluator_->GetGradient(joint_vals, dist_result); grad_results.push_back(result); } } @@ -239,20 +241,20 @@ void runSimpleCollisionTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const } // Step 3: Setup collision - auto trajopt_collision_cnt_config = std::make_shared(0.2, 1); + auto trajopt_collision_cnt_config = std::make_shared(0.2, 1); trajopt_collision_cnt_config->collision_margin_buffer = 0.05; - auto collision_cnt_cache = std::make_shared(100); + auto collision_cnt_cache = std::make_shared(100); trajopt_ifopt::DiscreteCollisionEvaluator::Ptr collision_cnt_evaluator = std::make_shared( collision_cnt_cache, manip, env, trajopt_collision_cnt_config); auto collision_cnt = std::make_shared(collision_cnt_evaluator, vars[0]); qp_problem->addConstraintSet(collision_cnt); - auto trajopt_collision_cost_config = std::make_shared(0.3, 1); + auto trajopt_collision_cost_config = std::make_shared(0.3, 1); trajopt_collision_cost_config->collision_margin_buffer = 0.05; - auto collision_cost_cache = std::make_shared(100); + auto collision_cost_cache = std::make_shared(100); trajopt_ifopt::DiscreteCollisionEvaluator::Ptr collision_cost_evaluator = std::make_shared( collision_cost_cache, manip, env, trajopt_collision_cost_config);