From cf32e192a004e4b2b59c7a7c94e030e1369fa23a Mon Sep 17 00:00:00 2001 From: Jack-ReframeSystems Date: Wed, 10 Apr 2024 12:54:35 +0000 Subject: [PATCH 1/2] Fix mismatched delete/free in OSQPModel --- .../include/trajopt_sco/osqp_interface.hpp | 22 +++++++++---------- trajopt_sco/src/osqp_interface.cpp | 2 +- 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/trajopt_sco/include/trajopt_sco/osqp_interface.hpp b/trajopt_sco/include/trajopt_sco/osqp_interface.hpp index 49fb9902..d2d1b78f 100644 --- a/trajopt_sco/include/trajopt_sco/osqp_interface.hpp +++ b/trajopt_sco/include/trajopt_sco/osqp_interface.hpp @@ -60,17 +60,17 @@ class OSQPModel : public Model ConstraintTypeVector cnt_types_; /**< constraints types */ DblVec solution_; /**< optimizizer's solution for current model */ - std::unique_ptr P_; /**< Takes ownership of OSQPData.P to avoid having to deallocate manually */ - std::unique_ptr A_; /**< Takes ownership of OSQPData.A to avoid having to deallocate manually */ - std::vector P_row_indices_; /**< row indices for P, CSC format */ - std::vector P_column_pointers_; /**< column pointers for P, CSC format */ - DblVec P_csc_data_; /**< P values in CSC format */ - Eigen::VectorXd q_; /**< linear part of the objective */ - - std::vector A_row_indices_; /**< row indices for constraint matrix, CSC format */ - std::vector A_column_pointers_; /**< column pointers for constraint matrix, CSC format */ - DblVec A_csc_data_; /**< constraint matrix values in CSC format */ - DblVec l_, u_; /**< linear constraints upper and lower limits */ + std::unique_ptr P_; /**< Takes ownership of OSQPData.P to avoid having to deallocate manually */ + std::unique_ptr A_; /**< Takes ownership of OSQPData.A to avoid having to deallocate manually */ + std::vector P_row_indices_; /**< row indices for P, CSC format */ + std::vector P_column_pointers_; /**< column pointers for P, CSC format */ + DblVec P_csc_data_; /**< P values in CSC format */ + Eigen::VectorXd q_; /**< linear part of the objective */ + + std::vector A_row_indices_; /**< row indices for constraint matrix, CSC format */ + std::vector A_column_pointers_; /**< column pointers for constraint matrix, CSC format */ + DblVec A_csc_data_; /**< constraint matrix values in CSC format */ + DblVec l_, u_; /**< linear constraints upper and lower limits */ QuadExpr objective_; /**< objective QuadExpr expression */ diff --git a/trajopt_sco/src/osqp_interface.cpp b/trajopt_sco/src/osqp_interface.cpp index cdb7e6f3..b4f90203 100644 --- a/trajopt_sco/src/osqp_interface.cpp +++ b/trajopt_sco/src/osqp_interface.cpp @@ -36,7 +36,7 @@ Model::Ptr createOSQPModel(const ModelConfig::ConstPtr& config = nullptr) return std::make_shared(config); } -OSQPModel::OSQPModel(const ModelConfig::ConstPtr& config) : P_(nullptr), A_(nullptr) +OSQPModel::OSQPModel(const ModelConfig::ConstPtr& config) : P_(nullptr, free), A_(nullptr, free) { // tuning parameters to be less accurate, but add a polishing step if (config != nullptr) From 10082c99bc662064533c833cf23318e2abc26faa Mon Sep 17 00:00:00 2001 From: Jack-ReframeSystems Date: Fri, 12 Apr 2024 21:09:24 +0000 Subject: [PATCH 2/2] Clang format 14 formatting --- .run-clang-format | 2 +- trajopt/src/collision_terms.cpp | 72 +++++++------------ .../vhacd/src/btConvexHullComputer.cpp | 26 ++++--- .../cartesian_position_constraint.cpp | 9 +-- .../trajopt_sqp/src/ifopt_qp_problem.cpp | 6 +- .../trajopt_sqp/src/trajopt_qp_problem.cpp | 12 ++-- trajopt_sco/include/trajopt_sco/bpmpd_io.hpp | 12 ++-- .../include/trajopt_sco/osqp_interface.hpp | 22 +++--- trajopt_sco/src/modeling_utils.cpp | 9 +-- 9 files changed, 64 insertions(+), 106 deletions(-) diff --git a/.run-clang-format b/.run-clang-format index 540f40e3..db12c95b 100755 --- a/.run-clang-format +++ b/.run-clang-format @@ -1,2 +1,2 @@ #!/bin/bash -find . -type f -regex '.*\.\(cpp\|hpp\|cc\|cxx\|h\|hxx\)' -exec clang-format-8 -style=file -i {} \; +find . -type f -regex '.*\.\(cpp\|hpp\|cc\|cxx\|h\|hxx\)' -exec clang-format -style=file -i {} \; diff --git a/trajopt/src/collision_terms.cpp b/trajopt/src/collision_terms.cpp index faaffa35..8bf03229 100644 --- a/trajopt/src/collision_terms.cpp +++ b/trajopt/src/collision_terms.cpp @@ -801,12 +801,10 @@ void CollisionEvaluator::removeInvalidContactResults(tesseract_collision::Contac contact_results.begin(), contact_results.end(), [=, &pair_data](const tesseract_collision::ContactResult& r) { switch (evaluator_type_) { - case CollisionExpressionEvaluatorType::START_FREE_END_FREE: - { + case CollisionExpressionEvaluatorType::START_FREE_END_FREE: { break; } - case CollisionExpressionEvaluatorType::START_FIXED_END_FREE: - { + case CollisionExpressionEvaluatorType::START_FIXED_END_FREE: { if (r.cc_type[0] == tesseract_collision::ContinuousCollisionType::CCType_Time0) return true; @@ -815,8 +813,7 @@ void CollisionEvaluator::removeInvalidContactResults(tesseract_collision::Contac break; } - case CollisionExpressionEvaluatorType::START_FREE_END_FIXED: - { + case CollisionExpressionEvaluatorType::START_FREE_END_FIXED: { if (r.cc_type[0] == tesseract_collision::ContinuousCollisionType::CCType_Time1) return true; @@ -825,12 +822,10 @@ void CollisionEvaluator::removeInvalidContactResults(tesseract_collision::Contac break; } - case CollisionExpressionEvaluatorType::START_FREE_END_FREE_WEIGHTED_SUM: - { + case CollisionExpressionEvaluatorType::START_FREE_END_FREE_WEIGHTED_SUM: { break; } - case CollisionExpressionEvaluatorType::START_FIXED_END_FREE_WEIGHTED_SUM: - { + case CollisionExpressionEvaluatorType::START_FIXED_END_FREE_WEIGHTED_SUM: { if (r.cc_type[0] == tesseract_collision::ContinuousCollisionType::CCType_Time0) return true; @@ -839,8 +834,7 @@ void CollisionEvaluator::removeInvalidContactResults(tesseract_collision::Contac break; } - case CollisionExpressionEvaluatorType::START_FREE_END_FIXED_WEIGHTED_SUM: - { + case CollisionExpressionEvaluatorType::START_FREE_END_FIXED_WEIGHTED_SUM: { if (r.cc_type[0] == tesseract_collision::ContinuousCollisionType::CCType_Time1) return true; @@ -849,8 +843,7 @@ void CollisionEvaluator::removeInvalidContactResults(tesseract_collision::Contac break; } - default: - { + default: { PRINT_AND_THROW("Invalid CollisionExpressionEvaluatorType for " "CollisionEvaluator::removeInvalidContactResults!"); } @@ -889,8 +882,7 @@ SingleTimestepCollisionEvaluator::SingleTimestepCollisionEvaluator( switch (evaluator_type_) { - case CollisionExpressionEvaluatorType::SINGLE_TIME_STEP: - { + case CollisionExpressionEvaluatorType::SINGLE_TIME_STEP: { fn_ = std::bind(&SingleTimestepCollisionEvaluator::CalcDistExpressionsSingleTimeStep, this, std::placeholders::_1, @@ -898,8 +890,7 @@ SingleTimestepCollisionEvaluator::SingleTimestepCollisionEvaluator( std::placeholders::_3); break; } - case CollisionExpressionEvaluatorType::SINGLE_TIME_STEP_WEIGHTED_SUM: - { + case CollisionExpressionEvaluatorType::SINGLE_TIME_STEP_WEIGHTED_SUM: { fn_ = std::bind(&SingleTimestepCollisionEvaluator::CalcDistExpressionsSingleTimeStepW, this, std::placeholders::_1, @@ -907,8 +898,7 @@ SingleTimestepCollisionEvaluator::SingleTimestepCollisionEvaluator( std::placeholders::_3); break; } - default: - { + default: { PRINT_AND_THROW("Invalid CollisionExpressionEvaluatorType for SingleTimestepCollisionEvaluator!"); } } @@ -1035,8 +1025,7 @@ DiscreteCollisionEvaluator::DiscreteCollisionEvaluator(tesseract_kinematics::Joi switch (evaluator_type_) { - case CollisionExpressionEvaluatorType::START_FREE_END_FREE: - { + case CollisionExpressionEvaluatorType::START_FREE_END_FREE: { fn_ = std::bind(&DiscreteCollisionEvaluator::CalcDistExpressionsBothFree, this, std::placeholders::_1, @@ -1044,8 +1033,7 @@ DiscreteCollisionEvaluator::DiscreteCollisionEvaluator(tesseract_kinematics::Joi std::placeholders::_3); break; } - case CollisionExpressionEvaluatorType::START_FIXED_END_FREE: - { + case CollisionExpressionEvaluatorType::START_FIXED_END_FREE: { fn_ = std::bind(&DiscreteCollisionEvaluator::CalcDistExpressionsEndFree, this, std::placeholders::_1, @@ -1053,8 +1041,7 @@ DiscreteCollisionEvaluator::DiscreteCollisionEvaluator(tesseract_kinematics::Joi std::placeholders::_3); break; } - case CollisionExpressionEvaluatorType::START_FREE_END_FIXED: - { + case CollisionExpressionEvaluatorType::START_FREE_END_FIXED: { fn_ = std::bind(&DiscreteCollisionEvaluator::CalcDistExpressionsStartFree, this, std::placeholders::_1, @@ -1062,8 +1049,7 @@ DiscreteCollisionEvaluator::DiscreteCollisionEvaluator(tesseract_kinematics::Joi std::placeholders::_3); break; } - case CollisionExpressionEvaluatorType::START_FREE_END_FREE_WEIGHTED_SUM: - { + case CollisionExpressionEvaluatorType::START_FREE_END_FREE_WEIGHTED_SUM: { fn_ = std::bind(&DiscreteCollisionEvaluator::CalcDistExpressionsBothFreeW, this, std::placeholders::_1, @@ -1071,8 +1057,7 @@ DiscreteCollisionEvaluator::DiscreteCollisionEvaluator(tesseract_kinematics::Joi std::placeholders::_3); break; } - case CollisionExpressionEvaluatorType::START_FIXED_END_FREE_WEIGHTED_SUM: - { + case CollisionExpressionEvaluatorType::START_FIXED_END_FREE_WEIGHTED_SUM: { fn_ = std::bind(&DiscreteCollisionEvaluator::CalcDistExpressionsEndFreeW, this, std::placeholders::_1, @@ -1080,8 +1065,7 @@ DiscreteCollisionEvaluator::DiscreteCollisionEvaluator(tesseract_kinematics::Joi std::placeholders::_3); break; } - case CollisionExpressionEvaluatorType::START_FREE_END_FIXED_WEIGHTED_SUM: - { + case CollisionExpressionEvaluatorType::START_FREE_END_FIXED_WEIGHTED_SUM: { fn_ = std::bind(&DiscreteCollisionEvaluator::CalcDistExpressionsStartFreeW, this, std::placeholders::_1, @@ -1089,8 +1073,7 @@ DiscreteCollisionEvaluator::DiscreteCollisionEvaluator(tesseract_kinematics::Joi std::placeholders::_3); break; } - default: - { + default: { PRINT_AND_THROW("Invalid CollisionExpressionEvaluatorType for DiscreteCollisionEvaluator!"); } } @@ -1279,8 +1262,7 @@ CastCollisionEvaluator::CastCollisionEvaluator(tesseract_kinematics::JointGroup: switch (evaluator_type_) { - case CollisionExpressionEvaluatorType::START_FREE_END_FREE: - { + case CollisionExpressionEvaluatorType::START_FREE_END_FREE: { fn_ = std::bind(&CastCollisionEvaluator::CalcDistExpressionsBothFree, this, std::placeholders::_1, @@ -1288,8 +1270,7 @@ CastCollisionEvaluator::CastCollisionEvaluator(tesseract_kinematics::JointGroup: std::placeholders::_3); break; } - case CollisionExpressionEvaluatorType::START_FIXED_END_FREE: - { + case CollisionExpressionEvaluatorType::START_FIXED_END_FREE: { fn_ = std::bind(&CastCollisionEvaluator::CalcDistExpressionsEndFree, this, std::placeholders::_1, @@ -1297,8 +1278,7 @@ CastCollisionEvaluator::CastCollisionEvaluator(tesseract_kinematics::JointGroup: std::placeholders::_3); break; } - case CollisionExpressionEvaluatorType::START_FREE_END_FIXED: - { + case CollisionExpressionEvaluatorType::START_FREE_END_FIXED: { fn_ = std::bind(&CastCollisionEvaluator::CalcDistExpressionsStartFree, this, std::placeholders::_1, @@ -1306,8 +1286,7 @@ CastCollisionEvaluator::CastCollisionEvaluator(tesseract_kinematics::JointGroup: std::placeholders::_3); break; } - case CollisionExpressionEvaluatorType::START_FREE_END_FREE_WEIGHTED_SUM: - { + case CollisionExpressionEvaluatorType::START_FREE_END_FREE_WEIGHTED_SUM: { fn_ = std::bind(&CastCollisionEvaluator::CalcDistExpressionsBothFreeW, this, std::placeholders::_1, @@ -1315,8 +1294,7 @@ CastCollisionEvaluator::CastCollisionEvaluator(tesseract_kinematics::JointGroup: std::placeholders::_3); break; } - case CollisionExpressionEvaluatorType::START_FIXED_END_FREE_WEIGHTED_SUM: - { + case CollisionExpressionEvaluatorType::START_FIXED_END_FREE_WEIGHTED_SUM: { fn_ = std::bind(&CastCollisionEvaluator::CalcDistExpressionsEndFreeW, this, std::placeholders::_1, @@ -1324,8 +1302,7 @@ CastCollisionEvaluator::CastCollisionEvaluator(tesseract_kinematics::JointGroup: std::placeholders::_3); break; } - case CollisionExpressionEvaluatorType::START_FREE_END_FIXED_WEIGHTED_SUM: - { + case CollisionExpressionEvaluatorType::START_FREE_END_FIXED_WEIGHTED_SUM: { fn_ = std::bind(&CastCollisionEvaluator::CalcDistExpressionsStartFreeW, this, std::placeholders::_1, @@ -1333,8 +1310,7 @@ CastCollisionEvaluator::CastCollisionEvaluator(tesseract_kinematics::JointGroup: std::placeholders::_3); break; } - default: - { + default: { PRINT_AND_THROW("Invalid CollisionExpressionEvaluatorType for CastCollisionEvaluator!"); } } diff --git a/trajopt_ext/vhacd/src/btConvexHullComputer.cpp b/trajopt_ext/vhacd/src/btConvexHullComputer.cpp index bf3dfb58..e155661a 100644 --- a/trajopt_ext/vhacd/src/btConvexHullComputer.cpp +++ b/trajopt_ext/vhacd/src/btConvexHullComputer.cpp @@ -115,8 +115,8 @@ class btConvexHullInternal Int128 result; __asm__("addq %[bl], %[rl]\n\t" "adcq %[bh], %[rh]\n\t" - : [ rl ] "=r"(result.low), [ rh ] "=r"(result.high) - : "0"(low), "1"(high), [ bl ] "g"(b.low), [ bh ] "g"(b.high) + : [rl] "=r"(result.low), [rh] "=r"(result.high) + : "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high) : "cc"); return result; #else @@ -131,8 +131,8 @@ class btConvexHullInternal Int128 result; __asm__("subq %[bl], %[rl]\n\t" "sbbq %[bh], %[rh]\n\t" - : [ rl ] "=r"(result.low), [ rh ] "=r"(result.high) - : "0"(low), "1"(high), [ bl ] "g"(b.low), [ bh ] "g"(b.high) + : [rl] "=r"(result.low), [rh] "=r"(result.high) + : "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high) : "cc"); return result; #else @@ -145,8 +145,8 @@ class btConvexHullInternal #ifdef USE_X86_64_ASM __asm__("addq %[bl], %[rl]\n\t" "adcq %[bh], %[rh]\n\t" - : [ rl ] "=r"(low), [ rh ] "=r"(high) - : "0"(low), "1"(high), [ bl ] "g"(b.low), [ bh ] "g"(b.high) + : [rl] "=r"(low), [rh] "=r"(high) + : "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high) : "cc"); #else uint64_t lo = low + b.low; @@ -705,7 +705,7 @@ btConvexHullInternal::Int128 btConvexHullInternal::Int128::mul(int64_t a, int64_ Int128 result; #ifdef USE_X86_64_ASM - __asm__("imulq %[b]" : "=a"(result.low), "=d"(result.high) : "0"(a), [ b ] "r"(b) : "cc"); + __asm__("imulq %[b]" : "=a"(result.low), "=d"(result.high) : "0"(a), [b] "r"(b) : "cc"); return result; #else @@ -729,7 +729,7 @@ btConvexHullInternal::Int128 btConvexHullInternal::Int128::mul(uint64_t a, uint6 Int128 result; #ifdef USE_X86_64_ASM - __asm__("mulq %[b]" : "=a"(result.low), "=d"(result.high) : "0"(a), [ b ] "r"(b) : "cc"); + __asm__("mulq %[b]" : "=a"(result.low), "=d"(result.high) : "0"(a), [b] "r"(b) : "cc"); #else DMul::mul(a, b, result.low, result.high); @@ -771,8 +771,8 @@ int32_t btConvexHullInternal::Rational64::compare(const Rational64& b) const "decb %%bh\n\t" // now bx=0x0000 if difference is zero, 0xff01 if it is negative, 0x0001 if it is positive // (i.e., same sign as difference) "shll $16, %%ebx\n\t" // ebx has same sign as difference - : "=&b"(result), [ tmp ] "=&r"(tmp), "=a"(dummy) - : "a"(denominator), [ bn ] "g"(b.numerator), [ tn ] "g"(numerator), [ bd ] "g"(b.denominator) + : "=&b"(result), [tmp] "=&r"(tmp), "=a"(dummy) + : "a"(denominator), [bn] "g"(b.numerator), [tn] "g"(numerator), [bd] "g"(b.denominator) : "%rdx", "cc"); return result ? result ^ sign // if sign is +1, only bit 0 of result is inverted, which does not change the sign of // result (and cannot result in zero) @@ -1063,8 +1063,7 @@ void btConvexHullInternal::computeInternal(int32_t start, int32_t end, Intermedi result.minYx = NULL; result.maxYx = NULL; return; - case 2: - { + case 2: { Vertex* v = originalVertices[start]; Vertex* w = v + 1; if (v->point != w->point) @@ -1130,8 +1129,7 @@ void btConvexHullInternal::computeInternal(int32_t start, int32_t end, Intermedi } } // lint -fallthrough - case 1: - { + case 1: { Vertex* v = originalVertices[start]; v->edges = NULL; v->next = v; diff --git a/trajopt_ifopt/src/constraints/cartesian_position_constraint.cpp b/trajopt_ifopt/src/constraints/cartesian_position_constraint.cpp index 0893bdda..d68977a2 100644 --- a/trajopt_ifopt/src/constraints/cartesian_position_constraint.cpp +++ b/trajopt_ifopt/src/constraints/cartesian_position_constraint.cpp @@ -93,8 +93,7 @@ CartPosConstraint::CartPosConstraint(CartPosInfo info, switch (info_.type) { - case CartPosInfo::Type::TARGET_ACTIVE: - { + case CartPosInfo::Type::TARGET_ACTIVE: { error_function_ = [this](const Eigen::Isometry3d& target_tf, const Eigen::Isometry3d& source_tf) -> Eigen::VectorXd { Eigen::VectorXd err = tesseract_common::calcTransformError(source_tf, target_tf); @@ -125,8 +124,7 @@ CartPosConstraint::CartPosConstraint(CartPosInfo info, break; } - case CartPosInfo::Type::SOURCE_ACTIVE: - { + case CartPosInfo::Type::SOURCE_ACTIVE: { error_function_ = [this](const Eigen::Isometry3d& target_tf, const Eigen::Isometry3d& source_tf) -> Eigen::VectorXd { Eigen::VectorXd err = tesseract_common::calcTransformError(target_tf, source_tf); @@ -156,8 +154,7 @@ CartPosConstraint::CartPosConstraint(CartPosInfo info, }; break; } - case CartPosInfo::Type::BOTH_ACTIVE: - { + case CartPosInfo::Type::BOTH_ACTIVE: { error_function_ = [this](const Eigen::Isometry3d& target_tf, const Eigen::Isometry3d& source_tf) -> Eigen::VectorXd { Eigen::VectorXd err = tesseract_common::calcTransformError(target_tf, source_tf); diff --git a/trajopt_optimizers/trajopt_sqp/src/ifopt_qp_problem.cpp b/trajopt_optimizers/trajopt_sqp/src/ifopt_qp_problem.cpp index f049ddf1..c585631b 100644 --- a/trajopt_optimizers/trajopt_sqp/src/ifopt_qp_problem.cpp +++ b/trajopt_optimizers/trajopt_sqp/src/ifopt_qp_problem.cpp @@ -45,16 +45,14 @@ void IfoptQPProblem::addCostSet(ifopt::ConstraintSet::Ptr constraint_set, CostPe { switch (penalty_type) { - case CostPenaltyType::SQUARED: - { + case CostPenaltyType::SQUARED: { // Must link the variables to the constraint since that happens in AddConstraintSet constraint_set->LinkWithVariables(nlp_->GetOptVariables()); auto cost = std::make_shared(constraint_set); nlp_->AddCostSet(cost); break; } - case CostPenaltyType::ABSOLUTE: - { + case CostPenaltyType::ABSOLUTE: { // Must link the variables to the constraint since that happens in AddConstraintSet constraint_set->LinkWithVariables(nlp_->GetOptVariables()); auto cost = std::make_shared(constraint_set); diff --git a/trajopt_optimizers/trajopt_sqp/src/trajopt_qp_problem.cpp b/trajopt_optimizers/trajopt_sqp/src/trajopt_qp_problem.cpp index 7cb90124..f4fe34b1 100644 --- a/trajopt_optimizers/trajopt_sqp/src/trajopt_qp_problem.cpp +++ b/trajopt_optimizers/trajopt_sqp/src/trajopt_qp_problem.cpp @@ -60,8 +60,7 @@ void TrajOptQPProblem::addCostSet(ifopt::ConstraintSet::Ptr constraint_set, Cost std::vector cost_bounds = constraint_set->GetBounds(); switch (penalty_type) { - case CostPenaltyType::SQUARED: - { + case CostPenaltyType::SQUARED: { for (const auto& bound : cost_bounds) { if (!trajopt_ifopt::isBoundsEquality(bound)) @@ -71,8 +70,7 @@ void TrajOptQPProblem::addCostSet(ifopt::ConstraintSet::Ptr constraint_set, Cost squared_costs_.AddComponent(constraint_set); break; } - case CostPenaltyType::ABSOLUTE: - { + case CostPenaltyType::ABSOLUTE: { for (const auto& bound : cost_bounds) { if (!trajopt_ifopt::isBoundsEquality(bound)) @@ -82,8 +80,7 @@ void TrajOptQPProblem::addCostSet(ifopt::ConstraintSet::Ptr constraint_set, Cost abs_costs_.AddComponent(constraint_set); break; } - case CostPenaltyType::HINGE: - { + case CostPenaltyType::HINGE: { for (const auto& bound : cost_bounds) { if (!trajopt_ifopt::isBoundsInEquality(bound)) @@ -93,8 +90,7 @@ void TrajOptQPProblem::addCostSet(ifopt::ConstraintSet::Ptr constraint_set, Cost hinge_costs_.AddComponent(constraint_set); break; } - default: - { + default: { throw std::runtime_error("Unsupport CostPenaltyType!"); } } diff --git a/trajopt_sco/include/trajopt_sco/bpmpd_io.hpp b/trajopt_sco/include/trajopt_sco/bpmpd_io.hpp index 81135583..ea541b24 100644 --- a/trajopt_sco/include/trajopt_sco/bpmpd_io.hpp +++ b/trajopt_sco/include/trajopt_sco/bpmpd_io.hpp @@ -22,16 +22,14 @@ void ser(int fp, T& x, SerMode mode) { switch (mode) { - case SER: - { + case SER: { T xcopy = x; ssize_t n = write(fp, &xcopy, sizeof(T)); assert(n == sizeof(T)); UNUSED(n); break; } - case DESER: - { + case DESER: { ssize_t n = read(fp, &x, sizeof(T)); assert(n == sizeof(T)); UNUSED(n); @@ -47,15 +45,13 @@ void ser(int fp, std::vector& x, SerMode mode) ser(fp, size, mode); switch (mode) { - case SER: - { + case SER: { long n = write(fp, x.data(), sizeof(T) * size); assert(static_cast(n) == sizeof(T) * size); UNUSED(n); break; } - case DESER: - { + case DESER: { x.resize(size); long n = read(fp, x.data(), sizeof(T) * size); assert(static_cast(n) == sizeof(T) * size); diff --git a/trajopt_sco/include/trajopt_sco/osqp_interface.hpp b/trajopt_sco/include/trajopt_sco/osqp_interface.hpp index d2d1b78f..fada8afc 100644 --- a/trajopt_sco/include/trajopt_sco/osqp_interface.hpp +++ b/trajopt_sco/include/trajopt_sco/osqp_interface.hpp @@ -60,17 +60,17 @@ class OSQPModel : public Model ConstraintTypeVector cnt_types_; /**< constraints types */ DblVec solution_; /**< optimizizer's solution for current model */ - std::unique_ptr P_; /**< Takes ownership of OSQPData.P to avoid having to deallocate manually */ - std::unique_ptr A_; /**< Takes ownership of OSQPData.A to avoid having to deallocate manually */ - std::vector P_row_indices_; /**< row indices for P, CSC format */ - std::vector P_column_pointers_; /**< column pointers for P, CSC format */ - DblVec P_csc_data_; /**< P values in CSC format */ - Eigen::VectorXd q_; /**< linear part of the objective */ - - std::vector A_row_indices_; /**< row indices for constraint matrix, CSC format */ - std::vector A_column_pointers_; /**< column pointers for constraint matrix, CSC format */ - DblVec A_csc_data_; /**< constraint matrix values in CSC format */ - DblVec l_, u_; /**< linear constraints upper and lower limits */ + std::unique_ptr P_; /**< Takes ownership of OSQPData.P to avoid having to deallocate manually */ + std::unique_ptr A_; /**< Takes ownership of OSQPData.A to avoid having to deallocate manually */ + std::vector P_row_indices_; /**< row indices for P, CSC format */ + std::vector P_column_pointers_; /**< column pointers for P, CSC format */ + DblVec P_csc_data_; /**< P values in CSC format */ + Eigen::VectorXd q_; /**< linear part of the objective */ + + std::vector A_row_indices_; /**< row indices for constraint matrix, CSC format */ + std::vector A_column_pointers_; /**< column pointers for constraint matrix, CSC format */ + DblVec A_csc_data_; /**< constraint matrix values in CSC format */ + DblVec l_, u_; /**< linear constraints upper and lower limits */ QuadExpr objective_; /**< objective QuadExpr expression */ diff --git a/trajopt_sco/src/modeling_utils.cpp b/trajopt_sco/src/modeling_utils.cpp index a39e1e31..2699c210 100644 --- a/trajopt_sco/src/modeling_utils.cpp +++ b/trajopt_sco/src/modeling_utils.cpp @@ -184,21 +184,18 @@ ConvexObjective::Ptr CostFromErrFunc::convex(const DblVec& x, Model* model) } switch (pen_type_) { - case SQUARED: - { + case SQUARED: { QuadExpr quad = exprSquare(aff); exprScale(quad, weight); out->addQuadExpr(quad); break; } - case ABS: - { + case ABS: { exprScale(aff, weight); out->addAbs(aff, 1); break; } - case HINGE: - { + case HINGE: { exprScale(aff, weight); out->addHinge(aff, 1); break;