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()
def _start_sim(self, scene_file, render_scene_file, headless, sim_timestep, render): '''Starts the simulation. We use different scene files for training and rendering, as unused objects still slow down the simulation. :param sim_timestep: The timestep between actionable frames. The physics simulation has a timestep of 5ms, but the control commands are repeated until the *sim_timestep* is reached''' sim = PyRep() scene_file = [scene_file if not render else render_scene_file][0] scene_file = join(dirname(abspath(__file__)), scene_file) sim.launch(scene_file, headless=headless) # Need sim_timestep set to custom in CoppeliaSim Scene for this method to work. sim.set_simulation_timestep(dt=sim_timestep) sim.start() return sim
class 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 DomainRandomizationEnvironment(Environment): """Each environment has a scene.""" def __init__(self, action_mode: ActionMode, dataset_root='', obs_config=ObservationConfig(), headless=False, static_positions: bool = False, randomize_every: RandomizeEvery = RandomizeEvery.EPISODE, frequency: int = 1, visual_randomization_config=None, dynamics_randomization_config=None): super().__init__(action_mode, dataset_root, obs_config, headless, static_positions) self._randomize_every = randomize_every self._frequency = frequency self._visual_rand_config = visual_randomization_config self._dynamics_rand_config = dynamics_randomization_config def launch(self): if self._pyrep is not None: raise RuntimeError('Already called launch!') self._pyrep = PyRep() self._pyrep.launch(join(DIR_PATH, TTT_FILE), headless=self._headless) self._pyrep.set_simulation_timestep(0.005) self._robot = Robot(Panda(), PandaGripper()) self._scene = DomainRandomizationScene(self._pyrep, self._robot, self._obs_config, self._randomize_every, self._frequency, self._visual_rand_config, self._dynamics_rand_config) self._set_arm_control_action() # Raise the domain randomized floor. Shape('Floor').set_position(Dummy('FloorAnchor').get_position())
class robotEnv(object): def __init__(self, scene_name, reward_dense, boundary): self.pr = PyRep() SCENE_FILE = join(dirname(abspath(__file__)), scene_name) self.pr.launch(SCENE_FILE, headless=True) self.pr.start() self.pr.set_simulation_timestep(0.05) if scene_name != 'robot_scene.ttt': #youbot_navig2 home_dir = os.path.expanduser('~') os.chdir(join(home_dir, 'robotics_drl')) self.pr.import_model('robot.ttm') # Vision sensor handles self.camera_top = VisionSensor('Vision_sensor') # self.camera_top.set_render_mode(RenderMode.OPENGL3) # self.camera_top.set_resolution([256,256]) self.camera_arm = VisionSensor('Vision_sensor1') self.camera_arm.set_render_mode(RenderMode.OPENGL3) self.camera_arm.set_resolution([256, 256]) self.reward_dense = reward_dense self.reward_termination = 1 if self.reward_dense else 0 self.boundary = boundary # Robot links robot_links = [] links_size = [3, 5, 5, 1] for i in range(len(links_size)): robot_links.append( [Shape('link%s_%s' % (i, j)) for j in range(links_size[i])]) links_color = [[0, 0, 1], [0, 1, 0], [1, 0, 0]] color_i = 0 for j in robot_links: if color_i > 2: color_i = 0 for i in j: i.remove_texture() i.set_color(links_color[color_i]) color_i += 1 def render(self, view='top'): if view == 'top': img = self.camera_top.capture_rgb() * 256 # (dim,dim,3) elif view == 'arm': img = self.camera_arm.capture_rgb() return img def terminate(self): self.pr.stop() # Stop the simulation self.pr.shutdown() def sample_action(self): return [(2 * random.random() - 1) for _ in range(self.action_space)] def rand_bound(self): x = random.uniform(-self.boundary, self.boundary) y = random.uniform(-self.boundary, self.boundary) orientation = random.random() * 2 * pi return x, y, orientation def action_type(self): return 'continuous' def observation_space(self): _, obs = self.get_observation() return obs.shape def step_limit(self): return 250
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]
parser = argparse.ArgumentParser() parser.add_argument("task", help="The task file to test.") args = parser.parse_args() python_file = os.path.join(TASKS_PATH, args.task) if not os.path.isfile(python_file): raise RuntimeError('Could not find the task file: %s' % python_file) task_class = task_file_to_task_class(args.task) DIR_PATH = os.path.dirname(os.path.abspath(__file__)) sim = PyRep() ttt_file = os.path.join(DIR_PATH, '..', 'rlbench', TTT_FILE) sim.launch(ttt_file, headless=True) sim.step_ui() sim.set_simulation_timestep(0.005) sim.step_ui() sim.start() robot = Robot(Panda(), PandaGripper()) active_task = task_class(sim, robot) obs = ObservationConfig() obs.set_all(False) scene = Scene(sim, robot, obs) try: task_smoke(active_task, scene, variation=2) except TaskValidationError as e: sim.shutdown() raise e sim.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()
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")
self.task_file = python_file self.reload_python() self.save_task() print('Rename complete') if __name__ == '__main__': setup_list_completer() pr = PyRep() ttt_file = join(CURRENT_DIR, '..', 'rlbench', TTT_FILE) pr.launch(ttt_file, responsive_ui=True) pr.step_ui() pr.set_simulation_timestep(0.005) pr.step_ui() robot = Robot(Panda(), PandaGripper()) cam_config = CameraConfig(rgb=True, depth=False, mask=False, render_mode=RenderMode.OPENGL) obs_config = ObservationConfig() obs_config.set_all(False) obs_config.right_shoulder_camera = cam_config obs_config.left_shoulder_camera = cam_config obs_config.wrist_camera = cam_config scene = Scene(pr, robot, obs_config) loaded_task = LoadedTask(pr, scene, robot) print(' ,')
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)
from pyrep import PyRep from pyrep.robots.arms.panda import Panda from pyrep.robots.arms.lbr_iiwa_14_r820 import LBRIwaa14R820 as Kuka from pyrep.objects.shape import Shape from pyrep.const import PrimitiveShape from pyrep.errors import ConfigurationPathError from robot import EZGripper import numpy as np import math LOOPS = 10 SCENE_FILE = join(dirname(abspath(__file__)), 'coppelia_scenes/kuka.ttt') pr = PyRep() pr.launch(SCENE_FILE, headless=False) pr.set_simulation_timestep(dt=0.10) pr.start() agent = Kuka() agent.set_control_loop_enabled(True) agent.set_motor_locked_at_zero_velocity(True) agent.set_joint_forces(np.ones([ 7, ]) * 800) # We could have made this target in the scene, but lets create one dynamically target = Shape('target') target.set_respondable(False) target.set_dynamic(False) position_min, position_max = [0.8, -0.2, 2.0], [1.0, 0.2, 2.0] starting_joint_positions = agent.get_joint_positions()
class Environment(object): """Each environment has a scene.""" def __init__(self, action_config: ActionConfig, dataset_root: str = '', obs_config=ObservationConfig(), headless=False, static_positions: bool = False, robot_configuration='rattler'): self._dataset_root = dataset_root self._action_config = action_config self._obs_config = obs_config self._headless = headless self._static_positions = static_positions self._robot_configuration = robot_configuration if robot_configuration not in SUPPORTED_ROBOTS.keys(): raise ValueError('robot_configuration must be one of %s' % str(SUPPORTED_ROBOTS.keys())) self._check_dataset_structure() self._pyrep = None self._robot = None self._scene = None self._prev_task = None def _set_control_action(self): self._robot.robot_body.set_control_loop_enabled(True) if (self._action_config.robot_action_config == SnakeRobotActionConfig.ABS_JOINT_VELOCITY or self._action_config.robot_action_config == SnakeRobotActionConfig.DELTA_JOINT_VELOCITY): self._robot.robot_body.set_control_loop_enabled(False) self._robot.robot_body.set_motor_locked_at_zero_velocity(True) elif (self._action_config.robot_action_config == SnakeRobotActionConfig.ABS_JOINT_POSITION or self._action_config.robot_action_config == SnakeRobotActionConfig.DELTA_JOINT_POSITION or self._action_config.robot_action_config == SnakeRobotActionConfig.TRIGON_MODEL_PARAM): self._robot.robot_body.set_control_loop_enabled(True) elif (self._action_config.robot_action_config == SnakeRobotActionConfig.ABS_JOINT_TORQUE or self._action_config.robot_action_config == SnakeRobotActionConfig.DELTA_JOINT_TORQUE): self._robot.robot_body.set_control_loop_enabled(False) else: raise RuntimeError('Unrecognised action configuration.') 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) @staticmethod def _string_to_task(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) 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() def shutdown(self): if self._pyrep is not None: 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_config, self._dataset_root, self._obs_config, self._static_positions)
def main(): data = deque() ground_truth = deque() # RTDE Stuff #rtd = rtde_helper('conf/record_configuration.xml', 'localhost', 30004, 125) # ZMQ stuff zmq_port = "5556" context = zmq.Context() socket = context.socket(zmq.PUSH) socket.bind("tcp://*:%s" % zmq_port) # Launch the application with a scene file in headless mode pr = PyRep() pr.launch('/home/sarthak/PycharmProjects/research_ws/scenes/bill_dummy.ttt', headless=True) pr.set_simulation_timestep(0.1) expr = Experiment(5, 0) ur10 = UR10() pr.start() # Start the simulation time.sleep(0.1) pr.step() velocities = [5, 5, 5, 5, 5, 5] ur10.set_joint_target_velocities(velocities) sim_step = 0 STEPS = 150000 buffer = deque() pr.step() time.sleep(5) data = np.load("/home/sarthak/PycharmProjects/research_ws/data/real-data/dataset14.npy") data_store = [] #ctr = 0 while True:#sim_step <= STEPS: #q, _ = rtd.get_joint_states() q = data[sim_step][51:57] q[1], q[3] = q[1] + 1.57079, q[3] + 1.57079 hum_pose = list(data[sim_step][-3:]) # ur10.set_joint_target_positions(q) #q = np.random.randn(6,) #print(q) ur10.set_joint_target_positions(q) hum_pose[2] = 0 vrep.simSetObjectPosition(expr.get_object_handle("Bill"), expr.world_handle, hum_pose) # raw distance readings base = expr.get_proximity_reading_from('Base') elbow = expr.get_proximity_reading_from('Elbow') tool = expr.get_proximity_reading_from('Tool') # 3d point readings with id base_pc, base_obj = expr.get_points_from('Base') elbow_pc, elbow_obj = expr.get_points_from('Elbow') tool_pc, tool_obj = expr.get_points_from('Tool') base_obs = np.hstack([base_pc, base_obj]) elbow_obs = np.hstack([elbow_pc, elbow_obj]) tool_obs = np.hstack([tool_pc, tool_obj]) complete_obs = np.vstack([base_obs, elbow_obs, tool_obs]) # human position #hum_position = np.array(expr.get_human_position()).reshape(1, 3) if len(buffer) == 1: buffer.popleft() else: buffer.append(complete_obs) if len(buffer) > 0: buff_stack = np.vstack(buffer) socket.send_pyobj([buff_stack, hum_pose, q]) pr.step() sim_step += 1 time.sleep(0.008) rtd.stop() pr.stop() # Stop the simulation pr.shutdown() # Close the application import sys sys.exit()
def main(): data = deque() ground_truth = deque() # RTDE Stuff rtd = rtde_helper('conf/record_configuration.xml', 'localhost', 30004, 125) # ZMQ stuff zmq_port = "5556" context = zmq.Context() socket = context.socket(zmq.PUSH) socket.bind("tcp://*:%s" % zmq_port) # Launch the application with a scene file in headless mode pr = PyRep() pr.launch('/home/sarthak/PycharmProjects/research_ws/scenes/bill_new.ttt', headless=True) pr.set_simulation_timestep(0.1) expr = Experiment(5, 0) ur10 = UR10() pr.start() # Start the simulation time.sleep(0.1) pr.step() velocities = [5, 5, 5, 5, 5, 5] ur10.set_joint_target_velocities(velocities) sim_step = 0 STEPS = 150000 buffer = deque() depth_cam = vrep.simGetObjectHandle('eyecam') # Bill head cam # js_thread = Thread(target=sim_work, args=(rtd, pr, ur10)) pr.step() time.sleep(5) data_store = [] while True: #sim_step <= STEPS: q, _ = rtd.get_joint_states() # ur10.set_joint_target_positions(q) #q = np.random.randn(6,) #print(q) ur10.set_joint_target_positions(q) # raw distance readings base = expr.get_proximity_reading_from('Base') elbow = expr.get_proximity_reading_from('Elbow') tool = expr.get_proximity_reading_from('Tool') # 3d point readings with id base_pc, base_obj = expr.get_points_from('Base') elbow_pc, elbow_obj = expr.get_points_from('Elbow') tool_pc, tool_obj = expr.get_points_from('Tool') base_obs = np.hstack([base_pc, base_obj]) elbow_obs = np.hstack([elbow_pc, elbow_obj]) tool_obs = np.hstack([tool_pc, tool_obj]) complete_obs = np.vstack([base_obs, elbow_obs, tool_obs]) # human position hum_position = np.array(expr.get_human_position()).reshape(1, 3) if len(buffer) == 1: buffer.popleft() else: buffer.append(complete_obs) if len(buffer) > 0: buff_stack = np.vstack(buffer) socket.send_pyobj([buff_stack, hum_position, q]) pr.step() sim_step += 1 # np.save('data.npy', np.vstack(data_store)) # np.save('gt.npy', np.vstack(ground_truth)) rtd.stop() pr.stop() # Stop the simulation pr.shutdown() # Close the application import sys sys.exit()
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, ): 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 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 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) 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)
class EnvironmentImpl(Environment): """Each environment has a scene.""" @staticmethod def all_task_names(): return tuple(o[0] for o in getmembers(tasks) if isclass(o[1])) def __init__(self, task_name: str, obs_config: ObservationConfig = ObservationConfig(task_low_dim_state=True), action_mode: ActionMode = ActionMode(), arm_name: str = "Panda", gripper_name: str = "Panda_gripper"): super(EnvironmentImpl, self).__init__(task_name) self._arm_name = arm_name self._gripper_name = gripper_name self._action_mode = action_mode self._obs_config = obs_config # TODO: modify the task/robot/arm/gripper to support early instantiation before v-rep launched self._task = None self._pyrep = None self._robot = None self._scene = None self._variation_number = 0 self._reset_called = False self._prev_ee_velocity = None self._update_info_dict() def init(self, display=False): if self._pyrep is not None: self.finalize() with suppress_std_out_and_err(): self._pyrep = PyRep() # TODO: TTT_FILE should be defined by robot, but now robot depends on launched pyrep self._pyrep.launch(join(DIR_PATH, TTT_FILE), headless=not display) self._pyrep.set_simulation_timestep(0.005) # TODO: Load arm and gripper from name self._robot = Robot(Panda(), PandaGripper()) self._scene = Scene(self._pyrep, self._robot, self._obs_config) self._set_arm_control_action() # Str comparison because sometimes class comparison doesn't work. if self._task is not None: self._task.unload() self._task = self._get_class_by_name(self._task_name, tasks)(self._pyrep, self._robot) self._scene.load(self._task) self._pyrep.start() def finalize(self): with suppress_std_out_and_err(): self._pyrep.shutdown() self._pyrep = None def reset(self, random: bool = True) -> StepDict: logging.info('Resetting task: %s' % self._task.get_name()) self._scene.reset() try: # TODO: let desc be constant desc = self._scene.init_episode(self._variation_number, max_attempts=MAX_RESET_ATTEMPTS, randomly_place=random) except (BoundaryError, WaypointError) as e: raise TaskEnvironmentError( 'Could not place the task %s in the scene. This should not ' 'happen, please raise an issues on this task.' % self._task.get_name()) from e ctr_loop = self._robot.arm.joints[0].is_control_loop_enabled() locked = self._robot.arm.joints[0].is_motor_locked_at_zero_velocity() self._robot.arm.set_control_loop_enabled(False) self._robot.arm.set_motor_locked_at_zero_velocity(True) self._reset_called = True self._robot.arm.set_control_loop_enabled(ctr_loop) self._robot.arm.set_motor_locked_at_zero_velocity(locked) # Returns a list o f descriptions and the first observation return {'s': self._scene.get_observation().get_low_dim_data(), "opt": desc} def step(self, last_step: StepDict) -> (StepDict, bool): # returns observation, reward, done, info if not self._reset_called: raise RuntimeError("Call 'reset' before calling 'step' on a task.") assert 'a' in last_step, "Key 'a' for action not in last_step, maybe you passed a wrong dict ?" # action should contain 1 extra value for gripper open close state arm_action = np.array(last_step['a'][:-1]) ee_action = last_step['a'][-1] current_ee = (1.0 if self._robot.gripper.get_open_amount()[0] > 0.9 else 0.0) if ee_action > 0.0: ee_action = 1.0 elif ee_action < -0.0: ee_action = 0.0 if self._action_mode.arm == ArmActionMode.ABS_JOINT_VELOCITY: self._assert_action_space(arm_action, (len(self._robot.arm.joints),)) self._robot.arm.set_joint_target_velocities(arm_action) elif self._action_mode.arm == ArmActionMode.DELTA_JOINT_VELOCITY: self._assert_action_space(arm_action, (len(self._robot.arm.joints),)) cur = np.array(self._robot.arm.get_joint_velocities()) self._robot.arm.set_joint_target_velocities(cur + arm_action) elif self._action_mode.arm == ArmActionMode.ABS_JOINT_POSITION: self._assert_action_space(arm_action, (len(self._robot.arm.joints),)) self._robot.arm.set_joint_target_positions(arm_action) elif self._action_mode.arm == ArmActionMode.DELTA_JOINT_POSITION: self._assert_action_space(arm_action, (len(self._robot.arm.joints),)) cur = np.array(self._robot.arm.get_joint_positions()) self._robot.arm.set_joint_target_positions(cur + arm_action) elif self._action_mode.arm == ArmActionMode.ABS_EE_POSE: self._assert_action_space(arm_action, (7,)) self._ee_action(list(arm_action)) elif self._action_mode.arm == ArmActionMode.DELTA_EE_POSE: self._assert_action_space(arm_action, (7,)) a_x, a_y, a_z, a_qx, a_qy, a_qz, a_qw = arm_action x, y, z, qx, qy, qz, qw = self._robot.arm.get_tip().get_pose() new_rot = Quaternion(a_qw, a_qx, a_qy, a_qz) * Quaternion(qw, qx, qy, qz) qw, qx, qy, qz = list(new_rot) new_pose = [a_x + x, a_y + y, a_z + z] + [qx, qy, qz, qw] self._ee_action(list(new_pose)) elif self._action_mode.arm == ArmActionMode.ABS_EE_VELOCITY: self._assert_action_space(arm_action, (7,)) pose = self._robot.arm.get_tip().get_pose() new_pos = np.array(pose) + (arm_action * DT) self._ee_action(list(new_pos)) elif self._action_mode.arm == ArmActionMode.DELTA_EE_VELOCITY: self._assert_action_space(arm_action, (7,)) if self._prev_ee_velocity is None: self._prev_ee_velocity = np.zeros((7,)) self._prev_ee_velocity += arm_action pose = self._robot.arm.get_tip().get_pose() pose = np.array(pose) new_pose = pose + (self._prev_ee_velocity * DT) self._ee_action(list(new_pose)) elif self._action_mode.arm == ArmActionMode.ABS_JOINT_TORQUE: self._assert_action_space(arm_action, (len(self._robot.arm.joints),)) self._torque_action(arm_action) elif self._action_mode.arm == ArmActionMode.DELTA_JOINT_TORQUE: cur = np.array(self._robot.arm.get_joint_forces()) new_action = cur + arm_action self._torque_action(new_action) else: raise RuntimeError('Unrecognised action mode.') if current_ee != ee_action: done = False while not done: done = self._robot.gripper.actuate(ee_action, velocity=0.04) self._pyrep.step() self._task.step() if ee_action == 0.0: # If gripper close action, the check for grasp. for g_obj in self._task.get_graspable_objects(): self._robot.gripper.grasp(g_obj) else: # If gripper opem action, the check for ungrasp. self._robot.gripper.release() self._scene.step() success, terminate = self._task.success() last_step['r'] = int(success) next_step = {'s': self._scene.get_observation().get_low_dim_data(), "opt": None} return last_step, next_step, terminate def name(self) -> str: return self._task_name # ------------- private methods ------------- # def _update_info_dict(self): # update info dict self._info["action mode"] = self._action_mode self._info["observation mode"] = self._obs_config # TODO: action dim should related to robot, not action mode, here we fixed it temporally self._info["action dim"] = (8,) self._info["action low"] = np.zeros(self._info["action dim"], dtype=np.float32) - 1. self._info["action high"] = np.zeros(self._info["action dim"], dtype=np.float32) + 1. self._info["state dim"] = (73,) self._info["state low"] = np.zeros(self._info["state dim"], dtype=np.float32) - 100. self._info["state high"] = np.zeros(self._info["state dim"], dtype=np.float32) + 100. self._info["reward low"] = -np.inf self._info["reward high"] = np.inf def _set_arm_control_action(self): self._robot.arm.set_control_loop_enabled(True) if self._action_mode.arm in (ArmActionMode.ABS_JOINT_VELOCITY, 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 in (ArmActionMode.ABS_JOINT_POSITION, ArmActionMode.DELTA_JOINT_POSITION, ArmActionMode.ABS_EE_POSE, ArmActionMode.DELTA_EE_POSE, ArmActionMode.ABS_EE_VELOCITY, ArmActionMode.DELTA_EE_VELOCITY): self._robot.arm.set_control_loop_enabled(True) elif self._action_mode.arm in (ArmActionMode.ABS_JOINT_TORQUE, ArmActionMode.DELTA_JOINT_TORQUE): self._robot.arm.set_control_loop_enabled(False) else: raise RuntimeError('Unrecognised action mode.') def sample_variation(self) -> int: self._variation_number = np.random.randint(0, self._task.variation_count()) return self._variation_number def _assert_action_space(self, action, expected_shape): if np.shape(action) != expected_shape: raise RuntimeError( 'Expected the action shape to be: %s, but was shape: %s' % ( str(expected_shape), str(np.shape(action)))) def _torque_action(self, action): self._robot.arm.set_joint_target_velocities([(TORQUE_MAX_VEL if t < 0 else -TORQUE_MAX_VEL) for t in action]) self._robot.arm.set_joint_forces(np.abs(action)) def _ee_action(self, action): try: joint_positions = self._robot.arm.solve_ik(action[:3], quaternion=action[3:]) self._robot.arm.set_joint_target_positions(joint_positions) except IKError as e: raise InvalidActionError("Could not find a path.") from e self._pyrep.step() @staticmethod def _get_class_by_name(class_name: str, model: ModuleType) -> TaskClass: all_class_dict = {} for o in getmembers(model): if isclass(o[1]): all_class_dict[o[0]] = o[1] if class_name not in all_class_dict: raise NotImplementedError(f"No class {class_name} found in {model.__name__} !") return all_class_dict[class_name]