def set_sim_block_pos(self, block_name, position):
assert self._simulated
for block in self._blocks:
if block.name == block_name:
Gazebo.set_model_pose(block_name,
new_pose=Pose(position=position))
rospy.sleep(1)
def add_block(self, block):
if self._simulated:
Gazebo.load_gazebo_model(block.name,
Pose(position=block.initial_pos),
block.resource)
# Waiting model to be loaded
rospy.sleep(1)
else:
self._block_states_subs.append(
rospy.Subscriber(block.resource, TransformStamped,
self._vicon_update_block_states))
self._blocks.append(block)
def terminate(self):
for sub in self._block_states_subs:
sub.unregister()
if self._simulated:
for block in self._blocks:
Gazebo.delete_gazebo_model(block.name)
Gazebo.delete_gazebo_model('table')
else:
ready = False
while not ready:
ans = input('Are you ready to exit?[Yes/No]\n')
if ans.lower() == 'yes' or ans.lower() == 'y':
ready = True
self._moveit_scene.remove_world_object('table')
def _reset_sim(self):
"""
reset the simulation
"""
# Randomize start position of blocks
for block in self._blocks:
block_random_delta = np.zeros(2)
while np.linalg.norm(block_random_delta) < 0.1:
block_random_delta = np.random.uniform(
-block.random_delta_range,
block.random_delta_range,
size=2)
Gazebo.set_model_pose(
block.name,
new_pose=Pose(position=Point(x=block.initial_pos.x +
block_random_delta[0],
y=block.initial_pos.y +
block_random_delta[1],
z=block.initial_pos.z)))
def initialize(self):
if self._simulated:
Gazebo.load_gazebo_model(
'table', Pose(position=Point(x=0.75, y=0.0, z=0.0)),
osp.join(World.MODEL_DIR, 'cafe_table/model.sdf'))
Gazebo.load_gazebo_model(
'block', Pose(position=Point(x=0.5725, y=0.1265, z=0.90)),
osp.join(World.MODEL_DIR, 'block/model.urdf'))
block = Block(name='block',
initial_pos=(0.5725, 0.1265, 0.90),
random_delta_range=0.15,
resource=osp.join(World.MODEL_DIR,
'block/model.urdf'))
# Waiting models to be loaded
rospy.sleep(1)
self._block_states_subs.append(
rospy.Subscriber('/gazebo/model_states', ModelStates,
self._gazebo_update_block_states))
self._blocks.append(block)
else:
for vicon_topic in VICON_TOPICS:
block = Block(name='block',
initial_pos=(0.5725, 0.1265, 0.90),
random_delta_range=0.15,
resource=vicon_topic)
self._block_states_subs.append(
rospy.Subscriber(block.resource, TransformStamped,
self._vicon_update_block_states))
self._blocks.append(block)
# Add table to moveit
pose_stamped = PoseStamped()
pose_stamped.header.frame_id = self._frame_id
pose_stamped.pose.position.x = 0.655
pose_stamped.pose.position.y = 0
# Leave redundant space
pose_stamped.pose.position.z = -0.02
pose_stamped.pose.orientation.x = 0
pose_stamped.pose.orientation.y = 0
pose_stamped.pose.orientation.z = 0
pose_stamped.pose.orientation.w = 1.0
self._moveit_scene.add_box('table', pose_stamped, (1.0, 0.9, 0.1))
def initialize(self):
"""Initialize the block world."""
if self._simulated:
Gazebo.load_gazebo_model(
'table', Pose(position=Point(x=0.35, y=-0.5, z=0.85)),
osp.join(World.MODEL_DIR, 'table/model.urdf'))
for block in self._blocks:
Gazebo.load_gazebo_model(block.name,
Pose(position=block.initial_pos),
block.resource)
try:
block_states_sub = rospy.Subscriber(
'/gazebo/model_states', ModelStates,
self._gazebo_update_block_states)
self._block_states_subs.append(block_states_sub)
# Must get the first msg from gazebo.
rospy.wait_for_message('/gazebo/model_states',
ModelStates,
timeout=2)
except rospy.ROSException as e:
print(
"Topic /gazebo/model_states is not available, aborting...")
print("Error message: ", e)
exit()
else:
try:
self._sawyer_cali_marker_sub = rospy.Subscriber(
SAWYER_CALI_VICON_TOPIC, TransformStamped,
vicon_update_cali)
# Must get the first msg for calibration object from vicon.
rospy.wait_for_message(SAWYER_CALI_VICON_TOPIC,
TransformStamped,
timeout=2)
except rospy.ROSException as e:
print("Topic {} is not available, aborting...".format(
SAWYER_CALI_VICON_TOPIC))
print("Error message: ", e)
exit()
for vicon_topic in VICON_TOPICS:
block_name = 'block_{}'.format(len(self._blocks))
block = Block(name=block_name,
size=[0.1, 0.15, 0.065],
initial_pos=(0.55, 0., 0.03),
random_delta_range=0.15,
resource=vicon_topic)
try:
block_state_sub = rospy.Subscriber(
block.resource, TransformStamped,
self._vicon_update_block_state)
self._block_states_subs.append(block_state_sub)
# Must get the first msg for block from vicon.
rospy.wait_for_message(block.resource,
TransformStamped,
timeout=2)
except rospy.ROSException as e:
print("Topic {} is not available, aborting...".format(
block.resource))
print("Error message: ", e)
exit()
self._blocks.append(block)
# Add table to moveit
# moveit needs a sleep before adding object
rospy.sleep(1)
pose_stamped_table = PoseStamped()
pose_stamped_table.header.frame_id = self._frame_id
pose_stamped_table.pose.position.x = 0.655
pose_stamped_table.pose.position.y = 0
# Leave redundant space
pose_stamped_table.pose.position.z = -0.02
pose_stamped_table.pose.orientation.x = 0
pose_stamped_table.pose.orientation.y = 0
pose_stamped_table.pose.orientation.z = 0
pose_stamped_table.pose.orientation.w = 1.0
self._moveit_scene.add_box('table', pose_stamped_table,
(1.0, 0.9, 0.1))
# Add calibration marker to moveit
if self._simulated:
rospy.sleep(1)
pose_stamped_marker = PoseStamped()
pose_stamped_marker.header.frame_id = self._frame_id
pose_stamped_marker.pose.position.x = 1.055
pose_stamped_marker.pose.position.y = -0.404
# Leave redundant space
pose_stamped_marker.pose.position.z = 0.06
pose_stamped_marker.pose.orientation.x = 0
pose_stamped_marker.pose.orientation.y = 0
pose_stamped_marker.pose.orientation.z = 0
pose_stamped_marker.pose.orientation.w = 1.0
self._moveit_scene.add_box('marker', pose_stamped_marker,
(0.09, 0.08, 0.06))
# Add blocks to moveit
for block in self._blocks:
rospy.sleep(1)
pose_stamped_block = PoseStamped()
pose_stamped_block.header.frame_id = self._frame_id
pos = block.position
pose_stamped_block.pose.position = pos
orientation = block.orientation
pose_stamped_block.pose.orientation = orientation
self._moveit_scene.add_box(
block.name, pose_stamped_block,
(block.size[0], block.size[1], block.size[2]))
# add the block to the allowed collision matrix
rospy.sleep(1)
self._moveit_scene_controller.add_object_to_acm(block.name)