def _setup_state_validity(self, sawyer_robot):
self_collision_fn = self._setup_self_collision_fn(sawyer_robot)
collision_fn = self._setup_collision_fn(sawyer_robot)
self.svc = StateValidityChecker(self_col_func=self_collision_fn,
col_func=collision_fn,
validity_funcs=None)
# State Validity Formulation #
##############################
# There is no self-collision for point object in R2
self_col_fn = None
# Create a collision function that combines two box_collision functions
b1 = partial(box_collision, coordinates=[[2, 2], [4, 4]])
b2 = partial(box_collision, coordinates=[[6, 1], [8, 10]])
def col_fn(sample):
return all([b1(sample), b2(sample)])
# 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
})
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
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)
# 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)
# In this case, we only have a self_col_fn.
svc = StateValidityChecker(
self_col_func=self_collision_fn, col_func=None, validity_funcs=None)
graph = ig.Graph()
start = [0, 0, 0, 0, 0, 0, 0]
end = [1.5262755737449423, -0.1698540226273928, 2.7788151824762055,
2.4546623466066135, 0.7146948867821279, 2.7671787952787184, 2.606128412644311]
graph.add_vertex("start")
# Start is always at the 0 index.
graph.vs[0]["value"] = start
graph.add_vertex("goal")
# Goal is always at the 1 index.
graph.vs[1]["value"] = end
for sample in samples: