robot now changes speed inbetween two trajectory points

config/config.yaml

 panda_mqtt_connector:
   server: "tcp://localhost:1883"
-  robot_speed_factor: .95
+  robot_speed_factor: .5
   robot_planning_mode: "fluid_path" # "fluid_path" or "cartesian_path"
-  default_trajectory: "square" # "square" or "circle", everything else is ignored
+  default_trajectory: "circle" # "square" or "circle", everything else is ignored
   topics:
     robotConfig: "robotconfig"
     dataConfig: "dataconfig"
   zone_size: 0.5
   zones:
     - "1 1"
-    - "0 1"
+    - "-1 -1 1"
   parts:
     panda:
       Link0: "panda::panda_link0"

src/TimedPlanner.cpp

@@ -23,7 +23,6 @@ namespace PlannerState {
 
   const double default_velocity = 1.0;
   const std::string default_planning_mode = TrajectoryUtil::FLUID_PATH;
-  const bool useCircle = true;
 
   bool isLooping = true;
 
@@ -106,6 +105,30 @@ bool loadWaypoints(std::vector<geometry_msgs::Pose> &waypoints, const ros::NodeH
   }
 }
 
+static std::vector<moveit_msgs::RobotTrajectory> split(moveit::planning_interface::MoveGroupInterface &group, const moveit::planning_interface::MoveGroupInterface::Plan &plan) {
+
+  std::vector<moveit_msgs::RobotTrajectory> trajectories;
+
+  auto currentState = group.getCurrentState();
+  auto robotModel = currentState->getRobotModel();
+
+  robot_trajectory::RobotTrajectory rt(robotModel, "panda_arm");
+  rt.setRobotTrajectoryMsg(*currentState, plan.start_state_, plan.trajectory_);
+
+  for (int wpIndex = 0; wpIndex < rt.getWayPointCount() - 1; ++wpIndex) {
+    robot_trajectory::RobotTrajectory wpTrajectory{robotModel, "panda_arm"};
+    moveit::core::RobotState waypointState{robotModel};
+    wpTrajectory.insertWayPoint(0, rt.getWayPoint(wpIndex), 0);
+    wpTrajectory.insertWayPoint(1, rt.getWayPoint(wpIndex+1), rt.getWayPointDurationFromPrevious(wpIndex+1));
+    moveit_msgs::RobotTrajectory wpTrajectoryMsg{};
+    wpTrajectory.getRobotTrajectoryMsg(wpTrajectoryMsg);
+    trajectories.push_back(wpTrajectoryMsg);
+  }
+
+  return trajectories;
+
+}
+
 void doMotion(const ros::NodeHandle &node_handle, moveit::planning_interface::MoveGroupInterface &group) {
 
   bool newTrajectoryAvailable = false;
@@ -129,24 +152,33 @@ void doMotion(const ros::NodeHandle &node_handle, moveit::planning_interface::Mo
     node_handle.getParam("new_trajectory_available", newTrajectoryAvailable);
     node_handle.getParam("loop_trajectory", PlannerState::isLooping);
 
-
-    double velocity;
-    if (!node_handle.getParam("robot_speed_factor", velocity)) {
-      velocity = PlannerState::default_velocity;
-    }
-
     std::string planning_mode;
     if (!node_handle.getParam("robot_planning_mode", planning_mode)) {
       planning_mode = PlannerState::default_planning_mode;
     }
 
-    ROS_INFO_STREAM("Planning the next trajectory in " << planning_mode << " with velocity " << velocity);
+    ROS_INFO_STREAM("Planning the next trajectory in " << planning_mode);
 
     moveit::planning_interface::MoveGroupInterface::Plan plan;
 
     if (PlannerState::traj_util.computePathToPose(group, plan, PlannerState::raw_trajectory.at(i), planning_mode, 0.8,
-                                                  0.8, velocity)) {
-      group.execute(plan);
+                                                  0.8)) {
+      for (auto trajectory : split(group, plan)) {
+        double motionSpeedFactor;
+        if (!node_handle.getParam("robot_speed_factor", motionSpeedFactor)) {
+          motionSpeedFactor = PlannerState::default_velocity;
+        }
+        if (motionSpeedFactor <= 0 || motionSpeedFactor > 1.0) {
+          ROS_ERROR_STREAM("Invalid motion speed factor " << motionSpeedFactor << ". Must be in (0,1].");
+          return;
+        }
+        ROS_INFO_STREAM("Moving to the next waypoint with speed factor " << motionSpeedFactor);
+        TrajectoryUtil::applyMotionSpeedFactor(trajectory, motionSpeedFactor);
+        moveit_msgs::MoveItErrorCodes errorCode = group.execute(trajectory);
+        if (errorCode.val != errorCode.SUCCESS) {
+          ROS_ERROR_STREAM("Error Code from executing segment: " << errorCode);
+        }
+      }
     } else {
       ROS_ERROR_STREAM("The planner was unable to compute a path to the pose with the given settings.");
     }

src/util/TrajectoryUtil.cpp

@@ -11,7 +11,7 @@ bool TrajectoryUtil::computePathToPose(moveit::planning_interface::MoveGroupInte
                                        moveit::planning_interface::MoveGroupInterface::Plan &plan,
                                        geometry_msgs::Pose targetPose,
                                        const std::string& pathType, double maxVelocityFactor,
-                                       double maxAccelerationFactor, double motionSpeedFactor) {
+                                       double maxAccelerationFactor) {
   if (maxVelocityFactor <= 0 || maxVelocityFactor > 1.0) {
     ROS_ERROR_STREAM("Invalid maximum velocity scaling factor " << maxVelocityFactor << ". Must be in (0,1].");
     return false;
@@ -20,10 +20,6 @@ bool TrajectoryUtil::computePathToPose(moveit::planning_interface::MoveGroupInte
     ROS_ERROR_STREAM("Invalid maximum acceleration scaling factor " << maxAccelerationFactor << ". Must be in (0,1].");
     return false;
   }
-  if (motionSpeedFactor <= 0 || motionSpeedFactor > 1.0) {
-    ROS_ERROR_STREAM("Invalid motion speed factor " << motionSpeedFactor << ". Must be in (0,1].");
-    return false;
-  }
 
   group.setStartStateToCurrentState();
   group.setPlanningTime(10.0);
@@ -67,14 +63,11 @@ bool TrajectoryUtil::computePathToPose(moveit::planning_interface::MoveGroupInte
     ROS_ERROR("Invalid type of path provided.");
     return false;
   }
-
-  manipulateVelocity(plan, motionSpeedFactor);
   return true;
 }
 
-void TrajectoryUtil::manipulateVelocity(moveit::planning_interface::MoveGroupInterface::Plan &plan, double velocity) {
-  ROS_INFO_STREAM("slowing down plan with " << plan.trajectory_.joint_trajectory.points.size() << " joints");
-  for (auto &point : plan.trajectory_.joint_trajectory.points) {
+void TrajectoryUtil::applyMotionSpeedFactor(moveit_msgs::RobotTrajectory &trajectory, double velocity) {
+  for (auto &point : trajectory.joint_trajectory.points) {
     point.time_from_start *= 1 / velocity;
     for (double &v : point.velocities) {
       v *= velocity;
@@ -84,5 +77,4 @@ void TrajectoryUtil::manipulateVelocity(moveit::planning_interface::MoveGroupInt
       acceleration = sqrt(std::abs(acceleration));
     }
   }
-
 }

src/util/TrajectoryUtil.h

@@ -25,11 +25,11 @@ public:
                          moveit::planning_interface::MoveGroupInterface::Plan &plan,
                          geometry_msgs::Pose targetPose,
                          const std::string &pathType, double maxVelocityFactor,
-                         double maxAccelerationFactor, double motionSpeedFactor);
+                         double maxAccelerationFactor);
 
+  static void applyMotionSpeedFactor(moveit_msgs::RobotTrajectory &trajectory, double velocity);
 private:
 
-  static void manipulateVelocity(moveit::planning_interface::MoveGroupInterface::Plan &plan, double velocity);
 };
 
 #endif //PANDA_SIMULATION_TRAJECTORYUTIL_H
\ No newline at end of file