Mark iii dvrk

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}
-)
+)

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 
+
+
+
+

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 

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
+

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
+