Add better check for executing trajectories such that the rope isn't pull too tight
Save and restore the rope's color whenever a restore from checkpoint happens. Note: this requires changes in trajoptpy.
Remove consecutive cross pairs to simplify knot topologies and detect more of them
Access group once when saving/loading result files because accessing multiple times seemed to cause corrupted files in some cases
Change grab_rope to only grab the capsules in grasp region (and not the adjacent ones) and color red the grabbed capsules for visualization
Add TODO comment to be done in the future