problems with hand-eye robot calibration

We wanted to use the robot hand-eye calibration for calibrating a zed mini camera connected to the robot hand. These are the problems we faced:

as documented in https://github.com/pairlab/franka_arm_infra/issues/22#issuecomment-1496617340 , we had the problem with realsense running. running https://github.com/pairlab/franka_arm_infra/issues/22#issuecomment-1505870134 causes workstation freezing, so we had to restart.
the aruco_board configuations as written in https://github.com/pairlab/franka_arm_infra/blob/ebb6b4fd4de964238b1ff9ee4e5929788029c863/robot_toolkit/config/robot_camera_calibration.yaml#L19C1-L24C1 is different from actual one added in https://github.com/pairlab/franka_arm_infra/tree/master/robot_toolkit/calibration_board. The number of columns, rows are different, the sizes are different.

at the moment based on documentation, move_robot_automatically and robot_pose_collector (["ros_tf", "zmq"]) are entangled. However, when we tesed back in time, I am pretty sure we did move robot manually while using ros_tf. based on the code and documentation this is not possible. This entanglement seems to be unnecessary and need to be fixed:

robot_pose_collector:
values = ["ros_tf", "zmq"]
This specifies, wheter to read end-effector pose from a TF tree or using libfranka.
"ros_tf" only works when "move_robot_automatically" is True
"zmq" only works when "move_robot_automatically" is False

and

move_robot_automatically
values = [True or False]
If set to True, the robot moves its end-effector to randomly sampled configurations around the initial end-effector pose. "robot_pose_collector" has to be "ros_tf"
If set to false, the user is expected to move the end-effector manually and press "Enter" to collect each pose measurement. "robot_pose_colector" has to be "zmq"

Why we need this part of the code: https://github.com/pairlab/franka_arm_infra/blob/ebb6b4fd4de964238b1ff9ee4e5929788029c863/robot_toolkit/robot_arm_algos/src/config_reader.py#L60-L62

Finally, when we removed those lines of the code and running camera calibration with data collection, we had the following results:

INFO     [_data_collector.py:89] stopping data collection
Traceback (most recent call last):
File "robot_camera_calibration.py", line 41, in <module>
main(args)
File "robot_camera_calibration.py", line 31, in main
robot_camera_calibrator.run_calibration()
File "/root/git/robot_toolkit/robot_arm_algos/src/robot_camera_calibration/robot_camera_calibrator.py", line 45, in run_calibration
results = self.run_calibration_all_solvers()
File "/root/git/robot_toolkit/robot_arm_algos/src/robot_camera_calibration/_calibration_solver.py", line 46, in run_calibration_all_solvers
out.append((self.run_calibration_solver(solver_name), solver_name))
File "/root/git/robot_toolkit/robot_arm_algos/src/robot_camera_calibration/_calibration_solver.py", line 29, in run_calibration_solver
output_rvec, output_tvec = cv2.calibrateHandEye(self.A_rvecs,
cv2.error: OpenCV(4.8.1) /io/opencv/modules/calib3d/src/calibration_handeye.cpp:524: error: (-7:Iterations do not converge) Rotation normalization issue: determinant(R) is null in function 'normalizeRotation'
root@sf3202msi-Z490-A-PRO-Aegis-RS:~/git/robot_toolkit# vim config/robot_zed_camera_calibration.yaml
root@sf3202msi-Z490-A-PRO-Aegis-RS:~/git/robot_toolkit# vim config/robot_zed_camera_calibration.yaml
root@sf3202msi-Z490-A-PRO-Aegis-RS:~/git/robot_toolkit# cat INFO     [_data_collector.py:89] stopping data collection
Traceback (most recent call last):
File "robot_camera_calibration.py", line 41, in <module>
main(args)
File "robot_camera_calibration.py", line 31, in main
robot_camera_calibrator.run_calibration()
File "/root/git/robot_toolkit/robot_arm_algos/src/robot_camera_calibration/robot_camera_calibrator.py", line 45, in run_calibration
results = self.run_calibration_all_solvers()
File "/root/git/robot_toolkit/robot_arm_algos/src/robot_camera_calibration/_calibration_solver.py", line 46, in run_calibration_all_solvers
out.append((self.run_calibration_solver(solver_name), solver_name))
File "/root/git/robot_toolkit/robot_arm_algos/src/robot_camera_calibration/_calibration_solver.py", line 29, in run_calibration_solver
output_rvec, output_tvec = cv2.calibrateHandEye(self.A_rvecs,
cv2.error: OpenCV(4.8.1) /io/opencv/modules/calib3d/src/calibration_handeye.cpp:524: error: (-7:Iterations do not converge) Rotation normalization issue: determinant(R) is null in function 'normalizeRotation'
root@sf3202msi-Z490-A-PRO-Aegis-RS:~/git/robot_toolkit# vim config/robot_zed_camera_calibration.yaml
root@sf3202msi-Z490-A-PRO-Aegis-RS:~/git/robot_toolkit# vim config/robot_zed_camera_calibration.yaml
root@sf3202msi-Z490-A-PRO-Aegis-RS:~/git/robot_toolkit# cat robot_arm_algos/src/robot_camera_calibration/data/file_zed.txt
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.3142142157497399,-0.281232321905755,-1.5769491028769251,-0.08505167671485281,0.10869832690184308,0.39249290501489786
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.28773136905876123,-0.2209121417232348,-0.829223978364923,-0.20964332708926126,0.017531313822759916,0.3672961434396176
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.2570181779444119,-0.09746567994537958,-1.0699099031113966,-0.15603926586472833,-0.027514982324621445,0.29307311079805554
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.21535897793733225,-0.8021849904215289,-2.6183627779871346,0.0804890806733553,0.14759499758703634,0.302972669369931
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.14409498246604852,-0.2052178522624006,-1.504308288031509,-0.11400544279069431,0.1049010721062046,0.362044018415615
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.10768996297912165,-0.13775094708694482,-1.5388697554875341,-0.09747698001689804,0.09620180060062024,0.2818964855091452
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.0761867753585027,0.1071534366043921,1.6475733127998262,0.18841268404313805,-0.11382140603083668,0.4070216521176688
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.19160444938074436,0.2583456645203763,1.65504216147805,0.19073127701996334,-0.0506569473583837,0.3367004287030583
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.6170967953795665,-0.12106721094334894,1.2168673382563386,0.12034627122336379,-0.10865791043168911,0.40943607485617844
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.09648597580536017,0.369950401393866,1.7875017050482902,0.10220678523705298,-0.044714769070031656,0.3207522259643255
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.2617917295063692,0.29022046091289866,1.2313526959379266,0.15115300797894204,-0.029634361001287492,0.38088352021160315
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.18329912971167342,0.1809686748966991,1.5395180231517518,0.14863225520732926,-0.07761643915144989,0.373717923052905
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.19528621108989774,0.08985369436358373,1.6527944705778939,0.15209171133351346,-0.15196760613214502,0.31990031388617146
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.297447071725141,0.24085158978157642,1.546094400268486,0.14347003109971654,-0.1026537255446094,0.28603619027253285
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.03862430333273748,0.6825080702572005,2.7623647704478467,0.11155853079526043,0.06213323505774098,0.3183670489239787
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.31360633716881847,0.0025016099403963443,1.1930977189119265,0.12625739421008036,-0.11892818921360301,0.39162148275970066
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.31020300715752364,-0.36940737908506005,-1.524298466857078,-0.13405980383137303,0.06831699191997168,0.36715485470681464
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.4660244603311664,-0.2176859173440443,-1.0497244832213528,-0.12445600909599651,0.05747638488640651,0.34198397139094505
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.5430288027947624,-0.15697679934028275,-0.5509116306435331,-0.12872590786435661,-0.005455670150886657,0.3376021974329135
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.3085194934692491,-0.563938718316035,-2.108400115039594,0.016448794225385432,0.13990007202207771,0.3105786093327803
root@sf3202msi-Z490-A-PRO-Aegis-RS:~/git/robot_toolkit# vim robot_arm_algos/src/robot_camera_calibration/_calibration_solver.py
root@sf3202msi-Z490-A-PRO-Aegis-RS:~/git/robot_toolkit# vim config/robot_zed_camera_calibration.yaml
root@sf3202msi-Z490-A-PRO-Aegis-RS:~/git/robot_toolkit# python3 robot_camera_calibration.py --config_file config/robot_zed_camera_calibration.yaml
WARNING - 2023-10-12 18:38:03,219 - rigid_transformations - autolab_core not installed as catkin package, RigidTransform ros methods will be unavailable
INFO     [_data_collector.py:142] Make sure you point the robot to a good initial                         configuration looking at the calibration tag
DEBUG    [_calibration_solver.py:38] Output of calibration for solver: HAND_EYE_DANIILIDIS is: [[-0.99929219 -0.01292216 -0.03532908  0.05952314]
[-0.01277403 -0.76678294  0.64177936 -0.2188662 ]
[-0.03538291  0.6417764   0.76607513  0.06314412]
[ 0.          0.          0.          1.        ]]
DEBUG    [_calibration_solver.py:39] This yields rotation error (mean, variance) in degrees = ([22.47739167  1.50464277 -4.11897341],[  288.65680172   238.53353548 12574.97380088])
DEBUG    [_calibration_solver.py:40] This yields translation error (mean, variance) in cms - ([ 2.76434745 -9.86979657  5.75722915],[130.73247947  99.71161665  48.45092654])
DEBUG    [_calibration_solver.py:38] Output of calibration for solver: HAND_EYE_HORAUD is: [[ 9.99472705e-01 -2.36764565e-02  2.22202025e-02  2.41210128e+15]
[-2.36764565e-02 -9.99719639e-01 -2.63117199e-04 -1.49018579e+17]
[ 2.22202025e-02 -2.63117199e-04 -9.99753066e-01  7.97629263e+15]
[ 0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00000000e+00]]
DEBUG    [_calibration_solver.py:39] This yields rotation error (mean, variance) in degrees = ([22.47739167  1.50464277 -4.11897341],[  288.65680172   238.53353548 12574.97380088])
DEBUG    [_calibration_solver.py:40] This yields translation error (mean, variance) in cms - ([101.57894737   0.           0.        ],[155.40166205   0.           0.        ])
/root/git/robot_toolkit/robot_arm_algos/src/robot_camera_calibration/_calibration_data_utils.py:26: UserWarning: Gimbal lock detected. Setting third angle to zero since it is not possible to uniquely determine all angles.
rot_error = [difference_r.as](http://difference_r.as/)_euler('xyz', degrees=True)
DEBUG    [_calibration_solver.py:38] Output of calibration for solver: HAND_EYE_PARK is: [[nan nan nan nan]
[nan nan nan nan]
[nan nan nan nan]
[ 0.  0.  0.  1.]]
DEBUG    [_calibration_solver.py:39] This yields rotation error (mean, variance) in degrees = ([nan nan  0.],[nan nan  0.])
DEBUG    [_calibration_solver.py:40] This yields translation error (mean, variance) in cms - ([nan nan nan],[nan nan nan])
DEBUG    [_calibration_solver.py:38] Output of calibration for solver: HAND_EYE_TSAI is: [[ 1.00000000e+00 -3.25830441e-17  3.00673646e-18 -2.29293690e+15]
[ 3.25830441e-17  1.00000000e+00 -7.57317884e-18  1.51590318e+17]
[-3.00673646e-18  7.57317884e-18  1.00000000e+00 -8.11339294e+15]
[ 0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00000000e+00]]
DEBUG    [_calibration_solver.py:39] This yields rotation error (mean, variance) in degrees = ([22.47739167  1.50464277 -4.11897341],[  288.65680172   238.53353548 12574.97380088])
DEBUG    [_calibration_solver.py:40] This yields translation error (mean, variance) in cms - ([-99.21052632   0.           0.        ],[38.85041551  0.          0.        ])
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.3142142157497399,-0.281232321905755,-1.5769491028769251,-0.08505167671485281,0.10869832690184308,0.39249290501489786
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.28773136905876123,-0.2209121417232348,-0.829223978364923,-0.20964332708926126,0.017531313822759916,0.3672961434396176
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.2570181779444119,-0.09746567994537958,-1.0699099031113966,-0.15603926586472833,-0.027514982324621445,0.29307311079805554
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.21535897793733225,-0.8021849904215289,-2.6183627779871346,0.0804890806733553,0.14759499758703634,0.302972669369931
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.14409498246604852,-0.2052178522624006,-1.504308288031509,-0.11400544279069431,0.1049010721062046,0.362044018415615
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.10768996297912165,-0.13775094708694482,-1.5388697554875341,-0.09747698001689804,0.09620180060062024,0.2818964855091452
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.0761867753585027,0.1071534366043921,1.6475733127998262,0.18841268404313805,-0.11382140603083668,0.4070216521176688
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.19160444938074436,0.2583456645203763,1.65504216147805,0.19073127701996334,-0.0506569473583837,0.3367004287030583
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.6170967953795665,-0.12106721094334894,1.2168673382563386,0.12034627122336379,-0.10865791043168911,0.40943607485617844
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.09648597580536017,0.369950401393866,1.7875017050482902,0.10220678523705298,-0.044714769070031656,0.3207522259643255
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.2617917295063692,0.29022046091289866,1.2313526959379266,0.15115300797894204,-0.029634361001287492,0.38088352021160315
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.18329912971167342,0.1809686748966991,1.5395180231517518,0.14863225520732926,-0.07761643915144989,0.373717923052905
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.19528621108989774,0.08985369436358373,1.6527944705778939,0.15209171133351346,-0.15196760613214502,0.31990031388617146
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.297447071725141,0.24085158978157642,1.546094400268486,0.14347003109971654,-0.1026537255446094,0.28603619027253285
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.03862430333273748,0.6825080702572005,2.7623647704478467,0.11155853079526043,0.06213323505774098,0.3183670489239787
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.31360633716881847,0.0025016099403963443,1.1930977189119265,0.12625739421008036,-0.11892818921360301,0.39162148275970066
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.31020300715752364,-0.36940737908506005,-1.524298466857078,-0.13405980383137303,0.06831699191997168,0.36715485470681464
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.4660244603311664,-0.2176859173440443,-1.0497244832213528,-0.12445600909599651,0.05747638488640651,0.34198397139094505
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.5430288027947624,-0.15697679934028275,-0.5509116306435331,-0.12872590786435661,-0.005455670150886657,0.3376021974329135
-2.558460273327198,0.07857191948800965,1.7354735040216558,0.36318761110305786,0.025253981351852417,0.5358628630638123,-0.3085194934692491,-0.563938718316035,-2.108400115039594,0.016448794225385432,0.13990007202207771,0.3105786093327803
root@sf3202msi-Z490-A-PRO-Aegis-RS:~/git/robot_toolkit# vim robot_arm_algos/src/robot_camera_calibration/_calibration_solver.py
root@sf3202msi-Z490-A-PRO-Aegis-RS:~/git/robot_toolkit# vim config/robot_zed_camera_calibration.yaml
root@sf3202msi-Z490-A-PRO-Aegis-RS:~/git/robot_toolkit# python3 robot_camera_calibration.py --config_file config/robot_zed_camera_calibration.yaml
WARNING - 2023-10-12 18:38:03,219 - rigid_transformations - autolab_core not installed as catkin package, RigidTransform ros methods will be unavailable
INFO     [_data_collector.py:142] Make sure you point the robot to a good initial                         configuration looking at the calibration tag
DEBUG    [_calibration_solver.py:38] Output of calibration for solver: HAND_EYE_DANIILIDIS is: [[-0.99929219 -0.01292216 -0.03532908  0.05952314]
[-0.01277403 -0.76678294  0.64177936 -0.2188662 ]
[-0.03538291  0.6417764   0.76607513  0.06314412]
[ 0.          0.          0.          1.        ]]
DEBUG    [_calibration_solver.py:39] This yields rotation error (mean, variance) in degrees = ([22.47739167  1.50464277 -4.11897341],[  288.65680172   238.53353548 12574.97380088])
DEBUG    [_calibration_solver.py:40] This yields translation error (mean, variance) in cms - ([ 2.76434745 -9.86979657  5.75722915],[130.73247947  99.71161665  48.45092654])
DEBUG    [_calibration_solver.py:38] Output of calibration for solver: HAND_EYE_HORAUD is: [[ 9.99472705e-01 -2.36764565e-02  2.22202025e-02  2.41210128e+15]
[-2.36764565e-02 -9.99719639e-01 -2.63117199e-04 -1.49018579e+17]
[ 2.22202025e-02 -2.63117199e-04 -9.99753066e-01  7.97629263e+15]
[ 0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00000000e+00]]
DEBUG    [_calibration_solver.py:39] This yields rotation error (mean, variance) in degrees = ([22.47739167  1.50464277 -4.11897341],[  288.65680172   238.53353548 12574.97380088])
DEBUG    [_calibration_solver.py:40] This yields translation error (mean, variance) in cms - ([101.57894737   0.           0.        ],[155.40166205   0.           0.        ])
/root/git/robot_toolkit/robot_arm_algos/src/robot_camera_calibration/_calibration_data_utils.py:26: UserWarning: Gimbal lock detected. Setting third angle to zero since it is not possible to uniquely determine all angles.
rot_error = [difference_r.as](http://difference_r.as/)_euler('xyz', degrees=True)
DEBUG    [_calibration_solver.py:38] Output of calibration for solver: HAND_EYE_PARK is: [[nan nan nan nan]
[nan nan nan nan]
[nan nan nan nan]
[ 0.  0.  0.  1.]]
DEBUG    [_calibration_solver.py:39] This yields rotation error (mean, variance) in degrees = ([nan nan  0.],[nan nan  0.])
DEBUG    [_calibration_solver.py:40] This yields translation error (mean, variance) in cms - ([nan nan nan],[nan nan nan])
DEBUG    [_calibration_solver.py:38] Output of calibration for solver: HAND_EYE_TSAI is: [[ 1.00000000e+00 -3.25830441e-17  3.00673646e-18 -2.29293690e+15]
[ 3.25830441e-17  1.00000000e+00 -7.57317884e-18  1.51590318e+17]
[-3.00673646e-18  7.57317884e-18  1.00000000e+00 -8.11339294e+15]
[ 0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00000000e+00]]
DEBUG    [_calibration_solver.py:39] This yields rotation error (mean, variance) in degrees = ([22.47739167  1.50464277 -4.11897341],[  288.65680172   238.53353548 12574.97380088])
DEBUG    [_calibration_solver.py:40] This yields translation error (mean, variance) in cms - ([-99.21052632   0.           0.        ],[38.85041551  0.          0.        ])

What are the data that is saved in the calib_data_file_name; from the documentation it not clear. Also, we were wondering why had the first 6 columns with fixed values?

uoft-cs-robotics / robot_system_tools

problems with hand-eye robot calibration #33