(
start,
goals,
simulator: vpb.PyBulletSimulator,
planning_time: float = 30.,
planner: str = "RRTConnect",
**kwargs
)
| 18 | |
| 19 | |
| 20 | def solve( |
| 21 | start, |
| 22 | goals, |
| 23 | simulator: vpb.PyBulletSimulator, |
| 24 | planning_time: float = 30., |
| 25 | planner: str = "RRTConnect", |
| 26 | **kwargs |
| 27 | ): |
| 28 | space = ob.RealVectorStateSpace() |
| 29 | for l, h in zip(simulator.lows, simulator.highs): |
| 30 | space.addDimension(l, h) |
| 31 | space.setup() |
| 32 | |
| 33 | n_dims = space.getDimension() |
| 34 | |
| 35 | def set_state(state, state_list): |
| 36 | for i in range(n_dims): |
| 37 | state[i] = state_list[i] |
| 38 | |
| 39 | def get_state(state): |
| 40 | return [state[i] for i in range(n_dims)] |
| 41 | |
| 42 | def valid(state): |
| 43 | simulator.set_joint_positions(get_state(state)) |
| 44 | return not simulator.in_collision() |
| 45 | |
| 46 | ss = og.SimpleSetup(space) |
| 47 | si = ss.getSpaceInformation() |
| 48 | ss.setStateValidityChecker(ob.StateValidityCheckerFn(valid)) |
| 49 | |
| 50 | si.setStateValidityCheckingResolution(0.001) |
| 51 | |
| 52 | start_state = ob.State(space) |
| 53 | set_state(start_state, start) |
| 54 | ss.setStartState(start_state) |
| 55 | |
| 56 | goal_states = ob.GoalStates(si) |
| 57 | for goal in goals: |
| 58 | goal_state = ob.State(space) |
| 59 | set_state(goal_state, goal) |
| 60 | goal_states.addState(goal_state) |
| 61 | |
| 62 | ss.setGoal(goal_states) |
| 63 | planner = eval(f"og.{planner}(si)") |
| 64 | ss.setPlanner(planner) |
| 65 | |
| 66 | for k, v in kwargs.items(): |
| 67 | eval(f"planner.{k}({v})") |
| 68 | |
| 69 | ss.setup() |
| 70 | result = ss.solve(planning_time) |
| 71 | |
| 72 | if result and ss.haveExactSolutionPath(): |
| 73 | ss.simplifySolution() |
| 74 | path = ss.getSolutionPath() |
| 75 | path.interpolate(100) |
| 76 | |
| 77 | states = [] |
no test coverage detected