Hello ,

I have written an optimizer which takes in the Cartesian points for trajectory as input. It minimizes the cost function for minimum energy and minimum tracking error while respecting the joint constraints and publishes the optimized joint angle, joint velocity and joint acceleration. These optimized joint state (joint angle, joint velocity and joint acceleration) are published on a topic /opt_states using a custom msgs. Which looks like this,

std_msgs/Float64MultiArray q
std_msgs/Float64MultiArray qdot
std_msgs/Float64MultiArray qddot

And my publisher looks something like this,

while (ros::ok){

jointspace::OptStates my_msg;

for(unsigned int k = 0; k < obj.waypoints_qdot.size()/N_JOINT; k++){

  for (int l = 0; l < N_JOINT; l++)  // publish optimized states on joints in reihenfolge
  {
    my_msg.q.data.push_back( obj.waypoints_qopt.at(N_JOINT*k + l) );
    my_msg.qdot.data.push_back( x.at(N_JOINT*k + l) );
    my_msg.qddot.data.push_back( obj.waypoints_qddot_opt.at(N_JOINT*k + l) );
  }
  opt_pub.publish(my_msg);
  my_msg.q.data.clear();
  my_msg.qdot.data.clear();
  my_msg.qddot.data.clear();
}
ros::spinOnce();
loop_rate.sleep(); }

This is how a published msgs looks like,

q: 
  layout: 
     dim: []
     data_offset: 0
  data: [-0.8444594138944582, -1.9449202285763922, 1.5527375118691336, -0.5633344922531738, -0.06515792426703262, 0.6348876499722212]
qdot: 
  layout: 
    dim: []
    data_offset: 0
  data: [-0.0028649207465767496, 0.0012889015828207683, -0.004133070528183946, -0.0007000750448130076, 0.002019343067587155, 0.0002235910527874273]
qddot: 
  layout: 
    dim: []
    data_offset: 0
  data: [0.2753360769333574, -0.12364479853872068, 0.3966394215812615, 0.06722786929302467, -0.1936560051829433, -0.02143128991688705]

This publisher works fine and I can see my msgs getting published. Now I have a action client which subscribes to topic /opt_states and then send the goal to the arm_controller/follow_joint_trajectory. This action Client also works fine and I can see my position and velocity msgs inside the callback and also at the arm_controller/follow_joint_trajectory.

I used these links to write this action Client. link

And I can see the robot following the optimized joint positions . Now the problem is, the robot is not following the velocity values or velocity profile. There can be issue with publisher and subscriber, or am I misusing the position_controllers/JointTrajectoryControlle. Even though the controller gets the velocity data to follow the velocity profile but it fails to do so. Which is visible through rqt multiplot. I am not sure what I am missing here. (plots are in following git hub link)

My action client can be found at link

I am using:

ROS Melodic , 18.04
gazebo-9.0.0
Universal Robot (UR5) `https://github.com/ros-industrial/universal_robot/tree/melodic-devel`

Similar question which resembles my situation, link

Possible Issue:

I am not sure how to set `time_from_start ` for each new point or should I keep it constant.
May be I am using the wrong controller type or controller is not set up properly. 
PID values !