RRTConnect Motion Planning Failed - "Motion planning start tree could not be initialized"

[huyyxy]

Description
When running the provided code, I encountered an error related to the RRTConnect motion planning algorithm. The error occurs when attempting to plan a path for the Franka arm using the move_arm method. The specific error message is:

Error:   RRTConnect: Motion planning start tree could not be initialized!
         at line 219 in /project/src/ompl/geometric/planners/rrt/src/RRTConnect.cpp
[Genesis] [20:59:41] [WARNING] Path planning failed. Returning empty path.

This error prevents the Franka arm from moving to the desired position, and the path planning process fails.

Steps to Reproduce

Initialize the GenesisHelper class.

Call the move_arm method with a target position (e.g., [0.5, 0.0, 0.5]).

Observe the error in the console.

Expected Behavior

The Franka arm should successfully plan and execute a path to the target position without errors.

Actual Behavior

The motion planning fails with the error message mentioned above, and the arm does not move.

Code Snippet

Here is the relevant part of the code where the error occurs:

def move_arm(self, position, orientation=None):
    """Move the Franka arm to a specified position."""
    if not self.franka or not self.end_effector:
        return False

    try:
        pos = np.array(position)
        quat = np.array(orientation if orientation else [0, 1, 0, 0])

        # Inverse kinematics to get target joint positions
        qpos = self.franka.inverse_kinematics(
            link=self.end_effector,
            pos=pos,
            quat=quat,
        )

        # Plan the path
        path = self.franka.plan_path(
            qpos_goal=qpos,
            num_waypoints=200,  # 2-second duration
        )

        # Execute the planned path
        for waypoint in path:
            self.franka.control_dofs_position(waypoint)
            self.scene.step()

        # Wait for the final waypoint
        for _ in range(100):
            self.scene.step()

        return True

    except Exception as e:
        traceback.print_exc()
        print(f"Error moving arm: {str(e)}")
        return False

Environment

Python Version: 3.9.21

Genesis Version: 0.2.1

Operating System: Ubuntu 20.04

Hardware: NVIDIA GeForce RTX 4060 Ti

[huyyxy]

I would like to ask why these two parts are not used:

# Plan the motion path
path = franka.plan_path(
    qpos_goal     = qpos,
    num_waypoints = 200, # 2-second duration
)

# Execute the planned path
for waypoint in path:
    franka.control_dofs_position(waypoint)
    scene.step()

I noticed that in the previous code, there is always a process of planning the motion path. Why is this process not used when the robotic arm is lifting the block after grabbing it?

Once the motion planning path is used, the following error occurs:

Error: RRTConnect: Motion planning start tree could not be initialized!
at line 219 in /project/src/ompl/geometric/planners/rrt/src/RRTConnect.cpp
[Genesis] [21:32:37] [WARNING] Path planning failed. Returning empty path.

[bxtbold]

[Genesis] [20:59:41] [WARNING] Path planning failed. Returning empty path.

@huyyxy It seems like the planning was failed. Mind sharing the whole script?