def get_imdb(self):
ur5_kin = UR5Kin(self.t_scale)
raw_data_file = os.path.join(self.data_path, self.config_file_name)
output_file_name = self.config_file_name[:-4]+'_lowlyap.csv'
output_file_path = os.path.join(self.output_path, output_file_name)
low_lyap_count = 0
try:
os.remove(output_file_path)
except OSError:
pass
with open(raw_data_file) as raw_data, open(output_file_path, "a") as output_file:
reader = list(csv.DictReader(raw_data))
writer = csv.writer(output_file)
output_hearder = self.joint_names + self.cube_pose_header
writer.writerow(output_hearder)
for row in tqdm(reader):
joint_config = self.get_joint_config_from_row(row)
X_H = ur5_kin.get_ee_pose(joint_config)
X_Ts = self.get_XTs_from_row(row)
lyap_func = LyapEH()
lyap_buff = np.array([])
for X_T in X_Ts:
if X_T[2,3]>self.table_height:
lyap_func.X_T = X_T
lyap_temp = lyap_func.cmpt_Lyap(X_H)
lyap_buff = np.append(lyap_buff, lyap_temp)
lyap = np.amin(lyap_buff)
lyap_func.X_T = X_Ts[np.argmin(lyap_buff)]
# dataset augmentation: if True, the generated imdb will be 6x bigger
if lyap < self.lyap_threshold:
writer.writerow(row.values())
low_lyap_count += 1
print('Low Lyap Sample Size: %i' % low_lyap_count)
def __init__(self, data_path, output_path, dataset_focus):
self.data_path = data_path
self.output_path = output_path
self.output_needs_header = True
self.dataset_focus = dataset_focus
config_metadata = self.load_metadata()
self.table_height = float(config_metadata['table_height'])
self.low_lyap_threshold = float(config_metadata['low_lyap_threshold'])
self.grasp_offset = float(config_metadata['grasp_offset'])
self.t_scale = float(config_metadata['t_scale'])
self.ur5_kin = UR5Kin(self.t_scale)
self.lyap_func = LyapEH()
def __init__(self, cnn_infer, t_scale):
self.d_theta = 0.1
self.t_scale = float(t_scale)
self.grasp_offset = 0.05 # grasp offset along global Z axis
self.window = tk.Tk()
self.window.title('APCNet Simulator Demo')
# Create a canvas that can fit the above video source size
self.canvas = tk.Canvas(self.window, width = 848, height = 480)
self.canvas.pack()
# Button that lets the user take a snapshot
self.start_bt = tk.Button(self.window, text='Start', width=50, command=self.start_bt_func)
self.start_bt.pack(anchor=tk.CENTER, expand=True)
self.home_bt = tk.Button(self.window, text="Home", width=50, command=arm2home_sim)
self.home_bt.pack(anchor=tk.CENTER, expand=True)
self.shuffle_bt = tk.Button(self.window, text="Shuffle Cubes", width=50, command=move_cubes_sim)
self.shuffle_bt.pack(anchor=tk.CENTER, expand=True)
self.delay = 30
self.cv2_img = None
self.update()
self.is_start = False
if cnn_infer == "infer":
self.apcNet = APCNetSiam()
self.apcNet.model_fld = 's2r_test02_2048_dense'
self.apcNet.load_ckpt(load_best=False)
self.preprocess = transforms.Compose([transforms.ToPILImage(),
transforms.CenterCrop(480),
transforms.Resize((224, 224)),
transforms.ToTensor(),
transforms.Normalize(mean=[0.485, 0.456, 0.406],
std=[0.229, 0.224, 0.225])])
self.ur5_kin = UR5Kin(self.t_scale)
self.lyap_func = LyapEH()
self.apcNet_infer = cnn_infer
self.image_topic = "/camera/rgb/image_raw"
self.joint_names = ['shoulder_pan_joint', 'shoulder_lift_joint',
'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']
# Init msg for publishing
self.joint_traj = JointTrajectory()
self.joint_traj.joint_names = self.joint_names
self.real_joint_traj = JointTrajectory()
self.real_joint_traj.joint_names = self.joint_names
self.old_time = time.time()
self.lyap_vctrl_old = np.zeros([6,1])
self.real_joint_traj.points = [JointTrajectoryPoint(positions = [0]*6,
velocities= [0]*6,
time_from_start=rospy.Duration(0.1))]
# Approximated Synchronised Topics
self.image_capturer_sub = message_filters.Subscriber(
self.image_topic, Image)
self.q_sub = message_filters.Subscriber(
"/arm_controller/state", JointTrajectoryControllerState)
self.synced_msgs = message_filters.ApproximateTimeSynchronizer(
[ self.image_capturer_sub, self.q_sub], 10, 0.005)
self.synced_msgs.registerCallback(self.image_callback)
self.sim_joint_command_pub = rospy.Publisher("/arm_controller/command",
JointTrajectory, queue_size=10)
def __init__(self, cnn_infer):
# load metadata
config_metadata = self.load_metadata()
self.t_scale = float(config_metadata['t_scale'])
self.grasp_offset = float(config_metadata['grasp_offset'])
self.table_height = float(config_metadata['table_height'])
self.lyap_switch_threshold = 0.0
self.switch_control = False
# Load Local and Global Net
if cnn_infer == "infer":
# Config Global Network
self.apcNet_local = DirectSiamese()
self.apcNet_local.model_fld = 'direct_siamese_e2e_zoom_01'
self.apcNet_local.load_ckpt(load_best=False)
# Config Local Network
self.apcNet_global = DirectSiamese()
self.apcNet_global.model_fld = 'direct_siamese_e2e_01'
self.apcNet_global.load_ckpt(load_best=False)
self.ur5_kin = UR5Kin(self.t_scale)
self.lyap_func = LyapEH()
self.apcNet_infer = cnn_infer
self.image_topic = "/sim_camera/image_raw"
self.joint_names = [
'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
]
# Init msg for publishing
self.joint_traj = JointTrajectory()
self.joint_traj.joint_names = self.joint_names
self.vel_ctrl_old = np.zeros([6, 1])
# Approximated Synchronised Topics
self.image_capturer_sub = message_filters.Subscriber(
self.image_topic, Image)
self.q_sub = message_filters.Subscriber(
"/arm_controller/state", JointTrajectoryControllerState)
self.synced_msgs = message_filters.ApproximateTimeSynchronizer(
[self.image_capturer_sub, self.q_sub], 10, 0.005)
self.synced_msgs.registerCallback(self.image_callback)
self.sim_joint_command_pub = rospy.Publisher("/arm_controller/command",
JointTrajectory,
queue_size=10)
self.real_joint_command_pub = rospy.Publisher("/ur_driver/joint_speed",
JointTrajectory,
queue_size=10)
class RawData2IMDB():
"""
This Class Interprets raw data collected from the simulator to the CNN imdb.
"""
def __init__(self, data_path, output_path, dataset_focus):
self.data_path = data_path
self.output_path = output_path
self.output_needs_header = True
self.dataset_focus = dataset_focus
config_metadata = self.load_metadata()
self.table_height = float(config_metadata['table_height'])
self.low_lyap_threshold = float(config_metadata['low_lyap_threshold'])
self.grasp_offset = float(config_metadata['grasp_offset'])
self.t_scale = float(config_metadata['t_scale'])
self.ur5_kin = UR5Kin(self.t_scale)
self.lyap_func = LyapEH()
def get_XT_from_Qt(self, Q, t):
R = tf.transformations.quaternion_matrix(Q)
I = tf.transformations.identity_matrix() # 4x4 Identity_matrix
X = R+tf.transformations.translation_matrix(t)-I
X[2, 3] = X[2, 3]+self.grasp_offset
X = self.scale_translation(X)
return X
def get_XTs_from_row(self, row):
Q1 = [row['cube1_a'], row['cube1_b'], row['cube1_c'], row['cube1_d']]
t1 = [row['cube1_x'], row['cube1_y'], row['cube1_z']]
Q2 = [row['cube2_a'], row['cube2_b'], row['cube2_c'], row['cube2_d']]
t2 = [row['cube2_x'], row['cube2_y'], row['cube2_z']]
Q3 = [row['cube3_a'], row['cube3_b'], row['cube3_c'], row['cube3_d']]
t3 = [row['cube3_x'], row['cube3_y'], row['cube3_z']]
XT1 = self.get_XT_from_Qt(Q1, t1)
XT2 = self.get_XT_from_Qt(Q2, t2)
XT3 = self.get_XT_from_Qt(Q3, t3)
X_T_candidates = [XT1, XT2, XT3]
X_Ts = []
for X_T_temp in X_T_candidates:
if X_T_temp[2,3]>self.table_height*self.t_scale:
X_Ts.append(X_T_temp)
return X_Ts
def get_joint_config_from_row(self, row):
joint_config = [row['shoulder_pan_joint'],
row['shoulder_lift_joint'],
row['elbow_joint'],
row['wrist_1_joint'],
row['wrist_2_joint'],
row['wrist_3_joint']]
joint_config = np.array([float(i) for i in joint_config])
joint_config = np.around(joint_config, decimals=4)
joint_config = joint_config.reshape((6,1))
return joint_config
def get_imdb(self):
raw_data_file = os.path.join(self.data_path, 'armbot_dataset_3cubes.csv')
output_file_name = 'imdb_one_shot.csv'
output_file_path = os.path.join(self.output_path, output_file_name)
try:
os.remove(output_file_path)
except OSError:
pass
with open(raw_data_file) as raw_data, open(output_file_path, "a") as output_file:
reader = list(csv.DictReader(raw_data))
writer = csv.writer(output_file)
for row in reader[10:11]:
image_name = row['image_name']
joint_config = self.get_joint_config_from_row(row)
X_H = self.ur5_kin.get_ee_pose(joint_config)
X_Ts = self.get_XTs_from_row(row)
# veced_poses = ur5_kin.get_veced_joint_poses(joint_config)
lyap_buff = np.array([])
for X_T in X_Ts:
if X_T[2,3]>self.table_height:
self.lyap_func.X_T = X_T
lyap_temp = self.lyap_func.cmpt_Lyap(X_H)
lyap_buff = np.append(lyap_buff, lyap_temp)
lyap = np.amin(lyap_buff)
self.lyap_func.X_T = X_Ts[np.argmin(lyap_buff)]
# dataset augmentation: if True, the generated imdb will be 6x bigger
output_hearder = ['image_name', 'lyap', 'pd_1', 'pd_2','pd_3',
'pd_4', 'pd_5', 'pd_6', 's_pan', 's_lift',
'elbow', 'w1', 'w2','w3']
if self.output_needs_header:
writer.writerow(output_hearder)
self.output_needs_header = False
if self.dataset_focus == 'global':
meet_collect_criterion = lyap >= 5e-4
new_image_name = image_name
elif self.dataset_focus == 'local':
meet_collect_criterion = lyap <= self.low_lyap_threshold
else:
sys.exit("The Dataset Focus Needs to Be Either \'global\' or \'local\'")
if meet_collect_criterion:
pd_Lyap = list()
for i in range(6):
self.cmpt_mean_value_point(joint_config, i, self.lyap_func.X_T, 0.05)
pd_matrix = self.ur5_kin.get_pd_X_H(joint_config, i)
pd_analyt = self.lyap_func.cmpt_pd_Lyap(X_H, pd_matrix)
print('pd_analyt: %.4f' % pd_analyt)
pd_Lyap.append(pd_analyt)
new_row = [image_name]+[lyap]+pd_Lyap+[row['shoulder_pan_joint'],
row['shoulder_lift_joint'],
row['elbow_joint'],
row['wrist_1_joint'],
row['wrist_2_joint'],
row['wrist_3_joint']]
writer.writerow(new_row)
def scale_translation(self, X):
X[0:3, 3] = self.t_scale*X[0:3, 3]
return X
def cmpt_mean_value_point(self, joint_config, joint_index, X_T, delta):
# LHS
G = np.copy(X_T)
A, B = self.ur5_kin.get_constant_DH_mats(joint_config, joint_index)
inv_A = np.linalg.inv(A)
inv_B = np.linalg.inv(B)
# Rotation matrix Z Linearisation
C = np.zeros([4, 4]); C[0, 0] = -1; C[1, 1] = -1
D = np.zeros([4, 4]); D[0, 1] = 1; D[1, 0] = -1
E = np.zeros([4, 4]); E[0, 1] = -1; E[1, 0] = 1;
F = np.zeros([4, 4]); F[0, 1] = 1; F[1, 0] = -1
H = np.zeros([4, 4]); H[0, 0] = 1; H[1, 1] = 1
# print(C, D, E, F, H)
# for a quadratic equation ax^2+x^2+c
M1 = np.transpose(inv_B.dot(inv_A).dot(G))
M2 = np.transpose(inv_B.dot(C).dot(inv_A).dot(G))
M3 = np.transpose(inv_B.dot(D).dot(inv_A).dot(G))
M4 = inv_B.dot(E).dot(inv_A).dot(G)
M5 = inv_B.dot(F).dot(inv_A).dot(G)
M6 = inv_B.dot(H).dot(inv_A).dot(G)
a = np.trace(M2.dot(M5))
b = np.trace(M3.dot(M5)+M2.dot(M6))
c = np.trace(M1.dot(M5)+M2.dot(M4)-M5)
d = np.trace(M3.dot(M4)+M1.dot(M6)-M6)
e = np.trace(M1.dot(M4)-M4)
# RHS
perturbed_joint_config = np.copy(joint_config)
perturbed_joint_config[joint_index, 0] = perturbed_joint_config[joint_index, 0]+delta
lyap_left = self.lyap_func.cmpt_Lyap(self.ur5_kin.get_ee_pose(joint_config))
lyap_right = self.lyap_func.cmpt_Lyap(self.ur5_kin.get_ee_pose(
perturbed_joint_config))
slope = (lyap_right-lyap_left)/(delta)
e = e+slope/2.0
# coefficients p[] for quartic equation
p = [4.0*a+2.0*c+e, 4.0*b+2.0*d, 2.0*c+2.0*e, 2.0*d, e]
roots = np.roots(p)
real_roots = roots[np.isreal(roots)].real
mean_value_points = 2.0*np.arctan(real_roots)
left_end = joint_config[joint_index, 0] - 5*delta
right_end = joint_config[joint_index, 0] + 5*delta
mean_value_point = mean_value_points[(left_end<mean_value_points) &
(mean_value_points<right_end)]
print(mean_value_point)
print(joint_config[joint_index,0])
c = np.copy(joint_config)
c[joint_index, 0] = mean_value_point[0]
pd_matrix = self.ur5_kin.get_pd_X_H(c, joint_index)
pd_analyt = self.lyap_func.cmpt_pd_Lyap(self.ur5_kin.get_ee_pose(c), pd_matrix)
print('slope: %.4f, mvp_pd: %.4f' % (slope, pd_analyt))
# return mean_value_point
def load_metadata(self):
metadata_path = os.path.expanduser('~/apclab_dev/src/config.yml')
with open(metadata_path, 'r') as config_file:
try:
config_metadata = yaml.load(config_file)
except yaml.YAMLError as exc:
print(exc)
return config_metadata