Skip to content

Commit 5fec3c2

Browse files
mpowelsonmpowelson
committed
Initial planner implementation
1 parent 2b09f7e commit 5fec3c2

File tree

2 files changed

+106
-96
lines changed

2 files changed

+106
-96
lines changed

‎tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_motion_planner.h

+6-5
Original file line numberDiff line numberDiff line change
@@ -29,6 +29,7 @@
2929
#include <tesseract_common/macros.h>
3030
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
3131
#include <ifopt/problem.h>
32+
#include <trajopt_sqp/sqp_callback.h>
3233
TESSERACT_COMMON_IGNORE_WARNINGS_POP
3334

3435
#include <tesseract_motion_planners/core/planner.h>
@@ -39,10 +40,10 @@ namespace tesseract_planning
3940
class TrajOptIfoptMotionPlannerStatusCategory;
4041

4142
using TrajOptIfoptProblemGeneratorFn =
42-
std::function<std::shared_ptr<ifopt::Problem>(const std::string&,
43-
const PlannerRequest&,
44-
const TrajOptIfoptPlanProfileMap&,
45-
const TrajOptIfoptCompositeProfileMap&)>;
43+
std::function<std::shared_ptr<TrajOptIfoptProblem>(const std::string&,
44+
const PlannerRequest&,
45+
const TrajOptIfoptPlanProfileMap&,
46+
const TrajOptIfoptCompositeProfileMap&)>;
4647

4748
class TrajOptIfoptMotionPlanner : public MotionPlanner
4849
{
@@ -79,7 +80,7 @@ class TrajOptIfoptMotionPlanner : public MotionPlanner
7980
// sco::BasicTrustRegionSQPParameters params;
8081

8182
/** @brief Callback functions called on each iteration of the optimization (Optional) */
82-
// std::vector<sco::Optimizer::Callback> callbacks;
83+
std::vector<trajopt_sqp::SQPCallback::Ptr> callbacks;
8384

8485
/**
8586
* @brief Sets up the opimizer and solves a SQP problem read from json with no callbacks and dafault parameterss

‎tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt_ifopt/trajopt_ifopt_motion_planner.cpp

+100-91
Original file line numberDiff line numberDiff line change
@@ -28,12 +28,14 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
2828
#include <console_bridge/console.h>
2929
#include <tesseract_environment/core/utils.h>
3030
#include <boost/date_time/posix_time/posix_time.hpp>
31+
#include <trajopt_sqp/trust_region_sqp_solver.h>
32+
#include <trajopt_sqp/osqp_eigen_solver.h>
3133
TESSERACT_COMMON_IGNORE_WARNINGS_POP
3234

3335
#include <tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_motion_planner.h>
3436
#include <tesseract_command_language/command_language.h>
3537
#include <tesseract_command_language/utils/utils.h>
36-
//#include <tesseract_motion_planners/core/utils.h>
38+
#include <tesseract_motion_planners/core/utils.h>
3739

3840
using namespace trajopt;
3941

@@ -83,102 +85,109 @@ bool TrajOptIfoptMotionPlanner::terminate()
8385
void TrajOptIfoptMotionPlanner::clear()
8486
{
8587
// params = sco::BasicTrustRegionSQPParameters();
86-
// callbacks.clear();
88+
callbacks.clear();
8789
}
8890

8991
tesseract_common::StatusCode TrajOptIfoptMotionPlanner::solve(const PlannerRequest& request,
9092
PlannerResponse& response,
9193
bool verbose) const
9294
{
93-
// if (!checkUserInput(request))
94-
// {
95-
// response.status =
96-
// tesseract_common::StatusCode(TrajOptIfoptMotionPlannerStatusCategory::ErrorInvalidInput, status_category_);
97-
// return response.status;
98-
// }
99-
// std::shared_ptr<ifopt::Problem> problem;
100-
// if (request.data)
101-
// {
102-
// problem = std::static_pointer_cast<trajopt::TrajOptProb>(request.data);
103-
// }
104-
// else
105-
// {
106-
// if (!problem_generator)
107-
// {
108-
// CONSOLE_BRIDGE_logError("TrajOptPlanner does not have a problem generator specified.");
109-
// response.status =
110-
// tesseract_common::StatusCode(TrajOptIfoptMotionPlannerStatusCategory::ErrorInvalidInput,
111-
// status_category_);
112-
// return response.status;
113-
// }
114-
// problem = problem_generator(name_, request, plan_profiles, composite_profiles);
115-
// response.data = problem;
116-
// }
117-
118-
// // Set Log Level
119-
// if (verbose)
120-
// util::gLogLevel = util::LevelInfo;
121-
// else
122-
// util::gLogLevel = util::LevelWarn;
123-
124-
// // Create optimizer
125-
// sco::BasicTrustRegionSQP opt(problem);
126-
// opt.setParameters(params);
127-
// opt.initialize(trajToDblVec(problem->GetInitTraj()));
128-
129-
// // Add all callbacks
130-
// for (const sco::Optimizer::Callback& callback : callbacks)
131-
// {
132-
// opt.addCallback(callback);
133-
// }
134-
135-
// // Optimize
136-
// auto tStart = boost::posix_time::second_clock::local_time();
137-
// opt.optimize();
138-
// CONSOLE_BRIDGE_logInform("planning time: %.3f", (boost::posix_time::second_clock::local_time() -
139-
// tStart).seconds()); if (opt.results().status != sco::OptStatus::OPT_CONVERGED)
140-
// {
141-
// response.status =
142-
// tesseract_common::StatusCode(TrajOptIfoptMotionPlannerStatusCategory::FailedToFindValidSolution,
143-
// status_category_);
144-
// return response.status;
145-
// }
146-
147-
// // Get the results
148-
// tesseract_common::TrajArray trajectory = getTraj(opt.x(), problem->GetVars());
149-
150-
// // Flatten the results to make them easier to process
151-
// response.results = request.seed;
152-
// auto results_flattened = flattenProgramToPattern(response.results, request.instructions);
153-
// auto instructions_flattened = flattenProgram(request.instructions);
154-
155-
// // Loop over the flattened results and add them to response if the input was a plan instruction
156-
// Eigen::Index result_index = 0;
157-
// for (std::size_t idx = 0; idx < instructions_flattened.size(); idx++)
158-
// {
159-
// // If idx is zero then this should be the start instruction
160-
// assert((idx == 0) ? isPlanInstruction(instructions_flattened.at(idx).get()) : true);
161-
// assert((idx == 0) ? isMoveInstruction(results_flattened[idx].get()) : true);
162-
// if (isPlanInstruction(instructions_flattened.at(idx).get()))
163-
// {
164-
// // This instruction corresponds to a composite. Set all results in that composite to the results
165-
// const auto* plan_instruction = instructions_flattened.at(idx).get().cast_const<PlanInstruction>();
166-
// if (plan_instruction->isStart())
167-
// {
168-
// assert(idx == 0);
169-
// assert(isMoveInstruction(results_flattened[idx].get()));
170-
// auto* move_instruction = results_flattened[idx].get().cast<MoveInstruction>();
171-
// move_instruction->getWaypoint().cast<StateWaypoint>()->position = trajectory.row(result_index++);
172-
// }
173-
// else
174-
// {
175-
// auto* move_instructions = results_flattened[idx].get().cast<CompositeInstruction>();
176-
// for (auto& instruction : *move_instructions)
177-
// instruction.cast<MoveInstruction>()->getWaypoint().cast<StateWaypoint>()->position =
178-
// trajectory.row(result_index++);
179-
// }
180-
// }
181-
// }
95+
if (!checkUserInput(request))
96+
{
97+
response.status =
98+
tesseract_common::StatusCode(TrajOptIfoptMotionPlannerStatusCategory::ErrorInvalidInput, status_category_);
99+
return response.status;
100+
}
101+
std::shared_ptr<TrajOptIfoptProblem> problem;
102+
if (request.data)
103+
{
104+
problem = std::static_pointer_cast<TrajOptIfoptProblem>(request.data);
105+
}
106+
else
107+
{
108+
if (!problem_generator)
109+
{
110+
CONSOLE_BRIDGE_logError("TrajOptIfoptPlanner does not have a problem generator specified.");
111+
response.status =
112+
tesseract_common::StatusCode(TrajOptIfoptMotionPlannerStatusCategory::ErrorInvalidInput, status_category_);
113+
return response.status;
114+
}
115+
problem = problem_generator(name_, request, plan_profiles, composite_profiles);
116+
response.data = problem;
117+
}
118+
119+
// Create optimizer
120+
/** @todo Enable solver selection (e.g. IPOPT) */
121+
auto qp_solver = std::make_shared<trajopt_sqp::OSQPEigenSolver>();
122+
trajopt_sqp::TrustRegionSQPSolver solver(qp_solver);
123+
/** @todo Set these as the defaults in trajopt and allow setting */
124+
qp_solver->solver_.settings()->setVerbosity(verbose);
125+
qp_solver->solver_.settings()->setWarmStart(true);
126+
qp_solver->solver_.settings()->setPolish(true);
127+
qp_solver->solver_.settings()->setAdaptiveRho(false);
128+
qp_solver->solver_.settings()->setMaxIteraction(8192);
129+
qp_solver->solver_.settings()->setAbsoluteTolerance(1e-4);
130+
qp_solver->solver_.settings()->setRelativeTolerance(1e-6);
131+
132+
// Add all callbacks
133+
for (const trajopt_sqp::SQPCallback::Ptr& callback : callbacks)
134+
{
135+
solver.registerCallback(callback);
136+
}
137+
138+
// solve
139+
solver.verbose = verbose;
140+
141+
auto tStart = boost::posix_time::second_clock::local_time();
142+
solver.Solve(*(problem->nlp));
143+
CONSOLE_BRIDGE_logInform("planning time: %.3f", (boost::posix_time::second_clock::local_time() - tStart).seconds());
144+
145+
// Check success
146+
if (solver.getStatus() != trajopt_sqp::SQPStatus::NLP_CONVERGED)
147+
{
148+
response.status = tesseract_common::StatusCode(TrajOptIfoptMotionPlannerStatusCategory::FailedToFindValidSolution,
149+
status_category_);
150+
return response.status;
151+
}
152+
153+
// Get the results - This can likely be simplified if we get rid of the traj array
154+
Eigen::VectorXd x = problem->nlp->GetOptVariables()->GetValues();
155+
Eigen::Map<trajopt::TrajArray> trajectory(x.data(),
156+
static_cast<Eigen::Index>(problem->vars.size()),
157+
static_cast<Eigen::Index>(problem->vars[0]->GetValues().size()));
158+
159+
// Flatten the results to make them easier to process
160+
response.results = request.seed;
161+
auto results_flattened = flattenProgramToPattern(response.results, request.instructions);
162+
auto instructions_flattened = flattenProgram(request.instructions);
163+
164+
// Loop over the flattened results and add them to response if the input was a plan instruction
165+
Eigen::Index result_index = 0;
166+
for (std::size_t idx = 0; idx < instructions_flattened.size(); idx++)
167+
{
168+
// If idx is zero then this should be the start instruction
169+
assert((idx == 0) ? isPlanInstruction(instructions_flattened.at(idx).get()) : true);
170+
assert((idx == 0) ? isMoveInstruction(results_flattened[idx].get()) : true);
171+
if (isPlanInstruction(instructions_flattened.at(idx).get()))
172+
{
173+
// This instruction corresponds to a composite. Set all results in that composite to the results
174+
const auto* plan_instruction = instructions_flattened.at(idx).get().cast_const<PlanInstruction>();
175+
if (plan_instruction->isStart())
176+
{
177+
assert(idx == 0);
178+
assert(isMoveInstruction(results_flattened[idx].get()));
179+
auto* move_instruction = results_flattened[idx].get().cast<MoveInstruction>();
180+
move_instruction->getWaypoint().cast<StateWaypoint>()->position = trajectory.row(result_index++);
181+
}
182+
else
183+
{
184+
auto* move_instructions = results_flattened[idx].get().cast<CompositeInstruction>();
185+
for (auto& instruction : *move_instructions)
186+
instruction.cast<MoveInstruction>()->getWaypoint().cast<StateWaypoint>()->position =
187+
trajectory.row(result_index++);
188+
}
189+
}
190+
}
182191

183192
response.status =
184193
tesseract_common::StatusCode(TrajOptIfoptMotionPlannerStatusCategory::SolutionFound, status_category_);

0 commit comments

Comments
 (0)