diff --git a/adapter_package/src/adapter_package.cpp b/adapter_package/src/adapter_package.cpp
index 4b37a9d419a6230e4eb06ef325fef51469a61559..ac45da75811caaf772f2590d5ec37ff253f431b5 100644
--- a/adapter_package/src/adapter_package.cpp
+++ b/adapter_package/src/adapter_package.cpp
@@ -38,20 +38,14 @@ void move_toCallback(const std_msgs::String::ConstPtr& msg)
     std::string strA ("A");
     std::string strB ("B");
     if (strA == msg->data) {
-        waypoints.push_back(target_pose_1);
+        group.setPoseTarget(target_pose_1);
     } else if (strB == msg->data) {
-        waypoints.push_back(target_pose_2);
+        group.setPoseTarget(target_pose_2);
     } else {
         ROS_WARN("ADAPTER PACKAGE: Wrong message.");
         return;
     }
 
-    ///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;
-    //group.execute(plan);
-    group.setPoseTarget(target_pose_2);
     group.move();
     ROS_INFO("ADAPTER PACKAGE: Finished movement.");