diff --git a/trajopt_common/src/collision_types.cpp b/trajopt_common/src/collision_types.cpp index 3ce4844d..d449899e 100644 --- a/trajopt_common/src/collision_types.cpp +++ b/trajopt_common/src/collision_types.cpp @@ -41,7 +41,7 @@ double CollisionCoeffData::getDefaultCollisionCoeff() const { return default_col void CollisionCoeffData::setCollisionCoeff(const std::string& obj1, const std::string& obj2, double collision_coeff) { auto key = tesseract_common::makeOrderedLinkPair(obj1, obj2); - lookup_table_[key] = collision_coeff; + lookup_table_.insert_or_assign(key, collision_coeff); if (tesseract_common::almostEqualRelativeAndAbs(collision_coeff, 0.0)) zero_coeff_.insert(key); @@ -72,9 +72,9 @@ const std::set& CollisionCoeffData::getPairsWit bool CollisionCoeffData::operator==(const CollisionCoeffData& rhs) const { - static auto max_diff = static_cast(std::numeric_limits::epsilon()); + static constexpr auto max_diff = static_cast(std::numeric_limits::epsilon()); - auto value_eq = [](const double& v1, const double& v2) { + static const auto value_eq = [](double v1, double v2) { return tesseract_common::almostEqualRelativeAndAbs(v1, v2, max_diff); }; @@ -147,35 +147,28 @@ double LinkMaxError::getMaxErrorWithBuffer() const void GradientResultsSet::add(const GradientResults& gradient_result) { - // Update max error for LinkA and LinkB excluding values at T1 for (std::size_t i = 0; i < 2; ++i) { - if (gradient_result.gradients[i].has_gradient && - gradient_result.gradients[i].cc_type != tesseract_collision::ContinuousCollisionType::CCType_Time1) + const auto& g = gradient_result.gradients[i]; + if (!g.has_gradient) + continue; + + // Update max error for LinkA and LinkB excluding values at T1 + if (g.cc_type != tesseract_collision::ContinuousCollisionType::CCType_Time1) { max_error[i].has_error[0] = true; - - if (gradient_result.error > max_error[i].error[0]) - max_error[i].error[0] = gradient_result.error; - - if (gradient_result.error_with_buffer > max_error[i].error_with_buffer[0]) - max_error[i].error_with_buffer[0] = gradient_result.error_with_buffer; + max_error[i].error[0] = std::max(max_error[i].error[0], gradient_result.error); + max_error[i].error_with_buffer[0] = + std::max(max_error[i].error_with_buffer[0], gradient_result.error_with_buffer); } - } - // Update max error for LinkA and LinkB excluding values at T0 - for (std::size_t i = 0; i < 2; ++i) - { - if (gradient_result.gradients[i].has_gradient && - gradient_result.gradients[i].cc_type != tesseract_collision::ContinuousCollisionType::CCType_Time0) + // Update max error for LinkA and LinkB excluding values at T0 + if (g.cc_type != tesseract_collision::ContinuousCollisionType::CCType_Time0) { max_error[i].has_error[1] = true; - - if (gradient_result.error > max_error[i].error[1]) - max_error[i].error[1] = gradient_result.error; - - if (gradient_result.error_with_buffer > max_error[i].error_with_buffer[1]) - max_error[i].error_with_buffer[1] = gradient_result.error_with_buffer; + max_error[i].error[1] = std::max(max_error[i].error[1], gradient_result.error); + max_error[i].error_with_buffer[1] = + std::max(max_error[i].error_with_buffer[1], gradient_result.error_with_buffer); } } diff --git a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/continuous_collision_constraint.h b/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/continuous_collision_constraint.h index f29c4436..3e6de7b9 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/continuous_collision_constraint.h +++ b/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/continuous_collision_constraint.h @@ -60,6 +60,14 @@ class ContinuousCollisionConstraint : public ifopt::ConstraintSet bool fixed_sparsity = false, const std::string& name = "LVSCollision"); + ContinuousCollisionConstraint(std::shared_ptr collision_evaluator, + std::vector> position_vars, + bool vars0_fixed, + bool vars1_fixed, + int max_num_cnt = 1, + bool fixed_sparsity = false, + const std::string& name = "LVSCollision"); + /** * @brief Returns the values associated with the constraint. * @warning Make sure that the values returns are not just the violation but the constraint values. @@ -101,11 +109,14 @@ class ContinuousCollisionConstraint : public ifopt::ConstraintSet /** @brief The number of joints in a single JointPosition */ long n_dof_; + /** @brief The per-position_var max constraints */ + std::size_t max_num_cnt_per_segment_{ 0 }; + /** @brief Bounds on the constraint value. Default: std::vector(1, ifopt::BoundSmallerZero) */ std::vector bounds_; /** @brief Pointers to the vars used by this constraint. */ - std::array, 2> position_vars_; + std::vector> position_vars_; bool vars0_fixed_{ false }; bool vars1_fixed_{ false }; @@ -113,10 +124,11 @@ class ContinuousCollisionConstraint : public ifopt::ConstraintSet /** @brief Used to initialize jacobian because snopt sparsity cannot change */ bool fixed_sparsity_{ false }; + mutable std::string var_set_name_; mutable std::once_flag init_flag_; mutable std::vector> triplet_list_; - void initSparsity() const; + void init() const; }; } // namespace trajopt_ifopt 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 183bb140..88cec255 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 @@ -60,10 +60,10 @@ class ContinuousCollisionEvaluator ContinuousCollisionEvaluator() = default; virtual ~ContinuousCollisionEvaluator() = default; - ContinuousCollisionEvaluator(const ContinuousCollisionEvaluator&) = default; - ContinuousCollisionEvaluator& operator=(const ContinuousCollisionEvaluator&) = default; - ContinuousCollisionEvaluator(ContinuousCollisionEvaluator&&) = default; - ContinuousCollisionEvaluator& operator=(ContinuousCollisionEvaluator&&) = default; + ContinuousCollisionEvaluator(const ContinuousCollisionEvaluator&) = delete; + ContinuousCollisionEvaluator& operator=(const ContinuousCollisionEvaluator&) = delete; + ContinuousCollisionEvaluator(ContinuousCollisionEvaluator&&) = delete; + ContinuousCollisionEvaluator& operator=(ContinuousCollisionEvaluator&&) = delete; /** * @brief Given joint names and values calculate the collision results for this evaluator diff --git a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/discrete_collision_constraint.h b/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/discrete_collision_constraint.h index f49927fd..fa8e0e2d 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/discrete_collision_constraint.h +++ b/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/discrete_collision_constraint.h @@ -47,7 +47,13 @@ class DiscreteCollisionConstraint : public ifopt::ConstraintSet std::shared_ptr position_var, int max_num_cnt = 1, bool fixed_sparsity = false, - const std::string& name = "DiscreteCollisionV3"); + const std::string& name = "DiscreteCollision"); + + DiscreteCollisionConstraint(std::shared_ptr collision_evaluator, + std::vector> position_vars, + int max_num_cnt = 1, + bool fixed_sparsity = false, + const std::string& name = "DiscreteCollision"); /** * @brief Returns the values associated with the constraint. @@ -75,21 +81,12 @@ class DiscreteCollisionConstraint : public ifopt::ConstraintSet */ void FillJacobianBlock(std::string var_set, Jacobian& jac_block) const override; - /** @brief Calculates the values associated with the constraint */ - Eigen::VectorXd CalcValues(const Eigen::Ref& joint_vals) const; - /** * @brief Sets the bounds on the collision distance * @param bounds New bounds that will be set. Should be size 1 */ void SetBounds(const std::vector& bounds); - /** - * @brief Fills the jacobian block associated with the constraint - * @param jac_block Block of the overall jacobian associated with these constraints - */ - void CalcJacobianBlock(const Eigen::Ref& joint_vals, Jacobian& jac_block) const; - /** * @brief Get the collision evaluator. This exposed for plotter callbacks * @return The collision evaluator @@ -98,22 +95,26 @@ class DiscreteCollisionConstraint : public ifopt::ConstraintSet private: /** @brief The number of joints in a single JointPosition */ - long n_dof_; + long n_dof_{ 0 }; + + /** @brief The per-position_var max constraints */ + std::size_t max_num_cnt_per_var_{ 0 }; /** @brief Bounds on the constraint value. Default: std::vector(1, ifopt::BoundSmallerZero) */ std::vector bounds_; /** @brief Pointers to the vars used by this constraint. */ - std::shared_ptr position_var_; + std::vector> position_vars_; std::shared_ptr collision_evaluator_; /** @brief Used to initialize jacobian because snopt sparsity cannot change */ bool fixed_sparsity_{ false }; + mutable std::string var_set_name_; mutable std::once_flag init_flag_; mutable std::vector> triplet_list_; - void initSparsity() const; + void init() const; }; } // namespace trajopt_ifopt 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 e4948095..e8a3126b 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 @@ -59,10 +59,10 @@ class DiscreteCollisionEvaluator DiscreteCollisionEvaluator() = default; virtual ~DiscreteCollisionEvaluator() = default; - DiscreteCollisionEvaluator(const DiscreteCollisionEvaluator&) = default; - DiscreteCollisionEvaluator& operator=(const DiscreteCollisionEvaluator&) = default; - DiscreteCollisionEvaluator(DiscreteCollisionEvaluator&&) = default; - DiscreteCollisionEvaluator& operator=(DiscreteCollisionEvaluator&&) = default; + DiscreteCollisionEvaluator(const DiscreteCollisionEvaluator&) = delete; + DiscreteCollisionEvaluator& operator=(const DiscreteCollisionEvaluator&) = delete; + DiscreteCollisionEvaluator(DiscreteCollisionEvaluator&&) = delete; + DiscreteCollisionEvaluator& operator=(DiscreteCollisionEvaluator&&) = delete; /** * @brief Given joint names and values calculate the collision results for this evaluator diff --git a/trajopt_ifopt/src/constraints/collision/continuous_collision_constraint.cpp b/trajopt_ifopt/src/constraints/collision/continuous_collision_constraint.cpp index 5c4799c6..9abf8688 100644 --- a/trajopt_ifopt/src/constraints/collision/continuous_collision_constraint.cpp +++ b/trajopt_ifopt/src/constraints/collision/continuous_collision_constraint.cpp @@ -45,69 +45,121 @@ ContinuousCollisionConstraint::ContinuousCollisionConstraint( int max_num_cnt, bool fixed_sparsity, const std::string& name) - : ifopt::ConstraintSet(max_num_cnt, name) + : ContinuousCollisionConstraint( + std::move(collision_evaluator), + std::vector>{ std::move(position_vars[0]), std::move(position_vars[1]) }, + vars0_fixed, + vars1_fixed, + max_num_cnt, + fixed_sparsity, + name) +{ +} + +ContinuousCollisionConstraint::ContinuousCollisionConstraint( + std::shared_ptr collision_evaluator, + std::vector> position_vars, + bool vars0_fixed, + bool vars1_fixed, + int max_num_cnt, + bool fixed_sparsity, + const std::string& name) + : ifopt::ConstraintSet(max_num_cnt * static_cast(position_vars.size() > 1 ? (position_vars.size() - 1) : 0), + name) , position_vars_(std::move(position_vars)) , vars0_fixed_(vars0_fixed) , vars1_fixed_(vars1_fixed) , collision_evaluator_(std::move(collision_evaluator)) , fixed_sparsity_(fixed_sparsity) { - if (position_vars_[0] == nullptr && position_vars_[1] == nullptr) - throw std::runtime_error("position_vars contains a nullptr!"); + if (position_vars_.size() < 2) + throw std::runtime_error("ContinuousCollisionConstraint: position_vars must contain at least two variables for " + "continuous collision"); - // Set n_dof_ for convenience - n_dof_ = position_vars_[0]->size(); - if (!(n_dof_ > 0)) - throw std::runtime_error("position_vars[0] is empty!"); + for (const auto& v : position_vars_) + { + if (v == nullptr) + throw std::runtime_error("ContinuousCollisionConstraint: position_vars contains a nullptr!"); + } + + // All vars must have same size + n_dof_ = position_vars_.front()->size(); + if (n_dof_ <= 0) + throw std::runtime_error("ContinuousCollisionConstraint: first position var is empty!"); - if (position_vars_[0]->size() != position_vars_[1]->size()) - throw std::runtime_error("position_vars are not the same size!"); + for (const auto& v : position_vars_) + { + if (v->size() != n_dof_) + throw std::runtime_error("ContinuousCollisionConstraint: all position_vars must have same size!"); + } - if (vars0_fixed_ && vars1_fixed_) - throw std::runtime_error("position_vars are both fixed!"); + if (position_vars_.size() == 2 && vars0_fixed_ && vars1_fixed_) + throw std::runtime_error("ContinuousCollisionConstraint: when number vars is two, both ends cannot be fixed!"); if (max_num_cnt < 1) - throw std::runtime_error("max_num_cnt must be greater than zero!"); + throw std::runtime_error("ContinuousCollisionConstraint: max_num_cnt must be greater than zero!"); + + max_num_cnt_per_segment_ = static_cast(max_num_cnt); + + const std::size_t num_segments = position_vars_.size() - 1; + const std::size_t total_rows = max_num_cnt_per_segment_ * num_segments; + + if (static_cast(total_rows) != GetRows()) + throw std::runtime_error("ContinuousCollisionConstraint: internal row count mismatch"); - bounds_ = std::vector(static_cast(max_num_cnt), ifopt::BoundSmallerZero); + bounds_ = std::vector(total_rows, ifopt::BoundSmallerZero); } Eigen::VectorXd ContinuousCollisionConstraint::GetValues() const { const double margin_buffer = collision_evaluator_->GetCollisionMarginBuffer(); - Eigen::VectorXd values = Eigen::VectorXd::Constant(static_cast(bounds_.size()), -margin_buffer); + Eigen::VectorXd values = Eigen::VectorXd::Constant(GetRows(), -margin_buffer); - auto collision_data = collision_evaluator_->CalcCollisionData( - position_vars_[0]->value(), position_vars_[1]->value(), vars0_fixed_, vars1_fixed_, bounds_.size()); + const std::size_t num_segments = position_vars_.size() - 1; - if (collision_data->gradient_results_sets.empty()) - return values; - - const std::size_t cnt = std::min(collision_data->gradient_results_sets.size(), bounds_.size()); - if (!vars0_fixed_ && !vars1_fixed_) + for (std::size_t seg = 0; seg < num_segments; ++seg) { - for (std::size_t i = 0; i < cnt; ++i) + const auto& var0 = position_vars_[seg]; + const auto& var1 = position_vars_[seg + 1]; + + const bool seg_vars0_fixed = (seg == 0) ? vars0_fixed_ : false; + const bool seg_vars1_fixed = (seg == num_segments - 1) ? vars1_fixed_ : false; + + auto collision_data = collision_evaluator_->CalcCollisionData( + var0->value(), var1->value(), seg_vars0_fixed, seg_vars1_fixed, max_num_cnt_per_segment_); + + if (collision_data->gradient_results_sets.empty()) + continue; + + const std::size_t row_offset = seg * max_num_cnt_per_segment_; + const std::size_t cnt = + std::min(max_num_cnt_per_segment_, collision_data->gradient_results_sets.size()); + + if (!seg_vars0_fixed && !seg_vars1_fixed) { - const trajopt_common::GradientResultsSet& r = collision_data->gradient_results_sets[i]; - values(static_cast(i)) = r.coeff * r.getMaxError(); + for (std::size_t i = 0; i < cnt; ++i) + { + const trajopt_common::GradientResultsSet& r = collision_data->gradient_results_sets[i]; + values(static_cast(row_offset + i)) = r.coeff * r.getMaxError(); + } } - } - else if (!vars0_fixed_) - { - for (std::size_t i = 0; i < cnt; ++i) + else if (!seg_vars0_fixed) { - 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(); + for (std::size_t i = 0; i < cnt; ++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(row_offset + i)) = r.coeff * r.getMaxErrorT0(); + } } - } - else - { - for (std::size_t i = 0; i < cnt; ++i) + else // !seg_vars1_fixed { - 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(); + for (std::size_t i = 0; i < cnt; ++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(row_offset + i)) = r.coeff * r.getMaxErrorT1(); + } } } @@ -117,82 +169,125 @@ Eigen::VectorXd ContinuousCollisionConstraint::GetValues() const // Set the limits on the constraint values std::vector ContinuousCollisionConstraint::GetBounds() const { return bounds_; } -void ContinuousCollisionConstraint::initSparsity() const +void ContinuousCollisionConstraint::init() const { + const auto* parent_set0 = position_vars_.front()->getParent()->getParent(); + if (parent_set0 == nullptr) + throw std::runtime_error("ContinuousCollisionConstraint: invalid variable parent"); + + for (const auto& v : position_vars_) + { + const auto* ps = v->getParent()->getParent(); + if (ps != parent_set0) + throw std::runtime_error("ContinuousCollisionConstraint: all vars must belong to the same variable set"); + } + + var_set_name_ = parent_set0->GetName(); + if (!fixed_sparsity_) return; - // Setting to zeros because snopt sparsity cannot change - triplet_list_.reserve(static_cast(bounds_.size()) * static_cast(position_vars_[0]->size())); + triplet_list_.clear(); - for (Eigen::Index i = 0; i < static_cast(bounds_.size()); i++) // NOLINT + const std::size_t num_segments = position_vars_.size() - 1; + const Eigen::Index total_rows = GetRows(); + + triplet_list_.reserve(static_cast(total_rows) * static_cast(2 * n_dof_)); + + for (std::size_t seg = 0; seg < num_segments; ++seg) { - for (Eigen::Index j = 0; j < n_dof_; j++) + const auto& var0 = position_vars_[seg]; + const auto& var1 = position_vars_[seg + 1]; + + const Eigen::Index base_idx0 = var0->getIndex(); + const Eigen::Index base_idx1 = var1->getIndex(); + + const std::size_t row_base = seg * max_num_cnt_per_segment_; + for (int k = 0; k < max_num_cnt_per_segment_; ++k) { - triplet_list_.emplace_back(i, position_vars_[0]->getIndex() + j, 0); - triplet_list_.emplace_back(i, position_vars_[1]->getIndex() + j, 0); + const auto row = static_cast(row_base + static_cast(k)); + for (Eigen::Index j = 0; j < n_dof_; ++j) + { + triplet_list_.emplace_back(row, base_idx0 + j, 0.0); + triplet_list_.emplace_back(row, base_idx1 + j, 0.0); + } } } } void ContinuousCollisionConstraint::FillJacobianBlock(std::string var_set, Jacobian& jac_block) const { + std::call_once(init_flag_, &ContinuousCollisionConstraint::init, this); + // Only modify the jacobian if this constraint uses var_set - if (var_set != position_vars_.front()->getParent()->getParent()->GetName()) // NOLINT + if (var_set != var_set_name_) // NOLINT return; - // Setting to zeros because snopt sparsity cannot change - std::call_once(init_flag_, &ContinuousCollisionConstraint::initSparsity, this); - - // Setting to zeros because snopt sparsity cannot change + // Setting to zeros because SNOPT sparsity cannot change if (!triplet_list_.empty()) // NOLINT jac_block.setFromTriplets(triplet_list_.begin(), triplet_list_.end()); // NOLINT - // Calculate collisions - auto collision_data = collision_evaluator_->CalcCollisionData( - position_vars_[0]->value(), position_vars_[1]->value(), vars0_fixed_, vars1_fixed_, bounds_.size()); - - if (collision_data->gradient_results_sets.empty()) - return; + const std::size_t num_segments = position_vars_.size() - 1; - /** @todo Should use triplet list and setFromTriplets */ - const std::size_t cnt = std::min(collision_data->gradient_results_sets.size(), bounds_.size()); - if (!vars0_fixed_) + for (std::size_t seg = 0; seg < num_segments; ++seg) { - for (std::size_t i = 0; i < cnt; ++i) + const auto& var0 = position_vars_[seg]; + const auto& var1 = position_vars_[seg + 1]; + + const bool seg_vars0_fixed = (seg == 0) ? vars0_fixed_ : false; + const bool seg_vars1_fixed = (seg == num_segments - 1) ? vars1_fixed_ : false; + + auto collision_data = collision_evaluator_->CalcCollisionData( + var0->value(), var1->value(), seg_vars0_fixed, seg_vars1_fixed, max_num_cnt_per_segment_); + + if (collision_data->gradient_results_sets.empty()) + continue; + + const std::size_t row_offset = seg * max_num_cnt_per_segment_; + const std::size_t cnt = + std::min(max_num_cnt_per_segment_, collision_data->gradient_results_sets.size()); + + // T0 contributions + if (!seg_vars0_fixed) { - 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]) + const Eigen::Index base_idx0 = var0->getIndex(); + for (std::size_t i = 0; i < cnt; ++i) { - double max_error_with_buffer = r.getMaxErrorWithBufferT0(); - if (!vars1_fixed_) - max_error_with_buffer = r.getMaxErrorWithBuffer(); + 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(); + if (!seg_vars1_fixed) + max_error_with_buffer = r.getMaxErrorWithBuffer(); - Eigen::VectorXd grad_vec = getWeightedAvgGradientT0(r, max_error_with_buffer, position_vars_[0]->size()); + Eigen::VectorXd grad_vec = getWeightedAvgGradientT0(r, max_error_with_buffer, n_dof_); - // Collision is 1 x n_dof - for (int j = 0; j < n_dof_; j++) - jac_block.coeffRef(static_cast(i), position_vars_[0]->getIndex() + j) = -1.0 * grad_vec[j]; + const int row = static_cast(row_offset + i); + for (Eigen::Index j = 0; j < n_dof_; ++j) + jac_block.coeffRef(row, static_cast(base_idx0 + j)) = -1.0 * grad_vec[j]; + } } } - } - if (!vars1_fixed_) - { - for (std::size_t i = 0; i < cnt; ++i) + // T1 contributions + if (!seg_vars1_fixed) { - 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]) + const Eigen::Index base_idx1 = var1->getIndex(); + for (std::size_t i = 0; i < cnt; ++i) { - double max_error_with_buffer = r.getMaxErrorWithBufferT1(); - if (!vars0_fixed_) - max_error_with_buffer = r.getMaxErrorWithBuffer(); + 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(); + if (!seg_vars0_fixed) + max_error_with_buffer = r.getMaxErrorWithBuffer(); - Eigen::VectorXd grad_vec = getWeightedAvgGradientT1(r, max_error_with_buffer, position_vars_[1]->size()); + Eigen::VectorXd grad_vec = getWeightedAvgGradientT1(r, max_error_with_buffer, n_dof_); - // Collision is 1 x n_dof - for (int j = 0; j < n_dof_; j++) - jac_block.coeffRef(static_cast(i), position_vars_[1]->getIndex() + j) = -1.0 * grad_vec[j]; + const int row = static_cast(row_offset + i); + for (Eigen::Index j = 0; j < n_dof_; ++j) + jac_block.coeffRef(row, static_cast(base_idx1 + j)) = -1.0 * grad_vec[j]; + } } } } @@ -200,7 +295,7 @@ void ContinuousCollisionConstraint::FillJacobianBlock(std::string var_set, Jacob void ContinuousCollisionConstraint::SetBounds(const std::vector& bounds) { - assert(bounds.size() == 1); + assert(bounds.size() == GetRows()); bounds_ = bounds; } diff --git a/trajopt_ifopt/src/constraints/collision/continuous_collision_evaluators.cpp b/trajopt_ifopt/src/constraints/collision/continuous_collision_evaluators.cpp index 5db21ed3..9718c23a 100644 --- a/trajopt_ifopt/src/constraints/collision/continuous_collision_evaluators.cpp +++ b/trajopt_ifopt/src/constraints/collision/continuous_collision_evaluators.cpp @@ -106,11 +106,10 @@ LVSContinuousCollisionEvaluator::CalcCollisionData(const Eigen::Refget(key); - if (it != nullptr) + if (auto* cached = collision_cache_->get(key); cached != nullptr) { CONSOLE_BRIDGE_logDebug("Using cached collision check"); - return *it; + return *cached; } auto data = std::make_shared(); @@ -118,68 +117,54 @@ LVSContinuousCollisionEvaluator::CalcCollisionData(const Eigen::Refcontact_results_map) { - using ShapeGrsType = std::map, trajopt_common::GradientResultsSet>; - ShapeGrsType shape_grs; + using ShapeKey = std::pair; + using ShapeGrsMap = std::map; + ShapeGrsMap shape_grs; + const double coeff = coeff_data_.getCollisionCoeff(pair.first.first, pair.first.second); - for (const tesseract_collision::ContactResult& dist_result : pair.second) + const auto& results = pair.second; + + for (const tesseract_collision::ContactResult& dist_result : results) { 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()) + ShapeKey shape_key{ shape_hash0, shape_hash1 }; + + auto [it_shape, inserted] = shape_grs.try_emplace(shape_key); + auto& grs = it_shape->second; + + if (inserted) { - trajopt_common::GradientResultsSet grs; grs.key = pair.first; grs.shape_key = shape_key; grs.coeff = coeff; grs.is_continuous = true; - grs.results.reserve(pair.second.size()); - grs.add(CalcGradientData(dof_vals0, dof_vals1, dist_result)); - shape_grs[shape_key] = grs; - } - else - { - it->second.add(CalcGradientData(dof_vals0, dof_vals1, dist_result)); + grs.results.reserve(results.size()); } + + grs.add(CalcGradientData(dof_vals0, dof_vals1, dist_result)); } - // This is not as efficient as it could be. Need to update Tesseract to store per subhshape key const std::size_t new_size = data->gradient_results_sets.size() + shape_grs.size(); data->gradient_results_sets.reserve(new_size); - std::transform(shape_grs.begin(), - shape_grs.end(), - std::back_inserter(data->gradient_results_sets), - std::bind(&ShapeGrsType::value_type::second, std::placeholders::_1)); // NOLINT + for (auto& kv : shape_grs) + data->gradient_results_sets.push_back(std::move(kv.second)); } if (data->gradient_results_sets.size() > bounds_size) { - if (!vars0_fixed && !vars1_fixed) - { - std::sort(data->gradient_results_sets.begin(), - data->gradient_results_sets.end(), - [](const trajopt_common::GradientResultsSet& a, const trajopt_common::GradientResultsSet& b) { - return a.getMaxErrorWithBuffer() > b.getMaxErrorWithBuffer(); - }); - } - else if (!vars0_fixed) - { - std::sort(data->gradient_results_sets.begin(), - data->gradient_results_sets.end(), - [](const trajopt_common::GradientResultsSet& a, const trajopt_common::GradientResultsSet& b) { - return a.getMaxErrorWithBufferT0() > b.getMaxErrorWithBufferT0(); - }); - } - else - { - std::sort(data->gradient_results_sets.begin(), - data->gradient_results_sets.end(), - [](const trajopt_common::GradientResultsSet& a, const trajopt_common::GradientResultsSet& b) { - return a.getMaxErrorWithBufferT1() > b.getMaxErrorWithBufferT1(); - }); - } + auto cmp = [vars0_fixed, vars1_fixed](const trajopt_common::GradientResultsSet& a, + const trajopt_common::GradientResultsSet& b) { + if (!vars0_fixed && !vars1_fixed) + return a.getMaxErrorWithBuffer() > b.getMaxErrorWithBuffer(); + if (!vars0_fixed) + return a.getMaxErrorWithBufferT0() > b.getMaxErrorWithBufferT0(); + return a.getMaxErrorWithBufferT1() > b.getMaxErrorWithBufferT1(); + }; + + std::sort(data->gradient_results_sets.begin(), data->gradient_results_sets.end(), cmp); + // (Optional future tweak: partial_sort + erase to keep only top bounds_size) } collision_cache_->put(key, data); @@ -359,79 +344,65 @@ LVSDiscreteCollisionEvaluator::CalcCollisionData(const Eigen::Refget(key); - if (it != nullptr) + if (auto* cached = collision_cache_->get(key); cached != nullptr) { CONSOLE_BRIDGE_logDebug("Using cached collision check"); - return *it; + return *cached; } auto data = std::make_shared(); CalcCollisionsHelper(data->contact_results_map, dof_vals0, dof_vals1, vars0_fixed, vars1_fixed); + for (const auto& pair : data->contact_results_map) { - using ShapeGrsType = std::map, trajopt_common::GradientResultsSet>; - ShapeGrsType shape_grs; + using ShapeKey = std::pair; + using ShapeGrsMap = std::map; + ShapeGrsMap shape_grs; + const double coeff = coeff_data_.getCollisionCoeff(pair.first.first, pair.first.second); - for (const tesseract_collision::ContactResult& dist_result : pair.second) + const auto& results = pair.second; + + for (const tesseract_collision::ContactResult& dist_result : results) { 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()) + ShapeKey shape_key{ shape_hash0, shape_hash1 }; + + auto [it_shape, inserted] = shape_grs.try_emplace(shape_key); + auto& grs = it_shape->second; + + if (inserted) { - trajopt_common::GradientResultsSet grs; grs.key = pair.first; grs.shape_key = shape_key; grs.coeff = coeff; grs.is_continuous = true; - grs.results.reserve(pair.second.size()); - grs.add(CalcGradientData(dof_vals0, dof_vals1, dist_result)); - shape_grs[shape_key] = grs; - } - else - { - it->second.add(CalcGradientData(dof_vals0, dof_vals1, dist_result)); + grs.results.reserve(results.size()); } + + grs.add(CalcGradientData(dof_vals0, dof_vals1, dist_result)); } - // This is not as efficient as it could be. Need to update Tesseract to store per subhshape key const std::size_t new_size = data->gradient_results_sets.size() + shape_grs.size(); data->gradient_results_sets.reserve(new_size); - std::transform(shape_grs.begin(), - shape_grs.end(), - std::back_inserter(data->gradient_results_sets), - std::bind(&ShapeGrsType::value_type::second, std::placeholders::_1)); // NOLINT + for (auto& kv : shape_grs) + data->gradient_results_sets.push_back(std::move(kv.second)); } if (data->gradient_results_sets.size() > bounds_size) { - if (!vars0_fixed && !vars1_fixed) - { - std::sort(data->gradient_results_sets.begin(), - data->gradient_results_sets.end(), - [](const trajopt_common::GradientResultsSet& a, const trajopt_common::GradientResultsSet& b) { - return a.getMaxErrorWithBuffer() > b.getMaxErrorWithBuffer(); - }); - } - else if (!vars0_fixed) - { - std::sort(data->gradient_results_sets.begin(), - data->gradient_results_sets.end(), - [](const trajopt_common::GradientResultsSet& a, const trajopt_common::GradientResultsSet& b) { - return a.getMaxErrorWithBufferT0() > b.getMaxErrorWithBufferT0(); - }); - } - else - { - std::sort(data->gradient_results_sets.begin(), - data->gradient_results_sets.end(), - [](const trajopt_common::GradientResultsSet& a, const trajopt_common::GradientResultsSet& b) { - return a.getMaxErrorWithBufferT1() > b.getMaxErrorWithBufferT1(); - }); - } + auto cmp = [vars0_fixed, vars1_fixed](const trajopt_common::GradientResultsSet& a, + const trajopt_common::GradientResultsSet& b) { + if (!vars0_fixed && !vars1_fixed) + return a.getMaxErrorWithBuffer() > b.getMaxErrorWithBuffer(); + if (!vars0_fixed) + return a.getMaxErrorWithBufferT0() > b.getMaxErrorWithBufferT0(); + return a.getMaxErrorWithBufferT1() > b.getMaxErrorWithBufferT1(); + }; + + std::sort(data->gradient_results_sets.begin(), data->gradient_results_sets.end(), cmp); + // Same optional partial_sort+erase idea as above. } collision_cache_->put(key, data); diff --git a/trajopt_ifopt/src/constraints/collision/discrete_collision_constraint.cpp b/trajopt_ifopt/src/constraints/collision/discrete_collision_constraint.cpp index c14cee61..4879e02b 100644 --- a/trajopt_ifopt/src/constraints/collision/discrete_collision_constraint.cpp +++ b/trajopt_ifopt/src/constraints/collision/discrete_collision_constraint.cpp @@ -45,98 +45,148 @@ DiscreteCollisionConstraint::DiscreteCollisionConstraint( int max_num_cnt, bool fixed_sparsity, const std::string& name) - : ifopt::ConstraintSet(max_num_cnt, name) - , position_var_(std::move(position_var)) + : DiscreteCollisionConstraint(std::move(collision_evaluator), + std::vector>{ std::move(position_var) }, + max_num_cnt, + fixed_sparsity, + name) +{ +} + +DiscreteCollisionConstraint::DiscreteCollisionConstraint( + std::shared_ptr collision_evaluator, + std::vector> position_vars, + int max_num_cnt, + bool fixed_sparsity, + const std::string& name) + : ifopt::ConstraintSet(max_num_cnt * static_cast(position_vars.size()), name) + , position_vars_(std::move(position_vars)) , collision_evaluator_(std::move(collision_evaluator)) , fixed_sparsity_(fixed_sparsity) { - // Set n_dof_ for convenience - n_dof_ = position_var_->size(); - assert(n_dof_ > 0); + if (position_vars_.empty()) + throw std::runtime_error("DiscreteCollisionConstraint: position_vars must not be empty"); if (max_num_cnt < 1) throw std::runtime_error("max_num_cnt must be greater than zero!"); - bounds_ = std::vector(static_cast(max_num_cnt), ifopt::BoundSmallerZero); + max_num_cnt_per_var_ = static_cast(max_num_cnt); + + // Total DOFs = sum of sizes (no contiguity assumption) + n_dof_ = 0; + for (const auto& v : position_vars_) + n_dof_ += v->size(); + + assert(n_dof_ > 0); + + bounds_ = std::vector(static_cast(GetRows()), ifopt::BoundSmallerZero); } -Eigen::VectorXd DiscreteCollisionConstraint::GetValues() const { return CalcValues(position_var_->value()); } +Eigen::VectorXd DiscreteCollisionConstraint::GetValues() const +{ + const double margin_buffer = collision_evaluator_->GetCollisionMarginBuffer(); + + Eigen::VectorXd values = Eigen::VectorXd::Constant(static_cast(bounds_.size()), -margin_buffer); + + std::size_t offset = 0; + for (const auto& v : position_vars_) + { + const trajopt_common::CollisionCacheData::ConstPtr collision_data = + collision_evaluator_->CalcCollisions(v->value(), max_num_cnt_per_var_); + if (collision_data->gradient_results_sets.empty()) + { + offset += max_num_cnt_per_var_; + continue; + } + + const std::size_t cnt = std::min(max_num_cnt_per_var_, collision_data->gradient_results_sets.size()); + for (std::size_t i = 0; i < cnt; ++i) + { + const trajopt_common::GradientResultsSet& r = collision_data->gradient_results_sets[i]; + values(static_cast(offset + i)) = r.coeff * r.getMaxErrorT0(); + } + + offset += max_num_cnt_per_var_; + } + + return values; +} // Set the limits on the constraint values std::vector DiscreteCollisionConstraint::GetBounds() const { return bounds_; } void DiscreteCollisionConstraint::FillJacobianBlock(std::string var_set, Jacobian& jac_block) const { + std::call_once(init_flag_, &DiscreteCollisionConstraint::init, this); + // Only modify the jacobian if this constraint uses var_set - if (var_set != position_var_->getParent()->getParent()->GetName()) // NOLINT + if (var_set != var_set_name_) // NOLINT return; - CalcJacobianBlock(position_var_->value(), jac_block); // NOLINT -} + // Setting to zeros because SNOPT sparsity cannot change + if (!triplet_list_.empty()) // NOLINT + jac_block.setFromTriplets(triplet_list_.begin(), triplet_list_.end()); // NOLINT -Eigen::VectorXd DiscreteCollisionConstraint::CalcValues(const Eigen::Ref& joint_vals) const -{ - // Check the collisions - const trajopt_common::CollisionCacheData::ConstPtr collision_data = - collision_evaluator_->CalcCollisions(joint_vals, bounds_.size()); - const double margin_buffer = collision_evaluator_->GetCollisionMarginBuffer(); - Eigen::VectorXd values = Eigen::VectorXd::Constant(static_cast(bounds_.size()), -margin_buffer); + for (std::size_t idx = 0; idx < position_vars_.size(); ++idx) + { + const auto& v = position_vars_[idx]; + const trajopt_common::CollisionCacheData::ConstPtr collision_data = + collision_evaluator_->CalcCollisions(v->value(), max_num_cnt_per_var_); - if (collision_data->gradient_results_sets.empty()) - return values; + if (collision_data->gradient_results_sets.empty()) + continue; - const std::size_t cnt = std::min(bounds_.size(), collision_data->gradient_results_sets.size()); - for (std::size_t i = 0; i < cnt; ++i) - { - const trajopt_common::GradientResultsSet& r = collision_data->gradient_results_sets[i]; - values(static_cast(i)) = r.coeff * r.getMaxErrorT0(); - } + const std::size_t cnt = std::min(max_num_cnt_per_var_, collision_data->gradient_results_sets.size()); + const std::size_t row_offset = idx * max_num_cnt_per_var_; - return values; + for (std::size_t i = 0; i < cnt; ++i) + { + const trajopt_common::GradientResultsSet& r = collision_data->gradient_results_sets[i]; + Eigen::VectorXd grad_vec = getWeightedAvgGradientT0(r, r.getMaxErrorWithBufferT0(), v->size()); + + const Eigen::Index base_idx = v->getIndex(); + const Eigen::Index sz = v->size(); + const int row = static_cast(row_offset + i); + + for (Eigen::Index j = 0; j < sz; ++j) + jac_block.coeffRef(row, static_cast(base_idx + j)) = -1.0 * grad_vec[j]; + } + } } void DiscreteCollisionConstraint::SetBounds(const std::vector& bounds) { - assert(bounds.size() == 1); + assert(bounds.size() == GetRows()); bounds_ = bounds; } -void DiscreteCollisionConstraint::initSparsity() const +void DiscreteCollisionConstraint::init() const { + const auto* parent_set = position_vars_.front()->getParent()->getParent(); + var_set_name_ = parent_set->GetName(); + for (const auto& v : position_vars_) + { + const auto* v_parent_set = v->getParent()->getParent(); + if (v_parent_set != parent_set) + throw std::runtime_error("DiscreteCollisionConstraint: all vars must belong to the same variable set"); + } + if (!fixed_sparsity_) return; - triplet_list_.reserve(static_cast(bounds_.size()) * static_cast(position_var_->size())); - for (Eigen::Index i = 0; i < static_cast(bounds_.size()); i++) - for (Eigen::Index j = 0; j < n_dof_; j++) - triplet_list_.emplace_back(i, position_var_->getIndex() + j, 0); -} - -void DiscreteCollisionConstraint::CalcJacobianBlock(const Eigen::Ref& joint_vals, - Jacobian& jac_block) const -{ - // Setting to zeros because snopt sparsity cannot change - std::call_once(init_flag_, &DiscreteCollisionConstraint::initSparsity, this); - - // Setting to zeros because snopt sparsity cannot change - if (!triplet_list_.empty()) // NOLINT - jac_block.setFromTriplets(triplet_list_.begin(), triplet_list_.end()); // NOLINT - - const trajopt_common::CollisionCacheData::ConstPtr collision_data = - collision_evaluator_->CalcCollisions(joint_vals, bounds_.size()); - if (collision_data->gradient_results_sets.empty()) - return; + const Eigen::Index n_rows = GetRows(); + triplet_list_.clear(); + triplet_list_.reserve(static_cast(n_rows) * static_cast(n_dof_)); - /** @todo Probably should use a triplet list and setFromTriplets */ - const std::size_t cnt = std::min(bounds_.size(), collision_data->gradient_results_sets.size()); - for (std::size_t i = 0; i < cnt; ++i) + for (Eigen::Index i = 0; i < n_rows; ++i) { - const trajopt_common::GradientResultsSet& r = collision_data->gradient_results_sets[i]; - Eigen::VectorXd grad_vec = getWeightedAvgGradientT0(r, r.getMaxErrorWithBufferT0(), position_var_->size()); - - // Collision is 1 x n_dof - for (int j = 0; j < n_dof_; j++) - jac_block.coeffRef(static_cast(i), position_var_->getIndex() + j) = -1.0 * grad_vec[j]; + for (const auto& v : position_vars_) + { + const Eigen::Index base_idx = v->getIndex(); + const Eigen::Index sz = v->size(); + for (Eigen::Index j = 0; j < sz; ++j) + triplet_list_.emplace_back(i, base_idx + j, 0.0); + } } } diff --git a/trajopt_ifopt/src/constraints/collision/discrete_collision_evaluators.cpp b/trajopt_ifopt/src/constraints/collision/discrete_collision_evaluators.cpp index 05cc77d4..7e08213e 100644 --- a/trajopt_ifopt/src/constraints/collision/discrete_collision_evaluators.cpp +++ b/trajopt_ifopt/src/constraints/collision/discrete_collision_evaluators.cpp @@ -108,39 +108,39 @@ SingleTimestepCollisionEvaluator::CalcCollisions(const Eigen::Refcontact_results_map) { - using ShapeGrsType = std::map, trajopt_common::GradientResultsSet>; - ShapeGrsType shape_grs; + using ShapeKey = std::pair; + using ShapeGrsMap = std::map; + + if (pair.second.empty()) + continue; + + ShapeGrsMap shape_grs; const double coeff = coeff_data_.getCollisionCoeff(pair.first.first, pair.first.second); - for (const tesseract_collision::ContactResult& dist_result : pair.second) + + for (const auto& dist_result : pair.second) { 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()) + const ShapeKey shape_key{ shape_hash0, shape_hash1 }; + + auto [it, inserted] = shape_grs.try_emplace(shape_key); + auto& grs = it->second; + + if (inserted) { - trajopt_common::GradientResultsSet grs; grs.key = pair.first; grs.shape_key = shape_key; grs.coeff = coeff; grs.results.reserve(pair.second.size()); - grs.add(GetGradient(dof_vals, dist_result)); - shape_grs[shape_key] = grs; } - else - { - it->second.add(GetGradient(dof_vals, dist_result)); - } - } - // This is not as efficient as it could be. Need to update Tesseract to store per subhshape key - const std::size_t new_size = data->gradient_results_sets.size() + shape_grs.size(); - data->gradient_results_sets.reserve(new_size); + grs.add(GetGradient(dof_vals, dist_result)); + } - std::transform(shape_grs.begin(), - shape_grs.end(), - std::back_inserter(data->gradient_results_sets), - std::bind(&ShapeGrsType::value_type::second, std::placeholders::_1)); // NOLINT + // Move results out instead of copying + data->gradient_results_sets.reserve(data->gradient_results_sets.size() + shape_grs.size()); + for (auto& kv : shape_grs) + data->gradient_results_sets.emplace_back(std::move(kv.second)); } if (data->gradient_results_sets.size() > bounds_size) diff --git a/trajopt_ifopt/src/constraints/collision/weighted_average_methods.cpp b/trajopt_ifopt/src/constraints/collision/weighted_average_methods.cpp index 27004f12..d8d48dd0 100644 --- a/trajopt_ifopt/src/constraints/collision/weighted_average_methods.cpp +++ b/trajopt_ifopt/src/constraints/collision/weighted_average_methods.cpp @@ -50,7 +50,7 @@ Eigen::VectorXd getWeightedAvgGradientT0(const trajopt_common::GradientResultsSe const double w = (std::max(grad.error_with_buffer, 0.0) / max_error_with_buffer); assert(!(w < 0)); total_weight += w; - grad_vec += w * (grad.gradients[i].scale * grad.gradients[i].gradient); + grad_vec.noalias() += w * (grad.gradients[i].scale * grad.gradients[i].gradient); ++cnt; } } @@ -61,7 +61,8 @@ Eigen::VectorXd getWeightedAvgGradientT0(const trajopt_common::GradientResultsSe return grad_vec; assert(total_weight > 0); - return (1.0 / total_weight) * grad_results_set.coeff * grad_vec; + const double scale = grad_results_set.coeff / total_weight; + return scale * grad_vec; } Eigen::VectorXd getWeightedAvgGradientT1(const trajopt_common::GradientResultsSet& grad_results_set, @@ -88,7 +89,7 @@ Eigen::VectorXd getWeightedAvgGradientT1(const trajopt_common::GradientResultsSe const double w = (std::max(grad.error_with_buffer, 0.0) / max_error_with_buffer); assert(!(w < 0)); total_weight += w; - grad_vec += w * (grad.cc_gradients[i].scale * grad.cc_gradients[i].gradient); + grad_vec.noalias() += w * (grad.cc_gradients[i].scale * grad.cc_gradients[i].gradient); ++cnt; } } @@ -99,6 +100,7 @@ Eigen::VectorXd getWeightedAvgGradientT1(const trajopt_common::GradientResultsSe return grad_vec; assert(total_weight > 0); - return (1.0 / total_weight) * grad_results_set.coeff * grad_vec; + const double scale = grad_results_set.coeff / total_weight; + return scale * grad_vec; } } // namespace trajopt_ifopt diff --git a/trajopt_ifopt/src/constraints/joint_acceleration_constraint.cpp b/trajopt_ifopt/src/constraints/joint_acceleration_constraint.cpp index cda80533..c90889ac 100644 --- a/trajopt_ifopt/src/constraints/joint_acceleration_constraint.cpp +++ b/trajopt_ifopt/src/constraints/joint_acceleration_constraint.cpp @@ -33,7 +33,7 @@ TRAJOPT_IGNORE_WARNINGS_POP namespace trajopt_ifopt { JointAccelConstraint::JointAccelConstraint(const Eigen::VectorXd& targets, - const std::vector >& position_vars, + const std::vector>& position_vars, const Eigen::VectorXd& coeffs, const std::string& name) : ifopt::ConstraintSet(static_cast(targets.size()) * static_cast(position_vars.size()), name) @@ -78,26 +78,29 @@ JointAccelConstraint::JointAccelConstraint(const Eigen::VectorXd& targets, Eigen::VectorXd JointAccelConstraint::GetValues() const { - Eigen::VectorXd acceleration(static_cast(n_dof_) * position_vars_.size()); + const std::size_t n = position_vars_.size(); + Eigen::VectorXd acceleration(n_dof_ * static_cast(n)); - // Forward Diff - for (std::size_t ind = 0; ind < position_vars_.size() - 2; ind++) + // Forward diff for timesteps [0, n-3] + for (std::size_t i = 0; i < n - 2; ++i) { - auto vals1 = position_vars_[ind]->value(); - auto vals2 = position_vars_[ind + 1]->value(); - auto vals3 = position_vars_[ind + 2]->value(); - const Eigen::VectorXd single_step = (vals3 - 2 * vals2 + vals1); - acceleration.block(n_dof_ * static_cast(ind), 0, n_dof_, 1) = coeffs_.cwiseProduct(single_step); + const Eigen::VectorXd& q0 = position_vars_[i]->value(); + const Eigen::VectorXd& q1 = position_vars_[i + 1]->value(); + const Eigen::VectorXd& q2 = position_vars_[i + 2]->value(); + + const Eigen::VectorXd single_step = q2 - 2.0 * q1 + q0; + acceleration.segment(static_cast(i) * n_dof_, n_dof_) = coeffs_.cwiseProduct(single_step); } - // Backward Diff - for (std::size_t ind = position_vars_.size() - 2; ind < position_vars_.size(); ind++) + // Backward diff for the last two timesteps [n-2, n-1] + for (std::size_t i = n - 2; i < n; ++i) { - auto vals1 = position_vars_[ind]->value(); - auto vals2 = position_vars_[ind - 1]->value(); - auto vals3 = position_vars_[ind - 2]->value(); - const Eigen::VectorXd single_step = (vals3 - 2 * vals2 + vals1); - acceleration.block(n_dof_ * static_cast(ind), 0, n_dof_, 1) = coeffs_.cwiseProduct(single_step); + const Eigen::VectorXd& q0 = position_vars_[i]->value(); // q_i + const Eigen::VectorXd& q1 = position_vars_[i - 1]->value(); // q_{i-1} + const Eigen::VectorXd& q2 = position_vars_[i - 2]->value(); // q_{i-2} + + const Eigen::VectorXd single_step = q2 - 2.0 * q1 + q0; + acceleration.segment(static_cast(i) * n_dof_, n_dof_) = coeffs_.cwiseProduct(single_step); } return acceleration; @@ -113,54 +116,54 @@ void JointAccelConstraint::FillJacobianBlock(std::string var_set, Jacobian& jac_ if (var_set != position_vars_.front()->getParent()->getParent()->GetName()) return; - std::vector > triplet_list; - triplet_list.reserve(static_cast(n_dof_ * 3 * n_vars_)); + // Each timestep contributes 3 nonzeros per DOF + std::vector> triplets; + triplets.reserve(static_cast(3 * n_dof_ * n_vars_)); - // jac block will be (n_vars-1)*n_dof x n_dof - - Eigen::Index prev_idx2 = -1; - Eigen::Index prev_idx1 = -1; - Eigen::Index idx = -1; - Eigen::Index post_idx1 = -1; - Eigen::Index post_idx2 = -1; - for (std::size_t i = 0; i < n_vars_; i++) + const std::size_t n = position_vars_.size(); + for (std::size_t i = 0; i < n; ++i) { - idx = position_vars_[i]->getIndex(); - - if (i > 0) - prev_idx1 = position_vars_[i - 1]->getIndex(); - - if (i > 1) - prev_idx2 = position_vars_[i - 2]->getIndex(); - - if (i < n_vars_ - 1) - post_idx1 = position_vars_[i + 1]->getIndex(); - - if (i < n_vars_ - 2) - post_idx2 = position_vars_[i + 2]->getIndex(); + const Eigen::Index row_offset = static_cast(i) * n_dof_; - for (Eigen::Index j = 0; j < n_dof_; j++) + if (i < n - 2) { - // The last two variable are special and only effect the last two constraints. - // Everything else effects 3 - - triplet_list.emplace_back(idx + j, idx + j, 1.0 * coeffs_[j]); - - if (i > 0 && i < n_vars_ - 1) - triplet_list.emplace_back(prev_idx1 + j, idx + j, -2.0 * coeffs_[j]); - - if (i > 1) - triplet_list.emplace_back(prev_idx2 + j, idx + j, 1.0 * coeffs_[j]); - - if (i >= (n_vars_ - 3) && i <= (n_vars_ - 2)) - triplet_list.emplace_back(post_idx1 + j, idx + j, -2.0 * coeffs_[j]); - - if (i >= (n_vars_ - 4) && i <= (n_vars_ - 3)) - triplet_list.emplace_back(post_idx2 + j, idx + j, 1.0 * coeffs_[j]); + // Forward diff at timestep i: depends on q_i, q_{i+1}, q_{i+2} + const Eigen::Index col_i = position_vars_[i]->getIndex(); + const Eigen::Index col_ip1 = position_vars_[i + 1]->getIndex(); + const Eigen::Index col_ip2 = position_vars_[i + 2]->getIndex(); + + for (Eigen::Index k = 0; k < n_dof_; ++k) + { + const double c = coeffs_[k]; + const Eigen::Index row = row_offset + k; + + // a_i = c * (q_{i+2} - 2*q_{i+1} + q_i) + triplets.emplace_back(row, col_i + k, c); // ∂a/∂q_i + triplets.emplace_back(row, col_ip1 + k, -2.0 * c); // ∂a/∂q_{i+1} + triplets.emplace_back(row, col_ip2 + k, c); // ∂a/∂q_{i+2} + } + } + else + { + // Backward diff at timesteps n-2 and n-1: depends on q_{i-2}, q_{i-1}, q_i + const Eigen::Index col_im2 = position_vars_[i - 2]->getIndex(); + const Eigen::Index col_im1 = position_vars_[i - 1]->getIndex(); + const Eigen::Index col_i = position_vars_[i]->getIndex(); + + for (Eigen::Index k = 0; k < n_dof_; ++k) + { + const double c = coeffs_[k]; + const Eigen::Index row = row_offset + k; + + // a_i = c * (q_{i-2} - 2*q_{i-1} + q_i) + triplets.emplace_back(row, col_im2 + k, c); // ∂a/∂q_{i-2} + triplets.emplace_back(row, col_im1 + k, -2.0 * c); // ∂a/∂q_{i-1} + triplets.emplace_back(row, col_i + k, c); // ∂a/∂q_i + } } } - jac_block.setFromTriplets(triplet_list.begin(), triplet_list.end()); // NOLINT + jac_block.setFromTriplets(triplets.begin(), triplets.end()); // NOLINT } } // namespace trajopt_ifopt diff --git a/trajopt_ifopt/src/constraints/joint_jerk_constraint.cpp b/trajopt_ifopt/src/constraints/joint_jerk_constraint.cpp index 74a0209c..c24cfd82 100644 --- a/trajopt_ifopt/src/constraints/joint_jerk_constraint.cpp +++ b/trajopt_ifopt/src/constraints/joint_jerk_constraint.cpp @@ -78,30 +78,38 @@ JointJerkConstraint::JointJerkConstraint(const Eigen::VectorXd& targets, Eigen::VectorXd JointJerkConstraint::GetValues() const { - Eigen::VectorXd acceleration(static_cast(n_dof_) * position_vars_.size()); - // Forward Diff - for (std::size_t ind = 0; ind < position_vars_.size() - 3; ind++) + const std::size_t n = position_vars_.size(); + Eigen::VectorXd jerk(n_dof_ * static_cast(n)); + + // Forward diff for timesteps [0, n-4] + // j_i = coeffs_ .* (-q_i + 3*q_{i+1} - 3*q_{i+2} + q_{i+3}) + for (std::size_t i = 0; i < n - 3; ++i) { - auto vals1 = position_vars_[ind]->value(); - auto vals2 = position_vars_[ind + 1]->value(); - auto vals3 = position_vars_[ind + 2]->value(); - auto vals4 = position_vars_[ind + 3]->value(); - const Eigen::VectorXd single_step = (3.0 * vals2) - (3.0 * vals3) - vals1 + vals4; - acceleration.block(n_dof_ * static_cast(ind), 0, n_dof_, 1) = coeffs_.cwiseProduct(single_step); + const Eigen::VectorXd& q0 = position_vars_[i]->value(); + const Eigen::VectorXd& q1 = position_vars_[i + 1]->value(); + const Eigen::VectorXd& q2 = position_vars_[i + 2]->value(); + const Eigen::VectorXd& q3 = position_vars_[i + 3]->value(); + + const Eigen::VectorXd single_step = -q0 + 3.0 * q1 - 3.0 * q2 + q3; + + jerk.segment(static_cast(i) * n_dof_, n_dof_) = coeffs_.cwiseProduct(single_step); } - // Backward Diff - for (std::size_t ind = position_vars_.size() - 3; ind < position_vars_.size(); ind++) + // Backward diff for timesteps [n-3, n-1] + // j_i = coeffs_ .* ( q_i - 3*q_{i-1} + 3*q_{i-2} - q_{i-3} ) + for (std::size_t i = n - 3; i < n; ++i) { - auto vals1 = position_vars_[ind]->value(); - auto vals2 = position_vars_[ind - 1]->value(); - auto vals3 = position_vars_[ind - 2]->value(); - auto vals4 = position_vars_[ind - 3]->value(); - const Eigen::VectorXd single_step = vals1 - (3.0 * vals2) + (3.0 * vals3) - vals4; - acceleration.block(n_dof_ * static_cast(ind), 0, n_dof_, 1) = coeffs_.cwiseProduct(single_step); + const Eigen::VectorXd& q0 = position_vars_[i]->value(); // q_i + const Eigen::VectorXd& q1 = position_vars_[i - 1]->value(); // q_{i-1} + const Eigen::VectorXd& q2 = position_vars_[i - 2]->value(); // q_{i-2} + const Eigen::VectorXd& q3 = position_vars_[i - 3]->value(); // q_{i-3} + + const Eigen::VectorXd single_step = q0 - 3.0 * q1 + 3.0 * q2 - q3; + + jerk.segment(static_cast(i) * n_dof_, n_dof_) = coeffs_.cwiseProduct(single_step); } - return acceleration; + return jerk; } // Set the limits on the constraint values (in this case just the targets) @@ -114,71 +122,57 @@ void JointJerkConstraint::FillJacobianBlock(std::string var_set, Jacobian& jac_b if (var_set != position_vars_.front()->getParent()->getParent()->GetName()) return; - // Reserve enough room in the sparse matrix - std::vector> triplet_list; - triplet_list.reserve(static_cast(n_dof_ * 4)); + std::vector> triplets; + // Each timestep depends on 4 positions → 4 nonzeros per DOF + triplets.reserve(static_cast(4 * n_dof_ * n_vars_)); - // jac block will be (n_vars-1)*n_dof x n_dof - Eigen::Index prev_idx3 = -1; - Eigen::Index prev_idx2 = -1; - Eigen::Index prev_idx1 = -1; - Eigen::Index idx = -1; - Eigen::Index post_idx1 = -1; - Eigen::Index post_idx2 = -1; - Eigen::Index post_idx3 = -1; + const std::size_t n = position_vars_.size(); - for (std::size_t i = 0; i < n_vars_; i++) + for (std::size_t i = 0; i < n; ++i) { - idx = position_vars_[i]->getIndex(); - - if (i > 0) - prev_idx1 = position_vars_[i - 1]->getIndex(); - - if (i > 1) - prev_idx2 = position_vars_[i - 2]->getIndex(); - - if (i > 2) - prev_idx3 = position_vars_[i - 3]->getIndex(); - - if (i < n_vars_ - 1) - post_idx1 = position_vars_[i + 1]->getIndex(); - - if (i < n_vars_ - 2) - post_idx2 = position_vars_[i + 2]->getIndex(); - - if (i < n_vars_ - 3) - post_idx3 = position_vars_[i + 3]->getIndex(); + const Eigen::Index row_offset = static_cast(i) * n_dof_; - for (int j = 0; j < n_dof_; j++) + if (i < n - 3) { - // The last two variable are special and only effect the last two constraints. - // Everything else effects 3 - if (i < n_vars_ - 3) - triplet_list.emplace_back(idx + j, idx + j, -1.0 * coeffs_[j]); - - if (i > 0 && i < n_vars_ - 2) - triplet_list.emplace_back(prev_idx1 + j, idx + j, 3.0 * coeffs_[j]); - - if (i > 1 && i < n_vars_ - 1) - triplet_list.emplace_back(prev_idx2 + j, idx + j, -3.0 * coeffs_[j]); - - if (i > 2) - triplet_list.emplace_back(prev_idx3 + j, idx + j, 1.0 * coeffs_[j]); - - if (i >= (n_vars_ - 3) && i <= (n_vars_ - 1)) - triplet_list.emplace_back(idx + j, idx + j, 1.0 * coeffs_[j]); - - if (i >= (n_vars_ - 4) && i <= (n_vars_ - 2)) - triplet_list.emplace_back(post_idx1 + j, idx + j, -3.0 * coeffs_[j]); - - if (i >= (n_vars_ - 5) && i <= (n_vars_ - 3)) - triplet_list.emplace_back(post_idx2 + j, idx + j, 3.0 * coeffs_[j]); - - if (i >= (n_vars_ - 6) && i <= (n_vars_ - 4)) - triplet_list.emplace_back(post_idx3 + j, idx + j, -1.0 * coeffs_[j]); + // Forward diff: j_i = c * (-q_i + 3*q_{i+1} - 3*q_{i+2} + q_{i+3}) + const Eigen::Index col_i = position_vars_[i]->getIndex(); + const Eigen::Index col_ip1 = position_vars_[i + 1]->getIndex(); + const Eigen::Index col_ip2 = position_vars_[i + 2]->getIndex(); + const Eigen::Index col_ip3 = position_vars_[i + 3]->getIndex(); + + for (Eigen::Index k = 0; k < n_dof_; ++k) + { + const double c = coeffs_[k]; + const Eigen::Index row = row_offset + k; + + triplets.emplace_back(row, col_i + k, -c); // ∂j_i/∂q_i + triplets.emplace_back(row, col_ip1 + k, 3.0 * c); // ∂j_i/∂q_{i+1} + triplets.emplace_back(row, col_ip2 + k, -3.0 * c); // ∂j_i/∂q_{i+2} + triplets.emplace_back(row, col_ip3 + k, c); // ∂j_i/∂q_{i+3} + } + } + else + { + // Backward diff: j_i = c * (q_i - 3*q_{i-1} + 3*q_{i-2} - q_{i-3}) + const Eigen::Index col_i = position_vars_[i]->getIndex(); + const Eigen::Index col_im1 = position_vars_[i - 1]->getIndex(); + const Eigen::Index col_im2 = position_vars_[i - 2]->getIndex(); + const Eigen::Index col_im3 = position_vars_[i - 3]->getIndex(); + + for (Eigen::Index k = 0; k < n_dof_; ++k) + { + const double c = coeffs_[k]; + const Eigen::Index row = row_offset + k; + + triplets.emplace_back(row, col_i + k, c); // ∂j_i/∂q_i + triplets.emplace_back(row, col_im1 + k, -3.0 * c); // ∂j_i/∂q_{i-1} + triplets.emplace_back(row, col_im2 + k, 3.0 * c); // ∂j_i/∂q_{i-2} + triplets.emplace_back(row, col_im3 + k, -c); // ∂j_i/∂q_{i-3} + } } } - jac_block.setFromTriplets(triplet_list.begin(), triplet_list.end()); // NOLINT + + jac_block.setFromTriplets(triplets.begin(), triplets.end()); // NOLINT } } // namespace trajopt_ifopt diff --git a/trajopt_ifopt/src/constraints/joint_velocity_constraint.cpp b/trajopt_ifopt/src/constraints/joint_velocity_constraint.cpp index bbe5ea32..72c92b84 100644 --- a/trajopt_ifopt/src/constraints/joint_velocity_constraint.cpp +++ b/trajopt_ifopt/src/constraints/joint_velocity_constraint.cpp @@ -85,17 +85,20 @@ Eigen::VectorXd JointVelConstraint::GetValues() const // vel(var[1, 1]) - represents the joint velocity of DOF index 1 at timestep 1 // // Velocity V = vel(var[0, 0]), vel(var[0, 1]), vel(var[0, 2]), vel(var[1, 0]), vel(var[1, 1]), vel(var[1, 2]), etc - Eigen::VectorXd velocity(static_cast(n_dof_) * (static_cast(n_vars_) - 1)); - // Eigen::VectorXd val = this->GetVariables()->GetComponent(var_set_)->GetValues(); + // Number of velocity segments (one less than number of position vars) + const Eigen::Index n_segments = n_vars_ - 1; - // Forward differentiation for the first point - for (std::size_t ind = 0; ind < position_vars_.size() - 1; ind++) + Eigen::VectorXd velocity(n_dof_ * n_segments); + + for (Eigen::Index seg = 0; seg < n_segments; ++seg) { - auto vals1 = position_vars_[ind]->value(); - auto vals2 = position_vars_[ind + 1]->value(); - const Eigen::VectorXd single_step = (vals2 - vals1); - velocity.block(n_dof_ * static_cast(ind), 0, n_dof_, 1) = coeffs_.cwiseProduct(single_step); + // q_i and q_{i+1} + const Eigen::VectorXd& q0 = position_vars_[static_cast(seg)]->value(); + const Eigen::VectorXd& q1 = position_vars_[static_cast(seg + 1)]->value(); + + // v_i = coeffs_ .* (q_{i+1} - q_i) + velocity.segment(seg * n_dof_, n_dof_) = coeffs_.cwiseProduct(q1 - q0); } return velocity; @@ -111,33 +114,32 @@ void JointVelConstraint::FillJacobianBlock(std::string var_set, Jacobian& jac_bl if (var_set != position_vars_.front()->getParent()->getParent()->GetName()) return; - // Reserve enough room in the sparse matrix - std::vector> triplet_list; - triplet_list.reserve(static_cast(n_vars_ * n_dof_)); + const Eigen::Index n_segments = n_vars_ - 1; - // The first and last variable are special and only effect the first and last constraint. Everything else effects 2 - Eigen::Index prev_idx = -1; - Eigen::Index idx = position_vars_[0]->getIndex(); - for (int j = 0; j < n_dof_; j++) - triplet_list.emplace_back(idx + j, idx + j, -1.0 * coeffs_[j]); + // Each segment contributes 2 * n_dof_ nonzeros (− and +) + std::vector> triplets; + triplets.reserve(static_cast(2 * n_segments * n_dof_)); - for (std::size_t i = 1; i < (n_vars_ - 1); i++) + for (Eigen::Index seg = 0; seg < n_segments; ++seg) { - prev_idx = position_vars_[i - 1]->getIndex(); - idx = position_vars_[i]->getIndex(); - for (int j = 0; j < n_dof_; j++) + const Eigen::Index row_offset = seg * n_dof_; + + // Column indices in this var_set for q_seg and q_{seg+1} + const Eigen::Index col0 = position_vars_[static_cast(seg)]->getIndex(); + const Eigen::Index col1 = position_vars_[static_cast(seg + 1)]->getIndex(); + + for (Eigen::Index k = 0; k < n_dof_; ++k) { - triplet_list.emplace_back(idx + j, idx + j, -1.0 * coeffs_[j]); - triplet_list.emplace_back(prev_idx + j, idx + j, 1.0 * coeffs_[j]); + const double c = coeffs_[k]; + const Eigen::Index row = row_offset + k; + + // v(seg,k) = c * (q1 - q0) + triplets.emplace_back(row, col0 + k, -c); // ∂v/∂q_seg + triplets.emplace_back(row, col1 + k, c); // ∂v/∂q_{seg+1} } } - prev_idx = position_vars_[static_cast(n_vars_ - 2)]->getIndex(); - idx = position_vars_[static_cast(n_vars_ - 1)]->getIndex(); - for (int j = 0; j < n_dof_; j++) - triplet_list.emplace_back(prev_idx + j, idx + j, 1.0 * coeffs_[j]); - - jac_block.setFromTriplets(triplet_list.begin(), triplet_list.end()); // NOLINT + jac_block.setFromTriplets(triplets.begin(), triplets.end()); // NOLINT } } // namespace trajopt_ifopt diff --git a/trajopt_ifopt/src/costs/absolute_cost.cpp b/trajopt_ifopt/src/costs/absolute_cost.cpp index 6a732296..0ae67afb 100644 --- a/trajopt_ifopt/src/costs/absolute_cost.cpp +++ b/trajopt_ifopt/src/costs/absolute_cost.cpp @@ -47,38 +47,57 @@ double AbsoluteCost::GetCost() const { // This takes the absolute value of the errors const Eigen::VectorXd error = calcBoundsViolations(constraint_->GetValues(), constraint_->GetBounds()); - const double cost = weights_.transpose() * error; - return cost; + return weights_.dot(error); } void AbsoluteCost::FillJacobianBlock(std::string var_set, Jacobian& jac_block) const { // Get a Jacobian block the size necessary for the constraint - Jacobian cnt_jac_block; int var_size = 0; for (const auto& vars : GetVariables()->GetComponents()) { if (vars->GetName() == var_set) // NOLINT + { var_size = vars->GetRows(); + break; + } } if (var_size == 0) // NOLINT - throw std::runtime_error("Unable to find var_set."); - - cnt_jac_block.resize(constraint_->GetRows(), var_size); // NOLINT + throw std::runtime_error("AbsoluteCost: Unable to find var_set '" + var_set + "'."); - // Get the Jacobian Block from the constraint + // Get the Jacobian block from the underlying constraint + Jacobian cnt_jac_block; + cnt_jac_block.resize(n_constraints_, var_size); // NOLINT constraint_->FillJacobianBlock(var_set, cnt_jac_block); - // Apply the chain rule. See doxygen for this class - // There are two w's that cancel out resulting in w_error / error.abs(). - // This breaks down if the weights are not positive but the constructor takes the absolute - // value of the weights to avoid this issue. + // Compute signed coefficients: coeff_i = weights_[i] * sign(error_i) const Eigen::ArrayXd error = calcBoundsErrors(constraint_->GetValues(), constraint_->GetBounds()); - const Eigen::ArrayXd w_error = error * weights_.array(); - const Eigen::VectorXd coeff = w_error / error.abs(); - jac_block = coeff.sparseView().eval() * cnt_jac_block; // NOLINT + Eigen::VectorXd coeff(n_constraints_); + for (Eigen::Index i = 0; i < error.size(); ++i) + { + const double e = error[i]; + if (std::abs(e) < 1e-12) + coeff[i] = 0.0; // subgradient at 0 + else if (e > 0.0) + coeff[i] = weights_[i]; + else + coeff[i] = -weights_[i]; + } + + // Scale each row of the constraint Jacobian by coeff[row] + for (int outer = 0; outer < cnt_jac_block.outerSize(); ++outer) + { + for (Jacobian::InnerIterator it(cnt_jac_block, outer); it; ++it) + { + // it.row() is the row index in [0, n_constraints_) + it.valueRef() *= coeff[it.row()]; + } + } + + // Output the scaled block + jac_block = cnt_jac_block; } } // namespace trajopt_ifopt diff --git a/trajopt_ifopt/src/costs/squared_cost.cpp b/trajopt_ifopt/src/costs/squared_cost.cpp index 8f9b98eb..5972ffd0 100644 --- a/trajopt_ifopt/src/costs/squared_cost.cpp +++ b/trajopt_ifopt/src/costs/squared_cost.cpp @@ -46,31 +46,42 @@ SquaredCost::SquaredCost(ifopt::ConstraintSet::Ptr constraint, const Eigen::Ref< double SquaredCost::GetCost() const { Eigen::VectorXd error = calcBoundsErrors(constraint_->GetValues(), constraint_->GetBounds()); - double const cost = error.transpose() * weights_.asDiagonal() * error; - return cost; + // cost = sum_i w_i * e_i^2 + return (weights_.array() * error.array().square()).sum(); } void SquaredCost::FillJacobianBlock(std::string var_set, Jacobian& jac_block) const { // Get a Jacobian block the size necessary for the constraint - Jacobian cnt_jac_block; int var_size = 0; for (const auto& vars : GetVariables()->GetComponents()) { if (vars->GetName() == var_set) // NOLINT + { var_size = vars->GetRows(); + break; + } } if (var_size == 0) // NOLINT - throw std::runtime_error("Unable to find var_set."); - - cnt_jac_block.resize(constraint_->GetRows(), var_size); // NOLINT + throw std::runtime_error("SquaredCost: Unable to find var_set '" + var_set + "'."); - // Get the Jacobian Block from the constraint + // Get the Jacobian block from the constraint + Jacobian cnt_jac_block; + cnt_jac_block.resize(n_constraints_, var_size); // NOLINT constraint_->FillJacobianBlock(var_set, cnt_jac_block); - // Apply the chain rule. See doxygen for this class - Eigen::VectorXd error = calcBoundsErrors(constraint_->GetValues(), constraint_->GetBounds()); - jac_block = 2 * error.transpose().sparseView() * weights_.asDiagonal() * cnt_jac_block; // NOLINT + // error = bounds error vector (length = n_constraints_) + const Eigen::VectorXd error = calcBoundsErrors(constraint_->GetValues(), constraint_->GetBounds()); + + // coeff_i = 2 * w_i * e_i + const Eigen::VectorXd coeff = (2.0 * (weights_.array() * error.array())).matrix(); + + // Gradient row: 1 x var_size + // (dense row vector = coeff^T * J) + const Eigen::RowVectorXd grad = coeff.transpose() * cnt_jac_block; + + // Convert to sparse and return + jac_block = grad.sparseView(); } } // namespace trajopt_ifopt diff --git a/trajopt_ifopt/src/utils/ifopt_utils.cpp b/trajopt_ifopt/src/utils/ifopt_utils.cpp index 2cfabd58..da43be13 100644 --- a/trajopt_ifopt/src/utils/ifopt_utils.cpp +++ b/trajopt_ifopt/src/utils/ifopt_utils.cpp @@ -47,7 +47,7 @@ bool isBoundsGreaterFinite(const ifopt::Bounds& bounds) bool isBoundsSmallerFinite(const ifopt::Bounds& bounds) { - return (std::isfinite(bounds.upper_) && !isFinite(bounds.lower_)); + return (isFinite(bounds.upper_) && !isFinite(bounds.lower_)); } bool isBoundsInEquality(const ifopt::Bounds& bounds) @@ -70,10 +70,15 @@ std::vector toBounds(const Eigen::Ref& low const Eigen::Ref& upper_limits) { assert(lower_limits.size() == upper_limits.size()); // NOLINT - Eigen::MatrixX2d limits(lower_limits.rows(), 2); - limits.col(0) = lower_limits; - limits.col(1) = upper_limits; - return toBounds(limits); + + const auto n = lower_limits.size(); + std::vector bounds; + bounds.reserve(static_cast(n)); + + for (Eigen::Index i = 0; i < n; ++i) + bounds.emplace_back(lower_limits[i], upper_limits[i]); + + return bounds; } std::vector interpolate(const Eigen::Ref& start, @@ -81,55 +86,64 @@ std::vector interpolate(const Eigen::Ref Eigen::Index steps) { assert(start.size() == end.size()); // NOLINT + assert(steps >= 2); // NOLINT: avoid division by zero + const Eigen::VectorXd delta = (end - start) / static_cast(steps - 1); Eigen::VectorXd running = start; + std::vector results; - for (Eigen::Index i = 0; i < steps; i++) + results.reserve(static_cast(steps)); + + for (Eigen::Index i = 0; i < steps; ++i) { results.push_back(running); running += delta; } + return results; } Eigen::VectorXd getClosestValidPoint(const Eigen::Ref& input, const std::vector& bounds) { - // Convert Bounds to VectorXds - Eigen::VectorXd bound_lower(static_cast(bounds.size())); - Eigen::VectorXd bound_upper(static_cast(bounds.size())); - for (std::size_t i = 0; i < bounds.size(); i++) + assert(input.size() == static_cast(bounds.size())); // NOLINT + + Eigen::VectorXd lower(input.size()); + Eigen::VectorXd upper(input.size()); + + for (Eigen::Index i = 0; i < input.size(); ++i) { - bound_lower[static_cast(i)] = bounds[i].lower_; - bound_upper[static_cast(i)] = bounds[i].upper_; + lower[i] = bounds[static_cast(i)].lower_; + upper[i] = bounds[static_cast(i)].upper_; } - // If input is outside a bound, force it to the boundary - return input.cwiseMax(bound_lower).cwiseMin(bound_upper); + return input.cwiseMax(lower).cwiseMin(upper); } Eigen::VectorXd calcBoundsErrors(const Eigen::Ref& input, const std::vector& bounds) { - assert(input.rows() == static_cast(bounds.size())); // NOLINT + assert(input.size() == static_cast(bounds.size())); // NOLINT - // Convert constraint bounds to VectorXd - Eigen::ArrayXd bound_lower(input.rows()); - Eigen::ArrayXd bound_upper(input.rows()); - for (std::size_t i = 0; i < bounds.size(); i++) + Eigen::ArrayXd lower(input.size()); + Eigen::ArrayXd upper(input.size()); + for (Eigen::Index i = 0; i < input.size(); ++i) { - bound_lower[static_cast(i)] = bounds[i].lower_; - bound_upper[static_cast(i)] = bounds[i].upper_; + const auto& b = bounds[static_cast(i)]; + lower[i] = b.lower_; + upper[i] = b.upper_; } - // Values will be negative if they violate the constrain - const Eigen::ArrayXd zero = Eigen::ArrayXd::Zero(input.rows()); - const Eigen::ArrayXd dist_from_lower = (input.array() - bound_lower).min(zero); - const Eigen::ArrayXd dist_from_upper = (input.array() - bound_upper).max(zero); + const Eigen::ArrayXd diff_lower = input.array() - lower; + const Eigen::ArrayXd diff_upper = input.array() - upper; + + const Eigen::ArrayXd dist_from_lower = diff_lower.min(0.0); + const Eigen::ArrayXd dist_from_upper = diff_upper.max(0.0); + const Eigen::ArrayXd worst_error = (dist_from_upper.abs() > dist_from_lower.abs()).select(dist_from_upper, dist_from_lower); - return worst_error; + return worst_error.matrix(); } Eigen::VectorXd calcBoundsViolations(const Eigen::Ref& input, @@ -144,25 +158,26 @@ ifopt::VectorXd calcNumericalCostGradient(const double* x, ifopt::Problem& nlp, const int n = nlp.GetNumberOfOptimizationVariables(); ifopt::Problem::Jacobian jac(1, n); + if (nlp.HasCostTerms()) { - const double step_size = epsilon; - - // calculate forward difference by disturbing each optimization variable const double g = nlp.EvaluateCostFunction(x); std::vector x_new(x, x + n); + for (int i = 0; i < n; ++i) { - x_new[static_cast(i)] += step_size; // disturb + x_new[static_cast(i)] += epsilon; // disturb const double g_new = nlp.EvaluateCostFunction(x_new.data()); - jac.coeffRef(0, i) = (g_new - g) / step_size; - x_new[static_cast(i)] = x[i]; // reset for next iteration + jac.coeffRef(0, i) = (g_new - g) / epsilon; + x_new[static_cast(i)] = x[i]; // reset } } + else + { + jac.setZero(); + } - // Set problem values back to the original values. nlp.SetVariables(cache_vars.data()); - return jac.row(0).transpose(); } @@ -175,29 +190,26 @@ ifopt::Jacobian calcNumericalConstraintGradient(const double* x, ifopt::Problem& ifopt::Problem::Jacobian jac(m, n); jac.reserve(static_cast(m) * static_cast(n)); - if (nlp.GetNumberOfConstraints() > 0) + if (nlp.GetNumberOfConstraints() > 0 && n > 0) { - const double step_size = epsilon; - - // calculate forward difference by disturbing each optimization variable const ifopt::Problem::VectorXd g = nlp.EvaluateConstraints(x); std::vector x_new(x, x + n); + ifopt::Problem::VectorXd delta_g(m); + for (int i = 0; i < n; ++i) { - x_new[static_cast(i)] += step_size; // disturb + x_new[static_cast(i)] += epsilon; const ifopt::Problem::VectorXd g_new = nlp.EvaluateConstraints(x_new.data()); - ifopt::Problem::VectorXd delta_g = (g_new - g) / step_size; + delta_g = (g_new - g) / epsilon; for (int j = 0; j < m; ++j) - jac.coeffRef(j, i) = delta_g(j); + jac.coeffRef(j, i) = delta_g[j]; - x_new[static_cast(i)] = x[i]; // reset for next iteration + x_new[static_cast(i)] = x[i]; } } - // Set problem values back to the original values. nlp.SetVariables(cache_vars.data()); - return jac; } @@ -205,33 +217,41 @@ ifopt::Jacobian calcNumericalConstraintGradient(ifopt::Component& variables, ifopt::ConstraintSet& constraint_set, double epsilon) { - Eigen::VectorXd x = variables.GetValues(); - const int n = variables.GetRows(); const int m = constraint_set.GetRows(); + ifopt::Problem::Jacobian jac(m, n); jac.reserve(static_cast(m) * static_cast(n)); - if (!constraint_set.GetBounds().empty()) + // Nothing to do if there are no constraints, no variables, or no bounds + if (m == 0 || n == 0 || constraint_set.GetBounds().empty()) + return jac; + + // Cache current variable values + Eigen::VectorXd x = variables.GetValues(); + + // Base constraint values at x + const ifopt::Problem::VectorXd g = constraint_set.GetValues(); + + Eigen::VectorXd x_new = x; + ifopt::Problem::VectorXd delta_g(m); + + // Forward-difference approximation for each variable + for (Eigen::Index i = 0; i < n; ++i) { - // calculate forward difference by disturbing each optimization variable - const ifopt::Problem::VectorXd g = constraint_set.GetValues(); - Eigen::VectorXd x_new = x; - for (Eigen::Index i = 0; i < n; ++i) - { - x_new(i) = x(i) + epsilon; // disturb - variables.SetVariables(x_new); - const ifopt::Problem::VectorXd g_new = constraint_set.GetValues(); - ifopt::Problem::VectorXd delta_g = (g_new - g) / epsilon; + x_new(i) = x(i) + epsilon; // disturb variable i + variables.SetVariables(x_new); - for (int j = 0; j < m; ++j) - jac.coeffRef(j, i) = delta_g(j); + const ifopt::Problem::VectorXd g_new = constraint_set.GetValues(); + delta_g = (g_new - g) / epsilon; - x_new(i) = x(i); // reset for next iteration - } + for (int j = 0; j < m; ++j) + jac.coeffRef(j, static_cast(i)) = delta_g(j); + + x_new(i) = x(i); // reset for next iteration } - // Set problem values back to the original values. + // Restore original variables variables.SetVariables(x); return jac; diff --git a/trajopt_ifopt/test/simple_collision_unit.cpp b/trajopt_ifopt/test/simple_collision_unit.cpp index 50dc8210..0c13441d 100644 --- a/trajopt_ifopt/test/simple_collision_unit.cpp +++ b/trajopt_ifopt/test/simple_collision_unit.cpp @@ -125,11 +125,6 @@ TEST_F(SimpleCollisionTest, spheres) // NOLINT nlp.PrintCurrent(); std::cout << "Jacobian: \n" << nlp.GetJacobianOfConstraints() << '\n'; - auto error_calculator = [&](const Eigen::Ref& x) { return cnt->CalcValues(x); }; - trajopt_ifopt::SparseMatrix const num_jac_block = - trajopt_ifopt::calcForwardNumJac(error_calculator, positions[0], 1e-4); - std::cout << "Numerical Jacobian: \n" << num_jac_block << '\n'; - // 5) choose solver and options ifopt::IpoptSolver ipopt; ipopt.SetOption("derivative_test", "first-order"); diff --git a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/expressions.h b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/expressions.h index f76c94b7..3d616403 100644 --- a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/expressions.h +++ b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/expressions.h @@ -7,107 +7,333 @@ namespace trajopt_sqp { +/** + * @brief Base class for a collection of scalar expressions evaluated at a common decision vector. + * + * The class represents a vector-valued function + * + * \f[ + * f(x) = \begin{bmatrix} f_0(x) \\ f_1(x) \\ \vdots \\ f_{n-1}(x) \end{bmatrix}, + * \f] + * + * where each component may be affine, quadratic, or more general, depending on the concrete subclass. + */ struct Exprs { virtual ~Exprs() = default; - virtual Eigen::VectorXd values(const Eigen::Ref& x) const = 0; + + /** + * @brief Evaluate all expressions at the given decision vector. + * @param x Decision vector at which to evaluate the expressions. + * @return Vector of expression values \f$f(x)\f. + */ + virtual Eigen::VectorXd values(const Eigen::Ref& x) const = 0; }; +/** + * @brief Vector of affine expressions of the form \f$f(x) = c + A x\f. + * + * Each row of @ref linear_coeffs and the corresponding entry in @ref constants define + * a single scalar affine expression: + * + * \f[ + * f_i(x) = c_i + a_i^\top x, + * \f] + * + * where \f$c_i\f$ is @ref constants(i) and \f$a_i^\top\f$ is @ref linear_coeffs.row(i). + */ struct AffExprs : Exprs { + /** + * @brief Constant term \f$c\f$ for each affine expression. + * + * Size: number of expressions. + */ Eigen::VectorXd constants; + + /** + * @brief Linear coefficient matrix \f$A\f$ for the affine expressions. + * + * Each row corresponds to a single expression, and each column corresponds to a + * decision variable. Size: (num_exprs × num_vars). + */ SparseMatrix linear_coeffs; - Eigen::VectorXd values(const Eigen::Ref& x) const override final; + + /** + * @brief Evaluate the affine expressions at the given decision vector. + * + * Computes + * \f[ + * f(x) = c + A x. + * \f] + * + * @param x Decision vector. + * @return Vector of affine expression values \f$f(x)\f. + */ + Eigen::VectorXd values(const Eigen::Ref& x) const override final; }; +/** + * @brief Vector of quadratic expressions and their aggregated objective contribution. + * + * Each scalar expression has the form + * + * \f[ + * f_i(x) = c_i + a_i^\top x + x^\top Q_i x, + * \f] + * + * where: + * - @ref constants(i) stores \f$c_i\f + * - @ref linear_coeffs.row(i) stores \f$a_i^\top\f + * - @ref quadratic_coeffs[i] stores the sparse matrix \f$Q_i\f. + * + * In addition, @ref objective_linear_coeffs and @ref objective_quadratic_coeffs may be + * used to accumulate the sum of all expressions into a single quadratic objective. + */ struct QuadExprs : Exprs { QuadExprs() = default; + + /** + * @brief Construct a quadratic expression container with given dimensions. + * + * Allocates and sizes the primary storage for a problem with @p num_cost scalar + * expressions and @p num_vars decision variables: + * + * - @ref constants has size @p num_cost + * - @ref linear_coeffs has size @p num_cost × @p num_vars + * - @ref objective_linear_coeffs has size @p num_vars + * - @ref objective_quadratic_coeffs has size @p num_vars × @p num_vars + * - @ref quadratic_coeffs is reserved to hold @p num_cost sparse matrices. + * + * @param num_cost Number of scalar expressions. + * @param num_vars Number of decision variables. + */ QuadExprs(Eigen::Index num_cost, Eigen::Index num_vars); /** - * @brief The constant for the equations - * @details Each row represents a different equations constant + * @brief Constant term \f$c_i\f for each quadratic expression. + * @details Entry @c constants(i) is the constant for expression \f$f_i(x)\f. */ Eigen::VectorXd constants; /** - * @brief The linear coefficients for the equations - * @details Each row represents a different equation linear coefficients + * @brief Linear coefficient matrix \f$a_i^\top\f for each expression. + * @details Row @c linear_coeffs.row(i) contains the linear coefficients + * associated with expression \f$f_i(x)\f. + * + * Dimensions: (num_expressions × num_vars). */ SparseMatrix linear_coeffs; /** - * @brief The quadratic coefficients for each equation (aka. row) - * @details Each entry represents a different equation quadratic coefficients + * @brief Quadratic coefficient matrices \f$Q_i\f for each expression. + * @details Entry @c quadratic_coeffs[i] is the sparse matrix \f$Q_i\f for + * expression \f$f_i(x)\f. If a given expression is purely affine, + * the corresponding matrix may be empty (zero non-zeros). */ std::vector quadratic_coeffs; /** - * @brief The objective linear coefficients - * @details This is the sum of the equation gradient coefficients (linear_coeffs) + * @brief Aggregated objective linear coefficients. + * @details This typically holds the sum of the per-expression linear + * contributions, e.g. when forming a single objective + * + * \f[ + * F(x) = \sum_i f_i(x) = C + g^\top x + x^\top H x, + * \f] + * + * where @ref objective_linear_coeffs stores \f$g\f. */ Eigen::VectorXd objective_linear_coeffs; /** - * @brief The objective quadratic coefficients - * @details This is the sum of the equation quadratic coefficients (quadratic_coeffs) + * @brief Aggregated objective quadratic coefficients. + * @details This typically holds the sum of the per-expression quadratic + * contributions \f$Q_i\f when forming a single objective, i.e. + * @ref objective_quadratic_coeffs stores \f$H\f in + * + * \f[ + * F(x) = \sum_i f_i(x) = C + g^\top x + x^\top H x. + * \f] + * + * Dimensions: (num_vars × num_vars). */ SparseMatrix objective_quadratic_coeffs; - Eigen::VectorXd values(const Eigen::Ref& x) const override final; + /** + * @brief Evaluate all quadratic expressions at the given decision vector. + * + * Computes, for each \f$i\f, + * + * \f[ + * f_i(x) = c_i + a_i^\top x + x^\top Q_i x. + * \f] + * + * @param x Decision vector. + * @return Vector of expression values \f$f(x)\f. + */ + Eigen::VectorXd values(const Eigen::Ref& x) const override final; }; /** - * @brief Given the function error and jacobian calculate the linear approximation - * @details This was derived using the reference below, but the CostFromFunc::convex in the original trajopt is - * different References: - * https://www.khanacademy.org/math/multivariable-calculus/applications-of-multivariable-derivatives/tangent-planes-and-local-linearization/a/local-linearization - * Where: - * x = x0 - * f(x0​) = func_errors - * ∇f(x0​) = func_jacobian - * - * Therefore: - * a = f(x0​) - ∇f(x0​) * x - * b = ∇f(x0​) - * - * @param func_errors The functions error - * @param func_jacobian The functions jacobian - * @param x The value used to calculate the error and jacobian - * @return A linear approximation + * @brief Build a local affine (first-order) approximation of a vector-valued function. + * + * This constructs an affine model + * + * \f[ + * \hat{f}(x) = a + B x + * \f] + * + * around a fixed expansion point \f$x_0\f (here given by @p x), such that: + * + * - \f$\hat{f}(x_0) = f(x_0)\f$ (matches the function value at @p x) + * - \f$\nabla \hat{f}(x_0) = \nabla f(x_0)\f$ (matches the Jacobian at @p x) + * + * Given: + * + * - @p func_error \f$\equiv f(x_0)\f$ + * - @p func_jacobian \f$\equiv \nabla f(x_0)\f$ + * + * the affine approximation can be written as + * + * \f[ + * \hat{f}(x) = f(x_0) + \nabla f(x_0)\,(x - x_0) + * = \underbrace{\big(f(x_0) - \nabla f(x_0)\,x_0\big)}_{a} + * + \underbrace{\nabla f(x_0)}_{B}\,x. + * \f] + * + * This function returns an @ref AffExprs where: + * + * - `constants = func_error - func_jacobian * x` (the vector @f$a@f$) + * - `linear_coeffs = func_jacobian` (the matrix @f$B@f$) + * + * @details + * The derivation follows the standard local linearization (tangent plane) of a multivariable function. + * For a good conceptual reference, see: + * + * https://www.khanacademy.org/math/multivariable-calculus/applications-of-multivariable-derivatives/tangent-planes-and-local-linearization/a/local-linearization + * + * @param func_error Function value @f$f(x_0)@f$ at the linearization point. + * @param func_jacobian Jacobian @f$\nabla f(x_0)@f$ at the linearization point. + * @param x Linearization point @f$x_0@f$ used to compute @p func_error and @p func_jacobian. + * @return Affine expressions representing the local linear approximation @f$\hat{f}(x)@f$. */ AffExprs createAffExprs(const Eigen::Ref& func_error, const Eigen::Ref& func_jacobian, const Eigen::Ref& x); /** - * @brief Given the function error, jacobian and hessians calculate the quadratic approximation - * @details This was derived using the reference below, but the CostFromFunc::convex in the original trajopt is - * different References: - * https://www.khanacademy.org/math/multivariable-calculus/applications-of-multivariable-derivatives/quadratic-approximations/a/quadratic-approximation - * Where: - * x = x0 - * f(x0​) = func_errors - * ∇f(x0​) = func_jacobian - * H = func_hessians - * - * Therefore: - * c = 0.5 * H​(x0​); - * a = f(x0​) - ∇f(x0​) * x + x.transpose() * c * x - * b = ∇f(x0​) - (2 * c) * x - * - * @param func_errors The functions error - * @param func_jacobian The functions jacobian - * @param func_hessians The functions hessian - * @param x The value used to calculate the error, jacobian and hessian - * @return A quadratic approximation + * @brief Build a local quadratic (second-order) approximation of a vector-valued function. + * + * This constructs, for each scalar component \f$f_i\f$ of a vector-valued function + * \f$f : \mathbb{R}^n \to \mathbb{R}^m\f$, a quadratic model around the expansion + * point \f$x_0\f (given by @p x): + * + * \f[ + * \hat{f}_i(x) = + * a_i + * + b_i^\top x + * + x^\top C_i x, + * \f] + * + * such that the model matches the function value, gradient, and Hessian at \f$x_0\f: + * + * - \f$\hat{f}_i(x_0) = f_i(x_0)\f$ + * - \f$\nabla \hat{f}_i(x_0) = \nabla f_i(x_0)\f$ + * - \f$\nabla^2 \hat{f}_i(x_0) = H_i\f$ + * + * where: + * + * - @p func_errors \f$\equiv f(x_0)\f \in \mathbb{R}^m\f$ + * - @p func_jacobian \f$\equiv \nabla f(x_0)\f \in \mathbb{R}^{m \times n}\f$ + * - @p func_hessians \f$\equiv \{H_i\}_{i=1}^m\f$, one Hessian per component + * - @p x \f$\equiv x_0\f$ + * + * Following the standard second-order Taylor expansion around \f$x_0\f\f$: + * + * \f[ + * f_i(x) \approx f_i(x_0) + * + \nabla f_i(x_0)^\top (x - x_0) + * + \tfrac{1}{2}(x - x_0)^\top H_i (x - x_0), + * \f] + * + * we collect terms in powers of \f$x\f$ and define: + * + * \f[ + * C_i = \tfrac{1}{2} H_i, + * \f] + * \f[ + * a_i = f_i(x_0) - \nabla f_i(x_0)^\top x_0 + x_0^\top C_i x_0, + * \f] + * \f[ + * b_i^\top = \nabla f_i(x_0)^\top - (2 C_i x_0)^\top. + * \f] + * + * In the returned @ref QuadExprs this corresponds to: + * + * - `constants(i) = a_i` + * - `linear_coeffs.row(i) = b_i^T` + * - `quadratic_coeffs[i] = C_i` + * + * @details + * The derivation follows the standard quadratic approximation of multivariable functions. + * For a conceptual overview, see: + * + * https://www.khanacademy.org/math/multivariable-calculus/applications-of-multivariable-derivatives/quadratic-approximations/a/quadratic-approximation + * + * Note that this implementation differs from `CostFromFunc::convex` in the original TrajOpt code, + * but the underlying Taylor expansion idea is the same. + * + * @param func_errors Function values @f$f(x_0)@f$ at the expansion point. + * @param func_jacobian Jacobian @f$\nabla f(x_0)@f$ at the expansion point. + * @param func_hessians Per-component Hessians @f$\{H_i\}@f$ at the expansion point; size must equal @p + * func_errors.rows(). + * @param x Expansion point @f$x_0@f$ used to compute @p func_errors, @p func_jacobian, and @p + * func_hessians. + * @return Quadratic expressions representing the local second-order approximation \f$\hat{f}(x)\f$. */ QuadExprs createQuadExprs(const Eigen::Ref& func_errors, const Eigen::Ref& func_jacobian, const std::vector& func_hessians, const Eigen::Ref& x); +/** + * @brief Construct a quadratic representation for the element-wise square of affine expressions. + * + * Given an affine expression + * + * f_i(x) = a_i + b_i^T x, + * + * for each row i in @p aff_expr, this function builds a quadratic model + * + * g_i(x) = f_i(x)^2 + * = (a_i + b_i^T x)^2 + * = a_i^2 + 2 a_i b_i^T x + x^T (b_i b_i^T) x. + * + * In the returned @ref QuadExprs this corresponds to: + * + * - `constants(i)` = a_i² + * - `linear_coeffs.row(i)` = 2 a_i b_i^T + * - `quadratic_coeffs[i]` = b_i b_i^T + * + * Additionally, this function accumulates the per-equation contributions into an + * aggregate quadratic objective of the form + * + * J(x) = sum_i g_i(x) = sum_i f_i(x)^2, + * + * by setting: + * + * - `objective_linear_coeffs` = ∑_i (linear_coeffs.row(i))^T + * - `objective_quadratic_coeffs`= ∑_i quadratic_coeffs[i] + * + * This is useful when you want to convert a sum-of-squares objective based on + * affine residuals into a single quadratic form suitable for QP solvers. + * + * @param aff_expr Affine expressions f(x) = a + Bx to be squared element-wise. + * @return A @ref QuadExprs encoding g(x) = f(x) ∘ f(x) (element-wise square), + * along with the aggregated quadratic objective coefficients. + */ QuadExprs squareAffExprs(const AffExprs& aff_expr); } // namespace trajopt_sqp diff --git a/trajopt_optimizers/trajopt_sqp/src/expressions.cpp b/trajopt_optimizers/trajopt_sqp/src/expressions.cpp index 7d570503..39cc1efb 100644 --- a/trajopt_optimizers/trajopt_sqp/src/expressions.cpp +++ b/trajopt_optimizers/trajopt_sqp/src/expressions.cpp @@ -2,7 +2,13 @@ namespace trajopt_sqp { -Eigen::VectorXd AffExprs::values(const Eigen::Ref& x) const { return constants + (linear_coeffs * x); } +Eigen::VectorXd AffExprs::values(const Eigen::Ref& x) const +{ + // Avoid building a temporary for (linear_coeffs * x) + constants + Eigen::VectorXd result = constants; + result.noalias() += linear_coeffs * x; + return result; +} QuadExprs::QuadExprs(Eigen::Index num_cost, Eigen::Index num_vars) : constants(Eigen::VectorXd::Zero(num_cost)) @@ -13,18 +19,30 @@ QuadExprs::QuadExprs(Eigen::Index num_cost, Eigen::Index num_vars) quadratic_coeffs.reserve(static_cast(num_cost)); } -Eigen::VectorXd QuadExprs::values(const Eigen::Ref& x) const +Eigen::VectorXd QuadExprs::values(const Eigen::Ref& x) const { - const Eigen::VectorXd result_lin = constants + (linear_coeffs * x); - Eigen::VectorXd result_quad = result_lin; - assert(result_quad.rows() == static_cast(quadratic_coeffs.size())); - for (std::size_t i = 0; i < quadratic_coeffs.size(); ++i) + // Start with affine part: c + A x + Eigen::VectorXd result = constants; + result.noalias() += linear_coeffs * x; + + if (quadratic_coeffs.empty()) + return result; + + // Reuse a single scratch vector for Q_i * x to avoid per-iteration allocations + Eigen::VectorXd tmp(x.size()); + const auto n = static_cast(quadratic_coeffs.size()); + assert(result.rows() == n); + + for (Eigen::Index i = 0; i < n; ++i) { - const auto& eq_quad_coeffs = quadratic_coeffs[i]; - if (eq_quad_coeffs.nonZeros() > 0) - result_quad(static_cast(i)) += (x.transpose() * eq_quad_coeffs * x); // NOLINT + const SparseMatrix& Q = quadratic_coeffs[static_cast(i)]; + if (Q.nonZeros() == 0) + continue; + + tmp.noalias() = Q * x; + result(i) += x.dot(tmp); // xᵀ Q x } - return result_quad; + return result; } AffExprs createAffExprs(const Eigen::Ref& func_error, @@ -33,7 +51,9 @@ AffExprs createAffExprs(const Eigen::Ref& func_error, { AffExprs aff_expr; - aff_expr.constants = func_error - (func_jacobian * x); + // constants = f(x₀) − J(x₀) x₀ + aff_expr.constants = func_error; + aff_expr.constants.noalias() -= func_jacobian * x; aff_expr.linear_coeffs = func_jacobian; return aff_expr; @@ -44,23 +64,37 @@ QuadExprs createQuadExprs(const Eigen::Ref& func_errors, const std::vector& func_hessians, const Eigen::Ref& x) { - QuadExprs quad_exprs; + const Eigen::Index num_cost = func_errors.rows(); + const Eigen::Index num_vars = func_jacobian.cols(); + + QuadExprs quad_exprs(num_cost, num_vars); + + // constants = f(x₀) − J(x₀) x₀ + quad_exprs.constants = func_errors; + quad_exprs.constants.noalias() -= func_jacobian * x; - quad_exprs.quadratic_coeffs.resize(static_cast(func_errors.rows())); quad_exprs.linear_coeffs.resize(func_jacobian.rows(), func_jacobian.cols()); - quad_exprs.constants = func_errors - (func_jacobian * x); // NOLINT + quad_exprs.linear_coeffs.setZero(); // keep existing behavior: rows with no Hessian stay zero + quad_exprs.quadratic_coeffs.resize(static_cast(num_cost)); - for (Eigen::Index i = 0; i < func_errors.rows(); ++i) // NOLINT + // Reuse scratch for H_i * x + Eigen::VectorXd tmp(num_vars); + + for (Eigen::Index i = 0; i < num_cost; ++i) { - const auto& eq_hessian = func_hessians[static_cast(i)]; - if (eq_hessian.nonZeros() > 0) - { - auto& c = quad_exprs.quadratic_coeffs[static_cast(i)]; - c = 0.5 * eq_hessian; + const auto& H = func_hessians[static_cast(i)]; + if (H.nonZeros() == 0) + continue; - quad_exprs.constants(i) += (x.transpose() * c * x); - quad_exprs.linear_coeffs.row(i) = func_jacobian.row(i) - ((2.0 * c) * x).transpose(); - } + auto& Q = quad_exprs.quadratic_coeffs[static_cast(i)]; + Q = 0.5 * H; // store ½ H + + // ½ xᵀ H x + tmp.noalias() = H * x; + quad_exprs.constants(i) += 0.5 * x.dot(tmp); + + // linear row: J_i − H x + quad_exprs.linear_coeffs.row(i) = func_jacobian.row(i) - tmp.transpose(); } return quad_exprs; @@ -68,23 +102,28 @@ QuadExprs createQuadExprs(const Eigen::Ref& func_errors, QuadExprs squareAffExprs(const AffExprs& aff_expr) { - QuadExprs quad_expr; + QuadExprs quad_expr(aff_expr.constants.rows(), aff_expr.linear_coeffs.cols()); + + // Per-equation constant term: a_i² quad_expr.constants = aff_expr.constants.array().square(); - quad_expr.linear_coeffs = (2 * aff_expr.constants).asDiagonal() * aff_expr.linear_coeffs; + + // Per-equation linear term: 2 a_i b_iᵀ + quad_expr.linear_coeffs = (2.0 * aff_expr.constants).asDiagonal() * aff_expr.linear_coeffs; + quad_expr.quadratic_coeffs.resize(static_cast(aff_expr.constants.rows())); - quad_expr.objective_linear_coeffs = Eigen::VectorXd::Zero(aff_expr.linear_coeffs.cols()); - quad_expr.objective_quadratic_coeffs.resize(aff_expr.linear_coeffs.cols(), aff_expr.linear_coeffs.cols()); + // Objective aggregates + quad_expr.objective_linear_coeffs = + quad_expr.linear_coeffs.transpose() * Eigen::VectorXd::Ones(quad_expr.linear_coeffs.rows()); + + quad_expr.objective_quadratic_coeffs.resize(aff_expr.linear_coeffs.cols(), aff_expr.linear_coeffs.cols()); + // Start as empty (all zeros) + // objective_quadratic_coeffs will be the sum of per-equation Q_i for (Eigen::Index i = 0; i < aff_expr.constants.rows(); ++i) { - // Increment the objective linear coefficients - quad_expr.objective_linear_coeffs += quad_expr.linear_coeffs.row(i); - - // Now calculate the quadratic coefficients - auto eq_affexpr_coeffs = aff_expr.linear_coeffs.row(i); - const SparseMatrix eq_quadexpr_coeffs = eq_affexpr_coeffs.transpose() * eq_affexpr_coeffs; + auto eq_affexpr_coeffs = aff_expr.linear_coeffs.row(i); // b_iᵀ + const SparseMatrix eq_quadexpr_coeffs = eq_affexpr_coeffs.transpose() * eq_affexpr_coeffs; // b_i b_iᵀ - // Store the quadtratic coeffs and increment the objective quadratic coefficients if (eq_quadexpr_coeffs.nonZeros() > 0) { quad_expr.quadratic_coeffs[static_cast(i)] = eq_quadexpr_coeffs; diff --git a/trajopt_optimizers/trajopt_sqp/src/osqp_eigen_solver.cpp b/trajopt_optimizers/trajopt_sqp/src/osqp_eigen_solver.cpp index caba5563..38eab2fb 100644 --- a/trajopt_optimizers/trajopt_sqp/src/osqp_eigen_solver.cpp +++ b/trajopt_optimizers/trajopt_sqp/src/osqp_eigen_solver.cpp @@ -28,7 +28,10 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include TRAJOPT_IGNORE_WARNINGS_POP -const bool OSQP_COMPARE_DEBUG_MODE = false; +namespace +{ +constexpr bool OSQP_COMPARE_DEBUG_MODE = false; +} namespace trajopt_sqp { @@ -75,6 +78,13 @@ bool OSQPEigenSolver::clear() solver_->clearSolver(); solver_->data()->clearHessianMatrix(); solver_->data()->clearLinearConstraintsMatrix(); + + num_vars_ = 0; + num_cnts_ = 0; + gradient_.resize(0); + bounds_lower_.resize(0); + bounds_upper_.resize(0); + solver_status_ = QPSolverStatus::UNITIALIZED; return true; } @@ -82,7 +92,13 @@ bool OSQPEigenSolver::solve() { // In order to call initSolver, everything must have already been set, so we call it right before solving if (!solver_->isInitialized()) // NOLINT - solver_->initSolver(); + { + if (!solver_->initSolver()) + { + solver_status_ = QPSolverStatus::QP_ERROR; + return false; + } + } if (OSQP_COMPARE_DEBUG_MODE) { @@ -185,11 +201,7 @@ bool OSQPEigenSolver::solve() return false; } -Eigen::VectorXd OSQPEigenSolver::getSolution() -{ - Eigen::VectorXd solution = solver_->getSolution(); - return solution; -} +Eigen::VectorXd OSQPEigenSolver::getSolution() { return solver_->getSolution(); } bool OSQPEigenSolver::updateHessianMatrix(const SparseMatrix& hessian) { @@ -198,15 +210,10 @@ bool OSQPEigenSolver::updateHessianMatrix(const SparseMatrix& hessian) const SparseMatrix cleaned = 2.0 * hessian.pruned(1e-7, 1); // Any value < 1e-7 will be removed if (solver_->isInitialized()) - { - const bool success = solver_->updateHessianMatrix(cleaned); - return success; - } + return solver_->updateHessianMatrix(cleaned); solver_->data()->clearHessianMatrix(); - const bool success = solver_->data()->setHessianMatrix(cleaned); - - return success; + return solver_->data()->setHessianMatrix(cleaned); } bool OSQPEigenSolver::updateGradient(const Eigen::Ref& gradient) @@ -254,13 +261,9 @@ bool OSQPEigenSolver::updateLinearConstraintsMatrix(const SparseMatrix& linearCo const SparseMatrix cleaned = linearConstraintsMatrix.pruned(1e-7, 1); // Any value < 1e-7 will be removed if (solver_->isInitialized()) - { - const bool success = solver_->updateLinearConstraintsMatrix(cleaned); - return success; - } + return solver_->updateLinearConstraintsMatrix(cleaned); - const bool success = solver_->data()->setLinearConstraintsMatrix(cleaned); - return success; + return solver_->data()->setLinearConstraintsMatrix(cleaned); } } // namespace trajopt_sqp diff --git a/trajopt_optimizers/trajopt_sqp/src/trajopt_qp_problem.cpp b/trajopt_optimizers/trajopt_sqp/src/trajopt_qp_problem.cpp index 29379918..ce85fd4c 100644 --- a/trajopt_optimizers/trajopt_sqp/src/trajopt_qp_problem.cpp +++ b/trajopt_optimizers/trajopt_sqp/src/trajopt_qp_problem.cpp @@ -82,6 +82,17 @@ struct TrajOptQPProblem::Implementation // This should be the center of the bounds Eigen::VectorXd constraint_constant_; + // Cached bounds (assumed static over SQP iterations) + std::vector nlp_cnt_bounds_; + std::vector hinge_cnt_bounds_; + std::vector abs_cnt_bounds_; + + std::vector squared_cost_bounds_; + std::vector abs_cost_bounds_; + std::vector hinge_cost_bounds_; + + std::vector var_bounds_; + void addVariableSet(const std::shared_ptr& variable_set); void addConstraintSet(const std::shared_ptr& constraint_set); @@ -244,32 +255,46 @@ void TrajOptQPProblem::Implementation::addCostSet(const std::shared_ptr(n_nlp_cnts)); + cost_names_.reserve(static_cast(n_nlp_costs)); // Get NLP Cost and Constraint Names for Debug Print for (const auto& cnt : constraints_.GetComponents()) { - // Loop over each constraint in the set - for (Eigen::Index j = 0; j < cnt->GetRows(); j++) + for (Eigen::Index j = 0; j < cnt->GetRows(); ++j) constraint_names_.push_back(cnt->GetName() + "_" + std::to_string(j)); } for (const auto& cost : squared_costs_.GetComponents()) { - std::vector cost_bounds = cost->GetBounds(); - for (Eigen::Index j = 0; j < cost->GetRows(); j++) + const std::vector cost_bounds = cost->GetBounds(); + for (Eigen::Index j = 0; j < cost->GetRows(); ++j) { assert(trajopt_ifopt::isBoundsEquality(cost_bounds[static_cast(j)])); - squared_costs_target_(j) = cost_bounds[static_cast(j)].lower_; cost_names_.push_back(cost->GetName() + "_" + std::to_string(j)); } @@ -277,11 +302,10 @@ void TrajOptQPProblem::Implementation::setup() for (const auto& cost : abs_costs_.GetComponents()) { - // Add to the qp problem solver constraints abs_constraints_.AddComponent(cost); - std::vector cost_bounds = cost->GetBounds(); - for (Eigen::Index j = 0; j < cost->GetRows(); j++) + const std::vector cost_bounds = cost->GetBounds(); + for (Eigen::Index j = 0; j < cost->GetRows(); ++j) { assert(trajopt_ifopt::isBoundsEquality(cost_bounds[static_cast(j)])); cost_names_.push_back(cost->GetName() + "_" + std::to_string(j)); @@ -290,56 +314,66 @@ void TrajOptQPProblem::Implementation::setup() for (const auto& cost : hinge_costs_.GetComponents()) { - // Add to the qp problem solver constraints hinge_constraints_.AddComponent(cost); - std::vector cost_bounds = cost->GetBounds(); - for (Eigen::Index j = 0; j < cost->GetRows(); j++) + const std::vector cost_bounds = cost->GetBounds(); + for (Eigen::Index j = 0; j < cost->GetRows(); ++j) { assert(trajopt_ifopt::isBoundsInEquality(cost_bounds[static_cast(j)])); cost_names_.push_back(cost->GetName() + "_" + std::to_string(j)); } } + // Cache flattened bounds for costs (used in exact cost evaluation) + squared_cost_bounds_ = squared_costs_.GetBounds(); + abs_cost_bounds_ = abs_costs_.GetBounds(); + hinge_cost_bounds_ = hinge_costs_.GetBounds(); + //////////////////////////////////////////////////////// // Get NLP bounds and detect constraint type //////////////////////////////////////////////////////// - Eigen::VectorXd nlp_bounds_l(getNumNLPConstraints()); - Eigen::VectorXd nlp_bounds_u(getNumNLPConstraints()); - // Convert constraint bounds to VectorXd - std::vector cnt_bounds = constraints_.GetBounds(); - for (Eigen::Index i = 0; i < getNumNLPConstraints(); i++) + nlp_cnt_bounds_ = constraints_.GetBounds(); + + Eigen::VectorXd nlp_bounds_l(n_nlp_cnts); + Eigen::VectorXd nlp_bounds_u(n_nlp_cnts); + + const std::vector cnt_bounds = constraints_.GetBounds(); + for (Eigen::Index i = 0; i < n_nlp_cnts; ++i) { nlp_bounds_l[i] = cnt_bounds[static_cast(i)].lower_; nlp_bounds_u[i] = cnt_bounds[static_cast(i)].upper_; } - // Detect constraint type - Eigen::VectorXd nlp_bounds_diff = nlp_bounds_u - nlp_bounds_l; - constraint_types_.resize(static_cast(getNumNLPConstraints())); - for (std::size_t i = 0; i < static_cast(nlp_bounds_diff.size()); i++) + const Eigen::VectorXd nlp_bounds_diff = nlp_bounds_u - nlp_bounds_l; + constraint_types_.resize(static_cast(n_nlp_cnts)); + + for (std::size_t i = 0; i < static_cast(nlp_bounds_diff.size()); ++i) { if (std::abs(nlp_bounds_diff[static_cast(i)]) < 1e-3) { constraint_types_[i] = ConstraintType::EQ; - // Add 2 slack variables for L1 loss - num_qp_vars_ += 2; + num_qp_vars_ += 2; // L1 slack pair num_qp_cnts_ += 2; } else { constraint_types_[i] = ConstraintType::INEQ; - // Add 1 slack variable for hinge loss - num_qp_vars_ += 1; + num_qp_vars_ += 1; // hinge slack num_qp_cnts_ += 1; } } + // Cache constraint bounds for hinge / abs constraint composites + hinge_cnt_bounds_ = hinge_constraints_.GetBounds(); + abs_cnt_bounds_ = abs_constraints_.GetBounds(); + + // Cache variable bounds (used in updateNLPVariableBounds) + var_bounds_ = variables_->GetBounds(); + // Initialize the constraint bounds bounds_lower_ = Eigen::VectorXd::Constant(num_qp_cnts_, -double(INFINITY)); bounds_upper_ = Eigen::VectorXd::Constant(num_qp_cnts_, double(INFINITY)); - // Set initialized initialized_ = true; } @@ -375,41 +409,50 @@ Eigen::VectorXd TrajOptQPProblem::Implementation::evaluateConvexCosts(const Eige if (getNumNLPCosts() == 0) return {}; - Eigen::VectorXd var_block = var_vals.head(getNumNLPVars()); - Eigen::VectorXd costs = Eigen::VectorXd::Zero(getNumNLPCosts()); - if (squared_costs_.GetRows() > 0) + const Eigen::Index nlp_vars = getNumNLPVars(); + const Eigen::Index n_sq = squared_costs_.GetRows(); + const Eigen::Index n_hinge = hinge_costs_.GetRows(); + const Eigen::Index n_abs = abs_costs_.GetRows(); + const Eigen::Index total_cost = n_sq + n_hinge + n_abs; + + Eigen::VectorXd costs = Eigen::VectorXd::Zero(total_cost); + Eigen::VectorXd var_block = var_vals.head(nlp_vars); + + // Squared costs (already convexified into squared_objective_nlp_) + if (n_sq > 0) { - costs.head(squared_costs_.GetRows()) = squared_objective_nlp_.values(var_block); - assert(!(costs.head(squared_costs_.GetRows()).array() < -1e-8).any()); + costs.head(n_sq) = squared_objective_nlp_.values(var_block); + assert(!(costs.head(n_sq).array() < -1e-8).any()); } - if (hinge_costs_.GetRows() > 0) + // Hinge costs + if (n_hinge > 0) { - const Eigen::VectorXd hinge_cnt_constant = constraint_constant_.topRows(hinge_costs_.GetRows()); - auto hinge_cnt_jac = constraint_matrix_.block(0, 0, hinge_constraints_.GetRows(), getNumNLPVars()); + const Eigen::VectorXd hinge_cnt_constant = constraint_constant_.topRows(n_hinge); + const auto hinge_cnt_jac = constraint_matrix_.block(0, 0, hinge_constraints_.GetRows(), nlp_vars); const Eigen::VectorXd hinge_convex_value = hinge_cnt_constant + hinge_cnt_jac * var_block; - const Eigen::VectorXd hinge_cost = - trajopt_ifopt::calcBoundsViolations(hinge_convex_value, hinge_costs_.GetBounds()); + const Eigen::VectorXd hinge_cost = trajopt_ifopt::calcBoundsViolations(hinge_convex_value, hinge_cost_bounds_); - costs.middleRows(squared_costs_.GetRows(), hinge_costs_.GetRows()) = hinge_cost; - assert(!(costs.middleRows(squared_costs_.GetRows(), hinge_costs_.GetRows()).array() < -1e-8).any()); + costs.segment(n_sq, n_hinge) = hinge_cost; + assert(!(costs.segment(n_sq, n_hinge).array() < -1e-8).any()); } - if (abs_costs_.GetRows() > 0) + // Absolute costs + if (n_abs > 0) { - const Eigen::VectorXd abs_cnt_constant = - constraint_constant_.middleRows(hinge_costs_.GetRows(), abs_costs_.GetRows()); - auto abs_cnt_jac = constraint_matrix_.block(hinge_costs_.GetRows(), 0, abs_constraints_.GetRows(), getNumNLPVars()); - + const Eigen::Index abs_row_offset = n_hinge; + const Eigen::VectorXd abs_cnt_constant = constraint_constant_.segment(abs_row_offset, n_abs); + const auto abs_cnt_jac = constraint_matrix_.block(abs_row_offset, 0, abs_constraints_.GetRows(), nlp_vars); const Eigen::VectorXd abs_convex_value = abs_cnt_constant + abs_cnt_jac * var_block; - const Eigen::VectorXd abs_cost = - trajopt_ifopt::calcBoundsViolations(abs_convex_value, abs_costs_.GetBounds()).cwiseAbs(); - costs.middleRows(squared_costs_.GetRows() + hinge_costs_.GetRows(), abs_costs_.GetRows()) = abs_cost; - assert(!(costs.middleRows(squared_costs_.GetRows() + hinge_costs_.GetRows(), abs_costs_.GetRows()).array() < -1e-8) - .any()); + // calcBoundsViolations already returns |violation| + const Eigen::VectorXd abs_cost = trajopt_ifopt::calcBoundsViolations(abs_convex_value, abs_cost_bounds_); + + costs.segment(n_sq + n_hinge, n_abs) = abs_cost; + assert(!(costs.segment(n_sq + n_hinge, n_abs).array() < -1e-8).any()); } + return costs; } @@ -423,23 +466,22 @@ double TrajOptQPProblem::Implementation::evaluateTotalExactCost(const Eigen::Ref if (squared_costs_.GetRows() > 0) { - Eigen::VectorXd error = trajopt_ifopt::calcBoundsViolations(squared_costs_.GetValues(), squared_costs_.GetBounds()); - assert((error.array() < 0).any() == false); + const Eigen::VectorXd error = trajopt_ifopt::calcBoundsViolations(squared_costs_.GetValues(), squared_cost_bounds_); + assert(!(error.array() < 0.0).any()); g += error.squaredNorm(); } if (abs_costs_.GetRows() > 0) { - Eigen::VectorXd error = - trajopt_ifopt::calcBoundsViolations(abs_costs_.GetValues(), abs_costs_.GetBounds()).cwiseAbs(); - assert((error.array() < 0).any() == false); + const Eigen::VectorXd error = trajopt_ifopt::calcBoundsViolations(abs_costs_.GetValues(), abs_cost_bounds_); + assert(!(error.array() < 0.0).any()); g += error.sum(); } if (hinge_costs_.GetRows() > 0) { - Eigen::VectorXd error = trajopt_ifopt::calcBoundsViolations(hinge_costs_.GetValues(), hinge_costs_.GetBounds()); - assert((error.array() < 0).any() == false); + const Eigen::VectorXd error = trajopt_ifopt::calcBoundsViolations(hinge_costs_.GetValues(), hinge_cost_bounds_); + assert(!(error.array() < 0.0).any()); g += error.sum(); } @@ -454,25 +496,26 @@ Eigen::VectorXd TrajOptQPProblem::Implementation::evaluateExactCosts(const Eigen setVariables(var_vals.data()); Eigen::VectorXd g(getNumNLPCosts()); - Eigen::Index start_index = 0; + Eigen::Index idx = 0; + if (squared_costs_.GetRows() > 0) { - g.topRows(squared_costs_.GetRows()) = - trajopt_ifopt::calcBoundsViolations(squared_costs_.GetValues(), squared_costs_.GetBounds()).array().square(); - start_index += squared_costs_.GetRows(); + const Eigen::VectorXd err = trajopt_ifopt::calcBoundsViolations(squared_costs_.GetValues(), squared_cost_bounds_); + g.segment(idx, squared_costs_.GetRows()) = err.array().square().matrix(); + idx += squared_costs_.GetRows(); } if (abs_costs_.GetRows() > 0) { - g.middleRows(start_index, abs_costs_.GetRows()) = - trajopt_ifopt::calcBoundsViolations(abs_costs_.GetValues(), abs_costs_.GetBounds()).cwiseAbs(); - start_index += abs_costs_.GetRows(); + const Eigen::VectorXd err = trajopt_ifopt::calcBoundsViolations(abs_costs_.GetValues(), abs_cost_bounds_); + g.segment(idx, abs_costs_.GetRows()) = err; + idx += abs_costs_.GetRows(); } if (hinge_costs_.GetRows() > 0) { - g.middleRows(start_index, hinge_costs_.GetRows()) = - trajopt_ifopt::calcBoundsViolations(hinge_costs_.GetValues(), hinge_costs_.GetBounds()); + const Eigen::VectorXd err = trajopt_ifopt::calcBoundsViolations(hinge_costs_.GetValues(), hinge_cost_bounds_); + g.segment(idx, hinge_costs_.GetRows()) = err; } return g; @@ -487,7 +530,7 @@ TrajOptQPProblem::Implementation::evaluateConvexConstraintViolations(const Eigen var_vals.head(getNumNLPVars()); // NOLINT const Eigen::VectorXd constraint_value = constraint_constant_.middleRows(row_index, getNumNLPConstraints()) + result_lin; - return trajopt_ifopt::calcBoundsViolations(constraint_value, constraints_.GetBounds()); + return trajopt_ifopt::calcBoundsViolations(constraint_value, nlp_cnt_bounds_); } Eigen::VectorXd @@ -495,7 +538,7 @@ TrajOptQPProblem::Implementation::evaluateExactConstraintViolations(const Eigen: { setVariables(var_vals.data()); const Eigen::VectorXd cnt_vals = constraints_.GetValues(); - return trajopt_ifopt::calcBoundsViolations(cnt_vals, constraints_.GetBounds()); + return trajopt_ifopt::calcBoundsViolations(cnt_vals, nlp_cnt_bounds_); } void TrajOptQPProblem::Implementation::scaleBoxSize(double& scale) @@ -561,10 +604,7 @@ void TrajOptQPProblem::Implementation::print() const Eigen::Index TrajOptQPProblem::Implementation::getNumNLPVars() const { return variables_->GetRows(); } -Eigen::Index TrajOptQPProblem::Implementation::getNumNLPConstraints() const -{ - return static_cast(constraints_.GetBounds().size()); -} +Eigen::Index TrajOptQPProblem::Implementation::getNumNLPConstraints() const { return constraints_.GetRows(); } Eigen::Index TrajOptQPProblem::Implementation::getNumNLPCosts() const { @@ -694,92 +734,89 @@ void TrajOptQPProblem::Implementation::linearizeConstraints() const SparseMatrix hinge_cnt_jac = hinge_constraints_.GetJacobian(); const SparseMatrix abs_cnt_jac = abs_constraints_.GetJacobian(); - // Create triplet list of nonzero constraints - using T = Eigen::Triplet; - std::vector tripletList; - tripletList.reserve(static_cast(nlp_cnt_jac.nonZeros() + hinge_cnt_jac.nonZeros() + - abs_cnt_jac.nonZeros() + num_qp_vars_) * - 3); + std::vector> triplets; + const Eigen::Index nnz_base = nlp_cnt_jac.nonZeros() + hinge_cnt_jac.nonZeros() + abs_cnt_jac.nonZeros(); + // Rough but closer than *3 + triplets.reserve(static_cast(nnz_base + num_qp_vars_ + // diag + hinge_costs_.GetRows() + (2L * abs_costs_.GetRows()) + + (2L * getNumNLPConstraints()))); - // Add hinge solver constraint jacobian to triplet list + // hinge constraints for (int k = 0; k < hinge_cnt_jac.outerSize(); ++k) - { for (SparseMatrix::InnerIterator it(hinge_cnt_jac, k); it; ++it) - { - tripletList.emplace_back(it.row(), it.col(), it.value()); - } - } + triplets.emplace_back(it.row(), it.col(), it.value()); - // Add abs solver constraint jacobian to triplet list + // abs constraints (shifted rows) Eigen::Index current_row_index = hinge_constraints_.GetRows(); for (int k = 0; k < abs_cnt_jac.outerSize(); ++k) - { for (SparseMatrix::InnerIterator it(abs_cnt_jac, k); it; ++it) - { - tripletList.emplace_back(current_row_index + it.row(), it.col(), it.value()); - } - } + triplets.emplace_back(current_row_index + it.row(), it.col(), it.value()); - // Add nlp constraint jacobian to triplet list + // nlp constraints (shift again) current_row_index += abs_constraints_.GetRows(); for (int k = 0; k < nlp_cnt_jac.outerSize(); ++k) - { for (SparseMatrix::InnerIterator it(nlp_cnt_jac, k); it; ++it) - { - tripletList.emplace_back(current_row_index + it.row(), it.col(), it.value()); - } - } + triplets.emplace_back(current_row_index + it.row(), it.col(), it.value()); - // Add the hinge variables to each hinge constraint - Eigen::Index current_column_index = getNumNLPVars(); - for (Eigen::Index i = 0; i < static_cast(hinge_costs_.GetRows()); i++) - tripletList.emplace_back(i, current_column_index++, -1); + // hinge slack vars + Eigen::Index current_col_index = getNumNLPVars(); + for (Eigen::Index i = 0; i < hinge_costs_.GetRows(); ++i) + triplets.emplace_back(i, current_col_index++, -1.0); - // Add the absolute variables to each hinge constraint + // abs slack vars current_row_index = hinge_costs_.GetRows(); - for (Eigen::Index i = 0; i < static_cast(abs_costs_.GetRows()); i++) + for (Eigen::Index i = 0; i < abs_costs_.GetRows(); ++i) { - tripletList.emplace_back(current_row_index + i, current_column_index++, 1); - tripletList.emplace_back(current_row_index + i, current_column_index++, -1); + triplets.emplace_back(current_row_index + i, current_col_index++, 1.0); + triplets.emplace_back(current_row_index + i, current_col_index++, -1.0); } - // Add the slack variables to each constraint + // constraint slack vars current_row_index += abs_costs_.GetRows(); - for (Eigen::Index i = 0; i < static_cast(constraint_types_.size()); i++) + for (Eigen::Index i = 0; i < static_cast(constraint_types_.size()); ++i) { if (constraint_types_[static_cast(i)] == ConstraintType::EQ) { - tripletList.emplace_back(current_row_index + i, current_column_index++, 1); - tripletList.emplace_back(current_row_index + i, current_column_index++, -1); + triplets.emplace_back(current_row_index + i, current_col_index++, 1.0); + triplets.emplace_back(current_row_index + i, current_col_index++, -1.0); } else { - tripletList.emplace_back(current_row_index + i, current_column_index++, -1); + triplets.emplace_back(current_row_index + i, current_col_index++, -1.0); } } // Add a diagonal matrix for the variable limits (including slack variables since the merit coeff is only applied in // the cost) below the actual constraints current_row_index = nlp_cnt_jac.rows() + hinge_cnt_jac.rows() + abs_cnt_jac.rows(); - for (Eigen::Index i = 0; i < num_qp_vars_; i++) - tripletList.emplace_back(current_row_index + i, i, 1); + for (Eigen::Index i = 0; i < num_qp_vars_; ++i) + triplets.emplace_back(current_row_index + i, i, 1.0); // Insert the triplet list into the sparse matrix constraint_matrix_.resize(num_qp_cnts_, num_qp_vars_); - constraint_matrix_.reserve(nlp_cnt_jac.nonZeros() + hinge_cnt_jac.nonZeros() + abs_cnt_jac.nonZeros() + num_qp_vars_); - constraint_matrix_.setFromTriplets(tripletList.begin(), tripletList.end()); // NOLINT + constraint_matrix_.reserve(nnz_base + num_qp_vars_); + constraint_matrix_.setFromTriplets(triplets.begin(), triplets.end()); // NOLINT } void TrajOptQPProblem::Implementation::updateConstraintsConstantExpression() { - const long total_num_cnt = (getNumNLPConstraints() + hinge_constraints_.GetRows() + abs_constraints_.GetRows()); + const Eigen::Index n_nlp = getNumNLPVars(); + const Eigen::Index n_hinge = hinge_constraints_.GetRows(); + const Eigen::Index n_abs = abs_constraints_.GetRows(); + const Eigen::Index n_nlp_cnt = getNumNLPConstraints(); + const Eigen::Index total_num_cnt = n_hinge + n_abs + n_nlp_cnt; + if (total_num_cnt == 0) return; // Get values about which we will linearize - const Eigen::VectorXd x_initial = variables_->GetValues().head(getNumNLPVars()); - Eigen::Index current_row_index = 0; - if (hinge_constraints_.GetRows() > 0) + const Eigen::VectorXd x_initial = variables_->GetValues().head(n_nlp); + + // One mat-vec for all constraint rows (excluding slack columns) + const Eigen::VectorXd lin = constraint_matrix_.block(0, 0, total_num_cnt, n_nlp) * x_initial; + + Eigen::Index row = 0; + if (n_hinge > 0) { // Get values about which we will linearize const Eigen::VectorXd cnt_initial_value = hinge_constraints_.GetValues(); @@ -798,14 +835,11 @@ void TrajOptQPProblem::Implementation::updateConstraintsConstantExpression() // to calculate the merit of the solve. // The block excludes the slack variables - const SparseMatrix jac = - constraint_matrix_.block(current_row_index, 0, hinge_constraints_.GetRows(), getNumNLPVars()); - constraint_constant_.middleRows(current_row_index, hinge_constraints_.GetRows()) = - (cnt_initial_value - jac * x_initial); - current_row_index += hinge_constraints_.GetRows(); + constraint_constant_.segment(row, n_hinge) = cnt_initial_value - lin.segment(row, n_hinge); + row += n_hinge; } - if (abs_constraints_.GetRows() > 0) + if (n_abs > 0) { // Get values about which we will linearize const Eigen::VectorXd cnt_initial_value = abs_constraints_.GetValues(); @@ -824,14 +858,11 @@ void TrajOptQPProblem::Implementation::updateConstraintsConstantExpression() // to calculate the merit of the solve. // The block excludes the slack variables - const SparseMatrix jac = - constraint_matrix_.block(current_row_index, 0, abs_constraints_.GetRows(), getNumNLPVars()); - constraint_constant_.middleRows(current_row_index, abs_constraints_.GetRows()) = - (cnt_initial_value - jac * x_initial); - current_row_index += abs_constraints_.GetRows(); + constraint_constant_.segment(row, n_abs) = cnt_initial_value - lin.segment(row, n_abs); + row += n_abs; } - if (constraints_.GetRows() > 0) + if (n_nlp_cnt > 0) { const Eigen::VectorXd cnt_initial_value = constraints_.GetValues(); @@ -850,69 +881,75 @@ void TrajOptQPProblem::Implementation::updateConstraintsConstantExpression() // to calculate the merit of the solve. // The block excludes the slack variables - const SparseMatrix jac = constraint_matrix_.block(current_row_index, 0, getNumNLPConstraints(), getNumNLPVars()); - constraint_constant_.middleRows(current_row_index, getNumNLPConstraints()) = (cnt_initial_value - jac * x_initial); + constraint_constant_.segment(row, n_nlp_cnt) = cnt_initial_value - lin.segment(row, n_nlp_cnt); } } void TrajOptQPProblem::Implementation::updateNLPConstraintBounds() { - const long total_num_cnt = (getNumNLPConstraints() + hinge_constraints_.GetRows() + abs_constraints_.GetRows()); + const Eigen::Index n_hinge = hinge_constraints_.GetRows(); + const Eigen::Index n_abs = abs_constraints_.GetRows(); + const Eigen::Index n_nlp_cnt = getNumNLPConstraints(); + const Eigen::Index total_num_cnt = n_hinge + n_abs + n_nlp_cnt; + if (total_num_cnt == 0) return; Eigen::VectorXd cnt_bound_lower(total_num_cnt); Eigen::VectorXd cnt_bound_upper(total_num_cnt); - Eigen::Index current_row_index{ 0 }; + Eigen::Index row = 0; - // Convert hinge constraint bounds to VectorXd - std::vector hinge_cnt_bounds = hinge_constraints_.GetBounds(); - for (Eigen::Index i = 0; i < hinge_constraints_.GetRows(); i++) + // Hinge constraint bounds + for (Eigen::Index i = 0; i < n_hinge; ++i) { - cnt_bound_lower[current_row_index + i] = hinge_cnt_bounds[static_cast(i)].lower_; - cnt_bound_upper[current_row_index + i] = hinge_cnt_bounds[static_cast(i)].upper_; + const auto& b = hinge_cnt_bounds_[static_cast(i)]; + cnt_bound_lower[row + i] = b.lower_; + cnt_bound_upper[row + i] = b.upper_; } - current_row_index += hinge_constraints_.GetRows(); + row += n_hinge; - // Convert absolute constraint bounds to VectorXd - std::vector absolute_cnt_bounds = abs_constraints_.GetBounds(); - for (Eigen::Index i = 0; i < abs_constraints_.GetRows(); i++) + // Absolute constraint bounds + for (Eigen::Index i = 0; i < n_abs; ++i) { - cnt_bound_lower[current_row_index + i] = absolute_cnt_bounds[static_cast(i)].lower_; - cnt_bound_upper[current_row_index + i] = absolute_cnt_bounds[static_cast(i)].upper_; + const auto& b = abs_cnt_bounds_[static_cast(i)]; + cnt_bound_lower[row + i] = b.lower_; + cnt_bound_upper[row + i] = b.upper_; } - current_row_index += abs_constraints_.GetRows(); + row += n_abs; - // Convert nlp constraint bounds to VectorXd - std::vector cnt_bounds = constraints_.GetBounds(); - for (Eigen::Index i = 0; i < getNumNLPConstraints(); i++) + // NLP constraint bounds + for (Eigen::Index i = 0; i < n_nlp_cnt; ++i) { - cnt_bound_lower[current_row_index + i] = cnt_bounds[static_cast(i)].lower_; - cnt_bound_upper[current_row_index + i] = cnt_bounds[static_cast(i)].upper_; + const auto& b = nlp_cnt_bounds_[static_cast(i)]; + cnt_bound_lower[row + i] = b.lower_; + cnt_bound_upper[row + i] = b.upper_; } const Eigen::VectorXd linearized_cnt_lower = cnt_bound_lower - constraint_constant_; const Eigen::VectorXd linearized_cnt_upper = cnt_bound_upper - constraint_constant_; - // Insert linearized constraint bounds bounds_lower_.topRows(total_num_cnt) = linearized_cnt_lower; bounds_upper_.topRows(total_num_cnt) = linearized_cnt_upper; } void TrajOptQPProblem::Implementation::updateNLPVariableBounds() { - // This is equivalent to BasicTrustRegionSQP::setTrustBoxConstraints + // Equivalent to BasicTrustRegionSQP::setTrustBoxConstraints + const Eigen::Index n_nlp_vars = getNumNLPVars(); + const Eigen::Index n_hinge = hinge_constraints_.GetRows(); + const Eigen::Index n_abs = abs_constraints_.GetRows(); + const Eigen::VectorXd x_initial = variables_->GetValues(); // Set the variable limits once - const std::vector var_bounds = variables_->GetBounds(); - Eigen::VectorXd var_bounds_lower(getNumNLPVars()); - Eigen::VectorXd var_bounds_upper(getNumNLPVars()); - for (Eigen::Index i = 0; i < getNumNLPVars(); i++) + Eigen::VectorXd var_bounds_lower(n_nlp_vars); + Eigen::VectorXd var_bounds_upper(n_nlp_vars); + + for (Eigen::Index i = 0; i < n_nlp_vars; ++i) { - const auto& bounds = var_bounds[static_cast(i)]; - var_bounds_lower[i] = bounds.lower_; - var_bounds_upper[i] = bounds.upper_; + const auto& b = var_bounds_[static_cast(i)]; + var_bounds_lower[i] = b.lower_; + var_bounds_upper[i] = b.upper_; } // Calculate box constraints, while limiting to variable bounds and maintaining the trust region size @@ -920,9 +957,11 @@ void TrajOptQPProblem::Implementation::updateNLPVariableBounds() (x_initial.cwiseMin(var_bounds_upper - box_size_) - box_size_).cwiseMax(var_bounds_lower); const Eigen::VectorXd var_bounds_upper_final = (x_initial.cwiseMax(var_bounds_lower + box_size_) + box_size_).cwiseMin(var_bounds_upper); - const Eigen::Index var_row_index = getNumNLPConstraints() + hinge_constraints_.GetRows() + abs_constraints_.GetRows(); - bounds_lower_.block(var_row_index, 0, var_bounds_lower_final.size(), 1) = var_bounds_lower_final; - bounds_upper_.block(var_row_index, 0, var_bounds_upper_final.size(), 1) = var_bounds_upper_final; + + const Eigen::Index var_row_index = getNumNLPConstraints() + n_hinge + n_abs; + + bounds_lower_.segment(var_row_index, n_nlp_vars) = var_bounds_lower_final; + bounds_upper_.segment(var_row_index, n_nlp_vars) = var_bounds_upper_final; } void TrajOptQPProblem::Implementation::updateSlackVariableBounds() 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 b81d3355..1408c2cf 100644 --- a/trajopt_optimizers/trajopt_sqp/src/trust_region_sqp_solver.cpp +++ b/trajopt_optimizers/trajopt_sqp/src/trust_region_sqp_solver.cpp @@ -244,6 +244,9 @@ void TrustRegionSQPSolver::runTrustRegionLoop() // Solve the current QP problem status_ = solveQPProblem(); + if (status_ == SQPStatus::CALLBACK_STOPPED) + return; // Respect callbacks and exit gracefully + if (status_ != SQPStatus::RUNNING) { qp_solver_failures++; @@ -291,11 +294,12 @@ void TrustRegionSQPSolver::runTrustRegionLoop() return; } - if (results_.approx_merit_improve / results_.best_exact_merit < params.min_approx_improve_frac) + const double denom = std::max(std::abs(results_.best_exact_merit), 1e-12); + const double approx_frac = results_.approx_merit_improve / denom; + if (approx_frac < params.min_approx_improve_frac) { - CONSOLE_BRIDGE_logDebug("Converged because improvement ratio was small (%.3e < %.3e)", - results_.approx_merit_improve / results_.best_exact_merit, - params.min_approx_improve_frac); + CONSOLE_BRIDGE_logDebug( + "Converged because improvement ratio was small (%.3e < %.3e)", approx_frac, params.min_approx_improve_frac); status_ = SQPStatus::NLP_CONVERGED; return; } @@ -370,7 +374,11 @@ SQPStatus TrustRegionSQPSolver::solveQPProblem() results_.new_exact_merit = results_.new_costs.sum() + results_.new_constraint_violations.dot(results_.merit_error_coeffs); results_.exact_merit_improve = results_.best_exact_merit - results_.new_exact_merit; - results_.merit_improve_ratio = results_.exact_merit_improve / results_.approx_merit_improve; + // results_.merit_improve_ratio = results_.exact_merit_improve / results_.approx_merit_improve; + if (std::abs(results_.approx_merit_improve) < 1e-12) + results_.merit_improve_ratio = 0.0; // or 1.0, or whatever convention you want + else + results_.merit_improve_ratio = results_.exact_merit_improve / results_.approx_merit_improve; // The variable are changed to the new values to calculated data but must be set // to best var vals because the new values may not improve the merit which is determined later.