Skip to content

Commit 7fa43a8

Browse files
mpowelsonmpowelson
committed
building
1 parent 5c5509e commit 7fa43a8

15 files changed

+2533
-6
lines changed

‎tesseract/tesseract_planning/tesseract_motion_planners/CMakeLists.txt

Lines changed: 28 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -14,7 +14,9 @@ find_package(tesseract REQUIRED)
1414
find_package(tesseract_common REQUIRED)
1515
find_package(tesseract_command_language REQUIRED)
1616
find_package(trajopt REQUIRED)
17+
find_package(trajopt_ifopt REQUIRED)
1718
find_package(trajopt_sco REQUIRED)
19+
find_package(trajopt_sqp REQUIRED)
1820
find_package(cmake_common_scripts REQUIRED)
1921
# serialization was required because ompl does not include find_dependency for its required dependencies
2022
find_package(Boost REQUIRED COMPONENTS serialization system filesystem program_options)
@@ -161,12 +163,32 @@ target_include_directories(${PROJECT_NAME}_trajopt SYSTEM PUBLIC
161163
${EIGEN3_INCLUDE_DIRS}
162164
${Boost_INCLUDE_DIRS})
163165

164-
configure_package(NAMESPACE tesseract
165-
TARGETS ${PROJECT_NAME}_core
166-
${PROJECT_NAME}_descartes
167-
${PROJECT_NAME}_ompl
168-
${PROJECT_NAME}_simple
169-
${PROJECT_NAME}_trajopt)
166+
# Trajopt_ifopt Planner
167+
add_library(${PROJECT_NAME}_trajopt_ifopt SHARED
168+
src/trajopt_ifopt/trajopt_ifopt_collision_config.cpp
169+
src/trajopt_ifopt/trajopt_ifopt_motion_planner.cpp
170+
src/trajopt_ifopt/trajopt_ifopt_utils.cpp
171+
src/trajopt_ifopt/profile/trajopt_ifopt_default_plan_profile.cpp
172+
src/trajopt_ifopt/profile/trajopt_ifopt_default_composite_profile.cpp
173+
src/trajopt_ifopt/problem_generators/default_problem_generator.cpp)
174+
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})
178+
target_include_directories(${PROJECT_NAME}_trajopt_ifopt PUBLIC
179+
"$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
180+
"$<INSTALL_INTERFACE:include>")
181+
target_include_directories(${PROJECT_NAME}_trajopt_ifopt SYSTEM PUBLIC
182+
${EIGEN3_INCLUDE_DIRS}
183+
${Boost_INCLUDE_DIRS})
184+
185+
configure_package(${PROJECT_NAME}_core
186+
${PROJECT_NAME}_descartes
187+
${PROJECT_NAME}_ompl
188+
${PROJECT_NAME}_simple
189+
${PROJECT_NAME}_trajopt
190+
${PROJECT_NAME}_trajopt_ifopt
191+
)
170192

171193
# Mark cpp header files for installation
172194
install(DIRECTORY include/${PROJECT_NAME}

‎tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt_ifopt/problem_generators/default_problem_generator.h

Lines changed: 45 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,45 @@
1+
/**
2+
* @file default_problem_generator.h
3+
* @brief Generates a trajopt problem from a planner request
4+
*
5+
* @author Levi Armstrong
6+
* @date April 18, 2018
7+
* @version TODO
8+
* @bug No known bugs
9+
*
10+
* @copyright Copyright (c) 2020, Southwest Research Institute
11+
*
12+
* @par License
13+
* Software License Agreement (Apache License)
14+
* @par
15+
* Licensed under the Apache License, Version 2.0 (the "License");
16+
* you may not use this file except in compliance with the License.
17+
* You may obtain a copy of the License at
18+
* http://www.apache.org/licenses/LICENSE-2.0
19+
* @par
20+
* Unless required by applicable law or agreed to in writing, software
21+
* distributed under the License is distributed on an "AS IS" BASIS,
22+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
23+
* See the License for the specific language governing permissions and
24+
* limitations under the License.
25+
*/
26+
#ifndef TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_DEFAULT_PROBLEM_GENERATOR_H
27+
#define TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_DEFAULT_PROBLEM_GENERATOR_H
28+
29+
#include <tesseract_common/macros.h>
30+
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
31+
#include <ifopt/problem.h>
32+
TESSERACT_COMMON_IGNORE_WARNINGS_POP
33+
34+
#include <tesseract_motion_planners/core/types.h>
35+
#include <tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_profile.h>
36+
37+
namespace tesseract_planning
38+
{
39+
ifopt::Problem DefaultTrajoptProblemGenerator(const std::string& name,
40+
const PlannerRequest& request,
41+
const TrajOptIfoptPlanProfileMap& plan_profiles,
42+
const TrajOptIfoptCompositeProfileMap& composite_profiles);
43+
44+
} // namespace tesseract_planning
45+
#endif

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

Lines changed: 146 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,146 @@
1+
/**
2+
* @file trajopt_default_composite_profile.h
3+
* @brief
4+
*
5+
* @author Levi Armstrong
6+
* @date June 18, 2020
7+
* @version TODO
8+
* @bug No known bugs
9+
*
10+
* @copyright Copyright (c) 2020, Southwest Research Institute
11+
*
12+
* @par License
13+
* Software License Agreement (Apache License)
14+
* @par
15+
* Licensed under the Apache License, Version 2.0 (the "License");
16+
* you may not use this file except in compliance with the License.
17+
* You may obtain a copy of the License at
18+
* http://www.apache.org/licenses/LICENSE-2.0
19+
* @par
20+
* Unless required by applicable law or agreed to in writing, software
21+
* distributed under the License is distributed on an "AS IS" BASIS,
22+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
23+
* See the License for the specific language governing permissions and
24+
* limitations under the License.
25+
*/
26+
27+
#ifndef TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_DEFAULT_COMPOSITE_PROFILE_H
28+
#define TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_DEFAULT_COMPOSITE_PROFILE_H
29+
30+
#include <tesseract_common/macros.h>
31+
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
32+
#include <vector>
33+
#include <Eigen/Geometry>
34+
#include <Eigen/Core>
35+
TESSERACT_COMMON_IGNORE_WARNINGS_POP
36+
37+
#include <tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_collision_config.h>
38+
#include <tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_profile.h>
39+
40+
namespace tesseract_planning
41+
{
42+
class TrajOptIfoptDefaultCompositeProfile : public TrajOptIfoptCompositeProfile
43+
{
44+
public:
45+
TrajOptIfoptDefaultCompositeProfile() = default;
46+
TrajOptIfoptDefaultCompositeProfile(const tinyxml2::XMLElement& xml_element);
47+
48+
// /** @brief The type of contact test to perform: FIRST, CLOSEST, ALL */
49+
// tesseract_collision::ContactTestType contact_test_type = tesseract_collision::ContactTestType::ALL;
50+
// /** @brief Configuration info for collisions that are modeled as costs */
51+
// CollisionCostConfig collision_cost_config;
52+
// /** @brief Configuration info for collisions that are modeled as constraints */
53+
// CollisionConstraintConfig collision_constraint_config;
54+
// /** @brief If true, a joint velocity cost with a target of 0 will be applied for all timesteps Default: true*/
55+
// bool smooth_velocities = true;
56+
// /** @brief This default to all ones, but allows you to weight different joints */
57+
// Eigen::VectorXd velocity_coeff;
58+
// /** @brief If true, a joint acceleration cost with a target of 0 will be applied for all timesteps Default: false*/
59+
// bool smooth_accelerations = true;
60+
// /** @brief This default to all ones, but allows you to weight different joints */
61+
// Eigen::VectorXd acceleration_coeff;
62+
// /** @brief If true, a joint jerk cost with a target of 0 will be applied for all timesteps Default: false*/
63+
// bool smooth_jerks = true;
64+
// /** @brief This default to all ones, but allows you to weight different joints */
65+
// Eigen::VectorXd jerk_coeff;
66+
// /** @brief If true, applies a cost to avoid kinematic singularities */
67+
// bool avoid_singularity = false;
68+
// /** @brief Optimization weight associated with kinematic singularity avoidance */
69+
// double avoid_singularity_coeff = 5.0;
70+
71+
// /** @brief Set the resolution at which state validity needs to be verified in order for a motion between two states
72+
// * to be considered valid in post checking of trajectory returned by TrajOptIfopt.
73+
// *
74+
// * The resolution is equal to longest_valid_segment_fraction * state_space.getMaximumExtent()
75+
// *
76+
// * Note: The planner takes the conservative of either longest_valid_segment_fraction or longest_valid_segment_length.
77+
// */
78+
// double longest_valid_segment_fraction = 0.01; // 1%
79+
80+
// /** @brief Set the resolution at which state validity needs to be verified in order for a motion between two states
81+
// * to be considered valid. If norm(state1 - state0) > longest_valid_segment_length.
82+
// *
83+
// * Note: This gets converted to longest_valid_segment_fraction.
84+
// * longest_valid_segment_fraction = longest_valid_segment_length / state_space.getMaximumExtent()
85+
// */
86+
// double longest_valid_segment_length = 0.5;
87+
88+
// /**@brief Special link collision cost distances */
89+
// trajopt::SafetyMarginData::Ptr special_collision_cost{ nullptr };
90+
// /**@brief Special link collision constraint distances */
91+
// trajopt::SafetyMarginData::Ptr special_collision_constraint{ nullptr };
92+
93+
// void apply(trajopt::ProblemConstructionInfo& pci,
94+
// int start_index,
95+
// int end_index,
96+
// const ManipulatorInfo& manip_info,
97+
// const std::vector<std::string>& active_links,
98+
// const std::vector<int>& fixed_indices) override;
99+
100+
// tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override;
101+
102+
//protected:
103+
// void addCollisionCost(trajopt::ProblemConstructionInfo& pci,
104+
// int start_index,
105+
// int end_index,
106+
// const std::vector<int>& fixed_indices) const;
107+
108+
// void addCollisionConstraint(trajopt::ProblemConstructionInfo& pci,
109+
// int start_index,
110+
// int end_index,
111+
// const std::vector<int>& fixed_indices) const;
112+
113+
// void addVelocitySmoothing(trajopt::ProblemConstructionInfo& pci,
114+
// int start_index,
115+
// int end_index,
116+
// const std::vector<int>& fixed_indices) const;
117+
118+
// void addAccelerationSmoothing(trajopt::ProblemConstructionInfo& pci,
119+
// int start_index,
120+
// int end_index,
121+
// const std::vector<int>& fixed_indices) const;
122+
123+
// void addJerkSmoothing(trajopt::ProblemConstructionInfo& pci,
124+
// int start_index,
125+
// int end_index,
126+
// const std::vector<int>& fixed_indices) const;
127+
128+
// void addConstraintErrorFunctions(trajopt::ProblemConstructionInfo& pci,
129+
// int start_index,
130+
// int end_index,
131+
// const std::vector<int>& fixed_indices) const;
132+
133+
// void addAvoidSingularity(trajopt::ProblemConstructionInfo& pci,
134+
// int start_index,
135+
// int end_index,
136+
// const std::string& link,
137+
// const std::vector<int>& fixed_indices) const;
138+
139+
// void smoothMotionTerms(const tinyxml2::XMLElement& xml_element,
140+
// bool& enabled,
141+
// Eigen::VectorXd& coeff,
142+
// std::size_t& length);
143+
};
144+
} // namespace tesseract_planning
145+
146+
#endif // TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_DEFAULT_COMPOSITE_PROFILE_H

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

Lines changed: 96 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,96 @@
1+
/**
2+
* @file TrajOptIfopt_default_plan_profile.h
3+
* @brief
4+
*
5+
* @author Levi Armstrong
6+
* @date June 18, 2020
7+
* @version TODO
8+
* @bug No known bugs
9+
*
10+
* @copyright Copyright (c) 2020, Southwest Research Institute
11+
*
12+
* @par License
13+
* Software License Agreement (Apache License)
14+
* @par
15+
* Licensed under the Apache License, Version 2.0 (the "License");
16+
* you may not use this file except in compliance with the License.
17+
* You may obtain a copy of the License at
18+
* http://www.apache.org/licenses/LICENSE-2.0
19+
* @par
20+
* Unless required by applicable law or agreed to in writing, software
21+
* distributed under the License is distributed on an "AS IS" BASIS,
22+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
23+
* See the License for the specific language governing permissions and
24+
* limitations under the License.
25+
*/
26+
27+
#ifndef TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_DEFAULT_PLAN_PROFILE_H
28+
#define TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_DEFAULT_PLAN_PROFILE_H
29+
30+
#include <tesseract_common/macros.h>
31+
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
32+
#include <Eigen/Geometry>
33+
TESSERACT_COMMON_IGNORE_WARNINGS_POP
34+
35+
#include <tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_profile.h>
36+
37+
namespace tesseract_planning
38+
{
39+
class TrajOptIfoptDefaultPlanProfile : public TrajOptIfoptPlanProfile
40+
{
41+
public:
42+
using Ptr = std::shared_ptr<TrajOptIfoptDefaultPlanProfile>;
43+
using ConstPtr = std::shared_ptr<const TrajOptIfoptDefaultPlanProfile>;
44+
45+
TrajOptIfoptDefaultPlanProfile() = default;
46+
TrajOptIfoptDefaultPlanProfile(const tinyxml2::XMLElement& xml_element);
47+
48+
// Eigen::VectorXd cartesian_coeff{ Eigen::VectorXd::Constant(1, 1, 5) };
49+
// Eigen::VectorXd joint_coeff{ Eigen::VectorXd::Constant(1, 1, 5) };
50+
// TrajOptIfopt::TermType term_type{ TrajOptIfopt::TermType::TT_CNT };
51+
52+
// /** @brief Error function that is set as a constraint for each timestep.
53+
// *
54+
// * This is a vector of std::tuple<Error Function, Error Function Jacobian, Constraint Type, Coeff>, the error
55+
// * function, constraint type, and coeff is required, but the jacobian is optional (nullptr).
56+
// *
57+
// * Error Function:
58+
// * arg: VectorXd will be all of the joint values for one timestep.
59+
// * return: VectorXd of violations for each joint. Anything != 0 will be a violation
60+
// *
61+
// * Error Function Jacobian:
62+
// * arg: VectorXd will be all of the joint values for one timestep.
63+
// * return: Eigen::MatrixXd that represents the change in the error function with respect to joint values
64+
// *
65+
// * Error Constraint Type
66+
// *
67+
// * Coefficients/Weights
68+
// *
69+
// */
70+
// std::vector<std::tuple<sco::VectorOfVector::func, sco::MatrixOfVector::func, sco::ConstraintType, Eigen::VectorXd>>
71+
// constraint_error_functions;
72+
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;
79+
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;
86+
87+
// tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override;
88+
89+
//protected:
90+
// void addConstraintErrorFunctions(TrajOptIfopt::ProblemConstructionInfo& pci, const std::vector<int>& fixed_steps) const;
91+
92+
// void addAvoidSingularity(TrajOptIfopt::ProblemConstructionInfo& pci, const std::vector<int>& fixed_steps) const;
93+
};
94+
} // namespace tesseract_planning
95+
96+
#endif // TESSERACT_MOTION_PLANNERS_TrajOptIfopt_IFOPT_DEFAULT_PLAN_PROFILE_H

0 commit comments

Comments
 (0)