def parallel_connect_worker(batches, interp_fn, distance_fn, sim_context_cls,
sim_config):
sim_context = sim_context_cls(sim_config, setup=False)
sim_context.setup(sim_overrides={"run_parallel": True, "use_gui": False})
sim = sim_context.get_sim_instance()
_ = sim_context.get_logger()
_ = sim_context.get_state_space()
svc = sim_context.get_state_validity()
sawyer_robot = sim_context.get_robot()
_ = sawyer_robot.get_simulator_id()
# Disabled collisions during planning with certain exclusions in place.
# collision_exclusions = sim_context.get_collision_exclusions()
collision_exclusions = {}
with DisabledCollisionsContext(sim, **collision_exclusions):
connections = []
for batch in batches:
q_sample = batch[0]
neighbors = batch[1]
for q_near in neighbors:
local_path = interp_fn(np.array(q_near),
np.array(q_sample),
steps=10)
valid = subdivision_evaluate(svc.validate, local_path)
if valid:
connections.append(
[q_near, q_sample,
distance_fn(local_path)])
return connections
def _compute_probabilities(self, K_noisy_trajectories):
S = np.zeros((self.K, self.N))
P = np.zeros((self.K, self.N))
exp_values = np.zeros((self.K, self.N))
with DisabledCollisionsContext(self.sim):
for k in range(self.K):
for i in range(self.N):
S[k][i] = self._state_cost(K_noisy_trajectories[k][i]) + \
self._control_cost(K_noisy_trajectories[k])/self.N
# S[k][i] = self._control_cost(K_noisy_trajectories[k]) / self.N
S_max_for_each_i = np.max(S, axis=0)
S_min_for_each_i = np.min(S, axis=0)
for k in range(self.K):
for i in range(self.N):
exp_values[k][i] = np.exp(
(self.h * -1 * (S[k][i] - S_min_for_each_i[i])) /
(S_max_for_each_i[i] - S_min_for_each_i[i]))
sum_exp_values_for_each_i = np.sum(exp_values, axis=0)
for k in range(self.K):
for i in range(self.N):
P[k][i] = exp_values[k][i] / sum_exp_values_for_each_i[i]
return P
def parallel_projection_worker(num_samples, tsr, sim_context_cls, sim_config):
sim_context = sim_context_cls(sim_config, setup=False)
sim_context.setup(sim_overrides={"run_parallel": True, "use_gui": False})
sim = sim_context.get_sim_instance()
_ = sim_context.get_logger()
scs = sim_context.get_state_space()
svc = sim_context.get_state_validity()
sawyer_robot = sim_context.get_robot()
_ = sawyer_robot.get_simulator_id()
valid_samples = []
# Disabled collisions during planning with certain eclusions in place.
with DisabledCollisionsContext(sim, [], []):
while len(valid_samples) < num_samples:
sample = scs.sample()
if svc.validate(sample):
q_constrained = project_config(sawyer_robot, np.array(
sample), np.array(sample), tsr, .1, .01)
normalized_q_constrained = []
if q_constrained is not None:
for value in q_constrained:
normalized_q_constrained.append(
wrap_to_interval(value))
else:
continue
if svc.validate(normalized_q_constrained):
valid_samples.append(normalized_q_constrained)
return valid_samples
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)
def _compute_trajectory_cost(self):
cost = 0
with DisabledCollisionsContext(self.sim):
for i in range(self.N):
cost += self._state_cost(self.trajectory[i])
state_cost = cost
cost += self._control_cost(self.trajectory)
if self.debug:
print("State Cost = ", state_cost)
print("Control Cost = ", cost - state_cost)
print("Total Cost = ", cost)
print("")
return cost
def main():
sim_context = SawyerSimContext()
sim = sim_context.get_sim_instance()
_ = 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)
############
# PLANNING #
############
valid_samples = []
# Disabled collisions during planning with certain eclusions in place.
with DisabledCollisionsContext(sim, [], []):
while True:
sample = state_space.sample()
if svc.validate(sample):
valid_samples.append(sample)
if len(valid_samples) >= 1:
break
# Generate local plan between two points and execute local plan.
steps = 100
move_time = 5
start_pos = [0] * 7
print(np.array(sawyer_robot.get_current_joint_states()[0:7]))
print(np.array(valid_samples[0]))
qt, qdt, qddt = interpolate_5poly(np.array(start_pos[0:7]),
np.array(valid_samples[0]), steps)
traj = list(
zip([move_time * n / steps for n in range(0, steps)],
[list(q) for q in qt]))
print(traj)
sawyer_robot.execute_trajectory(traj)
# Loop until someone shuts us down
try:
while True:
sim.step()
except KeyboardInterrupt:
p.disconnect()
sys.exit(0)
def parallel_projection_worker(num_samples, sim_context_cls, sim_config):
sim_context = sim_context_cls(sim_config)
sim = sim_context.get_sim_instance()
_ = sim_context.get_logger()
scs = 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]
valid_samples = []
# Utilizes RPY convention
T0_w = xyzrpy2trans([.7, 0, 0, 0, 0, 0], degrees=False)
# Utilizes RPY convention
Tw_e = xyzrpy2trans([-.2, 0, 1.0, np.pi / 2, np.pi, 0], degrees=False)
# Utilizes RPY convention
Bw = bounds_matrix(
[(0, 100), (-100, 100), (-100, 100)
], # allow some tolerance in the z and y and only positve in x
[(-.07, .07), (-.07, .07), (-.07, .07)
]) # any rotation about z, with limited rotation about x, and y.
tsr = TSR(T0_w=T0_w, Tw_e=Tw_e, Bw=Bw, manipindex=0, bodyandlink=16)
# Disabled collisions during planning with certain eclusions in place.
with DisabledCollisionsContext(sim, [], []):
while len(valid_samples) < num_samples:
sample = scs.sample()
if svc.validate(sample):
q_constrained = project_config(sawyer_robot, np.array(sample),
np.array(sample), tsr, .1, .01)
normalized_q_constrained = []
if q_constrained is not None:
for value in q_constrained:
normalized_q_constrained.append(
wrap_to_interval(value))
else:
continue
if svc.validate(normalized_q_constrained):
valid_samples.append(normalized_q_constrained)
return valid_samples
def main():
sim_context = SawyerSimContext()
sim = sim_context.get_sim_instance()
_ = 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)
valid_samples = []
# Disabled collisions during planning with certain eclusions in place.
with DisabledCollisionsContext(sim, [], []):
while True:
sample = state_space.sample()
if svc.validate(sample):
valid_samples.append(sample)
if len(valid_samples) >= 1:
break
# Generate local plan between two points and execute local plan.
start_pos = [0] * 7
sawyer_robot.move_to_joint_pos(start_pos)
time.sleep(.1)
# This interpolate_5poly will be replaced with the sequence of points generated by a planner.
qt, _, _ = interpolate_5poly(np.array(start_pos[0:7]),
np.array(valid_samples[0]), 50)
jtc = JointTrajectoryCurve()
traj = jtc.generate_trajectory(qt, move_time=2)
sawyer_robot.execute_trajectory(traj)
# Loop until someone shuts us down
try:
while True:
sim.step()
except KeyboardInterrupt:
p.disconnect()
sys.exit(0)
def main():
config = {}
config["sim_objects"] = [{
"object_name": "Ground",
"model_file_or_sim_id": "plane.urdf",
"position": [0, 0, 0]
}, {
"object_name": "Table",
"model_file_or_sim_id": ASSETS_PATH + 'table.sdf',
"position": [0.0, 0, 0],
"orientation": [0, 0, 1.5708]
}]
sim_context = SawyerSimContext(config)
sim = sim_context.get_sim_instance()
_ = sim_context.get_logger()
scs = sim_context.get_state_space()
svc = sim_context.get_state_validity()
sawyer_robot = sim_context.get_robot()
_ = sawyer_robot.get_simulator_id()
table_obj = sim_context.get_sim_objects(names="Table")[0]
sawyer_robot.move_to_joint_pos([0, 0, 0, 0, 0, 0, 0])
time.sleep(1)
n_samples = 1000
valid_samples = []
starttime = timeit.default_timer()
# Disabled collisions during planning with certain eclusions in place.
with DisabledCollisionsContext(sim, [], []):
print("Sampling start time is :", starttime)
for i in range(0, n_samples):
sample = scs.sample()
if svc.validate(sample):
print(sample)
valid_samples.append(sample)
print("The time difference is :", timeit.default_timer() - starttime)
print("{} valid of {}".format(len(valid_samples), n_samples))
p.disconnect()
def parallel_sample_worker(num_samples, sim_context_cls, sim_config):
sim_context = sim_context_cls(sim_config, setup=False)
sim_context.setup(sim_overrides={"run_parallel": True, "use_gui": False})
sim = sim_context.get_sim_instance()
_ = sim_context.get_logger()
state_space = sim_context.get_state_space()
svc = sim_context.get_state_validity()
# Disabled collisions during planning with certain exclusions in place.
# collision_exclusions = sim_context.get_collision_exclusions()
collision_exclusions = {}
with DisabledCollisionsContext(sim, **collision_exclusions):
valid_samples = []
count = 0
while count < num_samples:
q_rand = np.array(state_space.sample())
if svc.validate(q_rand):
valid_samples.append(q_rand)
count += 1
return valid_samples
final_path = []
initial_start_point = planning_G.nodes[list(planning_G.nodes)[0]]['point']
for edge in planning_G.edges():
valid_samples = []
start_point = planning_G.nodes[edge[0]]['point']
end_point = planning_G.nodes[edge[1]]['point']
print(start_point, end_point)
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 = PRM(state_space,
svc,
interp_fn,
params={
'n_samples': 250,
'k': 6,
'ball_radius': .75
})
logger.info("Planning....")
def main():
################################
# Environment Checks and Flags #
################################
if os.environ.get('ROS_DISTRO'):
rospy.init_node("CAIRO_Sawyer_Simulator")
use_ros = True
else:
use_ros = False
########################################################
# Create the Simulator and pass it a Logger (optional) #
########################################################
logger = Logger()
sim = Simulator(logger=logger,
use_ros=use_ros,
use_gui=True,
use_real_time=True) # Initialize the Simulator
#####################################
# Create a Robot, or two, or three. #
#####################################
sawyer_robot = Sawyer("sawyer0", [0, 0, 0.9], fixed_base=1)
#############################################
# Create sim environment objects and assets #
#############################################
ground_plane = SimObject("Ground", "plane.urdf", [0, 0, 0])
sawyer_id = sawyer_robot.get_simulator_id()
sawyer_robot.move_to_joint_pos([
0.006427734375, -0.4784267578125, -2.6830537109375, -1.5901376953125,
0.1734560546875, 1.1468447265625, 1.310236328125
])
time.sleep(3)
# Exclude the ground plane and the pedestal feet from disabled collisions.
excluded_bodies = [ground_plane.get_simulator_id()] # the ground plane
pedestal_feet_idx = get_joint_info_by_name(sawyer_id, 'pedestal_feet').idx
# The (sawyer_idx, pedestal_feet_idx) tuple the ecluded from disabled collisions.
excluded_body_link_pairs = [(sawyer_id, pedestal_feet_idx)]
############
# PLANNING #
############
# Disabled collisions during planning with certain eclusions in place.
with DisabledCollisionsContext(sim, excluded_bodies,
excluded_body_link_pairs):
#########################
# STATE SPACE SELECTION #
#########################
# This inherently uses UniformSampler but a different sampling class could be injected.
state_space = SawyerConfigurationSpace()
##############################
# STATE VALIDITY FORMULATION #
##############################
# Certain links in Sawyer seem to be permentently in self collision. This is how to remove them by name when getting all link pairs to check for self collision.
excluded_pairs = [(get_joint_info_by_name(sawyer_id, "right_l1_2").idx,
get_joint_info_by_name(sawyer_id, "right_l0").idx),
(get_joint_info_by_name(sawyer_id, "right_l1_2").idx,
get_joint_info_by_name(sawyer_id, "head").idx)]
link_pairs = get_link_pairs(sawyer_id, excluded_pairs=excluded_pairs)
self_collision_fn = partial(self_collision_test,
robot=sawyer_robot,
link_pairs=link_pairs)
# Create constraint checks
def constraint_test(q):
upright_orientation = [
0.0005812598018143569, 0.017721236427960724,
-0.6896867930096543, 0.723890701324838
]
axis = 'z'
threshold = 15
world_pose, _ = sawyer_robot.solve_forward_kinematics(q)
return orientation(upright_orientation, world_pose[1], threshold,
axis)
# In this case, we only have a self_col_fn.
svc = StateValidityChecker(self_col_func=self_collision_fn,
col_func=None,
validity_funcs=[constraint_test])
#######
# PRM #
#######
# Use parametric linear interpolation with 5 steps between points.
interp = partial(parametric_lerp, steps=5)
# See params for PRM specific parameters
prm = PRM(state_space,
svc,
interp,
params={
'n_samples': 1000,
'k': 10,
'ball_radius': 3.0
})
logger.info("Planning....")
plan = prm.plan(
np.array([
0.006427734375, -0.4784267578125, -2.6830537109375,
-1.5901376953125, 0.1734560546875, 1.1468447265625,
1.310236328125
]),
np.array([
-0.9232412109375, 0.2353603515625, -2.51373828125,
-0.6898984375, 0.33058203125, 1.0955361328125, 1.14510546875
]))
logger.info("Plan found....")
print(len(plan))
print(plan)
# 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)
##########
# SPLINE #
##########
# 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=5)
print("Executing splined trajectory...")
sawyer_robot.execute_trajectory(traj)
try:
while True:
sim.step()
except KeyboardInterrupt:
p.disconnect()
sys.exit(0)
def parallel_sample_worker(num_samples):
########################################################
# Create the Simulator and pass it a Logger (optional) #
########################################################
logger = Logger()
if not Simulator.is_instantiated():
logger.warn("Simulator not instantiated, doing so now...")
sim = Simulator(logger=logger, use_ros=False, use_gui=False,
use_real_time=True) # Initialize the Simulator
else:
sim = Simulator.get_instance()
#####################################
# Create a Robot, or two, or three. #
#####################################
sawyer_robot = Sawyer("sawyer0", [0, 0, 0.9], fixed_base=1)
#############################################
# Create sim environment objects and assets #
#############################################
ground_plane = SimObject("Ground", "plane.urdf", [0, 0, 0])
sawyer_id = sawyer_robot.get_simulator_id()
# Exclude the ground plane and the pedestal feet from disabled collisions.
excluded_bodies = [ground_plane.get_simulator_id()] # the ground plane
pedestal_feet_idx = get_joint_info_by_name(sawyer_id, 'pedestal_feet').idx
# The (sawyer_idx, pedestal_feet_idx) tuple the ecluded from disabled collisions.
excluded_body_link_pairs = [(sawyer_id, pedestal_feet_idx)]
############
# SAMPLING #
############
valid_samples = []
# Disabled collisions during planning with certain eclusions in place.
with DisabledCollisionsContext(sim, excluded_bodies, excluded_body_link_pairs):
#########################
# STATE SPACE SELECTION #
#########################
# This inherently uses UniformSampler but a different sampling class could be injected.
state_space = SawyerConfigurationSpace()
##############################
# STATE VALIDITY FORMULATION #
##############################
# Certain links in Sawyer seem to be permentently in self collision. This is how to remove them by name when getting all link pairs to check for self collision.
excluded_pairs = [(get_joint_info_by_name(sawyer_id, "right_l1_2").idx, get_joint_info_by_name(sawyer_id, "right_l0").idx),
(get_joint_info_by_name(sawyer_id, "right_l1_2").idx, get_joint_info_by_name(sawyer_id, "head").idx)]
link_pairs = get_link_pairs(sawyer_id, excluded_pairs=excluded_pairs)
self_collision_fn = partial(
self_collision_test, robot=sawyer_robot, link_pairs=link_pairs)
# Create constraint checks
# def constraint_test(q):
# upright_orientation = [
# 0.0005812598018143569, 0.017721236427960724, -0.6896867930096543, 0.723890701324838]
# axis = 'z'
# threshold = 15
# world_pose, _ = sawyer_robot.solve_forward_kinematics(q)
# return orientation(upright_orientation, world_pose[1], threshold, axis)
# In this case, we only have a self_col_fn.
svc = StateValidityChecker(
self_col_func=self_collision_fn, col_func=None, validity_funcs=None)
# svc = StateValidityChecker(
# self_col_func=self_collision_fn, col_func=None, validity_funcs=[constraint_test])
count = 0
while count < num_samples:
q_rand = np.array(state_space.sample())
if svc.validate(q_rand):
valid_samples.append(q_rand)
count += 1
return valid_samples
#############################################
# Create sim environment objects and assets #
#############################################
ground_plane = SimObject("Ground", "plane.urdf", [0, 0, 0])
# Exclude the ground plane and the pedestal feet from disabled collisions.
excluded_bodies = [ground_plane.get_simulator_id()] # the ground plane
pedestal_feet_idx = get_joint_info_by_name(sawyer_id, 'pedestal_feet').idx
# The (sawyer_idx, pedestal_feet_idx) tuple the ecluded from disabled collisions.
excluded_body_link_pairs = [(sawyer_id, pedestal_feet_idx)]
############
# PLANNING #
############
# Disabled collisions during planning with certain eclusions in place.
with DisabledCollisionsContext(sim, excluded_bodies, excluded_body_link_pairs):
#########################
# STATE SPACE SELECTION #
#########################
# This inherently uses UniformSampler but a different sampling class could be injected.
state_space = SawyerConfigurationSpace()
##############################
# STATE VALIDITY FORMULATION #
##############################
# Certain links in Sawyer seem to be permentently in self collision. This is how to remove them by name when getting all link pairs to check for self collision.
excluded_pairs = [(get_joint_info_by_name(sawyer_id, "right_l1_2").idx, get_joint_info_by_name(sawyer_id, "right_l0").idx),
(get_joint_info_by_name(sawyer_id, "right_l1_2").idx, get_joint_info_by_name(sawyer_id, "head").idx)]
link_pairs = get_link_pairs(sawyer_id, excluded_pairs=excluded_pairs)
self_collision_fn = partial(
self_collision_test, robot=sawyer_robot, link_pairs=link_pairs)
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_id = sawyer_robot.get_simulator_id()
ground_plane = sim_context.get_sim_objects(['Ground'])[0]
# Exclude the ground plane and the pedestal feet from disabled collisions.
excluded_bodies = [ground_plane.get_simulator_id()] # the ground plane
pedestal_feet_idx = get_joint_info_by_name(sawyer_id, 'pedestal_feet').idx
# The (sawyer_idx, pedestal_feet_idx) tuple the ecluded from disabled collisions.
excluded_body_link_pairs = [(sawyer_id, pedestal_feet_idx)]
############
# PLANNING #
############
# Disabled collisions during planning with certain eclusions in place.
with DisabledCollisionsContext(sim, excluded_bodies,
excluded_body_link_pairs):
#######
# PRM #
#######
# Use parametric linear interpolation with 10 steps between points.
interp = partial(parametric_lerp, steps=10)
# See params for PRM specific parameters
prm = PRM(state_space,
svc,
interp,
params={
'n_samples': 3000,
'k': 6,
'ball_radius': 2.0
})
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....")
print(len(path))
##########
# SPLINE #
##########
# 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=10)
sawyer_robot.execute_trajectory(traj)
try:
while True:
sim.step()
except KeyboardInterrupt:
p.disconnect()
sys.exit(0)