def test_check_collision(self):
c1 = Shape('colliding_cube0')
c2 = Shape('colliding_cube1')
self.assertTrue(c1.check_collision(c2))
franka = Franka()
# set franka to home joints
franka.home(env)
# initiate the camera and get an color image and depth image
cam = Camera()
img = cam.capture_bgr()
depth_img = cam.capture_depth(in_meters=True)
# TODO: detect the location of the chessboard and the chess pieces from the image
O_chesses_pos, X_chesses_pos, chessboard_pos = cv_get_position(
img, depth_img)
# chessboard and chesses
chesses_pos = {'X': X_chesses_pos, 'O': O_chesses_pos}
chesses = [Shape('chess_X'+str(chess_num)) for chess_num in range(1,6)]+\
[Shape('chess_O'+str(chess_num)) for chess_num in range(1,6)]
# create chessboard instance
board = Board()
# Decide who first (X always first)
system('clear')
instream = input('Do you want to play first hand?[y/n]')
if instream != 'n':
ai = tictactoe_ai('O')
player_chess = 'X'
Ai_chess = 'O'
turn = 'player'
else:
ai = tictactoe_ai('X')
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_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.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):
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()
class PioneerEnv(object):
def __init__(self,
escene_name='proximity_sensor.ttt',
start=[100, 100],
goal=[180, 500],
rand_area=[100, 450],
path_resolution=5.0,
margin=0.,
margin_to_goal=0.5,
action_max=[.5, 1.],
action_min=[0., 0.],
_load_path=True,
path_name="PathNodes",
type_of_planning="PID",
type_replay_buffer="random",
max_laser_range=1.0,
headless=False):
SCENE_FILE = join(dirname(abspath(__file__)), escene_name)
self.pr = PyRep()
self.pr.launch(SCENE_FILE, headless=headless)
self.pr.start()
self.agent = Pioneer("Pioneer_p3dx",
type_of_planning=type_of_planning,
type_replay_buffer=type_replay_buffer)
self.agent.set_control_loop_enabled(False)
self.initial_joint_positions = self.agent.get_joint_positions()
self.initial_position = self.agent.get_position()
print(f"Agent initial position: {self.initial_position}")
self.vision_map = VisionSensor("VisionMap")
self.vision_map.handle_explicitly()
self.vision_map_handle = self.vision_map.get_handle()
self.floor = Shape("Floor_respondable")
self.perspective_angle = self.vision_map.get_perspective_angle # degrees
self.resolution = self.vision_map.get_resolution # list [resx, resy]
self.margin = margin
self.margin_to_goal = margin_to_goal
self.rand_area = rand_area
self.start = start
self.goal = goal
self.type_of_planning = type_of_planning
self.max_laser_range = max_laser_range
self.max_angle_orientation = np.pi
scene_image = self.vision_map.capture_rgb() * 255
scene_image = np.flipud(scene_image)
self.rew_weights = [1, 10000, 5000]
self.start_position = Dummy("Start").get_position()
self.goal_position = Dummy("Goal").get_position() # [x, y, z]
self.local_goal_aux = Dummy("Local_goal_aux")
self.local_goal_aux_pos_list = [[-3.125, 2.175, 0], [2.9, 3.575, 0],
self.goal_position, self.goal_position]
self.ind_local_goal_aux = 0
self.max_distance = get_distance([-7.5, -4.1, 0.], self.goal_position)
self.distance_to_goal_m1 = get_distance(self.start_position,
self.goal_position)
self.c_lin_vel = self.c_ang_vel = self.b_lin_vel = self.b_ang_vel = 0.
self.action_max = action_max
self.action_min = action_min
if type_of_planning == 'PID':
self.planning_info_logger = get_logger("./loggers",
"Planning_Info.log")
self.image_path = None
if exists("./paths/" + path_name + ".npy") and _load_path:
print("Load Path..")
self.image_path = np.load("./paths/" + path_name + ".npy",
allow_pickle=True)
else:
print("Planning...")
self.image_path = self.Planning(
scene_image,
self.start,
self.goal,
self.rand_area,
path_resolution=path_resolution,
logger=self.planning_info_logger)
assert self.image_path is not None, "path should not be a Nonetype"
self.real_path = self.path_image2real(self.image_path,
self.start_position)
# project in coppelia sim
sim_drawing_points = 0
point_size = 10 #[pix]
duplicate_tolerance = 0
parent_obj_handle = self.floor.get_handle()
max_iter_count = 999999
ambient_diffuse = (255, 0, 0)
blue_ambient_diffuse = (0, 0, 255)
point_container = sim.simAddDrawingObject(
sim_drawing_points,
point_size,
duplicate_tolerance,
parent_obj_handle,
max_iter_count,
ambient_diffuse=ambient_diffuse)
local_point_container = sim.simAddDrawingObject(
sim_drawing_points,
point_size,
duplicate_tolerance,
parent_obj_handle,
max_iter_count,
ambient_diffuse=blue_ambient_diffuse)
# debug
for point in self.real_path:
point_data = (point[0], point[1], 0)
sim.simAddDrawingObjectItem(point_container, point_data)
# You need to get the real coord in the real world
assert local_point_container is not None, "point container shouldn't be empty"
self.agent.load_point_container(local_point_container)
self.agent.load_path(self.real_path)
def reset(self):
self.pr.stop()
if self.type_of_planning == 'PID':
self.agent.local_goal_reset()
self.pr.start()
self.pr.step()
sensor_state, distance_to_goal, orientation_to_goal = self._get_state()
self.distance_to_goal_m1 = distance_to_goal
self.orientation_to_goal_m1 = orientation_to_goal
sensor_state_n, distance_to_goal, orientation_to_goal = self.normalize_states(
sensor_state, distance_to_goal, orientation_to_goal)
# pos_x, pos_y = self.agent.get_position()[:-1]
return np.hstack((sensor_state_n, distance_to_goal,
orientation_to_goal)), sensor_state
def step(self, action, pf_f=0):
self.agent.set_joint_target_velocities(action)
self.pr.step() # Step the physics simulation
scene_image = self.vision_map.capture_rgb() * 255 # numpy -> [w, h, 3]
sensor_state, distance_to_goal, orientation_to_goal = self._get_state()
self.b_lin_vel, self.b_ang_vel = self.c_lin_vel, self.c_ang_vel
self.c_lin_vel, self.c_ang_vel = self.agent.get_velocity(
) # 3d coordinates
self.c_lin_vel = np.linalg.norm(self.c_lin_vel)
self.c_ang_vel = self.c_ang_vel[-1] # w/r z
reward, done = self._get_reward(sensor_state, distance_to_goal,
orientation_to_goal, pf_f)
sensor_state_n, distance_to_goal, orientation_to_goal = self.normalize_states(
sensor_state, distance_to_goal, orientation_to_goal)
# pos_x, pos_y = self.agent.get_position()[:-1]
state = np.hstack(
(sensor_state_n, distance_to_goal, orientation_to_goal))
return state, reward, scene_image, done, sensor_state
def _get_state(self):
sensor_state = np.array([
proxy_sensor.read()
for proxy_sensor in self.agent.proximity_sensors
]) # list of distances. -1 if not detect anything
goal_transformed = world_to_robot_frame(
self.agent.get_position(), self.goal_position,
self.agent.get_orientation()[-1])
# distance_to_goal = get_distance(goal_transformed[:-1], np.array([0,0])) # robot frame
distance_to_goal = get_distance(self.agent.get_position()[:-1],
self.goal_position[:-1])
orientation_to_goal = np.arctan2(goal_transformed[1],
goal_transformed[0])
return sensor_state, distance_to_goal, orientation_to_goal
def _get_reward(self,
sensor_state,
distance_to_goal,
orientation_to_goal,
pf_f=0):
done = False
r_local_goal = self.update_local_goal_aux()
r_target = -(distance_to_goal - self.distance_to_goal_m1)
r_vel = self.c_lin_vel / self.action_max[0]
# r_orientation = - (orientation_to_goal - self.orientation_to_goal_m1)
reward = self.rew_weights[0] * (r_local_goal + r_vel) # - pf_f/20)
# distance_to_goal = get_distance(agent_position[:-1], goal[:-1])
self.distance_to_goal_m1 = distance_to_goal
self.orientation_to_goal_m1 = orientation_to_goal
# collision check
if self.collision_check(sensor_state, self.margin):
reward = -1 * self.rew_weights[1]
done = True
# goal achievement
if distance_to_goal < self.margin_to_goal:
reward = self.rew_weights[2]
done = True
return reward, done
def shutdown(self):
self.pr.stop()
self.pr.shutdown()
def model_update(self, method="DDPG", pretraining_loop=False):
if method == "DDPG": self.agent.trainer.update()
elif method == "IL":
loss = self.agent.trainer.IL_update()
return loss
elif method == "CoL":
loss = self.agent.trainer.CoL_update(pretraining_loop)
else:
raise NotImplementedError()
def normalize_states(self, sensor_state, distance_to_goal,
orientation_to_goal):
sensor_state = self.normalize_laser(sensor_state, self.norm_func)
distance_to_goal = self.norm_func(distance_to_goal, self.max_distance)
orientation_to_goal = self.norm_func(orientation_to_goal,
self.max_angle_orientation)
return sensor_state, distance_to_goal, orientation_to_goal
def normalize_laser(self, obs, norm_func):
return np.array([
norm_func(o, self.max_laser_range) if o >= 0 else -1 for o in obs
])
def norm_func(self, x, max_value):
return 2 * (1 - min(x, max_value) / max_value) - 1
def set_margin(self, margin):
self.margin = margin
# def set_start_goal_position(self, start_pos, goal_pos):
def update_local_goal_aux(self):
pos = self.agent.get_position()[:-1]
distance = get_distance(pos, self.local_goal_aux.get_position()[:-1])
if distance < 0.5:
self.local_goal_aux.set_position(
self.local_goal_aux_pos_list[self.ind_local_goal_aux])
self.ind_local_goal_aux += 1
return -1 * distance**2
@staticmethod
def collision_check(observations, margin):
if observations.sum() == -1 * observations.shape[0]:
return False
elif observations[observations >= 0].min() < margin:
return True
else:
return False
@staticmethod
def Planning(Map,
start,
goal,
rand_area,
path_resolution=5.0,
logger=None):
"""
:parameter Map(ndarray): Image that planning over with
"""
Map = Image.fromarray(Map.astype(np.uint8)).convert('L')
path, n_paths = main(Map,
start,
goal,
rand_area,
path_resolution=path_resolution,
logger=logger,
show_animation=False)
if path is not None:
np.save("./paths/PathNodes_" + str(n_paths) + ".npy", path)
return path
else:
logger.info("Not found Path")
return None
@staticmethod
def path_image2real(image_path, start):
"""
image_path: np.array[pix] points of the image path
start_array: np.array
"""
scale = 13.0 / 512 # [m/pix]
x_init = start[0]
y_init = start[1]
deltas = [(image_path[i + 1][0] - image_path[i][0],
image_path[i + 1][1] - image_path[i][1])
for i in range(image_path.shape[0] - 1)]
path = np.zeros((image_path.shape[0], 3))
path[0, :] = np.array([x_init, y_init, 0])
for i in range(1, image_path.shape[0]):
path[i, :] = np.array([
path[i - 1, 0] + deltas[i - 1][0] * scale,
path[i - 1, 1] - deltas[i - 1][1] * scale, 0
])
rot_mat = np.diagflat([-1, -1, 1])
tras_mat = np.zeros_like(path)
tras_mat[:, 0] = np.ones_like(path.shape[0]) * 0.3
tras_mat[:, 1] = np.ones_like(path.shape[0]) * 4.65
path = path @ rot_mat + tras_mat
return np.flip(path, axis=0)
def create_primitive_object(self,
shape_type,
position,
mass,
orientation=(0., 0., 0., 1.),
radius=0.5,
half_extents=(.5, .5, .5),
height=1.,
filename=None,
mesh_scale=(1., 1., 1.),
plane_normal=(0., 0., 1.),
rgba_color=None,
specular_color=None,
frame_position=None,
frame_orientation=None,
vertices=None,
indices=None,
uvs=None,
normals=None,
flags=-1):
"""Create a primitive object in the simulator. This is basically the combination of `create_visual_shape`,
`create_collision_shape`, and `create_body`.
Args:
shape_type (int): type of shape; GEOM_SPHERE (=2), GEOM_BOX (=3), GEOM_CAPSULE (=7), GEOM_CYLINDER (=4),
GEOM_PLANE (=6), GEOM_MESH (=5)
position (np.array[float[3]]): Cartesian world position of the base
mass (float): mass of the base, in kg (if using SI units)
orientation (np.array[float[4]]): Orientation of base as quaternion [x,y,z,w]
radius (float): only for GEOM_SPHERE, GEOM_CAPSULE, GEOM_CYLINDER
half_extents (np.array[float[3]], list/tuple of 3 floats): only for GEOM_BOX.
height (float): only for GEOM_CAPSULE, GEOM_CYLINDER (height = length).
filename (str): Filename for GEOM_MESH, currently only Wavefront .obj. Will create convex hulls for each
object (marked as 'o') in the .obj file.
mesh_scale (np.array[float[3]], list/tuple of 3 floats): scale of mesh (only for GEOM_MESH).
plane_normal (np.array[float[3]], list/tuple of 3 floats): plane normal (only for GEOM_PLANE).
rgba_color (list/tuple of 4 floats): color components for red, green, blue and alpha, each in range [0..1].
specular_color (list/tuple of 3 floats): specular reflection color, red, green, blue components in range
[0..1]
frame_position (np.array[float[3]]): translational offset of the visual and collision shape with respect
to the link frame.
frame_orientation (np.array[float[4]]): rotational offset (quaternion x,y,z,w) of the visual and collision
shape with respect to the link frame.
vertices (list[np.array[float[3]]]): Instead of creating a mesh from obj file, you can provide vertices,
indices, uvs and normals
indices (list[int]): triangle indices, should be a multiple of 3.
uvs (list of np.array[2]): uv texture coordinates for vertices. Use `changeVisualShape` to choose the
texture image. The number of uvs should be equal to number of vertices.
normals (list[np.array[float[3]]]): vertex normals, number should be equal to number of vertices.
flags (int): unused / to be decided
Returns:
int: non-negative unique id for primitive object, or -1 for failure
"""
# check given parameters
color = rgba_color[:3] if rgba_color is not None else None
orientation = get_rpy_from_quaternion(
orientation).tolist() if orientation is not None else None
static = True if mass == 0 else False
mass = mass if mass <= 0 else mass
position = list(position)
# shape
if shape_type == self.GEOM_BOX:
size = [
2 * half_extents[0], 2 * half_extents[1], 2 * half_extents[2]
]
shape = Shape.create(PrimitiveShape.CUBOID,
size=size,
mass=mass,
color=color,
position=position,
orientation=orientation,
static=static)
elif shape_type == self.GEOM_SPHERE:
shape = Shape.create(PrimitiveShape.SPHERE,
size=[1.],
mass=mass,
color=[1, 0, 0],
position=position,
orientation=orientation,
static=static)
elif shape_type == self.GEOM_CYLINDER:
shape = Shape.create(PrimitiveShape.CYLINDER,
size=[radius, height],
mass=mass,
color=color,
position=position,
orientation=orientation,
static=static)
elif shape_type == self.GEOM_CONE:
shape = Shape.create(PrimitiveShape.CONE,
size=[radius, height],
mass=color,
position=position,
orientation=orientation,
static=static)
# elif shape_type == self.GEOM_MESH:
# # TODO: use trimesh library
# shape = Shape.create_mesh()
else:
raise NotImplementedError(
"Primitive object not defined for the given shape type.")
# save shape and return unique id
self._body_cnt += 1
self._bodies[self._body_cnt] = shape
return self._body_cnt
class LightBulbOut(Task):
def init_task(self) -> None:
self.bulb_visual = Shape('light_bulb')
self.bulb_glass_visual = Shape('bulb')
self.holders = [Shape('bulb_holder%d' % i) for i in range(2)]
self.bulb = Shape('bulb_phys')
self.conditions = [NothingGrasped(self.robot.gripper)]
self.boundary = Shape('spawn_boundary')
self.register_graspable_objects([self.bulb])
def init_episode(self, index: int) -> List[str]:
self._variation_index = index
b = SpawnBoundary([self.boundary])
for holder in self.holders:
b.sample(holder, min_distance=0.01)
ProximitySensor('success').set_position(
[0, 0, 0],
relative_to=self.holders[index % 2],
reset_dynamics=False)
w1 = Dummy('waypoint1')
w1.set_orientation([-np.pi, 0, -np.pi], reset_dynamics=False)
w4 = Dummy('waypoint4')
w4.set_orientation([-np.pi, 0, +3.735 * 10**(-1)],
reset_dynamics=False)
target_color_name, target_color_rgb = colors[index]
color_choice = np.random.choice(list(range(index)) +
list(range(index + 1, len(colors))),
size=1,
replace=False)[0]
_, distractor_color_rgb = colors[color_choice]
self.holders[index % 2].set_color(target_color_rgb)
other_index = {0: 1, 1: 0}
self.holders[other_index[index % 2]].set_color(distractor_color_rgb)
self.register_success_conditions([
DetectedCondition(self.bulb, ProximitySensor('success')),
NothingGrasped(self.robot.gripper)
])
return [
'put the bulb in the %s holder' % target_color_name,
'screw the bulb out and leave it in the %s stand' %
target_color_name,
'remove the bulb from the lamp and put it in the %s stand' %
target_color_name,
'take the light bulb out of the lamp and place it in the %s '
'holder' % target_color_name,
'grasp the light bulb, twist it anti clockwise until it is no '
'longer attached to the lamp, and drop it into the %s stand' %
target_color_name
]
def variation_count(self) -> int:
return len(colors)
def step(self) -> None:
if DetectedCondition(self.bulb, ProximitySensor('lamp_detector'),
negated=True).condition_met() \
== (True, True):
self.bulb_glass_visual.set_color([1.0, 1.0, 1.0])
def cleanup(self) -> None:
if self.bulb_glass_visual:
self.bulb_glass_visual.set_color([1.0, 1.0, 0.0])
def test_get_shape(self):
cube = Shape('dynamic_cube')
self.assertIsInstance(cube, Shape)
def test_decimate_mesh(self):
visual = Shape('cracker_box_visual')
_, old_indices, _ = visual.get_mesh_data()
new_mesh = visual.decimate_mesh(0.2)
_, new_indices, _ = new_mesh.get_mesh_data()
self.assertLess(len(new_indices), len(old_indices) * 0.3)
def setUp(self):
super().setUp()
self.dynamic_cube = Shape('dynamic_cube')
class DroneBase(RobotComponent):
"""Base class for representing drone movement."""
def __init__(self, count: int, num_propellers: int,
distance_from_target: float, name: str):
"""Count is used for when we have multiple copies of mobile bases.
:param count: used for multiple copies of robots
:param num_propellers: number of actuated propellers
:param name: string with robot name (same as base in vrep model).
"""
joint_names = [
'%s_propeller_joint%s' % (name, str(i + 1))
for i in range(num_propellers)
]
super().__init__(count, name, joint_names)
self.dist1 = distance_from_target
self.num_propellers = num_propellers
self.static_propeller_velocity = 5.335
self.pParam = 2
self.iParam = 0
self.dParam = 0
self.vParam = -2
self.cumul = 0
self.lastE = 0
self.pAlphaE = 0
self.pBetaE = 0
self.psp2 = 0
self.psp1 = 0
self.prevEuler = 0
suffix = '' if count == 0 else '#%d' % (count - 1)
propeller_names = [
'%s_propeller_respondable%s%s' % (name, str(i + 1), suffix)
for i in range(self.num_propellers)
]
self.propellers = self.get_propeller_handles(
propeller_names) # use handles instead of shapes here
# Motion planning handles
self.intermediate_target_base = Dummy('%s_intermediate_target_base%s' %
(name, suffix))
self.target_base = Shape('%s_target_base%s' % (name, suffix))
self._object_base = vrep.simGetObjectHandle(
'%s_base%s' % (name, suffix)
) # object instead of collection of objects for quadracopter
# Robot parameters and handle
self.z_pos = self.get_position()[2]
self.target_z = self.target_base.get_position()[-1]
def get_propeller_handles(self, propeller_names: List[str]):
""" Gets handles of propellers according to their names.
:param: A List containing the names of propellers (str).
:return: A List containing the handles of propellers.
"""
propellers = [] # get handles of propellers
for i in range(self.num_propellers):
propellers.append(vrep.simGetScriptHandle(propeller_names[i]))
return propellers
def set_cartesian_position(self, position: List[float]):
self.set_position(position)
def get_cartesian_position(self):
return self.get_position()
def set_propeller_velocities(self, velocities: List[float]):
""" Sets the particle velocities of propellers.
:param velocities: A List containing the particle velocities for propellers.
"""
for j in range(self.num_propellers):
vrep.simSetScriptSimulationParameter(self.propellers[j],
'particleVelocity',
velocities[j])
def get_2d_pose(self) -> List[float]:
"""Gets the 2D (top-down) pose of the robot [x, y, yaw].
:return: A List containing the x, y, yaw (in radians).
"""
return (self.get_position()[:2] + self.get_orientation()[-1:])
def set_2d_pose(self, pose: List[float]) -> None:
"""Sets the 2D (top-down) pose of the robot [x, y, yaw]
:param pose: A List containing the x, y, yaw (in radians).
"""
x, y, yaw = pose
self.set_position([x, y, self.z_pos])
self.set_orientation([0, 0, yaw])
def get_3d_pose(self) -> List[float]: #
"""Gets the 3D pose of the robot [x, y, z, yaw].
:return: A List containing the x, y, z, yaw (in radians).
"""
return (self.get_position()[:3] + self.get_orientation()[-1:])
def set_3d_pose(self, pose: List[float]) -> None:
"""Sets the 3D pose of the robot [x, y, z, yaw]
:param pose: A List containing the x, y, z, yaw (in radians).
"""
x, y, z, yaw = pose
self.set_position([x, y, z])
self.set_orientation([0, 0, yaw])
def assess_collision(self):
"""Silent detection of the robot base with all other entities present in the scene.
:return: True if collision is detected
"""
return vrep.simCheckCollision(self._object_base,
vrep.sim_handle_all) == 1
def get_linear_path(self,
position: List[float],
angle=0) -> HolonomicConfigurationPath:
"""Initialize linear path and check for collision along it.
Must specify either rotation in euler or quaternions, but not both!
:param position: The x, y position of the target.
:param angle: The z orientation of the target (in radians).
:raises: ConfigurationPathError if no path could be created.
:return: A linear path in the 2d space.
"""
position_base = self.get_position()
angle_base = self.get_orientation()[-1]
self.target_base.set_position(
[position[0], position[1], self.target_z])
self.target_base.set_orientation([0, 0, angle])
handle_base = self.get_handle()
handle_target_base = self.target_base.get_handle()
_, ret_floats, _, _ = utils.script_call(
'getBoxAdjustedMatrixAndFacingAngle@PyRep',
PYREP_SCRIPT_TYPE,
ints=[handle_base, handle_target_base])
m = ret_floats[:-1]
angle = ret_floats[-1]
self.intermediate_target_base.set_position([
m[3] - m[0] * self.dist1, m[7] - m[4] * self.dist1, self.target_z
])
self.intermediate_target_base.set_orientation([0, 0, angle])
self.target_base.set_orientation([0, 0, angle])
path = [[position_base[0], position_base[1], angle_base],
[position[0], position[1], angle]]
if self._check_collision_linear_path(path):
raise ConfigurationPathError(
'Could not create path. '
'An object was detected on the linear path.')
return HolonomicConfigurationPath(self, path)
def get_nonlinear_path(
self,
position: List[float],
angle=0,
boundaries=2,
path_pts=600,
ignore_collisions=False,
algorithm=Algos.RRTConnect) -> HolonomicConfigurationPath:
"""Gets a non-linear (planned) configuration path given a target pose.
:param position: The x, y, z position of the target.
:param angle: The z orientation of the target (in radians).
:param boundaries: A float defining the path search in x and y direction
[[-boundaries,boundaries],[-boundaries,boundaries]].
:param path_pts: The number of sampled points returned from the
computed path
:param ignore_collisions: If collision checking should be disabled.
:param algorithm: Algorithm used to compute path
:raises: ConfigurationPathError if no path could be created.
:return: A non-linear path (x,y,angle) in the xy configuration space.
"""
path = self._get_nonlinear_path_points(position, angle, boundaries,
path_pts, ignore_collisions,
algorithm)
return HolonomicConfigurationPath(self, path)
def _get_nonlinear_path_points(
self,
position: List[float],
angle=0,
boundaries=2,
path_pts=600,
ignore_collisions=False,
algorithm=Algos.RRTConnect) -> List[List[float]]:
"""Gets a non-linear (planned) configuration path given a target pose.
:param position: The x, y, z position of the target.
:param angle: The z orientation of the target (in radians).
:param boundaries: A float defining the path search in x and y direction
[[-boundaries,boundaries],[-boundaries,boundaries]].
:param path_pts: number of sampled points returned from the computed path
:param ignore_collisions: If collision checking should be disabled.
:param algorithm: Algorithm used to compute path
:raises: ConfigurationPathError if no path could be created.
:return: A non-linear path (x,y,angle) in the xy configuration space.
"""
# Base dummy required to be parent of the robot tree
# self.base_ref.set_parent(None)
# self.set_parent(self.base_ref)
# Missing the dist1 for intermediate target
self.target_base.set_position(
[position[0], position[1], self.target_z])
self.target_base.set_orientation([0, 0, angle])
handle_base = self.get_handle()
handle_target_base = self.target_base.get_handle()
# Despite verbosity being set to 0, OMPL spits out a lot of text
with utils.suppress_std_out_and_err():
_, ret_floats, _, _ = utils.script_call(
'getNonlinearPathMobile@PyRep',
PYREP_SCRIPT_TYPE,
ints=[
handle_base, handle_target_base, self._object_base,
int(ignore_collisions), path_pts
],
floats=[boundaries],
strings=[algorithm.value])
# self.set_parent(None)
# self.base_ref.set_parent(self)
if len(ret_floats) == 0:
raise ConfigurationPathError('Could not create path.')
path = []
for i in range(0, len(ret_floats) // 3):
inst = ret_floats[3 * i:3 * i + 3]
if i > 0:
dist_change = sqrt((inst[0] - prev_inst[0])**2 +
(inst[1] - prev_inst[1])**2)
else:
dist_change = 0
inst.append(dist_change)
path.append(inst)
prev_inst = inst
return path
def _check_collision_linear_path(self, path):
"""Check for collision on a linear path from start to goal
:param path: A list containing start and goal as [x,y,yaw]
:return: A bool, True if collision was detected
"""
start = path[0]
end = path[1]
m = (end[1] - start[1]) / (end[0] - start[0])
b = start[1] - m * start[0]
x_range = [start[0], end[0]]
x_span = start[0] - end[0]
incr = round(abs(x_span) / 50, 3)
if x_range[1] < x_range[0]:
incr = -incr
x = x_range[0]
for k in range(50):
x += incr
y = m * x + b
self.set_2d_pose([x, y, start[-1] if k < 46 else end[-1]])
status_collision = self.assess_collision()
if status_collision == True:
break
return status_collision
def get_base_actuation(self):
"""PIDV controller.
:return: A list with actuations including thrust and three other values,
and a bool representing target is reached.
"""
relative_targetPos = self.target_base.get_position(relative_to=self)
relative_targetOri = self.get_orientation(relative_to=self)
if (sqrt((relative_targetPos[0])**2 +
(relative_targetPos[1])**2) - self.dist1
) < 0.1 and relative_targetOri[-1] < 0.1 * np.pi / 180:
return [0, 0, 0, 0], True
# Vertical control:
targetPos = self.intermediate_target_base.get_position()
pos = self.get_position()
l, _ = vrep.simGetVelocity(self.get_handle())
e = (targetPos[2] - pos[2])
self.cumul = self.cumul + e
pv = self.pParam * e
thrust = self.static_propeller_velocity + pv + self.iParam * self.cumul + self.dParam * (
e - self.lastE) + l[2] * self.vParam
self.lastE = e
# Horizontal control:
sp = self.intermediate_target_base.get_position(relative_to=self)
m = vrep.simGetObjectMatrix(self.get_handle(), -1)
vx = [1, 0, 0, 1]
vx = np.matmul(np.array(m + [0, 0, 0, 1]).reshape(4, 4), vx)[:3]
vy = [0, 1, 0, 1]
vy = np.matmul(np.array(m + [0, 0, 0, 1]).reshape(4, 4), vy)[:3]
alphaE = (vy[2] - m[11])
alphaCorr = 0.25 * alphaE + 2.1 * (alphaE - self.pAlphaE)
betaE = (vx[2] - m[11])
betaCorr = -0.25 * betaE - 2.1 * (betaE - self.pBetaE)
self.pAlphaE = alphaE
self.pBetaE = betaE
alphaCorr = alphaCorr + sp[1] * 0.005 + 1 * (sp[1] - self.psp2)
betaCorr = betaCorr - sp[0] * 0.005 - 1 * (sp[0] - self.psp1)
self.psp2 = sp[1]
self.psp1 = sp[0]
# Rotational control:
euler = self.get_orientation(relative_to=self.intermediate_target_base)
rotCorr = euler[2] * 0.1 + 2 * (euler[2] - self.prevEuler)
self.prevEuler = euler[2]
return [thrust, alphaCorr, betaCorr, rotCorr], False
def set_base_angular_velocites(self, params: List[float]):
"""Calls required functions to achieve desired omnidirectional effect.
The name should be changed to be set_base_particle_velocites!
:param params: A List with thrust and three factors for moving.
"""
assert self.num_propellers == 4
particlesTargetVelocities = [0, 0, 0, 0]
thrust = params[0]
alphaCorr = params[1]
betaCorr = params[2]
rotCorr = params[3]
particlesTargetVelocities[0] = thrust * (1 - alphaCorr + betaCorr +
rotCorr)
particlesTargetVelocities[1] = thrust * (1 - alphaCorr - betaCorr -
rotCorr)
particlesTargetVelocities[2] = thrust * (1 + alphaCorr - betaCorr +
rotCorr)
particlesTargetVelocities[3] = thrust * (1 + alphaCorr + betaCorr -
rotCorr)
self.set_propeller_velocities(particlesTargetVelocities)
pr.step()
def move_arm(position, quaternion, ignore_collisions=False):
arm_path = arm.get_path(position,
quaternion=quaternion,
ignore_collisions=ignore_collisions)
arm_path.visualize()
done = False
while not done:
done = arm_path.step()
pr.step()
arm_path.clear_visualization()
cuboid = Shape('cuboid')
goal = Shape('goal')
grasp_point = Dummy('grasp_point')
drive_pos = cuboid.get_position()
drive_pos[1] -= 0.3
print('Driving to cube ...')
drive_to_position(drive_pos, 0)
grasp_point_raised = grasp_point.get_position()
grasp_point_raised[2] += 0.075
print('Move arm above cube ...')
move_arm(grasp_point_raised, grasp_point.get_quaternion())
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()
self._starting_gripper_joint_pos = robot.gripper.get_joint_positions()
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)
self._robot.arm.set_joint_target_velocities(
[0] * len(self._robot.arm.joints))
self._robot.gripper.set_joint_positions(
self._starting_gripper_joint_pos)
self._robot.gripper.set_joint_target_velocities(
[0] * len(self._robot.gripper.joints))
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=((
1.0 if self._robot.gripper.get_open_amount()[0] > 0.9 else 0.0)
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 get_object_position_relative_to_cameras(self, obj):
""" returns the position of an object relative to all enabled cameras in observation configuration """
obj_pos_left_shoulder = obj.get_position(
relative_to=self._cam_over_shoulder_left
) if self._cam_over_shoulder_left.still_exists() else None
obj_pos_right_shoulder = obj.get_position(
relative_to=self._cam_over_shoulder_right
) if self._cam_over_shoulder_right.still_exists() else None
obj_pos_front = obj.get_position(
relative_to=self._cam_front) if self._cam_front.still_exists(
) else None
obj_pos_wrist = obj.get_position(
relative_to=self._cam_wrist) if self._cam_wrist.still_exists(
) else None
return {
"left_shoulder_camera": obj_pos_left_shoulder,
"right_shoulder_camera": obj_pos_right_shoulder,
"front_camera": obj_pos_front,
"wrist_camera": obj_pos_wrist
}
def get_object_pose_relative_to_cameras(self, obj):
""" returns the pose of an object relative to all enabled cameras in observation configuration """
obj_pos_left_shoulder = obj.get_pose(
relative_to=self._cam_over_shoulder_left
) if self._cam_over_shoulder_left.still_exists() else None
obj_pos_right_shoulder = obj.get_pose(
relative_to=self._cam_over_shoulder_right
) if self._cam_over_shoulder_right.still_exists() else None
obj_pos_front = obj.get_pose(
relative_to=self._cam_front) if self._cam_front.still_exists(
) else None
obj_pos_wrist = obj.get_pose(
relative_to=self._cam_wrist) if self._cam_wrist.still_exists(
) else None
return {
"left_shoulder_camera": obj_pos_left_shoulder,
"right_shoulder_camera": obj_pos_right_shoulder,
"front_camera": obj_pos_front,
"wrist_camera": obj_pos_wrist
}
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_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):
with self.assertRaises(NoParentError):
parent = self.dummy.get_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_still_exists(self):
self.assertTrue(self.dynamic_cube.still_exists())
self.assertFalse(Shape(-1).still_exists())
def init_task(self) -> None:
self._success_sensor = ProximitySensor('success')
self._books = [Shape('book2'), Shape('book1'), Shape('book0')]
self._waypoints_idxs = [5, 11, -1]
self.register_graspable_objects(self._books)
def test_create_primitive_simple(self):
pr = Shape.create(PrimitiveShape.CUBOID, size=[1., 1., 1.])
self.assertIsInstance(pr, Shape)
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()
self._starting_gripper_joint_pos = robot.gripper.get_joint_positions()
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_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._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()
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._has_init_task = True
self._variation_index = 0
def init_episode(
self,
index: int,
randomly_place: bool = True,
max_attempts: int = 5,
#added
change_position=False) -> 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)
if randomly_place:
self._place_task(change_position=change_position)
break
# 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)
self._robot.arm.set_joint_target_velocities(
[0] * len(self._robot.arm.joints))
self._robot.gripper.set_joint_positions(
self._starting_gripper_joint_pos)
self._robot.gripper.set_joint_target_velocities(
[0] * len(self._robot.gripper.joints))
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._pyrep.step_ui() for _ in range(20)]
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
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)
obs = Observation(
left_shoulder_rgb=(lsc_ob.rgb_noise.apply(
self._cam_over_shoulder_left.capture_rgb())
if lsc_ob.rgb else None),
left_shoulder_depth=(lsc_ob.depth_noise.apply(
self._cam_over_shoulder_left.capture_depth())
if lsc_ob.depth else None),
right_shoulder_rgb=(rsc_ob.rgb_noise.apply(
self._cam_over_shoulder_right.capture_rgb())
if rsc_ob.rgb else None),
right_shoulder_depth=(rsc_ob.depth_noise.apply(
self._cam_over_shoulder_right.capture_depth())
if rsc_ob.depth else None),
wrist_rgb=(wc_ob.rgb_noise.apply(self._cam_wrist.capture_rgb())
if wc_ob.rgb else None),
wrist_depth=(wc_ob.depth_noise.apply(
self._cam_wrist.capture_depth()) if wc_ob.depth else None),
left_shoulder_mask=(lsc_mask_fn(
self._cam_over_shoulder_left_mask.capture_rgb())
if lsc_ob.mask else None),
right_shoulder_mask=(rsc_mask_fn(
self._cam_over_shoulder_right_mask.capture_rgb())
if rsc_ob.mask else None),
wrist_mask=(wc_mask_fn(self._cam_wrist_mask.capture_rgb())
if wc_ob.mask else None),
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_amount=((
1.0 if self._robot.gripper.get_open_amount()[0] > 0.9 else 0.0)
if self._obs_config.gripper_open_amount else
None),
gripper_pose=(np.array(tip.get_pose())
if self._obs_config.gripper_pose 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),
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,
func=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, func)
success, term = self._active_task.success()
if success:
break
point.end_of_path()
path.clear_visualization()
if success:
# We can quit early because we have finished the task
break
# TODO: need to decide how I do the gripper actions
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, func)
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, func)
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, func)
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, func)
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, func)
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 _demo_record_step(self, demo_list, record, func):
if record:
demo_list.append(self.get_observation())
if func is not None:
func()
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_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_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_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)
def _place_task(self, change_position=False) -> 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,
change_position=change_position)
def test_import_shape(self):
ob = Shape.import_shape(
path.join(ASSET_DIR, 'cracker_box/textured_simple.obj'))
self.assertIsInstance(ob, Shape)
def test_step(self):
cube = Shape('dynamic_cube')
start_pos = cube.get_position()
[self.pyrep.step() for _ in range(2)]
end_pos = cube.get_position()
self.assertNotEqual(start_pos, end_pos)
def test_import_mesh(self):
ob = Shape.import_mesh(path.join(ASSET_DIR, 'test_mesh_bowl.obj'))
self.assertIsInstance(ob, Shape)
def test_step(self):
cube = Shape('dynamic_cube')
start_pos = cube.get_position()
[self.pyrep.step() for _ in range(2)]
end_pos = cube.get_position()
self.assertFalse(np.allclose(start_pos, end_pos))
def test_create_mesh(self):
ob = Shape.create_mesh(
vertices=[-0.1, -0.1, 0.0, -0.1, 0.1, 0.0, 0.1, 0.0, 0.0],
indices=[0, 1, 2])
self.assertIsInstance(ob, Shape)
def __init__(self,
escene_name='proximity_sensor.ttt',
start=[100, 100],
goal=[180, 500],
rand_area=[100, 450],
path_resolution=5.0,
margin=0.,
margin_to_goal=0.5,
action_max=[.5, 1.],
action_min=[0., 0.],
_load_path=True,
path_name="PathNodes",
type_of_planning="PID",
type_replay_buffer="random",
max_laser_range=1.0,
headless=False):
SCENE_FILE = join(dirname(abspath(__file__)), escene_name)
self.pr = PyRep()
self.pr.launch(SCENE_FILE, headless=headless)
self.pr.start()
self.agent = Pioneer("Pioneer_p3dx",
type_of_planning=type_of_planning,
type_replay_buffer=type_replay_buffer)
self.agent.set_control_loop_enabled(False)
self.initial_joint_positions = self.agent.get_joint_positions()
self.initial_position = self.agent.get_position()
print(f"Agent initial position: {self.initial_position}")
self.vision_map = VisionSensor("VisionMap")
self.vision_map.handle_explicitly()
self.vision_map_handle = self.vision_map.get_handle()
self.floor = Shape("Floor_respondable")
self.perspective_angle = self.vision_map.get_perspective_angle # degrees
self.resolution = self.vision_map.get_resolution # list [resx, resy]
self.margin = margin
self.margin_to_goal = margin_to_goal
self.rand_area = rand_area
self.start = start
self.goal = goal
self.type_of_planning = type_of_planning
self.max_laser_range = max_laser_range
self.max_angle_orientation = np.pi
scene_image = self.vision_map.capture_rgb() * 255
scene_image = np.flipud(scene_image)
self.rew_weights = [1, 10000, 5000]
self.start_position = Dummy("Start").get_position()
self.goal_position = Dummy("Goal").get_position() # [x, y, z]
self.local_goal_aux = Dummy("Local_goal_aux")
self.local_goal_aux_pos_list = [[-3.125, 2.175, 0], [2.9, 3.575, 0],
self.goal_position, self.goal_position]
self.ind_local_goal_aux = 0
self.max_distance = get_distance([-7.5, -4.1, 0.], self.goal_position)
self.distance_to_goal_m1 = get_distance(self.start_position,
self.goal_position)
self.c_lin_vel = self.c_ang_vel = self.b_lin_vel = self.b_ang_vel = 0.
self.action_max = action_max
self.action_min = action_min
if type_of_planning == 'PID':
self.planning_info_logger = get_logger("./loggers",
"Planning_Info.log")
self.image_path = None
if exists("./paths/" + path_name + ".npy") and _load_path:
print("Load Path..")
self.image_path = np.load("./paths/" + path_name + ".npy",
allow_pickle=True)
else:
print("Planning...")
self.image_path = self.Planning(
scene_image,
self.start,
self.goal,
self.rand_area,
path_resolution=path_resolution,
logger=self.planning_info_logger)
assert self.image_path is not None, "path should not be a Nonetype"
self.real_path = self.path_image2real(self.image_path,
self.start_position)
# project in coppelia sim
sim_drawing_points = 0
point_size = 10 #[pix]
duplicate_tolerance = 0
parent_obj_handle = self.floor.get_handle()
max_iter_count = 999999
ambient_diffuse = (255, 0, 0)
blue_ambient_diffuse = (0, 0, 255)
point_container = sim.simAddDrawingObject(
sim_drawing_points,
point_size,
duplicate_tolerance,
parent_obj_handle,
max_iter_count,
ambient_diffuse=ambient_diffuse)
local_point_container = sim.simAddDrawingObject(
sim_drawing_points,
point_size,
duplicate_tolerance,
parent_obj_handle,
max_iter_count,
ambient_diffuse=blue_ambient_diffuse)
# debug
for point in self.real_path:
point_data = (point[0], point[1], 0)
sim.simAddDrawingObjectItem(point_container, point_data)
# You need to get the real coord in the real world
assert local_point_container is not None, "point container shouldn't be empty"
self.agent.load_point_container(local_point_container)
self.agent.load_path(self.real_path)
class TestShapes(TestCore):
def setUp(self):
super().setUp()
self.dynamic_cube = Shape('dynamic_cube')
def test_create_primitive_simple(self):
pr = Shape.create(PrimitiveShape.CUBOID, size=[1., 1., 1.])
self.assertIsInstance(pr, Shape)
def test_create_primitive_complex(self):
pr = Shape.create(PrimitiveShape.CUBOID,
size=[1., 1., 1.],
mass=2.0,
smooth=True,
respondable=True,
static=False,
position=[1.1, 1.2, 1.3],
orientation=[0.1, 0.2, 0.3],
color=[0.7, 0.8, 0.9])
self.assertIsInstance(pr, Shape)
self.assertTrue(np.allclose(pr.get_position(), [1.1, 1.2, 1.3]))
self.assertTrue(np.allclose(pr.get_orientation(), [0.1, 0.2, 0.3]))
self.assertTrue(np.allclose(pr.get_color(), [0.7, 0.8, 0.9]))
def test_import_shape(self):
ob = Shape.import_shape(
path.join(ASSET_DIR, 'cracker_box/textured_simple.obj'))
self.assertIsInstance(ob, Shape)
def test_import_mesh(self):
ob = Shape.import_mesh(path.join(ASSET_DIR, 'test_mesh_bowl.obj'))
self.assertIsInstance(ob, Shape)
def test_create_mesh(self):
ob = Shape.create_mesh(
vertices=[-0.1, -0.1, 0.0, -0.1, 0.1, 0.0, 0.1, 0.0, 0.0],
indices=[0, 1, 2])
self.assertIsInstance(ob, Shape)
def test_convex_decompose(self):
ob = Shape.import_mesh(path.join(ASSET_DIR, 'test_mesh_bowl.obj'))
self.assertIsInstance(ob, Shape)
cd_1 = ob.get_convex_decomposition()
self.assertIsInstance(cd_1, Shape)
self.assertNotEqual(ob, cd_1)
cd_2 = ob.get_convex_decomposition(morph=True)
self.assertIsInstance(cd_2, Shape)
self.assertEqual(ob, cd_2)
def test_get_set_color(self):
self.dynamic_cube.set_color([.5] * 3)
self.assertEqual(self.dynamic_cube.get_color(), [.5] * 3)
def test_get_set_mass(self):
self.dynamic_cube.set_mass(3.5)
self.assertEqual(self.dynamic_cube.get_mass(), 3.5)
def test_get_set_respondable(self):
self.dynamic_cube.set_respondable(False)
self.assertFalse(self.dynamic_cube.is_respondable())
self.dynamic_cube.set_respondable(True)
self.assertTrue(self.dynamic_cube.is_respondable())
def test_get_set_dynamic(self):
self.dynamic_cube.set_dynamic(False)
self.assertFalse(self.dynamic_cube.is_dynamic())
self.dynamic_cube.set_dynamic(True)
self.assertTrue(self.dynamic_cube.is_dynamic())
def test_get_mesh_data(self):
vertices, indices, normals = self.dynamic_cube.get_mesh_data()
n_vertices = 8
n_faces = 12
self.assertEqual(vertices.shape, (n_vertices, 3))
self.assertEqual(indices.shape, (n_faces, 3))
self.assertEqual(normals.shape, (n_faces * 3, 3))
def test_set_texture(self):
_, texture = self.pyrep.create_texture(
path.join(ASSET_DIR, 'wood_texture.jpg'))
self.dynamic_cube.set_texture(texture, TextureMappingMode.CUBE)
self.assertEqual(texture.get_texture_id(),
self.dynamic_cube.get_texture().get_texture_id())
def test_get_shape_viz(self):
visual = Shape('cracker_box_visual')
info = visual.get_shape_viz(index=0)
self.assertIsInstance(info.vertices, np.ndarray)
self.assertEqual(info.vertices.shape[1], 3)
self.assertIsInstance(info.indices, np.ndarray)
self.assertEqual(info.indices.shape[1], 3)
self.assertIsInstance(info.normals, np.ndarray)
self.assertEqual(info.normals.shape[1], 3)
self.assertIsInstance(info.shading_angle, float)
self.assertIsInstance(info.colors, np.ndarray)
self.assertTupleEqual(info.colors.shape, (9, ))
self.assertIsInstance(info.texture, np.ndarray)
self.assertTupleEqual(info.texture.shape, (512, 512, 4))
self.assertIsInstance(info.texture_id, int)
self.assertIsInstance(info.texture_coords, np.ndarray)
self.assertEqual(info.texture_coords.shape[1], 2)
self.assertIsInstance(info.texture_apply_mode, int)
self.assertIsInstance(info.texture_options, int)
def test_apply_texture(self):
visual = Shape('cracker_box_visual')
info = visual.get_shape_viz(index=0)
self.assertNotEqual(info.texture_coords.size, 0)
self.assertNotEqual(info.texture.size, 0)
texture = info.texture[:, :, [2, 1, 0, 3]] # rgba -> bgra
visual.apply_texture(info.texture_coords, texture, is_rgba=True)
if __name__ == "__main__":
env = Env(scene('Tic_tac_toe.ttt'))
env.start()
# franka
franka = Franka()
# set franka to home joints
franka.home(env)
# initiate the camera
cam = Camera()
imgs = []
# chessboard and chesses
chessboard = Shape('chessboard')
chesses = {
'X': [Shape('chess_X' + str(chess_num)) for chess_num in range(1, 6)],
'O': [Shape('chess_O' + str(chess_num)) for chess_num in range(1, 6)]
}
# create chessboard instance
board = Board()
imgs.append(cam.capture_bgr())
# Decide who first (X always first)
system('clear')
instream = input('Do you want to play first hand?[y/n]')
if instream != 'n':
ai = tictactoe_ai('O')
player_chess = 'X'
def boundary_root(self) -> Object:
return Shape('boundary_root')
def init_task(self) -> None:
charger_success = ProximitySensor('charger_success')
charger = Shape('charger')
self.register_graspable_objects([charger])
self.register_success_conditions(
[DetectedCondition(charger, charger_success)])
class StackCups(Task):
def init_task(self) -> None:
success_sensor = ProximitySensor('success')
self.cup1 = Shape('cup1')
self.cup2 = Shape('cup2')
self.cup3 = Shape('cup3')
self.cup1_visual = Shape('cup1_visual')
self.cup2_visual = Shape('cup2_visual')
self.cup3_visaul = Shape('cup3_visual')
self.boundary = SpawnBoundary([Shape('boundary')])
self.register_graspable_objects([self.cup1, self.cup2, self.cup3])
self.register_success_conditions([
DetectedCondition(self.cup1, success_sensor),
DetectedCondition(self.cup3, success_sensor),
NothingGrasped(self.robot.gripper)
])
def init_episode(self, index: int) -> List[str]:
self.variation_index = index
target_color_name, target_rgb = colors[index]
random_idx = np.random.choice(len(colors))
while random_idx == index:
random_idx = np.random.choice(len(colors))
_, other1_rgb = colors[random_idx]
random_idx = np.random.choice(len(colors))
while random_idx == index:
random_idx = np.random.choice(len(colors))
_, other2_rgb = colors[random_idx]
self.cup2_visual.set_color(target_rgb)
self.cup1_visual.set_color(other1_rgb)
self.cup3_visaul.set_color(other2_rgb)
self.boundary.clear()
self.boundary.sample(self.cup2,
min_distance=0.05,
min_rotation=(0, 0, 0),
max_rotation=(0, 0, 0))
self.boundary.sample(self.cup1,
min_distance=0.05,
min_rotation=(0, 0, 0),
max_rotation=(0, 0, 0))
self.boundary.sample(self.cup3,
min_distance=0.05,
min_rotation=(0, 0, 0),
max_rotation=(0, 0, 0))
return [
'stack the other cups on top of the %s cup' % target_color_name,
'place two of the cups onto the odd cup out',
'put the remaining two cups on top of the %s cup' %
target_color_name,
'pick up and set the cups down into the %s cup' %
target_color_name,
'create a stack of cups with the %s cup as its base' %
target_color_name,
'keeping the %s cup on the table, stack the other two onto it' %
target_color_name
]
def variation_count(self) -> int:
return len(colors)
def test_equality(self):
cube2 = Shape('dynamic_cube')
self.assertEqual(self.dynamic_cube, cube2)
def init_task(self) -> None:
self.steak = Shape('steak')
self.chicken = Shape('chicken')
self.success_sensor = ProximitySensor('success')
self.register_graspable_objects([self.chicken, self.steak])
self.w1 = Dummy('waypoint1')
