Smooth arm trajectory with JointTrajectoryPoint
I'm using FollowJointTrajectoryGoal() from controlmsgs to send a trajectory to my robot. Its trajectory is a JointTrajectory from trajectorymsgs. 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_start
s. 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!
Asked by matt on 2016-09-20 14:42:10 UTC
Answers
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
To read about NLOPT
link. To read about IPOPT
link
Also, Have a look at the simulation video link Video
Asked by amjack on 2020-12-02 04:41:45 UTC
Comments
Hi ! Any updates on it? Thanks
Asked by Maestre on 2018-03-14 05:31:41 UTC