Skip to content
This repository is currently being migrated. It's locked while the migration is in progress.

Commit f6a2403

Browse files
Omid Heidaridavetcoleman
Omid Heidari
authored and
davetcoleman
committed
MVP TrajOpt Planner Plugin (moveit#1593)
* added solve from tesseract. added launch file improved test_trajopt node edited package.xml * TrajOpt Planner (Empty Template) (moveit#1478) * Created EmptyPlan, a MoveIt planning plugin template * added trajopt from example_planner * fixed trajopt_planner_mannager * modified emaptyplan_planner_manager.cpp * applied Dave's modifications * The rest of the modifications from Dave's comments * removed moveit_ros_planning dependency from package.xml * fixed clangformat error and dependency issu * added include directory * edited package.xml * added include * remove include * remove version_gte from package.xml * updated the setup assitant to work with trajopt planner * removed unsuded part in initialize() from trajopt_planner_manager.cpp, kept the initialize() itself for later use * Update moveit_planners/trajopt/src/trajopt_planner_manager.cpp Co-Authored-By: Dave Coleman <dave@picknik.ai> * added README added description to trajopt_interface_plugin_description.xml * updated resp req updated planning context working trajopt without any velocity constraint * added trajopt_interface class * changed the architectrue and used TrajoptInterface class to contain the solve function details. Created a TrajOptInterface object in TrajoptPlanningContext class instead * added kinematic_terms in problem_description, added comments anout term_type added ProblemInfo, TermInfo, InitInfo, BasicInfo in kinematic_terms, added operator() added ConstructProblem added some param for problem_info * moved rotVec and concat from kinematics_term header to implementation in problem_Description, added term_type used MotionPlaneRequest from planning_interface instead that of moveit_msgs for solve function input added generateInitiaTrajectory added basic_info to problem_info * added panda_omple_planning added checkParameterSize function in problem_description added JointPoseTermInfo::hatch in problem_description added res.error.code.val to check the request in trajopt_interface.cpp convert req.goal_constraint to JointPoseTermInfo in trajopt_interface.cpp feed the response with the trajopt solution in trajopt_interface.cpp remove comments from trajopt_planning_context.cpp * addressed the comments fixed start state corrected the yaml file multiple joint constraints * addressed some comments leave setup assistant changes for later on another PR cleaned up test_trajopt added unittest * added kinematic DOF of the active joint model group to TrajOptProblem class added name_ for ROS_DEBUG_STREAM_NAMED addressed the comments moved launch file to moveit_tutorial moved yaml file to panda_moveit_config used bullet for collision checking * handled reading the request from MotionPlanning Display in RViz * Added comments about initial trajectory changed my email address renamed rotVec and concat added tesseract license * clang format * applied Henning's comments * added catkin ignore to ignore this package for now as it has dependencies. The next PR wont have any dependencies * clang format * addressed the comments * renamed hatch to addObjectiveTerms * added a unittest for goal tolerance * addressed Dave's comments * added catkin_ignore * addressed Bryce's comments
1 parent 70537f0 commit f6a2403

19 files changed

+2334
-3
lines changed

‎.gitignore

+3-3
Original file line numberDiff line numberDiff line change
@@ -51,10 +51,10 @@ qtcreator-*
5151
# Vim
5252
*.swp
5353

54-
# Catkin custom files
55-
CATKIN_IGNORE
56-
5754
# Continous Integration
5855
.moveit_ci
5956

6057
*.pyc
58+
59+
#gdb
60+
*.gdb

‎moveit_planners/trajopt/CATKIN_IGNORE

Whitespace-only changes.

‎moveit_planners/trajopt/CMakeLists.txt

+113
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,113 @@
1+
cmake_minimum_required(VERSION 2.8.12)
2+
project(moveit_planners_trajopt)
3+
4+
add_compile_options(-std=c++14)
5+
6+
if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE)
7+
set(CMAKE_BUILD_TYPE Release)
8+
endif()
9+
10+
find_package(catkin REQUIRED
11+
COMPONENTS
12+
moveit_core
13+
moveit_visual_tools
14+
moveit_ros_planning
15+
moveit_ros_planning_interface
16+
pluginlib
17+
roscpp
18+
rosparam_shortcuts
19+
trajopt
20+
)
21+
22+
catkin_package(
23+
INCLUDE_DIRS include
24+
LIBRARIES
25+
${PROJECT_NAME}
26+
INCLUDE_DIRS
27+
CATKIN_DEPENDS
28+
roscpp
29+
pluginlib
30+
moveit_core
31+
moveit_visual_tools
32+
moveit_ros_planning_interface
33+
rosparam_shortcuts
34+
)
35+
36+
# The following include_directory should have include folder of the new planner.
37+
include_directories(
38+
include
39+
${catkin_INCLUDE_DIRS}
40+
SYSTEM
41+
${Boost_INCLUDE_DIRS}
42+
${catkin_INCLUDE_DIRS}
43+
)
44+
45+
add_library(
46+
${PROJECT_NAME}
47+
src/trajopt_interface.cpp
48+
src/problem_description.cpp
49+
src/kinematic_terms.cpp
50+
src/trajopt_planning_context.cpp
51+
)
52+
53+
target_link_libraries(
54+
${PROJECT_NAME}
55+
${catkin_LIBRARIES}
56+
${Boost_LIBRARIES}
57+
)
58+
59+
set_target_properties(
60+
${PROJECT_NAME}
61+
PROPERTIES
62+
VERSION
63+
"${${PROJECT_NAME}_VERSION}"
64+
)
65+
66+
# TrajOpt planning plugin
67+
add_library(moveit_trajopt_planner_plugin src/trajopt_planner_manager.cpp)
68+
set_target_properties(moveit_trajopt_planner_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
69+
target_link_libraries(moveit_trajopt_planner_plugin ${PROJECT_NAME} ${catkin_LIBRARIES})
70+
71+
#############
72+
## Install ##
73+
#############
74+
75+
# Mark executables and/or libraries for installation
76+
install(
77+
TARGETS
78+
moveit_trajopt_planner_plugin
79+
ARCHIVE DESTINATION
80+
${CATKIN_PACKAGE_LIB_DESTINATION}
81+
LIBRARY DESTINATION
82+
${CATKIN_PACKAGE_LIB_DESTINATION}
83+
RUNTIME DESTINATION
84+
${CATKIN_PACKAGE_BIN_DESTINATION}
85+
)
86+
87+
# Mark cpp header files for installation
88+
install(
89+
DIRECTORY
90+
include/trajopt_interface/
91+
DESTINATION
92+
${CATKIN_PACKAGE_INCLUDE_DESTINATION}
93+
)
94+
95+
install(FILES trajopt_interface_plugin_description.xml
96+
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
97+
98+
#############
99+
## Testing ##
100+
#############
101+
102+
if(CATKIN_ENABLE_TESTING)
103+
find_package(rostest REQUIRED)
104+
105+
add_rostest_gtest(trajectory_test test/trajectory_test.test test/trajectory_test.cpp)
106+
target_link_libraries(
107+
trajectory_test
108+
${LIBRARY_NAME}
109+
${catkin_LIBRARIES}
110+
${PROJECT_NAME}
111+
)
112+
113+
endif()

‎moveit_planners/trajopt/README.md

+1
Original file line numberDiff line numberDiff line change
@@ -0,0 +1 @@
1+
As of August 2019, this is a work in progress towards adding trajopt motion planning algorithm to MoveIt as a planner plugin.

‎moveit_planners/trajopt/include/trajopt_interface/kinematic_terms.h

+93
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,93 @@
1+
#pragma once
2+
3+
#include <moveit/planning_scene/planning_scene.h>
4+
5+
#include <Eigen/Geometry>
6+
7+
namespace trajopt_interface
8+
{
9+
/**
10+
* @brief Extracts the vector part of quaternion
11+
*/
12+
inline Eigen::Vector3d quaternionRotationVector(const Eigen::Matrix3d& matrix)
13+
{
14+
Eigen::Quaterniond quaternion;
15+
quaternion = matrix;
16+
return Eigen::Vector3d(quaternion.x(), quaternion.y(), quaternion.z());
17+
}
18+
19+
/**
20+
* @brief Appends b to a of type VectorXd
21+
*/
22+
inline Eigen::VectorXd concatVector(const Eigen::VectorXd& vector_a, const Eigen::VectorXd& vector_b)
23+
{
24+
Eigen::VectorXd out(vector_a.size() + vector_b.size());
25+
out.topRows(vector_a.size()) = vector_a;
26+
out.middleRows(vector_a.size(), vector_b.size()) = vector_b;
27+
return out;
28+
}
29+
30+
/**
31+
* @brief Appends b to a of type T
32+
*/
33+
template <typename T>
34+
inline std::vector<T> concatVector(const std::vector<T>& vector_a, const std::vector<T>& vector_b)
35+
{
36+
std::vector<T> out;
37+
out.insert(out.end(), vector_a.begin(), vector_a.end());
38+
out.insert(out.end(), vector_b.begin(), vector_b.end());
39+
return out;
40+
}
41+
42+
/**
43+
* @brief Used to calculate the error for StaticCartPoseTermInfo
44+
* This is converted to a cost or constraint using TrajOptCostFromErrFunc or TrajOptConstraintFromErrFunc
45+
*/
46+
struct CartPoseErrCalculator : public sco::VectorOfVector
47+
{
48+
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
49+
Eigen::Isometry3d target_pose_inv_;
50+
planning_scene::PlanningSceneConstPtr planning_scene_;
51+
std::string link_;
52+
Eigen::Isometry3d tcp_;
53+
54+
CartPoseErrCalculator(const Eigen::Isometry3d& pose, const planning_scene::PlanningSceneConstPtr planning_scene,
55+
const std::string& link, Eigen::Isometry3d tcp = Eigen::Isometry3d::Identity())
56+
: target_pose_inv_(pose.inverse()), planning_scene_(planning_scene), link_(link), tcp_(tcp)
57+
{
58+
}
59+
60+
Eigen::VectorXd operator()(const Eigen::VectorXd& dof_vals) const override;
61+
};
62+
63+
// TODO(omid): The following should be added and adjusted from trajopt
64+
// JointPosEqCost
65+
// JointPosIneqCost
66+
// JointPosEqConstraint
67+
// JointPosIneqConstraint
68+
69+
struct JointVelErrCalculator : sco::VectorOfVector
70+
{
71+
/** @brief Velocity target */
72+
double target_;
73+
/** @brief Upper tolerance */
74+
double upper_tol_;
75+
/** @brief Lower tolerance */
76+
double lower_tol_;
77+
JointVelErrCalculator() : target_(0.0), upper_tol_(0.0), lower_tol_(0.0)
78+
{
79+
}
80+
JointVelErrCalculator(double target, double upper_tol, double lower_tol)
81+
: target_(target), upper_tol_(upper_tol), lower_tol_(lower_tol)
82+
{
83+
}
84+
85+
Eigen::VectorXd operator()(const Eigen::VectorXd& var_vals) const;
86+
};
87+
88+
struct JointVelJacobianCalculator : sco::MatrixOfVector
89+
{
90+
Eigen::MatrixXd operator()(const Eigen::VectorXd& var_vals) const;
91+
};
92+
93+
} // namespace trajopt_interface

0 commit comments

Comments
 (0)