def __init__(self, env):
self.space = ob.CompoundStateSpace()
self.setup = og.SimpleSetup(self.space)
bounds = ob.RealVectorBounds(1)
bounds.setLow(0)
bounds.setHigh(float(env.width) - 0.000000001)
self.m1 = mySpace1()
self.m1.setBounds(bounds)
bounds.setHigh(float(env.height) - 0.000000001)
self.m2 = mySpace1()
self.m2.setBounds(bounds)
self.space.addSubspace(self.m1, 1.0)
self.space.addSubspace(self.m2, 1.0)
isValidFn = ob.StateValidityCheckerFn(partial(isValid, env.grid))
self.setup.setStateValidityChecker(isValidFn)
state = ob.CompoundState(self.space)
state()[0][0] = env.start[0]
state()[1][0] = env.start[1]
self.start = ob.State(state)
gstate = ob.CompoundState(self.space)
gstate()[0][0] = env.goal[0]
gstate()[1][0] = env.goal[1]
self.goal = ob.State(gstate)
self.setup.setStartAndGoalStates(self.start, self.goal)
def make_ompl_state_space(self, planner_params,
state_sampler_rng: np.random.RandomState,
plot: bool):
state_space = ob.CompoundStateSpace()
min_x, max_x, min_y, max_y, min_z, max_z = planner_params['extent']
gripper_subspace = ob.RealVectorStateSpace(3)
gripper_bounds = ob.RealVectorBounds(3)
# these bounds are not used for sampling
gripper_bounds.setLow(0, min_x)
gripper_bounds.setHigh(0, max_x)
gripper_bounds.setLow(1, min_y)
gripper_bounds.setHigh(1, max_y)
gripper_bounds.setLow(2, min_z)
gripper_bounds.setHigh(2, max_z)
gripper_subspace.setBounds(gripper_bounds)
gripper_subspace.setName("gripper")
state_space.addSubspace(gripper_subspace, weight=1)
rope_subspace = ob.RealVectorStateSpace(RopeDraggingScenario.n_links *
3)
rope_bounds = ob.RealVectorBounds(RopeDraggingScenario.n_links * 3)
# these bounds are not used for sampling
rope_bounds.setLow(-1000)
rope_bounds.setHigh(1000)
rope_subspace.setBounds(rope_bounds)
rope_subspace.setName("rope")
state_space.addSubspace(rope_subspace, weight=1)
# extra subspace component for the variance, which is necessary to pass information from propagate to constraint checker
stdev_subspace = ob.RealVectorStateSpace(1)
stdev_bounds = ob.RealVectorBounds(1)
stdev_bounds.setLow(-1000)
stdev_bounds.setHigh(1000)
stdev_subspace.setBounds(stdev_bounds)
stdev_subspace.setName("stdev")
state_space.addSubspace(stdev_subspace, weight=0)
# extra subspace component for the number of diverged steps
num_diverged_subspace = ob.RealVectorStateSpace(1)
num_diverged_bounds = ob.RealVectorBounds(1)
num_diverged_bounds.setLow(-1000)
num_diverged_bounds.setHigh(1000)
num_diverged_subspace.setBounds(num_diverged_bounds)
num_diverged_subspace.setName("stdev")
state_space.addSubspace(num_diverged_subspace, weight=0)
def _state_sampler_allocator(state_space):
return RopeDraggingStateSampler(state_space,
scenario_ompl=self,
extent=planner_params['extent'],
rng=state_sampler_rng,
plot=plot)
state_space.setStateSamplerAllocator(
ob.StateSamplerAllocator(_state_sampler_allocator))
return state_space