class Plane3D:
def __init__(self, headless=False):
self._pr = PyRep()
self._pr.launch(scene_file=scene_file, headless=headless)
self._pr.start()
self.workspace_base = Shape("workspace")
self.workspace = self._get_worksapce()
self.camera = VisionSensor("camera")
self.obstacles = []
self.velocity_scale = 0
self.repiration_cycle = 0
def _get_worksapce(self):
base_pos = self.workspace_base.get_position()
bbox = self.workspace_base.get_bounding_box()
min_pos = [bbox[2 * i] + base_pos[i] for i in range(3)]
max_pos = [bbox[2 * i + 1] + base_pos[i] for i in range(3)]
return [min_pos, max_pos]
def reset(self, obstacle_num, velocity_scale, respiration_cycle=0):
for obstacle in self.obstacles:
obstacle.remove()
self._pr.step()
self.velocity_scale = velocity_scale
self.repiration_cycle = respiration_cycle
self.obstacles = []
for i in range(obstacle_num):
obs = Obstacle2D.create_random_obstacle(
workspace=self.workspace,
velocity_scale=velocity_scale,
respiration_cycle=respiration_cycle)
self._pr.step()
self.obstacles.append(obs)
def get_image(self):
rgb = self.camera.capture_rgb()
return rgb
def step(self):
# update config
for obs in self.obstacles:
if self.repiration_cycle > 0:
obs.respire()
if self.velocity_scale > 0:
obs.keep_velocity()
self._pr.step()
def generate_random_map(total_objects, rectilinear=True):
# Set the random seed in the main module
base = Shape('floor')
base.set_collidable(False)
bounding_box = base.get_bounding_box()
size_x = bounding_box[1] - bounding_box[0]
size_y = bounding_box[3] - bounding_box[2]
objects = []
for i in range(total_objects):
while True:
random_x_size = random.uniform(0.1 * size_x, 0.5 * size_x)
random_y_size = random.uniform(0.1 * size_y, 0.5 * size_y)
random_x_pos = random.uniform(random_x_size / 2,
size_x - random_x_size / 2) - 0.5
random_y_pos = random.uniform(random_y_size / 2,
size_y - random_y_size / 2) - 0.5
yaw = 0 if rectilinear else random.uniform(0, math.pi)
new_shape = Shape.create(
type=PrimitiveShape.CUBOID,
size=[random_x_size, random_y_size, .05],
position=[random_x_pos, random_y_pos, 0.025],
orientation=[0, 0, yaw],
static=True,
renderable=True)
new_shape.set_collidable(True)
if new_shape.check_collision() == False:
objects.append(new_shape)
break
else:
new_shape.remove()
return objects
class Scene(object):
"""Controls what is currently in the vrep scene. This is used for making
sure that the tasks are easily reachable. This may be just replaced by
environment. Responsible for moving all the objects. """
def __init__(self,
pyrep: PyRep,
robot: Robot,
obs_config=ObservationConfig()):
self._pyrep = pyrep
self._robot = robot
self._obs_config = obs_config
self._active_task = None
self._inital_task_state = None
self._start_arm_joint_pos = robot.arm.get_joint_positions()
if robot.is_grip():
self._starting_gripper_joint_pos = robot.gripper.get_joint_positions(
)
else:
self._starting_gripper_joint_pos = None
self._workspace = Shape('workspace')
self._workspace_boundary = SpawnBoundary([self._workspace])
self._cam_over_shoulder_left = VisionSensor('cam_over_shoulder_left')
self._cam_over_shoulder_right = VisionSensor('cam_over_shoulder_right')
self._cam_wrist = VisionSensor('cam_wrist')
self._cam_front = VisionSensor('cam_front')
self._cam_over_shoulder_left_mask = VisionSensor(
'cam_over_shoulder_left_mask')
self._cam_over_shoulder_right_mask = VisionSensor(
'cam_over_shoulder_right_mask')
self._cam_wrist_mask = VisionSensor('cam_wrist_mask')
self._cam_front_mask = VisionSensor('cam_front_mask')
self._has_init_task = self._has_init_episode = False
self._variation_index = 0
self._initial_robot_state = (robot.arm.get_configuration_tree(),
robot.gripper.get_configuration_tree())
# Set camera properties from observation config
self._set_camera_properties()
x, y, z = self._workspace.get_position()
minx, maxx, miny, maxy, _, _ = self._workspace.get_bounding_box()
self._workspace_minx = x - np.fabs(minx)
self._workspace_maxx = x + maxx
self._workspace_miny = y - np.fabs(miny)
self._workspace_maxy = y + maxy
self._workspace_minz = z
self._workspace_maxz = z + 1.0 # 1M above workspace
def load(self, task: Task) -> None:
"""Loads the task and positions at the centre of the workspace.
:param task: The task to load in the scene.
"""
task.load() # Load the task in to the scene
# Set at the centre of the workspace
task.get_base().set_position(self._workspace.get_position())
self._inital_task_state = task.get_state()
self._active_task = task
self._initial_task_pose = task.boundary_root().get_orientation()
self._has_init_task = self._has_init_episode = False
self._variation_index = 0
def unload(self) -> None:
"""Clears the scene. i.e. removes all tasks. """
if self._active_task is not None:
self._robot.gripper.release()
if self._has_init_task:
self._active_task.cleanup_()
self._active_task.unload()
self._active_task = None
self._variation_index = 0
def init_task(self) -> None:
self._active_task.init_task()
self._inital_task_state = self._active_task.get_state()
self._has_init_task = True
self._variation_index = 0
def init_episode(self,
index: int,
randomly_place: bool = True,
max_attempts: int = 5) -> List[str]:
"""Calls the task init_episode and puts randomly in the workspace.
"""
self._variation_index = index
if not self._has_init_task:
self.init_task()
# Try a few times to init and place in the workspace
attempts = 0
descriptions = None
while attempts < max_attempts:
descriptions = self._active_task.init_episode(index)
try:
if (randomly_place
and not self._active_task.is_static_workspace()):
self._place_task()
self._active_task.validate()
break
except (BoundaryError, WaypointError) as e:
self._active_task.cleanup_()
self._active_task.restore_state(self._inital_task_state)
attempts += 1
if attempts >= max_attempts:
raise e
# Let objects come to rest
[self._pyrep.step() for _ in range(STEPS_BEFORE_EPISODE_START)]
self._has_init_episode = True
return descriptions
def reset(self) -> None:
"""Resets the joint angles. """
self._robot.gripper.release()
arm, gripper = self._initial_robot_state
self._pyrep.set_configuration_tree(arm)
self._pyrep.set_configuration_tree(gripper)
self._robot.arm.set_joint_positions(self._start_arm_joint_pos,
disable_dynamics=True)
self._robot.arm.set_joint_target_velocities(
[0] * len(self._robot.arm.joints))
if self._robot.is_grip():
self._robot.gripper.set_joint_positions(
self._starting_gripper_joint_pos, disable_dynamics=True)
self._robot.gripper.set_joint_target_velocities(
[0] * len(self._robot.gripper.joints))
else:
self._robot.gripper.release()
if self._active_task is not None and self._has_init_task:
self._active_task.cleanup_()
self._active_task.restore_state(self._inital_task_state)
self._active_task.set_initial_objects_in_scene()
def get_observation(self) -> Observation:
tip = self._robot.arm.get_tip()
joint_forces = None
if self._obs_config.joint_forces:
fs = self._robot.arm.get_joint_forces()
vels = self._robot.arm.get_joint_target_velocities()
joint_forces = self._obs_config.joint_forces_noise.apply(
np.array([-f if v < 0 else f for f, v in zip(fs, vels)]))
ee_forces_flat = None
if self._obs_config.gripper_touch_forces:
ee_forces = self._robot.gripper.get_touch_sensor_forces()
ee_forces_flat = []
for eef in ee_forces:
ee_forces_flat.extend(eef)
ee_forces_flat = np.array(ee_forces_flat)
lsc_ob = self._obs_config.left_shoulder_camera
rsc_ob = self._obs_config.right_shoulder_camera
wc_ob = self._obs_config.wrist_camera
fc_ob = self._obs_config.front_camera
lsc_mask_fn = (rgb_handles_to_mask
if lsc_ob.masks_as_one_channel else lambda x: x)
rsc_mask_fn = (rgb_handles_to_mask
if rsc_ob.masks_as_one_channel else lambda x: x)
wc_mask_fn = (rgb_handles_to_mask
if wc_ob.masks_as_one_channel else lambda x: x)
fc_mask_fn = (rgb_handles_to_mask
if fc_ob.masks_as_one_channel else lambda x: x)
def get_rgb_depth(sensor: VisionSensor, get_rgb: bool, get_depth: bool,
rgb_noise: NoiseModel, depth_noise: NoiseModel,
depth_in_meters: bool):
rgb = depth = None
if sensor is not None and (get_rgb or get_depth):
sensor.handle_explicitly()
if get_rgb:
rgb = sensor.capture_rgb()
if rgb_noise is not None:
rgb = rgb_noise.apply(rgb)
if get_depth:
depth = sensor.capture_depth(depth_in_meters)
if depth_noise is not None:
depth = depth_noise.apply(depth)
return rgb, depth
def get_mask(sensor: VisionSensor, mask_fn):
mask = None
if sensor is not None:
sensor.handle_explicitly()
mask = mask_fn(sensor.capture_rgb())
return mask
left_shoulder_rgb, left_shoulder_depth = get_rgb_depth(
self._cam_over_shoulder_left, lsc_ob.rgb, lsc_ob.depth,
lsc_ob.rgb_noise, lsc_ob.depth_noise, lsc_ob.depth_in_meters)
right_shoulder_rgb, right_shoulder_depth = get_rgb_depth(
self._cam_over_shoulder_right, rsc_ob.rgb, rsc_ob.depth,
rsc_ob.rgb_noise, rsc_ob.depth_noise, rsc_ob.depth_in_meters)
wrist_rgb, wrist_depth = get_rgb_depth(self._cam_wrist, wc_ob.rgb,
wc_ob.depth, wc_ob.rgb_noise,
wc_ob.depth_noise,
wc_ob.depth_in_meters)
front_rgb, front_depth = get_rgb_depth(self._cam_front, fc_ob.rgb,
fc_ob.depth, fc_ob.rgb_noise,
fc_ob.depth_noise,
fc_ob.depth_in_meters)
left_shoulder_mask = get_mask(self._cam_over_shoulder_left_mask,
lsc_mask_fn) if lsc_ob.mask else None
right_shoulder_mask = get_mask(self._cam_over_shoulder_right_mask,
rsc_mask_fn) if rsc_ob.mask else None
wrist_mask = get_mask(self._cam_wrist_mask,
wc_mask_fn) if wc_ob.mask else None
front_mask = get_mask(self._cam_front_mask,
fc_mask_fn) if fc_ob.mask else None
obs = Observation(
left_shoulder_rgb=left_shoulder_rgb,
left_shoulder_depth=left_shoulder_depth,
right_shoulder_rgb=right_shoulder_rgb,
right_shoulder_depth=right_shoulder_depth,
wrist_rgb=wrist_rgb,
wrist_depth=wrist_depth,
front_rgb=front_rgb,
front_depth=front_depth,
left_shoulder_mask=left_shoulder_mask,
right_shoulder_mask=right_shoulder_mask,
wrist_mask=wrist_mask,
front_mask=front_mask,
joint_velocities=(self._obs_config.joint_velocities_noise.apply(
np.array(self._robot.arm.get_joint_velocities()))
if self._obs_config.joint_velocities else None),
joint_positions=(self._obs_config.joint_positions_noise.apply(
np.array(self._robot.arm.get_joint_positions()))
if self._obs_config.joint_positions else None),
joint_forces=(joint_forces
if self._obs_config.joint_forces else None),
gripper_open=(self._robot.gripper.if_open()
if self._obs_config.gripper_open else None),
gripper_pose=(np.array(tip.get_pose())
if self._obs_config.gripper_pose else None),
gripper_matrix=(np.reshape(tip.get_matrix(), (3, 4))
if self._obs_config.gripper_matrix else None),
gripper_touch_forces=(ee_forces_flat
if self._obs_config.gripper_touch_forces else
None),
gripper_joint_positions=(
np.array(self._robot.gripper.get_joint_positions())
if self._obs_config.gripper_joint_positions else None),
wrist_camera_matrix=(np.reshape(self._cam_wrist.get_matrix(),
(3, 4))
if self._cam_wrist.still_exists() else None),
task_low_dim_state=(self._active_task.get_low_dim_state() if
self._obs_config.task_low_dim_state else None))
obs = self._active_task.decorate_observation(obs)
return obs
def step(self):
self._pyrep.step()
self._active_task.step()
def get_demo(self,
record: bool = True,
callable_each_step: Callable[[Observation], None] = None,
randomly_place: bool = True) -> Demo:
"""Returns a demo (list of observations)"""
if not self._has_init_task:
self.init_task()
if not self._has_init_episode:
self.init_episode(self._variation_index,
randomly_place=randomly_place)
self._has_init_episode = False
waypoints = self._active_task.get_waypoints()
if len(waypoints) == 0:
raise NoWaypointsError('No waypoints were found.',
self._active_task)
demo = []
if record:
self._pyrep.step() # Need this here or get_force doesn't work...
demo.append(self.get_observation())
while True:
success = False
for i, point in enumerate(waypoints):
point.start_of_path()
try:
path = point.get_path()
except ConfigurationPathError as e:
raise DemoError(
'Could not get a path for waypoint %d.' % i,
self._active_task) from e
ext = point.get_ext()
path.visualize()
done = False
success = False
while not done:
done = path.step()
self.step()
self._demo_record_step(demo, record, callable_each_step)
success, term = self._active_task.success()
point.end_of_path()
path.clear_visualization()
if len(ext) > 0:
contains_param = False
start_of_bracket = -1
gripper = self._robot.gripper
if 'open_gripper(' in ext:
gripper.release()
start_of_bracket = ext.index('open_gripper(') + 13
contains_param = ext[start_of_bracket] != ')'
if not contains_param:
done = False
while not done:
done = gripper.actuate(1.0, 0.04)
self._pyrep.step()
self._active_task.step()
if self._obs_config.record_gripper_closing:
self._demo_record_step(
demo, record, callable_each_step)
elif 'close_gripper(' in ext:
start_of_bracket = ext.index('close_gripper(') + 14
contains_param = ext[start_of_bracket] != ')'
if not contains_param:
done = False
while not done:
done = gripper.actuate(0.0, 0.04)
self._pyrep.step()
self._active_task.step()
if self._obs_config.record_gripper_closing:
self._demo_record_step(
demo, record, callable_each_step)
if contains_param:
rest = ext[start_of_bracket:]
num = float(rest[:rest.index(')')])
done = False
while not done:
done = gripper.actuate(num, 0.04)
self._pyrep.step()
self._active_task.step()
if self._obs_config.record_gripper_closing:
self._demo_record_step(demo, record,
callable_each_step)
if 'close_gripper(' in ext:
for g_obj in self._active_task.get_graspable_objects():
gripper.grasp(g_obj)
self._demo_record_step(demo, record, callable_each_step)
if not self._active_task.should_repeat_waypoints() or success:
break
# Some tasks may need additional physics steps
# (e.g. ball rowling to goal)
if not success:
for _ in range(10):
self._pyrep.step()
self._active_task.step()
self._demo_record_step(demo, record, callable_each_step)
success, term = self._active_task.success()
if success:
break
success, term = self._active_task.success()
if not success:
raise DemoError('Demo was completed, but was not successful.',
self._active_task)
return Demo(demo)
def get_observation_config(self) -> ObservationConfig:
return self._obs_config
def check_target_in_workspace(self, target_pos: np.ndarray) -> bool:
x, y, z = target_pos
return (self._workspace_maxx > x > self._workspace_minx
and self._workspace_maxy > y > self._workspace_miny
and self._workspace_maxz > z > self._workspace_minz)
def _demo_record_step(self, demo_list, record, func):
if record:
demo_list.append(self.get_observation())
if func is not None:
func(self.get_observation())
def _set_camera_properties(self) -> None:
def _set_rgb_props(rgb_cam: VisionSensor, rgb: bool, depth: bool,
conf: CameraConfig):
if not (rgb or depth):
rgb_cam.remove()
else:
rgb_cam.set_explicit_handling(1)
rgb_cam.set_resolution(conf.image_size)
rgb_cam.set_render_mode(conf.render_mode)
def _set_mask_props(mask_cam: VisionSensor, mask: bool,
conf: CameraConfig):
if not mask:
mask_cam.remove()
else:
mask_cam.set_explicit_handling(1)
mask_cam.set_resolution(conf.image_size)
_set_rgb_props(self._cam_over_shoulder_left,
self._obs_config.left_shoulder_camera.rgb,
self._obs_config.left_shoulder_camera.depth,
self._obs_config.left_shoulder_camera)
_set_rgb_props(self._cam_over_shoulder_right,
self._obs_config.right_shoulder_camera.rgb,
self._obs_config.right_shoulder_camera.depth,
self._obs_config.right_shoulder_camera)
_set_rgb_props(self._cam_wrist, self._obs_config.wrist_camera.rgb,
self._obs_config.wrist_camera.depth,
self._obs_config.wrist_camera)
_set_rgb_props(self._cam_front, self._obs_config.front_camera.rgb,
self._obs_config.front_camera.depth,
self._obs_config.front_camera)
_set_mask_props(self._cam_over_shoulder_left_mask,
self._obs_config.left_shoulder_camera.mask,
self._obs_config.left_shoulder_camera)
_set_mask_props(self._cam_over_shoulder_right_mask,
self._obs_config.right_shoulder_camera.mask,
self._obs_config.right_shoulder_camera)
_set_mask_props(self._cam_wrist_mask,
self._obs_config.wrist_camera.mask,
self._obs_config.wrist_camera)
_set_mask_props(self._cam_front_mask,
self._obs_config.front_camera.mask,
self._obs_config.front_camera)
def _place_task(self) -> None:
self._workspace_boundary.clear()
# Find a place in the robot workspace for task
self._active_task.boundary_root().set_orientation(
self._initial_task_pose)
min_rot, max_rot = self._active_task.base_rotation_bounds()
self._workspace_boundary.sample(self._active_task.boundary_root(),
min_rotation=min_rot,
max_rotation=max_rot)
class TestObjects(TestCore):
def setUp(self):
super().setUp()
self.dynamic_cube = Shape('dynamic_cube')
self.cubes_under_dummy = Dummy('cubes_under_dummy')
self.cube0 = Shape('cube0')
self.dummy = Dummy('dummy')
self.simple_model = Shape('simple_model')
def test_get_object_type(self):
self.assertEqual(Object.get_object_type('dynamic_cube'),
ObjectType.SHAPE)
self.assertEqual(Object.get_object_type('dummy'), ObjectType.DUMMY)
def test_get_object_name(self):
self.assertEqual(Object.get_object_name('dynamic_cube'),
'dynamic_cube')
self.assertEqual(
Object.get_object_name(self.dynamic_cube.get_handle()),
'dynamic_cube')
def test_get_object(self):
self.assertEqual(Object.get_object('dynamic_cube'), self.dynamic_cube)
self.assertEqual(Object.get_object('dummy'), self.dummy)
self.assertEqual(Object.get_object(self.dynamic_cube.get_handle()),
self.dynamic_cube)
self.assertEqual(Object.get_object(self.dummy.get_handle()),
self.dummy)
def test_equality(self):
cube2 = Shape('dynamic_cube')
self.assertEqual(self.dynamic_cube, cube2)
def test_get_handle(self):
self.assertGreater(self.dynamic_cube.get_handle(), 0)
def test_get_type(self):
self.assertEqual(self.dynamic_cube.get_type(), ObjectType.SHAPE)
def test_still_exists(self):
self.assertTrue(self.dynamic_cube.still_exists())
self.dynamic_cube.remove()
self.assertFalse(self.dynamic_cube.still_exists())
def test_object_exists(self):
yes = Object.exists('dynamic_cube')
no = Object.exists('dynamic_cubeee')
self.assertTrue(yes)
self.assertFalse(no)
def test_get_set_name(self):
self.dynamic_cube.set_name('test1')
self.assertEqual(self.dynamic_cube.get_name(), 'test1')
def test_get_set_position(self):
position = self.cube0.get_position()
self.assertIsInstance(position, np.ndarray)
self.assertEqual(position.shape, (3, ))
self.cube0.set_position([0.1, 0.1, 0.1], self.cubes_under_dummy)
self.assertTrue(
np.allclose(self.cube0.get_position(self.cubes_under_dummy),
[0.1, 0.1, 0.1]))
self.cube0.set_position([0.2, 0.2, 0.2])
self.assertTrue(np.allclose(self.cube0.get_position(),
[0.2, 0.2, 0.2]))
def test_get_set_orientation(self):
orientation = self.cube0.get_orientation()
self.assertIsInstance(orientation, np.ndarray)
self.assertEqual(orientation.shape, (3, ))
self.cube0.set_orientation([0.0, 0.0, 0.2], self.cubes_under_dummy)
self.assertTrue(
np.allclose(self.cube0.get_orientation(self.cubes_under_dummy),
[0.0, 0.0, 0.2]))
self.cube0.set_orientation([0.0, 0.0, 0.3])
self.assertTrue(
np.allclose(self.cube0.get_orientation(), [0.0, 0.0, 0.3]))
def test_get_set_quaternion(self):
quaternion = self.cube0.get_quaternion()
self.assertIsInstance(quaternion, np.ndarray)
self.assertEqual(quaternion.shape, (4, ))
# x, y, z, w
self.cube0.set_quaternion([1., 0., 0., 0.], self.cubes_under_dummy)
self.assertTrue(
np.allclose(self.cube0.get_quaternion(self.cubes_under_dummy),
[1., 0., 0., 0.]))
self.cube0.set_quaternion([np.sqrt(0.5), 0, 0., np.sqrt(0.5)])
self.assertTrue(
np.allclose(
self.cube0.get_quaternion(),
[np.sqrt(0.5), 0, 0., np.sqrt(0.5)]))
def test_get_velocity(self):
linear_vel, angular_vel = self.cube0.get_velocity()
self.assertIsInstance(linear_vel, np.ndarray)
self.assertEqual(linear_vel.shape, (3, ))
self.assertIsInstance(angular_vel, np.ndarray)
self.assertEqual(angular_vel.shape, (3, ))
def test_get_set_parent(self):
self.dynamic_cube.set_parent(self.dummy)
parent = self.dynamic_cube.get_parent()
self.assertEqual(parent, self.dummy)
def test_get_set_parent_not_in_place(self):
init_pos = self.dynamic_cube.get_position()
self.dynamic_cube.set_parent(self.dummy, keep_in_place=False)
parent = self.dynamic_cube.get_parent()
self.assertEqual(parent, self.dummy)
self.assertFalse(
np.allclose(init_pos, self.dynamic_cube.get_position()))
def test_get_parent_when_orphan(self):
parent = self.dummy.get_parent()
self.assertIsNone(parent)
def test_get_matrix(self):
self.assertEqual(len(self.dynamic_cube.get_matrix()), 12)
def test_get_set_collidable(self):
self.dynamic_cube.set_collidable(False)
self.assertFalse(self.dynamic_cube.is_collidable())
self.dynamic_cube.set_collidable(True)
self.assertTrue(self.dynamic_cube.is_collidable())
def test_get_set_measurable(self):
self.dynamic_cube.set_measurable(False)
self.assertFalse(self.dynamic_cube.is_measurable())
self.dynamic_cube.set_measurable(True)
self.assertTrue(self.dynamic_cube.is_measurable())
def test_get_set_detectable(self):
self.dynamic_cube.set_detectable(False)
self.assertFalse(self.dynamic_cube.is_detectable())
self.dynamic_cube.set_detectable(True)
self.assertTrue(self.dynamic_cube.is_detectable())
def test_get_set_renderable(self):
self.dynamic_cube.set_renderable(False)
self.assertFalse(self.dynamic_cube.is_renderable())
self.dynamic_cube.set_renderable(True)
self.assertTrue(self.dynamic_cube.is_renderable())
def test_is_model(self):
self.assertFalse(self.dynamic_cube.is_model())
self.assertTrue(self.simple_model.is_model())
def test_set_model(self):
self.simple_model.set_model(False)
self.dynamic_cube.set_model(True)
self.assertFalse(self.simple_model.is_model())
self.assertTrue(self.dynamic_cube.is_model())
def test_remove(self):
self.dynamic_cube.remove()
self.simple_model.remove()
self.assertFalse(self.dynamic_cube.still_exists())
self.assertFalse(self.simple_model.still_exists())
self.assertFalse(Object.exists('dynamic_cube'))
self.assertFalse(Object.exists('simple_model'))
def test_dynamic_object(self):
# Can't really test this. So lets just make sure it doesn't error
self.dynamic_cube.reset_dynamic_object()
def test_get_bounding_box(self):
bb = self.dynamic_cube.get_bounding_box()
self.assertTrue(np.allclose(bb, [-0.05, 0.05] * 3))
def test_get_objects_in_tree(self):
dummys = [Dummy('nested_dummy%d' % i) for i in range(3)]
objects = dummys[0].get_objects_in_tree(exclude_base=False,
first_generation_only=False)
self.assertListEqual(objects, dummys)
for obj in objects:
self.assertIs(type(obj), Dummy)
self.assertListEqual(
dummys[0].get_objects_in_tree(exclude_base=True,
first_generation_only=False),
dummys[1:])
self.assertListEqual(
dummys[0].get_objects_in_tree(exclude_base=False,
first_generation_only=True),
dummys[:-1])
def test_get_extention_string(self):
self.assertEqual(self.dynamic_cube.get_extension_string(), 'test')
def test_get_configuration_tree(self):
config = self.dynamic_cube.get_configuration_tree()
self.assertIsNotNone(config)
def test_rotate(self):
self.dynamic_cube.rotate([0.02, 0.04, 0.06])
self.assertTrue(
np.allclose(self.dynamic_cube.get_orientation(),
[0.02, 0.04, 0.06]))
def test_get_set_model_collidable(self):
self.simple_model.set_model_collidable(False)
self.assertFalse(self.simple_model.is_model_collidable())
self.simple_model.set_model_collidable(True)
self.assertTrue(self.simple_model.is_model_collidable())
def test_get_set_model_measurable(self):
self.simple_model.set_model_measurable(False)
self.assertFalse(self.simple_model.is_model_measurable())
self.simple_model.set_model_measurable(True)
self.assertTrue(self.simple_model.is_model_measurable())
def test_get_set_model_detectable(self):
self.simple_model.set_model_detectable(False)
self.assertFalse(self.simple_model.is_model_detectable())
self.simple_model.set_model_detectable(True)
self.assertTrue(self.simple_model.is_model_detectable())
def test_get_set_model_renderable(self):
self.simple_model.set_model_renderable(False)
self.assertFalse(self.simple_model.is_model_renderable())
self.simple_model.set_model_renderable(True)
self.assertTrue(self.simple_model.is_model_renderable())
def test_get_set_model_dynamic(self):
self.simple_model.set_model_dynamic(False)
self.assertFalse(self.simple_model.is_model_dynamic())
self.simple_model.set_model_dynamic(True)
self.assertTrue(self.simple_model.is_model_dynamic())
def test_get_set_model_respondable(self):
self.simple_model.set_model_respondable(False)
self.assertFalse(self.simple_model.is_model_respondable())
self.simple_model.set_model_respondable(True)
self.assertTrue(self.simple_model.is_model_respondable())
def test_check_collision(self):
c1 = Shape('colliding_cube0')
c2 = Shape('colliding_cube1')
self.assertTrue(c1.check_collision(c2))
def test_check_collision_all(self):
c1 = Shape('colliding_cube0')
self.assertTrue(c1.check_collision(None))
def test_copy(self):
cube1 = self.cube0.copy()
self.assertGreater(cube1.get_handle(), 0)
self.assertIsInstance(cube1, Shape)
self.assertNotEqual(self.cube0, cube1)
def test_check_distance(self):
dist = self.dummy.check_distance(self.cube0)
self.assertAlmostEqual(dist, 1.4629, places=3)
def test_set_get_bullet_friction(self):
self.dynamic_cube.set_bullet_friction(0.7)
friction = self.dynamic_cube.get_bullet_friction()
self.assertAlmostEqual(friction, 0.7, places=1)
def test_set_get_explicit_handling(self):
cam = VisionSensor.create((640, 480))
flag_orig = cam.get_explicit_handling()
cam.set_explicit_handling(value=1)
flag = cam.get_explicit_handling()
self.assertEqual(flag, 1)
cam.set_explicit_handling(value=0)
flag = cam.get_explicit_handling()
self.assertEqual(flag, 0)
cam.set_explicit_handling(flag_orig)
cam.remove()
# Start Simulation
pr = PyRep()
pr.launch(SCENE_FILE, headless=False)
pr.start()
robot = Shape('start_pose')
vision_sensor = VisionSensor('vision_sensor')
# Generate a random map
randomMapGenerator.generate_random_map(no_of_objects, False)
# Setup occ_grid
occ_grid = OccupancyGrid()
setupOccGrid(occ_grid, vision_sensor)
# Compute block size from shape in simulation
bounding_box = robot.get_bounding_box()
block_size_x = int(
round(bounding_box[1] - bounding_box[0], 3) / occ_grid.resolution)
block_size_y = int(
round(bounding_box[3] - bounding_box[2], 3) / occ_grid.resolution)
submapper = GridSubmapper(occ_grid)
submapper.process(block_size_x, block_size_y)
planner = SubmapPlanner(occ_grid, block_size_x, block_size_y)
cur_pos = planner.getPath(submapper.submaps, only_start=True)
del submapper
del planner
ccd = CCRA(occ_grid, block_size_x, block_size_y)
class TestObjects(TestCore):
def setUp(self):
super().setUp()
self.dynamic_cube = Shape('dynamic_cube')
self.cubes_under_dummy = Dummy('cubes_under_dummy')
self.cube0 = Shape('cube0')
self.dummy = Dummy('dummy')
self.simple_model = Shape('simple_model')
def test_equality(self):
cube2 = Shape('dynamic_cube')
self.assertEqual(self.dynamic_cube, cube2)
def test_get_handle(self):
self.assertGreater(self.dynamic_cube.get_handle(), 0)
def test_still_exists(self):
self.assertTrue(self.dynamic_cube.still_exists())
self.assertFalse(Shape(-1).still_exists())
def test_object_exists(self):
yes = Object.exists('dynamic_cube')
no = Object.exists('dynamic_cubeee')
self.assertTrue(yes)
self.assertFalse(no)
def test_get_set_name(self):
self.dynamic_cube.set_name('test1')
self.assertEqual(self.dynamic_cube.get_name(), 'test1')
def test_get_set_position(self):
self.cube0.set_position([0.1, 0.1, 0.1], self.cubes_under_dummy)
self.assertTrue(
np.allclose(self.cube0.get_position(self.cubes_under_dummy),
[0.1, 0.1, 0.1]))
self.cube0.set_position([0.2, 0.2, 0.2])
self.assertTrue(np.allclose(self.cube0.get_position(),
[0.2, 0.2, 0.2]))
def test_get_set_orientation(self):
self.cube0.set_orientation([0.0, 0.0, 0.2], self.cubes_under_dummy)
self.assertTrue(
np.allclose(self.cube0.get_orientation(self.cubes_under_dummy),
[0.0, 0.0, 0.2]))
self.cube0.set_orientation([0.0, 0.0, 0.3])
self.assertTrue(
np.allclose(self.cube0.get_orientation(), [0.0, 0.0, 0.3]))
def test_get_set_quaternion(self):
# x, y, z, w
self.cube0.set_quaternion([1., 0., 0., 0.], self.cubes_under_dummy)
self.assertTrue(
np.allclose(self.cube0.get_quaternion(self.cubes_under_dummy),
[1., 0., 0., 0.]))
self.cube0.set_quaternion([np.sqrt(0.5), 0, 0., np.sqrt(0.5)])
self.assertTrue(
np.allclose(
self.cube0.get_quaternion(),
[np.sqrt(0.5), 0, 0., np.sqrt(0.5)]))
def test_get_set_parent(self):
self.dynamic_cube.set_parent(self.dummy)
parent = self.dynamic_cube.get_parent()
self.assertEqual(parent, self.dummy)
def test_get_set_parent_not_in_place(self):
init_pos = self.dynamic_cube.get_position()
self.dynamic_cube.set_parent(self.dummy, keep_in_place=False)
parent = self.dynamic_cube.get_parent()
self.assertEqual(parent, self.dummy)
self.assertFalse(
np.allclose(init_pos, self.dynamic_cube.get_position()))
def test_get_parent_when_orphan(self):
parent = self.dummy.get_parent()
self.assertIsNone(parent)
def test_get_matrix(self):
self.assertEqual(len(self.dynamic_cube.get_matrix()), 12)
def test_get_set_collidable(self):
self.dynamic_cube.set_collidable(False)
self.assertFalse(self.dynamic_cube.is_collidable())
self.dynamic_cube.set_collidable(True)
self.assertTrue(self.dynamic_cube.is_collidable())
def test_get_set_measurable(self):
self.dynamic_cube.set_measurable(False)
self.assertFalse(self.dynamic_cube.is_measurable())
self.dynamic_cube.set_measurable(True)
self.assertTrue(self.dynamic_cube.is_measurable())
def test_get_set_detectable(self):
self.dynamic_cube.set_detectable(False)
self.assertFalse(self.dynamic_cube.is_detectable())
self.dynamic_cube.set_detectable(True)
self.assertTrue(self.dynamic_cube.is_detectable())
def test_get_set_renderable(self):
self.dynamic_cube.set_renderable(False)
self.assertFalse(self.dynamic_cube.is_renderable())
self.dynamic_cube.set_renderable(True)
self.assertTrue(self.dynamic_cube.is_renderable())
def test_is_model(self):
self.assertFalse(self.dynamic_cube.is_model())
self.assertTrue(self.simple_model.is_model())
def test_set_model(self):
self.simple_model.set_model(False)
self.dynamic_cube.set_model(True)
self.assertFalse(self.simple_model.is_model())
self.assertTrue(self.dynamic_cube.is_model())
def test_remove(self):
self.dynamic_cube.remove()
self.simple_model.remove()
self.assertFalse(self.dynamic_cube.still_exists())
self.assertFalse(self.simple_model.still_exists())
self.assertFalse(Object.exists('dynamic_cube'))
self.assertFalse(Object.exists('simple_model'))
def test_dynamic_object(self):
# Can't really test this. So lets just make sure it doesn't error
self.dynamic_cube.reset_dynamic_object()
def test_get_bounding_box(self):
bb = self.dynamic_cube.get_bounding_box()
self.assertTrue(np.allclose(bb, [-0.05, 0.05] * 3))
def test_get_objects_in_tree(self):
dummys = [Dummy('nested_dummy%d' % i) for i in range(3)]
self.assertListEqual(
dummys[0].get_objects_in_tree(exclude_base=False,
first_generation_only=False), dummys)
self.assertListEqual(
dummys[0].get_objects_in_tree(exclude_base=True,
first_generation_only=False),
dummys[1:])
self.assertListEqual(
dummys[0].get_objects_in_tree(exclude_base=False,
first_generation_only=True),
dummys[:-1])
def test_get_extention_string(self):
self.assertEqual(self.dynamic_cube.get_extension_string(), 'test')
def test_get_configuration_tree(self):
config = self.dynamic_cube.get_configuration_tree()
self.assertIsNotNone(config)
def test_rotate(self):
self.dynamic_cube.rotate([0.02, 0.04, 0.06])
self.assertTrue(
np.allclose(self.dynamic_cube.get_orientation(),
[0.02, 0.04, 0.06]))
def test_get_set_model_collidable(self):
self.simple_model.set_model_collidable(False)
self.assertFalse(self.simple_model.is_model_collidable())
self.simple_model.set_model_collidable(True)
self.assertTrue(self.simple_model.is_model_collidable())
def test_get_set_model_measurable(self):
self.simple_model.set_model_measurable(False)
self.assertFalse(self.simple_model.is_model_measurable())
self.simple_model.set_model_measurable(True)
self.assertTrue(self.simple_model.is_model_measurable())
def test_get_set_model_detectable(self):
self.simple_model.set_model_detectable(False)
self.assertFalse(self.simple_model.is_model_detectable())
self.simple_model.set_model_detectable(True)
self.assertTrue(self.simple_model.is_model_detectable())
def test_get_set_model_renderable(self):
self.simple_model.set_model_renderable(False)
self.assertFalse(self.simple_model.is_model_renderable())
self.simple_model.set_model_renderable(True)
self.assertTrue(self.simple_model.is_model_renderable())
def test_get_set_model_dynamic(self):
self.simple_model.set_model_dynamic(False)
self.assertFalse(self.simple_model.is_model_dynamic())
self.simple_model.set_model_dynamic(True)
self.assertTrue(self.simple_model.is_model_dynamic())
def test_get_set_model_respondable(self):
self.simple_model.set_model_respondable(False)
self.assertFalse(self.simple_model.is_model_respondable())
self.simple_model.set_model_respondable(True)
self.assertTrue(self.simple_model.is_model_respondable())
def test_check_collision(self):
c1 = Shape('colliding_cube0')
c2 = Shape('colliding_cube1')
self.assertTrue(c1.check_collision(c2))
def test_check_collision_all(self):
c1 = Shape('colliding_cube0')
self.assertTrue(c1.check_collision(None))
def test_copy(self):
cube1 = self.cube0.copy()
self.assertGreater(cube1.get_handle(), 0)
self.assertIsInstance(cube1, Shape)
self.assertNotEqual(self.cube0, cube1)
def test_check_distance(self):
dist = self.dummy.check_distance(self.cube0)
self.assertAlmostEqual(dist, 1.4629, places=3)