Moveit Motion planning API, waypoints not increasing with time [closed]

edit

I just started trying to work with the motion planner api as per https://ros-planning.github.io/moveit.... To actually try executing it I followed this with

  moveit::planning_interface::MoveGroupInterface move_group_interface(PLANNING_GROUP);
  moveit::planning_interface::MoveGroupInterface::Plan my_plan;
  my_plan.trajectory_ = response.trajectory;
  move_group_interface.execute(my_plan);

The error given when trying to actually move the robot :

[ INFO] [1621434276.856536029]: Execution request received
[ERROR] [1621434276.857951441]: Trajectory message contains waypoints that are not strictly increasing in time.
[ WARN] [1621434276.858424280]: Controller scaled_pos_joint_traj_controller failed with error INVALID_GOAL: Trajectory message contains waypoints that are not strictly increasing in time.
[ WARN] [1621434276.858571201]: Controller handle scaled_pos_joint_traj_controller reports status FAILED
[ INFO] [1621434276.858625647]: Completed trajectory execution with status FAILED ...
[ INFO] [1621434276.858725778]: Execution completed: FAILED
[ INFO] [1621434276.858971024]: ABORTED: Solution found but controller failed during execution

When I worked just with the move_group_interface and encountered this error, the solution was to eliminate a first current waypoint and let the robot figure it out.

In this case, however, I am not sure if it's the same cause, what state would that be? If i comment out planning_scene->getCurrentStateNonConst().setToDefaultValues(joint_model_group, "up");, the robot doesn't start from its starting upwards pose. I am not yet familiar with how the api works, excuse me if there's an obvious solution to it.

I am using a UR5, ubuntu 18.04, ros melodic.

Any help is appreciated, be it ideas , links etc.

my node:

#include <pluginlib/class_loader.h>
#include <ros/ros.h>

#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/planning_interface/planning_interface.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/kinematic_constraints/utils.h>
#include <moveit_msgs/DisplayTrajectory.h>
#include <moveit_msgs/PlanningScene.h>
#include <moveit_visual_tools/moveit_visual_tools.h>

#include <boost/scoped_ptr.hpp>
#include <boost/foreach.hpp>
#include <boost/lexical_cast.hpp>

# include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>

#define foreach BOOST_FOREACH



int main(int argc, char** argv)
{
  const std::string node_name = "motion_planning_api";
  ros::init(argc, argv, node_name);
  ros::AsyncSpinner spinner(1);
  spinner.start();
  ros::NodeHandle node_handle("~");

  const std::string PLANNING_GROUP = "manipulator";
  robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
  const moveit::core::RobotModelPtr& robot_model = robot_model_loader.getModel();

  moveit::core::RobotStatePtr robot_state(new moveit::core::RobotState(robot_model));
  const moveit::core::JointModelGroup* joint_model_group = robot_state->getJointModelGroup(PLANNING_GROUP);

  planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model));


  planning_scene->getCurrentStateNonConst().setToDefaultValues(joint_model_group, "up");

  boost::scoped_ptr<pluginlib::ClassLoader<planning_interface::PlannerManager>> planner_plugin_loader;
  planning_interface::PlannerManagerPtr planner_instance;
  std::string planner_plugin_name;

  if (!node_handle.getParam("planning_plugin", planner_plugin_name))
    ROS_FATAL_STREAM("Could not find planner plugin name");
  try
  {
    planner_plugin_loader.reset(new pluginlib::ClassLoader<planning_interface::PlannerManager>(
        "moveit_core", "planning_interface::PlannerManager"));
  }
  catch (pluginlib::PluginlibException& ex)
  {
    ROS_FATAL_STREAM("Exception while creating planning plugin loader " << ex.what());
  }
  try
  {
    planner_instance.reset(planner_plugin_loader->createUnmanagedInstance(planner_plugin_name));
    if (!planner_instance->initialize(robot_model, node_handle.getNamespace()))
      ROS_FATAL_STREAM("Could not initialize planner instance");
    ROS_INFO_STREAM("Using planning interface '" << planner_instance->getDescription() << "'");
  }
  catch (pluginlib::PluginlibException& ex)
  {
    const std::vector<std::string>& classes = planner_plugin_loader->getDeclaredClasses();
    std::stringstream ss;
    for (const auto& cls : classes)
      ss << cls << " ";
    ROS_ERROR_STREAM("Exception while loading planner '" << planner_plugin_name << "': " << ex.what() << std::endl
                                                         << "Available plugins: " << ss.str());
  }


  namespace rvt = rviz_visual_tools;
  moveit_visual_tools::MoveItVisualTools visual_tools("base_link");
  visual_tools.loadRobotStatePub("/display_robot_state");
  visual_tools.enableBatchPublishing();
  visual_tools.deleteAllMarkers();  // clear all old markers
  visual_tools.trigger();

  visual_tools ...