class SawyerSimContext(AbstractSimContext):
def __init__(self, configuration=None, setup=True, planning_context=None):
self.config = configuration if configuration is not None else {}
self.planning_context = planning_context if planning_context is not None else SawyerPlanningContext(
)
if setup:
self.setup()
def setup(self, sim_overrides=None):
sim_config = self.config.get("sim", {
"run_parallel": False,
"use_real_time": True,
"use_gui": True
})
if sim_overrides is not None:
for key, value in sim_overrides.items():
sim_config[key] = value
logger_config = self.config.get("logger", {
"handlers": ['logging'],
"level": "debug"
})
sawyer_config = self.config.get("sawyer", {
"robot_name": "sawyer0",
"position": [0, 0, 0.9],
"fixed_base": True
})
sim_obj_configs = self.config.get(
"sim_objects", [{
"object_name": "Ground",
"model_file_or_sim_id": "plane.urdf",
"position": [0, 0, 0]
}])
if os.environ.get('ROS_DISTRO'):
rospy.init_node("CAIRO_Sawyer_Simulator")
use_ros = True
else:
use_ros = False
self.logger = Logger(**logger_config)
self.sim = Simulator(logger=self.logger, use_ros=use_ros, **sim_config)
self.logger.info("Simulator {} instantiated with config {}".format(
self.sim, sim_config))
# Disable rendering while models load
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
self.sawyer_robot = Sawyer(**sawyer_config)
self.sim_objects = [SimObject(**config) for config in sim_obj_configs]
# Turn rendering back on
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
self._setup_state_validity(self.sawyer_robot)
# self._setup_collision_exclusions()
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)
def _setup_collision_exclusions(self):
ground_plane = self.get_sim_objects(['Ground'])[0]
sawyer_id = self.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)]
self.collision_exclusions = {
"excluded_bodies": excluded_bodies,
"excluded_body_link_pairs": excluded_body_link_pairs
}
def _setup_self_collision_fn(self, sawyer_robot):
sawyer_id = self.sawyer_robot.get_simulator_id()
excluded_pairs = [
(get_joint_info_by_name(sawyer_id, 'right_l6').idx,
get_joint_info_by_name(sawyer_id,
'right_connector_plate_base').idx)
]
link_pairs = get_link_pairs(sawyer_id, excluded_pairs=excluded_pairs)
return partial(self_collision_test,
robot=sawyer_robot,
link_pairs=link_pairs)
def _setup_collision_fn(self, sawyer_robot):
collision_body_ids = self.sim.get_collision_bodies()
if len(collision_body_ids) == 0: return None
collision_fns = []
for col_body_id in collision_body_ids:
collision_fns.append(
partial(robot_body_collision_test,
robot=sawyer_robot,
object_body_id=col_body_id))
return partial(multi_collision_test,
robot_object_collision_fns=collision_fns)
def get_logger(self):
return self.logger
def get_sim_instance(self):
return self.sim.get_instance()
def get_sim_objects(self, names=None):
if names is None:
return self.sim_objects
else:
return [
sim_obj for sim_obj in self.sim_objects
if sim_obj._name in names
]
def get_robot(self):
return self.sawyer_robot
def get_state_validity(self):
return self.svc
def get_state_space(self):
return self.planning_context.get_state_space()
def get_collision_exclusions(self):
return self.collision_exclusions
def main():
"""
Illustrates the difference between running within a ROS environment, and running the vanilla Simulator.
In sourcing your ROS environment (e.g. source /opt/ros/melodic/setup.bash) certain ROS specific environment variables
are set, such as ROS_DISTRO. This could be used to pivot between using ROS and not using ROS. This is really
for illustrative purposes mostly.
For your own scripts, you could simply write them under the assumption that you will either be using ROS or not,
and the user of your script should be aware of this fact or may run into issues running in either environment.
"""
if os.environ.get('ROS_DISTRO'):
# The Logger class is a wrapper around Python's own logging, print(), and rospy logging. See cairo_simulator.log.Logger
logger = Logger(handlers=['ros'], level='info')
# Awkwardly, the logging level for ROS must be set in the init_node function call. I do not see a workaround as of right now.
rospy.init_node("CAIRO_Sawyer_Simulator", log_level=rospy.DEBUG)
# Example of a logging statement. There is also .debug() .warn() .crit() .err()
logger.info("INFO")
# If using ROS, make sure to set the use_ros flag in the simulator.
use_ros = True
use_real_time = False
# Initialize the Simulator by passing in the logger to be used internally and any appropraite flags.
sim = Simulator(logger=logger,
use_ros=use_ros,
use_real_time=use_real_time)
ground_plane = SimObject("Ground", "plane.urdf", [0, 0, 0])
sawyer_robot = Sawyer("sawyer0", [0, 0, 1.0])
# Try calling ROS specific methods within the Simulator.
try:
logger.info("Attempting to execute ROS-based methods.")
sim.publish_object_states()
sim.publish_robot_states()
logger.info(
"Successfully executed ROS-based methods in the Simulator.")
except Exception as e:
logger.info("Failed to execute ROS methods within the Simulator.")
logger.info(e)
time.sleep(1)
p.disconnect()
else:
# If running this script without sourcing any ROS environment, this else block section should execute.
# Notice we've indicated Python's logger in our Logger class.
logger = Logger(handlers=['logging'], level='info')
use_ros = False
use_real_time = False
sim = Simulator(
logger=logger, use_ros=use_ros,
use_real_time=use_real_time) # Initialize the Simulator
ground_plane = SimObject("Ground", "plane.urdf", [0, 0, 0])
sawyer_robot = Sawyer("sawyer0", [0, 0, 1.0])
# This methods should indeed fail and the except block should be reached.
try:
logger.info("Attempting to execute ROS-based methods.")
sim.publish_object_states()
sim.publish_robot_states()
except Exception as e:
logger.info(
"Failed to execute ROS-based methods within the Simulator.")
logger.info(e)
time.sleep(1)
p.disconnect()
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)