def get_position(self, handle):
"""Uses the cached positions to return position of any given object handle
"""
# self.object_positions is updated by the __update_all_object_positions() method
pos = self.object_positions[self.objects.index(handle)].tolist()
for idx, val in enumerate(pos):
pos[idx] = utility.rnd(pos[idx])
return pos
def get_position(self, handle):
"""Uses the cached positions to return position of any given object handle
"""
# self.object_positions is updated by the __update_all_object_positions() method
pos = self.object_positions[self.objects.index(handle)].tolist()
for idx, val in enumerate(pos):
pos[idx] = utility.rnd(pos[idx])
return pos
ra.goto_position([Decimal('-0.24'), Decimal('-0.10'), Decimal('0.12')])
status("", ra)
ra.enable_grip(False)
status("", ra)
ra.goto_position([Decimal('-0.26'), Decimal('-0.10'), Decimal('0.10')])
status("", ra)
quit()
pos = ra.get_position(ra.cylinder_handle)
status("After getting position of cylinder", ra)
pos[0] += utility.rnd(0.0)
ra.goto_position(pos)
status("After going to the cylinder", ra)
ra.enable_grip(True)
status("After enabling grip", ra)
pos = ra.get_position(ra.cylinder_handle)
status("After fetching position", ra)
pos[2] *= 6
ra.goto_position(pos)
status("After lifting", ra)
pos = ra.get_position(ra.bin_handle)
def __init__(self, vrep_ip: str, vrep_port: int):
"""Prepares the actions, states and other environment variables
"""
self.robot = RobotArm(vrep_ip, vrep_port)
self.tolerance = utility.rnd(config.TOLERANCE)
self.unit_step = utility.rnd(config.UNIT_STEP_SIZE)
dim = self.robot.get_env_dimensions()
# Actions #########################################################
# The actions the agent can take - either goto some x,y,z position
# or engage/disengage claw
x_range_actions = np.arange(dim[0][0], dim[0][1], self.unit_step)
y_range_actions = np.arange(dim[1][0], dim[1][1], self.unit_step)
z_range_actions = np.arange(dim[2][0], dim[2][1], self.unit_step)
# Actions consist of
# a) Gripper Enable/Disable
# b) Goto location (x, y, z)
self.action_type1 = 'move_gripper'
self.action_type2 = 'engage_gripper'
self.actions = []
self.actions.append([self.action_type2, True])
self.actions.append([self.action_type2, False])
print(x_range_actions[1:-1])
print(y_range_actions[1:-1])
print(z_range_actions[1:-1])
for x in x_range_actions[1:-1]:
for y in y_range_actions[1:-1]:
for z in z_range_actions[1:-1]:
self.actions.append([self.action_type1, [x, y, z]])
self.total_actions = len(self.actions)
# States #########################################################
# States consist of
# a) Position of the object (x, y, z coordinates)
# b) If it is held by gripper or not
# c) Position of the gripper (x, y, z coordinates)
x_range = np.arange(dim[0][0], dim[0][1], self.tolerance)
y_range = np.arange(dim[1][0], dim[1][1], self.tolerance)
z_range = np.arange(dim[2][0], dim[2][1], self.tolerance)
self.states = []
self.invalid_state = config.INVALID_STATE
for x in x_range:
for y in y_range:
for z in z_range:
for b in [True, False]:
for xa in x_range_actions:
for ya in y_range_actions:
for za in z_range_actions:
self.states.append(
[x, y, z, b, xa, ya, za])
# invalid state, the last state. This state suggests that the object is outside the environment.
self.states.append(self.invalid_state)
self.total_states = len(self.states)
self.invalid_states_index = self.total_states - 1
log_and_display("There are {0} actions.".format(self.total_actions))
log_and_display("There are {0} states.".format(self.total_states))
self.episode_object_gripped = False
self.environment_breached = False
self.is_success = False
self.actionstate_prev = {}
self.actionstate_curr = {}
import utility
import decimal
def status(title, ra):
print("")
print(title)
print("Is object held> ", ra.is_object_held())
print("Is object in bin> ", ra.is_object_in_bin())
print("------------------------>")
ra = RobotArm('127.0.0.1', 19997)
ra.restart_sim()
pos = ra.get_position(ra.cylinder_handle)
pos[2] += utility.rnd(0.02)
ra.goto_position(pos)
status("", ra)
time.sleep(2)
ra.enable_grip(True)
status("", ra)
time.sleep(2)
pos[2] *= 6
ra.goto_position(pos)
time.sleep(2)
"""
print(ra.get_env_dimensions())
pos = ra.get_position(ra.gripper_handle)
print(pos)
pos[0] += utility.rnd(0.01)
def waist_fit(z, beam_width, weight, **kwargs):
imsave = kwargs.get('savepath', False)
disp = kwargs.get('print', False)
# Import Modules
import numpy as np
import matplotlib.pyplot as plt
import math
from scipy.special import erf
from scipy.optimize import curve_fit
import utility as ut
# Convert z-positions to array with 0 = lowest position and units are mm
z = np.array(z)
z = (z - z[0]) * (25.4 / 1000)
# Define the fit function
def fit_waist(z, w_0, f):
z_0 = (math.pi * np.square(w_0)) / (572 * 1E-6)
return w_0 * np.sqrt(1 + np.square((z - f) / z_0))
param_guess = [4E-3, np.mean(z)]
# Normalize the weights
weight = weight / np.amax(weight)
# Do the fit
param, covar = curve_fit(fit_waist,
z,
beam_width,
p0=param_guess,
sigma=weight)
# Plot the fit and data
z_cont = np.linspace(0, np.amax(z), 100)
if flength == 'fit':
fit = fit_waist(z_cont, param[0], param[1])
if flength != 'fit':
fit = fit_waist(z_cont, param[0])
fig = plt.figure('Beam Fit')
plt.clf()
ax = fig.add_subplot(111)
plt.plot(z, beam_width, 'bo', label='Data')
plt.plot(z_cont, fit, 'k--', label='Fit')
plt.axis([-.1, 1.1 * np.amax(z), 0, 1.25 * np.amax(beam_width)])
plt.xlabel('z (mm)')
plt.ylabel('Spot Size (mm)')
plt.legend()
bbox_props = dict(boxstyle='square', fc='.9', ec='k', alpha=1.0)
fig_text = 'Beam Waist: ' + ut.rnd(1000 * param[0], 2) + ' +/- ' + ut.rnd(
1000 * np.sqrt(covar[0, 0]), 2) + ' micron'
plt.text(.02,
.97,
fig_text,
fontsize=10,
bbox=bbox_props,
va='top',
ha='left',
transform=ax.transAxes)
if imsave != False:
ut.create_dir(imsave)
plt.savefig(imsave + '/beam_fit.png')
if disp == True:
print('w0: ' + ut.rnd(1000 * param[0], 2) + ' um\nz0: ' +
ut.rnd((math.pi * np.square(1000 * param[0])) /
(572 * 1E-3), 2) + ' um\nf: ' + ut.rnd(param[1], 2) +
' mm')
plt.show()
def prof_solo(profnum, z, **kwargs):
# Define kwargs
imsave = kwargs.get('savefile', 'none')
fitrange = kwargs.get('fit_range', 'full')
xstep = kwargs.get('xstep', 'auto')
units = kwargs.get('units', 'microns')
# Import modules
import numpy as np
import matplotlib.pyplot as plt
import math
from scipy.special import erf
from scipy.optimize import curve_fit
import pylab
import os
import gen_reader as gr
import utility as ut
# Define fit function (erf for low to high power)
def fit_erf(x, a, x_0, w, offset):
return (a / 2.) * (1 + erf((np.sqrt(2) * (x - x_0)) / w)) + offset
# Read in data from file
if xstep == 'auto':
matrix = gr.reader('/home/chris/anaconda/data/' + str(date) +
'/lv/prof' + str(profnum) + '/prof' + str(profnum) +
'_amp25_zpos_micron_' + z + '.txt',
header=False)
pos, pows = (.166E-3 / 25.) * matrix[:, 0], matrix[:, 1]
if xstep != 'auto':
matrix = gr.reader('/home/chris/anaconda/data/' + str(date) +
'/lv/profman' + str.zfill(profnum, 2) + '/profman' +
str.zfill(profnum, 2) + '_z-pos-' + units +
str.zfill(z, 5) + '_x-step-' + units +
str.zfill(xstep, 5) + '.txt',
header=False)
pows = matrix[:, 0]
pos = 1E-3 * np.arange(0, float(xstep) * len(pows), float(xstep))
if units == 'minches':
pos = pos * 25.4
# Make position array and power array (normalized low to high power)
if pows[0] > pows[-1]:
pows_norm = np.flipud(np.array(pows)) / np.amax(np.array(pows))
else:
pows_norm = np.array(pows) / np.amax(np.array(pows))
# Get initial parameter guesses
param_guess = [1, 0, 0, 0]
# Guess for w
bnds = ut.bound_finder(pows_norm, [.1, .9])
param_guess[2] = pos[bnds[1]] - pos[bnds[0]]
# Guess for x_0
for i in range(len(pows)):
if pows_norm[i] <= .5:
param_guess[1] = pos[i]
continue
else:
if pows_norm[i] - .5 >= .5 - pows_norm[i - 1]:
param_guess[1] = pos[i - 1]
break
else:
break
# Do the fit
if fitrange != 'full':
param, covar = curve_fit(fit_erf,
pos[fitrange[0]:fitrange[1]],
pows_norm[fitrange[0]:fitrange[1]],
p0=param_guess)
if fitrange == 'full':
param, covar = curve_fit(fit_erf, pos, pows_norm, p0=param_guess)
# Plot the fits
x = np.linspace(0, np.amax(pos), 100)
fit = fit_erf(x, param[0], param[1], param[2], param[3])
fig = plt.figure('laser_prof')
plt.clf()
ax = fig.add_subplot(111)
plt.plot(pos, pows_norm, 'bo', label='Data')
plt.plot(x, fit, 'k--', label='Fit')
plt.axis([0, 1.1 * np.amax(pos), 0, 1.25])
plt.xlabel('Razor Blade Position (mm)')
plt.ylabel('Normalized Laser Power')
plt.legend()
bbox_props = dict(boxstyle='square', fc='.9', ec='k', alpha=1.0)
fig_text = 'Spot Size: ' + ut.rnd(1000 * param[2], 2) + ' +/- ' + ut.rnd(
1000 * np.sqrt(covar[2, 2]), 2) + ' micron'
#fig_text='Amplitude: '+repr(round(param[0],2))+'\nOffset: '+repr(round(param[1],2))+' mm'+'\nSpot Size: '+repr(1000*round(param[2]),3)+' micron'
plt.text(.02,
.97,
fig_text,
fontsize=10,
bbox=bbox_props,
va='top',
ha='left',
transform=ax.transAxes)
if imsave != 'none':
ut.create_dir(imsave)
plt.savefig(imsave + 'beam_profile_' + str.zfill(z, 5) + '.png')
plt.show()
return param[2], np.sqrt(covar[2, 2]), pos, pows_norm
import decimal
def status(title, ra):
print("")
print(title)
print("Is object held> ", ra.is_object_held())
print("Is object in bin> ", ra.is_object_in_bin())
print("------------------------>")
ra = RobotArm('127.0.0.1', 19997)
ra.restart_sim()
pos = ra.get_position(ra.cylinder_handle)
pos[2] += utility.rnd(0.02)
ra.goto_position(pos)
status("", ra)
time.sleep(2)
ra.enable_grip(True)
status("", ra)
time.sleep(2)
pos[2] *= 6
ra.goto_position(pos)
time.sleep(2)
"""
print(ra.get_env_dimensions())
pos = ra.get_position(ra.gripper_handle)
print(pos)
pos[0] += utility.rnd(0.01)
pos[1] += utility.rnd(0.01)
def status(title, ra):
print("")
print(title)
print("Is object held> ", ra.is_object_held())
print("Is object in bin> ", ra.is_object_in_bin())
print("------------------------>")
ra = RobotArm('127.0.0.1', 19997)
ra.restart_sim()
pos = ra.get_position(ra.cylinder_handle)
status("After getting position of cylinder", ra)
pos[0] += utility.rnd(0.0)
ra.goto_position(pos)
status("After going to the cylinder", ra)
ra.enable_grip(True)
status("After enabling grip", ra)
pos = ra.get_position(ra.cylinder_handle)
status("After fetching position", ra)
pos[2] = utility.rnd(0.10)
ra.goto_position(pos)
status("After lifting", ra)
pos = ra.get_position(ra.bin_handle)
Q_TABLE_FILE = 'qtables/qtable.txt.npy'
# File where per-episodic data will be saved: episode id, is success?, total rewards, total actions.
PLOT_FILE = 'qtables/episodes.txt'
# A filler state which represents an invalid state.
INVALID_STATE = [-100, -100, -100, False, -100, -100, -100]
# The step size to use to discretize the environment
UNIT_STEP_SIZE = 0.02
# A value to judge nearness
TOLERANCE = 0.01
# A finer value to judge nearness
TOLERANCE_FINER = 0.005
# Initial position of the arm (to be placed before any episode starts)
INIT_ARM_POSITION = [utility.rnd(-0.30), utility.rnd(-0.10), utility.rnd(0.14)]
# Dimension of the environment. [[xmin, xmax], [ymin, ymax], [zmin, zmax]]
ENV_DIMENSION = [[utility.rnd(-0.32), utility.rnd(-0.19)],
[utility.rnd(-0.12), utility.rnd(-0.05)],
[utility.rnd(0.00), utility.rnd(0.15)]]
# Some artificial delays to let V-REP stabilize after an action
SLEEP_VAL = 0.4
SLEEP_VAL_MIN = 0.3
# Distance between object and bin when object is in bin
CYLINDER_BIN_DISTANCE = 0.013
# Rewards
REWARD_TERMINATION = -10
return np.sqrt(x2 + y2 + z2)
def status(title, ra):
print("")
print(title)
print("Is object held> ", ra.is_object_held())
print("Is object in bin> ", ra.is_object_in_bin())
print("------------------------>")
ra = RobotArm('127.0.0.1', 19997)
ra.restart_sim()
pos = ra.get_position(ra.cylinder_handle)
status("After getting position of cylinder", ra)
pos[0] += utility.rnd(0.0)
ra.goto_position(pos)
status("After going to the cylinder", ra)
ra.enable_grip(True)
status("After enabling grip", ra)
pos = ra.get_position(ra.cylinder_handle)
status("After fetching position", ra)
pos[2] = utility.rnd(0.10)
ra.goto_position(pos)
status("After lifting", ra)
pos = ra.get_position(ra.bin_handle)
Q_TABLE_FILE = 'qtables/qtable.txt.npy'
# File where per-episodic data will be saved: episode id, is success?, total rewards, total actions.
PLOT_FILE = 'qtables/episodes.txt'
# A filler state which represents an invalid state.
INVALID_STATE = [-100, -100, -100, False, -100, -100, -100]
# The step size to use to discretize the environment
UNIT_STEP_SIZE = 0.02
# A value to judge nearness
TOLERANCE = 0.01
# A finer value to judge nearness
TOLERANCE_FINER = 0.005
# Initial position of the arm (to be placed before any episode starts)
INIT_ARM_POSITION = [utility.rnd(-0.30), utility.rnd(-0.10), utility.rnd(0.14)]
# Dimension of the environment. [[xmin, xmax], [ymin, ymax], [zmin, zmax]]
ENV_DIMENSION = [[utility.rnd(-0.32), utility.rnd(-0.19)],
[utility.rnd(-0.12), utility.rnd(-0.05)],
[utility.rnd(0.00), utility.rnd(0.15)]]
# Some artificial delays to let V-REP stabilize after an action
SLEEP_VAL = 0.4
SLEEP_VAL_MIN = 0.3
# Distance between object and bin when object is in bin
CYLINDER_BIN_DISTANCE = 0.013
# Rewards
REWARD_TERMINATION = -10