Closed amalnanavati closed 1 year ago
If the start configuration is invalid, the plan fails. It is up to us to to check that the start configuration is valid and, if not, make alternate planning calls.
I'm thinking that eventually we should have a series of fallbacks (e.g., a Selector With Memory) for every MoveTo behavior. First it'll try the MoveTo action with all the constraints, and if that fails, it will remove the constraints one-at-a-time, in a process specified by us, to attempt to plan with fewer and fewer constraints. We would be able to specify which constraints are crucial, and which can be eliminated if need be.
Currently, if the start configuarion is in an invalid state, MoveIt attempts a plan that fixes that. This results in plans that (a) ignore constraints we have set; and (b) look very strange, because they are effectively a combination of one non-constrained plan and another constrained plan.
This PR removes those parts of the planning pipeline.