diff --git a/CMakeLists.txt b/CMakeLists.txt
index 2b315d3ee71d0bb78402ba2a107db379b880afb0..dc49f334d753a972d1988d22478b18b498b67559 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -51,13 +51,13 @@ catkin_package(CATKIN_DEPENDS
 # Specify additional locations of header files Your package locations should be listed before other locations
 include_directories(${catkin_INCLUDE_DIRS})
 
-add_executable(SampleConstraintPlanner src/SampleConstraintPlanner.cpp)
-add_executable(SampleTimedCartesianPlanner src/SampleTimedCartesianPlanner.cpp)
-add_executable(SampleSimpleMotion src/SampleSimpleMotion.cpp)
-add_executable(MinimalSimpleMotion src/MinimalSimpleMotion.cpp)
-
-# Specify libraries to link a library or executable target against
-target_link_libraries(SampleConstraintPlanner ${catkin_LIBRARIES})
-target_link_libraries(SampleTimedCartesianPlanner ${catkin_LIBRARIES})
-target_link_libraries(SampleSimpleMotion ${catkin_LIBRARIES})
-target_link_libraries(MinimalSimpleMotion ${catkin_LIBRARIES})
+add_executable(BasicCartesianPlanner src/BasicCartesianPlanner.cpp)
+add_executable(BasicJointSpacePlanner src/BasicJointSpacePlanner.cpp)
+add_executable(ObstacleAwarePlanner src/ObstacleAwarePlanner.cpp)
+add_executable(GraspPlanner src/GraspPlanner.cpp)
+
+## Add cmake target dependencies of the executable
+target_link_libraries(BasicCartesianPlanner ${catkin_LIBRARIES})
+target_link_libraries(BasicJointSpacePlanner ${catkin_LIBRARIES})
+target_link_libraries(ObstacleAwarePlanner ${catkin_LIBRARIES})
+target_link_libraries(GraspPlanner ${catkin_LIBRARIES})
diff --git a/launch/simple_simulation.launch b/launch/basic_cartesian_planner.launch
similarity index 51%
rename from launch/simple_simulation.launch
rename to launch/basic_cartesian_planner.launch
index 46e92068d77b1b4e2e6f02bc9175552800abd7b0..08b8ee94d354e46f78857be4c06d94d4928ebf99 100644
--- a/launch/simple_simulation.launch
+++ b/launch/basic_cartesian_planner.launch
@@ -4,8 +4,6 @@
 
     <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" />
 
-    <node name="rqt_console" pkg="rqt_console" type="rqt_console" />
-
-    <node pkg="sample_applications" type="SampleSimpleMotion" name="SampleSimpleMotion" />
+    <node pkg="sample_applications" type="BasicCartesianPlanner" name="BasicCartesianPlannerInstance" respawn="false" output="screen"/>
 
 </launch>
diff --git a/launch/constraint_simulation.launch b/launch/basic_joint_space_planner.launch
similarity index 50%
rename from launch/constraint_simulation.launch
rename to launch/basic_joint_space_planner.launch
index 238a49a20b43fc32a01eb905e0bca2d8fbb0cf5a..37722cde2302b8e5934a6fdac3a68723bec146b1 100644
--- a/launch/constraint_simulation.launch
+++ b/launch/basic_joint_space_planner.launch
@@ -4,8 +4,6 @@
 
     <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" />
 
-    <node name="rqt_console" pkg="rqt_console" type="rqt_console" />
-
-    <node pkg="sample_applications" type="SampleConstraintPlanner" name="SampleConstraintPlanner" />
+    <node pkg="sample_applications" type="BasicJointSpacePlanner" name="BasicJointSpacePlannerInstance" respawn="false" output="screen"/>
 
 </launch>
diff --git a/launch/grasp_planner.launch b/launch/grasp_planner.launch
new file mode 100644
index 0000000000000000000000000000000000000000..e1cbf6c4d40f803cc52acf2a99b8005312e7ee64
--- /dev/null
+++ b/launch/grasp_planner.launch
@@ -0,0 +1,13 @@
+<launch>
+
+    <include file="$(find panda_simulation)/launch/simulation.launch"/>
+
+    <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" />
+
+    <node name="spawn_object" pkg="gazebo_ros" type="spawn_model" args="-urdf -z 1 -file $(find tutorial_moveit_motion_planning)/models/object.urdf -model c_object" respawn="false" output="screen" />
+    <node name="spawn_table1" pkg="gazebo_ros" type="spawn_model" args="-urdf -z 1 -file $(find tutorial_moveit_motion_planning)/models/table1.urdf -model c_table1" respawn="false" output="screen" />
+    <node name="spawn_table2" pkg="gazebo_ros" type="spawn_model" args="-urdf -z 1 -file $(find tutorial_moveit_motion_planning)/models/table2.urdf -model c_table2" respawn="false" output="screen" />
+
+    <node pkg="sample_applications" type="GraspPlanner" name="GraspPlannerInstance" respawn="false" output="screen"/>
+
+</launch>
diff --git a/launch/minimal_simulation.launch b/launch/minimal_simulation.launch
deleted file mode 100644
index 89e45937886d145e20552326aac3bcd36ec3f715..0000000000000000000000000000000000000000
--- a/launch/minimal_simulation.launch
+++ /dev/null
@@ -1,7 +0,0 @@
-<launch>
-
-    <include file="$(find panda_simulation)/launch/simulation.launch"/>
-
-    <node pkg="sample_applications" type="MinimalSimpleMotion" name="MinimalSimpleMotion" />
-
-</launch>
diff --git a/launch/obstacle_aware_planner.launch b/launch/obstacle_aware_planner.launch
new file mode 100644
index 0000000000000000000000000000000000000000..cbd564c10bff2df4c02bf29fa58a09a9c2dd75f4
--- /dev/null
+++ b/launch/obstacle_aware_planner.launch
@@ -0,0 +1,11 @@
+<launch>
+
+    <include file="$(find panda_simulation)/launch/simulation.launch"/>
+
+    <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" />
+
+    <node name="spawn_box" pkg="gazebo_ros" type="spawn_model" args="-urdf -z 1 -file $(find tutorial_moveit_motion_planning)/models/box.urdf -model box" respawn="false" output="screen" />
+
+    <node pkg="sample_applications" type="ObstacleAwarePlanner" name="ObstacleAwarePlannerInstance" respawn="false" output="screen"/>
+
+</launch>
diff --git a/launch/timed_cartesian_simulation.launch b/launch/timed_cartesian_simulation.launch
deleted file mode 100644
index d688ff03d7d2d42d699a847c6ead6a7bec767278..0000000000000000000000000000000000000000
--- a/launch/timed_cartesian_simulation.launch
+++ /dev/null
@@ -1,11 +0,0 @@
-<launch>
-
-    <include file="$(find panda_simulation)/launch/simulation.launch"/>
-
-    <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" />
-
-    <node name="rqt_console" pkg="rqt_console" type="rqt_console" />
-
-    <node pkg="sample_applications" type="SampleTimedCartesianPlanner" name="SampleTimedCartesianPlanner" />
-
-</launch>
diff --git a/models/box.urdf b/models/box.urdf
new file mode 100644
index 0000000000000000000000000000000000000000..6821cbefdf72624777075976b17904d93255fbfa
--- /dev/null
+++ b/models/box.urdf
@@ -0,0 +1,29 @@
+<robot name="box">
+    <link name="world"/>
+    <gazebo reference="world">
+        <static>true</static>
+    </gazebo>
+    <joint name="fixed" type="fixed">
+        <parent link="world"/>
+        <child link="real_box"/>
+    </joint>
+    <link name="real_box">
+        <inertial>
+            <origin xyz="0.45 -0.2 0.0" />
+            <mass value="1.0" />
+            <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0" />
+        </inertial>
+        <visual>
+            <origin xyz="0.45 -0.2 0.0"/>
+            <geometry>
+                <box size="0.4 0.1 0.3" />
+            </geometry>
+        </visual>
+        <collision>
+            <origin xyz="0.45 -0.2 0.0"/>
+            <geometry>
+                <box size="0.4 0.1 0.3" />
+            </geometry>
+        </collision>
+    </link>
+</robot>
\ No newline at end of file
diff --git a/src/BasicCartesianPlanner.cpp b/src/BasicCartesianPlanner.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5240eab83513efcdf0b5eedf80c8fd55d41f7706
--- /dev/null
+++ b/src/BasicCartesianPlanner.cpp
@@ -0,0 +1,100 @@
+#include <moveit/move_group_interface/move_group_interface.h>
+
+#include <moveit_msgs/DisplayTrajectory.h>
+#include <moveit_msgs/CollisionObject.h>
+
+#include <moveit_visual_tools/moveit_visual_tools.h>
+#include <moveit/trajectory_processing/iterative_time_parameterization.h>
+
+#include <trajectory_msgs/JointTrajectoryPoint.h>
+
+/**
+ * simple demo of a robotic motions based on cartesian paths
+ */
+int main(int argc, char **argv) {
+    // init the node
+    ros::init(argc, argv, "basic_cartesian_planner");
+    ros::NodeHandle node_handle;
+    ros::AsyncSpinner spinner(1);
+    spinner.start();
+
+    // setup the planning environment
+    static const std::string PLANNING_GROUP = "panda_arm";
+    moveit::planning_interface::MoveGroupInterface group(PLANNING_GROUP);
+
+    // visualization setup
+    // MoveIts visual tools provide capabilities for visualizing objects, robots, and trajectories in RViz
+    namespace rvt = rviz_visual_tools;
+    moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0");
+
+    // Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations
+    visual_tools.enableBatchPublishing();
+
+    // remote control is an introspection tool that allows users to step through a high level script
+    // via buttons and keyboard shortcuts in RViz
+    visual_tools.loadRemoteControl();
+    ros::Duration(3.0).sleep();
+
+    // we can use visual_tools to wait for user input
+    visual_tools.prompt("Press 'next' to move the simulated robot. \n");
+    visual_tools.trigger();
+
+    group.setStartStateToCurrentState();
+
+    // You can plan a Cartesian path directly by specifying a list of waypoints
+    // for the end-effector to go through. Note that we are starting
+    // from the new start state above.
+    std::vector<geometry_msgs::Pose> waypoints;
+
+    // initial pose of the robot
+    geometry_msgs::Pose start_pose = group.getCurrentPose().pose;
+
+    // waypoints of the target trajectory
+    geometry_msgs::Pose target_pose_1 = group.getCurrentPose().pose;
+    geometry_msgs::Pose target_pose_2 = group.getCurrentPose().pose;
+    geometry_msgs::Pose target_pose_3 = group.getCurrentPose().pose;
+
+    target_pose_1.position.z = start_pose.position.z - 0.4;
+    target_pose_1.position.y = start_pose.position.y;
+    target_pose_1.position.x = start_pose.position.x + 0.5;
+    waypoints.push_back(target_pose_1);
+
+    target_pose_2.position.z = 0.6;
+    target_pose_2.position.y = -0.6;
+    target_pose_2.position.x = 0;
+    waypoints.push_back(target_pose_2);
+
+    target_pose_3.position.z = 0.6;
+    target_pose_3.position.y = -0.032;
+    target_pose_3.position.x = -0.6;
+    waypoints.push_back(target_pose_3);
+
+    group.setPlanningTime(10.0);
+
+    // create a plan object and compute the cartesian motion
+    moveit::planning_interface::MoveGroupInterface::Plan plan;
+    moveit_msgs::RobotTrajectory trajectory_msg;
+    group.computeCartesianPath(waypoints, 0.01, 0.0, trajectory_msg, true);
+    plan.trajectory_ = trajectory_msg;
+
+    // uncomment to modify maximum velocity and acceleration of the motion to be planned
+    /*
+    robot_trajectory::RobotTrajectory rt(group.getCurrentState()->getRobotModel(), PLANNING_GROUP);
+    trajectory_processing::IterativeParabolicTimeParameterization iptp;
+
+    double max_velocity_scaling_factor  = 0.1;
+    double max_acceleration_scaling_factor = 0.1;
+
+    iptp.computeTimeStamps(rt, max_velocity_scaling_factor, max_acceleration_scaling_factor);
+    rt.getRobotTrajectoryMsg(trajectory_msg);
+    */
+
+    visual_tools.trigger();
+    visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to move the simulated robot");
+
+    // execute the planned motion
+    group.execute(plan);
+
+    ros::shutdown();
+    return 0;
+}
\ No newline at end of file
diff --git a/src/SampleSimpleMotion.cpp b/src/BasicJointSpacePlanner.cpp
similarity index 53%
rename from src/SampleSimpleMotion.cpp
rename to src/BasicJointSpacePlanner.cpp
index d74b1253f0d5c0a7a3b5bcd671029de880626e40..e99010f5c54098239997056c5a891ce0860209e7 100644
--- a/src/SampleSimpleMotion.cpp
+++ b/src/BasicJointSpacePlanner.cpp
@@ -1,10 +1,6 @@
 #include <moveit/move_group_interface/move_group_interface.h>
 #include <moveit/planning_scene_interface/planning_scene_interface.h>
 
-#include <moveit_msgs/DisplayRobotState.h>
-#include <moveit_msgs/DisplayTrajectory.h>
-
-#include <moveit_msgs/AttachedCollisionObject.h>
 #include <moveit_msgs/CollisionObject.h>
 
 #include <moveit_visual_tools/moveit_visual_tools.h>
@@ -14,45 +10,45 @@
 /**
  * simple demo of a robotic motion
  */
-int main(int argc, char** argv)
-{
-    ros::init(argc, argv, "SIMPLE PLANNER");
+int main(int argc, char **argv) {
+    // init the node
+    ros::init(argc, argv, "basic_joint_space_planner");
     ros::NodeHandle node_handle;
     ros::AsyncSpinner spinner(1);
     spinner.start();
 
-    // wait for robot init of robot_state_initializer
-    ROS_INFO_NAMED("simple_planner", ">>>>>>>>>>>>>>>>> WAITING FOR ROBOT INIT <<<<<<<<<<<<<<<<");
-    ros::Duration(5.0).sleep();
-    ROS_INFO_NAMED("simple_planner", ">>>>>>>>>>>>>>>>> WAKING UP AFTER INIT <<<<<<<<<<<<<<<<");
-
+    // setup the planning environment
     static const std::string PLANNING_GROUP = "panda_arm";
     moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
     moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
-    const robot_state::JointModelGroup* joint_model_group =
-            move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);
 
-    // Visualization Setup
+    // visualization setup
+    // MoveIts visual tools provide capabilities for visualizing objects, robots, and trajectories in RViz
     namespace rvt = rviz_visual_tools;
     moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0");
-    visual_tools.deleteAllMarkers();
-    visual_tools.loadRemoteControl();
-
-    Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
-    text_pose.translation().z() = 1.75;
-    visual_tools.publishText(text_pose, "simple_planner node", rvt::WHITE, rvt::XLARGE);
 
     // Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations
+    visual_tools.enableBatchPublishing();
+
+    // remote control is an introspection tool that allows users to step through a high level script
+    // via buttons and keyboard shortcuts in RViz
+    visual_tools.loadRemoteControl();
+    ros::Duration(3.0).sleep();
+
+    // we can use visual_tools to wait for user input
+    visual_tools.prompt("Press 'next' to plan the motion. \n");
     visual_tools.trigger();
 
-    // Getting Basic Information
-    ROS_INFO_NAMED("simple_planner", "Planning frame: %s", move_group.getPlanningFrame().c_str());
-    ROS_INFO_NAMED("simple_planner", "End effector link: %s", move_group.getEndEffectorLink().c_str());
-    ROS_INFO_NAMED("simple_planner", "Available Planning Groups:");
+    // getting basic information
+    ROS_INFO("Planning frame: %s", move_group.getPlanningFrame().c_str());
+    ROS_INFO("End effector link: %s", move_group.getEndEffectorLink().c_str());
+    ROS_INFO("Available Planning Groups:");
     std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(),
               std::ostream_iterator<std::string>(std::cout, ", "));
 
-    move_group.setStartState(*move_group.getCurrentState());
+    move_group.setStartStateToCurrentState();
+
+    // define start and target pose (by specifying the desired pose of the end-effector)
     geometry_msgs::Pose another_pose;
     another_pose.orientation.w = 1.0;
     another_pose.position.x = 0.4;
@@ -60,16 +56,25 @@ int main(int argc, char** argv)
     another_pose.position.z = 0.9;
     move_group.setPoseTarget(another_pose);
 
+    // uncomment to modify maximum velocity and acceleration of the motion to be planned
+    /*
+
+    move_group.setMaxVelocityScalingFactor(0.1);
+    move_group.setMaxAccelerationScalingFactor(0.1);
+
+     */
+
+    // create a plan object and plan the motion
     moveit::planning_interface::MoveGroupInterface::Plan my_plan;
     bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
-    ROS_INFO_NAMED("simple_planner", "Visualizing constraint plan %s", success ? "" : "FAILED");
+    ROS_INFO("Visualizing plan %s", success ? "" : "FAILED");
 
-    visual_tools.prompt("Press 'next' to move the simulated robot.");
     visual_tools.trigger();
+    visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
 
-    // Move the simulated robot in gazebo
+    // Move the (simulated) robot in gazebo
     move_group.move();
 
     ros::shutdown();
     return 0;
-}
\ No newline at end of file
+}
diff --git a/src/GraspPlanner.cpp b/src/GraspPlanner.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0eabfe58fb3715fb3f7cb29bd26ec16736ac38bd
--- /dev/null
+++ b/src/GraspPlanner.cpp
@@ -0,0 +1,352 @@
+// ROS
+#include <ros/ros.h>
+
+// MoveIt!
+#include <moveit/planning_scene_interface/planning_scene_interface.h>
+#include <moveit/move_group_interface/move_group_interface.h>
+
+// TF2
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+
+geometry_msgs::Quaternion q_pick_object;
+
+geometry_msgs::Pose getTargetPickPose(){
+
+    std::vector<double> co_dim{0.02, 0.02, 0.2};
+    std::vector<double> co_pose{0.5, 0.0, 0.2};
+
+    geometry_msgs::Pose target_pose;
+
+    tf2::Quaternion orientation_gripper(-0.924, 0.383, 0, 0);
+    tf2::Quaternion orientation_object(q_pick_object.x, q_pick_object.y, q_pick_object.z, q_pick_object.w);
+
+    tf2::Quaternion orientation = orientation_gripper * orientation_object;
+    orientation.setZ(orientation.getZ() * -1);
+    orientation.setY(orientation.getY() * -1);
+    orientation.setX(orientation.getX() * -1);
+
+    target_pose.orientation  = tf2::toMsg(orientation);
+    target_pose.position.x = co_pose[0];
+    target_pose.position.y = co_pose[1];
+    target_pose.position.z = co_pose[2] + 0.3;
+
+    return target_pose;
+}
+
+void openGripper(trajectory_msgs::JointTrajectory& posture)
+{
+    // BEGIN_SUB_TUTORIAL open_gripper
+    /* Add both finger joints of panda robot. */
+    posture.joint_names.resize(2);
+    posture.joint_names[0] = "panda_finger_joint1";
+    posture.joint_names[1] = "panda_finger_joint2";
+
+    /* Set them as open, wide enough for the object to fit. */
+    posture.points.resize(1);
+    posture.points[0].positions.resize(2);
+    posture.points[0].positions[0] = 0.04;
+    posture.points[0].positions[1] = 0.04;
+    posture.points[0].time_from_start = ros::Duration(0.5);
+    // END_SUB_TUTORIAL
+}
+
+void closedGripper(trajectory_msgs::JointTrajectory& posture)
+{
+    // BEGIN_SUB_TUTORIAL closed_gripper
+    /* Add both finger joints of panda robot. */
+    posture.joint_names.resize(2);
+    posture.joint_names[0] = "panda_finger_joint1";
+    posture.joint_names[1] = "panda_finger_joint2";
+
+    /* Set them as closed. */
+    posture.points.resize(1);
+    posture.points[0].positions.resize(2);
+    posture.points[0].positions[0] = 0.0;
+    posture.points[0].positions[1] = 0.0;
+    std::cout << "pos: " << posture.points[0].positions[0] << std::endl;
+
+    posture.points[0].effort.resize(2);
+    posture.points[0].effort[0] = 10;
+    posture.points[0].effort[1] = 10;
+
+    std::cout << "s:" << posture.points[0].effort.size() << std::endl;
+    posture.points[0].time_from_start = ros::Duration(0.5);
+    // END_SUB_TUTORIAL
+}
+
+void pick(moveit::planning_interface::MoveGroupInterface& move_group)
+{
+    // BEGIN_SUB_TUTORIAL pick1
+    // Create a vector of grasps to be attempted, currently only creating single grasp.
+    // This is essentially useful when using a grasp generator to generate and test multiple grasps.
+    std::vector<moveit_msgs::Grasp> grasps;
+    grasps.resize(1);
+
+    // Setting grasp pose
+    // ++++++++++++++++++++++
+    // This is the pose of panda_link8. |br|
+    // From panda_link8 to the palm of the eef the distance is 0.058, the cube starts 0.01 before 5.0 (half of the length
+    // of the cube). |br|
+    // Therefore, the position for panda_link8 = 5 - (length of cube/2 - distance b/w panda_link8 and palm of eef - some
+    // extra padding)
+    grasps[0].grasp_pose.header.frame_id = "panda_link0";
+    /*tf2::Quaternion orientation;
+    orientation.setRPY(-M_PI / 2, -M_PI / 4, -M_PI / 2);
+    grasps[0].grasp_pose.pose.orientation = tf2::toMsg(orientation);
+    grasps[0].grasp_pose.pose.position.x = 0.415;
+    grasps[0].grasp_pose.pose.position.y = 0;
+    grasps[0].grasp_pose.pose.position.z = 0.5;*/
+
+    grasps[0].grasp_pose.pose = getTargetPickPose();
+    // Setting pre-grasp approach
+    // ++++++++++++++++++++++++++
+    /* Defined with respect to frame_id */
+    grasps[0].pre_grasp_approach.direction.header.frame_id = "panda_link0";
+    /* Direction is set as positive x axis */
+    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;
+
+    // Setting post-grasp retreat
+    // ++++++++++++++++++++++++++
+    /* Defined with respect to frame_id */
+    grasps[0].post_grasp_retreat.direction.header.frame_id = "panda_link0";
+    /* Direction is set as positive z axis */
+    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;
+    grasps[0].max_contact_force = 12;
+
+    //std::vector<std::string> atos {"c_object", "panda_rightfinger", "panda_leftfinger"};
+
+    //grasps[0].allowed_touch_objects = atos;
+    // Setting posture of eef before grasp
+    // +++++++++++++++++++++++++++++++++++
+    openGripper(grasps[0].pre_grasp_posture);
+    // END_SUB_TUTORIAL
+
+    // BEGIN_SUB_TUTORIAL pick2
+    // Setting posture of eef during grasp
+    // +++++++++++++++++++++++++++++++++++
+    closedGripper(grasps[0].grasp_posture);
+    // END_SUB_TUTORIAL
+
+    // BEGIN_SUB_TUTORIAL pick3
+    // Set support surface as table1.
+    move_group.setSupportSurfaceName("c_table1");
+    // Call pick to pick up the object using the grasps given
+    move_group.setMaxVelocityScalingFactor(0.05);
+    move_group.setMaxAccelerationScalingFactor(0.05);
+    move_group.pick("c_object", grasps);
+    //move_group.attachObject("c_object");
+    // END_SUB_TUTORIAL
+}
+
+void place(moveit::planning_interface::MoveGroupInterface& group)
+{
+    // BEGIN_SUB_TUTORIAL place
+    // TODO(@ridhwanluthra) - Calling place function may lead to "All supplied place locations failed. Retrying last
+    // location in
+    // verbose mode." This is a known issue and we are working on fixing it. |br|
+    // Create a vector of placings to be attempted, currently only creating single place location.
+    std::vector<moveit_msgs::PlaceLocation> place_location;
+    place_location.resize(1);
+
+    // Setting place location pose
+    // +++++++++++++++++++++++++++
+    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 = tf2::toMsg(orientation);
+
+    /* While placing it is the exact location of the center of the object. */
+    place_location[0].place_pose.pose.position.x = 0;
+    place_location[0].place_pose.pose.position.y = 0.5;
+    place_location[0].place_pose.pose.position.z = 0.3;
+
+    // Setting pre-place approach
+    // ++++++++++++++++++++++++++
+    /* Defined with respect to frame_id */
+    place_location[0].pre_place_approach.direction.header.frame_id = "panda_link0";
+    /* Direction is set as negative z axis */
+    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;
+
+    // Setting post-grasp retreat
+    // ++++++++++++++++++++++++++
+    /* Defined with respect to frame_id */
+    place_location[0].post_place_retreat.direction.header.frame_id = "panda_link0";
+    /* Direction is set as negative y axis */
+    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;
+
+    // Setting posture of eef after placing object
+    // +++++++++++++++++++++++++++++++++++++++++++
+    /* Similar to the pick case */
+    openGripper(place_location[0].post_place_posture);
+
+    // Set support surface as table2.
+    group.setSupportSurfaceName("c_table2");
+    // Call place to place the object using the place locations given.
+    group.setMaxVelocityScalingFactor(0.05);
+    group.setMaxAccelerationScalingFactor(0.05);
+    group.place("c_object", place_location);
+    // END_SUB_TUTORIAL
+}
+
+void addCollisionObjects(moveit::planning_interface::PlanningSceneInterface& planning_scene_interface)
+{
+    // BEGIN_SUB_TUTORIAL table1
+    //
+    // Creating Environment
+    // ^^^^^^^^^^^^^^^^^^^^
+    // Create vector to hold 4 collision objects.
+
+    std::vector<moveit_msgs::CollisionObject> collision_objects;
+    collision_objects.resize(4);
+
+
+    // Add the first table where the cube will originally be kept.
+    collision_objects[0].id = "c_table1";
+    collision_objects[0].header.frame_id = "panda_link0";
+
+    // Define the primitive and its dimensions.
+    collision_objects[0].primitives.resize(1);
+    collision_objects[0].primitives[0].type = collision_objects[0].primitives[0].BOX;
+    collision_objects[0].primitives[0].dimensions.resize(3);
+    collision_objects[0].primitives[0].dimensions[0] = 0.2;
+    collision_objects[0].primitives[0].dimensions[1] = 0.4;
+    collision_objects[0].primitives[0].dimensions[2] = 0.2;
+
+    // Define the pose of the table.
+    collision_objects[0].primitive_poses.resize(1);
+    collision_objects[0].primitive_poses[0].position.x = 0.5;
+    collision_objects[0].primitive_poses[0].position.y = 0;
+    collision_objects[0].primitive_poses[0].position.z = 0.1;
+    // END_SUB_TUTORIAL
+
+    collision_objects[0].operation = collision_objects[0].ADD;
+
+    // BEGIN_SUB_TUTORIAL table2
+    // Add the second table where we will be placing the cube.
+    collision_objects[1].id = "c_table2";
+    collision_objects[1].header.frame_id = "panda_link0";
+
+    // Define the primitive and its dimensions.
+    collision_objects[1].primitives.resize(1);
+    collision_objects[1].primitives[0].type = collision_objects[1].primitives[0].BOX;
+    collision_objects[1].primitives[0].dimensions.resize(3);
+    collision_objects[1].primitives[0].dimensions[0] = 0.4;
+    collision_objects[1].primitives[0].dimensions[1] = 0.2;
+    collision_objects[1].primitives[0].dimensions[2] = 0.2;
+
+    // Define the pose of the table.
+    collision_objects[1].primitive_poses.resize(1);
+    collision_objects[1].primitive_poses[0].position.x = 0;
+    collision_objects[1].primitive_poses[0].position.y = 0.5;
+    collision_objects[1].primitive_poses[0].position.z = 0.1;
+    // END_SUB_TUTORIAL
+
+    collision_objects[1].operation = collision_objects[1].ADD;
+
+    // BEGIN_SUB_TUTORIAL object
+    // Define the object that we will be manipulating
+    collision_objects[2].header.frame_id = "panda_link0";
+    collision_objects[2].id = "c_object";
+
+    //Define the primitive and its dimensions.
+    collision_objects[2].primitives.resize(1);
+    collision_objects[2].primitives[0].type = collision_objects[1].primitives[0].BOX;
+    collision_objects[2].primitives[0].dimensions.resize(3);
+    collision_objects[2].primitives[0].dimensions[0] = 0.02;
+    collision_objects[2].primitives[0].dimensions[1] = 0.02;
+    collision_objects[2].primitives[0].dimensions[2] = 0.2;
+
+    //Define the pose of the object.
+    collision_objects[2].primitive_poses.resize(1);
+    collision_objects[2].primitive_poses[0].position.x = 0.5;
+    collision_objects[2].primitive_poses[0].position.y = 0;
+    collision_objects[2].primitive_poses[0].position.z = 0.3;
+
+    geometry_msgs::Pose target_pose;
+    tf2::Quaternion orientation;
+    orientation.setRPY(0, 0, 0);
+
+    geometry_msgs::Pose object_pose;
+    collision_objects[2].primitive_poses[0].orientation = tf2::toMsg(orientation);
+    q_pick_object.w = tf2::toMsg(orientation).w;
+    q_pick_object.x = tf2::toMsg(orientation).x;
+    q_pick_object.y = tf2::toMsg(orientation).y;
+    q_pick_object.z = tf2::toMsg(orientation).z;
+    // END_SUB_TUTORIAL
+
+    // the surface
+    collision_objects[2].operation = collision_objects[2].ADD;
+
+    collision_objects[3].header.frame_id = "panda_link0";
+    collision_objects[3].id = "surface";
+
+    // Define the primitive and its dimensions.
+    collision_objects[3].primitives.resize(1);
+    collision_objects[3].primitives[0].type = collision_objects[1].primitives[0].BOX;
+    collision_objects[3].primitives[0].dimensions.resize(3);
+    collision_objects[3].primitives[0].dimensions[0] = 3.0;
+    collision_objects[3].primitives[0].dimensions[1] = 3.0;
+    collision_objects[3].primitives[0].dimensions[2] = 0.1;
+
+    // Define the pose of the object.
+    collision_objects[3].primitive_poses.resize(1);
+    collision_objects[3].primitive_poses[0].position.x = 0;
+    collision_objects[3].primitive_poses[0].position.y = 0;
+    collision_objects[3].primitive_poses[0].position.z = -0.1;
+    // END_SUB_TUTORIAL
+
+    collision_objects[3].operation = collision_objects[3].ADD;
+
+    planning_scene_interface.applyCollisionObjects(collision_objects);
+}
+
+int main(int argc, char** argv)
+{
+    ros::init(argc, argv, "panda_arm_pick_place");
+    ros::NodeHandle nh;
+    ros::AsyncSpinner spinner(1);
+    spinner.start();
+
+    ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>>> WAITING FOR ROBOT INIT <<<<<<<<<<<<<<<<");
+    ros::Duration(5.0).sleep();
+    ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>>> WAKING UP AFTER INIT <<<<<<<<<<<<<<<<");
+
+    if(!ros::param::has("max_grasp_approach_velocity"))
+    {
+        ros::param::set("max_grasp_approach_velocity", 0.05);
+        ros::param::set("max_grasp_approach_acceleration", 0.05);
+
+        ros::param::set("max_grasp_transition_velocity", 0.05);
+        ros::param::set("max_grasp_transition_acceleration", 0.05);
+    }
+
+    ros::WallDuration(1.0).sleep();
+    moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
+    moveit::planning_interface::MoveGroupInterface group("panda_arm");
+    group.setPlanningTime(45.0);
+
+    addCollisionObjects(planning_scene_interface);
+
+    // Wait a bit for ROS things to initialize
+    ros::WallDuration(1.0).sleep();
+
+    pick(group);
+
+    ros::WallDuration(1.0).sleep();
+
+    place(group);
+
+    //std::vector<std::string>objectsToRemove{"object"};
+    //planning_scene_interface.removeCollisionObjects(objectsToRemove);
+
+    ros::waitForShutdown();
+    return 0;
+}
\ No newline at end of file
diff --git a/src/MinimalSimpleMotion.cpp b/src/MinimalSimpleMotion.cpp
deleted file mode 100644
index 1665ccde94a5ef18a1d0e5914d5b3c0ffcd7ad2f..0000000000000000000000000000000000000000
--- a/src/MinimalSimpleMotion.cpp
+++ /dev/null
@@ -1,57 +0,0 @@
-#include <moveit/move_group_interface/move_group_interface.h>
-#include <moveit/planning_scene_interface/planning_scene_interface.h>
-
-#include <moveit_msgs/DisplayRobotState.h>
-#include <moveit_msgs/DisplayTrajectory.h>
-
-#include <trajectory_msgs/JointTrajectoryPoint.h>
-
-/**
- * minimal demo of constraint aware planning
- */
-int main(int argc, char** argv)
-{
-    ros::init(argc, argv, "SIMPLE PLANNER");
-    ros::NodeHandle node_handle;
-    ros::AsyncSpinner spinner(1);
-    spinner.start();
-
-    // wait for robot init of robot_state_initializer
-    ROS_INFO_NAMED("simple_planner", ">>>>>>>>>>>>>>>>> WAITING FOR ROBOT INIT <<<<<<<<<<<<<<<<");
-    ros::Duration(5.0).sleep();
-    ROS_INFO_NAMED("simple_planner", ">>>>>>>>>>>>>>>>> WAKING UP AFTER INIT <<<<<<<<<<<<<<<<");
-
-    static const std::string PLANNING_GROUP = "panda_arm";
-    moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
-    moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
-    const robot_state::JointModelGroup* joint_model_group =
-            move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);
-
-    Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
-    text_pose.translation().z() = 1.75;
-
-    // Getting Basic Information
-    ROS_INFO_NAMED("simple_planner", "Planning frame: %s", move_group.getPlanningFrame().c_str());
-    ROS_INFO_NAMED("simple_planner", "End effector link: %s", move_group.getEndEffectorLink().c_str());
-    ROS_INFO_NAMED("simple_planner", "Available Planning Groups:");
-    std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(),
-              std::ostream_iterator<std::string>(std::cout, ", "));
-
-    move_group.setStartState(*move_group.getCurrentState());
-    geometry_msgs::Pose another_pose;
-    another_pose.orientation.w = 1.0;
-    another_pose.position.x = 0.4;
-    another_pose.position.y = -0.4;
-    another_pose.position.z = 0.9;
-    move_group.setPoseTarget(another_pose);
-
-    moveit::planning_interface::MoveGroupInterface::Plan my_plan;
-    bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
-    ROS_INFO_NAMED("simple_planner", "Visualizing constraint plan %s", success ? "" : "FAILED");
-
-    // Move the simulated robot in gazebo
-    move_group.move();
-
-    ros::shutdown();
-    return 0;
-}
diff --git a/src/SampleConstraintPlanner.cpp b/src/ObstacleAwarePlanner.cpp
similarity index 61%
rename from src/SampleConstraintPlanner.cpp
rename to src/ObstacleAwarePlanner.cpp
index d49ca191aef019c1a7d0ca98afe88c247381dc01..ce7f6c248e655bcea8a571ada01824a9a034ed2f 100644
--- a/src/SampleConstraintPlanner.cpp
+++ b/src/ObstacleAwarePlanner.cpp
@@ -1,56 +1,45 @@
 #include <moveit/move_group_interface/move_group_interface.h>
 #include <moveit/planning_scene_interface/planning_scene_interface.h>
+#include <moveit/trajectory_processing/iterative_time_parameterization.h>
 
-#include <moveit_msgs/DisplayRobotState.h>
-#include <moveit_msgs/DisplayTrajectory.h>
-
-#include <moveit_msgs/AttachedCollisionObject.h>
 #include <moveit_msgs/CollisionObject.h>
 
 #include <moveit_visual_tools/moveit_visual_tools.h>
-#include <moveit/trajectory_processing/iterative_time_parameterization.h>
+
 #include <trajectory_msgs/JointTrajectoryPoint.h>
 
+#include <gazebo_msgs/SpawnModel.h> // service definition for spawning things in gazebo
+#include <ros/ros.h>
+#include <ros/package.h>
+
 /**
- * simple demo of constraint aware planning
+ * simple demo of obstacle aware planning
  */
-int main(int argc, char** argv)
-{
-    ros::init(argc, argv, "CONSTRAINT PLANNER");
+int main(int argc, char **argv) {
+    // init the node
+    ros::init(argc, argv, "obstacle_aware_planner");
     ros::NodeHandle node_handle;
     ros::AsyncSpinner spinner(1);
     spinner.start();
 
-    // wait for robot init of robot_state_initializer
-    ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>>> WAITING FOR ROBOT INIT <<<<<<<<<<<<<<<<");
-    ros::Duration(5.0).sleep();
-    ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>>> WAKING UP AFTER INIT <<<<<<<<<<<<<<<<");
-
+    // setup the planning environment
     static const std::string PLANNING_GROUP = "panda_arm";
     moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
     moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
-    const robot_state::JointModelGroup* joint_model_group =
-            move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);
 
     // Visualization Setup
+    // MoveIts visual tools provide capabilities for visualizing objects, robots, and trajectories in RViz
     namespace rvt = rviz_visual_tools;
     moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0");
     visual_tools.deleteAllMarkers();
-    visual_tools.loadRemoteControl();
-
-    Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
-    text_pose.translation().z() = 1.75;
-    visual_tools.publishText(text_pose, "Sample constraint planner node", rvt::WHITE, rvt::XLARGE);
 
     // Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations
-    visual_tools.trigger();
+    visual_tools.enableBatchPublishing();
 
-    // Getting Basic Information
-    ROS_INFO_NAMED("constraint_planner", "Planning frame: %s", move_group.getPlanningFrame().c_str());
-    ROS_INFO_NAMED("constraint_planner", "End effector link: %s", move_group.getEndEffectorLink().c_str());
-    ROS_INFO_NAMED("constraint_planner", "Available Planning Groups:");
-    std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(),
-              std::ostream_iterator<std::string>(std::cout, ", "));
+    // remote control is an introspection tool that allows users to step through a high level script
+    // via buttons and keyboard shortcuts in RViz
+    visual_tools.loadRemoteControl();
+    ros::Duration(3.0).sleep();
 
     // Define a collision object ROS message.
     moveit_msgs::CollisionObject collision_object;
@@ -65,12 +54,12 @@ int main(int argc, char** argv)
     primitive.dimensions.resize(3);
     primitive.dimensions[0] = 0.4;
     primitive.dimensions[1] = 0.1;
-    primitive.dimensions[2] = 0.4;
+    primitive.dimensions[2] = 0.3;
 
     // Define a pose for the box (specified relative to frame_id)
     geometry_msgs::Pose box_pose;
     box_pose.orientation.w = 1.0;
-    box_pose.position.x = 0.4;
+    box_pose.position.x = 0.45;
     box_pose.position.y = -0.2;
     box_pose.position.z = 1.0;
 
@@ -81,14 +70,23 @@ int main(int argc, char** argv)
     std::vector<moveit_msgs::CollisionObject> collision_objects;
     collision_objects.push_back(collision_object);
 
-    // Now, let's add the collision object into the world
-    ROS_INFO_NAMED("constraint_planner", "Add an object into the world");
+    // We can use visual_tools to wait for user input
+    visual_tools.trigger();
+    visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object appears in RViz");
+
+    // Add the collision object into the world
+    ROS_INFO("Add an object into the world");
     planning_scene_interface.addCollisionObjects(collision_objects);
 
-    // Show text in RViz of status
-    visual_tools.publishText(text_pose, "Add object", rvt::WHITE, rvt::XLARGE);
+    shape_msgs::SolidPrimitive shape;
+    shape.dimensions.resize(3);
+    shape.dimensions[0] = 0.4;
+    shape.dimensions[1] = 0.1;
+    shape.dimensions[2] = 0.3;
+    shape.type = shape.BOX;
+
     visual_tools.trigger();
-    visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object appears in RViz");
+    visual_tools.prompt("Press 'next' to plan the obstacle aware motion");
 
     // Now when we plan a trajectory it will avoid the obstacle
     move_group.setStartState(*move_group.getCurrentState());
@@ -98,11 +96,13 @@ int main(int argc, char** argv)
     another_pose.position.y = -0.4;
     another_pose.position.z = 0.9;
     move_group.setPoseTarget(another_pose);
-    //move_group.setMaxVelocityScalingFactor(0.5);
+
+    move_group.setMaxVelocityScalingFactor(0.5);
+    move_group.setMaxAccelerationScalingFactor(0.5);
 
     moveit::planning_interface::MoveGroupInterface::Plan my_plan;
     bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
-    ROS_INFO_NAMED("constraint_planner", "Visualizing constraint plan %s", success ? "" : "FAILED");
+    ROS_INFO_NAMED("obstacle_aware_planner", "Visualizing constraint plan %s", success ? "" : "FAILED");
 
     visual_tools.prompt("Press 'next' to move the simulated robot.");
     visual_tools.trigger();
diff --git a/src/SampleTimedCartesianPlanner.cpp b/src/SampleTimedCartesianPlanner.cpp
deleted file mode 100644
index c28975bbb004d184de623b86e721962e671d8077..0000000000000000000000000000000000000000
--- a/src/SampleTimedCartesianPlanner.cpp
+++ /dev/null
@@ -1,118 +0,0 @@
-#include <moveit/move_group_interface/move_group_interface.h>
-
-#include <moveit_msgs/DisplayRobotState.h>
-#include <moveit_msgs/DisplayTrajectory.h>
-
-#include <moveit_msgs/AttachedCollisionObject.h>
-#include <moveit_msgs/CollisionObject.h>
-
-#include <moveit_visual_tools/moveit_visual_tools.h>
-#include <moveit/trajectory_processing/iterative_time_parameterization.h>
-#include <trajectory_msgs/JointTrajectoryPoint.h>
-
-/**
- * allows time/velocity-constraint planning for cartesian paths
- */
-int main(int argc, char **argv)
-{
-    ros::init(argc, argv, "sample_timed_cartesian");
-    ros::NodeHandle node_handle;
-    ros::AsyncSpinner spinner(1);
-    spinner.start();
-
-    // wait for robot init of robot_state_initializer
-    ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>>> WAITING FOR ROBOT INIT <<<<<<<<<<<<<<<<");
-    ros::Duration(5.0).sleep();
-    ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>>> WAKING UP AFTER INIT <<<<<<<<<<<<<<<<");
-
-    // Visualization Setup
-    namespace rvt = rviz_visual_tools;
-    moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0");
-    visual_tools.deleteAllMarkers();
-    visual_tools.loadRemoteControl();
-
-    Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
-    text_pose.translation().z() = 1.75;
-    visual_tools.publishText(text_pose, "Sample timed cartesian planner node", rvt::WHITE, rvt::XLARGE);
-
-    // Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations
-    visual_tools.trigger();
-
-    moveit::planning_interface::MoveGroupInterface group("panda_arm");
-
-    ros::Publisher display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);
-    moveit_msgs::DisplayTrajectory display_trajectory;
-
-    moveit::planning_interface::MoveGroupInterface::Plan plan;
-    group.setStartStateToCurrentState();
-
-    std::vector<geometry_msgs::Pose> waypoints;
-
-    // initial pose of the robot
-    geometry_msgs::Pose start_pose = group.getCurrentPose().pose;
-
-    // waypoints of the target trajectory
-    geometry_msgs::Pose target_pose_1 = group.getCurrentPose().pose;
-    geometry_msgs::Pose target_pose_2 = group.getCurrentPose().pose;
-    geometry_msgs::Pose target_pose_3 = group.getCurrentPose().pose;
-
-    target_pose_1.position.z = start_pose.position.z - 0.4;
-    target_pose_1.position.y = start_pose.position.y;
-    target_pose_1.position.x = start_pose.position.x + 0.5;
-    waypoints.push_back(target_pose_1);
-
-    target_pose_2.position.z = 0.6;
-    target_pose_2.position.y = -0.6;
-    target_pose_2.position.x = 0;
-    waypoints.push_back(target_pose_2);
-
-    target_pose_3.position.z = 0.6;
-    target_pose_3.position.y = -0.032;
-    target_pose_3.position.x = -0.6;
-    waypoints.push_back(target_pose_3);
-
-    moveit_msgs::RobotTrajectory trajectory_msg;
-    group.setPlanningTime(10.0);
-
-    double fraction = group.computeCartesianPath(waypoints,0.01,0.0,trajectory_msg, false);
-
-    // The trajectory needs to be modified so it will include velocities as well.
-    // Create a RobotTrajectory object
-    robot_trajectory::RobotTrajectory rt(group.getCurrentState()->getRobotModel(), "panda_arm");
-
-    // Get a RobotTrajectory from trajectory
-    rt.setRobotTrajectoryMsg(*group.getCurrentState(), trajectory_msg);
-
-    // Create a IterativeParabolicTimeParameterization object
-    trajectory_processing::IterativeParabolicTimeParameterization iptp;
-    // Compute computeTimeStamps
-    bool success = iptp.computeTimeStamps(rt);
-    ROS_INFO("Computed time stamp %s",success?"SUCCEDED":"FAILED");
-    rt.getRobotTrajectoryMsg(trajectory_msg);
-
-    // adjust velocities, accelerations and time_from_start
-    for(int i = 0; i < trajectory_msg.joint_trajectory.points.size(); i++)
-    {
-        trajectory_msg.joint_trajectory.points[i].time_from_start.operator*=(2);
-        for(int j = 0; j < trajectory_msg.joint_trajectory.points.at(i).velocities.size(); j++)
-        {
-            if(trajectory_msg.joint_trajectory.points.at(i).velocities.at(j) != 0.0){
-                trajectory_msg.joint_trajectory.points.at(i).velocities.at(j) /= 2;
-            }
-        }
-
-        for(int j = 0; j < trajectory_msg.joint_trajectory.points.at(i).accelerations.size(); j++)
-        {
-            trajectory_msg.joint_trajectory.points.at(i).accelerations.at(j) =
-                    sqrt(std::abs(trajectory_msg.joint_trajectory.points.at(i).accelerations.at(j)));
-        }
-    }
-
-    plan.trajectory_ = trajectory_msg;
-    ROS_INFO("Visualizing plan 4 (cartesian path) (%.2f%% acheived)",fraction * 100.0);
-
-    group.execute(plan);
-
-    ros::shutdown();
-    return 0;
-}
\ No newline at end of file