Constant TCP velocity time parameterization

[marip8]

This PR implements a time parameterization algorithm for maintaining a constant TCP speed. The approach works by:

• Calculating FK at each waypoint in the trajectory to get the pose of the TCP
• Creating a line path between adjacent TCP poses
• Parameterize the line with a trapezoidal velocity profile
• Compute the joint velocity/acceleration at each waypoint given the TCP Cartesian velocity/acceleration
• Validate that the joint velocity/acceleration is within the limits of the robot

Before

https://github.com/ros-industrial-consortium/scan_n_plan_workshop/assets/18411310/9e829756-0953-4379-be9b-b353c9112faf

After

https://github.com/ros-industrial-consortium/scan_n_plan_workshop/assets/18411310/77bbd6f7-bb43-4043-910b-714a9963e6f9

[DavidSpielman]

https://github.com/ros-industrial-consortium/scan_n_plan_workshop/assets/76972296/28c7f333-cf26-4d54-b3b3-d161c1d259c2

While testing various tool paths, I came across this one. I am not sure what is causing the sporadic back and forth motion of the robot. The rest of the motions were smooth as expected.

[marip8]

unusual_motion_plan.mp4 While testing various tool paths, I came across this one. I am not sure what is causing the sporadic back and forth motion of the robot. The rest of the motions were smooth as expected.

I think this is actually a tool path issue, not a time parameterization issue. It looks like there is a small tool path that was created in the mesh that caused this