diff --git a/challenge_dishwasher/src/challenge_dishwasher/custom_place.py b/challenge_dishwasher/src/challenge_dishwasher/custom_place.py
index 2e0e95440c..4224263214 100644
--- a/challenge_dishwasher/src/challenge_dishwasher/custom_place.py
+++ b/challenge_dishwasher/src/challenge_dishwasher/custom_place.py
@@ -57,7 +57,7 @@ def place_pose(ud):
 
         @cb_interface(outcomes=['done'])
         def open_gripper(ud):
-            arm.resolve().send_gripper_goal("open", timeout=0)
+            arm.resolve().gripper.send_goal("open", timeout=0)
             rospy.sleep(3.0)
             robot.speech.speak("This is where I usually place my cup")
             return 'done'
diff --git a/challenge_dishwasher/src/challenge_dishwasher/open_dishwasher.py b/challenge_dishwasher/src/challenge_dishwasher/open_dishwasher.py
index debb9cc507..e978133867 100644
--- a/challenge_dishwasher/src/challenge_dishwasher/open_dishwasher.py
+++ b/challenge_dishwasher/src/challenge_dishwasher/open_dishwasher.py
@@ -105,7 +105,7 @@ def __init__(self, robot, dishwasher_id):
 
         @cb_interface(outcomes=['done'])
         def _pre_grab_handle(ud):
-            robot.rightArm.send_gripper_goal("open", timeout=0)
+            robot.rightArm.gripper.send_goal("open", timeout=0)
             robot.rightArm._send_joint_trajectory([[0, 0.2519052373022729913, 0.7746500794619434, 1.3944848321343395,
                                                    -1.829999276180074, 0.6947045024700284, 0.1889253710114966]],
                                                  timeout=rospy.Duration(0))
@@ -118,7 +118,7 @@ def _grab_handle(ud):
             robot.speech.speak('I hope this goes right!', block=False)
             fs = frame_stamped("dishwasher", 0.42, 0, 0.8, roll=math.pi / 2, pitch=0, yaw=math.pi)
             robot.rightArm.send_goal(fs.projectToFrame(robot.robot_name + "/base_link", robot.tf_listener))
-            robot.rightArm.send_gripper_goal("close")
+            robot.rightArm.gripper.send_goal("close")
             return 'done'
 
         @cb_interface(outcomes=['done'])
diff --git a/challenge_dishwasher/src/challenge_dishwasher/simple_grab.py b/challenge_dishwasher/src/challenge_dishwasher/simple_grab.py
index 4fc6cba050..517c487a2d 100644
--- a/challenge_dishwasher/src/challenge_dishwasher/simple_grab.py
+++ b/challenge_dishwasher/src/challenge_dishwasher/simple_grab.py
@@ -86,13 +86,13 @@ def execute(self, ud):
             self.robot.speech.speak('I am sorry but I cannot move my arm to the object position', block=False)
             rospy.logerr('Grasp failed')
             arm.reset()
-            arm.send_gripper_goal('close', timeout=0.0)
+            arm.gripper.send_goal('close', timeout=0.0)
             return 'failed'
 
         # Close gripper
-        arm.send_gripper_goal('close')
+        arm.gripper.send_goal('close')
 
-        arm.occupied_by = None  # TODO: what to do if we don't have an entity
+        arm.gripper.occupied_by = None  # TODO: what to do if we don't have an entity
 
         # Lift
         rospy.loginfo('Start lifting')
@@ -142,7 +142,7 @@ def __init__(self, robot, arm):
         @cb_interface(outcomes=['succeeded'])
         def prepare_grasp(ud):
             # Open gripper (non-blocking)
-            arm.resolve().send_gripper_goal('open', timeout=0)
+            arm.resolve().gripper.send_goal('open', timeout=0)
 
             # Torso up (non-blocking)
             robot.torso.high()
@@ -151,7 +151,7 @@ def prepare_grasp(ud):
             arm.resolve().send_joint_trajectory('prepare_grasp', timeout=0)
 
             # Open gripper
-            arm.resolve().send_gripper_goal('open', timeout=0.0)
+            arm.resolve().gripper.send_goal('open', timeout=0.0)
             return 'succeeded'
 
         with self:
diff --git a/challenge_final/src/challenge_final/navigate_to_and_grab_rack.py b/challenge_final/src/challenge_final/navigate_to_and_grab_rack.py
index 2dac8ae49d..0a0bde067b 100644
--- a/challenge_final/src/challenge_final/navigate_to_and_grab_rack.py
+++ b/challenge_final/src/challenge_final/navigate_to_and_grab_rack.py
@@ -35,7 +35,7 @@ def send_joint_goal(position_array, wait_for_motion_done=True):
                 arm.wait_for_motion_done()
 
         def send_gripper_goal(open_close_string, max_torque=1.0):
-            arm.send_gripper_goal(open_close_string, max_torque=max_torque)
+            arm.gripper.send_goal(open_close_string, max_torque=max_torque)
             rospy.sleep(1.0)  # Does not work with motion_done apparently
 
         def show_image(package_name, path_to_image_in_package):
diff --git a/challenge_presentation/src/challenge_presentation/presentation.py b/challenge_presentation/src/challenge_presentation/presentation.py
index f16b43ba64..6631c9dfad 100644
--- a/challenge_presentation/src/challenge_presentation/presentation.py
+++ b/challenge_presentation/src/challenge_presentation/presentation.py
@@ -110,11 +110,11 @@ def execute(self, userdata=None):
             if self.right_arm is not None:
                 function_list.append(partial(self.right_arm.send_joint_trajectory, "show_gripper"))
             if self.left_arm is not None:
-                function_list.append(partial(self.left_arm.send_gripper_goal, "open"))
-                function_list.append(partial(self.left_arm.send_gripper_goal, "close"))
+                function_list.append(partial(self.left_arm.gripper.send_goal, "open"))
+                function_list.append(partial(self.left_arm.gripper.send_goal, "close"))
             if self.right_arm is not None:
-                function_list.append(partial(self.right_arm.send_gripper_goal, "open"))
-                function_list.append(partial(self.right_arm.send_gripper_goal, "close"))
+                function_list.append(partial(self.right_arm.gripper.send_goal, "open"))
+                function_list.append(partial(self.right_arm.gripper.send_goal, "close"))
             if self.left_arm is not None:
                 function_list.append(partial(self.left_arm.reset))
             if self.right_arm is not None:
@@ -165,9 +165,9 @@ def execute(self, userdata=None):
                 if self.right_arm is not None:
                     self.right_arm.reset()
                 if self.left_arm is not None:
-                    self.left_arm.send_gripper_goal("close")
+                    self.left_arm.gripper.send_goal("close")
                 if self.right_arm is not None:
-                    self.right_arm.send_gripper_goal("close")
+                    self.right_arm.gripper.send_goal("close")
                 self.robot.torso.reset()
                 self.robot.head.reset()
 
diff --git a/challenge_presentation/src/challenge_presentation/presentation_hero.py b/challenge_presentation/src/challenge_presentation/presentation_hero.py
index e49bb59b51..976815a766 100755
--- a/challenge_presentation/src/challenge_presentation/presentation_hero.py
+++ b/challenge_presentation/src/challenge_presentation/presentation_hero.py
@@ -120,8 +120,8 @@ def execute(self, userdata=None):
         function_list.append(partial(self.robot.speech.speak, self.trans.GRIPPER, language=self.language,
                                      voice=self.voice, block=False))
         function_list.append(partial(self.arm.send_joint_goal, "show_gripper"))
-        function_list.append(partial(self.arm.send_gripper_goal, "open"))
-        function_list.append(partial(self.arm.send_gripper_goal, "close"))
+        function_list.append(partial(self.arm.gripper.send_goal, "open"))
+        function_list.append(partial(self.arm.gripper.send_goal, "close"))
         function_list.append(partial(self.robot.speech.speak, self.trans.GRIPPER_CAMERA, language=self.language,
                                      voice=self.voice, block=True))
         function_list.append(partial(self.arm.reset))
@@ -179,7 +179,7 @@ def execute(self, userdata=None):
             if self.preempt_requested():
                 self.robot.speech.speak("Sorry, but I have to stop my introduction")
                 self.arm.reset()
-                self.arm.send_gripper_goal("close")
+                self.arm.gripper.send_goal("close")
                 self.robot.torso.reset()
                 self.robot.head.reset()
                 self.arm.wait_for_motion_done()
diff --git a/challenge_set_a_table/src/challenge_set_a_table_states/manipulate_machine.py b/challenge_set_a_table/src/challenge_set_a_table_states/manipulate_machine.py
index 771c1deb25..6ae5c2503a 100644
--- a/challenge_set_a_table/src/challenge_set_a_table_states/manipulate_machine.py
+++ b/challenge_set_a_table/src/challenge_set_a_table_states/manipulate_machine.py
@@ -136,7 +136,7 @@ def __init__(self, robot, place_designator):
 
     def execute(self, userdata=None):
         # Try to place the object
-        item = ds.EdEntityDesignator(robot=self._robot, id=arm.occupied_by.id)
+        item = ds.EdEntityDesignator(robot=self._robot, id=arm.gripper.occupied_by.id)
         arm_designator = ds.OccupiedArmDesignator(self._robot,
                                                   arm_properties={
                                                       "required_trajectories": ["prepare_place"],
diff --git a/challenge_set_the_table/src/challenge_set_the_table/navigate_to_and_close_cupboard_drawer.py b/challenge_set_the_table/src/challenge_set_the_table/navigate_to_and_close_cupboard_drawer.py
index f21904cd6d..94132c8963 100644
--- a/challenge_set_the_table/src/challenge_set_the_table/navigate_to_and_close_cupboard_drawer.py
+++ b/challenge_set_the_table/src/challenge_set_the_table/navigate_to_and_close_cupboard_drawer.py
@@ -28,12 +28,12 @@ def send_joint_goal(position_array, wait_for_motion_done=True):
                 arm.wait_for_motion_done()
 
         def send_gripper_goal(open_close_string):
-            arm.send_gripper_goal(open_close_string)
+            arm.gripper.send_goal(open_close_string)
             rospy.sleep(1.0)  # Does not work with motion_done apparently
 
         @cb_interface(outcomes=['done'])
         def _pre_grab_handle(_):
-            arm.send_gripper_goal("open", timeout=0)
+            arm.gripper.send_goal("open", timeout=0)
             send_joint_goal([0, -0, 0, -1.57, 1.57])
             return 'done'
 
diff --git a/challenge_set_the_table/src/challenge_set_the_table/navigate_to_and_open_cupboard_drawer.py b/challenge_set_the_table/src/challenge_set_the_table/navigate_to_and_open_cupboard_drawer.py
index 3df5a720fa..7a232945f8 100644
--- a/challenge_set_the_table/src/challenge_set_the_table/navigate_to_and_open_cupboard_drawer.py
+++ b/challenge_set_the_table/src/challenge_set_the_table/navigate_to_and_open_cupboard_drawer.py
@@ -28,12 +28,12 @@ def send_joint_goal(position_array, wait_for_motion_done=True):
                 arm.wait_for_motion_done()
 
         def send_gripper_goal(open_close_string):
-            arm.send_gripper_goal(open_close_string)
+            arm.gripper.send_goal(open_close_string)
             rospy.sleep(1.0)  # Does not work with motion_done apparently
 
         @cb_interface(outcomes=['done'])
         def _pre_grab_handle(_):
-            arm.send_gripper_goal("open", timeout=0)
+            arm.gripper.send_goal("open", timeout=0)
             send_joint_goal([0, -0, 0, -1.57, 1.57])
             return 'done'
 
diff --git a/challenge_set_the_table/src/challenge_set_the_table/navigate_to_and_pick_item_from_cupboard_drawer.py b/challenge_set_the_table/src/challenge_set_the_table/navigate_to_and_pick_item_from_cupboard_drawer.py
index af2882e44b..b1d6e48d44 100644
--- a/challenge_set_the_table/src/challenge_set_the_table/navigate_to_and_pick_item_from_cupboard_drawer.py
+++ b/challenge_set_the_table/src/challenge_set_the_table/navigate_to_and_pick_item_from_cupboard_drawer.py
@@ -37,7 +37,7 @@ def send_joint_goal(position_array, wait_for_motion_done=True):
                 arm.wait_for_motion_done()
 
         def send_gripper_goal(open_close_string, max_torque=0.1):
-            arm.send_gripper_goal(open_close_string, max_torque=max_torque)
+            arm.gripper.send_goal(open_close_string, max_torque=max_torque)
             rospy.sleep(1.0)  # Does not work with motion_done apparently
 
         def show_image(package_name, path_to_image_in_package):
diff --git a/challenge_set_the_table/src/challenge_set_the_table/navigate_to_and_place_item_on_table.py b/challenge_set_the_table/src/challenge_set_the_table/navigate_to_and_place_item_on_table.py
index cfc343ccaf..13c0bf164e 100644
--- a/challenge_set_the_table/src/challenge_set_the_table/navigate_to_and_place_item_on_table.py
+++ b/challenge_set_the_table/src/challenge_set_the_table/navigate_to_and_place_item_on_table.py
@@ -91,7 +91,7 @@ def send_joint_trajectory(goal_array, wait_for_motion_done=True):
                 arm.wait_for_motion_done()
 
         def send_gripper_goal(open_close_string):
-            arm.send_gripper_goal(open_close_string)
+            arm.gripper.send_goal(open_close_string)
             rospy.sleep(1.0)  # Does not work with motion_done apparently
 
         @cb_interface(outcomes=['done'], input_keys=["item_picked"])
diff --git a/challenge_storing_groceries/src/challenge_storing_groceries/manipulate_machine.py b/challenge_storing_groceries/src/challenge_storing_groceries/manipulate_machine.py
index 46281e081f..5d2e8bb25f 100644
--- a/challenge_storing_groceries/src/challenge_storing_groceries/manipulate_machine.py
+++ b/challenge_storing_groceries/src/challenge_storing_groceries/manipulate_machine.py
@@ -158,7 +158,7 @@ def __init__(self, robot, place_designator):
 
     def execute(self, userdata=None):
         # Try to place the object
-        item = ds.EdEntityDesignator(robot=self._robot, id=arm.occupied_by.id)
+        item = ds.EdEntityDesignator(robot=self._robot, id=arm.gripper.occupied_by.id)
         arm_designator = ds.OccupiedArmDesignator(self._robot, {"required_goals": ["reset", "handover_to_human"],
                                                                 "required_gripper_types": [arms.GripperTypes.GRASPING]})
         resolved_arm = arm_designator.resolve()
diff --git a/challenge_take_out_the_garbage/src/challenge_take_out_the_garbage/drop_down.py b/challenge_take_out_the_garbage/src/challenge_take_out_the_garbage/drop_down.py
index f43fd948b9..85dc878130 100644
--- a/challenge_take_out_the_garbage/src/challenge_take_out_the_garbage/drop_down.py
+++ b/challenge_take_out_the_garbage/src/challenge_take_out_the_garbage/drop_down.py
@@ -36,7 +36,7 @@ def execute(self, userdata=None):
         # Arm to position in a safe way
         arm.send_joint_goal('handover')
         arm.wait_for_motion_done()
-        arm.send_gripper_goal('open')
+        arm.gripper.send_goal('open')
         arm.wait_for_motion_done()
         arm._arm._send_joint_trajectory(
             [[0.4, -1.0, 0.0, -1.0, 0.0],[0.4, -1.0, 0.0, -1.57, 0.0], [0.4, -1.0, 0.0, -1.0, 0.0],
@@ -44,7 +44,7 @@ def execute(self, userdata=None):
         arm.wait_for_motion_done()
         arm.send_joint_goal('reset')
         arm.wait_for_motion_done()
-        # arm.send_gripper_goal('close')
+        # arm.gripper.send_goal('close')
         # arm.wait_for_motion_done()
         return "succeeded"
 
diff --git a/challenge_take_out_the_garbage/src/challenge_take_out_the_garbage/pick_up.py b/challenge_take_out_the_garbage/src/challenge_take_out_the_garbage/pick_up.py
index 9cf83768db..b8f74ce586 100644
--- a/challenge_take_out_the_garbage/src/challenge_take_out_the_garbage/pick_up.py
+++ b/challenge_take_out_the_garbage/src/challenge_take_out_the_garbage/pick_up.py
@@ -132,16 +132,16 @@ def execute(self, userdata=None):
             # This opening and closing is to make sure that the gripper is empty and closed before measuring the forces
             # It is necessary to close the gripper since the gripper is also closed at the final measurement
 
-            # arm.send_gripper_goal('open')
+            # arm.gripper.send_goal('open')
             # arm.wait_for_motion_done()
-            # arm.send_gripper_goal('close', max_torque=1.0)
+            # arm.gripper.send_goal('close', max_torque=1.0)
             # arm.wait_for_motion_done()
 
             arm_weight = measure_force.get_force()
             rospy.loginfo("Empty weight %s", arm_weight)
 
             # Open gripper
-            arm.send_gripper_goal('open')
+            arm.gripper.send_goal('open')
             arm.wait_for_motion_done()
 
             # Go down and grab
@@ -154,7 +154,7 @@ def execute(self, userdata=None):
             # self._robot.torso.send_goal("grab_trash_down")
             # self._robot.torso.wait_for_motion_done()
             # rospy.sleep(3)
-            arm.send_gripper_goal('close', max_torque=1.0)
+            arm.gripper.send_goal('close', max_torque=1.0)
             arm.wait_for_motion_done()
 
             # Go up and back to pre grasp position
@@ -182,7 +182,7 @@ def execute(self, userdata=None):
 
         self._robot.speech.speak("Look at this I can pick up the trash!")
         handed_entity = EntityInfo(id="trash")
-        arm.occupied_by = handed_entity
+        arm.gripper.occupied_by = handed_entity
 
         return "succeeded"
 
diff --git a/robot_skills/package.xml b/robot_skills/package.xml
index 224aa46e24..5ea9ab7726 100644
--- a/robot_skills/package.xml
+++ b/robot_skills/package.xml
@@ -40,6 +40,7 @@
   <exec_depend>python-mock</exec_depend>
 
   <test_depend>test_tools</test_depend>
+  <test_depend>python-six</test_depend>
 
   <doc_depend>python-sphinx</doc_depend>
   <doc_depend>python-sphinx-autoapi-pip</doc_depend>
diff --git a/robot_skills/src/robot_skills/mockbot.py b/robot_skills/src/robot_skills/mockbot.py
index 06d8d1a45d..b70a1c9859 100755
--- a/robot_skills/src/robot_skills/mockbot.py
+++ b/robot_skills/src/robot_skills/mockbot.py
@@ -7,6 +7,7 @@
 import mock
 import random
 import os
+import six
 
 # ROS
 import geometry_msgs
@@ -45,22 +46,83 @@ def old_query(spec, choices, timeout=10):
     raise Exception('robot.ears.recognize IS REMOVED. Use `robot.hmi.query`')
 
 
+class AlteredMagicMock(mock.MagicMock):
+    def assert_called_with(self, *args, **kwargs):
+        """
+        Assert that the mock was called with the specified arguments.
+
+        Raises an AssertionError if the args and keyword args passed in are
+        different to the last call to the mock.
+        """
+        if self.call_args is None:
+            expected = self._format_mock_call_signature(args, kwargs)
+            raise AssertionError('Expected call: %s\nNot called' % (expected,))
+
+        def _error_message(cause):
+            msg = self._format_mock_failure_message(args, kwargs)
+            if six.PY2 and cause is not None:
+                # Tack on some diagnostics for Python without __cause__
+                msg = '%s\n%s' % (msg, str(cause))
+            return msg
+
+        expected = self._call_matcher((args, kwargs))
+        actual = self._call_matcher(self.call_args)
+        if not self.equal_or_more_arguments(expected, actual):
+            cause = expected if isinstance(expected, Exception) else None
+            six.raise_from(AssertionError(_error_message(cause)), cause)
+
+    def assert_any_call(self, *args, **kwargs):
+        """
+        Assert the mock has been called with the specified arguments.
+
+        The assert passes if the mock has *ever* been called, unlike
+        `assert_called_with` and `assert_called_once_with` that only pass if
+        the call is the most recent one.
+        """
+        expected = self._call_matcher((args, kwargs))
+        actual = [self._call_matcher(c) for c in self.call_args_list]
+        if not any([self.equal_or_more_arguments(expected, c) for c in actual]):
+            cause = expected if isinstance(expected, Exception) else None
+            expected_string = self._format_mock_call_signature(args, kwargs)
+            six.raise_from(AssertionError("'%s' call not found" % expected_string), cause)
+
+    @staticmethod
+    def equal_or_more_arguments(expected, actual):
+        """
+        Check whether the actual call contains at least the arguments and keyword arguments of the expected call.
+        But still return true if the actual call contains more (keyword) arguments than expected
+        """
+        # check args
+        for i in range(len(expected[0])):
+            if i >= len(actual[0]):
+                return False
+            if expected[0][i] != actual[0][i]:
+                return False
+        # check kwargs
+        for k in expected[1]:
+            if k not in actual[1]:
+                return False
+            if expected[1][k] != actual[1][k]:
+                return False
+        return True
+
+
 class MockedRobotPart(object):
     def __init__(self, robot_name, tf_listener, *args, **kwargs):
         self.robot_name = robot_name
         self.tf_listener = tf_listener
 
-        self.load_param = mock.MagicMock()
-        self.wait_for_connections = mock.MagicMock()
-        self.create_simple_action_client = mock.MagicMock()
-        self.create_service_client = mock.MagicMock()
-        self.create_subscriber = mock.MagicMock()
-        self._add_connection = mock.MagicMock()
-        self.operational = mock.MagicMock()
-        self.subscribe_hardware_status = mock.MagicMock()
-        self.unsubscribe_hardware_status = mock.MagicMock()
-        self.process_hardware_status = mock.MagicMock()
-        self.reset = mock.MagicMock()
+        self.load_param = AlteredMagicMock()
+        self.wait_for_connections = AlteredMagicMock()
+        self.create_simple_action_client = AlteredMagicMock()
+        self.create_service_client = AlteredMagicMock()
+        self.create_subscriber = AlteredMagicMock()
+        self._add_connection = AlteredMagicMock()
+        self.operational = AlteredMagicMock()
+        self.subscribe_hardware_status = AlteredMagicMock()
+        self.unsubscribe_hardware_status = AlteredMagicMock()
+        self.process_hardware_status = AlteredMagicMock()
+        self.reset = AlteredMagicMock()
 
 
 class Arm(MockedRobotPart):
@@ -74,19 +136,19 @@ def __init__(self, robot_name, tf_listener, get_joint_states, side):
 
         self._base_offset = random_kdl_vector()
 
-        self.default_configurations = mock.MagicMock()
-        self.default_trajectories = mock.MagicMock()
-        self.has_joint_goal = mock.MagicMock()
-        self.has_joint_trajectory = mock.MagicMock()
-        self.cancel_goals = mock.MagicMock()
-        self.close = mock.MagicMock()
-        self.send_goal = mock.MagicMock()
-        self.send_joint_goal = mock.MagicMock()
-        self.send_joint_trajectory = mock.MagicMock()
-        self.reset = mock.MagicMock()
-        self._send_joint_trajectory = mock.MagicMock()
-        self._publish_marker = mock.MagicMock()
-        self.wait_for_motion_done = mock.MagicMock()
+        self.default_configurations = AlteredMagicMock()
+        self.default_trajectories = AlteredMagicMock()
+        self.has_joint_goal = AlteredMagicMock()
+        self.has_joint_trajectory = AlteredMagicMock()
+        self.cancel_goals = AlteredMagicMock()
+        self.close = AlteredMagicMock()
+        self.send_goal = AlteredMagicMock()
+        self.send_joint_goal = AlteredMagicMock()
+        self.send_joint_trajectory = AlteredMagicMock()
+        self.reset = AlteredMagicMock()
+        self._send_joint_trajectory = AlteredMagicMock()
+        self._publish_marker = AlteredMagicMock()
+        self.wait_for_motion_done = AlteredMagicMock()
 
         # add parts
         self.gripper = Gripper(robot_name, tf_listener)
@@ -104,29 +166,31 @@ class Gripper(MockedRobotPart):
     def __init__(self, robot_name, tf_listener, *args, **kwargs):
         super(Gripper, self).__init__(robot_name, tf_listener)
         self.occupied_by = None
-        self.send_goal = mock.MagicMock()
+        self.send_goal = AlteredMagicMock()
+
 
 class HandoverDetector(MockedRobotPart):
     def __init__(self, robot_name, tf_listener, *args, **kwargs):
         super(HandoverDetector, self).__init__(robot_name, tf_listener)
-        self.handover_to_human = mock.MagicMock()
-        self.handover_to_robot = mock.MagicMock()
+        self.handover_to_human = AlteredMagicMock()
+        self.handover_to_robot = AlteredMagicMock()
+
 
 class Base(MockedRobotPart):
     def __init__(self, robot_name, tf_listener, *args, **kwargs):
         super(Base, self).__init__(robot_name, tf_listener)
-        self.move = mock.MagicMock()
-        self.turn_towards = mock.MagicMock()
-        self.force_drive = mock.MagicMock()
+        self.move = AlteredMagicMock()
+        self.turn_towards = AlteredMagicMock()
+        self.force_drive = AlteredMagicMock()
         self.get_location = lambda: FrameStamped(random_kdl_frame(), "/map")
-        self.set_initial_pose = mock.MagicMock()
-        self.wait_for_motion_done = mock.MagicMock()
-        self.go = mock.MagicMock()
-        self.reset_costmap = mock.MagicMock()
-        self.cancel_goal = mock.MagicMock()
-        self.analyzer = mock.MagicMock()
-        self.global_planner = mock.MagicMock()
-        self.local_planner = mock.MagicMock()
+        self.set_initial_pose = AlteredMagicMock()
+        self.wait_for_motion_done = AlteredMagicMock()
+        self.go = AlteredMagicMock()
+        self.reset_costmap = AlteredMagicMock()
+        self.cancel_goal = AlteredMagicMock()
+        self.analyzer = AlteredMagicMock()
+        self.global_planner = AlteredMagicMock()
+        self.local_planner = AlteredMagicMock()
         self.local_planner.getStatus = mock.MagicMock(return_value="arrived")  # always arrive for now
         self.global_planner.getPlan = mock.MagicMock(return_value=["dummy_plan"])  # always arrive for now
 
@@ -136,73 +200,73 @@ def __init__(self, robot_name, tf_listener, *args, **kwargs):
         super(Hmi, self).__init__(robot_name, tf_listener)
 
         self.query = mock_query
-        self.show_image = mock.MagicMock()
-        self.show_image_from_msg = mock.MagicMock()
+        self.show_image = AlteredMagicMock()
+        self.show_image_from_msg = AlteredMagicMock()
         self.old_query = old_query
-        self.reset = mock.MagicMock()
-        self.restart_dragonfly = mock.MagicMock()
+        self.reset = AlteredMagicMock()
+        self.restart_dragonfly = AlteredMagicMock()
 
 
 class EButton(MockedRobotPart):
     def __init__(self, robot_name, tf_listener, *args, **kwargs):
         super(EButton, self).__init__(robot_name, tf_listener)
-        self.close = mock.MagicMock()
-        self._listen = mock.MagicMock()
-        self.read_ebutton = mock.MagicMock()
+        self.close = AlteredMagicMock()
+        self._listen = AlteredMagicMock()
+        self.read_ebutton = AlteredMagicMock()
 
 
 class Head(MockedRobotPart):
     def __init__(self, robot_name, tf_listener, *args, **kwargs):
         super(Head, self).__init__(robot_name, tf_listener)
-        self.reset = mock.MagicMock()
-        self.close = mock.MagicMock()
-        self.set_pan_tilt = mock.MagicMock()
-        self.send_goal = mock.MagicMock()
-        self.cancel_goal = mock.MagicMock()
-        self.reset = mock.MagicMock()
-        self.look_at_hand = mock.MagicMock()
-        self.wait = mock.MagicMock()
-        self.getGoal = mock.MagicMock()
-        self.atGoal = mock.MagicMock()
-        self.look_at_standing_person = mock.MagicMock()
-        self.look_at_point = mock.MagicMock()
-        self.look_at_ground_in_front_of_robot = mock.MagicMock()  # TODO: Must return a EntityInfo
-        self.setPanTiltGoal = mock.MagicMock()
-        self.setLookAtGoal = mock.MagicMock()
-        self.cancelGoal = mock.MagicMock()
-        self.wait_for_motion_done = mock.MagicMock()
-        self._setHeadReferenceGoal = mock.MagicMock()
-        self.__feedbackCallback = mock.MagicMock()
-        self.__doneCallback = mock.MagicMock()
+        self.reset = AlteredMagicMock()
+        self.close = AlteredMagicMock()
+        self.set_pan_tilt = AlteredMagicMock()
+        self.send_goal = AlteredMagicMock()
+        self.cancel_goal = AlteredMagicMock()
+        self.reset = AlteredMagicMock()
+        self.look_at_hand = AlteredMagicMock()
+        self.wait = AlteredMagicMock()
+        self.getGoal = AlteredMagicMock()
+        self.atGoal = AlteredMagicMock()
+        self.look_at_standing_person = AlteredMagicMock()
+        self.look_at_point = AlteredMagicMock()
+        self.look_at_ground_in_front_of_robot = AlteredMagicMock()  # TODO: Must return a EntityInfo
+        self.setPanTiltGoal = AlteredMagicMock()
+        self.setLookAtGoal = AlteredMagicMock()
+        self.cancelGoal = AlteredMagicMock()
+        self.wait_for_motion_done = AlteredMagicMock()
+        self._setHeadReferenceGoal = AlteredMagicMock()
+        self.__feedbackCallback = AlteredMagicMock()
+        self.__doneCallback = AlteredMagicMock()
 
 
 class Perception(MockedRobotPart):
     def __init__(self, robot_name, tf_listener, *args, **kwargs):
         super(Perception, self).__init__(robot_name, tf_listener)
-        self.reset = mock.MagicMock()
-        self.close = mock.MagicMock()
-        self.learn_person = mock.MagicMock()
-        self.detect_faces = mock.MagicMock()
-        self.get_best_face_recognition = mock.MagicMock()
-        self.get_rgb_depth_caminfo = mock.MagicMock()
+        self.reset = AlteredMagicMock()
+        self.close = AlteredMagicMock()
+        self.learn_person = AlteredMagicMock()
+        self.detect_faces = AlteredMagicMock()
+        self.get_best_face_recognition = AlteredMagicMock()
+        self.get_rgb_depth_caminfo = AlteredMagicMock()
         self.project_roi = lambda *args, **kwargs: VectorStamped(random.random(), random.random(), random.random(), "/map")
 
 
 class Lights(MockedRobotPart):
     def __init__(self, robot_name, tf_listener, *args, **kwargs):
         super(Lights, self).__init__(robot_name, tf_listener)
-        self.close = mock.MagicMock()
-        self.set_color = mock.MagicMock()
-        self.set_color_colorRGBA = mock.MagicMock()
-        self.on = mock.MagicMock()
-        self.off = mock.MagicMock()
+        self.close = AlteredMagicMock()
+        self.set_color = AlteredMagicMock()
+        self.set_color_colorRGBA = AlteredMagicMock()
+        self.on = AlteredMagicMock()
+        self.off = AlteredMagicMock()
 
 
 class Speech(MockedRobotPart):
     def __init__(self, robot_name, tf_listener, *args, **kwargs):
         super(Speech, self).__init__(robot_name, tf_listener)
-        self.close = mock.MagicMock()
-        self.speak = mock.MagicMock()
+        self.close = AlteredMagicMock()
+        self.speak = AlteredMagicMock()
 
 
 class Torso(MockedRobotPart):
@@ -211,18 +275,18 @@ def __init__(self, robot_name, tf_listener, get_joint_states, *args, **kwargs):
 
         self.get_joint_states = get_joint_states
 
-        self.close = mock.MagicMock()
-        self.send_goal = mock.MagicMock()
-        self._send_goal = mock.MagicMock()
-        self.high = mock.MagicMock()
-        self.medium = mock.MagicMock()
-        self.low = mock.MagicMock()
-        self.reset = mock.MagicMock()
-        self.wait = mock.MagicMock()
-        self.cancel_goal = mock.MagicMock()
-        self._receive_torso_measurement = mock.MagicMock()
-        self.get_position = mock.MagicMock()
-        self.wait_for_motion_done = mock.MagicMock()
+        self.close = AlteredMagicMock()
+        self.send_goal = AlteredMagicMock()
+        self._send_goal = AlteredMagicMock()
+        self.high = AlteredMagicMock()
+        self.medium = AlteredMagicMock()
+        self.low = AlteredMagicMock()
+        self.reset = AlteredMagicMock()
+        self.wait = AlteredMagicMock()
+        self.cancel_goal = AlteredMagicMock()
+        self._receive_torso_measurement = AlteredMagicMock()
+        self.get_position = AlteredMagicMock()
+        self.wait_for_motion_done = AlteredMagicMock()
 
 
 class ED(MockedRobotPart):
@@ -235,7 +299,7 @@ def generate_random_entity(id=None, type=None):
             else:
                 entity_info.id = id
             entity_info.type = random.choice(["random_from_magicmock", "human", "coke", "fanta"])
-            # entity.data = mock.MagicMock()
+            # entity.data = AlteredMagicMock()
             entity_info.data = ""
 
             entity = from_entity_info(entity_info)
@@ -258,13 +322,13 @@ def __init__(self, robot_name, tf_listener, *args, **kwargs):
         self.get_closest_entity = lambda *args, **kwargs: random.choice(self._entities.values())
         self.get_entity = lambda id=None: self._entities[id]
         self.reset = lambda *args, **kwargs: self._dynamic_entities.clear()
-        self.navigation = mock.MagicMock()
-        self.navigation.get_position_constraint = mock.MagicMock()
-        self.update_entity = mock.MagicMock()
+        self.navigation = AlteredMagicMock()
+        self.navigation.get_position_constraint = AlteredMagicMock()
+        self.update_entity = AlteredMagicMock()
         self.get_closest_possible_person_entity = lambda *args, **kwargs: self.generate_random_entity()
         self.get_closest_laser_entity = lambda *args, **kwargs: self.generate_random_entity()
-        self.get_entity_info = mock.MagicMock()
-        self.wait_for_connections = mock.MagicMock()
+        self.get_entity_info = AlteredMagicMock()
+        self.wait_for_connections = AlteredMagicMock()
 
         self._person_names = []
 
@@ -341,10 +405,10 @@ def __init__(self, *args, **kwargs):
 
         super(Mockbot, self).__init__(robot_name=robot_name, wait_services=False, tf_listener=MockedTfListener)
 
-        self.publish_target = mock.MagicMock()
-        self.tf_transform_pose = mock.MagicMock()
-        self.close = mock.MagicMock()
-        self.get_joint_states = mock.MagicMock()
+        self.publish_target = AlteredMagicMock()
+        self.tf_transform_pose = AlteredMagicMock()
+        self.close = AlteredMagicMock()
+        self.get_joint_states = AlteredMagicMock()
 
         self.add_body_part('hmi', Hmi(self.robot_name, self.tf_listener))
 
@@ -367,7 +431,7 @@ def __init__(self, *args, **kwargs):
         self.add_body_part('perception', Perception(self.robot_name, self.tf_listener))
 
         # Miscellaneous
-        self.pub_target = mock.MagicMock()
+        self.pub_target = AlteredMagicMock()
 
         # Grasp offsets
         # TODO: Don't hardcode, load from parameter server to make robot independent.
diff --git a/robot_skills/src/robot_skills/util/kdl_conversions.py b/robot_skills/src/robot_skills/util/kdl_conversions.py
index b680054f1e..978e6d889b 100644
--- a/robot_skills/src/robot_skills/util/kdl_conversions.py
+++ b/robot_skills/src/robot_skills/util/kdl_conversions.py
@@ -87,6 +87,9 @@ def __eq__(self, other):
         else:
             return False
 
+    def __ne__(self, other):
+        return not self.__eq__(other)
+
 
 def point_msg_to_kdl_vector(point):
     """
diff --git a/robot_smach_states/src/robot_smach_states/manipulation/grab.py b/robot_smach_states/src/robot_smach_states/manipulation/grab.py
index b92ab79abd..e64f09f3b9 100644
--- a/robot_smach_states/src/robot_smach_states/manipulation/grab.py
+++ b/robot_smach_states/src/robot_smach_states/manipulation/grab.py
@@ -63,7 +63,7 @@ def execute(self, userdata=None):
         arm.wait_for_motion_done()
 
         # Open gripper
-        arm.send_gripper_goal('open', timeout=0.0)
+        arm.gripper.send_goal('open', timeout=0.0)
         arm.wait_for_motion_done()
 
         # Make sure the head looks at the entity
@@ -176,7 +176,7 @@ def execute(self, userdata=None):
         #                      ):
         #     rospy.logerr('Pre-grasp failed:')
         #     arm.reset()
-        #     arm.send_gripper_goal('close', timeout=None)
+        #     arm.gripper.send_goal('close', timeout=None)
         #     return 'failed'
 
         # Grasp
@@ -185,13 +185,13 @@ def execute(self, userdata=None):
             self.robot.speech.speak('I am sorry but I cannot move my arm to the object position', block=False)
             rospy.logerr('Grasp failed')
             arm.reset()
-            arm.send_gripper_goal('close', timeout=0.0)
+            arm.gripper.send_goal('close', timeout=0.0)
             return 'failed'
 
         # Close gripper
-        arm.send_gripper_goal('close')
+        arm.gripper.send_goal('close')
 
-        arm.occupied_by = grab_entity
+        arm.gripper.occupied_by = grab_entity
 
         # Lift
         goal_bl = grasp_framestamped.projectToFrame(self.robot.robot_name + "/base_link",
@@ -230,19 +230,19 @@ def execute(self, userdata=None):
         arm.send_joint_goal('carrying_pose', timeout=0.0)
 
         result = 'succeeded'
-        if self._check_occupancy:
+        if self._check_occupancy and hasattr(arm.gripper, 'grasp_sensor'):
             # Check if the object is present in the gripper
-            if arm.object_in_gripper_measurement.is_empty:
+            if arm.gripper.grasp_sensor.object_in_gripper_measurement.is_empty:
                 # If state is empty, grasp has failed
                 result = "failed"
                 rospy.logerr("Gripper is not holding an object")
                 self.robot.speech.speak("Whoops, something went terribly wrong")
-                arm.occupied_by = None  # Set the object the arm is holding to None
+                arm.gripper.occupied_by = None  # Set the object the arm is holding to None
             else:
                 # State is holding, grasp succeeded.
                 # If unknown: sensor not there, assume gripper is holding and hope for the best
                 result = "succeeded"
-                if arm.object_in_gripper_measurement.is_unknown:
+                if arm.gripper.grasp_sensor.object_in_gripper_measurement.is_unknown:
                     rospy.logwarn("GripperMeasurement unknown")
 
         # Reset head
@@ -301,7 +301,7 @@ def execute(self, userdata=None):
         if self._robot.robot_name == "amigo":
             self._robot.torso.reset()  # Move up to make resetting of the arm safer.
         if arm is not None:
-            arm.send_gripper_goal('close')
+            arm.gripper.send_goal('close')
         self._robot.head.reset()  # Sends a goal
         self._robot.head.cancel_goal()  # And cancels it...
         if arm is not None:
diff --git a/robot_smach_states/src/robot_smach_states/manipulation/manipulation.py b/robot_smach_states/src/robot_smach_states/manipulation/manipulation.py
index ca2f5f2dac..684e534507 100644
--- a/robot_smach_states/src/robot_smach_states/manipulation/manipulation.py
+++ b/robot_smach_states/src/robot_smach_states/manipulation/manipulation.py
@@ -62,7 +62,7 @@ def execute(self, userdata=None):
             rospy.logerr("Could not resolve arm")
             return "failed"
 
-        object_in_arm = arm.occupied_by
+        object_in_arm = arm.gripper.occupied_by
         if not object_in_arm:
             rospy.logerr("Unoccupied arm sent to HandOverToHuman, arm should be occupied when entering this state.")
 
@@ -70,7 +70,7 @@ def execute(self, userdata=None):
 
         attempt = 0
 
-        while not arm.handover_to_human(timeout=10) and attempt < 2:
+        while not arm.handover_detector.handover_to_human(timeout=10) and attempt < 2:
             self.robot.speech.speak("Please take it from my gripper.", block=False)
             attempt += 1
 
@@ -78,11 +78,11 @@ def execute(self, userdata=None):
         self.robot.ed.update_entity(id=object_in_arm.id, action='remove')
 
         self.robot.speech.speak("I will open my gripper now.", block=False)
-        arm.send_gripper_goal('open')
+        arm.gripper.send_goal('open')
         arm.reset()
         arm.wait_for_motion_done()
 
-        arm.occupied_by = None
+        arm.gripper.occupied_by = None
 
         return 'succeeded'
 
@@ -213,22 +213,22 @@ def execute(self, userdata=None):
             return "failed"
 
         if self.item_designator:
-            arm.occupied_by = self.item_designator.resolve()
+            arm.gripper.occupied_by = self.item_designator.resolve()
         else:
             if self.item_label != "":
                 handed_entity = EntityInfo(id=self.item_label)
-                arm.occupied_by = handed_entity
+                arm.gripper.occupied_by = handed_entity
             else:
                 rospy.logerr("No grabbed entity designator and no label for dummy entity given")
                 return "failed"
 
-        if arm.handover_to_robot(self.timeout):
-            arm.send_gripper_goal('close')
+        if arm.handover_detector.handover_to_robot(self.timeout):
+            arm.gripper.send_goal('close')
             rospy.sleep(2.0)
             return "succeeded"
         else:
             rospy.logwarn("Eventhough no force is felt the gripper will close")
-            arm.send_gripper_goal('close')
+            arm.gripper.send_goal('close')
             rospy.sleep(2.0)
             return "timeout"
 
@@ -266,9 +266,9 @@ def execute(self, userdata=None):
                 rospy.logerr("Could not resolve {0}".format(self.grab_entity_designator))
 
             if self.gripperstate != GripperState.OPEN:
-                arm.occupied_by = entity
+                arm.gripper.occupied_by = entity
 
-        if arm.send_gripper_goal(self.gripperstate, timeout=self.timeout):
+        if arm.gripper.send_goal(self.gripperstate, timeout=self.timeout):
             return 'succeeded'
         else:
             return 'failed'
diff --git a/robot_smach_states/src/robot_smach_states/manipulation/place.py b/robot_smach_states/src/robot_smach_states/manipulation/place.py
index 73b71ba1b3..50c2fd3c87 100644
--- a/robot_smach_states/src/robot_smach_states/manipulation/place.py
+++ b/robot_smach_states/src/robot_smach_states/manipulation/place.py
@@ -149,18 +149,18 @@ def execute(self, userdata=None):
                              timeout=10, pre_grasp=False):
             rospy.logwarn("Cannot place the object, dropping it...")
 
-        place_entity = arm.occupied_by
+        place_entity = arm.gripper.occupied_by
         if not place_entity:
             rospy.logerr("Arm not holding an entity to place. This should never happen")
         else:
             self._robot.ed.update_entity(place_entity.id, frame_stamped=placement_fs)
-            arm.occupied_by = None
+            arm.gripper.occupied_by = None
 
         # Open gripper
         # Since we cannot reliably wait for the gripper, just set this timeout
-        arm.send_gripper_goal('open', timeout=2.0)
+        arm.gripper.send_goal('open', timeout=2.0)
 
-        arm.occupied_by = None
+        arm.gripper.occupied_by = None
 
         # Retract
         place_pose_bl = placement_fs.projectToFrame(self._robot.robot_name+'/base_link',
@@ -179,7 +179,7 @@ def execute(self, userdata=None):
             arm.cancel_goals()
 
         # Close gripper
-        arm.send_gripper_goal('close', timeout=0.0)
+        arm.gripper.send_goal('close', timeout=0.0)
 
         arm.reset()
         arm.wait_for_motion_done()
diff --git a/robot_smach_states/test/manipulation/test_manipulation.py b/robot_smach_states/test/manipulation/test_manipulation.py
index 72bce9ec39..cd7cd310bd 100644
--- a/robot_smach_states/test/manipulation/test_manipulation.py
+++ b/robot_smach_states/test/manipulation/test_manipulation.py
@@ -34,7 +34,7 @@ def test_handover_to_human(self):
         self.robot.arms["leftArm"].send_joint_goal.assert_any_call('handover_to_human', max_joint_vel=mock.ANY, timeout=mock.ANY)
         self.robot.arms["leftArm"].send_joint_goal.assert_any_call('reset', max_joint_vel=mock.ANY, timeout=mock.ANY)
 
-        self.robot.arms["leftArm"].gripper.send_goal.assert_any_call('open', mock.ANY, max_torque=mock.ANY)
+        self.robot.arms["leftArm"].gripper.send_goal.assert_any_call('open')
         self.assertIsNone(self.robot.arms["leftArm"].gripper.occupied_by)
 
 
@@ -67,8 +67,8 @@ def test_handover_from_human(self):
 
         self.robot.arms["rightArm"].handover_detector.handover_to_robot.assert_called_once()
 
-        self.robot.arms["rightArm"].gripper.send_goal.assert_any_call('open', mock.ANY, max_torque=mock.ANY)
-        self.robot.arms["rightArm"].gripper.send_goal.assert_called_with('close', mock.ANY, max_torque=mock.ANY)
+        self.robot.arms["rightArm"].gripper.send_goal.assert_any_call('open')
+        self.robot.arms["rightArm"].gripper.send_goal.assert_called_with('close')
         self.assertEqual(self.robot.arms["rightArm"].gripper.occupied_by, self.entity)
 
 
@@ -84,7 +84,7 @@ def setUp(self):
     def test_set_open(self):
         state = SetGripper(self.robot, self.arm_ds, 'open')
         self.assertEqual(state.execute(), "succeeded")
-        self.robot.arms["leftArm"].gripper.send_goal.assert_called_once_with('open', mock.ANY, max_torque=mock.ANY)
+        self.robot.arms["leftArm"].gripper.send_goal.assert_called_once_with('open')
 
 
 class TestSetGripperClose(unittest.TestCase):
@@ -101,7 +101,7 @@ def setUp(self):
     def test_set_close(self):
         state = SetGripper(self.robot, self.arm_ds, 'close', self.entity_ds)
         self.assertEqual(state.execute(), "succeeded")
-        self.robot.arms["leftArm"].gripper.send_goal.assert_called_once_with('close', mock.ANY, max_torque=mock.ANY)
+        self.robot.arms["leftArm"].gripper.send_goal.assert_called_once_with('close')
         self.assertEqual(self.robot.arms["leftArm"].gripper.occupied_by, self.entity)