initial test to reduce speed properly

config/config.yaml

 panda_mqtt_connector:
   server: "tcp://localhost:1883"
+  robot_speed_factor: .15
+  robot_planning_mode: "fluid_path" # "fluid_path" or "cartesian_path"
   topics:
     robotConfig: "robotconfig"
     dataConfig: "dataconfig"
   zone_size: 0.5
   zones:
     - "1 1"
-    - "-1 -1 1"
+    - "0 1"
   parts:
     panda:
       Link0: "panda::panda_link0"

src/TimedPlanner.cpp

@@ -24,7 +24,6 @@ namespace PlannerState {
   const double default_velocity = 1.0;
   const std::string default_planning_mode = TrajectoryUtil::FLUID_PATH;
 
-  bool isInitialized = false;
   bool isLooping = true;
 
 }
@@ -176,7 +175,7 @@ void doMotion(const ros::NodeHandle &node_handle, moveit::planning_interface::Mo
     }
 
     // wake up if we have trajectory
-    while (PlannerState::raw_trajectory.size() == 0) {
+    while (PlannerState::raw_trajectory.empty()) {
       if (newTrajectoryAvailable) {
         updateRawTrajectory(node_handle, &group);
         i = 0;
@@ -185,7 +184,7 @@ void doMotion(const ros::NodeHandle &node_handle, moveit::planning_interface::Mo
     }
 
     double velocity = 0.0;
-    std::string planning_mode = "";
+    std::string planning_mode;
 
     if (!node_handle.getParam("robot_speed_factor", velocity)) {
       velocity = PlannerState::default_velocity;
@@ -198,7 +197,7 @@ void doMotion(const ros::NodeHandle &node_handle, moveit::planning_interface::Mo
     ROS_INFO_STREAM("Planning the next trajectory in " << planning_mode << " with velocity " << velocity);
 
     moveit::planning_interface::MoveGroupInterface::Plan plan;
-    PlannerState::traj_util.computePathToPose(group, plan, PlannerState::raw_trajectory.at(i), planning_mode, velocity);
+    PlannerState::traj_util.computePathToPose(group, plan, PlannerState::raw_trajectory.at(i), planning_mode, 0.8, 0.8, velocity);
     group.execute(plan);
 
     // make sure the robot moves in an infinite circle

src/util/TrajectoryUtil.cpp

@@ -8,42 +8,70 @@ const std::string TrajectoryUtil::CARTESIAN_PATH = "cartesian_path";
 const std::string TrajectoryUtil::FLUID_PATH = "fluid_path";
 
 bool TrajectoryUtil::computePathToPose(moveit::planning_interface::MoveGroupInterface &group,
-                       moveit::planning_interface::MoveGroupInterface::Plan &plan, geometry_msgs::Pose targetPose,
-                       std::string pathType, double velocity)
-{
-    if(velocity < 0.1 || velocity > 1.0){
-        ROS_ERROR("Invalid velocity configured. ");
+                                       moveit::planning_interface::MoveGroupInterface::Plan &plan,
+                                       geometry_msgs::Pose targetPose,
+                                       const std::string& pathType, double maxVelocityFactor,
+                                       double maxAccelerationFactor, double motionSpeedFactor) {
+  if (maxVelocityFactor <= 0 || maxVelocityFactor > 1.0) {
+    ROS_ERROR_STREAM("Invalid maximum velocity scaling factor " << maxVelocityFactor << ". Must be in (0,1].");
     return false;
   }
-
-    if (pathType.compare(CARTESIAN_PATH) == 0)
-    {
-        if(!computeCartesianPath(group, plan, targetPose))
-        {
+  if (maxAccelerationFactor <= 0 || maxAccelerationFactor > 1.0) {
+    ROS_ERROR_STREAM("Invalid maximum acceleration scaling factor " << maxAccelerationFactor << ". Must be in (0,1].");
     return false;
-        }else{
-            manipulateVelocity(velocity, group, plan, CARTESIAN_PATH);
-            return true;
   }
+  if (motionSpeedFactor <= 0 || motionSpeedFactor > 1.0) {
+    ROS_ERROR_STREAM("Invalid motion speed factor " << motionSpeedFactor << ". Must be in (0,1].");
+    return false;
   }
 
-    if(pathType.compare(FLUID_PATH) == 0)
-    {
   group.setStartStateToCurrentState();
   group.setPlanningTime(10.0);
-
   group.setPoseTarget(targetPose);
-        manipulateVelocity(velocity, group, plan, FLUID_PATH);
-        return (group.plan(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
+  group.setMaxVelocityScalingFactor(maxVelocityFactor);
+  group.setMaxAccelerationScalingFactor(maxAccelerationFactor);
+
+  if (pathType == CARTESIAN_PATH) {
+    std::vector<geometry_msgs::Pose> single_traj{targetPose};
+
+    moveit_msgs::RobotTrajectory trajectory_msg;
+    double fraction = group.computeCartesianPath(single_traj, 0.01, 0.0, trajectory_msg, true);
+    ROS_INFO("Planned cartesian plan (%.2f%% achieved)", fraction * 100.0);
+
+    robot_trajectory::RobotTrajectory rt(group.getCurrentState()->getRobotModel(), "panda_arm");
+    rt.setRobotTrajectoryMsg(*group.getCurrentState(), trajectory_msg);
+    trajectory_processing::IterativeParabolicTimeParameterization iptp{};
+
+    ROS_ERROR_STREAM("Before time parameterization, the trajectory takes " << rt.getDuration() << "s");
+
+    if (iptp.computeTimeStamps(rt)) {
+      ROS_ERROR_STREAM("After first time parameterization, the trajectory takes " << rt.getDuration() << "s");
     }
 
-    ROS_ERROR("Invalid type of path provided.");
+    if (iptp.computeTimeStamps(rt, maxVelocityFactor)) {
+      ROS_INFO("Computed time stamps.");
+      rt.getRobotTrajectoryMsg(trajectory_msg);
+      plan.trajectory_ = trajectory_msg;
 
+      ROS_ERROR_STREAM("After second time parameterization, the trajectory takes " << rt.getDuration() << "s");
+    } else {
+      ROS_ERROR("Computation of cartesian path has failed.");
       return false;
     }
+  } else if (pathType == FLUID_PATH) {
+    if (group.plan(plan) != moveit::planning_interface::MoveItErrorCode::SUCCESS) {
+      ROS_ERROR("Computation of fluid path has failed.");
+    }
+  } else {
+    ROS_ERROR("Invalid type of path provided.");
+    return false;
+  }
+
+  manipulateVelocity(plan, motionSpeedFactor);
+}
 
-bool TrajectoryUtil::initWaypoints(std::vector<geometry_msgs::Pose> &waypoints, ros::NodeHandle &node_handle, geometry_msgs::Pose &initialPose)
-{
+bool TrajectoryUtil::initWaypoints(std::vector<geometry_msgs::Pose> &waypoints, ros::NodeHandle &node_handle,
+                                   geometry_msgs::Pose &initialPose) {
   std::vector<double> x_values;
   std::vector<double> y_values;
   std::vector<double> z_values;
@@ -51,12 +79,10 @@ bool TrajectoryUtil::initWaypoints(std::vector<geometry_msgs::Pose> &waypoints,
   // check if no trajectory is configured
   if (!node_handle.getParam("trajectory_pos_x", x_values) ||
       !node_handle.getParam("trajectory_pos_y", y_values) ||
-       !node_handle.getParam("trajectory_pos_z", z_values) )
-    {
+      !node_handle.getParam("trajectory_pos_z", z_values)) {
     return false;
   } else {
-        for(int i = 0; i < x_values.size(); i++)
-        {
+    for (int i = 0; i < x_values.size(); i++) {
       geometry_msgs::Pose pose = initialPose;
       pose.position.x = x_values.at(i);
       pose.position.y = y_values.at(i);
@@ -69,57 +95,17 @@ bool TrajectoryUtil::initWaypoints(std::vector<geometry_msgs::Pose> &waypoints,
   }
 }
 
-bool TrajectoryUtil::computeCartesianPath(moveit::planning_interface::MoveGroupInterface &group,
-                                          moveit::planning_interface::MoveGroupInterface::Plan &plan,
-                                          const geometry_msgs::Pose &targetPose) {
-
-    std::vector<geometry_msgs::Pose> single_traj{targetPose};
-    group.setStartStateToCurrentState();
-    group.setPlanningTime(10.0);
-
-    moveit_msgs::RobotTrajectory trajectory_msg;
-    double fraction = group.computeCartesianPath(single_traj, 0.01, 0.0, trajectory_msg, false);
-
-    robot_trajectory::RobotTrajectory rt(group.getCurrentState()->getRobotModel(), "panda_arm");
-    rt.setRobotTrajectoryMsg(*group.getCurrentState(), trajectory_msg);
-    trajectory_processing::IterativeParabolicTimeParameterization iptp;
-
-    bool success = iptp.computeTimeStamps(rt);
-    ROS_INFO("Computed time stamp %s", success ? "SUCCEDED" : "FAILED");
-
-    if(!success){
-        return false;
-    }
-
-    rt.getRobotTrajectoryMsg(trajectory_msg);
-    ROS_INFO("Planned cartesian plan (%.2f%% acheived)", fraction * 100.0);
-    plan.trajectory_ = trajectory_msg;
-
-    return true;
-}
-
-void TrajectoryUtil::manipulateVelocity(double velocity, moveit::planning_interface::MoveGroupInterface &group,
-                                        moveit::planning_interface::MoveGroupInterface::Plan &plan, std::string pathType)
-{
-    if(pathType.compare(FLUID_PATH))
-    {
-        group.setMaxVelocityScalingFactor(velocity);
+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) {
+    point.time_from_start *= 1 / velocity;
+    for (double &v : point.velocities) {
+      v *= velocity;
     }
 
-    if(pathType.compare(CARTESIAN_PATH))
-    {
-        for (int i = 0; i < plan.trajectory_.joint_trajectory.points.size(); i++) {
-            plan.trajectory_.joint_trajectory.points[i].time_from_start.operator*=((1 / velocity));
-            for (int j = 0; j < plan.trajectory_.joint_trajectory.points.at(i).velocities.size(); j++) {
-                if (plan.trajectory_.joint_trajectory.points.at(i).velocities.at(j) != 0.0) {
-                    plan.trajectory_.joint_trajectory.points.at(i).velocities.at(j) *= velocity;
+    for (double &acceleration : point.accelerations) {
+      acceleration = sqrt(std::abs(acceleration));
     }
   }
 
-            for (int j = 0; j < plan.trajectory_.joint_trajectory.points.at(i).accelerations.size(); j++) {
-                plan.trajectory_.joint_trajectory.points.at(i).accelerations.at(j) =
-                        sqrt(std::abs(plan.trajectory_.joint_trajectory.points.at(i).accelerations.at(j)));
-            }
-        }
-    }
 }

src/util/TrajectoryUtil.h

@@ -22,19 +22,17 @@ public:
   static const std::string FLUID_PATH;
 
   bool computePathToPose(moveit::planning_interface::MoveGroupInterface &group,
-                           moveit::planning_interface::MoveGroupInterface::Plan &plan, geometry_msgs::Pose targetPose,
-                           std::string pathType, double velocity);
+                         moveit::planning_interface::MoveGroupInterface::Plan &plan,
+                         geometry_msgs::Pose targetPose,
+                         const std::string &pathType, double maxVelocityFactor,
+                         double maxAccelerationFactor, double motionSpeedFactor);
 
-    bool initWaypoints(std::vector<geometry_msgs::Pose> &waypoints, ros::NodeHandle &node_handle, geometry_msgs::Pose &initialPose);
+  bool initWaypoints(std::vector<geometry_msgs::Pose> &waypoints, ros::NodeHandle &node_handle,
+                     geometry_msgs::Pose &initialPose);
 
 private:
 
-    bool computeCartesianPath(moveit::planning_interface::MoveGroupInterface &group,
-                              moveit::planning_interface::MoveGroupInterface::Plan &plan,
-                              const geometry_msgs::Pose &targetPose);
-
-    void manipulateVelocity(double velocity, moveit::planning_interface::MoveGroupInterface &group,
-                            moveit::planning_interface::MoveGroupInterface::Plan &plan, std::string pathType);
+  void manipulateVelocity(moveit::planning_interface::MoveGroupInterface::Plan &plan, double velocity);
 };
 
 #endif //PANDA_SIMULATION_TRAJECTORYUTIL_H
\ No newline at end of file