Skip to content

Commit 0f2a50e

Browse files
mpowelsonmpowelson
committed
Fix problem generator
1 parent 11d2a6d commit 0f2a50e

File tree

6 files changed

+163
-440
lines changed

6 files changed

+163
-440
lines changed

‎tesseract/tesseract_planning/tesseract_motion_planners/CMakeLists.txt

Lines changed: 8 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -172,15 +172,18 @@ add_library(${PROJECT_NAME}_trajopt_ifopt SHARED
172172
src/trajopt_ifopt/profile/trajopt_ifopt_default_composite_profile.cpp
173173
src/trajopt_ifopt/problem_generators/default_problem_generator.cpp)
174174
target_link_libraries(${PROJECT_NAME}_trajopt_ifopt PUBLIC ${Boost_LIBRARIES} ${PROJECT_NAME}_core trajopt::trajopt_ifopt trajopt::trajopt_sqp)
175-
tesseract_target_compile_options(${PROJECT_NAME}_trajopt_ifopt PUBLIC)
176-
tesseract_clang_tidy(${PROJECT_NAME}_trajopt_ifopt)
177-
tesseract_code_coverage(${PROJECT_NAME}_trajopt_ifopt ALL EXCLUDE ${COVERAGE_EXCLUDE})
175+
target_compile_options(${PROJECT_NAME}_trajopt_ifopt PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE})
176+
target_compile_options(${PROJECT_NAME}_trajopt_ifopt PUBLIC ${TESSERACT_COMPILE_OPTIONS_PUBLIC})
177+
target_compile_definitions(${PROJECT_NAME}_trajopt_ifopt PUBLIC ${TESSERACT_COMPILE_DEFINITIONS})
178+
target_clang_tidy(${PROJECT_NAME}_trajopt_ifopt ARGUMENTS ${TESSERACT_CLANG_TIDY_ARGS} ENABLE ${TESSERACT_ENABLE_CLANG_TIDY})
179+
target_cxx_version(${PROJECT_NAME}_trajopt_ifopt PUBLIC VERSION ${TESSERACT_CXX_VERSION})
180+
target_code_coverage(${PROJECT_NAME}_trajopt_ifopt ALL EXCLUDE ${COVERAGE_EXCLUDE} ENABLE ${TESSERACT_ENABLE_TESTING})
178181
target_include_directories(${PROJECT_NAME}_trajopt_ifopt PUBLIC
179182
"$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
180183
"$<INSTALL_INTERFACE:include>")
181184
target_include_directories(${PROJECT_NAME}_trajopt_ifopt SYSTEM PUBLIC
182-
${EIGEN3_INCLUDE_DIRS}
183-
${Boost_INCLUDE_DIRS})
185+
${EIGEN3_INCLUDE_DIRS}
186+
${Boost_INCLUDE_DIRS})
184187

185188
configure_package(${PROJECT_NAME}_core
186189
${PROJECT_NAME}_descartes

‎tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_plan_profile.h

Lines changed: 13 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -70,21 +70,21 @@ class TrajOptIfoptDefaultPlanProfile : public TrajOptIfoptPlanProfile
7070
// std::vector<std::tuple<sco::VectorOfVector::func, sco::MatrixOfVector::func, sco::ConstraintType, Eigen::VectorXd>>
7171
// constraint_error_functions;
7272

73-
// void apply(TrajOptIfopt::ProblemConstructionInfo& pci,
74-
// const Eigen::Isometry3d& cartesian_waypoint,
75-
// const Instruction& parent_instruction,
76-
// const ManipulatorInfo& manip_info,
77-
// const std::vector<std::string>& active_links,
78-
// int index) override;
73+
void apply(std::shared_ptr<ifopt::Problem>& nlp,
74+
const Eigen::Isometry3d& cartesian_waypoint,
75+
const Instruction& parent_instruction,
76+
const ManipulatorInfo& manip_info,
77+
const std::vector<std::string>& active_links,
78+
int index) override;
7979

80-
// void apply(TrajOptIfopt::ProblemConstructionInfo& pci,
81-
// const Eigen::VectorXd& joint_waypoint,
82-
// const Instruction& parent_instruction,
83-
// const ManipulatorInfo& manip_info,
84-
// const std::vector<std::string>& active_links,
85-
// int index) override;
80+
void apply(std::shared_ptr<ifopt::Problem>& nlp,
81+
const Eigen::VectorXd& joint_waypoint,
82+
const Instruction& parent_instruction,
83+
const ManipulatorInfo& manip_info,
84+
const std::vector<std::string>& active_links,
85+
int index) override;
8686

87-
// tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override;
87+
tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override;
8888

8989
//protected:
9090
// void addConstraintErrorFunctions(TrajOptIfopt::ProblemConstructionInfo& pci, const std::vector<int>& fixed_steps) const;

‎tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_profile.h

Lines changed: 21 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -31,6 +31,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
3131
#include <trajopt/problem_description.hpp>
3232
#include <vector>
3333
#include <memory>
34+
#include <ifopt/problem.h>
3435
TESSERACT_COMMON_IGNORE_WARNINGS_POP
3536

3637
#include <tesseract_command_language/core/instruction.h>
@@ -44,21 +45,21 @@ class TrajOptIfoptPlanProfile
4445
using Ptr = std::shared_ptr<TrajOptIfoptPlanProfile>;
4546
using ConstPtr = std::shared_ptr<const TrajOptIfoptPlanProfile>;
4647

47-
// virtual void apply(trajopt::ProblemConstructionInfo& pci,
48-
// const Eigen::Isometry3d& cartesian_waypoint,
49-
// const Instruction& parent_instruction,
50-
// const ManipulatorInfo& manip_info,
51-
// const std::vector<std::string>& active_links,
52-
// int index) = 0;
48+
virtual void apply(std::shared_ptr<ifopt::Problem>& nlp,
49+
const Eigen::Isometry3d& cartesian_waypoint,
50+
const Instruction& parent_instruction,
51+
const ManipulatorInfo& manip_info,
52+
const std::vector<std::string>& active_links,
53+
int index) = 0;
5354

54-
// virtual void apply(trajopt::ProblemConstructionInfo& pci,
55-
// const Eigen::VectorXd& joint_waypoint,
56-
// const Instruction& parent_instruction,
57-
// const ManipulatorInfo& manip_info,
58-
// const std::vector<std::string>& active_links,
59-
// int index) = 0;
55+
virtual void apply(std::shared_ptr<ifopt::Problem>& nlp,
56+
const Eigen::VectorXd& joint_waypoint,
57+
const Instruction& parent_instruction,
58+
const ManipulatorInfo& manip_info,
59+
const std::vector<std::string>& active_links,
60+
int index) = 0;
6061

61-
// virtual tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const = 0;
62+
virtual tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const = 0;
6263
};
6364

6465
class TrajOptIfoptCompositeProfile
@@ -67,14 +68,14 @@ class TrajOptIfoptCompositeProfile
6768
using Ptr = std::shared_ptr<TrajOptIfoptCompositeProfile>;
6869
using ConstPtr = std::shared_ptr<const TrajOptIfoptCompositeProfile>;
6970

70-
// virtual void apply(trajopt::ProblemConstructionInfo& pci,
71-
// int start_index,
72-
// int end_index,
73-
// const ManipulatorInfo& manip_info,
74-
// const std::vector<std::string>& active_links,
75-
// const std::vector<int>& fixed_indices) = 0;
71+
virtual void apply(std::shared_ptr<ifopt::Problem>& nlp,
72+
int start_index,
73+
int end_index,
74+
const ManipulatorInfo& manip_info,
75+
const std::vector<std::string>& active_links,
76+
const std::vector<int>& fixed_indices) = 0;
7677

77-
// virtual tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const = 0;
78+
virtual tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const = 0;
7879
};
7980

8081
using TrajOptIfoptCompositeProfileMap = std::unordered_map<std::string, TrajOptIfoptCompositeProfile::Ptr>;

‎tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt/problem_generators/default_problem_generator.cpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -161,7 +161,7 @@ trajopt::TrajOptProb::Ptr DefaultTrajoptProblemGenerator(const std::string& name
161161
std::string profile = getProfileString(plan_instruction->getProfile(), name, request.plan_profile_remapping);
162162
TrajOptPlanProfile::Ptr cur_plan_profile =
163163
getProfile<TrajOptPlanProfile>(profile, plan_profiles, std::make_shared<TrajOptDefaultPlanProfile>());
164-
if (!start_plan_profile)
164+
if (!cur_plan_profile)
165165
throw std::runtime_error("TrajOptPlannerUniversalConfig: Invalid profile");
166166

167167
if (plan_instruction->isLinear())

0 commit comments

Comments
 (0)