diff --git a/ur10_moveit_config/config/kinematics.yaml b/ur10_moveit_config/config/kinematics.yaml
index 969ba14252bfe9c5dc61a6a01a078447e08f69bb..8168af825f6d52d4a1fa2c471f5a6f341d8c290e 100644
--- a/ur10_moveit_config/config/kinematics.yaml
+++ b/ur10_moveit_config/config/kinematics.yaml
@@ -2,4 +2,10 @@ manipulator:
kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
kinematics_solver_search_resolution: 0.005
kinematics_solver_timeout: 0.005
- kinematics_solver_attempts: 3
\ No newline at end of file
+ kinematics_solver_attempts: 3
+
+manipulator_new:
+ kinematics_solver: ur_kinematics/UR10KinematicsPlugin
+ kinematics_solver_search_resolution: 0.005
+ kinematics_solver_timeout: 0.005
+ kinematics_solver_attempts: 3
diff --git a/ur_kinematics/CMakeLists.txt b/ur_kinematics/CMakeLists.txt
index d7d5f2adf97086f73b7042a9da49e4f3edadbfe3..a63d597bb118da810e02aeb53c763555fc56af13 100644
--- a/ur_kinematics/CMakeLists.txt
+++ b/ur_kinematics/CMakeLists.txt
@@ -4,11 +4,10 @@ project(ur_kinematics)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
-find_package(catkin REQUIRED)
+find_package(catkin REQUIRED COMPONENTS roscpp geometry_msgs moveit_core pluginlib)
## System dependencies are found with CMake's conventions
-# find_package(Boost REQUIRED COMPONENTS system)
-
+find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
@@ -49,7 +48,9 @@ find_package(catkin REQUIRED)
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
INCLUDE_DIRS include
- LIBRARIES ur10_kin ur5_kin
+ LIBRARIES ur10_kin ur5_kin ur10_moveit_plugin ur5_moveit_plugin
+ CATKIN_DEPENDS pluginlib
+ DEPENDS moveit_core
# CATKIN_DEPENDS other_catkin_pkg
# DEPENDS system_lib
)
@@ -60,8 +61,12 @@ catkin_package(
## Specify additional locations of header files
## Your package locations should be listed before other locations
+include_directories(SYSTEM ${Boost_INCLUDE_DIR})
include_directories(include ${catkin_INCLUDE_DIRS})
+link_directories(${Boost_LIBRARY_DIRS})
+link_directories(${catkin_LIBRARY_DIRS})
+
## Declare a cpp library
# add_library(ur_kinematics
# src/${PROJECT_NAME}/ur_kinematics.cpp
@@ -72,6 +77,20 @@ set_target_properties(ur10_kin PROPERTIES COMPILE_DEFINITIONS "UR10_PARAMS")
add_library(ur5_kin src/ur_kin.cpp)
set_target_properties(ur5_kin PROPERTIES COMPILE_DEFINITIONS "UR5_PARAMS")
+add_library(ur10_moveit_plugin src/ur_moveit_plugin.cpp)
+set_target_properties(ur10_moveit_plugin PROPERTIES COMPILE_DEFINITIONS "UR10_PARAMS")
+target_link_libraries(ur10_moveit_plugin
+ ${catkin_LIBRARIES}
+ ${Boost_LIBRARIES}
+ ur10_kin)
+
+add_library(ur5_moveit_plugin src/ur_moveit_plugin.cpp)
+set_target_properties(ur5_moveit_plugin PROPERTIES COMPILE_DEFINITIONS "UR5_PARAMS")
+target_link_libraries(ur5_moveit_plugin
+ ${catkin_LIBRARIES}
+ ${Boost_LIBRARIES}
+ ur5_kin)
+
## Declare a cpp executable
# add_executable(ur_kinematics_node src/ur_kinematics_node.cpp)
@@ -124,11 +143,10 @@ install(DIRECTORY include/${PROJECT_NAME}/
# )
## Mark other files for installation (e.g. launch and bag files, etc.)
-# install(FILES
-# # myfile1
-# # myfile2
-# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-# )
+install(FILES
+ ur_moveit_plugins.xml
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
#############
## Testing ##
diff --git a/ur_kinematics/include/ur_kinematics/ur_kin.h b/ur_kinematics/include/ur_kinematics/ur_kin.h
index 30b677235030e0d2f36496497159804d0ad73b30..2f5f45204d1dc196402c1729da253e2855c43313 100644
--- a/ur_kinematics/include/ur_kinematics/ur_kin.h
+++ b/ur_kinematics/include/ur_kinematics/ur_kin.h
@@ -86,6 +86,9 @@ namespace ur_kinematics {
// @param T The 4x4 end effector pose in row-major ordering
void forward(const double* q, double* T);
+ void forward_all(const double* q, double* T1, double* T2, double* T3,
+ double* T4, double* T5, double* T6);
+
// @param T The 4x4 end effector pose in row-major ordering
// @param q_sols An 8x6 array of doubles returned, all angles should be in [0,2*PI)
// @param q6_des An optional parameter which designates what the q6 value should take
diff --git a/ur_kinematics/package.xml b/ur_kinematics/package.xml
index 497106dc7f787c89b614d9a9a421e2180dc7ab0d..14b9a67dd8edfad680c9ad9f1c79bca86162a253 100644
--- a/ur_kinematics/package.xml
+++ b/ur_kinematics/package.xml
@@ -4,6 +4,7 @@
<version>1.0.2</version>
<description>
Provides forward and inverse kinematics for Universal robot designs.
+ See http://hdl.handle.net/1853/50782 for details.
</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
@@ -42,15 +43,22 @@
<!-- <run_depend>message_runtime</run_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
+
<buildtool_depend>catkin</buildtool_depend>
+ <build_depend>moveit_core</build_depend>
+ <build_depend>roscpp</build_depend>
+ <build_depend>geometry_msgs</build_depend>
+ <build_depend>pluginlib</build_depend>
- <!-- The export tag contains other, unspecified, tags -->
- <export>
- <!-- You can specify that this package is a metapackage here: -->
- <!-- <metapackage/> -->
+ <run_depend>moveit_core</run_depend>
+ <run_depend>roscpp</run_depend>
+ <run_depend>geometry_msgs</run_depend>
+ <run_depend>pluginlib</run_depend>
- <!-- Other tools can request additional information be placed here -->
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <moveit_core plugin="${prefix}/ur_moveit_plugins.xml"/>
</export>
</package>
diff --git a/ur_kinematics/src/ur_kin.cpp b/ur_kinematics/src/ur_kin.cpp
index 57144541808a2b1fd4a07d56dbaf942f8515692a..d10759dab9997e8e328eccc8d5b0805343f85d89 100644
--- a/ur_kinematics/src/ur_kin.cpp
+++ b/ur_kinematics/src/ur_kin.cpp
@@ -34,6 +34,120 @@ namespace ur_kinematics {
*T = 0.0; T++; *T = 0.0; T++; *T = 0.0; T++; *T = 1.0;
}
+ void forward_all(const double* q, double* T1, double* T2, double* T3,
+ double* T4, double* T5, double* T6) {
+ double s1 = sin(*q), c1 = cos(*q); q++; // q1
+ double q23 = *q, q234 = *q, s2 = sin(*q), c2 = cos(*q); q++; // q2
+ double s3 = sin(*q), c3 = cos(*q); q23 += *q; q234 += *q; q++; // q3
+ q234 += *q; q++; // q4
+ double s5 = sin(*q), c5 = cos(*q); q++; // q5
+ double s6 = sin(*q), c6 = cos(*q); // q6
+ double s23 = sin(q23), c23 = cos(q23);
+ double s234 = sin(q234), c234 = cos(q234);
+
+ *T1 = c1; T1++;
+ *T1 = 0; T1++;
+ *T1 = s1; T1++;
+ *T1 = 0; T1++;
+ *T1 = s1; T1++;
+ *T1 = 0; T1++;
+ *T1 = -c1; T1++;
+ *T1 = 0; T1++;
+ *T1 = 0; T1++;
+ *T1 = 1; T1++;
+ *T1 = 0; T1++;
+ *T1 =d1; T1++;
+ *T1 = 0; T1++;
+ *T1 = 0; T1++;
+ *T1 = 0; T1++;
+ *T1 = 1; T1++;
+
+ *T2 = c1*c2; T2++;
+ *T2 = -c1*s2; T2++;
+ *T2 = s1; T2++;
+ *T2 =a2*c1*c2; T2++;
+ *T2 = c2*s1; T2++;
+ *T2 = -s1*s2; T2++;
+ *T2 = -c1; T2++;
+ *T2 =a2*c2*s1; T2++;
+ *T2 = s2; T2++;
+ *T2 = c2; T2++;
+ *T2 = 0; T2++;
+ *T2 = d1 + a2*s2; T2++;
+ *T2 = 0; T2++;
+ *T2 = 0; T2++;
+ *T2 = 0; T2++;
+ *T2 = 1; T2++;
+
+ *T3 = c23*c1; T3++;
+ *T3 = -s23*c1; T3++;
+ *T3 = s1; T3++;
+ *T3 =c1*(a3*c23 + a2*c2); T3++;
+ *T3 = c23*s1; T3++;
+ *T3 = -s23*s1; T3++;
+ *T3 = -c1; T3++;
+ *T3 =s1*(a3*c23 + a2*c2); T3++;
+ *T3 = s23; T3++;
+ *T3 = c23; T3++;
+ *T3 = 0; T3++;
+ *T3 = d1 + a3*s23 + a2*s2; T3++;
+ *T3 = 0; T3++;
+ *T3 = 0; T3++;
+ *T3 = 0; T3++;
+ *T3 = 1; T3++;
+
+ *T4 = c234*c1; T4++;
+ *T4 = s1; T4++;
+ *T4 = s234*c1; T4++;
+ *T4 =c1*(a3*c23 + a2*c2) + d4*s1; T4++;
+ *T4 = c234*s1; T4++;
+ *T4 = -c1; T4++;
+ *T4 = s234*s1; T4++;
+ *T4 =s1*(a3*c23 + a2*c2) - d4*c1; T4++;
+ *T4 = s234; T4++;
+ *T4 = 0; T4++;
+ *T4 = -c234; T4++;
+ *T4 = d1 + a3*s23 + a2*s2; T4++;
+ *T4 = 0; T4++;
+ *T4 = 0; T4++;
+ *T4 = 0; T4++;
+ *T4 = 1; T4++;
+
+ *T5 = s1*s5 + c234*c1*c5; T5++;
+ *T5 = -s234*c1; T5++;
+ *T5 = c5*s1 - c234*c1*s5; T5++;
+ *T5 =c1*(a3*c23 + a2*c2) + d4*s1 + d5*s234*c1; T5++;
+ *T5 = c234*c5*s1 - c1*s5; T5++;
+ *T5 = -s234*s1; T5++;
+ *T5 = - c1*c5 - c234*s1*s5; T5++;
+ *T5 =s1*(a3*c23 + a2*c2) - d4*c1 + d5*s234*s1; T5++;
+ *T5 = s234*c5; T5++;
+ *T5 = c234; T5++;
+ *T5 = -s234*s5; T5++;
+ *T5 = d1 + a3*s23 + a2*s2 - d5*c234; T5++;
+ *T5 = 0; T5++;
+ *T5 = 0; T5++;
+ *T5 = 0; T5++;
+ *T5 = 1; T5++;
+
+ *T6 = c6*(s1*s5 + c234*c1*c5) - s234*c1*s6; T6++;
+ *T6 = - s6*(s1*s5 + c234*c1*c5) - s234*c1*c6; T6++;
+ *T6 = c5*s1 - c234*c1*s5; T6++;
+ *T6 =d6*(c5*s1 - c234*c1*s5) + c1*(a3*c23 + a2*c2) + d4*s1 + d5*s234*c1; T6++;
+ *T6 = - c6*(c1*s5 - c234*c5*s1) - s234*s1*s6; T6++;
+ *T6 = s6*(c1*s5 - c234*c5*s1) - s234*c6*s1; T6++;
+ *T6 = - c1*c5 - c234*s1*s5; T6++;
+ *T6 =s1*(a3*c23 + a2*c2) - d4*c1 - d6*(c1*c5 + c234*s1*s5) + d5*s234*s1; T6++;
+ *T6 = c234*s6 + s234*c5*c6; T6++;
+ *T6 = c234*c6 - s234*c5*s6; T6++;
+ *T6 = -s234*s5; T6++;
+ *T6 = d1 + a3*s23 + a2*s2 - d5*c234 - d6*s234*s5; T6++;
+ *T6 = 0; T6++;
+ *T6 = 0; T6++;
+ *T6 = 0; T6++;
+ *T6 = 1; T6++;
+ }
+
int inverse(const double* T, double* q_sols, double q6_des) {
int num_sols = 0;
double T02 = -*T; T++; double T00 = *T; T++; double T01 = *T; T++; double T03 = -*T; T++;