Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add TrajOpt Ifopt planner #443

Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension


Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions .github/workflows/windows_noetic_build.yml
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,6 @@ jobs:

rosdep install -q --from-paths . --ignore-src -y

catkin_make_isolated --install --force-cmake --only-pkg-with-deps tesseract_collision tesseract_common tesseract_environment tesseract_geometry tesseract_kinematics tesseract_scene_graph tesseract_support tesseract_urdf tesseract_visualization tesseract_motion_planners tesseract_process_managers tesseract_time_parameterization tesseract_python --cmake-args -DCMAKE_BUILD_TYPE=Release -DINSTALL_OMPL=ON -DINSTALL_OMPL_TAG=master
catkin_make_isolated --install --force-cmake --only-pkg-with-deps tesseract_collision tesseract_common tesseract_environment tesseract_geometry tesseract_kinematics tesseract_scene_graph tesseract_support tesseract_urdf tesseract_visualization tesseract_motion_planners tesseract_process_managers tesseract_time_parameterization tesseract_python --cmake-args -DCMAKE_BUILD_TYPE=Release -DINSTALL_OMPL=ON -DINSTALL_OMPL_TAG=master -DBUILD_IPOPT=OFF -DBUILD_SNOPT=OFF
call "D:\a\tesseract\tesseract\install_isolated\setup.bat"
catkin_make_isolated --install --force-cmake --pkg tesseract_collision tesseract_common tesseract_environment tesseract_geometry tesseract_kinematics tesseract_scene_graph tesseract_support tesseract_urdf tesseract_visualization tesseract_motion_planners tesseract_process_managers tesseract_time_parameterization --cmake-args -DCMAKE_BUILD_TYPE=Release -DTESSERACT_ENABLE_TESTING=ON -DINSTALL_OMPL=ON -DINSTALL_OMPL_TAG=master
catkin_make_isolated --install --force-cmake --pkg tesseract_collision tesseract_common tesseract_environment tesseract_geometry tesseract_kinematics tesseract_scene_graph tesseract_support tesseract_urdf tesseract_visualization tesseract_motion_planners tesseract_process_managers tesseract_time_parameterization --cmake-args -DCMAKE_BUILD_TYPE=Release -DTESSERACT_ENABLE_TESTING=ON -DINSTALL_OMPL=ON -DINSTALL_OMPL_TAG=master -DBUILD_IPOPT=OFF -DBUILD_SNOPT=OFF
Original file line number Diff line number Diff line change
Expand Up @@ -384,6 +384,7 @@ struct ContactTestData
*/
enum class CollisionEvaluatorType
{
NONE,
DISCRETE,
LVS_DISCRETE,
CONTINUOUS,
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -264,6 +264,24 @@ class CartesianWaypoint

/** @brief The Cartesian Waypoint */
Eigen::Isometry3d waypoint;
/** @brief Distance below waypoint that is allowed. Should be size = 6. First 3 elements are dx, dy, dz. The last 3
* elements are angle axis error allowed (Eigen::AngleAxisd.axis() * Eigen::AngleAxisd.angle()) */
Eigen::VectorXd lower_tolerance;
/** @brief Distance above waypoint that is allowed. Should be size = 6. First 3 elements are dx, dy, dz. The last 3
* elements are angle axis error allowed (Eigen::AngleAxisd.axis() * Eigen::AngleAxisd.angle())*/
Eigen::VectorXd upper_tolerance;

bool isToleranced() const
{
// Check if they are empty
if (lower_tolerance.size() == 0 || upper_tolerance.size() == 0)
return false;

// Check if they are close to 0
Eigen::VectorXd range = upper_tolerance - lower_tolerance;
double sum = range.sum();
return (sum < 1e-5);
}
};

} // namespace tesseract_planning
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -265,13 +265,29 @@ class JointWaypoint
inline operator Eigen::Ref<Eigen::VectorXd>() { return Eigen::Ref<Eigen::VectorXd>(waypoint); }

//////////////////////////////////
// Cartesian Waypoint Container //
// Joint Waypoint Container //
//////////////////////////////////

#endif // SWIG

Eigen::VectorXd waypoint;
std::vector<std::string> joint_names;
/** @brief Joint distance below waypoint that is allowed. Each element should be <= 0 */
Eigen::VectorXd lower_tolerance;
/** @brief Joint distance above waypoint that is allowed. Each element should be >= 0 */
Eigen::VectorXd upper_tolerance;

bool isToleranced() const
{
// Check if they are empty
if (lower_tolerance.size() == 0 || upper_tolerance.size() == 0)
return false;

// Check if they are close to 0
Eigen::VectorXd range = upper_tolerance - lower_tolerance;
double sum = range.sum();
return (sum < 1e-5);
}
};
} // namespace tesseract_planning

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,10 @@ find_package(tesseract_environment REQUIRED)
find_package(tesseract_common REQUIRED)
find_package(tesseract_command_language REQUIRED)
find_package(trajopt REQUIRED)
find_package(trajopt_ifopt REQUIRED)
find_package(trajopt_sco REQUIRED)
find_package(trajopt_sqp REQUIRED)
find_package(cmake_common_scripts REQUIRED)
# serialization was required because ompl does not include find_dependency for its required dependencies
find_package(Boost REQUIRED COMPONENTS serialization system filesystem program_options)

Expand Down Expand Up @@ -166,12 +169,35 @@ target_include_directories(${PROJECT_NAME}_trajopt SYSTEM PUBLIC
${EIGEN3_INCLUDE_DIRS}
${Boost_INCLUDE_DIRS})

# Trajopt_ifopt Planner
add_library(${PROJECT_NAME}_trajopt_ifopt SHARED
src/trajopt_ifopt/trajopt_ifopt_motion_planner.cpp
src/trajopt_ifopt/trajopt_ifopt_utils.cpp
src/trajopt_ifopt/profile/trajopt_ifopt_default_plan_profile.cpp
src/trajopt_ifopt/profile/trajopt_ifopt_default_composite_profile.cpp
src/trajopt_ifopt/problem_generators/default_problem_generator.cpp)
target_link_libraries(${PROJECT_NAME}_trajopt_ifopt PUBLIC ${Boost_LIBRARIES} ${PROJECT_NAME}_core trajopt::trajopt_ifopt trajopt::trajopt_sqp)
target_compile_options(${PROJECT_NAME}_trajopt_ifopt PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE})
target_compile_options(${PROJECT_NAME}_trajopt_ifopt PUBLIC ${TESSERACT_COMPILE_OPTIONS_PUBLIC})
target_compile_definitions(${PROJECT_NAME}_trajopt_ifopt PUBLIC ${TESSERACT_COMPILE_DEFINITIONS})
target_clang_tidy(${PROJECT_NAME}_trajopt_ifopt ARGUMENTS ${TESSERACT_CLANG_TIDY_ARGS} ENABLE ${TESSERACT_ENABLE_CLANG_TIDY})
target_cxx_version(${PROJECT_NAME}_trajopt_ifopt PUBLIC VERSION ${TESSERACT_CXX_VERSION})
target_code_coverage(${PROJECT_NAME}_trajopt_ifopt ALL EXCLUDE ${COVERAGE_EXCLUDE} ENABLE ${TESSERACT_ENABLE_TESTING})
target_include_directories(${PROJECT_NAME}_trajopt_ifopt PUBLIC
"$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
"$<INSTALL_INTERFACE:include>")
target_include_directories(${PROJECT_NAME}_trajopt_ifopt SYSTEM PUBLIC
${EIGEN3_INCLUDE_DIRS}
${Boost_INCLUDE_DIRS})

configure_package(NAMESPACE tesseract
TARGETS ${PROJECT_NAME}_core
${PROJECT_NAME}_descartes
${PROJECT_NAME}_ompl
${PROJECT_NAME}_simple
${PROJECT_NAME}_trajopt)
${PROJECT_NAME}_trajopt
${PROJECT_NAME}_trajopt_ifopt
)

# Mark cpp header files for installation
install(DIRECTORY include/${PROJECT_NAME}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <Eigen/Geometry>
TESSERACT_COMMON_IGNORE_WARNINGS_POP

#include <tesseract_command_language/cartesian_waypoint.h>
#include <tesseract_command_language/joint_waypoint.h>
#include <tesseract_motion_planners/trajopt/profile/trajopt_profile.h>

#ifdef SWIG
Expand Down Expand Up @@ -81,14 +83,14 @@ class TrajOptDefaultPlanProfile : public TrajOptPlanProfile
constraint_error_functions;

void apply(trajopt::ProblemConstructionInfo& pci,
const Eigen::Isometry3d& cartesian_waypoint,
const CartesianWaypoint& cartesian_waypoint,
const Instruction& parent_instruction,
const ManipulatorInfo& manip_info,
const std::vector<std::string>& active_links,
int index) const override;

void apply(trajopt::ProblemConstructionInfo& pci,
const Eigen::VectorXd& joint_waypoint,
const JointWaypoint& joint_waypoint,
const Instruction& parent_instruction,
const ManipulatorInfo& manip_info,
const std::vector<std::string>& active_links,
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
TESSERACT_COMMON_IGNORE_WARNINGS_POP

#include <tesseract_command_language/core/instruction.h>
#include <tesseract_command_language/joint_waypoint.h>
#include <tesseract_command_language/cartesian_waypoint.h>
#include <tesseract_command_language/types.h>

#ifdef SWIG
Expand All @@ -58,14 +60,14 @@ class TrajOptPlanProfile
TrajOptPlanProfile& operator=(TrajOptPlanProfile&&) = default;

virtual void apply(trajopt::ProblemConstructionInfo& pci,
const Eigen::Isometry3d& cartesian_waypoint,
const CartesianWaypoint& cartesian_waypoint,
const Instruction& parent_instruction,
const ManipulatorInfo& manip_info,
const std::vector<std::string>& active_links,
int index) const = 0;

virtual void apply(trajopt::ProblemConstructionInfo& pci,
const Eigen::VectorXd& joint_waypoint,
const JointWaypoint& joint_waypoint,
const Instruction& parent_instruction,
const ManipulatorInfo& manip_info,
const std::vector<std::string>& active_links,
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -63,6 +63,13 @@ trajopt::TermInfo::Ptr createJointWaypointTermInfo(const Eigen::VectorXd& j_wp,
const Eigen::VectorXd& coeffs,
trajopt::TermType type);

trajopt::TermInfo::Ptr createTolerancedJointWaypointTermInfo(const Eigen::VectorXd& j_wp,
const Eigen::VectorXd& lower_tol,
const Eigen::VectorXd& upper_tol,
int index,
const Eigen::VectorXd& coeffs,
trajopt::TermType type);

trajopt::TermInfo::Ptr createCollisionTermInfo(
int start_index,
int end_index,
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,46 @@
/**
* @file default_problem_generator.h
* @brief Generates a trajopt problem from a planner request
*
* @author Levi Armstrong
* @date April 18, 2018
* @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_DEFAULT_PROBLEM_GENERATOR_H
#define TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_DEFAULT_PROBLEM_GENERATOR_H

#include <tesseract_common/macros.h>
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <ifopt/problem.h>
TESSERACT_COMMON_IGNORE_WARNINGS_POP

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

namespace tesseract_planning
{
std::shared_ptr<TrajOptIfoptProblem>
DefaultTrajoptProblemGenerator(const std::string& name,
const PlannerRequest& request,
const TrajOptIfoptPlanProfileMap& plan_profiles,
const TrajOptIfoptCompositeProfileMap& composite_profiles);

} // namespace tesseract_planning
#endif
Original file line number Diff line number Diff line change
@@ -0,0 +1,98 @@
/**
* @file trajopt_default_composite_profile.h
* @brief
*
* @author Levi Armstrong
* @date June 18, 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_DEFAULT_COMPOSITE_PROFILE_H
#define TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_DEFAULT_COMPOSITE_PROFILE_H

#include <tesseract_common/macros.h>
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <vector>
#include <Eigen/Geometry>
#include <Eigen/Core>
#include <trajopt_ifopt/constraints/collision_evaluators.h>
TESSERACT_COMMON_IGNORE_WARNINGS_POP

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

namespace tesseract_planning
{
class TrajOptIfoptDefaultCompositeProfile : public TrajOptIfoptCompositeProfile
{
public:
TrajOptIfoptDefaultCompositeProfile() = default;
TrajOptIfoptDefaultCompositeProfile(const tinyxml2::XMLElement& xml_element);

/** @brief Configuration info for collisions that are modeled as costs */
trajopt::TrajOptCollisionConfig::Ptr collision_cost_config;
/** @brief Configuration info for collisions that are modeled as constraints */
trajopt::TrajOptCollisionConfig::Ptr collision_constraint_config;
/** @brief If true, a joint velocity cost with a target of 0 will be applied for all timesteps Default: true*/
bool smooth_velocities = true;
/** @brief This default to all ones, but allows you to weight different joints */
Eigen::VectorXd velocity_coeff;
/** @brief If true, a joint acceleration cost with a target of 0 will be applied for all timesteps Default: false*/
bool smooth_accelerations = true;
/** @brief This default to all ones, but allows you to weight different joints */
Eigen::VectorXd acceleration_coeff;
/** @brief If true, a joint jerk cost with a target of 0 will be applied for all timesteps Default: false*/
bool smooth_jerks = true;
/** @brief This default to all ones, but allows you to weight different joints */
Eigen::VectorXd jerk_coeff;

/** @brief Set the resolution at which state validity needs to be verified in order for a motion between two states
* to be considered valid in post checking of trajectory returned by TrajOptIfopt.
*
* The resolution is equal to longest_valid_segment_fraction * state_space.getMaximumExtent()
*
* Note: The planner takes the conservative of either longest_valid_segment_fraction or longest_valid_segment_length.
*/
double longest_valid_segment_fraction = 0.01; // 1%

/** @brief Set the resolution at which state validity needs to be verified in order for a motion between two states
* to be considered valid. If norm(state1 - state0) > longest_valid_segment_length.
*
* Note: This gets converted to longest_valid_segment_fraction.
* longest_valid_segment_fraction = longest_valid_segment_length / state_space.getMaximumExtent()
*/
double longest_valid_segment_length = 0.5;

/** @brief Special link collision cost distances */
trajopt::SafetyMarginData::Ptr special_collision_cost{ nullptr };
/** @brief Special link collision constraint distances */
trajopt::SafetyMarginData::Ptr special_collision_constraint{ nullptr };

void apply(TrajOptIfoptProblem& problem,
int start_index,
int end_index,
const 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;
};
} // namespace tesseract_planning

#endif // TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_DEFAULT_COMPOSITE_PROFILE_H
Loading