Skip to content

Commit 8b79339

Browse files
Levi ArmstrongLevi-Armstrong
Levi Armstrong
authored and
Levi-Armstrong
committed
Add TrajOpt Solver Profile
1 parent 94fc3e1 commit 8b79339

File tree

14 files changed

+207
-38
lines changed

14 files changed

+207
-38
lines changed

‎tesseract/tesseract_planning/tesseract_motion_planners/CMakeLists.txt

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -144,6 +144,7 @@ add_library(${PROJECT_NAME}_trajopt
144144
src/trajopt/trajopt_utils.cpp
145145
src/trajopt/profile/trajopt_default_plan_profile.cpp
146146
src/trajopt/profile/trajopt_default_composite_profile.cpp
147+
src/trajopt/profile/trajopt_default_solver_profile.cpp
147148
src/trajopt/problem_generators/default_problem_generator.cpp
148149
src/trajopt/serialize.cpp
149150
src/trajopt/deserialize.cpp)

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

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -39,7 +39,8 @@ namespace tesseract_planning
3939
trajopt::TrajOptProb::Ptr DefaultTrajoptProblemGenerator(const std::string& name,
4040
const PlannerRequest& request,
4141
const TrajOptPlanProfileMap& plan_profiles,
42-
const TrajOptCompositeProfileMap& composite_profiles);
42+
const TrajOptCompositeProfileMap& composite_profiles,
43+
const TrajOptSolverProfileMap& solver_profiles);
4344

4445
} // namespace tesseract_planning
4546
#endif

‎tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt/profile/trajopt_default_solver_profile.h

Lines changed: 52 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,52 @@
1+
/**
2+
* @file trajopt_default_solver_profile.h
3+
* @brief
4+
*
5+
* @author Levi Armstrong
6+
* @date December 13, 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+
#ifndef TESSERACT_MOTION_PLANNERS_TRAJOPT_DEFAULT_SOLVER_PROFILE_H
27+
#define TESSERACT_MOTION_PLANNERS_TRAJOPT_DEFAULT_SOLVER_PROFILE_H
28+
29+
#include <tesseract_motion_planners/trajopt/profile/trajopt_profile.h>
30+
#include <trajopt_sco/solver_interface.hpp>
31+
32+
namespace tesseract_planning
33+
{
34+
/** @brief The contains the default solver parameters available for setting up TrajOpt */
35+
class TrajOptDefaultSolverProfile : public TrajOptSolverProfile
36+
{
37+
public:
38+
using Ptr = std::shared_ptr<TrajOptDefaultSolverProfile>;
39+
using ConstPtr = std::shared_ptr<const TrajOptDefaultSolverProfile>;
40+
41+
/** @brief The Convex solver to use */
42+
sco::ModelType convex_solver{ sco::ModelType::OSQP };
43+
44+
/** @brief Optimization paramters */
45+
sco::BasicTrustRegionSQPParameters opt_info;
46+
47+
void apply(trajopt::ProblemConstructionInfo& pci) const override;
48+
49+
tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override;
50+
};
51+
} // namespace tesseract_planning
52+
#endif // TESSERACT_MOTION_PLANNERS_TRAJOPT_DEFAULT_SOLVER_PROFILE_H

‎tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt/profile/trajopt_profile.h

Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -77,6 +77,18 @@ class TrajOptCompositeProfile
7777
virtual tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const = 0;
7878
};
7979

80+
class TrajOptSolverProfile
81+
{
82+
public:
83+
using Ptr = std::shared_ptr<TrajOptSolverProfile>;
84+
using ConstPtr = std::shared_ptr<const TrajOptSolverProfile>;
85+
86+
virtual void apply(trajopt::ProblemConstructionInfo& pci) const = 0;
87+
88+
virtual tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const = 0;
89+
};
90+
91+
using TrajOptSolverProfileMap = std::unordered_map<std::string, TrajOptSolverProfile::ConstPtr>;
8092
using TrajOptCompositeProfileMap = std::unordered_map<std::string, TrajOptCompositeProfile::ConstPtr>;
8193
using TrajOptPlanProfileMap = std::unordered_map<std::string, TrajOptPlanProfile::ConstPtr>;
8294
} // namespace tesseract_planning

‎tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt/trajopt_motion_planner.h

Lines changed: 13 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -41,7 +41,8 @@ class TrajOptMotionPlannerStatusCategory;
4141
using TrajOptProblemGeneratorFn = std::function<trajopt::TrajOptProb::Ptr(const std::string&,
4242
const PlannerRequest&,
4343
const TrajOptPlanProfileMap&,
44-
const TrajOptCompositeProfileMap&)>;
44+
const TrajOptCompositeProfileMap&,
45+
const TrajOptSolverProfileMap&)>;
4546

4647
class TrajOptMotionPlanner : public MotionPlanner
4748
{
@@ -59,20 +60,24 @@ class TrajOptMotionPlanner : public MotionPlanner
5960

6061
TrajOptProblemGeneratorFn problem_generator;
6162

63+
/**
64+
* @brief The available solver profiles
65+
* @details This is used to look up solver parameters by the planner
66+
*/
67+
TrajOptSolverProfileMap solver_profiles;
68+
6269
/**
6370
* @brief The available composite profiles
64-
*
65-
* Composite instruction is a way to namespace or organize your planning problem. The composite instruction has a
66-
* profile which is used for applying multy waypoint costs and constraints like joint smoothing, collision avoidance,
67-
* and velocity smoothing.
71+
* @details Composite instruction is a way to namespace or organize your planning problem. The composite instruction
72+
* has a profile which is used for applying multy waypoint costs and constraints like joint smoothing, collision
73+
* avoidance, and velocity smoothing.
6874
*/
6975
TrajOptCompositeProfileMap composite_profiles;
7076

7177
/**
7278
* @brief The available plan profiles
73-
*
74-
* Plan instruction profiles are used to control waypoint specific information like fixed waypoint, toleranced
75-
* waypoint, corner distance waypoint, etc.
79+
* @details Plan instruction profiles are used to control waypoint specific information like fixed waypoint,
80+
* toleranced waypoint, corner distance waypoint, etc.
7681
*/
7782
TrajOptPlanProfileMap plan_profiles;
7883

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

Lines changed: 13 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -27,6 +27,7 @@
2727
#include <tesseract_motion_planners/trajopt/problem_generators/default_problem_generator.h>
2828
#include <tesseract_motion_planners/trajopt/profile/trajopt_default_composite_profile.h>
2929
#include <tesseract_motion_planners/trajopt/profile/trajopt_default_plan_profile.h>
30+
#include <tesseract_motion_planners/trajopt/profile/trajopt_default_solver_profile.h>
3031
#include <tesseract_motion_planners/core/utils.h>
3132
#include <tesseract_motion_planners/planner_utils.h>
3233

@@ -36,7 +37,8 @@ namespace tesseract_planning
3637
trajopt::TrajOptProb::Ptr DefaultTrajoptProblemGenerator(const std::string& name,
3738
const PlannerRequest& request,
3839
const TrajOptPlanProfileMap& plan_profiles,
39-
const TrajOptCompositeProfileMap& composite_profiles)
40+
const TrajOptCompositeProfileMap& composite_profiles,
41+
const TrajOptSolverProfileMap& solver_profiles)
4042
{
4143
auto pci = std::make_shared<trajopt::ProblemConstructionInfo>(request.tesseract);
4244

@@ -58,6 +60,16 @@ trajopt::TrajOptProb::Ptr DefaultTrajoptProblemGenerator(const std::string& name
5860
throw std::runtime_error(error_msg);
5961
}
6062

63+
// Apply Solver parameters
64+
std::string profile = request.instructions.getProfile();
65+
profile = getProfileString(profile, name, PlannerProfileRemapping());
66+
TrajOptSolverProfile::ConstPtr solver_profile =
67+
getProfile<TrajOptSolverProfile>(profile, solver_profiles, std::make_shared<TrajOptDefaultSolverProfile>());
68+
if (!solver_profile)
69+
throw std::runtime_error("TrajOptSolverConfig: Invalid profile");
70+
71+
solver_profile->apply(*pci);
72+
6173
// Flatten the input for planning
6274
auto instructions_flat = flattenProgram(request.instructions);
6375
auto seed_flat = flattenProgramToPattern(request.seed, request.instructions);
@@ -74,7 +86,6 @@ trajopt::TrajOptProb::Ptr DefaultTrajoptProblemGenerator(const std::string& name
7486

7587
std::size_t start_index = 0; // If it has a start instruction then skip first instruction in instructions_flat
7688
int index = 0;
77-
std::string profile;
7889
Waypoint start_waypoint = NullWaypoint();
7990
Instruction placeholder_instruction = NullInstruction();
8091
const Instruction* start_instruction = nullptr;

‎tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt/profile/trajopt_default_solver_profile.cpp

Lines changed: 39 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,39 @@
1+
/**
2+
* @file trajopt_default_solver_profile.cpp
3+
* @brief
4+
*
5+
* @author Levi Armstrong
6+
* @date December 13, 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+
#include <tesseract_motion_planners/trajopt/profile/trajopt_default_solver_profile.h>
28+
29+
namespace tesseract_planning
30+
{
31+
void TrajOptDefaultSolverProfile::apply(trajopt::ProblemConstructionInfo& pci) const
32+
{
33+
pci.basic_info.convex_solver = convex_solver;
34+
pci.opt_info = opt_info;
35+
}
36+
37+
tinyxml2::XMLElement* TrajOptDefaultSolverProfile::toXML(tinyxml2::XMLDocument& /*doc*/) const { return nullptr; }
38+
39+
} // namespace tesseract_planning

‎tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt/trajopt_motion_planner.cpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -123,7 +123,7 @@ tesseract_common::StatusCode TrajOptMotionPlanner::solve(const PlannerRequest& r
123123

124124
try
125125
{
126-
problem = problem_generator(name_, request, plan_profiles, composite_profiles);
126+
problem = problem_generator(name_, request, plan_profiles, composite_profiles, solver_profiles);
127127
}
128128
catch (std::exception& e)
129129
{

0 commit comments

Comments
 (0)