.. DO NOT EDIT. .. THIS FILE WAS AUTOMATICALLY GENERATED BY SPHINX-GALLERY. .. TO MAKE CHANGES, EDIT THE SOURCE PYTHON FILE: .. "examples/robolab/example_curi_transformation_verification_with_optitrack.py" .. LINE NUMBERS ARE GIVEN BELOW. .. only:: html .. note:: :class: sphx-glr-download-link-note :ref:`Go to the end ` to download the full example code .. rst-class:: sphx-glr-example-title .. _sphx_glr_examples_robolab_example_curi_transformation_verification_with_optitrack.py: CURI FK transformation verification with optitrack ======================== Verify the precision of transformation matrix by optitrack .. GENERATED FROM PYTHON SOURCE LINES 7-149 .. code-block:: default import matplotlib.pyplot as plt import numpy as np import pandas as pd import rofunc as rf def process_ros_optitrack_csv(path): data = np.array(pd.read_csv(path))[:, 4:] time = np.reshape(np.array(pd.read_csv(path))[:, 0], (-1, 1)) * 1e-11 time = time - np.int_(time) data = np.hstack((time, data)) return data def process_ros_joint_states_csv(path): data = np.array(pd.read_csv(path))[:, 11:18] time = np.reshape(np.array(pd.read_csv(path))[:, 0], (-1, 1)) * 1e-11 time = time - np.int_(time) data = np.hstack((time, data)) return data def compute_errors(arm_joint_left, arm_joint_right, robot_pose, end_pose_left, end_pose_right): optitrack_position_left = end_pose_left[:3] - robot_pose[:3] # optitrack_position_left[0], optitrack_position_left[1] = optitrack_position_left[1], optitrack_position_left[0] optitrack_position_right = end_pose_right[:3] - robot_pose[:3] # optitrack_position_right[0], optitrack_position_right[1] = optitrack_position_right[1], optitrack_position_right[0] transform_pose_left, transform_pose_right = rf.robolab.transform_optitrackbasetoarmend(np.array([0, -0.7, 0.1]), arm_joint_left, arm_joint_right) transform_position_left = transform_pose_left[:3, 3] transform_position_right = transform_pose_right[:3, 3] errors_left = optitrack_position_left - transform_position_left errors_right = optitrack_position_right - transform_position_right return errors_left, errors_right if __name__ == '__main__': robot_pose = process_ros_optitrack_csv( '/home/ubuntu/Data/optitrack test/csv_20230426/robotpose.csv') end_pose_left = process_ros_optitrack_csv( '/home/ubuntu/Data/optitrack test/csv_20230426/leftpose.csv') end_pose_right = process_ros_optitrack_csv( '/home/ubuntu/Data/optitrack test/csv_20230426/leftpose.csv') arm_joint_left = process_ros_joint_states_csv( '/home/ubuntu/Data/optitrack test/csv_20230426/leftjoint.csv') arm_joint_right = process_ros_joint_states_csv( '/home/ubuntu/Data/optitrack test/csv_20230426/leftjoint.csv') index = np.zeros(len(robot_pose)) for i in range(len(robot_pose)): arr = arm_joint_left[:, 0] value = end_pose_left[i, 0] difference_array = np.absolute(arr - value) index[i] = difference_array.argmin() arm_joint_left = np.array([arm_joint_left[int(i)] for i in index]) arm_joint_right = arm_joint_left t_1 = np.arange(0, 20, 20 / len(end_pose_left)) plt.plot(t_1, (end_pose_left[:, 1] - robot_pose[:, 1]), label='optitrack') errors_left = np.zeros([len(robot_pose), 3]) errors_right = np.zeros([len(robot_pose), 3]) transform_position_left = np.zeros([len(arm_joint_left), 3]) transform_position_right = np.zeros([len(arm_joint_left), 3]) for i in range(len(arm_joint_left)): transform_pose_left, transform_pose_right = rf.robolab.transform_optitrackbasetoarmend(np.array([0, -0.7, 0.1]), arm_joint_left[i, 1:], arm_joint_right[i, 1:]) transform_position_left[i] = transform_pose_left[:3, 3] transform_position_right[i] = transform_pose_right[:3, 3] for i in range(len(robot_pose)): errors_left[i], errors_right[i] = compute_errors(arm_joint_left[i, 1:], arm_joint_right[i, 1:], robot_pose[i, 1:], end_pose_left[i, 1:], end_pose_right[i, 1:]) t_2 = np.arange(0, 20, 20/len(arm_joint_left)) plt.plot(t_2, transform_position_left[:, 0], label='transform') plt.legend(loc="upper right", prop={'size': 12}) plt.show() t = np.arange(0, len(robot_pose)/120, 1/120) errors = np.zeros(len(errors_left)) for i in range(len(errors_left)): errors[i] = np.sqrt(errors_left[i, 0] ** 2 + errors_left[i, 1] ** 2 + errors_left[i, 2] ** 2) plt.plot(t, errors) # plt.savefig('/home/ubuntu/Data/optitrack test/csv_20230426/errors_1.png', dpi=300) plt.show() plt.figure(figsize=(20, 10)) plt.subplot(3, 2, 1) # plt.xlabel('s', fontweight="bold", fontdict={'family': 'Times New Roman'}, fontsize=18) plt.ylabel('(x/m)', fontweight="bold", fontdict={'family': 'Times New Roman'}, fontsize=18) plt.xticks(fontproperties='Times New Roman', size=12) plt.yticks(fontproperties='Times New Roman', size=12) plt.plot(t, errors_left[:, 0], color="royalblue", label='left_arm', linewidth=1.5) plt.legend(loc="upper right", prop={'size': 12}) plt.subplot(3, 2, 2) # plt.xlabel('s', fontweight="bold", fontdict={'family': 'Times New Roman'}, fontsize=18) # plt.ylabel('m', fontweight="bold", fontdict={'family': 'Times New Roman'}, fontsize=18) plt.xticks(fontproperties='Times New Roman', size=12) plt.yticks(fontproperties='Times New Roman', size=12) plt.plot(t, errors_right[:, 0], color="royalblue", label='right_arm', linewidth=1.5) plt.legend(loc="upper right", prop={'size': 12}) plt.subplot(3, 2, 3) # plt.xlabel('s', fontweight="bold", fontdict={'family': 'Times New Roman'}, fontsize=18) plt.ylabel('y/m', fontweight="bold", fontdict={'family': 'Times New Roman'}, fontsize=18) plt.xticks(fontproperties='Times New Roman', size=12) plt.yticks(fontproperties='Times New Roman', size=12) plt.plot(t, errors_left[:, 1], color="royalblue", label='left_arm', linewidth=1.5) # plt.legend(loc="upper right", prop={'size': 12}) plt.subplot(3, 2, 4) # plt.xlabel('s', fontweight="bold", fontdict={'family': 'Times New Roman'}, fontsize=18) # plt.ylabel('(m)', fontweight="bold", fontdict={'family': 'Times New Roman'}, fontsize=18) plt.xticks(fontproperties='Times New Roman', size=12) plt.yticks(fontproperties='Times New Roman', size=12) plt.plot(t, errors_right[:, 1], color="royalblue", label='right_arm', linewidth=1.5) # plt.legend(loc="upper right", prop={'size': 12}) plt.subplot(3, 2, 5) plt.xlabel('t/s', fontweight="bold", fontdict={'family': 'Times New Roman'}, fontsize=18) plt.ylabel('z/m', fontweight="bold", fontdict={'family': 'Times New Roman'}, fontsize=18) plt.xticks(fontproperties='Times New Roman', size=12) plt.yticks(fontproperties='Times New Roman', size=12) plt.plot(t, errors_left[:, 2], color="royalblue", label='left_arm', linewidth=1.5) # plt.legend(loc="upper right", prop={'size': 12}) plt.subplot(3, 2, 6) plt.xlabel('t/s', fontweight="bold", fontdict={'family': 'Times New Roman'}, fontsize=18) # plt.ylabel('m/s', fontweight="bold", fontdict={'family': 'Times New Roman'}, fontsize=18) plt.xticks(fontproperties='Times New Roman', size=12) plt.yticks(fontproperties='Times New Roman', size=12) plt.plot(t, errors_right[:, 2], color="royalblue", label='right_arm', linewidth=1.5) # plt.legend(loc="upper right", prop={'size': 12}) # plt.savefig('/home/ubuntu/Data/optitrack test/csv_20230426/errors_2.png', dpi=300) plt.show() .. rst-class:: sphx-glr-timing **Total running time of the script:** (0 minutes 0.000 seconds) .. _sphx_glr_download_examples_robolab_example_curi_transformation_verification_with_optitrack.py: .. only:: html .. container:: sphx-glr-footer sphx-glr-footer-example .. container:: sphx-glr-download sphx-glr-download-python :download:`Download Python source code: example_curi_transformation_verification_with_optitrack.py ` .. container:: sphx-glr-download sphx-glr-download-jupyter :download:`Download Jupyter notebook: example_curi_transformation_verification_with_optitrack.ipynb ` .. only:: html .. rst-class:: sphx-glr-signature `Gallery generated by Sphinx-Gallery `_