Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Fix mismatched delete/free in OSQPModel #398

Closed
wants to merge 2 commits into from
Closed

Fix mismatched delete/free in OSQPModel #398

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
cf32e19
Fix mismatched delete/free in OSQPModel
Jack-ReframeSystems Apr 10, 2024
10082c9
Clang format 14 formatting
Jack-ReframeSystems Apr 12, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion .run-clang-format
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,2 +1,2 @@
#!/bin/bash
find . -type f -regex '.*\.\(cpp\|hpp\|cc\|cxx\|h\|hxx\)' -exec clang-format-8 -style=file -i {} \;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Just for consistency please revert change.

find . -type f -regex '.*\.\(cpp\|hpp\|cc\|cxx\|h\|hxx\)' -exec clang-format -style=file -i {} \;
72 changes: 24 additions & 48 deletions trajopt/src/collision_terms.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand All @@ -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;

Expand All @@ -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;

Expand All @@ -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;

Expand All @@ -849,8 +843,7 @@ void CollisionEvaluator::removeInvalidContactResults(tesseract_collision::Contac

break;
}
default:
{
default: {
PRINT_AND_THROW("Invalid CollisionExpressionEvaluatorType for "
"CollisionEvaluator::removeInvalidContactResults!");
}
Expand Down Expand Up @@ -889,26 +882,23 @@ SingleTimestepCollisionEvaluator::SingleTimestepCollisionEvaluator(

switch (evaluator_type_)
{
case CollisionExpressionEvaluatorType::SINGLE_TIME_STEP:
{
case CollisionExpressionEvaluatorType::SINGLE_TIME_STEP: {
fn_ = std::bind(&SingleTimestepCollisionEvaluator::CalcDistExpressionsSingleTimeStep,
this,
std::placeholders::_1,
std::placeholders::_2,
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,
std::placeholders::_2,
std::placeholders::_3);
break;
}
default:
{
default: {
PRINT_AND_THROW("Invalid CollisionExpressionEvaluatorType for SingleTimestepCollisionEvaluator!");
}
}
Expand Down Expand Up @@ -1035,62 +1025,55 @@ 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,
std::placeholders::_2,
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,
std::placeholders::_2,
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,
std::placeholders::_2,
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,
std::placeholders::_2,
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,
std::placeholders::_2,
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,
std::placeholders::_2,
std::placeholders::_3);
break;
}
default:
{
default: {
PRINT_AND_THROW("Invalid CollisionExpressionEvaluatorType for DiscreteCollisionEvaluator!");
}
}
Expand Down Expand Up @@ -1279,62 +1262,55 @@ 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,
std::placeholders::_2,
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,
std::placeholders::_2,
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,
std::placeholders::_2,
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,
std::placeholders::_2,
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,
std::placeholders::_2,
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,
std::placeholders::_2,
std::placeholders::_3);
break;
}
default:
{
default: {
PRINT_AND_THROW("Invalid CollisionExpressionEvaluatorType for CastCollisionEvaluator!");
}
}
Expand Down
26 changes: 12 additions & 14 deletions trajopt_ext/vhacd/src/btConvexHullComputer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand All @@ -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;
Expand Down Expand Up @@ -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
Expand All @@ -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<uint64_t, uint32_t>::mul(a, b, result.low, result.high);
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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;
Expand Down
9 changes: 3 additions & 6 deletions trajopt_ifopt/src/constraints/cartesian_position_constraint.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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);
Expand Down
6 changes: 2 additions & 4 deletions trajopt_optimizers/trajopt_sqp/src/ifopt_qp_problem.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<trajopt_ifopt::SquaredCost>(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<trajopt_ifopt::AbsoluteCost>(constraint_set);
Expand Down
Loading
Loading