def main():
if os.environ.get('ROS_DISTRO'):
rospy.init_node("CAIRO_Sawyer_Simulator")
use_ros = True
else:
use_ros = False
use_real_time = True
logger = Logger()
sim = Simulator(logger=logger, use_ros=use_ros, use_real_time=use_real_time) # Initialize the Simulator
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
p.setPhysicsEngineParameter(enableFileCaching=0)
ground_plane = SimObject("Ground", "plane.urdf", [0,0,0])
# Add a table and a Sawyer robot
table = SimObject("Table", ASSETS_PATH + 'table.sdf', (0.9, 0, 0), (0, 0, 1.5708)) # Table rotated 90deg along z-axis
print(p.getNumJoints(table.get_simulator_id()))
sawyer_robot = Sawyer("sawyer0", [0, 0, 0.8], fixed_base=1)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
sim_obj = SimObject('cube0', 'cube_small.urdf', (0.75, 0, .55))
sim_obj = SimObject('cube1', 'cube_small.urdf', (0.74, 0.05, .55))
sim_obj = SimObject('cube2', 'cube_small.urdf', (0.67, -0.1, .55))
sim_obj = SimObject('cube3', 'cube_small.urdf', (0.69, 0.1, .55))
joint_config = sawyer_robot.solve_inverse_kinematics([0.9,0,1.5], [0,0,0,1])
#sawyer_robot.move_to_joint_pos(joint_config)
# Loop until someone shuts us down
try:
while True:
sim.step()
except KeyboardInterrupt:
p.disconnect()
sys.exit(0)
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)
if valid:
graph.add_edge(graph.vs['value'].index(end), graph.vs['value'].index(list(q_near)), **{'weight': pair[2]})
break
graph_path = graph.get_shortest_paths(
'start', 'goal', weights='weight', mode='ALL')[0]
points = [graph.vs[idx]['value'] for idx in graph_path]
pairs = list(zip(points, points[1:]))
segments = [parametric_lerp(np.array(p[0]), np.array(p[1]), steps=20)
for p in pairs]
segments = [[list(val) for val in seg] for seg in segments]
path = []
for seg in segments:
path = path + seg
##########
# SPLINE #
##########
# splinging uses numpy so needs to be converted
path = [np.array(p) for p in path]
print(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)