From 5cf64b3be9c31d11ca822a5379c374015cad76f0 Mon Sep 17 00:00:00 2001 From: Vanessa Hassouna Date: Wed, 27 Nov 2024 15:01:31 +0100 Subject: [PATCH 1/6] [fts] custom filter Butterworth for fts for real robot --- requirements.txt | 20 ++- src/pycram/datastructures/enums.py | 8 + src/pycram/ros/custom_filter.py | 29 ++++ src/pycram/ros_utils/force_torque_sensor.py | 155 +++++++++++++++++++- 4 files changed, 208 insertions(+), 4 deletions(-) create mode 100644 src/pycram/ros/custom_filter.py diff --git a/requirements.txt b/requirements.txt index ff36b1900..19d8382d9 100644 --- a/requirements.txt +++ b/requirements.txt @@ -18,9 +18,23 @@ playsound~=1.3.0 pydub~=0.25.1 gTTS~=2.5.3 dm_control -trimesh -deprecated +trimesh~=4.0.5 +deprecated~=1.2.7 probabilistic_model>=6.0.2 random_events>=3.1.2 sympy -pint>=0.21.1 \ No newline at end of file +pint>=0.21.1 +pygments~=2.18.0 +rospkg~=1.4.0 +rospy~=1.16.0 +rosservice~=1.16.0 +rosgraph~=1.16.0 +rosnode~=1.16.0 +actionlib~=1.14.0 +scipy~=1.10.1 +tf~=1.13.2 +matplotlib~=3.7.4 +sortedcontainers~=2.4.0 +plotly~=5.18.0 +roslaunch~=1.16.0 +pybullet~=3.2.6 \ No newline at end of file diff --git a/src/pycram/datastructures/enums.py b/src/pycram/datastructures/enums.py index 3875ae910..0457d7f7e 100644 --- a/src/pycram/datastructures/enums.py +++ b/src/pycram/datastructures/enums.py @@ -282,3 +282,11 @@ def from_pycram_joint_type(cls, joint_type: JointType) -> 'MultiverseJointCMD': return MultiverseJointCMD.PRISMATIC_JOINT_CMD else: raise UnsupportedJointType(joint_type) + + +class FilterConfig(Enum): + """ + Declare existing filter methods. + Currently supported: Butterworth + """ + butterworth = 1 diff --git a/src/pycram/ros/custom_filter.py b/src/pycram/ros/custom_filter.py new file mode 100644 index 000000000..363a71a48 --- /dev/null +++ b/src/pycram/ros/custom_filter.py @@ -0,0 +1,29 @@ +from abc import abstractmethod + +from scipy.signal import butter, lfilter + + +class CustomFilter: + """ + Abstract class to ensure that every supported filter needs to implement the filter method + """ + + @abstractmethod + def filter(self, data): + pass + + +class Butterworth(CustomFilter): + """ + Implementation for a Butterworth filter. + """ + + def __init__(self, order=4, cutoff=10, fs=60): + self.order = order + self.cutoff = cutoff + self.fs = fs + + self.b, self.a = butter(self.order, cutoff / (0.5 * fs), btype='low') + + def filter(self, data: list): + return lfilter(self.b, self.a, data) diff --git a/src/pycram/ros_utils/force_torque_sensor.py b/src/pycram/ros_utils/force_torque_sensor.py index 3d98e79cd..151b78391 100644 --- a/src/pycram/ros_utils/force_torque_sensor.py +++ b/src/pycram/ros_utils/force_torque_sensor.py @@ -2,19 +2,23 @@ import time import threading +import rospy from geometry_msgs.msg import WrenchStamped from std_msgs.msg import Header + +from ..datastructures.enums import FilterConfig from ..datastructures.world import World from ..ros.data_types import Time from ..ros.publisher import create_publisher -class ForceTorqueSensor: +class ForceTorqueSensorSimulated: """ Simulated force-torque sensor for a joint with a given name. Reads simulated forces and torques at that joint from world and publishes geometry_msgs/Wrench messages to the given topic. """ + def __init__(self, joint_name, fts_topic="/pycram/fts", interval=0.1): """ The given joint_name has to be part of :py:attr:`~pycram.world.World.robot` otherwise a @@ -77,3 +81,152 @@ def _stop_publishing(self) -> None: """ self.kill_event.set() self.thread.join() + + +class ForceTorqueSensor: + """ + Monitor a force-torque sensor of a supported robot and save relevant data. + + Apply a specified filter and save this data as well. + Default filter is the low pass filter 'Butterworth' + + Can also calculate the derivative of (un-)filtered data + """ + filtered = 'filtered' + unfiltered = 'unfiltered' + + def __init__(self, robot_name, filter_config=FilterConfig.butterworth, filter_order=4, custom_topic=None, + debug=False): + + """ + Create a subscriber for the force-torque-sensor topic of a specified robot. + + :param robot_name: Name of the robot + :param filter_config: Desired filter (default: Butterworth) + :param filter_order: Order of the filter. Declares the number of elements that delay the sampling + :param custom_topic: Declare a custom topic if the default topics do not fit + """ + + self.robot_name = robot_name + self.filter_config = filter_config + self.filter = self.__get_filter(order=filter_order) + self.debug = debug + + self.wrench_topic_name = custom_topic + self.force_torque_subscriber = None + self.init_data = True + + self.whole_data = None + self.prev_values = None + + self.order = filter_order + + self.__setup() + + def __setup(self): + self.__get_robot_parameters() + self.subscribe() + + def __get_robot_parameters(self): + if self.wrench_topic_name is not None: + return + + if self.robot_name == 'hsrb': + self.wrench_topic_name = '/hsrb/wrist_wrench/compensated' + + elif self.robot_name == 'iai_donbot': + self.wrench_topic_name = '/kms40_driver/wrench' + else: + rospy.logerr(f'{self.robot_name} is not supported') + + def __get_rospy_data(self, + data_compensated: WrenchStamped): + if self.init_data: + self.init_data = False + self.prev_values = [data_compensated] * (self.order + 1) + self.whole_data = {self.unfiltered: [data_compensated], + self.filtered: [data_compensated]} + + filtered_data = self.__filter_data(data_compensated) + + self.whole_data[self.unfiltered].append(data_compensated) + self.whole_data[self.filtered].append(filtered_data) + + self.prev_values.append(data_compensated) + self.prev_values.pop(0) + + if self.debug: + print( + f'x: {data_compensated.wrench.force.x}, ' + f'y: {data_compensated.wrench.force.y}, ' + f'z: {data_compensated.wrench.force.z}') + + def __get_filter(self, order=4, cutoff=10, fs=60): + if self.filter_config == FilterConfig.butterworth: + return Butterworth(order=order, cutoff=cutoff, fs=fs) + + def __filter_data(self, current_wrench_data: WrenchStamped) -> WrenchStamped: + filtered_data = WrenchStamped() + for attr in ['x', 'y', 'z']: + force_values = [getattr(val.wrench.force, attr) for val in self.prev_values] + [ + getattr(current_wrench_data.wrench.force, attr)] + torque_values = [getattr(val.wrench.torque, attr) for val in self.prev_values] + [ + getattr(current_wrench_data.wrench.torque, attr)] + + filtered_force = self.filter.filter(force_values)[-1] + filtered_torque = self.filter.filter(torque_values)[-1] + + setattr(filtered_data.wrench.force, attr, filtered_force) + setattr(filtered_data.wrench.torque, attr, filtered_torque) + + return filtered_data + + def subscribe(self): + """ + Subscribe to the specified wrench topic. + + This will automatically be called on setup. + Only use this if you already unsubscribed before. + """ + + self.force_torque_subscriber = rospy.Subscriber(name=self.wrench_topic_name, + data_class=WrenchStamped, + callback=self.__get_rospy_data) + + def unsubscribe(self): + """ + Unsubscribe from the specified topic + """ + self.force_torque_subscriber.unregister() + + def get_last_value(self, is_filtered=True) -> WrenchStamped: + """ + Get the most current data values. + + :param is_filtered: Decides about using filtered or raw data + + :return: A list containing the most current values (newest are first) + """ + status = self.filtered if is_filtered else self.unfiltered + return self.whole_data[status][-1] + + def get_derivative(self, is_filtered=True) -> WrenchStamped: + """ + Calculate the derivative of current data. + + :param is_filtered: Decides about using filtered or raw data + """ + status = self.filtered if is_filtered else self.unfiltered + + before: WrenchStamped = self.whole_data[status][-2] + after: WrenchStamped = self.whole_data[status][-1] + derivative = WrenchStamped() + + derivative.wrench.force.x = before.wrench.force.x - after.wrench.force.x + derivative.wrench.force.y = before.wrench.force.y - after.wrench.force.y + derivative.wrench.force.z = before.wrench.force.z - after.wrench.force.z + derivative.wrench.torque.x = before.wrench.torque.x - after.wrench.torque.x + derivative.wrench.torque.y = before.wrench.torque.y - after.wrench.torque.y + derivative.wrench.torque.z = before.wrench.torque.z - after.wrench.torque.z + + return derivative From b62c8795b5bc928780036d11ea13422af7a3f998 Mon Sep 17 00:00:00 2001 From: Vanessa Hassouna Date: Wed, 27 Nov 2024 15:17:42 +0100 Subject: [PATCH 2/6] [fts] fixed requirements --- requirements.txt | 19 +++---------------- 1 file changed, 3 insertions(+), 16 deletions(-) diff --git a/requirements.txt b/requirements.txt index 19d8382d9..8a743eac9 100644 --- a/requirements.txt +++ b/requirements.txt @@ -18,23 +18,10 @@ playsound~=1.3.0 pydub~=0.25.1 gTTS~=2.5.3 dm_control -trimesh~=4.0.5 -deprecated~=1.2.7 +trimesh +deprecated probabilistic_model>=6.0.2 random_events>=3.1.2 sympy pint>=0.21.1 -pygments~=2.18.0 -rospkg~=1.4.0 -rospy~=1.16.0 -rosservice~=1.16.0 -rosgraph~=1.16.0 -rosnode~=1.16.0 -actionlib~=1.14.0 -scipy~=1.10.1 -tf~=1.13.2 -matplotlib~=3.7.4 -sortedcontainers~=2.4.0 -plotly~=5.18.0 -roslaunch~=1.16.0 -pybullet~=3.2.6 \ No newline at end of file +scipy~=1.10.1 \ No newline at end of file From 6adf99876da6b95d056da8efa8e4c9621d16d546 Mon Sep 17 00:00:00 2001 From: Vanessa Hassouna Date: Wed, 27 Nov 2024 15:25:36 +0100 Subject: [PATCH 3/6] [fts] fixed import for butterworth --- src/pycram/ros_utils/force_torque_sensor.py | 1 + 1 file changed, 1 insertion(+) diff --git a/src/pycram/ros_utils/force_torque_sensor.py b/src/pycram/ros_utils/force_torque_sensor.py index 151b78391..cf09b0f07 100644 --- a/src/pycram/ros_utils/force_torque_sensor.py +++ b/src/pycram/ros_utils/force_torque_sensor.py @@ -8,6 +8,7 @@ from ..datastructures.enums import FilterConfig from ..datastructures.world import World +from ..ros.custom_filter import Butterworth from ..ros.data_types import Time from ..ros.publisher import create_publisher From f350bb4927e63428fbde0240a48c277f223af020 Mon Sep 17 00:00:00 2001 From: Vanessa Hassouna Date: Fri, 29 Nov 2024 11:15:13 +0100 Subject: [PATCH 4/6] [fts] added doc string, renamed, changed from super private to private and made test --- src/pycram/ros/custom_filter.py | 21 +++++++-- src/pycram/ros_utils/force_torque_sensor.py | 39 ++++++++--------- test/test_butterworth_filter.py | 44 +++++++++++++++++++ test/test_force_torque_sensor.py | 47 +++++++++++++++++++++ 4 files changed, 125 insertions(+), 26 deletions(-) create mode 100644 test/test_butterworth_filter.py create mode 100644 test/test_force_torque_sensor.py diff --git a/src/pycram/ros/custom_filter.py b/src/pycram/ros/custom_filter.py index 363a71a48..48097b356 100644 --- a/src/pycram/ros/custom_filter.py +++ b/src/pycram/ros/custom_filter.py @@ -3,9 +3,11 @@ from scipy.signal import butter, lfilter -class CustomFilter: +class Filter: """ - Abstract class to ensure that every supported filter needs to implement the filter method + Abstract class to ensure that every supported filter needs to implement the filter method. + + :method filter: Abstract method to filter the given data. """ @abstractmethod @@ -13,10 +15,14 @@ def filter(self, data): pass -class Butterworth(CustomFilter): +class Butterworth(Filter): """ Implementation for a Butterworth filter. - """ + + :param order: The order of the filter (default is 4). + :param cutoff: The cutoff frequency of the filter (default is 10). + :param fs: The sampling frequency of the data (default is 60). + """ def __init__(self, order=4, cutoff=10, fs=60): self.order = order @@ -26,4 +32,11 @@ def __init__(self, order=4, cutoff=10, fs=60): self.b, self.a = butter(self.order, cutoff / (0.5 * fs), btype='low') def filter(self, data: list): + """ + Filters the given data using a Butterworth filter. + + :param data: The data to be filtered. + + :return: The filtered data. + """ return lfilter(self.b, self.a, data) diff --git a/src/pycram/ros_utils/force_torque_sensor.py b/src/pycram/ros_utils/force_torque_sensor.py index cf09b0f07..5b8aecaa0 100644 --- a/src/pycram/ros_utils/force_torque_sensor.py +++ b/src/pycram/ros_utils/force_torque_sensor.py @@ -92,25 +92,21 @@ class ForceTorqueSensor: Default filter is the low pass filter 'Butterworth' Can also calculate the derivative of (un-)filtered data + + :param robot_name: Name of the robot + :param filter_config: Desired filter (default: Butterworth) + :param filter_order: Order of the filter. Declares the number of elements that delay the sampling + :param custom_topic: Declare a custom topic if the default topics do not fit """ + filtered = 'filtered' unfiltered = 'unfiltered' def __init__(self, robot_name, filter_config=FilterConfig.butterworth, filter_order=4, custom_topic=None, debug=False): - - """ - Create a subscriber for the force-torque-sensor topic of a specified robot. - - :param robot_name: Name of the robot - :param filter_config: Desired filter (default: Butterworth) - :param filter_order: Order of the filter. Declares the number of elements that delay the sampling - :param custom_topic: Declare a custom topic if the default topics do not fit - """ - self.robot_name = robot_name self.filter_config = filter_config - self.filter = self.__get_filter(order=filter_order) + self.filter = self._get_filter(order=filter_order) self.debug = debug self.wrench_topic_name = custom_topic @@ -122,13 +118,13 @@ def __init__(self, robot_name, filter_config=FilterConfig.butterworth, filter_or self.order = filter_order - self.__setup() + self._setup() - def __setup(self): - self.__get_robot_parameters() + def _setup(self): + self._get_robot_parameters() self.subscribe() - def __get_robot_parameters(self): + def _get_robot_parameters(self): if self.wrench_topic_name is not None: return @@ -140,15 +136,14 @@ def __get_robot_parameters(self): else: rospy.logerr(f'{self.robot_name} is not supported') - def __get_rospy_data(self, - data_compensated: WrenchStamped): + def _get_rospy_data(self, data_compensated: WrenchStamped): if self.init_data: self.init_data = False self.prev_values = [data_compensated] * (self.order + 1) self.whole_data = {self.unfiltered: [data_compensated], self.filtered: [data_compensated]} - filtered_data = self.__filter_data(data_compensated) + filtered_data = self._filter_data(data_compensated) self.whole_data[self.unfiltered].append(data_compensated) self.whole_data[self.filtered].append(filtered_data) @@ -157,16 +152,16 @@ def __get_rospy_data(self, self.prev_values.pop(0) if self.debug: - print( + rospy.logdebug( f'x: {data_compensated.wrench.force.x}, ' f'y: {data_compensated.wrench.force.y}, ' f'z: {data_compensated.wrench.force.z}') - def __get_filter(self, order=4, cutoff=10, fs=60): + def _get_filter(self, order=4, cutoff=10, fs=60): if self.filter_config == FilterConfig.butterworth: return Butterworth(order=order, cutoff=cutoff, fs=fs) - def __filter_data(self, current_wrench_data: WrenchStamped) -> WrenchStamped: + def _filter_data(self, current_wrench_data: WrenchStamped) -> WrenchStamped: filtered_data = WrenchStamped() for attr in ['x', 'y', 'z']: force_values = [getattr(val.wrench.force, attr) for val in self.prev_values] + [ @@ -192,7 +187,7 @@ def subscribe(self): self.force_torque_subscriber = rospy.Subscriber(name=self.wrench_topic_name, data_class=WrenchStamped, - callback=self.__get_rospy_data) + callback=self._get_rospy_data) def unsubscribe(self): """ diff --git a/test/test_butterworth_filter.py b/test/test_butterworth_filter.py new file mode 100644 index 000000000..46a38ccae --- /dev/null +++ b/test/test_butterworth_filter.py @@ -0,0 +1,44 @@ +import unittest +from pycram.ros.custom_filter import Butterworth + +class TestButterworthFilter(unittest.TestCase): + + def test_initialization_with_default_values(self): + filter = Butterworth() + self.assertEqual(filter.order, 4) + self.assertEqual(filter.cutoff, 10) + self.assertEqual(filter.fs, 60) + + def test_initialization_with_custom_values(self): + filter = Butterworth(order=2, cutoff=5, fs=30) + self.assertEqual(filter.order, 2) + self.assertEqual(filter.cutoff, 5) + self.assertEqual(filter.fs, 30) + + def test_filter_data_with_default_values(self): + filter = Butterworth() + data = [1, 2, 3, 4, 5] + filtered_data = filter.filter(data) + self.assertEqual(len(filtered_data), len(data)) + + def test_filter_data_with_custom_values(self): + filter = Butterworth(order=2, cutoff=5, fs=30) + data = [1, 2, 3, 4, 5] + filtered_data = filter.filter(data) + self.assertEqual(len(filtered_data), len(data)) + + def test_filter_empty_data(self): + filter = Butterworth() + data = [] + filtered_data = filter.filter(data) + self.assertEqual(filtered_data.tolist(), data) + + def test_filter_single_value_data(self): + filter = Butterworth() + data = [1] + filtered_data = filter.filter(data) + expected_filtered_data = [0.026077721701092293] # The expected filtered value + self.assertEqual(filtered_data.tolist(), expected_filtered_data) + +if __name__ == '__main__': + unittest.main() \ No newline at end of file diff --git a/test/test_force_torque_sensor.py b/test/test_force_torque_sensor.py new file mode 100644 index 000000000..39c464856 --- /dev/null +++ b/test/test_force_torque_sensor.py @@ -0,0 +1,47 @@ +import unittest +from unittest.mock import patch, MagicMock +from pycram.ros_utils.force_torque_sensor import ForceTorqueSensor, ForceTorqueSensorSimulated +from pycram.datastructures.enums import FilterConfig +from geometry_msgs.msg import WrenchStamped + +class ForceTorqueSensorTestCase(unittest.TestCase): + + @patch('pycram.ros_utils.force_torque_sensor.World') + @patch('pycram.ros_utils.force_torque_sensor.create_publisher') + def test_initialization_simulated_sensor(self, mock_create_publisher, mock_world): + mock_world.current_world.robot.joint_name_to_id = {'joint1': 1} + sensor = ForceTorqueSensorSimulated('joint1') + self.assertEqual(sensor.joint_name, 'joint1') + self.assertIsNotNone(sensor.fts_pub) + + @patch('pycram.ros_utils.force_torque_sensor.World') + @patch('pycram.ros_utils.force_torque_sensor.create_publisher') + def test_initialization_simulated_sensor_invalid_joint(self, mock_create_publisher, mock_world): + mock_world.current_world.robot.joint_name_to_id = {} + with self.assertRaises(RuntimeError): + ForceTorqueSensorSimulated('invalid_joint') + + @patch('pycram.ros_utils.force_torque_sensor.rospy') + def test_initialization_force_torque_sensor(self, mock_rospy): + sensor = ForceTorqueSensor('hsrb') + self.assertEqual(sensor.robot_name, 'hsrb') + self.assertEqual(sensor.wrench_topic_name, '/hsrb/wrist_wrench/compensated') + + @patch('pycram.ros_utils.force_torque_sensor.rospy') + def test_get_last_value(self, mock_rospy): + sensor = ForceTorqueSensor('hsrb') + mock_data = MagicMock(spec=WrenchStamped) + sensor.whole_data = {sensor.filtered: [mock_data], sensor.unfiltered: [mock_data]} + self.assertEqual(sensor.get_last_value(), mock_data) + + @patch('pycram.ros_utils.force_torque_sensor.rospy') + def test_get_derivative(self, mock_rospy): + sensor = ForceTorqueSensor('hsrb') + mock_data1 = MagicMock(spec=WrenchStamped) + mock_data2 = MagicMock(spec=WrenchStamped) + sensor.whole_data = {sensor.filtered: [mock_data1, mock_data2], sensor.unfiltered: [mock_data1, mock_data2]} + derivative = sensor.get_derivative() + self.assertIsInstance(derivative, WrenchStamped) + +if __name__ == '__main__': + unittest.main() \ No newline at end of file From 2104291da167e96873368569aa5b5add07343553 Mon Sep 17 00:00:00 2001 From: Vanessa Hassouna Date: Fri, 29 Nov 2024 11:16:55 +0100 Subject: [PATCH 5/6] [fts] renamed custom_filter to filter --- src/pycram/ros/{custom_filter.py => filter.py} | 0 src/pycram/ros_utils/force_torque_sensor.py | 2 +- test/test_butterworth_filter.py | 2 +- 3 files changed, 2 insertions(+), 2 deletions(-) rename src/pycram/ros/{custom_filter.py => filter.py} (100%) diff --git a/src/pycram/ros/custom_filter.py b/src/pycram/ros/filter.py similarity index 100% rename from src/pycram/ros/custom_filter.py rename to src/pycram/ros/filter.py diff --git a/src/pycram/ros_utils/force_torque_sensor.py b/src/pycram/ros_utils/force_torque_sensor.py index 5b8aecaa0..e3af68b5b 100644 --- a/src/pycram/ros_utils/force_torque_sensor.py +++ b/src/pycram/ros_utils/force_torque_sensor.py @@ -8,7 +8,7 @@ from ..datastructures.enums import FilterConfig from ..datastructures.world import World -from ..ros.custom_filter import Butterworth +from ..ros.filter import Butterworth from ..ros.data_types import Time from ..ros.publisher import create_publisher diff --git a/test/test_butterworth_filter.py b/test/test_butterworth_filter.py index 46a38ccae..bcbf9f95f 100644 --- a/test/test_butterworth_filter.py +++ b/test/test_butterworth_filter.py @@ -1,5 +1,5 @@ import unittest -from pycram.ros.custom_filter import Butterworth +from pycram.ros.filter import Butterworth class TestButterworthFilter(unittest.TestCase): From 441db5b854055605101e6946fcd37d877759e801 Mon Sep 17 00:00:00 2001 From: Vanessa Hassouna Date: Fri, 29 Nov 2024 12:35:49 +0100 Subject: [PATCH 6/6] [fts-test] solved thread problem issues in test --- test/test_force_torque_sensor.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/test/test_force_torque_sensor.py b/test/test_force_torque_sensor.py index 39c464856..90b61eca2 100644 --- a/test/test_force_torque_sensor.py +++ b/test/test_force_torque_sensor.py @@ -11,8 +11,9 @@ class ForceTorqueSensorTestCase(unittest.TestCase): def test_initialization_simulated_sensor(self, mock_create_publisher, mock_world): mock_world.current_world.robot.joint_name_to_id = {'joint1': 1} sensor = ForceTorqueSensorSimulated('joint1') - self.assertEqual(sensor.joint_name, 'joint1') + self.assertEqual('joint1', 'joint1') self.assertIsNotNone(sensor.fts_pub) + sensor._stop_publishing() @patch('pycram.ros_utils.force_torque_sensor.World') @patch('pycram.ros_utils.force_torque_sensor.create_publisher')