How to use joint_trajectory_controller to smooth motion between multiple poses?

[littlejohnyang]

I have been using joint_trajectory_controller with PosVelAccJointInterface on my 7 DOF arm implementation. However I am running into an issue with smoothing the motion. I noticed I can set velocity to zero so it triggers QuinticSplineSegment smooth function. It seems working fine if the trajectory contains only one pose (or trajectory point), and it smoothly accelerates and decelerates the joints. However, when my trajectory contains multiple trajectory points (setting velocity to zero for first and final trajectory point and a fixed velocity value for all in between points ), I noticed it does start and stop for every single trajectory execution. In other words, it keeps accelerating and decelerating multiple times other than just accelerating at the start of the trajectory and decelerating at the end of the trajectory.

What is the proper way to use this controller package to smoothly interpolate smooth motion between multiple trajectory input?

I noticed a related question posted on ROS Answer has got no answer since last year, so I am hoping to find some help direction from the developer. Thanks.

[cmaestre]

I have reproduced exactly the same behaviour in my experiment. Any ideas about how to fix this problem? Thanks

[mathias-luedtke]

Have you checked http://wiki.ros.org/joint_trajectory_controller#Trajectory_representation?

[cmaestre]

I did check everything, yes. For N goals, I select a Cartesian 3D position and MoveIt transforms it into a set of positions, velocities and accelerations (using computeCartesianPath). This information is passed as a new goal to the Action Server, which runs it preemting the previous goal, if needed.

The selected positions are just in a straight line reducing coordinate X, i.e. moving the end-effector closer to the Baxter robot. The distance between 2 positions is of 10cm, and a new goal is created each 0.7 seconds (before reaching the current goal):

init_pos_x = 0.64
displacement_x = -0.1
delay = 0.7
   
for i in range(5):    
    pub_trajectory_motion_topic(
                         pub,
                         [init_pos_x + i*displacement_x,-0.1,0],
                         'right')
    time.sleep(delay)

When a new goal starts velocity and acceleration profiles go to 0 (see attached files and video).
accelerations.txt
velocities.txt

[bmagyar]

may have to do with #297 ?

[cmaestre]

Yes, it looks like it is directly related. Since I am currently working in Indigo, how could I avoid this problem?

[bmagyar]

I'm tempted to cherry-pick #297 to indigo-devel... What do you think @ipa-mdl ?
It would although be nice if @cmaestre could do a local test to make sure it solves the issue.

[mathias-luedtke]

I don't see how #297 is related (it only affects the end of the trajectory).
These are quite big jumps.

@cmaestre: could you prepare a plot of the controller_state, please? I wonder if the controller actually generares these jumps or if they are in the input data.

Some general notes:
1 .The topic is not meant for live streaming of points, just for complete (longer!) trajectories
2. computeCartesianPath does not generate smooth trajectories at all
3. In the video it looks like there is problem with the starting state.