Skip to content
Snippets Groups Projects
Commit 6b2369b9 authored by hyatt's avatar hyatt
Browse files

initiate commit to

parent fd9082e7
Branches
Tags
No related merge requests found
Hide whitespace changes
Inline Side-by-side
ur10_moveit_config/config/kinematics.yaml
+ 7
− 1
View file @ 6b2369b9
......@@ -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
ur_kinematics/CMakeLists.txt
+ 27
− 9
View file @ 6b2369b9
......@@ -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 ##
......
ur_kinematics/include/ur_kinematics/ur_kin.h
+ 3
− 0
View file @ 6b2369b9
......@@ -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
......
ur_kinematics/package.xml
+ 13
− 5
View file @ 6b2369b9
......@@ -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>
ur_kinematics/src/ur_kin.cpp
+ 114
− 0
View file @ 6b2369b9
......@@ -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++;
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment