Skip to content

Commit

Permalink
New TrajOpt IFOPT profiles
Browse files Browse the repository at this point in the history
  • Loading branch information
Levi-Armstrong committed Dec 28, 2024
1 parent ad151fb commit 66d144f
Show file tree
Hide file tree
Showing 20 changed files with 284 additions and 306 deletions.
10 changes: 5 additions & 5 deletions tesseract_examples/src/car_seat_example.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -69,7 +69,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
#include <tesseract_motion_planners/trajopt/profile/trajopt_osqp_solver_profile.h>
#include <tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_composite_profile.h>
#include <tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_plan_profile.h>
#include <tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_solver_profile.h>
#include <tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_osqp_solver_profile.h>
#include <tesseract_motion_planners/core/utils.h>

#include <tesseract_visualization/visualization.h>
Expand Down Expand Up @@ -306,10 +306,10 @@ bool CarSeatExample::run()
trajopt_ifopt_plan_profile->cartesian_coeff = Eigen::VectorXd::Ones(6);
trajopt_ifopt_plan_profile->joint_coeff = Eigen::VectorXd::Ones(8);

auto trajopt_ifopt_solver_profile = std::make_shared<TrajOptIfoptDefaultSolverProfile>();
trajopt_ifopt_solver_profile->opt_info.max_iterations = 200;
trajopt_ifopt_solver_profile->opt_info.min_approx_improve = 1e-3;
trajopt_ifopt_solver_profile->opt_info.min_trust_box_size = 1e-3;
auto trajopt_ifopt_solver_profile = std::make_shared<TrajOptIfoptOSQPSolverProfile>();
trajopt_ifopt_solver_profile->opt_params.max_iterations = 200;
trajopt_ifopt_solver_profile->opt_params.min_approx_improve = 1e-3;
trajopt_ifopt_solver_profile->opt_params.min_trust_box_size = 1e-3;

profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_ifopt_composite_profile);
profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_ifopt_plan_profile);
Expand Down
6 changes: 3 additions & 3 deletions tesseract_examples/src/pick_and_place_example.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
#include <tesseract_motion_planners/trajopt/profile/trajopt_osqp_solver_profile.h>
#include <tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_composite_profile.h>
#include <tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_plan_profile.h>
#include <tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_solver_profile.h>
#include <tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_osqp_solver_profile.h>
#include <tesseract_motion_planners/core/utils.h>

#include <tesseract_command_language/profile_dictionary.h>
Expand Down Expand Up @@ -255,8 +255,8 @@ bool PickAndPlaceExample::run()
trajopt_ifopt_composite_profile->jerk_coeff = Eigen::VectorXd::Ones(1);
trajopt_ifopt_composite_profile->longest_valid_segment_length = 0.05;

auto trajopt_ifopt_solver_profile = std::make_shared<TrajOptIfoptDefaultSolverProfile>();
trajopt_ifopt_solver_profile->opt_info.max_iterations = 100;
auto trajopt_ifopt_solver_profile = std::make_shared<TrajOptIfoptOSQPSolverProfile>();
trajopt_ifopt_solver_profile->opt_params.max_iterations = 100;

profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_ifopt_plan_profile);
profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_ifopt_composite_profile);
Expand Down
10 changes: 5 additions & 5 deletions tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -56,7 +56,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
#include <tesseract_motion_planners/trajopt/profile/trajopt_osqp_solver_profile.h>
#include <tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_composite_profile.h>
#include <tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_plan_profile.h>
#include <tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_solver_profile.h>
#include <tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_osqp_solver_profile.h>
#include <tesseract_motion_planners/core/utils.h>

#include <tesseract_task_composer/core/task_composer_context.h>
Expand Down Expand Up @@ -251,11 +251,11 @@ bool PuzzlePieceAuxillaryAxesExample::run()
trajopt_ifopt_composite_profile->smooth_jerks = true;
trajopt_ifopt_composite_profile->jerk_coeff = Eigen::VectorXd::Ones(1);

auto trajopt_ifopt_solver_profile = std::make_shared<TrajOptIfoptDefaultSolverProfile>();
auto trajopt_ifopt_solver_profile = std::make_shared<TrajOptIfoptOSQPSolverProfile>();
// trajopt_ifopt_solver_profile->convex_solver_settings.adaptive_rho = 0;
trajopt_ifopt_solver_profile->opt_info.max_iterations = 200;
trajopt_ifopt_solver_profile->opt_info.min_approx_improve = 1e-3;
trajopt_ifopt_solver_profile->opt_info.min_trust_box_size = 1e-3;
trajopt_ifopt_solver_profile->opt_params.max_iterations = 200;
trajopt_ifopt_solver_profile->opt_params.min_approx_improve = 1e-3;
trajopt_ifopt_solver_profile->opt_params.min_trust_box_size = 1e-3;

profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_ifopt_plan_profile);
profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_ifopt_composite_profile);
Expand Down
10 changes: 5 additions & 5 deletions tesseract_examples/src/puzzle_piece_example.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -56,7 +56,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
#include <tesseract_motion_planners/trajopt/profile/trajopt_osqp_solver_profile.h>
#include <tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_composite_profile.h>
#include <tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_plan_profile.h>
#include <tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_solver_profile.h>
#include <tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_osqp_solver_profile.h>
#include <tesseract_motion_planners/core/utils.h>

#include <tesseract_task_composer/core/task_composer_context.h>
Expand Down Expand Up @@ -242,10 +242,10 @@ bool PuzzlePieceExample::run()
trajopt_ifopt_composite_profile->smooth_jerks = true;
trajopt_ifopt_composite_profile->jerk_coeff = Eigen::VectorXd::Ones(1);

auto trajopt_ifopt_solver_profile = std::make_shared<TrajOptIfoptDefaultSolverProfile>();
trajopt_ifopt_solver_profile->opt_info.max_iterations = 200;
trajopt_ifopt_solver_profile->opt_info.min_approx_improve = 1e-3;
trajopt_ifopt_solver_profile->opt_info.min_trust_box_size = 1e-3;
auto trajopt_ifopt_solver_profile = std::make_shared<TrajOptIfoptOSQPSolverProfile>();
trajopt_ifopt_solver_profile->opt_params.max_iterations = 200;
trajopt_ifopt_solver_profile->opt_params.min_approx_improve = 1e-3;
trajopt_ifopt_solver_profile->opt_params.min_trust_box_size = 1e-3;

profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_ifopt_plan_profile);
profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_ifopt_composite_profile);
Expand Down
2 changes: 1 addition & 1 deletion tesseract_motion_planners/CHANGELOG.rst
Original file line number Diff line number Diff line change
Expand Up @@ -68,7 +68,7 @@ Changelog for package tesseract_motion_planners
* More compact descartes collision logging output (`#460 <https://github.com/tesseract-robotics/tesseract_planning/issues/460>`_)
* Better debugging feedback on failed Descartes plan (`#401 <https://github.com/tesseract-robotics/tesseract_planning/issues/401>`_)
Co-authored-by: Levi Armstrong <[email protected]>
* Add convex_solver_settings to TrajOptIfoptDefaultSolverProfile (`#425 <https://github.com/tesseract-robotics/tesseract_planning/issues/425>`_)
* Add convex_solver_settings to TrajOptIfoptOSQPSolverProfile (`#425 <https://github.com/tesseract-robotics/tesseract_planning/issues/425>`_)
Co-authored-by: Levi Armstrong <[email protected]>
* Fix descartes planner check if the graph built
* Add time parameterization interface (`#455 <https://github.com/tesseract-robotics/tesseract_planning/issues/455>`_)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,9 @@ namespace boost::serialization
template <class Archive>
void serialize(Archive& ar, OSQPSettings& settings, const unsigned int version); // NOLINT

template <class Archive>
void serialize(Archive& ar, sco::BasicTrustRegionSQPParameters& params, const unsigned int version); // NOLINT

} // namespace boost::serialization

namespace tesseract_planning
Expand Down
3 changes: 1 addition & 2 deletions tesseract_motion_planners/trajopt_ifopt/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -5,12 +5,11 @@ find_package(trajopt_sqp REQUIRED)
add_library(
${PROJECT_NAME}_trajopt_ifopt SHARED
src/trajopt_ifopt_motion_planner.cpp
src/trajopt_ifopt_problem.cpp
src/trajopt_ifopt_utils.cpp
src/profile/trajopt_ifopt_profile.cpp
src/profile/trajopt_ifopt_default_plan_profile.cpp
src/profile/trajopt_ifopt_default_composite_profile.cpp
src/profile/trajopt_ifopt_default_solver_profile.cpp)
src/profile/trajopt_ifopt_osqp_solver_profile.cpp)
target_link_libraries(
${PROJECT_NAME}_trajopt_ifopt
PUBLIC ${PROJECT_NAME}_core
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,7 @@ class TrajOptIfoptPlanProfile;
class TrajOptIfoptCompositeProfile;
class TrajOptIfoptSolverProfile;

class TrajOptIfoptDefaultSolverProfile;
class TrajOptIfoptOSQPSolverProfile;
class TrajOptIfoptDefaultPlanProfile;
class TrajOptIfoptDefaultCompositeProfile;
} // namespace tesseract_planning
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -89,8 +89,6 @@ class TrajOptIfoptDefaultCompositeProfile : public TrajOptIfoptCompositeProfile
const std::vector<std::string>& active_links,
const std::vector<int>& fixed_indices) const override;

tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override;

protected:
friend class boost::serialization::access;
template <class Archive>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -64,8 +64,6 @@ class TrajOptIfoptDefaultPlanProfile : public TrajOptIfoptPlanProfile
const std::vector<std::string>& active_links,
int index) const override;

tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override;

protected:
friend class boost::serialization::access;
template <class Archive>
Expand Down

This file was deleted.

Original file line number Diff line number Diff line change
@@ -0,0 +1,89 @@
/**
* @file trajopt_osqp_solver_profile.h
* @brief
*
* @author Levi Armstrong
* @date December 13, 2020
* @version TODO
* @bug No known bugs
*
* @copyright Copyright (c) 2020, Southwest Research Institute
*
* @par License
* Software License Agreement (Apache License)
* @par
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
* http://www.apache.org/licenses/LICENSE-2.0
* @par
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_OSQP_SOLVER_PROFILE_H
#define TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_OSQP_SOLVER_PROFILE_H

#include <tesseract_common/macros.h>
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <memory>
#include <trajopt_sqp/fwd.h>
#include <trajopt_sqp/types.h>
TESSERACT_COMMON_IGNORE_WARNINGS_POP

#include <tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_profile.h>

namespace OsqpEigen
{
class Settings;
}

namespace boost::serialization
{
template <class Archive>
void serialize(Archive& ar, OsqpEigen::Settings& osqp_eigen_settings, const unsigned int version); // NOLINT

template <class Archive>
void serialize(Archive& ar, trajopt_sqp::SQPParameters& params, const unsigned int version); // NOLINT

} // namespace boost::serialization

namespace tesseract_planning
{
/** @brief The contains the default solver parameters available for setting up TrajOpt */
class TrajOptIfoptOSQPSolverProfile : public TrajOptIfoptSolverProfile
{
public:
using Ptr = std::shared_ptr<TrajOptIfoptOSQPSolverProfile>;
using ConstPtr = std::shared_ptr<const TrajOptIfoptOSQPSolverProfile>;

TrajOptIfoptOSQPSolverProfile();
~TrajOptIfoptOSQPSolverProfile() override;
TrajOptIfoptOSQPSolverProfile(const TrajOptIfoptOSQPSolverProfile&) = delete;
TrajOptIfoptOSQPSolverProfile& operator=(const TrajOptIfoptOSQPSolverProfile&) = delete;
TrajOptIfoptOSQPSolverProfile(TrajOptIfoptOSQPSolverProfile&&) = delete;
TrajOptIfoptOSQPSolverProfile&& operator=(TrajOptIfoptOSQPSolverProfile&&) = delete;

/** @brief The OSQP convex solver settings to use */
std::unique_ptr<OsqpEigen::Settings> qp_settings{ nullptr };

/** @brief Optimization parameters */
trajopt_sqp::SQPParameters opt_params{};

std::unique_ptr<trajopt_sqp::TrustRegionSQPSolver> create(bool verbose = false) const override;

protected:
friend class boost::serialization::access;
template <class Archive>
void serialize(Archive&, const unsigned int); // NOLINT

/** @brief Optimization callbacks */
virtual std::vector<std::shared_ptr<trajopt_sqp::SQPCallback>> createOptimizationCallbacks() const;
};
} // namespace tesseract_planning

BOOST_CLASS_EXPORT_KEY(tesseract_planning::TrajOptIfoptOSQPSolverProfile)

#endif // TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_OSQP_SOLVER_PROFILE_H
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <vector>
#include <memory>
#include <trajopt_ifopt/fwd.h>
#include <trajopt_sqp/fwd.h>
TESSERACT_COMMON_IGNORE_WARNINGS_POP

#include <tesseract_common/fwd.h>
Expand Down Expand Up @@ -75,8 +76,6 @@ class TrajOptIfoptPlanProfile : public Profile
const std::vector<std::string>& active_links,
int index) const = 0;

virtual tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const = 0;

protected:
friend class boost::serialization::access;
template <class Archive>
Expand Down Expand Up @@ -104,8 +103,6 @@ class TrajOptIfoptCompositeProfile : public Profile
const std::vector<std::string>& active_links,
const std::vector<int>& fixed_indices) const = 0;

virtual tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const = 0;

protected:
friend class boost::serialization::access;
template <class Archive>
Expand All @@ -120,16 +117,14 @@ class TrajOptIfoptSolverProfile : public Profile

TrajOptIfoptSolverProfile();

virtual std::unique_ptr<trajopt_sqp::TrustRegionSQPSolver> create(bool verbose = false) const = 0;

/**
* @brief A utility function for getting profile ID
* @return The profile ID used when storing in profile dictionary
*/
static std::size_t getStaticKey();

virtual void apply(TrajOptIfoptProblem& problem) const = 0;

virtual tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const = 0;

protected:
friend class boost::serialization::access;
template <class Archive>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -60,24 +60,13 @@ struct TrajOptIfoptProblem
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
// LCOV_EXCL_STOP

TrajOptIfoptProblem();

trajopt_sqp::SQPParameters opt_info;

// These are required for Tesseract to configure Descartes
std::shared_ptr<const tesseract_environment::Environment> environment;
tesseract_scene_graph::SceneState env_state;

// Kinematic Objects
std::shared_ptr<const tesseract_kinematics::JointGroup> manip;

std::vector<std::shared_ptr<trajopt_sqp::SQPCallback>> callbacks;

/** @brief The OSQP convex solver settings to use */
std::unique_ptr<OsqpEigen::Settings> convex_solver_settings{ nullptr };

std::shared_ptr<trajopt_sqp::QPSolver> qp_solver;

std::shared_ptr<trajopt_sqp::QPProblem> nlp;
std::vector<std::shared_ptr<const trajopt_ifopt::JointPosition>> vars;
};
Expand Down
Loading

0 comments on commit 66d144f

Please sign in to comment.