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]
