Skip to content
New issue

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

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

Already on GitHub? Sign in to your account

Update depends #337

Merged
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions dependencies.repos
Original file line number Diff line number Diff line change
@@ -1,11 +1,11 @@
- git:
local-name: ros_industrial_cmake_boilerplate
uri: https://github.com/ros-industrial/ros_industrial_cmake_boilerplate.git
version: 0.4.5
version: 0.4.7
- git:
local-name: tesseract
uri: https://github.com/ros-industrial-consortium/tesseract.git
version: master
version: 0.18.0
- git:
local-name: opw_kinematics
uri: https://github.com/Jmeyer1292/opw_kinematics.git
Expand Down
3 changes: 2 additions & 1 deletion trajopt_ifopt/test/inverse_kinematics_constraint_unit.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,7 @@ TRAJOPT_IGNORE_WARNINGS_PUSH
#include <tesseract_environment/environment.h>
#include <tesseract_environment/utils.h>
#include <tesseract_common/types.h>
#include <tesseract_common/utils.h>
TRAJOPT_IGNORE_WARNINGS_POP

#include <trajopt_ifopt/constraints/inverse_kinematics_constraint.h>
Expand Down Expand Up @@ -106,7 +107,7 @@ TEST_F(InverseKinematicsConstraintUnit, GetValue) // NOLINT

// Get the value (distance from IK position)
Eigen::VectorXd values = constraint->GetValues();
EXPECT_TRUE(values.isApprox(Eigen::VectorXd::Zero(n_dof)));
EXPECT_TRUE(almostEqualRelativeAndAbs(values, Eigen::VectorXd::Zero(n_dof)));

// Check that jac wrt constraint_var is identity
{
Expand Down
20 changes: 10 additions & 10 deletions trajopt_sco/include/trajopt_sco/solver_interface.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -115,12 +115,12 @@ struct VarRep
using Ptr = std::shared_ptr<VarRep>;

VarRep(std::size_t _index, std::string _name, void* _creator)
: index(_index), name(std::move(_name)), removed(false), creator(_creator)
: index(_index), name(std::move(_name)), creator(_creator)
{
}
std::size_t index;
std::string name;
bool removed;
bool removed{ false };
void* creator;
};

Expand Down Expand Up @@ -193,10 +193,10 @@ struct AffExpr
AffExpr(AffExpr&&) = default;
AffExpr& operator=(AffExpr&&) = default;

explicit AffExpr(double a) : constant(a) {}
explicit AffExpr(const Var& v) : coeffs(1, 1), vars(1, v) {}
explicit AffExpr(double a);
explicit AffExpr(const Var& v);

size_t size() const { return coeffs.size(); }
size_t size() const;
double value(const double* x) const;
double value(const DblVec& x) const;
};
Expand All @@ -210,10 +210,10 @@ struct QuadExpr
VarVector vars1;
VarVector vars2;
QuadExpr() = default;
explicit QuadExpr(double a) : affexpr(a) {}
explicit QuadExpr(const Var& v) : affexpr(v) {}
explicit QuadExpr(AffExpr aff) : affexpr(std::move(aff)) {}
size_t size() const { return coeffs.size(); }
explicit QuadExpr(double a);
explicit QuadExpr(const Var& v);
explicit QuadExpr(AffExpr aff);
size_t size() const;
double value(const double* x) const;
double value(const DblVec& x) const;
};
Expand Down Expand Up @@ -249,7 +249,7 @@ class ModelType
friend std::ostream& operator<<(std::ostream& os, const ModelType& cs);

private:
Value value_;
Value value_{ Value::AUTO_SOLVER };
};

std::vector<ModelType> availableSolvers();
Expand Down
1 change: 0 additions & 1 deletion trajopt_sco/src/modeling.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,6 @@ TRAJOPT_IGNORE_WARNINGS_POP
#include <trajopt_sco/modeling.hpp>
#include <trajopt_sco/sco_common.hpp>
#include <trajopt_utils/logging.hpp>
#include <trajopt_utils/macros.h>

namespace sco
{
Expand Down
19 changes: 14 additions & 5 deletions trajopt_sco/src/solver_interface.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,6 @@ TRAJOPT_IGNORE_WARNINGS_PUSH
TRAJOPT_IGNORE_WARNINGS_POP

#include <trajopt_sco/solver_interface.hpp>
#include <trajopt_utils/macros.h>

namespace sco
{
Expand Down Expand Up @@ -62,6 +61,10 @@ void simplify2(IntVec& inds, DblVec& vals)
}
}

AffExpr::AffExpr(double a) : constant(a) {}
AffExpr::AffExpr(const Var& v) : coeffs(1, 1), vars(1, v) {}
size_t AffExpr::size() const { return coeffs.size(); }

double AffExpr::value(const double* x) const
{
double out = constant;
Expand All @@ -80,6 +83,12 @@ double AffExpr::value(const DblVec& x) const
}
return out;
}

QuadExpr::QuadExpr(double a) : affexpr(a) {}
QuadExpr::QuadExpr(const Var& v) : affexpr(v) {}
QuadExpr::QuadExpr(AffExpr aff) : affexpr(std::move(aff)) {}
size_t QuadExpr::size() const { return coeffs.size(); }

double QuadExpr::value(const DblVec& x) const
{
double out = affexpr.value(x);
Expand Down Expand Up @@ -215,9 +224,9 @@ std::ostream& operator<<(std::ostream& os, const ModelType& cs)
return os;
}

ModelType::ModelType() { value_ = ModelType::AUTO_SOLVER; }
ModelType::ModelType(const ModelType::Value& v) { value_ = v; }
ModelType::ModelType(const int& v) { value_ = static_cast<Value>(v); }
ModelType::ModelType() = default;
ModelType::ModelType(const ModelType::Value& v) : value_(v) {}
ModelType::ModelType(const int& v) : value_(static_cast<Value>(v)) {}
ModelType::ModelType(const std::string& s)
{
for (unsigned int i = 0; i < ModelType::MODEL_NAMES_.size(); ++i)
Expand Down Expand Up @@ -351,6 +360,6 @@ Model::Ptr createModel(ModelType model_type, const ModelConfig::ConstPtr& model_
std::stringstream solver_instatiation_error;
solver_instatiation_error << "Failed to create solver: unknown solver " << solver << std::endl;
PRINT_AND_THROW(solver_instatiation_error.str());
return Model::Ptr();
return {};
}
} // namespace sco
2 changes: 1 addition & 1 deletion trajopt_sco/test/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -53,7 +53,7 @@ target_compile_options(${PROJECT_NAME}-test PRIVATE ${TRAJOPT_COMPILE_OPTIONS_PR
target_compile_definitions(${PROJECT_NAME}-test PRIVATE ${TRAJOPT_COMPILE_DEFINITIONS}
TRAJOPT_IFOPT_DIR="${CMAKE_SOURCE_DIR}")
target_cxx_version(${PROJECT_NAME}-test PRIVATE VERSION ${TRAJOPT_CXX_VERSION})
# target_clang_tidy(${PROJECT_NAME}-test ENABLE ${TRAJOPT_ENABLE_CLANG_TIDY})
target_clang_tidy(${PROJECT_NAME}-test ENABLE ${TRAJOPT_ENABLE_CLANG_TIDY})
add_gtest_discover_tests(${PROJECT_NAME}-test)
if(HAVE_BPMPD)
add_dependencies(${PROJECT_NAME}-test bpmpd_caller)
Expand Down
Loading