diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..d039fd6
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1 @@
+CATKIN_IGNORE
\ No newline at end of file
diff --git a/panda_softhand/package.xml b/panda_softhand/package.xml
index 893125a..3d92324 100644
--- a/panda_softhand/package.xml
+++ b/panda_softhand/package.xml
@@ -1,5 +1,5 @@
 <?xml version="1.0"?>
-<package>
+<package format="2">
   <name>panda_softhand</name>
   <version>1.0.0</version>
   <description>The panda_softhand package</description>
@@ -11,6 +11,11 @@
 
   <buildtool_depend>catkin</buildtool_depend>
 
+  <exec_depend>panda_softhand_description</exec_depend>
+  <exec_depend>panda_softhand_control</exec_depend>
+  <exec_depend>panda_softhand_safety</exec_depend>
+  <exec_depend>panda_softhand_moveit_config</exec_depend>
+
   <export>
     <metapackage/>
   </export>
diff --git a/panda_softhand_control/CMakeLists.txt b/panda_softhand_control/CMakeLists.txt
index 9cd9880..6de362e 100644
--- a/panda_softhand_control/CMakeLists.txt
+++ b/panda_softhand_control/CMakeLists.txt
@@ -35,6 +35,7 @@ add_service_files(FILES
   pose_control.srv
   joint_control.srv
   set_object.srv
+  complex_grasp.srv
 )
 
 generate_messages(DEPENDENCIES
diff --git a/panda_softhand_control/configs/task_sequences_params.yaml b/panda_softhand_control/configs/task_sequences_params.yaml
index aba96b2..91acb5a 100644
--- a/panda_softhand_control/configs/task_sequences_params.yaml
+++ b/panda_softhand_control/configs/task_sequences_params.yaml
@@ -10,13 +10,18 @@ task_sequencer:
   pos_controller: position_joint_trajectory_controller
   # Impedance controller
   imp_controller: cartesian_impedance_controller_softbots_stiff_matrix
+  # Name of the topic where the object pose is published
+  object_topic_name: /irim_demo/chosen_object
 
   # The joint configuration for home
   home_joints: [-0.035, -0.109, -0.048, -1.888, 0.075, 1.797, -0.110] 
   # Transform from object frame to grasp pose frame (xyzrpy) (DEFAULT)
-  grasp_transform: [0.0, 0.0, 0.030, -3.1415, 0.0, 0.0]
+  # grasp_transform: [-0.04, -0.04, 0.145, -3.1415, 0.0, 0.0] (default)
+  grasp_transform: [-0.08, -0.05, 0.025, -3.039, -0.185, -0.166]
   # Transform from grasp pose frame to pre-grasp pose frame (xyzrpy)
   pre_grasp_transform: [0.0, 0.0, -0.1, 0.0, 0.0, 0.0]
+  # The joint configuration for place
+  place_joints: [-0.068, 0.152, 0.193, -2.352, 0.0405, 2.519, 0.117]
   # The joint configuration for handover
   handover_joints: [-0.101, 0.161, 0.159, -1.651, 2.023, 2.419, -0.006]
   # The threshold on tau_ext for handover hand opening
@@ -24,9 +29,9 @@ task_sequencer:
 
   # The personalized grasp pose maps for different objects
   poses_map:
-    "kettle" : [0.066, 0.012, 0.203, -2.973, 0.620, -0.032]
-    "teddy" : [0.046, -0.238, 0.106, 3.131, -0.052, -0.400]
-    "ball" : [-0.055, 0.113, 0.07, -3.104, 0.122, -0.241]
-    "two_cubes" : [-0.055, 0.113, 0.061, -3.104, 0.122, -0.241]
-    "bottle" : [0.068, 0.077, 0.019, -2.455, -0.249, 0.409]
-    "helmet" : [0.120, -0.039, 0.179, 3.007, 0.200, 0.247]
\ No newline at end of file
+    "object1" : [-0.08, -0.05, 0.025, -3.039, -0.185, -0.166]
+    "object2" : [-0.08, -0.05, 0.025, -3.039, -0.185, -0.166]
+    "object3" : [-0.08, -0.05, 0.025, -3.039, -0.185, -0.166]
+    "object4" : [-0.08, -0.05, 0.025, -3.039, -0.185, -0.166]
+    "object5" : [-0.08, -0.05, 0.025, -3.039, -0.185, -0.166]
+    "object6" : [-0.08, -0.05, 0.025, -3.039, -0.185, -0.166]
\ No newline at end of file
diff --git a/panda_softhand_control/include/panda_softhand_control/TaskSequencer.h b/panda_softhand_control/include/panda_softhand_control/TaskSequencer.h
index d787130..a9b7e80 100644
--- a/panda_softhand_control/include/panda_softhand_control/TaskSequencer.h
+++ b/panda_softhand_control/include/panda_softhand_control/TaskSequencer.h
@@ -12,10 +12,11 @@ Email: gpollayil@gmail.com, mathewjosepollayil@gmail.com  */
 #include "std_msgs/Bool.h"
 #include "std_srvs/SetBool.h"
 #include "panda_softhand_control/set_object.h"
+#include "panda_softhand_control/complex_grasp.h"
 #include "geometry_msgs/Pose.h"
 #include <controller_manager_msgs/SwitchController.h>
 #include <franka_msgs/FrankaState.h>
-#include <franka_control/ErrorRecoveryActionGoal.h>
+#include <franka_msgs/ErrorRecoveryActionGoal.h>
 
 // Parsing includes
 #include <XmlRpcValue.h>
@@ -56,6 +57,18 @@ class TaskSequencer {
         // Callback for simple grasp task service
         bool call_simple_grasp_task(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res);
 
+        // Callback for complex grasp task service (goes to specified pose)
+        bool call_complex_grasp_task(panda_softhand_control::complex_grasp::Request &req, panda_softhand_control::complex_grasp::Response &res);
+
+        // Callback for simple place task service
+        bool call_simple_place_task(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res);
+
+        // Callback for simple home task service
+        bool call_simple_home_task(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res);
+
+        // Callback for simple handover task service
+        bool call_simple_handover_task(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res);
+
         // Callback for set object service
         bool call_set_object(panda_softhand_control::set_object::Request &req, panda_softhand_control::set_object::Response &res);
 
@@ -84,14 +97,21 @@ class TaskSequencer {
         // The switch controller service name
         std::string switch_service_name = "/controller_manager/switch_controller";
 
-        // Topic and service names
-        std::string object_topic_name;
+        // Service names
         std::string franka_state_topic_name = "/franka_state_controller/franka_states";
         std::string grasp_task_service_name;
+        std::string complex_grasp_task_service_name;
+        std::string place_task_service_name;
+        std::string home_task_service_name;
+        std::string handover_task_service_name;
         std::string set_object_service_name;
 
         // Service Servers
         ros::ServiceServer grasp_task_server;
+        ros::ServiceServer complex_grasp_task_server;
+        ros::ServiceServer place_task_server;
+        ros::ServiceServer home_task_server;
+        ros::ServiceServer handover_task_server;
         ros::ServiceServer set_object_server;
 
         // The XmlRpc value for parsing complex params
@@ -102,14 +122,16 @@ class TaskSequencer {
         std::string robot_joints_name;              // Name of the robot joints (without the number of the joints)
         std::string pos_controller;                 // Name of position controller
         std::string imp_controller;                 // Name of impedance controller
+        std::string object_topic_name;              // Name of the topic where the object pose is published
         std::vector<double> home_joints;
         std::vector<double> grasp_transform;
         geometry_msgs::Pose grasp_T;
         std::vector<double> pre_grasp_transform;
         geometry_msgs::Pose pre_grasp_T;
+        std::vector<double> place_joints;
         std::vector<double> handover_joints;
         double handover_thresh;
 
         std::map<std::string, std::vector<double>> poses_map;     // The map containing the notable poses
 
-};
\ No newline at end of file
+};
diff --git a/panda_softhand_control/launch/launchSimulateObjectPose.launch b/panda_softhand_control/launch/launchSimulateObjectPose.launch
index bfd05ae..42f7270 100644
--- a/panda_softhand_control/launch/launchSimulateObjectPose.launch
+++ b/panda_softhand_control/launch/launchSimulateObjectPose.launch
@@ -2,7 +2,7 @@
 <launch>
 
 	<!-- Temporarily publishing the object pose and tf. TODO: comment out when using vision -->
-	<node name="rostopic_object_pose_node" pkg="rostopic" type="rostopic" args="pub -r 10 object_pose geometry_msgs/Pose 
+	<node name="rostopic_object_pose_node" pkg="rostopic" type="rostopic" args="pub -r 10 /irim_demo/aruco_chosen_object/pose/pose geometry_msgs/Pose 
 	'{position: {x: 0.627, y: -0.040, z: 0.049}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 0.0}}'"/>
 
 	<node name="world_to_object" pkg="tf" type="static_transform_publisher" args="0.627 -0.040 0.049 0.0 0.0 0.0 world object 100"/>
diff --git a/panda_softhand_control/src/TaskSequencer.cpp b/panda_softhand_control/src/TaskSequencer.cpp
index ac13d5f..9a987ef 100644
--- a/panda_softhand_control/src/TaskSequencer.cpp
+++ b/panda_softhand_control/src/TaskSequencer.cpp
@@ -8,7 +8,7 @@ Email: gpollayil@gmail.com, mathewjosepollayil@gmail.com  */
 #include "panda_softhand_control/TaskSequencer.h"
 
 TaskSequencer::TaskSequencer(ros::NodeHandle& nh_){
-    
+
     // Initializing Node Handle
     this->nh = nh_;
 
@@ -17,8 +17,7 @@ TaskSequencer::TaskSequencer(ros::NodeHandle& nh_){
         ROS_ERROR("The parsing of task parameters went wrong. Be careful, using default values...");
     }
 
-    // Initializing the object subscriber and waiting (TODO: parse the topic name)
-    this->object_topic_name = "/object_pose";
+    // Initializing the object subscriber and waiting (the object topic name is parsed now)
     this->object_sub = this->nh.subscribe(this->object_topic_name, 1, &TaskSequencer::get_object_pose, this);
     ros::topic::waitForMessage<geometry_msgs::Pose>(this->object_topic_name, ros::Duration(2.0));
 
@@ -27,7 +26,7 @@ TaskSequencer::TaskSequencer(ros::NodeHandle& nh_){
     ros::topic::waitForMessage<franka_msgs::FrankaState>("/" + this->robot_name + this->franka_state_topic_name, ros::Duration(2.0));
 
     // Initializing the tau_ext norm and franka recovery publishers
-    this->pub_franka_recovery = this->nh.advertise<franka_control::ErrorRecoveryActionGoal>("/" + this->robot_name + "/franka_control/error_recovery/goal", 1);
+    this->pub_franka_recovery = this->nh.advertise<franka_msgs::ErrorRecoveryActionGoal>("/" + this->robot_name + "/franka_control/error_recovery/goal", 1);
     this->pub_tau_ext_norm = this->nh.advertise<std_msgs::Float64>("tau_ext_norm", 1);
 
     // Initializing Panda SoftHand Client (TODO: Return error if initialize returns false)
@@ -35,10 +34,18 @@ TaskSequencer::TaskSequencer(ros::NodeHandle& nh_){
 
     // Setting the task service names
     this->grasp_task_service_name = "grasp_task_service";
+    this->complex_grasp_task_service_name = "complex_grasp_task_service";
+    this->place_task_service_name = "place_task_service";
+    this->home_task_service_name = "home_task_service";
+    this->handover_task_service_name = "handover_task_service";
     this->set_object_service_name = "set_object_service";
 
     // Advertising the services
     this->grasp_task_server = this->nh.advertiseService(this->grasp_task_service_name, &TaskSequencer::call_simple_grasp_task, this);
+    this->complex_grasp_task_server = this->nh.advertiseService(this->complex_grasp_task_service_name, &TaskSequencer::call_complex_grasp_task, this);
+    this->place_task_server = this->nh.advertiseService(this->place_task_service_name, &TaskSequencer::call_simple_place_task, this);
+    this->home_task_server = this->nh.advertiseService(this->home_task_service_name, &TaskSequencer::call_simple_home_task, this);
+    this->handover_task_server = this->nh.advertiseService(this->handover_task_service_name, &TaskSequencer::call_simple_handover_task, this);
     this->set_object_server = this->nh.advertiseService(this->set_object_service_name, &TaskSequencer::call_set_object, this);
 
     // Spinning once
@@ -47,7 +54,7 @@ TaskSequencer::TaskSequencer(ros::NodeHandle& nh_){
 }
 
 TaskSequencer::~TaskSequencer(){
-    
+
     // Nothing to do here yet
 }
 
@@ -79,6 +86,12 @@ bool TaskSequencer::parse_task_params(){
 		success = false;
 	}
 
+    if(!ros::param::get("/task_sequencer/object_topic_name", this->object_topic_name)){
+		ROS_WARN("The param 'object_topic_name' not found in param server! Using default.");
+		this->object_topic_name = "/irim_demo/chosen_object";
+		success = false;
+	}
+
 	if(!ros::param::get("/task_sequencer/home_joints", this->home_joints)){
 		ROS_WARN("The param 'home_joints' not found in param server! Using default.");
 		this->home_joints = {-0.035, -0.109, -0.048, -1.888, 0.075, 1.797, -0.110};
@@ -111,6 +124,12 @@ bool TaskSequencer::parse_task_params(){
 		success = false;
 	}
 
+    if(!ros::param::get("/task_sequencer/place_joints", this->place_joints)){
+		ROS_WARN("The param 'place_joints' not found in param server! Using default.");
+		this->place_joints = {-0.136, 0.794, -0.115, -1.337, 0.250, 2.217, -0.479};
+		success = false;
+	}
+
     if(!ros::param::get("/task_sequencer/handover_thresh", this->handover_thresh)){
 		ROS_WARN("The param 'handover_thresh' not found in param server! Using default.");
 		this->handover_thresh = 4.5;
@@ -134,8 +153,8 @@ bool TaskSequencer::parse_task_params(){
             std::cout << it.first << " : [ ";
             for(auto vec_it : it.second){
                 std::cout << vec_it << " ";
-            } 
-            std::cout << "]" << std::endl;     
+            }
+            std::cout << "]" << std::endl;
         }
     }
 
@@ -144,7 +163,7 @@ bool TaskSequencer::parse_task_params(){
 
 // Convert xyzrpy vector to geometry_msgs Pose
 geometry_msgs::Pose TaskSequencer::convert_vector_to_pose(std::vector<double> input_vec){
-    
+
     // Creating temporary variables
     geometry_msgs::Pose output_pose;
     Eigen::Affine3d output_affine;
@@ -155,8 +174,8 @@ geometry_msgs::Pose TaskSequencer::convert_vector_to_pose(std::vector<double> in
     Eigen::Matrix3d rotation = Eigen::Matrix3d(Eigen::AngleAxisd(input_vec[5], Eigen::Vector3d::UnitZ())
         * Eigen::AngleAxisd(input_vec[4], Eigen::Vector3d::UnitY())
         * Eigen::AngleAxisd(input_vec[3], Eigen::Vector3d::UnitX()));
-    output_affine.linear() = rotation;    
-    
+    output_affine.linear() = rotation;
+
     // Converting to geometry_msgs and returning
     tf::poseEigenToMsg(output_affine, output_pose);
     return output_pose;
@@ -220,12 +239,12 @@ void TaskSequencer::get_franka_state(const franka_msgs::FrankaState::ConstPtr &m
     // Publishing norm
     std_msgs::Float64 norm_msg; norm_msg.data = this->tau_ext_norm;
     this->pub_tau_ext_norm.publish(norm_msg);
-    
+
 }
 
 // Callback for simple grasp task service
 bool TaskSequencer::call_simple_grasp_task(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res){
-    
+
     // Checking the request for correctness
     if(!req.data){
         ROS_WARN("Did you really want to call the simple grasp task service with data = false?");
@@ -251,6 +270,9 @@ bool TaskSequencer::call_simple_grasp_task(std_srvs::SetBool::Request &req, std_
     tf::poseEigenToMsg(object_pose_aff * grasp_transform_aff * pre_grasp_transform_aff, pre_grasp_pose);
     tf::poseEigenToMsg(object_pose_aff * grasp_transform_aff, grasp_pose);
 
+    // Couting object pose for debugging
+    std::cout << "Object position is \n" << object_pose_aff.translation() << std::endl;
+
     // 2) Going to pregrasp pose
     if(!this->panda_softhand_client.call_pose_service(pre_grasp_pose, false) || !this->franka_ok){
         ROS_ERROR("Could not go to the specified pre grasp pose.");
@@ -283,15 +305,171 @@ bool TaskSequencer::call_simple_grasp_task(std_srvs::SetBool::Request &req, std_
         return false;
     }
 
-    // 6) Going to handover joint config
+    // Now, everything finished well
+    res.success = true;
+    res.message = "The service call_simple_grasp_task was correctly performed!";
+    return true;
+}
+
+// Callback for complex grasp task service (goes to specified pose)
+bool TaskSequencer::call_complex_grasp_task(panda_softhand_control::complex_grasp::Request &req, panda_softhand_control::complex_grasp::Response &res){
+
+    // Checking the request for correctness
+    if(!req.data){
+        ROS_WARN("Did you really want to call the complex grasp task service with data = false?");
+        res.success = true;
+        res.message = "The service call_complex_grasp_task done correctly with false request!";
+        return true;
+    }
+
+    // 1) Going to home configuration
+    if(!this->panda_softhand_client.call_joint_service(this->home_joints) || !this->franka_ok){
+        ROS_ERROR("Could not go to the specified home joint configuration.");
+        res.success = false;
+        res.message = "The service call_complex_grasp_task was NOT performed correctly!";
+        return false;
+    }
+
+    // Computing the grasp and pregrasp pose and converting to geometry_msgs Pose
+    Eigen::Affine3d object_pose_aff; tf::poseMsgToEigen(req.object_pose, object_pose_aff);
+    Eigen::Affine3d grasp_transform_aff; tf::poseMsgToEigen(this->grasp_T, grasp_transform_aff);
+    Eigen::Affine3d pre_grasp_transform_aff; tf::poseMsgToEigen(this->pre_grasp_T, pre_grasp_transform_aff);
+
+    geometry_msgs::Pose pre_grasp_pose; geometry_msgs::Pose grasp_pose;
+    tf::poseEigenToMsg(object_pose_aff * grasp_transform_aff * pre_grasp_transform_aff, pre_grasp_pose);
+    tf::poseEigenToMsg(object_pose_aff * grasp_transform_aff, grasp_pose);
+
+    // Couting object pose for debugging
+    std::cout << "Object position is \n" << object_pose_aff.translation() << std::endl;
+
+    // 2) Going to pregrasp pose
+    if(!this->panda_softhand_client.call_pose_service(pre_grasp_pose, false) || !this->franka_ok){
+        ROS_ERROR("Could not go to the specified pre grasp pose.");
+        res.success = false;
+        res.message = "The service call_complex_grasp_task was NOT performed correctly!";
+        return false;
+    }
+
+    // 3) Going to grasp pose
+    if(!this->panda_softhand_client.call_slerp_service(grasp_pose, false) || !this->franka_ok){
+        ROS_ERROR("Could not go to the specified grasp pose.");
+        res.success = false;
+        res.message = "The service call_complex_grasp_task was NOT performed correctly!";
+        return false;
+    }
+
+    // 4) Performing complex grasp
+    if(!this->panda_softhand_client.call_hand_service(1.0, 2.0) || !this->franka_ok){
+        ROS_ERROR("Could not perform the complex grasp.");
+        res.success = false;
+        res.message = "The service call_complex_grasp_task was NOT performed correctly!";
+        return false;
+    }
+
+    // 4) Lifting the grasped? object
+    if(!this->panda_softhand_client.call_joint_service(this->home_joints) || !this->franka_ok){
+        ROS_ERROR("Could not lift to the specified pose.");
+        res.success = false;
+        res.message = "The service call_complex_grasp_task was NOT performed correctly!";
+        return false;
+    }
+
+    // Now, everything finished well
+    res.success = true;
+    res.message = "The service call_complex_grasp_task was correctly performed!";
+    return true;
+
+}
+
+
+// Callback for simple place task service
+bool TaskSequencer::call_simple_place_task(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res){
+
+    // Checking the request for correctness
+    if(!req.data){
+        ROS_WARN("Did you really want to call the simple place task service with data = false?");
+        res.success = true;
+        res.message = "The service call_simple_place_task done correctly with false request!";
+        return true;
+    }
+
+    // 1) Going to place joint config
+    if(!this->panda_softhand_client.call_joint_service(this->place_joints) || !this->franka_ok){
+        ROS_ERROR("Could not go to the specified place joint config.");
+        res.success = false;
+        res.message = "The service call_simple_place_task was NOT performed correctly!";
+        return false;
+    }
+
+    // 2) Opening hand
+    if(!this->panda_softhand_client.call_hand_service(0.0, 2.0) || !this->franka_ok){
+        ROS_ERROR("Could not open the hand.");
+        res.success = false;
+        res.message = "The service call_simple_place_task was NOT performed correctly!";
+        return false;
+    }
+
+    // Now, everything finished well
+    res.success = true;
+    res.message = "The service call_simple_place_task was correctly performed!";
+    return true;
+
+}
+
+// Callback for simple home task service
+bool TaskSequencer::call_simple_home_task(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res){
+
+    // Checking the request for correctness
+    if(!req.data){
+        ROS_WARN("Did you really want to call the simple home task service with data = false?");
+        res.success = true;
+        res.message = "The service call_simple_home_task done correctly with false request!";
+        return true;
+    }
+
+    // 1) Going to home joint config
+    if(!this->panda_softhand_client.call_joint_service(this->home_joints) || !this->franka_ok){
+        ROS_ERROR("Could not go to the specified home joint config.");
+        res.success = false;
+        res.message = "The service call_simple_home_task was NOT performed correctly!";
+        return false;
+    }
+
+    // 2) Opening hand
+    if(!this->panda_softhand_client.call_hand_service(0.0, 2.0) || !this->franka_ok){
+        ROS_ERROR("Could not open the hand.");
+        res.success = false;
+        res.message = "The service call_simple_home_task was NOT performed correctly!";
+        return false;
+    }
+
+    // Now, everything finished well
+    res.success = true;
+    res.message = "The service call_simple_home_task was correctly performed!";
+    return true;
+
+}
+
+// Callback for simple handover task service
+bool TaskSequencer::call_simple_handover_task(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res){
+
+    // Checking the request for correctness
+    if(!req.data){
+        ROS_WARN("Did you really want to call the simple handover task service with data = false?");
+        res.success = true;
+        res.message = "The service call_simple_handover_task done correctly with false request!";
+        return true;
+    }
+
+    // 1) Going to handover joint config
     if(!this->panda_softhand_client.call_joint_service(this->handover_joints) || !this->franka_ok){
         ROS_ERROR("Could not go to the specified handover joint config.");
         res.success = false;
-        res.message = "The service call_simple_grasp_task was NOT performed correctly!";
+        res.message = "The service call_simple_handover_task was NOT performed correctly!";
         return false;
     }
 
-    // 7) Waiting for threshold or for some time
+    // 2) Waiting for threshold or for some time
     sleep(1);       // Sleeping for a second to avoid robot stopping peaks
     bool hand_open = false; ros::Time init_time = ros::Time::now(); ros::Time now_time;
     double base_tau_ext = this->tau_ext_norm;           // Saving the present tau for later computation of variation
@@ -306,26 +484,27 @@ bool TaskSequencer::call_simple_grasp_task(std_srvs::SetBool::Request &req, std_
         if((now_time - init_time) > ros::Duration(10, 0)){
             hand_open = true;
             if(DEBUG) ROS_WARN_STREAM("Opening condition reached!" << " TIMEOUT!");
-            if(DEBUG) ROS_WARN_STREAM("The initial time was " << init_time << ", now it is " << now_time 
+            if(DEBUG) ROS_WARN_STREAM("The initial time was " << init_time << ", now it is " << now_time
                 << ", the difference is " << (now_time - init_time) << " and the timeout thresh is " << ros::Duration(10, 0));
         }
     }
 
-    // 8) Opening hand 
+    // 3) Opening hand
     if(!this->panda_softhand_client.call_hand_service(0.0, 2.0) || !this->franka_ok){
         ROS_ERROR("Could not open the hand.");
         res.success = false;
-        res.message = "The service call_simple_grasp_task was NOT performed correctly!";
+        res.message = "The service call_simple_handover_task was NOT performed correctly!";
         return false;
     }
 
     // Now, everything finished well
     res.success = true;
-    res.message = "The service call_simple_grasp_task was correctly performed!";
+    res.message = "The service call_simple_handover_task was correctly performed!";
     return true;
+
 }
 
-// Callback for handshake task service
+// Callback for set object task service
 bool TaskSequencer::call_set_object(panda_softhand_control::set_object::Request &req, panda_softhand_control::set_object::Response &res){
 
     // Checking if the parsed map contains the requested object
@@ -346,4 +525,4 @@ bool TaskSequencer::call_set_object(panda_softhand_control::set_object::Request
     ROS_INFO_STREAM("Grasp pose changed. Object set to " << req.object_name << ".");
     res.result = true;
     return res.result;
-}
\ No newline at end of file
+}
diff --git a/panda_softhand_control/srv/complex_grasp.srv b/panda_softhand_control/srv/complex_grasp.srv
new file mode 100644
index 0000000..1e73f72
--- /dev/null
+++ b/panda_softhand_control/srv/complex_grasp.srv
@@ -0,0 +1,5 @@
+bool data
+geometry_msgs/Pose object_pose
+---
+bool success
+string message
\ No newline at end of file
diff --git a/panda_softhand_description/configs/panda_soma.rviz b/panda_softhand_description/configs/panda_soma.rviz
index efc64b7..51e0eea 100644
--- a/panda_softhand_description/configs/panda_soma.rviz
+++ b/panda_softhand_description/configs/panda_soma.rviz
@@ -7,7 +7,7 @@ Panels:
         - /TF1
         - /Trajectory1
       Splitter Ratio: 0.470842332
-    Tree Height: 689
+    Tree Height: 660
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -106,6 +106,7 @@ Visualization Manager:
           Alpha: 1
           Show Axes: false
           Show Trail: false
+          Value: true
         right_hand_ee_link:
           Alpha: 1
           Show Axes: false
@@ -355,6 +356,7 @@ Visualization Manager:
             Alpha: 1
             Show Axes: false
             Show Trail: false
+            Value: true
           right_hand_ee_link:
             Alpha: 1
             Show Axes: false
@@ -546,6 +548,8 @@ Visualization Manager:
           Value: false
         panda_K:
           Value: false
+        panda_NE:
+          Value: true
         panda_link0:
           Value: false
         panda_link1:
@@ -658,9 +662,10 @@ Visualization Manager:
                       panda_link6:
                         panda_link7:
                           panda_link8:
-                            panda_EE:
-                              panda_K:
-                                {}
+                            panda_NE:
+                              panda_EE:
+                                panda_K:
+                                  {}
                             right_hand_ee_link:
                               {}
                             right_hand_panda_flange:
@@ -770,6 +775,7 @@ Visualization Manager:
           Alpha: 1
           Show Axes: false
           Show Trail: false
+          Value: true
         right_hand_ee_link:
           Alpha: 1
           Show Axes: false
@@ -1011,7 +1017,7 @@ Window Geometry:
   Height: 1056
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd00000004000000000000017800000396fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000280000034b000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000024005200760069007a00560069007300750061006c0054006f006f006c00730047007500690100000379000000450000004300fffffffb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004a00ffffff000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007470000003efc0100000002fb0000000800540069006d00650100000000000007470000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000004b40000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000001870000038afc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006300fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002e00000334000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000024005200760069007a00560069007300750061006c0054006f006f006c007300470075006901000003690000004f0000004c00fffffffb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000005000ffffff00000001000001110000038afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002e0000038a000000b700fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000074700000040fc0100000002fb0000000800540069006d00650100000000000007470000038300fffffffb0000000800540069006d00650100000000000004500000000000000000000004a10000038a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   RvizVisualToolsGui:
     collapsed: false
   Selection:
@@ -1025,5 +1031,5 @@ Window Geometry:
   Views:
     collapsed: false
   Width: 1863
-  X: 57
-  Y: 1104
+  X: 1977
+  Y: 24
diff --git a/panda_softhand_description/launch/launchPandaSoftHand.launch b/panda_softhand_description/launch/launchPandaSoftHand.launch
index dc1d6ad..3be6ff3 100644
--- a/panda_softhand_description/launch/launchPandaSoftHand.launch
+++ b/panda_softhand_description/launch/launchPandaSoftHand.launch
@@ -8,7 +8,7 @@
 
     <!-- Panda params -->
     <arg name="panda_arm_enabled" default="true"/>
-    <arg name="robot_ip" default="172.16.0.102"/>
+    <arg name="robot_ip" default="172.16.0.3"/>
     <arg name="load_gripper" default="false"/>
 
     <!-- Hand/Glove params -->
@@ -36,7 +36,7 @@
 	</node>
 
     <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
-    
+
     <!-- Panda namespace -->
     <group ns="panda_arm">
 
@@ -56,8 +56,8 @@
         <rosparam command="load" file="$(find panda_controllers)/config/panda_controllers.yaml" />
         <node name="state_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="franka_state_controller"/>
         <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen"  args="position_joint_trajectory_controller"/>
-        <node name="controller_stopper" pkg="controller_manager" type="spawner" respawn="false" output="screen"  args="--stopped cartesian_velocity_controller"/>
-        
+        <!-- <node name="controller_stopper" pkg="controller_manager" type="spawner" respawn="false" output="screen"  args="stopped cartesian_velocity_controller"/> -->
+
         <!-- Panda joint state publisher -->
         <node name="joint_state_publisher" type="joint_state_publisher" pkg="joint_state_publisher" output="screen">
             <rosparam if="$(arg load_gripper)" param="source_list">[franka_state_controller/joint_states, franka_gripper/joint_states] </rosparam>
@@ -89,12 +89,12 @@
   	<node name="world_to_planning_scene" pkg="panda_softhand_control" type="world_to_planning_scene_node" args="$(find panda_softhand_description)/worlds/$(arg environment_name).world --skipIfco" output="screen"/>
 
     <rosparam command="load" file="$(find soft_hand_controllers)/config/controllers.yaml" />
-    
+
     <!-- Hand namespace -->
     <group ns="right_hand">
-        
+
         <group if="$(arg right_hand_enabled)">
-            
+
             <!-- A copy of robot description -->
             <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find panda_softhand_description)/robot/panda_softhand.urdf.xacro'" unless="$(arg load_gripper)" />
 
diff --git a/panda_softhand_description/worlds/panda_soma.world b/panda_softhand_description/worlds/panda_soma.world
index 098296a..808d5a5 100755
--- a/panda_softhand_description/worlds/panda_soma.world
+++ b/panda_softhand_description/worlds/panda_soma.world
@@ -2,38 +2,183 @@
 <sdf version="1.4">
   <!-- We use a custom world for the panda + softhand. -->
   <!-- One can change this world to his needs or use another one. -->
-  
+
   <world name="default">
-	
-		<!-- box (like a table) -->
+
+	<!-- box (like a robot base) -->
+    <!-- <model name="rob_base">
+			<pose> -0.08 0 -0.03 0 0 0</pose>
+			<static>true</static>
+			<link name='linkRobBase'>
+				<pose>0 0 0 0 0 0</pose>
+
+				<collision name="my_collisionRobBase">
+		 			<pose>0 0 0 0 0 0</pose>
+					<geometry>
+		        <box>
+		          <size>0.30 1 0.04</size>
+		        </box>
+		      </geometry>
+	      </collision>
+
+				<visual name="my_visualRobBase">
+					<geometry>
+		        <box>
+					<size>0.30 1 0.04</size>
+		        </box>
+		      </geometry>
+	      </visual>
+
+				<self_collide>0</self_collide>
+				<gravity>1</gravity>
+			</link>
+    </model> -->
+
+	<!-- box (like a table) -->
     <model name="table">
-			<pose> 0.6 0 -0.05 0 0 0</pose>
+			<pose> 0.65 0 -0.05 0 0 0</pose>
 			<static>true</static>
 			<link name='linkTable'>
 				<pose>0 0 0 0 0 0</pose>
-				
+
 				<collision name="my_collisionTable">
 		 			<pose>0 0 0 0 0 0</pose>
 					<geometry>
 		        <box>
-		          <size>1 1 0.05</size>
+		          <size>1.2 1 0.03</size>
 		        </box>
 		      </geometry>
 	      </collision>
-				
+
 				<visual name="my_visualTable">
 					<geometry>
 		        <box>
-							<size>1 1 0.05</size>
+					<size>1.2 1 0.03</size>
 		        </box>
 		      </geometry>
 	      </visual>
-				
-				<self_collide>0</self_collide>		
+
+				<self_collide>0</self_collide>
 				<gravity>1</gravity>
 			</link>
     </model>
 
+	<!-- box (like a wall behind) -->
+    <!-- <model name="wall_behind">
+			<pose> -0.30 0 0.5 0 0 0</pose>
+			<static>true</static>
+			<link name='linkWallBehind'>
+				<pose>0 0 0 0 0 0</pose>
+
+				<collision name="my_collisionWallBehind">
+		 			<pose>0 0 0 0 0 0</pose>
+					<geometry>
+		        <box>
+		          <size>0.05 1 1</size>
+		        </box>
+		      </geometry>
+	      </collision>
+
+				<visual name="my_visualWallBehind">
+					<geometry>
+		        <box>
+					<size>0.05 1 1</size>
+		        </box>
+		      </geometry>
+	      </visual>
+
+				<self_collide>0</self_collide>
+				<gravity>1</gravity>
+			</link>
+    </model> -->
+
+	<!-- box (like a wall near the camera) -->
+    <!--<model name="wall_camera">
+			<pose> 0.20 -0.49 0.2 0 0 0</pose>
+			<static>true</static>
+			<link name='linkWallCamera'>
+				<pose>0 0 0 0 0 0</pose>
+
+				<collision name="my_collisionWallCamera">
+		 			<pose>0 0 0 0 0 0</pose>
+					<geometry>
+		        <box>
+		          <size>1.5 0.02 1</size>
+		        </box>
+		      </geometry>
+	      </collision>
+
+				<visual name="my_visualWallCamera">
+					<geometry>
+		        <box>
+					<size>1.5 0.02 1</size>
+		        </box>
+		      </geometry>
+	      </visual>
+
+				<self_collide>0</self_collide>
+				<gravity>1</gravity>
+			</link>
+    </model>-->
+
+	<!-- box (like a wall near the control pc) -->
+    <!--<model name="wall_pc">
+			<pose> 0.20 0.30 0.2 0 0 0</pose>
+			<static>true</static>
+			<link name='linkWallPc'>
+				<pose>0 0 0 0 0 0</pose>
+
+				<collision name="my_collisionWallPc">
+		 			<pose>0 0 0 0 0 0</pose>
+					<geometry>
+		        <box>
+		          <size>1.5 0.02 1</size>
+		        </box>
+		      </geometry>
+	      </collision>
+
+				<visual name="my_visualWallPc">
+					<geometry>
+		        <box>
+					<size>1.5 0.02 1</size>
+		        </box>
+		      </geometry>
+	      </visual>
+
+				<self_collide>0</self_collide>
+				<gravity>1</gravity>
+			</link>
+    </model> -->
+
+	<!-- box (like a container for placing) -->
+  <!--  <model name="box_container">
+			<pose> 0.65 0.13 0.01 0 0 0</pose>
+			<static>true</static>
+			<link name='linkBoxContainer'>
+				<pose>0 0 0 0 0 0</pose>
+
+				<collision name="my_collisionBoxContainer">
+		 			<pose>0 0 0 0 0 0</pose>
+					<geometry>
+		        <box>
+		          <size>0.37 0.29 0.1</size>
+		        </box>
+		      </geometry>
+	      </collision>
+
+				<visual name="my_visualBoxContainer">
+					<geometry>
+		        <box>
+					<size>0.37 0.29 0.1</size>
+		        </box>
+		      </geometry>
+	      </visual>
+
+				<self_collide>0</self_collide>
+				<gravity>1</gravity>
+			</link>
+    </model>-->
+
   </world>
-  
+
 </sdf>
diff --git a/panda_softhand_moveit_config/CHANGELOG.rst b/panda_softhand_moveit_config/CHANGELOG.rst
index 9b32223..14f8757 100644
--- a/panda_softhand_moveit_config/CHANGELOG.rst
+++ b/panda_softhand_moveit_config/CHANGELOG.rst
@@ -1,9 +1,9 @@
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-Changelog for package panda_moveit_config
+Changelog for package panda_softhand_moveit_config
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
 0.7.0 (2018-11-09)
 ------------------
-* Initial release of panda_moveit_config into Melodic, including OMPL, CHOMP and STOMP configs
+* Initial release of panda_softhand_moveit_config into Melodic, including OMPL, CHOMP and STOMP configs
   We moved/merged this repo from https://github.com/frankaemika/franka_ros.
 * Contributors: Dave Coleman, Florian Walch, Mike Lautman, Raghavender Sahdev
diff --git a/panda_softhand_moveit_config/config/panda_arm.srdf.xacro b/panda_softhand_moveit_config/config/panda_arm.srdf.xacro
index 31575d6..e267c8e 100644
--- a/panda_softhand_moveit_config/config/panda_arm.srdf.xacro
+++ b/panda_softhand_moveit_config/config/panda_arm.srdf.xacro
@@ -4,6 +4,6 @@
     A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
 -->
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda_softhand">
-  <xacro:include filename="$(find panda_moveit_config)/config/panda_arm.xacro" />
+  <xacro:include filename="$(find panda_softhand_moveit_config)/config/panda_arm.xacro" />
   <xacro:panda_arm />
 </robot>
diff --git a/panda_softhand_moveit_config/config/panda_arm.xacro b/panda_softhand_moveit_config/config/panda_arm.xacro
index c0c5433..e2a9fc5 100644
--- a/panda_softhand_moveit_config/config/panda_arm.xacro
+++ b/panda_softhand_moveit_config/config/panda_arm.xacro
@@ -43,6 +43,7 @@
     <disable_collisions link1="panda_link4" link2="panda_link7" reason="Never" />
     <disable_collisions link1="panda_link5" link2="panda_link6" reason="Adjacent" />
     <disable_collisions link1="panda_link6" link2="panda_link7" reason="Adjacent" />
+    <disable_collisions link1="panda_link7" link2="panda_link8" reason="Adjacent" />
 
     <!--ADDITIONALLY ADDED-->
 
@@ -220,6 +221,30 @@
     <disable_collisions link1="panda_link7" link2="right_hand_softhand_round_base" reason="Never" />
     <disable_collisions link1="panda_link7" link2="right_hand_panda_flange" reason="Never" />
 
+    <disable_collisions link1="panda_link8" link2="right_hand_index_distal_link" reason="Never" />
+    <disable_collisions link1="panda_link8" link2="right_hand_index_knuckle_link" reason="Never" />
+    <disable_collisions link1="panda_link8" link2="right_hand_index_middle_link" reason="Never" />
+    <disable_collisions link1="panda_link8" link2="right_hand_index_proximal_link" reason="Never" />
+    <disable_collisions link1="panda_link8" link2="right_hand_little_distal_link" reason="Never" />
+    <disable_collisions link1="panda_link8" link2="right_hand_little_knuckle_link" reason="Never" />
+    <disable_collisions link1="panda_link8" link2="right_hand_little_middle_link" reason="Never" />
+    <disable_collisions link1="panda_link8" link2="right_hand_little_proximal_link" reason="Never" />
+    <disable_collisions link1="panda_link8" link2="right_hand_middle_distal_link" reason="Never" />
+    <disable_collisions link1="panda_link8" link2="right_hand_middle_knuckle_link" reason="Never" />
+    <disable_collisions link1="panda_link8" link2="right_hand_middle_middle_link" reason="Never" />
+    <disable_collisions link1="panda_link8" link2="right_hand_middle_proximal_link" reason="Never" />
+    <disable_collisions link1="panda_link8" link2="right_hand_palm_link" reason="Never" />
+    <disable_collisions link1="panda_link8" link2="right_hand_ring_distal_link" reason="Never" />
+    <disable_collisions link1="panda_link8" link2="right_hand_ring_knuckle_link" reason="Never" />
+    <disable_collisions link1="panda_link8" link2="right_hand_ring_middle_link" reason="Never" />
+    <disable_collisions link1="panda_link8" link2="right_hand_ring_proximal_link" reason="Never" />
+    <disable_collisions link1="panda_link8" link2="right_hand_thumb_distal_link" reason="Never" />
+    <disable_collisions link1="panda_link8" link2="right_hand_thumb_knuckle_link" reason="Never" />
+    <disable_collisions link1="panda_link8" link2="right_hand_thumb_proximal_link" reason="Never" />
+    <disable_collisions link1="panda_link8" link2="right_hand_softhand_round_base" reason="Never" />
+    <disable_collisions link1="panda_link8" link2="right_hand_panda_flange" reason="Never" />
+
+
     <disable_collisions link1="right_hand_panda_flange" link2="right_hand_index_distal_link" reason="Never" />
     <disable_collisions link1="right_hand_panda_flange" link2="right_hand_index_knuckle_link" reason="Never" />
     <disable_collisions link1="right_hand_panda_flange" link2="right_hand_index_middle_link" reason="Never" />
diff --git a/panda_softhand_moveit_config/config/panda_arm_hand.srdf.xacro b/panda_softhand_moveit_config/config/panda_arm_hand.srdf.xacro
index 39f7db9..4c738b3 100644
--- a/panda_softhand_moveit_config/config/panda_arm_hand.srdf.xacro
+++ b/panda_softhand_moveit_config/config/panda_arm_hand.srdf.xacro
@@ -4,8 +4,8 @@
     A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
 -->
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda">
-  <xacro:include filename="$(find panda_moveit_config)/config/panda_arm.xacro" />
-  <xacro:include filename="$(find panda_moveit_config)/config/hand.xacro" />
+  <xacro:include filename="$(find panda_softhand_moveit_config)/config/panda_arm.xacro" />
+  <xacro:include filename="$(find panda_softhand_moveit_config)/config/hand.xacro" />
   <xacro:panda_arm />
   <xacro:hand />
   <!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
diff --git a/panda_softhand_moveit_config/config/sensors_kinect_pointcloud.yaml b/panda_softhand_moveit_config/config/sensors_kinect_pointcloud.yaml
index 92e7095..e870208 100644
--- a/panda_softhand_moveit_config/config/sensors_kinect_pointcloud.yaml
+++ b/panda_softhand_moveit_config/config/sensors_kinect_pointcloud.yaml
@@ -1,6 +1,6 @@
 sensors:
   - sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
-    point_cloud_topic: /camera/depth_registered/points
+    point_cloud_topic: /trial_camera/depth_registered/points # Added a trial_ in front to disable octomap
     max_range: 5.0
     point_subsample: 1
     padding_offset: 0.1
diff --git a/panda_softhand_moveit_config/launch/chomp_planning_pipeline.launch.xml b/panda_softhand_moveit_config/launch/chomp_planning_pipeline.launch.xml
index ffbc72e..7e5d445 100644
--- a/panda_softhand_moveit_config/launch/chomp_planning_pipeline.launch.xml
+++ b/panda_softhand_moveit_config/launch/chomp_planning_pipeline.launch.xml
@@ -10,6 +10,6 @@
 
    <param name="collision_detector" value="Hybrid" />
 
-   <rosparam command="load" file="$(find panda_moveit_config)/config/chomp_planning.yaml" />
+   <rosparam command="load" file="$(find panda_softhand_moveit_config)/config/chomp_planning.yaml" />
 
 </launch>
diff --git a/panda_softhand_moveit_config/launch/default_warehouse_db.launch b/panda_softhand_moveit_config/launch/default_warehouse_db.launch
index 6901960..2e60c9d 100644
--- a/panda_softhand_moveit_config/launch/default_warehouse_db.launch
+++ b/panda_softhand_moveit_config/launch/default_warehouse_db.launch
@@ -2,10 +2,10 @@
 
   <arg name="reset" default="false"/>
   <!-- If not specified, we'll use a default database location -->
-  <arg name="moveit_warehouse_database_path" default="$(find panda_moveit_config)/default_warehouse_mongo_db" />
+  <arg name="moveit_warehouse_database_path" default="$(find panda_softhand_moveit_config)/default_warehouse_mongo_db" />
 
   <!-- Launch the warehouse with the configured database location -->
-  <include file="$(find panda_moveit_config)/launch/warehouse.launch">
+  <include file="$(find panda_softhand_moveit_config)/launch/warehouse.launch">
     <arg name="moveit_warehouse_database_path" value="$(arg moveit_warehouse_database_path)" />
   </include>
 
diff --git a/panda_softhand_moveit_config/launch/demo.launch b/panda_softhand_moveit_config/launch/demo.launch
index a8a9a86..bd87314 100644
--- a/panda_softhand_moveit_config/launch/demo.launch
+++ b/panda_softhand_moveit_config/launch/demo.launch
@@ -3,7 +3,7 @@
   <!-- By default, we do not start a database (it can be large) -->
   <arg name="db" default="false" />
   <!-- Allow user to specify database location -->
-  <arg name="db_path" default="$(find panda_moveit_config)/default_warehouse_mongo_db" />
+  <arg name="db_path" default="$(find panda_softhand_moveit_config)/default_warehouse_mongo_db" />
 
   <!-- By default, we are not in debug mode -->
   <arg name="debug" default="false" />
@@ -20,7 +20,7 @@
   <arg name="use_gui" default="false" />
 
   <!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->
-  <include file="$(find panda_moveit_config)/launch/planning_context.launch">
+  <include file="$(find panda_softhand_moveit_config)/launch/planning_context.launch">
     <arg name="load_robot_description" value="true"/>
   </include>
 
@@ -38,7 +38,7 @@
   <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
 
   <!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
-  <include file="$(find panda_moveit_config)/launch/move_group.launch">
+  <include file="$(find panda_softhand_moveit_config)/launch/move_group.launch">
     <arg name="allow_trajectory_execution" value="true"/>
     <arg name="fake_execution" value="true"/>
     <arg name="info" value="true"/>
@@ -46,13 +46,13 @@
   </include>
 
   <!-- Run Rviz -->
-  <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch">
+  <include file="$(find panda_softhand_moveit_config)/launch/moveit_rviz.launch">
     <arg name="rviz_tutorial" value="$(arg rviz_tutorial)"/>
     <arg name="debug" value="$(arg debug)"/>
   </include>
 
   <!-- If database loading was enabled, start mongodb as well -->
-  <include file="$(find panda_moveit_config)/launch/default_warehouse_db.launch" if="$(arg db)">
+  <include file="$(find panda_softhand_moveit_config)/launch/default_warehouse_db.launch" if="$(arg db)">
     <arg name="moveit_warehouse_database_path" value="$(arg db_path)"/>
   </include>
 
diff --git a/panda_softhand_moveit_config/launch/demo_chomp.launch b/panda_softhand_moveit_config/launch/demo_chomp.launch
index 1ae28f9..8d697e5 100644
--- a/panda_softhand_moveit_config/launch/demo_chomp.launch
+++ b/panda_softhand_moveit_config/launch/demo_chomp.launch
@@ -3,7 +3,7 @@
   <!-- By default, we do not start a database (it can be large) -->
   <arg name="db" default="false" />
   <!-- Allow user to specify database location -->
-  <arg name="db_path" default="$(find panda_moveit_config)/default_warehouse_mongo_db" />
+  <arg name="db_path" default="$(find panda_softhand_moveit_config)/default_warehouse_mongo_db" />
 
   <!-- By default, we are not in debug mode -->
   <arg name="debug" default="false" />
@@ -20,7 +20,7 @@
   <arg name="use_gui" default="false" />
 
   <!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->
-  <include file="$(find panda_moveit_config)/launch/planning_context.launch">
+  <include file="$(find panda_softhand_moveit_config)/launch/planning_context.launch">
     <arg name="load_robot_description" value="true"/>
   </include>
 
@@ -38,7 +38,7 @@
   <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
 
   <!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
-  <include file="$(find panda_moveit_config)/launch/move_group.launch">
+  <include file="$(find panda_softhand_moveit_config)/launch/move_group.launch">
     <arg name="allow_trajectory_execution" value="true"/>
     <arg name="fake_execution" value="true"/>
     <arg name="info" value="true"/>
@@ -47,13 +47,13 @@
   </include>
 
   <!-- Run Rviz and load the default config to see the state of the move_group node -->
-  <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch">
+  <include file="$(find panda_softhand_moveit_config)/launch/moveit_rviz.launch">
     <arg name="rviz_tutorial" value="$(arg rviz_tutorial)"/>
     <arg name="debug" value="$(arg debug)"/>
   </include>
 
   <!-- If database loading was enabled, start mongodb as well -->
-  <include file="$(find panda_moveit_config)/launch/default_warehouse_db.launch" if="$(arg db)">
+  <include file="$(find panda_softhand_moveit_config)/launch/default_warehouse_db.launch" if="$(arg db)">
     <arg name="moveit_warehouse_database_path" value="$(arg db_path)"/>
   </include>
 
diff --git a/panda_softhand_moveit_config/launch/fake_moveit_controller_manager.launch.xml b/panda_softhand_moveit_config/launch/fake_moveit_controller_manager.launch.xml
index c95563d..71d07a0 100644
--- a/panda_softhand_moveit_config/launch/fake_moveit_controller_manager.launch.xml
+++ b/panda_softhand_moveit_config/launch/fake_moveit_controller_manager.launch.xml
@@ -4,6 +4,6 @@
   <param name="moveit_controller_manager" value="moveit_fake_controller_manager/MoveItFakeControllerManager"/>
 
   <!-- The rest of the params are specific to this plugin -->
-  <rosparam file="$(find panda_moveit_config)/config/fake_controllers.yaml"/>
+  <rosparam file="$(find panda_softhand_moveit_config)/config/fake_controllers.yaml"/>
 
 </launch>
diff --git a/panda_softhand_moveit_config/launch/move_group.launch b/panda_softhand_moveit_config/launch/move_group.launch
index ccc7150..528cec0 100644
--- a/panda_softhand_moveit_config/launch/move_group.launch
+++ b/panda_softhand_moveit_config/launch/move_group.launch
@@ -2,7 +2,7 @@
 
   <arg name="load_gripper" default="true" />
 
-  <include file="$(find panda_moveit_config)/launch/planning_context.launch">
+  <include file="$(find panda_softhand_moveit_config)/launch/planning_context.launch">
     <arg name="load_gripper" value="$(arg load_gripper)" />
   </include>
 
@@ -12,7 +12,7 @@
   <arg name="debug" default="false" />
   <arg unless="$(arg debug)" name="launch_prefix" value="" />
   <arg     if="$(arg debug)" name="launch_prefix"
-	   value="gdb -x $(find panda_moveit_config)/launch/gdb_settings.gdb --ex run --args" />
+	   value="gdb -x $(find panda_softhand_moveit_config)/launch/gdb_settings.gdb --ex run --args" />
 
   <!-- Verbose Mode Option -->
   <arg name="info" default="$(arg debug)" />
@@ -27,12 +27,12 @@
   <arg name="publish_monitored_planning_scene" default="true"/>
 
   <!-- Planning Functionality -->
-  <include ns="move_group" file="$(find panda_moveit_config)/launch/planning_pipeline.launch.xml">
+  <include ns="move_group" file="$(find panda_softhand_moveit_config)/launch/planning_pipeline.launch.xml">
     <arg name="pipeline" value="$(arg planner)" />
   </include>
 
   <!-- Trajectory Execution Functionality -->
-  <include ns="move_group" file="$(find panda_moveit_config)/launch/trajectory_execution.launch.xml" if="$(arg allow_trajectory_execution)">
+  <include ns="move_group" file="$(find panda_softhand_moveit_config)/launch/trajectory_execution.launch.xml" if="$(arg allow_trajectory_execution)">
     <arg name="moveit_manage_controllers" value="true" />
     <arg name="moveit_controller_manager" value="panda" if="$(eval not arg('fake_execution') and not arg('load_gripper'))"/>
     <arg name="moveit_controller_manager" value="panda_gripper" if="$(eval not arg('fake_execution') and arg('load_gripper'))"/>
@@ -40,7 +40,7 @@
   </include>
 
   <!-- Sensors Functionality -->
-  <include ns="move_group" file="$(find panda_moveit_config)/launch/sensor_manager.launch.xml" if="$(arg allow_trajectory_execution)">
+  <include ns="move_group" file="$(find panda_softhand_moveit_config)/launch/sensor_manager.launch.xml" if="$(arg allow_trajectory_execution)">
     <arg name="moveit_sensor_manager" value="panda" />
   </include>
 
diff --git a/panda_softhand_moveit_config/launch/moveit_rviz.launch b/panda_softhand_moveit_config/launch/moveit_rviz.launch
index 016e5dc..82b2e8a 100644
--- a/panda_softhand_moveit_config/launch/moveit_rviz.launch
+++ b/panda_softhand_moveit_config/launch/moveit_rviz.launch
@@ -5,12 +5,12 @@
   <arg     if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />
 
   <arg name="rviz_tutorial" default="false" />
-  <arg unless="$(arg rviz_tutorial)" name="command_args" value="-d $(find panda_moveit_config)/launch/moveit.rviz" />
-  <arg     if="$(arg rviz_tutorial)" name="command_args" value="-d $(find panda_moveit_config)/launch/moveit_empty.rviz" />
+  <arg unless="$(arg rviz_tutorial)" name="command_args" value="-d $(find panda_softhand_moveit_config)/launch/moveit.rviz" />
+  <arg     if="$(arg rviz_tutorial)" name="command_args" value="-d $(find panda_softhand_moveit_config)/launch/moveit_empty.rviz" />
 
   <node name="$(anon rviz)" launch-prefix="$(arg launch_prefix)" pkg="rviz" type="rviz" respawn="false"
 	args="$(arg command_args)" output="screen">
-    <rosparam command="load" file="$(find panda_moveit_config)/config/kinematics.yaml"/>
+    <rosparam command="load" file="$(find panda_softhand_moveit_config)/config/kinematics.yaml"/>
   </node>
 
 </launch>
diff --git a/panda_softhand_moveit_config/launch/ompl_planning_pipeline.launch.xml b/panda_softhand_moveit_config/launch/ompl_planning_pipeline.launch.xml
index f6794d5..0ee39e4 100644
--- a/panda_softhand_moveit_config/launch/ompl_planning_pipeline.launch.xml
+++ b/panda_softhand_moveit_config/launch/ompl_planning_pipeline.launch.xml
@@ -17,6 +17,6 @@
   <param name="request_adapters" value="$(arg planning_adapters)" />
   <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
 
-  <rosparam command="load" file="$(find panda_moveit_config)/config/ompl_planning.yaml"/>
+  <rosparam command="load" file="$(find panda_softhand_moveit_config)/config/ompl_planning.yaml"/>
 
 </launch>
diff --git a/panda_softhand_moveit_config/launch/panda_control_moveit_rviz.launch b/panda_softhand_moveit_config/launch/panda_control_moveit_rviz.launch
index 748f260..ec02a62 100644
--- a/panda_softhand_moveit_config/launch/panda_control_moveit_rviz.launch
+++ b/panda_softhand_moveit_config/launch/panda_control_moveit_rviz.launch
@@ -9,9 +9,9 @@
     <arg name="load_gripper" value="$(arg load_gripper)" />
   </include>
 
-  <include file="$(find panda_moveit_config)/launch/panda_moveit.launch">
+  <include file="$(find panda_softhand_moveit_config)/launch/panda_moveit.launch">
     <arg name="load_gripper" value="$(arg load_gripper)" />
   </include>
 
-  <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" if="$(arg launch_rviz)" />
+  <include file="$(find panda_softhand_moveit_config)/launch/moveit_rviz.launch" if="$(arg launch_rviz)" />
 </launch>
diff --git a/panda_softhand_moveit_config/launch/panda_gripper_moveit_controller_manager.launch.xml b/panda_softhand_moveit_config/launch/panda_gripper_moveit_controller_manager.launch.xml
index 27ffcea..db59fa1 100644
--- a/panda_softhand_moveit_config/launch/panda_gripper_moveit_controller_manager.launch.xml
+++ b/panda_softhand_moveit_config/launch/panda_gripper_moveit_controller_manager.launch.xml
@@ -3,5 +3,5 @@
   <arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
   <param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>
   <!-- load controller_list -->
-  <rosparam file="$(find panda_moveit_config)/config/panda_gripper_controllers.yaml"/>
+  <rosparam file="$(find panda_softhand_moveit_config)/config/panda_gripper_controllers.yaml"/>
  </launch>
diff --git a/panda_softhand_moveit_config/launch/panda_moveit.launch b/panda_softhand_moveit_config/launch/panda_moveit.launch
index 8559413..550a445 100644
--- a/panda_softhand_moveit_config/launch/panda_moveit.launch
+++ b/panda_softhand_moveit_config/launch/panda_moveit.launch
@@ -4,7 +4,7 @@
 
   <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="position_joint_trajectory_controller"/>
 
-  <include file="$(find panda_moveit_config)/launch/move_group.launch">
+  <include file="$(find panda_softhand_moveit_config)/launch/move_group.launch">
     <arg name="load_gripper" value="$(arg load_gripper)" />
   </include>
 </launch>
diff --git a/panda_softhand_moveit_config/launch/panda_moveit_controller_manager.launch.xml b/panda_softhand_moveit_config/launch/panda_moveit_controller_manager.launch.xml
index ea4c0ad..a09b324 100644
--- a/panda_softhand_moveit_config/launch/panda_moveit_controller_manager.launch.xml
+++ b/panda_softhand_moveit_config/launch/panda_moveit_controller_manager.launch.xml
@@ -3,5 +3,5 @@
   <arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
   <param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>
   <!-- load controller_list -->
-  <rosparam file="$(find panda_moveit_config)/config/panda_controllers.yaml"/>
+  <rosparam file="$(find panda_softhand_moveit_config)/config/panda_controllers.yaml"/>
  </launch>
diff --git a/panda_softhand_moveit_config/launch/planning_context.launch b/panda_softhand_moveit_config/launch/planning_context.launch
index e961339..5b0af6b 100644
--- a/panda_softhand_moveit_config/launch/planning_context.launch
+++ b/panda_softhand_moveit_config/launch/planning_context.launch
@@ -12,17 +12,17 @@
   <param if="$(eval arg('load_robot_description') and not arg('load_gripper'))" name="$(arg robot_description)" command="$(find xacro)/xacro --inorder '$(find franka_description)/robots/panda_arm.urdf.xacro'"/>
 
   <!-- The semantic description that corresponds to the URDF -->
-  <param name="$(arg robot_description)_semantic" command="$(find xacro)/xacro --inorder '$(find panda_moveit_config)/config/panda_arm_hand.srdf.xacro'" if="$(arg load_gripper)" />
-  <param name="$(arg robot_description)_semantic" command="$(find xacro)/xacro --inorder '$(find panda_moveit_config)/config/panda_arm.srdf.xacro'" unless="$(arg load_gripper)" />
+  <param name="$(arg robot_description)_semantic" command="$(find xacro)/xacro --inorder '$(find panda_softhand_moveit_config)/config/panda_arm_hand.srdf.xacro'" if="$(arg load_gripper)" />
+  <param name="$(arg robot_description)_semantic" command="$(find xacro)/xacro --inorder '$(find panda_softhand_moveit_config)/config/panda_arm.srdf.xacro'" unless="$(arg load_gripper)" />
   
   <!-- Load updated joint limits (override information from URDF) -->
   <group ns="$(arg robot_description)_planning">
-    <rosparam command="load" file="$(find panda_moveit_config)/config/joint_limits.yaml"/>
+    <rosparam command="load" file="$(find panda_softhand_moveit_config)/config/joint_limits.yaml"/>
   </group>
 
   <!-- Load default settings for kinematics; these settings are overridden by settings in a node's namespace -->
   <group ns="$(arg robot_description)_kinematics">
-    <rosparam command="load" file="$(find panda_moveit_config)/config/kinematics.yaml"/>
+    <rosparam command="load" file="$(find panda_softhand_moveit_config)/config/kinematics.yaml"/>
   </group>
   
 </launch>
diff --git a/panda_softhand_moveit_config/launch/planning_pipeline.launch.xml b/panda_softhand_moveit_config/launch/planning_pipeline.launch.xml
index ac6838d..ce686a5 100644
--- a/panda_softhand_moveit_config/launch/planning_pipeline.launch.xml
+++ b/panda_softhand_moveit_config/launch/planning_pipeline.launch.xml
@@ -5,6 +5,6 @@
 
   <arg name="pipeline" default="ompl" />
 
-  <include file="$(find panda_moveit_config)/launch/$(arg pipeline)_planning_pipeline.launch.xml" />
+  <include file="$(find panda_softhand_moveit_config)/launch/$(arg pipeline)_planning_pipeline.launch.xml" />
 
 </launch>
diff --git a/panda_softhand_moveit_config/launch/run_benchmark_ompl.launch b/panda_softhand_moveit_config/launch/run_benchmark_ompl.launch
index 6bf8ba2..0b0a4ff 100644
--- a/panda_softhand_moveit_config/launch/run_benchmark_ompl.launch
+++ b/panda_softhand_moveit_config/launch/run_benchmark_ompl.launch
@@ -4,19 +4,19 @@
   <arg name="cfg" />
 
   <!-- Load URDF -->
-  <include file="$(find panda_moveit_config)/launch/planning_context.launch">
+  <include file="$(find panda_softhand_moveit_config)/launch/planning_context.launch">
     <arg name="load_robot_description" value="true"/>
   </include>
 
   <!-- Start the database -->
-  <include file="$(find panda_moveit_config)/launch/warehouse.launch">
+  <include file="$(find panda_softhand_moveit_config)/launch/warehouse.launch">
     <arg name="moveit_warehouse_database_path" value="moveit_ompl_benchmark_warehouse"/>
   </include>  
 
   <!-- Start Benchmark Executable -->
   <node name="$(anon moveit_benchmark)" pkg="moveit_ros_benchmarks" type="moveit_run_benchmark" args="$(arg cfg) --benchmark-planners" respawn="false" output="screen">
-    <rosparam command="load" file="$(find panda_moveit_config)/config/kinematics.yaml"/>
-    <rosparam command="load" file="$(find panda_moveit_config)/config/ompl_planning.yaml"/>
+    <rosparam command="load" file="$(find panda_softhand_moveit_config)/config/kinematics.yaml"/>
+    <rosparam command="load" file="$(find panda_softhand_moveit_config)/config/ompl_planning.yaml"/>
   </node>
 
 </launch>
diff --git a/panda_softhand_moveit_config/launch/sensor_manager.launch.xml b/panda_softhand_moveit_config/launch/sensor_manager.launch.xml
index 67aa2fd..38922ad 100644
--- a/panda_softhand_moveit_config/launch/sensor_manager.launch.xml
+++ b/panda_softhand_moveit_config/launch/sensor_manager.launch.xml
@@ -3,16 +3,17 @@
   <!-- This file makes it easy to include the settings for sensor managers -->
 
   <!-- Param for kinect config -->
-  <rosparam command="load" file="$(find panda_moveit_config)/config/sensors_kinect_pointcloud.yaml" />
+  <rosparam command="load" file="$(find panda_softhand_moveit_config)/config/sensors_kinect_pointcloud.yaml" />
 
   <!-- Params for the octomap monitor -->
   <!--  <param name="octomap_frame" type="string" value="some frame in which the robot moves" /> -->
-  <param name="octomap_frame" type="string" value="camera_rgb_optical_frame" />
+  <!-- Adding a trial_ in front of the frame name for disabling octomap -->
+  <param name="octomap_frame" type="string" value="trial_camera_rgb_optical_frame" />   
   <param name="octomap_resolution" type="double" value="0.05" />
   <param name="max_range" type="double" value="5.0" />
 
   <!-- Load the robot specific sensor manager; this sets the moveit_sensor_manager ROS parameter -->
   <arg name="moveit_sensor_manager" default="panda" />
-  <include file="$(find panda_moveit_config)/launch/$(arg moveit_sensor_manager)_moveit_sensor_manager.launch.xml" />
+  <include file="$(find panda_softhand_moveit_config)/launch/$(arg moveit_sensor_manager)_moveit_sensor_manager.launch.xml" />
 
 </launch>
diff --git a/panda_softhand_moveit_config/launch/setup_assistant.launch b/panda_softhand_moveit_config/launch/setup_assistant.launch
index 95d88d7..274ba52 100644
--- a/panda_softhand_moveit_config/launch/setup_assistant.launch
+++ b/panda_softhand_moveit_config/launch/setup_assistant.launch
@@ -8,7 +8,7 @@
 
   <!-- Run -->
   <node pkg="moveit_setup_assistant" type="moveit_setup_assistant" name="moveit_setup_assistant" 
-	args="--config_pkg=panda_moveit_config"
+	args="--config_pkg=panda_softhand_moveit_config"
 	launch-prefix="$(arg launch_prefix)"
         output="screen" />
 
diff --git a/panda_softhand_moveit_config/launch/stomp_planning_pipeline.launch.xml b/panda_softhand_moveit_config/launch/stomp_planning_pipeline.launch.xml
index 7e1c5b8..d390a01 100644
--- a/panda_softhand_moveit_config/launch/stomp_planning_pipeline.launch.xml
+++ b/panda_softhand_moveit_config/launch/stomp_planning_pipeline.launch.xml
@@ -14,6 +14,6 @@
   <param name="planning_plugin" value="$(arg planning_plugin)" />
   <param name="request_adapters" value="$(arg planning_adapters)" />
   <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
-  <rosparam command="load" file="$(find panda_moveit_config)/config/stomp_planning.yaml"/>
+  <rosparam command="load" file="$(find panda_softhand_moveit_config)/config/stomp_planning.yaml"/>
 
 </launch>
diff --git a/panda_softhand_moveit_config/launch/trajectory_execution.launch.xml b/panda_softhand_moveit_config/launch/trajectory_execution.launch.xml
index b6c03c9..1bffbea 100644
--- a/panda_softhand_moveit_config/launch/trajectory_execution.launch.xml
+++ b/panda_softhand_moveit_config/launch/trajectory_execution.launch.xml
@@ -15,6 +15,6 @@
   
   <!-- Load the robot specific controller manager; this sets the moveit_controller_manager ROS parameter -->
   <arg name="moveit_controller_manager" default="panda" />
-  <include file="$(find panda_moveit_config)/launch/$(arg moveit_controller_manager)_moveit_controller_manager.launch.xml" />
+  <include file="$(find panda_softhand_moveit_config)/launch/$(arg moveit_controller_manager)_moveit_controller_manager.launch.xml" />
   
 </launch>
diff --git a/panda_softhand_moveit_config/launch/warehouse.launch b/panda_softhand_moveit_config/launch/warehouse.launch
index a4a1374..8bf3f18 100644
--- a/panda_softhand_moveit_config/launch/warehouse.launch
+++ b/panda_softhand_moveit_config/launch/warehouse.launch
@@ -4,7 +4,7 @@
   <arg name="moveit_warehouse_database_path" />
 
   <!-- Load warehouse parameters -->  
-  <include file="$(find panda_moveit_config)/launch/warehouse_settings.launch.xml" />
+  <include file="$(find panda_softhand_moveit_config)/launch/warehouse_settings.launch.xml" />
 
   <!-- Run the DB server -->
   <node name="$(anon mongo_wrapper_ros)" cwd="ROS_HOME" type="mongo_wrapper_ros.py" pkg="warehouse_ros_mongo">
diff --git a/panda_softhand_moveit_config/package.xml b/panda_softhand_moveit_config/package.xml
index be8ecca..dfb8466 100644
--- a/panda_softhand_moveit_config/package.xml
+++ b/panda_softhand_moveit_config/package.xml
@@ -2,7 +2,7 @@
   <name>panda_softhand_moveit_config</name>
   <version>1.0.0</version>
   <description>
-     A package, based on panda_moveit_config, with all the configuration and launch files for using the Panda and Pisa/IIT SoftHand with MoveIt!
+     A package, based on panda_softhand_moveit_config, with all the configuration and launch files for using the Panda and Pisa/IIT SoftHand with MoveIt!
   </description>
   <author email="georgejose.pollayil@phd.unipi.it">George Jose Pollayil</author>
   <maintainer email="mathewjose.pollayil@phd.unipi.it">Mathew Jose Pollayil</maintainer>