diff --git a/.clang-tidy b/.clang-tidy index 94194d7c..4fdbe459 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -1,78 +1,76 @@ --- Checks: > -*, - clang-diagnostic-*, - -clang-diagnostic-unknown-warning-option, - clang-analyzer-*, - -clang-analyzer-cplusplus*, bugprone-*, -bugprone-easily-swappable-parameters, - -bugprone-exception-escape, + clang-analyzer-*, + -clang-analyzer-cplusplus*, + clang-diagnostic-*, cppcoreguidelines-*, - -cppcoreguidelines-macro-usage, - -cppcoreguidelines-pro-type-static-cast-downcast, - -cppcoreguidelines-pro-type-vararg, - -cppcoreguidelines-pro-type-union-access, - -cppcoreguidelines-pro-bounds-array-to-pointer-decay, - -cppcoreguidelines-pro-bounds-pointer-arithmetic, - -cppcoreguidelines-pro-bounds-constant-array-index, + -cppcoreguidelines-avoid-const-or-ref-data-members, -cppcoreguidelines-avoid-magic-numbers, + -cppcoreguidelines-macro-usage, -cppcoreguidelines-non-private-member-variables-in-classes, -cppcoreguidelines-owning-memory, + -cppcoreguidelines-pro-bounds-array-to-pointer-decay, + -cppcoreguidelines-pro-bounds-constant-array-index, + -cppcoreguidelines-pro-bounds-pointer-arithmetic, + -cppcoreguidelines-pro-type-static-cast-downcast, + -cppcoreguidelines-pro-type-union-access, + -cppcoreguidelines-pro-type-vararg, misc-*, -misc-include-cleaner, - -misc-non-private-member-variables-in-classes, -misc-no-recursion, + -misc-non-private-member-variables-in-classes, modernize-*, - -modernize-use-trailing-return-type, -modernize-use-nodiscard, + -modernize-use-trailing-return-type, performance-*, readability-*, + -readability-avoid-unconditional-preprocessor-if, -readability-braces-around-statements, - -readability-named-parameter, - -readability-magic-numbers, - -readability-isolate-declaration, -readability-function-cognitive-complexity, - -readability-use-anyofallof, -readability-identifier-length, - -readability-suspicious-call-argument + -readability-magic-numbers, + -readability-named-parameter, + -readability-suspicious-call-argument, WarningsAsErrors: > -*, - clang-diagnostic-*, - -clang-diagnostic-unknown-warning-option, - clang-analyzer-*, - -clang-analyzer-cplusplus*, bugprone-*, -bugprone-easily-swappable-parameters, - -bugprone-exception-escape, + clang-analyzer-*, + -clang-analyzer-core.uninitialized.UndefReturn, + -clang-analyzer-cplusplus*, + -clang-analyzer-optin.core.EnumCastOutOfRange, + clang-diagnostic-*, cppcoreguidelines-*, - -cppcoreguidelines-macro-usage, - -cppcoreguidelines-pro-type-static-cast-downcast, - -cppcoreguidelines-pro-type-vararg, - -cppcoreguidelines-pro-type-union-access, - -cppcoreguidelines-pro-bounds-array-to-pointer-decay, - -cppcoreguidelines-pro-bounds-pointer-arithmetic, - -cppcoreguidelines-pro-bounds-constant-array-index, + -cppcoreguidelines-avoid-const-or-ref-data-members, -cppcoreguidelines-avoid-magic-numbers, + -cppcoreguidelines-macro-usage, -cppcoreguidelines-non-private-member-variables-in-classes, -cppcoreguidelines-owning-memory, + -cppcoreguidelines-pro-bounds-array-to-pointer-decay, + -cppcoreguidelines-pro-bounds-constant-array-index, + -cppcoreguidelines-pro-bounds-pointer-arithmetic, + -cppcoreguidelines-pro-type-static-cast-downcast, + -cppcoreguidelines-pro-type-union-access, + -cppcoreguidelines-pro-type-vararg, misc-*, -misc-include-cleaner, - -misc-non-private-member-variables-in-classes, -misc-no-recursion, + -misc-non-private-member-variables-in-classes, modernize-*, - -modernize-use-trailing-return-type, -modernize-use-nodiscard, + -modernize-use-trailing-return-type, performance-*, readability-*, + -readability-avoid-unconditional-preprocessor-if, -readability-braces-around-statements, - -readability-named-parameter, - -readability-magic-numbers, - -readability-isolate-declaration, - -readability-function-cognitive-complexity - -readability-use-anyofallof, + -readability-function-cognitive-complexity, -readability-identifier-length, - -readability-suspicious-call-argument + -readability-magic-numbers, + -readability-named-parameter, + -readability-suspicious-call-argument, HeaderFilterRegex: '.*' FormatStyle: none CheckOptions: diff --git a/trajopt/include/trajopt/problem_description.hpp b/trajopt/include/trajopt/problem_description.hpp index ce2b25c4..bae5aa73 100644 --- a/trajopt/include/trajopt/problem_description.hpp +++ b/trajopt/include/trajopt/problem_description.hpp @@ -35,25 +35,25 @@ enum class TermType : char inline TermType operator|(TermType lhs, TermType rhs) { - using T = std::underlying_type::type; + using T = std::underlying_type_t; return TermType(static_cast(lhs) | static_cast(rhs)); } inline TermType operator&(TermType lhs, TermType rhs) { - using T = std::underlying_type::type; + using T = std::underlying_type_t; return TermType(static_cast(lhs) & static_cast(rhs)); } inline TermType operator^(TermType lhs, TermType rhs) { - using T = std::underlying_type::type; + using T = std::underlying_type_t; return TermType(static_cast(lhs) ^ static_cast(rhs)); } inline TermType operator~(TermType rhs) { - using T = std::underlying_type::type; + using T = std::underlying_type_t; return TermType(~static_cast(rhs)); } diff --git a/trajopt/include/trajopt/typedefs.hpp b/trajopt/include/trajopt/typedefs.hpp index f0ab30d8..d42da492 100644 --- a/trajopt/include/trajopt/typedefs.hpp +++ b/trajopt/include/trajopt/typedefs.hpp @@ -84,7 +84,7 @@ class TrajOptCostFromErrFunc : public sco::CostFromErrFunc, public Plotter // If error function has a inherited from TrajOptVectorOfVector, call its Plot function if (auto* plt = dynamic_cast(f_.get())) { - Eigen::VectorXd dof_vals = sco::getVec(x, vars_); + const Eigen::VectorXd dof_vals = sco::getVec(x, vars_); plt->Plot(plotter, dof_vals); } } @@ -120,7 +120,7 @@ class TrajOptConstraintFromErrFunc : public sco::ConstraintFromErrFunc, public P // If error function has a inherited from TrajOptVectorOfVector, call its Plot function if (auto* plt = dynamic_cast(f_.get())) { - Eigen::VectorXd dof_vals = sco::getVec(x, vars_); + const Eigen::VectorXd dof_vals = sco::getVec(x, vars_); plt->Plot(plotter, dof_vals); } } diff --git a/trajopt/src/collision_terms.cpp b/trajopt/src/collision_terms.cpp index c720a932..05a5bec2 100644 --- a/trajopt/src/collision_terms.cpp +++ b/trajopt/src/collision_terms.cpp @@ -25,6 +25,10 @@ TRAJOPT_IGNORE_WARNINGS_POP namespace trajopt { + +namespace +{ + void CollisionsToDistances(const ContactResultVectorWrapper& dist_results, DblVec& dists) { dists.clear(); @@ -194,6 +198,8 @@ void DebugPrintInfo(const tesseract_collision::ContactResult& res, std::printf("\n"); } +} // namespace + GradientResults CollisionEvaluator::GetGradient(const Eigen::VectorXd& dofvals, const tesseract_collision::ContactResult& contact_result, const std::array& data, @@ -290,7 +296,7 @@ GradientResults CollisionEvaluator::GetGradient(const Eigen::VectorXd& dofvals0, tesseract_common::jacobianChangeRefPoint(jac, link_transform.linear() * contact_result.nearest_points_local[i]); #ifndef NDEBUG - Eigen::Isometry3d test_link_transform = manip_->calcFwdKin(dofvalst)[contact_result.link_names[i]]; + const Eigen::Isometry3d test_link_transform = manip_->calcFwdKin(dofvalst)[contact_result.link_names[i]]; assert(test_link_transform.isApprox(link_transform, 0.0001)); Eigen::MatrixXd jac_test; @@ -301,7 +307,7 @@ GradientResults CollisionEvaluator::GetGradient(const Eigen::VectorXd& dofvals0, dofvalst, contact_result.link_names[i], contact_result.nearest_points_local[i]); - bool check = jac.isApprox(jac_test, 1e-3); + const bool check = jac.isApprox(jac_test, 1e-3); assert(check == true); #endif @@ -337,7 +343,7 @@ void CollisionEvaluator::CollisionsToDistanceExpressions(sco::AffExprVector& exp const DblVec& x, bool isTimestep1) { - Eigen::VectorXd dofvals = sco::getVec(x, vars); + const Eigen::VectorXd dofvals = sco::getVec(x, vars); // All collision data is in world coordinate system. This provides the // transform for converting data between world frame and manipulator @@ -375,7 +381,7 @@ void CollisionEvaluator::CollisionsToDistanceExpressionsW(sco::AffExprVector& ex const DblVec& x, bool isTimestep1) { - Eigen::VectorXd dofvals = sco::getVec(x, vars); + const Eigen::VectorXd dofvals = sco::getVec(x, vars); // All collision data is in world coordinate system. This provides the // transform for converting data between world frame and manipulator @@ -425,7 +431,8 @@ void CollisionEvaluator::CollisionsToDistanceExpressionsW(sco::AffExprVector& ex if (res.distance < worst_dist) worst_dist = res.distance; - double weight = 100.0 * sco::pospart(grad.data[static_cast(i)] + safety_margin_buffer_ - res.distance); + const double weight = + 100.0 * sco::pospart(grad.data[static_cast(i)] + safety_margin_buffer_ - res.distance); total_weight[i] += weight; dist_grad[i] += (weight * grad.gradients[i].gradient); } @@ -464,9 +471,9 @@ void CollisionEvaluator::CollisionsToDistanceExpressionsContinuousW( const DblVec& x, bool isTimestep1) { - Eigen::VectorXd dofvals0 = sco::getVec(x, vars0); - Eigen::VectorXd dofvals1 = sco::getVec(x, vars1); - Eigen::VectorXd dofvalst = Eigen::VectorXd::Zero(dofvals0.size()); + const Eigen::VectorXd dofvals0 = sco::getVec(x, vars0); + const Eigen::VectorXd dofvals1 = sco::getVec(x, vars1); + const Eigen::VectorXd dofvalst = Eigen::VectorXd::Zero(dofvals0.size()); // All collision data is in world coordinate system. This provides the // transform for converting data between world frame and manipulator @@ -519,7 +526,8 @@ void CollisionEvaluator::CollisionsToDistanceExpressionsContinuousW( if (res.distance < worst_dist) worst_dist = res.distance; - double weight = 100.0 * sco::pospart(grad.data[static_cast(i)] + safety_margin_buffer_ - res.distance); + const double weight = + 100.0 * sco::pospart(grad.data[static_cast(i)] + safety_margin_buffer_ - res.distance); total_weight[i] += weight; dist_grad[i] += (weight * grad.gradients[i].gradient); } @@ -602,27 +610,27 @@ CollisionEvaluator::CollisionEvaluator(tesseract_kinematics::JointGroup::ConstPt void CollisionEvaluator::CalcDists(const DblVec& x, DblVec& dists) { - ContactResultVectorConstPtr dist_results = GetContactResultVectorCached(x); + const ContactResultVectorConstPtr dist_results = GetContactResultVectorCached(x); CollisionsToDistances(*dist_results, dists); } inline std::size_t hash(const DblVec& x) { return boost::hash_range(x.begin(), x.end()); } ContactResultVectorConstPtr CollisionEvaluator::GetContactResultVectorCached(const DblVec& x) { - std::pair pair = GetContactResultCached(x); + const std::pair pair = GetContactResultCached(x); return pair.second; } ContactResultMapConstPtr CollisionEvaluator::GetContactResultMapCached(const DblVec& x) { - std::pair pair = GetContactResultCached(x); + const std::pair pair = GetContactResultCached(x); return pair.first; } std::pair CollisionEvaluator::GetContactResultCached(const DblVec& x) { - std::size_t key = hash(sco::getDblVec(x, GetVars())); + const std::size_t key = hash(sco::getDblVec(x, GetVars())); auto* it = m_cache.get(key); if (it != nullptr) { @@ -655,7 +663,7 @@ void CollisionEvaluator::CalcDistExpressionsStartFree(const DblVec& x, sco::AffExprVector& exprs, std::vector>& exprs_data) { - ContactResultVectorConstPtr dist_vec = GetContactResultVectorCached(x); + const ContactResultVectorConstPtr dist_vec = GetContactResultVectorCached(x); const ContactResultVectorWrapper& dist_results = *dist_vec; sco::AffExprVector exprs0; @@ -675,7 +683,7 @@ void CollisionEvaluator::CalcDistExpressionsEndFree(const DblVec& x, sco::AffExprVector& exprs, std::vector>& exprs_data) { - ContactResultVectorConstPtr dist_vec = GetContactResultVectorCached(x); + const ContactResultVectorConstPtr dist_vec = GetContactResultVectorCached(x); const ContactResultVectorWrapper& dist_results = *dist_vec; sco::AffExprVector exprs1; @@ -695,7 +703,7 @@ void CollisionEvaluator::CalcDistExpressionsBothFree(const DblVec& x, sco::AffExprVector& exprs, std::vector>& exprs_data) { - ContactResultVectorConstPtr dist_vec = GetContactResultVectorCached(x); + const ContactResultVectorConstPtr dist_vec = GetContactResultVectorCached(x); const ContactResultVectorWrapper& dist_results = *dist_vec; sco::AffExprVector exprs0, exprs1; @@ -721,7 +729,7 @@ void CollisionEvaluator::CalcDistExpressionsStartFreeW(const DblVec& x, sco::AffExprVector& exprs, std::vector>& exprs_data) { - ContactResultMapConstPtr dist_results = GetContactResultMapCached(x); + const ContactResultMapConstPtr dist_results = GetContactResultMapCached(x); sco::AffExprVector exprs0; CollisionsToDistanceExpressionsContinuousW(exprs0, exprs_data, *dist_results, vars0_, vars1_, x, false); @@ -739,7 +747,7 @@ void CollisionEvaluator::CalcDistExpressionsEndFreeW(const DblVec& x, sco::AffExprVector& exprs, std::vector>& exprs_data) { - ContactResultMapConstPtr dist_results = GetContactResultMapCached(x); + const ContactResultMapConstPtr dist_results = GetContactResultMapCached(x); sco::AffExprVector exprs1; CollisionsToDistanceExpressionsContinuousW(exprs1, exprs_data, *dist_results, vars0_, vars1_, x, true); @@ -757,7 +765,7 @@ void CollisionEvaluator::CalcDistExpressionsBothFreeW(const DblVec& x, sco::AffExprVector& exprs, std::vector>& exprs_data) { - ContactResultMapConstPtr dist_results = GetContactResultMapCached(x); + const ContactResultMapConstPtr dist_results = GetContactResultMapCached(x); sco::AffExprVector exprs0, exprs1; std::vector> exprs_data0, exprs_data1; @@ -780,7 +788,7 @@ void CollisionEvaluator::CalcDistExpressionsSingleTimeStep(const DblVec& x, sco::AffExprVector& exprs, std::vector>& exprs_data) { - ContactResultVectorConstPtr dist_vec = GetContactResultVectorCached(x); + const ContactResultVectorConstPtr dist_vec = GetContactResultVectorCached(x); const ContactResultVectorWrapper& dist_results = *dist_vec; CollisionsToDistanceExpressions(exprs, exprs_data, dist_results, vars0_, x, false); @@ -797,7 +805,7 @@ void CollisionEvaluator::CalcDistExpressionsSingleTimeStepW(const DblVec& x, sco::AffExprVector& exprs, std::vector>& exprs_data) { - ContactResultMapConstPtr dist_results = GetContactResultMapCached(x); + const ContactResultMapConstPtr dist_results = GetContactResultMapCached(x); CollisionsToDistanceExpressionsW(exprs, exprs_data, *dist_results, vars0_, x, false); assert(dist_results->size() == exprs.size()); @@ -928,7 +936,7 @@ SingleTimestepCollisionEvaluator::SingleTimestepCollisionEvaluator( void SingleTimestepCollisionEvaluator::CalcCollisions(const DblVec& x, tesseract_collision::ContactResultMap& dist_results) { - Eigen::VectorXd joint_vals = sco::getVec(x, vars0_); + const Eigen::VectorXd joint_vals = sco::getVec(x, vars0_); CalcCollisions(joint_vals, dist_results); } @@ -978,10 +986,10 @@ void SingleTimestepCollisionEvaluator::CalcDistExpressions(const DblVec& x, void SingleTimestepCollisionEvaluator::Plot(const std::shared_ptr& plotter, const DblVec& x) { - ContactResultVectorConstPtr dist_vec = GetContactResultVectorCached(x); + const ContactResultVectorConstPtr dist_vec = GetContactResultVectorCached(x); const ContactResultVectorWrapper& dist_results = *dist_vec; - Eigen::VectorXd dofvals = sco::getVec(x, vars0_); + const Eigen::VectorXd dofvals = sco::getVec(x, vars0_); tesseract_collision::ContactResultVector dist_results_copy(dist_results.size()); Eigen::VectorXd safety_distance(dist_results.size()); @@ -999,9 +1007,9 @@ void SingleTimestepCollisionEvaluator::Plot(const std::shared_ptrcalcJacobian(dofvals, res.link_names[0], res.nearest_points[0]); - Eigen::VectorXd dist_grad = -res.normal.transpose() * jac.topRows(3); + const Eigen::VectorXd dist_grad = -res.normal.transpose() * jac.topRows(3); - Eigen::Isometry3d pose2 = manip_->calcFwdKin(dofvals + dist_grad)[res.link_names[0]]; + const Eigen::Isometry3d pose2 = manip_->calcFwdKin(dofvals + dist_grad)[res.link_names[0]]; tesseract_visualization::ArrowMarker am(res.nearest_points[0], pose2 * res.nearest_points[0]); am.material = std::make_shared("collision_error_material"); @@ -1014,7 +1022,7 @@ void SingleTimestepCollisionEvaluator::Plot(const std::shared_ptrgetPairSafetyMarginData(link1, link2)[0]; }; - tesseract_visualization::ContactResultsMarker cm(manip_->getActiveLinkNames(), dist_results_copy, margin_fn); + const tesseract_visualization::ContactResultsMarker cm(manip_->getActiveLinkNames(), dist_results_copy, margin_fn); plotter->plotMarker(cm); } @@ -1111,8 +1119,8 @@ DiscreteCollisionEvaluator::DiscreteCollisionEvaluator( void DiscreteCollisionEvaluator::CalcCollisions(const DblVec& x, tesseract_collision::ContactResultMap& dist_results) { - Eigen::VectorXd s0 = sco::getVec(x, vars0_); - Eigen::VectorXd s1 = sco::getVec(x, vars1_); + const Eigen::VectorXd s0 = sco::getVec(x, vars0_); + const Eigen::VectorXd s1 = sco::getVec(x, vars1_); CalcCollisions(s0, s1, dist_results); } @@ -1124,7 +1132,7 @@ void DiscreteCollisionEvaluator::CalcCollisions(const Eigen::Ref& plotter, const DblVec& x) { - ContactResultVectorConstPtr dist_vec = GetContactResultVectorCached(x); + const ContactResultVectorConstPtr dist_vec = GetContactResultVectorCached(x); const ContactResultVectorWrapper& dist_results = *dist_vec; - Eigen::VectorXd dofvals0 = sco::getVec(x, vars0_); - Eigen::VectorXd dofvals1 = sco::getVec(x, vars1_); + const Eigen::VectorXd dofvals0 = sco::getVec(x, vars0_); + const Eigen::VectorXd dofvals1 = sco::getVec(x, vars1_); - tesseract_common::TransformMap state0 = manip_->calcFwdKin(dofvals0); - tesseract_common::TransformMap state1 = manip_->calcFwdKin(dofvals1); + const tesseract_common::TransformMap state0 = manip_->calcFwdKin(dofvals0); + const tesseract_common::TransformMap state1 = manip_->calcFwdKin(dofvals1); tesseract_collision::ContactResultVector dist_results_copy(dist_results.size()); Eigen::VectorXd safety_distance(dist_results.size()); @@ -1225,9 +1233,9 @@ void DiscreteCollisionEvaluator::Plot(const std::shared_ptrcalcFwdKin(dofvals0 + dist_grad)[res.link_names[0]]; + const Eigen::Isometry3d pose2 = manip_->calcFwdKin(dofvals0 + dist_grad)[res.link_names[0]]; tesseract_visualization::ArrowMarker am(res.nearest_points[0], pose2 * res.nearest_points_local[0]); am.material = std::make_shared("collision_error_material"); @@ -1245,9 +1253,9 @@ void DiscreteCollisionEvaluator::Plot(const std::shared_ptrcalcFwdKin(dofvals0 + dist_grad)[res.link_names[1]]; + const Eigen::Isometry3d pose2 = manip_->calcFwdKin(dofvals0 + dist_grad)[res.link_names[1]]; tesseract_visualization::ArrowMarker am(res.nearest_points[1], pose2 * res.nearest_points_local[1]); am.material = std::make_shared("collision_error_material"); @@ -1260,7 +1268,7 @@ void DiscreteCollisionEvaluator::Plot(const std::shared_ptrgetPairSafetyMarginData(link1, link2)[0]; }; - tesseract_visualization::ContactResultsMarker cm(manip_->getActiveLinkNames(), dist_results_copy, margin_fn); + const tesseract_visualization::ContactResultsMarker cm(manip_->getActiveLinkNames(), dist_results_copy, margin_fn); plotter->plotMarker(cm); } @@ -1359,8 +1367,8 @@ CastCollisionEvaluator::CastCollisionEvaluator( void CastCollisionEvaluator::CalcCollisions(const DblVec& x, tesseract_collision::ContactResultMap& dist_results) { - Eigen::VectorXd s0 = sco::getVec(x, vars0_); - Eigen::VectorXd s1 = sco::getVec(x, vars1_); + const Eigen::VectorXd s0 = sco::getVec(x, vars0_); + const Eigen::VectorXd s1 = sco::getVec(x, vars1_); CalcCollisions(s0, s1, dist_results); } @@ -1372,7 +1380,7 @@ void CastCollisionEvaluator::CalcCollisions(const Eigen::RefcalcFwdKin(subtraj.row(i)); @@ -1454,10 +1462,10 @@ void CastCollisionEvaluator::CalcDistExpressions(const DblVec& x, void CastCollisionEvaluator::Plot(const std::shared_ptr& plotter, const DblVec& x) { - ContactResultVectorConstPtr dist_vec = GetContactResultVectorCached(x); + const ContactResultVectorConstPtr dist_vec = GetContactResultVectorCached(x); const ContactResultVectorWrapper& dist_results = *dist_vec; - Eigen::VectorXd dofvals = sco::getVec(x, vars0_); + const Eigen::VectorXd dofvals = sco::getVec(x, vars0_); tesseract_collision::ContactResultVector dist_results_copy(dist_results.size()); Eigen::VectorXd safety_distance(dist_results.size()); @@ -1480,9 +1488,9 @@ void CastCollisionEvaluator::Plot(const std::shared_ptrlink_name, // res.nearest_points_local[0]); bool check = jac.isApprox(jac_test, 1e-3); assert(check == true); - Eigen::VectorXd dist_grad = -res.normal.transpose() * jac.topRows(3); + const Eigen::VectorXd dist_grad = -res.normal.transpose() * jac.topRows(3); - Eigen::Isometry3d pose2 = manip_->calcFwdKin(dofvals + dist_grad)[res.link_names[0]]; + const Eigen::Isometry3d pose2 = manip_->calcFwdKin(dofvals + dist_grad)[res.link_names[0]]; tesseract_visualization::ArrowMarker am(res.nearest_points[0], pose2 * res.nearest_points_local[0]); am.material = std::make_shared("collision_error_material"); am.material->color << 1, 1, 1, 1; @@ -1499,8 +1507,8 @@ void CastCollisionEvaluator::Plot(const std::shared_ptrlink_name, // res.nearest_points_local[1]); bool check = jac.isApprox(jac_test, 1e-3); assert(check == true) - Eigen::VectorXd dist_grad = res.normal.transpose() * jac.topRows(3); - Eigen::Isometry3d pose2 = manip_->calcFwdKin(dofvals + dist_grad)[res.link_names[1]]; + const Eigen::VectorXd dist_grad = res.normal.transpose() * jac.topRows(3); + const Eigen::Isometry3d pose2 = manip_->calcFwdKin(dofvals + dist_grad)[res.link_names[1]]; tesseract_visualization::ArrowMarker am(res.nearest_points[1], pose2 * res.nearest_points_local[1]); am.material = std::make_shared("collision_error_material"); am.material->color << 1, 1, 1, 1; @@ -1512,7 +1520,7 @@ void CastCollisionEvaluator::Plot(const std::shared_ptrgetPairSafetyMarginData(link1, link2)[0]; }; - tesseract_visualization::ContactResultsMarker cm(manip_->getActiveLinkNames(), dist_results_copy, margin_fn); + const tesseract_visualization::ContactResultsMarker cm(manip_->getActiveLinkNames(), dist_results_copy, margin_fn); plotter->plotMarker(cm); } @@ -1595,7 +1603,7 @@ sco::ConvexObjective::Ptr CollisionCost::convex(const sco::DblVec& x, sco::Model // Contains the contact distance threshold and coefficient for the given link pair const std::array& data = exprs_data[i]; - sco::AffExpr viol = sco::exprSub(sco::AffExpr(data[0]), exprs[i]); + const sco::AffExpr viol = sco::exprSub(sco::AffExpr(data[0]), exprs[i]); out->addHinge(viol, data[1]); } return out; @@ -1702,7 +1710,7 @@ sco::ConvexConstraints::Ptr CollisionConstraint::convex(const sco::DblVec& x, sc // Contains the contact distance threshold and coefficient for the given link pair const std::array& data = exprs_data[i]; - sco::AffExpr viol = sco::exprSub(sco::AffExpr(data[0]), exprs[i]); + const sco::AffExpr viol = sco::exprSub(sco::AffExpr(data[0]), exprs[i]); out->addIneqCnt(sco::exprMult(viol, data[1])); } return out; diff --git a/trajopt/src/file_write_callback.cpp b/trajopt/src/file_write_callback.cpp index 00fc1ac3..302e5e78 100644 --- a/trajopt/src/file_write_callback.cpp +++ b/trajopt/src/file_write_callback.cpp @@ -54,7 +54,7 @@ void WriteFile(const std::shared_ptr& file, } // Calc cartesian pose - tesseract_common::TransformMap state = manip->calcFwdKin(joint_angles); + const tesseract_common::TransformMap state = manip->calcFwdKin(joint_angles); for (const auto& it : state) { @@ -113,7 +113,7 @@ std::function WriteCallback(std::shared_p } // Write cartesian pose labels - std::vector pose_str = std::vector{ "x", "y", "z", "q_w", "q_x", "q_y", "q_z" }; + const std::vector pose_str = std::vector{ "x", "y", "z", "q_w", "q_x", "q_y", "q_z" }; for (const auto& i : pose_str) { *file << ',' << i; diff --git a/trajopt/src/kinematic_terms.cpp b/trajopt/src/kinematic_terms.cpp index 8bc47fbb..7b472753 100644 --- a/trajopt/src/kinematic_terms.cpp +++ b/trajopt/src/kinematic_terms.cpp @@ -110,7 +110,7 @@ DynamicCartPoseErrCalculator::DynamicCartPoseErrCalculator( error_function = [lower_tolerance, upper_tolerance](const Eigen::Isometry3d& target_tf, const Eigen::Isometry3d& source_tf) -> Eigen::VectorXd { // Calculate the error using tesseract_common::calcTransformError or equivalent - VectorXd err = tesseract_common::calcTransformError(target_tf, source_tf); + const VectorXd err = tesseract_common::calcTransformError(target_tf, source_tf); // Apply tolerances return applyTolerances(err, lower_tolerance, upper_tolerance); @@ -121,8 +121,8 @@ DynamicCartPoseErrCalculator::DynamicCartPoseErrCalculator( VectorXd DynamicCartPoseErrCalculator::operator()(const VectorXd& dof_vals) const { tesseract_common::TransformMap state = manip_->calcFwdKin(dof_vals); - Isometry3d source_tf = state[source_frame_] * source_frame_offset_; - Isometry3d target_tf = state[target_frame_] * target_frame_offset_; + const Isometry3d source_tf = state[source_frame_] * source_frame_offset_; + const Isometry3d target_tf = state[target_frame_] * target_frame_offset_; VectorXd err = error_function(target_tf, source_tf); @@ -176,8 +176,8 @@ MatrixXd DynamicCartPoseJacCalculator::operator()(const VectorXd& dof_vals) cons { // Duplicated from calcForwardNumJac in trajopt_sco/src/num_diff.cpp, but with ignoring tolerances tesseract_common::TransformMap state = manip_->calcFwdKin(dof_vals); - Isometry3d source_tf = state[source_frame_] * source_frame_offset_; - Isometry3d target_tf = state[target_frame_] * target_frame_offset_; + const Isometry3d source_tf = state[source_frame_] * source_frame_offset_; + const Isometry3d target_tf = state[target_frame_] * target_frame_offset_; Eigen::MatrixXd jac0(indices_.size(), dof_vals.size()); Eigen::VectorXd dof_vals_pert = dof_vals; @@ -185,8 +185,8 @@ MatrixXd DynamicCartPoseJacCalculator::operator()(const VectorXd& dof_vals) cons { dof_vals_pert(i) = dof_vals(i) + epsilon_; tesseract_common::TransformMap state_pert = manip_->calcFwdKin(dof_vals_pert); - Isometry3d source_tf_pert = state_pert[source_frame_] * source_frame_offset_; - Isometry3d target_tf_pert = state_pert[target_frame_] * target_frame_offset_; + const Isometry3d source_tf_pert = state_pert[source_frame_] * source_frame_offset_; + const Isometry3d target_tf_pert = state_pert[target_frame_] * target_frame_offset_; VectorXd error_diff = tesseract_common::calcJacobianTransformErrorDiff(target_tf, target_tf_pert, source_tf, source_tf_pert); @@ -254,7 +254,7 @@ CartPoseErrCalculator::CartPoseErrCalculator( error_function_ = [lower_tolerance, upper_tolerance](const Eigen::Isometry3d& target_tf, const Eigen::Isometry3d& source_tf) -> Eigen::VectorXd { // Calculate the error using tesseract_common::calcTransformError or equivalent - VectorXd err = tesseract_common::calcTransformError(source_tf, target_tf); + const VectorXd err = tesseract_common::calcTransformError(source_tf, target_tf); // Apply tolerances return applyTolerances(err, lower_tolerance, upper_tolerance); @@ -265,7 +265,7 @@ CartPoseErrCalculator::CartPoseErrCalculator( error_function_ = [lower_tolerance, upper_tolerance](const Eigen::Isometry3d& target_tf, const Eigen::Isometry3d& source_tf) -> Eigen::VectorXd { // Calculate the error using tesseract_common::calcTransformError or equivalent - VectorXd err = tesseract_common::calcTransformError(target_tf, source_tf); + const VectorXd err = tesseract_common::calcTransformError(target_tf, source_tf); // Apply tolerances return applyTolerances(err, lower_tolerance, upper_tolerance); @@ -277,8 +277,8 @@ CartPoseErrCalculator::CartPoseErrCalculator( VectorXd CartPoseErrCalculator::operator()(const VectorXd& dof_vals) const { tesseract_common::TransformMap state = manip_->calcFwdKin(dof_vals); - Isometry3d source_tf = state[source_frame_] * source_frame_offset_; - Isometry3d target_tf = state[target_frame_] * target_frame_offset_; + const Isometry3d source_tf = state[source_frame_] * source_frame_offset_; + const Isometry3d target_tf = state[target_frame_] * target_frame_offset_; VectorXd err = error_function_(target_tf, source_tf); @@ -334,7 +334,7 @@ CartPoseJacCalculator::CartPoseJacCalculator( const Eigen::Isometry3d& target_tf, const Eigen::Isometry3d& source_tf) -> VectorXd { tesseract_common::TransformMap perturbed_state = manip_->calcFwdKin(vals); - Isometry3d perturbed_target_tf = perturbed_state[target_frame_] * target_frame_offset_; + const Isometry3d perturbed_target_tf = perturbed_state[target_frame_] * target_frame_offset_; VectorXd error_diff = tesseract_common::calcJacobianTransformErrorDiff(source_tf, target_tf, perturbed_target_tf); VectorXd reduced_error_diff(indices_.size()); @@ -350,7 +350,7 @@ CartPoseJacCalculator::CartPoseJacCalculator( const Eigen::Isometry3d& target_tf, const Eigen::Isometry3d& source_tf) -> VectorXd { tesseract_common::TransformMap perturbed_state = manip_->calcFwdKin(vals); - Isometry3d perturbed_source_tf = perturbed_state[source_frame_] * source_frame_offset_; + const Isometry3d perturbed_source_tf = perturbed_state[source_frame_] * source_frame_offset_; VectorXd error_diff = tesseract_common::calcJacobianTransformErrorDiff(target_tf, source_tf, perturbed_source_tf); VectorXd reduced_error_diff(indices_.size()); @@ -365,15 +365,15 @@ CartPoseJacCalculator::CartPoseJacCalculator( MatrixXd CartPoseJacCalculator::operator()(const VectorXd& dof_vals) const { tesseract_common::TransformMap state = manip_->calcFwdKin(dof_vals); - Isometry3d source_tf = state[source_frame_] * source_frame_offset_; - Isometry3d target_tf = state[target_frame_] * target_frame_offset_; + const Isometry3d source_tf = state[source_frame_] * source_frame_offset_; + const Isometry3d target_tf = state[target_frame_] * target_frame_offset_; Eigen::MatrixXd jac0(indices_.size(), dof_vals.size()); Eigen::VectorXd dof_vals_pert = dof_vals; for (int i = 0; i < dof_vals.size(); ++i) { dof_vals_pert(i) = dof_vals(i) + epsilon_; - VectorXd error_diff = error_diff_function_(dof_vals_pert, target_tf, source_tf); + const VectorXd error_diff = error_diff_function_(dof_vals_pert, target_tf, source_tf); jac0.col(i) = error_diff / epsilon_; dof_vals_pert(i) = dof_vals(i); } @@ -444,7 +444,7 @@ Eigen::VectorXd JointVelErrCalculator::operator()(const VectorXd& var_vals) cons assert(var_vals.rows() % 2 == 0); // Top half of the vector are the joint values. The bottom half are the 1/dt values auto half = static_cast(var_vals.rows() / 2); - int num_vels = half - 1; + const int num_vels = half - 1; // (x1-x0)*(1/dt) VectorXd vel = (var_vals.segment(1, num_vels) - var_vals.segment(0, num_vels)).array() * var_vals.segment(half + 1, num_vels).array(); @@ -489,7 +489,7 @@ VectorXd JointAccErrCalculator::operator()(const VectorXd& var_vals) const { assert(var_vals.rows() % 2 == 0); auto half = static_cast(var_vals.rows() / 2); - int num_acc = half - 2; + const int num_acc = half - 2; VectorXd vels = vel_calc(var_vals); // v1-v0 @@ -505,18 +505,18 @@ VectorXd JointAccErrCalculator::operator()(const VectorXd& var_vals) const MatrixXd JointAccJacCalculator::operator()(const VectorXd& var_vals) const { auto num_vals = static_cast(var_vals.rows()); - int half = num_vals / 2; + const int half = num_vals / 2; MatrixXd jac = MatrixXd::Zero(half - 2, num_vals); VectorXd vels = vel_calc(var_vals); MatrixXd vel_jac = vel_jac_calc(var_vals); for (int i = 0; i < jac.rows(); i++) { - int dt_1_index = i + half + 1; - int dt_2_index = dt_1_index + 1; - double dt_1 = var_vals(dt_1_index); - double dt_2 = var_vals(dt_2_index); - double total_dt = dt_1 + dt_2; + const int dt_1_index = i + half + 1; + const int dt_2_index = dt_1_index + 1; + const double dt_1 = var_vals(dt_1_index); + const double dt_2 = var_vals(dt_2_index); + const double total_dt = dt_1 + dt_2; jac(i, i) = 2.0 * (vel_jac(i + 1, i) - vel_jac(i, i)) / total_dt; jac(i, i + 1) = 2.0 * (vel_jac(i + 1, i + 1) - vel_jac(i, i + 1)) / total_dt; @@ -536,7 +536,7 @@ VectorXd JointJerkErrCalculator::operator()(const VectorXd& var_vals) const { assert(var_vals.rows() % 2 == 0); auto half = static_cast(var_vals.rows() / 2); - int num_jerk = half - 3; + const int num_jerk = half - 3; VectorXd acc = acc_calc(var_vals); VectorXd acc_diff = acc.segment(1, num_jerk) - acc.segment(0, num_jerk); @@ -552,7 +552,7 @@ VectorXd JointJerkErrCalculator::operator()(const VectorXd& var_vals) const MatrixXd JointJerkJacCalculator::operator()(const VectorXd& var_vals) const { auto num_vals = static_cast(var_vals.rows()); - int half = num_vals / 2; + const int half = num_vals / 2; MatrixXd jac = MatrixXd::Zero(half - 3, num_vals); VectorXd acc = acc_calc(var_vals); @@ -560,13 +560,13 @@ MatrixXd JointJerkJacCalculator::operator()(const VectorXd& var_vals) const for (int i = 0; i < jac.rows(); i++) { - int dt_1_index = i + half + 1; - int dt_2_index = dt_1_index + 1; - int dt_3_index = dt_2_index + 1; - double dt_1 = var_vals(dt_1_index); - double dt_2 = var_vals(dt_2_index); - double dt_3 = var_vals(dt_3_index); - double total_dt = dt_1 + dt_2 + dt_3; + const int dt_1_index = i + half + 1; + const int dt_2_index = dt_1_index + 1; + const int dt_3_index = dt_2_index + 1; + const double dt_1 = var_vals(dt_1_index); + const double dt_2 = var_vals(dt_2_index); + const double dt_3 = var_vals(dt_3_index); + const double total_dt = dt_1 + dt_2 + dt_3; jac(i, i) = 3.0 * (acc_jac(i + 1, i) - acc_jac(i, i)) / total_dt; jac(i, i + 1) = 3.0 * (acc_jac(i + 1, i + 1) - acc_jac(i, i + 1)) / total_dt; @@ -601,12 +601,12 @@ MatrixXd TimeCostJacCalculator::operator()(const VectorXd& var_vals) const VectorXd AvoidSingularityErrCalculator::operator()(const VectorXd& var_vals) const { // Calculate the SVD of the jacobian at this joint state - MatrixXd jacobian = fwd_kin_->calcJacobian(var_vals, link_name_); - Eigen::JacobiSVD svd(jacobian, Eigen::ComputeThinU | Eigen::ComputeThinV); + const MatrixXd jacobian = fwd_kin_->calcJacobian(var_vals, link_name_); + const Eigen::JacobiSVD svd(jacobian, Eigen::ComputeThinU | Eigen::ComputeThinV); // Get the U and V vectors for the smallest singular value - double smallest_sv = svd.singularValues().tail(1)(0); - double cost = 1.0 / (smallest_sv + lambda_); + const double smallest_sv = svd.singularValues().tail(1)(0); + const double cost = 1.0 / (smallest_sv + lambda_); const static double smallest_allowable_sv = 0.1; const double threshold = 1.0 / (smallest_allowable_sv + lambda_); @@ -623,10 +623,10 @@ MatrixXd AvoidSingularityJacCalculator::jacobianPartialDerivative(const VectorXd { // Calculate the jacobian for the given joint perturbed by some epsilon Eigen::VectorXd joints(state); - double eps = eps_; + const double eps = eps_; joints(jntIdx) += eps; - MatrixXd jacobian_increment = fwd_kin_->calcJacobian(joints, link_name_); + const MatrixXd jacobian_increment = fwd_kin_->calcJacobian(joints, link_name_); return (jacobian_increment - jacobian) / eps; } @@ -636,13 +636,13 @@ MatrixXd AvoidSingularityJacCalculator::operator()(const VectorXd& var_vals) con cost_jacobian.resize(1, var_vals.size()); // Calculate the SVD of the jacobian at this joint state - MatrixXd jacobian = fwd_kin_->calcJacobian(var_vals, link_name_); - Eigen::JacobiSVD svd(jacobian, Eigen::ComputeThinU | Eigen::ComputeThinV); + const MatrixXd jacobian = fwd_kin_->calcJacobian(var_vals, link_name_); + const Eigen::JacobiSVD svd(jacobian, Eigen::ComputeThinU | Eigen::ComputeThinV); // Get the U and V vectors for the smallest singular value - double smallest_sv = svd.singularValues().tail(1)(0); + const double smallest_sv = svd.singularValues().tail(1)(0); VectorXd ui = svd.matrixU().rightCols(1); - VectorXd vi = svd.matrixV().rightCols(1); + const VectorXd vi = svd.matrixV().rightCols(1); // Calculate the jacobian partial derivative for each joint, perturbing it slightly for (Eigen::Index jntIdx = 0; jntIdx < var_vals.size(); ++jntIdx) diff --git a/trajopt/src/problem_description.cpp b/trajopt/src/problem_description.cpp index 4f8f59ea..6c30cbc4 100644 --- a/trajopt/src/problem_description.cpp +++ b/trajopt/src/problem_description.cpp @@ -35,7 +35,7 @@ void ensure_only_members(const Json::Value& v, const char** fields, int nvalid) bool valid = false; for (int j = 0; j < nvalid; ++j) { - JSONCPP_STRING member_name = it.name(); + const JSONCPP_STRING member_name = it.name(); if (strcmp(member_name.c_str(), fields[j]) == 0) { valid = true; @@ -169,7 +169,7 @@ void ProblemConstructionInfo::readCosts(const Json::Value& v) json_marshal::childFromJson(it, type, "type"); json_marshal::childFromJson(it, use_time, "use_time", false); LOG_DEBUG("reading term: %s", type.c_str()); - TermInfo::Ptr term = TermInfo::fromName(type); + const TermInfo::Ptr term = TermInfo::fromName(type); if (!term) PRINT_AND_THROW(boost::format("failed to construct cost named %s") % type); @@ -199,7 +199,7 @@ void ProblemConstructionInfo::readConstraints(const Json::Value& v) json_marshal::childFromJson(it, type, "type"); json_marshal::childFromJson(it, use_time, "use_time", false); LOG_DEBUG("reading term: %s", type.c_str()); - TermInfo::Ptr term = TermInfo::fromName(type); + const TermInfo::Ptr term = TermInfo::fromName(type); if (!term) PRINT_AND_THROW(boost::format("failed to construct constraint named %s") % type); @@ -223,7 +223,7 @@ void ProblemConstructionInfo::readInitInfo(const Json::Value& v) std::string type_str; json_marshal::childFromJson(v, type_str, "type"); json_marshal::childFromJson(v, init_info.dt, "dt", 1.0); - int n_steps = basic_info.n_steps; + const int n_steps = basic_info.n_steps; auto n_dof = static_cast(kin->numJoints()); if (boost::iequals(type_str, "stationary")) @@ -597,7 +597,7 @@ void UserDefinedTermInfo::fromJson(ProblemConstructionInfo& /*pci*/, const Json: void UserDefinedTermInfo::hatch(TrajOptProb& prob) { - int n_dof = static_cast(prob.GetKin()->numJoints()); + const int n_dof = static_cast(prob.GetKin()->numJoints()); // Apply error calculator as either cost or constraint if (static_cast(term_type & TermType::TT_COST)) @@ -644,7 +644,7 @@ void UserDefinedTermInfo::hatch(TrajOptProb& prob) { if (std::find(fixed_steps.begin(), fixed_steps.end(), s) == fixed_steps.end()) { - std::string type_str = (constraint_type == sco::ConstraintType::EQ) ? "EQ" : "INEQ"; + const std::string type_str = (constraint_type == sco::ConstraintType::EQ) ? "EQ" : "INEQ"; if (jacobian_function == nullptr) { prob.addConstraint( @@ -702,17 +702,17 @@ void DynamicCartPoseTermInfo::fromJson(ProblemConstructionInfo& pci, const Json: json_marshal::childFromJson( params, target_frame_offset_wxyz, "target_frame_offset_wxyz", Eigen::Vector4d(1, 0, 0, 0)); - Eigen::Quaterniond q(source_frame_offset_wxyz(0), - source_frame_offset_wxyz(1), - source_frame_offset_wxyz(2), - source_frame_offset_wxyz(3)); + const Eigen::Quaterniond q(source_frame_offset_wxyz(0), + source_frame_offset_wxyz(1), + source_frame_offset_wxyz(2), + source_frame_offset_wxyz(3)); source_frame_offset.linear() = q.matrix(); source_frame_offset.translation() = source_frame_offset_xyz; - Eigen::Quaterniond target_q(target_frame_offset_wxyz(0), - target_frame_offset_wxyz(1), - target_frame_offset_wxyz(2), - target_frame_offset_wxyz(3)); + const Eigen::Quaterniond target_q(target_frame_offset_wxyz(0), + target_frame_offset_wxyz(1), + target_frame_offset_wxyz(2), + target_frame_offset_wxyz(3)); target_frame_offset.linear() = target_q.matrix(); target_frame_offset.translation() = target_frame_offset_xyz; @@ -726,8 +726,8 @@ void DynamicCartPoseTermInfo::fromJson(ProblemConstructionInfo& pci, const Json: PRINT_AND_THROW(boost::format("invalid target frame: %s") % target_frame); } - bool source_active = pci.kin->isActiveLinkName(source_frame); - bool target_active = pci.kin->isActiveLinkName(target_frame); + const bool source_active = pci.kin->isActiveLinkName(source_frame); + const bool target_active = pci.kin->isActiveLinkName(target_frame); if (!(source_active && target_active)) { @@ -749,7 +749,7 @@ void DynamicCartPoseTermInfo::fromJson(ProblemConstructionInfo& pci, const Json: void DynamicCartPoseTermInfo::hatch(TrajOptProb& prob) { - int n_dof = static_cast(prob.GetKin()->numJoints()); + const int n_dof = static_cast(prob.GetKin()->numJoints()); // Next parse the coeff and if not zero add the indice and coeff std::vector ic; @@ -843,17 +843,17 @@ void CartPoseTermInfo::fromJson(ProblemConstructionInfo& pci, const Json::Value& json_marshal::childFromJson( params, target_frame_offset_wxyz, "target_frame_offset_wxyz", Eigen::Vector4d(1, 0, 0, 0)); - Eigen::Quaterniond q(source_frame_offset_wxyz(0), - source_frame_offset_wxyz(1), - source_frame_offset_wxyz(2), - source_frame_offset_wxyz(3)); + const Eigen::Quaterniond q(source_frame_offset_wxyz(0), + source_frame_offset_wxyz(1), + source_frame_offset_wxyz(2), + source_frame_offset_wxyz(3)); source_frame_offset.linear() = q.matrix(); source_frame_offset.translation() = source_frame_offset_xyz; - Eigen::Quaterniond target_q(target_frame_offset_wxyz(0), - target_frame_offset_wxyz(1), - target_frame_offset_wxyz(2), - target_frame_offset_wxyz(3)); + const Eigen::Quaterniond target_q(target_frame_offset_wxyz(0), + target_frame_offset_wxyz(1), + target_frame_offset_wxyz(2), + target_frame_offset_wxyz(3)); target_frame_offset.linear() = target_q.matrix(); target_frame_offset.translation() = target_frame_offset_xyz; @@ -867,8 +867,8 @@ void CartPoseTermInfo::fromJson(ProblemConstructionInfo& pci, const Json::Value& PRINT_AND_THROW(boost::format("invalid target frame: %s") % target_frame); } - bool source_active = pci.kin->isActiveLinkName(source_frame); - bool target_active = pci.kin->isActiveLinkName(target_frame); + const bool source_active = pci.kin->isActiveLinkName(source_frame); + const bool target_active = pci.kin->isActiveLinkName(target_frame); if (source_active && target_active) { PRINT_AND_THROW(boost::format("source '%s' and target '%s' are both active") % source_frame % target_frame); @@ -893,7 +893,7 @@ void CartPoseTermInfo::fromJson(ProblemConstructionInfo& pci, const Json::Value& void CartPoseTermInfo::hatch(TrajOptProb& prob) { - int n_dof = static_cast(prob.GetKin()->numJoints()); + const int n_dof = static_cast(prob.GetKin()->numJoints()); // Next parse the coeff and if not zero add the index and coeff std::vector ic; @@ -991,7 +991,7 @@ void CartVelTermInfo::fromJson(ProblemConstructionInfo& pci, const Json::Value& void CartVelTermInfo::hatch(TrajOptProb& prob) { - int n_dof = static_cast(prob.GetKin()->numJoints()); + const int n_dof = static_cast(prob.GetKin()->numJoints()); if (term_type == (TermType::TT_COST | TermType::TT_USE_TIME)) { @@ -1042,7 +1042,7 @@ void JointPosTermInfo::fromJson(ProblemConstructionInfo& pci, const Json::Value& FAIL_IF_FALSE(v.isMember("params")); const Json::Value& params = v["params"]; - unsigned int n_dof = pci.kin->numJoints(); + const unsigned int n_dof = pci.kin->numJoints(); json_marshal::childFromJson(params, targets, "targets"); // Optional Parameters @@ -1058,7 +1058,7 @@ void JointPosTermInfo::fromJson(ProblemConstructionInfo& pci, const Json::Value& void JointPosTermInfo::hatch(TrajOptProb& prob) { - unsigned int n_dof = prob.GetKin()->numJoints(); + const unsigned int n_dof = prob.GetKin()->numJoints(); // If optional parameter not given, set to default if (coeffs.empty()) @@ -1079,7 +1079,7 @@ void JointPosTermInfo::hatch(TrajOptProb& prob) // last_step += 1; if (last_step < first_step) { - int tmp = first_step; + const int tmp = first_step; first_step = last_step; last_step = tmp; CONSOLE_BRIDGE_logWarn("Last time step for JointPosTerm comes before first step. Reversing them."); @@ -1094,13 +1094,13 @@ void JointPosTermInfo::hatch(TrajOptProb& prob) checkParameterSize(lower_tols, n_dof, "JointPosTermInfo lower_tols", true); // Check if tolerances are all zeros - bool is_upper_zeros = + const bool is_upper_zeros = std::all_of(upper_tols.begin(), upper_tols.end(), [](double i) { return trajopt_common::doubleEquals(i, 0.); }); - bool is_lower_zeros = + const bool is_lower_zeros = std::all_of(lower_tols.begin(), lower_tols.end(), [](double i) { return trajopt_common::doubleEquals(i, 0.); }); // Get vars associated with joints - trajopt::VarArray vars = prob.GetVars(); + const trajopt::VarArray vars = prob.GetVars(); trajopt::VarArray joint_vars = vars.block(0, 0, vars.rows(), static_cast(n_dof)); if (prob.GetHasTime()) CONSOLE_BRIDGE_logInform("JointPosTermInfo does not differ based on setting of TermType::TT_USE_TIME"); @@ -1161,7 +1161,7 @@ void JointVelTermInfo::fromJson(ProblemConstructionInfo& pci, const Json::Value& FAIL_IF_FALSE(v.isMember("params")); const Json::Value& params = v["params"]; - unsigned int n_dof = pci.kin->numJoints(); + const unsigned int n_dof = pci.kin->numJoints(); json_marshal::childFromJson(params, targets, "targets"); // Optional Parameters @@ -1177,7 +1177,7 @@ void JointVelTermInfo::fromJson(ProblemConstructionInfo& pci, const Json::Value& void JointVelTermInfo::hatch(TrajOptProb& prob) { - unsigned int n_dof = prob.GetKin()->numJoints(); + const unsigned int n_dof = prob.GetKin()->numJoints(); // If optional parameter not given, set to default if (coeffs.empty()) @@ -1198,7 +1198,7 @@ void JointVelTermInfo::hatch(TrajOptProb& prob) last_step += 1; if (last_step < first_step) { - int tmp = first_step; + const int tmp = first_step; first_step = last_step; last_step = tmp; CONSOLE_BRIDGE_logWarn("Last time step for JointVelTerm comes before first step. Reversing them."); @@ -1213,13 +1213,13 @@ void JointVelTermInfo::hatch(TrajOptProb& prob) assert(first_step >= 0); // Check if tolerances are all zeros - bool is_upper_zeros = + const bool is_upper_zeros = std::all_of(upper_tols.begin(), upper_tols.end(), [](double i) { return trajopt_common::doubleEquals(i, 0.); }); - bool is_lower_zeros = + const bool is_lower_zeros = std::all_of(lower_tols.begin(), lower_tols.end(), [](double i) { return trajopt_common::doubleEquals(i, 0.); }); // Get vars associated with joints - trajopt::VarArray vars = prob.GetVars(); + const trajopt::VarArray vars = prob.GetVars(); trajopt::VarArray joint_vars = vars.block(0, 0, vars.rows(), static_cast(n_dof)); if (term_type == (TermType::TT_COST | TermType::TT_USE_TIME)) @@ -1230,13 +1230,14 @@ void JointVelTermInfo::hatch(TrajOptProb& prob) for (std::size_t j = 0; j < n_dof; j++) { // Get a vector of a single column of vars - sco::VarVector joint_vars_vec = joint_vars.cblock(first_step, static_cast(j), last_step - first_step + 1); - sco::VarVector time_vars_vec = vars.cblock(first_step, vars.cols() - 1, last_step - first_step + 1); + const sco::VarVector joint_vars_vec = + joint_vars.cblock(first_step, static_cast(j), last_step - first_step + 1); + const sco::VarVector time_vars_vec = vars.cblock(first_step, vars.cols() - 1, last_step - first_step + 1); // If the tolerances are 0, an equality cost is set if (is_upper_zeros && is_lower_zeros) { - DblVec single_jnt_coeffs = DblVec(num_vels * 2, coeffs[j]); + const DblVec single_jnt_coeffs = DblVec(num_vels * 2, coeffs[j]); auto f = std::make_shared(targets[j], upper_tols[j], lower_tols[j]); auto dfdx = std::make_shared(); prob.addCost(std::make_shared(f, @@ -1249,7 +1250,7 @@ void JointVelTermInfo::hatch(TrajOptProb& prob) // Otherwise it's a hinged "inequality" cost else { - DblVec single_jnt_coeffs = DblVec(num_vels * 2, coeffs[j]); + const DblVec single_jnt_coeffs = DblVec(num_vels * 2, coeffs[j]); auto f = std::make_shared(targets[j], upper_tols[j], lower_tols[j]); auto dfdx = std::make_shared(); prob.addCost(std::make_shared(f, @@ -1269,13 +1270,14 @@ void JointVelTermInfo::hatch(TrajOptProb& prob) for (std::size_t j = 0; j < n_dof; j++) { // Get a vector of a single column of vars - sco::VarVector joint_vars_vec = joint_vars.cblock(first_step, static_cast(j), last_step - first_step + 1); - sco::VarVector time_vars_vec = vars.cblock(first_step, vars.cols() - 1, last_step - first_step + 1); + const sco::VarVector joint_vars_vec = + joint_vars.cblock(first_step, static_cast(j), last_step - first_step + 1); + const sco::VarVector time_vars_vec = vars.cblock(first_step, vars.cols() - 1, last_step - first_step + 1); // If the tolerances are 0, an equality cnt is set if (is_upper_zeros && is_lower_zeros) { - DblVec single_jnt_coeffs = DblVec(num_vels * 2, coeffs[j]); + const DblVec single_jnt_coeffs = DblVec(num_vels * 2, coeffs[j]); auto f = std::make_shared(targets[j], upper_tols[j], lower_tols[j]); auto dfdx = std::make_shared(); prob.addConstraint( @@ -1289,7 +1291,7 @@ void JointVelTermInfo::hatch(TrajOptProb& prob) // Otherwise it's a hinged "inequality" constraint else { - DblVec single_jnt_coeffs = DblVec(num_vels * 2, coeffs[j]); + const DblVec single_jnt_coeffs = DblVec(num_vels * 2, coeffs[j]); auto f = std::make_shared(targets[j], upper_tols[j], lower_tols[j]); auto dfdx = std::make_shared(); prob.addConstraint( @@ -1355,7 +1357,7 @@ void JointAccTermInfo::fromJson(ProblemConstructionInfo& pci, const Json::Value& FAIL_IF_FALSE(v.isMember("params")); const Json::Value& params = v["params"]; - unsigned int n_dof = pci.kin->numJoints(); + const unsigned int n_dof = pci.kin->numJoints(); json_marshal::childFromJson(params, targets, "targets"); // Optional Parameters @@ -1371,7 +1373,7 @@ void JointAccTermInfo::fromJson(ProblemConstructionInfo& pci, const Json::Value& void JointAccTermInfo::hatch(TrajOptProb& prob) { - unsigned int n_dof = prob.GetKin()->numJoints(); + const unsigned int n_dof = prob.GetKin()->numJoints(); // If optional parameter not given, set to default if (coeffs.empty()) @@ -1393,7 +1395,7 @@ void JointAccTermInfo::hatch(TrajOptProb& prob) last_step += 2; if (last_step < first_step) { - int tmp = first_step; + const int tmp = first_step; first_step = last_step; last_step = tmp; CONSOLE_BRIDGE_logWarn("Last time step for JointAccTerm comes before first step. Reversing them."); @@ -1406,13 +1408,13 @@ void JointAccTermInfo::hatch(TrajOptProb& prob) checkParameterSize(lower_tols, n_dof, "JointAccTermInfo lower_tols", true); // Check if tolerances are all zeros - bool is_upper_zeros = + const bool is_upper_zeros = std::all_of(upper_tols.begin(), upper_tols.end(), [](double i) { return trajopt_common::doubleEquals(i, 0.); }); - bool is_lower_zeros = + const bool is_lower_zeros = std::all_of(lower_tols.begin(), lower_tols.end(), [](double i) { return trajopt_common::doubleEquals(i, 0.); }); // Get vars associated with joints - trajopt::VarArray vars = prob.GetVars(); + const trajopt::VarArray vars = prob.GetVars(); trajopt::VarArray joint_vars = vars.block(0, 0, vars.rows(), static_cast(n_dof)); if (term_type == (TermType::TT_COST | TermType::TT_USE_TIME)) @@ -1476,7 +1478,7 @@ void JointJerkTermInfo::fromJson(ProblemConstructionInfo& pci, const Json::Value FAIL_IF_FALSE(v.isMember("params")); const Json::Value& params = v["params"]; - unsigned int n_dof = pci.kin->numJoints(); + const unsigned int n_dof = pci.kin->numJoints(); json_marshal::childFromJson(params, targets, "targets"); @@ -1493,7 +1495,7 @@ void JointJerkTermInfo::fromJson(ProblemConstructionInfo& pci, const Json::Value void JointJerkTermInfo::hatch(TrajOptProb& prob) { - unsigned int n_dof = prob.GetKin()->numJoints(); + const unsigned int n_dof = prob.GetKin()->numJoints(); // If optional parameter not given, set to default if (coeffs.empty()) @@ -1515,7 +1517,7 @@ void JointJerkTermInfo::hatch(TrajOptProb& prob) last_step += 4; if (last_step < first_step) { - int tmp = first_step; + const int tmp = first_step; first_step = last_step; last_step = tmp; CONSOLE_BRIDGE_logWarn("Last time step for JointJerkTerm comes before first step. Reversing them."); @@ -1528,13 +1530,13 @@ void JointJerkTermInfo::hatch(TrajOptProb& prob) checkParameterSize(lower_tols, n_dof, "JointJerkTermInfo lower_tols", true); // Check if tolerances are all zeros - bool is_upper_zeros = + const bool is_upper_zeros = std::all_of(upper_tols.begin(), upper_tols.end(), [](double i) { return trajopt_common::doubleEquals(i, 0.); }); - bool is_lower_zeros = + const bool is_lower_zeros = std::all_of(lower_tols.begin(), lower_tols.end(), [](double i) { return trajopt_common::doubleEquals(i, 0.); }); // Get vars associated with joints - trajopt::VarArray vars = prob.GetVars(); + const trajopt::VarArray vars = prob.GetVars(); trajopt::VarArray joint_vars = vars.block(0, 0, vars.rows(), static_cast(n_dof)); if (term_type == (TermType::TT_COST | TermType::TT_USE_TIME)) @@ -1598,7 +1600,7 @@ void CollisionTermInfo::fromJson(ProblemConstructionInfo& pci, const Json::Value FAIL_IF_FALSE(v.isMember("params")); const Json::Value& params = v["params"]; - int n_steps = pci.basic_info.n_steps; + const int n_steps = pci.basic_info.n_steps; int collision_evaluator_type{ 0 }; json_marshal::childFromJson(params, collision_evaluator_type, "evaluator_type", 0); json_marshal::childFromJson(params, use_weighted_sum, "use_weighted_sum", false); @@ -1701,7 +1703,7 @@ void CollisionTermInfo::fromJson(ProblemConstructionInfo& pci, const Json::Value for (auto i = first_step; i <= last_step; ++i) { auto index = static_cast(i - first_step); - trajopt_common::SafetyMarginData::Ptr& data = info[index]; + const trajopt_common::SafetyMarginData::Ptr& data = info[index]; for (const auto& p : pair) { data->setPairSafetyMarginData(link, p, pair_dist_pen[index], pair_coeffs[index]); @@ -1726,7 +1728,7 @@ void CollisionTermInfo::fromJson(ProblemConstructionInfo& pci, const Json::Value void CollisionTermInfo::hatch(TrajOptProb& prob) { - int n_dof = static_cast(prob.GetKin()->numJoints()); + const int n_dof = static_cast(prob.GetKin()->numJoints()); if (term_type == TermType::TT_COST) { @@ -1735,8 +1737,8 @@ void CollisionTermInfo::hatch(TrajOptProb& prob) bool discrete_continuous = (evaluator_type == CollisionEvaluatorType::DISCRETE_CONTINUOUS); for (int i = first_step; i < last_step; ++i) { - bool current_fixed = std::find(fixed_steps.begin(), fixed_steps.end(), i) != fixed_steps.end(); - bool next_fixed = std::find(fixed_steps.begin(), fixed_steps.end(), i + 1) != fixed_steps.end(); + const bool current_fixed = std::find(fixed_steps.begin(), fixed_steps.end(), i) != fixed_steps.end(); + const bool next_fixed = std::find(fixed_steps.begin(), fixed_steps.end(), i + 1) != fixed_steps.end(); CollisionExpressionEvaluatorType expression_evaluator_type{}; if (!current_fixed && !next_fixed) @@ -1807,8 +1809,8 @@ void CollisionTermInfo::hatch(TrajOptProb& prob) bool discrete_continuous = (evaluator_type == CollisionEvaluatorType::DISCRETE_CONTINUOUS); for (int i = first_step; i < last_step; ++i) { - bool current_fixed = std::find(fixed_steps.begin(), fixed_steps.end(), i) != fixed_steps.end(); - bool next_fixed = std::find(fixed_steps.begin(), fixed_steps.end(), i + 1) != fixed_steps.end(); + const bool current_fixed = std::find(fixed_steps.begin(), fixed_steps.end(), i) != fixed_steps.end(); + const bool next_fixed = std::find(fixed_steps.begin(), fixed_steps.end(), i + 1) != fixed_steps.end(); CollisionExpressionEvaluatorType expression_evaluator_type{}; if (!current_fixed && !next_fixed) diff --git a/trajopt/src/trajectory_costs.cpp b/trajopt/src/trajectory_costs.cpp index dd952c81..c16fe92c 100644 --- a/trajopt/src/trajectory_costs.cpp +++ b/trajopt/src/trajectory_costs.cpp @@ -117,8 +117,8 @@ double JointPosIneqCost::value(const DblVec& xvec) Eigen::MatrixXd pos = traj.block(first_step_, 0, last_step_ - first_step_ + 1, traj.cols()); // Subtract targets to center about 0 and then subtract from tolerance Eigen::MatrixXd diff0 = (pos.rowwise() - targets_.transpose()); - Eigen::MatrixXd diff1 = (diff0.rowwise() - upper_tols_.transpose()) * coeffs_.asDiagonal(); - Eigen::MatrixXd diff2 = ((diff0 * -1).rowwise() + lower_tols_.transpose()) * coeffs_.asDiagonal(); + const Eigen::MatrixXd diff1 = (diff0.rowwise() - upper_tols_.transpose()) * coeffs_.asDiagonal(); + const Eigen::MatrixXd diff2 = ((diff0 * -1).rowwise() + lower_tols_.transpose()) * coeffs_.asDiagonal(); // Applies hinge, multiplies it by a diagonal matrix of coefficients, sums each corresponding value, and converts to // vector return diff1.cwiseMax(0).sum() + diff2.cwiseMax(0).sum(); @@ -232,8 +232,8 @@ DblVec JointPosIneqConstraint::value(const DblVec& xvec) Eigen::MatrixXd pos = traj.block(first_step_, 0, last_step_ - first_step_ + 1, traj.cols()); // Subtract targets to center about 0 and then subtract from tolerance Eigen::MatrixXd diff0 = (pos.rowwise() - targets_.transpose()); - Eigen::MatrixXd diff1 = (diff0.rowwise() - upper_tols_.transpose()) * coeffs_.asDiagonal(); - Eigen::MatrixXd diff2 = ((diff0 * -1).rowwise() + lower_tols_.transpose()) * coeffs_.asDiagonal(); + const Eigen::MatrixXd diff1 = (diff0.rowwise() - upper_tols_.transpose()) * coeffs_.asDiagonal(); + const Eigen::MatrixXd diff2 = ((diff0 * -1).rowwise() + lower_tols_.transpose()) * coeffs_.asDiagonal(); // Applies hinge, multiplies it by a diagonal matrix of coefficients, sums each corresponding value, and converts to // vector Eigen::MatrixXd out(diff1.rows(), diff1.cols() + diff2.cols()); @@ -348,8 +348,8 @@ double JointVelIneqCost::value(const DblVec& xvec) Eigen::MatrixXd vel = diffAxis0(traj.block(first_step_, 0, last_step_ - first_step_ + 1, traj.cols())); // Subtract targets_ to center about 0 and then subtract from tolerance Eigen::MatrixXd diff0 = (vel.rowwise() - targets_.transpose()); - Eigen::MatrixXd diff1 = (diff0.rowwise() - upper_tols_.transpose()) * coeffs_.asDiagonal(); - Eigen::MatrixXd diff2 = ((diff0 * -1).rowwise() + lower_tols_.transpose()) * coeffs_.asDiagonal(); + const Eigen::MatrixXd diff1 = (diff0.rowwise() - upper_tols_.transpose()) * coeffs_.asDiagonal(); + const Eigen::MatrixXd diff2 = ((diff0 * -1).rowwise() + lower_tols_.transpose()) * coeffs_.asDiagonal(); // Applies hinge, multiplies it by a diagonal matrix of coefficients, sums each corresponding value, and converts to // vector return diff1.cwiseMax(0).sum() + diff2.cwiseMax(0).sum(); @@ -465,8 +465,8 @@ DblVec JointVelIneqConstraint::value(const DblVec& xvec) Eigen::MatrixXd vel = diffAxis0(traj.block(first_step_, 0, last_step_ - first_step_ + 1, traj.cols())); // Subtract targets_ to center about 0 and then subtract from tolerance Eigen::MatrixXd diff0 = (vel.rowwise() - targets_.transpose()); - Eigen::MatrixXd diff1 = (diff0.rowwise() - upper_tols_.transpose()) * coeffs_.asDiagonal(); - Eigen::MatrixXd diff2 = ((diff0 * -1).rowwise() + lower_tols_.transpose()) * coeffs_.asDiagonal(); + const Eigen::MatrixXd diff1 = (diff0.rowwise() - upper_tols_.transpose()) * coeffs_.asDiagonal(); + const Eigen::MatrixXd diff2 = ((diff0 * -1).rowwise() + lower_tols_.transpose()) * coeffs_.asDiagonal(); // Applies hinge, multiplies it by a diagonal matrix of coefficients, sums each corresponding value, and converts to // vector Eigen::MatrixXd out(diff1.rows(), diff1.cols() + diff2.cols()); @@ -585,8 +585,8 @@ double JointAccIneqCost::value(const DblVec& xvec) Eigen::MatrixXd acc = diffAxis0(diffAxis0(traj.block(first_step_, 0, last_step_ - first_step_ + 1, traj.cols()))); // Subtract targets_ to center about 0 and then subtract from tolerance Eigen::MatrixXd diff0 = (acc.rowwise() - targets_.transpose()); - Eigen::MatrixXd diff1 = (diff0.rowwise() - upper_tols_.transpose()) * coeffs_.asDiagonal(); - Eigen::MatrixXd diff2 = ((diff0 * -1).rowwise() + lower_tols_.transpose()) * coeffs_.asDiagonal(); + const Eigen::MatrixXd diff1 = (diff0.rowwise() - upper_tols_.transpose()) * coeffs_.asDiagonal(); + const Eigen::MatrixXd diff2 = ((diff0 * -1).rowwise() + lower_tols_.transpose()) * coeffs_.asDiagonal(); // Applies hinge, multiplies it by a diagonal matrix of coefficients, sums each corresponding value, and converts to // vector return diff1.cwiseMax(0).sum() + diff2.cwiseMax(0).sum(); @@ -707,8 +707,8 @@ DblVec JointAccIneqConstraint::value(const DblVec& xvec) Eigen::MatrixXd acc = diffAxis0(diffAxis0(traj.block(first_step_, 0, last_step_ - first_step_ + 1, traj.cols()))); // Subtract targets_ to center about 0 and then subtract from tolerance Eigen::MatrixXd diff0 = (acc.rowwise() - targets_.transpose()); - Eigen::MatrixXd diff1 = (diff0.rowwise() - upper_tols_.transpose()) * coeffs_.asDiagonal(); - Eigen::MatrixXd diff2 = ((diff0 * -1).rowwise() + lower_tols_.transpose()) * coeffs_.asDiagonal(); + const Eigen::MatrixXd diff1 = (diff0.rowwise() - upper_tols_.transpose()) * coeffs_.asDiagonal(); + const Eigen::MatrixXd diff2 = ((diff0 * -1).rowwise() + lower_tols_.transpose()) * coeffs_.asDiagonal(); // Applies hinge, multiplies it by a diagonal matrix of coefficients, sums each corresponding value, and converts to // vector Eigen::MatrixXd out(diff1.rows(), diff1.cols() + diff2.cols()); @@ -831,8 +831,8 @@ double JointJerkIneqCost::value(const DblVec& xvec) diffAxis0(diffAxis0(diffAxis0(traj.block(first_step_, 0, last_step_ - first_step_ + 1, traj.cols())))); // Subtract targets_ to center about 0 and then subtract from tolerance Eigen::MatrixXd diff0 = (jerk.rowwise() - targets_.transpose()); - Eigen::MatrixXd diff1 = (diff0.rowwise() - upper_tols_.transpose()) * coeffs_.asDiagonal(); - Eigen::MatrixXd diff2 = ((diff0 * -1).rowwise() + lower_tols_.transpose()) * coeffs_.asDiagonal(); + const Eigen::MatrixXd diff1 = (diff0.rowwise() - upper_tols_.transpose()) * coeffs_.asDiagonal(); + const Eigen::MatrixXd diff2 = ((diff0 * -1).rowwise() + lower_tols_.transpose()) * coeffs_.asDiagonal(); // Applies hinge, multiplies it by a diagonal matrix of coefficients, sums each corresponding value, and converts to // vector return diff1.cwiseMax(0).sum() + diff2.cwiseMax(0).sum(); @@ -955,8 +955,8 @@ DblVec JointJerkIneqConstraint::value(const DblVec& xvec) Eigen::MatrixXd acc = diffAxis0(diffAxis0(traj.block(first_step_, 0, last_step_ - first_step_ + 1, traj.cols()))); // Subtract targets_ to center about 0 and then subtract from tolerance Eigen::MatrixXd diff0 = (acc.rowwise() - targets_.transpose()); - Eigen::MatrixXd diff1 = (diff0.rowwise() - upper_tols_.transpose()) * coeffs_.asDiagonal(); - Eigen::MatrixXd diff2 = ((diff0 * -1).rowwise() + lower_tols_.transpose()) * coeffs_.asDiagonal(); + const Eigen::MatrixXd diff1 = (diff0.rowwise() - upper_tols_.transpose()) * coeffs_.asDiagonal(); + const Eigen::MatrixXd diff2 = ((diff0 * -1).rowwise() + lower_tols_.transpose()) * coeffs_.asDiagonal(); // Applies hinge, multiplies it by a diagonal matrix of coefficients, sums each corresponding value, and converts to // vector Eigen::MatrixXd out(diff1.rows(), diff1.cols() + diff2.cols()); diff --git a/trajopt/src/utils.cpp b/trajopt/src/utils.cpp index c8752d8e..f3626c6e 100644 --- a/trajopt/src/utils.cpp +++ b/trajopt/src/utils.cpp @@ -41,7 +41,7 @@ void AddVarArrays(sco::OptProb& prob, const std::vector& name_prefix, const std::vector& newvars) { - std::size_t n_arr = name_prefix.size(); + const std::size_t n_arr = name_prefix.size(); assert(static_cast(n_arr) == newvars.size()); std::vector index(n_arr); @@ -82,9 +82,9 @@ void AddVarArrays(sco::OptProb& prob, void AddVarArray(sco::OptProb& prob, int rows, int cols, const std::string& name_prefix, VarArray& newvars) { - std::vector arrs(1, &newvars); - std::vector prefixes(1, name_prefix); - std::vector colss(1, cols); + const std::vector arrs(1, &newvars); + const std::vector prefixes(1, name_prefix); + const std::vector colss(1, cols); AddVarArrays(prob, rows, colss, prefixes, arrs); } diff --git a/trajopt/test/cart_position_optimization_unit.cpp b/trajopt/test/cart_position_optimization_unit.cpp index d6e873d0..ccf540d0 100644 --- a/trajopt/test/cart_position_optimization_unit.cpp +++ b/trajopt/test/cart_position_optimization_unit.cpp @@ -68,14 +68,14 @@ TEST(CartPositionOptimizationTrajoptSCO, cart_position_optimization_trajopt_sco) } // 1) Load Robot - tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/arm_around_table.urdf"); - tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/pr2.srdf"); - ResourceLocator::Ptr locator = std::make_shared(); + const tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/arm_around_table.urdf"); + const tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/pr2.srdf"); + const ResourceLocator::Ptr locator = std::make_shared(); auto env = std::make_shared(); env->init(urdf_file, srdf_file, locator); // Extract necessary kinematic information - tesseract_kinematics::JointGroup::ConstPtr manip = env->getJointGroup("right_arm"); + const tesseract_kinematics::JointGroup::ConstPtr manip = env->getJointGroup("right_arm"); ProblemConstructionInfo pci(env); @@ -88,11 +88,11 @@ TEST(CartPositionOptimizationTrajoptSCO, cart_position_optimization_trajopt_sco) pci.kin = manip; // Populate Init Info - SceneState current_state = pci.env->getState(); + const SceneState current_state = pci.env->getState(); Eigen::VectorXd start_pos(manip->numJoints()); start_pos << 0, 0, 0, -1.0, 0, -1, 0.0; if (DEBUG) - std::cout << "Joint Limits:\n" << manip->getLimits().joint_limits.transpose() << std::endl; + std::cout << "Joint Limits:\n" << manip->getLimits().joint_limits.transpose() << '\n'; pci.init_info.type = InitInfo::GIVEN_TRAJ; pci.init_info.data = tesseract_common::TrajArray(1, pci.kin->numJoints()); @@ -103,7 +103,7 @@ TEST(CartPositionOptimizationTrajoptSCO, cart_position_optimization_trajopt_sco) auto target_pose = manip->calcFwdKin(joint_target).at("r_gripper_tool_frame"); if (DEBUG) - std::cout << "target_pose:\n" << target_pose.matrix() << std::endl; + std::cout << "target_pose:\n" << target_pose.matrix() << '\n'; { auto pose = std::make_shared(); @@ -132,13 +132,13 @@ TEST(CartPositionOptimizationTrajoptSCO, cart_position_optimization_trajopt_sco) auto optimized_pose = manip->calcFwdKin(traj.row(0)).at("r_gripper_tool_frame"); EXPECT_TRUE(target_pose.translation().isApprox(optimized_pose.translation(), 1e-4)); - Eigen::Quaterniond target_q(target_pose.rotation()); - Eigen::Quaterniond optimized_q(optimized_pose.rotation()); + const Eigen::Quaterniond target_q(target_pose.rotation()); + const Eigen::Quaterniond optimized_q(optimized_pose.rotation()); EXPECT_TRUE(target_q.isApprox(optimized_q, 1e-5)); if (DEBUG) { - std::cout << "Initial: " << prob->GetInitTraj() << std::endl; - std::cout << "Results: " << traj << std::endl; + std::cout << "Initial: " << prob->GetInitTraj() << '\n'; + std::cout << "Results: " << traj << '\n'; } } diff --git a/trajopt/test/cast_cost_attached_unit.cpp b/trajopt/test/cast_cost_attached_unit.cpp index 5c2f8f0b..76bce085 100644 --- a/trajopt/test/cast_cost_attached_unit.cpp +++ b/trajopt/test/cast_cost_attached_unit.cpp @@ -50,10 +50,10 @@ class CastAttachedTest : public testing::TestWithParam void SetUp() override { - tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/boxbot.urdf"); - tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/boxbot.srdf"); + const tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/boxbot.urdf"); + const tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/boxbot.srdf"); - ResourceLocator::Ptr locator = std::make_shared(); + const ResourceLocator::Ptr locator = std::make_shared(); EXPECT_TRUE(env_->init(urdf_file, srdf_file, locator)); gLogLevel = trajopt_common::LevelError; @@ -113,7 +113,7 @@ void runLinkWithGeomTest(const Environment::Ptr& env, const Visualization::Ptr& env->applyCommand(std::make_shared("box_attached", true)); - Json::Value root = readJsonFile(std::string(TRAJOPT_DATA_DIR) + "/config/box_cast_test.json"); + const Json::Value root = readJsonFile(std::string(TRAJOPT_DATA_DIR) + "/config/box_cast_test.json"); std::unordered_map ipos; ipos["boxbot_x_joint"] = -1.9; @@ -122,12 +122,12 @@ void runLinkWithGeomTest(const Environment::Ptr& env, const Visualization::Ptr& // plotter_->plotScene(); - TrajOptProb::Ptr prob = ConstructProblem(root, env); + const TrajOptProb::Ptr prob = ConstructProblem(root, env); ASSERT_TRUE(!!prob); std::vector collisions; - tesseract_scene_graph::StateSolver::UPtr state_solver = prob->GetEnv()->getStateSolver(); - ContinuousContactManager::Ptr manager = prob->GetEnv()->getContinuousContactManager(); + const tesseract_scene_graph::StateSolver::UPtr state_solver = prob->GetEnv()->getStateSolver(); + const ContinuousContactManager::Ptr manager = prob->GetEnv()->getContinuousContactManager(); manager->setActiveCollisionObjects(prob->GetKin()->getActiveLinkNames()); manager->setDefaultCollisionMarginData(0); @@ -173,7 +173,7 @@ void runLinkWithoutGeomTest(const Environment::Ptr& env, const Visualization::Pt env->applyCommand(std::make_shared("box_attached2", true)); - Json::Value root = readJsonFile(std::string(TRAJOPT_DATA_DIR) + "/config/box_cast_test.json"); + const Json::Value root = readJsonFile(std::string(TRAJOPT_DATA_DIR) + "/config/box_cast_test.json"); std::unordered_map ipos; ipos["boxbot_x_joint"] = -1.9; @@ -182,12 +182,12 @@ void runLinkWithoutGeomTest(const Environment::Ptr& env, const Visualization::Pt // plotter_->plotScene(); - TrajOptProb::Ptr prob = ConstructProblem(root, env); + const TrajOptProb::Ptr prob = ConstructProblem(root, env); ASSERT_TRUE(!!prob); std::vector collisions; - tesseract_scene_graph::StateSolver::UPtr state_solver = prob->GetEnv()->getStateSolver(); - ContinuousContactManager::Ptr manager = prob->GetEnv()->getContinuousContactManager(); + const tesseract_scene_graph::StateSolver::UPtr state_solver = prob->GetEnv()->getStateSolver(); + const ContinuousContactManager::Ptr manager = prob->GetEnv()->getContinuousContactManager(); manager->setActiveCollisionObjects(prob->GetKin()->getActiveLinkNames()); manager->setDefaultCollisionMarginData(0); diff --git a/trajopt/test/cast_cost_octomap_unit.cpp b/trajopt/test/cast_cost_octomap_unit.cpp index 3e314fa1..ecf2d491 100644 --- a/trajopt/test/cast_cost_octomap_unit.cpp +++ b/trajopt/test/cast_cost_octomap_unit.cpp @@ -52,10 +52,10 @@ class CastOctomapTest : public testing::TestWithParam void SetUp() override { - tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/boxbot_world.urdf"); - tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/boxbot.srdf"); + const tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/boxbot_world.urdf"); + const tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/boxbot.srdf"); - ResourceLocator::Ptr locator = std::make_shared(); + const ResourceLocator::Ptr locator = std::make_shared(); EXPECT_TRUE(env_->init(urdf_file, srdf_file, locator)); gLogLevel = trajopt_common::LevelError; @@ -64,7 +64,7 @@ class CastOctomapTest : public testing::TestWithParam // plotter_.reset(new tesseract_ros::ROSBasicPlotting(env_)); octomap::Pointcloud point_cloud; - double delta = 0.05; + const double delta = 0.05; auto length = static_cast(1 / delta); for (int x = 0; x < length; ++x) @@ -74,11 +74,11 @@ class CastOctomapTest : public testing::TestWithParam -0.5F + static_cast(y * delta), -0.5F + static_cast(z * delta)); - std::shared_ptr octree = std::make_shared(2 * delta); + const std::shared_ptr octree = std::make_shared(2 * delta); octree->insertPointCloud(point_cloud, octomap::point3d(0, 0, 0)); // Next add objects that can be attached/detached to the scene - Octree::Ptr coll_octree = std::make_shared(octree, OctreeSubType::BOX); + const Octree::Ptr coll_octree = std::make_shared(octree, OctreeSubType::BOX); auto vis_box = std::make_shared(1.0, 1.0, 1.0); auto visual = std::make_shared(); @@ -106,7 +106,7 @@ void runTest(const Environment::Ptr& env, const Visualization::Ptr& plotter, boo { CONSOLE_BRIDGE_logDebug("CastOctomapTest, boxes"); - Json::Value root = readJsonFile(std::string(TRAJOPT_DATA_DIR) + "/config/box_cast_test.json"); + const Json::Value root = readJsonFile(std::string(TRAJOPT_DATA_DIR) + "/config/box_cast_test.json"); std::unordered_map ipos; ipos["boxbot_x_joint"] = -1.9; @@ -115,12 +115,12 @@ void runTest(const Environment::Ptr& env, const Visualization::Ptr& plotter, boo // plotter_->plotScene(); - TrajOptProb::Ptr prob = ConstructProblem(root, env); + const TrajOptProb::Ptr prob = ConstructProblem(root, env); ASSERT_TRUE(!!prob); std::vector collisions; - tesseract_scene_graph::StateSolver::UPtr state_solver = prob->GetEnv()->getStateSolver(); - ContinuousContactManager::Ptr manager = prob->GetEnv()->getContinuousContactManager(); + const tesseract_scene_graph::StateSolver::UPtr state_solver = prob->GetEnv()->getStateSolver(); + const ContinuousContactManager::Ptr manager = prob->GetEnv()->getContinuousContactManager(); manager->setActiveCollisionObjects(prob->GetKin()->getActiveLinkNames()); manager->setDefaultCollisionMarginData(0); diff --git a/trajopt/test/cast_cost_unit.cpp b/trajopt/test/cast_cost_unit.cpp index 5b853a96..0d868b09 100644 --- a/trajopt/test/cast_cost_unit.cpp +++ b/trajopt/test/cast_cost_unit.cpp @@ -44,10 +44,10 @@ class CastTest : public testing::TestWithParam void SetUp() override { - boost::filesystem::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/boxbot.urdf"); - boost::filesystem::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/boxbot.srdf"); + const boost::filesystem::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/boxbot.urdf"); + const boost::filesystem::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/boxbot.srdf"); - ResourceLocator::Ptr locator = std::make_shared(); + const ResourceLocator::Ptr locator = std::make_shared(); EXPECT_TRUE(env_->init(urdf_file, srdf_file, locator)); gLogLevel = trajopt_common::LevelError; @@ -61,7 +61,7 @@ void runTest(const Environment::Ptr& env, const Visualization::Ptr& plotter, boo { CONSOLE_BRIDGE_logDebug("CastTest, boxes"); - Json::Value root = readJsonFile(std::string(TRAJOPT_DATA_DIR) + "/config/box_cast_test.json"); + const Json::Value root = readJsonFile(std::string(TRAJOPT_DATA_DIR) + "/config/box_cast_test.json"); std::unordered_map ipos; ipos["boxbot_x_joint"] = -1.9; @@ -70,12 +70,12 @@ void runTest(const Environment::Ptr& env, const Visualization::Ptr& plotter, boo // plotter_->plotScene(); - TrajOptProb::Ptr prob = ConstructProblem(root, env); + const TrajOptProb::Ptr prob = ConstructProblem(root, env); ASSERT_TRUE(!!prob); std::vector collisions; - tesseract_scene_graph::StateSolver::UPtr state_solver = prob->GetEnv()->getStateSolver(); - ContinuousContactManager::Ptr manager = prob->GetEnv()->getContinuousContactManager(); + const tesseract_scene_graph::StateSolver::UPtr state_solver = prob->GetEnv()->getStateSolver(); + const ContinuousContactManager::Ptr manager = prob->GetEnv()->getContinuousContactManager(); manager->setActiveCollisionObjects(prob->GetKin()->getActiveLinkNames()); manager->setDefaultCollisionMarginData(0); @@ -109,7 +109,7 @@ void runTest(const Environment::Ptr& env, const Visualization::Ptr& plotter, boo plotter->clear(); collisions.clear(); - std::cout << getTraj(opt->x(), prob->GetVars()) << std::endl; + std::cout << getTraj(opt->x(), prob->GetVars()) << '\n'; found = checkTrajectory( collisions, *manager, *state_solver, prob->GetKin()->getJointNames(), getTraj(opt->x(), prob->GetVars()), config); diff --git a/trajopt/test/cast_cost_world_unit.cpp b/trajopt/test/cast_cost_world_unit.cpp index de8601ec..057f1d86 100644 --- a/trajopt/test/cast_cost_world_unit.cpp +++ b/trajopt/test/cast_cost_world_unit.cpp @@ -49,10 +49,10 @@ class CastWorldTest : public testing::TestWithParam void SetUp() override { - tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/boxbot_world.urdf"); - tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/boxbot.srdf"); + const tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/boxbot_world.urdf"); + const tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/boxbot.srdf"); - ResourceLocator::Ptr locator = std::make_shared(); + const ResourceLocator::Ptr locator = std::make_shared(); EXPECT_TRUE(env_->init(urdf_file, srdf_file, locator)); gLogLevel = trajopt_common::LevelError; @@ -90,7 +90,7 @@ void runTest(const Environment::Ptr& env, const Visualization::Ptr& plotter, boo { CONSOLE_BRIDGE_logDebug("CastWorldTest, boxes"); - Json::Value root = readJsonFile(std::string(TRAJOPT_DATA_DIR) + "/config/box_cast_test.json"); + const Json::Value root = readJsonFile(std::string(TRAJOPT_DATA_DIR) + "/config/box_cast_test.json"); std::unordered_map ipos; ipos["boxbot_x_joint"] = -1.9; @@ -99,12 +99,12 @@ void runTest(const Environment::Ptr& env, const Visualization::Ptr& plotter, boo // plotter_->plotScene(); - TrajOptProb::Ptr prob = ConstructProblem(root, env); + const TrajOptProb::Ptr prob = ConstructProblem(root, env); ASSERT_TRUE(!!prob); std::vector collisions; - tesseract_scene_graph::StateSolver::UPtr state_solver = prob->GetEnv()->getStateSolver(); - ContinuousContactManager::Ptr manager = prob->GetEnv()->getContinuousContactManager(); + const tesseract_scene_graph::StateSolver::UPtr state_solver = prob->GetEnv()->getStateSolver(); + const ContinuousContactManager::Ptr manager = prob->GetEnv()->getContinuousContactManager(); manager->setActiveCollisionObjects(prob->GetKin()->getActiveLinkNames()); manager->setDefaultCollisionMarginData(0); diff --git a/trajopt/test/interface_unit.cpp b/trajopt/test/interface_unit.cpp index 13147ffc..541fa4d8 100644 --- a/trajopt/test/interface_unit.cpp +++ b/trajopt/test/interface_unit.cpp @@ -77,7 +77,7 @@ TEST_F(InterfaceTest, initial_trajectory_cpp_interface) jv->term_type = TT_COST; pci.cost_infos.push_back(jv); - TrajOptProb::Ptr prob = ConstructProblem(pci); + const TrajOptProb::Ptr prob = ConstructProblem(pci); ASSERT_TRUE(!!prob); trajopt::TrajArray initial_trajectory = prob->GetInitTraj(); @@ -127,7 +127,7 @@ TEST_F(InterfaceTest, initial_trajectory_time_cpp_interface) jv->term_type = TT_COST | TT_USE_TIME; pci.cost_infos.push_back(jv); - TrajOptProb::Ptr prob = ConstructProblem(pci); + const TrajOptProb::Ptr prob = ConstructProblem(pci); ASSERT_TRUE(!!prob); trajopt::TrajArray initial_trajectory = prob->GetInitTraj(); @@ -173,7 +173,7 @@ TEST_F(InterfaceTest, initial_trajectory_json_interface) ipos["r_wrist_roll_joint"] = 0; env_->setState(ipos); - TrajOptProb::Ptr prob = ConstructProblem(root, env_, env_->getFwdKinematics()); + const TrajOptProb::Ptr prob = ConstructProblem(root, env_, env_->getFwdKinematics()); ASSERT_TRUE(!!prob); trajopt::TrajArray initial_trajectory = prob->GetInitTraj(); @@ -208,7 +208,7 @@ TEST_F(InterfaceTest, initial_trajectory_time_json_interface) ipos["r_wrist_roll_joint"] = 0; env_->setState(ipos); - TrajOptProb::Ptr prob = ConstructProblem(root, env_, env_->getFwdKinematics()); + const TrajOptProb::Ptr prob = ConstructProblem(root, env_, env_->getFwdKinematics()); ASSERT_TRUE(!!prob); trajopt::TrajArray initial_trajectory = prob->GetInitTraj(); diff --git a/trajopt/test/joint_costs_unit.cpp b/trajopt/test/joint_costs_unit.cpp index 144e66e3..5ffdef3a 100644 --- a/trajopt/test/joint_costs_unit.cpp +++ b/trajopt/test/joint_costs_unit.cpp @@ -41,10 +41,10 @@ class CostsTest : public testing::TestWithParam void SetUp() override { - tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/arm_around_table.urdf"); - tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/pr2.srdf"); + const tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/arm_around_table.urdf"); + const tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/pr2.srdf"); - ResourceLocator::Ptr locator = std::make_shared(); + const ResourceLocator::Ptr locator = std::make_shared(); EXPECT_TRUE(env_->init(urdf_file, srdf_file, locator)); gLogLevel = trajopt_common::LevelError; @@ -104,7 +104,7 @@ TEST_F(CostsTest, equality_jointPos) // NOLINT jv2->term_type = TermType::TT_COST; pci.cost_infos.push_back(jv2); - TrajOptProb::Ptr prob = ConstructProblem(pci); + const TrajOptProb::Ptr prob = ConstructProblem(pci); ASSERT_TRUE(!!prob); sco::BasicTrustRegionSQP opt(prob); @@ -114,7 +114,7 @@ TEST_F(CostsTest, equality_jointPos) // NOLINT } opt.initialize(trajToDblVec(prob->GetInitTraj())); - double tStart = GetClock(); + const double tStart = GetClock(); opt.optimize(); @@ -211,7 +211,7 @@ TEST_F(CostsTest, inequality_jointPos) // NOLINT jv3->term_type = TermType::TT_COST; pci.cost_infos.push_back(jv3); - TrajOptProb::Ptr prob = ConstructProblem(pci); + const TrajOptProb::Ptr prob = ConstructProblem(pci); ASSERT_TRUE(!!prob); sco::BasicTrustRegionSQP opt(prob); @@ -221,7 +221,7 @@ TEST_F(CostsTest, inequality_jointPos) // NOLINT } opt.initialize(trajToDblVec(prob->GetInitTraj())); - double tStart = GetClock(); + const double tStart = GetClock(); opt.optimize(); CONSOLE_BRIDGE_logDebug("planning time: %.3f", GetClock() - tStart); @@ -306,7 +306,7 @@ TEST_F(CostsTest, equality_jointVel) // NOLINT jv2->term_type = TermType::TT_COST; pci.cost_infos.push_back(jv2); - TrajOptProb::Ptr prob = ConstructProblem(pci); + const TrajOptProb::Ptr prob = ConstructProblem(pci); ASSERT_TRUE(!!prob); sco::BasicTrustRegionSQP opt(prob); @@ -316,7 +316,7 @@ TEST_F(CostsTest, equality_jointVel) // NOLINT } opt.initialize(trajToDblVec(prob->GetInitTraj())); - double tStart = GetClock(); + const double tStart = GetClock(); opt.optimize(); @@ -413,7 +413,7 @@ TEST_F(CostsTest, inequality_jointVel) // NOLINT jv3->term_type = TermType::TT_COST; pci.cost_infos.push_back(jv3); - TrajOptProb::Ptr prob = ConstructProblem(pci); + const TrajOptProb::Ptr prob = ConstructProblem(pci); ASSERT_TRUE(!!prob); sco::BasicTrustRegionSQP opt(prob); @@ -423,7 +423,7 @@ TEST_F(CostsTest, inequality_jointVel) // NOLINT } opt.initialize(trajToDblVec(prob->GetInitTraj())); - double tStart = GetClock(); + const double tStart = GetClock(); opt.optimize(); CONSOLE_BRIDGE_logDebug("planning time: %.3f", GetClock() - tStart); @@ -511,7 +511,7 @@ TEST_F(CostsTest, equality_jointVel_time) // NOLINT jv2->term_type = TermType::TT_COST | TermType::TT_USE_TIME; pci.cost_infos.push_back(jv2); - TrajOptProb::Ptr prob = ConstructProblem(pci); + const TrajOptProb::Ptr prob = ConstructProblem(pci); ASSERT_TRUE(!!prob); sco::BasicTrustRegionSQP opt(prob); @@ -521,7 +521,7 @@ TEST_F(CostsTest, equality_jointVel_time) // NOLINT } opt.initialize(trajToDblVec(prob->GetInitTraj())); - double tStart = GetClock(); + const double tStart = GetClock(); opt.optimize(); @@ -624,7 +624,7 @@ TEST_F(CostsTest, inequality_jointVel_time) // NOLINT jv3->term_type = TermType::TT_COST | TermType::TT_USE_TIME; pci.cost_infos.push_back(jv3); - TrajOptProb::Ptr prob = ConstructProblem(pci); + const TrajOptProb::Ptr prob = ConstructProblem(pci); ASSERT_TRUE(!!prob); sco::BasicTrustRegionSQP opt(prob); @@ -634,7 +634,7 @@ TEST_F(CostsTest, inequality_jointVel_time) // NOLINT } opt.initialize(trajToDblVec(prob->GetInitTraj())); - double tStart = GetClock(); + const double tStart = GetClock(); opt.optimize(); CONSOLE_BRIDGE_logDebug("planning time: %.3f", GetClock() - tStart); @@ -719,7 +719,7 @@ TEST_F(CostsTest, equality_jointAcc) // NOLINT jv2->term_type = TermType::TT_COST; pci.cost_infos.push_back(jv2); - TrajOptProb::Ptr prob = ConstructProblem(pci); + const TrajOptProb::Ptr prob = ConstructProblem(pci); ASSERT_TRUE(!!prob); sco::BasicTrustRegionSQP opt(prob); @@ -729,7 +729,7 @@ TEST_F(CostsTest, equality_jointAcc) // NOLINT } opt.initialize(trajToDblVec(prob->GetInitTraj())); - double tStart = GetClock(); + const double tStart = GetClock(); opt.optimize(); @@ -740,7 +740,7 @@ TEST_F(CostsTest, equality_jointAcc) // NOLINT double accel{ std::numeric_limits::max() }; for (auto j = 0; j < output.cols(); ++j) { - int i = 0; + const int i = 0; accel = output(i, j) - 2 * output(i + 1, j) + output(i + 2, j); EXPECT_NEAR(accel, cnt_targ, cnt_tol); } @@ -828,7 +828,7 @@ TEST_F(CostsTest, inequality_jointAcc) // NOLINT jv3->term_type = TermType::TT_COST; pci.cost_infos.push_back(jv3); - TrajOptProb::Ptr prob = ConstructProblem(pci); + const TrajOptProb::Ptr prob = ConstructProblem(pci); ASSERT_TRUE(!!prob); sco::BasicTrustRegionSQP opt(prob); @@ -838,7 +838,7 @@ TEST_F(CostsTest, inequality_jointAcc) // NOLINT } opt.initialize(trajToDblVec(prob->GetInitTraj())); - double tStart = GetClock(); + const double tStart = GetClock(); opt.optimize(); CONSOLE_BRIDGE_logDebug("planning time: %.3f", GetClock() - tStart); diff --git a/trajopt/test/kinematic_costs_unit.cpp b/trajopt/test/kinematic_costs_unit.cpp index 9cbaebdb..c5d283b4 100644 --- a/trajopt/test/kinematic_costs_unit.cpp +++ b/trajopt/test/kinematic_costs_unit.cpp @@ -39,32 +39,34 @@ class KinematicCostsTest : public testing::Test void SetUp() override { - tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/arm_around_table.urdf"); - tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/pr2.srdf"); + const tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/arm_around_table.urdf"); + const tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/pr2.srdf"); - ResourceLocator::Ptr locator = std::make_shared(); + const ResourceLocator::Ptr locator = std::make_shared(); EXPECT_TRUE(env_->init(urdf_file, srdf_file, locator)); gLogLevel = trajopt_common::LevelError; } }; -static std::string toString(const Eigen::MatrixXd& mat) +namespace +{ +std::string toString(const Eigen::MatrixXd& mat) { std::stringstream ss; ss << mat; return ss.str(); } -static void checkJacobian(const sco::VectorOfVector& f, - const sco::MatrixOfVector& dfdx, - const Eigen::VectorXd& values, - const double epsilon) +void checkJacobian(const sco::VectorOfVector& f, + const sco::MatrixOfVector& dfdx, + const Eigen::VectorXd& values, + const double epsilon) { - Eigen::MatrixXd numerical = sco::calcForwardNumJac(f, values, epsilon); - Eigen::MatrixXd analytical = dfdx(values); + const Eigen::MatrixXd numerical = sco::calcForwardNumJac(f, values, epsilon); + const Eigen::MatrixXd analytical = dfdx(values); - bool pass = numerical.isApprox(analytical, 1e-5); + const bool pass = numerical.isApprox(analytical, 1e-5); EXPECT_TRUE(pass); if (!pass) { @@ -72,24 +74,25 @@ static void checkJacobian(const sco::VectorOfVector& f, CONSOLE_BRIDGE_logError("Analytical:\n %s", toString(analytical).c_str()); } } +} // namespace TEST_F(KinematicCostsTest, CartPoseJacCalculator) // NOLINT { CONSOLE_BRIDGE_logDebug("KinematicCostsTest, CartPoseJacCalculator"); - tesseract_kinematics::JointGroup::Ptr kin = env_->getJointGroup("right_arm"); + const tesseract_kinematics::JointGroup::Ptr kin = env_->getJointGroup("right_arm"); - std::string source_frame = env_->getRootLinkName(); - std::string target_frame = "r_gripper_tool_frame"; - Eigen::Isometry3d source_frame_offset = env_->getState().link_transforms.at(target_frame); - Eigen::Isometry3d target_frame_offset = + const std::string source_frame = env_->getRootLinkName(); + const std::string target_frame = "r_gripper_tool_frame"; + const Eigen::Isometry3d source_frame_offset = env_->getState().link_transforms.at(target_frame); + const Eigen::Isometry3d target_frame_offset = Eigen::Isometry3d::Identity() * Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitZ()); Eigen::VectorXd values(7); values << -1.1, 1.2, -3.3, -1.4, 5.5, -1.6, 7.7; - CartPoseErrCalculator f(kin, source_frame, target_frame, source_frame_offset, target_frame_offset); - CartPoseJacCalculator dfdx(kin, source_frame, target_frame, source_frame_offset, target_frame_offset); + const CartPoseErrCalculator f(kin, source_frame, target_frame, source_frame_offset, target_frame_offset); + const CartPoseJacCalculator dfdx(kin, source_frame, target_frame, source_frame_offset, target_frame_offset); checkJacobian(f, dfdx, values, 1.0e-5); } diff --git a/trajopt/test/numerical_ik_unit.cpp b/trajopt/test/numerical_ik_unit.cpp index e2ba8bbd..4a5a3af5 100644 --- a/trajopt/test/numerical_ik_unit.cpp +++ b/trajopt/test/numerical_ik_unit.cpp @@ -40,10 +40,10 @@ class NumericalIKTest : public testing::TestWithParam Visualization::Ptr plotter_; /**< Trajopt Plotter */ void SetUp() override { - tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/arm_around_table.urdf"); - tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/pr2.srdf"); + const tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/arm_around_table.urdf"); + const tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/pr2.srdf"); - ResourceLocator::Ptr locator = std::make_shared(); + const ResourceLocator::Ptr locator = std::make_shared(); EXPECT_TRUE(env_->init(urdf_file, srdf_file, locator)); // Create plotting tool @@ -61,14 +61,14 @@ void runTest(const Environment::Ptr& env, const Visualization::Ptr& /*plotter*/, { CONSOLE_BRIDGE_logDebug("NumericalIKTest, numerical_ik1"); - Json::Value root = readJsonFile(std::string(TRAJOPT_DATA_DIR) + "/config/numerical_ik1.json"); + const Json::Value root = readJsonFile(std::string(TRAJOPT_DATA_DIR) + "/config/numerical_ik1.json"); // plotter_->plotScene(); ProblemConstructionInfo pci(env); pci.fromJson(root); pci.basic_info.convex_solver = sco::ModelType::OSQP; - TrajOptProb::Ptr prob = ConstructProblem(pci); + const TrajOptProb::Ptr prob = ConstructProblem(pci); ASSERT_TRUE(!!prob); sco::BasicTrustRegionSQP::Ptr opt; @@ -89,12 +89,12 @@ void runTest(const Environment::Ptr& env, const Visualization::Ptr& /*plotter*/, CONSOLE_BRIDGE_logDebug("DOF: %d", prob->GetNumDOF()); opt->initialize(DblVec(static_cast(prob->GetNumDOF()), 0)); - double tStart = GetClock(); + const double tStart = GetClock(); CONSOLE_BRIDGE_logDebug("Size: %d", opt->x().size()); std::stringstream ss; ss << toVectorXd(opt->x()).transpose(); CONSOLE_BRIDGE_logDebug("Initial Vars: %s", ss.str().c_str()); - Eigen::Isometry3d change_base = prob->GetEnv()->getLinkTransform(prob->GetKin()->getBaseLinkName()); + const Eigen::Isometry3d change_base = prob->GetEnv()->getLinkTransform(prob->GetKin()->getBaseLinkName()); Eigen::Isometry3d initial_pose = prob->GetKin()->calcFwdKin(toVectorXd(opt->x())).at("l_gripper_tool_frame"); initial_pose = change_base * initial_pose; @@ -103,7 +103,7 @@ void runTest(const Environment::Ptr& env, const Visualization::Ptr& /*plotter*/, CONSOLE_BRIDGE_logDebug("Initial Position: %s", ss.str().c_str()); tesseract_common::Timer stopwatch; stopwatch.start(); - sco::OptStatus status = opt->optimize(); + const sco::OptStatus status = opt->optimize(); stopwatch.stop(); CONSOLE_BRIDGE_logError("Test took %f seconds.", stopwatch.elapsedSeconds()); CONSOLE_BRIDGE_logDebug("Status: %s", sco::statusToString(status).c_str()); diff --git a/trajopt/test/planning_unit.cpp b/trajopt/test/planning_unit.cpp index de47a865..294874bd 100644 --- a/trajopt/test/planning_unit.cpp +++ b/trajopt/test/planning_unit.cpp @@ -1,7 +1,6 @@ #include TRAJOPT_IGNORE_WARNINGS_PUSH #include -#include #include #include #include @@ -44,10 +43,10 @@ class PlanningTest : public testing::TestWithParam Visualization::Ptr plotter_; /**< Trajopt Plotter */ void SetUp() override { - tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/arm_around_table.urdf"); - tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/pr2.srdf"); + const tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/arm_around_table.urdf"); + const tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/pr2.srdf"); - ResourceLocator::Ptr locator = std::make_shared(); + const ResourceLocator::Ptr locator = std::make_shared(); EXPECT_TRUE(env_->init(urdf_file, srdf_file, locator)); // Create plotting tool @@ -65,7 +64,7 @@ void runTest(const Environment::Ptr& env, bool use_multi_threaded) { CONSOLE_BRIDGE_logDebug("PlanningTest, arm_around_table"); - Json::Value root = readJsonFile(std::string(TRAJOPT_DATA_DIR) + "/config/arm_around_table.json"); + const Json::Value root = readJsonFile(std::string(TRAJOPT_DATA_DIR) + "/config/arm_around_table.json"); std::unordered_map ipos; ipos["torso_lift_joint"] = 0; @@ -83,12 +82,12 @@ void runTest(const Environment::Ptr& env, bool use_multi_threaded) ProblemConstructionInfo pci(env); pci.fromJson(root); pci.basic_info.convex_solver = sco::ModelType::OSQP; - TrajOptProb::Ptr prob = ConstructProblem(pci); + const TrajOptProb::Ptr prob = ConstructProblem(pci); ASSERT_TRUE(!!prob); std::vector collisions; - tesseract_scene_graph::StateSolver::UPtr state_solver = prob->GetEnv()->getStateSolver(); - ContinuousContactManager::Ptr manager = prob->GetEnv()->getContinuousContactManager(); + const tesseract_scene_graph::StateSolver::UPtr state_solver = prob->GetEnv()->getStateSolver(); + const ContinuousContactManager::Ptr manager = prob->GetEnv()->getContinuousContactManager(); manager->setActiveCollisionObjects(prob->GetKin()->getActiveLinkNames()); manager->setDefaultCollisionMarginData(0); @@ -120,8 +119,8 @@ void runTest(const Environment::Ptr& env, bool use_multi_threaded) // } opt->initialize(trajToDblVec(prob->GetInitTraj())); - double tStart = GetClock(); - sco::OptStatus status = opt->optimize(); + const double tStart = GetClock(); + const sco::OptStatus status = opt->optimize(); EXPECT_TRUE(status == sco::OptStatus::OPT_CONVERGED); CONSOLE_BRIDGE_logDebug("planning time: %.3f", GetClock() - tStart); diff --git a/trajopt/test/simple_collision_unit.cpp b/trajopt/test/simple_collision_unit.cpp index 79ef1317..85afa3a4 100644 --- a/trajopt/test/simple_collision_unit.cpp +++ b/trajopt/test/simple_collision_unit.cpp @@ -45,10 +45,10 @@ class SimpleCollisionTest : public testing::TestWithParam void SetUp() override { - boost::filesystem::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/spherebot.urdf"); - boost::filesystem::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/spherebot.srdf"); + const boost::filesystem::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/spherebot.urdf"); + const boost::filesystem::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/spherebot.srdf"); - ResourceLocator::Ptr locator = std::make_shared(); + const ResourceLocator::Ptr locator = std::make_shared(); EXPECT_TRUE(env_->init(urdf_file, srdf_file, locator)); // gLogLevel = trajopt_common::LevelDebug; @@ -62,7 +62,7 @@ void runTest(const Environment::Ptr& env, const Visualization::Ptr& plotter, boo { CONSOLE_BRIDGE_logDebug("SimpleCollisionTest, spheres"); - Json::Value root = readJsonFile(std::string(TRAJOPT_DATA_DIR) + "/config/simple_collision_test.json"); + const Json::Value root = readJsonFile(std::string(TRAJOPT_DATA_DIR) + "/config/simple_collision_test.json"); std::unordered_map ipos; ipos["spherebot_x_joint"] = -0.75; @@ -71,12 +71,12 @@ void runTest(const Environment::Ptr& env, const Visualization::Ptr& plotter, boo // plotter_->plotScene(); - TrajOptProb::Ptr prob = ConstructProblem(root, env); + const TrajOptProb::Ptr prob = ConstructProblem(root, env); ASSERT_TRUE(!!prob); std::vector collisions; - tesseract_scene_graph::StateSolver::UPtr state_solver = prob->GetEnv()->getStateSolver(); - DiscreteContactManager::Ptr manager = prob->GetEnv()->getDiscreteContactManager(); + const tesseract_scene_graph::StateSolver::UPtr state_solver = prob->GetEnv()->getStateSolver(); + const DiscreteContactManager::Ptr manager = prob->GetEnv()->getDiscreteContactManager(); manager->setActiveCollisionObjects(prob->GetKin()->getActiveLinkNames()); manager->setDefaultCollisionMarginData(0.2); @@ -115,7 +115,7 @@ void runTest(const Environment::Ptr& env, const Visualization::Ptr& plotter, boo plotter->clear(); collisions.clear(); - std::cout << getTraj(opt->x(), prob->GetVars()) << std::endl; + std::cout << getTraj(opt->x(), prob->GetVars()) << '\n'; found = checkTrajectory( collisions, *manager, *state_solver, prob->GetKin()->getJointNames(), getTraj(opt->x(), prob->GetVars()), config); diff --git a/trajopt/test/trajopt_test_utils.hpp b/trajopt/test/trajopt_test_utils.hpp index d4d47f59..75267c55 100644 --- a/trajopt/test/trajopt_test_utils.hpp +++ b/trajopt/test/trajopt_test_utils.hpp @@ -13,7 +13,7 @@ inline Json::Value readJsonFile(const std::string& fname) Json::Value root; Json::Reader reader; std::ifstream fh(fname.c_str()); - bool parse_success = reader.parse(fh, root); + const bool parse_success = reader.parse(fh, root); if (!parse_success) throw std::runtime_error("failed to parse " + fname); return root; diff --git a/trajopt_common/include/trajopt_common/macros.h b/trajopt_common/include/trajopt_common/macros.h index 5d3bffea..61671136 100644 --- a/trajopt_common/include/trajopt_common/macros.h +++ b/trajopt_common/include/trajopt_common/macros.h @@ -85,7 +85,7 @@ do \ { \ std::cerr << "\033[1;31mERROR " << (s) << "\033[0m\n"; \ - std::cerr << "at " << __FILE__ << ":" << __LINE__ << std::endl; \ + std::cerr << "at " << __FILE__ << ":" << __LINE__ << '\n'; \ std::stringstream ss; \ ss << (s); \ throw std::runtime_error(ss.str()); \ diff --git a/trajopt_common/src/collision_utils.cpp b/trajopt_common/src/collision_utils.cpp index bfecb250..ba0323ed 100644 --- a/trajopt_common/src/collision_utils.cpp +++ b/trajopt_common/src/collision_utils.cpp @@ -151,18 +151,18 @@ void calcGradient(GradientResults& results, link_gradient.jacobian = jac.topRows(3); link_gradient.gradient = link_gradient.translation_vector.transpose() * link_gradient.jacobian; - //#ifndef NDEBUG // This is good for checking discrete evaluators - // Eigen::Isometry3d test_link_transform = manip->calcFwdKin(dofvals, it->link_name); - // Eigen::Isometry3d temp1 = world_to_base * test_link_transform; - // Eigen::Isometry3d temp2 = link_transform * it->transform.inverse(); - // assert(temp1.isApprox(temp2, 0.0001)); + // #ifndef NDEBUG // This is good for checking discrete evaluators + // Eigen::Isometry3d test_link_transform = manip->calcFwdKin(dofvals, it->link_name); + // Eigen::Isometry3d temp1 = world_to_base * test_link_transform; + // Eigen::Isometry3d temp2 = link_transform * it->transform.inverse(); + // assert(temp1.isApprox(temp2, 0.0001)); // Eigen::MatrixXd jac_test; // jac_test.resize(6, manip->numJoints()); // tesseract_kinematics::numericalJacobian(jac_test, world_to_base, *manip, dofvals, it->link_name, // contact_result.nearest_points_local[i]); bool check = link_gradient.jacobian.isApprox(jac_test.topRows(3), 1e-3); // assert(check == true); - //#endif + // #endif } GradientResults getGradient(const Eigen::VectorXd& dofvals, diff --git a/trajopt_common/src/utils.cpp b/trajopt_common/src/utils.cpp index 3e3b8235..cbfbaff8 100644 --- a/trajopt_common/src/utils.cpp +++ b/trajopt_common/src/utils.cpp @@ -14,7 +14,7 @@ void SafetyMarginData::setPairSafetyMarginData(const std::string& obj1, double safety_margin, double safety_margin_coeff) { - std::array data({ safety_margin, safety_margin_coeff }); + const std::array data({ safety_margin, safety_margin_coeff }); auto key = tesseract_common::makeOrderedLinkPair(obj1, obj2); pair_lookup_table_[key] = data; @@ -66,7 +66,7 @@ Eigen::Isometry3d addTwist(const Eigen::Isometry3d& t1, { Eigen::Isometry3d t2; t2.setIdentity(); - Eigen::Vector3d angle_axis = (t1.rotation().inverse() * twist.tail(3)) * dt; + const Eigen::Vector3d angle_axis = (t1.rotation().inverse() * twist.tail(3)) * dt; t2.linear() = t1.rotation() * Eigen::AngleAxisd(angle_axis.norm(), angle_axis.normalized()); t2.translation() = t1.translation() + twist.head(3) * dt; return t2; diff --git a/trajopt_ifopt/include/trajopt_ifopt/constraints/cartesian_position_constraint.h b/trajopt_ifopt/include/trajopt_ifopt/constraints/cartesian_position_constraint.h index 263553a8..b5d34f4d 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/constraints/cartesian_position_constraint.h +++ b/trajopt_ifopt/include/trajopt_ifopt/constraints/cartesian_position_constraint.h @@ -99,7 +99,7 @@ class CartPosConstraint : public ifopt::ConstraintSet using ErrorDiffFunctionType = std::function; - CartPosConstraint(CartPosInfo info, + CartPosConstraint(const CartPosInfo& info, std::shared_ptr position_var, const std::string& name = "CartPos"); diff --git a/trajopt_ifopt/include/trajopt_ifopt/costs/absolute_cost.h b/trajopt_ifopt/include/trajopt_ifopt/costs/absolute_cost.h index b008e4e8..b863f6f9 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/costs/absolute_cost.h +++ b/trajopt_ifopt/include/trajopt_ifopt/costs/absolute_cost.h @@ -81,7 +81,7 @@ class AbsoluteCost : public ifopt::CostTerm * @brief Constructs a CostTerm that converts a constraint into a cost with a sum squared error * @param constraint Input constraint to be converted to a cost */ - AbsoluteCost(ifopt::ConstraintSet::Ptr constraint); + AbsoluteCost(const ifopt::ConstraintSet::Ptr& constraint); /** * @brief Constructs a CostTerm that converts a constraint into a cost with a weighted sum squared error * @param constraint Input constraint to be converted to a cost diff --git a/trajopt_ifopt/include/trajopt_ifopt/costs/squared_cost.h b/trajopt_ifopt/include/trajopt_ifopt/costs/squared_cost.h index 76af0fad..03cea127 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/costs/squared_cost.h +++ b/trajopt_ifopt/include/trajopt_ifopt/costs/squared_cost.h @@ -81,7 +81,7 @@ class SquaredCost : public ifopt::CostTerm * @brief Constructs a CostTerm that converts a constraint into a cost with a sum squared error * @param constraint Input constraint to be converted to a cost */ - SquaredCost(ifopt::ConstraintSet::Ptr constraint); + SquaredCost(const ifopt::ConstraintSet::Ptr& constraint); /** * @brief Constructs a CostTerm that converts a constraint into a cost with a weighted sum squared error * @param constraint Input constraint to be converted to a cost diff --git a/trajopt_ifopt/src/constraints/cartesian_line_constraint.cpp b/trajopt_ifopt/src/constraints/cartesian_line_constraint.cpp index 20123c32..99f71992 100644 --- a/trajopt_ifopt/src/constraints/cartesian_line_constraint.cpp +++ b/trajopt_ifopt/src/constraints/cartesian_line_constraint.cpp @@ -93,12 +93,12 @@ CartLineConstraint::CartLineConstraint(CartLineInfo info, const Eigen::Isometry3d& target_tf, const Eigen::Isometry3d& source_tf) -> Eigen::VectorXd { tesseract_common::TransformMap perturbed_state = info_.manip->calcFwdKin(vals); - Eigen::Isometry3d perturbed_source_tf = perturbed_state[info_.source_frame] * info_.source_frame_offset; - Eigen::Isometry3d target_tf1 = perturbed_state[info_.target_frame] * info_.target_frame_offset1; - Eigen::Isometry3d target_tf2 = perturbed_state[info_.target_frame] * info_.target_frame_offset2; + const Eigen::Isometry3d perturbed_source_tf = perturbed_state[info_.source_frame] * info_.source_frame_offset; + const Eigen::Isometry3d target_tf1 = perturbed_state[info_.target_frame] * info_.target_frame_offset1; + const Eigen::Isometry3d target_tf2 = perturbed_state[info_.target_frame] * info_.target_frame_offset2; // For Jacobian Calc, we need the inverse of the nearest point, D, to new Pose, C, on the constraint line AB - Eigen::Isometry3d perturbed_target_tf = GetLinePoint(perturbed_source_tf, target_tf1, target_tf2); + const Eigen::Isometry3d perturbed_target_tf = GetLinePoint(perturbed_source_tf, target_tf1, target_tf2); Eigen::VectorXd error_diff = tesseract_common::calcJacobianTransformErrorDiff( target_tf, perturbed_target_tf, source_tf, perturbed_source_tf); @@ -114,12 +114,12 @@ CartLineConstraint::CartLineConstraint(CartLineInfo info, Eigen::VectorXd CartLineConstraint::CalcValues(const Eigen::Ref& joint_vals) const { tesseract_common::TransformMap state = info_.manip->calcFwdKin(joint_vals); - Eigen::Isometry3d source_tf = state[info_.source_frame] * info_.source_frame_offset; - Eigen::Isometry3d target_tf1 = state[info_.target_frame] * info_.target_frame_offset1; - Eigen::Isometry3d target_tf2 = state[info_.target_frame] * info_.target_frame_offset2; + const Eigen::Isometry3d source_tf = state[info_.source_frame] * info_.source_frame_offset; + const Eigen::Isometry3d target_tf1 = state[info_.target_frame] * info_.target_frame_offset1; + const Eigen::Isometry3d target_tf2 = state[info_.target_frame] * info_.target_frame_offset2; // For Jacobian Calc, we need the inverse of the nearest point, D, to new Pose, C, on the constraint line AB - Eigen::Isometry3d target_tf = GetLinePoint(source_tf, target_tf1, target_tf2); + const Eigen::Isometry3d target_tf = GetLinePoint(source_tf, target_tf1, target_tf2); // pose error is the vector from the new_pose to nearest point on line AB, line_point // the below method is equivalent to the position constraint; using the line point as the target point @@ -134,7 +134,7 @@ Eigen::VectorXd CartLineConstraint::CalcValues(const Eigen::RefGetVariables()->GetComponent(position_var_->GetName())->GetValues(); + const Eigen::VectorXd joint_vals = this->GetVariables()->GetComponent(position_var_->GetName())->GetValues(); return CalcValues(joint_vals); } @@ -151,12 +151,12 @@ void CartLineConstraint::CalcJacobianBlock(const Eigen::RefcalcFwdKin(joint_vals); - Eigen::Isometry3d source_tf = state[info_.source_frame] * info_.source_frame_offset; - Eigen::Isometry3d target_tf1 = state[info_.target_frame] * info_.target_frame_offset1; - Eigen::Isometry3d target_tf2 = state[info_.target_frame] * info_.target_frame_offset2; + const Eigen::Isometry3d source_tf = state[info_.source_frame] * info_.source_frame_offset; + const Eigen::Isometry3d target_tf1 = state[info_.target_frame] * info_.target_frame_offset1; + const Eigen::Isometry3d target_tf2 = state[info_.target_frame] * info_.target_frame_offset2; // For Jacobian Calc, we need the inverse of the nearest point, D, to new Pose, C, on the constraint line AB - Eigen::Isometry3d target_tf = GetLinePoint(source_tf, target_tf1, target_tf2); + const Eigen::Isometry3d target_tf = GetLinePoint(source_tf, target_tf1, target_tf2); // Reserve enough room in the sparse matrix std::vector > triplet_list; @@ -170,7 +170,7 @@ void CartLineConstraint::CalcJacobianBlock(const Eigen::RefcalcFwdKin(joint_vals); - Eigen::Isometry3d source_tf = state[info_.source_frame] * info_.source_frame_offset; - Eigen::Isometry3d target_tf1 = state[info_.target_frame] * info_.target_frame_offset1; - Eigen::Isometry3d target_tf2 = state[info_.target_frame] * info_.target_frame_offset2; + const Eigen::Isometry3d source_tf = state[info_.source_frame] * info_.source_frame_offset; + const Eigen::Isometry3d target_tf1 = state[info_.target_frame] * info_.target_frame_offset1; + const Eigen::Isometry3d target_tf2 = state[info_.target_frame] * info_.target_frame_offset2; // For Jacobian Calc, we need the inverse of the nearest point, D, to new Pose, C, on the constraint line AB - Eigen::Isometry3d target_tf = GetLinePoint(source_tf, target_tf1, target_tf2); + const Eigen::Isometry3d target_tf = GetLinePoint(source_tf, target_tf1, target_tf2); Eigen::MatrixXd jac0 = info_.manip->calcJacobian(joint_vals, info_.source_frame, info_.source_frame_offset.translation()); @@ -220,9 +220,9 @@ void CartLineConstraint::CalcJacobianBlock(const Eigen::RefGetVariables()->GetComponent(position_var_->GetName())->GetValues(); + const Eigen::VectorXd joint_vals = this->GetVariables()->GetComponent(position_var_->GetName())->GetValues(); CalcJacobianBlock(joint_vals, jac_block); // NOLINT } @@ -270,16 +270,16 @@ Eigen::Isometry3d CartLineConstraint::GetLinePoint(const Eigen::Isometry3d& sour const Eigen::Isometry3d& target_tf2) const { // distance 1; distance from new pose to first point on line - Eigen::Vector3d d1 = (source_tf.translation() - target_tf1.translation()).array().abs(); + const Eigen::Vector3d d1 = (source_tf.translation() - target_tf1.translation()).array().abs(); // Get the line - Eigen::Vector3d line = target_tf2.translation() - target_tf1.translation(); + const Eigen::Vector3d line = target_tf2.translation() - target_tf1.translation(); // Point D, the nearest point on line AB to point C, can be found with: // (AC - (AC * AB)) * AB Eigen::Isometry3d line_point; - Eigen::Vector3d line_norm = line.normalized(); - double mag = d1.dot(line_norm); + const Eigen::Vector3d line_norm = line.normalized(); + const double mag = d1.dot(line_norm); // If point C is not between the line endpoints, set nearest point to endpoint if (mag > 1.0) @@ -296,9 +296,9 @@ Eigen::Isometry3d CartLineConstraint::GetLinePoint(const Eigen::Isometry3d& sour } // The orientation of the line_point is found using quaternion SLERP - Eigen::Quaterniond quat_a(target_tf1.rotation()); - Eigen::Quaterniond quat_b(target_tf2.rotation()); - Eigen::Quaterniond slerp = quat_a.slerp(mag, quat_b); + const Eigen::Quaterniond quat_a(target_tf1.rotation()); + const Eigen::Quaterniond quat_b(target_tf2.rotation()); + const Eigen::Quaterniond slerp = quat_a.slerp(mag, quat_b); line_point.linear() = slerp.toRotationMatrix(); return line_point; diff --git a/trajopt_ifopt/src/constraints/cartesian_position_constraint.cpp b/trajopt_ifopt/src/constraints/cartesian_position_constraint.cpp index bcdfc08a..34091395 100644 --- a/trajopt_ifopt/src/constraints/cartesian_position_constraint.cpp +++ b/trajopt_ifopt/src/constraints/cartesian_position_constraint.cpp @@ -113,7 +113,7 @@ CartPosConstraint::CartPosConstraint(CartPosInfo info, const Eigen::Isometry3d& target_tf, const Eigen::Isometry3d& source_tf) -> Eigen::VectorXd { tesseract_common::TransformMap perturbed_state = info_.manip->calcFwdKin(vals); - Eigen::Isometry3d perturbed_target_tf = perturbed_state[info_.target_frame] * info_.target_frame_offset; + const Eigen::Isometry3d perturbed_target_tf = perturbed_state[info_.target_frame] * info_.target_frame_offset; Eigen::VectorXd error_diff = tesseract_common::calcJacobianTransformErrorDiff(source_tf, target_tf, perturbed_target_tf); @@ -145,7 +145,7 @@ CartPosConstraint::CartPosConstraint(CartPosInfo info, const Eigen::Isometry3d& target_tf, const Eigen::Isometry3d& source_tf) -> Eigen::VectorXd { tesseract_common::TransformMap perturbed_state = info_.manip->calcFwdKin(vals); - Eigen::Isometry3d perturbed_source_tf = perturbed_state[info_.source_frame] * info_.source_frame_offset; + const Eigen::Isometry3d perturbed_source_tf = perturbed_state[info_.source_frame] * info_.source_frame_offset; Eigen::VectorXd error_diff = tesseract_common::calcJacobianTransformErrorDiff(target_tf, source_tf, perturbed_source_tf); @@ -176,8 +176,8 @@ CartPosConstraint::CartPosConstraint(CartPosInfo info, const Eigen::Isometry3d& target_tf, const Eigen::Isometry3d& source_tf) -> Eigen::VectorXd { tesseract_common::TransformMap perturbed_state = info_.manip->calcFwdKin(vals); - Eigen::Isometry3d perturbed_source_tf = perturbed_state[info_.source_frame] * info_.source_frame_offset; - Eigen::Isometry3d perturbed_target_tf = perturbed_state[info_.target_frame] * info_.target_frame_offset; + const Eigen::Isometry3d perturbed_source_tf = perturbed_state[info_.source_frame] * info_.source_frame_offset; + const Eigen::Isometry3d perturbed_target_tf = perturbed_state[info_.target_frame] * info_.target_frame_offset; Eigen::VectorXd error_diff = tesseract_common::calcJacobianTransformErrorDiff( target_tf, perturbed_target_tf, source_tf, perturbed_source_tf); @@ -193,27 +193,27 @@ CartPosConstraint::CartPosConstraint(CartPosInfo info, } } -CartPosConstraint::CartPosConstraint(CartPosInfo info, +CartPosConstraint::CartPosConstraint(const CartPosInfo& info, std::shared_ptr position_var, const std::string& name) - : CartPosConstraint(std::move(info), std::move(position_var), Eigen::VectorXd::Ones(info.indices.rows()), name) + : CartPosConstraint(info, std::move(position_var), Eigen::VectorXd::Ones(info.indices.rows()), name) { } Eigen::VectorXd CartPosConstraint::CalcValues(const Eigen::Ref& joint_vals) const { tesseract_common::TransformMap state = info_.manip->calcFwdKin(joint_vals); - Eigen::Isometry3d source_tf = state[info_.source_frame] * info_.source_frame_offset; - Eigen::Isometry3d target_tf = state[info_.target_frame] * info_.target_frame_offset; + const Eigen::Isometry3d source_tf = state[info_.source_frame] * info_.source_frame_offset; + const Eigen::Isometry3d target_tf = state[info_.target_frame] * info_.target_frame_offset; - Eigen::VectorXd err = error_function_(target_tf, source_tf); + const Eigen::VectorXd err = error_function_(target_tf, source_tf); return coeffs_.cwiseProduct(err); } Eigen::VectorXd CartPosConstraint::GetValues() const { - VectorXd joint_vals = this->GetVariables()->GetComponent(position_var_->GetName())->GetValues(); + const VectorXd joint_vals = this->GetVariables()->GetComponent(position_var_->GetName())->GetValues(); return CalcValues(joint_vals); } @@ -230,8 +230,8 @@ void CartPosConstraint::CalcJacobianBlock(const Eigen::RefcalcFwdKin(joint_vals); - Eigen::Isometry3d source_tf = state[info_.source_frame] * info_.source_frame_offset; - Eigen::Isometry3d target_tf = state[info_.target_frame] * info_.target_frame_offset; + const Eigen::Isometry3d source_tf = state[info_.source_frame] * info_.source_frame_offset; + const Eigen::Isometry3d target_tf = state[info_.target_frame] * info_.target_frame_offset; std::vector > triplet_list; triplet_list.reserve(static_cast(n_dof_ * info_.indices.size())); @@ -244,7 +244,7 @@ void CartPosConstraint::CalcJacobianBlock(const Eigen::RefGetVariables()->GetComponent(position_var_->GetName())->GetValues(); + const VectorXd joint_vals = this->GetVariables()->GetComponent(position_var_->GetName())->GetValues(); CalcJacobianBlock(joint_vals, jac_block); // NOLINT } @@ -344,7 +344,7 @@ Eigen::Isometry3d CartPosConstraint::GetTargetPose() const { return info_.target Eigen::Isometry3d CartPosConstraint::GetCurrentPose() const { - VectorXd joint_vals = this->GetVariables()->GetComponent(position_var_->GetName())->GetValues(); + const VectorXd joint_vals = this->GetVariables()->GetComponent(position_var_->GetName())->GetValues(); return info_.manip->calcFwdKin(joint_vals)[info_.source_frame] * info_.source_frame_offset; } } // namespace trajopt_ifopt diff --git a/trajopt_ifopt/src/constraints/collision/continuous_collision_constraint.cpp b/trajopt_ifopt/src/constraints/collision/continuous_collision_constraint.cpp index 5ff0038f..aa695017 100644 --- a/trajopt_ifopt/src/constraints/collision/continuous_collision_constraint.cpp +++ b/trajopt_ifopt/src/constraints/collision/continuous_collision_constraint.cpp @@ -83,9 +83,9 @@ ContinuousCollisionConstraint::ContinuousCollisionConstraint( Eigen::VectorXd ContinuousCollisionConstraint::GetValues() const { // Get current joint values - Eigen::VectorXd joint_vals0 = this->GetVariables()->GetComponent(position_vars_[0]->GetName())->GetValues(); - Eigen::VectorXd joint_vals1 = this->GetVariables()->GetComponent(position_vars_[1]->GetName())->GetValues(); - double margin_buffer = collision_evaluator_->GetCollisionConfig().collision_margin_buffer; + const Eigen::VectorXd joint_vals0 = this->GetVariables()->GetComponent(position_vars_[0]->GetName())->GetValues(); + const Eigen::VectorXd joint_vals1 = this->GetVariables()->GetComponent(position_vars_[1]->GetName())->GetValues(); + const double margin_buffer = collision_evaluator_->GetCollisionConfig().collision_margin_buffer; Eigen::VectorXd values = Eigen::VectorXd::Constant(static_cast(bounds_.size()), -margin_buffer); auto collision_data = @@ -139,8 +139,8 @@ void ContinuousCollisionConstraint::FillJacobianBlock(std::string var_set, Jacob jac_block.setFromTriplets(triplet_list_.begin(), triplet_list_.end()); // NOLINT // Calculate collisions - Eigen::VectorXd joint_vals0 = this->GetVariables()->GetComponent(position_vars_[0]->GetName())->GetValues(); - Eigen::VectorXd joint_vals1 = this->GetVariables()->GetComponent(position_vars_[1]->GetName())->GetValues(); + const Eigen::VectorXd joint_vals0 = this->GetVariables()->GetComponent(position_vars_[0]->GetName())->GetValues(); + const Eigen::VectorXd joint_vals1 = this->GetVariables()->GetComponent(position_vars_[1]->GetName())->GetValues(); auto collision_data = collision_evaluator_->CalcCollisionData(joint_vals0, joint_vals1, position_vars_fixed_, bounds_.size()); diff --git a/trajopt_ifopt/src/constraints/collision/continuous_collision_evaluators.cpp b/trajopt_ifopt/src/constraints/collision/continuous_collision_evaluators.cpp index 411185f9..3e09e517 100644 --- a/trajopt_ifopt/src/constraints/collision/continuous_collision_evaluators.cpp +++ b/trajopt_ifopt/src/constraints/collision/continuous_collision_evaluators.cpp @@ -92,7 +92,7 @@ LVSContinuousCollisionEvaluator::CalcCollisionData(const Eigen::Ref& position_vars_fixed, std::size_t bounds_size) { - std::size_t key = trajopt_common::getHash(*collision_config_, dof_vals0, dof_vals1); + const std::size_t key = trajopt_common::getHash(*collision_config_, dof_vals0, dof_vals1); auto* it = collision_cache_->get(key); if (it != nullptr) { @@ -182,7 +182,7 @@ void LVSContinuousCollisionEvaluator::CalcCollisionsHelper(tesseract_collision:: // The first step is to see if the distance between two states is larger than the longest valid segment. If larger // the collision checking is broken up into multiple casted collision checks such that each check is less then // the longest valid segment length. - double dist = (dof_vals1 - dof_vals0).norm(); + const double dist = (dof_vals1 - dof_vals0).norm(); // If not empty then there are links that are not part of the kinematics object that can move (dynamic environment) if (!diff_active_link_names_.empty()) @@ -205,9 +205,10 @@ void LVSContinuousCollisionEvaluator::CalcCollisionsHelper(tesseract_collision:: } // Contains the contact distance threshold and coefficient for the given link pair - double dist = collision_config_->contact_manager_config.margin_data.getPairCollisionMargin(pair.first.first, - pair.first.second); - double coeff = collision_config_->collision_coeff_data.getPairCollisionCoeff(pair.first.first, pair.first.second); + const double dist = collision_config_->contact_manager_config.margin_data.getPairCollisionMargin(pair.first.first, + pair.first.second); + const double coeff = + collision_config_->collision_coeff_data.getPairCollisionCoeff(pair.first.first, pair.first.second); const Eigen::Vector3d data = { dist, collision_config_->collision_margin_buffer, coeff }; trajopt_common::removeInvalidContactResults(pair.second, data, position_vars_fixed); }; @@ -216,7 +217,7 @@ void LVSContinuousCollisionEvaluator::CalcCollisionsHelper(tesseract_collision:: dist > collision_config_->longest_valid_segment_length) { // Calculate the number state to interpolate - long cnt = static_cast(std::ceil(dist / collision_config_->longest_valid_segment_length)) + 1; + const long cnt = static_cast(std::ceil(dist / collision_config_->longest_valid_segment_length)) + 1; // Create interpolated trajectory between two states that satisfies the longest valid segment length. tesseract_common::TrajArray subtraj(cnt, dof_vals0.size()); @@ -225,8 +226,8 @@ void LVSContinuousCollisionEvaluator::CalcCollisionsHelper(tesseract_collision:: // Perform casted collision checking for sub trajectory and store results in contacts_vector tesseract_collision::ContactResultMap contacts{ dist_results }; - int last_state_idx{ static_cast(subtraj.rows()) - 1 }; - double dt = 1.0 / double(last_state_idx); + const int last_state_idx{ static_cast(subtraj.rows()) - 1 }; + const double dt = 1.0 / double(last_state_idx); for (int i = 0; i < subtraj.rows() - 1; ++i) { tesseract_common::TransformMap state0 = get_state_fn_(subtraj.row(i)); @@ -263,7 +264,7 @@ LVSContinuousCollisionEvaluator::CalcGradientData(const Eigen::Refcontact_manager_config.margin_data.getPairCollisionMargin( + const double margin = collision_config_->contact_manager_config.margin_data.getPairCollisionMargin( contact_results.link_names[0], contact_results.link_names[1]); return trajopt_common::getGradient( @@ -330,7 +331,7 @@ LVSDiscreteCollisionEvaluator::CalcCollisionData(const Eigen::Ref& position_vars_fixed, std::size_t bounds_size) { - std::size_t key = getHash(*collision_config_, dof_vals0, dof_vals1); + const std::size_t key = getHash(*collision_config_, dof_vals0, dof_vals1); auto* it = collision_cache_->get(key); if (it != nullptr) { @@ -437,9 +438,10 @@ void LVSDiscreteCollisionEvaluator::CalcCollisionsHelper(tesseract_collision::Co } // Contains the contact distance threshold and coefficient for the given link pair - double dist = collision_config_->contact_manager_config.margin_data.getPairCollisionMargin(pair.first.first, - pair.first.second); - double coeff = collision_config_->collision_coeff_data.getPairCollisionCoeff(pair.first.first, pair.first.second); + const double dist = collision_config_->contact_manager_config.margin_data.getPairCollisionMargin(pair.first.first, + pair.first.second); + const double coeff = + collision_config_->collision_coeff_data.getPairCollisionCoeff(pair.first.first, pair.first.second); const Eigen::Vector3d data = { dist, collision_config_->collision_margin_buffer, coeff }; // Don't include contacts at the fixed state @@ -449,7 +451,7 @@ void LVSDiscreteCollisionEvaluator::CalcCollisionsHelper(tesseract_collision::Co // The first step is to see if the distance between two states is larger than the longest valid segment. If larger // the collision checking is broken up into multiple casted collision checks such that each check is less then // the longest valid segment length. - double dist = (dof_vals1 - dof_vals0).norm(); + const double dist = (dof_vals1 - dof_vals0).norm(); long cnt = 2; if (collision_config_->type == tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS && dist > collision_config_->longest_valid_segment_length) @@ -465,8 +467,8 @@ void LVSDiscreteCollisionEvaluator::CalcCollisionsHelper(tesseract_collision::Co // Perform casted collision checking for sub trajectory and store results in contacts_vector tesseract_collision::ContactResultMap contacts{ dist_results }; - int last_state_idx{ static_cast(subtraj.rows()) - 1 }; - double dt = 1.0 / double(last_state_idx); + const int last_state_idx{ static_cast(subtraj.rows()) - 1 }; + const double dt = 1.0 / double(last_state_idx); for (int i = 0; i < subtraj.rows(); ++i) { tesseract_common::TransformMap state0 = get_state_fn_(subtraj.row(i)); @@ -491,7 +493,7 @@ LVSDiscreteCollisionEvaluator::CalcGradientData(const Eigen::Refcontact_manager_config.margin_data.getPairCollisionMargin( + const double margin = collision_config_->contact_manager_config.margin_data.getPairCollisionMargin( contact_results.link_names[0], contact_results.link_names[1]); return trajopt_common::getGradient( diff --git a/trajopt_ifopt/src/constraints/collision/continuous_collision_numerical_constraint.cpp b/trajopt_ifopt/src/constraints/collision/continuous_collision_numerical_constraint.cpp index acc5270a..7c7059aa 100644 --- a/trajopt_ifopt/src/constraints/collision/continuous_collision_numerical_constraint.cpp +++ b/trajopt_ifopt/src/constraints/collision/continuous_collision_numerical_constraint.cpp @@ -83,9 +83,9 @@ ContinuousCollisionNumericalConstraint::ContinuousCollisionNumericalConstraint( Eigen::VectorXd ContinuousCollisionNumericalConstraint::GetValues() const { // Get current joint values - Eigen::VectorXd joint_vals0 = this->GetVariables()->GetComponent(position_vars_[0]->GetName())->GetValues(); - Eigen::VectorXd joint_vals1 = this->GetVariables()->GetComponent(position_vars_[1]->GetName())->GetValues(); - double margin_buffer = collision_evaluator_->GetCollisionConfig().collision_margin_buffer; + const Eigen::VectorXd joint_vals0 = this->GetVariables()->GetComponent(position_vars_[0]->GetName())->GetValues(); + const Eigen::VectorXd joint_vals1 = this->GetVariables()->GetComponent(position_vars_[1]->GetName())->GetValues(); + const double margin_buffer = collision_evaluator_->GetCollisionConfig().collision_margin_buffer; Eigen::VectorXd values = Eigen::VectorXd::Constant(static_cast(bounds_.size()), -margin_buffer); auto collision_data = @@ -137,11 +137,11 @@ void ContinuousCollisionNumericalConstraint::FillJacobianBlock(std::string var_s if (!triplet_list_.empty()) // NOLINT jac_block.setFromTriplets(triplet_list_.begin(), triplet_list_.end()); // NOLINT - double margin_buffer = collision_evaluator_->GetCollisionConfig().collision_margin_buffer; + const double margin_buffer = collision_evaluator_->GetCollisionConfig().collision_margin_buffer; // Calculate collisions Eigen::VectorXd joint_vals0 = this->GetVariables()->GetComponent(position_vars_[0]->GetName())->GetValues(); - Eigen::VectorXd joint_vals1 = this->GetVariables()->GetComponent(position_vars_[1]->GetName())->GetValues(); + const Eigen::VectorXd joint_vals1 = this->GetVariables()->GetComponent(position_vars_[1]->GetName())->GetValues(); auto collision_data = collision_evaluator_->CalcCollisionData(joint_vals0, joint_vals1, position_vars_fixed_, bounds_.size()); @@ -151,7 +151,7 @@ void ContinuousCollisionNumericalConstraint::FillJacobianBlock(std::string var_s const std::size_t cnt = std::min(bounds_.size(), collision_data->gradient_results_sets.size()); Eigen::VectorXd jv = joint_vals0; - double delta = 1e-8; + const double delta = 1e-8; for (int j = 0; j < n_dof_; j++) { jv(j) = joint_vals0(j) + delta; diff --git a/trajopt_ifopt/src/constraints/collision/discrete_collision_constraint.cpp b/trajopt_ifopt/src/constraints/collision/discrete_collision_constraint.cpp index d8da0304..41221f38 100644 --- a/trajopt_ifopt/src/constraints/collision/discrete_collision_constraint.cpp +++ b/trajopt_ifopt/src/constraints/collision/discrete_collision_constraint.cpp @@ -71,7 +71,7 @@ DiscreteCollisionConstraint::DiscreteCollisionConstraint( Eigen::VectorXd DiscreteCollisionConstraint::GetValues() const { // Get current joint values - Eigen::VectorXd joint_vals = this->GetVariables()->GetComponent(position_var_->GetName())->GetValues(); + const Eigen::VectorXd joint_vals = this->GetVariables()->GetComponent(position_var_->GetName())->GetValues(); return CalcValues(joint_vals); } @@ -86,7 +86,7 @@ void DiscreteCollisionConstraint::FillJacobianBlock(std::string var_set, Jacobia return; // Get current joint values - VectorXd joint_vals = this->GetVariables()->GetComponent(position_var_->GetName())->GetValues(); + const VectorXd joint_vals = this->GetVariables()->GetComponent(position_var_->GetName())->GetValues(); CalcJacobianBlock(joint_vals, jac_block); // NOLINT } @@ -94,9 +94,9 @@ void DiscreteCollisionConstraint::FillJacobianBlock(std::string var_set, Jacobia Eigen::VectorXd DiscreteCollisionConstraint::CalcValues(const Eigen::Ref& joint_vals) const { // Check the collisions - trajopt_common::CollisionCacheData::ConstPtr collision_data = + const trajopt_common::CollisionCacheData::ConstPtr collision_data = collision_evaluator_->CalcCollisions(joint_vals, bounds_.size()); - double margin_buffer = collision_evaluator_->GetCollisionConfig().collision_margin_buffer; + const double margin_buffer = collision_evaluator_->GetCollisionConfig().collision_margin_buffer; Eigen::VectorXd values = Eigen::VectorXd::Constant(static_cast(bounds_.size()), -margin_buffer); if (collision_data->gradient_results_sets.empty()) @@ -125,7 +125,7 @@ void DiscreteCollisionConstraint::CalcJacobianBlock(const Eigen::RefCalcCollisions(joint_vals, bounds_.size()); if (collision_data->gradient_results_sets.empty()) return; diff --git a/trajopt_ifopt/src/constraints/collision/discrete_collision_evaluators.cpp b/trajopt_ifopt/src/constraints/collision/discrete_collision_evaluators.cpp index af9da85b..ff36e1e1 100644 --- a/trajopt_ifopt/src/constraints/collision/discrete_collision_evaluators.cpp +++ b/trajopt_ifopt/src/constraints/collision/discrete_collision_evaluators.cpp @@ -87,7 +87,7 @@ std::shared_ptr SingleTimestepCollisionEvaluator::CalcCollisions(const Eigen::Ref& dof_vals, std::size_t bounds_size) { - std::size_t key = getHash(*collision_config_, dof_vals); + const std::size_t key = getHash(*collision_config_, dof_vals); auto* it = collision_cache_->get(key); if (it != nullptr) { @@ -176,9 +176,10 @@ void SingleTimestepCollisionEvaluator::CalcCollisionsHelper(const Eigen::Refcontact_manager_config.margin_data.getPairCollisionMargin(pair.first.first, - pair.first.second); - double coeff = collision_config_->collision_coeff_data.getPairCollisionCoeff(pair.first.first, pair.first.second); + const double dist = collision_config_->contact_manager_config.margin_data.getPairCollisionMargin(pair.first.first, + pair.first.second); + const double coeff = + collision_config_->collision_coeff_data.getPairCollisionCoeff(pair.first.first, pair.first.second); const Eigen::Vector2d data = { dist, coeff }; auto end = std::remove_if( pair.second.begin(), pair.second.end(), [&data, this](const tesseract_collision::ContactResult& r) { @@ -195,7 +196,7 @@ SingleTimestepCollisionEvaluator::GetGradient(const Eigen::VectorXd& dofvals, const tesseract_collision::ContactResult& contact_result) { // Contains the contact distance threshold and coefficient for the given link pair - double margin = collision_config_->contact_manager_config.margin_data.getPairCollisionMargin( + const double margin = collision_config_->contact_manager_config.margin_data.getPairCollisionMargin( contact_result.link_names[0], contact_result.link_names[1]); return trajopt_common::getGradient( diff --git a/trajopt_ifopt/src/constraints/collision/discrete_collision_numerical_constraint.cpp b/trajopt_ifopt/src/constraints/collision/discrete_collision_numerical_constraint.cpp index 90f9d9ec..34968bc6 100644 --- a/trajopt_ifopt/src/constraints/collision/discrete_collision_numerical_constraint.cpp +++ b/trajopt_ifopt/src/constraints/collision/discrete_collision_numerical_constraint.cpp @@ -71,7 +71,7 @@ DiscreteCollisionNumericalConstraint::DiscreteCollisionNumericalConstraint( Eigen::VectorXd DiscreteCollisionNumericalConstraint::GetValues() const { // Get current joint values - Eigen::VectorXd joint_vals = this->GetVariables()->GetComponent(position_var_->GetName())->GetValues(); + const Eigen::VectorXd joint_vals = this->GetVariables()->GetComponent(position_var_->GetName())->GetValues(); return CalcValues(joint_vals); } @@ -86,7 +86,7 @@ void DiscreteCollisionNumericalConstraint::FillJacobianBlock(std::string var_set return; // Get current joint values - VectorXd joint_vals = this->GetVariables()->GetComponent(position_var_->GetName())->GetValues(); + const VectorXd joint_vals = this->GetVariables()->GetComponent(position_var_->GetName())->GetValues(); CalcJacobianBlock(joint_vals, jac_block); // NOLINT } @@ -96,7 +96,7 @@ DiscreteCollisionNumericalConstraint::CalcValues(const Eigen::RefCalcCollisions(joint_vals, bounds_.size()); - double margin_buffer = collision_evaluator_->GetCollisionConfig().collision_margin_buffer; + const double margin_buffer = collision_evaluator_->GetCollisionConfig().collision_margin_buffer; Eigen::VectorXd values = Eigen::VectorXd::Constant(static_cast(bounds_.size()), -margin_buffer); if (collision_data->gradient_results_sets.empty()) @@ -131,10 +131,10 @@ void DiscreteCollisionNumericalConstraint::CalcJacobianBlock(const Eigen::Refgradient_results_sets.size()); - double margin_buffer = collision_evaluator_->GetCollisionConfig().collision_margin_buffer; + const double margin_buffer = collision_evaluator_->GetCollisionConfig().collision_margin_buffer; Eigen::VectorXd jv = joint_vals; - double delta = 1e-8; + const double delta = 1e-8; for (int j = 0; j < n_dof_; j++) { jv(j) = joint_vals(j) + delta; @@ -150,12 +150,12 @@ void DiscreteCollisionNumericalConstraint::CalcJacobianBlock(const Eigen::Refgradient_results_sets.begin(), collision_data_delta->gradient_results_sets.end(), fn); if (it != collision_data_delta->gradient_results_sets.end()) { - double dist_delta = baseline.coeff * (it->getMaxErrorT0() - baseline.getMaxErrorT0()); + const double dist_delta = baseline.coeff * (it->getMaxErrorT0() - baseline.getMaxErrorT0()); jac_block.coeffRef(i, j) = dist_delta / delta; } else { - double dist_delta = baseline.coeff * ((-1.0 * margin_buffer) - baseline.getMaxErrorT0()); + const double dist_delta = baseline.coeff * ((-1.0 * margin_buffer) - baseline.getMaxErrorT0()); jac_block.coeffRef(i, j) = dist_delta / delta; } } @@ -173,8 +173,8 @@ void DiscreteCollisionNumericalConstraint::CalcJacobianBlock(const Eigen::Ref(i), j) = -1.0 * grad_vec[j]; // ++i; // } - // std::cout << "Numerical Jacobian:" << std::endl << jac_block << std::endl; - // std::cout << "Col Grad Jacobian:" << std::endl << jac_block_debug << std::endl; + // std::cout << "Numerical Jacobian:" << '\n' << jac_block << '\n'; + // std::cout << "Col Grad Jacobian:" << '\n' << jac_block_debug << '\n'; // //#endif } diff --git a/trajopt_ifopt/src/constraints/collision/weighted_average_methods.cpp b/trajopt_ifopt/src/constraints/collision/weighted_average_methods.cpp index 49754950..4e55eedf 100644 --- a/trajopt_ifopt/src/constraints/collision/weighted_average_methods.cpp +++ b/trajopt_ifopt/src/constraints/collision/weighted_average_methods.cpp @@ -48,7 +48,7 @@ Eigen::VectorXd getWeightedAvgGradientT0(const trajopt_common::GradientResultsSe if (grad_results_set.max_error[i].error_with_buffer[0] > 0) { assert(grad_results_set.max_error[i].has_error[0]); - double w = (std::max(grad.error_with_buffer, 0.0) / max_error_with_buffer); + 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); @@ -86,7 +86,7 @@ Eigen::VectorXd getWeightedAvgGradientT1(const trajopt_common::GradientResultsSe if (grad_results_set.max_error[i].error_with_buffer[1] > 0) { assert(grad_results_set.max_error[i].has_error[1]); - double w = (std::max(grad.error_with_buffer, 0.0) / max_error_with_buffer); + 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); diff --git a/trajopt_ifopt/src/constraints/inverse_kinematics_constraint.cpp b/trajopt_ifopt/src/constraints/inverse_kinematics_constraint.cpp index 5dff8e32..b30c7ed2 100644 --- a/trajopt_ifopt/src/constraints/inverse_kinematics_constraint.cpp +++ b/trajopt_ifopt/src/constraints/inverse_kinematics_constraint.cpp @@ -75,7 +75,7 @@ InverseKinematicsConstraint::CalcValues(const Eigen::Ref& tesseract_kinematics::KinGroupIKInputs inputs; inputs.emplace_back(target_pose_, kinematic_info_->working_frame, kinematic_info_->tcp_frame); - tesseract_kinematics::IKSolutions target_joint_position = + const tesseract_kinematics::IKSolutions target_joint_position = kinematic_info_->manip->calcInvKin(inputs, seed_joint_position); assert(!target_joint_position.empty()); @@ -84,8 +84,8 @@ InverseKinematicsConstraint::CalcValues(const Eigen::Ref& double error_norm = std::numeric_limits::max(); for (const auto& sol : target_joint_position) { - Eigen::VectorXd cur_error = sol - joint_vals; - double cur_error_norm = cur_error.norm(); + const Eigen::VectorXd cur_error = sol - joint_vals; + const double cur_error_norm = cur_error.norm(); if (cur_error_norm < error_norm) { error_norm = cur_error_norm; @@ -98,8 +98,8 @@ InverseKinematicsConstraint::CalcValues(const Eigen::Ref& Eigen::VectorXd InverseKinematicsConstraint::GetValues() const { // Get the two variables - Eigen::VectorXd seed_joint_position = this->GetVariables()->GetComponent(seed_var_->GetName())->GetValues(); - Eigen::VectorXd joint_vals = this->GetVariables()->GetComponent(constraint_var_->GetName())->GetValues(); + const Eigen::VectorXd seed_joint_position = this->GetVariables()->GetComponent(seed_var_->GetName())->GetValues(); + const Eigen::VectorXd joint_vals = this->GetVariables()->GetComponent(constraint_var_->GetName())->GetValues(); return CalcValues(joint_vals, seed_joint_position); } diff --git a/trajopt_ifopt/src/constraints/joint_acceleration_constraint.cpp b/trajopt_ifopt/src/constraints/joint_acceleration_constraint.cpp index 7bd58d7b..5472e7b0 100644 --- a/trajopt_ifopt/src/constraints/joint_acceleration_constraint.cpp +++ b/trajopt_ifopt/src/constraints/joint_acceleration_constraint.cpp @@ -89,7 +89,7 @@ Eigen::VectorXd JointAccelConstraint::GetValues() const auto vals1 = GetVariables()->GetComponent(position_vars_[ind]->GetName())->GetValues(); auto vals2 = GetVariables()->GetComponent(position_vars_[ind + 1]->GetName())->GetValues(); auto vals3 = GetVariables()->GetComponent(position_vars_[ind + 2]->GetName())->GetValues(); - Eigen::VectorXd single_step = (vals3 - 2 * vals2 + vals1); + const Eigen::VectorXd single_step = (vals3 - 2 * vals2 + vals1); acceleration.block(n_dof_ * static_cast(ind), 0, n_dof_, 1) = coeffs_.cwiseProduct(single_step); } @@ -99,7 +99,7 @@ Eigen::VectorXd JointAccelConstraint::GetValues() const auto vals1 = GetVariables()->GetComponent(position_vars_[ind]->GetName())->GetValues(); auto vals2 = GetVariables()->GetComponent(position_vars_[ind - 1]->GetName())->GetValues(); auto vals3 = GetVariables()->GetComponent(position_vars_[ind - 2]->GetName())->GetValues(); - Eigen::VectorXd single_step = (vals3 - 2 * vals2 + vals1); + const Eigen::VectorXd single_step = (vals3 - 2 * vals2 + vals1); acceleration.block(n_dof_ * static_cast(ind), 0, n_dof_, 1) = coeffs_.cwiseProduct(single_step); } @@ -117,7 +117,7 @@ void JointAccelConstraint::FillJacobianBlock(std::string var_set, Jacobian& jac_ if (it == index_map_.end()) // NOLINT return; - Eigen::Index i = it->second; + const Eigen::Index i = it->second; std::vector > triplet_list; triplet_list.reserve(static_cast(n_dof_ * 3)); diff --git a/trajopt_ifopt/src/constraints/joint_jerk_constraint.cpp b/trajopt_ifopt/src/constraints/joint_jerk_constraint.cpp index 98dfcea2..28d71b56 100644 --- a/trajopt_ifopt/src/constraints/joint_jerk_constraint.cpp +++ b/trajopt_ifopt/src/constraints/joint_jerk_constraint.cpp @@ -90,7 +90,7 @@ Eigen::VectorXd JointJerkConstraint::GetValues() const auto vals2 = GetVariables()->GetComponent(position_vars_[ind + 1]->GetName())->GetValues(); auto vals3 = GetVariables()->GetComponent(position_vars_[ind + 2]->GetName())->GetValues(); auto vals4 = GetVariables()->GetComponent(position_vars_[ind + 3]->GetName())->GetValues(); - Eigen::VectorXd single_step = (3.0 * vals2) - (3.0 * vals3) - vals1 + vals4; + 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); } @@ -101,7 +101,7 @@ Eigen::VectorXd JointJerkConstraint::GetValues() const auto vals2 = GetVariables()->GetComponent(position_vars_[ind - 1]->GetName())->GetValues(); auto vals3 = GetVariables()->GetComponent(position_vars_[ind - 2]->GetName())->GetValues(); auto vals4 = GetVariables()->GetComponent(position_vars_[ind - 3]->GetName())->GetValues(); - Eigen::VectorXd single_step = vals1 - (3.0 * vals2) + (3.0 * vals3) - vals4; + 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); } @@ -119,7 +119,7 @@ void JointJerkConstraint::FillJacobianBlock(std::string var_set, Jacobian& jac_b if (it == index_map_.end()) // NOLINT return; - Eigen::Index i = it->second; + const Eigen::Index i = it->second; // Reserve enough room in the sparse matrix std::vector> triplet_list; diff --git a/trajopt_ifopt/src/constraints/joint_position_constraint.cpp b/trajopt_ifopt/src/constraints/joint_position_constraint.cpp index 05d568e4..eb855ec9 100644 --- a/trajopt_ifopt/src/constraints/joint_position_constraint.cpp +++ b/trajopt_ifopt/src/constraints/joint_position_constraint.cpp @@ -69,7 +69,7 @@ JointPosConstraint::JointPosConstraint(const Eigen::VectorXd& targets, { for (long i = 0; i < n_dof_; i++) { - double w_target = coeffs_[i] * targets[i]; + const double w_target = coeffs_[i] * targets[i]; bounds[static_cast(i + j * n_dof_)] = ifopt::Bounds(w_target, w_target); } } diff --git a/trajopt_ifopt/src/constraints/joint_velocity_constraint.cpp b/trajopt_ifopt/src/constraints/joint_velocity_constraint.cpp index 77aee299..ee08ffb4 100644 --- a/trajopt_ifopt/src/constraints/joint_velocity_constraint.cpp +++ b/trajopt_ifopt/src/constraints/joint_velocity_constraint.cpp @@ -97,7 +97,7 @@ Eigen::VectorXd JointVelConstraint::GetValues() const { auto vals1 = this->GetVariables()->GetComponent(position_vars_[ind]->GetName())->GetValues(); auto vals2 = this->GetVariables()->GetComponent(position_vars_[ind + 1]->GetName())->GetValues(); - Eigen::VectorXd single_step = (vals2 - vals1); + const Eigen::VectorXd single_step = (vals2 - vals1); velocity.block(n_dof_ * static_cast(ind), 0, n_dof_, 1) = coeffs_.cwiseProduct(single_step); } @@ -117,7 +117,7 @@ void JointVelConstraint::FillJacobianBlock(std::string var_set, Jacobian& jac_bl if (it == index_map_.end()) // NOLINT return; - Eigen::Index i = it->second; + const Eigen::Index i = it->second; // Reserve enough room in the sparse matrix std::vector> triplet_list; diff --git a/trajopt_ifopt/src/costs/absolute_cost.cpp b/trajopt_ifopt/src/costs/absolute_cost.cpp index bdcb43b0..ba282c60 100644 --- a/trajopt_ifopt/src/costs/absolute_cost.cpp +++ b/trajopt_ifopt/src/costs/absolute_cost.cpp @@ -32,8 +32,8 @@ TRAJOPT_IGNORE_WARNINGS_POP namespace trajopt_ifopt { -AbsoluteCost::AbsoluteCost(ifopt::ConstraintSet::Ptr constraint) - : AbsoluteCost(std::move(constraint), Eigen::VectorXd::Ones(constraint->GetRows())) +AbsoluteCost::AbsoluteCost(const ifopt::ConstraintSet::Ptr& constraint) + : AbsoluteCost(constraint, Eigen::VectorXd::Ones(constraint->GetRows())) { } @@ -48,8 +48,8 @@ AbsoluteCost::AbsoluteCost(ifopt::ConstraintSet::Ptr constraint, const Eigen::Re double AbsoluteCost::GetCost() const { // This takes the absolute value of the errors - Eigen::VectorXd error = calcBoundsViolations(constraint_->GetValues(), constraint_->GetBounds()); - double cost = weights_.transpose() * error; + const Eigen::VectorXd error = calcBoundsViolations(constraint_->GetValues(), constraint_->GetBounds()); + const double cost = weights_.transpose() * error; return cost; } @@ -76,9 +76,9 @@ void AbsoluteCost::FillJacobianBlock(std::string var_set, Jacobian& jac_block) c // 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. - Eigen::ArrayXd error = calcBoundsErrors(constraint_->GetValues(), constraint_->GetBounds()); - Eigen::ArrayXd w_error = error * weights_.array(); - Eigen::VectorXd coeff = w_error / error.abs(); + 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 } diff --git a/trajopt_ifopt/src/costs/squared_cost.cpp b/trajopt_ifopt/src/costs/squared_cost.cpp index 7dc7f350..ca3d274b 100644 --- a/trajopt_ifopt/src/costs/squared_cost.cpp +++ b/trajopt_ifopt/src/costs/squared_cost.cpp @@ -32,8 +32,8 @@ TRAJOPT_IGNORE_WARNINGS_POP namespace trajopt_ifopt { -SquaredCost::SquaredCost(ifopt::ConstraintSet::Ptr constraint) - : SquaredCost(std::move(constraint), Eigen::VectorXd::Ones(constraint->GetRows())) +SquaredCost::SquaredCost(const ifopt::ConstraintSet::Ptr& constraint) + : SquaredCost(constraint, Eigen::VectorXd::Ones(constraint->GetRows())) { } @@ -48,7 +48,7 @@ SquaredCost::SquaredCost(ifopt::ConstraintSet::Ptr constraint, const Eigen::Ref< double SquaredCost::GetCost() const { Eigen::VectorXd error = calcBoundsErrors(constraint_->GetValues(), constraint_->GetBounds()); - double cost = error.transpose() * weights_.asDiagonal() * error; + double const cost = error.transpose() * weights_.asDiagonal() * error; return cost; } diff --git a/trajopt_ifopt/src/utils/ifopt_utils.cpp b/trajopt_ifopt/src/utils/ifopt_utils.cpp index f6fb6c25..b86644c4 100644 --- a/trajopt_ifopt/src/utils/ifopt_utils.cpp +++ b/trajopt_ifopt/src/utils/ifopt_utils.cpp @@ -62,6 +62,7 @@ bool isBoundsFiniteFinite(const ifopt::Bounds& bounds) { return (isFinite(bounds std::vector toBounds(const Eigen::Ref& limits) { std::vector bounds; + bounds.reserve(static_cast(limits.rows())); for (Eigen::Index i = 0; i < limits.rows(); i++) bounds.emplace_back(limits(i, 0), limits(i, 1)); return bounds; @@ -82,7 +83,7 @@ std::vector interpolate(const Eigen::Ref Eigen::Index steps) { assert(start.size() == end.size()); // NOLINT - Eigen::VectorXd delta = (end - start) / static_cast(steps - 1); + 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++) @@ -124,10 +125,11 @@ Eigen::VectorXd calcBoundsErrors(const Eigen::Ref& input, } // Values will be negative if they violate the constrain - Eigen::ArrayXd zero = Eigen::ArrayXd::Zero(input.rows()); - Eigen::ArrayXd dist_from_lower = (input.array() - bound_lower).min(zero); - Eigen::ArrayXd dist_from_upper = (input.array() - bound_upper).max(zero); - Eigen::ArrayXd worst_error = (dist_from_upper.abs() > dist_from_lower.abs()).select(dist_from_upper, dist_from_lower); + 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 worst_error = + (dist_from_upper.abs() > dist_from_lower.abs()).select(dist_from_upper, dist_from_lower); return worst_error; } @@ -142,19 +144,19 @@ ifopt::VectorXd calcNumericalCostGradient(const double* x, ifopt::Problem& nlp, { auto cache_vars = nlp.GetVariableValues(); - int n = nlp.GetNumberOfOptimizationVariables(); + const int n = nlp.GetNumberOfOptimizationVariables(); ifopt::Problem::Jacobian jac(1, n); if (nlp.HasCostTerms()) { - double step_size = epsilon; + const double step_size = epsilon; // calculate forward difference by disturbing each optimization variable - double g = nlp.EvaluateCostFunction(x); + 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 - double g_new = nlp.EvaluateCostFunction(x_new.data()); + 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 } @@ -170,22 +172,22 @@ ifopt::Jacobian calcNumericalConstraintGradient(const double* x, ifopt::Problem& { auto cache_vars = nlp.GetVariableValues(); - int n = nlp.GetNumberOfOptimizationVariables(); - int m = nlp.GetConstraints().GetRows(); + const int n = nlp.GetNumberOfOptimizationVariables(); + const int m = nlp.GetConstraints().GetRows(); ifopt::Problem::Jacobian jac(m, n); jac.reserve(static_cast(m) * static_cast(n)); if (nlp.GetNumberOfConstraints() > 0) { - double step_size = epsilon; + const double step_size = epsilon; // calculate forward difference by disturbing each optimization variable - ifopt::Problem::VectorXd g = nlp.EvaluateConstraints(x); + const ifopt::Problem::VectorXd g = nlp.EvaluateConstraints(x); std::vector x_new(x, x + n); for (int i = 0; i < n; ++i) { x_new[static_cast(i)] += step_size; // disturb - ifopt::Problem::VectorXd g_new = nlp.EvaluateConstraints(x_new.data()); + const ifopt::Problem::VectorXd g_new = nlp.EvaluateConstraints(x_new.data()); ifopt::Problem::VectorXd delta_g = (g_new - g) / step_size; for (int j = 0; j < m; ++j) @@ -207,21 +209,21 @@ ifopt::Jacobian calcNumericalConstraintGradient(ifopt::Component& variables, { Eigen::VectorXd x = variables.GetValues(); - int n = variables.GetRows(); - int m = constraint_set.GetRows(); + 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()) { // calculate forward difference by disturbing each optimization variable - ifopt::Problem::VectorXd g = constraint_set.GetValues(); + 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); - ifopt::Problem::VectorXd g_new = constraint_set.GetValues(); + const ifopt::Problem::VectorXd g_new = constraint_set.GetValues(); ifopt::Problem::VectorXd delta_g = (g_new - g) / epsilon; for (int j = 0; j < m; ++j) diff --git a/trajopt_ifopt/src/utils/numeric_differentiation.cpp b/trajopt_ifopt/src/utils/numeric_differentiation.cpp index a50b8042..fcd63829 100644 --- a/trajopt_ifopt/src/utils/numeric_differentiation.cpp +++ b/trajopt_ifopt/src/utils/numeric_differentiation.cpp @@ -29,13 +29,13 @@ namespace trajopt_ifopt { SparseMatrix calcForwardNumJac(const ErrorCalculator& f, const Eigen::Ref& x, double epsilon) { - Eigen::VectorXd y = f(x); + const Eigen::VectorXd y = f(x); Eigen::MatrixXd out(y.size(), x.size()); Eigen::VectorXd x_perturbed = x; for (int i = 0; i < x.size(); ++i) { x_perturbed(i) = x(i) + epsilon; - Eigen::VectorXd y_perturbed = f(x_perturbed); + const Eigen::VectorXd y_perturbed = f(x_perturbed); out.col(i) = (y_perturbed - y) / epsilon; x_perturbed(i) = x(i); } diff --git a/trajopt_ifopt/test/cartesian_line_constraint_unit.cpp b/trajopt_ifopt/test/cartesian_line_constraint_unit.cpp index 6802a642..4bac633c 100644 --- a/trajopt_ifopt/test/cartesian_line_constraint_unit.cpp +++ b/trajopt_ifopt/test/cartesian_line_constraint_unit.cpp @@ -42,10 +42,10 @@ class CartesianLineConstraintUnit : public testing::TestWithParam void SetUp() override { // Initialize Tesseract - tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/arm_around_table.urdf"); - tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/pr2.srdf"); + const tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/arm_around_table.urdf"); + const tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/pr2.srdf"); auto locator = std::make_shared(); - bool status = env->init(urdf_file, srdf_file, locator); + const bool status = env->init(urdf_file, srdf_file, locator); EXPECT_TRUE(status); // Extract necessary kinematic information @@ -57,7 +57,7 @@ class CartesianLineConstraintUnit : public testing::TestWithParam variables->AddComponent(var); // Add constraints - Eigen::VectorXd joint_position = Eigen::VectorXd::Ones(n_dof); + const Eigen::VectorXd joint_position = Eigen::VectorXd::Ones(n_dof); source_tf = manip->calcFwdKin(joint_position).at("r_gripper_tool_frame"); line_start_pose = source_tf; @@ -76,7 +76,7 @@ TEST_F(CartesianLineConstraintUnit, GetValue) // NOLINT Eigen::VectorXd joint_position = Eigen::VectorXd::Ones(n_dof); info = trajopt_ifopt::CartLineInfo(manip, "r_gripper_tool_frame", "base_link", line_start_pose, line_end_pose); - Eigen::VectorXd coeff = Eigen::VectorXd::Ones(info.indices.rows()); + const Eigen::VectorXd coeff = Eigen::VectorXd::Ones(info.indices.rows()); auto constraint = std::make_shared(info, var, coeff); constraint->LinkWithVariables(variables); @@ -102,7 +102,7 @@ TEST_F(CartesianLineConstraintUnit, GetValue) // NOLINT // distance error with a 3-4-5 triangle { - Eigen::VectorXd joint_position = Eigen::VectorXd::Ones(n_dof); + const Eigen::VectorXd joint_position = Eigen::VectorXd::Ones(n_dof); Eigen::Isometry3d start_pose_mod = line_start_pose; Eigen::Isometry3d end_pose_mod = line_end_pose; @@ -110,7 +110,7 @@ TEST_F(CartesianLineConstraintUnit, GetValue) // NOLINT end_pose_mod.translation() = end_pose_mod.translation() + Eigen::Vector3d(0.0, 0.3, 0.4); info = trajopt_ifopt::CartLineInfo(manip, "r_gripper_tool_frame", "base_link", start_pose_mod, end_pose_mod); - Eigen::VectorXd coeff = Eigen::VectorXd::Ones(info.indices.rows()); + const Eigen::VectorXd coeff = Eigen::VectorXd::Ones(info.indices.rows()); auto constraint = std::make_shared(info, var, coeff); constraint->LinkWithVariables(variables); @@ -125,15 +125,15 @@ TEST_F(CartesianLineConstraintUnit, FillJacobian) // NOLINT CONSOLE_BRIDGE_logDebug("CartesianPositionConstraintUnit, FillJacobian"); // Run FK to get target pose - Eigen::VectorXd joint_position = Eigen::VectorXd::Ones(n_dof); + const Eigen::VectorXd joint_position = Eigen::VectorXd::Ones(n_dof); Eigen::Isometry3d source_tf = manip->calcFwdKin(joint_position).at("r_gripper_tool_frame"); // Set the line endpoints st the target pose is on the line - Eigen::Isometry3d start_pose_mod = source_tf.translate(Eigen::Vector3d(-1.0, 0, 0)); - Eigen::Isometry3d end_pose_mod = source_tf.translate(Eigen::Vector3d(1.0, 0, 0)); + const Eigen::Isometry3d start_pose_mod = source_tf.translate(Eigen::Vector3d(-1.0, 0, 0)); + const Eigen::Isometry3d end_pose_mod = source_tf.translate(Eigen::Vector3d(1.0, 0, 0)); info = trajopt_ifopt::CartLineInfo(manip, "r_gripper_tool_frame", "base_link", start_pose_mod, end_pose_mod); - Eigen::VectorXd coeff = Eigen::VectorXd::Ones(info.indices.rows()); + const Eigen::VectorXd coeff = Eigen::VectorXd::Ones(info.indices.rows()); auto constraint = std::make_shared(info, var, coeff); constraint->LinkWithVariables(variables); @@ -148,7 +148,7 @@ TEST_F(CartesianLineConstraintUnit, FillJacobian) // NOLINT // Calculate jacobian numerically auto error_calculator = [&](const Eigen::Ref& x) { return constraint->CalcValues(x); }; - ifopt::ConstraintSet::Jacobian num_jac_block = + const ifopt::ConstraintSet::Jacobian num_jac_block = trajopt_ifopt::calcForwardNumJac(error_calculator, joint_position_mod, 1e-4); // Compare to constraint jacobian @@ -174,11 +174,11 @@ TEST_F(CartesianLineConstraintUnit, GetSetBounds) // NOLINT // Check that setting bounds works info = trajopt_ifopt::CartLineInfo(manip, "r_gripper_tool_frame", "base_link", line_start_pose, line_end_pose); - Eigen::VectorXd coeff = Eigen::VectorXd::Ones(info.indices.rows()); + const Eigen::VectorXd coeff = Eigen::VectorXd::Ones(info.indices.rows()); auto constraint = std::make_shared(info, var, coeff); constraint->LinkWithVariables(variables); - ifopt::Bounds bounds(-0.1234, 0.5678); + const ifopt::Bounds bounds(-0.1234, 0.5678); std::vector bounds_vec = std::vector(6, bounds); constraint->SetBounds(bounds_vec); @@ -198,7 +198,7 @@ TEST_F(CartesianLineConstraintUnit, IgnoreVariables) // NOLINT CONSOLE_BRIDGE_logDebug("CartesianPositionConstraintUnit, IgnoreVariables"); info = trajopt_ifopt::CartLineInfo(manip, "r_gripper_tool_frame", "base_link", line_start_pose, line_end_pose); - Eigen::VectorXd coeff = Eigen::VectorXd::Ones(info.indices.rows()); + const Eigen::VectorXd coeff = Eigen::VectorXd::Ones(info.indices.rows()); auto constraint = std::make_shared(info, var, coeff); constraint->LinkWithVariables(variables); diff --git a/trajopt_ifopt/test/cartesian_position_constraint_unit.cpp b/trajopt_ifopt/test/cartesian_position_constraint_unit.cpp index ed0752fc..9755503a 100644 --- a/trajopt_ifopt/test/cartesian_position_constraint_unit.cpp +++ b/trajopt_ifopt/test/cartesian_position_constraint_unit.cpp @@ -66,11 +66,11 @@ class CartesianPositionConstraintUnit : public testing::TestWithParam(); auto env = std::make_shared(); - bool status = env->init(urdf_file, srdf_file, locator); + const bool status = env->init(urdf_file, srdf_file, locator); EXPECT_TRUE(status); // Extract necessary kinematic information @@ -82,7 +82,7 @@ class CartesianPositionConstraintUnit : public testing::TestWithParam(cart_info, var0); nlp.AddConstraintSet(constraint); } @@ -95,7 +95,7 @@ TEST_F(CartesianPositionConstraintUnit, GetValue) // NOLINT // Run FK to get target pose Eigen::VectorXd joint_position = Eigen::VectorXd::Ones(n_dof); - Eigen::Isometry3d target_pose = kin_group->calcFwdKin(joint_position).at("r_gripper_tool_frame"); + const Eigen::Isometry3d target_pose = kin_group->calcFwdKin(joint_position).at("r_gripper_tool_frame"); constraint->SetTargetPose(target_pose); // Set the joints to the joint position that should satisfy it @@ -145,8 +145,8 @@ TEST_F(CartesianPositionConstraintUnit, FillJacobian) // NOLINT CONSOLE_BRIDGE_logDebug("CartesianPositionConstraintUnit, FillJacobian"); // Run FK to get target pose - Eigen::VectorXd joint_position = Eigen::VectorXd::Ones(n_dof); - Eigen::Isometry3d target_pose = kin_group->calcFwdKin(joint_position).at("r_gripper_tool_frame"); + const Eigen::VectorXd joint_position = Eigen::VectorXd::Ones(n_dof); + const Eigen::Isometry3d target_pose = kin_group->calcFwdKin(joint_position).at("r_gripper_tool_frame"); constraint->SetTargetPose(target_pose); // Modify one joint at a time @@ -159,7 +159,7 @@ TEST_F(CartesianPositionConstraintUnit, FillJacobian) // NOLINT // Calculate jacobian numerically auto error_calculator = [&](const Eigen::Ref& x) { return constraint->CalcValues(x); }; - trajopt_ifopt::SparseMatrix num_jac_block = + const trajopt_ifopt::SparseMatrix num_jac_block = trajopt_ifopt::calcForwardNumJac(error_calculator, joint_position_mod, 1e-4); // Compare to constraint jacobian @@ -167,15 +167,15 @@ TEST_F(CartesianPositionConstraintUnit, FillJacobian) // NOLINT trajopt_ifopt::SparseMatrix jac_block(num_jac_block.rows(), num_jac_block.cols()); constraint->CalcJacobianBlock(joint_position_mod, jac_block); // NOLINT EXPECT_TRUE(jac_block.isApprox(num_jac_block, 1e-3)); - // std::cout << "Numeric:\n" << num_jac_block.toDense() << std::endl; - // std::cout << "Analytic:\n" << jac_block.toDense() << std::endl; + // std::cout << "Numeric:\n" << num_jac_block.toDense() << '\n'; + // std::cout << "Analytic:\n" << jac_block.toDense() << '\n'; } { trajopt_ifopt::SparseMatrix jac_block(num_jac_block.rows(), num_jac_block.cols()); constraint->FillJacobianBlock("Joint_Position_0", jac_block); EXPECT_TRUE(jac_block.toDense().isApprox(num_jac_block.toDense(), 1e-3)); - // std::cout << "Numeric:\n" << num_jac_block.toDense() << std::endl; - // std::cout << "Analytic:\n" << jac_block.toDense() << std::endl; + // std::cout << "Numeric:\n" << num_jac_block.toDense() << '\n'; + // std::cout << "Analytic:\n" << jac_block.toDense() << '\n'; } } } @@ -189,13 +189,13 @@ TEST_F(CartesianPositionConstraintUnit, GetSetBounds) // NOLINT // Check that setting bounds works { - Eigen::VectorXd pos = Eigen::VectorXd::Ones(kin_group->numJoints()); + const Eigen::VectorXd pos = Eigen::VectorXd::Ones(kin_group->numJoints()); auto var0 = std::make_shared(pos, kin_group->getJointNames(), "Joint_Position_0"); - CartPosInfo cart_info(kin_group, "r_gripper_tool_frame", "base_footprint"); + const CartPosInfo cart_info(kin_group, "r_gripper_tool_frame", "base_footprint"); auto constraint_2 = std::make_shared(cart_info, var0); - ifopt::Bounds bounds(-0.1234, 0.5678); + const ifopt::Bounds bounds(-0.1234, 0.5678); std::vector bounds_vec = std::vector(6, bounds); constraint_2->SetBounds(bounds_vec); diff --git a/trajopt_ifopt/test/cast_cost_unit.cpp b/trajopt_ifopt/test/cast_cost_unit.cpp index 82d4fa47..01097e9c 100644 --- a/trajopt_ifopt/test/cast_cost_unit.cpp +++ b/trajopt_ifopt/test/cast_cost_unit.cpp @@ -67,9 +67,9 @@ class CastTest : public testing::TestWithParam void SetUp() override { - boost::filesystem::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/boxbot.urdf"); - boost::filesystem::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/boxbot.srdf"); - ResourceLocator::Ptr locator = std::make_shared(); + const boost::filesystem::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/boxbot.urdf"); + const boost::filesystem::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/boxbot.srdf"); + const ResourceLocator::Ptr locator = std::make_shared(); EXPECT_TRUE(env->init(urdf_file, srdf_file, locator)); gLogLevel = trajopt_common::LevelError; @@ -86,9 +86,9 @@ TEST_F(CastTest, boxes) // NOLINT env->setState(ipos); std::vector collisions; - tesseract_scene_graph::StateSolver::Ptr state_solver = env->getStateSolver(); - ContinuousContactManager::Ptr manager = env->getContinuousContactManager(); - tesseract_kinematics::JointGroup::ConstPtr manip = env->getJointGroup("manipulator"); + const tesseract_scene_graph::StateSolver::Ptr state_solver = env->getStateSolver(); + const ContinuousContactManager::Ptr manager = env->getContinuousContactManager(); + const tesseract_kinematics::JointGroup::ConstPtr manip = env->getJointGroup("manipulator"); manager->setActiveCollisionObjects(manip->getActiveLinkNames()); manager->setDefaultCollisionMarginData(0); @@ -127,23 +127,23 @@ TEST_F(CastTest, boxes) // NOLINT } // Step 3: Setup collision - double margin_coeff = 1; - double margin = 0.02; + const double margin_coeff = 1; + const double margin = 0.02; auto trajopt_collision_config = std::make_shared(margin, margin_coeff); trajopt_collision_config->type = tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS; trajopt_collision_config->collision_margin_buffer = 0.05; // 4) Add constraints { // Fix start position - std::vector fixed_vars = { vars[0] }; - Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(manip->numJoints(), 1); + const std::vector fixed_vars = { vars[0] }; + const Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(manip->numJoints(), 1); auto cnt = std::make_shared(positions[0], fixed_vars, coeffs); nlp.AddConstraintSet(cnt); } { // Fix end position - std::vector fixed_vars = { vars[2] }; - Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(manip->numJoints(), 1); + const std::vector fixed_vars = { vars[2] }; + const Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(manip->numJoints(), 1); auto cnt = std::make_shared(positions[2], fixed_vars, coeffs); nlp.AddConstraintSet(cnt); } @@ -155,7 +155,7 @@ TEST_F(CastTest, boxes) // NOLINT auto collision_evaluator = std::make_shared( collision_cache, manip, env, trajopt_collision_config); - std::array position_vars{ vars[i - 1], vars[i] }; + const std::array position_vars{ vars[i - 1], vars[i] }; auto cnt = std::make_shared( collision_evaluator, position_vars, position_vars_fixed, 1, true); nlp.AddConstraintSet(cnt); @@ -166,7 +166,7 @@ TEST_F(CastTest, boxes) // NOLINT } nlp.PrintCurrent(); - std::cout << "Jacobian: \n" << nlp.GetJacobianOfConstraints().toDense() << std::endl; + std::cout << "Jacobian: \n" << nlp.GetJacobianOfConstraints().toDense() << '\n'; // 5) choose solver and options ifopt::IpoptSolver ipopt; @@ -180,13 +180,13 @@ TEST_F(CastTest, boxes) // NOLINT // 6) solve ipopt.Solve(nlp); Eigen::VectorXd x = nlp.GetOptVariables()->GetValues(); - std::cout << x.transpose() << std::endl; + std::cout << x.transpose() << '\n'; EXPECT_TRUE(ipopt.GetReturnStatus() == 0); tesseract_common::TrajArray inputs(3, 2); inputs << -1.9, 0, 0, 1.9, 1.9, 3.8; - Eigen::Map results(x.data(), 3, 2); + const Eigen::Map results(x.data(), 3, 2); tesseract_collision::CollisionCheckConfig config; config.type = tesseract_collision::CollisionEvaluatorType::CONTINUOUS; diff --git a/trajopt_ifopt/test/collision_unit.cpp b/trajopt_ifopt/test/collision_unit.cpp index ce84e5ef..5d1b502f 100644 --- a/trajopt_ifopt/test/collision_unit.cpp +++ b/trajopt_ifopt/test/collision_unit.cpp @@ -60,14 +60,14 @@ class CollisionUnit : public testing::TestWithParam void SetUp() override { - boost::filesystem::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/boxbot.urdf"); - boost::filesystem::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/boxbot.srdf"); + const boost::filesystem::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/boxbot.urdf"); + const boost::filesystem::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/boxbot.srdf"); - ResourceLocator::Ptr locator = std::make_shared(); + const ResourceLocator::Ptr locator = std::make_shared(); EXPECT_TRUE(env->init(urdf_file, srdf_file, locator)); // Set up collision evaluator - tesseract_kinematics::JointGroup::ConstPtr kin = env->getJointGroup("manipulator"); + const tesseract_kinematics::JointGroup::ConstPtr kin = env->getJointGroup("manipulator"); auto config = std::make_shared(0.1, 1); auto collision_cache = std::make_shared(100); @@ -104,8 +104,8 @@ TEST_F(CollisionUnit, GetValueFillJacobian) // NOLINT ifopt::ConstraintSet::Jacobian jac_block; jac_block.resize(1, 2); constraint->FillJacobianBlock("Joint_Position_0", jac_block); - double dx = jac_block.coeff(0, 0); - double dy = jac_block.coeff(0, 1); + const double dx = jac_block.coeff(0, 0); + const double dy = jac_block.coeff(0, 1); EXPECT_NEAR(dx, 0.0, 1e-6); EXPECT_NEAR(dy, 0.0, 1e-6); } @@ -121,8 +121,8 @@ TEST_F(CollisionUnit, GetValueFillJacobian) // NOLINT ifopt::ConstraintSet::Jacobian jac_block; jac_block.resize(1, 2); constraint->FillJacobianBlock("Joint_Position_0", jac_block); - double dx = jac_block.coeff(0, 0); - double dy = jac_block.coeff(0, 1); + const double dx = jac_block.coeff(0, 0); + const double dy = jac_block.coeff(0, 1); EXPECT_NEAR(dx, 1.0, 1e-6); EXPECT_NEAR(dy, 0.0, 1e-6); } @@ -136,8 +136,8 @@ TEST_F(CollisionUnit, GetValueFillJacobian) // NOLINT ifopt::ConstraintSet::Jacobian jac_block; jac_block.resize(1, 2); constraint->FillJacobianBlock("Joint_Position_0", jac_block); - double dx = jac_block.coeff(0, 0); - double dy = jac_block.coeff(0, 1); + const double dx = jac_block.coeff(0, 0); + const double dy = jac_block.coeff(0, 1); EXPECT_NEAR(dx, 0.0, 1e-6); EXPECT_NEAR(dy, -1.0, 1e-6); } @@ -153,8 +153,8 @@ TEST_F(CollisionUnit, GetValueFillJacobian) // NOLINT ifopt::ConstraintSet::Jacobian jac_block; jac_block.resize(1, 2); constraint->FillJacobianBlock("Joint_Position_0", jac_block); - double dx = jac_block.coeff(0, 0); - double dy = jac_block.coeff(0, 1); + const double dx = jac_block.coeff(0, 0); + const double dy = jac_block.coeff(0, 1); EXPECT_NEAR(dx, 1.0, 1e-6); EXPECT_NEAR(dy, 0.0, 1e-6); } @@ -171,11 +171,11 @@ TEST_F(CollisionUnit, GetSetBounds) // NOLINT { Eigen::VectorXd pos(2); pos << -1.9, 0; - std::vector joint_names(2, "names"); + const std::vector joint_names(2, "names"); auto var0 = std::make_shared(pos, joint_names, "Joint_Position_0"); auto constraint_2 = std::make_shared(collision_evaluator, var0, 3); - ifopt::Bounds bounds(-0.1234, 0.5678); + const ifopt::Bounds bounds(-0.1234, 0.5678); std::vector bounds_vec = std::vector(1, bounds); constraint_2->SetBounds(bounds_vec); std::vector results_vec = constraint_2->GetBounds(); diff --git a/trajopt_ifopt/test/continuous_collision_gradient_unit.cpp b/trajopt_ifopt/test/continuous_collision_gradient_unit.cpp index 240d45a8..abbb75cb 100644 --- a/trajopt_ifopt/test/continuous_collision_gradient_unit.cpp +++ b/trajopt_ifopt/test/continuous_collision_gradient_unit.cpp @@ -68,10 +68,10 @@ class ContinuousCollisionGradientTest : public testing::TestWithParam(); + const ResourceLocator::Ptr locator = std::make_shared(); EXPECT_TRUE(env->init(urdf_file, srdf_file, locator)); gLogLevel = trajopt_common::LevelError; @@ -86,9 +86,9 @@ void runContinuousGradientTest(const Environment::Ptr& env, double coeff) env->setState(ipos); std::vector collisions; - tesseract_scene_graph::StateSolver::Ptr state_solver = env->getStateSolver(); - ContinuousContactManager::Ptr manager = env->getContinuousContactManager(); - tesseract_kinematics::JointGroup::ConstPtr manip = env->getJointGroup("manipulator"); + const tesseract_scene_graph::StateSolver::Ptr state_solver = env->getStateSolver(); + const ContinuousContactManager::Ptr manager = env->getContinuousContactManager(); + const tesseract_kinematics::JointGroup::ConstPtr manip = env->getJointGroup("manipulator"); manager->setActiveCollisionObjects(manip->getActiveLinkNames()); manager->setDefaultCollisionMarginData(0); @@ -127,8 +127,8 @@ void runContinuousGradientTest(const Environment::Ptr& env, double coeff) } // Step 3: Setup collision - double margin_coeff = coeff; - double margin = 0.02; + const double margin_coeff = coeff; + const double margin = 0.02; auto trajopt_collision_config = std::make_shared(margin, margin_coeff); trajopt_collision_config->type = tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS; trajopt_collision_config->collision_margin_buffer = 0.05; @@ -139,18 +139,18 @@ void runContinuousGradientTest(const Environment::Ptr& env, double coeff) auto collision_evaluator = std::make_shared( collision_cache, manip, env, trajopt_collision_config); - std::array position_vars{ vars[i - 1], vars[i] }; - std::array position_vars_fixed{ false, false }; + const std::array position_vars{ vars[i - 1], vars[i] }; + const std::array position_vars_fixed{ false, false }; auto cnt = std::make_shared( collision_evaluator, position_vars, position_vars_fixed, 3); nlp.AddConstraintSet(cnt); } - std::cout << "Jacobian: \n" << nlp.GetJacobianOfConstraints().toDense() << std::endl; + std::cout << "Jacobian: \n" << nlp.GetJacobianOfConstraints().toDense() << '\n'; std::vector init_vals{ -1.9, 0, 0, 1.9, 1.9, 3.8 }; - trajopt_ifopt::SparseMatrix num_jac_block = + const trajopt_ifopt::SparseMatrix num_jac_block = trajopt_ifopt::calcNumericalConstraintGradient(init_vals.data(), nlp, 1e-8); - std::cout << "Numerical Jacobian: \n" << num_jac_block.toDense() << std::endl; + std::cout << "Numerical Jacobian: \n" << num_jac_block.toDense() << '\n'; } TEST_F(ContinuousCollisionGradientTest, ContinuousCollisionGradientTest) // NOLINT diff --git a/trajopt_ifopt/test/cost_wrappers_unit.cpp b/trajopt_ifopt/test/cost_wrappers_unit.cpp index 80bb038c..a17c1ebd 100644 --- a/trajopt_ifopt/test/cost_wrappers_unit.cpp +++ b/trajopt_ifopt/test/cost_wrappers_unit.cpp @@ -102,7 +102,7 @@ TEST(CostWrapperUnit, SquaredCost) // NOLINT Eigen::VectorXd pos(1); pos << -4; positions.push_back(pos); - std::vector var_names = { "x" }; + const std::vector var_names = { "x" }; auto var = std::make_shared(pos, var_names); vars.push_back(var); nlp.AddVariableSet(var); @@ -122,8 +122,8 @@ TEST(CostWrapperUnit, SquaredCost) // NOLINT // Squared cost jacobian = 2 * y(x) * dy(x) // y(x) = x^2 + 4x + 3 // dy(x) = 2x + 4 - double x = positions[0](0); - double jac = 2 * (std::pow(x, 2) + 4 * x + 3) * (2 * x + 4); + const double x = positions[0](0); + const double jac = 2 * (std::pow(x, 2) + 4 * x + 3) * (2 * x + 4); EXPECT_NEAR(exact_jac(0, 0), jac, 1e-6); EXPECT_NEAR(numerical_jac(0, 0), jac, 1e-6); } @@ -141,7 +141,7 @@ TEST(CostWrapperUnit, WeightedSquaredCost) // NOLINT Eigen::VectorXd pos(1); pos << -4; positions.push_back(pos); - std::vector var_names = { "x" }; + const std::vector var_names = { "x" }; auto var = std::make_shared(pos, var_names); vars.push_back(var); nlp.AddVariableSet(var); @@ -163,8 +163,8 @@ TEST(CostWrapperUnit, WeightedSquaredCost) // NOLINT // Squared cost jacobian = 2 * y(x) * dy(x) // y(x) = x^2 + 4x + 3 // dy(x) = 2x + 4 - double x = positions[0](0); - double jac = 2 * weights(0) * (std::pow(x, 2) + 4 * x + 3) * (2 * x + 4); + const double x = positions[0](0); + const double jac = 2 * weights(0) * (std::pow(x, 2) + 4 * x + 3) * (2 * x + 4); EXPECT_NEAR(exact_jac(0, 0), jac, 1e-6); EXPECT_NEAR(numerical_jac(0, 0), jac, 1e-6); } @@ -182,7 +182,7 @@ TEST(CostWrapperUnit, AbsoluteCost) // NOLINT Eigen::VectorXd pos(1); pos << -4; positions.push_back(pos); - std::vector var_names = { "x" }; + const std::vector var_names = { "x" }; auto var = std::make_shared(pos, var_names); vars.push_back(var); nlp.AddVariableSet(var); @@ -202,8 +202,8 @@ TEST(CostWrapperUnit, AbsoluteCost) // NOLINT // Absolute cost jacobian = (y(x) / |y(x)|) * dy(x) // y(x) = x^2 + 4x + 3 // dy(x) = 2x + 4 - double x = positions[0](0); - double jac = ((std::pow(x, 2) + 4 * x + 3) / std::abs(std::pow(x, 2) + 4 * x + 3)) * (2 * x + 4); + const double x = positions[0](0); + const double jac = ((std::pow(x, 2) + 4 * x + 3) / std::abs(std::pow(x, 2) + 4 * x + 3)) * (2 * x + 4); EXPECT_NEAR(exact_jac(0, 0), jac, 1e-6); EXPECT_NEAR(numerical_jac(0, 0), jac, 1e-6); } @@ -221,7 +221,7 @@ TEST(CostWrapperUnit, WeightedAbsoluteCost) // NOLINT Eigen::VectorXd pos(1); pos << -4; positions.push_back(pos); - std::vector var_names = { "x" }; + const std::vector var_names = { "x" }; auto var = std::make_shared(pos, var_names); vars.push_back(var); nlp.AddVariableSet(var); @@ -243,8 +243,8 @@ TEST(CostWrapperUnit, WeightedAbsoluteCost) // NOLINT // Absolute cost jacobian = (y(x) / |y(x)|) * dy(x) // y(x) = x^2 + 4x + 3 // dy(x) = 2x + 4 - double x = positions[0](0); - double jac = (weights(0) * (std::pow(x, 2) + 4 * x + 3) / std::abs(std::pow(x, 2) + 4 * x + 3)) * (2 * x + 4); + const double x = positions[0](0); + const double jac = (weights(0) * (std::pow(x, 2) + 4 * x + 3) / std::abs(std::pow(x, 2) + 4 * x + 3)) * (2 * x + 4); EXPECT_NEAR(exact_jac(0, 0), jac, 1e-6); EXPECT_NEAR(numerical_jac(0, 0), jac, 1e-6); } diff --git a/trajopt_ifopt/test/discrete_collision_gradient_unit.cpp b/trajopt_ifopt/test/discrete_collision_gradient_unit.cpp index d14b108b..0a9a0704 100644 --- a/trajopt_ifopt/test/discrete_collision_gradient_unit.cpp +++ b/trajopt_ifopt/test/discrete_collision_gradient_unit.cpp @@ -65,10 +65,10 @@ class DiscreteCollisionGradientTest : public testing::TestWithParam void SetUp() override { - boost::filesystem::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/spherebot.urdf"); - boost::filesystem::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/spherebot.srdf"); + boost::filesystem::path const urdf_file(std::string(TRAJOPT_DATA_DIR) + "/spherebot.urdf"); + boost::filesystem::path const srdf_file(std::string(TRAJOPT_DATA_DIR) + "/spherebot.srdf"); - ResourceLocator::Ptr locator = std::make_shared(); + ResourceLocator::Ptr const locator = std::make_shared(); EXPECT_TRUE(env->init(urdf_file, srdf_file, locator)); } }; @@ -81,9 +81,9 @@ void runDiscreteGradientTest(const Environment::Ptr& env, double coeff) env->setState(ipos); std::vector collisions; - tesseract_scene_graph::StateSolver::Ptr state_solver = env->getStateSolver(); - DiscreteContactManager::Ptr manager = env->getDiscreteContactManager(); - tesseract_kinematics::JointGroup::ConstPtr manip = env->getJointGroup("manipulator"); + tesseract_scene_graph::StateSolver::Ptr const state_solver = env->getStateSolver(); + DiscreteContactManager::Ptr const manager = env->getDiscreteContactManager(); + const tesseract_kinematics::JointGroup::ConstPtr manip = env->getJointGroup("manipulator"); manager->setActiveCollisionObjects(manip->getActiveLinkNames()); manager->setDefaultCollisionMarginData(0); @@ -106,8 +106,8 @@ void runDiscreteGradientTest(const Environment::Ptr& env, double coeff) } // Step 3: Setup collision - double margin_coeff = coeff; - double margin = 0.2; + const double margin_coeff = coeff; + const double margin = 0.2; auto trajopt_collision_config = std::make_shared(margin, margin_coeff); trajopt_collision_config->collision_margin_buffer = 0.0; // 0.05 @@ -117,11 +117,11 @@ void runDiscreteGradientTest(const Environment::Ptr& env, double coeff) auto cnt = std::make_shared(collision_evaluator, vars[0], 3); nlp.AddConstraintSet(cnt); - std::cout << "Jacobian: \n" << nlp.GetJacobianOfConstraints().toDense() << std::endl; + std::cout << "Jacobian: \n" << nlp.GetJacobianOfConstraints().toDense() << '\n'; - trajopt_ifopt::SparseMatrix num_jac_block = + trajopt_ifopt::SparseMatrix const num_jac_block = trajopt_ifopt::calcNumericalConstraintGradient(positions[0].data(), nlp, 1e-8); - std::cout << "Numerical Jacobian: \n" << num_jac_block.toDense() << std::endl; + std::cout << "Numerical Jacobian: \n" << num_jac_block.toDense() << '\n'; } TEST_F(DiscreteCollisionGradientTest, DiscreteCollisionGradientTest) // NOLINT diff --git a/trajopt_ifopt/test/inverse_kinematics_constraint_unit.cpp b/trajopt_ifopt/test/inverse_kinematics_constraint_unit.cpp index a903d016..abd7b664 100644 --- a/trajopt_ifopt/test/inverse_kinematics_constraint_unit.cpp +++ b/trajopt_ifopt/test/inverse_kinematics_constraint_unit.cpp @@ -66,10 +66,10 @@ class InverseKinematicsConstraintUnit : public testing::TestWithParam(); - bool status = env->init(urdf_file, srdf_file, locator); + const tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/arm_around_table.urdf"); + const tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/pr2.srdf"); + const ResourceLocator::Ptr locator = std::make_shared(); + const bool status = env->init(urdf_file, srdf_file, locator); EXPECT_TRUE(status); // Extract necessary kinematic information @@ -106,7 +106,7 @@ TEST_F(InverseKinematicsConstraintUnit, GetValue) // NOLINT nlp.SetVariables(joint_position.data()); // Get the value (distance from IK position) - Eigen::VectorXd values = constraint->GetValues(); + const Eigen::VectorXd values = constraint->GetValues(); EXPECT_TRUE(almostEqualRelativeAndAbs(values, Eigen::VectorXd::Zero(n_dof))); // Check that jac wrt constraint_var is identity @@ -124,7 +124,7 @@ TEST_F(InverseKinematicsConstraintUnit, GetValue) // NOLINT auto error_calculator = [&](const Eigen::Ref& x) { return constraint->CalcValues(x, joint_position_single); }; - trajopt_ifopt::SparseMatrix num_jac_block = + const trajopt_ifopt::SparseMatrix num_jac_block = trajopt_ifopt::calcForwardNumJac(error_calculator, joint_position_single, 1e-4); EXPECT_TRUE(jac_block.isApprox(num_jac_block)); } @@ -147,7 +147,7 @@ TEST_F(InverseKinematicsConstraintUnit, GetSetBounds) // NOLINT // Check that setting bounds works { - Eigen::VectorXd pos = Eigen::VectorXd::Ones(n_dof); + const Eigen::VectorXd pos = Eigen::VectorXd::Ones(n_dof); auto var0 = std::make_shared(pos, kin_group->getJointNames(), "Joint_Position_0"); auto var1 = std::make_shared(pos, kin_group->getJointNames(), "Joint_Position_1"); @@ -155,7 +155,7 @@ TEST_F(InverseKinematicsConstraintUnit, GetSetBounds) // NOLINT auto constraint_2 = std::make_shared(target_pose, kinematic_info, var0, var1); - ifopt::Bounds bounds(-0.1234, 0.5678); + const ifopt::Bounds bounds(-0.1234, 0.5678); std::vector bounds_vec = std::vector(static_cast(n_dof), bounds); constraint_2->SetBounds(bounds_vec); diff --git a/trajopt_ifopt/test/joint_terms_unit.cpp b/trajopt_ifopt/test/joint_terms_unit.cpp index 4c3a8eed..fb4e178a 100644 --- a/trajopt_ifopt/test/joint_terms_unit.cpp +++ b/trajopt_ifopt/test/joint_terms_unit.cpp @@ -47,7 +47,7 @@ TEST(JointTermsUnit, JointPosConstraintUnit) // NOLINT CONSOLE_BRIDGE_logDebug("JointTermsUnit, JointPosConstraintUnit"); std::vector position_vars; - std::vector joint_names(10, "name"); + const std::vector joint_names(10, "name"); Eigen::VectorXd init1(10); init1 << 0, 1, 2, 3, 4, 5, 6, 7, 8, 9; position_vars.push_back(std::make_shared(init1, joint_names, "test_var1")); @@ -59,9 +59,9 @@ TEST(JointTermsUnit, JointPosConstraintUnit) // NOLINT Eigen::VectorXd targets(10); targets << 20, 21, 22, 23, 24, 25, 26, 27, 28, 29; - std::string name("test_cnt"); - Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(10, 1); - JointPosConstraint position_cnt(targets, position_vars, coeffs, name); + const std::string name("test_cnt"); + const Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(10, 1); + const JointPosConstraint position_cnt(targets, position_vars, coeffs, name); EXPECT_EQ(position_cnt.GetRows(), targets.size() * static_cast(position_vars.size())); EXPECT_EQ(position_cnt.GetName(), name); @@ -84,7 +84,7 @@ TEST(JointTermsUnit, JointVelConstraintUnit) // NOLINT x_vals.reserve(27); for (int i = -13; i < 14; ++i) { - std::vector joint_names{ "x", "y" }; + const std::vector joint_names{ "x", "y" }; Eigen::VectorXd val(2); val << f(i), f(i); auto var = std::make_shared(val, joint_names, "test_var_" + std::to_string(position_vars.size())); @@ -95,8 +95,8 @@ TEST(JointTermsUnit, JointVelConstraintUnit) // NOLINT Eigen::VectorXd targets(2); targets << 0, 0; - std::string name("test_joint_vel_cnt"); - Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(1, 1); + const std::string name("test_joint_vel_cnt"); + const Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(1, 1); JointVelConstraint velocity_cnt(targets, position_vars, coeffs, name); // Must link with variables or GetValues and GetJacobian throw exception. @@ -114,7 +114,7 @@ TEST(JointTermsUnit, JointVelConstraintUnit) // NOLINT { for (Eigen::Index j = 0; j < targets.size(); ++j) { - double expected_val = (f(x_vals[i + 1]) - f(x_vals[i])); + const double expected_val = (f(x_vals[i + 1]) - f(x_vals[i])); EXPECT_NEAR(velocity_vals((static_cast(i) * targets.size()) + j), expected_val, 1e-6); } } @@ -150,7 +150,7 @@ TEST(JointTermsUnit, JointVelConstraintMinimumUnit) // NOLINT x_vals.reserve(2); for (int i = -13; i < -11; ++i) { - std::vector joint_names{ "x", "y" }; + const std::vector joint_names{ "x", "y" }; Eigen::VectorXd val(2); val << f(i), f(i); auto var = std::make_shared(val, joint_names, "test_var_" + std::to_string(position_vars.size())); @@ -161,8 +161,8 @@ TEST(JointTermsUnit, JointVelConstraintMinimumUnit) // NOLINT Eigen::VectorXd targets(2); targets << 0, 0; - std::string name("test_joint_vel_cnt"); - Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(1, 1); + const std::string name("test_joint_vel_cnt"); + const Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(1, 1); JointVelConstraint velocity_cnt(targets, position_vars, coeffs, name); // Must link with variables or GetValues and GetJacobian throw exception. @@ -180,7 +180,7 @@ TEST(JointTermsUnit, JointVelConstraintMinimumUnit) // NOLINT { for (Eigen::Index j = 0; j < targets.size(); ++j) { - double expected_val = (f(x_vals[i + 1]) - f(x_vals[i])); + const double expected_val = (f(x_vals[i + 1]) - f(x_vals[i])); EXPECT_NEAR(velocity_vals((static_cast(i) * targets.size()) + j), expected_val, 1e-6); } } @@ -216,7 +216,7 @@ TEST(JointTermsUnit, JointAccelConstraintUnit) // NOLINT x_vals.reserve(27); for (int i = -13; i < 14; ++i) { - std::vector joint_names{ "x", "y" }; + const std::vector joint_names{ "x", "y" }; Eigen::VectorXd val(2); val << f(i), f(i); auto var = std::make_shared(val, joint_names, "test_var_" + std::to_string(position_vars.size())); @@ -227,8 +227,8 @@ TEST(JointTermsUnit, JointAccelConstraintUnit) // NOLINT Eigen::VectorXd targets(2); targets << 0, 0; - std::string name("test_joint_accel_cnt"); - Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(1, 1); + const std::string name("test_joint_accel_cnt"); + const Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(1, 1); JointAccelConstraint accel_cnt(targets, position_vars, coeffs, name); // Must link with variables or GetValues and GetJacobian throw exception. @@ -246,7 +246,7 @@ TEST(JointTermsUnit, JointAccelConstraintUnit) // NOLINT { for (Eigen::Index j = 0; j < targets.size(); ++j) { - double expected_val = (f(x_vals[i]) - 2.0 * f(x_vals[i + 1]) + f(x_vals[i + 2])); + const double expected_val = (f(x_vals[i]) - 2.0 * f(x_vals[i + 1]) + f(x_vals[i + 2])); EXPECT_NEAR(accel_vals((static_cast(i) * targets.size()) + j), expected_val, 1e-6); } } @@ -256,7 +256,7 @@ TEST(JointTermsUnit, JointAccelConstraintUnit) // NOLINT { for (Eigen::Index j = 0; j < targets.size(); ++j) { - double expected_val = (f(x_vals[i]) - 2.0 * f(x_vals[i - 1]) + f(x_vals[i - 2])); + const double expected_val = (f(x_vals[i]) - 2.0 * f(x_vals[i - 1]) + f(x_vals[i - 2])); EXPECT_NEAR(accel_vals((static_cast(i) * targets.size()) + j), expected_val, 1e-6); } } @@ -292,7 +292,7 @@ TEST(JointTermsUnit, JointAccelConstraintMinimumUnit) // NOLINT x_vals.reserve(4); for (int i = -13; i < -9; ++i) { - std::vector joint_names{ "x", "y" }; + const std::vector joint_names{ "x", "y" }; Eigen::VectorXd val(2); val << f(i), f(i); auto var = std::make_shared(val, joint_names, "test_var_" + std::to_string(position_vars.size())); @@ -303,8 +303,8 @@ TEST(JointTermsUnit, JointAccelConstraintMinimumUnit) // NOLINT Eigen::VectorXd targets(2); targets << 0, 0; - std::string name("test_joint_accel_cnt"); - Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(1, 1); + const std::string name("test_joint_accel_cnt"); + const Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(1, 1); JointAccelConstraint accel_cnt(targets, position_vars, coeffs, name); // Must link with variables or GetValues and GetJacobian throw exception. @@ -322,7 +322,7 @@ TEST(JointTermsUnit, JointAccelConstraintMinimumUnit) // NOLINT { for (Eigen::Index j = 0; j < targets.size(); ++j) { - double expected_val = (f(x_vals[i]) - 2.0 * f(x_vals[i + 1]) + f(x_vals[i + 2])); + const double expected_val = (f(x_vals[i]) - 2.0 * f(x_vals[i + 1]) + f(x_vals[i + 2])); EXPECT_NEAR(accel_vals((static_cast(i) * targets.size()) + j), expected_val, 1e-6); } } @@ -332,7 +332,7 @@ TEST(JointTermsUnit, JointAccelConstraintMinimumUnit) // NOLINT { for (Eigen::Index j = 0; j < targets.size(); ++j) { - double expected_val = (f(x_vals[i]) - 2.0 * f(x_vals[i - 1]) + f(x_vals[i - 2])); + const double expected_val = (f(x_vals[i]) - 2.0 * f(x_vals[i - 1]) + f(x_vals[i - 2])); EXPECT_NEAR(accel_vals((static_cast(i) * targets.size()) + j), expected_val, 1e-6); } } @@ -368,7 +368,7 @@ TEST(JointTermsUnit, JointJerkConstraintUnit) // NOLINT x_vals.reserve(27); for (int i = -13; i < 14; ++i) { - std::vector joint_names{ "x", "y" }; + const std::vector joint_names{ "x", "y" }; Eigen::VectorXd val(2); val << f(i), f(i); auto var = std::make_shared(val, joint_names, "test_var_" + std::to_string(position_vars.size())); @@ -379,8 +379,8 @@ TEST(JointTermsUnit, JointJerkConstraintUnit) // NOLINT Eigen::VectorXd targets(2); targets << 0, 0; - std::string name("test_joint_jerk_cnt"); - Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(1, 1); + const std::string name("test_joint_jerk_cnt"); + const Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(1, 1); JointJerkConstraint jerk_cnt(targets, position_vars, coeffs, name); // Must link with variables or GetValues and GetJacobian throw exception. @@ -398,7 +398,7 @@ TEST(JointTermsUnit, JointJerkConstraintUnit) // NOLINT { for (Eigen::Index j = 0; j < targets.size(); ++j) { - double expected_val = (-f(x_vals[i]) + 3.0 * f(x_vals[i + 1]) - 3.0 * f(x_vals[i + 2])) + f(x_vals[i + 3]); + const double expected_val = (-f(x_vals[i]) + 3.0 * f(x_vals[i + 1]) - 3.0 * f(x_vals[i + 2])) + f(x_vals[i + 3]); EXPECT_NEAR(jerk_vals((static_cast(i) * targets.size()) + j), expected_val, 1e-6); } } @@ -408,7 +408,7 @@ TEST(JointTermsUnit, JointJerkConstraintUnit) // NOLINT { for (Eigen::Index j = 0; j < targets.size(); ++j) { - double expected_val = f(x_vals[i]) - 3.0 * f(x_vals[i - 1]) + 3.0 * f(x_vals[i - 2]) - f(x_vals[i - 3]); + const double expected_val = f(x_vals[i]) - 3.0 * f(x_vals[i - 1]) + 3.0 * f(x_vals[i - 2]) - f(x_vals[i - 3]); EXPECT_NEAR(jerk_vals((static_cast(i) * targets.size()) + j), expected_val, 1e-6); } } @@ -444,7 +444,7 @@ TEST(JointTermsUnit, JointJerkConstraintMinimumUnit) // NOLINT x_vals.reserve(6); for (int i = -13; i < -7; ++i) { - std::vector joint_names{ "x", "y" }; + const std::vector joint_names{ "x", "y" }; Eigen::VectorXd val(2); val << f(i), f(i); auto var = std::make_shared(val, joint_names, "test_var_" + std::to_string(position_vars.size())); @@ -455,8 +455,8 @@ TEST(JointTermsUnit, JointJerkConstraintMinimumUnit) // NOLINT Eigen::VectorXd targets(2); targets << 0, 0; - std::string name("test_joint_jerk_cnt"); - Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(1, 1); + const std::string name("test_joint_jerk_cnt"); + const Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(1, 1); JointJerkConstraint jerk_cnt(targets, position_vars, coeffs, name); // Must link with variables or GetValues and GetJacobian throw exception. @@ -474,7 +474,7 @@ TEST(JointTermsUnit, JointJerkConstraintMinimumUnit) // NOLINT { for (Eigen::Index j = 0; j < targets.size(); ++j) { - double expected_val = (-f(x_vals[i]) + 3.0 * f(x_vals[i + 1]) - 3.0 * f(x_vals[i + 2])) + f(x_vals[i + 3]); + const double expected_val = (-f(x_vals[i]) + 3.0 * f(x_vals[i + 1]) - 3.0 * f(x_vals[i + 2])) + f(x_vals[i + 3]); EXPECT_NEAR(jerk_vals((static_cast(i) * targets.size()) + j), expected_val, 1e-6); } } @@ -484,7 +484,7 @@ TEST(JointTermsUnit, JointJerkConstraintMinimumUnit) // NOLINT { for (Eigen::Index j = 0; j < targets.size(); ++j) { - double expected_val = f(x_vals[i]) - 3.0 * f(x_vals[i - 1]) + 3.0 * f(x_vals[i - 2]) - f(x_vals[i - 3]); + const double expected_val = f(x_vals[i]) - 3.0 * f(x_vals[i - 1]) + 3.0 * f(x_vals[i - 2]) - f(x_vals[i - 3]); EXPECT_NEAR(jerk_vals((static_cast(i) * targets.size()) + j), expected_val, 1e-6); } } diff --git a/trajopt_ifopt/test/simple_collision_unit.cpp b/trajopt_ifopt/test/simple_collision_unit.cpp index 6477e10d..23ba3424 100644 --- a/trajopt_ifopt/test/simple_collision_unit.cpp +++ b/trajopt_ifopt/test/simple_collision_unit.cpp @@ -64,9 +64,9 @@ class SimpleCollisionTest : public testing::TestWithParam void SetUp() override { - boost::filesystem::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/spherebot.urdf"); - boost::filesystem::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/spherebot.srdf"); - ResourceLocator::Ptr locator = std::make_shared(); + boost::filesystem::path const urdf_file(std::string(TRAJOPT_DATA_DIR) + "/spherebot.urdf"); + boost::filesystem::path const srdf_file(std::string(TRAJOPT_DATA_DIR) + "/spherebot.srdf"); + ResourceLocator::Ptr const locator = std::make_shared(); EXPECT_TRUE(env->init(urdf_file, srdf_file, locator)); } }; @@ -82,8 +82,8 @@ TEST_F(SimpleCollisionTest, spheres) // NOLINT std::vector collisions; auto state_solver = env->getStateSolver(); - DiscreteContactManager::Ptr manager = env->getDiscreteContactManager(); - tesseract_kinematics::JointGroup::ConstPtr manip = env->getJointGroup("manipulator"); + DiscreteContactManager::Ptr const manager = env->getDiscreteContactManager(); + const tesseract_kinematics::JointGroup::ConstPtr manip = env->getJointGroup("manipulator"); manager->setActiveCollisionObjects(manip->getActiveLinkNames()); manager->setDefaultCollisionMarginData(0); @@ -106,13 +106,13 @@ TEST_F(SimpleCollisionTest, spheres) // NOLINT } // Step 3: Setup collision - double margin_coeff = 10; - double margin = 0.2; + const double margin_coeff = 10; + const double margin = 0.2; auto trajopt_collision_config = std::make_shared(margin, margin_coeff); trajopt_collision_config->collision_margin_buffer = 0.05; auto collision_cache = std::make_shared(100); - trajopt_ifopt::DiscreteCollisionEvaluator::Ptr collision_evaluator = + const trajopt_ifopt::DiscreteCollisionEvaluator::Ptr collision_evaluator = std::make_shared( collision_cache, manip, env, trajopt_collision_config); @@ -120,11 +120,12 @@ TEST_F(SimpleCollisionTest, spheres) // NOLINT nlp.AddConstraintSet(cnt); nlp.PrintCurrent(); - std::cout << "Jacobian: \n" << nlp.GetJacobianOfConstraints() << std::endl; + std::cout << "Jacobian: \n" << nlp.GetJacobianOfConstraints() << '\n'; auto error_calculator = [&](const Eigen::Ref& x) { return cnt->CalcValues(x); }; - trajopt_ifopt::SparseMatrix num_jac_block = trajopt_ifopt::calcForwardNumJac(error_calculator, positions[0], 1e-4); - std::cout << "Numerical Jacobian: \n" << num_jac_block << std::endl; + 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; @@ -137,13 +138,13 @@ TEST_F(SimpleCollisionTest, spheres) // NOLINT // 6) solve ipopt.Solve(nlp); Eigen::VectorXd x = nlp.GetOptVariables()->GetValues(); - std::cout << x.transpose() << std::endl; // NOLINT + std::cout << x.transpose() << '\n'; // NOLINT EXPECT_TRUE(ipopt.GetReturnStatus() == 0); tesseract_common::TrajArray inputs(1, 2); inputs << -0.75, 0.75; - Eigen::Map results(x.data(), 1, 2); + Eigen::Map const results(x.data(), 1, 2); bool found = checkTrajectory(collisions, *manager, *state_solver, manip->getJointNames(), inputs, *trajopt_collision_config); diff --git a/trajopt_ifopt/test/utils_unit.cpp b/trajopt_ifopt/test/utils_unit.cpp index 23891f40..cf930240 100644 --- a/trajopt_ifopt/test/utils_unit.cpp +++ b/trajopt_ifopt/test/utils_unit.cpp @@ -81,8 +81,8 @@ TEST(UtilsUnit, interpolate) // NOLINT { CONSOLE_BRIDGE_logDebug("UtilsUnit, interpolate"); - Eigen::VectorXd start = Eigen::VectorXd::Zero(10); - Eigen::VectorXd end = Eigen::VectorXd::Ones(10); + const Eigen::VectorXd start = Eigen::VectorXd::Zero(10); + const Eigen::VectorXd end = Eigen::VectorXd::Ones(10); std::vector results = interpolate(start, end, 11); // Check size @@ -101,52 +101,52 @@ TEST(UtilsUnit, interpolate) // NOLINT /** @brief Tests getClosestValidPoint: Input within Finite bounds*/ TEST(UtilsUnit, getClosestValidPoint1) // NOLINT { - ifopt::Bounds bound(-2.0, 2.0); - std::vector bounds(3, bound); - Eigen::VectorXd input = Eigen::VectorXd::Ones(3) * 1.0; - Eigen::VectorXd output = trajopt_ifopt::getClosestValidPoint(input, bounds); + const ifopt::Bounds bound(-2.0, 2.0); + const std::vector bounds(3, bound); + const Eigen::VectorXd input = Eigen::VectorXd::Ones(3) * 1.0; + const Eigen::VectorXd output = trajopt_ifopt::getClosestValidPoint(input, bounds); EXPECT_TRUE(output.isApprox(input)); } /** @brief Tests getClosestValidPoint: Input greater than Finite bounds*/ TEST(UtilsUnit, getClosestValidPoint2) // NOLINT { - ifopt::Bounds bound(-2.0, 2.0); - std::vector bounds(3, bound); - Eigen::VectorXd input = Eigen::VectorXd::Ones(3) * 3.0; - Eigen::VectorXd output = trajopt_ifopt::getClosestValidPoint(input, bounds); - Eigen::VectorXd desired_results = Eigen::VectorXd::Ones(3) * 2.0; + const ifopt::Bounds bound(-2.0, 2.0); + const std::vector bounds(3, bound); + const Eigen::VectorXd input = Eigen::VectorXd::Ones(3) * 3.0; + const Eigen::VectorXd output = trajopt_ifopt::getClosestValidPoint(input, bounds); + const Eigen::VectorXd desired_results = Eigen::VectorXd::Ones(3) * 2.0; EXPECT_TRUE(output.isApprox(desired_results)); } /** @brief Tests getClosestValidPoint: Input less than Finite bounds*/ TEST(UtilsUnit, getClosestValidPoint3) // NOLINT { - ifopt::Bounds bound(-2.0, 2.0); - std::vector bounds(3, bound); - Eigen::VectorXd input = Eigen::VectorXd::Ones(3) * -3.0; - Eigen::VectorXd output = trajopt_ifopt::getClosestValidPoint(input, bounds); - Eigen::VectorXd desired_results = Eigen::VectorXd::Ones(3) * -2.0; + const ifopt::Bounds bound(-2.0, 2.0); + const std::vector bounds(3, bound); + const Eigen::VectorXd input = Eigen::VectorXd::Ones(3) * -3.0; + const Eigen::VectorXd output = trajopt_ifopt::getClosestValidPoint(input, bounds); + const Eigen::VectorXd desired_results = Eigen::VectorXd::Ones(3) * -2.0; EXPECT_TRUE(output.isApprox(desired_results)); } /** @brief Tests getClosestValidPoint: Input within BoundGreaterZero*/ TEST(UtilsUnit, getClosestValidPoint4) // NOLINT { - ifopt::Bounds bound(ifopt::BoundGreaterZero); - std::vector bounds(3, bound); - Eigen::VectorXd input = Eigen::VectorXd::Ones(3) * 3.0e6; - Eigen::VectorXd output = trajopt_ifopt::getClosestValidPoint(input, bounds); + const ifopt::Bounds bound(ifopt::BoundGreaterZero); + const std::vector bounds(3, bound); + const Eigen::VectorXd input = Eigen::VectorXd::Ones(3) * 3.0e6; + const Eigen::VectorXd output = trajopt_ifopt::getClosestValidPoint(input, bounds); EXPECT_TRUE(output.isApprox(input)); } /** @brief Tests getClosestValidPoint: Input within BoundSmallerZero*/ TEST(UtilsUnit, getClosestValidPoint5) // NOLINT { - ifopt::Bounds bound(ifopt::BoundSmallerZero); - std::vector bounds(3, bound); - Eigen::VectorXd input = Eigen::VectorXd::Ones(3) * -3.0e6; - Eigen::VectorXd output = trajopt_ifopt::getClosestValidPoint(input, bounds); + const ifopt::Bounds bound(ifopt::BoundSmallerZero); + const std::vector bounds(3, bound); + const Eigen::VectorXd input = Eigen::VectorXd::Ones(3) * -3.0e6; + const Eigen::VectorXd output = trajopt_ifopt::getClosestValidPoint(input, bounds); EXPECT_TRUE(output.isApprox(input)); } @@ -154,104 +154,104 @@ TEST(UtilsUnit, getClosestValidPoint5) // NOLINT TEST(UtilsUnit, calcBoundsErrorsAndViolations) // NOLINT { { // BoundSmallerZero Outside bounds - ifopt::Bounds bound(ifopt::BoundSmallerZero); - std::vector bounds(3, bound); - Eigen::VectorXd input = Eigen::VectorXd::Constant(3, 3.5); - Eigen::VectorXd output = trajopt_ifopt::calcBoundsErrors(input, bounds); + const ifopt::Bounds bound(ifopt::BoundSmallerZero); + const std::vector bounds(3, bound); + const Eigen::VectorXd input = Eigen::VectorXd::Constant(3, 3.5); + const Eigen::VectorXd output = trajopt_ifopt::calcBoundsErrors(input, bounds); EXPECT_TRUE(output.isApprox(input)); - Eigen::VectorXd output2 = trajopt_ifopt::calcBoundsViolations(input, bounds); + const Eigen::VectorXd output2 = trajopt_ifopt::calcBoundsViolations(input, bounds); EXPECT_TRUE(output2.isApprox(output.cwiseAbs())); } { // BoundSmallerZero Inside bounds - ifopt::Bounds bound(ifopt::BoundSmallerZero); - std::vector bounds(3, bound); - Eigen::VectorXd input = Eigen::VectorXd::Constant(3, -3.5); - Eigen::VectorXd output = trajopt_ifopt::calcBoundsErrors(input, bounds); + const ifopt::Bounds bound(ifopt::BoundSmallerZero); + const std::vector bounds(3, bound); + const Eigen::VectorXd input = Eigen::VectorXd::Constant(3, -3.5); + const Eigen::VectorXd output = trajopt_ifopt::calcBoundsErrors(input, bounds); EXPECT_TRUE(output.isApprox(Eigen::VectorXd::Zero(3))); - Eigen::VectorXd output2 = trajopt_ifopt::calcBoundsViolations(input, bounds); + const Eigen::VectorXd output2 = trajopt_ifopt::calcBoundsViolations(input, bounds); EXPECT_TRUE(output2.isApprox(output.cwiseAbs())); } { // BoundGreaterZero Outside Bounds - ifopt::Bounds bound(ifopt::BoundGreaterZero); - std::vector bounds(3, bound); - Eigen::VectorXd input = Eigen::VectorXd::Constant(3, -3.5); - Eigen::VectorXd output = trajopt_ifopt::calcBoundsErrors(input, bounds); + const ifopt::Bounds bound(ifopt::BoundGreaterZero); + const std::vector bounds(3, bound); + const Eigen::VectorXd input = Eigen::VectorXd::Constant(3, -3.5); + const Eigen::VectorXd output = trajopt_ifopt::calcBoundsErrors(input, bounds); EXPECT_TRUE(output.isApprox(input)); - Eigen::VectorXd output2 = trajopt_ifopt::calcBoundsViolations(input, bounds); + const Eigen::VectorXd output2 = trajopt_ifopt::calcBoundsViolations(input, bounds); EXPECT_TRUE(output2.isApprox(output.cwiseAbs())); } { // BoundGreaterZero Inside Bounds - ifopt::Bounds bound(ifopt::BoundGreaterZero); - std::vector bounds(3, bound); - Eigen::VectorXd input = Eigen::VectorXd::Constant(3, 3.5); - Eigen::VectorXd output = trajopt_ifopt::calcBoundsErrors(input, bounds); + const ifopt::Bounds bound(ifopt::BoundGreaterZero); + const std::vector bounds(3, bound); + const Eigen::VectorXd input = Eigen::VectorXd::Constant(3, 3.5); + const Eigen::VectorXd output = trajopt_ifopt::calcBoundsErrors(input, bounds); EXPECT_TRUE(output.isApprox(Eigen::VectorXd::Zero(3))); - Eigen::VectorXd output2 = trajopt_ifopt::calcBoundsViolations(input, bounds); + const Eigen::VectorXd output2 = trajopt_ifopt::calcBoundsViolations(input, bounds); EXPECT_TRUE(output2.isApprox(output.cwiseAbs())); } { // BoundZero Outside Bounds Positive - ifopt::Bounds bound(ifopt::BoundZero); - std::vector bounds(3, bound); - Eigen::VectorXd input = Eigen::VectorXd::Constant(3, 3.5); - Eigen::VectorXd output = trajopt_ifopt::calcBoundsErrors(input, bounds); + const ifopt::Bounds bound(ifopt::BoundZero); + const std::vector bounds(3, bound); + const Eigen::VectorXd input = Eigen::VectorXd::Constant(3, 3.5); + const Eigen::VectorXd output = trajopt_ifopt::calcBoundsErrors(input, bounds); EXPECT_TRUE(output.isApprox(input)); - Eigen::VectorXd output2 = trajopt_ifopt::calcBoundsViolations(input, bounds); + const Eigen::VectorXd output2 = trajopt_ifopt::calcBoundsViolations(input, bounds); EXPECT_TRUE(output2.isApprox(output.cwiseAbs())); } { // BoundZero Outside Bounds Negative - ifopt::Bounds bound(ifopt::BoundZero); - std::vector bounds(3, bound); - Eigen::VectorXd input = Eigen::VectorXd::Constant(3, -3.5); - Eigen::VectorXd output = trajopt_ifopt::calcBoundsErrors(input, bounds); + const ifopt::Bounds bound(ifopt::BoundZero); + const std::vector bounds(3, bound); + const Eigen::VectorXd input = Eigen::VectorXd::Constant(3, -3.5); + const Eigen::VectorXd output = trajopt_ifopt::calcBoundsErrors(input, bounds); EXPECT_TRUE(output.isApprox(input)); - Eigen::VectorXd output2 = trajopt_ifopt::calcBoundsViolations(input, bounds); + const Eigen::VectorXd output2 = trajopt_ifopt::calcBoundsViolations(input, bounds); EXPECT_TRUE(output2.isApprox(output.cwiseAbs())); } { // BoundZero Inside Bounds - ifopt::Bounds bound(ifopt::BoundZero); - std::vector bounds(3, bound); - Eigen::VectorXd input = Eigen::VectorXd::Zero(3); - Eigen::VectorXd output = trajopt_ifopt::calcBoundsErrors(input, bounds); + const ifopt::Bounds bound(ifopt::BoundZero); + const std::vector bounds(3, bound); + const Eigen::VectorXd input = Eigen::VectorXd::Zero(3); + const Eigen::VectorXd output = trajopt_ifopt::calcBoundsErrors(input, bounds); EXPECT_TRUE(output.isApprox(input)); - Eigen::VectorXd output2 = trajopt_ifopt::calcBoundsViolations(input, bounds); + const Eigen::VectorXd output2 = trajopt_ifopt::calcBoundsViolations(input, bounds); EXPECT_TRUE(output2.isApprox(output.cwiseAbs())); } { // Custom Outside Bounds Positive - ifopt::Bounds bound(-3, 6); - std::vector bounds(3, bound); - Eigen::VectorXd input = Eigen::VectorXd::Constant(3, -3.5); - Eigen::VectorXd output = trajopt_ifopt::calcBoundsErrors(input, bounds); - Eigen::VectorXd viol = Eigen::VectorXd::Constant(3, -0.5); + const ifopt::Bounds bound(-3, 6); + const std::vector bounds(3, bound); + const Eigen::VectorXd input = Eigen::VectorXd::Constant(3, -3.5); + const Eigen::VectorXd output = trajopt_ifopt::calcBoundsErrors(input, bounds); + const Eigen::VectorXd viol = Eigen::VectorXd::Constant(3, -0.5); EXPECT_TRUE(output.isApprox(viol)); - Eigen::VectorXd output2 = trajopt_ifopt::calcBoundsViolations(input, bounds); + const Eigen::VectorXd output2 = trajopt_ifopt::calcBoundsViolations(input, bounds); EXPECT_TRUE(output2.isApprox(output.cwiseAbs())); } { // Custom Outside Bounds Negative - ifopt::Bounds bound(-3, 6); - std::vector bounds(3, bound); - Eigen::VectorXd input = Eigen::VectorXd::Constant(3, 6.5); - Eigen::VectorXd output = trajopt_ifopt::calcBoundsErrors(input, bounds); - Eigen::VectorXd viol = Eigen::VectorXd::Constant(3, 0.5); + const ifopt::Bounds bound(-3, 6); + const std::vector bounds(3, bound); + const Eigen::VectorXd input = Eigen::VectorXd::Constant(3, 6.5); + const Eigen::VectorXd output = trajopt_ifopt::calcBoundsErrors(input, bounds); + const Eigen::VectorXd viol = Eigen::VectorXd::Constant(3, 0.5); EXPECT_TRUE(output.isApprox(viol)); - Eigen::VectorXd output2 = trajopt_ifopt::calcBoundsViolations(input, bounds); + const Eigen::VectorXd output2 = trajopt_ifopt::calcBoundsViolations(input, bounds); EXPECT_TRUE(output2.isApprox(output.cwiseAbs())); } { // Custom Inside Bounds - ifopt::Bounds bound(-3, 6); - std::vector bounds(3, bound); - Eigen::VectorXd input = Eigen::VectorXd::Constant(3, 1); - Eigen::VectorXd output = trajopt_ifopt::calcBoundsErrors(input, bounds); + const ifopt::Bounds bound(-3, 6); + const std::vector bounds(3, bound); + const Eigen::VectorXd input = Eigen::VectorXd::Constant(3, 1); + const Eigen::VectorXd output = trajopt_ifopt::calcBoundsErrors(input, bounds); EXPECT_TRUE(output.isApprox(Eigen::VectorXd::Zero(3))); - Eigen::VectorXd output2 = trajopt_ifopt::calcBoundsViolations(input, bounds); + const Eigen::VectorXd output2 = trajopt_ifopt::calcBoundsViolations(input, bounds); EXPECT_TRUE(output2.isApprox(output.cwiseAbs())); } } diff --git a/trajopt_ifopt/test/variable_sets_unit.cpp b/trajopt_ifopt/test/variable_sets_unit.cpp index f714868d..817d896a 100644 --- a/trajopt_ifopt/test/variable_sets_unit.cpp +++ b/trajopt_ifopt/test/variable_sets_unit.cpp @@ -42,7 +42,7 @@ TEST(VariableSetsUnit, joint_position_1) // NOLINT { CONSOLE_BRIDGE_logDebug("VariableSetsUnit, joint_position_1"); - std::string name("test_var"); + const std::string name("test_var"); Eigen::VectorXd init(10); init << 0, 1, 2, 3, 4, 5, 6, 7, 8, 9; JointPosition position_var(init, std::vector(), name); @@ -60,7 +60,7 @@ TEST(VariableSetsUnit, joint_position_1) // NOLINT EXPECT_TRUE(changed.isApprox(position_var.GetValues())); // Check that setting bounds works - ifopt::Bounds bounds(-0.1234, 0.5678); + const ifopt::Bounds bounds(-0.1234, 0.5678); std::vector bounds_vec = std::vector(static_cast(init.size()), bounds); position_var.SetBounds(bounds_vec); std::vector results_vec = position_var.GetBounds(); diff --git a/trajopt_optimizers/trajopt_sqp/src/callbacks/joint_state_plotter.cpp b/trajopt_optimizers/trajopt_sqp/src/callbacks/joint_state_plotter.cpp index 42b5c334..bbe6872c 100644 --- a/trajopt_optimizers/trajopt_sqp/src/callbacks/joint_state_plotter.cpp +++ b/trajopt_optimizers/trajopt_sqp/src/callbacks/joint_state_plotter.cpp @@ -42,7 +42,7 @@ JointStatePlottingCallback::JointStatePlottingCallback(std::shared_ptrplotTrajectory(trajectory, *state_solver_); diff --git a/trajopt_optimizers/trajopt_sqp/src/expressions.cpp b/trajopt_optimizers/trajopt_sqp/src/expressions.cpp index c55f74a0..7d570503 100644 --- a/trajopt_optimizers/trajopt_sqp/src/expressions.cpp +++ b/trajopt_optimizers/trajopt_sqp/src/expressions.cpp @@ -15,7 +15,7 @@ QuadExprs::QuadExprs(Eigen::Index num_cost, Eigen::Index num_vars) Eigen::VectorXd QuadExprs::values(const Eigen::Ref& x) const { - Eigen::VectorXd result_lin = constants + (linear_coeffs * x); + 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) @@ -82,7 +82,7 @@ QuadExprs squareAffExprs(const AffExprs& aff_expr) // Now calculate the quadratic coefficients auto eq_affexpr_coeffs = aff_expr.linear_coeffs.row(i); - SparseMatrix eq_quadexpr_coeffs = eq_affexpr_coeffs.transpose() * eq_affexpr_coeffs; + const SparseMatrix eq_quadexpr_coeffs = eq_affexpr_coeffs.transpose() * eq_affexpr_coeffs; // Store the quadtratic coeffs and increment the objective quadratic coefficients if (eq_quadexpr_coeffs.nonZeros() > 0) diff --git a/trajopt_optimizers/trajopt_sqp/src/ifopt_qp_problem.cpp b/trajopt_optimizers/trajopt_sqp/src/ifopt_qp_problem.cpp index 915b5423..3f804d26 100644 --- a/trajopt_optimizers/trajopt_sqp/src/ifopt_qp_problem.cpp +++ b/trajopt_optimizers/trajopt_sqp/src/ifopt_qp_problem.cpp @@ -176,7 +176,7 @@ void IfoptQPProblem::updateGradient() // Set the gradient of the NLP costs //////////////////////////////////////////////////////// gradient_ = Eigen::VectorXd::Zero(num_qp_vars_); - SparseMatrix cost_jac = nlp_->GetJacobianOfCosts(); + const SparseMatrix cost_jac = nlp_->GetJacobianOfCosts(); /** * @note See CostFromFunc::convex in modeling_utils.cpp. Once Hessian has been implemented * @@ -248,7 +248,7 @@ void IfoptQPProblem::updateGradient() void IfoptQPProblem::linearizeConstraints() { - SparseMatrix jac = nlp_->GetJacobianOfConstraints(); + const SparseMatrix jac = nlp_->GetJacobianOfConstraints(); // Create triplet list of nonzero constraints using T = Eigen::Triplet; @@ -299,7 +299,7 @@ void IfoptQPProblem::updateCostsConstantExpression() // Get values about which we will linearize Eigen::VectorXd x_initial = nlp_->GetVariableValues().head(num_nlp_vars_); - Eigen::VectorXd cost_initial_value = nlp_->GetCosts().GetValues(); + const Eigen::VectorXd cost_initial_value = nlp_->GetCosts().GetValues(); // In the case of a QP problem the costs and constraints are represented as // quadratic functions is f(x) = a + b * x + c * x^2. @@ -316,8 +316,9 @@ void IfoptQPProblem::updateCostsConstantExpression() // The block excludes the slack variables /** @todo I am not sure this is correct because the gradient_ is not each individual cost function gradient */ - Eigen::VectorXd result_quad = x_initial.transpose() * hessian_.block(0, 0, num_nlp_vars_, num_nlp_vars_) * x_initial; - Eigen::VectorXd result_lin = x_initial.transpose() * gradient_.block(0, 0, num_nlp_vars_, num_nlp_costs_); + const Eigen::VectorXd result_quad = + x_initial.transpose() * hessian_.block(0, 0, num_nlp_vars_, num_nlp_vars_) * x_initial; + const Eigen::VectorXd result_lin = x_initial.transpose() * gradient_.block(0, 0, num_nlp_vars_, num_nlp_costs_); cost_constant_ = cost_initial_value - result_quad - result_lin; } @@ -327,8 +328,8 @@ void IfoptQPProblem::updateConstraintsConstantExpression() return; // Get values about which we will linearize - Eigen::VectorXd x_initial = nlp_->GetVariableValues().head(num_nlp_vars_); - Eigen::VectorXd cnt_initial_value = nlp_->GetConstraints().GetValues(); + const Eigen::VectorXd x_initial = nlp_->GetVariableValues().head(num_nlp_vars_); + const Eigen::VectorXd cnt_initial_value = nlp_->GetConstraints().GetValues(); // In the case of a QP problem the costs and constraints are represented as // quadratic functions is f(x) = a + b * x + c * x^2. @@ -345,7 +346,7 @@ void IfoptQPProblem::updateConstraintsConstantExpression() // to calculate the merit of the solve. // The block excludes the slack variables - SparseMatrix jac = constraint_matrix_.block(0, 0, num_nlp_cnts_, num_nlp_vars_); + const SparseMatrix jac = constraint_matrix_.block(0, 0, num_nlp_cnts_, num_nlp_vars_); constraint_constant_ = (cnt_initial_value - jac * x_initial); } @@ -365,8 +366,8 @@ void IfoptQPProblem::updateNLPConstraintBounds() cnt_bound_upper[i] = cnt_bounds[static_cast(i)].upper_; } - Eigen::VectorXd linearized_cnt_lower = cnt_bound_lower - constraint_constant_; - Eigen::VectorXd linearized_cnt_upper = cnt_bound_upper - constraint_constant_; + 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(num_nlp_cnts_) = linearized_cnt_lower; @@ -376,11 +377,11 @@ void IfoptQPProblem::updateNLPConstraintBounds() void IfoptQPProblem::updateNLPVariableBounds() { // This is eqivalent to BasicTrustRegionSQP::setTrustBoxConstraints - Eigen::VectorXd x_initial = nlp_->GetVariableValues(); + const Eigen::VectorXd x_initial = nlp_->GetVariableValues(); // Calculate box constraints - Eigen::VectorXd lower_box_cnt = x_initial - box_size_; - Eigen::VectorXd upper_box_cnt = x_initial + box_size_; + const Eigen::VectorXd lower_box_cnt = x_initial - box_size_; + const Eigen::VectorXd upper_box_cnt = x_initial + box_size_; // Set the variable limits once std::vector var_bounds = nlp_->GetBoundsOnOptimizationVariables(); @@ -393,10 +394,10 @@ void IfoptQPProblem::updateNLPVariableBounds() } // Apply box constraints and variable limits - Eigen::VectorXd var_bounds_lower_final = var_bounds_lower.cwiseMax(lower_box_cnt); + const Eigen::VectorXd var_bounds_lower_final = var_bounds_lower.cwiseMax(lower_box_cnt); // Add the extra check here that the upper is bigger than the lower. It seems that there can be issues when the // numbers get close to 0. - Eigen::VectorXd var_bounds_upper_final = var_bounds_upper.cwiseMin(upper_box_cnt).cwiseMax(var_bounds_lower); + const Eigen::VectorXd var_bounds_upper_final = var_bounds_upper.cwiseMin(upper_box_cnt).cwiseMax(var_bounds_lower); bounds_lower_.block(num_nlp_cnts_, 0, var_bounds_lower_final.size(), 1) = var_bounds_lower_final; bounds_upper_.block(num_nlp_cnts_, 0, var_bounds_upper_final.size(), 1) = var_bounds_upper_final; } @@ -436,8 +437,9 @@ Eigen::VectorXd IfoptQPProblem::evaluateConvexCosts(const Eigen::Ref& var_vals) { - Eigen::VectorXd result_lin = + const Eigen::VectorXd result_lin = constraint_matrix_.block(0, 0, num_nlp_cnts_, num_nlp_vars_) * var_vals.head(num_nlp_vars_); // NOLINT - Eigen::VectorXd constraint_value = constraint_constant_ + result_lin; + const Eigen::VectorXd constraint_value = constraint_constant_ + result_lin; return trajopt_ifopt::calcBoundsViolations(constraint_value, nlp_->GetBoundsOnConstraints()); } Eigen::VectorXd IfoptQPProblem::evaluateExactConstraintViolations(const Eigen::Ref& var_vals) { - Eigen::VectorXd cnt_vals = nlp_->EvaluateConstraints(var_vals.data()); + const Eigen::VectorXd cnt_vals = nlp_->EvaluateConstraints(var_vals.data()); return trajopt_ifopt::calcBoundsViolations(cnt_vals, nlp_->GetBoundsOnConstraints()); } @@ -499,7 +501,7 @@ Eigen::VectorXd IfoptQPProblem::getBoxSize() const { return box_size_; } void IfoptQPProblem::print() const { - Eigen::IOFormat format(3); + const Eigen::IOFormat format(3); std::cout << "-------------- QPProblem::print() --------------" << '\n'; std::cout << "Num NLP Vars: " << num_nlp_vars_ << '\n'; diff --git a/trajopt_optimizers/trajopt_sqp/src/osqp_eigen_solver.cpp b/trajopt_optimizers/trajopt_sqp/src/osqp_eigen_solver.cpp index 8d6c3a77..c6fadb01 100644 --- a/trajopt_optimizers/trajopt_sqp/src/osqp_eigen_solver.cpp +++ b/trajopt_optimizers/trajopt_sqp/src/osqp_eigen_solver.cpp @@ -79,7 +79,7 @@ bool OSQPEigenSolver::solve() if (OSQP_COMPARE_DEBUG_MODE) { - Eigen::IOFormat format(5); + const Eigen::IOFormat format(5); const auto* osqp_data = solver_->data()->getData(); std::cout << "OSQP Number of Variables:" << osqp_data->n << '\n'; std::cout << "OSQP Number of Constraints:" << osqp_data->m << '\n'; @@ -128,7 +128,7 @@ bool OSQPEigenSolver::solve() { if (OSQP_COMPARE_DEBUG_MODE) { - Eigen::IOFormat format(5); + const Eigen::IOFormat format(5); std::cout << "OSQP Solution: " << solver_->getSolution().transpose().format(format) << '\n'; } return true; @@ -144,8 +144,8 @@ bool OSQPEigenSolver::solve() std::cout << "\n---------------------------------------\n"; std::cout << std::scientific << "Primal Certificate (v): " << primal_certificate.transpose() << '\n'; - double first = bounds_lower_.transpose() * primal_certificate; - double second = bounds_upper_.transpose() * primal_certificate; + const double first = bounds_lower_.transpose() * primal_certificate; + const double second = bounds_upper_.transpose() * primal_certificate; std::cout << "A.transpose() * v = 0\n" << "l.transpose() * v = " << first << " u.transpose() * v = " << second << '\n'; @@ -186,12 +186,15 @@ Eigen::VectorXd OSQPEigenSolver::getSolution() // This code is required because there is a bug in OSQP 0.6.X scs_spalloc method /** @todo Remove when upgrading to OSQP 1.0.0 */ TRAJOPT_IGNORE_WARNINGS_PUSH -static void* csc_malloc(c_int n, c_int size) +namespace +{ + +void* csc_malloc(c_int n, c_int size) { return c_malloc(n * size); // NOLINT } -static void* csc_calloc(c_int n, c_int size) +void* csc_calloc(c_int n, c_int size) { return c_calloc(n, size); // NOLINT } @@ -234,17 +237,19 @@ csc* csc_spalloc_fix(c_int m, c_int n, c_int nzmax, c_int values, c_int triplet) return A; // NOLINT } + +} // namespace TRAJOPT_IGNORE_WARNINGS_POP bool OSQPEigenSolver::updateHessianMatrix(const SparseMatrix& hessian) { // Clean up values close to 0 // Also multiply by 2 because OSQP is multiplying by (1/2) for the objective fuction - SparseMatrix cleaned = 2.0 * hessian.pruned(1e-7, 1); // Any value < 1e-7 will be removed + const SparseMatrix cleaned = 2.0 * hessian.pruned(1e-7, 1); // Any value < 1e-7 will be removed if (solver_->isInitialized()) { - bool success = solver_->updateHessianMatrix(cleaned); + const bool success = solver_->updateHessianMatrix(cleaned); if (cleaned.nonZeros() == 0) /** @todo Remove when upgrading to OSQP 1.0.0 */ { csc_spfree_fix(solver_->data()->getData()->P); @@ -255,7 +260,7 @@ bool OSQPEigenSolver::updateHessianMatrix(const SparseMatrix& hessian) } solver_->data()->clearHessianMatrix(); - bool success = solver_->data()->setHessianMatrix(cleaned); + const bool success = solver_->data()->setHessianMatrix(cleaned); if (cleaned.nonZeros() == 0) /** @todo Remove when upgrading to OSQP 1.0.0 */ { csc_spfree_fix(solver_->data()->getData()->P); @@ -308,11 +313,11 @@ bool OSQPEigenSolver::updateLinearConstraintsMatrix(const SparseMatrix& linearCo assert(num_vars_ == linearConstraintsMatrix.cols()); solver_->data()->clearLinearConstraintsMatrix(); - SparseMatrix cleaned = linearConstraintsMatrix.pruned(1e-7, 1); // Any value < 1e-7 will be removed + const SparseMatrix cleaned = linearConstraintsMatrix.pruned(1e-7, 1); // Any value < 1e-7 will be removed if (solver_->isInitialized()) { - bool success = solver_->updateLinearConstraintsMatrix(cleaned); + const bool success = solver_->updateLinearConstraintsMatrix(cleaned); if (cleaned.nonZeros() == 0) /** @todo Remove when upgrading to OSQP 1.0.0 */ { csc_spfree_fix(solver_->data()->getData()->A); @@ -322,7 +327,7 @@ bool OSQPEigenSolver::updateLinearConstraintsMatrix(const SparseMatrix& linearCo return success; } - bool success = solver_->data()->setLinearConstraintsMatrix(cleaned); + const bool success = solver_->data()->setLinearConstraintsMatrix(cleaned); if (cleaned.nonZeros() == 0) /** @todo Remove when upgrading to OSQP 1.0.0 */ { csc_spfree_fix(solver_->data()->getData()->A); diff --git a/trajopt_optimizers/trajopt_sqp/src/trajopt_qp_problem.cpp b/trajopt_optimizers/trajopt_sqp/src/trajopt_qp_problem.cpp index a4c5af13..bfac6bc3 100644 --- a/trajopt_optimizers/trajopt_sqp/src/trajopt_qp_problem.cpp +++ b/trajopt_optimizers/trajopt_sqp/src/trajopt_qp_problem.cpp @@ -197,7 +197,7 @@ void TrajOptQPProblem::Implementation::addCostSet(const std::shared_ptrLinkWithVariables(variables_); - std::vector cost_bounds = constraint_set->GetBounds(); + const std::vector cost_bounds = constraint_set->GetBounds(); switch (penalty_type) { case CostPenaltyType::SQUARED: @@ -385,11 +385,12 @@ Eigen::VectorXd TrajOptQPProblem::Implementation::evaluateConvexCosts(const Eige if (hinge_costs_.GetRows() > 0) { - Eigen::VectorXd hinge_cnt_constant = constraint_constant_.topRows(hinge_costs_.GetRows()); + 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()); - Eigen::VectorXd hinge_convex_value = hinge_cnt_constant + hinge_cnt_jac * var_block; - Eigen::VectorXd hinge_cost = trajopt_ifopt::calcBoundsViolations(hinge_convex_value, hinge_costs_.GetBounds()); + 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()); costs.middleRows(squared_costs_.GetRows(), hinge_costs_.GetRows()) = hinge_cost; assert(!(costs.middleRows(squared_costs_.GetRows(), hinge_costs_.GetRows()).array() < -1e-8).any()); @@ -397,11 +398,13 @@ Eigen::VectorXd TrajOptQPProblem::Implementation::evaluateConvexCosts(const Eige if (abs_costs_.GetRows() > 0) { - Eigen::VectorXd abs_cnt_constant = constraint_constant_.middleRows(hinge_costs_.GetRows(), abs_costs_.GetRows()); + 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()); - Eigen::VectorXd abs_convex_value = abs_cnt_constant + abs_cnt_jac * var_block; - Eigen::VectorXd abs_cost = trajopt_ifopt::calcBoundsViolations(abs_convex_value, abs_costs_.GetBounds()).cwiseAbs(); + 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) @@ -478,11 +481,12 @@ Eigen::VectorXd TrajOptQPProblem::Implementation::evaluateExactCosts(const Eigen Eigen::VectorXd TrajOptQPProblem::Implementation::evaluateConvexConstraintViolations(const Eigen::Ref& var_vals) { - Eigen::Index row_index = hinge_constraints_.GetRows() + abs_costs_.GetRows(); + const Eigen::Index row_index = hinge_constraints_.GetRows() + abs_costs_.GetRows(); // NOLINTNEXTLINE Eigen::VectorXd result_lin = constraint_matrix_.block(row_index, 0, getNumNLPConstraints(), getNumNLPVars()) * var_vals.head(getNumNLPVars()); // NOLINT - Eigen::VectorXd constraint_value = constraint_constant_.middleRows(row_index, getNumNLPConstraints()) + result_lin; + const Eigen::VectorXd constraint_value = + constraint_constant_.middleRows(row_index, getNumNLPConstraints()) + result_lin; return trajopt_ifopt::calcBoundsViolations(constraint_value, constraints_.GetBounds()); } @@ -490,7 +494,7 @@ Eigen::VectorXd TrajOptQPProblem::Implementation::evaluateExactConstraintViolations(const Eigen::Ref& var_vals) { setVariables(var_vals.data()); - Eigen::VectorXd cnt_vals = constraints_.GetValues(); + const Eigen::VectorXd cnt_vals = constraints_.GetValues(); return trajopt_ifopt::calcBoundsViolations(cnt_vals, constraints_.GetBounds()); } @@ -515,7 +519,7 @@ void TrajOptQPProblem::Implementation::setConstraintMeritCoeff(const Eigen::Ref< void TrajOptQPProblem::Implementation::print() const { - Eigen::IOFormat format(3); + const Eigen::IOFormat format(3); std::cout << "-------------- QPProblem::print() --------------" << '\n'; std::cout << "Num NLP Vars: " << getNumNLPVars() << '\n'; @@ -579,7 +583,7 @@ void TrajOptQPProblem::Implementation::convexifyCosts() //////////////////////////////////////////////////////// gradient_ = Eigen::VectorXd::Zero(num_qp_vars_); - Eigen::VectorXd x_initial = variables_->GetValues().head(getNumNLPVars()); + const Eigen::VectorXd x_initial = variables_->GetValues().head(getNumNLPVars()); // Create triplet list of nonzero gradients std::vector> grad_triplet_list; @@ -590,8 +594,8 @@ void TrajOptQPProblem::Implementation::convexifyCosts() if (squared_costs_.GetRows() > 0) { squared_objective_nlp_ = QuadExprs(squared_costs_.GetRows(), getNumNLPVars()); - SparseMatrix cnt_jac = squared_costs_.GetJacobian(); - Eigen::VectorXd cnt_vals = squared_costs_.GetValues(); + const SparseMatrix cnt_jac = squared_costs_.GetJacobian(); + const Eigen::VectorXd cnt_vals = squared_costs_.GetValues(); // This is not correct should pass the value to createAffExprs then use bound to which could change the sign of the // affine expression @@ -686,9 +690,9 @@ void TrajOptQPProblem::Implementation::convexifyCosts() void TrajOptQPProblem::Implementation::linearizeConstraints() { - SparseMatrix nlp_cnt_jac = constraints_.GetJacobian(); - SparseMatrix hinge_cnt_jac = hinge_constraints_.GetJacobian(); - SparseMatrix abs_cnt_jac = abs_constraints_.GetJacobian(); + const SparseMatrix nlp_cnt_jac = constraints_.GetJacobian(); + 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; @@ -768,16 +772,16 @@ void TrajOptQPProblem::Implementation::linearizeConstraints() void TrajOptQPProblem::Implementation::updateConstraintsConstantExpression() { - long total_num_cnt = (getNumNLPConstraints() + hinge_constraints_.GetRows() + abs_constraints_.GetRows()); + const long total_num_cnt = (getNumNLPConstraints() + hinge_constraints_.GetRows() + abs_constraints_.GetRows()); if (total_num_cnt == 0) return; // Get values about which we will linearize - Eigen::VectorXd x_initial = variables_->GetValues().head(getNumNLPVars()); + const Eigen::VectorXd x_initial = variables_->GetValues().head(getNumNLPVars()); Eigen::Index current_row_index = 0; if (hinge_constraints_.GetRows() > 0) { // Get values about which we will linearize - Eigen::VectorXd cnt_initial_value = hinge_constraints_.GetValues(); + const Eigen::VectorXd cnt_initial_value = hinge_constraints_.GetValues(); // In the case of a QP problem the costs and constraints are represented as // quadratic functions is f(x) = a + b * x + c * x^2. @@ -794,7 +798,8 @@ void TrajOptQPProblem::Implementation::updateConstraintsConstantExpression() // to calculate the merit of the solve. // The block excludes the slack variables - SparseMatrix jac = constraint_matrix_.block(current_row_index, 0, hinge_constraints_.GetRows(), getNumNLPVars()); + 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(); @@ -802,7 +807,7 @@ void TrajOptQPProblem::Implementation::updateConstraintsConstantExpression() if (abs_constraints_.GetRows() > 0) { // Get values about which we will linearize - Eigen::VectorXd cnt_initial_value = abs_constraints_.GetValues(); + const Eigen::VectorXd cnt_initial_value = abs_constraints_.GetValues(); // In the case of a QP problem the costs and constraints are represented as // quadratic functions is f(x) = a + b * x + c * x^2. @@ -819,7 +824,8 @@ void TrajOptQPProblem::Implementation::updateConstraintsConstantExpression() // to calculate the merit of the solve. // The block excludes the slack variables - SparseMatrix jac = constraint_matrix_.block(current_row_index, 0, abs_constraints_.GetRows(), getNumNLPVars()); + 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(); @@ -827,7 +833,7 @@ void TrajOptQPProblem::Implementation::updateConstraintsConstantExpression() if (constraints_.GetRows() > 0) { - Eigen::VectorXd cnt_initial_value = constraints_.GetValues(); + const Eigen::VectorXd cnt_initial_value = constraints_.GetValues(); // In the case of a QP problem the costs and constraints are represented as // quadratic functions is f(x) = a + b * x + c * x^2. @@ -844,14 +850,14 @@ void TrajOptQPProblem::Implementation::updateConstraintsConstantExpression() // to calculate the merit of the solve. // The block excludes the slack variables - SparseMatrix jac = constraint_matrix_.block(current_row_index, 0, getNumNLPConstraints(), getNumNLPVars()); + 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); } } void TrajOptQPProblem::Implementation::updateNLPConstraintBounds() { - long total_num_cnt = (getNumNLPConstraints() + hinge_constraints_.GetRows() + abs_constraints_.GetRows()); + const long total_num_cnt = (getNumNLPConstraints() + hinge_constraints_.GetRows() + abs_constraints_.GetRows()); if (total_num_cnt == 0) return; @@ -885,8 +891,8 @@ void TrajOptQPProblem::Implementation::updateNLPConstraintBounds() cnt_bound_upper[current_row_index + i] = cnt_bounds[static_cast(i)].upper_; } - Eigen::VectorXd linearized_cnt_lower = cnt_bound_lower - constraint_constant_; - Eigen::VectorXd linearized_cnt_upper = cnt_bound_upper - constraint_constant_; + 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; @@ -896,11 +902,11 @@ void TrajOptQPProblem::Implementation::updateNLPConstraintBounds() void TrajOptQPProblem::Implementation::updateNLPVariableBounds() { // This is eqivalent to BasicTrustRegionSQP::setTrustBoxConstraints - Eigen::VectorXd x_initial = variables_->GetValues(); + const Eigen::VectorXd x_initial = variables_->GetValues(); // Calculate box constraints - Eigen::VectorXd lower_box_cnt = x_initial - box_size_; - Eigen::VectorXd upper_box_cnt = x_initial + box_size_; + const Eigen::VectorXd lower_box_cnt = x_initial - box_size_; + const Eigen::VectorXd upper_box_cnt = x_initial + box_size_; // Set the variable limits once std::vector var_bounds = variables_->GetBounds(); @@ -913,11 +919,11 @@ void TrajOptQPProblem::Implementation::updateNLPVariableBounds() } // Apply box constraints and variable limits - Eigen::VectorXd var_bounds_lower_final = var_bounds_lower.cwiseMax(lower_box_cnt); + const Eigen::VectorXd var_bounds_lower_final = var_bounds_lower.cwiseMax(lower_box_cnt); // Add the extra check here that the upper is bigger than the lower. It seems that there can be issues when the // numbers get close to 0. - Eigen::VectorXd var_bounds_upper_final = var_bounds_upper.cwiseMin(upper_box_cnt).cwiseMax(var_bounds_lower); - Eigen::Index var_row_index = getNumNLPConstraints() + hinge_constraints_.GetRows() + abs_constraints_.GetRows(); + const Eigen::VectorXd var_bounds_upper_final = var_bounds_upper.cwiseMin(upper_box_cnt).cwiseMax(var_bounds_lower); + 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; } 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 b2990f1f..f20c5d29 100644 --- a/trajopt_optimizers/trajopt_sqp/src/trust_region_sqp_solver.cpp +++ b/trajopt_optimizers/trajopt_sqp/src/trust_region_sqp_solver.cpp @@ -104,7 +104,7 @@ void TrustRegionSQPSolver::solve(const QPProblem::Ptr& qp_problem) // Convexification loop for (int convex_iteration = 1; convex_iteration < 100; convex_iteration++) { - double elapsed_time = std::chrono::duration(Clock::now() - start_time).count() / 1000.0; + const double elapsed_time = std::chrono::duration(Clock::now() - start_time).count() / 1000.0; if (elapsed_time > params.max_time) { CONSOLE_BRIDGE_logInform("Elapsed time %f has exceeded max time %f", elapsed_time, params.max_time); @@ -451,8 +451,8 @@ void TrustRegionSQPSolver::printStepInfo() const const std::vector& cost_names = qp_problem->getNLPCostNames(); for (Eigen::Index cost_number = 0; cost_number < static_cast(cost_names.size()); ++cost_number) { - double approx_improve = results_.best_costs[cost_number] - results_.new_approx_costs[cost_number]; - double exact_improve = results_.best_costs[cost_number] - results_.new_costs[cost_number]; + const double approx_improve = results_.best_costs[cost_number] - results_.new_approx_costs[cost_number]; + const double exact_improve = results_.best_costs[cost_number] - results_.new_costs[cost_number]; if (fabs(approx_improve) > 1e-8) std::printf("| %10s | %10.3e | %10.3e | %10.3e | %10.3e | %10.3e | %10.3e | %-15s\n", "----------", @@ -496,9 +496,9 @@ void TrustRegionSQPSolver::printStepInfo() const // Loop over constraints for (Eigen::Index cnt_number = 0; cnt_number < static_cast(constraint_names.size()); ++cnt_number) { - double approx_improve = + const double approx_improve = results_.best_constraint_violations[cnt_number] - results_.new_approx_constraint_violations[cnt_number]; - double exact_improve = + const double exact_improve = results_.best_constraint_violations[cnt_number] - results_.new_constraint_violations[cnt_number]; if (fabs(approx_improve) > 1e-8) std::printf("| %10.3e | %10.3e | %10.3e | %10.3e | %10.3e | %10.3e | %10.3e | %-15s\n", @@ -524,7 +524,7 @@ void TrustRegionSQPSolver::printStepInfo() const } // Constraint - std::string constraints_satisfied = + const std::string constraints_satisfied = (results_.new_constraint_violations.maxCoeff() < params.cnt_tolerance) ? "True" : "False"; std::printf("| %s |\n", std::string(88, '=').c_str()); std::printf("| %10s | %10.3e | %10.3e | %10.3e | %10s | %10s | %10s | SUM CONSTRAINTS (WITHOUT MERIT), Satisfied " diff --git a/trajopt_optimizers/trajopt_sqp/src/types.cpp b/trajopt_optimizers/trajopt_sqp/src/types.cpp index 462dde27..8c3a09c7 100644 --- a/trajopt_optimizers/trajopt_sqp/src/types.cpp +++ b/trajopt_optimizers/trajopt_sqp/src/types.cpp @@ -50,7 +50,7 @@ SQPResults::SQPResults(Eigen::Index num_vars, Eigen::Index num_cnts, Eigen::Inde void SQPResults::print() const { - Eigen::IOFormat format(3); + const Eigen::IOFormat format(3); std::cout << "-------------- SQPResults::print() --------------" << '\n'; std::cout << "best_exact_merit: " << best_exact_merit << '\n'; std::cout << "new_exact_merit: " << new_exact_merit << '\n'; diff --git a/trajopt_optimizers/trajopt_sqp/test/cart_position_optimization_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/cart_position_optimization_unit.cpp index 823ef320..718318af 100644 --- a/trajopt_optimizers/trajopt_sqp/test/cart_position_optimization_unit.cpp +++ b/trajopt_optimizers/trajopt_sqp/test/cart_position_optimization_unit.cpp @@ -67,9 +67,9 @@ class CartPositionOptimization : public testing::TestWithParam console_bridge::setLogLevel(console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_NONE); // 1) Load Robot - tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/arm_around_table.urdf"); - tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/pr2.srdf"); - tesseract_common::ResourceLocator::Ptr locator = std::make_shared(); + const tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/arm_around_table.urdf"); + const tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/pr2.srdf"); + const tesseract_common::ResourceLocator::Ptr locator = std::make_shared(); env = std::make_shared(); env->init(urdf_file, srdf_file, locator); } @@ -89,15 +89,15 @@ void runCartPositionOptimization(const trajopt_sqp::QPProblem::Ptr& qp_problem, qp_solver->solver_->settings()->setRelativeTolerance(1e-6); // Extract necessary kinematic information - tesseract_kinematics::JointGroup::ConstPtr manip = env->getJointGroup("right_arm"); + const tesseract_kinematics::JointGroup::ConstPtr manip = env->getJointGroup("right_arm"); // Get target position Eigen::VectorXd start_pos(manip->numJoints()); start_pos << 0.0, 0, 0, -1.0, 0, -1, -0.00; if (DEBUG) - std::cout << "Joint Limits:\n" << manip->getLimits().joint_limits.transpose() << std::endl; + std::cout << "Joint Limits:\n" << manip->getLimits().joint_limits.transpose() << '\n'; - Eigen::VectorXd joint_target = start_pos; + const Eigen::VectorXd joint_target = start_pos; Eigen::Isometry3d target_pose = manip->calcFwdKin(joint_target).at("r_gripper_tool_frame"); // 3) Add Variables @@ -114,7 +114,7 @@ void runCartPositionOptimization(const trajopt_sqp::QPProblem::Ptr& qp_problem, // 4) Add constraints for (const auto& var : vars) { - trajopt_ifopt::CartPosInfo cart_info( + const trajopt_ifopt::CartPosInfo cart_info( manip, "r_gripper_tool_frame", "base_footprint", Eigen::Isometry3d::Identity(), target_pose); auto cnt = std::make_shared(cart_info, var); qp_problem->addConstraintSet(cnt); @@ -129,13 +129,13 @@ void runCartPositionOptimization(const trajopt_sqp::QPProblem::Ptr& qp_problem, auto optimized_pose = manip->calcFwdKin(x).at("r_gripper_tool_frame"); EXPECT_TRUE(target_pose.translation().isApprox(optimized_pose.translation(), 1e-4)); - Eigen::Quaterniond target_q(target_pose.rotation()); - Eigen::Quaterniond optimized_q(optimized_pose.rotation()); + const Eigen::Quaterniond target_q(target_pose.rotation()); + const Eigen::Quaterniond optimized_q(optimized_pose.rotation()); EXPECT_TRUE(target_q.isApprox(optimized_q, 1e-5)); if (DEBUG) { - std::cout << x.transpose() << std::endl; + std::cout << x.transpose() << '\n'; qp_problem->print(); } } diff --git a/trajopt_optimizers/trajopt_sqp/test/cast_cost_attached_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/cast_cost_attached_unit.cpp index 48221fec..821c7691 100644 --- a/trajopt_optimizers/trajopt_sqp/test/cast_cost_attached_unit.cpp +++ b/trajopt_optimizers/trajopt_sqp/test/cast_cost_attached_unit.cpp @@ -75,10 +75,10 @@ class CastAttachedTest : public testing::TestWithParam void SetUp() override { - tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/boxbot.urdf"); - tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/boxbot.srdf"); + const tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/boxbot.urdf"); + const tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/boxbot.srdf"); - ResourceLocator::Ptr locator = std::make_shared(); + const ResourceLocator::Ptr locator = std::make_shared(); EXPECT_TRUE(env->init(urdf_file, srdf_file, locator)); // Create plotting tool @@ -140,9 +140,9 @@ void runCastAttachedLinkWithGeomTest(const trajopt_sqp::QPProblem::Ptr& qp_probl env->setState(ipos); std::vector collisions; - tesseract_scene_graph::StateSolver::Ptr state_solver = env->getStateSolver(); - ContinuousContactManager::Ptr manager = env->getContinuousContactManager(); - tesseract_kinematics::JointGroup::ConstPtr manip = env->getJointGroup("manipulator"); + const tesseract_scene_graph::StateSolver::Ptr state_solver = env->getStateSolver(); + const ContinuousContactManager::Ptr manager = env->getContinuousContactManager(); + const tesseract_kinematics::JointGroup::ConstPtr manip = env->getJointGroup("manipulator"); manager->setActiveCollisionObjects(manip->getActiveLinkNames()); manager->setDefaultCollisionMarginData(0); @@ -176,23 +176,23 @@ void runCastAttachedLinkWithGeomTest(const trajopt_sqp::QPProblem::Ptr& qp_probl } // Step 3: Setup collision - double margin_coeff = 20; - double margin = 0.3; + const double margin_coeff = 20; + const double margin = 0.3; auto trajopt_collision_config = std::make_shared(margin, margin_coeff); trajopt_collision_config->type = tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS; trajopt_collision_config->collision_margin_buffer = 0.05; // 4) Add constraints { // Fix start position - std::vector fixed_vars = { vars[0] }; - Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(manip->numJoints(), 5); + const std::vector fixed_vars = { vars[0] }; + const Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(manip->numJoints(), 5); auto cnt = std::make_shared(positions[0], fixed_vars, coeffs); qp_problem->addConstraintSet(cnt); } { // Fix end position - std::vector fixed_vars = { vars[2] }; - Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(manip->numJoints(), 5); + const std::vector fixed_vars = { vars[2] }; + const Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(manip->numJoints(), 5); auto cnt = std::make_shared(positions[2], fixed_vars, coeffs); qp_problem->addConstraintSet(cnt); } @@ -203,8 +203,8 @@ void runCastAttachedLinkWithGeomTest(const trajopt_sqp::QPProblem::Ptr& qp_probl auto collision_evaluator = std::make_shared( collision_cache, manip, env, trajopt_collision_config); - std::array position_vars{ vars[i - 1], vars[i] }; - std::array position_vars_fixed{ false, false }; + const std::array position_vars{ vars[i - 1], vars[i] }; + const std::array position_vars_fixed{ false, false }; auto cnt = std::make_shared( collision_evaluator, position_vars, position_vars_fixed, 3); qp_problem->addConstraintSet(cnt); @@ -228,13 +228,13 @@ void runCastAttachedLinkWithGeomTest(const trajopt_sqp::QPProblem::Ptr& qp_probl solver.verbose = true; solver.solve(qp_problem); Eigen::VectorXd x = qp_problem->getVariableValues(); - std::cout << x.transpose() << std::endl; + std::cout << x.transpose() << '\n'; EXPECT_TRUE(solver.getStatus() == trajopt_sqp::SQPStatus::NLP_CONVERGED); tesseract_common::TrajArray inputs(3, 2); inputs << -1.9, 0, 0, 1.9, 1.9, 3.8; - Eigen::Map results(x.data(), 3, 2); + const Eigen::Map results(x.data(), 3, 2); tesseract_collision::CollisionCheckConfig config; config.type = tesseract_collision::CollisionEvaluatorType::CONTINUOUS; @@ -260,9 +260,9 @@ void runCastAttachedLinkWithoutGeomTest(const trajopt_sqp::QPProblem::Ptr& qp_pr env->setState(ipos); std::vector collisions; - tesseract_scene_graph::StateSolver::Ptr state_solver = env->getStateSolver(); - ContinuousContactManager::Ptr manager = env->getContinuousContactManager(); - tesseract_kinematics::JointGroup::ConstPtr manip = env->getJointGroup("manipulator"); + const tesseract_scene_graph::StateSolver::Ptr state_solver = env->getStateSolver(); + const ContinuousContactManager::Ptr manager = env->getContinuousContactManager(); + const tesseract_kinematics::JointGroup::ConstPtr manip = env->getJointGroup("manipulator"); manager->setActiveCollisionObjects(manip->getActiveLinkNames()); manager->setDefaultCollisionMarginData(0); @@ -296,8 +296,8 @@ void runCastAttachedLinkWithoutGeomTest(const trajopt_sqp::QPProblem::Ptr& qp_pr } // Step 3: Setup collision - double margin_coeff = 10; - double margin = 0.02; + const double margin_coeff = 10; + const double margin = 0.02; auto trajopt_collision_config = std::make_shared(margin, margin_coeff); trajopt_collision_config->type = tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS; trajopt_collision_config->collision_margin_buffer = 0.01; @@ -305,15 +305,15 @@ void runCastAttachedLinkWithoutGeomTest(const trajopt_sqp::QPProblem::Ptr& qp_pr // 4) Add constraints { // Fix start position - std::vector fixed_vars = { vars[0] }; - Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(manip->numJoints(), 5); + const std::vector fixed_vars = { vars[0] }; + const Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(manip->numJoints(), 5); auto cnt = std::make_shared(positions[0], fixed_vars, coeffs); qp_problem->addConstraintSet(cnt); } { // Fix end position - std::vector fixed_vars = { vars[2] }; - Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(manip->numJoints(), 5); + const std::vector fixed_vars = { vars[2] }; + const Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(manip->numJoints(), 5); auto cnt = std::make_shared(positions[2], fixed_vars, coeffs); qp_problem->addConstraintSet(cnt); } @@ -325,7 +325,7 @@ void runCastAttachedLinkWithoutGeomTest(const trajopt_sqp::QPProblem::Ptr& qp_pr auto collision_evaluator = std::make_shared( collision_cache, manip, env, trajopt_collision_config); - std::array position_vars{ vars[i - 1], vars[i] }; + const std::array position_vars{ vars[i - 1], vars[i] }; auto cnt = std::make_shared( collision_evaluator, position_vars, position_vars_fixed, 2); @@ -352,13 +352,13 @@ void runCastAttachedLinkWithoutGeomTest(const trajopt_sqp::QPProblem::Ptr& qp_pr solver.verbose = true; solver.solve(qp_problem); Eigen::VectorXd x = qp_problem->getVariableValues(); - std::cout << x.transpose() << std::endl; + std::cout << x.transpose() << '\n'; EXPECT_TRUE(solver.getStatus() == trajopt_sqp::SQPStatus::NLP_CONVERGED); tesseract_common::TrajArray inputs(3, 2); inputs << -1.9, 0, 0, 1.9, 1.9, 3.8; - Eigen::Map results(x.data(), 3, 2); + const Eigen::Map results(x.data(), 3, 2); tesseract_collision::CollisionCheckConfig config; config.type = tesseract_collision::CollisionEvaluatorType::CONTINUOUS; diff --git a/trajopt_optimizers/trajopt_sqp/test/cast_cost_octomap_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/cast_cost_octomap_unit.cpp index ac882be2..19419a60 100644 --- a/trajopt_optimizers/trajopt_sqp/test/cast_cost_octomap_unit.cpp +++ b/trajopt_optimizers/trajopt_sqp/test/cast_cost_octomap_unit.cpp @@ -74,17 +74,17 @@ class CastOctomapTest : public testing::TestWithParam void SetUp() override { - tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/boxbot_world.urdf"); - tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/boxbot.srdf"); + const tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/boxbot_world.urdf"); + const tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/boxbot.srdf"); - ResourceLocator::Ptr locator = std::make_shared(); + const ResourceLocator::Ptr locator = std::make_shared(); EXPECT_TRUE(env->init(urdf_file, srdf_file, locator)); // Create plotting tool // plotter_.reset(new tesseract_ros::ROSBasicPlotting(env_)); octomap::Pointcloud point_cloud; - double delta = 0.05; + const double delta = 0.05; auto length = static_cast(1 / delta); for (int x = 0; x < length; ++x) @@ -94,11 +94,11 @@ class CastOctomapTest : public testing::TestWithParam -0.5F + static_cast(y * delta), -0.5F + static_cast(z * delta)); - std::shared_ptr octree = std::make_shared(2 * delta); + const std::shared_ptr octree = std::make_shared(2 * delta); octree->insertPointCloud(point_cloud, octomap::point3d(0, 0, 0)); // Next add objects that can be attached/detached to the scene - Octree::Ptr coll_octree = std::make_shared(octree, OctreeSubType::BOX); + const Octree::Ptr coll_octree = std::make_shared(octree, OctreeSubType::BOX); auto vis_box = std::make_shared(1.0, 1.0, 1.0); auto visual = std::make_shared(); @@ -130,9 +130,9 @@ void runCastOctomapTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const Env env->setState(ipos); std::vector collisions; - tesseract_scene_graph::StateSolver::Ptr state_solver = env->getStateSolver(); - ContinuousContactManager::Ptr manager = env->getContinuousContactManager(); - tesseract_kinematics::JointGroup::ConstPtr manip = env->getJointGroup("manipulator"); + const tesseract_scene_graph::StateSolver::Ptr state_solver = env->getStateSolver(); + const ContinuousContactManager::Ptr manager = env->getContinuousContactManager(); + const tesseract_kinematics::JointGroup::ConstPtr manip = env->getJointGroup("manipulator"); manager->setActiveCollisionObjects(manip->getActiveLinkNames()); manager->setDefaultCollisionMarginData(0); @@ -166,23 +166,23 @@ void runCastOctomapTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const Env } // Step 3: Setup collision - double margin_coeff = 10; - double margin = 0.02; + const double margin_coeff = 10; + const double margin = 0.02; auto trajopt_collision_config = std::make_shared(margin, margin_coeff); trajopt_collision_config->type = tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS; trajopt_collision_config->collision_margin_buffer = 0.05; // 4) Add constraints { // Fix start position - std::vector fixed_vars = { vars[0] }; - Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(manip->numJoints(), 5); + const std::vector fixed_vars = { vars[0] }; + const Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(manip->numJoints(), 5); auto cnt = std::make_shared(positions[0], fixed_vars, coeffs); qp_problem->addConstraintSet(cnt); } { // Fix end position - std::vector fixed_vars = { vars[2] }; - Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(manip->numJoints(), 5); + const std::vector fixed_vars = { vars[2] }; + const Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(manip->numJoints(), 5); auto cnt = std::make_shared(positions[2], fixed_vars, coeffs); qp_problem->addConstraintSet(cnt); } @@ -194,7 +194,7 @@ void runCastOctomapTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const Env auto collision_evaluator = std::make_shared( collision_cache, manip, env, trajopt_collision_config); - std::array position_vars{ vars[i - 1], vars[i] }; + const std::array position_vars{ vars[i - 1], vars[i] }; auto cnt = std::make_shared( collision_evaluator, position_vars, position_vars_fixed, 3); @@ -221,13 +221,13 @@ void runCastOctomapTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const Env solver.verbose = true; solver.solve(qp_problem); Eigen::VectorXd x = qp_problem->getVariableValues(); - std::cout << x.transpose() << std::endl; + std::cout << x.transpose() << '\n'; EXPECT_TRUE(solver.getStatus() == trajopt_sqp::SQPStatus::NLP_CONVERGED); tesseract_common::TrajArray inputs(3, 2); inputs << -1.9, 0, 0, 1.9, 1.9, 3.8; - Eigen::Map results(x.data(), 3, 2); + const Eigen::Map results(x.data(), 3, 2); tesseract_collision::CollisionCheckConfig config; config.type = tesseract_collision::CollisionEvaluatorType::CONTINUOUS; diff --git a/trajopt_optimizers/trajopt_sqp/test/cast_cost_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/cast_cost_unit.cpp index d33194ef..c5535704 100644 --- a/trajopt_optimizers/trajopt_sqp/test/cast_cost_unit.cpp +++ b/trajopt_optimizers/trajopt_sqp/test/cast_cost_unit.cpp @@ -67,9 +67,9 @@ class CastTest : public testing::TestWithParam void SetUp() override { - boost::filesystem::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/boxbot.urdf"); - boost::filesystem::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/boxbot.srdf"); - ResourceLocator::Ptr locator = std::make_shared(); + const boost::filesystem::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/boxbot.urdf"); + const boost::filesystem::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/boxbot.srdf"); + const ResourceLocator::Ptr locator = std::make_shared(); EXPECT_TRUE(env->init(urdf_file, srdf_file, locator)); } }; @@ -82,9 +82,9 @@ void runCastTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const Environmen env->setState(ipos); std::vector collisions; - tesseract_scene_graph::StateSolver::Ptr state_solver = env->getStateSolver(); - ContinuousContactManager::Ptr manager = env->getContinuousContactManager(); - tesseract_kinematics::JointGroup::ConstPtr manip = env->getJointGroup("manipulator"); + const tesseract_scene_graph::StateSolver::Ptr state_solver = env->getStateSolver(); + const ContinuousContactManager::Ptr manager = env->getContinuousContactManager(); + const tesseract_kinematics::JointGroup::ConstPtr manip = env->getJointGroup("manipulator"); manager->setActiveCollisionObjects(manip->getActiveLinkNames()); manager->setDefaultCollisionMarginData(0); @@ -118,23 +118,23 @@ void runCastTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const Environmen } // Step 3: Setup collision - double margin_coeff = 10; - double margin = 0.02; + const double margin_coeff = 10; + const double margin = 0.02; auto trajopt_collision_config = std::make_shared(margin, margin_coeff); trajopt_collision_config->type = tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS; trajopt_collision_config->collision_margin_buffer = 0.05; // 4) Add constraints { // Fix start position - std::vector fixed_vars = { vars[0] }; - Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(manip->numJoints(), 5); + const std::vector fixed_vars = { vars[0] }; + const Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(manip->numJoints(), 5); auto cnt = std::make_shared(positions[0], fixed_vars, coeffs); qp_problem->addConstraintSet(cnt); } { // Fix end position - std::vector fixed_vars = { vars[2] }; - Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(manip->numJoints(), 5); + const std::vector fixed_vars = { vars[2] }; + const Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(manip->numJoints(), 5); auto cnt = std::make_shared(positions[2], fixed_vars, coeffs); qp_problem->addConstraintSet(cnt); } @@ -146,7 +146,7 @@ void runCastTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const Environmen auto collision_evaluator = std::make_shared( collision_cache, manip, env, trajopt_collision_config); - std::array position_vars{ vars[i - 1], vars[i] }; + const std::array position_vars{ vars[i - 1], vars[i] }; auto cnt = std::make_shared( collision_evaluator, position_vars, position_vars_fixed, 3); @@ -173,13 +173,13 @@ void runCastTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const Environmen solver.verbose = true; solver.solve(qp_problem); Eigen::VectorXd x = qp_problem->getVariableValues(); - std::cout << x.transpose() << std::endl; + std::cout << x.transpose() << '\n'; EXPECT_TRUE(solver.getStatus() == trajopt_sqp::SQPStatus::NLP_CONVERGED); tesseract_common::TrajArray inputs(3, 2); inputs << -1.9, 0, 0, 1.9, 1.9, 3.8; - Eigen::Map results(x.data(), 3, 2); + const Eigen::Map results(x.data(), 3, 2); tesseract_collision::CollisionCheckConfig config; config.type = tesseract_collision::CollisionEvaluatorType::CONTINUOUS; diff --git a/trajopt_optimizers/trajopt_sqp/test/cast_cost_world_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/cast_cost_world_unit.cpp index 10ec0dcb..d8fa5238 100644 --- a/trajopt_optimizers/trajopt_sqp/test/cast_cost_world_unit.cpp +++ b/trajopt_optimizers/trajopt_sqp/test/cast_cost_world_unit.cpp @@ -71,10 +71,10 @@ class CastWorldTest : public testing::TestWithParam void SetUp() override { - tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/boxbot_world.urdf"); - tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/boxbot.srdf"); + const tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/boxbot_world.urdf"); + const tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/boxbot.srdf"); - ResourceLocator::Ptr locator = std::make_shared(); + const ResourceLocator::Ptr locator = std::make_shared(); EXPECT_TRUE(env->init(urdf_file, srdf_file, locator)); // Create plotting tool @@ -114,9 +114,9 @@ void runCastWorldTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const Envir env->setState(ipos); std::vector collisions; - tesseract_scene_graph::StateSolver::Ptr state_solver = env->getStateSolver(); - ContinuousContactManager::Ptr manager = env->getContinuousContactManager(); - tesseract_kinematics::JointGroup::ConstPtr manip = env->getJointGroup("manipulator"); + const tesseract_scene_graph::StateSolver::Ptr state_solver = env->getStateSolver(); + const ContinuousContactManager::Ptr manager = env->getContinuousContactManager(); + const tesseract_kinematics::JointGroup::ConstPtr manip = env->getJointGroup("manipulator"); manager->setActiveCollisionObjects(manip->getActiveLinkNames()); manager->setDefaultCollisionMarginData(0); @@ -150,23 +150,23 @@ void runCastWorldTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const Envir } // Step 3: Setup collision - double margin_coeff = 10; - double margin = 0.02; + const double margin_coeff = 10; + const double margin = 0.02; auto trajopt_collision_config = std::make_shared(margin, margin_coeff); trajopt_collision_config->type = tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS; trajopt_collision_config->collision_margin_buffer = 0.05; // 4) Add constraints { // Fix start position - std::vector fixed_vars = { vars[0] }; - Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(manip->numJoints(), 5); + const std::vector fixed_vars = { vars[0] }; + const Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(manip->numJoints(), 5); auto cnt = std::make_shared(positions[0], fixed_vars, coeffs); qp_problem->addConstraintSet(cnt); } { // Fix end position - std::vector fixed_vars = { vars[2] }; - Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(manip->numJoints(), 5); + const std::vector fixed_vars = { vars[2] }; + const Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(manip->numJoints(), 5); auto cnt = std::make_shared(positions[2], fixed_vars, coeffs); qp_problem->addConstraintSet(cnt); } @@ -178,7 +178,7 @@ void runCastWorldTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const Envir auto collision_evaluator = std::make_shared( collision_cache, manip, env, trajopt_collision_config); - std::array position_vars{ vars[i - 1], vars[i] }; + const std::array position_vars{ vars[i - 1], vars[i] }; auto cnt = std::make_shared( collision_evaluator, position_vars, position_vars_fixed, 3); @@ -205,13 +205,13 @@ void runCastWorldTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const Envir solver.verbose = true; solver.solve(qp_problem); Eigen::VectorXd x = qp_problem->getVariableValues(); - std::cout << x.transpose() << std::endl; + std::cout << x.transpose() << '\n'; EXPECT_TRUE(solver.getStatus() == trajopt_sqp::SQPStatus::NLP_CONVERGED); tesseract_common::TrajArray inputs(3, 2); inputs << -1.9, 0, 0, 1.9, 1.9, 3.8; - Eigen::Map results(x.data(), 3, 2); + const Eigen::Map results(x.data(), 3, 2); tesseract_collision::CollisionCheckConfig config; config.type = tesseract_collision::CollisionEvaluatorType::CONTINUOUS; diff --git a/trajopt_optimizers/trajopt_sqp/test/expressions_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/expressions_unit.cpp index dbb83e17..a51bd5c1 100644 --- a/trajopt_optimizers/trajopt_sqp/test/expressions_unit.cpp +++ b/trajopt_optimizers/trajopt_sqp/test/expressions_unit.cpp @@ -53,7 +53,7 @@ TEST(ExpressionsTest, AffExprsWithWeights) // NOLINT CONSOLE_BRIDGE_logDebug("ExpressionsTest, AffExprsWithWeights"); // f = (x(0) - x(1))^2 // weight = 5 - double w = 5; + const double w = 5; Eigen::Vector2d x(5, 1); Eigen::VectorXd e(1); e(0) = 16; @@ -104,7 +104,7 @@ TEST(ExpressionsTest, squareAffExprs1) // NOLINT e(0) = x(0) - x(1); Eigen::Vector2d J(1, -1); - AffExprs aff_exprs = trajopt_sqp::createAffExprs(e, J.transpose().sparseView(), x); + const AffExprs aff_exprs = trajopt_sqp::createAffExprs(e, J.transpose().sparseView(), x); EXPECT_NEAR(aff_exprs.values(x)(0), e(0), 1e-8); QuadExprs quad_exprs = trajopt_sqp::squareAffExprs(aff_exprs); diff --git a/trajopt_optimizers/trajopt_sqp/test/joint_acceleration_optimization_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/joint_acceleration_optimization_unit.cpp index 18aeb24d..7b1370fe 100644 --- a/trajopt_optimizers/trajopt_sqp/test/joint_acceleration_optimization_unit.cpp +++ b/trajopt_optimizers/trajopt_sqp/test/joint_acceleration_optimization_unit.cpp @@ -72,7 +72,7 @@ void runAccelerationConstraintOptimizationTest(const trajopt_sqp::QPProblem::Ptr // 2) Add Variables std::vector vars; - std::vector joint_names(7, "name"); + const std::vector joint_names(7, "name"); { auto pos = Eigen::VectorXd::Zero(7); auto var = std::make_shared(pos, joint_names, "Joint_Position_0"); @@ -93,7 +93,7 @@ void runAccelerationConstraintOptimizationTest(const trajopt_sqp::QPProblem::Ptr } // 3) Add constraints - Eigen::VectorXd start_pos = Eigen::VectorXd::Zero(7); + const Eigen::VectorXd start_pos = Eigen::VectorXd::Zero(7); std::vector start; start.push_back(vars.front()); Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(7, 5); @@ -101,14 +101,14 @@ void runAccelerationConstraintOptimizationTest(const trajopt_sqp::QPProblem::Ptr std::make_shared(start_pos, start, coeffs, "StartPosition"); qp_problem->addConstraintSet(start_constraint); - Eigen::VectorXd end_pos = Eigen::VectorXd::Ones(7) * 10; + const Eigen::VectorXd end_pos = Eigen::VectorXd::Ones(7) * 10; std::vector end; end.push_back(vars.back()); auto end_constraint = std::make_shared(end_pos, end, coeffs, "EndPosition"); qp_problem->addConstraintSet(end_constraint); // 4) Add costs - Eigen::VectorXd accel_target = Eigen::VectorXd::Zero(7); + const Eigen::VectorXd accel_target = Eigen::VectorXd::Zero(7); coeffs = Eigen::VectorXd::Constant(1, 1); auto accel_constraint = std::make_shared(accel_target, vars, coeffs, "ja"); qp_problem->addCostSet(accel_constraint, trajopt_sqp::CostPenaltyType::SQUARED); @@ -141,7 +141,7 @@ TEST_F(AccelerationConstraintOptimization, acceleration_constraint_optimization_ // 2) Add Variables std::vector vars; - std::vector joint_names(7, "name"); + const std::vector joint_names(7, "name"); { auto pos = Eigen::VectorXd::Zero(7); auto var = std::make_shared(pos, joint_names, "Joint_Position_0"); @@ -162,7 +162,7 @@ TEST_F(AccelerationConstraintOptimization, acceleration_constraint_optimization_ } // 3) Add constraints - Eigen::VectorXd start_pos = Eigen::VectorXd::Zero(7); + const Eigen::VectorXd start_pos = Eigen::VectorXd::Zero(7); std::vector start; start.push_back(vars.front()); @@ -171,28 +171,28 @@ TEST_F(AccelerationConstraintOptimization, acceleration_constraint_optimization_ std::make_shared(start_pos, start, coeffs, "StartPosition"); nlp_ipopt.AddConstraintSet(start_constraint); - Eigen::VectorXd end_pos = Eigen::VectorXd::Ones(7) * 10; + const Eigen::VectorXd end_pos = Eigen::VectorXd::Ones(7) * 10; std::vector end; end.push_back(vars.back()); auto end_constraint = std::make_shared(end_pos, end, coeffs, "EndPosition"); nlp_ipopt.AddConstraintSet(end_constraint); // 4) Add costs - Eigen::VectorXd accel_target = Eigen::VectorXd::Zero(7); + const Eigen::VectorXd accel_target = Eigen::VectorXd::Zero(7); coeffs = Eigen::VectorXd::Constant(1, 1); auto accel_constraint = std::make_shared(accel_target, vars, coeffs, "jv"); // Must link the variables to the constraint since that happens in AddConstraintSet accel_constraint->LinkWithVariables(nlp_ipopt.GetOptVariables()); - Eigen::VectorXd weights = Eigen::VectorXd::Constant(accel_constraint->GetRows(), 0.01); + const Eigen::VectorXd weights = Eigen::VectorXd::Constant(accel_constraint->GetRows(), 0.01); auto accel_cost = std::make_shared(accel_constraint, weights); nlp_ipopt.AddCostSet(accel_cost); if (DEBUG) { nlp_ipopt.PrintCurrent(); - std::cout << "Constraint Jacobian: \n" << nlp_ipopt.GetJacobianOfConstraints().toDense() << std::endl; - std::cout << "Cost Jacobian: \n" << nlp_ipopt.GetJacobianOfCosts() << std::endl; + std::cout << "Constraint Jacobian: \n" << nlp_ipopt.GetJacobianOfConstraints().toDense() << '\n'; + std::cout << "Cost Jacobian: \n" << nlp_ipopt.GetJacobianOfCosts() << '\n'; } ifopt::IpoptSolver solver; @@ -216,7 +216,7 @@ TEST_F(AccelerationConstraintOptimization, acceleration_constraint_optimization_ EXPECT_NEAR(x[i], 10.0, 1e-5); if (DEBUG) { - std::cout << x.transpose() << std::endl; + std::cout << x.transpose() << '\n'; nlp_ipopt.PrintCurrent(); } } diff --git a/trajopt_optimizers/trajopt_sqp/test/joint_jerk_optimization_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/joint_jerk_optimization_unit.cpp index b3e9b97c..40dd66f3 100644 --- a/trajopt_optimizers/trajopt_sqp/test/joint_jerk_optimization_unit.cpp +++ b/trajopt_optimizers/trajopt_sqp/test/joint_jerk_optimization_unit.cpp @@ -27,7 +27,6 @@ #include TRAJOPT_IGNORE_WARNINGS_PUSH #include -#include #include @@ -74,7 +73,7 @@ void runJerkConstraintOptimizationTest(const trajopt_sqp::QPProblem::Ptr& qp_pro // 2) Add Variables std::vector vars; - std::vector joint_names(7, "name"); + const std::vector joint_names(7, "name"); { auto pos = Eigen::VectorXd::Zero(7); auto var = std::make_shared(pos, joint_names, "Joint_Position_0"); @@ -99,7 +98,7 @@ void runJerkConstraintOptimizationTest(const trajopt_sqp::QPProblem::Ptr& qp_pro } // 3) Add constraints - Eigen::VectorXd start_pos = Eigen::VectorXd::Zero(7); + const Eigen::VectorXd start_pos = Eigen::VectorXd::Zero(7); std::vector start; start.push_back(vars.front()); Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(7, 5); @@ -107,14 +106,14 @@ void runJerkConstraintOptimizationTest(const trajopt_sqp::QPProblem::Ptr& qp_pro std::make_shared(start_pos, start, coeffs, "StartPosition"); qp_problem->addConstraintSet(start_constraint); - Eigen::VectorXd end_pos = Eigen::VectorXd::Ones(7) * 10; + const Eigen::VectorXd end_pos = Eigen::VectorXd::Ones(7) * 10; std::vector end; end.push_back(vars.back()); auto end_constraint = std::make_shared(end_pos, end, coeffs, "EndPosition"); qp_problem->addConstraintSet(end_constraint); // 4) Add costs - Eigen::VectorXd jerk_target = Eigen::VectorXd::Zero(7); + const Eigen::VectorXd jerk_target = Eigen::VectorXd::Zero(7); coeffs = Eigen::VectorXd::Constant(1, 1); auto jerk_constraint = std::make_shared(jerk_target, vars, coeffs, "ja"); qp_problem->addCostSet(jerk_constraint, trajopt_sqp::CostPenaltyType::SQUARED); diff --git a/trajopt_optimizers/trajopt_sqp/test/joint_position_optimization_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/joint_position_optimization_unit.cpp index f5af3b61..2f8fd133 100644 --- a/trajopt_optimizers/trajopt_sqp/test/joint_position_optimization_unit.cpp +++ b/trajopt_optimizers/trajopt_sqp/test/joint_position_optimization_unit.cpp @@ -73,7 +73,7 @@ void runJointPositionOptimizationTest(const trajopt_sqp::QPProblem::Ptr& qp_prob for (int ind = 0; ind < 2; ind++) { auto pos = Eigen::VectorXd::Zero(7); - std::vector joint_names(7, "name"); + const std::vector joint_names(7, "name"); auto var = std::make_shared(pos, joint_names, "Joint_Position_" + std::to_string(ind)); vars.push_back(var); @@ -81,16 +81,16 @@ void runJointPositionOptimizationTest(const trajopt_sqp::QPProblem::Ptr& qp_prob } // 3) Add constraints - Eigen::VectorXd start_pos = Eigen::VectorXd::Zero(7); + const Eigen::VectorXd start_pos = Eigen::VectorXd::Zero(7); std::vector start; start.push_back(vars.front()); - Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(7, 1); + const Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(7, 1); auto start_constraint = std::make_shared(start_pos, start, coeffs, "StartPosition"); qp_problem->addConstraintSet(start_constraint); - Eigen::VectorXd end_pos = Eigen::VectorXd::Ones(7); + const Eigen::VectorXd end_pos = Eigen::VectorXd::Ones(7); std::vector end; end.push_back(vars.back()); auto end_constraint = std::make_shared(end_pos, end, coeffs, "EndPosition"); @@ -125,7 +125,7 @@ TEST_F(JointPositionOptimization, joint_position_optimization_ipopt) // NOLINT for (int ind = 0; ind < 2; ind++) { auto pos = Eigen::VectorXd::Zero(7); - std::vector joint_names(7, "name"); + const std::vector joint_names(7, "name"); auto var = std::make_shared(pos, joint_names, "Joint_Position_" + std::to_string(ind)); vars.push_back(var); @@ -133,16 +133,16 @@ TEST_F(JointPositionOptimization, joint_position_optimization_ipopt) // NOLINT } // 3) Add constraints - Eigen::VectorXd start_pos = Eigen::VectorXd::Zero(7); + const Eigen::VectorXd start_pos = Eigen::VectorXd::Zero(7); std::vector start; start.push_back(vars.front()); - Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(7, 1); + const Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(7, 1); auto start_constraint = std::make_shared(start_pos, start, coeffs, "StartPosition"); nlp_ipopt.AddConstraintSet(start_constraint); - Eigen::VectorXd end_pos = Eigen::VectorXd::Ones(7); + const Eigen::VectorXd end_pos = Eigen::VectorXd::Ones(7); std::vector end; end.push_back(vars.back()); auto end_constraint = std::make_shared(end_pos, end, coeffs, "EndPosition"); @@ -164,7 +164,7 @@ TEST_F(JointPositionOptimization, joint_position_optimization_ipopt) // NOLINT EXPECT_NEAR(x[i], 1.0, 1e-5); if (DEBUG) { - std::cout << x.transpose() << std::endl; + std::cout << x.transpose() << '\n'; nlp_ipopt.PrintCurrent(); } } diff --git a/trajopt_optimizers/trajopt_sqp/test/joint_velocity_optimization_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/joint_velocity_optimization_unit.cpp index c2a2ed48..a6360966 100644 --- a/trajopt_optimizers/trajopt_sqp/test/joint_velocity_optimization_unit.cpp +++ b/trajopt_optimizers/trajopt_sqp/test/joint_velocity_optimization_unit.cpp @@ -72,7 +72,7 @@ void runVelocityConstraintOptimizationTest(const trajopt_sqp::QPProblem::Ptr& qp // 2) Add Variables std::vector vars; - std::vector joint_names(7, "name"); + const std::vector joint_names(7, "name"); { auto pos = Eigen::VectorXd::Zero(7); auto var = std::make_shared(pos, joint_names, "Joint_Position_0"); @@ -93,7 +93,7 @@ void runVelocityConstraintOptimizationTest(const trajopt_sqp::QPProblem::Ptr& qp } // 3) Add constraints - Eigen::VectorXd start_pos = Eigen::VectorXd::Zero(7); + const Eigen::VectorXd start_pos = Eigen::VectorXd::Zero(7); std::vector start; start.push_back(vars.front()); Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(7, 5); @@ -101,14 +101,14 @@ void runVelocityConstraintOptimizationTest(const trajopt_sqp::QPProblem::Ptr& qp std::make_shared(start_pos, start, coeffs, "StartPosition"); qp_problem->addConstraintSet(start_constraint); - Eigen::VectorXd end_pos = Eigen::VectorXd::Ones(7) * 10; + const Eigen::VectorXd end_pos = Eigen::VectorXd::Ones(7) * 10; std::vector end; end.push_back(vars.back()); auto end_constraint = std::make_shared(end_pos, end, coeffs, "EndPosition"); qp_problem->addConstraintSet(end_constraint); // 4) Add costs - Eigen::VectorXd vel_target = Eigen::VectorXd::Zero(7); + const Eigen::VectorXd vel_target = Eigen::VectorXd::Zero(7); coeffs = Eigen::VectorXd::Constant(1, 1); auto vel_constraint = std::make_shared(vel_target, vars, coeffs, "jv"); qp_problem->addCostSet(vel_constraint, trajopt_sqp::CostPenaltyType::SQUARED); @@ -139,7 +139,7 @@ TEST_F(VelocityConstraintOptimization, velocity_constraint_optimization_ipopt) // 2) Add Variables std::vector vars; - std::vector joint_names(7, "name"); + const std::vector joint_names(7, "name"); { auto pos = Eigen::VectorXd::Zero(7); auto var = std::make_shared(pos, joint_names, "Joint_Position_0"); @@ -160,7 +160,7 @@ TEST_F(VelocityConstraintOptimization, velocity_constraint_optimization_ipopt) } // 3) Add constraints - Eigen::VectorXd start_pos = Eigen::VectorXd::Zero(7); + const Eigen::VectorXd start_pos = Eigen::VectorXd::Zero(7); std::vector start; start.push_back(vars.front()); @@ -169,28 +169,28 @@ TEST_F(VelocityConstraintOptimization, velocity_constraint_optimization_ipopt) std::make_shared(start_pos, start, coeffs, "StartPosition"); nlp_ipopt.AddConstraintSet(start_constraint); - Eigen::VectorXd end_pos = Eigen::VectorXd::Ones(7) * 10; + const Eigen::VectorXd end_pos = Eigen::VectorXd::Ones(7) * 10; std::vector end; end.push_back(vars.back()); auto end_constraint = std::make_shared(end_pos, end, coeffs, "EndPosition"); nlp_ipopt.AddConstraintSet(end_constraint); // 4) Add costs - Eigen::VectorXd vel_target = Eigen::VectorXd::Zero(7); + const Eigen::VectorXd vel_target = Eigen::VectorXd::Zero(7); coeffs = Eigen::VectorXd::Constant(1, 1); auto vel_constraint = std::make_shared(vel_target, vars, coeffs, "jv"); // Must link the variables to the constraint since that happens in AddConstraintSet vel_constraint->LinkWithVariables(nlp_ipopt.GetOptVariables()); - Eigen::VectorXd weights = Eigen::VectorXd::Constant(vel_constraint->GetRows(), 0.01); + const Eigen::VectorXd weights = Eigen::VectorXd::Constant(vel_constraint->GetRows(), 0.01); auto vel_cost = std::make_shared(vel_constraint, weights); nlp_ipopt.AddCostSet(vel_cost); if (DEBUG) { nlp_ipopt.PrintCurrent(); - std::cout << "Constraint Jacobian: \n" << nlp_ipopt.GetJacobianOfConstraints().toDense() << std::endl; - std::cout << "Cost Jacobian: \n" << nlp_ipopt.GetJacobianOfCosts() << std::endl; + std::cout << "Constraint Jacobian: \n" << nlp_ipopt.GetJacobianOfConstraints().toDense() << '\n'; + std::cout << "Cost Jacobian: \n" << nlp_ipopt.GetJacobianOfCosts() << '\n'; } ifopt::IpoptSolver solver; @@ -212,7 +212,7 @@ TEST_F(VelocityConstraintOptimization, velocity_constraint_optimization_ipopt) EXPECT_NEAR(x[i], 10.0, 1e-5); if (DEBUG) { - std::cout << x.transpose() << std::endl; + std::cout << x.transpose() << '\n'; nlp_ipopt.PrintCurrent(); } } diff --git a/trajopt_optimizers/trajopt_sqp/test/numerical_ik_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/numerical_ik_unit.cpp index 39411fb7..3773d644 100644 --- a/trajopt_optimizers/trajopt_sqp/test/numerical_ik_unit.cpp +++ b/trajopt_optimizers/trajopt_sqp/test/numerical_ik_unit.cpp @@ -66,10 +66,10 @@ class NumericalIKTest : public testing::TestWithParam void SetUp() override { - tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/arm_around_table.urdf"); - tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/pr2.srdf"); + const tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/arm_around_table.urdf"); + const tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/pr2.srdf"); - ResourceLocator::Ptr locator = std::make_shared(); + const ResourceLocator::Ptr locator = std::make_shared(); EXPECT_TRUE(env->init(urdf_file, srdf_file, locator)); // Create plotting tool @@ -83,9 +83,9 @@ class NumericalIKTest : public testing::TestWithParam void runNumericalIKTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const Environment::Ptr& env) { - tesseract_scene_graph::StateSolver::Ptr state_solver = env->getStateSolver(); - ContinuousContactManager::Ptr manager = env->getContinuousContactManager(); - tesseract_kinematics::JointGroup::ConstPtr manip = env->getJointGroup("left_arm"); + const tesseract_scene_graph::StateSolver::Ptr state_solver = env->getStateSolver(); + const ContinuousContactManager::Ptr manager = env->getContinuousContactManager(); + const tesseract_kinematics::JointGroup::ConstPtr manip = env->getJointGroup("left_arm"); manager->setActiveCollisionObjects(manip->getActiveLinkNames()); manager->setDefaultCollisionMarginData(0); @@ -102,7 +102,8 @@ void runNumericalIKTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const Env target_pose.linear() = Eigen::Quaterniond(0, 0, 1, 0).toRotationMatrix(); target_pose.translation() = Eigen::Vector3d(0.4, 0, 0.8); - CartPosInfo cart_info(manip, "l_gripper_tool_frame", "base_footprint", Eigen::Isometry3d::Identity(), target_pose); + const CartPosInfo cart_info( + manip, "l_gripper_tool_frame", "base_footprint", Eigen::Isometry3d::Identity(), target_pose); auto cnt = std::make_shared(cart_info, var); qp_problem->addConstraintSet(cnt); @@ -139,7 +140,7 @@ void runNumericalIKTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const Env stopwatch.stop(); CONSOLE_BRIDGE_logError("Test took %f seconds.", stopwatch.elapsedSeconds()); - Eigen::VectorXd x = qp_problem->getVariableValues(); + const Eigen::VectorXd x = qp_problem->getVariableValues(); EXPECT_TRUE(solver.getStatus() == trajopt_sqp::SQPStatus::NLP_CONVERGED); diff --git a/trajopt_optimizers/trajopt_sqp/test/planning_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/planning_unit.cpp index 5ea90128..def43d85 100644 --- a/trajopt_optimizers/trajopt_sqp/test/planning_unit.cpp +++ b/trajopt_optimizers/trajopt_sqp/test/planning_unit.cpp @@ -27,7 +27,6 @@ #include TRAJOPT_IGNORE_WARNINGS_PUSH #include -#include #include #include #include @@ -69,10 +68,10 @@ class PlanningTest : public testing::TestWithParam void SetUp() override { - tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/arm_around_table.urdf"); - tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/pr2.srdf"); + const tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/arm_around_table.urdf"); + const tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/pr2.srdf"); - ResourceLocator::Ptr locator = std::make_shared(); + const ResourceLocator::Ptr locator = std::make_shared(); EXPECT_TRUE(env->init(urdf_file, srdf_file, locator)); // Create plotting tool @@ -98,9 +97,9 @@ void runPlanningTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const Enviro env->setState(ipos); std::vector collisions; - tesseract_scene_graph::StateSolver::Ptr state_solver = env->getStateSolver(); - ContinuousContactManager::Ptr manager = env->getContinuousContactManager(); - tesseract_kinematics::JointGroup::ConstPtr manip = env->getJointGroup("right_arm"); + const tesseract_scene_graph::StateSolver::Ptr state_solver = env->getStateSolver(); + const ContinuousContactManager::Ptr manager = env->getContinuousContactManager(); + const tesseract_kinematics::JointGroup::ConstPtr manip = env->getJointGroup("right_arm"); manager->setActiveCollisionObjects(manip->getActiveLinkNames()); manager->setDefaultCollisionMarginData(0); @@ -124,30 +123,30 @@ void runPlanningTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const Enviro qp_problem->addVariableSet(var); } - double margin_coeff = 20; - double margin = 0.025; + const double margin_coeff = 20; + const double margin = 0.025; auto trajopt_collision_config = std::make_shared(margin, margin_coeff); trajopt_collision_config->type = tesseract_collision::CollisionEvaluatorType::CONTINUOUS; trajopt_collision_config->collision_margin_buffer = 0.02; // Add costs { - Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(1, 1); + const Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(1, 1); auto cnt = std::make_shared(Eigen::VectorXd::Zero(7), vars, coeffs); qp_problem->addCostSet(cnt, trajopt_sqp::CostPenaltyType::SQUARED); } // Add constraints { // Fix start position - std::vector fixed_vars = { vars[0] }; - Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(manip->numJoints(), 5); + const std::vector fixed_vars = { vars[0] }; + const Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(manip->numJoints(), 5); auto cnt = std::make_shared(trajectory.row(0), fixed_vars, coeffs); qp_problem->addConstraintSet(cnt); } { // Fix end position - std::vector fixed_vars = { vars[5] }; - Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(manip->numJoints(), 5); + const std::vector fixed_vars = { vars[5] }; + const Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(manip->numJoints(), 5); auto cnt = std::make_shared(trajectory.row(5), fixed_vars, coeffs); qp_problem->addConstraintSet(cnt); } @@ -159,7 +158,7 @@ void runPlanningTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const Enviro auto collision_evaluator = std::make_shared( collision_cache, manip, env, trajopt_collision_config); - std::array position_vars{ vars[i - 1], vars[i] }; + const std::array position_vars{ vars[i - 1], vars[i] }; if (i == 1) position_vars_fixed = { true, false }; @@ -192,11 +191,11 @@ void runPlanningTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const Enviro solver.verbose = true; solver.solve(qp_problem); Eigen::VectorXd x = qp_problem->getVariableValues(); - std::cout << x.transpose() << std::endl; + std::cout << x.transpose() << '\n'; EXPECT_TRUE(solver.getStatus() == trajopt_sqp::SQPStatus::NLP_CONVERGED); - Eigen::Map results(x.data(), 6, 7); + const Eigen::Map results(x.data(), 6, 7); tesseract_collision::CollisionCheckConfig config; config.type = tesseract_collision::CollisionEvaluatorType::CONTINUOUS; diff --git a/trajopt_optimizers/trajopt_sqp/test/simple_collision_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/simple_collision_unit.cpp index 44b31eac..a3055868 100644 --- a/trajopt_optimizers/trajopt_sqp/test/simple_collision_unit.cpp +++ b/trajopt_optimizers/trajopt_sqp/test/simple_collision_unit.cpp @@ -85,7 +85,7 @@ class SimpleCollisionConstraintIfopt : public ifopt::ConstraintSet Eigen::VectorXd GetValues() const final { // Get current joint values - Eigen::VectorXd joint_vals = this->GetVariables()->GetComponent(position_var_->GetName())->GetValues(); + const Eigen::VectorXd joint_vals = this->GetVariables()->GetComponent(position_var_->GetName())->GetValues(); return CalcValues(joint_vals); } @@ -100,7 +100,7 @@ class SimpleCollisionConstraintIfopt : public ifopt::ConstraintSet return; // Get current joint values - VectorXd joint_vals = this->GetVariables()->GetComponent(position_var_->GetName())->GetValues(); + const VectorXd joint_vals = this->GetVariables()->GetComponent(position_var_->GetName())->GetValues(); CalcJacobianBlock(joint_vals, jac_block); // NOLINT } @@ -110,7 +110,7 @@ class SimpleCollisionConstraintIfopt : public ifopt::ConstraintSet Eigen::VectorXd err = Eigen::VectorXd::Zero(3); // Check the collisions - trajopt_common::CollisionCacheData::ConstPtr cdata = + const trajopt_common::CollisionCacheData::ConstPtr cdata = collision_evaluator_->CalcCollisions(joint_vals, bounds_.size()); if (cdata->contact_results_map.empty()) @@ -121,10 +121,10 @@ class SimpleCollisionConstraintIfopt : public ifopt::ConstraintSet { for (const auto& dist_result : pair.second) { - double dist = + const double dist = collision_evaluator_->GetCollisionConfig().contact_manager_config.margin_data.getPairCollisionMargin( dist_result.link_names[0], dist_result.link_names[1]); - double coeff = collision_evaluator_->GetCollisionConfig().collision_coeff_data.getPairCollisionCoeff( + const double coeff = collision_evaluator_->GetCollisionConfig().collision_coeff_data.getPairCollisionCoeff( dist_result.link_names[0], dist_result.link_names[1]); err[i++] += std::max(((dist - dist_result.distance) * coeff), 0.); } @@ -145,7 +145,7 @@ class SimpleCollisionConstraintIfopt : public ifopt::ConstraintSet jac_block.reserve(n_dof_ * 3); // Calculate collisions - trajopt_common::CollisionCacheData::ConstPtr cdata = + const trajopt_common::CollisionCacheData::ConstPtr cdata = collision_evaluator_->CalcCollisions(joint_vals, bounds_.size()); // Get gradients for all contacts @@ -155,7 +155,7 @@ class SimpleCollisionConstraintIfopt : public ifopt::ConstraintSet { for (const auto& dist_result : pair.second) { - trajopt_common::GradientResults result = collision_evaluator_->GetGradient(joint_vals, dist_result); + const trajopt_common::GradientResults result = collision_evaluator_->GetGradient(joint_vals, dist_result); grad_results.push_back(result); } } @@ -208,9 +208,9 @@ class SimpleCollisionTest : public testing::TestWithParam void SetUp() override { - boost::filesystem::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/spherebot.urdf"); - boost::filesystem::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/spherebot.srdf"); - ResourceLocator::Ptr locator = std::make_shared(); + const boost::filesystem::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/spherebot.urdf"); + const boost::filesystem::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/spherebot.srdf"); + const ResourceLocator::Ptr locator = std::make_shared(); EXPECT_TRUE(env->init(urdf_file, srdf_file, locator)); } }; @@ -225,9 +225,9 @@ void runSimpleCollisionTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const // plotter_->plotScene(); std::vector collisions; - tesseract_scene_graph::StateSolver::Ptr state_solver = env->getStateSolver(); - DiscreteContactManager::Ptr manager = env->getDiscreteContactManager(); - tesseract_kinematics::JointGroup::ConstPtr manip = env->getJointGroup("manipulator"); + const tesseract_scene_graph::StateSolver::Ptr state_solver = env->getStateSolver(); + const DiscreteContactManager::Ptr manager = env->getDiscreteContactManager(); + const tesseract_kinematics::JointGroup::ConstPtr manip = env->getJointGroup("manipulator"); manager->setActiveCollisionObjects(manip->getActiveLinkNames()); manager->setDefaultCollisionMarginData(0); @@ -251,7 +251,7 @@ void runSimpleCollisionTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const trajopt_collision_cnt_config->collision_margin_buffer = 0.05; auto collision_cnt_cache = std::make_shared(100); - trajopt_ifopt::DiscreteCollisionEvaluator::Ptr collision_cnt_evaluator = + const trajopt_ifopt::DiscreteCollisionEvaluator::Ptr collision_cnt_evaluator = std::make_shared( collision_cnt_cache, manip, env, trajopt_collision_cnt_config); auto collision_cnt = std::make_shared(collision_cnt_evaluator, vars[0]); @@ -261,13 +261,13 @@ void runSimpleCollisionTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const trajopt_collision_cost_config->collision_margin_buffer = 0.05; auto collision_cost_cache = std::make_shared(100); - trajopt_ifopt::DiscreteCollisionEvaluator::Ptr collision_cost_evaluator = + const trajopt_ifopt::DiscreteCollisionEvaluator::Ptr collision_cost_evaluator = std::make_shared( collision_cost_cache, manip, env, trajopt_collision_cost_config); auto collision_cost = std::make_shared(collision_cost_evaluator, vars[0]); qp_problem->addCostSet(collision_cost, trajopt_sqp::CostPenaltyType::HINGE); - Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(2, 1); + const Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(2, 1); auto jp_cost = std::make_shared(Eigen::Vector2d(0, 0), vars, coeffs); qp_problem->addCostSet(jp_cost, trajopt_sqp::CostPenaltyType::SQUARED); @@ -275,8 +275,9 @@ void runSimpleCollisionTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const qp_problem->print(); auto error_calculator = [&](const Eigen::Ref& x) { return collision_cnt->CalcValues(x); }; - trajopt_ifopt::SparseMatrix num_jac_block = trajopt_ifopt::calcForwardNumJac(error_calculator, positions[0], 1e-4); - std::cout << "Numerical Jacobian: \n" << num_jac_block << std::endl; + const trajopt_ifopt::SparseMatrix 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 auto qp_solver = std::make_shared(); @@ -300,11 +301,11 @@ void runSimpleCollisionTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const Eigen::VectorXd x = qp_problem->getVariableValues(); - std::cout << x.transpose() << std::endl; + std::cout << x.transpose() << '\n'; tesseract_common::TrajArray inputs(1, 2); inputs << -0.75, 0.75; - Eigen::Map results(x.data(), 1, 2); + const Eigen::Map results(x.data(), 1, 2); bool found = checkTrajectory( collisions, *manager, *state_solver, manip->getJointNames(), inputs, *trajopt_collision_cnt_config); diff --git a/trajopt_sco/include/trajopt_sco/bpmpd_io.hpp b/trajopt_sco/include/trajopt_sco/bpmpd_io.hpp index 271f4070..7ab80ec5 100644 --- a/trajopt_sco/include/trajopt_sco/bpmpd_io.hpp +++ b/trajopt_sco/include/trajopt_sco/bpmpd_io.hpp @@ -3,13 +3,14 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include #include +#include #include #include TRAJOPT_IGNORE_WARNINGS_POP namespace bpmpd_io { -enum SerMode +enum SerMode : std::uint8_t { DESER, SER @@ -23,14 +24,14 @@ void ser(int fp, T& x, SerMode mode) case SER: { T xcopy = x; - ssize_t n = write(fp, &xcopy, sizeof(T)); + const ssize_t n = write(fp, &xcopy, sizeof(T)); assert(n == sizeof(T)); UNUSED(n); break; } case DESER: { - ssize_t n = read(fp, &x, sizeof(T)); + const ssize_t n = read(fp, &x, sizeof(T)); assert(n == sizeof(T)); UNUSED(n); break; @@ -47,7 +48,7 @@ void ser(int fp, std::vector& x, SerMode mode) { case SER: { - long n = write(fp, x.data(), sizeof(T) * size); + const long n = write(fp, x.data(), sizeof(T) * size); assert(static_cast(n) == sizeof(T) * size); UNUSED(n); break; @@ -55,7 +56,7 @@ void ser(int fp, std::vector& x, SerMode mode) case DESER: { x.resize(size); - long n = read(fp, x.data(), sizeof(T) * size); + const long n = read(fp, x.data(), sizeof(T) * size); assert(static_cast(n) == sizeof(T) * size); UNUSED(n); break; diff --git a/trajopt_sco/include/trajopt_sco/modeling.hpp b/trajopt_sco/include/trajopt_sco/modeling.hpp index b67eed8d..9a3526af 100644 --- a/trajopt_sco/include/trajopt_sco/modeling.hpp +++ b/trajopt_sco/include/trajopt_sco/modeling.hpp @@ -272,7 +272,7 @@ inline void setVec(DblVec& x, const VarVector& vars, const VecType& vals) assert(vars.size() == vals.size()); for (std::size_t i = 0; i < vars.size(); ++i) { - x[static_cast(vars[i].var_rep->index)] = vals[i]; + x[vars[i].var_rep->index] = vals[i]; } } template diff --git a/trajopt_sco/include/trajopt_sco/sco_common.hpp b/trajopt_sco/include/trajopt_sco/sco_common.hpp index 9bfb574a..5bc9e971 100644 --- a/trajopt_sco/include/trajopt_sco/sco_common.hpp +++ b/trajopt_sco/include/trajopt_sco/sco_common.hpp @@ -26,7 +26,7 @@ using CntVector = std::vector; inline double vecSum(const DblVec& v) { double out = 0; - for (double i : v) + for (const double i : v) out += i; return out; } @@ -35,7 +35,7 @@ inline double vecSum(const DblVec& v) inline double vecAbsSum(const DblVec& v) { double out = 0; - for (double i : v) + for (const double i : v) out += fabs(i); return out; } @@ -50,7 +50,7 @@ inline double sq(double x) { return x * x; } inline double vecHingeSum(const DblVec& v) { double out = 0; - for (double i : v) + for (const double i : v) out += pospart(i); return out; } diff --git a/trajopt_sco/include/trajopt_sco/solver_interface.hpp b/trajopt_sco/include/trajopt_sco/solver_interface.hpp index d594536e..3c2b2443 100644 --- a/trajopt_sco/include/trajopt_sco/solver_interface.hpp +++ b/trajopt_sco/include/trajopt_sco/solver_interface.hpp @@ -141,7 +141,7 @@ struct Var double value(const DblVec& x) const { assert(var_rep->index < x.size()); - return x[static_cast(var_rep->index)]; + return x[var_rep->index]; } }; diff --git a/trajopt_sco/include/trajopt_sco/solver_utils.hpp b/trajopt_sco/include/trajopt_sco/solver_utils.hpp index e25ae2e4..bbd5be85 100644 --- a/trajopt_sco/include/trajopt_sco/solver_utils.hpp +++ b/trajopt_sco/include/trajopt_sco/solver_utils.hpp @@ -119,7 +119,8 @@ void eigenToCSC(Eigen::SparseMatrix& sparse_matrix, if (sizeof(T) != sizeof(int)) { - IntVec row_indices_int, column_pointers_int; + IntVec row_indices_int; + IntVec column_pointers_int; auto* si_p = sm_ref.get().innerIndexPtr(); row_indices_int.assign(si_p, si_p + sm_ref.get().nonZeros()); row_indices.clear(); diff --git a/trajopt_sco/src/bpmpd_caller.cpp b/trajopt_sco/src/bpmpd_caller.cpp index 89547f3a..0110c0ae 100644 --- a/trajopt_sco/src/bpmpd_caller.cpp +++ b/trajopt_sco/src/bpmpd_caller.cpp @@ -35,8 +35,8 @@ extern void bpmpd(int*, int main(int /*argc*/, char** /*argv*/) { - std::string working_dir = BPMPD_WORKING_DIR; - int err = chdir(working_dir.c_str()); + std::string const working_dir = BPMPD_WORKING_DIR; + int const err = chdir(working_dir.c_str()); if (err != 0) { std::cerr << "error going to BPMPD working dir\n"; diff --git a/trajopt_sco/src/bpmpd_interface.cpp b/trajopt_sco/src/bpmpd_interface.cpp index 1629de88..b6e522a7 100644 --- a/trajopt_sco/src/bpmpd_interface.cpp +++ b/trajopt_sco/src/bpmpd_interface.cpp @@ -164,7 +164,8 @@ Model::Ptr createBPMPDModel() pid_t popen2(const char* command, int* infp, int* outfp) { - std::array p_stdin{}, p_stdout{}; + std::array p_stdin{}; + std::array p_stdout{}; pid_t pid{ 0 }; if (pipe(p_stdin.data()) != 0 || pipe(p_stdout.data()) != 0) @@ -210,7 +211,7 @@ static int gPipeOut = 0; // NOLINT void fexit() { std::array text{ bpmpd_io::EXIT_CHAR }; - long n = write(gPipeIn, text.data(), 1); + long const n = write(gPipeIn, text.data(), 1); ALWAYS_ASSERT(n == 1); } @@ -236,7 +237,7 @@ BPMPDModel::BPMPDModel() Var BPMPDModel::addVar(const std::string& name) { - std::scoped_lock lock(m_mutex); + const std::scoped_lock lock(m_mutex); m_vars.emplace_back(std::make_shared(m_vars.size(), name, this)); m_lbs.push_back(-BPMPD_BIG); m_ubs.push_back(BPMPD_BIG); @@ -244,7 +245,7 @@ Var BPMPDModel::addVar(const std::string& name) } Cnt BPMPDModel::addEqCnt(const AffExpr& expr, const std::string& /*name*/) { - std::scoped_lock lock(m_mutex); + const std::scoped_lock lock(m_mutex); m_cnts.emplace_back(std::make_shared(m_cnts.size(), this)); m_cntExprs.push_back(expr); m_cntTypes.push_back(EQ); @@ -252,7 +253,7 @@ Cnt BPMPDModel::addEqCnt(const AffExpr& expr, const std::string& /*name*/) } Cnt BPMPDModel::addIneqCnt(const AffExpr& expr, const std::string& /*name*/) { - std::scoped_lock lock(m_mutex); + const std::scoped_lock lock(m_mutex); m_cnts.emplace_back(std::make_shared(m_cnts.size(), this)); m_cntExprs.push_back(expr); m_cntTypes.push_back(INEQ); @@ -265,7 +266,7 @@ Cnt BPMPDModel::addIneqCnt(const QuadExpr&, const std::string& /*name*/) void BPMPDModel::removeVars(const VarVector& vars) { - std::scoped_lock lock(m_mutex); + const std::scoped_lock lock(m_mutex); SizeTVec inds; vars2inds(vars, inds); for (const auto& var : vars) @@ -274,7 +275,7 @@ void BPMPDModel::removeVars(const VarVector& vars) void BPMPDModel::removeCnts(const CntVector& cnts) { - std::scoped_lock lock(m_mutex); + const std::scoped_lock lock(m_mutex); SizeTVec inds; cnts2inds(cnts, inds); for (auto& cnt : cnts) // NOLINT @@ -349,7 +350,7 @@ DblVec BPMPDModel::getVarValues(const VarVector& vars) const return out; } -#define DBG(expr) // cout << #expr << ": " << CSTR(expr) << std::endl +#define DBG(expr) // cout << #expr << ": " << CSTR(expr) << '\n' CvxOptStatus BPMPDModel::optimize() { @@ -363,11 +364,22 @@ CvxOptStatus BPMPDModel::optimize() // lbound[maxn+maxm], // ubound[maxn+maxm], primal[maxn+maxm], dual[maxn+maxm], big, opt; - std::size_t n = m_vars.size(); - std::size_t m = m_cnts.size(); - - IntVec acolcnt(n), acolidx, qcolcnt(n), qcolidx, status(m + n); - DblVec acolnzs, qcolnzs, rhs(m), obj(n, 0), lbound(m + n), ubound(m + n), primal(m + n), dual(m + n); + const std::size_t n = m_vars.size(); + const std::size_t m = m_cnts.size(); + + IntVec acolcnt(n); + IntVec acolidx; + IntVec qcolcnt(n); + IntVec qcolidx; + const IntVec status(m + n); + DblVec acolnzs; + DblVec qcolnzs; + DblVec rhs(m); + DblVec obj(n, 0); + DblVec lbound(m + n); + DblVec ubound(m + n); + const DblVec primal(m + n); + const DblVec dual(m + n); DBG(m_lbs); DBG(m_ubs); @@ -508,13 +520,13 @@ CvxOptStatus BPMPDModel::optimize() ubound); bpmpd_io::ser(gPipeIn, bi, bpmpd_io::SER); - // std::cout << "serialization time:" << end-start << std::endl; + // std::cout << "serialization time:" << end-start << '\n'; bpmpd_io::bpmpd_output bo; bpmpd_io::ser(gPipeOut, bo, bpmpd_io::DESER); m_soln = DblVec(bo.primal.begin(), bo.primal.begin() + static_cast(n)); - int retcode = bo.code; + const int retcode = bo.code; if (retcode == 2) return CVX_SOLVED; diff --git a/trajopt_sco/src/expr_ops.cpp b/trajopt_sco/src/expr_ops.cpp index d4082368..384db5f6 100644 --- a/trajopt_sco/src/expr_ops.cpp +++ b/trajopt_sco/src/expr_ops.cpp @@ -10,9 +10,9 @@ namespace sco QuadExpr exprMult(const AffExpr& affexpr1, const AffExpr& affexpr2) { QuadExpr out; - std::size_t naff1 = affexpr1.coeffs.size(); - std::size_t naff2 = affexpr2.coeffs.size(); - std::size_t nquad = naff1 * naff2; + const std::size_t naff1 = affexpr1.coeffs.size(); + const std::size_t naff2 = affexpr2.coeffs.size(); + const std::size_t nquad = naff1 * naff2; // Multiply the constants of the two expr out.affexpr.constant = affexpr1.constant * affexpr2.constant; @@ -55,8 +55,8 @@ QuadExpr exprSquare(const Var& a) QuadExpr exprSquare(const AffExpr& affexpr) { QuadExpr out; - std::size_t naff = affexpr.coeffs.size(); - std::size_t nquad = (naff * (naff + 1)) / 2; + const std::size_t naff = affexpr.coeffs.size(); + const std::size_t nquad = (naff * (naff + 1)) / 2; out.affexpr.constant = sq(affexpr.constant); diff --git a/trajopt_sco/src/gurobi_interface.cpp b/trajopt_sco/src/gurobi_interface.cpp index 5a83186a..580a728b 100644 --- a/trajopt_sco/src/gurobi_interface.cpp +++ b/trajopt_sco/src/gurobi_interface.cpp @@ -78,7 +78,7 @@ GurobiModel::GurobiModel() Var GurobiModel::addVar(const std::string& name) { - std::scoped_lock lock(m_mutex); + const std::scoped_lock lock(m_mutex); ENSURE_SUCCESS(GRBaddvar( m_model, 0, nullptr, nullptr, 0, -GRB_INFINITY, GRB_INFINITY, GRB_CONTINUOUS, const_cast(name.c_str()))); m_vars.push_back(std::make_shared(m_vars.size(), name, this)); @@ -87,7 +87,7 @@ Var GurobiModel::addVar(const std::string& name) Var GurobiModel::addVar(const std::string& name, double lb, double ub) { - std::scoped_lock lock(m_mutex); + const std::scoped_lock lock(m_mutex); ENSURE_SUCCESS(GRBaddvar(m_model, 0, nullptr, nullptr, 0, lb, ub, GRB_CONTINUOUS, const_cast(name.c_str()))); m_vars.push_back(std::make_shared(m_vars.size(), name, this)); return m_vars.back(); @@ -95,7 +95,7 @@ Var GurobiModel::addVar(const std::string& name, double lb, double ub) Cnt GurobiModel::addEqCnt(const AffExpr& expr, const std::string& name) { - std::scoped_lock lock(m_mutex); + const std::scoped_lock lock(m_mutex); LOG_TRACE("adding eq constraint: %s = 0", CSTR(expr)); IntVec inds; vars2inds(expr.vars, inds); @@ -113,7 +113,7 @@ Cnt GurobiModel::addEqCnt(const AffExpr& expr, const std::string& name) } Cnt GurobiModel::addIneqCnt(const AffExpr& expr, const std::string& name) { - std::scoped_lock lock(m_mutex); + const std::scoped_lock lock(m_mutex); LOG_TRACE("adding ineq: %s <= 0", CSTR(expr)); IntVec inds; vars2inds(expr.vars, inds); @@ -131,7 +131,7 @@ Cnt GurobiModel::addIneqCnt(const AffExpr& expr, const std::string& name) } Cnt GurobiModel::addIneqCnt(const QuadExpr& qexpr, const std::string& name) { - std::scoped_lock lock(m_mutex); + const std::scoped_lock lock(m_mutex); int numlnz = static_cast(qexpr.affexpr.size()); IntVec linds; vars2inds(qexpr.affexpr.vars, linds); @@ -167,7 +167,7 @@ void resetIndices(CntVector& cnts) void GurobiModel::removeVars(const VarVector& vars) { - std::scoped_lock lock(m_mutex); + const std::scoped_lock lock(m_mutex); IntVec inds; vars2inds(vars, inds); ENSURE_SUCCESS(GRBdelvars(m_model, static_cast(inds.size()), inds.data())); @@ -177,7 +177,7 @@ void GurobiModel::removeVars(const VarVector& vars) void GurobiModel::removeCnts(const CntVector& cnts) { - std::scoped_lock lock(m_mutex); + const std::scoped_lock lock(m_mutex); IntVec inds; cnts2inds(cnts, inds); ENSURE_SUCCESS(GRBdelconstrs(m_model, static_cast(inds.size()), inds.data())); diff --git a/trajopt_sco/src/modeling.cpp b/trajopt_sco/src/modeling.cpp index f0e23a8c..5185c9fb 100644 --- a/trajopt_sco/src/modeling.cpp +++ b/trajopt_sco/src/modeling.cpp @@ -17,19 +17,19 @@ void ConvexObjective::addAffExpr(const AffExpr& affexpr) { exprInc(quad_, affexp void ConvexObjective::addQuadExpr(const QuadExpr& quadexpr) { exprInc(quad_, quadexpr); } void ConvexObjective::addHinge(const AffExpr& affexpr, double coeff) { - Var hinge = model_->addVar("hinge", 0, static_cast(INFINITY)); + Var const hinge = model_->addVar("hinge", 0, static_cast(INFINITY)); vars_.push_back(hinge); ineqs_.push_back(affexpr); exprDec(ineqs_.back(), hinge); - AffExpr hinge_cost = exprMult(AffExpr(hinge), coeff); + AffExpr const hinge_cost = exprMult(AffExpr(hinge), coeff); exprInc(quad_, hinge_cost); } void ConvexObjective::addAbs(const AffExpr& affexpr, double coeff) { // Add variables that will enforce ABS - Var neg = model_->addVar("neg", 0, static_cast(INFINITY)); - Var pos = model_->addVar("pos", 0, static_cast(INFINITY)); + Var const neg = model_->addVar("neg", 0, static_cast(INFINITY)); + Var const pos = model_->addVar("pos", 0, static_cast(INFINITY)); vars_.push_back(neg); vars_.push_back(pos); // Coeff will be applied whenever neg/pos are not 0 @@ -74,7 +74,7 @@ void ConvexObjective::addL2Norm(const AffExprVector& ev) void ConvexObjective::addMax(const AffExprVector& ev) { - Var m = model_->addVar("max", static_cast(-INFINITY), static_cast(INFINITY)); + Var const m = model_->addVar("max", static_cast(-INFINITY), static_cast(INFINITY)); ineqs_.reserve(ineqs_.size() + ev.size()); for (const auto& i : ev) { @@ -179,7 +179,8 @@ VarVector OptProb::createVariables(const std::vector& names) VarVector OptProb::createVariables(const std::vector& names, const DblVec& lb, const DblVec& ub) { - std::size_t n_add = names.size(), n_cur = vars_.size(); + const std::size_t n_add = names.size(); + const std::size_t n_cur = vars_.size(); assert(lb.size() == n_add); assert(ub.size() == n_add); vars_.reserve(n_cur + n_add); @@ -280,7 +281,7 @@ DblVec OptProb::getClosestFeasiblePointQP(const DblVec& x) } model_->setVarBounds(vars_, lower_bounds_, upper_bounds_); model_->setObjective(obj); - CvxOptStatus status = model_->optimize(); + const CvxOptStatus status = model_->optimize(); if (status != CVX_SOLVED) { model_->writeToFile("/tmp/fail.lp"); diff --git a/trajopt_sco/src/modeling_utils.cpp b/trajopt_sco/src/modeling_utils.cpp index 71efc11c..cc2e5dfd 100644 --- a/trajopt_sco/src/modeling_utils.cpp +++ b/trajopt_sco/src/modeling_utils.cpp @@ -45,19 +45,20 @@ CostFromFunc::CostFromFunc(ScalarOfVector::Ptr f, VarVector vars, const std::str double CostFromFunc::value(const DblVec& x) { - Eigen::VectorXd x_eigen = getVec(x, vars_); + const Eigen::VectorXd x_eigen = getVec(x, vars_); return f_->call(x_eigen); } ConvexObjective::Ptr CostFromFunc::convex(const DblVec& x, Model* model) { - Eigen::VectorXd x_eigen = getVec(x, vars_); + const Eigen::VectorXd x_eigen = getVec(x, vars_); auto out = std::make_shared(model); if (!full_hessian_) { double val{ NAN }; - Eigen::VectorXd grad, hess; + Eigen::VectorXd grad; + Eigen::VectorXd hess; calcGradAndDiagHess(*f_, x_eigen, epsilon_, val, grad, hess); hess = hess.cwiseMax(Eigen::VectorXd::Zero(hess.size())); QuadExpr& quad = out->quad_; @@ -76,7 +77,7 @@ ConvexObjective::Ptr CostFromFunc::convex(const DblVec& x, Model* model) calcGradHess(f_, x_eigen, epsilon_, val, grad, hess); Eigen::MatrixXd pos_hess = Eigen::MatrixXd::Zero(x_eigen.size(), x_eigen.size()); - Eigen::SelfAdjointEigenSolver es(hess); + const Eigen::SelfAdjointEigenSolver es(hess); Eigen::VectorXd eigvals = es.eigenvalues(); Eigen::MatrixXd eigvecs = es.eigenvectors(); for (long int i = 0, end = x_eigen.size(); i != end; ++i) @@ -141,7 +142,7 @@ CostFromErrFunc::CostFromErrFunc(VectorOfVector::Ptr f, } double CostFromErrFunc::value(const DblVec& x) { - Eigen::VectorXd x_eigen = getVec(x, vars_); + const Eigen::VectorXd x_eigen = getVec(x, vars_); Eigen::VectorXd err = f_->call(x_eigen); switch (pen_type_) @@ -166,7 +167,7 @@ double CostFromErrFunc::value(const DblVec& x) } ConvexObjective::Ptr CostFromErrFunc::convex(const DblVec& x, Model* model) { - Eigen::VectorXd x_eigen = getVec(x, vars_); + const Eigen::VectorXd x_eigen = getVec(x, vars_); Eigen::MatrixXd jac = (dfdx_) ? dfdx_->call(x_eigen) : calcForwardNumJac(*f_, x_eigen, epsilon_); auto out = std::make_shared(model); Eigen::VectorXd y = f_->call(x_eigen); @@ -236,7 +237,7 @@ ConstraintFromErrFunc::ConstraintFromErrFunc(VectorOfVector::Ptr f, DblVec ConstraintFromErrFunc::value(const DblVec& x) { - Eigen::VectorXd x_eigen = getVec(x, vars_); + const Eigen::VectorXd x_eigen = getVec(x, vars_); Eigen::VectorXd err = f_->call(x_eigen); if (coeffs_.size() > 0) err.array() *= coeffs_.array(); @@ -245,7 +246,7 @@ DblVec ConstraintFromErrFunc::value(const DblVec& x) ConvexConstraints::Ptr ConstraintFromErrFunc::convex(const DblVec& x, Model* model) { - Eigen::VectorXd x_eigen = getVec(x, vars_); + const Eigen::VectorXd x_eigen = getVec(x, vars_); Eigen::MatrixXd jac = (dfdx_) ? dfdx_->call(x_eigen) : calcForwardNumJac(*f_, x_eigen, epsilon_); auto out = std::make_shared(model); Eigen::VectorXd y = f_->call(x_eigen); @@ -274,7 +275,7 @@ std::string AffExprToString(const AffExpr& aff) { if (i != 0) out.append(" + "); - std::string term = std::to_string(aff.coeffs[i]) + "*" + aff.vars[i].var_rep->name; + const std::string term = std::to_string(aff.coeffs[i]) + "*" + aff.vars[i].var_rep->name; out.append(term); } out.append(" + " + std::to_string(aff.constant)); diff --git a/trajopt_sco/src/num_diff.cpp b/trajopt_sco/src/num_diff.cpp index f5489409..3d1c2915 100644 --- a/trajopt_sco/src/num_diff.cpp +++ b/trajopt_sco/src/num_diff.cpp @@ -42,11 +42,11 @@ Eigen::VectorXd calcForwardNumGrad(const ScalarOfVector& f, const Eigen::VectorX { Eigen::VectorXd out(x.size()); Eigen::VectorXd xpert = x; - double y = f(x); + double const y = f(x); for (int i = 0; i < x.size(); ++i) { xpert(i) = x(i) + epsilon; - double ypert = f(xpert); + const double ypert = f(xpert); out(i) = (ypert - y) / epsilon; xpert(i) = x(i); } @@ -54,13 +54,13 @@ Eigen::VectorXd calcForwardNumGrad(const ScalarOfVector& f, const Eigen::VectorX } Eigen::MatrixXd calcForwardNumJac(const VectorOfVector& f, const Eigen::VectorXd& x, double epsilon) { - Eigen::VectorXd y = f(x); + const Eigen::VectorXd y = f(x); Eigen::MatrixXd out(y.size(), x.size()); Eigen::VectorXd xpert = x; for (int i = 0; i < x.size(); ++i) { xpert(i) = x(i) + epsilon; - Eigen::VectorXd ypert = f(xpert); + const Eigen::VectorXd ypert = f(xpert); out.col(i) = (ypert - y) / epsilon; xpert(i) = x(i); } @@ -81,9 +81,9 @@ void calcGradAndDiagHess(const ScalarOfVector& f, for (int i = 0; i < x.size(); ++i) { xpert(i) = x(i) + epsilon / 2; - double yplus = f(xpert); + const double yplus = f(xpert); xpert(i) = x(i) - epsilon / 2; - double yminus = f(xpert); + const double yminus = f(xpert); grad(i) = (yplus - yminus) / epsilon; hess(i) = (yplus + yminus - 2 * y) / (epsilon * epsilon / 4); xpert(i) = x(i); @@ -98,7 +98,7 @@ void calcGradHess(const ScalarOfVector::Ptr& f, Eigen::MatrixXd& hess) { y = f->call(x); - VectorOfVector::Ptr grad_func = forwardNumGrad(f, epsilon); + const VectorOfVector::Ptr grad_func = forwardNumGrad(f, epsilon); grad = grad_func->call(x); hess = calcForwardNumJac(*grad_func, x, epsilon); hess = (hess + hess.transpose()) / 2; diff --git a/trajopt_sco/src/optimizers.cpp b/trajopt_sco/src/optimizers.cpp index 31f67115..6b15d98d 100644 --- a/trajopt_sco/src/optimizers.cpp +++ b/trajopt_sco/src/optimizers.cpp @@ -29,6 +29,8 @@ std::ostream& operator<<(std::ostream& o, const OptResults& r) return o; } +namespace +{ // todo: use different coeffs for each constraint std::vector cntsToCosts(const std::vector& cnts, const std::vector& err_coeffs, @@ -53,6 +55,7 @@ std::vector cntsToCosts(const std::vectorgetVars(); assert(vars.size() == x.size()); - const DblVec &lb = prob_->getLowerBounds(), ub = prob_->getUpperBounds(); - DblVec lbtrust(x.size()), ubtrust(x.size()); + const DblVec& lb = prob_->getLowerBounds(); + const DblVec ub = prob_->getUpperBounds(); + DblVec lbtrust(x.size()); + DblVec ubtrust(x.size()); for (std::size_t i = 0; i < x.size(); ++i) { lbtrust[i] = fmax(x[i] - param_.trust_box_size, lb[i]); @@ -329,7 +334,7 @@ void BasicTrustRegionSQPResults::update(const OptResults& prev_opt_results, if (trajopt_common::GetLogLevel() >= trajopt_common::LevelDebug) { - DblVec cnt_costs1 = parent_.evaluateModelCosts(cnt_cost_models, model_var_vals); + const DblVec cnt_costs1 = parent_.evaluateModelCosts(cnt_cost_models, model_var_vals); DblVec cnt_costs2 = model_cnt_viols; for (unsigned i = 0; i < cnt_costs2.size(); ++i) cnt_costs2[i] *= merit_error_coeffs[i]; @@ -377,8 +382,8 @@ void BasicTrustRegionSQPResults::print() const std::printf("| %s | COSTS\n", std::string(88, '-').c_str()); for (std::size_t i = 0; i < old_cost_vals.size(); ++i) { - double approx_improve = old_cost_vals[i] - model_cost_vals[i]; - double exact_improve = old_cost_vals[i] - new_cost_vals[i]; + const double approx_improve = old_cost_vals[i] - model_cost_vals[i]; + const double exact_improve = old_cost_vals[i] - new_cost_vals[i]; if (fabs(approx_improve) > 1e-8) std::printf("| %10s | %10.3e | %10.3e | %10.3e | %10.3e | %10.3e | %10.3e | %-15s \n", "----------", @@ -416,8 +421,8 @@ void BasicTrustRegionSQPResults::print() const std::printf("| %s | CONSTRAINTS\n", std::string(88, '-').c_str()); for (std::size_t i = 0; i < old_cnt_viols.size(); ++i) { - double approx_improve = old_cnt_viols[i] - model_cnt_viols[i]; - double exact_improve = old_cnt_viols[i] - new_cnt_viols[i]; + const double approx_improve = old_cnt_viols[i] - model_cnt_viols[i]; + const double exact_improve = old_cnt_viols[i] - new_cnt_viols[i]; if (fabs(approx_improve) > 1e-8) std::printf("| %10.3e | %10.3e | %10.3e | %10.3e | %10.3e | %10.3e | %10.3e | %-15s \n", merit_error_coeffs[i], @@ -517,8 +522,8 @@ void BasicTrustRegionSQPResults::writeCosts(std::FILE* stream, bool header) cons std::fprintf(stream, "%s", "COSTS"); for (std::size_t i = 0; i < old_cost_vals.size(); ++i) { - double approx_improve = old_cost_vals[i] - model_cost_vals[i]; - double exact_improve = old_cost_vals[i] - new_cost_vals[i]; + const double approx_improve = old_cost_vals[i] - model_cost_vals[i]; + const double exact_improve = old_cost_vals[i] - new_cost_vals[i]; if (fabs(approx_improve) > 1e-8) { std::fprintf( @@ -553,8 +558,8 @@ void BasicTrustRegionSQPResults::writeConstraints(std::FILE* stream, bool header std::fprintf(stream, "%s", "CONSTRAINTS"); for (std::size_t i = 0; i < old_cnt_viols.size(); ++i) { - double approx_improve = old_cnt_viols[i] - model_cnt_viols[i]; - double exact_improve = old_cnt_viols[i] - new_cnt_viols[i]; + const double approx_improve = old_cnt_viols[i] - model_cnt_viols[i]; + const double exact_improve = old_cnt_viols[i] - new_cnt_viols[i]; if (fabs(approx_improve) > 1e-8) { std::fprintf(stream, @@ -630,9 +635,9 @@ void BasicTrustRegionSQPResults::printRaw() const OptStatus BasicTrustRegionSQP::optimize() { - std::vector var_names = getVarNames(prob_->getVars()); - std::vector cost_names = getCostNames(prob_->getCosts()); - std::vector constraints = prob_->getConstraints(); + const std::vector var_names = getVarNames(prob_->getVars()); + const std::vector cost_names = getCostNames(prob_->getCosts()); + const std::vector constraints = prob_->getConstraints(); std::vector cnt_names = getCntNames(constraints); std::vector merit_error_coeffs(constraints.size(), param_.initial_merit_error_coeff); BasicTrustRegionSQPResults iteration_results(var_names, cost_names, cnt_names, *this); @@ -668,7 +673,7 @@ OptStatus BasicTrustRegionSQP::optimize() { /* merit adjustment loop */ for (int iter = 1;; ++iter) { /* sqp loop */ - double elapsed_time = std::chrono::duration(Clock::now() - start_time).count() / 1000.0; + const double elapsed_time = std::chrono::duration(Clock::now() - start_time).count() / 1000.0; if (elapsed_time > param_.max_time) { LOG_INFO("Elapsed time %f has exceeded max time %f", elapsed_time, param_.max_time); @@ -710,19 +715,21 @@ OptStatus BasicTrustRegionSQP::optimize() // results_.cost_vals[i] << endl; // } - std::vector cost_models = convexifyCosts(prob_->getCosts(), results_.x, model_.get()); - std::vector cnt_models = convexifyConstraints(constraints, results_.x, model_.get()); - std::vector cnt_cost_models = cntsToCosts(cnt_models, merit_error_coeffs, model_.get()); + const std::vector cost_models = convexifyCosts(prob_->getCosts(), results_.x, model_.get()); + const std::vector cnt_models = + convexifyConstraints(constraints, results_.x, model_.get()); + const std::vector cnt_cost_models = + cntsToCosts(cnt_models, merit_error_coeffs, model_.get()); model_->update(); - for (ConvexObjective::Ptr& cost : cost_models) + for (const ConvexObjective::Ptr& cost : cost_models) cost->addConstraintsToModel(); - for (ConvexObjective::Ptr& cost : cnt_cost_models) + for (const ConvexObjective::Ptr& cost : cnt_cost_models) cost->addConstraintsToModel(); model_->update(); QuadExpr objective; - for (ConvexObjective::Ptr& co : cost_models) + for (const ConvexObjective::Ptr& co : cost_models) exprInc(objective, co->quad_); - for (ConvexObjective::Ptr& co : cnt_cost_models) + for (const ConvexObjective::Ptr& co : cnt_cost_models) exprInc(objective, co->quad_); // objective = cleanupExpr(objective); @@ -741,7 +748,7 @@ OptStatus BasicTrustRegionSQP::optimize() while (param_.trust_box_size >= param_.min_trust_box_size) { setTrustBoxConstraints(results_.x); - CvxOptStatus status = model_->optimize(); + const CvxOptStatus status = model_->optimize(); ++results_.n_qp_solves; if (status != CVX_SOLVED) @@ -908,6 +915,7 @@ OptStatus BasicTrustRegionSQP::optimize() LOG_INFO("\n==================\n%s==================", CSTR(results_)); callCallbacks(); + // NOLINTBEGIN(clang-analyzer-core.NonNullParamChecker) if (param_.log_results || trajopt_common::GetLogLevel() >= trajopt_common::LevelDebug) { std::fclose(log_solver_stream); @@ -915,6 +923,7 @@ OptStatus BasicTrustRegionSQP::optimize() std::fclose(log_costs_stream); std::fclose(log_constraints_stream); } + // NOLINTEND(clang-analyzer-core.NonNullParamChecker) return retval; } diff --git a/trajopt_sco/src/osqp_interface.cpp b/trajopt_sco/src/osqp_interface.cpp index 9701e254..0b3fed5e 100644 --- a/trajopt_sco/src/osqp_interface.cpp +++ b/trajopt_sco/src/osqp_interface.cpp @@ -51,9 +51,9 @@ OSQPModel::~OSQPModel() osqp_cleanup(osqp_workspace_); // Clean up memory - for (Var& var : vars_) + for (const Var& var : vars_) var.var_rep->removed = true; - for (Cnt& cnt : cnts_) + for (const Cnt& cnt : cnts_) cnt.cnt_rep->removed = true; OSQPModel::update(); @@ -61,7 +61,7 @@ OSQPModel::~OSQPModel() Var OSQPModel::addVar(const std::string& name) { - std::scoped_lock lock(mutex_); + const std::scoped_lock lock(mutex_); vars_.emplace_back(std::make_shared(vars_.size(), name, this)); lbs_.push_back(-OSQP_INFINITY); ubs_.push_back(OSQP_INFINITY); @@ -70,7 +70,7 @@ Var OSQPModel::addVar(const std::string& name) Cnt OSQPModel::addEqCnt(const AffExpr& expr, const std::string& /*name*/) { - std::scoped_lock lock(mutex_); + const std::scoped_lock lock(mutex_); cnts_.emplace_back(std::make_shared(cnts_.size(), this)); cnt_exprs_.push_back(expr); cnt_types_.push_back(EQ); @@ -79,7 +79,7 @@ Cnt OSQPModel::addEqCnt(const AffExpr& expr, const std::string& /*name*/) Cnt OSQPModel::addIneqCnt(const AffExpr& expr, const std::string& /*name*/) { - std::scoped_lock lock(mutex_); + const std::scoped_lock lock(mutex_); cnts_.emplace_back(std::make_shared(cnts_.size(), this)); cnt_exprs_.push_back(expr); cnt_types_.push_back(INEQ); @@ -90,7 +90,7 @@ Cnt OSQPModel::addIneqCnt(const QuadExpr&, const std::string& /*name*/) { throw void OSQPModel::removeVars(const VarVector& vars) { - std::scoped_lock lock(mutex_); + const std::scoped_lock lock(mutex_); SizeTVec inds; vars2inds(vars, inds); for (const auto& var : vars) @@ -99,7 +99,7 @@ void OSQPModel::removeVars(const VarVector& vars) void OSQPModel::removeCnts(const CntVector& cnts) { - std::scoped_lock lock(mutex_); + const std::scoped_lock lock(mutex_); SizeTVec inds; cnts2inds(cnts, inds); for (const auto& cnt : cnts) @@ -289,7 +289,7 @@ CvxOptStatus OSQPModel::optimize() if (OSQP_COMPARE_DEBUG_MODE) { - Eigen::IOFormat format(5); + const Eigen::IOFormat format(5); std::cout << "OSQP Number of Variables:" << osqp_data_.n << '\n'; std::cout << "OSQP Number of Constraints:" << osqp_data_.m << '\n'; @@ -345,7 +345,7 @@ CvxOptStatus OSQPModel::optimize() if (OSQP_COMPARE_DEBUG_MODE) { - Eigen::IOFormat format(5); + const Eigen::IOFormat format(5); Eigen::Map solution_vec(solution_.data(), static_cast(solution_.size())); std::cout << "OSQP Status Value: " << status << '\n'; std::cout << "OSQP Solution: " << solution_vec.transpose().format(format) << '\n'; @@ -373,7 +373,7 @@ void OSQPModel::writeToFile(const std::string& fname) const outStream << "Subject To\n"; for (std::size_t i = 0; i < cnt_exprs_.size(); ++i) { - std::string op = (cnt_types_[i] == INEQ) ? " <= " : " = "; + const std::string op = (cnt_types_[i] == INEQ) ? " <= " : " = "; outStream << cnt_exprs_[i] << op << 0 << "\n"; } diff --git a/trajopt_sco/src/qpoases_interface.cpp b/trajopt_sco/src/qpoases_interface.cpp index 7e1eccc0..1886201c 100644 --- a/trajopt_sco/src/qpoases_interface.cpp +++ b/trajopt_sco/src/qpoases_interface.cpp @@ -37,7 +37,7 @@ qpOASESModel::qpOASESModel() qpOASESModel::~qpOASESModel() = default; Var qpOASESModel::addVar(const std::string& name) { - std::scoped_lock lock(mutex_); + const std::scoped_lock lock(mutex_); vars_.emplace_back(std::make_shared(vars_.size(), name, this)); lb_.push_back(-QPOASES_INFTY); ub_.push_back(QPOASES_INFTY); @@ -46,7 +46,7 @@ Var qpOASESModel::addVar(const std::string& name) Cnt qpOASESModel::addEqCnt(const AffExpr& expr, const std::string& /*name*/) { - std::scoped_lock lock(mutex_); + const std::scoped_lock lock(mutex_); cnts_.emplace_back(std::make_shared(cnts_.size(), this)); cnt_exprs_.push_back(expr); cnt_types_.push_back(EQ); @@ -55,7 +55,7 @@ Cnt qpOASESModel::addEqCnt(const AffExpr& expr, const std::string& /*name*/) Cnt qpOASESModel::addIneqCnt(const AffExpr& expr, const std::string& /*name*/) { - std::scoped_lock lock(mutex_); + const std::scoped_lock lock(mutex_); cnts_.emplace_back(std::make_shared(cnts_.size(), this)); cnt_exprs_.push_back(expr); cnt_types_.push_back(INEQ); @@ -70,7 +70,7 @@ Cnt qpOASESModel::addIneqCnt(const QuadExpr&, const std::string& /*name*/) void qpOASESModel::removeVars(const VarVector& vars) { - std::scoped_lock lock(mutex_); + const std::scoped_lock lock(mutex_); IntVec inds; vars2inds(vars, inds); for (const auto& var : vars) @@ -79,7 +79,7 @@ void qpOASESModel::removeVars(const VarVector& vars) void qpOASESModel::removeCnts(const CntVector& cnts) { - std::scoped_lock lock(mutex_); + const std::scoped_lock lock(mutex_); IntVec inds; cnts2inds(cnts, inds); for (const auto& cnt : cnts) diff --git a/trajopt_sco/src/solver_interface.cpp b/trajopt_sco/src/solver_interface.cpp index 51b6c8fa..be35ea89 100644 --- a/trajopt_sco/src/solver_interface.cpp +++ b/trajopt_sco/src/solver_interface.cpp @@ -53,7 +53,7 @@ void simplify2(IntVec& inds, DblVec& vals) inds.resize(ind2val.size()); vals.resize(ind2val.size()); long unsigned int i_new = 0; - for (Int2Double::value_type& iv : ind2val) + for (const Int2Double::value_type& iv : ind2val) { inds[i_new] = iv.first; vals[i_new] = iv.second; @@ -116,25 +116,26 @@ Var Model::addVar(const std::string& name, double lb, double ub) } void Model::removeVar(const Var& var) { - VarVector vars(1, var); + const VarVector vars(1, var); removeVars(vars); } void Model::removeCnt(const Cnt& cnt) { - CntVector cnts(1, cnt); + const CntVector cnts(1, cnt); removeCnts(cnts); } double Model::getVarValue(const Var& var) const { - VarVector vars(1, var); + const VarVector vars(1, var); return getVarValues(vars)[0]; } void Model::setVarBounds(const Var& var, double lower, double upper) { - DblVec lowers(1, lower), uppers(1, upper); - VarVector vars(1, var); + const DblVec lowers(1, lower); + const DblVec uppers(1, upper); + const VarVector vars(1, var); setVarBounds(vars, lowers, uppers); } @@ -247,8 +248,8 @@ void ModelType::fromJson(const Json::Value& v) { try { - std::string ref = v.asString(); - ModelType cs(ref); + const std::string ref = v.asString(); + const ModelType cs(ref); value_ = cs.value_; } catch (const std::runtime_error&) diff --git a/trajopt_sco/src/solver_utils.cpp b/trajopt_sco/src/solver_utils.cpp index 660b4621..36f5227c 100644 --- a/trajopt_sco/src/solver_utils.cpp +++ b/trajopt_sco/src/solver_utils.cpp @@ -79,7 +79,8 @@ void exprToEigen(const QuadExpr& expr, } else { - Eigen::Index c{ 0 }, r{ 0 }; + Eigen::Index c{ 0 }; + Eigen::Index r{ 0 }; if (ind1[i] < ind2[i]) { r = static_cast(ind1[i]); diff --git a/trajopt_sco/test/small-problems-unit.cpp b/trajopt_sco/test/small-problems-unit.cpp index 8d5ad92a..84b397fe 100644 --- a/trajopt_sco/test/small-problems-unit.cpp +++ b/trajopt_sco/test/small-problems-unit.cpp @@ -4,8 +4,6 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include #include #include -#include -#include TRAJOPT_IGNORE_WARNINGS_POP #include @@ -31,6 +29,7 @@ void setupProblem(OptProb::Ptr& probptr, std::size_t nvars, ModelType convex_sol { probptr = std::make_shared(convex_solver); vector var_names; + var_names.reserve(nvars); for (std::size_t i = 0; i < nvars; ++i) { var_names.push_back((boost::format("x_%i") % i).str()); @@ -41,7 +40,6 @@ void setupProblem(OptProb::Ptr& probptr, std::size_t nvars, ModelType convex_sol void expectAllNear(const DblVec& x, const DblVec& y, double abstol) { EXPECT_EQ(x.size(), y.size()); - stringstream ss; LOG_INFO("checking %s ?= %s", CSTR(x), CSTR(y)); for (std::size_t i = 0; i < x.size(); ++i) EXPECT_NEAR(x[i], y[i], abstol); @@ -57,9 +55,9 @@ TEST_P(SQP, QuadraticSeparable) // NOLINT BasicTrustRegionSQP solver(prob); BasicTrustRegionSQPParameters& params = solver.getParameters(); params.trust_box_size = 100; - DblVec x = { 3, 4, 5 }; + const DblVec x = { 3, 4, 5 }; solver.initialize(x); - OptStatus status = solver.optimize(); + const OptStatus status = solver.optimize(); ASSERT_EQ(status, OPT_CONVERGED); expectAllNear(solver.x(), { 0, 1, 2 }, 1e-3); // todo: checks on number of iterations and function evaluates @@ -76,9 +74,9 @@ TEST_P(SQP, QuadraticNonseparable) // NOLINT params.trust_box_size = 100; params.min_trust_box_size = 1e-5; params.min_approx_improve = 1e-6; - DblVec x = { 3, 4, 5 }; + const DblVec x = { 3, 4, 5 }; solver.initialize(x); - OptStatus status = solver.optimize(); + const OptStatus status = solver.optimize(); ASSERT_EQ(status, OPT_CONVERGED); expectAllNear(solver.x(), { 1, 7, 2 }, .01); // todo: checks on number of iterations and function evaluates @@ -92,7 +90,7 @@ void testProblem(ScalarOfVector::Ptr f, ModelType convex_solver) { OptProb::Ptr prob; - std::size_t n = init.size(); + const std::size_t n = init.size(); assert(sol.size() == n); setupProblem(prob, n, convex_solver); prob->addCost(std::make_shared(std::move(f), prob->getVars(), "f", true)); @@ -106,7 +104,7 @@ void testProblem(ScalarOfVector::Ptr f, params.initial_merit_error_coeff = 1; solver.initialize(init); - OptStatus status = solver.optimize(); + const OptStatus status = solver.optimize(); EXPECT_EQ(status, OPT_CONVERGED); expectAllNear(solver.x(), sol, .01); } diff --git a/trajopt_sco/test/solver-interface-unit.cpp b/trajopt_sco/test/solver-interface-unit.cpp index dadcece9..290ee38b 100644 --- a/trajopt_sco/test/solver-interface-unit.cpp +++ b/trajopt_sco/test/solver-interface-unit.cpp @@ -32,7 +32,7 @@ TEST(SolverInterface, simplify2) // NOLINT TEST_P(SolverInterface, setup_problem) // NOLINT { - Model::Ptr solver = createModel(GetParam()); + const Model::Ptr solver = createModel(GetParam()); VarVector vars; for (int i = 0; i < 3; ++i) { @@ -43,15 +43,15 @@ TEST_P(SolverInterface, setup_problem) // NOLINT solver->update(); AffExpr aff; - std::cout << aff << std::endl; + std::cout << aff << '\n'; for (std::size_t i = 0; i < 3; ++i) { exprInc(aff, vars[i]); solver->setVarBounds(vars[i], 0, 10); } aff.constant -= 3; - std::cout << aff << std::endl; - QuadExpr affsquared = exprSquare(aff); + std::cout << aff << '\n'; + const QuadExpr affsquared = exprSquare(aff); solver->setObjective(affsquared); solver->update(); LOG_INFO("objective: %s", CSTR(affsquared)); @@ -75,7 +75,7 @@ TEST_P(SolverInterface, setup_problem) // NOLINT // Tests multiplying larger terms TEST_P(SolverInterface, DISABLED_ExprMult_test1) // NOLINT // QuadExpr not PSD { - Model::Ptr solver = createModel(GetParam()); + const Model::Ptr solver = createModel(GetParam()); VarVector vars; for (int i = 0; i < 3; ++i) { @@ -92,17 +92,17 @@ TEST_P(SolverInterface, DISABLED_ExprMult_test1) // NOLINT // QuadExpr not PSD solver->update(); AffExpr aff1; - std::cout << aff1 << std::endl; + std::cout << aff1 << '\n'; for (std::size_t i = 0; i < 3; ++i) { exprInc(aff1, vars[i]); solver->setVarBounds(vars[i], 0, 10); } aff1.constant -= 3; - std::cout << aff1 << std::endl; + std::cout << aff1 << '\n'; AffExpr aff2; - std::cout << aff2 << std::endl; + std::cout << aff2 << '\n'; for (std::size_t i = 3; i < 6; ++i) { exprInc(aff2, vars[i]); @@ -110,8 +110,8 @@ TEST_P(SolverInterface, DISABLED_ExprMult_test1) // NOLINT // QuadExpr not PSD } aff2.constant -= 5; - std::cout << aff2 << std::endl; - QuadExpr aff12 = exprMult(aff1, aff2); + std::cout << aff2 << '\n'; + const QuadExpr aff12 = exprMult(aff1, aff2); solver->setObjective(aff12); solver->update(); LOG_INFO("objective: %s", CSTR(aff12)); @@ -142,7 +142,7 @@ TEST_P(SolverInterface, ExprMult_test2) // NOLINT const double aff1_const = 0; const double aff2_const = 0; - Model::Ptr solver = createModel(GetParam()); + const Model::Ptr solver = createModel(GetParam()); VarVector vars; vars.push_back(solver->addVar("v1")); vars.push_back(solver->addVar("v2")); @@ -162,9 +162,9 @@ TEST_P(SolverInterface, ExprMult_test2) // NOLINT aff2.constant = aff2_const; aff2.coeffs[0] = v2_coeff; - std::cout << "aff1: " << aff1 << std::endl; - std::cout << "aff2: " << aff2 << std::endl; - QuadExpr aff12 = exprMult(aff1, aff2); + std::cout << "aff1: " << aff1 << '\n'; + std::cout << "aff2: " << aff2 << '\n'; + const QuadExpr aff12 = exprMult(aff1, aff2); solver->setObjective(aff12); solver->update(); LOG_INFO("objective: %s", CSTR(aff12)); @@ -177,10 +177,10 @@ TEST_P(SolverInterface, ExprMult_test2) // NOLINT for (std::size_t i = 0; i < 2; ++i) { soln[i] = solver->getVarValue(vars[i]); - std::cout << soln[i] << std::endl; + std::cout << soln[i] << '\n'; } - std::cout << "Result: " << aff12.value(soln) << std::endl; - double answer = (v1_coeff * v1_val + aff1_const) * (v2_coeff * v2_val + aff2_const); + std::cout << "Result: " << aff12.value(soln) << '\n'; + const double answer = (v1_coeff * v1_val + aff1_const) * (v2_coeff * v2_val + aff2_const); EXPECT_NEAR(aff12.value(soln), answer, 1e-6); } @@ -194,7 +194,7 @@ TEST_P(SolverInterface, ExprMult_test3) // NOLINT const double aff1_const = -3; const double aff2_const = -5; - Model::Ptr solver = createModel(GetParam()); + const Model::Ptr solver = createModel(GetParam()); VarVector vars; vars.push_back(solver->addVar("v1")); vars.push_back(solver->addVar("v2")); @@ -214,9 +214,9 @@ TEST_P(SolverInterface, ExprMult_test3) // NOLINT aff2.constant = aff2_const; aff2.coeffs[0] = v2_coeff; - std::cout << "aff1: " << aff1 << std::endl; - std::cout << "aff2: " << aff2 << std::endl; - QuadExpr aff12 = exprMult(aff1, aff2); + std::cout << "aff1: " << aff1 << '\n'; + std::cout << "aff2: " << aff2 << '\n'; + const QuadExpr aff12 = exprMult(aff1, aff2); solver->setObjective(aff12); solver->update(); LOG_INFO("objective: %s", CSTR(aff12)); @@ -229,10 +229,10 @@ TEST_P(SolverInterface, ExprMult_test3) // NOLINT for (std::size_t i = 0; i < 2; ++i) { soln[i] = solver->getVarValue(vars[i]); - std::cout << soln[i] << std::endl; + std::cout << soln[i] << '\n'; } - std::cout << "Result: " << aff12.value(soln) << std::endl; - double answer = (v1_coeff * v1_val + aff1_const) * (v2_coeff * v2_val + aff2_const); + std::cout << "Result: " << aff12.value(soln) << '\n'; + const double answer = (v1_coeff * v1_val + aff1_const) * (v2_coeff * v2_val + aff2_const); EXPECT_NEAR(aff12.value(soln), answer, 1e-6); } diff --git a/trajopt_sco/test/solver-utils-unit.cpp b/trajopt_sco/test/solver-utils-unit.cpp index 57e45903..63866f9c 100644 --- a/trajopt_sco/test/solver-utils-unit.cpp +++ b/trajopt_sco/test/solver-utils-unit.cpp @@ -18,16 +18,16 @@ using namespace sco; TEST(solver_utils, exprToEigen) // NOLINT { - int n_vars = 2; + const int n_vars = 2; std::vector x_info; VarVector x; for (std::size_t i = 0; i < static_cast(n_vars); ++i) { std::stringstream var_name; var_name << "x_" << i; - VarRep::Ptr x_el(std::make_shared(i, var_name.str(), nullptr)); + const VarRep::Ptr x_el(std::make_shared(i, var_name.str(), nullptr)); x_info.push_back(x_el); - x.push_back(Var(x_el)); + x.emplace_back(x_el); } // x_affine = [3, 2]*x + 1 @@ -36,8 +36,8 @@ TEST(solver_utils, exprToEigen) // NOLINT x_affine.coeffs = DblVec{ 3, 2 }; x_affine.constant = 1; - std::cout << "x_affine= " << x_affine << std::endl; - std::cout << "expecting A = [3, 2];" << std::endl << " u = [1]" << std::endl; + std::cout << "x_affine= " << x_affine << '\n'; + std::cout << "expecting A = [3, 2];" << '\n' << " u = [1]" << '\n'; Eigen::MatrixXd m_A_expected(1, n_vars); m_A_expected << 3, 2; Eigen::VectorXd v_u_expected(1); @@ -46,31 +46,26 @@ TEST(solver_utils, exprToEigen) // NOLINT Eigen::SparseVector v_A; exprToEigen(x_affine, v_A, n_vars); ASSERT_EQ(v_A.size(), m_A_expected.cols()); - Eigen::VectorXd v_A_d = v_A; - Eigen::VectorXd m_A_r = m_A_expected.row(0); - EXPECT_TRUE(v_A_d.isApprox(m_A_r)) << "error converting x_affine" - << " to Eigen::SparseVector. " - << "v_A :" << std::endl - << v_A << std::endl; + const Eigen::VectorXd v_A_d = v_A; + const Eigen::VectorXd m_A_r = m_A_expected.row(0); + EXPECT_TRUE(v_A_d.isApprox(m_A_r)) << "error converting x_affine" << " to Eigen::SparseVector. " << "v_A :" << '\n' + << v_A << '\n'; Eigen::SparseMatrix m_A; Eigen::VectorXd v_u; - AffExprVector x_affine_vector(1, x_affine); + const AffExprVector x_affine_vector(1, x_affine); exprToEigen(x_affine_vector, m_A, v_u, n_vars); ASSERT_EQ(v_u_expected.size(), v_u.size()); - EXPECT_TRUE(v_u_expected == v_u) << "v_u_expected != v_u" << std::endl << "v_u:" << std::endl << v_u << std::endl; - EXPECT_EQ(m_A.nonZeros(), 2) << "m_A.nonZeros() != 2" << std::endl; + EXPECT_TRUE(v_u_expected == v_u) << "v_u_expected != v_u" << '\n' << "v_u:" << '\n' << v_u << '\n'; + EXPECT_EQ(m_A.nonZeros(), 2) << "m_A.nonZeros() != 2" << '\n'; ASSERT_EQ(m_A.rows(), m_A_expected.rows()); ASSERT_EQ(m_A.cols(), m_A_expected.cols()); - EXPECT_TRUE(m_A.isApprox(m_A_expected)) << "error converting x_affine to " - << "Eigen::SparseMatrix. m_A :" << std::endl - << m_A << std::endl; + EXPECT_TRUE(m_A.isApprox(m_A_expected)) << "error converting x_affine to " << "Eigen::SparseMatrix. m_A :" << '\n' + << m_A << '\n'; QuadExpr x_squared = exprSquare(x_affine); - std::cout << "x_squared= " << x_squared << std::endl; - std::cout << "expecting Q = [9, 6;" << std::endl - << " 6, 4]" << std::endl - << " q = [6, 4]" << std::endl; + std::cout << "x_squared= " << x_squared << '\n'; + std::cout << "expecting Q = [9, 6;" << '\n' << " 6, 4]" << '\n' << " q = [6, 4]" << '\n'; Eigen::MatrixXd m_Q_expected(2, 2); m_Q_expected << 9, 6, 6, 4; DblVec v_q_expected{ 6, 4 }; @@ -79,78 +74,71 @@ TEST(solver_utils, exprToEigen) // NOLINT Eigen::VectorXd v_q; exprToEigen(x_squared, m_Q, v_q, n_vars); { - Eigen::Map v_q_e_eig(v_q_expected.data(), - static_cast(v_q_expected.size())); + const Eigen::Map v_q_e_eig(v_q_expected.data(), + static_cast(v_q_expected.size())); ASSERT_EQ(v_q.size(), v_q_e_eig.size()); - EXPECT_TRUE(v_q_e_eig.isApprox(v_q)) << "v_q_expected != v_q" << std::endl - << "v_q:" << std::endl - << v_q << std::endl; + EXPECT_TRUE(v_q_e_eig.isApprox(v_q)) << "v_q_expected != v_q" << '\n' << "v_q:" << '\n' << v_q << '\n'; } - EXPECT_TRUE(m_Q.isApprox(m_Q_expected)) << "error converting x_squared to " - << "Eigen::SparseMatrix. m_Q :" << std::endl - << m_Q << std::endl; - EXPECT_EQ(m_Q.nonZeros(), 4) << "m_Q.nonZeros() != 4" << std::endl; + EXPECT_TRUE(m_Q.isApprox(m_Q_expected)) << "error converting x_squared to " << "Eigen::SparseMatrix. m_Q :" << '\n' + << m_Q << '\n'; + EXPECT_EQ(m_Q.nonZeros(), 4) << "m_Q.nonZeros() != 4" << '\n'; exprToEigen(x_squared, m_Q, v_q, n_vars, true); { - Eigen::Map v_q_e_eig(v_q_expected.data(), - static_cast(v_q_expected.size())); - EXPECT_TRUE(v_q_e_eig.isApprox(v_q)) << "v_q_expected != v_q" << std::endl - << "v_q:" << std::endl - << v_q << std::endl; + const Eigen::Map v_q_e_eig(v_q_expected.data(), + static_cast(v_q_expected.size())); + EXPECT_TRUE(v_q_e_eig.isApprox(v_q)) << "v_q_expected != v_q" << '\n' << "v_q:" << '\n' << v_q << '\n'; } - EXPECT_TRUE(m_Q.isApprox(2 * m_Q_expected)) << "error converting x_squared to" - << " Eigen::SparseMatrix. m_Q :" << std::endl - << m_Q << std::endl; - EXPECT_EQ(m_Q.nonZeros(), 4) << "m_Q.nonZeros() != 4" << std::endl; + EXPECT_TRUE(m_Q.isApprox(2 * m_Q_expected)) + << "error converting x_squared to" << " Eigen::SparseMatrix. m_Q :" << '\n' + << m_Q << '\n'; + EXPECT_EQ(m_Q.nonZeros(), 4) << "m_Q.nonZeros() != 4" << '\n'; x_affine.coeffs = DblVec{ 0, 2 }; - std::cout << "x_affine= " << x_affine << std::endl; - std::cout << "expecting A = [0, 2];" << std::endl; + std::cout << "x_affine= " << x_affine << '\n'; + std::cout << "expecting A = [0, 2];" << '\n'; x_squared = exprSquare(x_affine); - std::cout << "x_squared= " << x_squared << std::endl; - std::cout << "expecting Q = [0, 0;" << std::endl << " 0, 4]" << std::endl; + std::cout << "x_squared= " << x_squared << '\n'; + std::cout << "expecting Q = [0, 0;" << '\n' << " 0, 4]" << '\n'; m_Q_expected.setZero(); m_Q_expected << 0, 0, 0, 4; exprToEigen(x_squared, m_Q, v_q, n_vars, false, false); - EXPECT_TRUE(m_Q.isApprox(m_Q_expected)) << "error converting x_squared to " - << "Eigen::SparseMatrix. m_Q :" << std::endl - << m_Q << std::endl; - EXPECT_EQ(m_Q.nonZeros(), 1) << "m_Q.nonZeros() != 1" << std::endl; + EXPECT_TRUE(m_Q.isApprox(m_Q_expected)) << "error converting x_squared to " << "Eigen::SparseMatrix. m_Q :" << '\n' + << m_Q << '\n'; + EXPECT_EQ(m_Q.nonZeros(), 1) << "m_Q.nonZeros() != 1" << '\n'; exprToEigen(x_squared, m_Q, v_q, n_vars, true, false); - EXPECT_TRUE(m_Q.isApprox(2 * m_Q_expected)) << "error converting x_squared to" - << " Eigen::SparseMatrix. m_Q :" << std::endl - << m_Q << std::endl; - EXPECT_EQ(m_Q.nonZeros(), 1) << "m_Q.nonZeros() != 1" << std::endl; + EXPECT_TRUE(m_Q.isApprox(2 * m_Q_expected)) + << "error converting x_squared to" << " Eigen::SparseMatrix. m_Q :" << '\n' + << m_Q << '\n'; + EXPECT_EQ(m_Q.nonZeros(), 1) << "m_Q.nonZeros() != 1" << '\n'; exprToEigen(x_squared, m_Q, v_q, n_vars, false, true); - EXPECT_TRUE(m_Q.isApprox(m_Q_expected)) << "error converting x_squared to " - << "Eigen::SparseMatrix. m_Q :" << std::endl - << m_Q << std::endl; - EXPECT_EQ(m_Q.nonZeros(), 2) << "m_Q.nonZeros() != 2" << std::endl; + EXPECT_TRUE(m_Q.isApprox(m_Q_expected)) << "error converting x_squared to " << "Eigen::SparseMatrix. m_Q :" << '\n' + << m_Q << '\n'; + EXPECT_EQ(m_Q.nonZeros(), 2) << "m_Q.nonZeros() != 2" << '\n'; exprToEigen(x_squared, m_Q, v_q, n_vars, true, true); - EXPECT_TRUE(m_Q.isApprox(2 * m_Q_expected)) << "error converting x_squared to" - << "Eigen::SparseMatrix. m_Q :" << std::endl - << m_Q << std::endl; - EXPECT_EQ(m_Q.nonZeros(), 2) << "m_Q.nonZeros() != 2" << std::endl; + EXPECT_TRUE(m_Q.isApprox(2 * m_Q_expected)) << "error converting x_squared to" << "Eigen::SparseMatrix. m_Q :" << '\n' + << m_Q << '\n'; + EXPECT_EQ(m_Q.nonZeros(), 2) << "m_Q.nonZeros() != 2" << '\n'; } TEST(solver_utils, eigenToTriplets) // NOLINT { Eigen::MatrixXd m_Q(2, 2); m_Q << 9, 0, 6, 4; - Eigen::SparseMatrix m_Q_sparse_expected = m_Q.sparseView(); - IntVec m_Q_i, m_Q_j; + const Eigen::SparseMatrix m_Q_sparse_expected = m_Q.sparseView(); + IntVec m_Q_i; + IntVec m_Q_j; DblVec m_Q_ij; eigenToTriplets(m_Q_sparse_expected, m_Q_i, m_Q_j, m_Q_ij); Eigen::SparseMatrix m_Q_sparse(2, 2); tripletsToEigen(m_Q_i, m_Q_j, m_Q_ij, m_Q_sparse); EXPECT_TRUE(m_Q_sparse.isApprox(m_Q)) << "m_Q != m_Q_sparse when converting " - << "m_Q -> triplets -> m_Q_sparse. m_Q:" << std::endl - << m_Q << std::endl; - EXPECT_EQ(m_Q_sparse.nonZeros(), 3) << "m_Q.nonZeros() != 3" << std::endl; + << "m_Q -> triplets -> m_Q_sparse. m_Q:" << '\n' + << m_Q << '\n'; + EXPECT_EQ(m_Q_sparse.nonZeros(), 3) << "m_Q.nonZeros() != 3" << '\n'; } TEST(solver_utils, eigenToCSC) // NOLINT @@ -173,9 +161,7 @@ TEST(solver_utils, eigenToCSC) // NOLINT EXPECT_TRUE(rows_i.size() == P.size()) << "rows_i.size() != P.size()"; EXPECT_TRUE((P == DblVec{ 1, 1, 1, 2, 8, 3, 9 })) << "bad P:\n" << CSTR(P); EXPECT_TRUE((rows_i == IntVec{ 0, 1, 2, 0, 2, 0, 1 })) << "bad rows_i:\n" << CSTR(rows_i); - EXPECT_TRUE((cols_p == IntVec{ 0, 3, 5, 7 })) << "cols_p not in " - << "CRC form:\n" - << CSTR(cols_p); + EXPECT_TRUE((cols_p == IntVec{ 0, 3, 5, 7 })) << "cols_p not in " << "CRC form:\n" << CSTR(cols_p); } { /* @@ -194,22 +180,22 @@ TEST(solver_utils, eigenToCSC) // NOLINT EXPECT_TRUE((rows_i == IntVec{ 1, 0 })) << "rows_i != data_j:\n" << CSTR(rows_i) << " vs\n" << CSTR((IntVec{ 1, 0 })); - EXPECT_TRUE((cols_p == IntVec{ 0, 1, 2, 2 })) << "cols_p not in " - << "CRC form:\n" - << CSTR(cols_p); + EXPECT_TRUE((cols_p == IntVec{ 0, 1, 2, 2 })) << "cols_p not in " << "CRC form:\n" << CSTR(cols_p); - std::vector rows_i_ll, cols_p_ll; - std::vector rows_i_ll_exp{ 1, 0 }; - std::vector cols_p_ll_exp{ 0, 1, 2, 2 }; + std::vector rows_i_ll; + std::vector cols_p_ll; + const std::vector rows_i_ll_exp{ 1, 0 }; + const std::vector cols_p_ll_exp{ 0, 1, 2, 2 }; eigenToCSC(M, rows_i_ll, cols_p_ll, P); EXPECT_TRUE(rows_i_ll.size() == P.size()) << "rows_i_ll.size() != P.size()"; EXPECT_TRUE((rows_i_ll == rows_i_ll_exp)); EXPECT_TRUE((cols_p_ll == cols_p_ll_exp)); - std::vector rows_i_ull, cols_p_ull; - std::vector rows_i_ull_exp{ 1, 0 }; - std::vector cols_p_ull_exp{ 0, 1, 2, 2 }; + std::vector rows_i_ull; + std::vector cols_p_ull; + const std::vector rows_i_ull_exp{ 1, 0 }; + const std::vector cols_p_ull_exp{ 0, 1, 2, 2 }; eigenToCSC(M, rows_i_ull, cols_p_ull, P); EXPECT_TRUE(rows_i_ull.size() == P.size()) << "rows_i_ll.size() != P.size()"; EXPECT_TRUE((rows_i_ull == rows_i_ull_exp)); @@ -229,9 +215,7 @@ TEST(solver_utils, eigenToCSC) // NOLINT EXPECT_TRUE(rows_i.size() == P.size()) << "rows_i.size() != P.size()"; EXPECT_TRUE((P == DblVec{ 6 })) << "bad P:\n" << CSTR(P); EXPECT_TRUE((rows_i == IntVec{ 2 })) << "rows_i != data_j:\n" << CSTR(rows_i) << " vs\n" << CSTR((IntVec{ 1, 0 })); - EXPECT_TRUE((cols_p == IntVec{ 0, 0, 1, 1 })) << "cols_p not in " - << "CRC form:\n" - << CSTR(cols_p); + EXPECT_TRUE((cols_p == IntVec{ 0, 0, 1, 1 })) << "cols_p not in " << "CRC form:\n" << CSTR(cols_p); } } @@ -253,11 +237,8 @@ TEST(solver_utils, eigenToCSC_upper_triangular) // NOLINT EXPECT_TRUE(rows_i.size() == P.size()) << "rows_i.size() != P.size()"; EXPECT_TRUE((P == DblVec{ 1, 2, 4, 9 })) << "bad P:\n" << CSTR(P); - EXPECT_TRUE((rows_i == IntVec{ 0, 0, 1, 2 })) << "rows_i != expected" - << ":\n" + EXPECT_TRUE((rows_i == IntVec{ 0, 0, 1, 2 })) << "rows_i != expected" << ":\n" << CSTR(rows_i) << " vs\n" << CSTR((IntVec{ 0, 0, 1, 2 })); - EXPECT_TRUE((cols_p == IntVec{ 0, 1, 3, 4 })) << "cols_p not in " - << "CRC form:\n" - << CSTR(cols_p); + EXPECT_TRUE((cols_p == IntVec{ 0, 1, 3, 4 })) << "cols_p not in " << "CRC form:\n" << CSTR(cols_p); }