def test_run_task_validator(self):
for task_file in TASKS:
test_name = task_file.split('.py')[0]
with self.subTest(task=test_name):
if test_name in FLAKY_TASKS:
self.skipTest('Flaky task.')
sim = PyRep()
ttt_file = os.path.join(
DIR_PATH, '..', '..', 'rlbench', TTT_FILE)
sim.launch(ttt_file, headless=True)
sim.step_ui()
sim.set_simulation_timestep(50.0)
sim.step_ui()
sim.start()
robot = Robot(Panda(), PandaGripper())
obs = ObservationConfig()
obs.set_all(False)
scene = Scene(sim, robot, obs)
sim.start()
task_class = task_file_to_task_class(task_file)
active_task = task_class(sim, robot)
try:
task_smoke(active_task, scene, variation=-1,
max_variations=2, success=0.25)
except Exception as e:
sim.stop()
sim.shutdown()
raise e
sim.stop()
sim.shutdown()
class BaseEnv:
def __init__(self, scene_file="", headless=False):
self.pr = PyRep()
# Launch the application with a scene file in headless mode
self.pr.launch(scene_file, headless=headless)
self.pr.start() # Start the simulation
def step(self):
self.pr.step()
def stop(self):
self.pr.stop()
self.pr.shutdown()
def load_scene_object_from_file(self, file_path):
respondable = self.pr.import_model(file_path)
visible = respondable.get_objects_in_tree(exclude_base=True)[0]
return SceneObject(respondable_part=respondable, visible_part=visible)
def load_mesh_from_file(self, file_path, scaling_factor=1):
shape = Shape.import_shape(filename=file_path,
scaling_factor=scaling_factor)
self.pr.step()
return shape
class TestSuctionCups(TestCore):
def setUp(self):
self.pyrep = PyRep()
self.pyrep.launch(path.join(ASSET_DIR, 'test_scene_robots.ttt'),
headless=True)
self.pyrep.step()
self.pyrep.start()
def test_get_suction_cup(self):
for cup_name, cup_type in SUCTION_CUPS:
with self.subTest(suction_cup=cup_name):
cup = cup_type()
self.assertIsInstance(cup, cup_type)
def test_grasp_and_release_with_suction(self):
for cup_name, cup_type in SUCTION_CUPS:
with self.subTest(suction_cup=cup_name):
suction_cube = Shape('%s_cube' % cup_name)
cube = Shape('cube')
cup = cup_type()
self.pyrep.step()
grasped = cup.grasp(cube)
self.assertFalse(grasped)
self.assertEqual(len(cup.get_grasped_objects()), 0)
grasped = cup.grasp(suction_cube)
self.assertTrue(grasped)
self.assertListEqual(cup.get_grasped_objects(), [suction_cube])
cup.release()
self.assertEqual(len(cup.get_grasped_objects()), 0)
class SimulationConsumerAbstract(mp.Process):
_id = 0
"""This class sole purpose is to better 'hide' all interprocess related code
from the user."""
def __init__(self, process_io, scene="", gui=False):
super().__init__(name="simulation_consumer_{}".format(
SimulationConsumerAbstract._id))
self._id = SimulationConsumerAbstract._id
SimulationConsumerAbstract._id += 1
self._scene = scene
self._gui = gui
self._process_io = process_io
np.random.seed()
def run(self):
self._pyrep = PyRep()
self._pyrep.launch(self._scene,
headless=not self._gui,
write_coppeliasim_stdout_to_file=True)
self._process_io["simulaton_ready"].set()
self._main_loop()
def _close_pipes(self):
self._process_io["command_pipe_out"].close()
self._process_io["return_value_pipe_in"].close()
# self._process_io["exception_pipe_in"].close() # let this one open
def _main_loop(self):
success = True
while success and not self._process_io["must_quit"].is_set():
success = self._consume_command()
self._pyrep.shutdown()
self._close_pipes()
def _consume_command(self):
try: # to execute the command and send result
success = True
command = self._process_io["command_pipe_out"].recv()
self._process_io["slot_in_command_queue"].release()
ret = command[0](self, *command[1], **command[2])
if command[0]._communicate_return_value:
self._communicate_return_value(ret)
except Exception as e: # print traceback, dont raise
traceback = format_exc()
success = False # return False: quit the main loop
self._process_io["exception_pipe_in"].send((e, traceback))
finally:
return success
def _communicate_return_value(self, value):
self._process_io["return_value_pipe_in"].send(value)
def signal_command_pipe_empty(self):
self._process_io["command_pipe_empty"].set()
while self._process_io["command_pipe_empty"].is_set():
time.sleep(0.1)
def good_bye(self):
pass
class TestCore(unittest.TestCase):
def setUp(self):
self.pyrep = PyRep()
self.pyrep.launch(path.join(ASSET_DIR, 'test_scene.ttt'),
headless=True)
self.pyrep.step()
self.pyrep.start()
def tearDown(self):
self.pyrep.stop()
self.pyrep.step_ui()
self.pyrep.shutdown()
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 Plane3D:
def __init__(self, headless=False):
self._pr = PyRep()
self._pr.launch(scene_file=scene_file, headless=headless)
self._pr.start()
self.workspace_base = Shape("workspace")
self.workspace = self._get_worksapce()
self.camera = VisionSensor("camera")
self.obstacles = []
self.velocity_scale = 0
self.repiration_cycle = 0
def _get_worksapce(self):
base_pos = self.workspace_base.get_position()
bbox = self.workspace_base.get_bounding_box()
min_pos = [bbox[2 * i] + base_pos[i] for i in range(3)]
max_pos = [bbox[2 * i + 1] + base_pos[i] for i in range(3)]
return [min_pos, max_pos]
def reset(self, obstacle_num, velocity_scale, respiration_cycle=0):
for obstacle in self.obstacles:
obstacle.remove()
self._pr.step()
self.velocity_scale = velocity_scale
self.repiration_cycle = respiration_cycle
self.obstacles = []
for i in range(obstacle_num):
obs = Obstacle2D.create_random_obstacle(
workspace=self.workspace,
velocity_scale=velocity_scale,
respiration_cycle=respiration_cycle)
self._pr.step()
self.obstacles.append(obs)
def get_image(self):
rgb = self.camera.capture_rgb()
return rgb
def step(self):
# update config
for obs in self.obstacles:
if self.repiration_cycle > 0:
obs.respire()
if self.velocity_scale > 0:
obs.keep_velocity()
self._pr.step()
class ReacherEnv(object):
def __init__(self):
self.pr = PyRep()
self.pr.launch(SCENE_FILE, headless=False)
self.pr.start()
# self.agent = Panda()
self.agent = Sawyer()
self.gripper = BaxterGripper()
self.agent.set_control_loop_enabled(False)
self.agent.set_motor_locked_at_zero_velocity(True)
self.target = Shape('target')
self.agent_ee_tip = self.agent.get_tip()
self.initial_joint_positions = self.agent.get_joint_positions()
def _get_state(self):
# Return state containing arm joint angles/velocities & target position
return (self.agent.get_joint_positions() +
self.agent.get_joint_velocities() + self.target.get_position())
def reset(self):
# Get a random position within a cuboid and set the target position
pos = list(np.random.uniform(POS_MIN, POS_MAX))
self.target.set_position(pos)
self.agent.set_joint_positions(self.initial_joint_positions)
self.pr.step()
return self._get_state()
def step(self, action):
self.agent.set_joint_target_velocities(action) # Execute action on arm
self.pr.step() # Step the physics simulation
ax, ay, az = self.agent_ee_tip.get_position()
tx, ty, tz = self.target.get_position()
# Reward is negative distance to target
distance = (ax - tx)**2 + (ay - ty)**2 + (az - tz)**2
done = False
if distance < 5:
done = True
self.gripper.actuate(
0, velocity=0.04
) # if done, close the hand, 0 for close and 1 for open.
self.pr.step() # Step the physics simulation
reward = -np.sqrt(distance)
return self._get_state(), reward, done
def shutdown(self):
self.pr.stop()
self.pr.shutdown()
class TestArmsAndConfigurationPaths(TestCore):
def setUp(self):
self.pyrep = PyRep()
self.pyrep.launch(path.join(
ASSET_DIR, 'test_scene_robots.ttt'), headless=True)
self.pyrep.step()
self.pyrep.start()
def test_get_gripper(self):
for gripper_name, gripper_type, vel in GRIPPERS:
with self.subTest(gripper=gripper_name):
gripper = gripper_type()
self.assertIsInstance(gripper, gripper_type)
def test_close_open_gripper(self):
for gripper_name, gripper_type, vel in GRIPPERS:
with self.subTest(gripper=gripper_name):
gripper = gripper_type()
self.pyrep.step()
done = False
i = 0
while not done:
done = gripper.actuate(0.0, velocity=vel)
self.pyrep.step()
i += 1
if i > 1000:
self.fail('Took too many steps to close')
done = False
i = 0
open_amount = 1.0 if gripper_name == 'Robotiq85Gripper' else 0.8
while not done:
done = gripper.actuate(open_amount, velocity=vel)
self.pyrep.step()
i += 1
if i > 1000:
self.fail('Took too many steps to open')
self.assertAlmostEqual(
gripper.get_open_amount()[0], open_amount, delta=0.05)
def test_get_duplicate_gripper(self):
g = BaxterGripper(1)
self.assertIsInstance(g, BaxterGripper)
def test_copy_gripper(self):
g = JacoGripper()
new_g = g.copy()
self.assertNotEqual(g, new_g)
class ReacherEnv(object):
def __init__(self):
self.pr = PyRep()
self.pr.launch(SCENE_FILE, headless=False)
self.pr.start()
self.agent = TurtleBot()
self.agent.set_control_loop_enabled(False)
self.agent.set_motor_locked_at_zero_velocity(True)
self.target = Shape.create(type=PrimitiveShape.SPHERE,
size=[0.05, 0.05, 0.05],
color=[7.0, 0.5, 0.5],
static=True,
respondable=False)
# self.target = Shape('target')
# self.agent_ee_tip = self.agent.get_tip()
self.initial_joint_positions = self.agent.get_joint_positions()
def _get_state(self):
# Return state containing arm joint angles/velocities & target position
return np.concatenate([
self.agent.get_base_actuation(),
self.agent.get_base_velocities(),
self.target.get_position()
])
def reset(self):
# Get a random position within a cuboid and set the target position
pos = list(np.random.uniform(POS_MIN, POS_MAX))
pos_agent = list(np.random.uniform([0, 0, 0], [0, 0, 0]))
self.target.set_position(pos)
self.agent.set_position(pos_agent)
return self._get_state()
def step(self, action):
self.agent.set_joint_target_velocities(action) # Execute action on arm
self.pr.step() # Step the physics simulation
ax, ay, az = self.agent.get_position()
tx, ty, tz = self.target.get_position()
# Reward is negative distance to target
reward = -np.sqrt((ax - tx)**2 + (ay - ty)**2 + (az - tz)**2)
print(int(reward * 100), int(ax * 100), int(ay * 100), int(az * 100),
int(tx * 100), int(ty * 100), int(tz * 100))
return reward, self._get_state()
def shutdown(self):
self.pr.stop()
self.pr.shutdown()
class TestScene(unittest.TestCase):
"""Tests the following:
- Getting observations from the scene
- Applying noise
- Applying domain randomization
"""
def setUp(self):
self.pyrep = PyRep()
self.pyrep.launch(join(environment.DIR_PATH, TTT_FILE), headless=True)
self.pyrep.set_simulation_timestep(0.005)
self.robot = Robot(Panda(), PandaGripper())
def tearDown(self):
self.pyrep.shutdown()
def test_sensor_noise_images(self):
cam_config = CameraConfig(rgb_noise=GaussianNoise(0.05, (.0, 1.)))
obs_config = ObservationConfig(left_shoulder_camera=cam_config,
joint_forces=False,
task_low_dim_state=False)
scene = Scene(self.pyrep, self.robot, obs_config)
scene.load(ReachTarget(self.pyrep, self.robot))
obs1 = scene.get_observation()
obs2 = scene.get_observation()
self.assertTrue(
np.array_equal(obs1.right_shoulder_rgb, obs2.right_shoulder_rgb))
self.assertFalse(
np.array_equal(obs1.left_shoulder_rgb, obs2.left_shoulder_rgb))
self.assertTrue(obs1.left_shoulder_rgb.max() <= 1.0)
self.assertTrue(obs1.left_shoulder_rgb.min() >= 0.0)
def test_sensor_noise_robot(self):
obs_config = ObservationConfig(
joint_velocities_noise=GaussianNoise(0.01),
joint_forces=False,
task_low_dim_state=False)
scene = Scene(self.pyrep, self.robot, obs_config)
scene.load(ReachTarget(self.pyrep, self.robot))
obs1 = scene.get_observation()
obs2 = scene.get_observation()
self.assertTrue(
np.array_equal(obs1.joint_positions, obs2.joint_positions))
self.assertFalse(
np.array_equal(obs1.joint_velocities, obs2.joint_velocities))
class VrepSim(object):
"""Class that interfaces with Habitat sim"""
def __init__(
self, configs, scene_path=None, seed=0
): # TODO: extend the arguments of constructor
self.sim_config = copy.deepcopy(configs.COMMON.SIMULATOR)
if scene_path is None:
raise RuntimeError("Please specify the .ttt v-rep scene file path!")
self.sim = PyRep()
self.sim.launch(scene_path, headless=False)
self.sim.start()
[self.sim.step() for _ in range(50)]
def __del__(self):
self.sim.stop()
self.sim.shutdown()
def reset(self):
"""Reset the Habitat simultor"""
raise NotImplemented("Reset method for v-rep has not been implemented")
class ReacherEnv(object):
def __init__(self):
self.pr = PyRep()
self.pr.launch(SCENE_FILE, headless=False)
self.pr.start()
self.agent = Pioneer()
self.agent.set_control_loop_enabled(False)
self.agent.set_motor_locked_at_zero_velocity(True)
self.target = Shape('target')
self.agent_ee_tip = self.agent.get_tip()
self.initial_joint_positions = self.agent.get_joint_positions()
def reset(self):
# Get a random position within a cuboid and set the target position
pos = list(np.random.uniform(POS_MIN, POS_MAX))
self.target.set_position(pos)
self.agent.set_joint_positions(self.initial_joint_positions)
return self._get_state()
def step(self, action):
self.agent.set_joint_target_velocities(action) # Execute action on arm
self.pr.step() # Step the physics simulation
ax, ay, az = self.agent_ee_tip.get_position()
tx, ty, tz = self.target.get_position()
# Reward is negative distance to target
reward = -np.sqrt((ax - tx)**2 + (ay - ty)**2 + (az - tz)**2)
return reward, self._get_state()
def _get_state(self):
# Return state containing arm joint angles/velocities & target position
return np.concatenate([
self.agent.get_joint_positions(),
self.agent.get_joint_velocities(),
self.target.get_position()
])
def shutdown(self):
self.pr.stop()
self.pr.shutdown()
class Environment:
def __init__(
self,
texture="/home/aecgroup/aecdata/Textures/mcgillManMade_600x600_png_selection/",
scene="/home/aecgroup/aecdata/Software/vrep_scenes/stereo_vision_robot_collection.ttt",
headless=True):
self.pyrep = PyRep()
self.pyrep.launch(scene, headless=headless)
min_distance = 0.5
max_distance = 5
max_speed = 0.5
path = texture
textures_names = listdir(path)
textures_list = [
self.pyrep.create_texture(path + name)[1]
for name in textures_names
]
self.screen = RandomScreen(min_distance, max_distance, max_speed,
textures_list)
self.robot = StereoVisionRobot(min_distance, max_distance)
self.pyrep.start()
def step(self):
# step simulation
self.pyrep.step()
# move screen
self.screen.increment_iteration()
def episode_reset(self, preinit=False):
# reset screen
self.screen.episode_reset(preinit=preinit)
# reset robot
self.robot.episode_reset()
# self.robot.set_vergence_position(to_angle(self.screen.distance))
self.pyrep.step()
def close(self):
self.pyrep.stop()
self.pyrep.shutdown()
class TestArmsAndConfigurationPaths(TestCore):
def setUp(self):
self.pyrep = PyRep()
self.pyrep.launch(path.join(
ASSET_DIR, 'test_scene_robots.ttt'), headless=True)
self.pyrep.step()
self.pyrep.start()
def test_get_gripper(self):
for gripper_name, gripper_type, vel in GRIPPERS:
with self.subTest(gripper=gripper_name):
gripper = gripper_type()
self.assertIsInstance(gripper, gripper_type)
def test_close_open_gripper(self):
for gripper_name, gripper_type, vel in GRIPPERS:
with self.subTest(gripper=gripper_name):
gripper = gripper_type()
self.pyrep.step()
done = False
i = 0
while not done:
done = gripper.actuate(0.0, velocity=vel)
self.pyrep.step()
i += 1
if i > 1000:
self.fail('Took too many steps to close')
done = False
i = 0
while not done:
done = gripper.actuate(0.8, velocity=vel)
self.pyrep.step()
i += 1
if i > 1000:
self.fail('Took too many steps to open')
self.assertAlmostEqual(
gripper.get_open_amount()[0], 0.8, delta=0.05)
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())
# import ipdb;ipdb.set_trace()
b = cos(angle) * 0.5
a = sin(angle) * 0.5
pt0 = (int(x - a * l - b * w),
int(y + b * l - a * w))
pt1 = (int(x + a * l - b * w),
int(y - b * l - a * w))
pt2 = (int(2 * x - pt0[0]), int(2 * y - pt0[1])) # (int(x + a * l + b * w), int(y - b * l + a * w))
pt3 = (int(2 * x - pt1[0]), int(2 * y - pt1[1]))
cv2.fillConvexPoly(img, np.array([pt0, pt1, pt2, pt3]), color=[height])
return img
if __name__ == "__main__":
# Unit test
pr = PyRep()
pr.launch("scene/blank_robotnik.ttt", headless=False)
pr.start()
# env range is 5*5
pos_min = [-2, -1.3, 0]
pos_max = [1.3, 2, 0]
obstacle_num=1
# agent
agent = nik('Robotnik')
# path
target = Shape.create(type=PrimitiveShape.SPHERE,
size=[0.05, 0.05, 0.05],
class PyRepModule(object):
def __init__(self, logger, headless=False):
self.logger = logger
self.callback = {
PyRepRequestType.initialize_part_to_scene:
self.initialize_part_to_scene,
PyRepRequestType.update_group_to_scene:
self.update_group_to_scene,
PyRepRequestType.get_assembly_point:
self.get_assembly_point,
PyRepRequestType.update_part_status:
self.update_part_status,
PyRepRequestType.get_cost_of_available_pair:
self.get_cost_of_available_pair
}
self.pr = PyRep()
self.pr.launch(headless=headless)
self.pr.start()
self.scene_path = "./test_scene"
# used to visualize and assembly
self.part_info = None
self.part_status = None
self.primitive_parts = {}
self.group_info = None
self.group_obj = {}
def initialize_server(self):
self.logger.info("Initialize PyRep Server")
host = SocketType.pyrep.value["host"]
port = SocketType.pyrep.value["port"]
self.server = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.server.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
self.server.bind((host, port))
self.server.listen(True)
self.host = host
self.port = port
try:
self.logger.info("Waiting for PyRep client {}:{}".format(
self.host, self.port))
self.connected_client, addr = self.server.accept()
self.logger.info("Connected to {}".format(addr))
except:
self.logger.info("PyRep Server Error")
finally:
self.server.close()
def get_callback(self, request):
return self.callback[request]
def save_scene(self, path):
self.pr.export_scene(path)
def close(self):
self.connected_client.close()
self.server.close()
self.pr.stop()
self.pr.shutdown()
#region socket function
def initialize_part_to_scene(self):
global ASSEMBLY_PAIR
self.logger.info("ready to initialize part to scene")
sendall_pickle(self.connected_client, True)
request = recvall_pickle(self.connected_client)
self.part_info = request["part_info"]
ASSEMBLY_PAIR = request["pair_info"]
self.logger.info("...initializing pyrep scene")
for part_name in self.part_info.keys():
self._import_part_info(part_name)
self.logger.info("End to initialize pyrep scene")
sendall_pickle(self.connected_client, True)
def _import_part_info(self, part_name):
assert self.part_info
self.logger.info("import part info of {}".format(part_name))
obj_path = self.part_info[part_name]["obj_file"]
# part_obj = Shape.import_mesh(obj_path, scaling_factor=0.001)
part_obj = ObjObject.create_object(obj_path=obj_path)
part_obj.set_name(part_name)
# import each assembly points to scene
assembly_points = self.part_info[part_name]["assembly_points"]
points = {}
for idx in assembly_points.keys():
ap = assembly_points[idx]
radius = ap["radius"] / 1000
depth = ap["depth"]
position = ap["pose"]["position"]
quaternion = ap["pose"]["quaternion"]
direction = ap["direction"]
assembly_point = Shape.create(PrimitiveShape.SPHERE,
size=[radius] * 3,
respondable=False,
static=True,
color=[1, 0, 0])
pose = position + quaternion
assembly_point.set_pose(pose)
assembly_point.set_name("{}_AP_{}".format(part_name, idx))
assembly_point.set_parent(part_obj.shape)
points[idx] = assembly_point
part_region_info = self.part_info[part_name]["region_info"]
region_info = {}
for region_id in part_region_info.keys():
position = part_region_info[region_id]["position"]
point_list = part_region_info[region_id]["points"]
region_shape = Shape.create(PrimitiveShape.SPHERE,
size=[0.05] * 3,
respondable=False,
static=True,
color=[0, 1, 0])
region_shape.set_name("{}_region_{}".format(part_name, region_id))
region_shape.set_parent(part_obj.shape)
region_shape.set_position(position)
for point_idx in point_list:
points[point_idx].set_parent(region_shape)
region_info[region_id] = {
"shape": region_shape,
"points": point_list
}
self.primitive_parts[part_name] = PartObject(part_object=part_obj,
assembly_points=points,
region_info=region_info)
def update_group_to_scene(self):
self.logger.info("ready to update group to scene")
sendall_pickle(self.connected_client, True)
request = recvall_pickle(self.connected_client)
new_group_info = request["group_info"]
self.logger.info("...updating group to scene")
self._update_group_obj(new_group_info)
self.logger.info("End to update group to scene")
sendall_pickle(self.connected_client, True)
def _update_group_obj(self, new_group_info):
for group_id in self.group_obj.keys():
self.group_obj[group_id].remove()
self.group_obj = {}
for group_id in new_group_info.keys():
if not new_group_info[group_id]["is_exist"]:
continue
self.logger.info("update group_{} to scene".format(group_id))
group_info = new_group_info[group_id]
obj_root = group_info["obj_root"]
file_list = get_file_list(obj_root)
base_obj = None
composed_parts = []
composed_objects = []
group_pose = load_yaml_to_dic(join(obj_root, "group_pose.yaml"))
composed_part_pose = {}
for file_path in file_list:
obj_name, ext = get_file_name(file_path)
if not ext == ".obj":
continue
if obj_name == "base": # import base object
base_obj = ObjObject.create_object(file_path)
base_obj.set_name("G{}".format(group_id))
else: # ikea_stefan_bottom_0
instance_id = obj_name.split("_")[-1]
part_name = obj_name.replace("_{}".format(instance_id), "")
obj = ObjObject.create_object(file_path)
obj.set_name("G{}_{}_{}".format(group_id, part_name,
instance_id))
part_instance = {
"part_name": part_name,
"instance_id": int(instance_id)
}
composed_parts.append(part_instance)
composed_part_idx = composed_parts.index(part_instance)
composed_part_pose[composed_part_idx] = group_pose[
obj_name]
object_dict = {
"group_object": obj,
"primitive": self.primitive_parts[part_name]
}
composed_objects.append(object_dict)
assert base_obj, "No base object"
composed_parts = tuple(composed_parts)
composed_objects = tuple(composed_objects)
for object_dict in composed_objects:
obj = object_dict["group_object"]
obj.set_parent(base_obj.shape)
self.group_obj[group_id] = GroupObject(base_obj, composed_parts,
composed_objects,
composed_part_pose)
def update_part_status(self):
self.logger.info("ready to update part_status")
sendall_pickle(self.connected_client, True)
request = recvall_pickle(self.connected_client)
self.part_status = request["part_status"]
self.logger.info("Update part_status")
sendall_pickle(self.connected_client, True)
#TODO: delete assembly point
def get_assembly_point(self):
self.logger.info("ready to get assembly point from scene")
sendall_pickle(self.connected_client, True)
request = recvall_pickle(self.connected_client)
group_id = int(request["group_id"])
connection_locs = request["connection_locs"]
connector_name = request["connector_name"]
#TODO:
self.logger.info(
"...get assembly points of group {} from scene".format(group_id))
target_group = self.group_obj[group_id]
assembly_points = None
try:
assembly_points = target_group.get_assembly_points(
locations=connection_locs,
connector_name=connector_name,
part_status=self.part_status)
except Exception as e:
self.logger.info("Error occur {}".format(e))
self.save_scene(
join(self.scene_path,
"error_scene_{}.ttt".format(get_time_stamp())))
self.logger.info("End to get assembly point from pyrep scene")
sendall_pickle(self.connected_client, assembly_points)
self.save_scene(
join(self.scene_path,
"test_scene_{}.ttt".format(get_time_stamp())))
def get_cost_of_available_pair(self):
self.logger.info("ready to get cost from scene")
sendall_pickle(self.connected_client, True)
request = recvall_pickle(self.connected_client)
group_id = int(request["group_id"])
pair_list = request["check_pair"]
self.logger.info("...get cost of group {} from scene".format(group_id))
target_group = self.group_obj[group_id]
pair_cost = None
try:
pair_cost = target_group.get_cost_between_pair(pair_list)
except Exception as e:
self.logger.info("Error occur {}".format(e))
self.save_scene("test_error_scene/error_scene_{}.ttt".format(
get_time_stamp()))
self.logger.info("End to get assembly point from pyrep scene")
sendall_pickle(self.connected_client, pair_cost)
self.save_scene("test_scene/test_scene_{}.ttt".format(
get_time_stamp()))
class 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]
if __name__ == '__main__':
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()
class SpecificWorker(GenericWorker):
def __init__(self, proxy_map):
super(SpecificWorker, self).__init__(proxy_map)
def __del__(self):
print('SpecificWorker destructor')
def setParams(self, params):
SCENE_FILE = '../../etc/basic_scene.ttt'
self.pr = PyRep()
self.pr.launch(SCENE_FILE, headless=False)
self.pr.start()
self.robot = TurtleBot()
self.drone = Shape('Quadricopter')
cam = VisionSensor("frontCamera")
self.camera = {
"handle":
cam,
"id":
0,
"angle":
np.radians(cam.get_perspective_angle()),
"width":
cam.get_resolution()[0],
"height":
cam.get_resolution()[1],
"focal": (cam.get_resolution()[0] / 2) /
np.tan(np.radians(cam.get_perspective_angle() / 2)),
"rgb":
np.array(0),
"depth":
np.ndarray(0)
}
self.joystick_newdata = []
self.speed_robot = []
self.speed_robot_ant = []
self.last_received_data_time = 0
self.once = False
#@QtCore.Slot()
def compute(self):
cont = 0
start = time.time()
while True:
self.pr.step()
self.read_camera(self.camera)
self.read_joystick()
elapsed = time.time() - start
if elapsed < 0.05:
time.sleep(0.05 - elapsed)
cont += 1
if elapsed > 1:
print("Freq -> ", cont)
cont = 0
start = time.time()
###########################################
def read_camera(self, cam):
image_float = cam["handle"].capture_rgb()
depth = cam["handle"].capture_depth(True)
image = cv2.normalize(src=image_float,
dst=None,
alpha=0,
beta=255,
norm_type=cv2.NORM_MINMAX,
dtype=cv2.CV_8U)
cam["rgb"] = RoboCompCameraRGBDSimple.TImage(cameraID=cam["id"],
width=cam["width"],
height=cam["height"],
depth=3,
focalx=cam["focal"],
focaly=cam["focal"],
alivetime=time.time(),
image=image.tobytes())
cam["depth"] = RoboCompCameraRGBDSimple.TDepth(
cameraID=cam["id"],
width=cam["handle"].get_resolution()[0],
height=cam["handle"].get_resolution()[1],
focalx=cam["focal"],
focaly=cam["focal"],
alivetime=time.time(),
depthFactor=1.0,
depth=depth.tobytes())
# try:
# self.camerargbdsimplepub_proxy.pushRGBD(cam["rgb"], cam["depth"])
# except Ice.Exception as e:
# print(e)
###########################################
### JOYSITCK read and move the robot
###########################################
def read_joystick(self):
if self.joystick_newdata: #and (time.time() - self.joystick_newdata[1]) > 0.1:
datos = self.joystick_newdata[0]
adv = 0.0
rot = 0.0
side = 0.0
height = 0.0
for x in datos.axes:
if x.name == "advance":
adv = x.value / 20 if np.abs(x.value) > 0.001 else 0
if x.name == "rotate":
side = x.value / 20 if np.abs(x.value) > 0.001 else 0
if x.name == "tilt":
height = x.value / 20 if np.abs(x.value) > 0.001 else 0
if x.name == "side":
rot = x.value / 15 if np.abs(x.value) > 0.001 else 0
print("Joystick ", adv, side, height, rot)
self.move_quad_target([adv, side, height, rot])
self.joystick_newdata = None
self.last_received_data_time = time.time()
#################################################################################
def move_quad_target(self, vels):
target = Shape('Quadricopter_target')
adv, side, height, rot = vels
current_pos = target.get_position(self.drone)
current_ori = target.get_orientation(self.drone)
target.set_position([
current_pos[0] - adv, current_pos[1] - side,
current_pos[2] - height
], self.drone)
target.set_orientation([
current_ori[0] - adv, current_ori[1] - side, current_ori[2] - rot
], self.drone)
##################################################################################
# SUBSCRIPTION to sendData method from JoystickAdapter interface
###################################################################################
def JoystickAdapter_sendData(self, data):
self.joystick_newdata = [data, time.time()]
##################################################################################
# Methods for CameraRGBDSimple
# ===============================================================================
#
# getAll
#
def CameraRGBDSimple_getAll(self, camera):
return RoboCompCameraRGBDSimple.TRGBD(self.cameras[camera]["rgb"],
self.cameras[camera]["depth"])
#
# getDepth
#
def CameraRGBDSimple_getDepth(self, camera):
return self.cameras[camera]["depth"]
#
# getImage
#
def CameraRGBDSimple_getImage(self, camera):
return self.cameras[camera]["rgb"]
"""Sets the 2D (top-down) pose of the robot [x, y, yaw]
:param pose: A List containing the x, y, yaw (in radians).
"""
x, y, yaw = pose
shape.set_position([x, y, shape.get_position()[2]])
shape.set_orientation([0, 0, yaw])
# Get Scene
SCENE_FILE = join(dirname(abspath(__file__)), '../scenes/scene_cpp.ttt')
# Set numpy printing options
np.set_printoptions(threshold=(100*100), formatter={'all':lambda x: str(x) + ','})
# Start Simulation
pr = PyRep()
pr.launch(SCENE_FILE, headless=True)
pr.start()
robot = Shape('robot')
goal = Shape('goal_pose')
robot.set_renderable(False)
goal.set_renderable(False)
vision_sensor = VisionSensor('vision_sensor')
# Since our "robot" is actually a shape we need to define our own 2d pose func
# current_pose = get_2d_pose(robot)
target_pose = get_2d_pose(goal)
current_pose = robot.get_position(vision_sensor)[:2] + robot.get_orientation()[-1:]
current_pose = (-current_pose[0], current_pose[1], current_pose[2])
class TestArmsAndConfigurationPaths(TestCore):
def setUp(self):
self.pyrep = PyRep()
self.pyrep.launch(path.join(
ASSET_DIR, 'test_scene_robots.ttt'), headless=True)
self.pyrep.step()
self.pyrep.start()
# It is enough to only test the constructor of each arm (in there we make
# assumptions about the structure of the arm model). All other tests
# can be run on one arm.
def test_get_arm(self):
for arm_name, arm_type in ARMS:
with self.subTest(arm=arm_name):
arm = arm_type()
self.assertIsInstance(arm, arm_type)
def test_set_ik_element_properties(self):
arm = Panda()
arm.set_ik_element_properties(constraint_gamma=False)
arm.set_ik_element_properties()
def test_get_configs_for_tip_pose(self):
arm = Panda()
waypoint = Dummy('Panda_waypoint')
configs = arm.get_configs_for_tip_pose(
waypoint.get_position(), waypoint.get_orientation())
self.assertIsNotNone(configs)
current_config = arm.get_joint_positions()
# Checks correct config (last)
arm.set_joint_positions(configs[-1])
self.assertTrue(np.allclose(
arm.get_tip().get_pose(), waypoint.get_pose(), atol=0.001))
# Checks correct config (first)
arm.set_joint_positions(configs[0])
self.assertTrue(np.allclose(
arm.get_tip().get_pose(), waypoint.get_pose(), atol=0.001))
# Checks order
prev_config_dist = 0
for config in configs:
config_dist = sum(
[(c - f)**2 for c, f in zip(current_config, config)])
# This test requires that the metric scale for each joint remains at
# 1.0 in _getConfigDistance lua function
self.assertLessEqual(prev_config_dist, config_dist)
prev_config_dist = config_dist
def test_get_path_from_cartesian_path(self):
arm = Panda()
cartesian_path = CartesianPath('Panda_cartesian_path')
path = arm.get_path_from_cartesian_path(cartesian_path)
self.assertIsNotNone(path)
def test_get_linear_path(self):
arm = Panda()
waypoint = Dummy('Panda_waypoint')
path = arm.get_linear_path(
waypoint.get_position(), waypoint.get_orientation())
self.assertIsNotNone(path)
def test_get_nonlinear_path(self):
arm = Panda()
waypoint = Dummy('Panda_waypoint')
path = arm.get_nonlinear_path(
waypoint.get_position(), waypoint.get_orientation())
self.assertIsNotNone(path)
def test_get_nonlinear_path_out_of_reach(self):
arm = Panda()
with self.assertRaises(ConfigurationPathError):
arm.get_nonlinear_path([99, 99, 99], [0.] * 3)
def test_get_linear_path_out_of_reach(self):
arm = Panda()
with self.assertRaises(ConfigurationPathError):
arm.get_linear_path([99, 99, 99], [0.] * 3)
def test_get_linear_path_and_step(self):
arm = Panda()
waypoint = Dummy('Panda_waypoint')
path = arm.get_linear_path(
waypoint.get_position(), waypoint.get_orientation())
self.assertIsNotNone(path)
done = False
while not done:
done = path.step()
self.pyrep.step()
self.assertTrue(np.allclose(
arm.get_tip().get_position(), waypoint.get_position(), atol=0.01))
def test_get_linear_path_and_get_end(self):
arm = Panda()
waypoint = Dummy('Panda_waypoint')
path = arm.get_linear_path(
waypoint.get_position(), waypoint.get_orientation())
path.set_to_end()
self.assertTrue(np.allclose(
arm.get_tip().get_position(), waypoint.get_position(), atol=0.001))
def test_get_linear_path_visualize(self):
arm = Panda()
waypoint = Dummy('Panda_waypoint')
path = arm.get_linear_path(
waypoint.get_position(), waypoint.get_orientation())
# Check that it does not error
path.visualize()
def test_get_duplicate_arm(self):
arm = UR3(1)
self.assertIsInstance(arm, UR3)
def test_copy_arm(self):
arm = UR10()
new_arm = arm.copy()
self.assertNotEqual(arm, new_arm)
def test_get_jacobian(self):
arm = Panda()
jacobian = arm.get_jacobian()
self.assertEqual(jacobian.shape, (7, 6))
class Environment(object):
"""Each environment has a scene."""
def __init__(
self,
action_mode: ActionMode,
dataset_root: str = '',
obs_config=ObservationConfig(),
headless=False,
static_positions: bool = False,
robot_configuration='panda',
randomize_every: RandomizeEvery = None,
frequency: int = 1,
visual_randomization_config: VisualRandomizationConfig = None,
dynamics_randomization_config: DynamicsRandomizationConfig = None,
attach_grasped_objects: bool = True):
self._dataset_root = dataset_root
self._action_mode = action_mode
self._obs_config = obs_config
self._headless = headless
self._static_positions = static_positions
self._robot_configuration = robot_configuration
self._randomize_every = randomize_every
self._frequency = frequency
self._visual_randomization_config = visual_randomization_config
self._dynamics_randomization_config = dynamics_randomization_config
self._attach_grasped_objects = attach_grasped_objects
if robot_configuration not in SUPPORTED_ROBOTS.keys():
raise ValueError('robot_configuration must be one of %s' %
str(SUPPORTED_ROBOTS.keys()))
if (randomize_every is not None and visual_randomization_config is None
and dynamics_randomization_config is None):
raise ValueError(
'If domain randomization is enabled, must supply either '
'visual_randomization_config or dynamics_randomization_config')
self._check_dataset_structure()
self._pyrep = None
self._robot = None
self._scene = None
self._prev_task = None
def _set_arm_control_action(self):
self._robot.arm.set_control_loop_enabled(True)
if (self._action_mode.arm == ArmActionMode.ABS_JOINT_VELOCITY or
self._action_mode.arm == ArmActionMode.DELTA_JOINT_VELOCITY):
self._robot.arm.set_control_loop_enabled(False)
self._robot.arm.set_motor_locked_at_zero_velocity(True)
elif (self._action_mode.arm == ArmActionMode.ABS_JOINT_POSITION
or self._action_mode.arm == ArmActionMode.DELTA_JOINT_POSITION
or self._action_mode.arm == ArmActionMode.ABS_EE_POSE_WORLD_FRAME
or self._action_mode.arm
== ArmActionMode.DELTA_EE_POSE_WORLD_FRAME
or self._action_mode.arm
== ArmActionMode.ABS_EE_POSE_PLAN_WORLD_FRAME
or self._action_mode.arm
== ArmActionMode.DELTA_EE_POSE_PLAN_WORLD_FRAME
or self._action_mode.arm == ArmActionMode.EE_POSE_PLAN_EE_FRAME
or self._action_mode.arm == ArmActionMode.EE_POSE_EE_FRAME):
self._robot.arm.set_control_loop_enabled(True)
elif (self._action_mode.arm == ArmActionMode.ABS_JOINT_TORQUE
or self._action_mode.arm == ArmActionMode.DELTA_JOINT_TORQUE):
self._robot.arm.set_control_loop_enabled(False)
else:
raise RuntimeError('Unrecognised action mode.')
def _check_dataset_structure(self):
if len(self._dataset_root) > 0 and not exists(self._dataset_root):
raise RuntimeError('Data set root does not exists: %s' %
self._dataset_root)
def _string_to_task(self, task_name: str):
task_name = task_name.replace('.py', '')
try:
class_name = ''.join(
[w[0].upper() + w[1:] for w in task_name.split('_')])
mod = importlib.import_module("rlbench.tasks.%s" % task_name)
except Exception as e:
raise RuntimeError(
'Tried to interpret %s as a task, but failed. Only valid tasks '
'should belong in the tasks/ folder' % task_name) from e
return getattr(mod, class_name)
def launch(self):
if self._pyrep is not None:
raise RuntimeError('Already called launch!')
self._pyrep = PyRep()
self._pyrep.launch(join(DIR_PATH, TTT_FILE), headless=self._headless)
arm_class, gripper_class, _ = SUPPORTED_ROBOTS[
self._robot_configuration]
# We assume the panda is already loaded in the scene.
if self._robot_configuration is not 'panda':
# Remove the panda from the scene
panda_arm = Panda()
panda_pos = panda_arm.get_position()
panda_arm.remove()
arm_path = join(DIR_PATH, 'robot_ttms',
self._robot_configuration + '.ttm')
self._pyrep.import_model(arm_path)
arm, gripper = arm_class(), gripper_class()
arm.set_position(panda_pos)
else:
arm, gripper = arm_class(), gripper_class()
self._robot = Robot(arm, gripper)
if self._randomize_every is None:
self._scene = Scene(self._pyrep, self._robot, self._obs_config)
else:
self._scene = DomainRandomizationScene(
self._pyrep, self._robot, self._obs_config,
self._randomize_every, self._frequency,
self._visual_randomization_config,
self._dynamics_randomization_config)
self._set_arm_control_action()
def shutdown(self):
if self._pyrep is not None:
self._pyrep.shutdown()
self._pyrep = None
def get_task(self, task_class: Type[Task]) -> TaskEnvironment:
# If user hasn't called launch, implicitly call it.
if self._pyrep is None:
self.launch()
self._scene.unload()
task = task_class(self._pyrep, self._robot)
self._prev_task = task
return TaskEnvironment(self._pyrep, self._robot, self._scene, task,
self._action_mode, self._dataset_root,
self._obs_config, self._static_positions,
self._attach_grasped_objects)
@property
def action_size(self):
arm_action_size = 0
gripper_action_size = 1 # Only one gripper style atm
if (self._action_mode.arm == ArmActionMode.ABS_JOINT_VELOCITY
or self._action_mode.arm == ArmActionMode.DELTA_JOINT_VELOCITY
or self._action_mode.arm == ArmActionMode.ABS_JOINT_POSITION
or self._action_mode.arm == ArmActionMode.DELTA_JOINT_POSITION
or self._action_mode.arm == ArmActionMode.ABS_JOINT_TORQUE
or self._action_mode.arm == ArmActionMode.DELTA_JOINT_TORQUE):
arm_action_size = SUPPORTED_ROBOTS[self._robot_configuration][2]
elif (self._action_mode.arm == ArmActionMode.ABS_EE_POSE_WORLD_FRAME or
self._action_mode.arm == ArmActionMode.DELTA_EE_POSE_WORLD_FRAME
or self._action_mode.arm
== ArmActionMode.ABS_EE_POSE_PLAN_WORLD_FRAME
or self._action_mode.arm
== ArmActionMode.DELTA_EE_POSE_PLAN_WORLD_FRAME
or self._action_mode.arm == ArmActionMode.EE_POSE_PLAN_EE_FRAME
or self._action_mode.arm == ArmActionMode.EE_POSE_EE_FRAME):
arm_action_size = 7 # pose is always 7
return arm_action_size + gripper_action_size
def get_demos(self,
task_name: str,
amount: int,
variation_number=0,
image_paths=False) -> List[Demo]:
if self._dataset_root is None or len(self._dataset_root) == 0:
raise RuntimeError(
"Can't ask for a stored demo when no dataset root provided.")
demos = utils.get_stored_demos(amount, image_paths, self._dataset_root,
variation_number, task_name,
self._obs_config)
return demos
- Linear (IK) paths.
- Scene manipulation (creating an object and moving it).
"""
from os.path import dirname, join, abspath
from pyrep import PyRep
from pyrep.robots.arms.panda import Panda
from pyrep.objects.shape import Shape
from pyrep.const import PrimitiveShape
from pyrep.errors import ConfigurationPathError
import numpy as np
import math
LOOPS = 10
SCENE_FILE = join(dirname(abspath(__file__)), 'panda_reach_target.ttt')
pr = PyRep()
pr.launch(SCENE_FILE, headless=False)
pr.start()
agent = Panda()
# We could have made this target in the scene, but lets create one dynamically
target = Shape.create(type=PrimitiveShape.SPHERE,
size=[0.05, 0.05, 0.05],
color=[1.0, 0.1, 0.1],
static=True,
respondable=False)
position_min, position_max = [0.8, -0.2, 1.0], [1.0, 0.2, 1.4]
starting_joint_positions = agent.get_joint_positions()
for i in range(LOOPS):
class EnvPollos(Env):
def __init__(self, ep_length=100):
"""
Pollos environment for testing purposes
:param dim: (int) the size of the dimensions you want to learn
:param ep_length: (int) the length of each episodes in timesteps
"""
self.vrep = vrep
logging.basicConfig(level=logging.DEBUG)
# they have to be simmetric. We interpret actions as angular increments
#self.action_space = Box(low=np.array([-0.1, -1.7, -2.5, -1.5, 0.0, 0.0]),
# high=np.array([0.1, 1.7, 2.5, 1.5, 0.0, 0.0]))
inc = 0.1
self.action_space = Box(low=np.array([-inc, -inc, -inc, -inc, 0,
-inc]),
high=np.array([inc, inc, inc, inc, 0, inc]))
self.observation_space = Box(low=np.array([-0.5, 0.0, -0.2]),
high=np.array([0.5, 1.0, 1.0]))
#
self.pr = PyRep()
self.pr.launch(SCENE_FILE, headless=False)
self.pr.start()
self.agent = UR10()
self.agent.max_velocity = 1
self.agent.set_control_loop_enabled(True)
self.agent.set_motor_locked_at_zero_velocity(True)
self.joints = [
Joint('UR10_joint1'),
Joint('UR10_joint2'),
Joint('UR10_joint3'),
Joint('UR10_joint4'),
Joint('UR10_joint5'),
Joint('UR10_joint6')
]
#self.joints_limits = [[j.get_joint_interval()[1][0],j.get_joint_interval()[1][0]+j.get_joint_interval()[1][1]]
# for j in self.joints]
self.high_joints_limits = [0.1, 1.7, 2.7, 0.0, 0.02, 0.3]
self.low_joints_limits = [-0.1, -0.2, 0.0, -1.5, -0.02, -0.5]
#self.agent_hand = Shape('hand')
self.initial_agent_tip_position = self.agent.get_tip().get_position()
self.initial_agent_tip_quaternion = self.agent.get_tip(
).get_quaternion()
self.initial_agent_tip_euler = self.agent.get_tip().get_orientation()
self.target = Dummy('UR10_target')
self.initial_target_orientation = self.target.get_orientation()
self.initial_joint_positions = self.agent.get_joint_positions()
self.pollo_target = Dummy('pollo_target')
self.pollo = Shape('pollo')
#self.table_target = Dummy('table_target')
self.initial_pollo_position = self.pollo.get_position()
self.initial_pollo_orientation = self.pollo.get_quaternion()
#self.table_target = Dummy('table_target')
#self.succion = Shape('suctionPad')
self.waypoints = [Dummy('dummy_gancho%d' % i) for i in range(4)]
# objects
#self.scene_objects = [Shape('table0'), Shape('Plane'), Shape('Plane0'), Shape('ConcretBlock')]
#self.agent_tip = self.agent.get_tip()
self.initial_distance = np.linalg.norm(
np.array(self.initial_pollo_position) -
np.array(self.initial_agent_tip_position))
# camera
self.camera = VisionSensor('kinect_depth')
self.camera_matrix_extrinsics = vrep.simGetObjectMatrix(
self.camera.get_handle(), -1)
self.np_camera_matrix_extrinsics = np.delete(
np.linalg.inv(self.vrepToNP(self.camera_matrix_extrinsics)), 3, 0)
width = 640.0
height = 480.0
angle = math.radians(57.0)
focalx_px = (width / 2.0) / math.tan(angle / 2.0)
focaly_px = (height / 2.0) / math.tan(angle / 2.0)
self.np_camera_matrix_intrinsics = np.array([[-focalx_px, 0.0, 320.0],
[0.0, -focalx_px, 240.0],
[0.0, 0.0, 1.0]])
self.reset()
def reset(self):
pos = list(
np.random.uniform([-0.1, -0.1, 0.0], [0.1, 0.1, 0.1]) +
self.initial_pollo_position)
self.pollo.set_position(pos)
self.pollo.set_quaternion(self.initial_pollo_orientation)
self.agent.set_joint_positions(self.initial_joint_positions)
self.initial_epoch_time = time.time()
while True: # wait for arm to stop
self.pr.step() # Step the physics simulation
a = self.agent.get_joint_velocities()
if not np.any(np.where(np.fabs(a) < 0.1, False, True)):
break
print("ENV RESET DONE")
return self._get_state()
def step(self, action):
if action is None:
self.pr.step()
return self._get_state(), 0.0, False, {}
# check for nan
if np.any(np.isnan(action)):
print(action)
return self._get_state(), -1.0, False, {}
# action[1] = np.interp(action[1], [-1,7,1.7], [-0.2,1.7])
# action[2] = np.interp(action[2], [-2.5,2.5], [0,2.5])
# action[3] = np.interp(action[3], [-1.5,1.5], [-1.5,0])
# add actions as increments to current joints value
new_joints = np.array(
self.agent.get_joint_positions()) + np.array(action)
# check limits
if np.any(np.greater(new_joints, self.high_joints_limits)) or np.any(
np.less(new_joints, self.low_joints_limits)):
logging.debug("New joints value out of limits r=-10")
return self._get_state(), -10.0, True, {}
# move the robot and wait to stop
self.agent.set_joint_target_positions(new_joints)
reloj = time.time()
while True: # wait for arm to stop
self.pr.step() # Step the physics simulation
a = self.agent.get_joint_velocities()
if not np.any(np.where(np.fabs(a) < 0.1, False,
True)) or (time.time() - reloj) > 3:
break
# compute relevant magnitudes
np_pollo_target = np.array(self.pollo_target.get_position())
np_robot_tip_position = np.array(self.agent.get_tip().get_position())
dist = np.linalg.norm(np_pollo_target - np_robot_tip_position)
altura = np.clip((np_pollo_target[2] - self.initial_pollo_position[2]),
0, 0.5)
for obj in self.scene_objects: # colisión con la mesa
if self.agent_hand.check_collision(obj):
logging.debug("Collision with objects r=-10")
return self._get_state(), -10.0, True, {}
if altura > 0.3: # pollo en mano
logging.debug("Height reached !!! r=0")
return self._get_state(), 30.0, True, {}
elif dist > self.initial_distance: # mano se aleja
logging.debug("Hand moving away from chicken r=-10")
return self._get_state(), -10.0, True, {}
if time.time() - self.initial_epoch_time > 5: # too much time
logging.debug("Time out. End of epoch r=-10")
return self._get_state(), -10.0, True, {}
# Reward
#pollo_height = np.exp(-altura*20) # para 0.3m pollo_height = 0.002, para 0m pollo_height = 1
reward = altura - dist
logging.debug("New joints value out of limits r=-10")
return self._get_state(), reward, False, {}
def close(self):
self.pr.stop()
self.pr.shutdown()
def render(self):
print("RENDER")
np_pollo_en_camara = self.getPolloEnCamara()
# capture depth image
depth = self.camera.capture_rgb()
circ = plt.Circle(
(int(np_pollo_en_camara[0]), int(np_pollo_en_camara[1])), 10)
plt.clf()
fig = plt.gcf()
ax = fig.gca()
ax.add_artist(circ)
ax.imshow(depth, cmap='hot')
plt.pause(0.000001)
# Aux
# transform env.pollo_target.get_position() to camera coordinates and project pollo_en_camera a image coordinates
def getPolloEnCamara(self):
np_pollo_target = np.array(self.pollo_target.get_position())
np_pollo_target_cam = self.np_camera_matrix_extrinsics.dot(
np.append([np_pollo_target], 1.0))
np_pollo_en_camara = self.np_camera_matrix_intrinsics.dot(
np_pollo_target_cam)
np_pollo_en_camara = np_pollo_en_camara / np_pollo_en_camara[2]
np_pollo_en_camara = np.delete(np_pollo_en_camara, 2)
return np_pollo_en_camara
def getPolloEnCamaraEx(self):
np_pollo_target = np.array(self.pollo_target.get_position())
np_pollo_en_camara = self.np_camera_matrix_extrinsics.dot(
np.append([np_pollo_target], 1.0))
return np_pollo_en_camara
def _get_state(self):
# Return state containing arm joint angles/velocities & target position
# return (self.agent.get_joint_positions() +
# self.agent.get_joint_velocities() +
# self.pollo_target.get_position())
p = self.getPolloEnCamaraEx()
j = self.agent.get_joint_positions()
#r = np.array([p[0],p[1],p[2],j[0],j[1],j[2],j[3],j[4],j[5]])
r = np.array([p[0], p[1], p[2]])
return r
def vrepToNP(self, c):
return np.array([[c[0], c[4], c[8], c[3]], [c[1], c[5], c[9], c[7]],
[c[2], c[6], c[10], c[11]], [0, 0, 0, 1]])
class CoppeliaSimEnvWrapper(EnvironmentSpec):
def __init__(self, visualize=True, mode="train", **params):
super().__init__(visualize=visualize, mode=mode)
# Scene selection
scene_file_path = os.path.join(os.getcwd(), 'simulation/UR5.ttt')
# Simulator launch
self.env = PyRep()
self.env.launch(scene_file_path, headless=False)
self.env.start()
self.env.step()
# Task related initialisations in Simulator
self.vision_sensor = objects.vision_sensor.VisionSensor(
"Vision_sensor")
self.gripper = objects.dummy.Dummy("UR5_target")
self.gripper_zero_pose = self.gripper.get_pose()
self.goal = objects.dummy.Dummy("goal_target")
self.goal_STL = objects.shape.Shape("goal")
self.goal_STL_zero_pose = self.goal_STL.get_pose()
self.grasped_STL = objects.shape.Shape("Peg")
self.stacking_area = objects.shape.Shape("Plane")
self.vision_sensor = objects.vision_sensor.VisionSensor(
"Vision_sensor")
self.step_counter = 0
self.max_step_count = 100
self.target_pose = None
self.initial_distance = None
self.image_width, self.image_height = 320, 240
self.vision_sensor.set_resolution(
(self.image_width, self.image_height))
self._history_len = 1
self._observation_space = Dict({
"cam_image":
Box(0,
255, [self.image_height, self.image_width, 1],
dtype=np.uint8)
})
self._action_space = Box(-1, 1, (3, ))
self._state_space = extend_space(self._observation_space,
self._history_len)
@property
def history_len(self):
return self._history_len
@property
def observation_space(self) -> Space:
return self._observation_space
@property
def state_space(self) -> Space:
return self._state_space
@property
def action_space(self) -> Space:
return self._action_space
def step(self, action):
done = False
info = {}
prev_distance_to_goal = self.distance_to_goal()
# Make a step in simulation
self.apply_controls(action)
self.env.step()
self.step_counter += 1
# Reward calculations
success_reward = self.success_check()
distance_reward = (prev_distance_to_goal -
self.distance_to_goal()) / self.initial_distance
reward = distance_reward + success_reward
# Check reset conditions
if self.step_counter > self.max_step_count:
done = True
logging.info('--------Reset: Timeout--------')
elif self.distance_to_goal() > 0.8:
done = True
logging.info('--------Reset: Too far from target--------')
elif self.collision_check():
done = True
logging.info('--------Reset: Collision--------')
return self.get_observation(), reward, done, info
def reset(self):
logging.info("Episode reset...")
self.step_counter = 0
self.env.stop()
self.env.start()
self.env.step()
self.setup_scene()
observation = self.get_observation()
return observation
# -------------- all methods above are required for any Gym environment, everything below is env-specific --------------
def distance_to_goal(self):
goal_pos = self.goal.get_position()
tip_pos = self.gripper.get_position()
return np.linalg.norm(np.array(tip_pos) - np.array(goal_pos))
def setup_goal(self):
goal_position = self.goal_STL_zero_pose[:3]
# 2D goal randomization
self.target_pose = [
goal_position[0] + (2 * np.random.rand() - 1.) * 0.1,
goal_position[1] + (2 * np.random.rand() - 1.) * 0.1,
goal_position[2]
]
self.target_pose = np.append(self.target_pose,
self.goal_STL_zero_pose[3:]).tolist()
self.goal_STL.set_pose(self.target_pose)
# Randomizing the RGB of the goal and the plane
rgb_values_goal = list(np.random.rand(3, ))
rgb_values_plane = list(np.random.rand(3, ))
self.goal_STL.set_color(rgb_values_goal)
self.stacking_area.set_color(rgb_values_plane)
self.initial_distance = self.distance_to_goal()
def setup_scene(self):
self.setup_goal()
self.gripper.set_pose(self.gripper_zero_pose)
def get_observation(self):
cam_image = self.vision_sensor.capture_rgb()
gray_image = np.uint8(
cv2.cvtColor(cam_image, cv2.COLOR_BGR2GRAY) * 255)
obs_image = np.expand_dims(gray_image, axis=2)
return {"cam_image": obs_image}
def collision_check(self):
return self.grasped_STL.check_collision(
self.stacking_area) or self.grasped_STL.check_collision(
self.goal_STL)
def success_check(self):
success_reward = 0.
if self.distance_to_goal() < 0.01:
success_reward = 1
logging.info('--------Success state--------')
return success_reward
def apply_controls(self, action):
gripper_position = self.gripper.get_position()
# predicted action is in range (-1, 1) so we are normalizing it to physical units
new_position = [
gripper_position[i] + (action[i] / 200.) for i in range(3)
]
self.gripper.set_position(new_position)
class ReacherEnv(object):
def __init__(self,
headless,
control_mode='end_position',
visual_control=False):
'''
:visual_control: bool, controlled by visual state or not (vector state).
'''
self.reward_offset = 10.0
self.reward_range = self.reward_offset # reward range for register gym env when using vectorized env wrapper
self.fall_down_offset = 0.1 # for judging the target object fall off the table
self.metadata = [] # gym env format
self.visual_control = visual_control
self.control_mode = control_mode
self.pr = PyRep()
if control_mode == 'end_position': # need to use different scene, the one with all joints in inverse kinematics mode
SCENE_FILE = join(dirname(abspath(__file__)),
'./scenes/sawyer_reacher_rl_new_ik.ttt')
elif control_mode == 'joint_velocity': # the scene with all joints in force/torche mode for forward kinematics
SCENE_FILE = join(dirname(abspath(__file__)),
'./scenes/sawyer_reacher_rl_new.ttt')
self.pr.launch(SCENE_FILE, headless=headless)
self.pr.start()
self.agent = Sawyer()
self.gripper = BaxterGripper()
self.gripper_left_pad = Shape(
'BaxterGripper_leftPad') # the pad on the gripper finger
self.proximity_sensor = ProximitySensor(
'BaxterGripper_attachProxSensor'
) # need the name of the sensor here
self.vision_sensor = VisionSensor(
'Vision_sensor') # need the name of the sensor here
if control_mode == 'end_position':
self.agent.set_control_loop_enabled(True) # if false, won't work
self.action_space = np.zeros(
4) # 3 DOF end position control + 1 rotation of gripper
elif control_mode == 'joint_velocity':
self.agent.set_control_loop_enabled(False)
self.action_space = np.zeros(
8) # 7 DOF velocity control + 1 rotation of gripper
else:
raise NotImplementedError
if self.visual_control == False:
self.observation_space = np.zeros(
17
) # position and velocity of 7 joints + position of the target
else:
self.observation_space = np.zeros(100) # dim of img!
self.agent.set_motor_locked_at_zero_velocity(True)
self.target = Shape('target') # object
self.tip_target = Dummy(
'Sawyer_target') # the target point of the tip to move towards
# self.table = Shape('diningTable')
self.agent_ee_tip = self.agent.get_tip()
self.tip_pos = self.agent_ee_tip.get_position()
self.tip_quat = self.agent_ee_tip.get_quaternion(
) # tip rotation as quaternion, if not control the rotation
# set a proper initial gesture/tip position
if control_mode == 'end_position':
# agent_position=self.agent.get_position()
# initial_pos_offset = [0.4, 0.3, 0.2] # initial relative position of gripper to the whole arm
# initial_pos = [(a + o) for (a, o) in zip(agent_position, initial_pos_offset)]
initial_pos = [0.3, 0.1, 0.9]
self.tip_target.set_position(initial_pos)
# one big step for rotation setting is enough, with reset_dynamics=True, set the rotation instantaneously
self.tip_target.set_orientation(
[0, 3.1415, 1.5707],
reset_dynamics=True) # make gripper face downwards
self.pr.step()
elif control_mode == 'joint_velocity':
self.initial_joint_positions = [
0.001815199851989746, -1.4224984645843506, 0.704303503036499,
2.54307222366333, 2.972468852996826, -0.4989511966705322,
4.105560302734375
]
self.agent.set_joint_positions(self.initial_joint_positions)
self.initial_joint_positions = self.agent.get_joint_positions()
self.initial_tip_positions = self.agent_ee_tip.get_position()
self.initial_target_positions = self.target.get_position()
def _get_state(self):
# Return state containing arm joint angles/velocities & target position
return np.array(self.agent.get_joint_positions() +
self.agent.get_joint_velocities() +
self.target.get_position())
def _is_holding(self):
'''
Return is holding the target or not, return bool.
'''
# Nothe that the collision check is not always accurate all the time,
# for continuous conllision, maybe only the first 4-5 frames of collision can be detected
pad_collide_object = self.gripper_left_pad.check_collision(self.target)
if pad_collide_object and self.proximity_sensor.is_detected(
self.target) == True:
return True
else:
return False
def _get_visual_state(self):
# Return a numpy array of size (width, height, 3)
return self.vision_sensor.capture_rgb(
) # A numpy array of size (width, height, 3)
def _is_holding(self):
# Return is holding the target or not, return bool
# Nothe that the collision check is not always accurate all the time,
# for continuous conllision, maybe only the first 4-5 frames of collision can be detected
pad_collide_object = self.gripper_left_pad.check_collision(self.target)
if pad_collide_object and self.proximity_sensor.is_detected(
self.target) == True:
return True
else:
return False
# def _move(self, action):
# '''
# Move the tip according to the action with inverse kinematics for 'end_position' control;
# with control of tip target in inverse kinematics mode instead of using .solve_ik() in forward kinematics mode.
# '''
# robot_moving_unit=0.01 # the amount of single step move of robot, not accurate
# moving_loop_itr=int(np.sum(np.abs(action[:3]))/robot_moving_unit)+1 # adaptive number of moving steps, with minimal of 1 step.
# # print(moving_loop_itr)
# small_step = list(1./moving_loop_itr*np.array(action)) # break the action into small steps, as the robot cannot move to the target position within one frame
# pos=self.agent_ee_tip.get_position()
# '''
# there is a mismatch between the object set_orientation() and get_orientation():
# the (x,y,z) in set_orientation() will be (y,x,-z) in get_orientation().
# '''
# ori_z=-self.agent_ee_tip.get_orientation()[2] # the minus is because the mismatch between the set and get
# assert len(small_step) == len(pos)+1 # 3 values for position, 1 value for rotation
# for _ in range(moving_loop_itr):
# for idx in range(len(pos)):
# pos[idx] += small_step[idx]
# self.tip_target.set_position(pos)
# self.pr.step()
# ''' deprecated! no need to use small steps for the rotation with reset_dynamics=True'''
# # ori_z+=small_step[3] # change the orientation along z-axis with a small step
# # self.tip_target.set_orientation([0,3.1415,ori_z], reset_dynamics=True) # make gripper face downwards
# # self.pr.step()
# ''' one big step for z-rotation is enough, with reset_dynamics=True, set the rotation instantaneously '''
# ori_z+=action[3]
# self.tip_target.set_orientation([0,3.1415,ori_z], reset_dynamics=True) # make gripper face downwards
# self.pr.step()
def _move(self, action):
'''
Move the tip according to the action with inverse kinematics for 'end_position' control;
with control of tip target in inverse kinematics mode instead of using .solve_ik() in forward kinematics mode.
Mode 2: a close-loop control, using ik.
'''
pos = self.gripper.get_position()
bounding_offset = 0.1
step_factor = 0.2 # small step factor mulitplied on the gradient step calculated by inverse kinematics
max_itr = 20 # maximum moving iterations
max_error = 0.1 # upper bound of distance error for movement at each call
rotation_norm = 5. # factor for normalization of rotation values
# check if state+action will be within of the bounding box, if so, move normally; else no action.
# x_min < x < x_max and y_min < y < y_max and z > z_min
if pos[0]+action[0]>POS_MIN[0]-bounding_offset and pos[0]+action[0]<POS_MAX[0]+bounding_offset \
and pos[1]+action[1] > POS_MIN[1]-bounding_offset and pos[1]+action[1] < POS_MAX[1]+bounding_offset \
and pos[2]+action[2] > POS_MIN[2]-bounding_offset:
'''
there is a mismatch between the object set_orientation() and get_orientation():
the (x,y,z) in set_orientation() will be (y,x,-z) in get_orientation().
'''
ori_z = -self.agent_ee_tip.get_orientation()[
2] # the minus is because the mismatch between the set and get
target_pos = np.array(self.agent_ee_tip.get_position()) + np.array(
action[:3])
diff = 1
itr = 0
# print('before: ', ori_z)
while np.sum(np.abs(diff)) > max_error and itr < max_itr:
itr += 1
# set pos in small step
cur_pos = self.agent_ee_tip.get_position()
diff = target_pos - cur_pos
pos = cur_pos + step_factor * diff
self.tip_target.set_position(pos.tolist())
self.pr.step()
''' one big step for z-rotation is enough '''
ori_z += rotation_norm * action[3]
self.tip_target.set_orientation([0, np.pi, ori_z
]) # make gripper face downwards
self.pr.step()
else:
# print("Potential Movement Out of the Bounding Box!")
pass # no action if potentially out of the bounding box
def reset(self):
# Get a random position within a cuboid and set the target position
max_itr = 10
pos = list(np.random.uniform(POS_MIN, POS_MAX))
self.target.set_position(pos)
self.target.set_orientation([0, 0, 0])
self.pr.step()
# changing the color or texture for domain randomization
self.target.set_color(
np.random.uniform(low=0, high=1, size=3).tolist()
) # set [r,g,b] 3 channel values of object color
# set end position to be initialized
if self.control_mode == 'end_position': # JointMode.IK
self.agent.set_control_loop_enabled(True)
self.tip_target.set_position(
self.initial_tip_positions
) # cannot set joint positions directly due to in ik mode nor force/torch mode
self.pr.step()
# prevent stuck case
itr = 0
while np.sum(
np.abs(
np.array(self.agent_ee_tip.get_position() -
np.array(self.initial_tip_positions)))
) > 0.1 and itr < max_itr:
itr += 1
self.step(np.random.uniform(
-0.2, 0.2,
4)) # take random actions for preventing the stuck cases
self.pr.step()
elif self.control_mode == 'joint_velocity': # JointMode.FORCE
self.agent.set_joint_positions(
self.initial_joint_positions
) # sometimes the gripper is stuck, cannot get back to initial
self.pr.step()
# self.table.set_collidable(True)
self.gripper_left_pad.set_collidable(
True
) # set the pad on the gripper to be collidable, so as to check collision
self.target.set_collidable(True)
if np.sum(self.gripper.get_open_amount()) < 1.5:
self.gripper.actuate(1, velocity=0.5) # open the gripper
self.pr.step()
if self.visual_control:
return self._get_visual_state()
else:
return self._get_state()
def step(self, action):
'''
Move the robot arm according to the action.
If control_mode=='joint_velocity', action is 7 dim of joint velocity values + 1 dim of gripper rotation.
if control_mode=='end_position', action is 3 dim of tip (end of robot arm) position values + 1 dim of gripper rotation.
'''
if self.control_mode == 'end_position':
if action is None or action.shape[0] != 4:
action = list(np.random.uniform(-0.1, 0.1, 4)) # random
self._move(action)
elif self.control_mode == 'joint_velocity':
if action is None or action.shape[0] != 7:
print('No actions or wrong action dimensions!')
action = list(np.random.uniform(-0.1, 0.1, 7)) # random
self.agent.set_joint_target_velocities(
action) # Execute action on arm
self.pr.step()
else:
raise NotImplementedError
ax, ay, az = self.gripper.get_position()
tx, ty, tz = self.target.get_position()
# Reward is negative distance to target
distance = (ax - tx)**2 + (ay - ty)**2 + (az - tz)**2
done = False
# print(self.proximity_sensor.is_detected(self.target))
current_vision = self.vision_sensor.capture_rgb(
) # capture a screenshot of the view with vision sensor
plt.imshow(current_vision)
plt.savefig('./img/vision.png')
reward = 0
# close the gripper if close enough to the object and the object is detected with the proximity sensor
if distance < 0.1 and self.proximity_sensor.is_detected(
self.target) == True:
# make sure the gripper is open before grasping
self.gripper.actuate(1, velocity=0.5)
self.pr.step()
self.gripper.actuate(
0, velocity=0.5
) # if done, close the hand, 0 for close and 1 for open.
self.pr.step() # Step the physics simulation
if self._is_holding():
# reward for hold here!
reward += 10
done = True
else:
self.gripper.actuate(1, velocity=0.5)
self.pr.step()
elif np.sum(
self.gripper.get_open_amount()
) < 1.5: # if gripper is closed due to collision or esle, open it; .get_open_amount() return list of gripper joint values
self.gripper.actuate(1, velocity=0.5)
self.pr.step()
else:
pass
reward -= np.sqrt(distance)
if tz < self.initial_target_positions[
2] - self.fall_down_offset: # the object fall off the table
done = True
reward = -self.reward_offset
if self.visual_control:
return self._get_visual_state(), reward, done, {}
else:
return self._get_state(), reward, done, {}
def shutdown(self):
self.pr.stop()
self.pr.shutdown()
class GraspEnv(object):
def __init__(self, headless, control_mode='joint_velocity'):
self.headless = headless
self.reward_offset = 10.0
self.reward_range = self.reward_offset
self.penalty_offset = 1
#self.penalty_offset = 1.
self.fall_down_offset = 0.1
self.metadata = [] #gym env argument
self.control_mode = control_mode
#launch and setup the scene, and set the proxy variables in present of the counterparts in the scene
self.pr = PyRep()
if control_mode == 'end_position':
SCENE_FILE = join(dirname(abspath(__file__)),
'./scenes/UnicarRobot_ik.ttt')
elif control_mode == 'joint_velocity':
SCENE_FILE = join(dirname(abspath(__file__)),
'./scenes/UnicarRobot.ttt')
self.pr.launch(SCENE_FILE, headless=headless)
self.pr.start()
self.agent = UnicarRobotArm() #drehkranz + UR10
#self.gripper = UnicarRobotGreifer()#suction
#self.suction = UnicarRobotGreifer()
self.suction = Shape("UnicarRobotGreifer_body_sub0")
self.proximity_sensor = ProximitySensor('UnicarRobotGreifer_sensor')
self.table = Shape('UnicarRobotTable')
self.carbody = Shape('UnicarRobotCarbody')
if control_mode == 'end_position':
self.agent.set_control_loop_enabled(True)
self.action_space = np.zeros(4)
elif control_mode == 'joint_velocity':
self.agent.set_control_loop_enabled(False)
self.action_space = np.zeros(7)
else:
raise NotImplementedError
#self.observation_space = np.zeros(17)
self.observation_space = np.zeros(20)
self.agent.set_motor_locked_at_zero_velocity(True)
self.target = Shape("UnicarRobotTarget") #Box
self.agent_ee_tip = self.agent.get_tip()
self.tip_target = Dummy('UnicarRobotArm_target')
self.tip_pos = self.agent_ee_tip.get_position()
# set a proper initial robot gesture or tip position
if control_mode == 'end_position':
initial_pos = [0, 1.5, 1.6]
self.tip_target.set_position(initial_pos)
#one big step for rotatin is enough, with reset_dynamics = True, set the rotation instantaneously
self.tip_target.set_orientation([0, 0, 0], reset_dynamics=True)
elif control_mode == 'joint_velocity':
self.initial_joint_positions = [0, 0, 0, 0, 0, 0, 0]
self.agent.set_joint_positions(self.initial_joint_positions)
self.pr.step()
self.initial_tip_positions = self.agent_ee_tip.get_position()
self.initial_target_positions = self.target.get_position()
def _get_state(self):
'''
Return state containging arm joint positions/velocities & target position.
'''
return np.array(self.agent.get_joint_positions() +
self.agent.get_joint_velocities() +
self.agent_ee_tip.get_position() +
self.agent_ee_tip.get_orientation()) #all 20
def _is_holding(self):
'''
Return the state of holding the UnicarRobotTarget or not, return bool value
'''
if self.proximity_sensor.is_detected(self.target) == True:
return True
else:
return False
def _move(self,
action,
bounding_offset=0.15,
step_factor=0.2,
max_itr=20,
max_error=0.05,
rotation_norm=5):
pos = self.suction.get_position()
if pos[0]+action[0] > POS_MIN[0]-bounding_offset and pos[0]+action[0] < POS_MAX[0]-bounding_offset \
and pos[1]+action[1] > POS_MIN[1]-bounding_offset and pos[1]+action[1] < POS_MAX[1]-bounding_offset \
and pos[2]+action[2] > POS_MIN[2]-2*bounding_offset:
ori_z = -self.agent_ee_tip.get_orientation(
)[2] # the minus is because the mismatch between the set_orientation() and get_orientation()
#ori_z = self.agent_ee_tip.get_orientation()[2]
target_pos = np.array(self.agent_ee_tip.get_position() +
np.array(action[:3]))
diff = 1
itr = 0
while np.sum(np.abs(diff)) > max_error and itr < max_itr:
itr += 1
# set pos in small step
cur_pos = self.agent_ee_tip.get_position()
diff = target_pos - cur_pos
pos = cur_pos + step_factor * diff
self.tip_target.set_position(pos.tolist())
self.pr.step()
ori_z += rotation_norm * action[3]
self.tip_target.set_orientation([0, np.pi, ori_z])
self.pr.step()
else:
print("Potantial Movement Out of the Bounding Box!")
self.pr.step()
def reinit(self):
self.shutdown()
self.__init__(self.headless)
def reset(self, random_target=False):
'''
Get a random position within a cuboid and set the target position
'''
# set target object
if random_target:
pos = list(np.random.uniform(POS_MIN, POS_MAX))
self.target.set_position(pos)
else:
self.target.set_position(self.initial_target_positions)
self.target.set_orientation([0, 0, 0])
self.pr.step()
#set end position to be initialized
if self.control_mode == 'end_position':
self.agent.set_control_loop_enabled(True) # IK mode
self.tip_target.set_position(self.initial_tip_positions)
self.pr.step()
itr = 0
max_itr = 10
while np.sum(
np.abs(
np.array(self.agent_ee_tip.get_position() -
np.array(self.initial_tip_positions)))
) > 0.1 and itr < max_itr:
itr += 1
self.step(np.random.uniform(-0.2, 0.2, 4))
self.pr.step()
elif self.control_mode == 'joint_velocity': #JointMode Force
self.agent.set_joint_positions(self.initial_joint_positions)
self.pr.step()
#set collidable, for collision detection
#self.gripper.set_collidable(True)
self.suction.set_collidable(True)
self.target.set_collidable(True)
return self._get_state() #return the current state of the environment
def step(self, action):
'''
move the robot arm according to the control mode
if control_mode == 'end_position' then action is 3 dim of tip (end of robot arm) position values + 1 dim rotation of suction
if control_mode == 'joint_velocity' then action is 7 dim of joint velocity + 1 dim of rotation of suction
'''
#initialization
done = False #episode finishes
reward = 0
hold_flag = False #hold the object or not
if self.control_mode == 'end_position':
if action is None or action.shape[0] != 4: #check action is valid
print('No actions or wrong action dimensions!')
action = list(np.random.uniform(-0.1, 0.1, 4))
self._move(action)
elif self.control_mode == 'joint_velocity':
if action is None or action.shape[0] != 7: #???
print('No actions or wrong action dimensions!')
action = list(np.random.uniform(-1, 1, 7))
self.agent.set_joint_target_velocities(action)
self.pr.step()
else:
raise NotImplementedError
#ax,ay,az = self.gripper.get_position()#gripper position
ax, ay, az = self.suction.get_position() #suction position
if math.isnan(
ax): #capture the broken suction cases during exploration
print("Suction position is nan.")
self.reinit()
done = True
desired_position_tip = [0.0, 1.5513, 1.74]
desired_orientation_tip = [-np.pi, 0, 0.001567]
tip_x, tip_y, tip_z = self.agent_ee_tip.get_position(
) #end_effector position
tip_row, tip_pitch, tip_yaw = self.agent_ee_tip.get_orientation(
) #end_effector orientation
tx, ty, tz = self.target.get_position() #box position
offset = 0.312 #augmented reward: offset of target position above the target object
#square distance between the gripper and the target object
sqr_distance = np.sqrt((tip_x - desired_position_tip[0])**2 +
(tip_y - desired_position_tip[1])**2 +
(tip_z - desired_position_tip[2])**2)
sqr_orientation = np.sqrt((tip_row - desired_orientation_tip[0])**2 +
(tip_pitch - desired_orientation_tip[1])**2 +
(tip_yaw - desired_orientation_tip[2])**2)
''' for visual-based control only, large time consumption! '''
# current_vision = self.vision_sensor.capture_rgb() # capture a screenshot of the view with vision sensor
# plt.imshow(current_vision)
# plt.savefig('./img/vision.png')
desired_orientation = [0, 0, -np.pi / 2]
desired_orientation_tip = [-np.pi, 0, 0.001567]
#Enable the suction if close enough to the object and the object is detected with the proximity sensor
if sqr_distance < 0.001 and self.proximity_sensor.is_detected(
self.target) == True and sqr_orientation < 0.001:
#make sure the suction is not worked
self.suction.release()
self.pr.step()
self.suction.grasp(self.target)
self.pr.step()
if self._is_holding():
reward += self.reward_offset
done = True
hold_flag = True
else:
self.suction.release()
self.pr.step()
else:
pass
#the base reward is negative distance from suction to target
#reward -= (np.sqrt(sqr_distance))
#case when the object is fall off the table
if tz < self.initial_target_positions[
2] - self.fall_down_offset: #tz is target(box) position in z direction
done = True
reward -= self.reward_offset
# Augmented reward for orientation: better grasping gesture if the suction has vertical orientation to the target object
desired_position_tip = [0.0, 1.5513, 1.74]
tip_x, tip_y, tip_z = self.agent_ee_tip.get_position()
tip_row, tip_pitch, tip_yaw = self.agent_ee_tip.get_orientation()
reward -= (np.sqrt((tip_x - desired_position_tip[0])**2 +
(tip_y - desired_position_tip[1])**2 +
(tip_z - desired_position_tip[2])**2) +
np.sqrt((tip_row - desired_orientation_tip[0])**2 +
(tip_pitch - desired_orientation_tip[1])**2 +
(tip_yaw - desired_orientation_tip[2])**2))
#Penalty for collision with the table
if self.suction.check_collision(
self.table) or self.suction.check_collision(
self.carbody) or self.agent.check_collision(
self.table) or self.suction.check_collision(
self.target) or self.agent.check_collision(
self.target):
reward -= self.penalty_offset
if math.isnan(reward):
reward = 0.
return self._get_state(), reward, done, {'finished': hold_flag}
def shutdown(self):
'''close the simulator'''
self.pr.stop()
self.pr.shutdown()
from pyrep import PyRep
pr = PyRep()
# Launch the application with a scene file in headless mode
pr.launch('/v-rep_model/Infinite_basket2.ttt', headless=True)
pr.start() # Start the simulation
# Do some stuff
pr.stop() # Stop the simulation
pr.shutdown() # Close the application
