def test_run_task_validator(self): for task_file in TASKS: test_name = task_file.split('.py')[0] with self.subTest(task=test_name): if test_name in FLAKY_TASKS: self.skipTest('Flaky task.') sim = PyRep() ttt_file = os.path.join( DIR_PATH, '..', '..', 'rlbench', TTT_FILE) sim.launch(ttt_file, headless=True) sim.step_ui() sim.set_simulation_timestep(50.0) sim.step_ui() sim.start() robot = Robot(Panda(), PandaGripper()) obs = ObservationConfig() obs.set_all(False) scene = Scene(sim, robot, obs) sim.start() task_class = task_file_to_task_class(task_file) active_task = task_class(sim, robot) try: task_smoke(active_task, scene, variation=-1, max_variations=2, success=0.25) except Exception as e: sim.stop() sim.shutdown() raise e sim.stop() sim.shutdown()
class BaseEnv: def __init__(self, scene_file="", headless=False): self.pr = PyRep() # Launch the application with a scene file in headless mode self.pr.launch(scene_file, headless=headless) self.pr.start() # Start the simulation def step(self): self.pr.step() def stop(self): self.pr.stop() self.pr.shutdown() def load_scene_object_from_file(self, file_path): respondable = self.pr.import_model(file_path) visible = respondable.get_objects_in_tree(exclude_base=True)[0] return SceneObject(respondable_part=respondable, visible_part=visible) def load_mesh_from_file(self, file_path, scaling_factor=1): shape = Shape.import_shape(filename=file_path, scaling_factor=scaling_factor) self.pr.step() return shape
class TestSuctionCups(TestCore): def setUp(self): self.pyrep = PyRep() self.pyrep.launch(path.join(ASSET_DIR, 'test_scene_robots.ttt'), headless=True) self.pyrep.step() self.pyrep.start() def test_get_suction_cup(self): for cup_name, cup_type in SUCTION_CUPS: with self.subTest(suction_cup=cup_name): cup = cup_type() self.assertIsInstance(cup, cup_type) def test_grasp_and_release_with_suction(self): for cup_name, cup_type in SUCTION_CUPS: with self.subTest(suction_cup=cup_name): suction_cube = Shape('%s_cube' % cup_name) cube = Shape('cube') cup = cup_type() self.pyrep.step() grasped = cup.grasp(cube) self.assertFalse(grasped) self.assertEqual(len(cup.get_grasped_objects()), 0) grasped = cup.grasp(suction_cube) self.assertTrue(grasped) self.assertListEqual(cup.get_grasped_objects(), [suction_cube]) cup.release() self.assertEqual(len(cup.get_grasped_objects()), 0)
class SimulationConsumerAbstract(mp.Process): _id = 0 """This class sole purpose is to better 'hide' all interprocess related code from the user.""" def __init__(self, process_io, scene="", gui=False): super().__init__(name="simulation_consumer_{}".format( SimulationConsumerAbstract._id)) self._id = SimulationConsumerAbstract._id SimulationConsumerAbstract._id += 1 self._scene = scene self._gui = gui self._process_io = process_io np.random.seed() def run(self): self._pyrep = PyRep() self._pyrep.launch(self._scene, headless=not self._gui, write_coppeliasim_stdout_to_file=True) self._process_io["simulaton_ready"].set() self._main_loop() def _close_pipes(self): self._process_io["command_pipe_out"].close() self._process_io["return_value_pipe_in"].close() # self._process_io["exception_pipe_in"].close() # let this one open def _main_loop(self): success = True while success and not self._process_io["must_quit"].is_set(): success = self._consume_command() self._pyrep.shutdown() self._close_pipes() def _consume_command(self): try: # to execute the command and send result success = True command = self._process_io["command_pipe_out"].recv() self._process_io["slot_in_command_queue"].release() ret = command[0](self, *command[1], **command[2]) if command[0]._communicate_return_value: self._communicate_return_value(ret) except Exception as e: # print traceback, dont raise traceback = format_exc() success = False # return False: quit the main loop self._process_io["exception_pipe_in"].send((e, traceback)) finally: return success def _communicate_return_value(self, value): self._process_io["return_value_pipe_in"].send(value) def signal_command_pipe_empty(self): self._process_io["command_pipe_empty"].set() while self._process_io["command_pipe_empty"].is_set(): time.sleep(0.1) def good_bye(self): pass
class TestCore(unittest.TestCase): def setUp(self): self.pyrep = PyRep() self.pyrep.launch(path.join(ASSET_DIR, 'test_scene.ttt'), headless=True) self.pyrep.step() self.pyrep.start() def tearDown(self): self.pyrep.stop() self.pyrep.step_ui() self.pyrep.shutdown()
def _start_sim(self, scene_file, render_scene_file, headless, sim_timestep, render): '''Starts the simulation. We use different scene files for training and rendering, as unused objects still slow down the simulation. :param sim_timestep: The timestep between actionable frames. The physics simulation has a timestep of 5ms, but the control commands are repeated until the *sim_timestep* is reached''' sim = PyRep() scene_file = [scene_file if not render else render_scene_file][0] scene_file = join(dirname(abspath(__file__)), scene_file) sim.launch(scene_file, headless=headless) # Need sim_timestep set to custom in CoppeliaSim Scene for this method to work. sim.set_simulation_timestep(dt=sim_timestep) sim.start() return sim
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()
class ReacherEnv(object): def __init__(self): self.pr = PyRep() self.pr.launch(SCENE_FILE, headless=False) self.pr.start() # self.agent = Panda() self.agent = Sawyer() self.gripper = BaxterGripper() self.agent.set_control_loop_enabled(False) self.agent.set_motor_locked_at_zero_velocity(True) self.target = Shape('target') self.agent_ee_tip = self.agent.get_tip() self.initial_joint_positions = self.agent.get_joint_positions() def _get_state(self): # Return state containing arm joint angles/velocities & target position return (self.agent.get_joint_positions() + self.agent.get_joint_velocities() + self.target.get_position()) def reset(self): # Get a random position within a cuboid and set the target position pos = list(np.random.uniform(POS_MIN, POS_MAX)) self.target.set_position(pos) self.agent.set_joint_positions(self.initial_joint_positions) self.pr.step() return self._get_state() def step(self, action): self.agent.set_joint_target_velocities(action) # Execute action on arm self.pr.step() # Step the physics simulation ax, ay, az = self.agent_ee_tip.get_position() tx, ty, tz = self.target.get_position() # Reward is negative distance to target distance = (ax - tx)**2 + (ay - ty)**2 + (az - tz)**2 done = False if distance < 5: done = True self.gripper.actuate( 0, velocity=0.04 ) # if done, close the hand, 0 for close and 1 for open. self.pr.step() # Step the physics simulation reward = -np.sqrt(distance) return self._get_state(), reward, done def shutdown(self): self.pr.stop() self.pr.shutdown()
class TestArmsAndConfigurationPaths(TestCore): def setUp(self): self.pyrep = PyRep() self.pyrep.launch(path.join( ASSET_DIR, 'test_scene_robots.ttt'), headless=True) self.pyrep.step() self.pyrep.start() def test_get_gripper(self): for gripper_name, gripper_type, vel in GRIPPERS: with self.subTest(gripper=gripper_name): gripper = gripper_type() self.assertIsInstance(gripper, gripper_type) def test_close_open_gripper(self): for gripper_name, gripper_type, vel in GRIPPERS: with self.subTest(gripper=gripper_name): gripper = gripper_type() self.pyrep.step() done = False i = 0 while not done: done = gripper.actuate(0.0, velocity=vel) self.pyrep.step() i += 1 if i > 1000: self.fail('Took too many steps to close') done = False i = 0 open_amount = 1.0 if gripper_name == 'Robotiq85Gripper' else 0.8 while not done: done = gripper.actuate(open_amount, velocity=vel) self.pyrep.step() i += 1 if i > 1000: self.fail('Took too many steps to open') self.assertAlmostEqual( gripper.get_open_amount()[0], open_amount, delta=0.05) def test_get_duplicate_gripper(self): g = BaxterGripper(1) self.assertIsInstance(g, BaxterGripper) def test_copy_gripper(self): g = JacoGripper() new_g = g.copy() self.assertNotEqual(g, new_g)
class ReacherEnv(object): def __init__(self): self.pr = PyRep() self.pr.launch(SCENE_FILE, headless=False) self.pr.start() self.agent = TurtleBot() self.agent.set_control_loop_enabled(False) self.agent.set_motor_locked_at_zero_velocity(True) self.target = Shape.create(type=PrimitiveShape.SPHERE, size=[0.05, 0.05, 0.05], color=[7.0, 0.5, 0.5], static=True, respondable=False) # self.target = Shape('target') # self.agent_ee_tip = self.agent.get_tip() self.initial_joint_positions = self.agent.get_joint_positions() def _get_state(self): # Return state containing arm joint angles/velocities & target position return np.concatenate([ self.agent.get_base_actuation(), self.agent.get_base_velocities(), self.target.get_position() ]) def reset(self): # Get a random position within a cuboid and set the target position pos = list(np.random.uniform(POS_MIN, POS_MAX)) pos_agent = list(np.random.uniform([0, 0, 0], [0, 0, 0])) self.target.set_position(pos) self.agent.set_position(pos_agent) return self._get_state() def step(self, action): self.agent.set_joint_target_velocities(action) # Execute action on arm self.pr.step() # Step the physics simulation ax, ay, az = self.agent.get_position() tx, ty, tz = self.target.get_position() # Reward is negative distance to target reward = -np.sqrt((ax - tx)**2 + (ay - ty)**2 + (az - tz)**2) print(int(reward * 100), int(ax * 100), int(ay * 100), int(az * 100), int(tx * 100), int(ty * 100), int(tz * 100)) return reward, self._get_state() def shutdown(self): self.pr.stop() self.pr.shutdown()
class TestScene(unittest.TestCase): """Tests the following: - Getting observations from the scene - Applying noise - Applying domain randomization """ def setUp(self): self.pyrep = PyRep() self.pyrep.launch(join(environment.DIR_PATH, TTT_FILE), headless=True) self.pyrep.set_simulation_timestep(0.005) self.robot = Robot(Panda(), PandaGripper()) def tearDown(self): self.pyrep.shutdown() def test_sensor_noise_images(self): cam_config = CameraConfig(rgb_noise=GaussianNoise(0.05, (.0, 1.))) obs_config = ObservationConfig(left_shoulder_camera=cam_config, joint_forces=False, task_low_dim_state=False) scene = Scene(self.pyrep, self.robot, obs_config) scene.load(ReachTarget(self.pyrep, self.robot)) obs1 = scene.get_observation() obs2 = scene.get_observation() self.assertTrue( np.array_equal(obs1.right_shoulder_rgb, obs2.right_shoulder_rgb)) self.assertFalse( np.array_equal(obs1.left_shoulder_rgb, obs2.left_shoulder_rgb)) self.assertTrue(obs1.left_shoulder_rgb.max() <= 1.0) self.assertTrue(obs1.left_shoulder_rgb.min() >= 0.0) def test_sensor_noise_robot(self): obs_config = ObservationConfig( joint_velocities_noise=GaussianNoise(0.01), joint_forces=False, task_low_dim_state=False) scene = Scene(self.pyrep, self.robot, obs_config) scene.load(ReachTarget(self.pyrep, self.robot)) obs1 = scene.get_observation() obs2 = scene.get_observation() self.assertTrue( np.array_equal(obs1.joint_positions, obs2.joint_positions)) self.assertFalse( np.array_equal(obs1.joint_velocities, obs2.joint_velocities))
class VrepSim(object): """Class that interfaces with Habitat sim""" def __init__( self, configs, scene_path=None, seed=0 ): # TODO: extend the arguments of constructor self.sim_config = copy.deepcopy(configs.COMMON.SIMULATOR) if scene_path is None: raise RuntimeError("Please specify the .ttt v-rep scene file path!") self.sim = PyRep() self.sim.launch(scene_path, headless=False) self.sim.start() [self.sim.step() for _ in range(50)] def __del__(self): self.sim.stop() self.sim.shutdown() def reset(self): """Reset the Habitat simultor""" raise NotImplemented("Reset method for v-rep has not been implemented")
class ReacherEnv(object): def __init__(self): self.pr = PyRep() self.pr.launch(SCENE_FILE, headless=False) self.pr.start() self.agent = Pioneer() self.agent.set_control_loop_enabled(False) self.agent.set_motor_locked_at_zero_velocity(True) self.target = Shape('target') self.agent_ee_tip = self.agent.get_tip() self.initial_joint_positions = self.agent.get_joint_positions() def reset(self): # Get a random position within a cuboid and set the target position pos = list(np.random.uniform(POS_MIN, POS_MAX)) self.target.set_position(pos) self.agent.set_joint_positions(self.initial_joint_positions) return self._get_state() def step(self, action): self.agent.set_joint_target_velocities(action) # Execute action on arm self.pr.step() # Step the physics simulation ax, ay, az = self.agent_ee_tip.get_position() tx, ty, tz = self.target.get_position() # Reward is negative distance to target reward = -np.sqrt((ax - tx)**2 + (ay - ty)**2 + (az - tz)**2) return reward, self._get_state() def _get_state(self): # Return state containing arm joint angles/velocities & target position return np.concatenate([ self.agent.get_joint_positions(), self.agent.get_joint_velocities(), self.target.get_position() ]) def shutdown(self): self.pr.stop() self.pr.shutdown()
class Environment: def __init__( self, texture="/home/aecgroup/aecdata/Textures/mcgillManMade_600x600_png_selection/", scene="/home/aecgroup/aecdata/Software/vrep_scenes/stereo_vision_robot_collection.ttt", headless=True): self.pyrep = PyRep() self.pyrep.launch(scene, headless=headless) min_distance = 0.5 max_distance = 5 max_speed = 0.5 path = texture textures_names = listdir(path) textures_list = [ self.pyrep.create_texture(path + name)[1] for name in textures_names ] self.screen = RandomScreen(min_distance, max_distance, max_speed, textures_list) self.robot = StereoVisionRobot(min_distance, max_distance) self.pyrep.start() def step(self): # step simulation self.pyrep.step() # move screen self.screen.increment_iteration() def episode_reset(self, preinit=False): # reset screen self.screen.episode_reset(preinit=preinit) # reset robot self.robot.episode_reset() # self.robot.set_vergence_position(to_angle(self.screen.distance)) self.pyrep.step() def close(self): self.pyrep.stop() self.pyrep.shutdown()
class TestArmsAndConfigurationPaths(TestCore): def setUp(self): self.pyrep = PyRep() self.pyrep.launch(path.join( ASSET_DIR, 'test_scene_robots.ttt'), headless=True) self.pyrep.step() self.pyrep.start() def test_get_gripper(self): for gripper_name, gripper_type, vel in GRIPPERS: with self.subTest(gripper=gripper_name): gripper = gripper_type() self.assertIsInstance(gripper, gripper_type) def test_close_open_gripper(self): for gripper_name, gripper_type, vel in GRIPPERS: with self.subTest(gripper=gripper_name): gripper = gripper_type() self.pyrep.step() done = False i = 0 while not done: done = gripper.actuate(0.0, velocity=vel) self.pyrep.step() i += 1 if i > 1000: self.fail('Took too many steps to close') done = False i = 0 while not done: done = gripper.actuate(0.8, velocity=vel) self.pyrep.step() i += 1 if i > 1000: self.fail('Took too many steps to open') self.assertAlmostEqual( gripper.get_open_amount()[0], 0.8, delta=0.05)
class DomainRandomizationEnvironment(Environment): """Each environment has a scene.""" def __init__(self, action_mode: ActionMode, dataset_root='', obs_config=ObservationConfig(), headless=False, static_positions: bool = False, randomize_every: RandomizeEvery = RandomizeEvery.EPISODE, frequency: int = 1, visual_randomization_config=None, dynamics_randomization_config=None): super().__init__(action_mode, dataset_root, obs_config, headless, static_positions) self._randomize_every = randomize_every self._frequency = frequency self._visual_rand_config = visual_randomization_config self._dynamics_rand_config = dynamics_randomization_config def launch(self): if self._pyrep is not None: raise RuntimeError('Already called launch!') self._pyrep = PyRep() self._pyrep.launch(join(DIR_PATH, TTT_FILE), headless=self._headless) self._pyrep.set_simulation_timestep(0.005) self._robot = Robot(Panda(), PandaGripper()) self._scene = DomainRandomizationScene(self._pyrep, self._robot, self._obs_config, self._randomize_every, self._frequency, self._visual_rand_config, self._dynamics_rand_config) self._set_arm_control_action() # Raise the domain randomized floor. Shape('Floor').set_position(Dummy('FloorAnchor').get_position())
# import ipdb;ipdb.set_trace() b = cos(angle) * 0.5 a = sin(angle) * 0.5 pt0 = (int(x - a * l - b * w), int(y + b * l - a * w)) pt1 = (int(x + a * l - b * w), int(y - b * l - a * w)) pt2 = (int(2 * x - pt0[0]), int(2 * y - pt0[1])) # (int(x + a * l + b * w), int(y - b * l + a * w)) pt3 = (int(2 * x - pt1[0]), int(2 * y - pt1[1])) cv2.fillConvexPoly(img, np.array([pt0, pt1, pt2, pt3]), color=[height]) return img if __name__ == "__main__": # Unit test pr = PyRep() pr.launch("scene/blank_robotnik.ttt", headless=False) pr.start() # env range is 5*5 pos_min = [-2, -1.3, 0] pos_max = [1.3, 2, 0] obstacle_num=1 # agent agent = nik('Robotnik') # path target = Shape.create(type=PrimitiveShape.SPHERE, size=[0.05, 0.05, 0.05],
class PyRepModule(object): def __init__(self, logger, headless=False): self.logger = logger self.callback = { PyRepRequestType.initialize_part_to_scene: self.initialize_part_to_scene, PyRepRequestType.update_group_to_scene: self.update_group_to_scene, PyRepRequestType.get_assembly_point: self.get_assembly_point, PyRepRequestType.update_part_status: self.update_part_status, PyRepRequestType.get_cost_of_available_pair: self.get_cost_of_available_pair } self.pr = PyRep() self.pr.launch(headless=headless) self.pr.start() self.scene_path = "./test_scene" # used to visualize and assembly self.part_info = None self.part_status = None self.primitive_parts = {} self.group_info = None self.group_obj = {} def initialize_server(self): self.logger.info("Initialize PyRep Server") host = SocketType.pyrep.value["host"] port = SocketType.pyrep.value["port"] self.server = socket.socket(socket.AF_INET, socket.SOCK_STREAM) self.server.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) self.server.bind((host, port)) self.server.listen(True) self.host = host self.port = port try: self.logger.info("Waiting for PyRep client {}:{}".format( self.host, self.port)) self.connected_client, addr = self.server.accept() self.logger.info("Connected to {}".format(addr)) except: self.logger.info("PyRep Server Error") finally: self.server.close() def get_callback(self, request): return self.callback[request] def save_scene(self, path): self.pr.export_scene(path) def close(self): self.connected_client.close() self.server.close() self.pr.stop() self.pr.shutdown() #region socket function def initialize_part_to_scene(self): global ASSEMBLY_PAIR self.logger.info("ready to initialize part to scene") sendall_pickle(self.connected_client, True) request = recvall_pickle(self.connected_client) self.part_info = request["part_info"] ASSEMBLY_PAIR = request["pair_info"] self.logger.info("...initializing pyrep scene") for part_name in self.part_info.keys(): self._import_part_info(part_name) self.logger.info("End to initialize pyrep scene") sendall_pickle(self.connected_client, True) def _import_part_info(self, part_name): assert self.part_info self.logger.info("import part info of {}".format(part_name)) obj_path = self.part_info[part_name]["obj_file"] # part_obj = Shape.import_mesh(obj_path, scaling_factor=0.001) part_obj = ObjObject.create_object(obj_path=obj_path) part_obj.set_name(part_name) # import each assembly points to scene assembly_points = self.part_info[part_name]["assembly_points"] points = {} for idx in assembly_points.keys(): ap = assembly_points[idx] radius = ap["radius"] / 1000 depth = ap["depth"] position = ap["pose"]["position"] quaternion = ap["pose"]["quaternion"] direction = ap["direction"] assembly_point = Shape.create(PrimitiveShape.SPHERE, size=[radius] * 3, respondable=False, static=True, color=[1, 0, 0]) pose = position + quaternion assembly_point.set_pose(pose) assembly_point.set_name("{}_AP_{}".format(part_name, idx)) assembly_point.set_parent(part_obj.shape) points[idx] = assembly_point part_region_info = self.part_info[part_name]["region_info"] region_info = {} for region_id in part_region_info.keys(): position = part_region_info[region_id]["position"] point_list = part_region_info[region_id]["points"] region_shape = Shape.create(PrimitiveShape.SPHERE, size=[0.05] * 3, respondable=False, static=True, color=[0, 1, 0]) region_shape.set_name("{}_region_{}".format(part_name, region_id)) region_shape.set_parent(part_obj.shape) region_shape.set_position(position) for point_idx in point_list: points[point_idx].set_parent(region_shape) region_info[region_id] = { "shape": region_shape, "points": point_list } self.primitive_parts[part_name] = PartObject(part_object=part_obj, assembly_points=points, region_info=region_info) def update_group_to_scene(self): self.logger.info("ready to update group to scene") sendall_pickle(self.connected_client, True) request = recvall_pickle(self.connected_client) new_group_info = request["group_info"] self.logger.info("...updating group to scene") self._update_group_obj(new_group_info) self.logger.info("End to update group to scene") sendall_pickle(self.connected_client, True) def _update_group_obj(self, new_group_info): for group_id in self.group_obj.keys(): self.group_obj[group_id].remove() self.group_obj = {} for group_id in new_group_info.keys(): if not new_group_info[group_id]["is_exist"]: continue self.logger.info("update group_{} to scene".format(group_id)) group_info = new_group_info[group_id] obj_root = group_info["obj_root"] file_list = get_file_list(obj_root) base_obj = None composed_parts = [] composed_objects = [] group_pose = load_yaml_to_dic(join(obj_root, "group_pose.yaml")) composed_part_pose = {} for file_path in file_list: obj_name, ext = get_file_name(file_path) if not ext == ".obj": continue if obj_name == "base": # import base object base_obj = ObjObject.create_object(file_path) base_obj.set_name("G{}".format(group_id)) else: # ikea_stefan_bottom_0 instance_id = obj_name.split("_")[-1] part_name = obj_name.replace("_{}".format(instance_id), "") obj = ObjObject.create_object(file_path) obj.set_name("G{}_{}_{}".format(group_id, part_name, instance_id)) part_instance = { "part_name": part_name, "instance_id": int(instance_id) } composed_parts.append(part_instance) composed_part_idx = composed_parts.index(part_instance) composed_part_pose[composed_part_idx] = group_pose[ obj_name] object_dict = { "group_object": obj, "primitive": self.primitive_parts[part_name] } composed_objects.append(object_dict) assert base_obj, "No base object" composed_parts = tuple(composed_parts) composed_objects = tuple(composed_objects) for object_dict in composed_objects: obj = object_dict["group_object"] obj.set_parent(base_obj.shape) self.group_obj[group_id] = GroupObject(base_obj, composed_parts, composed_objects, composed_part_pose) def update_part_status(self): self.logger.info("ready to update part_status") sendall_pickle(self.connected_client, True) request = recvall_pickle(self.connected_client) self.part_status = request["part_status"] self.logger.info("Update part_status") sendall_pickle(self.connected_client, True) #TODO: delete assembly point def get_assembly_point(self): self.logger.info("ready to get assembly point from scene") sendall_pickle(self.connected_client, True) request = recvall_pickle(self.connected_client) group_id = int(request["group_id"]) connection_locs = request["connection_locs"] connector_name = request["connector_name"] #TODO: self.logger.info( "...get assembly points of group {} from scene".format(group_id)) target_group = self.group_obj[group_id] assembly_points = None try: assembly_points = target_group.get_assembly_points( locations=connection_locs, connector_name=connector_name, part_status=self.part_status) except Exception as e: self.logger.info("Error occur {}".format(e)) self.save_scene( join(self.scene_path, "error_scene_{}.ttt".format(get_time_stamp()))) self.logger.info("End to get assembly point from pyrep scene") sendall_pickle(self.connected_client, assembly_points) self.save_scene( join(self.scene_path, "test_scene_{}.ttt".format(get_time_stamp()))) def get_cost_of_available_pair(self): self.logger.info("ready to get cost from scene") sendall_pickle(self.connected_client, True) request = recvall_pickle(self.connected_client) group_id = int(request["group_id"]) pair_list = request["check_pair"] self.logger.info("...get cost of group {} from scene".format(group_id)) target_group = self.group_obj[group_id] pair_cost = None try: pair_cost = target_group.get_cost_between_pair(pair_list) except Exception as e: self.logger.info("Error occur {}".format(e)) self.save_scene("test_error_scene/error_scene_{}.ttt".format( get_time_stamp())) self.logger.info("End to get assembly point from pyrep scene") sendall_pickle(self.connected_client, pair_cost) self.save_scene("test_scene/test_scene_{}.ttt".format( get_time_stamp()))
class PyRepIO(AbstractIO): """ This class is used to get/set values from/to a CoppeliaSim scene. It is based on PyRep (http://www.coppeliarobotics.com/helpFiles/en/PyRep.htm). """ def __init__(self, scene="", start=False, headless=False, responsive_ui=False, blocking=False): """ Starts the connection with the CoppeliaSim remote API server. :param str scene: path to a CoppeliaSim scene file :param bool start: whether to start the scene after loading it """ self._closed = False self._collision_handles = {} self._objects = {} self.pyrep = PyRep() self.pyrep.launch(scene, headless, responsive_ui, blocking) if start: self.start_simulation() def close(self): if not self._closed: self.pyrep.shutdown() self._closed = True def load_scene(self, scene_path, start=False): """ Loads a scene on the CoppeliaSim server. :param str scene_path: path to a CoppeliaSim scene file :param bool start: whether to directly start the simulation after loading the scene .. note:: It is assumed that the scene file is always available on the server side. """ self.stop_simulation() if not os.path.exists(scene_path): raise IOError("No such file or directory: '{}'".format(scene_path)) sim.simLoadScene(scene_path) if start: self.start_simulation() def start_simulation(self): """ Starts the simulation. """ self.pyrep.start() def restart_simulation(self): """ Re-starts the simulation. """ self.stop_simulation() self.start_simulation() def stop_simulation(self): """ Stops the simulation. """ self.pyrep.stop() def pause_simulation(self): """ Pauses the simulation. """ sim.simPauseSimulation() def resume_simulation(self): """ Resumes the simulation. """ self.start_simulation() def set_simulation_timestep(self, dt): """Sets the simulation time step. :param dt: The time step value. """ self.pyrep.set_simulation_timestep(dt) def get_simulation_state(self): """Retrieves current simulation state""" return lib.simGetSimulationState() def simulation_step(self): """Execute the next simulation step. If the physics simulation is not running, then this will only update the UI. """ self.pyrep.step() def ui_step(self): """Update the UI. This will not execute the next simulation step, even if the physics simulation is running. This is only applicable when PyRep was launched without a responsive UI. """ self.pyrep.step_ui() def get_motor_position(self, motor_name): """ Gets the motor current position. """ joint = self.get_object(motor_name) return joint.get_joint_position() def set_motor_position(self, motor_name, position): """ Sets the motor target position. """ joint = self.get_object(motor_name) joint.set_joint_target_position(position) def get_motor_force(self, motor_name): """ Retrieves the force or torque applied to a joint along/about its active axis. """ joint = self.get_object(motor_name) return joint.get_joint_force() def set_motor_force(self, motor_name, force): """ Sets the maximum force or torque that a joint can exert. """ joint = self.get_object(motor_name) joint.set_joint_force(force) def get_motor_limits(self, motor_name): """ Retrieves joint limits """ joint = self.get_object(motor_name) _, interval = joint.get_joint_interval() return interval[0], sum(interval) def get_object_position(self, object_name, relative_to_object=None): """ Gets the object position. """ scene_object = self.get_object(object_name) if relative_to_object is not None: relative_to_object = self.get_object(relative_to_object) return scene_object.get_position(relative_to_object) def set_object_position(self, object_name, position=[0, 0, 0]): """ Sets the object position. """ scene_object = self.get_object(object_name) scene_object.set_position(position) def get_object_orientation(self, object_name, relative_to_object=None): """ Gets the object orientation. """ scene_object = self.get_object(object_name) if relative_to_object is not None: relative_to_object = self.get_object(relative_to_object) return scene_object.get_orientation(relative_to_object) def get_object_handle(self, name): """ Gets the coppelia sim object handle. """ scene_object = self.get_object(name) return scene_object.get_handle() def get_collision_state(self, collision_name): """ Gets the collision state. """ return sim.simReadCollision(self.get_collision_handle(collision_name)) def get_collision_handle(self, collision): """ Gets a coppelia sim collisions handle. """ if collision not in self._collision_handles: h = sim.simGetCollisionHandle(collision) self._collision_handles[collision] = h return self._collision_handles[collision] def get_simulation_current_time(self): """ Gets the simulation current time. """ try: return lib.simGetSimulationTime() except CoppeliaIOErrors: return 0.0 def add_cube( self, name, size, mass=1.0, backface_culling=False, visible_edges=False, smooth=False, respondable=True, static=False, renderable=True, position=None, orientation=None, color=None, ): """ Add Cube :param name: Name of the created object. :param size: A list of the x, y, z dimensions. :param mass: A float representing the mass of the object. :param backface_culling: If backface culling is enabled. :param visible_edges: If the object will have visible edges. :param smooth: If the shape appears smooth. :param respondable: Shape is responsible. :param static: If the shape is static. :param renderable: If the shape is renderable. :param position: The x, y, z position. :param orientation: The z, y, z orientation (in radians). :param color: The r, g, b values of the shape. """ self._create_pure_shape( name, PrimitiveShape.CUBOID, size, mass, backface_culling, visible_edges, smooth, respondable, static, renderable, position, orientation, color, ) def add_sphere( self, name, size, mass=1.0, backface_culling=False, visible_edges=False, smooth=False, respondable=True, static=False, renderable=True, position=None, orientation=None, color=None, ): """ Add Sphere :param name: Name of the created object. :param size: A list of the x, y, z dimensions. :param mass: A float representing the mass of the object. :param backface_culling: If backface culling is enabled. :param visible_edges: If the object will have visible edges. :param smooth: If the shape appears smooth. :param respondable: Shape is responsible. :param static: If the shape is static. :param renderable: If the shape is renderable. :param position: The x, y, z position. :param orientation: The z, y, z orientation (in radians). :param color: The r, g, b values of the shape. """ self._create_pure_shape( name, PrimitiveShape.SPHERE, size, mass, backface_culling, visible_edges, smooth, respondable, static, renderable, position, orientation, color, ) def add_cylinder( self, name, size, mass=1.0, backface_culling=False, visible_edges=False, smooth=False, respondable=True, static=False, renderable=True, position=None, orientation=None, color=None, ): """ Add Cylinder :param name: Name of the created object. :param size: A list of the x, y, z dimensions. :param mass: A float representing the mass of the object. :param backface_culling: If backface culling is enabled. :param visible_edges: If the object will have visible edges. :param smooth: If the shape appears smooth. :param respondable: Shape is responsible. :param static: If the shape is static. :param renderable: If the shape is renderable. :param position: The x, y, z position. :param orientation: The z, y, z orientation (in radians). :param color: The r, g, b values of the shape. """ self._create_pure_shape( name, PrimitiveShape.CYLINDER, size, mass, backface_culling, visible_edges, smooth, respondable, static, renderable, position, orientation, color, ) def add_cone( self, name, size, mass=1.0, backface_culling=False, visible_edges=False, smooth=False, respondable=True, static=False, renderable=True, position=None, orientation=None, color=None, ): """ Add Cone :param name: Name of the created object. :param size: A list of the x, y, z dimensions. :param mass: A float representing the mass of the object. :param backface_culling: If backface culling is enabled. :param visible_edges: If the object will have visible edges. :param smooth: If the shape appears smooth. :param respondable: Shape is responsible. :param static: If the shape is static. :param renderable: If the shape is renderable. :param position: The x, y, z position. :param orientation: The z, y, z orientation (in radians). :param color: The r, g, b values of the shape. """ self._create_pure_shape( name, PrimitiveShape.CONE, size, mass, backface_culling, visible_edges, smooth, respondable, static, renderable, position, orientation, color, ) def change_object_name(self, obj, new_name): """ Change object name """ old_name = obj.get_name() if old_name in self._objects: self._objects.pop(old_name) obj.set_name(new_name) def _create_pure_shape( self, name, primitive_type, size, mass=1.0, backface_culling=False, visible_edges=False, smooth=False, respondable=True, static=False, renderable=True, position=None, orientation=None, color=None, ): """ Create Pure Shape :param name: Name of the created object. :param primitive_type: The type of primitive to shape. One of: PrimitiveShape.CUBOID PrimitiveShape.SPHERE PrimitiveShape.CYLINDER PrimitiveShape.CONE :param size: A list of the x, y, z dimensions. :param mass: A float representing the mass of the object. :param backface_culling: If backface culling is enabled. :param visible_edges: If the object will have visible edges. :param smooth: If the shape appears smooth. :param respondable: Shape is responsible. :param static: If the shape is static. :param renderable: If the shape is renderable. :param position: The x, y, z position. :param orientation: The z, y, z orientation (in radians). :param color: The r, g, b values of the shape. """ obj = Shape.create( primitive_type, size, mass, backface_culling, visible_edges, smooth, respondable, static, renderable, position, orientation, color, ) self.change_object_name(obj, name) def _create_object(self, name): """Creates pyrep object for the correct type""" # TODO implement other types handle = sim.simGetObjectHandle(name) o_type = sim.simGetObjectType(handle) if ObjectType(o_type) == ObjectType.JOINT: return Joint(handle) elif ObjectType(o_type) == ObjectType.SHAPE: return Shape(handle) else: return None def get_object(self, name): """ Gets CoppeliaSim scene object""" if name not in self._objects: self._objects[name] = self._create_object(name) return self._objects[name]
if __name__ == '__main__': parser = argparse.ArgumentParser() parser.add_argument("task", help="The task file to test.") args = parser.parse_args() python_file = os.path.join(TASKS_PATH, args.task) if not os.path.isfile(python_file): raise RuntimeError('Could not find the task file: %s' % python_file) task_class = task_file_to_task_class(args.task) DIR_PATH = os.path.dirname(os.path.abspath(__file__)) sim = PyRep() ttt_file = os.path.join(DIR_PATH, '..', 'rlbench', TTT_FILE) sim.launch(ttt_file, headless=True) sim.step_ui() sim.set_simulation_timestep(0.005) sim.step_ui() sim.start() robot = Robot(Panda(), PandaGripper()) active_task = task_class(sim, robot) obs = ObservationConfig() obs.set_all(False) scene = Scene(sim, robot, obs) try: task_smoke(active_task, scene, variation=2) except TaskValidationError as e: sim.shutdown()
class SpecificWorker(GenericWorker): def __init__(self, proxy_map): super(SpecificWorker, self).__init__(proxy_map) def __del__(self): print('SpecificWorker destructor') def setParams(self, params): SCENE_FILE = '../../etc/basic_scene.ttt' self.pr = PyRep() self.pr.launch(SCENE_FILE, headless=False) self.pr.start() self.robot = TurtleBot() self.drone = Shape('Quadricopter') cam = VisionSensor("frontCamera") self.camera = { "handle": cam, "id": 0, "angle": np.radians(cam.get_perspective_angle()), "width": cam.get_resolution()[0], "height": cam.get_resolution()[1], "focal": (cam.get_resolution()[0] / 2) / np.tan(np.radians(cam.get_perspective_angle() / 2)), "rgb": np.array(0), "depth": np.ndarray(0) } self.joystick_newdata = [] self.speed_robot = [] self.speed_robot_ant = [] self.last_received_data_time = 0 self.once = False #@QtCore.Slot() def compute(self): cont = 0 start = time.time() while True: self.pr.step() self.read_camera(self.camera) self.read_joystick() elapsed = time.time() - start if elapsed < 0.05: time.sleep(0.05 - elapsed) cont += 1 if elapsed > 1: print("Freq -> ", cont) cont = 0 start = time.time() ########################################### def read_camera(self, cam): image_float = cam["handle"].capture_rgb() depth = cam["handle"].capture_depth(True) image = cv2.normalize(src=image_float, dst=None, alpha=0, beta=255, norm_type=cv2.NORM_MINMAX, dtype=cv2.CV_8U) cam["rgb"] = RoboCompCameraRGBDSimple.TImage(cameraID=cam["id"], width=cam["width"], height=cam["height"], depth=3, focalx=cam["focal"], focaly=cam["focal"], alivetime=time.time(), image=image.tobytes()) cam["depth"] = RoboCompCameraRGBDSimple.TDepth( cameraID=cam["id"], width=cam["handle"].get_resolution()[0], height=cam["handle"].get_resolution()[1], focalx=cam["focal"], focaly=cam["focal"], alivetime=time.time(), depthFactor=1.0, depth=depth.tobytes()) # try: # self.camerargbdsimplepub_proxy.pushRGBD(cam["rgb"], cam["depth"]) # except Ice.Exception as e: # print(e) ########################################### ### JOYSITCK read and move the robot ########################################### def read_joystick(self): if self.joystick_newdata: #and (time.time() - self.joystick_newdata[1]) > 0.1: datos = self.joystick_newdata[0] adv = 0.0 rot = 0.0 side = 0.0 height = 0.0 for x in datos.axes: if x.name == "advance": adv = x.value / 20 if np.abs(x.value) > 0.001 else 0 if x.name == "rotate": side = x.value / 20 if np.abs(x.value) > 0.001 else 0 if x.name == "tilt": height = x.value / 20 if np.abs(x.value) > 0.001 else 0 if x.name == "side": rot = x.value / 15 if np.abs(x.value) > 0.001 else 0 print("Joystick ", adv, side, height, rot) self.move_quad_target([adv, side, height, rot]) self.joystick_newdata = None self.last_received_data_time = time.time() ################################################################################# def move_quad_target(self, vels): target = Shape('Quadricopter_target') adv, side, height, rot = vels current_pos = target.get_position(self.drone) current_ori = target.get_orientation(self.drone) target.set_position([ current_pos[0] - adv, current_pos[1] - side, current_pos[2] - height ], self.drone) target.set_orientation([ current_ori[0] - adv, current_ori[1] - side, current_ori[2] - rot ], self.drone) ################################################################################## # SUBSCRIPTION to sendData method from JoystickAdapter interface ################################################################################### def JoystickAdapter_sendData(self, data): self.joystick_newdata = [data, time.time()] ################################################################################## # Methods for CameraRGBDSimple # =============================================================================== # # getAll # def CameraRGBDSimple_getAll(self, camera): return RoboCompCameraRGBDSimple.TRGBD(self.cameras[camera]["rgb"], self.cameras[camera]["depth"]) # # getDepth # def CameraRGBDSimple_getDepth(self, camera): return self.cameras[camera]["depth"] # # getImage # def CameraRGBDSimple_getImage(self, camera): return self.cameras[camera]["rgb"]
"""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 shape.set_position([x, y, shape.get_position()[2]]) shape.set_orientation([0, 0, yaw]) # Get Scene SCENE_FILE = join(dirname(abspath(__file__)), '../scenes/scene_cpp.ttt') # Set numpy printing options np.set_printoptions(threshold=(100*100), formatter={'all':lambda x: str(x) + ','}) # Start Simulation pr = PyRep() pr.launch(SCENE_FILE, headless=True) pr.start() robot = Shape('robot') goal = Shape('goal_pose') robot.set_renderable(False) goal.set_renderable(False) vision_sensor = VisionSensor('vision_sensor') # Since our "robot" is actually a shape we need to define our own 2d pose func # current_pose = get_2d_pose(robot) target_pose = get_2d_pose(goal) current_pose = robot.get_position(vision_sensor)[:2] + robot.get_orientation()[-1:] current_pose = (-current_pose[0], current_pose[1], current_pose[2])
class TestArmsAndConfigurationPaths(TestCore): def setUp(self): self.pyrep = PyRep() self.pyrep.launch(path.join( ASSET_DIR, 'test_scene_robots.ttt'), headless=True) self.pyrep.step() self.pyrep.start() # It is enough to only test the constructor of each arm (in there we make # assumptions about the structure of the arm model). All other tests # can be run on one arm. def test_get_arm(self): for arm_name, arm_type in ARMS: with self.subTest(arm=arm_name): arm = arm_type() self.assertIsInstance(arm, arm_type) def test_set_ik_element_properties(self): arm = Panda() arm.set_ik_element_properties(constraint_gamma=False) arm.set_ik_element_properties() def test_get_configs_for_tip_pose(self): arm = Panda() waypoint = Dummy('Panda_waypoint') configs = arm.get_configs_for_tip_pose( waypoint.get_position(), waypoint.get_orientation()) self.assertIsNotNone(configs) current_config = arm.get_joint_positions() # Checks correct config (last) arm.set_joint_positions(configs[-1]) self.assertTrue(np.allclose( arm.get_tip().get_pose(), waypoint.get_pose(), atol=0.001)) # Checks correct config (first) arm.set_joint_positions(configs[0]) self.assertTrue(np.allclose( arm.get_tip().get_pose(), waypoint.get_pose(), atol=0.001)) # Checks order prev_config_dist = 0 for config in configs: config_dist = sum( [(c - f)**2 for c, f in zip(current_config, config)]) # This test requires that the metric scale for each joint remains at # 1.0 in _getConfigDistance lua function self.assertLessEqual(prev_config_dist, config_dist) prev_config_dist = config_dist def test_get_path_from_cartesian_path(self): arm = Panda() cartesian_path = CartesianPath('Panda_cartesian_path') path = arm.get_path_from_cartesian_path(cartesian_path) self.assertIsNotNone(path) def test_get_linear_path(self): arm = Panda() waypoint = Dummy('Panda_waypoint') path = arm.get_linear_path( waypoint.get_position(), waypoint.get_orientation()) self.assertIsNotNone(path) def test_get_nonlinear_path(self): arm = Panda() waypoint = Dummy('Panda_waypoint') path = arm.get_nonlinear_path( waypoint.get_position(), waypoint.get_orientation()) self.assertIsNotNone(path) def test_get_nonlinear_path_out_of_reach(self): arm = Panda() with self.assertRaises(ConfigurationPathError): arm.get_nonlinear_path([99, 99, 99], [0.] * 3) def test_get_linear_path_out_of_reach(self): arm = Panda() with self.assertRaises(ConfigurationPathError): arm.get_linear_path([99, 99, 99], [0.] * 3) def test_get_linear_path_and_step(self): arm = Panda() waypoint = Dummy('Panda_waypoint') path = arm.get_linear_path( waypoint.get_position(), waypoint.get_orientation()) self.assertIsNotNone(path) done = False while not done: done = path.step() self.pyrep.step() self.assertTrue(np.allclose( arm.get_tip().get_position(), waypoint.get_position(), atol=0.01)) def test_get_linear_path_and_get_end(self): arm = Panda() waypoint = Dummy('Panda_waypoint') path = arm.get_linear_path( waypoint.get_position(), waypoint.get_orientation()) path.set_to_end() self.assertTrue(np.allclose( arm.get_tip().get_position(), waypoint.get_position(), atol=0.001)) def test_get_linear_path_visualize(self): arm = Panda() waypoint = Dummy('Panda_waypoint') path = arm.get_linear_path( waypoint.get_position(), waypoint.get_orientation()) # Check that it does not error path.visualize() def test_get_duplicate_arm(self): arm = UR3(1) self.assertIsInstance(arm, UR3) def test_copy_arm(self): arm = UR10() new_arm = arm.copy() self.assertNotEqual(arm, new_arm) def test_get_jacobian(self): arm = Panda() jacobian = arm.get_jacobian() self.assertEqual(jacobian.shape, (7, 6))
class Environment(object): """Each environment has a scene.""" def __init__( self, action_mode: ActionMode, dataset_root: str = '', obs_config=ObservationConfig(), headless=False, static_positions: bool = False, robot_configuration='panda', randomize_every: RandomizeEvery = None, frequency: int = 1, visual_randomization_config: VisualRandomizationConfig = None, dynamics_randomization_config: DynamicsRandomizationConfig = None, attach_grasped_objects: bool = True): self._dataset_root = dataset_root self._action_mode = action_mode self._obs_config = obs_config self._headless = headless self._static_positions = static_positions self._robot_configuration = robot_configuration self._randomize_every = randomize_every self._frequency = frequency self._visual_randomization_config = visual_randomization_config self._dynamics_randomization_config = dynamics_randomization_config self._attach_grasped_objects = attach_grasped_objects if robot_configuration not in SUPPORTED_ROBOTS.keys(): raise ValueError('robot_configuration must be one of %s' % str(SUPPORTED_ROBOTS.keys())) if (randomize_every is not None and visual_randomization_config is None and dynamics_randomization_config is None): raise ValueError( 'If domain randomization is enabled, must supply either ' 'visual_randomization_config or dynamics_randomization_config') self._check_dataset_structure() self._pyrep = None self._robot = None self._scene = None self._prev_task = None def _set_arm_control_action(self): self._robot.arm.set_control_loop_enabled(True) if (self._action_mode.arm == ArmActionMode.ABS_JOINT_VELOCITY or self._action_mode.arm == ArmActionMode.DELTA_JOINT_VELOCITY): self._robot.arm.set_control_loop_enabled(False) self._robot.arm.set_motor_locked_at_zero_velocity(True) elif (self._action_mode.arm == ArmActionMode.ABS_JOINT_POSITION or self._action_mode.arm == ArmActionMode.DELTA_JOINT_POSITION or self._action_mode.arm == ArmActionMode.ABS_EE_POSE_WORLD_FRAME or self._action_mode.arm == ArmActionMode.DELTA_EE_POSE_WORLD_FRAME or self._action_mode.arm == ArmActionMode.ABS_EE_POSE_PLAN_WORLD_FRAME or self._action_mode.arm == ArmActionMode.DELTA_EE_POSE_PLAN_WORLD_FRAME or self._action_mode.arm == ArmActionMode.EE_POSE_PLAN_EE_FRAME or self._action_mode.arm == ArmActionMode.EE_POSE_EE_FRAME): self._robot.arm.set_control_loop_enabled(True) elif (self._action_mode.arm == ArmActionMode.ABS_JOINT_TORQUE or self._action_mode.arm == ArmActionMode.DELTA_JOINT_TORQUE): self._robot.arm.set_control_loop_enabled(False) else: raise RuntimeError('Unrecognised action mode.') def _check_dataset_structure(self): if len(self._dataset_root) > 0 and not exists(self._dataset_root): raise RuntimeError('Data set root does not exists: %s' % self._dataset_root) def _string_to_task(self, task_name: str): task_name = task_name.replace('.py', '') try: class_name = ''.join( [w[0].upper() + w[1:] for w in task_name.split('_')]) mod = importlib.import_module("rlbench.tasks.%s" % task_name) except Exception as e: raise RuntimeError( 'Tried to interpret %s as a task, but failed. Only valid tasks ' 'should belong in the tasks/ folder' % task_name) from e return getattr(mod, class_name) def launch(self): if self._pyrep is not None: raise RuntimeError('Already called launch!') self._pyrep = PyRep() self._pyrep.launch(join(DIR_PATH, TTT_FILE), headless=self._headless) arm_class, gripper_class, _ = SUPPORTED_ROBOTS[ self._robot_configuration] # We assume the panda is already loaded in the scene. if self._robot_configuration is not 'panda': # Remove the panda from the scene panda_arm = Panda() panda_pos = panda_arm.get_position() panda_arm.remove() arm_path = join(DIR_PATH, 'robot_ttms', self._robot_configuration + '.ttm') self._pyrep.import_model(arm_path) arm, gripper = arm_class(), gripper_class() arm.set_position(panda_pos) else: arm, gripper = arm_class(), gripper_class() self._robot = Robot(arm, gripper) if self._randomize_every is None: self._scene = Scene(self._pyrep, self._robot, self._obs_config) else: self._scene = DomainRandomizationScene( self._pyrep, self._robot, self._obs_config, self._randomize_every, self._frequency, self._visual_randomization_config, self._dynamics_randomization_config) self._set_arm_control_action() def shutdown(self): if self._pyrep is not None: self._pyrep.shutdown() self._pyrep = None def get_task(self, task_class: Type[Task]) -> TaskEnvironment: # If user hasn't called launch, implicitly call it. if self._pyrep is None: self.launch() self._scene.unload() task = task_class(self._pyrep, self._robot) self._prev_task = task return TaskEnvironment(self._pyrep, self._robot, self._scene, task, self._action_mode, self._dataset_root, self._obs_config, self._static_positions, self._attach_grasped_objects) @property def action_size(self): arm_action_size = 0 gripper_action_size = 1 # Only one gripper style atm if (self._action_mode.arm == ArmActionMode.ABS_JOINT_VELOCITY or self._action_mode.arm == ArmActionMode.DELTA_JOINT_VELOCITY or self._action_mode.arm == ArmActionMode.ABS_JOINT_POSITION or self._action_mode.arm == ArmActionMode.DELTA_JOINT_POSITION or self._action_mode.arm == ArmActionMode.ABS_JOINT_TORQUE or self._action_mode.arm == ArmActionMode.DELTA_JOINT_TORQUE): arm_action_size = SUPPORTED_ROBOTS[self._robot_configuration][2] elif (self._action_mode.arm == ArmActionMode.ABS_EE_POSE_WORLD_FRAME or self._action_mode.arm == ArmActionMode.DELTA_EE_POSE_WORLD_FRAME or self._action_mode.arm == ArmActionMode.ABS_EE_POSE_PLAN_WORLD_FRAME or self._action_mode.arm == ArmActionMode.DELTA_EE_POSE_PLAN_WORLD_FRAME or self._action_mode.arm == ArmActionMode.EE_POSE_PLAN_EE_FRAME or self._action_mode.arm == ArmActionMode.EE_POSE_EE_FRAME): arm_action_size = 7 # pose is always 7 return arm_action_size + gripper_action_size def get_demos(self, task_name: str, amount: int, variation_number=0, image_paths=False) -> List[Demo]: if self._dataset_root is None or len(self._dataset_root) == 0: raise RuntimeError( "Can't ask for a stored demo when no dataset root provided.") demos = utils.get_stored_demos(amount, image_paths, self._dataset_root, variation_number, task_name, self._obs_config) return demos
- Linear (IK) paths. - Scene manipulation (creating an object and moving it). """ from os.path import dirname, join, abspath from pyrep import PyRep from pyrep.robots.arms.panda import Panda from pyrep.objects.shape import Shape from pyrep.const import PrimitiveShape from pyrep.errors import ConfigurationPathError import numpy as np import math LOOPS = 10 SCENE_FILE = join(dirname(abspath(__file__)), 'panda_reach_target.ttt') pr = PyRep() pr.launch(SCENE_FILE, headless=False) pr.start() agent = Panda() # We could have made this target in the scene, but lets create one dynamically target = Shape.create(type=PrimitiveShape.SPHERE, size=[0.05, 0.05, 0.05], color=[1.0, 0.1, 0.1], static=True, respondable=False) position_min, position_max = [0.8, -0.2, 1.0], [1.0, 0.2, 1.4] starting_joint_positions = agent.get_joint_positions() for i in range(LOOPS):
class EnvPollos(Env): def __init__(self, ep_length=100): """ Pollos environment for testing purposes :param dim: (int) the size of the dimensions you want to learn :param ep_length: (int) the length of each episodes in timesteps """ self.vrep = vrep logging.basicConfig(level=logging.DEBUG) # they have to be simmetric. We interpret actions as angular increments #self.action_space = Box(low=np.array([-0.1, -1.7, -2.5, -1.5, 0.0, 0.0]), # high=np.array([0.1, 1.7, 2.5, 1.5, 0.0, 0.0])) inc = 0.1 self.action_space = Box(low=np.array([-inc, -inc, -inc, -inc, 0, -inc]), high=np.array([inc, inc, inc, inc, 0, inc])) self.observation_space = Box(low=np.array([-0.5, 0.0, -0.2]), high=np.array([0.5, 1.0, 1.0])) # self.pr = PyRep() self.pr.launch(SCENE_FILE, headless=False) self.pr.start() self.agent = UR10() self.agent.max_velocity = 1 self.agent.set_control_loop_enabled(True) self.agent.set_motor_locked_at_zero_velocity(True) self.joints = [ Joint('UR10_joint1'), Joint('UR10_joint2'), Joint('UR10_joint3'), Joint('UR10_joint4'), Joint('UR10_joint5'), Joint('UR10_joint6') ] #self.joints_limits = [[j.get_joint_interval()[1][0],j.get_joint_interval()[1][0]+j.get_joint_interval()[1][1]] # for j in self.joints] self.high_joints_limits = [0.1, 1.7, 2.7, 0.0, 0.02, 0.3] self.low_joints_limits = [-0.1, -0.2, 0.0, -1.5, -0.02, -0.5] #self.agent_hand = Shape('hand') self.initial_agent_tip_position = self.agent.get_tip().get_position() self.initial_agent_tip_quaternion = self.agent.get_tip( ).get_quaternion() self.initial_agent_tip_euler = self.agent.get_tip().get_orientation() self.target = Dummy('UR10_target') self.initial_target_orientation = self.target.get_orientation() self.initial_joint_positions = self.agent.get_joint_positions() self.pollo_target = Dummy('pollo_target') self.pollo = Shape('pollo') #self.table_target = Dummy('table_target') self.initial_pollo_position = self.pollo.get_position() self.initial_pollo_orientation = self.pollo.get_quaternion() #self.table_target = Dummy('table_target') #self.succion = Shape('suctionPad') self.waypoints = [Dummy('dummy_gancho%d' % i) for i in range(4)] # objects #self.scene_objects = [Shape('table0'), Shape('Plane'), Shape('Plane0'), Shape('ConcretBlock')] #self.agent_tip = self.agent.get_tip() self.initial_distance = np.linalg.norm( np.array(self.initial_pollo_position) - np.array(self.initial_agent_tip_position)) # camera self.camera = VisionSensor('kinect_depth') self.camera_matrix_extrinsics = vrep.simGetObjectMatrix( self.camera.get_handle(), -1) self.np_camera_matrix_extrinsics = np.delete( np.linalg.inv(self.vrepToNP(self.camera_matrix_extrinsics)), 3, 0) width = 640.0 height = 480.0 angle = math.radians(57.0) focalx_px = (width / 2.0) / math.tan(angle / 2.0) focaly_px = (height / 2.0) / math.tan(angle / 2.0) self.np_camera_matrix_intrinsics = np.array([[-focalx_px, 0.0, 320.0], [0.0, -focalx_px, 240.0], [0.0, 0.0, 1.0]]) self.reset() def reset(self): pos = list( np.random.uniform([-0.1, -0.1, 0.0], [0.1, 0.1, 0.1]) + self.initial_pollo_position) self.pollo.set_position(pos) self.pollo.set_quaternion(self.initial_pollo_orientation) self.agent.set_joint_positions(self.initial_joint_positions) self.initial_epoch_time = time.time() while True: # wait for arm to stop self.pr.step() # Step the physics simulation a = self.agent.get_joint_velocities() if not np.any(np.where(np.fabs(a) < 0.1, False, True)): break print("ENV RESET DONE") return self._get_state() def step(self, action): if action is None: self.pr.step() return self._get_state(), 0.0, False, {} # check for nan if np.any(np.isnan(action)): print(action) return self._get_state(), -1.0, False, {} # action[1] = np.interp(action[1], [-1,7,1.7], [-0.2,1.7]) # action[2] = np.interp(action[2], [-2.5,2.5], [0,2.5]) # action[3] = np.interp(action[3], [-1.5,1.5], [-1.5,0]) # add actions as increments to current joints value new_joints = np.array( self.agent.get_joint_positions()) + np.array(action) # check limits if np.any(np.greater(new_joints, self.high_joints_limits)) or np.any( np.less(new_joints, self.low_joints_limits)): logging.debug("New joints value out of limits r=-10") return self._get_state(), -10.0, True, {} # move the robot and wait to stop self.agent.set_joint_target_positions(new_joints) reloj = time.time() while True: # wait for arm to stop self.pr.step() # Step the physics simulation a = self.agent.get_joint_velocities() if not np.any(np.where(np.fabs(a) < 0.1, False, True)) or (time.time() - reloj) > 3: break # compute relevant magnitudes np_pollo_target = np.array(self.pollo_target.get_position()) np_robot_tip_position = np.array(self.agent.get_tip().get_position()) dist = np.linalg.norm(np_pollo_target - np_robot_tip_position) altura = np.clip((np_pollo_target[2] - self.initial_pollo_position[2]), 0, 0.5) for obj in self.scene_objects: # colisión con la mesa if self.agent_hand.check_collision(obj): logging.debug("Collision with objects r=-10") return self._get_state(), -10.0, True, {} if altura > 0.3: # pollo en mano logging.debug("Height reached !!! r=0") return self._get_state(), 30.0, True, {} elif dist > self.initial_distance: # mano se aleja logging.debug("Hand moving away from chicken r=-10") return self._get_state(), -10.0, True, {} if time.time() - self.initial_epoch_time > 5: # too much time logging.debug("Time out. End of epoch r=-10") return self._get_state(), -10.0, True, {} # Reward #pollo_height = np.exp(-altura*20) # para 0.3m pollo_height = 0.002, para 0m pollo_height = 1 reward = altura - dist logging.debug("New joints value out of limits r=-10") return self._get_state(), reward, False, {} def close(self): self.pr.stop() self.pr.shutdown() def render(self): print("RENDER") np_pollo_en_camara = self.getPolloEnCamara() # capture depth image depth = self.camera.capture_rgb() circ = plt.Circle( (int(np_pollo_en_camara[0]), int(np_pollo_en_camara[1])), 10) plt.clf() fig = plt.gcf() ax = fig.gca() ax.add_artist(circ) ax.imshow(depth, cmap='hot') plt.pause(0.000001) # Aux # transform env.pollo_target.get_position() to camera coordinates and project pollo_en_camera a image coordinates def getPolloEnCamara(self): np_pollo_target = np.array(self.pollo_target.get_position()) np_pollo_target_cam = self.np_camera_matrix_extrinsics.dot( np.append([np_pollo_target], 1.0)) np_pollo_en_camara = self.np_camera_matrix_intrinsics.dot( np_pollo_target_cam) np_pollo_en_camara = np_pollo_en_camara / np_pollo_en_camara[2] np_pollo_en_camara = np.delete(np_pollo_en_camara, 2) return np_pollo_en_camara def getPolloEnCamaraEx(self): np_pollo_target = np.array(self.pollo_target.get_position()) np_pollo_en_camara = self.np_camera_matrix_extrinsics.dot( np.append([np_pollo_target], 1.0)) return np_pollo_en_camara def _get_state(self): # Return state containing arm joint angles/velocities & target position # return (self.agent.get_joint_positions() + # self.agent.get_joint_velocities() + # self.pollo_target.get_position()) p = self.getPolloEnCamaraEx() j = self.agent.get_joint_positions() #r = np.array([p[0],p[1],p[2],j[0],j[1],j[2],j[3],j[4],j[5]]) r = np.array([p[0], p[1], p[2]]) return r def vrepToNP(self, c): return np.array([[c[0], c[4], c[8], c[3]], [c[1], c[5], c[9], c[7]], [c[2], c[6], c[10], c[11]], [0, 0, 0, 1]])
class CoppeliaSimEnvWrapper(EnvironmentSpec): def __init__(self, visualize=True, mode="train", **params): super().__init__(visualize=visualize, mode=mode) # Scene selection scene_file_path = os.path.join(os.getcwd(), 'simulation/UR5.ttt') # Simulator launch self.env = PyRep() self.env.launch(scene_file_path, headless=False) self.env.start() self.env.step() # Task related initialisations in Simulator self.vision_sensor = objects.vision_sensor.VisionSensor( "Vision_sensor") self.gripper = objects.dummy.Dummy("UR5_target") self.gripper_zero_pose = self.gripper.get_pose() self.goal = objects.dummy.Dummy("goal_target") self.goal_STL = objects.shape.Shape("goal") self.goal_STL_zero_pose = self.goal_STL.get_pose() self.grasped_STL = objects.shape.Shape("Peg") self.stacking_area = objects.shape.Shape("Plane") self.vision_sensor = objects.vision_sensor.VisionSensor( "Vision_sensor") self.step_counter = 0 self.max_step_count = 100 self.target_pose = None self.initial_distance = None self.image_width, self.image_height = 320, 240 self.vision_sensor.set_resolution( (self.image_width, self.image_height)) self._history_len = 1 self._observation_space = Dict({ "cam_image": Box(0, 255, [self.image_height, self.image_width, 1], dtype=np.uint8) }) self._action_space = Box(-1, 1, (3, )) self._state_space = extend_space(self._observation_space, self._history_len) @property def history_len(self): return self._history_len @property def observation_space(self) -> Space: return self._observation_space @property def state_space(self) -> Space: return self._state_space @property def action_space(self) -> Space: return self._action_space def step(self, action): done = False info = {} prev_distance_to_goal = self.distance_to_goal() # Make a step in simulation self.apply_controls(action) self.env.step() self.step_counter += 1 # Reward calculations success_reward = self.success_check() distance_reward = (prev_distance_to_goal - self.distance_to_goal()) / self.initial_distance reward = distance_reward + success_reward # Check reset conditions if self.step_counter > self.max_step_count: done = True logging.info('--------Reset: Timeout--------') elif self.distance_to_goal() > 0.8: done = True logging.info('--------Reset: Too far from target--------') elif self.collision_check(): done = True logging.info('--------Reset: Collision--------') return self.get_observation(), reward, done, info def reset(self): logging.info("Episode reset...") self.step_counter = 0 self.env.stop() self.env.start() self.env.step() self.setup_scene() observation = self.get_observation() return observation # -------------- all methods above are required for any Gym environment, everything below is env-specific -------------- def distance_to_goal(self): goal_pos = self.goal.get_position() tip_pos = self.gripper.get_position() return np.linalg.norm(np.array(tip_pos) - np.array(goal_pos)) def setup_goal(self): goal_position = self.goal_STL_zero_pose[:3] # 2D goal randomization self.target_pose = [ goal_position[0] + (2 * np.random.rand() - 1.) * 0.1, goal_position[1] + (2 * np.random.rand() - 1.) * 0.1, goal_position[2] ] self.target_pose = np.append(self.target_pose, self.goal_STL_zero_pose[3:]).tolist() self.goal_STL.set_pose(self.target_pose) # Randomizing the RGB of the goal and the plane rgb_values_goal = list(np.random.rand(3, )) rgb_values_plane = list(np.random.rand(3, )) self.goal_STL.set_color(rgb_values_goal) self.stacking_area.set_color(rgb_values_plane) self.initial_distance = self.distance_to_goal() def setup_scene(self): self.setup_goal() self.gripper.set_pose(self.gripper_zero_pose) def get_observation(self): cam_image = self.vision_sensor.capture_rgb() gray_image = np.uint8( cv2.cvtColor(cam_image, cv2.COLOR_BGR2GRAY) * 255) obs_image = np.expand_dims(gray_image, axis=2) return {"cam_image": obs_image} def collision_check(self): return self.grasped_STL.check_collision( self.stacking_area) or self.grasped_STL.check_collision( self.goal_STL) def success_check(self): success_reward = 0. if self.distance_to_goal() < 0.01: success_reward = 1 logging.info('--------Success state--------') return success_reward def apply_controls(self, action): gripper_position = self.gripper.get_position() # predicted action is in range (-1, 1) so we are normalizing it to physical units new_position = [ gripper_position[i] + (action[i] / 200.) for i in range(3) ] self.gripper.set_position(new_position)
class ReacherEnv(object): def __init__(self, headless, control_mode='end_position', visual_control=False): ''' :visual_control: bool, controlled by visual state or not (vector state). ''' self.reward_offset = 10.0 self.reward_range = self.reward_offset # reward range for register gym env when using vectorized env wrapper self.fall_down_offset = 0.1 # for judging the target object fall off the table self.metadata = [] # gym env format self.visual_control = visual_control self.control_mode = control_mode self.pr = PyRep() if control_mode == 'end_position': # need to use different scene, the one with all joints in inverse kinematics mode SCENE_FILE = join(dirname(abspath(__file__)), './scenes/sawyer_reacher_rl_new_ik.ttt') elif control_mode == 'joint_velocity': # the scene with all joints in force/torche mode for forward kinematics SCENE_FILE = join(dirname(abspath(__file__)), './scenes/sawyer_reacher_rl_new.ttt') self.pr.launch(SCENE_FILE, headless=headless) self.pr.start() self.agent = Sawyer() self.gripper = BaxterGripper() self.gripper_left_pad = Shape( 'BaxterGripper_leftPad') # the pad on the gripper finger self.proximity_sensor = ProximitySensor( 'BaxterGripper_attachProxSensor' ) # need the name of the sensor here self.vision_sensor = VisionSensor( 'Vision_sensor') # need the name of the sensor here if control_mode == 'end_position': self.agent.set_control_loop_enabled(True) # if false, won't work self.action_space = np.zeros( 4) # 3 DOF end position control + 1 rotation of gripper elif control_mode == 'joint_velocity': self.agent.set_control_loop_enabled(False) self.action_space = np.zeros( 8) # 7 DOF velocity control + 1 rotation of gripper else: raise NotImplementedError if self.visual_control == False: self.observation_space = np.zeros( 17 ) # position and velocity of 7 joints + position of the target else: self.observation_space = np.zeros(100) # dim of img! self.agent.set_motor_locked_at_zero_velocity(True) self.target = Shape('target') # object self.tip_target = Dummy( 'Sawyer_target') # the target point of the tip to move towards # self.table = Shape('diningTable') self.agent_ee_tip = self.agent.get_tip() self.tip_pos = self.agent_ee_tip.get_position() self.tip_quat = self.agent_ee_tip.get_quaternion( ) # tip rotation as quaternion, if not control the rotation # set a proper initial gesture/tip position if control_mode == 'end_position': # agent_position=self.agent.get_position() # initial_pos_offset = [0.4, 0.3, 0.2] # initial relative position of gripper to the whole arm # initial_pos = [(a + o) for (a, o) in zip(agent_position, initial_pos_offset)] initial_pos = [0.3, 0.1, 0.9] self.tip_target.set_position(initial_pos) # one big step for rotation setting is enough, with reset_dynamics=True, set the rotation instantaneously self.tip_target.set_orientation( [0, 3.1415, 1.5707], reset_dynamics=True) # make gripper face downwards self.pr.step() elif control_mode == 'joint_velocity': self.initial_joint_positions = [ 0.001815199851989746, -1.4224984645843506, 0.704303503036499, 2.54307222366333, 2.972468852996826, -0.4989511966705322, 4.105560302734375 ] self.agent.set_joint_positions(self.initial_joint_positions) self.initial_joint_positions = self.agent.get_joint_positions() self.initial_tip_positions = self.agent_ee_tip.get_position() self.initial_target_positions = self.target.get_position() def _get_state(self): # Return state containing arm joint angles/velocities & target position return np.array(self.agent.get_joint_positions() + self.agent.get_joint_velocities() + self.target.get_position()) def _is_holding(self): ''' Return is holding the target or not, return bool. ''' # Nothe that the collision check is not always accurate all the time, # for continuous conllision, maybe only the first 4-5 frames of collision can be detected pad_collide_object = self.gripper_left_pad.check_collision(self.target) if pad_collide_object and self.proximity_sensor.is_detected( self.target) == True: return True else: return False def _get_visual_state(self): # Return a numpy array of size (width, height, 3) return self.vision_sensor.capture_rgb( ) # A numpy array of size (width, height, 3) def _is_holding(self): # Return is holding the target or not, return bool # Nothe that the collision check is not always accurate all the time, # for continuous conllision, maybe only the first 4-5 frames of collision can be detected pad_collide_object = self.gripper_left_pad.check_collision(self.target) if pad_collide_object and self.proximity_sensor.is_detected( self.target) == True: return True else: return False # def _move(self, action): # ''' # Move the tip according to the action with inverse kinematics for 'end_position' control; # with control of tip target in inverse kinematics mode instead of using .solve_ik() in forward kinematics mode. # ''' # robot_moving_unit=0.01 # the amount of single step move of robot, not accurate # moving_loop_itr=int(np.sum(np.abs(action[:3]))/robot_moving_unit)+1 # adaptive number of moving steps, with minimal of 1 step. # # print(moving_loop_itr) # small_step = list(1./moving_loop_itr*np.array(action)) # break the action into small steps, as the robot cannot move to the target position within one frame # pos=self.agent_ee_tip.get_position() # ''' # there is a mismatch between the object set_orientation() and get_orientation(): # the (x,y,z) in set_orientation() will be (y,x,-z) in get_orientation(). # ''' # ori_z=-self.agent_ee_tip.get_orientation()[2] # the minus is because the mismatch between the set and get # assert len(small_step) == len(pos)+1 # 3 values for position, 1 value for rotation # for _ in range(moving_loop_itr): # for idx in range(len(pos)): # pos[idx] += small_step[idx] # self.tip_target.set_position(pos) # self.pr.step() # ''' deprecated! no need to use small steps for the rotation with reset_dynamics=True''' # # ori_z+=small_step[3] # change the orientation along z-axis with a small step # # self.tip_target.set_orientation([0,3.1415,ori_z], reset_dynamics=True) # make gripper face downwards # # self.pr.step() # ''' one big step for z-rotation is enough, with reset_dynamics=True, set the rotation instantaneously ''' # ori_z+=action[3] # self.tip_target.set_orientation([0,3.1415,ori_z], reset_dynamics=True) # make gripper face downwards # self.pr.step() def _move(self, action): ''' Move the tip according to the action with inverse kinematics for 'end_position' control; with control of tip target in inverse kinematics mode instead of using .solve_ik() in forward kinematics mode. Mode 2: a close-loop control, using ik. ''' pos = self.gripper.get_position() bounding_offset = 0.1 step_factor = 0.2 # small step factor mulitplied on the gradient step calculated by inverse kinematics max_itr = 20 # maximum moving iterations max_error = 0.1 # upper bound of distance error for movement at each call rotation_norm = 5. # factor for normalization of rotation values # check if state+action will be within of the bounding box, if so, move normally; else no action. # x_min < x < x_max and y_min < y < y_max and z > z_min if pos[0]+action[0]>POS_MIN[0]-bounding_offset and pos[0]+action[0]<POS_MAX[0]+bounding_offset \ and pos[1]+action[1] > POS_MIN[1]-bounding_offset and pos[1]+action[1] < POS_MAX[1]+bounding_offset \ and pos[2]+action[2] > POS_MIN[2]-bounding_offset: ''' there is a mismatch between the object set_orientation() and get_orientation(): the (x,y,z) in set_orientation() will be (y,x,-z) in get_orientation(). ''' ori_z = -self.agent_ee_tip.get_orientation()[ 2] # the minus is because the mismatch between the set and get target_pos = np.array(self.agent_ee_tip.get_position()) + np.array( action[:3]) diff = 1 itr = 0 # print('before: ', ori_z) while np.sum(np.abs(diff)) > max_error and itr < max_itr: itr += 1 # set pos in small step cur_pos = self.agent_ee_tip.get_position() diff = target_pos - cur_pos pos = cur_pos + step_factor * diff self.tip_target.set_position(pos.tolist()) self.pr.step() ''' one big step for z-rotation is enough ''' ori_z += rotation_norm * action[3] self.tip_target.set_orientation([0, np.pi, ori_z ]) # make gripper face downwards self.pr.step() else: # print("Potential Movement Out of the Bounding Box!") pass # no action if potentially out of the bounding box def reset(self): # Get a random position within a cuboid and set the target position max_itr = 10 pos = list(np.random.uniform(POS_MIN, POS_MAX)) self.target.set_position(pos) self.target.set_orientation([0, 0, 0]) self.pr.step() # changing the color or texture for domain randomization self.target.set_color( np.random.uniform(low=0, high=1, size=3).tolist() ) # set [r,g,b] 3 channel values of object color # set end position to be initialized if self.control_mode == 'end_position': # JointMode.IK self.agent.set_control_loop_enabled(True) self.tip_target.set_position( self.initial_tip_positions ) # cannot set joint positions directly due to in ik mode nor force/torch mode self.pr.step() # prevent stuck case itr = 0 while np.sum( np.abs( np.array(self.agent_ee_tip.get_position() - np.array(self.initial_tip_positions))) ) > 0.1 and itr < max_itr: itr += 1 self.step(np.random.uniform( -0.2, 0.2, 4)) # take random actions for preventing the stuck cases self.pr.step() elif self.control_mode == 'joint_velocity': # JointMode.FORCE self.agent.set_joint_positions( self.initial_joint_positions ) # sometimes the gripper is stuck, cannot get back to initial self.pr.step() # self.table.set_collidable(True) self.gripper_left_pad.set_collidable( True ) # set the pad on the gripper to be collidable, so as to check collision self.target.set_collidable(True) if np.sum(self.gripper.get_open_amount()) < 1.5: self.gripper.actuate(1, velocity=0.5) # open the gripper self.pr.step() if self.visual_control: return self._get_visual_state() else: return self._get_state() def step(self, action): ''' Move the robot arm according to the action. If control_mode=='joint_velocity', action is 7 dim of joint velocity values + 1 dim of gripper rotation. if control_mode=='end_position', action is 3 dim of tip (end of robot arm) position values + 1 dim of gripper rotation. ''' if self.control_mode == 'end_position': if action is None or action.shape[0] != 4: action = list(np.random.uniform(-0.1, 0.1, 4)) # random self._move(action) elif self.control_mode == 'joint_velocity': if action is None or action.shape[0] != 7: print('No actions or wrong action dimensions!') action = list(np.random.uniform(-0.1, 0.1, 7)) # random self.agent.set_joint_target_velocities( action) # Execute action on arm self.pr.step() else: raise NotImplementedError ax, ay, az = self.gripper.get_position() tx, ty, tz = self.target.get_position() # Reward is negative distance to target distance = (ax - tx)**2 + (ay - ty)**2 + (az - tz)**2 done = False # print(self.proximity_sensor.is_detected(self.target)) current_vision = self.vision_sensor.capture_rgb( ) # capture a screenshot of the view with vision sensor plt.imshow(current_vision) plt.savefig('./img/vision.png') reward = 0 # close the gripper if close enough to the object and the object is detected with the proximity sensor if distance < 0.1 and self.proximity_sensor.is_detected( self.target) == True: # make sure the gripper is open before grasping self.gripper.actuate(1, velocity=0.5) self.pr.step() self.gripper.actuate( 0, velocity=0.5 ) # if done, close the hand, 0 for close and 1 for open. self.pr.step() # Step the physics simulation if self._is_holding(): # reward for hold here! reward += 10 done = True else: self.gripper.actuate(1, velocity=0.5) self.pr.step() elif np.sum( self.gripper.get_open_amount() ) < 1.5: # if gripper is closed due to collision or esle, open it; .get_open_amount() return list of gripper joint values self.gripper.actuate(1, velocity=0.5) self.pr.step() else: pass reward -= np.sqrt(distance) if tz < self.initial_target_positions[ 2] - self.fall_down_offset: # the object fall off the table done = True reward = -self.reward_offset if self.visual_control: return self._get_visual_state(), reward, done, {} else: return self._get_state(), reward, done, {} def shutdown(self): self.pr.stop() self.pr.shutdown()
class GraspEnv(object): def __init__(self, headless, control_mode='joint_velocity'): self.headless = headless self.reward_offset = 10.0 self.reward_range = self.reward_offset self.penalty_offset = 1 #self.penalty_offset = 1. self.fall_down_offset = 0.1 self.metadata = [] #gym env argument self.control_mode = control_mode #launch and setup the scene, and set the proxy variables in present of the counterparts in the scene self.pr = PyRep() if control_mode == 'end_position': SCENE_FILE = join(dirname(abspath(__file__)), './scenes/UnicarRobot_ik.ttt') elif control_mode == 'joint_velocity': SCENE_FILE = join(dirname(abspath(__file__)), './scenes/UnicarRobot.ttt') self.pr.launch(SCENE_FILE, headless=headless) self.pr.start() self.agent = UnicarRobotArm() #drehkranz + UR10 #self.gripper = UnicarRobotGreifer()#suction #self.suction = UnicarRobotGreifer() self.suction = Shape("UnicarRobotGreifer_body_sub0") self.proximity_sensor = ProximitySensor('UnicarRobotGreifer_sensor') self.table = Shape('UnicarRobotTable') self.carbody = Shape('UnicarRobotCarbody') if control_mode == 'end_position': self.agent.set_control_loop_enabled(True) self.action_space = np.zeros(4) elif control_mode == 'joint_velocity': self.agent.set_control_loop_enabled(False) self.action_space = np.zeros(7) else: raise NotImplementedError #self.observation_space = np.zeros(17) self.observation_space = np.zeros(20) self.agent.set_motor_locked_at_zero_velocity(True) self.target = Shape("UnicarRobotTarget") #Box self.agent_ee_tip = self.agent.get_tip() self.tip_target = Dummy('UnicarRobotArm_target') self.tip_pos = self.agent_ee_tip.get_position() # set a proper initial robot gesture or tip position if control_mode == 'end_position': initial_pos = [0, 1.5, 1.6] self.tip_target.set_position(initial_pos) #one big step for rotatin is enough, with reset_dynamics = True, set the rotation instantaneously self.tip_target.set_orientation([0, 0, 0], reset_dynamics=True) elif control_mode == 'joint_velocity': self.initial_joint_positions = [0, 0, 0, 0, 0, 0, 0] self.agent.set_joint_positions(self.initial_joint_positions) self.pr.step() self.initial_tip_positions = self.agent_ee_tip.get_position() self.initial_target_positions = self.target.get_position() def _get_state(self): ''' Return state containging arm joint positions/velocities & target position. ''' return np.array(self.agent.get_joint_positions() + self.agent.get_joint_velocities() + self.agent_ee_tip.get_position() + self.agent_ee_tip.get_orientation()) #all 20 def _is_holding(self): ''' Return the state of holding the UnicarRobotTarget or not, return bool value ''' if self.proximity_sensor.is_detected(self.target) == True: return True else: return False def _move(self, action, bounding_offset=0.15, step_factor=0.2, max_itr=20, max_error=0.05, rotation_norm=5): pos = self.suction.get_position() if pos[0]+action[0] > POS_MIN[0]-bounding_offset and pos[0]+action[0] < POS_MAX[0]-bounding_offset \ and pos[1]+action[1] > POS_MIN[1]-bounding_offset and pos[1]+action[1] < POS_MAX[1]-bounding_offset \ and pos[2]+action[2] > POS_MIN[2]-2*bounding_offset: ori_z = -self.agent_ee_tip.get_orientation( )[2] # the minus is because the mismatch between the set_orientation() and get_orientation() #ori_z = self.agent_ee_tip.get_orientation()[2] target_pos = np.array(self.agent_ee_tip.get_position() + np.array(action[:3])) diff = 1 itr = 0 while np.sum(np.abs(diff)) > max_error and itr < max_itr: itr += 1 # set pos in small step cur_pos = self.agent_ee_tip.get_position() diff = target_pos - cur_pos pos = cur_pos + step_factor * diff self.tip_target.set_position(pos.tolist()) self.pr.step() ori_z += rotation_norm * action[3] self.tip_target.set_orientation([0, np.pi, ori_z]) self.pr.step() else: print("Potantial Movement Out of the Bounding Box!") self.pr.step() def reinit(self): self.shutdown() self.__init__(self.headless) def reset(self, random_target=False): ''' Get a random position within a cuboid and set the target position ''' # set target object if random_target: pos = list(np.random.uniform(POS_MIN, POS_MAX)) self.target.set_position(pos) else: self.target.set_position(self.initial_target_positions) self.target.set_orientation([0, 0, 0]) self.pr.step() #set end position to be initialized if self.control_mode == 'end_position': self.agent.set_control_loop_enabled(True) # IK mode self.tip_target.set_position(self.initial_tip_positions) self.pr.step() itr = 0 max_itr = 10 while np.sum( np.abs( np.array(self.agent_ee_tip.get_position() - np.array(self.initial_tip_positions))) ) > 0.1 and itr < max_itr: itr += 1 self.step(np.random.uniform(-0.2, 0.2, 4)) self.pr.step() elif self.control_mode == 'joint_velocity': #JointMode Force self.agent.set_joint_positions(self.initial_joint_positions) self.pr.step() #set collidable, for collision detection #self.gripper.set_collidable(True) self.suction.set_collidable(True) self.target.set_collidable(True) return self._get_state() #return the current state of the environment def step(self, action): ''' move the robot arm according to the control mode if control_mode == 'end_position' then action is 3 dim of tip (end of robot arm) position values + 1 dim rotation of suction if control_mode == 'joint_velocity' then action is 7 dim of joint velocity + 1 dim of rotation of suction ''' #initialization done = False #episode finishes reward = 0 hold_flag = False #hold the object or not if self.control_mode == 'end_position': if action is None or action.shape[0] != 4: #check action is valid print('No actions or wrong action dimensions!') action = list(np.random.uniform(-0.1, 0.1, 4)) self._move(action) elif self.control_mode == 'joint_velocity': if action is None or action.shape[0] != 7: #??? print('No actions or wrong action dimensions!') action = list(np.random.uniform(-1, 1, 7)) self.agent.set_joint_target_velocities(action) self.pr.step() else: raise NotImplementedError #ax,ay,az = self.gripper.get_position()#gripper position ax, ay, az = self.suction.get_position() #suction position if math.isnan( ax): #capture the broken suction cases during exploration print("Suction position is nan.") self.reinit() done = True desired_position_tip = [0.0, 1.5513, 1.74] desired_orientation_tip = [-np.pi, 0, 0.001567] tip_x, tip_y, tip_z = self.agent_ee_tip.get_position( ) #end_effector position tip_row, tip_pitch, tip_yaw = self.agent_ee_tip.get_orientation( ) #end_effector orientation tx, ty, tz = self.target.get_position() #box position offset = 0.312 #augmented reward: offset of target position above the target object #square distance between the gripper and the target object sqr_distance = np.sqrt((tip_x - desired_position_tip[0])**2 + (tip_y - desired_position_tip[1])**2 + (tip_z - desired_position_tip[2])**2) sqr_orientation = np.sqrt((tip_row - desired_orientation_tip[0])**2 + (tip_pitch - desired_orientation_tip[1])**2 + (tip_yaw - desired_orientation_tip[2])**2) ''' for visual-based control only, large time consumption! ''' # current_vision = self.vision_sensor.capture_rgb() # capture a screenshot of the view with vision sensor # plt.imshow(current_vision) # plt.savefig('./img/vision.png') desired_orientation = [0, 0, -np.pi / 2] desired_orientation_tip = [-np.pi, 0, 0.001567] #Enable the suction if close enough to the object and the object is detected with the proximity sensor if sqr_distance < 0.001 and self.proximity_sensor.is_detected( self.target) == True and sqr_orientation < 0.001: #make sure the suction is not worked self.suction.release() self.pr.step() self.suction.grasp(self.target) self.pr.step() if self._is_holding(): reward += self.reward_offset done = True hold_flag = True else: self.suction.release() self.pr.step() else: pass #the base reward is negative distance from suction to target #reward -= (np.sqrt(sqr_distance)) #case when the object is fall off the table if tz < self.initial_target_positions[ 2] - self.fall_down_offset: #tz is target(box) position in z direction done = True reward -= self.reward_offset # Augmented reward for orientation: better grasping gesture if the suction has vertical orientation to the target object desired_position_tip = [0.0, 1.5513, 1.74] tip_x, tip_y, tip_z = self.agent_ee_tip.get_position() tip_row, tip_pitch, tip_yaw = self.agent_ee_tip.get_orientation() reward -= (np.sqrt((tip_x - desired_position_tip[0])**2 + (tip_y - desired_position_tip[1])**2 + (tip_z - desired_position_tip[2])**2) + np.sqrt((tip_row - desired_orientation_tip[0])**2 + (tip_pitch - desired_orientation_tip[1])**2 + (tip_yaw - desired_orientation_tip[2])**2)) #Penalty for collision with the table if self.suction.check_collision( self.table) or self.suction.check_collision( self.carbody) or self.agent.check_collision( self.table) or self.suction.check_collision( self.target) or self.agent.check_collision( self.target): reward -= self.penalty_offset if math.isnan(reward): reward = 0. return self._get_state(), reward, done, {'finished': hold_flag} def shutdown(self): '''close the simulator''' self.pr.stop() self.pr.shutdown()
from pyrep import PyRep pr = PyRep() # Launch the application with a scene file in headless mode pr.launch('/v-rep_model/Infinite_basket2.ttt', headless=True) pr.start() # Start the simulation # Do some stuff pr.stop() # Stop the simulation pr.shutdown() # Close the application