Smooth arm trajectory with JointTrajectoryPoint

I'm using FollowJointTrajectoryGoal() from control_msgs to send a trajectory to my robot. Its trajectory is a JointTrajectory from trajectory_msgs. I have a list joint positions that make up the trajectory I'd like to send. My current approach is to create a JointTrajectoryPoint for each joint position in my list. The velocities list of each JointTrajectoryPoint is set to a list of zeros, and time_from_start is one second later than the time_from_start of the last point in the trajectory.

My trajectory is executed correctly, but it's not smooth. This is probably because the segments of the trajectory are executed faster than the differences between the time_from_starts. Is there a way to create a smooth JointTrajectory? I could try to implement spline interpolation to calculate velocities for each point. Would these velocities be the angular velocity of each joint when it reaches the corresponding JointTrajectoryPoint? Or is it the maximum angular velocity along that trajectory? If I do this, could I remove time_from_start, or set it to zero? Or would I also need to interpolate accelerations? All the examples that I've found set velocities to zero. Thanks!

edit retag close merge delete

Hi ! Any updates on it? Thanks

Sort by » oldest newest most voted As you said, your trajectory are executed correctly, but it's not smooth.

To have the smooth trajectory you need to have minimum energy (Kinetic and Potential). I am not sure how exactly you are calculating your joint_positions. You can create an optimization problem for this using direct method in which you discretise the optimization to minimize/maximize the cost function. You can write a cost function for total energy of a trajectory and optimize it using non linear programming. There are excellent open source libraries available to solve such non-linear problems, NLOPT , IPOPT and PSOPT. library requiring license includesSNOPT. There are bunch of other solutions available in the form of Framework, DRAKE link. klampt link. I personally would recommend for your problem, NLOPT as it is easy to program and you have many gradient and gradient free algorithms.

You can refer this sample code which does something what you asked using NLOPT, code link

Also, Have a look at the simulation video link Video

more