Skip to content

Commit

Permalink
Add new TrajOpt Profiles
Browse files Browse the repository at this point in the history
  • Loading branch information
@Levi-Armstrong
Levi-Armstrong committed Dec 24, 2024
1 parent e00ae6f commit 7bd8ecc
Show file tree
Hide file tree
Showing 29 changed files with 660 additions and 2,026 deletions.
10 changes: 5 additions & 5 deletions tesseract_examples/src/car_seat_example.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -66,7 +66,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
#include <tesseract_task_composer/core/task_composer_plugin_factory.h>

#include <tesseract_motion_planners/trajopt/profile/trajopt_default_composite_profile.h>
#include <tesseract_motion_planners/trajopt/profile/trajopt_default_solver_profile.h>
#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>
Expand Down Expand Up @@ -327,10 +327,10 @@ bool CarSeatExample::run()
trajopt_composite_profile->collision_cost_config.safety_margin_buffer = 0.01;
trajopt_composite_profile->collision_cost_config.coeff = 50;

auto trajopt_solver_profile = std::make_shared<TrajOptDefaultSolverProfile>();
trajopt_solver_profile->opt_info.max_iter = 200;
trajopt_solver_profile->opt_info.min_approx_improve = 1e-3;
trajopt_solver_profile->opt_info.min_trust_box_size = 1e-3;
auto trajopt_solver_profile = std::make_shared<TrajOptOSQPSolverProfile>();
trajopt_solver_profile->opt_params.max_iter = 200;
trajopt_solver_profile->opt_params.min_approx_improve = 1e-3;
trajopt_solver_profile->opt_params.min_trust_box_size = 1e-3;

profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_composite_profile);
profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_solver_profile);
Expand Down
6 changes: 3 additions & 3 deletions tesseract_examples/src/pick_and_place_example.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,7 +44,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP

#include <tesseract_motion_planners/trajopt/profile/trajopt_default_plan_profile.h>
#include <tesseract_motion_planners/trajopt/profile/trajopt_default_composite_profile.h>
#include <tesseract_motion_planners/trajopt/profile/trajopt_default_solver_profile.h>
#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>
Expand Down Expand Up @@ -280,8 +280,8 @@ bool PickAndPlaceExample::run()
trajopt_composite_profile->collision_cost_config.safety_margin_buffer = 0.01;
trajopt_composite_profile->collision_cost_config.coeff = 50;

auto trajopt_solver_profile = std::make_shared<TrajOptDefaultSolverProfile>();
trajopt_solver_profile->opt_info.max_iter = 100;
auto trajopt_solver_profile = std::make_shared<TrajOptOSQPSolverProfile>();
trajopt_solver_profile->opt_params.max_iter = 100;

profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_plan_profile);
profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_composite_profile);
Expand Down
15 changes: 6 additions & 9 deletions tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,7 +53,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP

#include <tesseract_motion_planners/trajopt/profile/trajopt_default_plan_profile.h>
#include <tesseract_motion_planners/trajopt/profile/trajopt_default_composite_profile.h>
#include <tesseract_motion_planners/trajopt/profile/trajopt_default_solver_profile.h>
#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>
Expand Down Expand Up @@ -279,14 +279,11 @@ bool PuzzlePieceAuxillaryAxesExample::run()
trajopt_composite_profile->collision_cost_config.type = trajopt::CollisionEvaluatorType::SINGLE_TIMESTEP;
trajopt_composite_profile->collision_cost_config.coeff = 1;

auto trajopt_solver_profile = std::make_shared<TrajOptDefaultSolverProfile>();
trajopt_solver_profile->convex_solver = sco::ModelType::OSQP;
auto convex_solver_config = std::make_shared<sco::OSQPModelConfig>();
convex_solver_config->settings.adaptive_rho = 0;
trajopt_solver_profile->convex_solver_config = convex_solver_config;
trajopt_solver_profile->opt_info.max_iter = 200;
trajopt_solver_profile->opt_info.min_approx_improve = 1e-3;
trajopt_solver_profile->opt_info.min_trust_box_size = 1e-3;
auto trajopt_solver_profile = std::make_shared<TrajOptOSQPSolverProfile>();
trajopt_solver_profile->settings.adaptive_rho = 0;
trajopt_solver_profile->opt_params.max_iter = 200;
trajopt_solver_profile->opt_params.min_approx_improve = 1e-3;
trajopt_solver_profile->opt_params.min_trust_box_size = 1e-3;

profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_plan_profile);
profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_composite_profile);
Expand Down
13 changes: 6 additions & 7 deletions tesseract_examples/src/puzzle_piece_example.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,7 +53,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP

#include <tesseract_motion_planners/trajopt/profile/trajopt_default_plan_profile.h>
#include <tesseract_motion_planners/trajopt/profile/trajopt_default_composite_profile.h>
#include <tesseract_motion_planners/trajopt/profile/trajopt_default_solver_profile.h>
#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>
Expand Down Expand Up @@ -267,12 +267,11 @@ bool PuzzlePieceExample::run()
trajopt_composite_profile->collision_cost_config.type = trajopt::CollisionEvaluatorType::SINGLE_TIMESTEP;
trajopt_composite_profile->collision_cost_config.coeff = 20;

auto trajopt_solver_profile = std::make_shared<TrajOptDefaultSolverProfile>();
trajopt_solver_profile->convex_solver = sco::ModelType::OSQP;
trajopt_solver_profile->opt_info.num_threads = 0;
trajopt_solver_profile->opt_info.max_iter = 200;
trajopt_solver_profile->opt_info.min_approx_improve = 1e-3;
trajopt_solver_profile->opt_info.min_trust_box_size = 1e-3;
auto trajopt_solver_profile = std::make_shared<TrajOptOSQPSolverProfile>();
trajopt_solver_profile->opt_params.num_threads = 0;
trajopt_solver_profile->opt_params.max_iter = 200;
trajopt_solver_profile->opt_params.min_approx_improve = 1e-3;
trajopt_solver_profile->opt_params.min_trust_box_size = 1e-3;

profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_plan_profile);
profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_composite_profile);
Expand Down
1 change: 1 addition & 0 deletions tesseract_motion_planners/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@ find_package(Boost REQUIRED)
find_package(Eigen3 REQUIRED)
find_package(Threads REQUIRED)
find_package(tesseract_common REQUIRED)
find_package(tesseract_kinematics REQUIRED)
find_package(tesseract_environment REQUIRED)
find_package(tesseract_command_language REQUIRED)

Expand Down
16 changes: 11 additions & 5 deletions tesseract_motion_planners/trajopt/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
find_package(trajopt REQUIRED)
find_package(trajopt_sco REQUIRED)
find_package(osqp REQUIRED)

# Trajopt Planner
add_library(
Expand All @@ -11,18 +12,19 @@ add_library(
src/profile/trajopt_profile.cpp
src/profile/trajopt_default_plan_profile.cpp
src/profile/trajopt_default_composite_profile.cpp
src/profile/trajopt_default_solver_profile.cpp
src/serialize.cpp
src/deserialize.cpp)
src/profile/trajopt_osqp_solver_profile.cpp)
target_link_libraries(
${PROJECT_NAME}_trajopt
PUBLIC ${PROJECT_NAME}_core
${PROJECT_NAME}_simple
trajopt::trajopt
trajopt::trajopt_common
trajopt::trajopt_sco
tesseract::tesseract_kinematics_core
tesseract::tesseract_environment
Boost::boost
Eigen3::Eigen)
Eigen3::Eigen
osqp::osqp)
target_compile_options(${PROJECT_NAME}_trajopt PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE})
target_compile_options(${PROJECT_NAME}_trajopt PUBLIC ${TESSERACT_COMPILE_OPTIONS_PUBLIC})
target_compile_definitions(${PROJECT_NAME}_trajopt PUBLIC ${TESSERACT_COMPILE_DEFINITIONS})
Expand Down Expand Up @@ -50,7 +52,11 @@ configure_component(
COMPONENT trajopt
NAMESPACE tesseract
TARGETS ${PROJECT_NAME}_trajopt
DEPENDENCIES "tesseract_motion_planners COMPONENTS core simple" trajopt trajopt_sco)
DEPENDENCIES
"tesseract_motion_planners COMPONENTS core simple"
trajopt
trajopt_sco
osqp)

if(TESSERACT_PACKAGE)
cpack_component(
Expand Down
67 changes: 0 additions & 67 deletions tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/deserialize.h
View file

This file was deleted.

2 changes: 1 addition & 1 deletion tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/fwd.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@ class TrajOptPlanProfile;
class TrajOptCompositeProfile;
class TrajOptSolverProfile;

class TrajOptDefaultSolverProfile;
class TrajOptOSQPSolverProfile;
class TrajOptDefaultPlanProfile;
class TrajOptDefaultCompositeProfile;
} // namespace tesseract_planning
Expand Down
55 changes: 6 additions & 49 deletions ...opt/include/tesseract_motion_planners/trajopt/profile/trajopt_default_composite_profile.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,7 +51,6 @@ class TrajOptDefaultCompositeProfile : public TrajOptCompositeProfile
using ConstPtr = std::shared_ptr<const TrajOptDefaultCompositeProfile>;

TrajOptDefaultCompositeProfile() = default;
TrajOptDefaultCompositeProfile(const tinyxml2::XMLElement& xml_element);

/** @brief The type of contact test to perform: FIRST, CLOSEST, ALL */
tesseract_collision::ContactTestType contact_test_type{ tesseract_collision::ContactTestType::ALL };
Expand Down Expand Up @@ -98,56 +97,14 @@ class TrajOptDefaultCompositeProfile : public TrajOptCompositeProfile
/**@brief Special link collision constraint distances */
std::shared_ptr<trajopt_common::SafetyMarginData> special_collision_constraint{ nullptr };

void apply(trajopt::ProblemConstructionInfo& pci,
int start_index,
int end_index,
const tesseract_common::ManipulatorInfo& manip_info,
const std::vector<std::string>& active_links,
const std::vector<int>& fixed_indices) const override;

tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override;
TrajOptTermInfos createCostAndConstraints(const tesseract_common::ManipulatorInfo& composite_manip_info,
const std::shared_ptr<const tesseract_environment::Environment>& env,
const std::vector<int>& fixed_indices,
int start_index,
int end_index) const override;

protected:
void addCollisionCost(trajopt::ProblemConstructionInfo& pci,
int start_index,
int end_index,
const std::vector<int>& fixed_indices) const;

void addCollisionConstraint(trajopt::ProblemConstructionInfo& pci,
int start_index,
int end_index,
const std::vector<int>& fixed_indices) const;

void addVelocitySmoothing(trajopt::ProblemConstructionInfo& pci,
int start_index,
int end_index,
const std::vector<int>& fixed_indices) const;

void addAccelerationSmoothing(trajopt::ProblemConstructionInfo& pci,
int start_index,
int end_index,
const std::vector<int>& fixed_indices) const;

void addJerkSmoothing(trajopt::ProblemConstructionInfo& pci,
int start_index,
int end_index,
const std::vector<int>& fixed_indices) const;

void addConstraintErrorFunctions(trajopt::ProblemConstructionInfo& pci,
int start_index,
int end_index,
const std::vector<int>& fixed_indices) const;

void addAvoidSingularity(trajopt::ProblemConstructionInfo& pci,
int start_index,
int end_index,
const std::string& link,
const std::vector<int>& fixed_indices) const;

static void smoothMotionTerms(const tinyxml2::XMLElement& xml_element,
bool& enabled,
Eigen::VectorXd& coeff,
std::size_t& length);
double computeLongestValidSegmentLength(const Eigen::MatrixX2d joint_limits) const;

friend class boost::serialization::access;
template <class Archive>
Expand Down
40 changes: 15 additions & 25 deletions .../trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_default_plan_profile.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,14 +45,21 @@ class TrajOptDefaultPlanProfile : public TrajOptPlanProfile
using ConstPtr = std::shared_ptr<const TrajOptDefaultPlanProfile>;

TrajOptDefaultPlanProfile() = default;
TrajOptDefaultPlanProfile(const tinyxml2::XMLElement& xml_element);

CartesianWaypointConfig cartesian_cost_config;
CartesianWaypointConfig cartesian_constraint_config;
JointWaypointConfig joint_cost_config;
JointWaypointConfig joint_constraint_config;

/** @brief Error function that is set as a constraint for each timestep.
TrajOptWaypointInfo createCostAndConstraints(const MoveInstructionPoly& move_instruction,
const tesseract_common::ManipulatorInfo& composite_manip_info,
const std::shared_ptr<const tesseract_environment::Environment>& env,
const std::vector<std::string>& active_links,
int index) const override;

protected:
/**
* @brief Error function that is set as a constraint for each timestep.
*
* This is a vector of std::tuple<Error Function, Error Function Jacobian, Constraint Type, Coeff>, the error
* function, constraint type, and coeff is required, but the jacobian is optional (nullptr).
Expand All @@ -70,29 +77,12 @@ class TrajOptDefaultPlanProfile : public TrajOptPlanProfile
* Coefficients/Weights
*
*/
std::vector<std::tuple<sco::VectorOfVector::func, sco::MatrixOfVector::func, sco::ConstraintType, Eigen::VectorXd>>
constraint_error_functions;

void apply(trajopt::ProblemConstructionInfo& pci,
const CartesianWaypointPoly& cartesian_waypoint,
const MoveInstructionPoly& parent_instruction,
const tesseract_common::ManipulatorInfo& manip_info,
const std::vector<std::string>& active_links,
int index) const override;

void apply(trajopt::ProblemConstructionInfo& pci,
const JointWaypointPoly& joint_waypoint,
const MoveInstructionPoly& parent_instruction,
const tesseract_common::ManipulatorInfo& manip_info,
const std::vector<std::string>& active_links,
int index) const override;

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

protected:
void addConstraintErrorFunctions(trajopt::ProblemConstructionInfo& pci, int index) const;

void addAvoidSingularity(trajopt::ProblemConstructionInfo& pci, const std::vector<int>& fixed_steps) const;
static std::shared_ptr<trajopt::TermInfo>
createConstraintFromErrorFunction(sco::VectorOfVector::func error_function,
sco::MatrixOfVector::func jacobian_function,
sco::ConstraintType type,
const Eigen::VectorXd& coeff,
int index);

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

0 comments on commit 7bd8ecc

Please sign in to comment.