brought current version of force-torque stuff onto this branch for be…

…tter testing.

CMakeLists.txt

@@ -186,6 +186,7 @@ catkin_install_python(PROGRAMS
 	scripts/tools/print_joint_state.py
 	scripts/examples/python_interface_example.py
 	scripts/examples/old_python_interface_example.py
+	scripts/force_torque_raw_filter.py
 	scripts/upload_env.py
   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
 

scripts/force_torque_raw_filter.py

@@ -0,0 +1,103 @@
+import string
+
+import geometry_msgs
+import rospy
+from geometry_msgs.msg import WrenchStamped
+from scipy.signal import butter, lfilter
+from collections import deque
+
+
+class ForceTorqueRawFilter:
+
+    def __init__(self,
+                 topic: string = "/hsrb/wrist_wrench/raw"):
+        self.trans_F_x = deque(maxlen=100 * 2)  # make queue size dynamic
+        self.trans_F_y = deque(maxlen=100 * 2)
+        self.trans_F_z = deque(maxlen=100 * 2)
+        self.trans_T_x = deque(maxlen=100 * 2)
+        self.trans_T_y = deque(maxlen=100 * 2)
+        self.trans_T_z = deque(maxlen=100 * 2)
+        self.topic = topic
+        self.wrench = WrenchStamped()
+        self.subscriber = rospy.Subscriber(name=topic,
+                                           data_class=WrenchStamped, callback=self.cb)
+
+        self.pub = rospy.Publisher(name='rebuilt_signal', data_class=WrenchStamped, queue_size=10)
+        self.rate = rospy.Rate(10)
+
+    def cb(self, rawdata: WrenchStamped):
+        self.wrench = rawdata
+
+        # Sample parameters for the filter
+        fs = 100  # Sample rate, Hz, currently set to Hz of the topic
+        cutoff = 20  # Desired cutoff frequency of the filter, Hz
+        order = 5  # Order of the filter
+
+        # Filter the data
+        filtered_data = self.butter_lowpass_filter(self.convert_my_shit(1), cutoff, fs, order)
+        filtered_data2 = self.butter_lowpass_filter(self.convert_my_shit(2), cutoff, fs, order)
+        filtered_data3 = self.butter_lowpass_filter(self.convert_my_shit(3), cutoff, fs, order)
+        filtered_data4 = self.butter_lowpass_filter(self.convert_my_shit(4), cutoff, fs, order)
+        filtered_data5 = self.butter_lowpass_filter(self.convert_my_shit(5), cutoff, fs, order)
+        filtered_data6 = self.butter_lowpass_filter(self.convert_my_shit(6), cutoff, fs, order)
+        # Plotting Force
+        self.pub.publish(
+            self.rebuild_signal(filtered_data[-1], filtered_data2[-1], filtered_data3[-1], filtered_data4[-1],
+                                filtered_data5[-1],
+                                filtered_data6[-1]))
+        # rebuilding signal for publishing
+
+    def rebuild_signal(self, F_x, F_y, F_z, T_x, T_y, T_z):
+
+        rebuild_force = geometry_msgs.msg.Vector3(F_x, F_y, F_z)
+        rebuild_torque = geometry_msgs.msg.Vector3(T_x, T_y, T_z)
+
+        rebuild_wrench = geometry_msgs.msg.Wrench(rebuild_force, rebuild_torque)
+
+        rebuild_wrenchStamped = geometry_msgs.msg.WrenchStamped(self.wrench.header, rebuild_wrench)
+        # print(rebuild_wrenchStamped)
+        return rebuild_wrenchStamped
+
+    def convert_my_shit(self, picker):
+        convstampF = geometry_msgs.msg.Vector3Stamped(header=self.wrench.header, vector=self.wrench.wrench.force)
+        convstampT = geometry_msgs.msg.Vector3Stamped(header=self.wrench.header, vector=self.wrench.wrench.torque)
+        # Instead of picker define an array that consists of the 6 values
+        if picker == 1:
+            self.trans_F_x.append(convstampF.vector.x)
+            return self.trans_F_x
+
+        elif picker == 2:
+            self.trans_F_y.append(convstampF.vector.y)
+            return self.trans_F_y
+
+        elif picker == 3:
+            self.trans_F_z.append(convstampF.vector.z)
+            return self.trans_F_z
+
+        elif picker == 4:
+            self.trans_T_x.append(convstampT.vector.x)
+            return self.trans_T_x
+
+        elif picker == 5:
+            self.trans_T_y.append(convstampT.vector.y)
+            return self.trans_T_y
+
+        elif picker == 6:
+            self.trans_T_z.append(convstampT.vector.z)
+            return self.trans_T_z
+
+    def butter_lowpass(self, cutoff, fs, order=5):
+        nyq = 0.5 * fs  # Nyquist Frequency
+        normal_cutoff = cutoff / nyq
+        return butter(order, normal_cutoff, btype='low', analog=False)
+
+    def butter_lowpass_filter(self, data, cutoff, fs, order=5):
+        b, a = self.butter_lowpass(cutoff, fs, order=order)
+        y = lfilter(b, a, data)
+        return y
+
+
+if __name__ == '__main__':
+    rospy.init_node('force_torque_raw_filter')
+    force = ForceTorqueRawFilter()
+    rospy.spin()

src/giskardpy/monitors/force_torque_monitor.py

@@ -2,13 +2,14 @@
 from typing import Optional
 
 import geometry_msgs
+import hsrb_interface.end_effector
 import rospy
 from geometry_msgs.msg import WrenchStamped
 
 import giskardpy.casadi_wrapper as cas
 from giskardpy.god_map import god_map
 from giskardpy.monitors.monitors import PayloadMonitor
-from giskardpy.suturo_types import ForceTorqueThresholds
+from giskardpy.suturo_types import ForceTorqueThresholds, ObjectTypes
 from giskardpy.utils import logging
 
 
@@ -20,22 +21,28 @@ class PayloadForceTorque(PayloadMonitor):
     """
 
     def __init__(self,
-                 # is_raw is necessary to switch between the raw and the compensated topic
-                 is_raw: bool,
                  # threshold_name is needed here for the class to be able to handle the suturo_types appropriately
                  threshold_name: string,
+                 # object_type is needed to differentiate between objects with different placing thresholds
+                 object_type: Optional[str] = None,
                  topic: string = "/hsrb/wrist_wrench/compensated",
+                 filter_topic: string = "rebuilt_signal",  # Might be unnecessary
                  name: Optional[str] = None,
                  start_condition: cas.Expression = cas.TrueSymbol
                  ):
-
-        if is_raw:
-            logging.logwarn("ForceTorque Monitor is now listening to the /hsrb/wrist_wrench/raw Topic!")
-            topic = "/hsrb/wrist_wrench/raw"
+        """
+        :param threshold_name: contains the name of the threshold that will be used (normally an action e.g. Placing)
+        :param object_type: is used to determine the type of object that is being placed, is left empty if no object is being placed
+        :param topic: the name of the topic
+        :param name: name of the monitor class
+        :param start_condition: the start condition of the monitor
+        """
 
         super().__init__(name=name, stay_true=False, start_condition=start_condition, run_call_in_thread=False)
+        self.object_type = object_type
         self.threshold_name = threshold_name
         self.topic = topic
+        self.filter_topic = filter_topic  # filter_topic is used for the rebuilt_signal topic of the raw filter
         self.wrench = WrenchStamped()
         self.subscriber = rospy.Subscriber(name=topic,
                                            data_class=WrenchStamped, callback=self.cb)
@@ -47,7 +54,7 @@ def force_T_map_transform(self, picker):
         """
         The force_T_map_transform method is used to transform the Vector data from the
         force-torque sensor frame into the map frame, so that the axis stay
-        the same, to ensure that the threshold check is actually done on the relevant axis.
+        the same, to ensure that the threshold check is actually done on the relevant/correct axis.
         """
         self.wrench.header.frame_id = god_map.world.search_for_link_name(self.wrench.header.frame_id)
 
@@ -77,48 +84,128 @@ def __call__(self):
 
         if self.threshold_name == ForceTorqueThresholds.FT_GraspWithCare.value:
 
-            force_threshold = 0.2
-            torque_threshold = 0.02
-
-            if (abs(rob_force.vector.y) > force_threshold or
-                    abs(rob_torque.vector.y) > torque_threshold):
-                self.state = True
-                print(f'HIT GWC: {rob_force.vector.x};{rob_torque.vector.y}')
-            else:
-                self.state = False
-                print(f'MISS GWC!: {rob_force.vector.x};{rob_torque.vector.y}')
-
+            # Makes sure that thresholds are only being checked if an attempt at grasping has been made
+            if hsrb_interface.end_effector.Gripper.get_distance() <= 0.002:
+                # Basic idea for checking whether an object has successfully been grasped or not:
+                # return false as long as threshold is being surpassed;
+                # return true when it stops being surpassed, so that goal stops when object is unexpectedly dropped
+                # or the HSR failed to grasp them at all
+
+                # case for grasping "normal" objects (namely Milk, Cereal and cups)
+                if self.object_type == ObjectTypes.OT_Standard.value:
+
+                    force_threshold = 0.2
+                    torque_threshold = 0.02
+
+                    if (abs(rob_force.vector.y) > force_threshold or
+                            abs(rob_torque.vector.y) > torque_threshold):
+                        self.state = False
+                        print(f'HIT GWC: {rob_force.vector.x};{rob_torque.vector.y}')
+                    else:
+                        self.state = True
+                        raise Exception("HSR failed to Grasp Object,Grasping threshold has been Undershot.") # might not be needed at all, since monitor basically gives out signal
+
+                # case for grasping cutlery
+                elif self.object_type == ObjectTypes.OT_Cutlery.value:
+
+                    force_threshold = 0.2
+                    torque_threshold = 0.02
+
+                    if (abs(rob_force.vector.y) > force_threshold or
+                            abs(rob_torque.vector.y) > torque_threshold):
+                        self.state = False
+                        print(f'HIT GWC: {rob_force.vector.x};{rob_torque.vector.y}')
+                    else:
+                        self.state = True
+                        raise Exception("HSR failed to Grasp Object,Grasping threshold has been Undershot.")
+
+                # case for grasping plate
+                elif self.object_type == ObjectTypes.OT_Plate.value:
+
+                    force_threshold = 0.2
+                    torque_threshold = 0.02
+
+                    if (abs(rob_force.vector.y) > force_threshold or
+                            abs(rob_torque.vector.y) > torque_threshold):
+                        self.state = False
+                        print(f'HIT GWC: {rob_force.vector.x};{rob_torque.vector.y}')
+                    else:
+                        self.state = True
+                        raise Exception("HSR failed to Grasp Object,Grasping threshold has been Undershot.")
+
+                # case for grasping Bowl
+                elif self.object_type == ObjectTypes.OT_Bowl.value:
+
+                    force_threshold = 0.2
+                    torque_threshold = 0.02
+
+                    if (abs(rob_force.vector.y) > force_threshold or
+                            abs(rob_torque.vector.y) > torque_threshold):
+                        self.state = False
+                        print(f'HIT GWC: {rob_force.vector.x};{rob_torque.vector.y}')
+                    else:
+                        self.state = True
+                        raise Exception("HSR failed to Grasp Object,Grasping threshold has been Undershot.")
+
+                # if no valid object_type has been declared in method parameters
+                else:
+                    raise Exception("No valid object_type found, unable to determine placing thresholds!")
+
+        # TODO: Add thresholds and cases for other object types
         elif self.threshold_name == ForceTorqueThresholds.FT_Placing.value:
 
-            force_x_threshold = 2.34
-            force_z_threshold = 1.0
-            torque_y_threshold = 0.45
-
-            if (abs(rob_force.vector.x) >= force_x_threshold and
-                    abs(rob_force.vector.z) >= force_z_threshold and
-                    abs(rob_torque.vector.y) >= torque_y_threshold):
-
-                self.state = True
-                print(f'HIT PLACING: {rob_force.vector.x};{rob_force.vector.z}')
-            else:
-                self.state = False
-                print(f'MISS PLACING!: {rob_force.vector.x};{rob_force.vector.z}')
-
-        elif (self.threshold_name == ForceTorqueThresholds.FT_GraspCutlery.value
-              & self.topic == "/hsrb/wrist_wrench/raw"):
-            # TODO: Add proper Thresholds and Checks for Cutlery
-            force_x_threshold = 0
-            force_y_threshold = 0
-            force_z_threshold = 0
-
-            if (abs(rob_force.vector.x) >= force_x_threshold and
-                    abs(rob_force.vector.y) >= force_y_threshold and
-                    abs(rob_force.vector.z) >= force_z_threshold):
-
-                self.state = True
-
+            # case for placing "normal" objects (namely Milk, Cereal and cups)
+            if self.object_type == ObjectTypes.OT_Standard.value:
+                force_x_threshold = 2.34
+                force_z_threshold = 1.0
+                torque_y_threshold = 0.45
+
+                if (abs(rob_force.vector.x) >= force_x_threshold and
+                        abs(rob_force.vector.z) >= force_z_threshold and
+                        abs(rob_torque.vector.y) >= torque_y_threshold):
+
+                    self.state = True
+                    print(f'HIT PLACING: X:{rob_force.vector.x};Z:{rob_force.vector.z};Y:{rob_torque.vector.y}')
+                else:
+                    self.state = False
+                    print(f'MISS PLACING!: X:{rob_force.vector.x};Z:{rob_force.vector.z};Y:{rob_torque.vector.y}')
+
+            # case for placing cutlery
+            elif self.object_type == ObjectTypes.OT_Cutlery.value:
+                self.topic = "~/rebuilt_signal"
+
+                if (self.threshold_name == ForceTorqueThresholds.FT_PlaceCutlery.value
+                        & self.topic == "rebuilt_signal"):
+                    # TODO: Add proper Thresholds and Checks for placing Cutlery
+                    print(f'filtered Force: {rob_force}')
+                    print(f'filtered Torque: {rob_torque}')
+                    force_x_threshold = 0
+                    force_y_threshold = 0
+                    force_z_threshold = 0
+
+                    if (abs(rob_force.vector.x) >= force_x_threshold and
+                            abs(rob_force.vector.y) >= force_y_threshold and
+                            abs(rob_force.vector.z) >= force_z_threshold):
+
+                        self.state = True
+                        print(f'HIT CUTLERY: X:{rob_force.vector.x};Z:{rob_force.vector.z};Y:{rob_torque.vector.y}')
+                    else:
+                        self.state = False
+                        print(f'MISS CUTLERY: X:{rob_force.vector.x};Z:{rob_force.vector.z};Y:{rob_torque.vector.y}')
+
+            # case for placing plates
+            elif self.object_type == ObjectTypes.OT_Plate.value:
+                #  TODO: Add proper placing logic for Plate
+                print("IT JUST WORKS - Todd Howard, at some point")
+
+            # case for placing bowls
+            elif self.object_type == ObjectTypes.OT_Bowl.value:
+                #  TODO: Add proper placing logic for Bowl
+                print("IT JUST WORKS - Todd Howard, at some point")
+
+            # if no valid object_type has been declared in method parameters
             else:
-                self.state = False
+                raise Exception("No valid object_type found, unable to determine placing thresholds!")
 
-        elif self.threshold_name != ForceTorqueThresholds.value:
-            logging.logerr("Please only use Values for threshold_name that can be found in the ForceTorqueThresholds!!")
+        else:
+            raise Exception("No valid threshold_name found, unable to determine proper course of action!")

src/giskardpy/python_interface/python_interface.py

@@ -1539,6 +1539,8 @@ def _feedback_cb(self, msg: MoveFeedback):
     def monitor_placing(self,
                         context,
                         goal_pose: PoseStamped,
+                        threshold_name: str = "",
+                        object_type: str = "",
                         tip_link: str = 'hand_palm_link',
                         velocity: float = 0.02):
         """
@@ -1549,8 +1551,8 @@ def monitor_placing(self,
         force_torque_trigger = self.monitors.add_monitor(monitor_class=PayloadForceTorque.__name__,
                                                          name=PayloadForceTorque.__name__,
                                                          start_condition='',
-                                                         threshold_name=ForceTorqueThresholds.FT_Placing.value,
-                                                         is_raw=False)
+                                                         threshold_name=threshold_name,
+                                                         object_type=object_type)
 
         self.motion_goals.add_motion_goal(motion_goal_class='Placing',
                                           context=context,
@@ -1569,25 +1571,30 @@ def monitor_grasp_carefully(self,
                                 from_above: bool = False,
                                 align_vertical: bool = False,
                                 reference_frame_alignment: Optional[str] = None,
+                                object_name: str = "",
+                                object_type: str = "",
+                                threshold_name: str = "",
                                 root_link: Optional[str] = None,
                                 tip_link: Optional[str] = None):
         """
-        adds monitor functionality for the GraspCarefully motion goal, goal now stops if force_threshold is overstepped,
+        adds monitor functionality to the reaching goal, thus making the original GraspCarefully motion goal redundant.
+        The goal now stops if force_threshold/torque_threshold is undershot,
         which means it essentially stops automatically if the HSR for example slips off of a door handle while trying
-        to open doors.
+        to open doors or fails to properly grip an object.
         """
         sleep = self.monitors.add_sleep(1.5)
         force_torque_trigger = self.monitors.add_monitor(monitor_class=PayloadForceTorque.__name__,
                                                          name=PayloadForceTorque.__name__,
                                                          start_condition='',
-                                                         threshold_name=ForceTorqueThresholds.FT_GraspWithCare.value,
-                                                         is_raw=False)
+                                                         threshold_name=threshold_name,
+                                                         object_type=object_type)
 
-        self.motion_goals.add_motion_goal(motion_goal_class='GraspCarefully',
+        self.motion_goals.add_motion_goal(motion_goal_class='Reaching',
                                           goal_pose=goal_pose,
                                           from_above=from_above,
                                           align_vertical=align_vertical,
                                           reference_frame_alignment=reference_frame_alignment,
+                                          object_name=object_name,
                                           root_link=root_link,
                                           tip_link=tip_link,
                                           end_condition=f'{force_torque_trigger} and {sleep}')