Skip to content

Commit

Permalink
Trajopt clang-tidy clean-up v2
Browse files Browse the repository at this point in the history
  • Loading branch information
rjoomen authored and Levi-Armstrong committed Jul 15, 2024
1 parent 56d8d46 commit 4a908dd
Show file tree
Hide file tree
Showing 32 changed files with 169 additions and 171 deletions.
3 changes: 2 additions & 1 deletion trajopt/include/trajopt/collision_terms.hpp
Original file line number Diff line number Diff line change
@@ -1,4 +1,5 @@
#pragma once
#include <cstdint>
#include <vector>
#include <memory>
#include <Eigen/Core>
Expand All @@ -24,7 +25,7 @@ using ContactResultVectorConstPtr = std::shared_ptr<const ContactResultVectorWra
/**
* @brief This contains the different types of expression evaluators used when performing continuous collision checking.
*/
enum class CollisionExpressionEvaluatorType
enum class CollisionExpressionEvaluatorType : std::uint8_t
{
START_FREE_END_FREE = 0, /**< @brief Both start and end state variables are free to be adjusted */
START_FREE_END_FIXED = 1, /**< @brief Only start state variables are free to be adjusted */
Expand Down
6 changes: 4 additions & 2 deletions trajopt/include/trajopt/fwd.hpp
Original file line number Diff line number Diff line change
@@ -1,10 +1,12 @@
#ifndef TRAJOPT_FWD_HPP
#define TRAJOPT_FWD_HPP

#include <cstdint>

namespace trajopt
{
// collision_terms.hpp
enum class CollisionExpressionEvaluatorType;
enum class CollisionExpressionEvaluatorType : std::uint8_t;
struct LinkGradientResults;
struct GradientResults;
struct CollisionEvaluator;
Expand All @@ -25,7 +27,7 @@ struct JointPosTermInfo;
struct JointVelTermInfo;
struct JointAccTermInfo;
struct JointJerkTermInfo;
enum class CollisionEvaluatorType;
enum class CollisionEvaluatorType : std::uint8_t;
struct CollisionTermInfo;
struct TotalTimeTermInfo;
struct AvoidSingularityTermInfo;
Expand Down
2 changes: 1 addition & 1 deletion trajopt/include/trajopt/json_marshal.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,7 @@ TRAJOPT_IGNORE_WARNINGS_POP
namespace Json
{
class Value;
}
} // namespace Json

namespace json_marshal
{
Expand Down
6 changes: 3 additions & 3 deletions trajopt/include/trajopt/problem_description.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,7 @@ TRAJOPT_IGNORE_WARNINGS_POP
namespace sco
{
struct OptResults;
}
} // namespace sco

namespace trajopt
{
Expand Down Expand Up @@ -169,7 +169,7 @@ struct InitInfo
In all cases the dt column (if present) is appended the selected method is defined.
*/
enum Type
enum Type : std::uint8_t
{
STATIONARY,
JOINT_INTERPOLATED,
Expand Down Expand Up @@ -581,7 +581,7 @@ struct JointJerkTermInfo : public TermInfo
JointJerkTermInfo() : TermInfo(TermType::TT_COST | TermType::TT_CNT) {}
};

enum class CollisionEvaluatorType
enum class CollisionEvaluatorType : std::uint8_t
{
SINGLE_TIMESTEP = 0,
DISCRETE_CONTINUOUS = 1,
Expand Down
8 changes: 4 additions & 4 deletions trajopt/src/collision_terms.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -809,7 +809,7 @@ void CollisionEvaluator::removeInvalidContactResults(tesseract_collision::Contac
const std::array<double, 2>& pair_data) const
{
auto end = std::remove_if(
contact_results.begin(), contact_results.end(), [=, &pair_data](const tesseract_collision::ContactResult& r) {
contact_results.begin(), contact_results.end(), [this, &pair_data](const tesseract_collision::ContactResult& r) {
switch (evaluator_type_)
{
case CollisionExpressionEvaluatorType::START_FREE_END_FREE:
Expand Down Expand Up @@ -1010,7 +1010,7 @@ void SingleTimestepCollisionEvaluator::Plot(const std::shared_ptr<tesseract_visu
}
}

auto margin_fn = [=](const std::string& link1, const std::string& link2) {
auto margin_fn = [this](const std::string& link1, const std::string& link2) {
return getSafetyMarginData()->getPairSafetyMarginData(link1, link2)[0];
};

Expand Down Expand Up @@ -1256,7 +1256,7 @@ void DiscreteCollisionEvaluator::Plot(const std::shared_ptr<tesseract_visualizat
}
}

auto margin_fn = [=](const std::string& link1, const std::string& link2) {
auto margin_fn = [this](const std::string& link1, const std::string& link2) {
return getSafetyMarginData()->getPairSafetyMarginData(link1, link2)[0];
};

Expand Down Expand Up @@ -1508,7 +1508,7 @@ void CastCollisionEvaluator::Plot(const std::shared_ptr<tesseract_visualization:
}
}

auto margin_fn = [=](const std::string& link1, const std::string& link2) {
auto margin_fn = [this](const std::string& link1, const std::string& link2) {
return getSafetyMarginData()->getPairSafetyMarginData(link1, link2)[0];
};

Expand Down
6 changes: 3 additions & 3 deletions trajopt/src/file_write_callback.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -88,9 +88,9 @@ void WriteFile(const std::shared_ptr<std::ofstream>& file,
*file << ',' << constraint;
}

*file << std::endl;
*file << '\n';
}
*file << std::endl;
*file << '\n' << std::flush;
} // namespace trajopt

std::function<void(sco::OptProb*, sco::OptResults&)> WriteCallback(std::shared_ptr<std::ofstream> file,
Expand Down Expand Up @@ -133,7 +133,7 @@ std::function<void(sco::OptProb*, sco::OptResults&)> WriteCallback(std::shared_p
*file << ',' << cnt->name();
}

*file << std::endl;
*file << '\n' << std::flush;

// return callback function
return [file, capture0 = prob->GetKin(), &capture1 = prob->GetVars()](auto&&, auto&& PH2) {
Expand Down
2 changes: 1 addition & 1 deletion trajopt/src/problem_description.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -243,7 +243,7 @@ void ProblemConstructionInfo::readInitInfo(const Json::Value& v)
for (int i = 0; i < n_steps; ++i)
{
DblVec row;
json_marshal::fromJsonArray(vdata[i], row, static_cast<int>(n_dof));
json_marshal::fromJsonArray(vdata[i], row, n_dof);
init_info.data.row(i) = trajopt_common::toVectorXd(row);
}
}
Expand Down
1 change: 0 additions & 1 deletion trajopt_common/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -26,7 +26,6 @@ set(UTILS_SOURCE_FILES
src/clock.cpp
src/collision_types.cpp
src/collision_utils.cpp
src/config.cpp
src/logging.cpp
src/utils.cpp)

Expand Down
3 changes: 2 additions & 1 deletion trajopt_common/include/trajopt_common/logging.hpp
Original file line number Diff line number Diff line change
@@ -1,12 +1,13 @@
#pragma once
#include <trajopt_common/macros.h>
#include <cstdint>
TRAJOPT_IGNORE_WARNINGS_PUSH
#include <cstdio>
TRAJOPT_IGNORE_WARNINGS_POP

namespace trajopt_common
{
enum LogLevel
enum LogLevel : std::uint8_t
{
LevelFatal = 0,
LevelError = 1,
Expand Down
10 changes: 0 additions & 10 deletions trajopt_common/include/trajopt_common/math.hpp

This file was deleted.

4 changes: 2 additions & 2 deletions trajopt_common/src/config.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -19,8 +19,8 @@ void CommandParser::read(int argc, char* argv[])
po::store(po::command_line_parser(argc, argv).options(od).run(), vm);
if (vm.count("help") != 0U)
{
std::cout << "usage: " << argv[0] << " [options]" << std::endl;
std::cout << od << std::endl;
std::cout << "usage: " << argv[0] << " [options]" << '\n';
std::cout << od << '\n';
exit(0);
}
po::notify(vm);
Expand Down
2 changes: 1 addition & 1 deletion trajopt_common/src/logging.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,7 @@ int LoggingInit()
{
const char* VALID_THRESH_VALUES = "FATAL ERROR WARN INFO DEBUG TRACE";

char* lvlc = getenv("TRAJOPT_LOG_THRESH");
const char* lvlc = getenv("TRAJOPT_LOG_THRESH");
std::string lvlstr;
if (lvlc == nullptr)
{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -46,7 +46,7 @@ struct CartPosInfo
using Ptr = std::shared_ptr<CartPosInfo>;
using ConstPtr = std::shared_ptr<const CartPosInfo>;

enum class Type
enum class Type : std::uint8_t
{
TARGET_ACTIVE,
SOURCE_ACTIVE,
Expand Down
10 changes: 6 additions & 4 deletions trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/fwd.h
Original file line number Diff line number Diff line change
@@ -1,12 +1,14 @@
#ifndef TRAJOPT_SQP_FWD_H
#define TRAJOPT_SQP_FWD_H

#include <cstdint>

namespace trajopt_sqp
{
// types.h
enum class ConstraintType;
enum class CostPenaltyType;
enum class SQPStatus;
enum class ConstraintType : std::uint8_t;
enum class CostPenaltyType : std::uint8_t;
enum class SQPStatus : std::uint8_t;
struct SQPParameters;
struct SQPResults;

Expand All @@ -19,7 +21,7 @@ struct QuadExprs;
class QPProblem;

// qp_solver.h
enum class QPSolverStatus;
enum class QPSolverStatus : std::uint8_t;
class QPSolver;

// osqp_eigen_solver.h
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,7 @@
namespace ifopt
{
class Problem;
}
} // namespace ifopt

namespace trajopt_sqp
{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,7 @@
namespace OsqpEigen
{
class Solver;
}
} // namespace OsqpEigen

namespace trajopt_sqp
{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@ class ConstraintSet;

namespace trajopt_sqp
{
enum class CostPenaltyType;
enum class CostPenaltyType : std::uint8_t;

/** @brief QP Problem Base */
class QPProblem
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,7 @@

namespace trajopt_sqp
{
enum class QPSolverStatus
enum class QPSolverStatus : std::uint8_t
{
UNITIALIZED,
INITIALIZED,
Expand Down
6 changes: 3 additions & 3 deletions trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/types.h
Original file line number Diff line number Diff line change
Expand Up @@ -30,13 +30,13 @@

namespace trajopt_sqp
{
enum class ConstraintType
enum class ConstraintType : std::uint8_t
{
EQ,
INEQ
};

enum class CostPenaltyType
enum class CostPenaltyType : std::uint8_t
{
SQUARED,
ABSOLUTE,
Expand Down Expand Up @@ -157,7 +157,7 @@ struct SQPResults
/**
* @brief Status codes for the SQP Optimization
*/
enum class SQPStatus
enum class SQPStatus : std::uint8_t
{
RUNNING, /**< Optimization is currently running */
NLP_CONVERGED, /**< NLP Successfully converged */
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,7 @@ CollisionPlottingCallback::CollisionPlottingCallback(std::shared_ptr<tesseract_v

void CollisionPlottingCallback::plot(const QPProblem& /*problem*/) // NOLINT Remove after implementation
{
std::cout << "Collision plotting has not been implemented. PRs welcome" << std::endl;
std::cout << "Collision plotting has not been implemented. PRs welcome" << '\n';
}

void CollisionPlottingCallback::addConstraintSet(
Expand Down
26 changes: 13 additions & 13 deletions trajopt_optimizers/trajopt_sqp/src/ifopt_qp_problem.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -501,23 +501,23 @@ void IfoptQPProblem::print() const
{
Eigen::IOFormat format(3);

std::cout << "-------------- QPProblem::print() --------------" << std::endl;
std::cout << "Num NLP Vars: " << num_nlp_vars_ << std::endl;
std::cout << "Num QP Vars: " << num_qp_vars_ << std::endl;
std::cout << "Num NLP Constraints: " << num_qp_cnts_ << std::endl;
std::cout << "-------------- QPProblem::print() --------------" << '\n';
std::cout << "Num NLP Vars: " << num_nlp_vars_ << '\n';
std::cout << "Num QP Vars: " << num_qp_vars_ << '\n';
std::cout << "Num NLP Constraints: " << num_qp_cnts_ << '\n';
std::cout << "Detected Constraint Type: ";
for (const auto& cnt : constraint_types_)
std::cout << static_cast<int>(cnt) << ", ";

std::cout << std::endl;
std::cout << "box_size_: " << box_size_.transpose().format(format) << std::endl; // NOLINT
std::cout << "constraint_merit_coeff_: " << constraint_merit_coeff_.transpose().format(format) << std::endl;
std::cout << '\n';
std::cout << "box_size_: " << box_size_.transpose().format(format) << '\n'; // NOLINT
std::cout << "constraint_merit_coeff_: " << constraint_merit_coeff_.transpose().format(format) << '\n';

std::cout << "Hessian:\n" << hessian_.toDense().format(format) << std::endl;
std::cout << "Gradient: " << gradient_.transpose().format(format) << std::endl;
std::cout << "Constraint Matrix:\n" << constraint_matrix_.toDense().format(format) << std::endl;
std::cout << "bounds_lower: " << bounds_lower_.transpose().format(format) << std::endl;
std::cout << "bounds_upper: " << bounds_upper_.transpose().format(format) << std::endl;
std::cout << "NLP values: " << nlp_->GetVariableValues().transpose().format(format) << std::endl;
std::cout << "Hessian:\n" << hessian_.toDense().format(format) << '\n';
std::cout << "Gradient: " << gradient_.transpose().format(format) << '\n';
std::cout << "Constraint Matrix:\n" << constraint_matrix_.toDense().format(format) << '\n';
std::cout << "bounds_lower: " << bounds_lower_.transpose().format(format) << '\n';
std::cout << "bounds_upper: " << bounds_upper_.transpose().format(format) << '\n';
std::cout << "NLP values: " << nlp_->GetVariableValues().transpose().format(format) << '\n';
}
} // namespace trajopt_sqp
Loading

0 comments on commit 4a908dd

Please sign in to comment.