diff --git a/CMakeLists.txt b/CMakeLists.txt index 8e717bb..339b587 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -21,7 +21,9 @@ endif() add_definitions(${EIGEN_DEFINITIONS}) -find_package(catkin REQUIRED COMPONENTS roscpp roslint tf tf2 tf2_eigen) +find_package(yaml-cpp REQUIRED) + +find_package(catkin REQUIRED COMPONENTS roscpp roslib roslint tf tf2 tf2_eigen) SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -std=gnu++0x") @@ -30,12 +32,13 @@ include_directories( ${catkin_INCLUDE_DIRS} ${catkin_INCLUDES} ${Eigen3_INCLUDE_DIRS} + ${YAML_CPP_INCLUDE_DIRS} ) catkin_package( DEPENDS ${Eigen3_DEP} LIBRARIES davinci_kinematics - CATKIN_DEPENDS roscpp + CATKIN_DEPENDS roscpp roslib INCLUDE_DIRS include ${EIGEN3_INCLUDE_DIRS} ) @@ -46,6 +49,7 @@ add_library(davinci_kinematics src/davinci_fwd_kinematics.cpp src/davinci_inv_ki target_link_libraries(davinci_kinematics ${catkin_LIBRARIES} + ${YAML_CPP_LIBRARIES} davinci_kinematic_definitions ) @@ -77,6 +81,63 @@ target_link_libraries(ik_demo ${catkin_LIBRARIES} ) +add_executable(refined_ik_demo examples/davinci_ik_refined_test.cpp) + +target_link_libraries(refined_ik_demo + davinci_kinematics + davinci_kinematic_definitions + ${catkin_LIBRARIES} +) + + +add_executable(davinci_fwd_fix_trial examples/davinci_fwd_fix_trial.cpp) + +target_link_libraries(davinci_fwd_fix_trial + davinci_kinematics + davinci_kinematic_definitions + ${catkin_LIBRARIES} +) + +add_executable(test_dh_params examples/test_dh_params.cpp) + +target_link_libraries(test_dh_params + davinci_kinematics + davinci_kinematic_definitions + ${catkin_LIBRARIES} + ) + +add_executable(test_new_ik_err examples/test_new_ik_err.cpp) + +target_link_libraries(test_new_ik_err + davinci_kinematics + davinci_kinematic_definitions + ${catkin_LIBRARIES} + ) + +add_executable(psm_intrinsic_calibration_auxiliary examples/psm_intrinsic_calibration_auxiliary.cpp) + +target_link_libraries(psm_intrinsic_calibration_auxiliary + davinci_kinematics + davinci_kinematic_definitions + ${catkin_LIBRARIES} + ) + +add_executable(test_dh_345 examples/test_dh_345.cpp) + +target_link_libraries(test_dh_345 + davinci_kinematics + davinci_kinematic_definitions + ${catkin_LIBRARIES} + ) + +add_executable(davinci_jacobian_test_rn examples/davinci_jacobian_test_rn.cpp) + +target_link_libraries(davinci_jacobian_test_rn + davinci_kinematics + davinci_kinematic_definitions + ${catkin_LIBRARIES} + ) + file(GLOB_RECURSE SRC_FILES_CPP src/*.cpp) file(GLOB_RECURSE TEST_FILES_CPP test/*.cpp) @@ -95,4 +156,4 @@ target_link_libraries(full_kinematics_test davinci_kinematics davinci_kinematic_definitions ${catkin_LIBRARIES} -) \ No newline at end of file +) diff --git a/config/20181118/psm1_dh.yaml b/config/20181118/psm1_dh.yaml new file mode 100644 index 0000000..1a2dbef --- /dev/null +++ b/config/20181118/psm1_dh.yaml @@ -0,0 +1,49 @@ +# DH PARAMETER RECOMMENDATION + +# 04 +# @Date: 20180813 +# @Data group count: 1 +# @Base frame z: anti-parallel to gravity. +# @Polaris z: parallel to base frame y. +# @Notes: Only did one calibration round to get the result. Note that the d2 and d3 changed rather significantly. +# @Quality test result: 0.94 mm + +# --- +# DH_0 TO DH_1 FRAME +theta_1: 3.141593 +alpha_1: -1.563196 +a_1: 0.005266 +d_1: 0.000000 + +# DH_1 TO DH_2 FRAME +theta_2: -1.560790 +alpha_2: 1.560804 +a_2: 0.000855 +d_2: -0.002970 + +# DH_2 TO DH_3 FRAME +theta_3: 0.000000 +alpha_3: 0.000000 +a_3: 0.000000 +d_3: -0.008697 + +# DH_3 TO DH_4 FRAME +theta_4: 0.083931 +alpha_4: -1.557303 +a_4: 0.000533 +d_4: -0.000116 + +# DH_4 TO DH_5 FRAME +theta_5: -1.641617 +alpha_5: -1.483756 +a_5: 0.008697 +d_5: -0.000070 + +# Scale Factors +j1_scale_factor: 1.0022 +j2_scale_factor: 0.978 +j3_scale_factor: 0.988530 + + + + diff --git a/config/20181118/psm2_dh.yaml b/config/20181118/psm2_dh.yaml new file mode 100644 index 0000000..c79e195 --- /dev/null +++ b/config/20181118/psm2_dh.yaml @@ -0,0 +1,38 @@ +# 02 +# Note: Average result from 20180809 Group. The passive joint not tilted. +# --- +# DH_0 TO DH_1 FRAME +theta_1: 3.141593 +alpha_1: -1.565842 +a_1: 0.003997 +d_1: 0.000000 + +# DH_1 TO DH_2 FRAME +theta_2: 1.596102 +alpha_2: -1.576262 +a_2: 0.000215 +d_2: 0.001068 + +# DH_2 TO DH_3 FRAME +theta_3: 0.000000 +alpha_3: 0.000000 +a_3: 0.000000 +d_3: -0.007804 + +# DH_3 TO DH_4 FRAME +theta_4: -3.051239 +alpha_4: -1.559642 +a_4: 0.000204 +d_4: 0.000002 + +# DH_4 TO DH_5 FRAME +theta_5: -1.619887 +alpha_5: -1.480664 +a_5: 0.008285 +d_5: 0.000054 + + +# Scale Factors +j1_scale_factor: 1.0 +j2_scale_factor: 0.9783 +j3_scale_factor: 0.989151 diff --git a/config/dh_info_format.yaml b/config/dh_info_format.yaml new file mode 100644 index 0000000..19dda1b --- /dev/null +++ b/config/dh_info_format.yaml @@ -0,0 +1,75 @@ +# DH PARAMETER RECOMMENDATION + +# PMS1/04 (number in archive folder) +# @Date: 20180813 +# @Data group count: 1 +# @Base frame z: anti-parallel to gravity. +# @Polaris z: parallel to base frame y. +# @Notes: Only did one calibration round to get the result. Note that the d2 and d3 changed rather significantly. +# @Quality test result: 0.94 mm + +# --- +# DH_0 TO DH_1 FRAME +theta_1: 3.141593 +alpha_1: -1.563155 +a_1: 0.005612 +d_1: 0.000000 + +# DH_1 TO DH_2 FRAME +theta_2: -1.569594 +alpha_2: 1.560272 +a_2: 0.000935 +d_2: -0.002785 + +# DH_2 TO DH_3 FRAME +theta_3: 0.000000 +alpha_3: 0.000000 +a_3: 0.000000 +d_3: -0.007221 + +# DH_3 TO DH_4 FRAME +theta_4: 0.097740 +alpha_4: -1.556479 +a_4: 0.000353 +d_4: -0.000189 + +# DH_4 TO DH_5 FRAME +theta_5: -1.639551 +alpha_5: -1.475644 +a_5: 0.008613 +d_5: -0.000082 + +# Scale Factors +j1_scale_factor: 1.0022 +j2_scale_factor: 0.98 +j3_scale_factor: 0.988877 + +# DH_3 TO DH_4 FRAME +#theta_4: 3.1415926 +#alpha_4: 1.570796 +#a_4: 0.0 +#d_4: 0.0 + +# DH_4 TO DH_5 FRAME +#theta_5: 1.570796 +#alpha_5: -1.570796 +#a_5: 0.0091 +#d_5: 0.0 + + +# DH_3 TO DH_4 FRAME +#theta_4: 0.097740 +#alpha_4: -1.556479 +#a_4: 0.000353 +#d_4: -0.000189 + +# DH_4 TO DH_5 FRAME +#theta_5: -1.570796 +#alpha_5: -1.570796 +#a_5: 0.00 +#d_5: -0.000082 + +#j1_scalloe_factor: 1 +#j2_scale_factor: 1 +#j3_scale_factor: 1 + diff --git a/config/psm1_dh.yaml b/config/psm1_dh.yaml new file mode 100644 index 0000000..d8f5703 --- /dev/null +++ b/config/psm1_dh.yaml @@ -0,0 +1,40 @@ +# DH PARAMETER RECOMMENDATION + +# @Date: 20181118 + +# --- +# DH_0 TO DH_1 FRAME +theta_1: 3.141593 +alpha_1: -1.562748 +a_1: 0.005491 +d_1: 0.000000 + +# DH_1 TO DH_2 FRAME +theta_2: -1.571575 +alpha_2: 1.561116 +a_2: 0.001086 +d_2: -0.003000 + +# DH_2 TO DH_3 FRAME +theta_3: 0.000000 +alpha_3: 0.000000 +a_3: 0.000000 +d_3: -0.008534 + +# DH_3 TO DH_4 FRAME +theta_4: 0.075072 +alpha_4: -1.561300 +a_4: 0.000477 +d_4: -0.000292 + +# DH_4 TO DH_5 FRAME +theta_5: -1.617966 +alpha_5: -1.476553 +a_5: 0.008543 +d_5: -0.000179 + +# Scale Factors +j1_scale_factor: 1.00 +j2_scale_factor: 0.98 +j3_scale_factor: 0.99 + diff --git a/config/psm1_dh_param_archive.yaml b/config/psm1_dh_param_archive.yaml new file mode 100644 index 0000000..d2a0317 --- /dev/null +++ b/config/psm1_dh_param_archive.yaml @@ -0,0 +1,130 @@ +# 01 +# @20180730 +# Note: Average Result from 20180730 test group. The data set was acquired by moving the Polaris to 7 different locations that covers the angle of the Polaris to base frame y from -90 degrees to +90 degrees. The base frame z is anti-parallel to gravity. + +# --- +# DH_0 TO DH_1 FRAME +theta_1: 3.141593 +alpha_1: -1.5635662857 +a_1: 0.0053527143 +d_1: 0.000000 + +# DH_1 TO DH_2 FRAME +theta_2: -1.5653778571 +alpha_2: 1.5610581429 +a_2: 0.0013977143 +d_2: -0.0030805714 +# Note. Would be better if we can use d_2: -0.0060977143 + +# DH_2 TO DH_3 FRAME +d_3: -0.0089284286 +j3_scale_factor: 0.988825 +# --- + + + +# 02 +# @20180801 +# Note: Result from 20180801 test group. The data set was taken once at the same Polaris 0 deg configuration. The base frame z is anti-parallel to gravity. + +# --- +# DH_0 TO DH_1 FRAME +theta_1: 3.141593 +alpha_1: -1.563676 +a_1: 0.005407 +d_1: 0.000000 + +# DH_1 TO DH_2 FRAME +theta_2: -1.568422 +alpha_2: 1.561059 +a_2: 0.000816 +d_2: -0.00284 + +# DH_2 TO DH_3 FRAME +d_3: -0.008385 +j3_scale_factor: 0.989023 +# --- + + +# 03 +# @20180805 +# Note: Average Result from 20180805 test group. The data sets was taken twice at the same Polaris 0 deg configuration. The base frame z is rotated about its y for about 70 degs. + +# --- +# DH_0 TO DH_1 FRAME +theta_1: 3.141593 +alpha_1: -1.5654545 +a_1: 0.0053875 +d_1: 0.000000 + +# DH_1 TO DH_2 FRAME +theta_2: -1.569949 +alpha_2: 1.563795 +a_2: 0.0002515 +d_2: -0.0018725 + +# DH_2 TO DH_3 FRAME +d_3: -0.0080085 +j3_scale_factor: 0.9882935 +# --- + +# 04 +# @Date: 20180813 +# @Data group count: 1 +# @Base frame z: anti-parallel to gravity. +# @Polaris z: parallel to base frame y. +# @Notes: Only did one calibration round to get the result. Note that the d2 and d3 changed rather significantly. +# @Quality test result: 0.94 mm + +# --- +# DH_0 TO DH_1 FRAME +theta_1: 3.141593 +alpha_1: -1.564580 +a_1: 0.005576 +d_1: 0.000000 + +# DH_1 TO DH_2 FRAME +theta_2: -1.569678 +alpha_2: 1.560197 +a_2: 0.000905 +d_2: -0.002937 + +# DH_2 TO DH_3 FRAME +d_3: -0.008814 + +# Scale Factors +j1_scale_factor: 1.0022 +j2_scale_factor: 0.9841 +j3_scale_factor: 0.988877 + + +# 05 +# @Date: 20180813 +# @Data group count: 2 +# @Base frame z: anti-parallel to gravity. +# @Polaris z: parallel to base frame y. +# @Notes: Only did one calibration round to get the result. Note that the d2 and d3 changed rather significantly. +# @Quality test result: + +# --- +# DH_0 TO DH_1 FRAME +theta_1: 3.141593 +alpha_1: -1.564064 +a_1: 0.005576 +d_1: 0.000000 + +# DH_1 TO DH_2 FRAME +theta_2: -1.5927655 +alpha_2: 1.561273 +a_2: 0.0010625 +d_2: -0.0029585 + +# DH_2 TO DH_3 FRAME +d_3: -0.0075395 + +# Scale Factors +j1_scale_factor: 1.0022 +j2_scale_factor: 0.9842 +j3_scale_factor: 0.9888915 + + diff --git a/config/psm1_dh_sim.yaml b/config/psm1_dh_sim.yaml new file mode 100644 index 0000000..b3ca2f9 --- /dev/null +++ b/config/psm1_dh_sim.yaml @@ -0,0 +1,22 @@ +# DH PARAMETER RECOMMENDATION +# This file is generated by matlab codes for da Vinci robot intrinsic calibration tool. +#23-Jul-2018 06:28:29 +# YOU MUST CHANGE THE FILENAME EXTENSION TO .YAML BEFORE YOU USE IT AS A DAVINCI KINEMATICS CONFIG FILE. +# --- +# DH_0 TO DH_1 FRAME +theta_1: 3.14159265359 +alpha_1: -1.57079632679 +a_1: 0.0053 +d_1: 0.000000 + +# DH_1 TO DH_2 FRAME +theta_2: -1.57079632679 +alpha_2: 1.57079632679 +a_2: 0.0008 +d_2: -0.0029 +j2_scale_factor: 0.9732 # Temp + + +# DH_2 TO DH_3 FRAME +d_3: -0.0087 +j3_scale_factor: 1 diff --git a/config/psm2_dh.yaml b/config/psm2_dh.yaml new file mode 100644 index 0000000..39ce2e8 --- /dev/null +++ b/config/psm2_dh.yaml @@ -0,0 +1,41 @@ +# 02 +# Note: Average result from 20180809 Group. The passive joint not tilted. + +# @Date:20181118 + +# --- +# DH_0 TO DH_1 FRAME +theta_1: 3.141593 +alpha_1: -1.565608 +a_1: 0.004181 +d_1: 0.000000 + +# DH_1 TO DH_2 FRAME +theta_2: 1.590870 +alpha_2: -1.576451 +a_2: 0.000128 +d_2: 0.001209 + +# DH_2 TO DH_3 FRAME +theta_3: 0.000000 +alpha_3: 0.000000 +a_3: 0.000000 +d_3: -0.007237 + +# DH_3 TO DH_4 FRAME +theta_4: -3.048162 +alpha_4: -1.561206 +a_4: 0.000011 +d_4: 0.000107 + +# DH_4 TO DH_5 FRAME +theta_5: -1.638860 +alpha_5: -1.476038 +a_5: 0.008469 +d_5: -0.000116 + + +# Scale Factors +j1_scale_factor: 0.999 +j2_scale_factor: 0.976 +j3_scale_factor: 0.989 diff --git a/config/psm2_dh_param_archive.yaml b/config/psm2_dh_param_archive.yaml new file mode 100644 index 0000000..27319a9 --- /dev/null +++ b/config/psm2_dh_param_archive.yaml @@ -0,0 +1,38 @@ +# 01 +# Note: Average result from 20180806 Group. The passive joint is tilted so that the base frame z is tilted for about 70 degrees. +# --- +# DH_0 TO DH_1 FRAME +theta_1: 3.141593 +alpha_1: -1.5639015 +a_1: 0.00322 +d_1: 0.000000 + +# DH_1 TO DH_2 FRAME +theta_2: 1.5989585 +alpha_2: -1.574802 +a_2: 0.000648 +d_2: 0.0010885 + +# DH_2 TO DH_3 FRAME +d_3: -0.0094045 +j3_scale_factor: 0.989672 + + +# 02 +# Note: Average result from 20180809 Group. The passive joint not tilted. +# --- +# DH_0 TO DH_1 FRAME +theta_1: 3.141593 +alpha_1: -1.5655045 +a_1: 0.003839 +d_1: 0.000000 + +# DH_1 TO DH_2 FRAME +theta_2: 1.5928065 +alpha_2: -1.575889 +a_2: 0.000337 +d_2: 0.001284 + +# DH_2 TO DH_3 FRAME +d_3: -0.0089265 +j3_scale_factor: 0.9894155 diff --git a/config/psm_generic.yaml b/config/psm_generic.yaml new file mode 100644 index 0000000..d848c55 --- /dev/null +++ b/config/psm_generic.yaml @@ -0,0 +1,17 @@ +# This file contains a generic definition of a PSM DH parameters. +# It should be placed under /cwru_davinci_kinematics/config/ folder. + +# DH_0 TO DH_1 FRAME +theta_1: 0.0 +alpha_1: 1.570796 # pi/2 +a_1: 0.0 +d_1: 0.0 +# DH_1 TO DH_2 FRAME +theta_2: 1.570796 +alpha_2: 1.570796 +a_2: 0 +d_2: 0 +j2_scale_factor: 1.0 +# DH_2 TO DH_3 FRAME +d_3: -0.0156 +j3_scale_factor: 1.0 diff --git a/examples/davinci_fwd_fix_trial.cpp b/examples/davinci_fwd_fix_trial.cpp new file mode 100644 index 0000000..3732483 --- /dev/null +++ b/examples/davinci_fwd_fix_trial.cpp @@ -0,0 +1,250 @@ +// RN +#include +#include +#include +#include +#include +#include + +#include + +int main(int argc, char **argv) +{ + davinci_kinematics::Forward dvrk_forward; + davinci_kinematics::Inverse dvrk_inverse; + + Eigen::Affine3d affine_wrist_wrt_base, affine_gripper_wrt_base; + + davinci_kinematics::Vectorq7x1 q_vec, q_vec_up, q_vec_bottom; + // 0.0, 0.0, 0.05, -0.9, 1.5, 0, 0.139, + // q_vec_up(0) = 0; + // q_vec_up(1) = 0; + // q_vec_up(2) = 0.05; + // q_vec_up(3) = -0.9; + // q_vec_up(4) = 1.5; + // q_vec_up(5) = 0; + // q_vec_up(6) = 0; + + // 0.0, 0.0, 0.21, 0, 0, 0, 0.139, 0,0,0,0,0,0,0, 10 + // q_vec_bottom(0) = 0; + // q_vec_bottom(1) = 0; + // q_vec_bottom(2) = 0.21; + // q_vec_bottom(3) = -0.9; + // q_vec_bottom(4) = 1.5; + // q_vec_bottom(5) = 0; + // q_vec_bottom(6) = 0; + + // affine_gripper_wrt_base = dvrk_forward.fwd_kin_solve(q_vec_up); + + // affine_wrist_wrt_base = dvrk_forward.get_wrist_wrt_base(); + + // std::cout << "q_vec_up" << std::endl; + // std::cout << q_vec_up.transpose() << std::endl << std::endl; + + // std::cout << "q_vec_up.affine_wrist_wrt_base.linear():" << std::endl; + // std::cout << affine_wrist_wrt_base.linear() << std::endl << std::endl; + // std::cout << "q_vec_up.affine_wrist_wrt_base.translation():" << std::endl; + // std::cout << affine_wrist_wrt_base.translation() << std::endl << std::endl; + + // std::cout << "q_vec_up.affine_gripper_wrt_base.linear():" << std::endl; + // std::cout << affine_gripper_wrt_base.linear() << std::endl << std::endl; + // std::cout << "q_vec_up.affine_gripper_wrt_base.translation():" << std::endl; + // std::cout << affine_gripper_wrt_base.translation() << std::endl << std::endl; + + // affine_gripper_wrt_base = dvrk_forward.fwd_kin_solve(q_vec_bottom); + + // affine_wrist_wrt_base = dvrk_forward.get_wrist_wrt_base(); + + // std::cout << "q_vec_bottom" << std::endl; + // std::cout << q_vec_bottom.transpose() << std::endl << std::endl; + + // std::cout << "q_vec_bottom.affine_wrist_wrt_base.linear():" << std::endl; + // std::cout << affine_wrist_wrt_base.linear() << std::endl << std::endl; + // std::cout << "q_vec_bottom.affine_wrist_wrt_base.translation():" << std::endl; + // std::cout << affine_wrist_wrt_base.translation() << std::endl << std::endl; + + // std::cout << "q_vec_bottom.affine_gripper_wrt_base.linear():" << std::endl; + // std::cout << affine_gripper_wrt_base.linear() << std::endl << std::endl; + // std::cout << "q_vec_bottom.affine_gripper_wrt_base.translation():" << std::endl; + // std::cout << affine_gripper_wrt_base.translation() << std::endl << std::endl; + + q_vec(0) = 0; + q_vec(1) = 0; + q_vec(2) = 0.05; + q_vec(3) = 0; + q_vec(4) = 0; + q_vec(5) = 0; + q_vec(6) = 0; + + for (int i = 0; i < 4; i++) + { + affine_gripper_wrt_base = dvrk_forward.fwd_kin_solve(q_vec); + affine_wrist_wrt_base = dvrk_forward.get_wrist_wrt_base(); + std::cout << "-----------------------------------------" << std::endl; + std::cout << "q_vec#" << i+1 << ": " << std::endl; + std::cout << q_vec.transpose() << std::endl << std::endl; + + std::cout << "q_vec#" << i+1 << ".affine_wrist_wrt_base.translation():" << std::endl; + std::cout << affine_wrist_wrt_base.translation() << std::endl << std::endl; + // std::cout << "q_vec#" << i+1 <<".affine_wrist_wrt_base.linear():" << std::endl; + // std::cout << affine_wrist_wrt_base.linear() << std::endl << std::endl; + + std::cout << "-----------------------------------------" << std::endl; + + q_vec(2) = q_vec(2) + 0.06; + + + } + + std::cout << "----------------RANDOM-POINTS--------------" << std::endl; + + std::cout << "-------PT01---------" << std::endl; + q_vec(0) = -1.0800; + q_vec(1) = -0.6600; + q_vec(2) = 0.0842; + affine_gripper_wrt_base = dvrk_forward.fwd_kin_solve(q_vec); + affine_wrist_wrt_base = dvrk_forward.get_wrist_wrt_base(); + std::cout << "q_vec#" << ": " << std::endl; + std::cout << q_vec.transpose() << std::endl << std::endl; + + std::cout << "q_vec#" << ".affine_wrist_wrt_base.translation():" << std::endl; + std::cout << affine_wrist_wrt_base.translation() << std::endl << std::endl; + std::cout << "--------------------" << std::endl; + + std::cout << "-------PT02---------" << std::endl; + q_vec(0) = 1.3500; + q_vec(1) = 0.3000; + q_vec(2) = 0.1166; + affine_gripper_wrt_base = dvrk_forward.fwd_kin_solve(q_vec); + affine_wrist_wrt_base = dvrk_forward.get_wrist_wrt_base(); + std::cout << "q_vec#" << ": " << std::endl; + std::cout << q_vec.transpose() << std::endl << std::endl; + + std::cout << "q_vec#" << ".affine_wrist_wrt_base.translation():" << std::endl; + std::cout << affine_wrist_wrt_base.translation() << std::endl << std::endl; + std::cout << "--------------------" << std::endl; + + std::cout << "-------PT03---------" << std::endl; + q_vec(0) = 1.3800; + q_vec(1) = 0.4800; + q_vec(2) = 0.1634; + affine_gripper_wrt_base = dvrk_forward.fwd_kin_solve(q_vec); + affine_wrist_wrt_base = dvrk_forward.get_wrist_wrt_base(); + std::cout << "q_vec#" << ": " << std::endl; + std::cout << q_vec.transpose() << std::endl << std::endl; + + std::cout << "q_vec#" << ".affine_wrist_wrt_base.translation():" << std::endl; + std::cout << affine_wrist_wrt_base.translation() << std::endl << std::endl; + std::cout << "--------------------" << std::endl; + + std::cout << "-------PT04---------" << std::endl; + q_vec(0) = 0.2400; + q_vec(1) = 0.3000; + q_vec(2) = 0.1922; + affine_gripper_wrt_base = dvrk_forward.fwd_kin_solve(q_vec); + affine_wrist_wrt_base = dvrk_forward.get_wrist_wrt_base(); + std::cout << "q_vec#" << ": " << std::endl; + std::cout << q_vec.transpose() << std::endl << std::endl; + + std::cout << "q_vec#" << ".affine_wrist_wrt_base.translation():" << std::endl; + std::cout << affine_wrist_wrt_base.translation() << std::endl << std::endl; + std::cout << "--------------------" << std::endl; + + std::cout << "-------PT05---------" << std::endl; + q_vec(0) = -1.3200; + q_vec(1) = -0.0800; + q_vec(2) = 0.0662; + affine_gripper_wrt_base = dvrk_forward.fwd_kin_solve(q_vec); + affine_wrist_wrt_base = dvrk_forward.get_wrist_wrt_base(); + std::cout << "q_vec#" << ": " << std::endl; + std::cout << q_vec.transpose() << std::endl << std::endl; + + std::cout << "q_vec#" << ".affine_wrist_wrt_base.translation():" << std::endl; + std::cout << affine_wrist_wrt_base.translation() << std::endl << std::endl; + std::cout << "--------------------" << std::endl; + + std::cout << "-------PT06---------" << std::endl; + q_vec(0) = -0.7800; + q_vec(1) = 0.1000; + q_vec(2) = 0.2174; + affine_gripper_wrt_base = dvrk_forward.fwd_kin_solve(q_vec); + affine_wrist_wrt_base = dvrk_forward.get_wrist_wrt_base(); + std::cout << "q_vec#" << ": " << std::endl; + std::cout << q_vec.transpose() << std::endl << std::endl; + + std::cout << "q_vec#" << ".affine_wrist_wrt_base.translation():" << std::endl; + std::cout << affine_wrist_wrt_base.translation() << std::endl << std::endl; + std::cout << "--------------------" << std::endl; + + std::cout << "-------PT07---------" << std::endl; + q_vec(0) = -0.4200; + q_vec(1) = -0.4000; + q_vec(2) = 0.1904; + affine_gripper_wrt_base = dvrk_forward.fwd_kin_solve(q_vec); + affine_wrist_wrt_base = dvrk_forward.get_wrist_wrt_base(); + std::cout << "q_vec#" << ": " << std::endl; + std::cout << q_vec.transpose() << std::endl << std::endl; + + std::cout << "q_vec#" << ".affine_wrist_wrt_base.translation():" << std::endl; + std::cout << affine_wrist_wrt_base.translation() << std::endl << std::endl; + std::cout << "--------------------" << std::endl; + + std::cout << "-------PT08---------" << std::endl; + q_vec(0) = 0.9900; + q_vec(1) = 0.5000; + q_vec(2) = 0.1382; + affine_gripper_wrt_base = dvrk_forward.fwd_kin_solve(q_vec); + affine_wrist_wrt_base = dvrk_forward.get_wrist_wrt_base(); + std::cout << "q_vec#" << ": " << std::endl; + std::cout << q_vec.transpose() << std::endl << std::endl; + + std::cout << "q_vec#" << ".affine_wrist_wrt_base.translation():" << std::endl; + std::cout << affine_wrist_wrt_base.translation() << std::endl << std::endl; + std::cout << "--------------------" << std::endl; + + std::cout << "-------PT09---------" << std::endl; + q_vec(0) = -1.4400; + q_vec(1) = -0.6200; + q_vec(2) = 0.1292; + affine_gripper_wrt_base = dvrk_forward.fwd_kin_solve(q_vec); + affine_wrist_wrt_base = dvrk_forward.get_wrist_wrt_base(); + std::cout << "q_vec#" << ": " << std::endl; + std::cout << q_vec.transpose() << std::endl << std::endl; + + std::cout << "q_vec#" << ".affine_wrist_wrt_base.translation():" << std::endl; + std::cout << affine_wrist_wrt_base.translation() << std::endl << std::endl; + std::cout << "--------------------" << std::endl; + + std::cout << "-------PT10---------" << std::endl; + q_vec(0) = -1.3500; + q_vec(1) = 0.3800; + q_vec(2) = 0.1310; + affine_gripper_wrt_base = dvrk_forward.fwd_kin_solve(q_vec); + affine_wrist_wrt_base = dvrk_forward.get_wrist_wrt_base(); + std::cout << "q_vec#" << ": " << std::endl; + std::cout << q_vec.transpose() << std::endl << std::endl; + + std::cout << "q_vec#" << ".affine_wrist_wrt_base.translation():" << std::endl; + std::cout << affine_wrist_wrt_base.translation() << std::endl << std::endl; + std::cout << "--------------------" << std::endl; + + + std::cout << "-------PT11--------" << std::endl; + q_vec(0) = -0.0; + q_vec(1) = 0.0; + q_vec(2) = 0.15; + affine_gripper_wrt_base = dvrk_forward.fwd_kin_solve(q_vec); + affine_wrist_wrt_base = dvrk_forward.get_wrist_wrt_base(); + std::cout << "q_vec#" << ": " << std::endl; + std::cout << q_vec.transpose() << std::endl << std::endl; + + std::cout << "q_vec#" << ".affine_wrist_wrt_base.translation():" << std::endl; + std::cout << affine_wrist_wrt_base.translation() << std::endl << std::endl; + std::cout << "--------------------" << std::endl; + + + + + + +} // main diff --git a/examples/davinci_ik_refined_test.cpp b/examples/davinci_ik_refined_test.cpp new file mode 100644 index 0000000..9e12d78 --- /dev/null +++ b/examples/davinci_ik_refined_test.cpp @@ -0,0 +1,178 @@ +// RN +#include +#include +#include +#include +#include +#include + +#include + +int main(int argc, char **argv) +{ + davinci_kinematics::Forward dvrk_forward; + davinci_kinematics::Inverse dvrk_inverse; + + Eigen::Vector3d w_wrt_base, q123; + + Eigen::Vector3d tip_coordinate; + + davinci_kinematics::Vectorq7x1 q_vec, err_vec, q_vec_ik, q_vec_ik_refined, q_vec_frozen_ik_refined; + davinci_kinematics::Vectorq7x1 err_vec_refined_ik; + q_vec << 0, 0, 0, 0, 0, 0, 0; + int err_cnt = 0; + + int err_cnt_refined = 0; + int refined_total_count = 0; + int entry_count = 0; + + Eigen::Affine3d affine_wrist_wrt_base, affine_gripper_wrt_base, affine_frame_wrt_base, affine_gripper_wrt_base1; + Eigen::Affine3d affine_wrist_wrt_base2, affine_gripper_wrt_base2, affine_frame_wrt_base2; + Eigen::Affine3d affine_gripper_wrt_base_frozen; + // wait to start receiving valid tf transforms + + Eigen::Vector3d tip_from_FK, tip_from_FK_of_IK, tip_from_FK_of_refined_IK, tip_err, tip_err_refined; + // note: with print-outs, takes about 45sec for 10,000 iterations, and got 0 errors + for (int itries = 0; itries < 1; itries++) + { + dvrk_forward.gen_rand_legal_jnt_vals(q_vec); + + // std::cout << "using q_vec = " << q_vec.transpose() << std::endl; + + // printf("gripper tip frame from FK: \n"); + q_vec << -0.168323, 0.0463498, 0.153502, 0.951687, -0.355543, -1.25153, 0; + + std::cout << "HELLO q_vec: " << q_vec.transpose() << std::endl; + + affine_gripper_wrt_base = dvrk_forward.fwd_kin_solve(q_vec); + // std::cout << "affine linear (R): " << std::endl; + // std::cout << affine_gripper_wrt_base.linear() << std::endl; + tip_from_FK = affine_gripper_wrt_base.translation(); + std::cout << "origin: " << tip_from_FK.transpose() << std::endl; + std::cout << std::endl; + std::cout << std::endl; + + if (dvrk_inverse.ik_solve(affine_gripper_wrt_base) > 0) + { + // std::cout << "INTO IF.. "<< std::endl; + // Validate the legal solution. + entry_count++; + q_vec_ik = dvrk_inverse.get_soln(); + + // printf("FK of IK soln: \n"); + affine_gripper_wrt_base1 = dvrk_forward.fwd_kin_solve(q_vec_ik); + tip_from_FK_of_IK = affine_gripper_wrt_base1.translation(); + err_vec = q_vec - q_vec_ik; + + // std::cout << std::endl; + // std::cout << std::endl; + tip_err = tip_from_FK - tip_from_FK_of_IK; + + if (tip_err.norm() > 0.001) + { + printf("excessive error!"); + err_cnt++; + } + + int refinement_outcome = 9; + + refinement_outcome = dvrk_inverse.ik_solve_refined(affine_gripper_wrt_base); + + + if (refinement_outcome > 0) + { + refined_total_count ++; + + q_vec_ik_refined = dvrk_inverse.get_soln_refined(); + affine_gripper_wrt_base2 = dvrk_forward.fwd_kin_solve(q_vec_ik_refined); + + tip_from_FK_of_refined_IK = affine_gripper_wrt_base2.translation(); + + // std::cout << "q_vec in: " << q_vec.transpose() << std::endl; + // std::cout << "q_vec_ik: " << q_vec_ik.transpose() << std::endl; + // std::cout << "q_vec_ik_refined: " << q_vec_ik_refined.transpose() << std::endl; + + + err_vec_refined_ik = q_vec - q_vec_ik_refined; + // std::cout << "err vec: " << err_vec.transpose() << std::endl; + // std::cout << "err_vec_refined_ik: " << err_vec_refined_ik.transpose() << std::endl; + // std::cout << std::endl; + tip_err_refined = tip_from_FK - tip_from_FK_of_refined_IK; + // std::cout << "tip err: " << tip_err.transpose() << std::endl; + // std::cout << "tip_err_refined err: " << tip_err_refined.transpose() << std::endl; + // std::cout << std::endl; + + // printf("jspace errvec norm: %f\n", err_vec.norm()); + // printf("tip pos err norm: %f:\n", tip_err.norm()); + // printf("jspace errvec norm refined: %f\n", err_vec_refined_ik.norm()); + // printf("tip pos err norm refined: %f:\n", tip_err_refined.norm()); + // std::cout << std::endl; + + // if (err_vec_refined_ik.norm() + tip_err_refined.norm() > 0.001) + if (tip_err_refined.norm() > 0.001) + { + printf("excessive error after refinement!"); + err_cnt_refined++; + } + + + // printf("itries = %d; err_cnt = %d", itries, err_cnt); + // std::cout << std::endl << "refined_total_count: " << refined_total_count << std::endl + // << "entry_count: " << entry_count << std::endl; + // std::cout << "err_cnt_refined: " << err_cnt_refined << std::endl; + + } else {std::cout << "NO BETTER SOLUTION!" << std::endl;} + + + } + + + tip_coordinate = tip_from_FK; + + std::cout << "tip_coordinate: \n" << tip_coordinate << std::endl; + + std::cout << "solving frozen IK for this tip coordinate.. \n"; + + dvrk_inverse.ik_solve_frozen_refined(tip_coordinate); + + q_vec_frozen_ik_refined = dvrk_inverse.get_soln_frozon_ik_refined(); + + std::cout << "q_vec_frozen_ik_refined: \n" << q_vec_frozen_ik_refined << std::endl; + + affine_gripper_wrt_base_frozen = dvrk_forward.fwd_kin_solve(q_vec_frozen_ik_refined); + + + std::cout << "TARGET FROZEN tip_coordinate: \n" << tip_coordinate << std::endl << std::endl; + std::cout << "affine_gripper_wrt_base_frozen.translation: \n" << affine_gripper_wrt_base_frozen.translation() << std::endl; + std::cout << "affine_gripper_wrt_base_frozen.linear: \n" << affine_gripper_wrt_base_frozen.linear() << std::endl << std::endl; + std::cout << "affine_gripper_wrt_base.translation: \n" << affine_gripper_wrt_base.translation() << std::endl; + std::cout << "affine_gripper_wrt_base.linear: \n" << affine_gripper_wrt_base.linear() << std::endl; + } + + + + double rate_0 = (entry_count - err_cnt)/double(entry_count); + double rate_1 = (entry_count - err_cnt_refined)/double(entry_count); + + auto t = std::time(nullptr); + auto tm = *std::localtime(&t); + + std::cout << std::endl << "\e[1m--- --- --- --- --- --- IK REFINED TEST REPORT --- --- --- --- --- ---\e[0m" << std::endl ; + std::cout << std::put_time(&tm, "%d-%m-%Y %H-%M-%S") << std::endl << std::endl; + std::cout << "Total trial number: " << entry_count << std::endl; + std::cout << "Without additional Jacobian correction, \e[31m\e[1m" << err_cnt << " \e[0mof them failed the error assessment*." << std::endl; + std::cout << "After adding the Jacobian addon, \e[31m\e[1m" << err_cnt_refined << " \e[0mof them did NOT pass." << std::endl; + std::cout.precision(6); + std::cout << "Success rate has been raised from \e[4m" << rate_0 << "\e[0m to \e[4m" << rate_1 << "\e[0m"<< std::endl; + + std::cout << std::endl << " * error assessment: " << std::endl + << " Only the translational error (the norm of the tip coordinates of the desired pose and the one obtained from the IK.):" + << std::endl + << " tip_err.norm() > 0.001" + << std::endl; + + + // printf("err_cnt = %d", err_cnt); + std::cout << std::endl; + return 0; + } diff --git a/examples/davinci_inv_kinematics_demo.cpp b/examples/davinci_inv_kinematics_demo.cpp index 05ccbd8..7f73e47 100644 --- a/examples/davinci_inv_kinematics_demo.cpp +++ b/examples/davinci_inv_kinematics_demo.cpp @@ -26,7 +26,7 @@ int main(int argc, char **argv) Eigen::Vector3d tip_from_FK, tip_from_FK_of_IK, tip_err; // note: with print-outs, takes about 45sec for 10,000 iterations, and got 0 errors - for (int itries = 0; itries < 10000; itries++) + for (int itries = 0; itries < 50; itries++) { dvrk_forward.gen_rand_legal_jnt_vals(q_vec); @@ -74,5 +74,7 @@ int main(int argc, char **argv) } } printf("err_cnt = %d", err_cnt); + + std::cout << "hello"; return 0; } diff --git a/examples/davinci_jacobian_demo.cpp b/examples/davinci_jacobian_demo.cpp index 8de4d17..0ff249d 100644 --- a/examples/davinci_jacobian_demo.cpp +++ b/examples/davinci_jacobian_demo.cpp @@ -41,6 +41,8 @@ int main(int argc, char **argv) davinci_kinematics::Forward davinci_fwd_solver; Eigen::MatrixXd Jacobian; + Eigen::MatrixXd rn_test; + Eigen::Vector3d dp_fk, dp_J, dp_err_vec; Eigen::Matrix3d R1, R2, dR; Eigen::Vector3d dphi_J, dphi_fk, dphi_err_vec; @@ -60,7 +62,7 @@ int main(int argc, char **argv) // holders for FK computations Eigen::Affine3d affine_fk1, affine_fk2; - for (int itries = 0; itries < 10000; itries++) + for (int itries = 0; itries < 50; itries++) // used to be 10000 { davinci_fwd_solver.gen_rand_legal_jnt_vals(q_vec1); std::cout << "using q_vec = " << q_vec1.transpose() << std::endl; @@ -130,5 +132,10 @@ int main(int argc, char **argv) std::cout << "dphi err norm = " << phi_err << std::endl; // express as a normalized error (scalar) std::cout << "fractional error: "<< phi_err/(dphi_fk.norm()) << std::endl << std::endl; + + std::cout << "The inverse of B is:\n" << Jacobian.inverse() << std::endl; + rn_test = Jacobian*Jacobian.inverse(); + std::cout << "The B*inv(B):\n" << rn_test << std::endl; + } } diff --git a/examples/davinci_jacobian_test_rn.cpp b/examples/davinci_jacobian_test_rn.cpp new file mode 100644 index 0000000..a73ebf8 --- /dev/null +++ b/examples/davinci_jacobian_test_rn.cpp @@ -0,0 +1,239 @@ +// +// Created by william on 06/11/18. +// + +// @TODO Add License Text. +// Copyright Wyatt S. Newman 2015 and Russell Jackson 2017 +// davinci_Jacobian_test_main.cpp +// get random legal joint values, q_vec; compute fk1 +// generate random perturbations, dq; compute fk2 +// compute dp_fk = fk2_translational()-fk1_translational() +// compute Jacobian +// compute dp_J = J*dq +// compare dp_J to dp_fk +// do analogous comparison for dphi1, dphi2, dphi3 + +// key lines of this program (do the following to use this Jacobian): +// Davinci_fwd_solver davinci_fwd_solver; //instantiate a forward-kinematics solver +// Jacobian = davinci_fwd_solver.compute_jacobian(q_vec1); //compute the Jacobian +// dp6x1 = Jacobian*dq_vec6x1; // here is the Jacobian-based approximation of incremental Cartesian pose change + + +#include +#include +#include + + +// fnc to generate random joint perturbations in range set by eps +void gen_rand_joint_perturbations(davinci_kinematics::Vectorq7x1 &dqvec, double eps = 0.000001) +{ + static unsigned int seed = time(NULL); + dqvec(6) = 0; + for (int i = 0; i < 6; i++) + { + dqvec(i) = eps * (1.0 - 0.5*static_cast (rand_r(&seed)) / static_cast (RAND_MAX)); + } + + + + // TODO delete + std::cout << "dqvec: " << std::endl; + std::cout << dqvec.transpose() << std::endl; + +} + + +void gen_rand_single_joint_perturbations(davinci_kinematics::Vectorq7x1 &dqvec, int joint_number, double eps = 0.000001) +{ + static unsigned int seed = time(NULL); + dqvec(6) = 0; + + + + for (int i = 0; i < 6; i++) + { + dqvec(i) = 0; + } + // joint_number from 0 as the first joint. + dqvec(joint_number) = eps * (1.0 - 0.5*static_cast (rand_r(&seed)) / static_cast (RAND_MAX)); + + // TODO delete + std::cout << "dqvec: " << std::endl; + std::cout << dqvec.transpose() << std::endl; + +} + + +/** + * @brief This program demonstrates the numerical jacobian + */ +int main(int argc, char **argv) +{ + ros::init(argc, argv, "davinci_Jacobian_test_rn"); + + davinci_kinematics::Forward davinci_fwd_solver; + Eigen::MatrixXd Jacobian; + + Eigen::MatrixXd rn_test; + + Eigen::Vector3d dp_fk, dp_J, dp_err_vec; + Eigen::Matrix3d R1, R2, dR; + Eigen::Vector3d dphi_J, dphi_fk, dphi_err_vec; + double p_err, phi_err; + int ans; + + davinci_fwd_solver.resetDhOffsetsMaps(); + davinci_fwd_solver.loadDHyamlfiles("dh_info_format","psm1_dh"); + + + davinci_kinematics::Vectorq7x1 q_vec1, q_vec2, dq_vec; + Eigen::VectorXd dq_vec6x1, dp6x1; + for (int i = 0; i < 7; i++) q_vec1(i) = 0.0; + + q_vec2 = q_vec1; + dq_vec = q_vec1; + + // 6x1 perturbation vector to be used with Jacobian + dq_vec6x1.resize(6, 1); + dp6x1.resize(6, 1); + // holders for FK computations + Eigen::Affine3d affine_fk1, affine_fk2; + + int tries_target(1); + + for (int itries = 0; itries < tries_target; itries++) // used to be 10000 + { + davinci_fwd_solver.gen_rand_legal_jnt_vals(q_vec1); + + std::cout << "using q_vec = " << q_vec1.transpose() << std::endl; + + std::vector p_fractional_err; + p_fractional_err.clear(); + std::vector r_fractional_err; + r_fractional_err.clear(); + + + + + affine_fk1 = davinci_fwd_solver.fwd_kin_solve(q_vec1, "psm1_dh"); +// affine_fk1 = davinci_fwd_solver.fwd_kin_solve(q_vec1); + + + + + for (int jt_n = 0; jt_n < 5; jt_n ++) { + ROS_WARN("Joint#%d", jt_n+1); + + gen_rand_single_joint_perturbations(dq_vec, jt_n); + + q_vec2 = q_vec1 + dq_vec; + // strip off 7'th joint variable, gripper opening angle, to get 6x1 vector of joint perturbations + for (int i = 0; i < 6; i++) dq_vec6x1(i) = dq_vec(i); + + // TODO make public + + dq_vec6x1(0) = davinci_fwd_solver.j1_scale_factor_map_["psm1_dh"] * dq_vec6x1(0); + dq_vec6x1(1) = davinci_fwd_solver.j2_scale_factor_map_["psm1_dh"] * dq_vec6x1(1); + dq_vec6x1(2) = davinci_fwd_solver.j3_scale_factor_map_["psm1_dh"] * dq_vec6x1(2); + + // compute FK for two joint-space poses, q_vec1, q_vec2, that are + // very close to each other (random perturbations) + // pose from first q_vec + ROS_INFO("gripper tip frame from FK: "); + + affine_fk2 = davinci_fwd_solver.fwd_kin_solve(q_vec2, "psm1_dh"); +// affine_fk2 = davinci_fwd_solver.fwd_kin_solve(q_vec2); + + // orientation from first q_vec + std::cout << "affine linear (R): " << std::endl; + std::cout << affine_fk1.linear() << std::endl << std::endl; + std::cout << "origin: " << affine_fk1.translation().transpose() << std::endl << std::endl; + // compute numerical perturbation of output, based on two FK calls: + dp_fk = affine_fk2.translation() - affine_fk1.translation(); + std::cout << "tip displacement due to perturbation of angles, per FK:" << std::endl; + std::cout << "dp_fk = " << dp_fk.transpose() << std::endl << std::endl; + // What rotation operator is required to change from orientation 1 to orientation 2? + dR = affine_fk2.linear() * affine_fk1.linear().transpose(); + std::cout << "Rotation operator for perturbation, per FK: " << std::endl; + std::cout << dR << std::endl; + // For perturbations, can get dPhi 3x1 vector from components of dR operator + dphi_fk(0) = -dR(1, 2); + dphi_fk(1) = dR(0, 2); + dphi_fk(2) = -dR(0, 1); + std::cout << "extracted dPhi vector from rotation operator:" << std::endl; + std::cout << "dphi_fk = " << dphi_fk.transpose() << std::endl; + + // here's a call to the function under test: computing the Jacobian about q_vec1 + // compute the Jacobian + Jacobian = davinci_fwd_solver.compute_jacobian(q_vec1, "psm1_dh"); +// Jacobian = davinci_fwd_solver.compute_jacobian(q_vec1); + + std::cout << "computed Jacobian: " << std::endl; + std::cout << Jacobian << std::endl << std::endl; + std::cout << "perturbation of first 6 joints: "; + + std::cout << "dq_vec6x1 = " << dq_vec6x1.transpose() << std::endl; + + + // here is the Jacobian-based approximation of incremental Cartesian pose change + dp6x1 = Jacobian * dq_vec6x1; + // first three elements are dx,dy,dz, and next 3 are dphix, dphiy, dphiz + std::cout << "6x1 perturbation of pose per Jacobian and joint perturbations:" << std::endl; + std::cout << "dp6x1 = " << dp6x1.transpose() << std::endl; + // strip off dx, dy, dz + dp_J = dp6x1.block<3, 1>(0, 0); + // compare Jacobian dp(dq) to FK dp(dq) + dp_err_vec = dp_fk - dp_J; + std::cout << "comparison of endpoint displacements, FK vs Jacobian: " << std::endl; + std::cout << "dp_err_vec = " << dp_err_vec.transpose() << std::endl; + p_err = dp_err_vec.norm(); + std::cout << "dp err norm = " << p_err << std::endl; + // express this as displacement disagreement vs magnitude of displacement + // e.g., percentage err (but expressed as a fraction, not percentage) + std::cout << "fractional error: " << p_err / (dp_fk.norm()) << std::endl << std::endl; + + std::cout << " = p_err/dp_fk.norm()" << std::endl; + std::cout << " p_err: " << p_err << std::endl; + std::cout << " dp_fk.norm(): " << dp_fk.norm() << std::endl; + + p_fractional_err.push_back(p_err / (dp_fk.norm()) ); + + // repeat for eval of angular Jacobian: + // (dphi_x, dphi_y, dphi_z) + dphi_J = dp6x1.block<3, 1>(3, 0); + dphi_err_vec = dphi_fk - dphi_J; + std::cout << "comparison of incremental rotation vectors, FK vs Jacobian: " << std::endl; + std::cout << "dphi_err_vec = " << dphi_err_vec.transpose() << std::endl; + phi_err = dphi_err_vec.norm(); + std::cout << "dphi err norm = " << phi_err << std::endl; + // express as a normalized error (scalar) + std::cout << "fractional error: " << phi_err / (dphi_fk.norm()) << std::endl << std::endl; + + std::cout << " = phi_err/dphi_fk.norm()" << std::endl; + std::cout << " phi_err: " << phi_err << std::endl; + std::cout << " dphi_fk.norm(): " << dphi_fk.norm() << std::endl; + + r_fractional_err.push_back(phi_err / (dphi_fk.norm()) ); + +// std::cout << "The inverse of B is:\n" << Jacobian.inverse() << std::endl; +// rn_test = Jacobian*Jacobian.inverse(); +// std::cout << "The B*inv(B):\n" << rn_test << std::endl; + } + + + std::cout << std::endl << "SUMMARY ====== " << std::endl; + for (int n = 0; n < 5; n ++) { + + std::cout << "p_fractional_err Joint#" << n+1 << " :" << p_fractional_err[n] << std::endl; + + } + std::cout << "------- " << std::endl; + for (int n = 0; n < 5; n ++) { + + std::cout << "r_fractional_err Joint#" << n+1 << " :" << r_fractional_err[n] << std::endl; + + } + + } +} + diff --git a/examples/psm_intrinsic_calibration_auxiliary.cpp b/examples/psm_intrinsic_calibration_auxiliary.cpp new file mode 100644 index 0000000..19b63a8 --- /dev/null +++ b/examples/psm_intrinsic_calibration_auxiliary.cpp @@ -0,0 +1,155 @@ +// +// Created by william on 30/07/18. +// + +#include +#include +#include +#include +#include + +#include + + +// Eigen::Affine3d printInfo(Eigen::Vector3d test_pt) { +// +// davinci_kinematics::Forward dvrk_forward; +// davinci_kinematics::Inverse dvrk_inverse; +// +// Eigen::Affine3d affine_wrist_wrt_base, affine_tip_wrt_base; +// +// davinci_kinematics::Vectorq7x1 q_vec_ik; +// +// dvrk_inverse.resetDhOffsetsMaps(); +// +// // loadDHyamlfiles(std::string yaml_name, std::string kinematic_set_name) +// +// +// dvrk_inverse.loadDHyamlfiles("psm1_dh","test_set"); +// // dvrk_inverse.loadDHyamlfiles("psm2_dh","test_set"); +// +// if (dvrk_inverse.ik_solve_frozen_refined(test_pt, "test_set") > 0) { +// +// ROS_INFO("Got a Frozen IK solution"); +// q_vec_ik = dvrk_inverse.get_soln_frozon_ik_refined("test_set"); +// std::cout << "q_vec_ik: " << std::endl << q_vec_ik.transpose() << std::endl; +// +// affine_tip_wrt_base = dvrk_inverse.fwd_kin_solve(q_vec_ik, "test_set"); +// +// affine_wrist_wrt_base = dvrk_inverse.get_wrist_wrt_base("test_set"); +// +// +// std::cout << "affine_wrist_wrt_base: " << std::endl +// << affine_wrist_wrt_base.translation().transpose() << std::endl; +// +// // std::cout << "affine_tip_wrt_base: " << std::endl +// // << affine_tip_wrt_base.translation().transpose() << std::endl; +// +// } else { +// ROS_WARN("Failed to get a solution"); +// } +// +// return affine_wrist_wrt_base; +// +// } + + +Eigen::Affine3d printInfo(davinci_kinematics::Vectorq7x1 q_vec) { + + + davinci_kinematics::Inverse dvrk_inverse; + + Eigen::Affine3d affine_wrist_wrt_base, affine_tip_wrt_base; + +// davinci_kinematics::Vectorq7x1 q_vec_ik; + + dvrk_inverse.resetDhOffsetsMaps(); + // dvrk_inverse.loadDHyamlfiles("psm1_dh","test_set"); + dvrk_inverse.loadDHyamlfiles("psm2_dh","test_set"); + + std::cout << "Your input q_vec: " << std::endl << q_vec.transpose() << std::endl; + + affine_tip_wrt_base = dvrk_inverse.fwd_kin_solve(q_vec, "test_set"); + + affine_wrist_wrt_base = dvrk_inverse.get_wrist_wrt_base("test_set"); + + + std::cout << "affine_wrist_wrt_base: " << std::endl + << affine_wrist_wrt_base.translation().transpose() << std::endl; + + + return affine_wrist_wrt_base; + +} + + + +int main(int argc, char **argv) +{ + Eigen::Vector3d test_pt; + std::vector wrist_affines; + + davinci_kinematics::Vectorq7x1 q_vec; + + wrist_affines.clear(); + +// test_pt << 0.03,0.02,-0.16; +// wrist_affines.push_back(printInfo(test_pt)); +// test_pt << -0.01,0.02,-0.16; +// wrist_affines.push_back(printInfo(test_pt)); +// +// test_pt << 0.03,0.06,-0.16; +// wrist_affines.push_back(printInfo(test_pt)); +// test_pt << -0.01,0.06, -0.16; +// wrist_affines.push_back(printInfo(test_pt)); +// +// test_pt << 0.03,0.02,-0.20; +// wrist_affines.push_back(printInfo(test_pt)); +// test_pt << -0.01,0.02,-0.20; +// wrist_affines.push_back(printInfo(test_pt)); +// +// test_pt << 0.03,0.06,-0.20; +// wrist_affines.push_back(printInfo(test_pt)); +// test_pt << -0.01,0.06, -0.20; +// wrist_affines.push_back(printInfo(test_pt)); + + + + q_vec << 0.177, -0.132, 0.160, 0, 0, 0, 0; + wrist_affines.push_back(printInfo(q_vec)); + + q_vec << -0.070, -0.133, 0.158, 0, 0, 0, 0; + wrist_affines.push_back(printInfo(q_vec)); + + q_vec << 0.179, -0.355, 0.170, 0, 0, 0, 0; + wrist_affines.push_back(printInfo(q_vec)); + + q_vec << -0.068, -0.360, 0.167, 0, 0, 0, 0; + wrist_affines.push_back(printInfo(q_vec)); + + q_vec << 0.144, -0.108, 0.200, 0, 0, 0, 0; + wrist_affines.push_back(printInfo(q_vec)); + + q_vec << -0.054, -0.109, 0.198, 0, 0, 0, 0; + wrist_affines.push_back(printInfo(q_vec)); + + q_vec << 0.146, -0.293, 0.208, 0, 0, 0, 0; + wrist_affines.push_back(printInfo(q_vec)); + + q_vec << -0.052, -0.295, 0.206, 0, 0, 0, 0; + wrist_affines.push_back(printInfo(q_vec)); + + std::cout << "% ---" << std::endl; + std::cout << "wrist_pt_1 = [" << wrist_affines[0].translation().transpose() << " 1];" << std::endl; + std::cout << "wrist_pt_2 = [" << wrist_affines[1].translation().transpose() << " 1];" << std::endl; + std::cout << "wrist_pt_3 = [" << wrist_affines[2].translation().transpose() << " 1];" << std::endl; + std::cout << "wrist_pt_4 = [" << wrist_affines[3].translation().transpose() << " 1];" << std::endl; + std::cout << "wrist_pt_5 = [" << wrist_affines[4].translation().transpose() << " 1];" << std::endl; + std::cout << "wrist_pt_6 = [" << wrist_affines[5].translation().transpose() << " 1];" << std::endl; + std::cout << "wrist_pt_7 = [" << wrist_affines[6].translation().transpose() << " 1];" << std::endl; + std::cout << "wrist_pt_8 = [" << wrist_affines[7].translation().transpose() << " 1];" << std::endl; + std::cout << "% ---" << std::endl; + + + return 1; +} diff --git a/examples/test_dh_345.cpp b/examples/test_dh_345.cpp new file mode 100644 index 0000000..7545216 --- /dev/null +++ b/examples/test_dh_345.cpp @@ -0,0 +1,95 @@ +// +// Created by William on 30/10/18. +// + +#include +#include +#include +#include +#include + +#include + +int main(int argc, char **argv) { + davinci_kinematics::Forward dvrk_forward; + davinci_kinematics::Inverse dvrk_inverse; + + Eigen::Vector3d w_wrt_base, q123; + + davinci_kinematics::Vectorq7x1 q_vec, err_vec, q_vec_ik; + q_vec << 0, 0, 0, 0, 0, 0, 0; + int err_cnt = 0; + int tip_err_cnt = 0; + double tip_err_sum = 0.0; + double tip_err_ave = 0.0; + + Eigen::Affine3d affine_wrist_wrt_base, affine_gripper_wrt_base, affine_frame_wrt_base; + // wait to start receiving valid tf transforms + + Eigen::Vector3d tip_from_FK, tip_from_FK_of_IK, tip_err; + // note: with print-outs, takes about 45sec for 10,000 iterations, and got 0 errors + + dvrk_inverse.resetDhOffsetsMaps(); + dvrk_inverse.loadDHyamlfiles("dh_info_format", "psm1_dh"); +// dvrk_inverse.loadDHyamlfiles("psm_generic","psm_generic"); + + dvrk_forward.resetDhOffsetsMaps(); + dvrk_forward.loadDHyamlfiles("dh_info_format", "psm1_dh"); +// dvrk_forward.loadDHyamlfiles("psm_generic","psm_generic"); + + + + + dvrk_forward.gen_rand_legal_jnt_vals(q_vec); + + std::cout << "using q_vec = " << q_vec.transpose() << std::endl; + +// std::cout << "Using Generic: ==================" << std::endl; +// printf("gripper tip frame from FK: \n"); +// affine_gripper_wrt_base = dvrk_forward.fwd_kin_solve(q_vec, "psm_generic"); +// std::cout << "affine linear (R): " << std::endl; +// std::cout << affine_gripper_wrt_base.linear() << std::endl; +// tip_from_FK = affine_gripper_wrt_base.translation(); +// std::cout << "origin: " << tip_from_FK.transpose() << std::endl; +// std::cout << std::endl; + + std::cout << "Using New: ==================" << std::endl; + printf("gripper tip frame from FK: \n"); + affine_gripper_wrt_base = dvrk_forward.fwd_kin_solve(q_vec, "psm1_dh"); + std::cout << "affine linear (R): " << std::endl; + std::cout << affine_gripper_wrt_base.linear() << std::endl; + tip_from_FK = affine_gripper_wrt_base.translation(); + std::cout << "origin: " << tip_from_FK.transpose() << std::endl; + std::cout << std::endl; + + if (dvrk_inverse.ik_solve_refined(affine_gripper_wrt_base, "psm1_dh") > 0) { + + q_vec_ik = dvrk_inverse.get_soln_refined("psm1_dh"); + + printf("FK of IK soln: \n"); + affine_gripper_wrt_base = dvrk_forward.fwd_kin_solve(q_vec_ik, "psm1_dh"); + + std::cout << "affine linear (R): " << std::endl; + std::cout << affine_gripper_wrt_base.linear() << std::endl; + tip_from_FK_of_IK = affine_gripper_wrt_base.translation(); + std::cout << "origin: "; + std::cout << tip_from_FK_of_IK.transpose() << std::endl; + + std::cout << std::endl; + std::cout << "q_vec in: " << q_vec.transpose() << std::endl; + std::cout << "q_vec_ik: " << q_vec_ik.transpose() << std::endl; + err_vec = q_vec - q_vec_ik; + std::cout << "err vec: " << err_vec.transpose() << std::endl; + std::cout << std::endl; + std::cout << std::endl; + + tip_err = tip_from_FK - tip_from_FK_of_IK; + std::cout << "tip err: " << tip_err.transpose() << std::endl; + printf("\033[31mtip pos err norm: %f:\n\033[0m", tip_err.norm()); + + } + + + return 1; + +} \ No newline at end of file diff --git a/examples/test_dh_params.cpp b/examples/test_dh_params.cpp new file mode 100644 index 0000000..778a0b1 --- /dev/null +++ b/examples/test_dh_params.cpp @@ -0,0 +1,113 @@ +// +// Created by william on 13/07/18. +// + + +#include +#include +#include +#include +#include +#include + +#include + + + + +int main(int argc, char **argv) +{ + +// davinci_kinematics::Forward dvrk_forward; + davinci_kinematics::Inverse dvrk_inverse; + + Eigen::Affine3d affine_wrist_wrt_base, affine_gripper_wrt_base; + davinci_kinematics::Vectorq7x1 q_vec, q_vec_up, q_vec_bottom; + davinci_kinematics::Vectorq7x1 err_vec, q_vec_ik, q_vec_ik_refined, q_vec_frozen_ik_refined; + + ROS_WARN("Forward test:"); + + q_vec(0) = 0; + q_vec(1) = 0; + q_vec(2) = 0.11; + q_vec(3) = 0; + q_vec(4) = 0; + q_vec(5) = 0; + q_vec(6) = 0; + + std::cout << "q_vec#============INPUT=BY=USER============================" << ": " << std::endl; + std::cout << q_vec.transpose() << std::endl << std::endl; + + affine_gripper_wrt_base = dvrk_inverse.fwd_kin_solve(q_vec); + + std::cout << "q_vec#" << "fwd_kin_solve() --- affine_gripper_wrt_base.translation():" << std::endl; + std::cout << affine_gripper_wrt_base.translation() << std::endl << std::endl; + affine_wrist_wrt_base = dvrk_inverse.get_wrist_wrt_base(); + std::cout << "affine_wrist_wrt_base: " << std::endl + << affine_wrist_wrt_base.translation().transpose() << std::endl; + + +// ROS_WARN("Inverse Test:"); +// +// if (dvrk_inverse.ik_solve(affine_gripper_wrt_base) > 0) { +// +// q_vec_ik = dvrk_inverse.get_soln(); +// std::cout << "q_vec_ik#=================================================" << ": " << std::endl; +// std::cout << q_vec_ik.transpose() << std::endl << std::endl; +// std::cout << "This should be the same as the input q_vec as the affine was generated by the q_vec in the first place" << std::endl; +// +// } + + + dvrk_inverse.resetDhOffsetsMaps(); + dvrk_inverse.loadDHyamlfiles("psm1_dh","psm1_dh"); + dvrk_inverse.loadDHyamlfiles("psm1_dh_sim","psm1_dh_sim"); + dvrk_inverse.loadDHyamlfiles("psm_generic","psm_generic"); + +// dvrk_forward.resetDhOffsetsMaps(); +// dvrk_forward.loadDHyamlfiles("psm1_dh","psm1_dh"); +// dvrk_forward.loadDHyamlfiles("psm_generic","psm_generic"); + +// dvrk_inverse.printAllDhMaps(); + +// affine_gripper_wrt_base = dvrk_forward.fwd_kin_solve(q_vec, "psm1_dh"); +// +// std::cout << "q_vec#" << "fwd_kin_solve() --- affine_gripper_wrt_base.translation():" << std::endl; +// std::cout << affine_gripper_wrt_base.translation() << std::endl << std::endl; +// std::cout << "Expect the see this different from the one that uses fwd_kin_solve(q_vec)." << std::endl; + + +// if (dvrk_inverse.ik_solve_refined(affine_gripper_wrt_base, "psm1_dh") > 0) { +// +// q_vec_ik = dvrk_inverse.get_soln_refined("psm1_dh"); +// std::cout << "q_vec_ik#=================================================" << ": " << std::endl; +// std::cout << q_vec_ik.transpose() << std::endl << std::endl; +// std::cout << "Expect to see the result is identical to q_vec" << std::endl; +// +// } + + affine_gripper_wrt_base = dvrk_inverse.fwd_kin_solve(q_vec, "psm1_dh_sim"); + std::cout << "q_vec_ik#" << "fwd_kin_solve() -psm1_dh_sim-- affine_gripper_wrt_base.translation():" << std::endl; + std::cout << affine_gripper_wrt_base.translation() << std::endl << std::endl; + affine_wrist_wrt_base = dvrk_inverse.get_wrist_wrt_base("psm1_dh_sim"); + std::cout << "affine_wrist_wrt_base: " << std::endl + << affine_wrist_wrt_base.translation().transpose() << std::endl; + + +// if (dvrk_inverse.ik_solve_frozen_refined(affine_gripper_wrt_base, "psm1_dh") > 0) { +// +// q_vec_ik = dvrk_inverse.get_soln_frozon_ik_refined("psm1_dh"); +// std::cout << "q_vec_ik#==========FROZEN===============================" << ": " << std::endl; +// std::cout << q_vec_ik.transpose() << std::endl << std::endl; +// std::cout << "Expect to see the result is identical to q_vec" << std::endl; +// } +// +// affine_gripper_wrt_base = dvrk_inverse.fwd_kin_solve(q_vec_ik, "psm1_dh"); +// std::cout << "q_vec_ik#" << "fwd_kin_solve() --- affine_gripper_wrt_base.translation():" << std::endl; +// std::cout << affine_gripper_wrt_base.translation() << std::endl << std::endl; + + + + + return 1; +} \ No newline at end of file diff --git a/examples/test_new_ik_err.cpp b/examples/test_new_ik_err.cpp new file mode 100644 index 0000000..2a898db --- /dev/null +++ b/examples/test_new_ik_err.cpp @@ -0,0 +1,219 @@ +// +// Created by william on 21/07/18. +// + +#include +#include +#include +#include +#include + +#include + +#include +#include + + +int main(int argc, char **argv) +{ + davinci_kinematics::Forward dvrk_forward; + davinci_kinematics::Inverse dvrk_inverse; + + Eigen::Vector3d w_wrt_base, q123; + + davinci_kinematics::Vectorq7x1 q_vec, err_vec, q_vec_ik, q_vec_gen_diff, q_vec_last; + q_vec << 0, 0, 0, 0, 0, 0, 0; + q_vec_last << 0, 0, 0, 0, 0, 0, 0; + q_vec_gen_diff << 0, 0, 0, 0, 0, 0, 0; + int err_cnt = 0; + int tip_err_cnt = 0; + double tip_err_sum = 0.0; + double tip_err_ave = 0.0; + + // To store the tip position with error less than 0.1 mm. + std::vector category_a; + // To store the tip position with error from 0.1 mm to 0.3 mm. + std::vector category_b; + // To store the tip position with error from 0.3 mm to 1.0 mm. + std::vector category_c; + // To store the tip position with error greater than 1.0 mm. + std::vector category_d; + std::vector::iterator it; + + category_a.clear(); + category_b.clear(); + category_c.clear(); + category_d.clear(); + + Eigen::Affine3d affine_wrist_wrt_base, affine_gripper_wrt_base, affine_frame_wrt_base; + // wait to start receiving valid tf transforms + + std::vector failed_init_gripper_poses; + std::vector::iterator it_affine; + failed_init_gripper_poses.clear(); + + Eigen::Vector3d tip_from_FK, tip_from_FK_of_IK, tip_err; + // note: with print-outs, takes about 45sec for 10,000 iterations, and got 0 errors + + dvrk_inverse.resetDhOffsetsMaps(); + dvrk_inverse.loadDHyamlfiles("dh_info_format","psm1_dh"); +// dvrk_inverse.loadDHyamlfiles("psm_generic","psm_generic"); + + dvrk_forward.resetDhOffsetsMaps(); + dvrk_forward.loadDHyamlfiles("dh_info_format","psm1_dh"); +// dvrk_forward.loadDHyamlfiles("psm_generic","psm_generic"); + +// dvrk_forward.loadDHyamlfiles("dh_info_format","dh_info_format"); + + int test_target = 100; + + bool new_joint_vals(false); + + for (int itries = 0; itries < test_target; itries++) + { + std::cout < 0) + { + // Validate the legal solution. + q_vec_ik = dvrk_inverse.get_soln_refined("psm1_dh"); + + printf("FK of IK soln: \n"); + affine_gripper_wrt_base = dvrk_forward.fwd_kin_solve(q_vec_ik, "psm1_dh"); + std::cout << "affine linear (R): " << std::endl; + std::cout << affine_gripper_wrt_base.linear() << std::endl; + tip_from_FK_of_IK = affine_gripper_wrt_base.translation(); + std::cout << "origin: "; + std::cout << tip_from_FK_of_IK.transpose() << std::endl; + + std::cout << std::endl; + std::cout << "q_vec in: " << q_vec.transpose() << std::endl; + std::cout << "q_vec_ik: " << q_vec_ik.transpose() << std::endl; + err_vec = q_vec - q_vec_ik; + + std::cout << "err vec: " << err_vec.transpose() << std::endl; + std::cout << std::endl; + std::cout << std::endl; + tip_err = tip_from_FK - tip_from_FK_of_IK; + std::cout << "tip err: " << tip_err.transpose() << std::endl; + printf("jspace errvec norm: %f\n", err_vec.norm()); + printf("\033[31mtip pos err norm: %f:\n\033[0m", tip_err.norm()); + + tip_err_sum = tip_err_sum + tip_err.norm(); + + + if (tip_err.norm() < 0.0001) { + category_a.push_back(tip_from_FK); + } else if (tip_err.norm() >= 0.0001 && tip_err.norm() < 0.0003) { + category_b.push_back(tip_from_FK); + } else if (tip_err.norm() >= 0.0003 && tip_err.norm() < 0.001) { + category_c.push_back(tip_from_FK); + } else { + category_d.push_back(tip_from_FK); + } + + + + if (err_vec.norm() + tip_err.norm() > 0.0001) + { + printf("excessive total error! (err_vec.norm() + tip_err.norm())\n"); + double error_total = err_vec.norm() + tip_err.norm(); + std::cout << "error_total: " << error_total << std::endl; + err_cnt++; + + if (tip_err.norm() > 0.0005) { + std::cout << "Excessive TIP error!" << std::endl; + tip_err_cnt++; + + } + + } + printf("itries = %d; err_cnt = %d; tip_err_cnt =%d", itries, err_cnt, tip_err_cnt); + } else { + itries--; + + failed_init_gripper_poses.push_back(affine_gripper_wrt_base); + + } + } + + tip_err_ave = tip_err_sum/test_target; + + std::cout << std::endl; + printf("err_cnt = %d, tip_err_cnt = %d\n", err_cnt, tip_err_cnt); + std::cout << "tip_err_sum: " << tip_err_sum << std::endl; + printf("tip_err_ave = %f\n", tip_err_ave); + + + + + // Output results to a csv file. + ROS_INFO("writting the csv file."); + Eigen::Vector3d current; + std::ofstream myfile; + + myfile.open ("example.csv"); + for (it = category_a.begin(); it != category_a.end(); it++) { + current = *it; + myfile << (current(0)) << "," << (current(1)) << "," << (current(2)) << ", 1 \n"; + } + for (it = category_b.begin(); it != category_b.end(); it++) { + current = *it; + myfile << (current(0)) << "," << (current(1)) << "," << (current(2)) << ", 2 \n"; + } + for (it = category_c.begin(); it != category_c.end(); it++) { + current = *it; + myfile << (current(0)) << "," << (current(1)) << "," << (current(2)) << ", 3 \n"; + } + for (it = category_d.begin(); it != category_d.end(); it++) { + current = *it; + myfile << (current(0)) << "," << (current(1)) << "," << (current(2)) << ", 4 \n"; + } + myfile.close(); + + + myfile.open ("failed_ones.csv"); + for (it_affine = failed_init_gripper_poses.begin(); it_affine!= failed_init_gripper_poses.end(); it_affine++) { + Eigen::Affine3d current; + Eigen::Vector3d current_vector; + current = *it_affine; + std::cout << "affine linear (R): " << std::endl; + std::cout << current.linear() << std::endl << std::endl; + std::cout << "origin: " << current.translation().transpose() << std::endl << std::endl; + + current_vector = current.translation(); + + myfile << (current_vector(0)) << "," << (current_vector(1)) << "," << (current_vector(2)) << " \n"; + + } + + + return 1; +} \ No newline at end of file diff --git a/include/cwru_davinci_kinematics/davinci_fwd_kinematics.h b/include/cwru_davinci_kinematics/davinci_fwd_kinematics.h index dc69eca..4443509 100644 --- a/include/cwru_davinci_kinematics/davinci_fwd_kinematics.h +++ b/include/cwru_davinci_kinematics/davinci_fwd_kinematics.h @@ -24,16 +24,20 @@ #define CWRU_DAVINCI_KINEMATICS_DAVINCI_FWD_KINEMATICS_H #include +#include #include #include +#include +#include + #include #include #include /** * @brief The davinci_kinematics namespace is where both the forward and inverse dvrk kinematic libraries are defined - * + * * This is to prevent namespace collisions. */ namespace davinci_kinematics @@ -46,7 +50,7 @@ typedef Eigen::Matrix Vectorq7x1; /** - * @brief The Forward kinematics library definition. + * @brief The Forward kinematics library definition. * * In addition to computing the SE(3) transform based on a set of joint angles, * This object also generates a Jacobian matrix. @@ -64,7 +68,7 @@ class Forward * Code Outline: * < 0: A binary indication of which joints are in violation: [-127, -1]. * 1: While technically reachable, this joint vector places the wrist inside the cannula. - * 2: The violation is due to the gripper jaw being open past its limits. The limits are dependant on joint index 5. + * 2: The violation is due to the gripper jaw being open past its limits. The limits are dependant on joint index 5. */ static int check_jnts(const Vectorq7x1& q_vec); @@ -80,7 +84,7 @@ class Forward /** * @brief Get the value of a joint angle by name * - * + * */ static bool get_jnt_val_by_name(std::string jnt_name, sensor_msgs::JointState jointState, double &qval); @@ -91,7 +95,7 @@ class Forward /** * @brief Given the full set of joint positions, compute the SE(3) transform of the Provide joint angles and prismatic displacement (q_vec[2]) w/rt DaVinci coords - * + * * will get translated to DH coords to solve fwd kin * return affine describing gripper pose w/rt base frame * @@ -120,7 +124,7 @@ class Forward * * @param q_vec (optional) The robot joint positions. If empty, the current joints are used for computation. * - * @return a 6x6 Eigen Matrix. + * @return a 6x6 Eigen Matrix. */ Eigen::MatrixXd compute_jacobian(const Vectorq7x1& q_vec); Eigen::MatrixXd compute_jacobian(); @@ -146,11 +150,14 @@ class Forward */ Eigen::Affine3d get_gripper_wrt_frame6() const; + Eigen::Affine3d get_wrist_wrt_base(); + Eigen::Affine3d get_wrist_wrt_base(std::string kinematic_set_name); + /** * @brief sets the transform from the joint frame 6 frame to the gripper frame. * * @param jaw_length the length of the gripper jaw. - * + * * @return The transform between the joint frame 6 and the gripper frame. */ void set_gripper_jaw_length(double jaw_length); @@ -165,7 +172,29 @@ class Forward return gripper_jaw_length_; } -protected: + + // RN + // Added on 19/07/18 as Mk2 Upgrades. + + Eigen::Affine3d fwd_kin_solve(const Vectorq7x1& q_vec, std::string kinematic_set_name); + + bool loadDHyamlfiles(std::string yaml_name, std::string kenimatic_set_name); + + void resetDhOffsetsMaps(); + + void resetDhGenericParams(); + + void printAllDhMaps(); + + Eigen::MatrixXd compute_jacobian(const Vectorq7x1& q_vec, std::string kinematic_set_name); + +// TODO make private later, it is put here to debug + std::map j1_scale_factor_map_; + std::map j2_scale_factor_map_; + std::map j3_scale_factor_map_; + + + protected: /** * @brief The following function takes args of DH thetas and d's and returns an affine transformation * @@ -197,10 +226,35 @@ class Forward { return affine_frame0_wrt_base_.inverse() * affine_products_[i]; }; + + + + // RN + Eigen::VectorXd theta_DH_offsets_generic_; + Eigen::VectorXd dval_DH_offsets_generic_; + Eigen::VectorXd DH_a_params_generic_; + Eigen::VectorXd DH_alpha_params_generic_; + double j1_scale_factor_generic_; + double j2_scale_factor_generic_; + double j3_scale_factor_generic_; + std::map theta_DH_offsets_map_; + std::map dval_DH_offsets_map_; + std::map DH_a_params_map_; + std::map DH_alpha_params_map_; + + + std::map current_joint_state__map_; + std::map Jacobian_map_; + + std::map affine_gripper_wrt_base_map_; + std::map affine_wrist_wrt_base_map_; + std::map> affine_products_map_; + + private: /** * @brief Given a vector of joint states in DaVinci coords, convert these into - * equivalent DH parameters, theta and d + * equivalent DH parameters, theta and d * * @param q_vec the vector of joint states. * @@ -233,6 +287,35 @@ class Forward */ double dh_var_to_qvec(double dh_val, int index); + + // RN + // Added on 19/07/18 as Mk2 Upgrades. + // TODO refactor after functional tests + + + + + + Vectorq7x1 convert_DH_vecs_to_qvec(const Eigen::VectorXd &thetas_DH_vec, + const Eigen::VectorXd &dvals_DH_vec, + std::string kinematic_set_name); + + void convert_qvec_to_DH_vecs(const Vectorq7x1& q_vec, + Eigen::VectorXd &thetas_DH_vec, + Eigen::VectorXd &dvals_DH_vec, + std::string kinematic_set_name); + + void fwd_kin_solve_DH(const Eigen::VectorXd& theta_vec, + const Eigen::VectorXd& d_vec, + std::string kinematic_set_name); + + + + + + std::string ros_pkg_path_; + + /** * @brief This is the internal joint state of the forward kinematics. */ @@ -240,7 +323,7 @@ class Forward /** * @brief This is the tranform aligning the robot with a world frame. - */ + */ Eigen::Affine3d affine_frame0_wrt_base_; /** @@ -255,6 +338,8 @@ class Forward */ Eigen::Affine3d affine_gripper_wrt_base_; + Eigen::Affine3d affine_wrist_wrt_base_; + /** * @brief This is the list of transforms from the robot base to each joint. * @@ -264,18 +349,21 @@ class Forward /** * @brief This is the list of theta offset values for the DaVinci DH joint parameters. - * + * * Stored intrinsicaly as part of the dvrk kinematics */ Eigen::VectorXd theta_DH_offsets_; /** * @brief This is the list of d offset values for the DaVinci DH joint parameters. - * + * * Stored intrinsicaly as part of the dvrk kinematics */ Eigen::VectorXd dval_DH_offsets_; + + + /** * @brief The 6x6 Jacobian of the current joint positions, only computed when the compute_jacobian method is called. * diff --git a/include/cwru_davinci_kinematics/davinci_inv_kinematics.h b/include/cwru_davinci_kinematics/davinci_inv_kinematics.h index a8efe30..31e8edd 100644 --- a/include/cwru_davinci_kinematics/davinci_inv_kinematics.h +++ b/include/cwru_davinci_kinematics/davinci_inv_kinematics.h @@ -18,7 +18,7 @@ /** - * @brief The inverse kinematics class (derived from the forward kinematics class) is for + * @brief The inverse kinematics class (derived from the forward kinematics class) is for * Computing an analytical inverse kinematics of the DaVinci robot. */ @@ -37,7 +37,8 @@ namespace davinci_kinematics { -class Inverse:private Forward +// RN 20/07/18 It used to be private inheritance. +class Inverse:public Forward { public: /** @@ -64,6 +65,38 @@ class Inverse:private Forward */ int ik_solve(Eigen::Affine3d const& desired_hand_pose); + // RN + int ik_solve_refined(Eigen::Affine3d const& desired_hand_pose); + bool solve_jacobian_ik(Eigen::Affine3d const& desired_hand_pose, Eigen::VectorXd &q_ik); + int ik_solve_frozen_refined(Eigen::Vector3d const& desired_tip_coordinate); + int ik_solve_frozen_refined(Eigen::Affine3d const& desired_hand_pose); + bool solve_jacobian_frozen_ik(Eigen::Vector3d const& desired_tip_coordinate, Eigen::VectorXd &q_ik); + + + int ik_solve(Eigen::Affine3d const& desired_hand_pose, std::string kinematic_set_name); // is this necessary? + int ik_solve_generic(Eigen::Affine3d const& desired_hand_pose); + + int ik_solve_refined(Eigen::Affine3d const& desired_hand_pose, + std::string kinematic_set_name); + + bool solve_jacobian_ik(Eigen::Affine3d const& desired_hand_pose, + Eigen::VectorXd &q_ik, + std::string kinematic_set_name); + + int ik_solve_frozen_refined(Eigen::Vector3d const& desired_tip_coordinate, + std::string kinematic_set_name); + + int ik_solve_frozen_refined(Eigen::Affine3d const& desired_hand_pose, + std::string kinematic_set_name); + + bool solve_jacobian_frozen_ik(Eigen::Vector3d const& desired_tip_coordinate, + Eigen::VectorXd &q_ik, + std::string kinematic_set_name); + + + void resetAllIkMaps(); + + /** * @brief get the properly computed (and validated) solution. * @@ -74,6 +107,30 @@ class Inverse:private Forward return q_vec_soln_; }; + // RN + Vectorq7x1 get_soln_refined() const + { + return q_vec_soln_refined_; + }; + + // TODO why this cannot add const at the end like other functions? + Vectorq7x1 get_soln_refined(std::string kinematic_set_name) + { + return q_vec_soln_refined_map_[kinematic_set_name]; + }; + + + // RN + Vectorq7x1 get_soln_frozon_ik_refined() const + { + return q_vec_soln_frozon_ik_refined_; + }; + + Vectorq7x1 get_soln_frozon_ik_refined(std::string kinematic_set_name) + { + return q_vec_soln_frozon_ik_refined_map_[kinematic_set_name]; + }; + /** * @brief gets the positional error of the inverse kinematics computations */ @@ -97,10 +154,18 @@ class Inverse:private Forward using Forward::set_gripper_jaw_length; private: + + bool debug_print = false; + bool debug_print_1 = false; + + + +// Forward davinci_fwd_solver_; + /** * @brief verifies that the proposed list of joint positions fit the hardware joint limits. * - * @param qvec A modifiable vector of joint angles. + * @param qvec A modifiable vector of joint angles. * * @return true if the joints are within the hardware limits. */ @@ -123,7 +188,7 @@ class Inverse:private Forward * @brief Computes the forward kinematics using only the first 3 joints. * * debug fnc: compute FK of wrist pt from q123 - * could have put this in IK instead... + * could have put this in IK instead... * * @param q123 a vector of 3 joint angles. (joints 1-3) * @@ -175,6 +240,16 @@ class Inverse:private Forward */ Vectorq7x1 q_vec_soln_; + /** + * RN + */ + Vectorq7x1 q_vec_soln_refined_; + + /** + * RN + */ + Vectorq7x1 q_vec_soln_frozon_ik_refined_; + /** * @brief The stored desired pose for which the inverse kinematics has been most recently completed */ @@ -185,8 +260,16 @@ class Inverse:private Forward */ // double min_dist_O4_to_gripper_tip_; + + std::map q_vec_soln_refined_map_; + std::map q_vec_soln_map_; + std::map q_vec_soln_frozon_ik_refined_map_; + std::map desired_hand_pose_map_; // is thie necessary? + double err_l_; double err_r_; + + }; } // namespace davinci_kinematics diff --git a/package.xml b/package.xml index 6ba2c67..687ec31 100644 --- a/package.xml +++ b/package.xml @@ -44,6 +44,7 @@ sensor_msgs tf geometry2 + roslib @@ -56,6 +57,7 @@ sensor_msgs tf geometry2 + roslib diff --git a/src/davinci_fwd_kinematics.cpp b/src/davinci_fwd_kinematics.cpp index 6e317a9..f0690ba 100644 --- a/src/davinci_fwd_kinematics.cpp +++ b/src/davinci_fwd_kinematics.cpp @@ -47,8 +47,9 @@ bool Forward::get_jnt_val_by_name(std::string jnt_name, sensor_msgs::JointState // given a vector of joint states in DaVinci coords, convert these into // equivalent DH parameters, theta and d -void Forward::convert_qvec_to_DH_vecs(const Vectorq7x1& q_vec, Eigen::VectorXd &thetas_DH_vec, - Eigen::VectorXd &dvals_DH_vec) +void Forward::convert_qvec_to_DH_vecs(const Vectorq7x1& q_vec, + Eigen::VectorXd &thetas_DH_vec, + Eigen::VectorXd &dvals_DH_vec) { thetas_DH_vec.resize(7); // +? -? @@ -62,7 +63,14 @@ void Forward::convert_qvec_to_DH_vecs(const Vectorq7x1& q_vec, Eigen::VectorXd & dvals_DH_vec.resize(7); dvals_DH_vec = dval_DH_offsets_; - dvals_DH_vec(2) += q_vec(2); + dvals_DH_vec(2) += q_vec(2); // RN original + + // dvals_DH_vec(2) += 0.988*q_vec(2); // RN 20180220 (TODO adjust DH_a1 at the same time!) + //dvals_DH_vec(1) += q_vec(2); + + // dvals_DH_vec(2) += 0.98*q_vec(2); // RN 20180319A2 + + } int Forward::check_jnts(const Vectorq7x1& q_vec) @@ -99,24 +107,31 @@ int Forward::check_jnts(const Vectorq7x1& q_vec) return 0; } + double Forward::dh_var_to_qvec(double dh_val, int index) { return (dh_val - DH_q_offsets[index]); } + + +// RN TODO deal with that .99 sacle factor // Eigen::VectorXd thetas_DH_vec_,dvals_DH_vec_; Vectorq7x1 Forward::convert_DH_vecs_to_qvec(const Eigen::VectorXd &thetas_DH_vec, const Eigen::VectorXd &dvals_DH_vec) { Vectorq7x1 q_vec; + for (int i = 0; i < 7; i++) { q_vec(i) = thetas_DH_vec(i)-theta_DH_offsets_(i); } q_vec(2) = dvals_DH_vec(2)-dval_DH_offsets_(2); + return q_vec; } + // given 4 DH parameters, compute the corresponding transform as an affine3d Eigen::Affine3d Forward::computeAffineOfDH(double a, double d, double alpha, double theta) { @@ -184,17 +199,29 @@ Forward::Forward() for (int i = 0; i < 7; i++) { theta_DH_offsets_(i) = DH_q_offsets[i]; + } // don't put prismatic displacement here theta_DH_offsets_(2) = 0.0; dval_DH_offsets_.resize(7); - dval_DH_offsets_<< 0, 0, DH_q_offsets[2], 0, 0, 0, 0; + // dval_DH_offsets_<< 0, 0 , DH_q_offsets[2], 0, 0, 0, 0; + +//dval_DH_offsets_<< 0, 0, 0, 0, 0, 0, 0; // RN 20180712A1 PSM1 + dval_DH_offsets_<< 0, 0, DH_q_offsets[2], 0, 0, 0, 0; // RN 20180713 EXPERIMENTAL + // resize MatrixXd Jacobian_ and initialize terms to 0's Jacobian_ = Eigen::MatrixXd::Zero(6, 6); + + resetDhGenericParams(); } + + + + + // provide DH theta and d values, return affine pose of gripper tip w/rt base frame // also computes all intermediate affine frames, w/rt base frame void Forward::fwd_kin_solve_DH(const Eigen::VectorXd& theta_vec, const Eigen::VectorXd& d_vec) @@ -205,6 +232,7 @@ void Forward::fwd_kin_solve_DH(const Eigen::VectorXd& theta_vec, const Eigen::Ve affines_i_wrt_iminus1.resize(7); Eigen::Affine3d xform; double a, d, theta, alpha; + for (int i = 0; i < 7; i++) { a = DH_a_params[i]; @@ -214,15 +242,41 @@ void Forward::fwd_kin_solve_DH(const Eigen::VectorXd& theta_vec, const Eigen::Ve xform = computeAffineOfDH(a, d, alpha, theta); affines_i_wrt_iminus1[i]= xform; } + affine_products_.resize(7); affine_products_[0] = affine_frame0_wrt_base_ * affines_i_wrt_iminus1[0]; + // RN Note that it starts from 1. for (int i = 1; i < 7; i++) { affine_products_[i] = affine_products_[i-1] * affines_i_wrt_iminus1[i]; } affine_gripper_wrt_base_ = affine_products_[6] * affine_gripper_wrt_frame6_; + + // RN added for wrist pt coordinate w/rt base frame + affine_wrist_wrt_base_ = affine_products_[2]; + } + + + + + + + +Eigen::Affine3d Forward::get_wrist_wrt_base() // RN +{ + return affine_wrist_wrt_base_; +} + + +Eigen::Affine3d Forward::get_wrist_wrt_base(std::string kinematic_set_name) // RN +{ + return this->affine_wrist_wrt_base_map_[kinematic_set_name]; +} + + + Eigen::Affine3d Forward::fwd_kin_solve(const Vectorq7x1& q_vec) { current_joint_state_ = q_vec; @@ -322,4 +376,518 @@ void Forward::gen_rand_legal_jnt_vals(Vectorq7x1 &qvec) } } + +// TODO +bool Forward::loadDHyamlfiles(std::string yaml_name, std::string kinematic_set_name) { + + std::string psm_yaml_path; + YAML::Node psm_dh_param_node; + + double j1_scale_factor; + double j2_scale_factor; + double j3_scale_factor; + Eigen::VectorXd theta_DH_offsets; + Eigen::VectorXd dval_DH_offsets; + Eigen::VectorXd DH_a_params; + Eigen::VectorXd DH_alpha_params; + + theta_DH_offsets.resize(7); + dval_DH_offsets.resize(7); + DH_a_params.resize(7); + DH_alpha_params.resize(7); + + + // Make them equal to generic ones first. + theta_DH_offsets = theta_DH_offsets_generic_; + dval_DH_offsets = dval_DH_offsets_generic_; + DH_a_params = DH_a_params_generic_; + DH_alpha_params = DH_alpha_params_generic_; + j1_scale_factor = j1_scale_factor_generic_; + j2_scale_factor = j2_scale_factor_generic_; + j3_scale_factor = j3_scale_factor_generic_; + + ros_pkg_path_ = ros::package::getPath("cwru_davinci_kinematics"); + psm_yaml_path = ros_pkg_path_ + "/config/" + yaml_name + ".yaml"; + + try { + ROS_INFO("Loading PSM DH parameter yaml file..."); + psm_dh_param_node = YAML::LoadFile(psm_yaml_path); + } catch (YAML::ParserException &e) { + ROS_WARN("Failed to yaml."); + std::cout << e.what() << std::endl; + return false; + } + + // Load thetas from yaml, + + try { + theta_DH_offsets[0] = psm_dh_param_node["theta_1"].as(); + } catch(const std::exception& e) { + std::cout << e.what() << std::endl; + ROS_WARN("Failed to load DH theta_1, will use generic instead."); + } + + try { + theta_DH_offsets[1] = psm_dh_param_node["theta_2"].as(); + } catch(const std::exception& e) { + std::cout << e.what() << std::endl; + ROS_WARN("Failed to load DH theta_2, will use generic instead."); + } + + try { + theta_DH_offsets[2] = psm_dh_param_node["theta_3"].as(); + } catch(const std::exception& e) { + std::cout << e.what() << std::endl; + ROS_WARN("Failed to load DH theta_3, will use generic instead."); + } + + try { + theta_DH_offsets[3] = psm_dh_param_node["theta_4"].as(); + } catch(const std::exception& e) { + std::cout << e.what() << std::endl; + ROS_WARN("Failed to load DH theta_4, will use generic instead."); + } + + try { + theta_DH_offsets[4] = psm_dh_param_node["theta_5"].as(); + } catch(const std::exception& e) { + std::cout << e.what() << std::endl; + ROS_WARN("Failed to load DH theta_5, will use generic instead."); + } + + + // Load alphas from yaml. + + + try { + DH_alpha_params[0] = psm_dh_param_node["alpha_1"].as(); + } catch(const std::exception& e) { + std::cout << e.what() << std::endl; + ROS_WARN("Failed to load DH alpha_1, will use generic instead."); + } + + try { + DH_alpha_params[1] = psm_dh_param_node["alpha_2"].as(); + } catch(const std::exception& e) { + std::cout << e.what() << std::endl; + ROS_WARN("Failed to load DH alpha_2, will use generic instead."); + } + + try { + DH_alpha_params[2] = psm_dh_param_node["alpha_3"].as(); + } catch(const std::exception& e) { + std::cout << e.what() << std::endl; + ROS_WARN("Failed to load DH alpha_3, will use generic instead."); + } + + try { + DH_alpha_params[3] = psm_dh_param_node["alpha_4"].as(); + } catch(const std::exception& e) { + std::cout << e.what() << std::endl; + ROS_WARN("Failed to load DH alpha_4, will use generic instead."); + } + + try { + DH_alpha_params[4] = psm_dh_param_node["alpha_5"].as(); + } catch(const std::exception& e) { + std::cout << e.what() << std::endl; + ROS_WARN("Failed to load DH alpha_5, will use generic instead."); + } + + + // Load a from yaml. + + + + try { + DH_a_params[0] = psm_dh_param_node["a_1"].as(); + } catch(const std::exception& e) { + std::cout << e.what() << std::endl; + ROS_WARN("Failed to load DH a_1, will use generic instead."); + } + + try { + DH_a_params[1] = psm_dh_param_node["a_2"].as(); + } catch(const std::exception& e) { + std::cout << e.what() << std::endl; + ROS_WARN("Failed to load DH a_2, will use generic instead."); + } + + try { + DH_a_params[2] = psm_dh_param_node["a_3"].as(); + } catch(const std::exception& e) { + std::cout << e.what() << std::endl; + ROS_WARN("Failed to load DH a_3, will use generic instead."); + } + + try { + DH_a_params[3] = psm_dh_param_node["a_4"].as(); + } catch(const std::exception& e) { + std::cout << e.what() << std::endl; + ROS_WARN("Failed to load DH a_4, will use generic instead."); + } + + try { + DH_a_params[4] = psm_dh_param_node["a_5"].as(); + } catch(const std::exception& e) { + std::cout << e.what() << std::endl; + ROS_WARN("Failed to load DH a_5, will use generic instead."); + } + + + // Load d from yaml. + + + try { + dval_DH_offsets[0] = psm_dh_param_node["d_1"].as(); + } catch(const std::exception& e) { + std::cout << e.what() << std::endl; + ROS_WARN("Failed to load DH d_1, will use generic instead."); + } + + try { + dval_DH_offsets[1] = psm_dh_param_node["d_2"].as(); + } catch(const std::exception& e) { + std::cout << e.what() << std::endl; + ROS_WARN("Failed to load DH d_2, will use generic instead."); + } + + try { + dval_DH_offsets[2] = psm_dh_param_node["d_3"].as(); + } catch(const std::exception& e) { + std::cout << e.what() << std::endl; + ROS_WARN("Failed to load DH d_3, will use generic instead."); + } + + try { + dval_DH_offsets[3] = psm_dh_param_node["d_4"].as(); + } catch(const std::exception& e) { + std::cout << e.what() << std::endl; + ROS_WARN("Failed to load DH d_4, will use generic instead."); + } + + try { + dval_DH_offsets[4] = psm_dh_param_node["d_5"].as(); + } catch(const std::exception& e) { + std::cout << e.what() << std::endl; + ROS_WARN("Failed to load DH d_5, will use generic instead."); + } + + + + try { + j1_scale_factor = psm_dh_param_node["j1_scale_factor"].as(); + } catch(const std::exception& e) { + std::cout << e.what() << std::endl; + ROS_WARN("Failed to load j1_scale_factor, will use generic instead."); + } + + try { + j2_scale_factor = psm_dh_param_node["j2_scale_factor"].as(); + } catch(const std::exception& e) { + std::cout << e.what() << std::endl; + ROS_WARN("Failed to load j2_scale_factor, will use generic instead."); + } + + try { + j3_scale_factor = psm_dh_param_node["j3_scale_factor"].as(); + } catch(const std::exception& e) { + std::cout << e.what() << std::endl; + ROS_WARN("Failed to load j3_scale_factor, will use generic instead."); + } + + + + ROS_INFO("Yaml loading complete!"); + + // Add to maps + theta_DH_offsets_map_[kinematic_set_name] = theta_DH_offsets; + dval_DH_offsets_map_[kinematic_set_name] = dval_DH_offsets; + DH_a_params_map_[kinematic_set_name] = DH_a_params; + DH_alpha_params_map_[kinematic_set_name] = DH_alpha_params; + j1_scale_factor_map_[kinematic_set_name] = j1_scale_factor; + j2_scale_factor_map_[kinematic_set_name] = j2_scale_factor; + j3_scale_factor_map_[kinematic_set_name] = j3_scale_factor; + + // Also initialise related variables if needed. + + + + + return true; + +} + + +void Forward::resetDhOffsetsMaps() { + + theta_DH_offsets_map_.clear(); + dval_DH_offsets_map_.clear(); + DH_a_params_map_.clear(); + DH_alpha_params_map_.clear(); + affine_gripper_wrt_base_map_.clear(); + affine_wrist_wrt_base_map_.clear(); + affine_products_map_.clear(); + current_joint_state__map_.clear(); + Jacobian_map_.clear(); + +} + + +void Forward::resetDhGenericParams() { + + theta_DH_offsets_generic_.resize(7); + dval_DH_offsets_generic_.resize(7); + DH_a_params_generic_.resize(7); + DH_alpha_params_generic_.resize(7); + + for (int i = 0; i < 7; i++) { + theta_DH_offsets_generic_(i) = DH_q_offsets[i]; + DH_alpha_params_generic_(i) = DH_alpha_params[i]; + DH_a_params_generic_(i) = DH_a_params[i]; + if (i == 2) { + theta_DH_offsets_generic_(i) = 0; + } + } + + dval_DH_offsets_generic_<< 0, 0, DH_q_offsets[2], 0, 0, 0, 0; + j1_scale_factor_generic_ = 1.0; + j2_scale_factor_generic_ = 1.0; + j3_scale_factor_generic_ = 1.0; + +} + + +void Forward::printAllDhMaps() { + + Eigen::VectorXd temp; + + std::cout << "theta_DH_offsets_map_ size: " << theta_DH_offsets_map_.size() << std::endl; + for (std::map::iterator it=theta_DH_offsets_map_.begin(); + it!=theta_DH_offsets_map_.end(); ++it) { + + std::cout << it->first << ":" << std::endl; + temp = it->second; + std::cout << temp.transpose() << std::endl; + } + + std::cout << "dval_DH_offsets_map_ size: " << dval_DH_offsets_map_.size() << std::endl; + for (std::map::iterator it=dval_DH_offsets_map_.begin(); + it!=dval_DH_offsets_map_.end(); ++it) { + + std::cout << it->first << ":" << std::endl; + temp = it->second; + std::cout << temp.transpose() << std::endl; + } + + std::cout << "DH_a_params_map_ size: " << DH_a_params_map_.size() << std::endl; + for (std::map::iterator it=DH_a_params_map_.begin(); + it!=DH_a_params_map_.end(); ++it) { + + std::cout << it->first << ":" << std::endl; + temp = it->second; + std::cout << temp.transpose() << std::endl; + } + + std::cout << "DH_alpha_params_map_ size: " << DH_alpha_params_map_.size() << std::endl; + for (std::map::iterator it=DH_alpha_params_map_.begin(); + it!=DH_alpha_params_map_.end(); ++it) { + + std::cout << it->first << ":" << std::endl; + temp = it->second; + std::cout << temp.transpose() << std::endl; + } + + + +} + + +Eigen::Affine3d Forward::fwd_kin_solve(const Vectorq7x1& q_vec, + std::string kinematic_set_name) { + + + current_joint_state__map_[kinematic_set_name] = q_vec; + + + Eigen::VectorXd thetas_DH_vec, dvals_DH_vec; + convert_qvec_to_DH_vecs(q_vec, thetas_DH_vec, dvals_DH_vec, kinematic_set_name); + + fwd_kin_solve_DH(thetas_DH_vec, dvals_DH_vec, kinematic_set_name); + + return affine_gripper_wrt_base_map_[kinematic_set_name]; + +} + + +void Forward::convert_qvec_to_DH_vecs(const Vectorq7x1& q_vec, + Eigen::VectorXd &thetas_DH_vec, + Eigen::VectorXd &dvals_DH_vec, + std::string kinematic_set_name) { + + thetas_DH_vec.resize(7); + thetas_DH_vec = theta_DH_offsets_map_[kinematic_set_name]; + + for (int i = 0; i < 7; i++) + { + + if (i == 0) { + + // Joint 1 has a scale factor to be added here. + thetas_DH_vec(i)+= j1_scale_factor_map_[kinematic_set_name] * q_vec(i); + + } else if (i == 1) { + + // Joint 2 has a scale factor to be added here. + thetas_DH_vec(i)+= j2_scale_factor_map_[kinematic_set_name] * q_vec(i); + + } else { + // skip the linear joint. + if (i == 2) continue; + thetas_DH_vec(i)+= q_vec(i); + + } + + + + } + + + dvals_DH_vec.resize(7); + dvals_DH_vec = dval_DH_offsets_map_[kinematic_set_name]; + + // Prismatic + dvals_DH_vec(2) = j3_scale_factor_map_[kinematic_set_name]*q_vec(2) + dvals_DH_vec(2); + + +} + + +void Forward::fwd_kin_solve_DH(const Eigen::VectorXd& theta_vec, + const Eigen::VectorXd& d_vec, + std::string kinematic_set_name) { + + std::vector affines_i_wrt_iminus1; + affines_i_wrt_iminus1.resize(7); + Eigen::Affine3d xform; + double a, d, theta, alpha; + + Eigen::VectorXd DH_a_params, DH_alpha_params; + std::vector affine_products; + DH_a_params = DH_a_params_map_[kinematic_set_name]; + DH_alpha_params = DH_alpha_params_map_[kinematic_set_name]; + + for (int i = 0; i < 7; i++) + { + a = DH_a_params[i]; + d = d_vec(i); + alpha = DH_alpha_params[i]; + theta = theta_vec(i); + xform = computeAffineOfDH(a, d, alpha, theta); + affines_i_wrt_iminus1[i]= xform; + } + + affine_products.resize(7); + affine_products[0] = affine_frame0_wrt_base_ * affines_i_wrt_iminus1[0]; + for (int i = 1; i < 7; i++) + { + affine_products[i] = affine_products[i-1] * affines_i_wrt_iminus1[i]; + } + + affine_gripper_wrt_base_map_[kinematic_set_name] = affine_products[6] * affine_gripper_wrt_frame6_; + + // RN added for wrist pt coordinate w/rt base frame + affine_wrist_wrt_base_map_[kinematic_set_name] = affine_products[2]; + + + + + // Also include the newly calculated affine products into its map. + affine_products_map_[kinematic_set_name] = affine_products; + +} + + + + +Eigen::MatrixXd Forward::compute_jacobian(const Vectorq7x1& q_vec, + std::string kinematic_set_name) { + + + Eigen::MatrixXd temp_jacobian; + temp_jacobian = Eigen::MatrixXd::Zero(6, 6); + + // use the jacobian to make the computation. + fwd_kin_solve(q_vec, kinematic_set_name); + + + + Eigen::Vector3d z_axis; + Eigen::Vector3d vec_tip_minus_Oi_wrt_base; + Eigen::Matrix3d R; + Eigen::Vector3d r_tip_wrt_base = affine_gripper_wrt_base_map_[kinematic_set_name].translation(); + Eigen::Vector3d z_axis0 = affine_frame0_wrt_base_.linear().col(2); + std::vector affine_products; + + affine_products = affine_products_map_[kinematic_set_name]; + + // angular Jacobian is just the z axes of each revolute joint (expressed in base frame); + // for prismatic joint, there is no angular contribution + // start from z_axis0 + + // Block of size (p,q), starting at (i,j) matrix.block(i,j); + temp_jacobian.block<3, 1>(3, 0) = z_axis0; + + + vec_tip_minus_Oi_wrt_base = r_tip_wrt_base - affine_frame0_wrt_base_.translation(); + temp_jacobian.block<3, 1>(0, 0) = z_axis0.cross(vec_tip_minus_Oi_wrt_base); + // 2nd joint: + // refer to previous joint's z axis + + + R = affine_products[0].linear(); + z_axis = R.col(2); + // Block of size (p,q), starting at (i,j) matrix.block(i,j); + temp_jacobian.block<3, 1>(3, 1) = z_axis; + vec_tip_minus_Oi_wrt_base = r_tip_wrt_base - affine_products[0].translation(); + temp_jacobian.block<3, 1>(0, 1) = z_axis.cross(vec_tip_minus_Oi_wrt_base); + + + // prismatic joint: + R = affine_products[1].linear(); + z_axis = R.col(2); + temp_jacobian.block<3, 1>(0, 2) = z_axis; + + // joints 4-6: + for (int i = 3; i < 6; i++) + { + R = affine_products[i-1].linear(); + z_axis = R.col(2); + // Block of size (p,q), starting at (i,j) matrix.block(i,j); + temp_jacobian.block<3, 1>(3, i) = z_axis; + vec_tip_minus_Oi_wrt_base = r_tip_wrt_base - affine_products[i-1].translation(); + temp_jacobian.block<3, 1>(0, i) = z_axis.cross(vec_tip_minus_Oi_wrt_base); + } + + Jacobian_map_[kinematic_set_name] = temp_jacobian; + + // translational Jacobian depends on joint's z-axis and vector from i'th axis to robot tip + return temp_jacobian; + + +} + + + + + + + + + + } // namespace davinci_kinematics diff --git a/src/davinci_inv_kinematics.cpp b/src/davinci_inv_kinematics.cpp index 143fe27..5ae51fe 100644 --- a/src/davinci_inv_kinematics.cpp +++ b/src/davinci_inv_kinematics.cpp @@ -34,6 +34,483 @@ Inverse::Inverse() : Forward() // Blank constructor. } + +bool Inverse::solve_jacobian_ik(Eigen::Affine3d const& desired_hand_pose, Eigen::VectorXd &q_ik) +{ + Eigen::Affine3d A_fwd,A_fwd2; + Eigen::Matrix3d R1,R2,R_err; + Eigen::Vector3d dxyz,dphi; + Eigen::Vector3d dxyz2; + Eigen::VectorXd q7(1); + Eigen::VectorXd dp,dq,k_rot_axis,q_updated, dq_temp; + Eigen::MatrixXd Jacobian; + + dp.resize(6); + dq.resize(7); + dq_temp.resize(7); + q_updated.resize(7); + double dtheta; + + q7 << q_ik(6); + + A_fwd = fwd_kin_solve(q_ik); + + R1 = desired_hand_pose.linear(); + R2 = A_fwd.linear(); + dxyz = desired_hand_pose.translation()-A_fwd.translation(); + R_err = R2*R1.transpose(); + Eigen::AngleAxisd angleAxis(R_err); + dtheta = angleAxis.angle(); + k_rot_axis = angleAxis.axis(); + dphi = -k_rot_axis*dtheta; + + dp.block<3,1>(0,0) = dxyz; + dp.block<3,1>(3,0) = dphi; + Jacobian = compute_jacobian(q_ik); + dq = Jacobian.inverse()*dp; + dq_temp = dq; + dq.resize(7); + dq.block<6,1>(0,0) = dq_temp; + dq.block<1,1>(6,0) = q7; + + + + if (debug_print_1) std::cout << std::endl << "\e[32m\e[1mEngaging Jacobian IK addon to improve the coarse IK!\e[0m" << std::endl; + if (debug_print_1) std::cout << std::endl << "\e[1m\e[94m * q_ik BEFORE updates: \e[0m" << q_ik.transpose() << std::endl; + if (debug_print_1) std::cout << " * dxyz.norm(): " << dxyz.norm() << std::endl; + if (debug_print_1) std::cout << " * dtheta: " << dtheta << std::endl; + + double err_xyz = -1; + double err_dtheta = -1; + int iteration_count = 0; + const int iter_max = 10000; + bool close_enough = false; // TODO not used yet + bool updated = false; + int update_count = 0; + double translational_tolerance = 0.0001; + + + //see if this is an improvement: + while ( (iteration_count < iter_max) && (!close_enough) ) + { + + iteration_count++; + + // Update q_ik with the current changes. + q_updated = q_ik + dq; + + // Calculate the new position the current changes in q_ik would cause + A_fwd2 = fwd_kin_solve(q_updated); + + // Calculate the distance form the goal + R2 = A_fwd2.linear(); + R_err = R2*R1.transpose(); + Eigen::AngleAxisd angleAxis2(R_err); + // get dxyz2 and dtheta2 + dxyz2 = desired_hand_pose.translation()-A_fwd2.translation(); + double dtheta2 = angleAxis2.angle(); + + // See if the distance decrease the gap + err_dtheta = fabs(dtheta)-fabs(dtheta2); //want this >0 + err_xyz = dxyz.norm()-dxyz2.norm(); //want this >0 + + + // If the result is better rebase q_ik and update the Jacobian, dp and dq acoordingly + // Otherwise half the dq, use the same base q_ik and try again + if ((err_dtheta>0)||(err_xyz>0)) { + updated = true; + update_count++; + + if (debug_print) + { + std::cout << std::endl + // << "------------------------" << std::endl + << "\e[1mITERATION#" << iteration_count << "\e[0m ------------" << std::endl + << "UPDATE#" << update_count << std::endl + << "updating q_ik: " << q_ik.transpose() << std::endl; + // std::cout << "Jacobian.inverse()*dp: " << Jacobian.inverse()*dp << std::endl; + std::cout << "\e[94m dq: " << dq.transpose() << " \e[0m " << std::endl; + } + + q_ik = q_updated; // q_ik rebased + + if (debug_print) + { + std::cout << "updated q_ik: " << q_ik.transpose() << std::endl << std::endl; + + std::cout << "Before THIS update dxyz.norm(): " << dxyz.norm() << std::endl; + std::cout << " dxyz: " << dxyz.transpose() << std::endl; + std::cout << " dtheta: " << dtheta << std::endl; + std::cout << " dp: " << dp.transpose() << std::endl; + std::cout << std::endl; + // std::cout << " Jacobian:" << std::endl << Jacobian << std::endl << std::endl; + // std::cout << " Jacobian Inverse:" << std::endl << Jacobian.inverse() << std::endl << std::endl; + } + + + // redo the Jacobian + A_fwd = fwd_kin_solve(q_ik); + + R1 = desired_hand_pose.linear(); + R2 = A_fwd.linear(); + dxyz = desired_hand_pose.translation()-A_fwd.translation(); + R_err = R2*R1.transpose(); + Eigen::AngleAxisd angleAxis(R_err); + dtheta = angleAxis.angle(); + k_rot_axis = angleAxis.axis(); + dphi = -k_rot_axis*dtheta; + + dp.block<3,1>(0,0) = dxyz; + dp.block<3,1>(3,0) = dphi; + Jacobian = compute_jacobian(q_ik); + + dq = Jacobian.inverse()*dp; + dq.resize(7); + dq.block<1,1>(6,0) = q7; + + // see if we have reduced the tranlational error below our tolerance. + if (dxyz.norm() < translational_tolerance) + { + close_enough = true; + std::cout << std::endl << "\e[32m\e[1mdxzy has been reduced to below " + << translational_tolerance*1000 << " mm \e[0m" <0)&&(err_xyz>0) failed." << std::endl; + dq = dq/2; // then go back to the beginning of this while loop + + } + + } // while + + + if (debug_print) + { + std::cout << std::endl << "Total Iteration count: " << iteration_count << std::endl; + std::cout << "The q_ik has been updated for \e[1m\e[34m" << update_count << "\e[0m times" << std::endl; + std::cout << "\e[1m\e[94m * q_ik AFTER updates: \e[0m" << q_ik.transpose() << std::endl; + // std::cout << " * dxyz2.norm(): " << dxyz2.norm() << std::endl; + // std::cout << " * dtheta2: " << dtheta2 << std::endl; + } + + if (update_count > 0) + { + if (debug_print) std::cout << std::endl << "\e[1m\e[32mJacobian did improve solution.\e[0m" << std::endl; + + if (dxyz.norm() < translational_tolerance) + { + if (debug_print) std::cout << "And the translational error has been reduced to sub-minimeter: " << dxyz.norm() << std::endl + << std::endl; + } else + { + if (debug_print) std::cout << "\e[31mBUT the translational error is stll above 0.1 mm: \e[0m" << dxyz.norm() << std::endl; + } + + return true; + } else if (update_count == 0) + { + if (debug_print) std::cout << std::endl << "\e[31m\e[1mJacobian did NOT improve solution even the slightest..\e[0m" << std::endl + << "q_ik unchanged.." << std::endl; + return false; + } + +} + +int Inverse::ik_solve_refined(Eigen::Affine3d const& desired_hand_pose) +{ + Eigen::VectorXd q_ik; + int count = 0; + const int one = 1; + const int two = 2; + const int minus_nine = -9; + + bool jacobian_result; + + + if (ik_solve(desired_hand_pose) > 0) + { + q_ik = get_soln(); + + // std::cout << std::endl << "\e[1mq_ik: \e[0m" << q_ik.transpose() << std::endl; + + jacobian_result = solve_jacobian_ik(desired_hand_pose, q_ik); + + // std::cout << std::endl << "IK Refinement Complete!" << std::endl; + + q_vec_soln_refined_ = q_ik; + + if (jacobian_result == true) + { + return 1; + } else if (jacobian_result == false) + { + return 0; + } + + + } else { + ROS_ERROR("Cannot get valid initial ik solution!"); + return -9; + } + + +} + + +bool Inverse::solve_jacobian_frozen_ik(Eigen::Vector3d const& desired_tip_coordinate, + Eigen::VectorXd &q_frozen_ik){ + Eigen::Affine3d A_fwd,A_fwd2; + Eigen::Matrix3d R1,R2,R_err; + Eigen::Vector3d dxyz,dphi,dq123; + Eigen::Vector3d dxyz2; + Eigen::VectorXd q7(1); + Eigen::VectorXd dp,dq,k_rot_axis,q_updated, dq_temp; + Eigen::MatrixXd Jacobian; + + // The upper left 3x3 of the original 6x6 Jacobian + Eigen::Matrix3d Jacobian_3x3; + + dp.resize(6); + dq.resize(7); + dq_temp.resize(7); + q_updated.resize(7); + double dtheta; + + q7 << q_frozen_ik(6); + + // q_frozen_ik SHOULD have the last 4 all 0s. + A_fwd = fwd_kin_solve(q_frozen_ik); + + // TODO not sure correct or not tho + // care only about dxyz, ignore the angular error. + dxyz = desired_tip_coordinate - A_fwd.translation(); + + Jacobian = compute_jacobian(q_frozen_ik); + Jacobian_3x3 = Jacobian.block<3,3>(0,0); + + dq123 = Jacobian_3x3.inverse()*dxyz; + + dq.resize(7); + dq.block<3,1>(0,0) = dq123; + dq(3) = 0; + dq(4) = 0; + dq(5) = 0; + dq(6) = 0; + + double err_xyz = -1; + double err_dtheta = -1; + int iteration_count = 0; + const int iter_max = 10000; + bool close_enough = false; + bool updated = false; + int update_count = 0; + double translational_tolerance = 0.0001; + bool debug_print = false; + + while ( (iteration_count < iter_max) && (!close_enough) ) { + + iteration_count++; + // Update q_ik with the current changes. q_updated should still have its 4 - 7 all 0s. + + if (iteration_count == iter_max) { + std::cout << "Jacobian (frozen) max iteration reached." << std::endl; + } + + q_updated = q_frozen_ik + dq; + + // Calculate the new position the current changes in q_ik would cause + A_fwd2 = fwd_kin_solve(q_updated); + + dxyz2 = desired_tip_coordinate - A_fwd2.translation(); + + err_xyz = dxyz.norm() - dxyz2.norm(); //want this >0 + + if (err_xyz > 0) { + + updated = true; + update_count++; + + q_frozen_ik = q_updated; // q_frozen_ik rebased + + // redo the Jacobian + A_fwd = fwd_kin_solve(q_frozen_ik); + + dxyz = desired_tip_coordinate - A_fwd.translation(); + + Jacobian = compute_jacobian(q_frozen_ik); + Jacobian_3x3 = Jacobian.block<3, 3>(0, 0); + dq123 = Jacobian_3x3.inverse() * dxyz; + + dq.resize(7); // not necessary... + dq.block<3, 1>(0, 0) = dq123; + dq(3) = 0; + dq(4) = 0; + dq(5) = 0; + dq(6) = 0; + + // see if we have reduced the TRANSLATION error to below our tolerance. + if (dxyz.norm() < translational_tolerance) { + close_enough = true; + if (debug_print) { + std::cout << std::endl << "\e[32m\e[1mFROZEN dxzy has been reduced to below " + << translational_tolerance * 1000 << " mm \e[0m" << std::endl; + } + } + + } else { + dq = dq / 2; // then go back to the beginning of this while loop + } + + } // while + + + if (update_count > 0) + { + + if (dxyz.norm() < translational_tolerance) + { + if (debug_print) { + std::cout << "And the translational error has been reduced to sub-minimeter: " << dxyz.norm() << std::endl + << std::endl; + } + } else + { + if (debug_print) { + std::cout << "\e[31mBUT the translational error is stll above 0.1 mm: \e[0m" << dxyz.norm() << std::endl; + } + } + + return true; + } else if (update_count == 0) + { + if (debug_print) { + std::cout << std::endl << "\e[31m\e[1mJacobian did NOT improve solution even the slightest..\e[0m" << std::endl + << "q_ik unchanged.." << std::endl; + } + return false; + } + +} + + + +int Inverse::ik_solve_frozen_refined(Eigen::Vector3d const& desired_tip_coordinate){ + Eigen::Vector3d q123; + Eigen::VectorXd q_frozen_ik; + + Eigen::VectorXd theta_vec, d_vec; + Vectorq7x1 qvec; + + Eigen::Affine3d desired_wrist_pose; + Eigen::Affine3d affine_frame0_wrt_base = this->get_frame0_wrt_base(); + + q_frozen_ik.resize(7); + + bool jacobian_result; + + Eigen::Vector3d desired_wrist_coordinate, desired_wrist_coordinate_wrt_frame_0; + desired_wrist_coordinate = desired_tip_coordinate * (1 - (gripper_jaw_length/desired_tip_coordinate.norm())); + + desired_wrist_pose.translation() = desired_wrist_coordinate; + desired_wrist_pose = affine_frame0_wrt_base.inverse() * desired_wrist_pose; + desired_wrist_coordinate_wrt_frame_0 = desired_wrist_pose.translation(); + + if (debug_print) { + std::cout << "gripper_jaw_length: " << gripper_jaw_length << std::endl; + + std::cout << "desired_wrist_coordinate w/rt Base frame: \n" << desired_wrist_coordinate << std::endl; + + std::cout << "desired_wrist_coordinate_wrt_frame_0: \n" << desired_wrist_coordinate_wrt_frame_0 << std::endl; + } + + + + //q123 = q123_from_wrist(desired_wrist_coordinate); // wrong! Do not use pose wrt base use pose wrt frame 0 intead... + + q123 = q123_from_wrist(desired_wrist_coordinate_wrt_frame_0); + + if (debug_print) { + std::cout << "q123: \n" << q123 << std::endl; + } + + // get DH vecs from q123 + theta_vec.resize(7); + theta_vec << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; + theta_vec(0) = q123(0); + theta_vec(1) = q123(1); + d_vec.resize(7); + d_vec << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; + d_vec(2) = q123(2); + + qvec = convert_DH_vecs_to_qvec(theta_vec, d_vec); + + if (debug_print) { + std::cout << "qvec: (converted from DH vecs) \n" << qvec << std::endl; + } + +// q_frozen_ik.block<3,1>(0,0) = q123; + + + q_frozen_ik(0) = qvec(0); + q_frozen_ik(1) = qvec(1); + q_frozen_ik(2) = qvec(2); + q_frozen_ik(3) = 0; + q_frozen_ik(4) = 0; + q_frozen_ik(5) = 0; + q_frozen_ik(6) = 0; + + jacobian_result = solve_jacobian_frozen_ik(desired_tip_coordinate, q_frozen_ik); + + q_vec_soln_frozon_ik_refined_ = q_frozen_ik; + + if (jacobian_result == true) + { +// +// ROS_WARN("DEBUG: q_vec_frozen_ik_refined:"); +// std::cout << q_vec_soln_frozon_ik_refined_ << std::endl; + + return 1; + } else if (jacobian_result == false) + { + return 0; + } + + + +} + +/// In order to be used directly by other packages that use old formats +/// This wrapper of the ik_solve_frozen_refined(Eigen::Vector3d const& desired_tip_coordinate) is created +int Inverse::ik_solve_frozen_refined(Eigen::Affine3d const& desired_hand_pose){ + + int result; + + Eigen::Vector3d desired_tip_coordinate_from_pose; + + // It takes only the coordinate of the pose the orientaion is discarded. + desired_tip_coordinate_from_pose = desired_hand_pose.translation(); + + result = ik_solve_frozen_refined(desired_tip_coordinate_from_pose); + // the q_vec_soln_frozon_ik_refined_ can be obtained by get_soln_frozon_ik_refined() + return result; + +} + + + Eigen::Vector3d Inverse::q123_from_wrist(Eigen::Vector3d wrist_pt) { // TODO(wsn) There is ambiguitity in that d3 might be negative. @@ -137,6 +614,10 @@ void Inverse::compute_w_from_tip(Eigen::Affine3d affine_gripper_tip, zvec_4a = -(zvec_5.cross(xvec_5)); } + + + + bool Inverse::fit_q_to_range(double q_min, double q_max, double &q) { q = fmod(q, 2.0 * M_PI); @@ -199,10 +680,17 @@ bool Inverse::fit_joints_to_range(Vectorq7x1 &qvec) return false; } + + + + + + int Inverse::ik_solve(Eigen::Affine3d const& desired_hand_pose) { // before doing anything else, premultiply to get everything in terms of the base. Eigen::Affine3d affine_frame0_wrt_base = this->get_frame0_wrt_base(); + desired_hand_pose_ = affine_frame0_wrt_base.inverse() * desired_hand_pose; // desired_hand_pose_ = desired_hand_pose; @@ -278,6 +766,7 @@ int Inverse::ik_solve(Eigen::Affine3d const& desired_hand_pose) // TODO(rcj, wsn) If possible resolve the ambiguity in compute_w_from_tip. Eigen::Vector3d w_wrt_base[2]; Eigen::Vector3d z_vec4[2]; + compute_w_from_tip(desired_hand_pose_, z_vec4[0], z_vec4[1], w_wrt_base[0], w_wrt_base[1]); // next step: get theta1, theta2, d3 soln from wrist position: @@ -293,11 +782,13 @@ int Inverse::ik_solve(Eigen::Affine3d const& desired_hand_pose) int index_1(index % 2); Eigen::Vector3d q123(q123_from_wrist(w_wrt_base[index_1])); - Vectorq7x1 q_sol_p = compute_q456(q123, z_vec4[index_2]); + + Vectorq7x1 q_sol_p = compute_q456(q123, z_vec4[index_2]); if (fit_joints_to_range(q_sol_p)) { q_sol.push_back(q_sol_p); + // compute the numerical errors. Eigen::Affine3d affine_test_fk = fwd_kin_solve(q_sol_p); Eigen::Matrix3d fwd_inv = affine_test_fk.rotation() * desired_hand_pose_.rotation().inverse(); @@ -365,8 +856,11 @@ Eigen::Vector3d Inverse::compute_fk_wrist(Eigen::Vector3d q123) return wrist_pt; } + + +// TODO why is this not using convert_qvec_to_DH_vecs() before fwd_kin_solve_DH()? Vectorq7x1 Inverse::compute_q456(Eigen::Vector3d q123, Eigen::Vector3d z_vec4) -{ + { Eigen::Affine3d affine_frame_wrt_base, affine_frame6_wrt_4, affine_frame6_wrt_5, fk_gripper_frame; Eigen::Vector3d z4_wrt_3, O_6_wrt_4, xvec6_wrt_5; Eigen::VectorXd theta_vec, d_vec; @@ -376,6 +870,7 @@ Vectorq7x1 Inverse::compute_q456(Eigen::Vector3d q123, Eigen::Vector3d z_vec4) theta_vec.resize(7); theta_vec << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; + theta_vec(0) = q123(0); theta_vec(1) = q123(1); @@ -429,4 +924,489 @@ Vectorq7x1 Inverse::compute_q456(Eigen::Vector3d q123, Eigen::Vector3d z_vec4) return convert_DH_vecs_to_qvec(theta_vec, d_vec); } + + + +int Inverse::ik_solve(Eigen::Affine3d const& desired_hand_pose, + std::string kinematic_set_name) { + + // TODO do we really need this? + +// // before doing anything else, premultiply to get everything in terms of the DH_frame_0. +// Eigen::Affine3d affine_frame0_wrt_base = this->get_frame0_wrt_base(); +// +// desired_hand_pose_map_[kinematic_set_name] = affine_frame0_wrt_base.inverse() * desired_hand_pose; +// +// Eigen::Vector3d z4_wrt_3, O_6_wrt_4, xvec6_wrt_5, O_5_wrt_base, zvec5_wrt_base; +// Eigen::Vector3d des_tip_origin, zvec_tip_wrt_base; +// Eigen::VectorXd theta_vec, d_vec; +// Eigen::Matrix3d R_tip_wrt_base; +// +// q_vec_soln_(0) = -10.0; +// q_vec_soln_(1) = -10.0; +// q_vec_soln_(2) = -10.0; +// q_vec_soln_(3) = -10.0; +// q_vec_soln_(4) = -10.0; +// q_vec_soln_(5) = -10.0; +// q_vec_soln_(6) = -10.0; +// +// R_tip_wrt_base = desired_hand_pose_map_[kinematic_set_name].linear(); +// zvec_tip_wrt_base = R_tip_wrt_base.col(2); +// O_5_wrt_base = des_tip_origin - zvec_tip_wrt_base * gripper_jaw_length; +// +// Eigen::Vector3d w_wrt_base[2]; +// Eigen::Vector3d z_vec4[2]; +// +// compute_w_from_tip(desired_hand_pose_, z_vec4[0], z_vec4[1], w_wrt_base[0], w_wrt_base[1]); + + +} + + + + +int Inverse::ik_solve_refined(Eigen::Affine3d const& desired_hand_pose, + std::string kinematic_set_name) { + + Eigen::VectorXd q_ik; + int count = 0; + + bool jacobian_result; + + // Still using ik_solve with original DH definitions to get an estimate out of the naive model. + if (ik_solve(desired_hand_pose) > 0) + { + q_ik = get_soln(); + + jacobian_result = solve_jacobian_ik(desired_hand_pose, q_ik, kinematic_set_name); + + q_vec_soln_refined_map_[kinematic_set_name] = q_ik; + + if (jacobian_result == true) + { + return 1; + } else if (jacobian_result == false) + { + ROS_ERROR("Cannot get accurate (0.1 mm) ik solution!"); + return 0; + } + + } else { + ROS_ERROR("Cannot get valid initial ik solution!"); + return -9; + } + +} + + + + + +bool Inverse::solve_jacobian_ik(Eigen::Affine3d const& desired_hand_pose, + Eigen::VectorXd &q_ik, + std::string kinematic_set_name){ + + Eigen::Affine3d A_fwd,A_fwd2; + Eigen::Matrix3d R1,R2,R_err; + Eigen::Vector3d dxyz,dphi; + Eigen::Vector3d dxyz2; + Eigen::VectorXd q7(1); + Eigen::VectorXd dp,dq,k_rot_axis,q_updated, dq_temp, dq_half_base, dq_double_base; + Eigen::MatrixXd Jacobian, Jacobian_inverse; + + double j1_scale_factor, j2_scale_factor, j3_scale_factor; + + j1_scale_factor = j1_scale_factor_map_[kinematic_set_name]; + j2_scale_factor = j2_scale_factor_map_[kinematic_set_name]; + j3_scale_factor = j3_scale_factor_map_[kinematic_set_name]; + + bool double_attempted(false); + bool half_attempted(false); + + dp.resize(6); + dq.resize(7); + dq_temp.resize(7); + q_updated.resize(7); + double dtheta; + + q7 << q_ik(6); + + A_fwd = fwd_kin_solve(q_ik, kinematic_set_name); + + R1 = desired_hand_pose.linear(); + R2 = A_fwd.linear(); + dxyz = desired_hand_pose.translation()-A_fwd.translation(); + R_err = R2*R1.transpose(); + Eigen::AngleAxisd angleAxis(R_err); + dtheta = angleAxis.angle(); + k_rot_axis = angleAxis.axis(); + dphi = -k_rot_axis*dtheta; + + dp.block<3,1>(0,0) = dxyz; + dp.block<3,1>(3,0) = dphi; + + Jacobian = compute_jacobian(q_ik, kinematic_set_name); + + dq = Jacobian.inverse()*dp; + dq_temp = dq; + dq.resize(7); + dq.block<6,1>(0,0) = dq_temp; + dq.block<1,1>(6,0) = q7; + + // +// j2_scale_factor = 1; +// j3_scale_factor = 1; + dq.block<1,1>(1,0) = dq.block<1,1>(0,0)/j1_scale_factor; + dq.block<1,1>(1,0) = dq.block<1,1>(1,0)/j2_scale_factor; + dq.block<1,1>(2,0) = dq.block<1,1>(2,0)/j3_scale_factor; + + + double err_xyz = -1; + double err_dtheta = -1; + int iteration_count = 0; + + const int iter_max = 15; + + bool close_enough = false; + bool updated = false; + int update_count = 0; + double translational_tolerance = 0.0002; + +//see if this is an improvement: + while ( (iteration_count < iter_max) && (!close_enough) ) + { + + iteration_count++; + + if (iteration_count == iter_max) { + std::cout <<"[" << kinematic_set_name << "] "<< "Jacobian Max iteration reached." << std::endl; + } + + + + // Update q_ik with the current changes. + q_updated = q_ik + dq; + + + + // Calculate the new position the current changes in q_ik would cause + A_fwd2 = fwd_kin_solve(q_updated, kinematic_set_name); + + // Calculate the distance form the goal + R2 = A_fwd2.linear(); + R_err = R2*R1.transpose(); + Eigen::AngleAxisd angleAxis2(R_err); + // get dxyz2 and dtheta2 + dxyz2 = desired_hand_pose.translation()-A_fwd2.translation(); + double dtheta2 = angleAxis2.angle(); + + // See if the distance decrease the gap + err_dtheta = fabs(dtheta)-fabs(dtheta2); //want this >0 + err_xyz = dxyz.norm()-dxyz2.norm(); //want this >0 + + // If the result is better rebase q_ik and update the Jacobian, dp and dq acoordingly + // Otherwise half the dq, use the same base q_ik and try again + if ((err_dtheta>0)||(err_xyz>0)) { + updated = true; + update_count++; + + q_ik = q_updated; // q_ik rebased + + // redo the Jacobian + A_fwd = fwd_kin_solve(q_ik, kinematic_set_name); + + R1 = desired_hand_pose.linear(); + R2 = A_fwd.linear(); + dxyz = desired_hand_pose.translation()-A_fwd.translation(); + R_err = R2*R1.transpose(); + Eigen::AngleAxisd angleAxis(R_err); + dtheta = angleAxis.angle(); + k_rot_axis = angleAxis.axis(); + dphi = -k_rot_axis*dtheta; + + dp.block<3,1>(0,0) = dxyz; + dp.block<3,1>(3,0) = dphi; + Jacobian = compute_jacobian(q_ik, kinematic_set_name); + + Jacobian_inverse = Jacobian.inverse(); + +// dq = Jacobian_inverse*dp; + + dq.resize(7); + + // TODO check + for (int n = 0; n < 6; n++) { + + double dqn(0); + + for (int i = 0; i < 6; i++) { + dqn = dqn + Jacobian_inverse(n, i)*dp(i); + } + + dq[n] = dqn; + + } + + dq.block<1,1>(6,0) = q7; + + + + // see if we have reduced the tranlational error below our tolerance. + if (dxyz.norm() < translational_tolerance) + { + close_enough = true; + std::cout << std::endl <<"[" << kinematic_set_name << "] "<< "\e[32m\e[1mdxzy has been reduced to below " + << translational_tolerance*1000 << " mm \e[0m" <0)&&(err_xyz>0) failed." << std::endl; + dq = dq/2; // then go back to the beginning of this while loop + + + } + + } // while + + // Report + if (update_count > 0) + { + std::cout << "[" << kinematic_set_name << "] " <<"\e[1m\e[32mJacobian IK has improved the initial solution.\e[0m" << std::endl; + + if (dxyz.norm() < translational_tolerance) + { + std::cout << "[" << kinematic_set_name << "] " <<"And the translational error has been reduced to sub-minimeter: " << dxyz.norm() << std::endl + << std::endl; + } else + { + std::cout << "[" << kinematic_set_name << "] " << "\e[31mBUT the translational error is stll above 0.1 mm: \e[0m" << dxyz.norm() << std::endl; + } + + std::cout << "[" << kinematic_set_name << "] " << "fabs(dtheta): " << fabs(dtheta) << std::endl; + + return true; + } else if (update_count == 0) + { + std::cout << std::endl << "[" << kinematic_set_name << "] " << "\e[31m\e[1mJacobian IK did NOT improve solution even the slightest..\e[0m" << std::endl + << "q_ik unchanged.." << std::endl; + return false; + } + + +} + +int Inverse::ik_solve_frozen_refined(Eigen::Vector3d const& desired_tip_coordinate, + std::string kinematic_set_name) { + + Eigen::Vector3d q123; + Eigen::VectorXd q_frozen_ik; + + Eigen::VectorXd theta_vec, d_vec; + Vectorq7x1 qvec; + + Eigen::Affine3d desired_wrist_pose; + Eigen::Affine3d affine_frame0_wrt_base = this->get_frame0_wrt_base(); + + q_frozen_ik.resize(7); + + bool jacobian_result; + + Eigen::Vector3d desired_wrist_coordinate, desired_wrist_coordinate_wrt_frame_0; + desired_wrist_coordinate = desired_tip_coordinate * (1 - gripper_jaw_length / desired_tip_coordinate.norm()); + + desired_wrist_pose.translation() = desired_wrist_coordinate; + desired_wrist_pose = affine_frame0_wrt_base.inverse() * desired_wrist_pose; + desired_wrist_coordinate_wrt_frame_0 = desired_wrist_pose.translation(); + + q123 = q123_from_wrist(desired_wrist_coordinate_wrt_frame_0); + + // get DH vecs from q123 + theta_vec.resize(7); + theta_vec << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; + theta_vec(0) = q123(0); + theta_vec(1) = q123(1); + d_vec.resize(7); + d_vec << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; + d_vec(2) = q123(2); + + // Note taht we still use the generic ik to get an estimate before the result is tested and improved by the Jacobian. + qvec = convert_DH_vecs_to_qvec(theta_vec, d_vec); + + q_frozen_ik(0) = qvec(0); + q_frozen_ik(1) = qvec(1); + q_frozen_ik(2) = qvec(2); + q_frozen_ik(3) = 0; + q_frozen_ik(4) = 0; + q_frozen_ik(5) = 0; + q_frozen_ik(6) = 0; + + jacobian_result = solve_jacobian_frozen_ik(desired_tip_coordinate, q_frozen_ik, kinematic_set_name); + + q_vec_soln_frozon_ik_refined_map_[kinematic_set_name] = q_frozen_ik; + + if (jacobian_result == true) + { + return 1; + } else if (jacobian_result == false) + { + return 0; + } + +} + + +bool Inverse::solve_jacobian_frozen_ik(Eigen::Vector3d const& desired_tip_coordinate, + Eigen::VectorXd &q_frozen_ik, + std::string kinematic_set_name) { + + Eigen::Affine3d A_fwd,A_fwd2; + Eigen::Matrix3d R1,R2,R_err; + Eigen::Vector3d dxyz,dphi,dq123; + Eigen::Vector3d dxyz2; + Eigen::VectorXd q7(1); + Eigen::VectorXd dp,dq,k_rot_axis,q_updated, dq_temp; + Eigen::MatrixXd Jacobian; + + // The upper left 3x3 of the original 6x6 Jacobian + Eigen::Matrix3d Jacobian_3x3; + + dp.resize(6); + dq.resize(7); + dq_temp.resize(7); + q_updated.resize(7); + double dtheta; + + q7 << q_frozen_ik(6); + + // q_frozen_ik SHOULD have the last 4 all 0s. + A_fwd = fwd_kin_solve(q_frozen_ik, kinematic_set_name); + + // care only about dxyz, ignore the angular error. + dxyz = desired_tip_coordinate - A_fwd.translation(); + + Jacobian = compute_jacobian(q_frozen_ik, kinematic_set_name); + Jacobian_3x3 = Jacobian.block<3,3>(0,0); + + dq123 = Jacobian_3x3.inverse()*dxyz; + + dq.resize(7); + dq.block<3,1>(0,0) = dq123; + dq(3) = 0; + dq(4) = 0; + dq(5) = 0; + dq(6) = 0; + + double err_xyz = -1; + double err_dtheta = -1; + int iteration_count = 0; + const int iter_max = 10000; + bool close_enough = false; + bool updated = false; + int update_count = 0; + double translational_tolerance = 0.0001; + bool debug_print = false; + + while ( (iteration_count < iter_max) && (!close_enough) ) { + + iteration_count++; + // Update q_ik with the current changes. q_updated should still have its 4 - 7 all 0s. + + if (iteration_count == iter_max) { + std::cout <<"[" << kinematic_set_name << "] "<< "Jacobian (frozen) max iteration reached." << std::endl; + } + + q_updated = q_frozen_ik + dq; + + // Calculate the new position the current changes in q_ik would cause + A_fwd2 = fwd_kin_solve(q_updated, kinematic_set_name); + + dxyz2 = desired_tip_coordinate - A_fwd2.translation(); + + err_xyz = dxyz.norm() - dxyz2.norm(); //want this >0 + + if (err_xyz > 0) { + + updated = true; + update_count++; + + q_frozen_ik = q_updated; // q_frozen_ik rebased + + // redo the Jacobian + A_fwd = fwd_kin_solve(q_frozen_ik, kinematic_set_name); + + dxyz = desired_tip_coordinate - A_fwd.translation(); + + Jacobian = compute_jacobian(q_frozen_ik, kinematic_set_name); + Jacobian_3x3 = Jacobian.block<3, 3>(0, 0); + dq123 = Jacobian_3x3.inverse() * dxyz; + + dq.resize(7); // not necessary... + dq.block<3, 1>(0, 0) = dq123; + dq(3) = 0; + dq(4) = 0; + dq(5) = 0; + dq(6) = 0; + + // see if we have reduced the TRANSLATION error to below our tolerance. + if (dxyz.norm() < translational_tolerance) { + close_enough = true; + if (debug_print) { + std::cout << std::endl << "[" << kinematic_set_name << "] "<<"\e[32m\e[1mFROZEN dxzy has been reduced to below " + << translational_tolerance * 1000 << " mm \e[0m" << std::endl; + } + } + + } else { + dq = dq / 2; // then go back to the beginning of this while loop + } + + } // while + + // Report + if (update_count > 0) + { + std::cout << "[" << kinematic_set_name << "] "<< "\e[1m\e[32mJacobian IK has improved the initial solution.\e[0m" << std::endl; + + if (dxyz.norm() < translational_tolerance) + { + std::cout << "[" << kinematic_set_name << "] "<< "And the translational error has been reduced to sub-minimeter: " << dxyz.norm() << std::endl + << std::endl; + } else + { + std::cout << "[" << kinematic_set_name << "] "<< "\e[31mBUT the translational error is stll above 0.1 mm: \e[0m" << dxyz.norm() << std::endl; + } + + std::cout << "[" << kinematic_set_name << "] "<< "fabs(dtheta): " << fabs(dtheta) << std::endl; + + return true; + } else if (update_count == 0) + { + std::cout << std::endl << "[" << kinematic_set_name << "] "<< "\e[31m\e[1mJacobian IK did NOT improve solution even the slightest..\e[0m" << std::endl + << "q_ik unchanged.." << std::endl; + return false; + } + +} + + +int Inverse::ik_solve_frozen_refined(Eigen::Affine3d const& desired_hand_pose, + std::string kinematic_set_name) { + + int result; + + Eigen::Vector3d desired_tip_coordinate_from_pose; + + // It takes only the coordinate of the pose the orientaion is discarded. + desired_tip_coordinate_from_pose = desired_hand_pose.translation(); + + result = ik_solve_frozen_refined(desired_tip_coordinate_from_pose, kinematic_set_name); + // the q_vec_soln_frozon_ik_refined_ can be obtained by get_soln_frozon_ik_refined() + return result; + +} + + + + } // namespace davinci_kinematics diff --git a/src/davinci_kinematic_definitions.cpp b/src/davinci_kinematic_definitions.cpp index c2f7f93..9ccd4dc 100644 --- a/src/davinci_kinematic_definitions.cpp +++ b/src/davinci_kinematic_definitions.cpp @@ -23,9 +23,14 @@ namespace davinci_kinematics { // origin0 coincident w/ origin1 -const double DH_a1 = 0.0; -// axis z1,z2 intersect -const double DH_a2 = 0.0; +//const double DH_a1 = 0.005970; // RN 20180712A1 PSM1 +const double DH_a1 = 0.0; // RN 20180713 Experimental + +// axis z1,z2 (prismatic) intersect +//const double DH_a2 = 0.001250 ; // RN 20180712A1 PSM1 +const double DH_a2 = 0.0; // RN 20180713 Experimental + + // axes z2 (prismatic) and z3 (shaft rot) intersect const double DH_a3 = 0.0; // axes z3 (shaft rot) and z4 (wrist bend) intersect @@ -37,18 +42,26 @@ const double DH_a6 = 0.0; // not sure what to do with this one const double DH_a7 = 0.0; -const double DH_d1 = 0.0; -// THIS IS VARIABLE -const double DH_d2 = 0.0; -const double DH_d3 = 0.0; -const double DH_d4 = 0.0; -const double DH_d5 = 0.0; -const double DH_d6 = 0.0; -const double DH_d7 = 0.0; +// const double DH_d1 = 0.0; +// // THIS IS VARIABLE +// const double DH_d2 = 0.0; +// const double DH_d3 = 0.0; +// const double DH_d4 = 0.0; +// const double DH_d5 = 0.0; +// const double DH_d6 = 0.0; +// const double DH_d7 = 0.0; // robot.DH.alpha= '[-pi/2 0 -pi/2 pi/2 -pi/2 0]'; -const double DH_alpha1 = M_PI/2.0; -const double DH_alpha2 = M_PI/2.0; + + +const double DH_alpha1 = M_PI/2.0; // RN Experimental/Original +//const double DH_alpha1 = -M_PI/2.0; // RN Experimental/New@1/3 + + +//const double DH_alpha2 = 1.562359; // RN 20180712A1 PSM1 + const double DH_alpha2 = M_PI/2.0; // RN Experimental + + // prismatic axis is aligned with tool-shaft spin axis const double DH_alpha3 = 0.0; const double DH_alpha4 = M_PI/2.0; @@ -69,8 +82,18 @@ const double cannula_short_length = .035; // must command this much displacement to get wrist-bend axis to intersect base origin const double insertion_offset = 0.0156; -const double DH_q_offset0 = 0.0; -const double DH_q_offset1 = M_PI/2.0; + +// THETAs + +//const double DH_q_offset0 = 0.0; +// const double DH_q_offset0 = 3.141593; // RN 20180712A1 PSM1 +const double DH_q_offset0 = 0.0; // RN 20180713 Experimental/Original +//const double DH_q_offset0 = M_PI; // RN 20180713 Experiental/New@2/3 + +const double DH_q_offset1 = M_PI/2.0; // RN 20180713 Experimental/Original +//const double DH_q_offset1 = -M_PI/2.0; // RN 20180713 Experimental/New@3/3 + + // erdem IK tested, sign is negative. q3 should be larger than abs(insertion_offset) const double DH_q_offset2 = -insertion_offset; const double DH_q_offset3 = M_PI; @@ -123,10 +146,10 @@ const double DH_a_params[7] = DH_a1, DH_a2, DH_a3, DH_a4, DH_a5, DH_a6, DH_a7 }; -const double DH_d_params[7] = -{ - DH_d1, DH_d2, DH_d3, DH_d4, DH_d5, DH_d6, DH_d7 -}; +// const double DH_d_params[7] = +// { +// DH_d1, DH_d2, DH_d3, DH_d4, DH_d5, DH_d6, DH_d7 +// }; const double DH_alpha_params[7] = {