Problem with path planning using multiple goal state with RRTconnect in Dubin's state space

Aperta
  • Qualcosa non va?
#946 2 commenti 0 reazioni 0 assegnatari Vedi su GitHub

Nessuno ha ancora preso questa issue.

Valutazione

Difficoltà
4/5
Tempo stimato
3-5 giorni
Idoneità per principianti
35/100
Tipo di issue
Bug
Chiarezza
Abbastanza chiara
Stato di attività
Ferma
Stack tecnologico
cpp
Ambito
robotics

Direzione di ricerca

Inizia con la riproduzione allegata ompl_goal_states_test.cpp.txt e segui la configurazione di GoalStates attraverso SimpleSetup fino a RRTConnect. Esamina come vengono campionati i molteplici stati obiettivo Dubins, incluso samplePosition_, quindi esegui l’esempio e confronta l’output del planner con la selezione prevista di un obiettivo fattibile. Il lavoro è completato quando il planner può selezionare un obiettivo fattibile invece di usare solo il primo stato.

Scritto dal modello di indicizzazione a partire dal testo della issue.

Descrizione

I am experimenting with planning paths using RRTconnect in Dubin's state space, and have been able to plan paths from a given starting state to the goal in the presence of obstacles. Now I am trying to use the GoalStates class to specify multiple goals, but somehow it is not working as expected. I have attached the source code I used.

State-space: Dubin's
Planner: RRTconnect
Start: 1 start state
Goal: 3 goal states in GoalStates

This is how I have set up the problem. The first goal state is (x1, y1, yaw1), and there is an obstacle very near to (x1, y1) position such that approach at yaw1 is blocked. The two other states are (x1, y1, yaw2) and (x1, y1, yaw3). Thus all the goal states have the same x and y coordinates, but different yaw angles. The two yaw angles, yaw2 and yaw3, are such that it is possible to compute paths from the start state to (x1, y1, yaw2) and (x1, y1, yaw3). I use the GoalStates object to add the three goal states. All the setup is done using SimpleSetup.

I thought that the RRTconnect planner would sample all the three goal states and would be able to plan a path to one of the two feasible goal states. However, it seems that only the first goal state was ever sampled. In the debugger I checked the goal details in the SimpleSetup object. It showed three goal states added, but the samplePosition_ remained unchanged at 1, indicating that only the first goal state was sampled. The console output also shows that there ever was one goal state used in planning, which leads to the planner not being able to compute a path as the yaw angle of the first goal state makes any approach to its x-y possible impossible. In the console output below, we can see the tree growing only from the start state but not from the goal as only the infeasible goal state is sampled. The type of solution after each call to solve remains as APPROXIMATE solution.

I am not sure why I am seeing this behavior, and I will appreciate any input!

Source code used to reproduce the issue: ompl_goal_states_test.cpp.txt

====== PROGRAM OUTPUT START ======
3 goal states, threshold = 2.22045e-16, memory address = 0x7ff58d706e08
Compound state [
RealVectorState [50 50]
SO2State [0]
]

Compound state [
RealVectorState [50 50]
SO2State [1.0472]
]

Compound state [
RealVectorState [50 50]
SO2State [1.5708]
]

Debug: RRTConnect: Planner range detected to be 28.598431
Info: RRTConnect: Starting planning with 1 states already in datastructure
Info: RRTConnect: Created 9347 states (9346 start + 1 goal)
Info: ProblemDefinition: Adding approximate solution from planner RRTConnect
Info: Solution found in 1.005351 seconds
====== PROGRAM OUTPUT End ======

Lingua principale
C++
Stelle
2.2k
Fork
718
Merge medio
16h 12m
PR unite (30g)
1

Preparare l'ambiente

Questo progetto non fornisce container di sviluppo, Dockerfile né guida per i contributori, quindi l'ambiente è a tuo carico: parti dal suo README e consulta la nostra guida al primo contributo per i passaggi generali.

Come iniziare

  1. Leggi tutta la issue e poi la guida ai contributi del progetto.
  2. Commenta sulla issue per dire che te ne occupi tu — evita che due persone facciano lo stesso lavoro.
  3. Fai un fork del repository e lavora su un branch.
  4. Apri una pull request che faccia riferimento al numero della issue.

Altre issue di ompl/ompl

Tutte le issue di ompl/ompl

Issue simili

Altre issue su C++

Ricevi le nuove issue nella tua casella

Un breve riepilogo di issue GitHub adatte ai principianti.