Task space trajecectories connecting more than two waypoints

[nepfaff]

At the moment, we only plan trajectories between two waypoints. This leads to smooth motion between two waypoints. However, the robot comes to a stop before starting with the next trajectory. It would be better to have an algorithm that takes a sequence of waypoints and computes a smooth trajectory going through all of them. This trajectory should be in task space. The main difference to the current interpolation is probably that we want the velocity at the goal to be non-zero. The velocity at individual waypoints should be determined by the entire waypoint sequence. I think that composite splines could be a method for achieving this (see here and here).

[nepfaff]

Easy resource on splines

[nepfaff]

Maybe possible to translate this code into MATLAB

[nepfaff]

This now works.

Next step is integrating this into a new main script:

• Use new waypoint.groupToPrevious attribute to group waypoints for splines. All waypoints that have this set to false should use the normal cubic task space trajectory algorithm rather than the spline one.
• Generate set points from waypoints before starting main loop for efficiency
• (Maybe) group set points into different stages (e.g. assign higher velocity to spline set points and lower velocity to others) => picking and placing could be slower than open air movements. However, this is not a priority and should only be done if everything else works using one list of set points generated before the while loop
• Test this by thinking about how to cleverly add waypoints to task 2 algorithms to avoid joint limit problems

[nepfaff]

Problem with the above plan: How to know when to open and close gripper

=> Use the "Maybe" option from above to group set points into different stages to have a list of structs that has the following attributes:

• setPointJointAngles
• setPointTimes
• samplePeriod
• gripperOpening Then iterate over this list of structs. Have another nested loop that does the actual iteration over length(times) and sends the positions. After this nested for loop, send gripper opening.

Create these structs by modifying existing taskSpaceSetPointsFromWaypoints.m

[nepfaff]

All done now