diff --git a/README.md b/README.md
index 4c495307811198d87d673d0d633d7c2ef9c9e4fa..507bf9538303a30a19e1af4a4b02e02622632034 100644
--- a/README.md
+++ b/README.md
@@ -1,6 +1,7 @@
 # Panda Grasping Package
 
-Allows to pick and place objects. The current maximum height, where the panda can grip (from above) ist 50cm. This package uses the old pick-and-place pipeline of MoveIt.
+Allows to pick and place objects. The current maximum height, where the panda can grip (from above) ist 50cm. This
+package uses the old pick-and-place pipeline of MoveIt.
 
 ## How to install
 
@@ -9,10 +10,13 @@ Allows to pick and place objects. The current maximum height, where the panda ca
 * catkin build & source the workspace
 * that's it
 
-## How to use 
+## How to use
 
 * u can start the configurable pick and place with: ```roslaunch panda_grasping simulation.launch```
     * to pick or place open a second console
-    * to pick type for example: ```rosservice call panda_grasping/PickObjectService '{pose: {position: {x: 0.5, y: 0.0, z: 0.2}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}, dimensions: {x: 0.02, y: 0.02, z: 0.2}}'```
-    * to place type for example ```rosservice call panda_grasping/PlaceObjectService '{pose: {position: {x: 0.0, y: 0.5, z: 0.2}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}, dimensions: {x: 0.02, y: 0.02, z: 0.2}}'```
-    * inside of the message u can update orientation (currenly only z is supported), position and size of the object to pick or place
+    * to pick type for
+      example: ```rosservice call panda_grasping/PickObjectService '{pose: {position: {x: 0.5, y: 0.0, z: 0.2}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}, dimensions: {x: 0.02, y: 0.02, z: 0.2}}'```
+    * to place type for
+      example ```rosservice call panda_grasping/PlaceObjectService '{pose: {position: {x: 0.0, y: 0.5, z: 0.2}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}, dimensions: {x: 0.02, y: 0.02, z: 0.2}}'```
+    * inside of the message u can update orientation (currenly only z is supported), position and size of the object to
+      pick or place
diff --git a/include/environment_util.h b/include/environment_util.h
index 01a6fe5fa06abb75e1f8018dfa131ae81be3e9b5..83918b7142b7572eaf8d19daed1c62bafc136600 100644
--- a/include/environment_util.h
+++ b/include/environment_util.h
@@ -18,42 +18,41 @@
 /**
  * Util for the construction of the scene.
  */
-class EnvironmentUtil {
-
+class EnvironmentUtil
+{
 public:
-
-    /**
-     * Adds a table plate to the planning scene.
-     * @param collision_objects current collision objects in the planning scene
-     * @param x_dimension width of the table
-     * @param y_dimension length of the table
-     */
-    void constructPlate(std::vector<moveit_msgs::CollisionObject> &collision_objects, double x_dimension,
-                        double y_dimension);
-
-    /**
-    * Adds a support surface to the planning scene (with ID "bin").
-    * @param collision_objects current collision objects in the planning scene
-    * @param id id of the support surface to create
-    * @param object_x_dimension x dimension object to be picked
-    * @param object_y_dimension y dimension object to be picked
-    * @param object_z_dimension z dimension object to be picked
-     *@param object_to_pick_pose pose of the object to be picked
-    */
-    void constructSupport(std::vector<moveit_msgs::CollisionObject> &collision_objects, std::string id,
-                          double object_x_dimension, double object_y_dimension, double object_z_dimension,
-                          geometry_msgs::Pose &object_to_pick_pose);
-
-    /**
-     * Adds a object to be picked/placed to the planning scene.
-     * @param collision_objects current collision objects in the planning scene
-     * @param id id of the object to create
-     * @param pose pose of the object to be picked
-     * @param dimensions dimensions of the object to be picked
-     */
-    moveit_msgs::CollisionObject
-    constructObjectToPick(std::vector<moveit_msgs::CollisionObject> &collision_objects, std::string id,
-                          geometry_msgs::Pose &pose, geometry_msgs::Vector3 &dimensions);
+  /**
+   * Adds a table plate to the planning scene.
+   * @param collision_objects current collision objects in the planning scene
+   * @param x_dimension width of the table
+   * @param y_dimension length of the table
+   */
+  void constructPlate(std::vector<moveit_msgs::CollisionObject>& collision_objects, double x_dimension,
+                      double y_dimension);
+
+  /**
+   * Adds a support surface to the planning scene (with ID "bin").
+   * @param collision_objects current collision objects in the planning scene
+   * @param id id of the support surface to create
+   * @param object_x_dimension x dimension object to be picked
+   * @param object_y_dimension y dimension object to be picked
+   * @param object_z_dimension z dimension object to be picked
+   *@param object_to_pick_pose pose of the object to be picked
+   */
+  void constructSupport(std::vector<moveit_msgs::CollisionObject>& collision_objects, std::string id,
+                        double object_x_dimension, double object_y_dimension, double object_z_dimension,
+                        geometry_msgs::Pose& object_to_pick_pose);
+
+  /**
+   * Adds a object to be picked/placed to the planning scene.
+   * @param collision_objects current collision objects in the planning scene
+   * @param id id of the object to create
+   * @param pose pose of the object to be picked
+   * @param dimensions dimensions of the object to be picked
+   */
+  moveit_msgs::CollisionObject constructObjectToPick(std::vector<moveit_msgs::CollisionObject>& collision_objects,
+                                                     std::string id, geometry_msgs::Pose& pose,
+                                                     geometry_msgs::Vector3& dimensions);
 };
 
-#endif //PANDA_GRASPING_ENVIRONMENT_UTIL_H
+#endif  // PANDA_GRASPING_ENVIRONMENT_UTIL_H
diff --git a/include/franka_gripper_util.h b/include/franka_gripper_util.h
index b0b8ecc2d2cc2cda51f8e2ed742541fab024108c..ef2e06e583951dcdff73defd9b7f53b72edcb5d8 100644
--- a/include/franka_gripper_util.h
+++ b/include/franka_gripper_util.h
@@ -5,16 +5,13 @@
 #ifndef PUBLIC_FRANKA_GRIPPER_UTIL_H
 #define PUBLIC_FRANKA_GRIPPER_UTIL_H
 
-
-class FrankaGripperUtil {
-
+class FrankaGripperUtil
+{
 public:
-    /**
-    * resets the real robot, to allow correct future movements and grasps
-    */
-    void resetGripperForNextAction();
-
+  /**
+   * resets the real robot, to allow correct future movements and grasps
+   */
+  void resetGripperForNextAction();
 };
 
-
-#endif //PUBLIC_FRANKA_GRIPPER_UTIL_H
+#endif  // PUBLIC_FRANKA_GRIPPER_UTIL_H
diff --git a/include/grasp_util.h b/include/grasp_util.h
index 66bdb7f373f7a92aaa51e3f2490c0ea6aa3eda33..0e125b00436a6d9d4a853e573506ffec1d675726 100644
--- a/include/grasp_util.h
+++ b/include/grasp_util.h
@@ -18,85 +18,84 @@
 /**
  * Util for grasping.
  */
-class GraspUtil {
-
+class GraspUtil
+{
 public:
-
-    /**
-     * distance of the center of the hand to the center of the finger tip
-     */
-    static constexpr tf2Scalar FRANKA_GRIPPER_DISTANCE{0.1034};
-
-    /**
-     * length of a finger
-     */
-    static constexpr tf2Scalar FRANKA_GRIPPER_FINGER_LENGTH{0.054};
-
-    /**
-     * size of the finger tip
-     */
-    static constexpr tf2Scalar FRANKA_GRIPPER_FINGERTIP_SIZE{0.018};
-
-    /**
-     * Sets up how far the gripper should be opened for the next action.
-     * @param posture empty posture object defining grasp
-     * @param amount total opening amount (space between "forks")
-     */
-    void setupOpenGripper(trajectory_msgs::JointTrajectory &posture, double amount);
-
-    /**
-    * Sets up how much the gripper should be closed for the next action.
-    * @param posture empty posture object defining grasp
-    * @param amount total close amount (space between "forks")
-    */
-    void setupClosedGripper(trajectory_msgs::JointTrajectory &posture, double amount);
-
-    /**
-     * Compute the "pre-pick"-pose
-     * @param object_to_pick_pose pose of the object to pick (currently only vertically oriented objects are supported)
-     * @param dimensions of the object
-     * @param gripper_orientation current gripper orientation relative to the link it is attached to
-     * @param reach specifies how far the gripper reaches around the object (0 means the center of the finger tip is at the object's corner)
-     * @return the "pre-pick"-pose
-     */
-    geometry_msgs::Pose
-    getPickFromAbovePose(geometry_msgs::Pose &object_to_pick_pose, geometry_msgs::Vector3 dimensions,
-                         geometry_msgs::Quaternion gripper_orientation,
-                         double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2);
-
-    /**
-     * Util method to pick an object from above.
-     * @param move_group an initialized MoveGroupInterface instance
-     * @param pick_pose via getPickFromAbovePose-method computed pre-pick-pose
-     * @param close_amount total close amount (space between "forks")
-     * @param open_amount total opening amount (space between "forks")
-     * @param supporting_surface_id ID of surface where object is picked from (e.g. the table)
-     * @param object_to_pick_id ID of the object to pick
-     * @return true if successfully picked
-     */
-    bool pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &pick_pose,
-                       geometry_msgs::Vector3 dimensions, double open_amount, std::string supporting_surface_id,
-                       std::string object_to_pick_id, bool plan_only = false);
-
-    /**
-     * Util method to place an object from above.
-     * @param move_group an initialized MoveGroupInterface instance
-     * @param place_pose target place pose for the object
-     * @param close_amount total close amount (space between "forks")
-     * @param open_amount total opening amount (space between "forks")
-     * @param supporting_surface_id ID of surface where object is placed on (e.g. the table)
-     * @param object_to_place_id ID of the object to place
-     */
-    bool placeFromAbove(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();
-
+  /**
+   * distance of the center of the hand to the center of the finger tip
+   */
+  static constexpr tf2Scalar FRANKA_GRIPPER_DISTANCE{ 0.1034 };
+
+  /**
+   * length of a finger
+   */
+  static constexpr tf2Scalar FRANKA_GRIPPER_FINGER_LENGTH{ 0.054 };
+
+  /**
+   * size of the finger tip
+   */
+  static constexpr tf2Scalar FRANKA_GRIPPER_FINGERTIP_SIZE{ 0.018 };
+
+  /**
+   * Sets up how far the gripper should be opened for the next action.
+   * @param posture empty posture object defining grasp
+   * @param amount total opening amount (space between "forks")
+   */
+  void setupOpenGripper(trajectory_msgs::JointTrajectory& posture, double amount);
+
+  /**
+   * Sets up how much the gripper should be closed for the next action.
+   * @param posture empty posture object defining grasp
+   * @param amount total close amount (space between "forks")
+   */
+  void setupClosedGripper(trajectory_msgs::JointTrajectory& posture, double amount);
+
+  /**
+   * Compute the "pre-pick"-pose
+   * @param object_to_pick_pose pose of the object to pick (currently only vertically oriented objects are supported)
+   * @param dimensions of the object
+   * @param gripper_orientation current gripper orientation relative to the link it is attached to
+   * @param reach specifies how far the gripper reaches around the object (0 means the center of the finger tip is at
+   * the object's corner)
+   * @return the "pre-pick"-pose
+   */
+  geometry_msgs::Pose getPickFromAbovePose(geometry_msgs::Pose& object_to_pick_pose, geometry_msgs::Vector3 dimensions,
+                                           geometry_msgs::Quaternion gripper_orientation,
+                                           double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2);
+
+  /**
+   * Util method to pick an object from above.
+   * @param move_group an initialized MoveGroupInterface instance
+   * @param pick_pose via getPickFromAbovePose-method computed pre-pick-pose
+   * @param close_amount total close amount (space between "forks")
+   * @param open_amount total opening amount (space between "forks")
+   * @param supporting_surface_id ID of surface where object is picked from (e.g. the table)
+   * @param object_to_pick_id ID of the object to pick
+   * @return true if successfully picked
+   */
+  bool pickFromAbove(moveit::planning_interface::MoveGroupInterface& move_group, geometry_msgs::Pose& pick_pose,
+                     geometry_msgs::Vector3 dimensions, double open_amount, std::string supporting_surface_id,
+                     std::string object_to_pick_id, bool plan_only = false);
+
+  /**
+   * Util method to place an object from above.
+   * @param move_group an initialized MoveGroupInterface instance
+   * @param place_pose target place pose for the object
+   * @param close_amount total close amount (space between "forks")
+   * @param open_amount total opening amount (space between "forks")
+   * @param supporting_surface_id ID of surface where object is placed on (e.g. the table)
+   * @param object_to_place_id ID of the object to place
+   */
+  bool placeFromAbove(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();
 };
 
-#endif //PANDA_GRASPING_GRASP_UTIL_H
+#endif  // PANDA_GRASPING_GRASP_UTIL_H
diff --git a/launch/simulation.launch b/launch/simulation.launch
index 6ca38627f1a1b666f0ee7bc232a97d402cdeb0bf..f8f48409915161a59f4fc698d648ba22b3677cc2 100644
--- a/launch/simulation.launch
+++ b/launch/simulation.launch
@@ -1,10 +1,10 @@
 <launch>
     <include file="$(find panda_simulation)/launch/simulation.launch"/>
 
-    <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" />
+    <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch"/>
     <!--<node name="rqt_console" pkg="rqt_console" type="rqt_console" />-->
 
-    <node pkg="panda_grasping" type="grasp_service" name="grasp_service" output="screen" />
+    <node pkg="panda_grasping" type="grasp_service" name="grasp_service" output="screen"/>
 
     <!-- sample commands -->
     <!-- rosservice call panda_grasping/PickObjectService '{pose: {position: {x: 0.5, y: 0.0, z: 0.2}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}, dimensions: {x: 0.02, y: 0.02, z: 0.2}}' -->
diff --git a/package.xml b/package.xml
index 94db6d09fcecffeab978784e6d6c1d115ae1e851..0ddc77e605dd980cf184db16b654a077bed4148b 100644
--- a/package.xml
+++ b/package.xml
@@ -1,44 +1,44 @@
 <?xml version="1.0"?>
 <package format="2">
-  <name>panda_grasping</name>
-  <version>0.1.0</version>
-  <description>The panda_grasping package</description>
+    <name>panda_grasping</name>
+    <version>0.1.0</version>
+    <description>The panda_grasping package</description>
 
-  <!-- One maintainer tag required, multiple allowed, one person per tag -->
-  <!-- Example:  -->
-  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
-  <maintainer email="sebastian@todo.todo">sebastian</maintainer>
+    <!-- One maintainer tag required, multiple allowed, one person per tag -->
+    <!-- Example:  -->
+    <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+    <maintainer email="sebastian@todo.todo">sebastian</maintainer>
 
 
-  <!-- One license tag required, multiple allowed, one license per tag -->
-  <!-- Commonly used license strings: -->
-  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
-  <license>TODO</license>
+    <!-- One license tag required, multiple allowed, one license per tag -->
+    <!-- Commonly used license strings: -->
+    <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+    <license>TODO</license>
 
-  <buildtool_depend>catkin</buildtool_depend>
+    <buildtool_depend>catkin</buildtool_depend>
 
-  <build_depend>message_generation</build_depend>
-  <build_depend>roscpp</build_depend>
-  <build_depend>std_msgs</build_depend>
+    <build_depend>message_generation</build_depend>
+    <build_depend>roscpp</build_depend>
+    <build_depend>std_msgs</build_depend>
 
-  <build_export_depend>roscpp</build_export_depend>
-  <build_export_depend>std_msgs</build_export_depend>
+    <build_export_depend>roscpp</build_export_depend>
+    <build_export_depend>std_msgs</build_export_depend>
 
-  <depend>moveit_core</depend>
-  <depend>moveit_ros_planning_interface</depend>
-  <depend>moveit_visual_tools</depend>
-  <depend>robot_state_publisher</depend>
-  <depend>tf2</depend>
-  <depend>franka_gripper</depend>
+    <depend>moveit_core</depend>
+    <depend>moveit_ros_planning_interface</depend>
+    <depend>moveit_visual_tools</depend>
+    <depend>robot_state_publisher</depend>
+    <depend>tf2</depend>
+    <depend>franka_gripper</depend>
 
-  <exec_depend>message_runtime</exec_depend>
-  <exec_depend>roscpp</exec_depend>
-  <exec_depend>std_msgs</exec_depend>
-  <exec_depend>pluginlib</exec_depend>
+    <exec_depend>message_runtime</exec_depend>
+    <exec_depend>roscpp</exec_depend>
+    <exec_depend>std_msgs</exec_depend>
+    <exec_depend>pluginlib</exec_depend>
 
-  <!-- The export tag contains other, unspecified, tags -->
-  <export>
-    <!-- Other tools can request additional information be placed here -->
+    <!-- The export tag contains other, unspecified, tags -->
+    <export>
+        <!-- Other tools can request additional information be placed here -->
 
-  </export>
+    </export>
 </package>
diff --git a/src/grasping/environment_util.cpp b/src/grasping/environment_util.cpp
index 54766834b5e4fb2a625ddabc44a907940babeea2..860f6160dc85e6a87a181f02e0786f100eab4d1e 100644
--- a/src/grasping/environment_util.cpp
+++ b/src/grasping/environment_util.cpp
@@ -5,80 +5,80 @@
 
 #include "environment_util.h"
 
-void EnvironmentUtil::constructPlate(std::vector<moveit_msgs::CollisionObject> &collision_objects, double x_dimension,
-                                     double y_dimension) {
+void EnvironmentUtil::constructPlate(std::vector<moveit_msgs::CollisionObject>& collision_objects, double x_dimension,
+                                     double y_dimension)
+{
+  moveit_msgs::CollisionObject plate;
 
-    moveit_msgs::CollisionObject plate;
+  plate.header.frame_id = "panda_link0";
+  plate.id = "plate";
 
-    plate.header.frame_id = "panda_link0";
-    plate.id = "plate";
+  plate.primitives.resize(1);
+  plate.primitives[0].type = shape_msgs::SolidPrimitive::BOX;
+  plate.primitives[0].dimensions.resize(3);
+  plate.primitives[0].dimensions[0] = x_dimension;
+  plate.primitives[0].dimensions[1] = y_dimension;
+  plate.primitives[0].dimensions[2] = 0.1;
 
-    plate.primitives.resize(1);
-    plate.primitives[0].type = shape_msgs::SolidPrimitive::BOX;
-    plate.primitives[0].dimensions.resize(3);
-    plate.primitives[0].dimensions[0] = x_dimension;
-    plate.primitives[0].dimensions[1] = y_dimension;
-    plate.primitives[0].dimensions[2] = 0.1;
+  plate.primitive_poses.resize(1);
+  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.resize(1);
-    plate.primitive_poses[0].position.x = 0;
-    plate.primitive_poses[0].position.y = 0;
-    plate.primitive_poses[0].position.z = -0.1;
+  plate.operation = plate.ADD;
 
-    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,
+void EnvironmentUtil::constructSupport(std::vector<moveit_msgs::CollisionObject>& collision_objects, std::string id,
                                        double object_x_dimension, double object_y_dimension, double object_z_dimension,
-                                       geometry_msgs::Pose &object_to_pick_pose) {
-
-    moveit_msgs::CollisionObject pick_support;
+                                       geometry_msgs::Pose& object_to_pick_pose)
+{
+  moveit_msgs::CollisionObject pick_support;
 
-    pick_support.header.frame_id = "panda_link0";
-    pick_support.id = id;
+  pick_support.header.frame_id = "panda_link0";
+  pick_support.id = id;
 
-    pick_support.primitives.resize(1);
-    pick_support.primitives[0].type = shape_msgs::SolidPrimitive::BOX;
-    pick_support.primitives[0].dimensions.resize(3);
-    pick_support.primitives[0].dimensions[0] = object_x_dimension;
-    pick_support.primitives[0].dimensions[1] = object_y_dimension;
-    pick_support.primitives[0].dimensions[2] = 0.04;
+  pick_support.primitives.resize(1);
+  pick_support.primitives[0].type = shape_msgs::SolidPrimitive::BOX;
+  pick_support.primitives[0].dimensions.resize(3);
+  pick_support.primitives[0].dimensions[0] = object_x_dimension;
+  pick_support.primitives[0].dimensions[1] = object_y_dimension;
+  pick_support.primitives[0].dimensions[2] = 0.04;
 
-    pick_support.primitive_poses.resize(1);
-    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.resize(1);
+  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.operation = pick_support.ADD;
+  pick_support.operation = pick_support.ADD;
 
-    collision_objects.push_back(pick_support);
+  collision_objects.push_back(pick_support);
 }
 
 moveit_msgs::CollisionObject
-EnvironmentUtil::constructObjectToPick(std::vector<moveit_msgs::CollisionObject> &collision_objects,
-                                       std::string id, geometry_msgs::Pose &pose, geometry_msgs::Vector3 &dimensions) {
-
-    moveit_msgs::CollisionObject object_to_pick;
+EnvironmentUtil::constructObjectToPick(std::vector<moveit_msgs::CollisionObject>& collision_objects, std::string id,
+                                       geometry_msgs::Pose& pose, geometry_msgs::Vector3& dimensions)
+{
+  moveit_msgs::CollisionObject object_to_pick;
 
-    object_to_pick.header.frame_id = "panda_link0";
-    object_to_pick.id = id;
+  object_to_pick.header.frame_id = "panda_link0";
+  object_to_pick.id = id;
 
-    object_to_pick.primitives.resize(1);
-    object_to_pick.primitives[0].type = shape_msgs::SolidPrimitive::BOX;
-    object_to_pick.primitives[0].dimensions.resize(3);
-    object_to_pick.primitives[0].dimensions[0] = dimensions.x;
-    object_to_pick.primitives[0].dimensions[1] = dimensions.y;
-    object_to_pick.primitives[0].dimensions[2] = dimensions.z;
+  object_to_pick.primitives.resize(1);
+  object_to_pick.primitives[0].type = shape_msgs::SolidPrimitive::BOX;
+  object_to_pick.primitives[0].dimensions.resize(3);
+  object_to_pick.primitives[0].dimensions[0] = dimensions.x;
+  object_to_pick.primitives[0].dimensions[1] = dimensions.y;
+  object_to_pick.primitives[0].dimensions[2] = dimensions.z;
 
-    object_to_pick.primitive_poses.resize(1);
-    object_to_pick.primitive_poses[0].position = pose.position;
-    object_to_pick.primitive_poses[0].orientation = pose.orientation;
+  object_to_pick.primitive_poses.resize(1);
+  object_to_pick.primitive_poses[0].position = pose.position;
+  object_to_pick.primitive_poses[0].orientation = pose.orientation;
 
-    object_to_pick.operation = object_to_pick.ADD;
+  object_to_pick.operation = object_to_pick.ADD;
 
-    collision_objects.push_back(object_to_pick);
+  collision_objects.push_back(object_to_pick);
 
-    return object_to_pick;
+  return object_to_pick;
 }
\ No newline at end of file
diff --git a/src/grasping/franka_gripper_util.cpp b/src/grasping/franka_gripper_util.cpp
index 66f1e2240705126dfb0d50cd821f4a0539e12559..46484f2e31bef3219b2d2ccc4d36993b6b594e90 100644
--- a/src/grasping/franka_gripper_util.cpp
+++ b/src/grasping/franka_gripper_util.cpp
@@ -10,26 +10,29 @@
 // Franka
 #include <franka_gripper/MoveAction.h>
 
-void FrankaGripperUtil::resetGripperForNextAction() {
-
-    actionlib::SimpleActionClient<franka_gripper::MoveAction> ac("franka_gripper/move", true);
-
-    ROS_INFO("Waiting for reset action server to start.");
-    ac.waitForServer();
-
-    ROS_INFO("Reset action server started, sending goal.");
-    franka_gripper::MoveGoal goal;
-    goal.speed = 0.08;
-    goal.width = 0.0;
-    ac.sendGoal(goal);
-
-    //wait for the action to return
-    bool finished_before_timeout = ac.waitForResult(ros::Duration(10.0));
-
-    if (finished_before_timeout) {
-        actionlib::SimpleClientGoalState state = ac.getState();
-        ROS_INFO("Reset action finished: %s", state.toString().c_str());
-    } else {
-        ROS_INFO("Reset action did not finish before the time out.");
-    }
+void FrankaGripperUtil::resetGripperForNextAction()
+{
+  actionlib::SimpleActionClient<franka_gripper::MoveAction> ac("franka_gripper/move", true);
+
+  ROS_INFO("Waiting for reset action server to start.");
+  ac.waitForServer();
+
+  ROS_INFO("Reset action server started, sending goal.");
+  franka_gripper::MoveGoal goal;
+  goal.speed = 0.08;
+  goal.width = 0.0;
+  ac.sendGoal(goal);
+
+  // wait for the action to return
+  bool finished_before_timeout = ac.waitForResult(ros::Duration(10.0));
+
+  if (finished_before_timeout)
+  {
+    actionlib::SimpleClientGoalState state = ac.getState();
+    ROS_INFO("Reset action finished: %s", state.toString().c_str());
+  }
+  else
+  {
+    ROS_INFO("Reset action did not finish before the time out.");
+  }
 }
diff --git a/src/grasping/grasp_service.cpp b/src/grasping/grasp_service.cpp
index 7e27a2a4de28d4004599d4bd1a48a57efdc080fe..62b2eaae96bbe83536146bc8971c5c5171c0e806 100644
--- a/src/grasping/grasp_service.cpp
+++ b/src/grasping/grasp_service.cpp
@@ -16,8 +16,9 @@
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #include <tf2_ros/transform_listener.h>
 
-namespace grasping_state {
-    bool object_picked = false;
+namespace grasping_state
+{
+bool object_picked = false;
 }
 
 /**
@@ -33,59 +34,60 @@ tf2_ros::Buffer tfBuffer;
  * @param group a initialized MoveGroupInterface instance
  * @param n a valid nodehandle
  */
-bool pickObject(panda_grasping::PickObject::Request &req,
-                panda_grasping::PickObject::Response &res, ros::NodeHandle &n,
-                moveit::planning_interface::PlanningSceneInterface &planning_scene_interface,
-                moveit::planning_interface::MoveGroupInterface &group) {
-
-    if (!grasping_state::object_picked) {
-
-        grasping_state::object_picked = true;
-
-        EnvironmentUtil env_util;
-        GraspUtil grasp_util;
-
-        std::vector<moveit_msgs::CollisionObject> collision_objects;
-
-        ROS_INFO("Adding plate.");
-        env_util.constructPlate(collision_objects, 3.0, 3.0);
-
-        ROS_INFO("Adding pick support.");
-        env_util.constructSupport(collision_objects, "pick_support", req.dimensions.x, req.dimensions.y,
-                                  req.dimensions.z,
-                                  req.pose);
-
-        ROS_INFO("Adding object.");
-        moveit_msgs::CollisionObject po = env_util.constructObjectToPick(collision_objects, "pick_object", req.pose,
-                                                                         req.dimensions);
-
-        ROS_INFO("Applying to scene.");
-        if (!planning_scene_interface.applyCollisionObjects(collision_objects)) {
-            res.success = false;
-            return false;
-        }
-
-        ROS_INFO("Retrieving the orientation of the gripper wrt. to the hand.");
-        geometry_msgs::Transform gripper_transform = tfBuffer.lookupTransform("panda_hand", "panda_link8",
-                                                                                     ros::Time(0)).transform;
+bool pickObject(panda_grasping::PickObject::Request& req, panda_grasping::PickObject::Response& res, ros::NodeHandle& n,
+                moveit::planning_interface::PlanningSceneInterface& planning_scene_interface,
+                moveit::planning_interface::MoveGroupInterface& group)
+{
+  if (!grasping_state::object_picked)
+  {
+    grasping_state::object_picked = true;
+
+    EnvironmentUtil env_util;
+    GraspUtil grasp_util;
+
+    std::vector<moveit_msgs::CollisionObject> collision_objects;
+
+    ROS_INFO("Adding plate.");
+    env_util.constructPlate(collision_objects, 3.0, 3.0);
+
+    ROS_INFO("Adding pick support.");
+    env_util.constructSupport(collision_objects, "pick_support", req.dimensions.x, req.dimensions.y, req.dimensions.z,
+                              req.pose);
+
+    ROS_INFO("Adding object.");
+    moveit_msgs::CollisionObject po =
+        env_util.constructObjectToPick(collision_objects, "pick_object", req.pose, req.dimensions);
+
+    ROS_INFO("Applying to scene.");
+    if (!planning_scene_interface.applyCollisionObjects(collision_objects))
+    {
+      res.success = false;
+      return false;
+    }
 
-        ROS_INFO("Computing pre-grasp-pose.");
-        geometry_msgs::Pose pick_pose = grasp_util.getPickFromAbovePose(req.pose, req.dimensions, gripper_transform.rotation);
+    ROS_INFO("Retrieving the orientation of the gripper wrt. to the hand.");
+    geometry_msgs::Transform gripper_transform =
+        tfBuffer.lookupTransform("panda_hand", "panda_link8", ros::Time(0)).transform;
 
-        ROS_INFO("Picking up ...");
-        // 0.08 = 8cm = maximum gripper opening
-        if (!grasp_util.pickFromAbove(group, pick_pose, req.dimensions, 0.08, "pick_support", "pick_object", req.plan_only)) {
-            res.success = false;
-            return false;
-        }
+    ROS_INFO("Computing pre-grasp-pose.");
+    geometry_msgs::Pose pick_pose =
+        grasp_util.getPickFromAbovePose(req.pose, req.dimensions, gripper_transform.rotation);
 
-        res.success = true;
-        return true;
+    ROS_INFO("Picking up ...");
+    // 0.08 = 8cm = maximum gripper opening
+    if (!grasp_util.pickFromAbove(group, pick_pose, req.dimensions, 0.08, "pick_support", "pick_object", req.plan_only))
+    {
+      res.success = false;
+      return false;
     }
 
-    ROS_ERROR("Error: Cannot pick object. An other object is already picked.");
-    res.success = false;
-    return false;
+    res.success = true;
+    return true;
+  }
+
+  ROS_ERROR("Error: Cannot pick object. An other object is already picked.");
+  res.success = false;
+  return false;
 }
 
 /**
@@ -96,70 +98,70 @@ bool pickObject(panda_grasping::PickObject::Request &req,
  * @param group a initialized MoveGroupInterface instance
  * @param n a valid nodehandle
  */
-bool
-placeObject(panda_grasping::PlaceObject::Request &req, panda_grasping::PlaceObject::Response &res, ros::NodeHandle &n,
-            moveit::planning_interface::PlanningSceneInterface &planning_scene_interface,
-            moveit::planning_interface::MoveGroupInterface &group) {
-
-    ROS_INFO_STREAM("received new place-request: " << req);
-
-    if (grasping_state::object_picked) {
-        EnvironmentUtil env_util;
-        GraspUtil grasp_util;
-
-        std::vector<moveit_msgs::CollisionObject> collision_objects;
-
-        ROS_INFO("Adding place support.");
-        env_util.constructSupport(collision_objects, "place_support", req.dimensions.x, req.dimensions.y,
-                                  req.dimensions.z,
-                                  req.pose);
-
-        ROS_INFO("Applying to scene.");
-        if (!planning_scene_interface.applyCollisionObjects(collision_objects)) {
-            res.success = false;
-            return false;
-        }
-
-        ROS_INFO("Placing object.");
-        if (!grasp_util.placeFromAbove(group, req.pose, 0.08, "place_support", "pick_object", req.plan_only)) {
-            res.success = false;
-            return false;
-        }
-
-        grasping_state::object_picked = false;
-        res.success = true;
-        return true;
+bool placeObject(panda_grasping::PlaceObject::Request& req, panda_grasping::PlaceObject::Response& res,
+                 ros::NodeHandle& n, moveit::planning_interface::PlanningSceneInterface& planning_scene_interface,
+                 moveit::planning_interface::MoveGroupInterface& group)
+{
+  ROS_INFO_STREAM("received new place-request: " << req);
+
+  if (grasping_state::object_picked)
+  {
+    EnvironmentUtil env_util;
+    GraspUtil grasp_util;
+
+    std::vector<moveit_msgs::CollisionObject> collision_objects;
+
+    ROS_INFO("Adding place support.");
+    env_util.constructSupport(collision_objects, "place_support", req.dimensions.x, req.dimensions.y, req.dimensions.z,
+                              req.pose);
+
+    ROS_INFO("Applying to scene.");
+    if (!planning_scene_interface.applyCollisionObjects(collision_objects))
+    {
+      res.success = false;
+      return false;
     }
 
-    ROS_ERROR("Error: Could not place object. Reason: There is currently no object attached to the gripper.");
-    res.success = false;
-    return false;
-}
-
+    ROS_INFO("Placing object.");
+    if (!grasp_util.placeFromAbove(group, req.pose, 0.08, "place_support", "pick_object", req.plan_only))
+    {
+      res.success = false;
+      return false;
+    }
 
-int main(int argc, char **argv) {
+    grasping_state::object_picked = false;
+    res.success = true;
+    return true;
+  }
 
-    ros::init(argc, argv, "grasp_service");
-    ros::NodeHandle n("panda_grasping");
-    ros::AsyncSpinner spinner(2);
-    spinner.start();
+  ROS_ERROR("Error: Could not place object. Reason: There is currently no object attached to the gripper.");
+  res.success = false;
+  return false;
+}
 
-    tf2_ros::TransformListener tfListener(tfBuffer);
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "grasp_service");
+  ros::NodeHandle n("panda_grasping");
+  ros::AsyncSpinner spinner(2);
+  spinner.start();
 
-    moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
-    moveit::planning_interface::MoveGroupInterface group("panda_arm");
+  tf2_ros::TransformListener tfListener(tfBuffer);
 
-    ros::ServiceServer pick_object_service = n.advertiseService<panda_grasping::PickObject::Request, panda_grasping::PickObject::Response>(
-            "PickObjectService",
-            boost::bind(&pickObject, _1, _2, boost::ref(n), boost::ref(planning_scene_interface), boost::ref(group)));
+  moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
+  moveit::planning_interface::MoveGroupInterface group("panda_arm");
 
+  ros::ServiceServer pick_object_service =
+      n.advertiseService<panda_grasping::PickObject::Request, panda_grasping::PickObject::Response>(
+          "PickObjectService",
+          boost::bind(&pickObject, _1, _2, boost::ref(n), boost::ref(planning_scene_interface), boost::ref(group)));
 
-    ros::ServiceServer place_object_service = n.advertiseService<panda_grasping::PlaceObject::Request, panda_grasping::PlaceObject::Response>(
-            "PlaceObjectService",
-            boost::bind(&placeObject, _1, _2, boost::ref(n), boost::ref(planning_scene_interface), boost::ref(group)));
+  ros::ServiceServer place_object_service =
+      n.advertiseService<panda_grasping::PlaceObject::Request, panda_grasping::PlaceObject::Response>(
+          "PlaceObjectService",
+          boost::bind(&placeObject, _1, _2, boost::ref(n), boost::ref(planning_scene_interface), boost::ref(group)));
 
-    ros::waitForShutdown();
+  ros::waitForShutdown();
 
-    return 0;
+  return 0;
 }
-
diff --git a/src/grasping/grasp_util.cpp b/src/grasping/grasp_util.cpp
index b89d05d4c7a49a20168f8ec3e5da4becd9aab221..d3e5bbcc6a2286fff38944f39753f6705462104e 100644
--- a/src/grasping/grasp_util.cpp
+++ b/src/grasping/grasp_util.cpp
@@ -7,182 +7,198 @@
 #include <franka_gripper/MoveAction.h>
 #include <franka_gripper/HomingGoal.h>
 
-void GraspUtil::setupOpenGripper(trajectory_msgs::JointTrajectory &posture, double amount) {
-    posture.joint_names.resize(2);
-    posture.joint_names[0] = "panda_finger_joint1";
-    posture.joint_names[1] = "panda_finger_joint2";
-
-    posture.points.resize(1);
-    posture.points[0].positions.resize(2);
-    posture.points[0].positions[0] = amount / 2;
-    posture.points[0].positions[1] = amount / 2;
-    posture.points[0].time_from_start = ros::Duration(0.5);
+void GraspUtil::setupOpenGripper(trajectory_msgs::JointTrajectory& posture, double amount)
+{
+  posture.joint_names.resize(2);
+  posture.joint_names[0] = "panda_finger_joint1";
+  posture.joint_names[1] = "panda_finger_joint2";
+
+  posture.points.resize(1);
+  posture.points[0].positions.resize(2);
+  posture.points[0].positions[0] = amount / 2;
+  posture.points[0].positions[1] = amount / 2;
+  posture.points[0].time_from_start = ros::Duration(0.5);
 }
 
-void GraspUtil::setupClosedGripper(trajectory_msgs::JointTrajectory &posture, double amount) {
-    posture.joint_names.resize(2);
-    posture.joint_names[0] = "panda_finger_joint1";
-    posture.joint_names[1] = "panda_finger_joint2";
-
-    posture.points.resize(1);
-    posture.points[0].positions.resize(2);
-    posture.points[0].positions[0] = amount / 2;
-    posture.points[0].positions[1] = amount / 2;
-    posture.points[0].time_from_start = ros::Duration(0.5);
+void GraspUtil::setupClosedGripper(trajectory_msgs::JointTrajectory& posture, double amount)
+{
+  posture.joint_names.resize(2);
+  posture.joint_names[0] = "panda_finger_joint1";
+  posture.joint_names[1] = "panda_finger_joint2";
+
+  posture.points.resize(1);
+  posture.points[0].positions.resize(2);
+  posture.points[0].positions[0] = amount / 2;
+  posture.points[0].positions[1] = amount / 2;
+  posture.points[0].time_from_start = ros::Duration(0.5);
 }
 
-geometry_msgs::Pose
-GraspUtil::getPickFromAbovePose(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Vector3 dimensions,
-                                geometry_msgs::Quaternion gripper_orientation_msg, double reach) {
-
-    // object orientation
-    tf2::Quaternion object_orientation;
-    tf2::fromMsg(object_pose_msg.orientation, object_orientation);
-
-    // gripper orientation
-    tf2::Quaternion gripper_orientation;
-    tf2::fromMsg(gripper_orientation_msg, gripper_orientation);
-
-    // approach orientation
-    double approach_roll, approach_pitch, approach_yaw;
-    approach_roll = M_PI;  // turn upside down
-    approach_pitch = 0;
-    approach_yaw = 0;
-    tf2::Quaternion approach_orientation;
-    approach_orientation.setRPY(approach_roll, approach_pitch, approach_yaw);
-
-    tf2::Quaternion orientation = object_orientation * gripper_orientation * approach_orientation;
-
-    // Actually, the maximum reach should be FRANKA_GRIPPER_FINGER_LENGTH - FRANKA_GRIPPER_FINGERTIP_SIZE/2, but half a
-    // finger length is considered a safety distance.
-    if (reach > FRANKA_GRIPPER_FINGER_LENGTH - FRANKA_GRIPPER_FINGERTIP_SIZE) {
-        ROS_WARN_STREAM("Reach around object reduced from " << reach << " to " << FRANKA_GRIPPER_FINGER_LENGTH -
-                                                                                  FRANKA_GRIPPER_FINGERTIP_SIZE << ".");
-        reach = FRANKA_GRIPPER_FINGER_LENGTH - FRANKA_GRIPPER_FINGERTIP_SIZE;
-    } else if (reach < -FRANKA_GRIPPER_FINGERTIP_SIZE / 2) {
-        ROS_WARN_STREAM("Reach around object increased from " << reach << " to 0." << ". Minimum allowed reach is "
-                                                              << (-FRANKA_GRIPPER_FINGERTIP_SIZE / 2) << ".");
-        reach = 0;
-    }
-
-    // The pose is 10 cm straight above the object to pick. This only works on upright boxes.
-    geometry_msgs::Pose target_pose;
-    target_pose.orientation = tf2::toMsg(orientation);
-    target_pose.position.x = object_pose_msg.position.x;
-    target_pose.position.y = object_pose_msg.position.y;
-    target_pose.position.z = object_pose_msg.position.z + (0.5 * dimensions.z) + FRANKA_GRIPPER_DISTANCE - reach;
-
-    return target_pose;
+geometry_msgs::Pose GraspUtil::getPickFromAbovePose(geometry_msgs::Pose& object_pose_msg,
+                                                    geometry_msgs::Vector3 dimensions,
+                                                    geometry_msgs::Quaternion gripper_orientation_msg, double reach)
+{
+  // object orientation
+  tf2::Quaternion object_orientation;
+  tf2::fromMsg(object_pose_msg.orientation, object_orientation);
+
+  // gripper orientation
+  tf2::Quaternion gripper_orientation;
+  tf2::fromMsg(gripper_orientation_msg, gripper_orientation);
+
+  // approach orientation
+  double approach_roll, approach_pitch, approach_yaw;
+  approach_roll = M_PI;  // turn upside down
+  approach_pitch = 0;
+  approach_yaw = 0;
+  tf2::Quaternion approach_orientation;
+  approach_orientation.setRPY(approach_roll, approach_pitch, approach_yaw);
+
+  tf2::Quaternion orientation = object_orientation * gripper_orientation * approach_orientation;
+
+  // Actually, the maximum reach should be FRANKA_GRIPPER_FINGER_LENGTH - FRANKA_GRIPPER_FINGERTIP_SIZE/2, but half a
+  // finger length is considered a safety distance.
+  if (reach > FRANKA_GRIPPER_FINGER_LENGTH - FRANKA_GRIPPER_FINGERTIP_SIZE)
+  {
+    ROS_WARN_STREAM("Reach around object reduced from "
+                    << reach << " to " << FRANKA_GRIPPER_FINGER_LENGTH - FRANKA_GRIPPER_FINGERTIP_SIZE << ".");
+    reach = FRANKA_GRIPPER_FINGER_LENGTH - FRANKA_GRIPPER_FINGERTIP_SIZE;
+  }
+  else if (reach < -FRANKA_GRIPPER_FINGERTIP_SIZE / 2)
+  {
+    ROS_WARN_STREAM("Reach around object increased from " << reach << " to 0."
+                                                          << ". Minimum allowed reach is "
+                                                          << (-FRANKA_GRIPPER_FINGERTIP_SIZE / 2) << ".");
+    reach = 0;
+  }
+
+  // The pose is 10 cm straight above the object to pick. This only works on upright boxes.
+  geometry_msgs::Pose target_pose;
+  target_pose.orientation = tf2::toMsg(orientation);
+  target_pose.position.x = object_pose_msg.position.x;
+  target_pose.position.y = object_pose_msg.position.y;
+  target_pose.position.z = object_pose_msg.position.z + (0.5 * dimensions.z) + FRANKA_GRIPPER_DISTANCE - reach;
+
+  return target_pose;
 }
 
-bool
-GraspUtil::pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &pick_pose,
-                         geometry_msgs::Vector3 dimensions,
-                         double open_amount, std::string supporting_surface_id, std::string object_to_pick_id, bool plan_only) {
-
-    std::vector<moveit_msgs::Grasp> grasps;
-    grasps.resize(1);
-    grasps[0].grasp_pose.header.frame_id = "panda_link0";
-    grasps[0].grasp_pose.pose = pick_pose;
-
-    // Setup pre-grasp approach
-    grasps[0].pre_grasp_approach.direction.header.frame_id = "panda_link0";
-    grasps[0].pre_grasp_approach.direction.vector.z = -1.0;
-    grasps[0].pre_grasp_approach.min_distance = 0.095;
-    grasps[0].pre_grasp_approach.desired_distance = 0.115;
-
-    // Setup post-grasp retreat
-    grasps[0].post_grasp_retreat.direction.header.frame_id = "panda_link0";
-    grasps[0].post_grasp_retreat.direction.vector.z = 1.0;
-    grasps[0].post_grasp_retreat.min_distance = 0.1;
-    grasps[0].post_grasp_retreat.desired_distance = 0.25;
-
-    // Setup the opening / closing - mechanism of the gripper
-
-    setupOpenGripper(grasps[0].pre_grasp_posture, open_amount);
-
-    if (dimensions.x <= 0.08) {
-        setupClosedGripper(grasps[0].grasp_posture, dimensions.x);
-    }
-
-    if (dimensions.x > 0.08 && dimensions.y <= 0.08) {
-        setupClosedGripper(grasps[0].grasp_posture, dimensions.y);
-    }
-
-    if (dimensions.x > 0.08 && dimensions.y > 0.08) {
-        ROS_ERROR("Could not grasp: Object to big (in any dimension).");
-        return false;
-    }
-
-    move_group.setSupportSurfaceName(supporting_surface_id);
-
-    moveit_msgs::MoveItErrorCodes miec = move_group.pick(object_to_pick_id, grasps, plan_only);
-
-    if (miec.val == miec.SUCCESS) {
-        return true;
-    }
-
-    ROS_ERROR_STREAM("Error while executing pick task. MoveItErrorCode: " << miec.val);
+bool GraspUtil::pickFromAbove(moveit::planning_interface::MoveGroupInterface& move_group,
+                              geometry_msgs::Pose& pick_pose, geometry_msgs::Vector3 dimensions, double open_amount,
+                              std::string supporting_surface_id, std::string object_to_pick_id, bool plan_only)
+{
+  std::vector<moveit_msgs::Grasp> grasps;
+  grasps.resize(1);
+  grasps[0].grasp_pose.header.frame_id = "panda_link0";
+  grasps[0].grasp_pose.pose = pick_pose;
+
+  // Setup pre-grasp approach
+  grasps[0].pre_grasp_approach.direction.header.frame_id = "panda_link0";
+  grasps[0].pre_grasp_approach.direction.vector.z = -1.0;
+  grasps[0].pre_grasp_approach.min_distance = 0.095;
+  grasps[0].pre_grasp_approach.desired_distance = 0.115;
+
+  // Setup post-grasp retreat
+  grasps[0].post_grasp_retreat.direction.header.frame_id = "panda_link0";
+  grasps[0].post_grasp_retreat.direction.vector.z = 1.0;
+  grasps[0].post_grasp_retreat.min_distance = 0.1;
+  grasps[0].post_grasp_retreat.desired_distance = 0.25;
+
+  // Setup the opening / closing - mechanism of the gripper
+
+  setupOpenGripper(grasps[0].pre_grasp_posture, open_amount);
+
+  if (dimensions.x <= 0.08)
+  {
+    setupClosedGripper(grasps[0].grasp_posture, dimensions.x);
+  }
+
+  if (dimensions.x > 0.08 && dimensions.y <= 0.08)
+  {
+    setupClosedGripper(grasps[0].grasp_posture, dimensions.y);
+  }
+
+  if (dimensions.x > 0.08 && dimensions.y > 0.08)
+  {
+    ROS_ERROR("Could not grasp: Object to big (in any dimension).");
     return false;
-}
-
-bool
-GraspUtil::placeFromAbove(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) {
-
-    std::vector<moveit_msgs::PlaceLocation> place_location;
-    place_location.resize(1);
-
-    place_location[0].place_pose.header.frame_id = "panda_link0";
-    //  tf2::Quaternion orientation;
-    //  orientation.setRPY(0, 0, M_PI / 2);
-    place_location[0].place_pose.pose.orientation.w = 1;// = tf2::toMsg(orientation);
-    place_location[0].place_pose.pose.position = place_pose.position;
+  }
 
-    place_location[0].pre_place_approach.direction.header.frame_id = "panda_link0";
-    place_location[0].pre_place_approach.direction.vector.z = -1.0;
-    place_location[0].pre_place_approach.min_distance = 0.095;
-    place_location[0].pre_place_approach.desired_distance = 0.115;
+  move_group.setSupportSurfaceName(supporting_surface_id);
 
-    place_location[0].post_place_retreat.direction.header.frame_id = "panda_link0";
-    place_location[0].post_place_retreat.direction.vector.z = 1.0;
-    place_location[0].post_place_retreat.min_distance = 0.1;
-    place_location[0].post_place_retreat.desired_distance = 0.25;
+  moveit_msgs::MoveItErrorCodes miec = move_group.pick(object_to_pick_id, grasps, plan_only);
 
-    setupOpenGripper(place_location[0].post_place_posture, open_amount);
-    move_group.setSupportSurfaceName(supporting_surface_id);
+  if (miec.val == miec.SUCCESS)
+  {
+    return true;
+  }
 
-    moveit_msgs::MoveItErrorCodes miec = move_group.place(object_to_place_id, place_location, plan_only);
-
-    if (miec.val == miec.SUCCESS) {
-        return true;
-    }
+  ROS_ERROR_STREAM("Error while executing pick task. MoveItErrorCode: " << miec.val);
+  return false;
+}
 
-    ROS_ERROR_STREAM("Error while executing place task. MoveItErrorCode: " << miec.val);
-    return false;
+bool GraspUtil::placeFromAbove(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)
+{
+  std::vector<moveit_msgs::PlaceLocation> place_location;
+  place_location.resize(1);
+
+  place_location[0].place_pose.header.frame_id = "panda_link0";
+  //  tf2::Quaternion orientation;
+  //  orientation.setRPY(0, 0, M_PI / 2);
+  place_location[0].place_pose.pose.orientation.w = 1;  // = tf2::toMsg(orientation);
+  place_location[0].place_pose.pose.position = place_pose.position;
+
+  place_location[0].pre_place_approach.direction.header.frame_id = "panda_link0";
+  place_location[0].pre_place_approach.direction.vector.z = -1.0;
+  place_location[0].pre_place_approach.min_distance = 0.095;
+  place_location[0].pre_place_approach.desired_distance = 0.115;
+
+  place_location[0].post_place_retreat.direction.header.frame_id = "panda_link0";
+  place_location[0].post_place_retreat.direction.vector.z = 1.0;
+  place_location[0].post_place_retreat.min_distance = 0.1;
+  place_location[0].post_place_retreat.desired_distance = 0.25;
+
+  setupOpenGripper(place_location[0].post_place_posture, open_amount);
+  move_group.setSupportSurfaceName(supporting_surface_id);
+
+  moveit_msgs::MoveItErrorCodes miec = move_group.place(object_to_place_id, place_location, plan_only);
+
+  if (miec.val == miec.SUCCESS)
+  {
+    return true;
+  }
+
+  ROS_ERROR_STREAM("Error while executing place task. MoveItErrorCode: " << miec.val);
+  return false;
 }
 
-bool GraspUtil::homeGripper() {
-    actionlib::SimpleActionClient<franka_gripper::MoveAction> moveActionClient("franka_gripper/move", true);
-    if (moveActionClient.waitForServer(ros::Duration(1.0))) {
-        moveActionClient.cancelAllGoals();
-    }
-
-    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;
-    }
-
-    ac.cancelAllGoals();
-    franka_gripper::HomingGoal goal;
-    ac.sendGoal(goal);
-    if (ac.waitForResult(ros::Duration(15.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;
-    }
+bool GraspUtil::homeGripper()
+{
+  actionlib::SimpleActionClient<franka_gripper::MoveAction> moveActionClient("franka_gripper/move", true);
+  if (moveActionClient.waitForServer(ros::Duration(1.0)))
+  {
+    moveActionClient.cancelAllGoals();
+  }
+
+  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;
+  }
+
+  ac.cancelAllGoals();
+  franka_gripper::HomingGoal goal;
+  ac.sendGoal(goal);
+  if (ac.waitForResult(ros::Duration(15.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;
+  }
 }