From 89661ad2b2d02d7c1cdf3e1a765accd56f9d724f Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Mon, 4 Dec 2023 09:45:26 -0600 Subject: [PATCH] Fix trajopt ifopt collision with fixed states (#372) --- .../include/trajopt_common/collision_utils.h | 4 +- trajopt_common/src/collision_utils.cpp | 43 ++++++++++++++++--- .../continuous_collision_evaluators.h | 10 +++-- .../continuous_collision_evaluators.cpp | 22 +++++----- .../trajopt_sqp/include/trajopt_sqp/types.h | 2 +- .../src/trust_region_sqp_solver.cpp | 4 +- 6 files changed, 61 insertions(+), 24 deletions(-) diff --git a/trajopt_common/include/trajopt_common/collision_utils.h b/trajopt_common/include/trajopt_common/collision_utils.h index 9af20305f..76ab042bb 100644 --- a/trajopt_common/include/trajopt_common/collision_utils.h +++ b/trajopt_common/include/trajopt_common/collision_utils.h @@ -47,9 +47,11 @@ std::size_t cantorHash(int shape_id, int subshape_id); * @brief Remove any results that are invalid. * Invalid state are contacts that occur at fixed states or have distances outside the threshold. * @param contact_results Contact results vector to process. + * @param position_vars_fixed Indicate if a state is fixed */ void removeInvalidContactResults(tesseract_collision::ContactResultVector& contact_results, - const Eigen::Vector3d& data); + const Eigen::Vector3d& data, + const std::array& position_vars_fixed); /** * @brief Extracts the gradient information based on the contact results diff --git a/trajopt_common/src/collision_utils.cpp b/trajopt_common/src/collision_utils.cpp index c4c7265aa..f99886f98 100644 --- a/trajopt_common/src/collision_utils.cpp +++ b/trajopt_common/src/collision_utils.cpp @@ -67,13 +67,44 @@ std::size_t cantorHash(int shape_id, int subshape_id) return static_cast(1 / 2.0 * (shape_id + subshape_id) * (shape_id + subshape_id + 1) + subshape_id); } -void removeInvalidContactResults(tesseract_collision::ContactResultVector& contact_results, const Eigen::Vector3d& data) +void removeInvalidContactResults(tesseract_collision::ContactResultVector& contact_results, + const Eigen::Vector3d& data, + const std::array& position_vars_fixed) { - auto end = std::remove_if( - contact_results.begin(), contact_results.end(), [=, &data](const tesseract_collision::ContactResult& r) { - /** @todo Is this correct? (Levi)*/ - return (!((data[0] + data[1]) > r.distance)); - }); + auto end = std::remove_if(contact_results.begin(), + contact_results.end(), + [=, &data, &position_vars_fixed](const tesseract_collision::ContactResult& r) { + /** @todo Is this correct? (Levi)*/ + if ((!((data[0] + data[1]) > r.distance))) + return true; + + if (!position_vars_fixed[0] && !position_vars_fixed[1]) + return false; + + if (position_vars_fixed[0]) + { + if (r.cc_type[0] != tesseract_collision::ContinuousCollisionType::CCType_None && + r.cc_type[0] != tesseract_collision::ContinuousCollisionType::CCType_Time0) + return false; + + if (r.cc_type[1] != tesseract_collision::ContinuousCollisionType::CCType_None && + r.cc_type[1] != tesseract_collision::ContinuousCollisionType::CCType_Time0) + return false; + } + + if (position_vars_fixed[1]) + { + if (r.cc_type[0] != tesseract_collision::ContinuousCollisionType::CCType_None && + r.cc_type[0] != tesseract_collision::ContinuousCollisionType::CCType_Time1) + return false; + + if (r.cc_type[1] != tesseract_collision::ContinuousCollisionType::CCType_None && + r.cc_type[1] != tesseract_collision::ContinuousCollisionType::CCType_Time1) + return false; + } + + return true; + }); contact_results.erase(end, contact_results.end()); } 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 31f5aa66c..9bb50c074 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 @@ -139,9 +139,10 @@ class LVSContinuousCollisionEvaluator : public ContinuousCollisionEvaluator CalcCollisionsCacheDataHelper(const Eigen::Ref& dof_vals0, const Eigen::Ref& dof_vals1); - void CalcCollisionsHelper(const Eigen::Ref& dof_vals0, + void CalcCollisionsHelper(tesseract_collision::ContactResultMap& dist_results, + const Eigen::Ref& dof_vals0, const Eigen::Ref& dof_vals1, - tesseract_collision::ContactResultMap& dist_results); + const std::array& position_vars_fixed); }; /** @@ -190,9 +191,10 @@ class LVSDiscreteCollisionEvaluator : public ContinuousCollisionEvaluator CalcCollisionsCacheDataHelper(const Eigen::Ref& dof_vals0, const Eigen::Ref& dof_vals1); - void CalcCollisionsHelper(const Eigen::Ref& dof_vals0, + void CalcCollisionsHelper(tesseract_collision::ContactResultMap& dist_results, + const Eigen::Ref& dof_vals0, const Eigen::Ref& dof_vals1, - tesseract_collision::ContactResultMap& dist_results); + const std::array& position_vars_fixed); }; } // namespace trajopt_ifopt #endif // TRAJOPT_IFOPT_CONTINUOUS_COLLISION_EVALUATOR_H diff --git a/trajopt_ifopt/src/constraints/collision/continuous_collision_evaluators.cpp b/trajopt_ifopt/src/constraints/collision/continuous_collision_evaluators.cpp index 5dbda07f0..188ffd85b 100644 --- a/trajopt_ifopt/src/constraints/collision/continuous_collision_evaluators.cpp +++ b/trajopt_ifopt/src/constraints/collision/continuous_collision_evaluators.cpp @@ -92,7 +92,7 @@ LVSContinuousCollisionEvaluator::CalcCollisionData(const Eigen::Ref(); - CalcCollisionsHelper(dof_vals0, dof_vals1, data->contact_results_map); + CalcCollisionsHelper(data->contact_results_map, dof_vals0, dof_vals1, position_vars_fixed); for (const auto& pair : data->contact_results_map) { @@ -165,9 +165,10 @@ LVSContinuousCollisionEvaluator::CalcCollisionData(const Eigen::Ref& dof_vals0, +void LVSContinuousCollisionEvaluator::CalcCollisionsHelper(tesseract_collision::ContactResultMap& dist_results, + const Eigen::Ref& dof_vals0, const Eigen::Ref& dof_vals1, - tesseract_collision::ContactResultMap& dist_results) + const std::array& position_vars_fixed) { // The first step is to see if the distance between two states is larger than the longest valid segment. If larger // the collision checking is broken up into multiple casted collision checks such that each check is less then @@ -186,7 +187,7 @@ void LVSContinuousCollisionEvaluator::CalcCollisionsHelper(const Eigen::Refcollision_coeff_data.getPairsWithZeroCoeff(); - auto filter = [this, &zero_coeff_pairs](tesseract_collision::ContactResultMap::PairType& pair) { + auto filter = [this, &zero_coeff_pairs, &position_vars_fixed](tesseract_collision::ContactResultMap::PairType& pair) { // Remove pairs with zero coeffs if (std::find(zero_coeff_pairs.begin(), zero_coeff_pairs.end(), pair.first) != zero_coeff_pairs.end()) { @@ -199,7 +200,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 }; - trajopt_common::removeInvalidContactResults(pair.second, data); /** @todo Should this be removed? levi */ + trajopt_common::removeInvalidContactResults(pair.second, data, position_vars_fixed); }; if (collision_config_->type == tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS && @@ -329,7 +330,7 @@ LVSDiscreteCollisionEvaluator::CalcCollisionData(const Eigen::Ref(); - CalcCollisionsHelper(dof_vals0, dof_vals1, data->contact_results_map); + CalcCollisionsHelper(data->contact_results_map, dof_vals0, dof_vals1, position_vars_fixed); for (const auto& pair : data->contact_results_map) { using ShapeGrsType = std::map, trajopt_common::GradientResultsSet>; @@ -401,9 +402,10 @@ LVSDiscreteCollisionEvaluator::CalcCollisionData(const Eigen::Ref& dof_vals0, +void LVSDiscreteCollisionEvaluator::CalcCollisionsHelper(tesseract_collision::ContactResultMap& dist_results, + const Eigen::Ref& dof_vals0, const Eigen::Ref& dof_vals1, - tesseract_collision::ContactResultMap& dist_results) + const std::array& position_vars_fixed) { // If not empty then there are links that are not part of the kinematics object that can move (dynamic environment) if (!diff_active_link_names_.empty()) @@ -417,7 +419,7 @@ void LVSDiscreteCollisionEvaluator::CalcCollisionsHelper(const Eigen::Refcollision_coeff_data.getPairsWithZeroCoeff(); - auto filter = [this, &zero_coeff_pairs](tesseract_collision::ContactResultMap::PairType& pair) { + auto filter = [this, &zero_coeff_pairs, &position_vars_fixed](tesseract_collision::ContactResultMap::PairType& pair) { // Remove pairs with zero coeffs if (std::find(zero_coeff_pairs.begin(), zero_coeff_pairs.end(), pair.first) != zero_coeff_pairs.end()) { @@ -432,7 +434,7 @@ void LVSDiscreteCollisionEvaluator::CalcCollisionsHelper(const Eigen::Refcollision_margin_buffer, coeff }; // Don't include contacts at the fixed state - trajopt_common::removeInvalidContactResults(pair.second, data); + trajopt_common::removeInvalidContactResults(pair.second, data, position_vars_fixed); }; // The first step is to see if the distance between two states is larger than the longest valid segment. If larger diff --git a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/types.h b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/types.h index b77c6809e..37a705def 100644 --- a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/types.h +++ b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/types.h @@ -215,7 +215,7 @@ enum class SQPStatus NLP_CONVERGED, /**< NLP Successfully converged */ ITERATION_LIMIT, /**< SQP Optimization reached iteration limit */ PENALTY_ITERATION_LIMIT, /**< SQP Optimization reached penalty iteration limit */ - TIME_LIMIT, /**< SQP Optimization reached reached limit */ + OPT_TIME_LIMIT, /**< SQP Optimization reached reached limit */ QP_SOLVER_ERROR, /**< QP Solver failed */ CALLBACK_STOPPED /**< Optimization stopped because callback returned false */ }; diff --git a/trajopt_optimizers/trajopt_sqp/src/trust_region_sqp_solver.cpp b/trajopt_optimizers/trajopt_sqp/src/trust_region_sqp_solver.cpp index cd12dcdba..630c823df 100644 --- a/trajopt_optimizers/trajopt_sqp/src/trust_region_sqp_solver.cpp +++ b/trajopt_optimizers/trajopt_sqp/src/trust_region_sqp_solver.cpp @@ -104,7 +104,7 @@ void TrustRegionSQPSolver::solve(const QPProblem::Ptr& qp_problem) if (elapsed_time > params.max_time) { CONSOLE_BRIDGE_logInform("Elapsed time %f has exceeded max time %f", elapsed_time, params.max_time); - status_ = SQPStatus::TIME_LIMIT; + status_ = SQPStatus::OPT_TIME_LIMIT; break; } @@ -127,7 +127,7 @@ void TrustRegionSQPSolver::solve(const QPProblem::Ptr& qp_problem) } // If status is iteration limit or time limit we need to exit penalty iteration loop - if (status_ == SQPStatus::ITERATION_LIMIT || status_ == SQPStatus::TIME_LIMIT) + if (status_ == SQPStatus::ITERATION_LIMIT || status_ == SQPStatus::OPT_TIME_LIMIT) break; // Set status to running