Planning for multiple moves crashes - no start point

[kaiConnected]

Hello tesseract devs,

in our project we face the problem, that we can't plan a path with more than two MoveInstructions. As soon as we add a third MoveInstruction the planning fails with the error message, that no start point has been set. But we couldn't find a way to add a start point. All we can do and did is set a state for the environment. Do you maybe know what causes this issue?
In the following I will add code extracts and the error message:

    OMPL_DEFAULT_NAMESPACE = "OMPLMotionPlannerTask"
    TRAJOPT_DEFAULT_NAMESPACE = "TrajOptMotionPlannerTask"


    # Initialize the resource locator and environment
    
    # Fill in the manipulator information. This is used to find the kinematic chain for the manipulator. This must
    # match the SRDF, although the exact tcp_frame can differ if a tool is used.
    manip_info = ManipulatorInfo()
    manip_info.tcp_frame = "tool0"
    manip_info.manipulator = "manipulator"
    manip_info.working_frame = "base_link"

    locator = GeneralResourceLocator()
    urdf_xml_path = FilesystemPath(locator.locateResource("/root/ws/src/tesseract_planning/resources/urdf/workStation.urdf").getFilePath())
    srdf_xml_path = FilesystemPath(locator.locateResource("/root/ws/src/tesseract_planning/resources/urdf/workStation.srdf").getFilePath())

    t_env = Environment()
    t_env.init(urdf_xml_path, srdf_xml_path, locator)
    joint_names = ["shoulder_pan_joint","shoulder_lift_joint","elbow_joint","wrist_1_joint","wrist_2_joint","wrist_3_joint"]


    # Set the initial state of the robot
    t_env.setState(joint_names, np.ones(6)*0.1)

    env = t_env
    manip = manip_info

    cur_state = env.getState()
    wp1 = CartesianWaypoint(Isometry3d.Identity() * Translation3d(0.5,0.5,0.5) * Quaterniond(1,0.0,0.0,0.0))
    wp2 = CartesianWaypoint(Isometry3d.Identity() * Translation3d(-0.5,0.5,0.5) * Quaterniond(1.0,0.0,0.0,0.0))
    wp3 = CartesianWaypoint(Isometry3d.Identity() * Translation3d(0.5,-0.5,0.5) * Quaterniond(1.0,0.0,0.0,0.0))
    instruction = MoveInstructionType_FREESPACE

    start_instruction = None
    plan_f1 = None
    start_instruction = MoveInstruction(CartesianWaypointPoly_wrap_CartesianWaypoint(wp1), instruction, "freespace_profile") 
    plan_f1 = MoveInstruction(CartesianWaypointPoly_wrap_CartesianWaypoint(wp2), instruction, "freespace_profile")
    plan_f2 = MoveInstruction(CartesianWaypointPoly_wrap_CartesianWaypoint(wp3), instruction, "freespace_profile")

    program = CompositeInstruction("DEFAULT")
    program.setManipulatorInfo(manip)
    program.appendMoveInstruction(MoveInstructionPoly_wrap_MoveInstruction(start_instruction))
    program.appendMoveInstruction(MoveInstructionPoly_wrap_MoveInstruction(plan_f1))
    program.appendMoveInstruction(MoveInstructionPoly_wrap_MoveInstruction(plan_f2))
  
    plan_profile = OMPLDefaultPlanProfile()
    plan_profile.planners.clear()
    plan_profile.planners.append(RRTConnectConfigurator())


    profiles = ProfileDictionary()
    ProfileDictionary_addProfile_OMPLPlanProfile(profiles,OMPL_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile)
    request = PlannerRequest()
    request.instructions = program
    request.env = env
    request.env_state = cur_state
    request.profiles = profiles
    

    ompl_planner = OMPLMotionPlanner(OMPL_DEFAULT_NAMESPACE)
    response=ompl_planner.solve(request)
    assert response.successful
    results_instruction = response.results


    # The OMPL program does not generate dense waypoints. This function will interpolate the results to generate
    # a dense set of waypoints.
    interpolated_results_instruction = generateInterpolatedProgram(results_instruction, cur_state, env, 3.14, 3.14, 3.14, 30)

    # Create the TrajOpt planner profile configurations. TrajOpt is used to optimize the random program generated
    # by OMPL
    from tesseract_robotics.tesseract_time_parameterization import TimeOptimalTrajectoryGeneration, InstructionsTrajectory, RuckigTrajectorySmoothing
    from tesseract_robotics.tesseract_motion_planners_trajopt import TrajOptDefaultPlanProfile, TrajOptDefaultCompositeProfile, \
    TrajOptProblemGeneratorFn, TrajOptMotionPlanner, ProfileDictionary_addProfile_TrajOptPlanProfile, \
    ProfileDictionary_addProfile_TrajOptCompositeProfile

    trajopt_plan_profile = TrajOptDefaultPlanProfile()
    trajopt_composite_profile = TrajOptDefaultCompositeProfile()

    trajopt_profiles = ProfileDictionary()
    ProfileDictionary_addProfile_TrajOptPlanProfile(trajopt_profiles, TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", trajopt_plan_profile)
    ProfileDictionary_addProfile_TrajOptCompositeProfile(trajopt_profiles, TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", trajopt_composite_profile)

    # Create the TrajOpt planner
    trajopt_planner = TrajOptMotionPlanner(TRAJOPT_DEFAULT_NAMESPACE)

    # Create the TrajOpt planning request and run the planner
    trajopt_request = PlannerRequest()
    trajopt_request.instructions = interpolated_results_instruction
    trajopt_request.env = env
    trajopt_request.env_state = cur_state
    trajopt_request.profiles = trajopt_profiles

    trajopt_response = trajopt_planner.solve(trajopt_request)
    assert trajopt_response.successful
        
    trajopt_results_instruction = trajopt_response.results

Info:    No planner specified. Using default.
Info:    LBKPIECE1: Attempting to use default projection.
Debug:   LBKPIECE1: Planner range detected to be 5.758648
Debug:   ParallelPlan.solveOne starting planner RRTConnect
Debug:   RRTConnect: Planner range detected to be 5.758648
Debug:   ParallelPlan.solveOne starting planner RRTConnect
Debug:   RRTConnect: Planner range detected to be 5.758648
Info:    RRTConnect: Starting planning with 64 states already in datastructure
Info:    RRTConnect: Starting planning with 64 states already in datastructure
Info:    RRTConnect: Created 67 states (65 start + 2 goal)
Debug:   ParallelPlan.solveOne: Solution found by RRTConnect in 0.301845 seconds
Info:    RRTConnect: Created 67 states (65 start + 2 goal)
Info:    ProblemDefinition: Adding approximate solution from planner RRTConnect
Debug:   ParallelPlan.solveOne: Solution found by RRTConnect in 0.364229 seconds
Info:    ParallelPlan::solve(): Solution found by one or more threads in 0.364570 seconds
Debug:   ParallelPlan.solveOne starting planner RRTConnect
Info:    RRTConnect: Starting planning with 67 states already in datastructure
Debug:   ParallelPlan.solveOne starting planner RRTConnect
Info:    RRTConnect: Starting planning with 67 states already in datastructure
Info:    RRTConnect: Created 81 states (72 start + 9 goal)
Debug:   ParallelPlan.solveOne: Solution found by RRTConnect in 3.793736 seconds
Info:    RRTConnect: Created 75 states (72 start + 3 goal)
Info:    ProblemDefinition: Adding approximate solution from planner RRTConnect
Debug:   ParallelPlan.solveOne: Solution found by RRTConnect in 3.802919 seconds
Info:    ParallelPlan::solve(): Solution found by one or more threads in 3.803152 seconds
Debug:   ParallelPlan.solveOne starting planner RRTConnect
Info:    RRTConnect: Starting planning with 81 states already in datastructure
Debug:   ParallelPlan.solveOne starting planner RRTConnect
Info:    RRTConnect: Starting planning with 75 states already in datastructure
Info:    RRTConnect: Created 75 states (72 start + 3 goal)
Info:    RRTConnect: Created 86 states (75 start + 11 goal)
Info:    ProblemDefinition: Adding approximate solution from planner RRTConnect
Debug:   ParallelPlan.solveOne: Solution found by RRTConnect in 0.876887 seconds
Info:    ParallelPlan::solve(): Solution found by one or more threads in 0.877205 seconds
Info:    SimpleSetup: Path simplification took 0.978598 seconds and changed from 3 to 2 states
Info:    No planner specified. Using default.
Info:    LBKPIECE1: Attempting to use default projection.
Debug:   LBKPIECE1: Planner range detected to be 5.758648
Debug:   ParallelPlan.solveOne starting planner RRTConnect
Debug:   RRTConnect: Planner range detected to be 5.758648
Debug:   ParallelPlan.solveOne starting planner RRTConnect
Debug:   RRTConnect: Planner range detected to be 5.758648
terminate called after throwing an instance of 'ompl::Exception'
  what():  RRTConnect: No start states specified

I would be grateful for any kind of help.

Best regards,
Kai

[johnwason]

Do you have the latest package version? Have you looked at the unit tests as examples?

https://github.com/tesseract-robotics/tesseract_python/tree/master/tesseract_python/tests/tesseract_motion_planning

[johnwason]

@Levi-Armstrong Any ideas?

[kaiConnected]

@johnwason Thanks for the reply!
I am using the newest version of tesseract_python.
I checked out those examples, but I didn't notice the "Test_simple_planner" until now. That one is the only test that uses more than two MoveInstructions so I will check it out now.

[kaiConnected]

After further investigation the approach used in "Test_Simple_Planner" seems to only support three waypoints. But even when testing it with three the third one has to be an empty waypoint.
I therefor believe that the approaches in the link you posted are all either not applicable or not working for more than two waypoints.