make iterative time parameterization tool use KinematicTrajectory

[isucan]

No description provided.

[isucan]

In the moveit_core package, the trajectory_processing library includes the IterativeParabolicTimeParameterization class; That class computes time staps for the waypoints of the robot along a given trajectory.

The issue that needs to be resolved is that the computeTimeStamps() function should operate directly on the robot_trajectory::RobotTrajectory class; currently, it instead converts the trajectory to a message (trajectory_msgs::JointTrajectory) then calls the computeTimeStamps() variant that operates on the message and then converts back to a RobotTrajectory.

What needs to be done is that the computation needs to be performed directly on the RobotTrajectory class, in place (no copying of states) and the version of computeTimeStamps() that operates on the message should be implemented in terms of the function that operates directly on RobotTrajectory, using conversion functions.

[tulku]

We have started working on this and committed the first changes to my fork.

I have a question, in the bug report you state that the original JointTrajectory should still be available as wrappers of the RobotTrajectory ones, whoever, they are not being use by anyone right one. Are they really necessary? (I've made them private and everything compiles fine).

[aleeper]

I think we should keep them; it makes it easier for folks who might already have a JointTrajectory and want to manually call the time parameterization in their own code (which I have done sometimes).

[isucan]

sorry for the late reply - pretty bad internet at the conf and the schedule was a bit hectic.
the code looks great, thanks! the functions you mention are indeed not used anywhere, but i agree with adam about keeping them. the implementation should be simple anyway, so there is no problem having them. we will just include in the docs the fact that conversions are performed and the use is not encouraged.

[tulku]

Ok, I'll leave them public using the conversions. Just asked to avoid possible dead code. Thanks!

[tulku]

Hi!

I have been spending quite some time now finishing this patch and run into a problem.

Part of the process that's done to the trajectory_msgs::JointTrajectory is calling a function called updateTrajectory which is suppose to "Takes the time differences, and updates the values in the trajectory". However this function also makes some velocity (although the code is commented) and acceleration calculations. This new values are stored in the JointTrajectory message.

It is very difficult to track where the velocity and acceleration information is stored in the way back from a trajectory_msgs::JointTrajectory to a robot_trajectory::RobotTrajectory. It actually seems that the values are discarded and that the only important result from the entire computeTimeStamps() method is, not surprisingly, to calculate new, smoother timestamps and modify the robot_trajectory::RobotTrajectory. The velocity values in a point are added to the JointStates, using some functions that say that are old and should not be used.

I have just pushed a new changeset with a new updateTrajectory that only updates the timestamps of the waypoints according to the previously calculated time_diff. If this is correct I should finish this issue tomorrow. (I still need to update the computeTimeStamps() functions.)

[isucan]

@tulku The velocities are stored for a point indeed, and previously they were stored in the message, for each trajectory point. When using the RobotTrajectory class, you will have to store the velocities in the RobotState instances, using the functions you found. The reason they are marked for removal is because they are horribly inefficient in terms of memory allocation for states. Nevertheless, the functions will remain there in the updated version of RobotState, so you should use them and we should not discard computed velocities.

[tulku]

@isucan Thanks for that information. If I understood you correctly, to update the RobotTrajectory we should:

• Update the duration_from_previous_ queue using the robot_trajectory::setWayPointDurationFromPrevious() taking the values from the time_diff vector.
• Update the velocities of each joint in each state of the waypoints_ queue asking for the velocities vector of each joint using joint_state::getVelocities() and then changing it's values.
• Update the accelerations in the same way as the velocities but using joint_state::getAccelerations().

This is done in updateTrajectory function of latest commit, which compiles but at the planner wont work correctly because the JointTrajectory msg is not correctly set and the later transformations fail.

[isucan]

Yep, this sounds good.

[acornacorn]

Is this issue complete? Can I close it?

[isucan]

This has been done for a while now, but I forgot to close it.