def main():
sim_context = SawyerSimContext()
sim = sim_context.get_sim_instance()
logger = sim_context.get_logger()
state_space = sim_context.get_state_space()
svc = sim_context.get_state_validity()
sawyer_robot = sim_context.get_robot()
_ = sawyer_robot.get_simulator_id()
_ = sim_context.get_sim_objects(['Ground'])[0]
sawyer_robot.move_to_joint_pos([0, 0, 0, 0, 0, 0, 0])
time.sleep(1)
with DisabledCollisionsContext(sim, [], []):
#######
# LazyPRM #
#######
# Use parametric linear interpolation with 10 steps between points.
interp = partial(parametric_lerp, steps=10)
# See params for PRM specific parameters
prm = LazyPRM(state_space,
svc,
interp,
params={
'n_samples': 4000,
'k': 8,
'ball_radius': 2.5
})
logger.info("Planning....")
plan = prm.plan(
np.array([0, 0, 0, 0, 0, 0, 0]),
np.array([
1.5262755737449423, -0.1698540226273928, 2.7788151824762055,
2.4546623466066135, 0.7146948867821279, 2.7671787952787184,
2.606128412644311
]))
# get_path() reuses the interp function to get the path between vertices of a successful plan
path = prm.get_path(plan)
if len(path) == 0:
logger.info("Planning failed....")
sys.exit(1)
logger.info("Plan found....")
# splinging uses numpy so needs to be converted
path = [np.array(p) for p in path]
# Create a MinJerk spline trajectory using JointTrajectoryCurve and execute
jtc = JointTrajectoryCurve()
traj = jtc.generate_trajectory(path, move_time=2)
sawyer_robot.execute_trajectory(traj)
key = input("Press any key to excute plan.")
try:
while True:
sim.step()
except KeyboardInterrupt:
p.disconnect()
sys.exit(0)
# In this case, we only have a col_fn.
svc = StateValidityChecker(self_col_func=None,
col_func=col_fn,
validity_funcs=None)
############################################
# Build the PRM and call the plan function #
############################################
# Create the PRM
interp = partial(parametric_lerp, steps=10)
prm = LazyPRM(r2_space,
svc,
interp,
params={
'n_samples': 2000,
'k': 10,
'ball_radius': .45
})
plan = prm.plan(np.array([1, 1]), np.array([9, 9]))
print(plan)
if len(plan) == 0:
print("Planning failed.")
exit(1)
########################################################################
# Interpolate between each point in found path through graph and plot #
#######################################################################
path = prm.get_path(plan)
vertices = [
state_space = planning_G.get_edge_data(*edge)['planning_space']
####################################
# SIMULATION AND PLANNING CONTEXTS #
####################################
with DisabledCollisionsContext(sim, [], []):
#######
# PRM #
#######
# Use parametric linear interpolation with 10 steps between points.
interp = partial(parametric_lerp, steps=10)
# See params for PRM specific parameters
prm = LazyPRM(state_space,
svc,
interp_fn,
params={
'n_samples': 2000,
'k': 8,
'ball_radius': 2.0
})
logger.info("Planning....")
plan = prm.plan(np.array(start_point), np.array(end_point))
# get_path() reuses the interp function to get the path between vertices of a successful plan
path = prm.get_path(plan)
if len(path) == 0:
logger.info("Planning failed....")
sys.exit(1)
logger.info("Plan found....")
# splinging uses numpy so needs to be converted
path = [np.array(p) for p in path]
logger.info("Length of path: {}".format(len(path)))