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 __init__(self,
robot_name,
position,
orientation=[0, 0, 0, 1],
fixed_base=0,
publish_full_state=False):
"""
Initialize a Sawyer Robot at coordinates (x,y,z) and add it to the simulator manager
Args:
robot_name (str): Name of the robot
urdf_file (str): Filepath to urdf file.
position (list): Point [x,y,z]
orientation (list): Quaternion [x,y,z,w]
fixed_base (int): 0 if base can be moved, 1 if fixed in place
urdf_flags (int): Bitwise flags.
publish_full_state (bool): True will publish more detailed state info., False will publish config/pose only.
"""
super().__init__(
robot_name,
ASSETS_PATH + 'sawyer_description/urdf/sawyer_static.urdf',
position, orientation, fixed_base)
if Simulator.using_ros():
# Should the full robot state be published each cycle (pos/vel/force), or just joint positions
self._publish_full_state = publish_full_state
self._pub_robot_state_full = rospy.Publisher(
'/%s/robot_state_full' % self._name, String, queue_size=0)
self._sub_head_pan = rospy.Subscriber(
'/%s/set_head_pan' % self._name, Float32, self.set_head_pan)
self._init_local_vars()
self._init_forward_kinematics(
ASSETS_PATH + 'sawyer_description/urdf/sawyer_static.urdf')
def init_sim_with_sawyer():
# Setup simulator
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
print("Path = ", os.path.abspath(ASSETS_PATH))
table = SimObject("Table", ASSETS_PATH + 'table.sdf', (0.9, 0, 0),
(0, 0, 1.5708)) # Table rotated 90deg along z-axis
sawyer_robot = Sawyer("sawyer0", [0, 0, 0.8], fixed_base=1)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
sim_obj1 = create_cuboid_obstacle(name='box0',
shape=p.GEOM_BOX,
mass=1,
position=[1.0, 0.05, .55],
size=[0.09, 0.09, 0.35])
# Interested in only PyBullet object IDs for obstacles
obstacles = [sim_obj1, table.get_simulator_id()]
return sim, sawyer_robot, obstacles
def execute_trajectory(self, trajectory_data):
'''
Execute a trajectory with the manipulator given positions and timings.
This function computes the velocities needed to make the timeline.
Ex: trajectory_data = [(1., [0,0,0,0,0,0,0]), (2.5, [1,0,0,0,0,0,0]), (4, [1,0,2.9,1.23,1.52,0,0])]
Sends robot to 3 waypoints over the course of 4 seconds
Args:
trajectory_data (list): Vector of (time, joint configuration) tuples, indicating which joint positions the robot should achieve at which times. Set time=0 for each waypoint if you don't care about timing. Joint configuration vector contents should correspond to the parameters that work for for move_to_joint_pos
'''
joint_count = (len(self._arm_dof_indices) +
len(self._gripper_dof_indices))
joint_positions = []
joint_velocities = []
# Initial robot position
last_time = 0
last_position = self.get_current_joint_states()
joint_positions = [copy.copy(last_position)]
joint_velocities = [[0] * joint_count]
for waypoint in trajectory_data:
target_duration = waypoint[0] - last_time
target_position = waypoint[1]
target_velocities = []
if target_duration < 0.001:
target_duration = 0.001 # No duration given, avoid divide-by-zero and move quickly
# Compute distance from current position, compute per-joint velocity to reach in (t - t_{-1}) seconds
# Each waypoint should have joint_count values
if len(target_position) != joint_count:
self.logger.warn(
"Bad trajectory waypoint passed to Manipulator %s. Had length %d. Aborting trajectory."
% (self._name, len(target_position)))
return
# Arm + Gripper velocity
max_velocities = self._arm_joint_velocity_max + self._gripper_joint_velocity_max
for i in range(len(target_position)):
distance_to_cover = abs(target_position[i] - last_position[i])
velocity = min(distance_to_cover / target_duration,
max_velocities[i])
target_velocities.append(velocity)
joint_positions.append(target_position)
joint_velocities.append(target_velocities)
last_time = waypoint[0]
last_position = target_position # 9-DoF arm+gripper position vector
# Now that joint_positions and joint_velocities are populated, we can execute the trajectory
sim = Simulator.get_instance()
sim.set_robot_trajectory(self._simulator_id, joint_positions,
joint_velocities)
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)
def __init__(self,
robot_name,
urdf_file,
position,
orientation=[0, 0, 0, 1],
fixed_base=1,
urdf_flags=0):
"""
Initialize a Robot at coordinates position=[x,y,z] and add it to the simulator manager
Args:
robot_name (str): Name of the robot
urdf_file (str): Filepath to urdf file.
position (list): Point [x,y,z]
orientation (list): Quaternion [x,y,z,w]
fixed_base (int): 0 if base can be moved, 1 if fixed in place
urdf_flags (int): Bitwise flags.
"""
super().__init__(robot_name,
urdf_file,
position,
orientation=orientation,
fixed_base=fixed_base,
urdf_flags=urdf_flags) # p.URDF_MERGE_FIXED_LINKS)
if Simulator.using_ros():
self._sub_position_update = rospy.Subscriber(
'/%s/move_to_joint_pos' % self._name, Float32MultiArray,
self.move_to_joint_pos_callback)
self._sub_position_vel_update = rospy.Subscriber(
'/%s/move_to_joint_pos_vel' % self._name, Float32MultiArray,
self.move_to_joint_pos_vel_callback)
self._sub_execute_trajectory = rospy.Subscriber(
'/%s/execute_trajectory' % self._name, String,
self.execute_trajectory_callback)
# self._ik_service = rospy.Service('/%s/ik_service', Pose, self.ik_service)
# Record joint indices of controllable DoF from PyBullet's loaded model.
self._arm_dof_indices = []
self._gripper_dof_indices = []
# Initialize joint position/velocity limit data structures
self._arm_joint_limits = []
self._arm_joint_velocity_max = [] # Max velocity for each arm joint
# Default velocity for moving the robot's joints
self._arm_joint_default_velocity = []
# List of indices of arm DoF within list of all non-fixed joints
self._arm_ik_indices = []
self._gripper_joint_limits = []
self._gripper_joint_velocity_max = [
] # Max velocity for each gripper joint
# Default velocity for moving the gripper's joints
self._gripper_joint_default_velocity = []
def get_jacobian(self,
q,
link_id=16,
com_trans=(-8.0726e-06, 0.0085838, -0.0049566),
vel_vec=[0.0] * 7,
accel_vec=[0.0] * 7):
# infill extra DOF around arm jonts (e.g. head pan, gripper etc,.)
q = np.insert(q, [1], 0.0)
q = np.append(q, [0.0, 0.0])
vel_vec = np.insert(vel_vec, [1], 0.0)
vel_vec = np.append(vel_vec, [0.0, 0.0])
accel_vec = np.insert(accel_vec, [1], 0.0)
accel_vec = np.append(accel_vec, [0.0, 0.0])
# q = [0.7876748441700757, 0.0, 0.6013938842119559, 0.8830634552952313, 0.8964338093527918, 1.0725899856959438, 1.0725862705452007, 1.0946949885092667, 0.0, 0.0]
vel_vec = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
accel_vec = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
client_id = Simulator.get_client_id()
jac_t, jac_r = p.calculateJacobian(self._simulator_id, link_id,
list(com_trans), list(q),
list(vel_vec), list(accel_vec),
client_id)
J = np.vstack([np.array(jac_t),
np.array(jac_r)])[:, [0, 2, 3, 4, 5, 6, 7]]
return J
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()
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 execute_trajectory(self, trajectory_data):
'''
Execute a trajectory with the Sawyer arm given positions and timings. This function computes the velocities needed to make the timeline.
Ex: trajectory_data = [(1., [0,0,0,0,0,0,0]), (2.5, [1,0,0,0,0,0,0,0]), (4, [1,0,2.9,1.23,1.52,0,0,0])]
Sends robot to 3 waypoints over the course of 4 seconds
@param trajectory_data Vector of (time, joint configuration) tuples, indicating which joint positions the robot should achieve at which times. Set time=0 for each waypoint if you don't care about timing. Joint configuration vectors can be 7, 8, or 9 floats corresponding to the parameter for move_to_joint_pos (7: arm only, 8: arm + gripper %open, 9: arm + gripper finger positions)
Args:
trajectory_data (list): List of tuples (time, configuration list)
'''
joint_positions = []
joint_velocities = []
# Initial robot position
last_time = 0
last_position = self.get_current_joint_states()
joint_positions = [copy.copy(last_position)]
joint_velocities = [[0] * 9]
for waypoint in trajectory_data:
target_duration = waypoint[0] - last_time
target_position = waypoint[1]
target_velocities = []
if target_duration < 0.001:
target_duration = 0.001 # No duration given, avoid divide-by-zero and move quickly
# Compute distance from current position, compute per-joint velocity to reach in (t - t_{-1}) seconds
# Each waypoint should have 7-9 values
if len(target_position) < 7 or len(target_position) > 9:
self.logger.warn(
"Bad trajectory waypoint passed to Sawyer %s. Had length %d. Aborting trajectory."
% (self._name, len(target_position)))
return
# target_position will be 9-DoF vector for arm+gripper position after this code block
if len(target_position) == 7:
# Keep old gripper position
target_position = target_position[:7] + last_position[7:9]
elif len(target_position) == 8: # Arm + Gripper %
next_pos_gripper = self.get_gripper_pct_finger_positions(
target_position[7])
target_position = target_position[:7] + next_pos_gripper
# Arm + Gripper velocity
max_velocities = self._arm_joint_velocity_max + self._gripper_joint_velocity_max
for i in range(len(target_position)):
distance_to_cover = abs(target_position[i] - last_position[i])
velocity = min(distance_to_cover / target_duration,
max_velocities[i])
target_velocities.append(velocity)
joint_positions.append(target_position)
joint_velocities.append(target_velocities)
last_time = waypoint[0]
last_position = target_position # 9-DoF arm+gripper position vector
# Now that joint_positions and joint_velocities are populated, we can execute the trajectory
sim = Simulator.get_instance()
sim.set_robot_trajectory(self._simulator_id, joint_positions,
joint_velocities)
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
parallel_e - parallel_s))
################################
# 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)
sawyer_id = sawyer_robot.get_simulator_id()
#############################################
# 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