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 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()
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 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 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 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 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]])
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
class BaseSimulator: def __init__(self, interactive, try_use_sim): self._interactive = interactive self._try_use_sim = try_use_sim if PyRep is not None and try_use_sim: self.with_pyrep = True self._pyrep_init() else: self.with_pyrep = False def _user_prompt(self, str_in): if self._interactive: input(str_in) else: print(str_in) def _pyrep_init(self): # pyrep self._pyrep = PyRep() scene_filepath = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'scene.ttt') self._pyrep.launch(scene_filepath, headless=False, responsive_ui=True) # target self._target = Dummy('target') # default camera self._default_camera = Camera('DefaultCamera') # default vision sensor self._default_vision_sensor = VisionSensor('DefaultVisionSensor') # vision sensors self._vision_sensor_0 = VisionSensor('vision_sensor_0') self._vision_sensor_1 = VisionSensor('vision_sensor_1') self._vision_sensor_2 = VisionSensor('vision_sensor_2') self._vision_sensor_3 = VisionSensor('vision_sensor_3') self._vision_sensor_4 = VisionSensor('vision_sensor_4') self._vision_sensor_5 = VisionSensor('vision_sensor_5') self._vision_sensors = [self._vision_sensor_0, self._vision_sensor_1, self._vision_sensor_2, self._vision_sensor_3, self._vision_sensor_4, self._vision_sensor_5] # vision sensor bodies self._vision_sensor_body_0 = Shape('vision_sensor_body_0') self._vision_sensor_body_1 = Shape('vision_sensor_body_1') self._vision_sensor_body_2 = Shape('vision_sensor_body_2') self._vision_sensor_body_3 = Shape('vision_sensor_body_3') self._vision_sensor_body_4 = Shape('vision_sensor_body_4') self._vision_sensor_body_5 = Shape('vision_sensor_body_5') self._vision_sensor_bodies =\ [self._vision_sensor_body_0, self._vision_sensor_body_1, self._vision_sensor_body_2, self._vision_sensor_body_3, self._vision_sensor_body_4, self._vision_sensor_body_5] # vision sensor rays self._vision_sensor_rays_0 = [Shape('ray{}vs0'.format(i)) for i in range(4)] self._vision_sensor_rays_1 = [Shape('ray{}vs1'.format(i)) for i in range(4)] self._vision_sensor_rays_2 = [Shape('ray{}vs2'.format(i)) for i in range(4)] self._vision_sensor_rays_3 = [Shape('ray{}vs3'.format(i)) for i in range(4)] self._vision_sensor_rays_4 = [Shape('ray{}vs4'.format(i)) for i in range(4)] self._vision_sensor_rays_5 = [Shape('ray{}vs5'.format(i)) for i in range(4)] self._vision_sensor_rays = [self._vision_sensor_rays_0, self._vision_sensor_rays_1, self._vision_sensor_rays_2, self._vision_sensor_rays_3, self._vision_sensor_rays_4, self._vision_sensor_rays_5] # objects self._dining_chair_0 = Shape('diningChair0') self._dining_chair_1 = Shape('diningChair1') self._dining_table = Shape('diningTable_visible') self._high_table_0 = Shape('highTable0') self._high_table_1 = Shape('highTable1') self._plant = Shape('indoorPlant_visible') self._sofa = Shape('sofa') self._swivel_chair = Shape('swivelChair') self._rack = Shape('_rack') self._cupboard = Shape('cupboard') self._box = Shape('Cuboid') self._objects = [self._dining_chair_0, self._dining_chair_1, self._dining_table, self._high_table_0, self._high_table_1, self._plant, self._sofa, self._swivel_chair, self._rack, self._cupboard, self._box] # spherical vision sensor self._spherical_vision_sensor = SphericalVisionSensor('sphericalVisionRGBAndDepth') # drone self._drone = Shape('Quadricopter') # robot self._robot = Arm(0, 'Mico', 6) self._robot_base = Dummy('Mico_dh_base') self._robot_target = Arm(0, 'MicoTarget', 6) # spline paths self._spline_paths = list() # primitive scene self._with_primitive_scene_vis = False def _update_path_visualization_pyrep(self, multi_spline_points, multi_spline_sdf_vals): if len(self._spline_paths) > 0: for spline_path_segs in self._spline_paths: for spline_path_seg in spline_path_segs: spline_path_seg.remove() self._spline_paths.clear() for spline_points, sdf_vals in zip(multi_spline_points, multi_spline_sdf_vals): sdf_flags_0 = sdf_vals[1:, 0] > 0 sdf_flags_1 = sdf_vals[:-1, 0] > 0 sdf_borders = sdf_flags_1 != sdf_flags_0 borders_indices = ivy.indices_where(sdf_borders) if borders_indices.shape[0] != 0: to_concat = (ivy.array([0], 'int32'), ivy.cast(borders_indices, 'int32')[:, 0], ivy.array([-1], 'int32')) else: to_concat = (ivy.array([0], 'int32'), ivy.array([-1], 'int32')) border_indices = ivy.concatenate(to_concat, 0) num_groups = border_indices.shape[0] - 1 spline_path = list() for i in range(num_groups): border_idx_i = int(ivy.to_numpy(border_indices[i]).item()) border_idx_ip1 = int(ivy.to_numpy(border_indices[i + 1]).item()) if i < num_groups - 1: control_group = spline_points[border_idx_i:border_idx_ip1] sdf_group = sdf_vals[border_idx_i:border_idx_ip1] else: control_group = spline_points[border_idx_i:] sdf_group = sdf_vals[border_idx_i:] num_points = control_group.shape[0] orientation_zeros = np.zeros((num_points, 3)) color = (0.2, 0.8, 0.2) if sdf_group[-1] > 0 else (0.8, 0.2, 0.2) control_poses = np.concatenate((ivy.to_numpy(control_group), orientation_zeros), -1) spline_path_seg = CartesianPath.create(show_orientation=False, show_position=False, line_size=8, path_color=color) spline_path_seg.insert_control_points(control_poses.tolist()) spline_path.append(spline_path_seg) self._spline_paths.append(spline_path) @staticmethod def depth_to_xyz(depth, inv_ext_mat, inv_calib_mat, img_dims): uniform_pixel_coords = ivy_vision.create_uniform_pixel_coords_image(img_dims) pixel_coords = uniform_pixel_coords * depth cam_coords = ivy_vision.ds_pixel_to_cam_coords(pixel_coords, inv_calib_mat, [], img_dims) return ivy_vision.cam_to_world_coords(cam_coords, inv_ext_mat)[..., 0:3] @staticmethod def get_pix_coords(): return ivy_vision.create_uniform_pixel_coords_image([360, 720])[..., 0:2] def setup_primitive_scene_no_sim(self, box_pos=None): # lists shape_matrices_list = list() shape_dims_list = list() this_dir = os.path.dirname(os.path.realpath(__file__)) for i in range(11): shape_mat = np.load(os.path.join(this_dir, 'no_sim/obj_inv_ext_mat_{}.npy'.format(i))) if i == 10 and box_pos is not None: shape_mat[..., -1:] = box_pos.reshape((1, 3, 1)) shape_matrices_list.append(ivy.array(shape_mat, 'float32')) shape_dims_list.append( ivy.array(np.load(os.path.join(this_dir, 'no_sim/obj_bbx_{}.npy'.format(i))), 'float32') ) # matices shape_matrices = ivy.concatenate(shape_matrices_list, 0) shape_dims = ivy.concatenate(shape_dims_list, 0) # sdf primitive_scene = PrimitiveScene(cuboid_ext_mats=ivy.inv(ivy_mech.make_transformation_homogeneous( shape_matrices))[..., 0:3, :], cuboid_dims=shape_dims) self.sdf = primitive_scene.sdf def setup_primitive_scene(self): # shape matrices shape_matrices = ivy.concatenate([ivy.reshape(ivy.array(obj.get_matrix(), 'float32'), (1, 3, 4)) for obj in self._objects], 0) # shape dims x_dims = ivy.concatenate([ivy.reshape(ivy.array( obj.get_bounding_box()[1] - obj.get_bounding_box()[0], 'float32'), (1, 1)) for obj in self._objects], 0) y_dims = ivy.concatenate([ivy.reshape(ivy.array( obj.get_bounding_box()[3] - obj.get_bounding_box()[2], 'float32'), (1, 1)) for obj in self._objects], 0) z_dims = ivy.concatenate([ivy.reshape(ivy.array( obj.get_bounding_box()[5] - obj.get_bounding_box()[4], 'float32'), (1, 1)) for obj in self._objects], 0) shape_dims = ivy.concatenate((x_dims, y_dims, z_dims), -1) # primitve scene visualization if self._with_primitive_scene_vis: scene_vis = [Shape.create(PrimitiveShape.CUBOID, ivy.to_numpy(shape_dim).tolist()) for shape_dim in shape_dims] [obj.set_matrix(ivy.to_numpy(shape_mat).reshape(-1).tolist()) for shape_mat, obj in zip(shape_matrices, scene_vis)] [obj.set_transparency(0.5) for obj in scene_vis] # sdf primitive_scene = PrimitiveScene(cuboid_ext_mats=ivy.inv(ivy_mech.make_transformation_homogeneous( shape_matrices))[..., 0:3, :], cuboid_dims=shape_dims) self.sdf = primitive_scene.sdf def update_path_visualization(self, multi_spline_points, multi_spline_sdf_vals, img_path): if not self.with_pyrep: if not self._interactive: return import matplotlib.pyplot as plt import matplotlib.image as mpimg plt.ion() plt.imshow(mpimg.imread(img_path)) plt.show() plt.pause(0.1) plt.ioff() return with utils.step_lock: self._update_path_visualization_pyrep(multi_spline_points, multi_spline_sdf_vals) def close(self): if self._interactive: input('\nPress enter to end demo.\n') print('\nClosing simulator...\n') # noinspection PyUnresolvedReferences def __del__(self): if self.with_pyrep: self._pyrep.stop() self._pyrep.shutdown() print('\nDemo finished.\n')
class SlideBlock(object ): # Clase de la función de la tarea de empujar un objeto def __init__(self, headless_mode: bool, variation: str): # Al inicializar la clase se carga PyRep, se carga la escena en el simulador, se carga el Robot y los objetos # de la escena y se inicializan las listas. self.pyrep = PyRep() self.variation = variation self.ttt_file = 'slide_block_' + self.variation + ".ttt" self.pyrep.launch(join(DIR_PATH, self.ttt_file), headless=headless_mode) self.robot = Robot(Panda(), PandaGripper(), Dummy('Panda_tip')) self.task = InitTask(variation) self.lists = Lists() def slide_block(self, slide_params: np.array): # Definición del punto de empuje wp1_pos_rel = np.array( [slide_params[0], slide_params[1], slide_params[2]]) wp1_pos_abs = wp1_pos_rel + self.task.wp0.get_position() wp1_or_rel = np.array([0.0, 0.0, slide_params[4]]) wp1_or_abs = wp1_or_rel + self.task.wp0.get_orientation() self.task.wp1.set_position(wp1_pos_abs) self.task.wp1.set_orientation(wp1_or_abs) # Definición del objetivo final distance = slide_params[3] orientation = slide_params[4] final_pos_rel = np.array([ distance * math.sin(orientation), distance * math.cos(orientation), 0 ]) final_pos_abs = final_pos_rel + self.task.wp1.get_position() final_or_abs = wp1_or_abs self.task.wp2.set_position(final_pos_abs) self.task.wp2.set_orientation(final_or_abs) # Definicion de la trayectoria del robot tray = [self.task.wp0, self.task.wp1, self.task.wp2] # Iniciar la simulacion self.pyrep.start() # Se declaran los parametros de la recompensa distance_slide = 0.0 distance_target0 = 0.0 distance_target1 = 0.0 or_target0 = 0.0 or_target1 = 0.0 # Para la primera variación, se cierra la pinza para poder empujar el objeto. if self.variation == '1block': done = False while not done: done = self.robot.gripper.actuate(0, velocity=0.05) self.pyrep.step() # Ejecución de la trayectoria for pos in tray: try: path = self.robot.arm.get_path(position=pos.get_position(), euler=pos.get_orientation()) # Step the simulation and advance the agent along the path done = False while not done: done = path.step() self.pyrep.step() if pos == self.task.wp1: # En WP1 se calcula el parametro d_slide distance_slide = self.robot.gripper.check_distance( self.task.block) elif pos == self.task.wp2: # En WP2 se calcula el parametro d_slide y el error de orientación distance_target0 = calc_distance( self.task.block.get_position(), self.task.target_wp0.get_position()) or_target0 = self.task.block.get_orientation( )[2] - self.task.target_wp0.get_orientation()[2] if self.variation == '2block': # En la 2da variacion se calculan los parametros para 2 soluciones distance_target1 = calc_distance( self.task.block.get_position(), self.task.target_wp1.get_position()) or_target1 = self.task.block.get_orientation( )[2] - self.task.target_wp1.get_orientation()[2] except ConfigurationPathError: # Si no se encuentra una configuracion para la trayectoria con los waypoints correspondientes # se asigna una recompensa de -85 print('Could not find path') reward = -85 self.pyrep.stop() # Stop the simulation self.lists.list_of_rewards.append(reward) self.lists.list_of_parameters.append(list(slide_params)) return -reward # Calculo de la recompensa reward = -(10 * distance**2 + 200 * distance_slide**2 + 200 * distance_target0**2 + 400 * distance_target1**2 + 3500 * distance_target0 * distance_target1 + 200 * np.abs(or_target0) * distance_target1 + 500 * np.abs(or_target1) * distance_target0) self.pyrep.stop() # Parar la simulación self.lists.list_of_rewards.append( reward) # Se guarda la recompensa del episodio self.lists.list_of_parameters.append( list(slide_params)) # Se guardan los parametros del episodio return -reward def clean_lists(self): self.lists = Lists() # Se limpian las listas def return_lists(self): return self.lists # Devolver las listas def shutdown(self): self.pyrep.shutdown() # Close the application
class PushButton(object): # Clase de la tarea def __init__(self, headless_mode: bool, variation='1button'): self.pyrep = PyRep() self.variation = variation self.ttt_file = 'push_button_' + self.variation + '.ttt' self.pyrep.launch(join(DIR_PATH, self.ttt_file), headless=headless_mode) self.robot = Robot(Panda(), PandaGripper(), Dummy('Panda_tip')) self.task = InitTask(self.variation) self.param = Parameters(self.variation) self.param.original_pos0 = self.task.joint0.get_joint_position() self.param.original_or = self.task.wp0.get_orientation() if self.variation == '2button': self.param.original_pos1 = self.task.joint1.get_joint_position() self.lists = Lists() def push_button(self, push_params: np.array): # Definición del punto de empuje push_pos_rel = np.array([push_params[0], push_params[1], push_params[2]]) push_pos = self.task.wp0.get_position() + push_pos_rel self.task.wp1.set_position(push_pos) # Definicion de la orientacion or_rel = np.array([push_params[3], push_params[4], push_params[5]]) or_abs = self.param.original_or + or_rel self.task.wp0.set_orientation(or_abs) self.task.wp1.set_orientation(or_abs) # Definicion de la trayectoria tray = [self.task.wp0, self.task.wp1, self.task.wp0] # Ejecución de la trayectoria self.pyrep.start() # Declaración de los parametros de la recompensa distance_objective0 = 0.0 distance_objective1 = 0.0 error_alpha = 0.0 error_beta = 0.0 error_gamma = 0.0 done = False # Cerrar la pinza para poder apretar el boton. while not done: done = self.robot.gripper.actuate(0, velocity=0.05) self.pyrep.step() # Ejecucion de la trayectoria for pos in tray: try: path = self.robot.arm.get_path(position=pos.get_position(), euler=pos.get_orientation()) # Step the simulation and advance the agent along the path done = False while not done: done = path.step() self.pyrep.step() if pos == self.task.wp1: # Cuando se llega a WP1 (al boton) se calculan los parametros de la recompensa distance_objective0 = self.robot.tip.check_distance(self.task.button_wp0) error_alpha = self.task.button_wp0.get_orientation()[0] - self.task.wp1.get_orientation()[0] error_beta = self.task.button_wp0.get_orientation()[1] - self.task.wp1.get_orientation()[1] error_gamma = self.task.button_wp0.get_orientation()[2] - self.task.wp1.get_orientation()[2] if self.variation == '2button': distance_objective1 = self.robot.tip.check_distance(self.task.button_wp1) except ConfigurationPathError: # Si no se encuentra una configuracion para la trayectoria se asigna una recompensa de -300 print('Could not find path') reward = -300 self.pyrep.stop() # Stop the simulation self.lists.list_of_rewards.append(reward) self.lists.list_of_parameters.append(list(push_params)) return -reward # Calculo de la recompensa. reward = (-400 * distance_objective0 ** 2 - 5 * error_alpha ** 2 - 5 * error_beta ** 2 - 1 * error_gamma ** 2 - 800 * distance_objective1 ** 2 - 3000 * distance_objective0 * distance_objective1) self.pyrep.stop() # Stop the simulation self.lists.list_of_rewards.append(reward) self.lists.list_of_parameters.append(list(push_params)) return -reward def clean_lists(self): self.lists = Lists() def return_lists(self): return self.lists def shutdown(self): self.pyrep.shutdown() # Close the application
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): # Reset the arm at the start of each 'episode' agent.set_joint_positions(starting_joint_positions) # Get a random position within a cuboid and set the target position pos = list(np.random.uniform(position_min, position_max)) target.set_position(pos) # Get a path to the target (rotate so z points down) try: path = agent.get_path(position=pos, euler=[0, math.radians(180), 0]) except ConfigurationPathError as e: print('Could not find path') continue # Step the simulation and advance the agent along the path done = False while not done: done = path.step() pr.step() print('Reached target %d!' % i) pr.stop() pr.shutdown()
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): self._dataset_root = dataset_root self._action_mode = action_mode self._obs_config = obs_config self._headless = headless self._static_positions = static_positions 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 or self._action_mode.arm == ArmActionMode.DELTA_EE_POSE or self._action_mode.arm == ArmActionMode.ABS_EE_VELOCITY or self._action_mode.arm == ArmActionMode.DELTA_EE_VELOCITY): 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) self._pyrep.set_simulation_timestep(0.005) self._robot = Robot(Panda(), PandaGripper()) self._scene = Scene(self._pyrep, self._robot, self._obs_config) self._set_arm_control_action() def shutdown(self): self._pyrep.shutdown() self._pyrep = None def get_task(self, task_class: Type[Task]) -> TaskEnvironment: # Str comparison because sometimes class comparison doesn't work. if self._prev_task is not None: self._prev_task.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)
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 """ logging.basicConfig(level=logging.DEBUG) # self.pr = PyRep() self.pr.launch(SCENE_FILE, headless=False) self.pr.start() self.agent = UR10() self.agent.max_velocity = 0.8 self.agent.set_control_loop_enabled(True) #self.agent.set_motor_locked_at_zero_velocity(True) self.MAX_INC = 0.2 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.initial_joint_positions = self.agent.get_joint_positions() 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.target = Dummy('UR10_target') self.pollo_target = Dummy('pollo_target') self.pollo = Shape('pollo') self.initial_pollo_position = self.pollo.get_position() self.initial_pollo_orientation = self.pollo.get_quaternion() self.table_target = Dummy('table_target') self.table_target = Dummy('table_target') # objects to check collisions self.scene_objects = [ Shape('table0'), Shape('Plane'), Shape('Plane0'), Shape('ConcretBlock') ] 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 return self._get_state() def step(self, action): if action is None: print(self.total_reward) return self._get_state(), -10.0, True, {} # check for nan if np.any(np.isnan(action)): print("NAN values ", action) self.NANS_COMING = True return self._get_state(), -10.0, True, {} # check for strange values # if np.any(np.greater(action, self.MAX_INC)) or np.any(np.less(action, -self.MAX_INC)): # print("Strange values ", action) # self.NANS_COMING = True # return self._get_state(), -10.0, True, {} # check for NAN in VREP get_position() if np.any(np.isnan(self.agent.get_tip().get_position())): print("NAN values in get_position()", action, self.agent.get_tip().get_position()) return self._get_state(), -10.0, True, {} self.pr.step() return self._get_state(), 0, True, {} 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 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]
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()
def simulate(scene_dir, cls_indices): # read 3d point cloud vertices npy (for visualization) vertex_npy = np.load("mesh_data/custom_vertex.npy") # loop over all scene files in scenes directory for scene_path in os.listdir(scene_dir): # check whether it's a scene file or not if not scene_path[-3:] == 'ttt': continue # create an output directory for each scene scene_out_dir = os.path.join('./sim_data/', scene_path[:-4]) if not os.path.isdir(scene_out_dir): os.mkdir(scene_out_dir) # launch scene file SCENE_FILE = os.path.join(os.path.dirname(os.path.abspath(__file__)), os.path.join(scene_dir, scene_path)) pr = PyRep() pr.launch(SCENE_FILE, headless=True) pr.start() pr.step() # define vision sensor camera = VisionSensor('cam') # set camera absolute pose to world coordinates camera.set_pose([0,0,0,0,0,0,1]) pr.step() # define scene shapes shapes = [] shapes.append(Shape('Shape1')) shapes.append(Shape('Shape2')) shapes.append(Shape('Shape3')) shapes.append(Shape('Shape4')) pr.step() print("Getting vision sensor intrinsics ...") # get vision sensor parameters cam_res = camera.get_resolution() cam_per_angle = camera.get_perspective_angle() ratio = cam_res[0]/cam_res[1] cam_angle_x = 0.0 cam_angle_y = 0.0 if (ratio > 1): cam_angle_x = cam_per_angle cam_angle_y = 2 * degrees(atan(tan(radians(cam_per_angle / 2)) / ratio)) else: cam_angle_x = 2 * degrees(atan(tan(radians(cam_per_angle / 2)) / ratio)) cam_angle_y = cam_per_angle # get vision sensor intrinsic matrix cam_focal_x = float(cam_res[0] / 2.0) / tan(radians(cam_angle_x / 2.0)) cam_focal_y = float(cam_res[1] / 2.0) / tan(radians(cam_angle_y / 2.0)) intrinsics = np.array([[cam_focal_x, 0.00000000e+00, float(cam_res[0]/2.0)], [0.00000000e+00, cam_focal_y, float(cam_res[1]/2.0)], [0.00000000e+00, 0.00000000e+00, 1.00000000e+00]]) # loop to get 5000 samples per scene for i in range(5000): print("Randomizing objects' poses ...") # set random pose to objects in the scene obj_colors = [] for shape in shapes: shape.set_pose([ random.uniform(-0.25,0.25), random.uniform(-0.25,0.25), random.uniform(0.25,2), random.uniform(-1,1), random.uniform(-1,1), random.uniform(-1,1), random.uniform(-1,1) ]) obj_colors.append([random.uniform(0,1), random.uniform(0,1), random.uniform(0,1)]) pr.step() print("Reading vision sensor RGB signal ...") # read vision sensor RGB image img = camera.capture_rgb() img = np.uint8(img * 255.0) print("Reading vision sensor depth signal ...") # read vision sensor depth image depth = camera.capture_depth() depth = np.uint8(depth * 255.0) print("Generating front mask for RGB signal ...") # generate RGB front mask front_mask = np.sum(img, axis=2) front_mask[front_mask != 0] = 255 front_mask = Image.fromarray(np.uint8(front_mask)) alpha_img = Image.fromarray(img) alpha_img.putalpha(front_mask) print("Saving sensor output ...") # save sensor output alpha_img.save(scene_out_dir+f'/{str(i).zfill(6)}-color.png') imsave(scene_out_dir+f'/{str(i).zfill(6)}-depth.png', depth) print("Getting objects' poses ...") # get poses for all objects in scene poses = [] for shape in shapes: poses.append(get_object_pose(shape, camera)) pose_mat = np.stack(poses, axis=2) # save pose visualization on RGB image img_viz = visualize_predictions(poses, cls_indices[scene_path], img, vertex_npy, intrinsics) imsave(scene_out_dir+f'/{str(i).zfill(6)}-viz.png', img_viz) print("Saving meta-data ...") # save meta-data .mat meta_dict = { 'cls_indexes' : np.array(cls_indices[scene_path]), 'intrinsic_matrix' : intrinsics, 'poses' : pose_mat } savemat(scene_out_dir+f'/{str(i).zfill(6)}-meta.mat', meta_dict) print("Performing semantic segmentation of RGB signal ...") # perform semantic segmentation of RGB image seg_img = camera.capture_rgb() seg_img = perform_segmentation(seg_img, cls_indices[scene_path], poses, vertex_npy, intrinsics) imsave(scene_out_dir+f'/{str(i).zfill(6)}-label.png', seg_img) # stop simulation pr.stop() pr.shutdown()
class PyRepNavGoalEnv(Env): def __init__(self, n_obs=7, n_act=3, render=False, seed=1337, scene_file="my_scene_turtlebot_navigation.ttt", dist_reach_thresh=0.3): self.n_obs = n_obs self.n_act = n_act # PPO self.action_space = spaces.Discrete(n_act) self.observation_space = spaces.Box(-np.inf, np.inf, shape=[ n_obs, ], dtype='float32') # self.observation_space = spaces.Box( # low=np.array([-0.8, -0.8, -0.8, -0.8, -3.1415]), # high=np.array([0.8, 0.8, 0.8, 0.8, 3.1415]), # dtype=np.float32) # self.action_space = spaces.Box(low=-5, high=5, shape=(4,), dtype=np.float32) self.scene_file = join(dirname(abspath(__file__)), scene_file) self.pr = PyRep() self.pr.launch(self.scene_file, headless=not render) self.pr.start() self.agent = TurtleBot() # We could have made this target in the scene, but lets create one dynamically self.target = Shape.create(type=PrimitiveShape.SPHERE, size=[0.05, 0.05, 0.05], color=[1.0, 0.1, 0.1], static=True, respondable=False) self.position_min, self.position_max = [-1.5, -1.5, 0.1], [1.5, 1.5, 0.1] self.target_pos = [] # initialized in reset self.starting_pose = [0.0, 0.0, 0.061] self.agent.set_motor_locked_at_zero_velocity(True) self.trajectory_step_counter = 0 self.done_dist_thresh = dist_reach_thresh def step(self, action): self._set_action(action) self.pr.step() self.trajectory_step_counter += 1 o = self._get_obs() r = self._get_reward() d = self._is_done() i = {} return o, r, d, i # obs, reward, done, info.. def reset(self): # Get a random position within a cuboid for the goal and set the target position self.target_pos = list( np.random.uniform(self.position_min, self.position_max)) # make sure it doesn*t spawn too close to robot... if self.target_pos[0] > 0 and self.target_pos[0] < 0.7: self.target_pos[0] = 0.7 elif self.target_pos[0] < 0 and self.target_pos[0] > -0.7: self.target_pos[0] = 0.7 if self.target_pos[1] > 0 and self.target_pos[1] < 0.7: self.target_pos[1] = 0.7 elif self.target_pos[1] < 0 and self.target_pos[1] > -0.7: self.target_pos[1] = 0.7 # hard goal goal for now: self.target_pos[0] = 1.1 self.target_pos[1] = 0 self.target.set_position(self.target_pos) # Reset robot position to starting position self.agent.set_position(self.starting_pose) agent_rot = self.agent.get_orientation() # agent_rot[2] = 0 agent_rot[2] = np.random.uniform(-3.1415, 3.1415) self.agent.set_orientation(agent_rot) self.agent.set_joint_target_velocities([0, 0]) # reset velocities self.trajectory_step_counter = 0 self.pr.step( ) # think we need this for first obs to already return the values we set here above... return self._get_obs() def render(self, mode='human'): pass def close(self): self.pr.stop() self.pr.shutdown() def seed(self, seed=None): pass def _get_obs(self): agent_pos = self.agent.get_2d_pose() agent_rot_rads = self.agent.get_orientation() # obs = [agent_pos[0], agent_pos[1], agent_rot_rads[2]] # x_pos, y_pos, abs z_orientation # achieved_goal = [agent_pos[0], agent_pos[1]] # desired_goal = [self.target_pos[0], self.target_pos[1]] # obs = {'observation': obs.copy(), 'achieved_goal': achieved_goal.copy(), # 'desired_goal': np.array(desired_goal.copy()), # 'non_noisy_obs': agent_pos.copy()} agent_joint_vels = self.agent.get_joint_velocities() obs = [ self.target_pos[0], self.target_pos[1], agent_pos[0], agent_pos[1], agent_rot_rads[2] ] for v in agent_joint_vels: obs.append(v) obs = [round(o, 3) for o in obs] return obs def _set_action(self, action): # set motor velocities target_vels = [] frac = 0.5 if action == 0: target_vels = [3.1415 * frac, 3.1415 * frac] elif action == 1: target_vels = [3.1415 * frac, -3.1415 * frac] elif action == 2: target_vels = [-3.1415 * frac, 3.1415 * frac] self.agent.set_joint_target_velocities(target_vels) def _get_reward(self): agent_pos = self.agent.get_2d_pose() dist = np.sqrt((self.target_pos[0] - agent_pos[0])**2 + (self.target_pos[1] - agent_pos[1])**2) if dist <= self.done_dist_thresh: return 0 else: return -1 def _is_done(self): agent_pos = self.agent.get_2d_pose() dist = np.sqrt((self.target_pos[0] - agent_pos[0])**2 + (self.target_pos[1] - agent_pos[1])**2) # print(dist) if dist <= self.done_dist_thresh: print("done because close") return True else: return False
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() raise e sim.shutdown() print('Validation successful!')
class ThreeObstacles(object): def __init__(self, headless_mode: bool): self.pyrep = PyRep() self.pyrep.launch(join(DIR_PATH, TTT_FILE), headless=headless_mode) self.robot = Robot(Panda(), PandaGripper(), Dummy('Panda_tip')) self.task = InitTask() self.lists = Lists() def avoidance_with_waypoints(self, wp_params: np.array): waypoint1, waypoint2 = self.get_waypoints_esf(wp_params) # Definición de la trayectoria tray = [ self.task.initial_pos, waypoint1, waypoint2, self.task.final_pos ] d_tray_1 = self.task.initial_pos.check_distance(waypoint1) d_tray_2 = waypoint1.check_distance(waypoint2) d_tray_3 = waypoint2.check_distance(self.task.final_pos) d_tray = d_tray_1 + d_tray_2 + d_tray_3 # Ejecución de la trayectoria self.pyrep.start() reward_long = -4 * d_tray**2 reward_dist = 0.0 for pos in tray: try: path = self.robot.arm.get_linear_path( position=pos.get_position(), euler=[0.0, np.radians(180), 0.0]) # Step the simulation and advance the agent along the path done = False while not done: done = path.step() self.pyrep.step() distance_obstacle0 = self.robot.gripper.check_distance( self.task.obstacle0) distance_obstacle1 = self.robot.gripper.check_distance( self.task.obstacle1) distance_obstacle2 = self.robot.gripper.check_distance( self.task.obstacle2) reward_dist -= (20 * np.exp(-300 * distance_obstacle0) + 20 * np.exp(-300 * distance_obstacle1) + 20 * np.exp(-300 * distance_obstacle2)) except ConfigurationPathError: reward = -400.0 self.pyrep.stop() self.lists.list_of_parameters.append(list(wp_params)) self.lists.list_of_rewards.append(reward) return -reward reward = reward_long + reward_dist self.pyrep.stop() self.lists.list_of_parameters.append(list(wp_params)) self.lists.list_of_rewards.append(reward) return -reward def shutdown(self): self.pyrep.shutdown() # Close the application def clean_lists(self): self.lists = Lists() def return_lists(self): return self.lists def get_waypoints_esf(self, wp_params: np.array): radio1 = wp_params[0] tita1 = wp_params[1] pos1_rel = np.array( [radio1 * np.sin(tita1), radio1 * np.cos(tita1), 0]) pos1_abs = pos1_rel + self.task.initial_pos.get_position() waypoint1 = Dummy.create() waypoint1.set_position(pos1_abs) radio2 = wp_params[2] tita2 = wp_params[3] pos2_rel = np.array( [radio2 * np.sin(tita2), radio2 * np.cos(tita2), 0]) pos2_abs = pos2_rel + pos1_abs waypoint2 = Dummy.create() waypoint2.set_position(pos2_abs) return waypoint1, waypoint2
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 DroneEnv(object): def __init__(self, random, headless=True, use_vision_sensor=False, seed=42, state="Normal", SCENE_FILE=None, reward_function_name='Normal', buffer_size=None, neta=0.9, restart=False): if reward_function_name not in list(rew_functions2.import_dict.keys()): print( "Wrong parameter passed on 'reward_function_name. Must be one of these: ", list(rew_functions.import_dict.keys())) if SCENE_FILE == None: if (use_vision_sensor): SCENE_FILE = join( dirname(abspath(__file__)) ) + '/../../scenes/ardrone_modeled_headless_vision_sensor.ttt' ## FIX else: SCENE_FILE = join( dirname(abspath(__file__)) ) + '/../../scenes/ardrone_modeled_headless.ttt' ## FIX else: SCENE_FILE = join(dirname(abspath(__file__))) + SCENE_FILE assert state in ['Normal', 'New_Double', 'New_action'] assert random in [False, 'Gaussian', 'Uniform','Discretized_Uniform'], \ "random should be one of these values [False, 'Gaussian', 'Uniform', 'Discretized_Uniform]" # assert random in [False, 'Gaussian', 'Uniform','Weighted','Discretized_Uniform'], \ # "random should be one of these values [False, 'Gaussian', 'Uniform', 'Weighted','Discretized_Uniform]" ## Setting Pyrep self.pr = PyRep() self.pr.launch(SCENE_FILE, headless=headless) self.pr.start() self.agent = Drone(count=0, name="Quadricopter", num_joints=4, use_vision_sensor=use_vision_sensor) self.agent.set_control_loop_enabled( False) ## Disables the built-in PID control on V-REP motors #self.agent.set_motor_locked_at_zero_velocity(True) ## When the force is set to zero, it locks the motor to prevent drifting self.agent.set_motor_locked_at_zero_velocity( False ) ## When the force is set to zero, it locks the motor to prevent drifting self.target = Shape('Quadricopter_target') ##Attributes self.action_space = self.agent.action_space if state == 'New_action': self.observation_space = 22 elif state == 'New_Double': self.observation_space = 36 else: self.observation_space = self.agent.observation_space self.restart = restart self.random = random self.dt = np.round(sim.simGetSimulationTimeStep(), 4) ## timestep ## Integrative buffer self.buffer_size = buffer_size # if self.buffer_size: # assert (isinstance(buffer_size,int)) and (buffer_size < 100) # self.integrative_buffer_size = self.buffer_size # self.integrative_buffer = np.empty((self.buffer_size, 3)) # 3 because its [x,y,z] or [roll,pitch,yaw] # self.neta = neta self.state = state ## initial observation self._initial_state() self.first_obs = True ## hack so it doesn't calculate it at the first time self._make_observation() self.last_state = self.observation[:18] self.weighted = False # Creating lists if self.random == 'Discretized_Uniform': self._create_discretized_uniform_list() self.ptr = 0 # self._creating_initialization_list() ## Setting seed self.seed = seed self._set_seed(self.seed, self.seed) ## Reward Functions self.reward_function = partial( rew_functions2.reward, rew_functions2.import_dict[reward_function_name]['weight_list']) keys = ["r_alive","radius","pitch","yaw","roll","pitch_vel","yaw_vel","roll_vel","lin_x_vel","lin_y_vel","lin_z_vel","norm_a", \ "std_a","death","integrative_error_x","integrative_error_y","integrative_error_z"] self.weight_dict = dict( zip( keys, rew_functions2.import_dict[reward_function_name] ['weight_list'])) def shutdown(self): self.pr.stop() self.pr.shutdown() def _make_observation(self): lst_o = [] # Pose of the drone drone_pos = self.agent.get_position(relative_to=None) rel_orient = self.agent.get_orientation(relative_to=self.target) global_orient = self.agent.get_orientation(relative_to=None) lin_vel, ang_vel = self.agent.get_velocity() # Pose of the target target_pos = self.target.get_position(relative_to=None) goal_lin_vel, goal_ang_vel = self.agent.get_velocity( self.target.get_handle()) # Relative pos: rel_pos = self.agent.get_position(relative_to=self.target) rel_ang_vel = ang_vel # Rotation matrix calculation (drone -> world) g11, g12, g13, g21, g22, g23, g31, g32, g33 = self.agent._rotatation_drone_world( global_orient) # State of the environment lst_o += list(rel_pos) lst_o += [g11, g12, g13, g21, g22, g23, g31, g32, g33] lst_o += rel_ang_vel lst_o += lin_vel # ## fifo # if self.buffer_size: # self.integrative_buffer[:-1] = self.integrative_buffer[1:]; self.integrative_buffer[-1] = rel_ang_vel ## FIFO # self.integrative_error = self._calc_accum_error(self.integrative_buffer, neta=self.neta) ## Actual State if self.state == 'New_action': if not self.first_obs: lst_o += list(self.current_action) else: lst_o += [0, 0, 0, 0] self.observation = np.array(lst_o).astype('float32') if not self.first_obs: if self.state == 'New_Double': self.observation = np.append(self.observation[:18], self.last_state) # Relative angular acceleration rel_ang_acc = ((self.observation[12:15] - self.last_state[12:15]) / self.dt) # Relative linear acceleration lin_acc = (self.observation[15:18] - self.last_state[15:18]) / self.dt else: if self.state == 'New_Double': self.observation = np.append(self.observation, self.observation) self.first_obs = False def _set_seed(self, random_seed, numpy_seed): random.seed(a=random_seed) np.random.seed(seed=numpy_seed) def _make_action(self, a): self.agent.set_thrust_and_torque(a, force_zero=False) self.current_action = a def step(self, action): if isinstance(action, dict): ac = np.zeros(4) for i in range(4): ac[i] = action['action{}'.format(i)] action = ac # Clipping the action taken action = np.clip(action, 0, self.agent.joints_max_velocity) # Actuate self._make_action(action) # Step self.pr.step() # Step the physics simulation self.last_state = self.observation[:18] # Observe self._make_observation() # Reward drone_pos, drone_orient, yaw,pitch, roll,yaw_vel,pitch_vel, roll_vel,lin_x_vel, lin_y_vel, lin_z_vel, \ norm_a, std_a = self._get_reward_data() reward, reward_dict = self.reward_function(self, self.radius, yaw, pitch, roll, yaw_vel, pitch_vel, roll_vel, lin_x_vel, lin_y_vel, lin_z_vel, norm_a, std_a, self.integrative_error) info = reward_dict # Check if state is terminal if self.weighted: stand_threshold = 11 elif self.random == 'Discretized_Uniform': stand_threshold = 6.5 else: stand_threshold = 3.2 done = (self.radius > stand_threshold) if done: reward += self.weight_dict['death'] return self.observation, reward, done, info def _get_reward_data(self): drone_pos = self.agent.get_position(relative_to=self.target) drone_orient = self.agent.get_orientation(relative_to=self.target) self.drone_orientation = drone_orient roll = drone_orient[0] pitch = drone_orient[1] yaw = drone_orient[2] roll_vel, pitch_vel, yaw_vel = self.observation[12:15] lin_x_vel, lin_y_vel, lin_z_vel = self.observation[15:18] self.radius = math.sqrt(drone_pos[0]**2 + drone_pos[1]**2 + drone_pos[2]**2) lin_x_vel, lin_y_vel, lin_z_vel = self.observation[15:18] norm_a = np.linalg.norm(self.current_action, ord=2) std_a = self.current_action.std() if not self.buffer_size: self.integrative_error = None return drone_pos, drone_orient, yaw, pitch, roll, yaw_vel, pitch_vel, roll_vel, lin_x_vel, lin_y_vel, lin_z_vel, norm_a, std_a def reset(self): ## Zeroing the integrative buffer: if self.buffer_size: self.integrative_buffer[:] = np.nan self.current_action = np.array([0, 0, 0, 0]) if self.restart == True: self.pr.stop() self._reset_position(how=self.random) self.pr.start() else: self._reset_position(how=self.random) self.first_obs = True self._make_observation() self.last_state = self.observation[:18] return self.observation def _reset_position(self, how=False): # self.pr.step() # self.agent.set_orientation([-0,0,-0]) if how == "Gaussian": # self._set_gaussian_position() position, orientation = utils._set_gaussian_position() self._set_pose(position, orientation) elif how == 'Uniform': position, orientation = utils._set_uniform_position() self._set_pose(position, orientation) # self._set_uniform_position() elif how == 'Discretized_Uniform': position, orientation = utils._set_discretized_uniform_position( self) self._set_pose(position, orientation) elif how == 'Weighted': raise NotImplementedError # if self.weighted == False: # self.weighted = True # else: # pass # chosen_position, chosen_orientation = self._sampling_weighted_multinomial() # self.agent.set_position(chosen_position) # self.agent.set_orientation(chosen_orientation.tolist()) else: self._set_initial_position() self.agent.set_thrust_and_torque(np.asarray([0.] * 4), force_zero=True) self.agent.set_joint_positions(self.initial_joint_positions) self.agent.set_joint_target_velocities(self.initial_joint_velocities) self.agent.set_joint_target_positions( self.initial_joint_target_positions) def _set_pose(self, position, orientation): self.agent.set_orientation(np.round(orientation, 2).tolist()) self.agent.set_position(np.round(position, 2).tolist()) def _set_initial_position(self): self.agent.set_position(self.initial_position) self.target.set_position(self.target_initial_position) self.agent.set_orientation(self.initial_orientation) self.target.set_orientation(self.target_initial_orientation) # self.agent.set_joint_positions(self.initial_joint_positions) def _create_discretized_uniform_list(self): num_discretization = 7 bound_of_distribuition = 1.5 size = 2 self.x_y_ticks = np.round( np.linspace(-bound_of_distribuition, bound_of_distribuition, num=num_discretization), 2) num_discretization = 11 bound_of_distribuition = 0.5 size = 1 z_ticks = np.round( np.linspace(-bound_of_distribuition, bound_of_distribuition, num=num_discretization), 2) self.z_ticks = (z_ticks + 1.7) num_discretization = 11 bound_of_distribuition = 1.57 / 2 size = 3 self.ang_ticks = np.round( np.linspace(-bound_of_distribuition, bound_of_distribuition, num=num_discretization), 2) def _initial_state(self): self.initial_joint_positions = self.agent.get_joint_positions() self.initial_position = self.agent.get_position() self.initial_orientation = self.agent.get_orientation() self.drone_orientation = self.initial_orientation self.target_initial_position = self.target.get_position() self.target_initial_orientation = self.target.get_orientation() self.initial_joint_velocities = self.agent.get_joint_velocities() self.initial_joint_target_velocities = self.agent.get_joint_target_velocities( ) self.initial_joint_target_positions = self.agent.get_joint_target_positions( ) self.current_action = np.array([0, 0, 0, 0])
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
class CoppeliaSimEnv(gym.Env): """ Superclass for CoppeliaSim gym environments. """ metadata = {"render.modes": ["human"]} def __init__(self, scene: str, dt: float, model: str = None, headless_mode: bool = False): """ Class constructor Args: scene: String, name of the scene to be loaded dt: Float, a time step of the simulation model: Optional[String], name of the model that to be imported headless_mode: Bool, define mode of the simulation """ self.seed() self._assets_path = os.path.join( os.path.dirname(os.path.realpath(__file__)), "assets") self._scenes_path = os.path.join(self._assets_path, "scenes") self._models_path = os.path.join(self._assets_path, "models") self._pr = PyRep() self._launch_scene(scene, headless_mode) self._import_model(model) if not headless_mode: self._clear_gui() self._pr.set_simulation_timestep(dt) self._pr.start() def _launch_scene(self, scene: str, headless_mode: bool): assert os.path.splitext(scene)[1] == ".ttt" assert scene in os.listdir(self._scenes_path) scene_path = os.path.join(self._scenes_path, scene) self._pr.launch(scene_path, headless=headless_mode) def _import_model(self, model: str): if model is not None: assert os.path.splitext(model)[1] == ".ttm" assert model in os.listdir(self._models_path) model_path = os.path.join(self._models_path, model) self._pr.import_model(model_path) @staticmethod def _clear_gui(): sim.simSetBoolParameter(simConst.sim_boolparam_browser_visible, False) sim.simSetBoolParameter(simConst.sim_boolparam_hierarchy_visible, False) sim.simSetBoolParameter(simConst.sim_boolparam_console_visible, False) def step(self, action): return NotImplementedError def reset(self): return NotImplementedError def close(self): self._pr.stop() self._pr.shutdown() def seed(self, seed: int = None): self.np_random, seed = seeding.np_random(seed) return [seed] def render(self, mode: str = "human") -> NoReturn: print("Not implemented yet")
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()
class ControllerTester: """ A class to test performance of torque controller based on VREP simulation """ def __init__(self): rospy.init_node("controller_tester", anonymous=True) rospy.loginfo("controller tester node is initialized...") self.target_pub = rospy.Publisher("target_joint_angles", JointState, queue_size=1) # Launch VREP using pyrep self.pr = PyRep() self.pr.launch( f"/home/{os.environ['USER']}/Documents/igr/src/software_interface/vrep_robot_control/in-bore.ttt", headless=False) self.inbore = CtRobot(name='inbore_arm', num_joints=4, joint_type=['r','r','r','p']) # Set up simulation parameter self.dt = 0.002 self.pr.start() self.pr.set_simulation_timestep(self.dt) # Set up dynamics properties of arm for j in range(1, self.inbore._num_joints + 1): self.inbore.arms[j].set_dynamic(False) self.inbore.arms[0].set_dynamic(False) # Set up joint properties for i in range(self.inbore._num_joints): self.inbore.joints[i].set_joint_mode(JointMode.PASSIVE) self.inbore.joints[i].set_control_loop_enabled(False) self.inbore.joints[i].set_motor_locked_at_zero_velocity(True) self.inbore.joints[i].set_joint_position(0) self.inbore.joints[i].set_joint_target_velocity(0) self.inbore.joints[1].set_joint_mode(JointMode.FORCE) self.inbore.joints[1].set_control_loop_enabled(False) self.inbore.joints[1].set_motor_locked_at_zero_velocity(True) self.inbore.arms[2].set_dynamic(True) self.inbore.joints[0].set_joint_mode(JointMode.FORCE) self.inbore.joints[0].set_control_loop_enabled(False) self.inbore.joints[0].set_motor_locked_at_zero_velocity(True) self.inbore.arms[1].set_dynamic(True) self.pr.step() # Generate trajectory t = sp.Symbol('t') # Step response traj = [ (-40/180*np.pi)*sp.ones(1), (30/180*np.pi)*sp.ones(1), (0/180*np.pi)*sp.ones(1), 0.000*sp.ones(1) ] # traj = [ # (-30/180*np.pi)*sp.sin(t*4), # (30/180*np.pi)*sp.cos(t*4), # (30/180*np.pi)*sp.cos(t*4), # 0.006*sp.sin(t/2)+0.006 # ] self.pos = [sp.lambdify(t, i) for i in traj] self.vel = [sp.lambdify(t, i.diff(t)) for i in traj] self.acc = [sp.lambdify(t, i.diff(t).diff(t)) for i in traj] def signal_handler(self, sig, frame): """ safely shutdown vrep when control C is pressed """ rospy.loginfo("Calling exit for pyrep") self.shutdown_vrep() rospy.signal_shutdown("from signal_handler") def shutdown_vrep(self): """ shutdown vrep safely """ rospy.loginfo("Stopping pyrep.") self.pr.stop() rospy.loginfo("V-REP shutting down.") self.pr.shutdown() def publish(self): t = 0 while not rospy.is_shutdown(): posd = [float(j(t)) for j in self.pos] veld = [float(j(t)) for j in self.vel] # accd = [float(j(t)) for j in self.acc] target = JointState() target.position = posd target.velocity = veld t = t + self.dt signal.signal(signal.SIGINT, self.signal_handler) self.target_pub.publish(target) self.pr.step()