def __init__(self):
attrs = check_settings()[0]
for attr in attrs:
if isinstance(attr, tuple):
setattr(self, attr[0], attr[1])
# Get default paths from dict
self.default = self.IMAGES_PATHS['default']
self.new_folder = self.IMAGES_PATHS['new_folder']
# Check if both folders exist
# Create if not the case
folder_exists(self.default, self.new_folder)
def state_save(self, i, name):
"""
Function that saves the state_next as a gif on the disk.
:param i : int
Index of the state_next to be saved(int)
:param name: str
Name of the file that will be saved on the disk.
:return: nothing
"""
folder_exists(self.path_save)
img = self.get_state(i)
if not self.is_recurrent:
n_frames = img.shape[2] / (self.input_depth)
img = np.split(img, n_frames, axis=2)
imageio.mimwrite(os.path.join(self.path_save,
"{}{}.gif".format(name, i)),
img,
fps=30)
def _verify_kube_download_folder(kubeconfig_path):
if not utils.folder_exists(kubeconfig_path):
exit(1)
def main():
with open(pathname(os.getcwd(), '..', '..', 'machines.json'), 'r') as f:
js = json.load(f)
DATA_PATH = js[IP]['DATA_PATH']
trial = js[IP]['trial_start']
while True:
if trial == js[IP]['trial_stop']:
break
start = time.time()
while time.time() - start <= 120.0:
SAVE_PATH = DATA_PATH + 'Trial_%d/' % trial
mkdir(SAVE_PATH)
p_mode_velocity_file = SAVE_PATH + 'p_mode_velocities.csv'
scene_object_file = SAVE_PATH + 'scene'
with Interface(mode='direct',
save=True,
filename=p_mode_velocity_file) as I:
# LOAD ROBOT
robot = I.load_model(ROBOT_URDF_PATH)
# LOAD TABLE
table = I.load_model(TABLE,
Pose(Point(0.4, 0.0, 0.05),
Euler(1.57, 0.0, 0.0)),
fixed_base=True,
scaling=1.0)
# TODO set camera
I.set_camera_pose(1.6, 60, -25, [0.2, 0.0, 0.7])
# LOAD_BLOCKS
blocks = load_blocks(I)
if len(blocks) != len(BLOCKS):
continue
time.sleep(1.0)
# Gripper ID
gripper_link = I.get_link_from_name(robot, END_EFFECTOR_LINK)
# Neutral Pose
neutralPose = Pose(Point(0.35, 0.0, 0.5),
Euler(0.0, 3.14, 1.57))
I.save_body_conf(save=True, filename=scene_object_file)
I.set_initial_state(robot)
offset = 0.005
x_offset = [0.0, offset, -offset, -offset]
y_offset = [0.0, offset, -offset, 0.0]
shuffle(blocks)
color = ['red']
for k in range(1):
# Set initial state
I.restore_body_conf()
success = False
# Plan motion
paths = []
velocities = OrderedDict()
for j in range(1):
initialize = True if j == 0 else False
path = plan(I,
robot,
gripper_link,
blocks,
neutralPose,
initialize=initialize,
x_offset=x_offset[k],
y_offset=y_offset[k])
if path is not None:
paths.append(path)
else:
paths = []
break
if len(paths) > 0:
I.reset_simulation(robot)
# time.sleep(1.0)
I.command(robot, paths[0][0])
I.command(robot, paths[0][1])
I.command(robot, paths[0][2])
I.gripper(robot, 'close')
I.command(robot, paths[0][3])
I.command(robot, paths[0][4])
I.command(robot, paths[0][5])
I.gripper(robot, 'open')
I.command(robot, paths[0][6])
if on_top(I, *blocks[0:2]):
I.keep_pose(body=robot)
I.vel.write_csv()
velocities = I.vel.array
I.restore_body_conf()
I.reset_simulation(robot)
I.replay_velocities(robot, velocities)
if on_top(I, *blocks[0:2]):
I.restore_body_conf()
I.reset_simulation(robot)
time.sleep(1.0)
I.replay_velocities(
robot,
velocities,
path=os.path.abspath(SAVE_PATH))
if on_top(I, *blocks[0:2]):
success = True
np.save(
os.path.join(SAVE_PATH, 'velocities'),
np.array(I.target_vel))
np.save(
os.path.join(SAVE_PATH,
'joints_configurations'),
np.array(I.joint_val))
trial += 1
break
else:
shutil.rmtree(SAVE_PATH)
if success:
break
else:
if folder_exists(SAVE_PATH):
shutil.rmtree(SAVE_PATH)
