I have algorithm in Moveit and I need to calculate the path length of motion planning (RRTConnect) anyone have any idea how I can start to write this code?
If you have the list of points that represent the generated path in 3D Cartesian coordinates, you can simply get the Euclidean norm between the first and the last point in the list.