diff --git a/test/grasping.test b/test/grasping.test
index 4625e3f870f202276c3cdccd1833a05efe8e745d..5765d69f50606ef7e5bc2165852b9177ca2aced9 100644
--- a/test/grasping.test
+++ b/test/grasping.test
@@ -1,12 +1,14 @@
 <launch>
-    <include file="$(find panda_simulation)/launch/simulation.launch"/>
+    <include file="$(find panda_simulation)/launch/simulation.launch">
+        <arg name="slow_simulation" value="false" />
+    </include>
 
     <node name="$(anon rviz)" pkg="rviz" type="rviz" respawn="false"
           args="-d $(find panda_grasping)/test/plan_visualizer.rviz" output="screen">
         <rosparam command="load" file="$(find panda_moveit_config)/config/kinematics.yaml"/>
     </node>
 
-    <node pkg="panda_grasping" type="grasp_service" name="grasp_service" output="screen"/>
+    <node pkg="panda_grasping" type="grasp_service" name="grasp_service" output="screen" respawn="true"/>
 
     <test test-name="tests_grasping" pkg="panda_grasping" type="tests_grasping" time-limit="3600.0"/>
 </launch>
\ No newline at end of file
diff --git a/test/test.cpp b/test/test.cpp
index 44b752c928db8b1ee60372d5d0f60ee8b2380872..185ea255f8b1f22361657f0486eb9be61c84435f 100644
--- a/test/test.cpp
+++ b/test/test.cpp
@@ -1,5 +1,3 @@
-#pragma clang diagnostic push
-#pragma ide diagnostic ignored "cert-err58-cpp"
 //
 // Created by Johannes Mey on 22.11.20.
 //
@@ -10,118 +8,164 @@
 #include "tf2/LinearMath/Quaternion.h"
 #include "tf2/LinearMath/Vector3.h"
 
+#include <moveit/planning_scene_interface/planning_scene_interface.h>
+#include <moveit/move_group_interface/move_group_interface.h>
+
 #include "panda_grasping/PickObject.h"
 #include "panda_grasping/PlaceObject.h"
 
 #include <gtest/gtest.h>
 
-ros::ServiceClient pickClient;
-ros::ServiceClient placeClient;
+class Grasping : public ::testing::Test {
+protected:
 
-std::vector<tf2::Vector3> objects{
-        tf2::Vector3(0.01, 0.1, 0.1),
-        tf2::Vector3(0.05, 0.1, 0.1),
-        tf2::Vector3(0.005, 0.1, 0.1)
-};
+    void SetUp() override {
+        ros::NodeHandle n("panda_grasping");
+        pickClient = n.serviceClient<panda_grasping::PickObject>("PickObjectService");
+        placeClient = n.serviceClient<panda_grasping::PlaceObject>("PlaceObjectService");
 
-std::vector<tf2::Vector3> points{
-        tf2::Vector3(0.5, 0.3, 0.2),
-        tf2::Vector3(-0.5, -0.3, 0.3),
-        tf2::Vector3(0.5, 0.3, 0),
-        tf2::Vector3(0.4, 0.5, 0.4)
-};
+        // clean up scene
+        CleanupScene();
+    }
 
 
-std::vector<double> angles{
-        M_PI_2,
-        1,
-        -M_PI_4
-};
+    static void CleanupScene() {
+        moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
 
+        for (auto p : planning_scene_interface.getObjects()) {
+            p.second.operation = moveit_msgs::CollisionObject::REMOVE;
+            planning_scene_interface.applyCollisionObject(p.second);
+        }
+        ASSERT_EQ(planning_scene_interface.getObjects().size(), 0);
+        ASSERT_EQ(planning_scene_interface.getAttachedObjects().size(), 0);
+    }
 
-void pickAndPlace(tf2::Vector3 size, tf2::Vector3 from, double fromAngle, tf2::Vector3 to, double toAngle) {
-    ROS_INFO_STREAM("starting test");
-    tf2::Quaternion orientation;
-    {
-        orientation.setRPY(0, 0, fromAngle);
+    // void TearDown() override {}
 
-        panda_grasping::PickObject pickObjectSrv;
-        pickObjectSrv.request.dimensions = tf2::toMsg(size);
-        tf2::toMsg(from, pickObjectSrv.request.pose.position);
-        pickObjectSrv.request.pose.orientation = tf2::toMsg(orientation);
+    ros::ServiceClient pickClient;
+    ros::ServiceClient placeClient;
 
-        ROS_INFO_STREAM("starting to pick");
+    std::vector<tf2::Vector3> objects{
+            tf2::Vector3(0.01, 0.1, 0.1),
+            tf2::Vector3(0.05, 0.1, 0.1),
+            tf2::Vector3(0.005, 0.1, 0.1)
+    };
 
-        EXPECT_TRUE(pickClient.exists());
-        EXPECT_TRUE(pickClient.call(pickObjectSrv));
-    }
-    // wait a little while
-    ros::Duration(0.5).sleep();
-    {
-        orientation.setRPY(0, 0, toAngle);
+    std::vector<tf2::Vector3> points{
+            tf2::Vector3(0.5, 0.3, 0.2),
+            tf2::Vector3(-0.5, -0.3, 0.3),
+            tf2::Vector3(0.5, 0.3, 0.1),
+            tf2::Vector3(0.4, 0.5, 0.4),
+
+    };
+
+    tf2::Vector3 nullLocation{-0.4, 0.5, 0};
 
-        panda_grasping::PlaceObject placeObjectSrv;
-        placeObjectSrv.request.dimensions = tf2::toMsg(size);
-        tf2::toMsg(to, placeObjectSrv.request.pose.position);
-        placeObjectSrv.request.pose.orientation = tf2::toMsg(orientation);
+    std::vector<double> angles{
+            M_PI_2,
+            1,
+            -M_PI_4
+    };
 
-        ROS_INFO_STREAM("starting to place");
+    void pickAndPlace(tf2::Vector3 size, tf2::Vector3 from, double fromAngle, tf2::Vector3 to, double toAngle) {
+        // wait a little while
+        ros::Duration(0.5).sleep();
+        ROS_INFO_STREAM("starting test");
+        tf2::Quaternion orientation;
+        {
+            orientation.setRPY(0, 0, fromAngle);
 
-        EXPECT_TRUE(placeClient.exists());
-        EXPECT_TRUE(placeClient.call(placeObjectSrv));
+            panda_grasping::PickObject pickObjectSrv;
+            pickObjectSrv.request.dimensions = tf2::toMsg(size);
+            tf2::toMsg(from, pickObjectSrv.request.pose.position);
+            pickObjectSrv.request.pose.orientation = tf2::toMsg(orientation);
 
+            ROS_INFO_STREAM("starting to pick");
 
-        ROS_INFO_STREAM("pick and place finished");
+            ASSERT_TRUE(pickClient.exists());
+            ASSERT_TRUE(pickClient.call(pickObjectSrv));
+        }
+        {
+            orientation.setRPY(0, 0, toAngle);
+
+            panda_grasping::PlaceObject placeObjectSrv;
+            placeObjectSrv.request.dimensions = tf2::toMsg(size);
+            tf2::toMsg(to, placeObjectSrv.request.pose.position);
+            placeObjectSrv.request.pose.orientation = tf2::toMsg(orientation);
+
+            ROS_INFO_STREAM("starting to place");
+
+            ASSERT_TRUE(placeClient.exists());
+            ASSERT_TRUE(placeClient.call(placeObjectSrv));
+
+
+            ROS_INFO_STREAM("pick and place finished");
+        }
     }
-}
+};
 
 
-TEST(Grasping, Grasping_WithoutRotation_Object_0) {
+TEST_F(Grasping, Grasping_WithoutRotation_Object_0_FromTo_01) {
     auto object = objects[0];
-
     pickAndPlace(object, points[0], 0, points[1], 0);
-    pickAndPlace(object, points[2], 0, points[3], 0);
 }
 
-TEST(Grasping, Grasping_WithoutRotation_Object_1) {
-    auto object = objects[1];
+TEST_F(Grasping, Grasping_WithoutRotation_Object_0_FromTo_Null1) {
+    auto object = objects[0];
+    pickAndPlace(object, nullLocation, 0, points[1], 0);
+}
 
+TEST_F(Grasping, Grasping_WithoutRotation_Object_1_FromTo_01) {
+    auto object = objects[1];
     pickAndPlace(object, points[0], 0, points[1], 0);
-    pickAndPlace(object, points[2], 0, points[3], 0);
 }
 
-TEST(Grasping, Grasping_WithoutRotation_Object_2) {
+TEST_F(Grasping, Grasping_WithoutRotation_Object_2_FromTo_01) {
     auto object = objects[2];
+    pickAndPlace(object, points[0], 0, points[1], 0);
+}
+
+TEST_F(Grasping, Grasping_WithoutRotation_Object_0_FromTo_1Null) {
+    auto object = objects[0];
+    pickAndPlace(object, points[1], 0, nullLocation, 0);
+}
 
+TEST_F(Grasping, Grasping_WithoutRotation_Object_0_FromTo_01_23) {
+    auto object = objects[0];
     pickAndPlace(object, points[0], 0, points[1], 0);
+    CleanupScene();
     pickAndPlace(object, points[2], 0, points[3], 0);
 }
 
-TEST(Grasping, GraspingWithRotation_Object_0_Rotations_00) {
+TEST_F(Grasping, Grasping_WithoutRotation_Object_0_FromTo_01_12) {
+    auto object = objects[0];
+    pickAndPlace(object, points[0], 0, points[1], 0);
+    CleanupScene();
+    pickAndPlace(object, points[1], 0, points[2], 0);
+}
+
+TEST_F(Grasping, GraspingWithRotation_Object_0_Rotations_00) {
     auto object = objects[0];
     auto fromAngle = angles[0];
     auto toAngle = angles[0];
 
     pickAndPlace(object, points[0], fromAngle, points[1], toAngle);
-    pickAndPlace(object, points[2], fromAngle, points[3], toAngle);
 }
 
-TEST(Grasping, GraspingWithRotation_Object_0_Rotations_01) {
+TEST_F(Grasping, GraspingWithRotation_Object_0_Rotations_01) {
     auto object = objects[0];
     auto fromAngle = angles[0];
     auto toAngle = angles[1];
 
     pickAndPlace(object, points[0], fromAngle, points[1], toAngle);
-    pickAndPlace(object, points[2], fromAngle, points[3], toAngle);
 }
 
-TEST(Grasping, GraspingWithRotation_Object_0_Rotations_12) {
+TEST_F(Grasping, GraspingWithRotation_Object_0_Rotations_12) {
     auto object = objects[0];
     auto fromAngle = angles[1];
     auto toAngle = angles[2];
 
     pickAndPlace(object, points[0], fromAngle, points[1], toAngle);
-    pickAndPlace(object, points[2], fromAngle, points[3], toAngle);
 }
 
 int main(int argc, char **argv) {
@@ -129,16 +173,10 @@ int main(int argc, char **argv) {
     ros::init(argc, argv, "grasping_test");
     ros::NodeHandle n("panda_grasping");
 
-
     ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>> WAITING FOR ROBOT INIT <<<<<<<<<<<<<<<");
     ros::Duration(3.0).sleep();
     ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>>> STARTING TEST AFTER INIT <<<<<<<<<<<<<<<<");
 
 
-    pickClient = n.serviceClient<panda_grasping::PickObject>("PickObjectService");
-    placeClient = n.serviceClient<panda_grasping::PlaceObject>("PlaceObjectService");
-
     return RUN_ALL_TESTS();
 }
-
-#pragma clang diagnostic pop
\ No newline at end of file