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 setParams(self, params): SCENE_FILE = '../../etc/omnirobot.ttt' #SCENE_FILE = '../etc/viriato_dwa.ttt' self.pr = PyRep() self.pr.launch(SCENE_FILE, headless=False) self.pr.start() #self.robot = Viriato() self.robot = YouBot() self.robot_object = Shape("youBot") #self.ViriatoBase_WheelRadius = 76.2 #mm real robot self.ViriatoBase_WheelRadius = 44 # mm coppelia self.ViriatoBase_DistAxes = 380. self.ViriatoBase_AxesLength = 422. self.ViriatoBase_Rotation_Factor = 8.1 # it should be (DistAxes + AxesLength) / 2 # Each laser is composed of two cameras. They are converted into a 360 virtual laser self.hokuyo_base_front_left = VisionSensor("hokuyo_base_front_left") self.hokuyo_base_front_right = VisionSensor("hokuyo_base_front_right") self.hokuyo_base_back_right = VisionSensor("hokuyo_base_back_right") self.hokuyo_base_back_left = VisionSensor("hokuyo_base_back_left") self.ldata = [] self.joystick_newdata = [] self.speed_robot = [] self.speed_robot_ant = [] self.last_received_data_time = 0
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 setParams(self, params): SCENE_FILE = '../../etc/basic_scene.ttt' self.pr = PyRep() self.pr.launch(SCENE_FILE, headless=False) self.pr.start() self.robot = TurtleBot() self.drone = Shape('Quadricopter') self.cameras = {} self.front_camera_name = "frontCamera" cam = VisionSensor(self.front_camera_name) self.cameras["frontCamera"] = {"handle": cam, "id": 0, "angle": np.radians(cam.get_perspective_angle()), "width": cam.get_resolution()[0], "height": cam.get_resolution()[1], "focal": (cam.get_resolution()[0] / 2) / np.tan(np.radians(cam.get_perspective_angle() / 2)), "rgb": np.array(0), "depth": np.ndarray(0)} self.joystick_newdata = [] self.speed_robot = [] self.speed_robot_ant = [] self.last_received_data_time = 0 self.once = False
def __init__(self): self.pr = PyRep() self.pr.launch(SCENE_FILE, headless=False) self.pr.start() self.agent = OmniRob(0, 4, 2, 'Omnirob', 'SICK_S300') self.agent.set_motor_locked_at_zero_velocity(True) self.agent.set_joint_mode(JointMode.FORCE) self.distance = Distance('Distance') self.starting_pose = self.agent.get_2d_pose() self.target = Shape.create(type=PrimitiveShape.SPHERE, size=[0.1, 0.1, 0.1], color=[1.0, 0.1, 0.1], static=True, respondable=False) high = np.array([np.inf] * 37) self.action_space = spaces.Box(np.array([0, -1, -1]), np.array([1, 1, 1]), dtype=np.float32) #self.observation_space = spaces.Box(shape=(37,), dtype=np.float32) self.observation_space = spaces.Box(-high, high, dtype=np.float32) self.obstacle_ranges = [[(-1.8, -2.5), (1.8, -1.5)], [(-1.8, 1.5), (1.8, 2.5)], [(2.4, -1.8), (3.6, 1.8)], [(-3.6, -1.8), (2.4, 1.8)]]
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 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 __init__(self, continuous_control=True, obs_lowdim=True, rpa=3, frames=4): self.pr = PyRep() SCENE_FILE = join(dirname(abspath(__file__)), 'reacher_v2.ttt') self.pr.launch(SCENE_FILE, headless=True) self.pr.start() self.continuous_control = continuous_control self.target = Shape('target') self.tip = Dummy('Reacher_tip') self.camera = VisionSensor('Vision_sensor') self.agent = Reacher() self.done = False self.rpa = rpa self.obs_lowdim = obs_lowdim self.frames = frames self.prev_obs = [] self.steps_ep = 0 self.increment = 4 * pi / 180 # to radians self.action_all = [[self.increment, self.increment], [-self.increment, -self.increment], [0, self.increment], [0, -self.increment], [self.increment, 0], [-self.increment, 0], [-self.increment, self.increment], [self.increment, -self.increment]]
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
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 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 __init__(self, headless_mode: bool, variation="1container"): # Inicialización de la tarea, lanzando PyRep, cargando la escena en el simulador, cargando el robot y los # elementos de la escena y inicializando las listas. self.pr = PyRep() self.variation = variation self.ttt_file = 'pick_and_place_' + self.variation + '.ttt' self.pr.launch(join(DIR_PATH, self.ttt_file), headless=headless_mode) self.robot = Robot(Panda(), PandaGripper(), Dummy('Panda_tip')) self.task = InitTask(self.variation) self.lists = Lists()
def __init__(self, move_tolerance=1e-3): self.pr = PyRep() self.pr.launch(headless=False) self.pr.start() self.move_tolerance = move_tolerance self.blocks = ['T', 'p', 'V', 'L', 'Z', 'b', 'a']
def __init__(self): self.pr = PyRep() self.pr.launch(SCENE_FILE, headless=True) self.pr.start() self.agent = Strirus() self.agent.set_control_loop_enabled(False) self.agent.set_motor_locked_at_zero_velocity(True) self.target = Shape('Goal') self.agent_current_pos = self.agent.get_position() self.initial_joint_positions = self.agent.get_joint_positions()
def __init__(self): self.pr = PyRep() self.pr.launch(SCENE_FILE, headless=False) self.pr.start() self.agent = Panda() 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 __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 __init__(self, headless=False): self._pr = PyRep() self._pr.launch(scene_file=scene_file, headless=headless) self._pr.start() self.workspace_base = Shape("workspace") self.workspace = self._get_worksapce() self.obstacles = [] self.velocity_scale = 0 self.repiration_cycle = 0
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 __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 """ # 5 points in 3D space forming the trajectory self.action_space = Box(low=-0.3, high=0.3, shape=(5,3), dtype=np.float32) self.observation_space = Box(low=np.array([-0.1, -0.2, 0, -1.5, 0.0, 0.0, -0.5, -0.2, -0.2]), high=np.array([0.1, 1.7, 2.5, 0, 0.0, 0.0, 0.5, 0.2, 1.0])) # self.pr = PyRep() self.pr.launch(SCENE_FILE, headless=False) self.pr.start() self.agent = UR10() self.agent.max_velocity = 1.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] print(self.joints_limits) self.agent_hand = Shape('hand') self.agent.set_control_loop_enabled(True) self.agent.set_motor_locked_at_zero_velocity(True) 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.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') # 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 __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 __init__(self): self.pr = PyRep() self.pr.launch(SCENE_FILE, headless=False) self.pr.start() self.agent = YouBot() self.agent.set_motor_locked_at_zero_velocity(True) 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.target_position = None
class Plane3D: def __init__(self, headless=False): self._pr = PyRep() self._pr.launch(scene_file=scene_file, headless=headless) self._pr.start() self.workspace_base = Shape("workspace") self.workspace = self._get_worksapce() self.camera = VisionSensor("camera") self.obstacles = [] self.velocity_scale = 0 self.repiration_cycle = 0 def _get_worksapce(self): base_pos = self.workspace_base.get_position() bbox = self.workspace_base.get_bounding_box() min_pos = [bbox[2 * i] + base_pos[i] for i in range(3)] max_pos = [bbox[2 * i + 1] + base_pos[i] for i in range(3)] return [min_pos, max_pos] def reset(self, obstacle_num, velocity_scale, respiration_cycle=0): for obstacle in self.obstacles: obstacle.remove() self._pr.step() self.velocity_scale = velocity_scale self.repiration_cycle = respiration_cycle self.obstacles = [] for i in range(obstacle_num): obs = Obstacle2D.create_random_obstacle( workspace=self.workspace, velocity_scale=velocity_scale, respiration_cycle=respiration_cycle) self._pr.step() self.obstacles.append(obs) def get_image(self): rgb = self.camera.capture_rgb() return rgb def step(self): # update config for obs in self.obstacles: if self.repiration_cycle > 0: obs.respire() if self.velocity_scale > 0: obs.keep_velocity() self._pr.step()
class TestSuctionCups(TestCore): def setUp(self): self.pyrep = PyRep() self.pyrep.launch(path.join(ASSET_DIR, 'test_scene_robots.ttt'), headless=True) self.pyrep.step() self.pyrep.start() def test_get_suction_cup(self): for cup_name, cup_type in SUCTION_CUPS: with self.subTest(suction_cup=cup_name): cup = cup_type() self.assertIsInstance(cup, cup_type) def test_grasp_and_release_with_suction(self): for cup_name, cup_type in SUCTION_CUPS: with self.subTest(suction_cup=cup_name): suction_cube = Shape('%s_cube' % cup_name) cube = Shape('cube') cup = cup_type() self.pyrep.step() grasped = cup.grasp(cube) self.assertFalse(grasped) self.assertEqual(len(cup.get_grasped_objects()), 0) grasped = cup.grasp(suction_cube) self.assertTrue(grasped) self.assertListEqual(cup.get_grasped_objects(), [suction_cube]) cup.release() self.assertEqual(len(cup.get_grasped_objects()), 0)
def __init__(self, visualize=True, mode="train", **params): super().__init__(visualize=visualize, mode=mode) # Scene selection scene_file_path = os.path.join( os.getcwd(), 'deep_simulation/scenes/UR10_gripper.ttt') # Simulator launch self.env = PyRep() self.env.launch(scene_file_path, headless=False) self.env.start() self.env.step() # Task related initialisations in Simulator self.vision_sensor = objects.vision_sensor.VisionSensor( "Vision_sensor") self.gripper = objects.dummy.Dummy("UR10_target") self.gripper_zero_pose = self.gripper.get_pose() self.goal = objects.dummy.Dummy("goal_target") self.goal_STL = objects.shape.Shape("goal") self.goal_STL_zero_pose = self.goal_STL.get_pose() self.grasped_STL = objects.shape.Shape("BaxterGripper") self.stacking_area = objects.shape.Shape("Plane") self.vision_sensor = objects.vision_sensor.VisionSensor( "Vision_sensor") self.baxter_gripper = BaxterGripper() self.UR10_arm = UR10() self.gripper_dummy = objects.dummy.Dummy("gripper_dummy") self.step_counter = 0 self.max_step_count = 100 self.target_pose = None self.initial_distance = None self.image_width, self.image_height = 320, 240 self.vision_sensor.set_resolution( (self.image_width, self.image_height)) self._history_len = 1 self._observation_space = Dict({ "cam_image": Box(0, 255, [self.image_height, self.image_width, 1], dtype=np.uint8) }) self._action_space = Box(-1, 1, (3, )) self._state_space = extend_space(self._observation_space, self._history_len)
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 __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()
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()
def launch(self): if self._pyrep is not None: raise RuntimeError('Already called launch!') self._pyrep = PyRep() self._pyrep.launch(join(DIR_PATH, TTT_FILE), headless=self._headless) self._pyrep.set_simulation_timestep(0.005) self._robot = Robot(Panda(), PandaGripper()) self._scene = DomainRandomizationScene(self._pyrep, self._robot, self._obs_config, self._randomize_every, self._frequency, self._visual_rand_config, self._dynamics_rand_config) self._set_arm_control_action() # Raise the domain randomized floor. Shape('Floor').set_position(Dummy('FloorAnchor').get_position())
def cop_from_prs(pr: PyRep, scene: Scene) -> Dict[str, list]: cop_obs = defaultdict(list) primitives = (p for p in scene.objects if hasattr(p, 'shape_type')) models = (m for m in scene.objects if hasattr(m, 'model_name')) for p in primitives: c_obj = Shape.create(type=PrimitiveShape[p.shape_type], position=p.position, color=[0.5, 0.0, 0.0], size=[p.width, p.length, p.height], visible_edges=True, mass=0.01) cop_obs[p.shape_type].append(c_obj) for m in models: # print(m.model_name) if m.model_name == "Camera": c_obj = VisionSensor.create([256, 256], position=m.position, orientation=[np.pi, 0.0, 0.0]) elif m.model_name == "Table": c_obj = create_table(pr, m.width, m.length, m.height) c_obj.set_position(prs_to_coppelia_pos(m.position, c_obj)) c_obj.set_orientation(reversed(m.orientation)) elif m.model_name == "Panda": c_obj = pr.import_model(f'models/{m.model_name}.ttm') bounds = c_obj.get_model_bounding_box() dims = np.array(bounds[1::2]) - np.array(bounds[0::2]) adjusted_position = prs_to_coppelia_pos( m.position, c_obj) + np.array( [0, dims[1] / 2.0 - m.length / 2.0, 0.0]) c_obj.set_position(adjusted_position) # c_obj.set_position(scenic_to_coppelia_pos(m.position, c_obj)) c_obj.set_orientation(reversed(m.orientation)) elif m.model_name == "RopeBucket": c_obj = create_rope(pr, m.num_rope_links) c_obj.set_position(prs_to_coppelia_pos(m.position, c_obj)) c_obj.set_orientation(reversed(m.orientation)) c_obj_two = pr.import_model('models/Bucket.ttm') attach_to_rope(pr, c_obj, c_obj_two) else: c_obj = pr.import_model(f'models/{m.model_name}.ttm') c_obj.set_position(prs_to_coppelia_pos(m.position, c_obj)) c_obj.set_orientation(reversed(m.orientation)) cop_obs[m.model_name].append(c_obj) return cop_obs
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) snake_robot_class, camera_class = SUPPORTED_ROBOTS[self._robot_configuration] # We assume the panda is already loaded in the scene. if self._robot_configuration != 'rattler': raise NotImplementedError("Not implemented the robot") else: snake_robot, camera = snake_robot_class(), camera_class() self._robot = Robot(snake_robot, camera) self._scene = Scene(self._pyrep, self._robot, self._obs_config) self._set_control_action()