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()
