diff --git a/CMakeLists.txt b/CMakeLists.txt
index 7176fa54eee2541f3de2fc2330b6dc1779e6f9b2..b3b288fba595ab0d1f95aef76a0cd4502a9f0b03 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -70,12 +70,20 @@ target_link_libraries(grasp_service ${catkin_LIBRARIES} EnvironmentUtil GraspUti
 
 add_dependencies(grasp_service panda_grasping_generate_messages_cpp)
 
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+
 if(CATKIN_ENABLE_TESTING)
     find_package(rostest REQUIRED)
+    add_rostest_gtest(tests_grasping test/grasping.test test/test.cpp)
+#    add_executable(tests_grasping test/test.cpp)
+    target_link_libraries(tests_grasping ${catkin_LIBRARIES})
+    add_dependencies(tests_grasping panda_grasping_generate_messages_cpp)
     add_rostest_gtest(tests_side_grasping test/side_grasping.test test/grasp_from_side_test.cpp)
     target_link_libraries(tests_side_grasping ${catkin_LIBRARIES} GraspUtil)
     add_dependencies(tests_side_grasping panda_grasping_generate_messages_cpp)
 endif()
 
-
-
diff --git a/include/grasp_util.h b/include/grasp_util.h
index 325a826306b0463eb56c2cda7adad2491e62a7e9..1403f64d74fe93f9fcc654c2b81814167aec4b14 100644
--- a/include/grasp_util.h
+++ b/include/grasp_util.h
@@ -161,6 +161,13 @@ public:
     bool placeFromSide(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &place_pose,
                         double open_amount, std::string supporting_surface_id, std::string object_to_place_id, bool plan_only = false);
 
+    /**
+     * Homes the gripper (in case it was abused before and is in an inconsistent state)
+     * This method is blocking with a maximum runtime of ~30s
+     * @return true if homing succeeded, false otherwise
+     */
+    static bool homeGripper();
+
 private:
 
     /**
diff --git a/src/grasping/environment_util.cpp b/src/grasping/environment_util.cpp
index 54766834b5e4fb2a625ddabc44a907940babeea2..6d8af3d85f7d18691629029c1c537c3379662ceb 100644
--- a/src/grasping/environment_util.cpp
+++ b/src/grasping/environment_util.cpp
@@ -24,10 +24,11 @@ void EnvironmentUtil::constructPlate(std::vector<moveit_msgs::CollisionObject> &
     plate.primitive_poses[0].position.x = 0;
     plate.primitive_poses[0].position.y = 0;
     plate.primitive_poses[0].position.z = -0.1;
+    plate.primitive_poses[0].orientation.w = 1;
 
     plate.operation = plate.ADD;
 
-    collision_objects.push_back(plate);
+//    collision_objects.push_back(plate);
 }
 
 void EnvironmentUtil::constructSupport(std::vector<moveit_msgs::CollisionObject> &collision_objects, std::string id,
@@ -50,6 +51,7 @@ void EnvironmentUtil::constructSupport(std::vector<moveit_msgs::CollisionObject>
     pick_support.primitive_poses[0].position.x = object_to_pick_pose.position.x;
     pick_support.primitive_poses[0].position.y = object_to_pick_pose.position.y;
     pick_support.primitive_poses[0].position.z = (object_to_pick_pose.position.z - (object_z_dimension / 2) - 0.02);
+    pick_support.primitive_poses[0].orientation.w = 1; // TODO align to object
 
     pick_support.operation = pick_support.ADD;
 
diff --git a/src/grasping/grasp_service.cpp b/src/grasping/grasp_service.cpp
index 7e27a2a4de28d4004599d4bd1a48a57efdc080fe..752d8eb1abcbaffe84196cb15220416aab7c2c1e 100644
--- a/src/grasping/grasp_service.cpp
+++ b/src/grasping/grasp_service.cpp
@@ -40,8 +40,6 @@ bool pickObject(panda_grasping::PickObject::Request &req,
 
     if (!grasping_state::object_picked) {
 
-        grasping_state::object_picked = true;
-
         EnvironmentUtil env_util;
         GraspUtil grasp_util;
 
@@ -79,6 +77,7 @@ bool pickObject(panda_grasping::PickObject::Request &req,
             return false;
         }
 
+        grasping_state::object_picked = true;
         res.success = true;
         return true;
     }
diff --git a/src/grasping/grasp_util.cpp b/src/grasping/grasp_util.cpp
index 7d9924561a137645e096641c6a4dbb3cb198b6fe..4698c3dc0902cc0680f1890f57e0987cb2f905f4 100644
--- a/src/grasping/grasp_util.cpp
+++ b/src/grasping/grasp_util.cpp
@@ -3,6 +3,8 @@
 //
 #include "grasp_util.h"
 #include <tf2/transform_datatypes.h>
+#include <franka_gripper/HomingAction.h>
+#include <franka_gripper/HomingGoal.h>
 
 void GraspUtil::setupOpenGripper(trajectory_msgs::JointTrajectory &posture, double amount) {
     posture.joint_names.resize(2);
@@ -333,3 +335,21 @@ GraspUtil::placeFromAbove(moveit::planning_interface::MoveGroupInterface &move_g
     ROS_ERROR_STREAM("Error while executing place task. MoveItErrorCode: " << miec.val);
     return false;
 }
+
+bool GraspUtil::homeGripper() {
+    actionlib::SimpleActionClient<franka_gripper::HomingAction> ac("/franka_gripper/homing", true);
+    if (!ac.waitForServer(ros::Duration(10.0))) {
+        ROS_ERROR("Gripper homing failed (action server not available).");
+        return false;
+    }
+    franka_gripper::HomingGoal goal;
+    ac.sendGoal(goal);
+    if (ac.waitForResult(ros::Duration(20.0))) {
+        actionlib::SimpleClientGoalState state = ac.getState();
+        ROS_INFO_STREAM("Homing finished: " <<  state.toString());
+        return state == actionlib::SimpleClientGoalState::SUCCEEDED;
+    } else {
+        ROS_ERROR("Gripper homing failed (with timeout).");
+        return false;
+    }
+}
diff --git a/test/grasping.test b/test/grasping.test
new file mode 100644
index 0000000000000000000000000000000000000000..5765d69f50606ef7e5bc2165852b9177ca2aced9
--- /dev/null
+++ b/test/grasping.test
@@ -0,0 +1,14 @@
+<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" 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/plan_visualizer.rviz b/test/plan_visualizer.rviz
index 65ff1615893a1b2c90dc9ba320be53f74fd67eb0..2f631356378c3b3dc72a04abff35b9b9d5f1893f 100644
--- a/test/plan_visualizer.rviz
+++ b/test/plan_visualizer.rviz
@@ -283,4 +283,4 @@ Window Geometry:
   QMainWindow State: 000000ff00000000fd00000002000000000000013b000004e1fc0200000003fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670000000080000003640000017200fffffffb0000000800480065006c00700100000334000000e10000000000000000fb0000000a00560069006500770073000000003b000004e1000000000000000000000003000004fe0000003ffc0100000001fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720100000000000004fe0000010000ffffff000004fe000004e100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Width: 1278
   X: 0
-  Y: 18
\ No newline at end of file
+  Y: 18
diff --git a/test/side_grasping.test b/test/side_grasping.test
index 0ba7f0e537d6499492b036b5dbcfe79c214b8795..78444dedfb2f922ab9678e701639f327d69812d9 100644
--- a/test/side_grasping.test
+++ b/test/side_grasping.test
@@ -1,12 +1,5 @@
 <launch>
     <include file="$(find panda_simulation)/launch/simulation.launch"/>
-
-  <!--  <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>-->
-
-
     <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" />
 
     <test test-name="tests_side_grasping" pkg="panda_grasping" type="tests_side_grasping" time-limit="3600.0"/>
diff --git a/test/test.cpp b/test/test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..185ea255f8b1f22361657f0486eb9be61c84435f
--- /dev/null
+++ b/test/test.cpp
@@ -0,0 +1,182 @@
+//
+// Created by Johannes Mey on 22.11.20.
+//
+
+#include "ros/ros.h"
+
+#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
+#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>
+
+class Grasping : public ::testing::Test {
+protected:
+
+    void SetUp() override {
+        ros::NodeHandle n("panda_grasping");
+        pickClient = n.serviceClient<panda_grasping::PickObject>("PickObjectService");
+        placeClient = n.serviceClient<panda_grasping::PlaceObject>("PlaceObjectService");
+
+        // clean up scene
+        CleanupScene();
+    }
+
+
+    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 TearDown() override {}
+
+    ros::ServiceClient pickClient;
+    ros::ServiceClient placeClient;
+
+    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)
+    };
+
+    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};
+
+    std::vector<double> angles{
+            M_PI_2,
+            1,
+            -M_PI_4
+    };
+
+    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);
+
+            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");
+
+            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_F(Grasping, Grasping_WithoutRotation_Object_0_FromTo_01) {
+    auto object = objects[0];
+    pickAndPlace(object, points[0], 0, points[1], 0);
+}
+
+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);
+}
+
+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_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);
+}
+
+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);
+}
+
+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);
+}
+
+int main(int argc, char **argv) {
+    testing::InitGoogleTest(&argc, 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 <<<<<<<<<<<<<<<<");
+
+
+    return RUN_ALL_TESTS();
+}