F #he 948 improve calibration process #56

carebare47 · 2023-06-13T16:40:58Z

Proposed changes

Now runs faster / uses less compute

Fix progress bar logic

/tf callback was missing most messages due to callback execution time being too long. Now saves all messages and extracts a configurable subsample (can specify an approximate number of evenly distributed datapoints per second to keep)

Save result of previous sphere_fit as initial_guess for next sphere_fit (reduces computation time)

Add small sleep after tf subscriber, sometimes the subscriber warm-up time was longer than the first progress checkpoint, giving us empty lists which then got passed to np.std and threw numpy RuntimeWarnings

Added functionality to read from a rosbag for testing purposes

Publishing a MarkerArray instead of lots of Markers (saves cpu time)

Checklist

Before posting a PR ensure that from each of the below categories AT LEAST ONE BOX HAS BEEN CHECKED. If more than one category is applicable then more can be checked. Also ensure that the proposed changes have been filled out with relevant information for reviewers.

Tests

No tests required to be added. (For small changes that will be tested by CI/CD infrastructure).
Added/Modified automated and PhantomHand CI tests (if a new class is added (Python or C++), the interface of that class must be unit tested).
Manually tested in simulation (if simulation specific or no hardware required to test the functionality).
Manually tested on hardware (if hardware specific or related).

Documentation

No documentation required to be added.
Added documentation (For any new feature, explain what it does and how to use it. Write the documentation in a relevant space, e.g. Github, Confluence, etc).
Updated documentation (For changes to pre-existing features mentioned in the documentation).

…e we actually have data points when we first call fit_sphere(...) which prevents three numpy RuntimeWarnings (we were sometimes calling np.std on an empty list)

…. Speeds up computation but potentially reduces accuracy. Needs testing

…r the next sphere_fit. Speeds up computation time

src/polhemus_ros_driver/knuckle_position_calibration_action_server.py

…antly faster), and fix parameter from launch file

launch/start.launch

jeferal · 2023-06-14T11:07:06Z

I reviewed and would accept, but first perhaps it would be useful to check how the calibration quality changes when changing the tf_data_divisor as we have discussed

carebare47 · 2023-06-14T11:07:49Z

I reviewed and would accept, but first perhaps it would be useful to check how the calibration quality changes when changing the tf_data_divisor as we have discussed

Agreed, checking that now

…s execution time being too long (the number of msgs recieved was system-dependant and made it impossible to compare accuracy of calibration fits between runs). Modified code to save all tf messsages and process them in get_knuckle_positions. Added code to load a bag file for testing/tuning/comparing accuracy of calibration fits. Now that _load_tf_callback is recieving all the messages, the average number of msgs per second has gone up from ~15 to ~100, drastically increasing the computation of sphere fit. But, now we can add code to downsample to a KNOWN number of 'datapoints per second', giving the same performance across systems and lettings us actually figure out how many datapoints per second we need for a reasonable calibration fit

…pling to approximately a 'known' number of samples-per-second. Switched to MarkerArrays for faster marker publishing. Also general tidying

carebare47 · 2023-06-15T17:28:44Z

I reviewed and would accept, but first perhaps it would be useful to check how the calibration quality changes when changing the tf_data_divisor as we have discussed

It turns out that the /tf subscriber was missing the vast majority of messages because the callback was taking too long to execute, so it wasn't possible to compare/find a good "data divisor" number because we couldn't be sure that we were receiving the same messages every time (even from a bag file).

Added code to plug in a bag file of glove movements for testing purposes and moved the processing out of the callback so that the code catches every /tf message (either live or from a bag). Now, instead of dividing an unknown amount of incoming data, we capture all the data and subsample to give us approximately desired_number_of_datapoints_per_sec. Should be consistent across different machines now.

The bag file I've been testing with has ~120 messages per second.

Setting desired_number_of_datapoints_per_sec to above 120:

Calibration score of 96.2%
Sum of time taken to process tf data, publish markers and calculate the sphere fits: ~19 seconds

Setting desired_number_of_datapoints_per_sec to 20:

Calibration score of 96.3%
Sum of time taken to process tf data, publish markers and calculate the sphere fits: ~2.5 seconds

Setting desired_number_of_datapoints_per_sec to 10:

Calibration score of 96.2%
Sum of time taken to process tf data, publish markers and calculate the sphere fits: ~1.5 seconds

(Tests performed on a Ryzen 9 3900X)

So, we should tune this value when we test on an events laptop in spectrum house soon

carebare47 · 2023-06-15T18:16:50Z

while I remember, must update the rviz configs in sr_glove_calibration_gui before merging this. Will do this during testing

… with a bag

bdenoun · 2023-06-26T10:34:27Z

src/polhemus_ros_driver/knuckle_position_calibration_action_server.py

+        self._marker_array.markers.append(data_point_marker)
+
+    def get_number_of_markers(self):
+        return self._marker_array.markers.__len__()


Although valid, why not

Suggested change

return self._marker_array.markers.__len__()

return len(self._marker_array.markers)

? Is the len method of marker_array does anything different than the len() operator on iterable?

bdenoun · 2023-06-26T10:37:50Z

src/polhemus_ros_driver/knuckle_position_calibration_action_server.py

-    def __init__(self, side: str = "right"):
+    def __init__(self, side: str = "right",
+                 desired_datapoints_per_sec: int = 20,
+                 testing_bag_file_path: str = ''):


Usually, when an argument is optional, we set it to None, because an empty string remains a string, which is the valid "type" of data.
Is there a reason you used the empty string?
If you want to change this to None, then this becomes

Suggested change

testing_bag_file_path: str = ''):

testing_bag_file_path: Optional[str] = None) -> None:

while adding from typing import Optional

bdenoun · 2023-06-26T10:39:10Z

src/polhemus_ros_driver/knuckle_position_calibration_action_server.py

-    def __init__(self, side: str = "right"):
+    def __init__(self, side: str = "right",
+                 desired_datapoints_per_sec: int = 20,
+                 testing_bag_file_path: str = ''):


Suggested change

testing_bag_file_path: str = ''):

testing_bag_file_path: str = '') -> None:

to add the output type hint

bdenoun · 2023-06-26T10:39:40Z

src/polhemus_ros_driver/knuckle_position_calibration_action_server.py

@@ -73,10 +73,10 @@ class DataMarker(Marker):
    """ Class to store data about a marker to be published to rviz. """
    _id = 0

-    def __init__(self, frame_id: str, point: Point, color: ColorRGBA, size=0.002):
+    def __init__(self, frame_id: str, time_stamp: rospy.Time, point: Point, color: ColorRGBA, size=0.002):


Adding output type hint

Suggested change

def __init__(self, frame_id: str, time_stamp: rospy.Time, point: Point, color: ColorRGBA, size=0.002):

def __init__(self, frame_id: str, time_stamp: rospy.Time, point: Point, color: ColorRGBA, size=0.002) -> None:

bdenoun · 2023-06-26T10:39:58Z

src/polhemus_ros_driver/knuckle_position_calibration_action_server.py

+        self._marker_array = MarkerArray()
+        self._pub = rospy.Publisher(f"/data_point_marker_{self.hand_prefix}", MarkerArray, queue_size=1000)
+
+    def add_marker(self, frame_id: str, time_stamp: rospy.Time, point: Point, color: ColorRGBA):


Suggested change

def add_marker(self, frame_id: str, time_stamp: rospy.Time, point: Point, color: ColorRGBA):

def add_marker(self, frame_id: str, time_stamp: rospy.Time, point: Point, color: ColorRGBA) -> None:

bdenoun · 2023-06-26T10:48:23Z

src/polhemus_ros_driver/knuckle_position_calibration_action_server.py

            self._action_server.publish_feedback(_feedback)

-        sub.unregister()
-        self._get_knuckle_positions(hand)
+        if self._testing_bag_file_path == '':


Suggested change

if self._testing_bag_file_path == '':

if self._testing_bag_file_path is None':

bdenoun · 2023-06-26T10:50:08Z

src/polhemus_ros_driver/knuckle_position_calibration_action_server.py


-    def _get_knuckle_positions(self, hand: Hand, plot=False):
+    def _get_knuckle_positions(self, hand: Hand, time_elapsed: float, plot=False):


Missing output type hint. I know it's not your fault, but this naming convention of _get_x that does not return anything (i.e. not a getter) confuses me a little... I think it would be better called _compute_knuckle_positions. What do you think?

bdenoun · 2023-06-26T10:50:28Z

src/polhemus_ros_driver/knuckle_position_calibration_action_server.py

        """
            Updates the current solution for all fingers on the selected hand.
            @param hand: Selected hand
+            @param time_elapsed: Time elapsed since the start of this calibration


Can you describe what's the unit? seconds? milliseconds?

bdenoun · 2023-06-26T10:52:01Z

src/polhemus_ros_driver/knuckle_position_calibration_action_server.py

@@ -509,11 +619,16 @@ def get_calibration_quality(hand: Hand):
        """
        quality_list: List[float] = []
        for finger in fingers:
+            if len(hand.finger_data[finger]['residual']) == 0:
+                print('no data recieved')


Suggested change

print('no data recieved')

rospy.loginfo(f'No data received for finger {finger}')

bdenoun · 2023-06-26T10:52:57Z

src/polhemus_ros_driver/knuckle_position_calibration_action_server.py

@@ -509,11 +619,16 @@ def get_calibration_quality(hand: Hand):
        """
        quality_list: List[float] = []
        for finger in fingers:
+            if len(hand.finger_data[finger]['residual']) == 0:
+                print('no data recieved')
            quality_list.append(np.std(hand.finger_data[finger]['residual']))


We still add the standard deviation of a non-existing distribution?

…adow-robot/polhemus_ros_driver into F_#HE_948_improve_calibration_process

carebare47 added 7 commits June 13, 2023 16:37

fix progress bar logic

611aa77

tf subscriber takes time to start up. Added a small sleep to make sur…

7018f5b

…e we actually have data points when we first call fit_sphere(...) which prevents three numpy RuntimeWarnings (we were sometimes calling np.std on an empty list)

add 'divisor' to tf callback, lets us only save every 'x' tf messages…

aa51421

…. Speeds up computation but potentially reduces accuracy. Needs testing

save results from previous sphere_fit and use as the initial guess fo…

a6da349

…r the next sphere_fit. Speeds up computation time

lint

dec9b13

tidying

814c95c

removed unused import

e024ce5

bdenoun suggested changes Jun 14, 2023

View reviewed changes

carebare47 added 4 commits June 14, 2023 09:31

add data divisor as input parameter (also raise exception on not-an-int)

5ed88bf

add data divisor as input parameter (also raise exception on not-an-int)

17d5fd4

fix initial guess storage (was per hand, now per finger, now signific…

717d929

…antly faster), and fix parameter from launch file

actually apply the fix I thought I'd apply in the previous commit

3b99a89

bdenoun reviewed Jun 14, 2023

View reviewed changes

launch/start.launch Outdated Show resolved Hide resolved

carebare47 added 3 commits June 14, 2023 16:47

now saves all tf messages and processes outside the callback, downsam…

2a8e44e

…pling to approximately a 'known' number of samples-per-second. Switched to MarkerArrays for faster marker publishing. Also general tidying

make sure is valid for large values of

c8cb706

add comment to explain

fefb9b8

now set timer rate to desired_num_datapoints_per_sec for testing mode…

5955143

… with a bag

bdenoun suggested changes Jun 26, 2023

View reviewed changes

carebare47 and others added 7 commits June 26, 2023 15:05

update default num datapoints per sec to 60

6992110

fix difference between bags and live tf messages, address PR comments

daf4b72

Merge branch 'F_#HE_948_improve_calibration_process' of github.com:sh…

c39a539

…adow-robot/polhemus_ros_driver into F_#HE_948_improve_calibration_process

oops

368f107

lint

c575a51

Merge branch 'noetic-devel' into F_#HE_948_improve_calibration_process

4dad4ee

Merge branch 'noetic-devel' into F_#HE_948_improve_calibration_process

c82377f

carebare47 added 2 commits August 31, 2023 11:42

fixing cal score calc

385df1a

lint

60b5fcf

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

F #he 948 improve calibration process #56

F #he 948 improve calibration process #56

carebare47 commented Jun 13, 2023 •

edited

Loading

jeferal commented Jun 14, 2023 •

edited

Loading

carebare47 commented Jun 14, 2023

carebare47 commented Jun 15, 2023

carebare47 commented Jun 15, 2023

bdenoun Jun 26, 2023

bdenoun Jun 26, 2023

bdenoun Jun 26, 2023

bdenoun Jun 26, 2023

bdenoun Jun 26, 2023

bdenoun Jun 26, 2023

bdenoun Jun 26, 2023

bdenoun Jun 26, 2023

bdenoun Jun 26, 2023

bdenoun Jun 26, 2023

	return self._marker_array.markers.__len__()
	return len(self._marker_array.markers)

	testing_bag_file_path: str = ''):
	testing_bag_file_path: Optional[str] = None) -> None:

	testing_bag_file_path: str = ''):
	testing_bag_file_path: str = '') -> None:

	def __init__(self, frame_id: str, time_stamp: rospy.Time, point: Point, color: ColorRGBA, size=0.002):
	def __init__(self, frame_id: str, time_stamp: rospy.Time, point: Point, color: ColorRGBA, size=0.002) -> None:

	def add_marker(self, frame_id: str, time_stamp: rospy.Time, point: Point, color: ColorRGBA):
	def add_marker(self, frame_id: str, time_stamp: rospy.Time, point: Point, color: ColorRGBA) -> None:

	if self._testing_bag_file_path == '':
	if self._testing_bag_file_path is None':


		def _get_knuckle_positions(self, hand: Hand, plot=False):
		def _get_knuckle_positions(self, hand: Hand, time_elapsed: float, plot=False):

	print('no data recieved')
	rospy.loginfo(f'No data received for finger {finger}')

F #he 948 improve calibration process #56

Are you sure you want to change the base?

F #he 948 improve calibration process #56

Conversation

carebare47 commented Jun 13, 2023 • edited Loading

Proposed changes

Checklist

Tests

Documentation

jeferal commented Jun 14, 2023 • edited Loading

carebare47 commented Jun 14, 2023

carebare47 commented Jun 15, 2023

carebare47 commented Jun 15, 2023

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

carebare47 commented Jun 13, 2023 •

edited

Loading

jeferal commented Jun 14, 2023 •

edited

Loading