Errors implementing MTC and isaac_ros_cumotion

[mjimenezm00]

Hi! I'm trying to implement MTC and isaac_ros_cumotion.
For testing, I'm running this stand alone test and I've been running into some issues.
In order to fix them, I've created this PR in addition to this change in MTC (although I believe it rather should be fixed in isaac_ros_cumotion_moveit).
Nevertheless, I sill can not plan correctly. Whenever I try to plan (except for when it's planning in the last stage move_to_retracted, as it sometimes does plan correctly in this case) I get these errors:

[cumotion_planner_node-1] [WARNING] [curobo] Start or End state in collision
[cumotion_planner_node-1] [WARNING] [curobo] [Batch 0] Start valid: True, End valid: False
[cumotion_planner_node-1] [WARNING] [curobo] [Batch 0] Start q: [-3.5101876258850098, -5.191460609436035, 1.2787216901779175, -2.7249746322631836, 2.9869866371154785, 3.85205078125]
[cumotion_planner_node-1] [WARNING] [curobo] [Batch 0] Goal q: [-3.5229547023773193, -2.661304473876953, 5.414190769195557, -1.5008268356323242, -1.6957992315292358, 3.5028371810913086]
[cumotion_planner_node-1] [ERROR 1747377335.403536666] [cumotion_planner]: Invalid planning query: MotionGenStatus.GRAPH_FAIL (execute_callback() at /home/mtorres/ros/cumotion/install/isaac_ros_cumotion/lib/python3.10/site-packages/isaac_ros_cumotion/cumotion_planner.py:857)

As I've added some logs, I can see that apparently it is the end state the one it is always invalid and causing the error, although it is within joint limits.
This same test using OMPL works fine. Any help is appreciated!

[mjimenezm00]

I added this PR with the fixes I performed while trying MTC and isaac_ros_cumotion: #35.
However, I still haven't found how to overcome the error described above.