Skip to content

Commit

Permalink
Initial planner implementation
Browse files Browse the repository at this point in the history
  • Loading branch information
@mpowelson
mpowelson committed Nov 13, 2020
1 parent 2b09f7e commit 5fec3c2
Show file tree
Hide file tree
Showing 2 changed files with 106 additions and 96 deletions.
11 changes: 6 additions & 5 deletions ...n_planners/include/tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_motion_planner.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,7 @@
#include <tesseract_common/macros.h>
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <ifopt/problem.h>
#include <trajopt_sqp/sqp_callback.h>
TESSERACT_COMMON_IGNORE_WARNINGS_POP

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

using TrajOptIfoptProblemGeneratorFn =
std::function<std::shared_ptr<ifopt::Problem>(const std::string&,
const PlannerRequest&,
const TrajOptIfoptPlanProfileMap&,
const TrajOptIfoptCompositeProfileMap&)>;
std::function<std::shared_ptr<TrajOptIfoptProblem>(const std::string&,
const PlannerRequest&,
const TrajOptIfoptPlanProfileMap&,
const TrajOptIfoptCompositeProfileMap&)>;

class TrajOptIfoptMotionPlanner : public MotionPlanner
{
Expand Down Expand Up @@ -79,7 +80,7 @@ class TrajOptIfoptMotionPlanner : public MotionPlanner
// sco::BasicTrustRegionSQPParameters params;

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

/**
* @brief Sets up the opimizer and solves a SQP problem read from json with no callbacks and dafault parameterss
Expand Down
191 changes: 100 additions & 91 deletions ...act_planning/tesseract_motion_planners/src/trajopt_ifopt/trajopt_ifopt_motion_planner.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,12 +28,14 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <console_bridge/console.h>
#include <tesseract_environment/core/utils.h>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <trajopt_sqp/trust_region_sqp_solver.h>
#include <trajopt_sqp/osqp_eigen_solver.h>
TESSERACT_COMMON_IGNORE_WARNINGS_POP

#include <tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_motion_planner.h>
#include <tesseract_command_language/command_language.h>
#include <tesseract_command_language/utils/utils.h>
//#include <tesseract_motion_planners/core/utils.h>
#include <tesseract_motion_planners/core/utils.h>

using namespace trajopt;

Expand Down Expand Up @@ -83,102 +85,109 @@ bool TrajOptIfoptMotionPlanner::terminate()
void TrajOptIfoptMotionPlanner::clear()
{
// params = sco::BasicTrustRegionSQPParameters();
// callbacks.clear();
callbacks.clear();
}

tesseract_common::StatusCode TrajOptIfoptMotionPlanner::solve(const PlannerRequest& request,
PlannerResponse& response,
bool verbose) const
{
// if (!checkUserInput(request))
// {
// response.status =
// tesseract_common::StatusCode(TrajOptIfoptMotionPlannerStatusCategory::ErrorInvalidInput, status_category_);
// return response.status;
// }
// std::shared_ptr<ifopt::Problem> problem;
// if (request.data)
// {
// problem = std::static_pointer_cast<trajopt::TrajOptProb>(request.data);
// }
// else
// {
// if (!problem_generator)
// {
// CONSOLE_BRIDGE_logError("TrajOptPlanner does not have a problem generator specified.");
// response.status =
// tesseract_common::StatusCode(TrajOptIfoptMotionPlannerStatusCategory::ErrorInvalidInput,
// status_category_);
// return response.status;
// }
// problem = problem_generator(name_, request, plan_profiles, composite_profiles);
// response.data = problem;
// }

// // Set Log Level
// if (verbose)
// util::gLogLevel = util::LevelInfo;
// else
// util::gLogLevel = util::LevelWarn;

// // Create optimizer
// sco::BasicTrustRegionSQP opt(problem);
// opt.setParameters(params);
// opt.initialize(trajToDblVec(problem->GetInitTraj()));

// // Add all callbacks
// for (const sco::Optimizer::Callback& callback : callbacks)
// {
// opt.addCallback(callback);
// }

// // Optimize
// auto tStart = boost::posix_time::second_clock::local_time();
// opt.optimize();
// CONSOLE_BRIDGE_logInform("planning time: %.3f", (boost::posix_time::second_clock::local_time() -
// tStart).seconds()); if (opt.results().status != sco::OptStatus::OPT_CONVERGED)
// {
// response.status =
// tesseract_common::StatusCode(TrajOptIfoptMotionPlannerStatusCategory::FailedToFindValidSolution,
// status_category_);
// return response.status;
// }

// // Get the results
// tesseract_common::TrajArray trajectory = getTraj(opt.x(), problem->GetVars());

// // Flatten the results to make them easier to process
// response.results = request.seed;
// auto results_flattened = flattenProgramToPattern(response.results, request.instructions);
// auto instructions_flattened = flattenProgram(request.instructions);

// // Loop over the flattened results and add them to response if the input was a plan instruction
// Eigen::Index result_index = 0;
// for (std::size_t idx = 0; idx < instructions_flattened.size(); idx++)
// {
// // If idx is zero then this should be the start instruction
// assert((idx == 0) ? isPlanInstruction(instructions_flattened.at(idx).get()) : true);
// assert((idx == 0) ? isMoveInstruction(results_flattened[idx].get()) : true);
// if (isPlanInstruction(instructions_flattened.at(idx).get()))
// {
// // This instruction corresponds to a composite. Set all results in that composite to the results
// const auto* plan_instruction = instructions_flattened.at(idx).get().cast_const<PlanInstruction>();
// if (plan_instruction->isStart())
// {
// assert(idx == 0);
// assert(isMoveInstruction(results_flattened[idx].get()));
// auto* move_instruction = results_flattened[idx].get().cast<MoveInstruction>();
// move_instruction->getWaypoint().cast<StateWaypoint>()->position = trajectory.row(result_index++);
// }
// else
// {
// auto* move_instructions = results_flattened[idx].get().cast<CompositeInstruction>();
// for (auto& instruction : *move_instructions)
// instruction.cast<MoveInstruction>()->getWaypoint().cast<StateWaypoint>()->position =
// trajectory.row(result_index++);
// }
// }
// }
if (!checkUserInput(request))
{
response.status =
tesseract_common::StatusCode(TrajOptIfoptMotionPlannerStatusCategory::ErrorInvalidInput, status_category_);
return response.status;
}
std::shared_ptr<TrajOptIfoptProblem> problem;
if (request.data)
{
problem = std::static_pointer_cast<TrajOptIfoptProblem>(request.data);
}
else
{
if (!problem_generator)
{
CONSOLE_BRIDGE_logError("TrajOptIfoptPlanner does not have a problem generator specified.");
response.status =
tesseract_common::StatusCode(TrajOptIfoptMotionPlannerStatusCategory::ErrorInvalidInput, status_category_);
return response.status;
}
problem = problem_generator(name_, request, plan_profiles, composite_profiles);
response.data = problem;
}

// Create optimizer
/** @todo Enable solver selection (e.g. IPOPT) */
auto qp_solver = std::make_shared<trajopt_sqp::OSQPEigenSolver>();
trajopt_sqp::TrustRegionSQPSolver solver(qp_solver);
/** @todo Set these as the defaults in trajopt and allow setting */
qp_solver->solver_.settings()->setVerbosity(verbose);
qp_solver->solver_.settings()->setWarmStart(true);
qp_solver->solver_.settings()->setPolish(true);
qp_solver->solver_.settings()->setAdaptiveRho(false);
qp_solver->solver_.settings()->setMaxIteraction(8192);
qp_solver->solver_.settings()->setAbsoluteTolerance(1e-4);
qp_solver->solver_.settings()->setRelativeTolerance(1e-6);

// Add all callbacks
for (const trajopt_sqp::SQPCallback::Ptr& callback : callbacks)
{
solver.registerCallback(callback);
}

// solve
solver.verbose = verbose;

auto tStart = boost::posix_time::second_clock::local_time();
solver.Solve(*(problem->nlp));
CONSOLE_BRIDGE_logInform("planning time: %.3f", (boost::posix_time::second_clock::local_time() - tStart).seconds());

// Check success
if (solver.getStatus() != trajopt_sqp::SQPStatus::NLP_CONVERGED)
{
response.status = tesseract_common::StatusCode(TrajOptIfoptMotionPlannerStatusCategory::FailedToFindValidSolution,
status_category_);
return response.status;
}

// Get the results - This can likely be simplified if we get rid of the traj array
Eigen::VectorXd x = problem->nlp->GetOptVariables()->GetValues();
Eigen::Map<trajopt::TrajArray> trajectory(x.data(),
static_cast<Eigen::Index>(problem->vars.size()),
static_cast<Eigen::Index>(problem->vars[0]->GetValues().size()));

// Flatten the results to make them easier to process
response.results = request.seed;
auto results_flattened = flattenProgramToPattern(response.results, request.instructions);
auto instructions_flattened = flattenProgram(request.instructions);

// Loop over the flattened results and add them to response if the input was a plan instruction
Eigen::Index result_index = 0;
for (std::size_t idx = 0; idx < instructions_flattened.size(); idx++)
{
// If idx is zero then this should be the start instruction
assert((idx == 0) ? isPlanInstruction(instructions_flattened.at(idx).get()) : true);
assert((idx == 0) ? isMoveInstruction(results_flattened[idx].get()) : true);
if (isPlanInstruction(instructions_flattened.at(idx).get()))
{
// This instruction corresponds to a composite. Set all results in that composite to the results
const auto* plan_instruction = instructions_flattened.at(idx).get().cast_const<PlanInstruction>();
if (plan_instruction->isStart())
{
assert(idx == 0);
assert(isMoveInstruction(results_flattened[idx].get()));
auto* move_instruction = results_flattened[idx].get().cast<MoveInstruction>();
move_instruction->getWaypoint().cast<StateWaypoint>()->position = trajectory.row(result_index++);
}
else
{
auto* move_instructions = results_flattened[idx].get().cast<CompositeInstruction>();
for (auto& instruction : *move_instructions)
instruction.cast<MoveInstruction>()->getWaypoint().cast<StateWaypoint>()->position =
trajectory.row(result_index++);
}
}
}

response.status =
tesseract_common::StatusCode(TrajOptIfoptMotionPlannerStatusCategory::SolutionFound, status_category_);
Expand Down

0 comments on commit 5fec3c2

Please sign in to comment.