def initialize(self):
R = rospy.get_param('~train_pub_rate', 100)
self.rate = rospy.Rate(R)
self.buffer_duration = rospy.get_param('~buffer_duration', 0.1)
self.buffer_size = rospy.get_param('~buffer_size', 500)
# Robot-specific setup implemented by derived class
self.setup_robot()
# Jitter robot initially
XI, YI = self.jitter_robot()
num_joints = np.size(YI, 1)
# Create and initialize SOLAR_GP model
self.solar = LocalModels(self.num_inducing,
wgen=self.wgen,
xdim=3,
ndim=num_joints * 2)
self.solar.initializeF(XI, YI)
# Serialize SOLAR_GP model into custom message and publish
SolarMsg = self.constructMsg(self.solar)
self.pub_solar.publish(SolarMsg)
# Create Data buffer listening on training input-output topics
self.TrainData = DataBuffer(self.x_topic, self.y_topic,
self.joint_names, self.buffer_duration,
self.buffer_size)
def __init__(self, topic):
"""
topic: name of end effector state topic
"""
self.model = LocalModels()
self.pred_pub = rospy.Publisher('prediction', Arrays, queue_size=10)
self.curX = []
self.curPose = PoseStamped()
rospy.Subscriber('solarGP', LocalGP, self.model_callback)
rospy.Subscriber(topic, EndpointState, self.x_callback, queue_size=10)
rospy.wait_for_service('set_teleop_pose')
self.set_teleop_pose = rospy.ServiceProxy('set_teleop_pose', SetPose)
class Solar_Trainer():
"""
This class creates a base Trainer module to train a SOLAR_GP model on training input-output pairs.
SOLAR_GP models are serialized and sent across a custom topic message interpreted by the SolarPredictor
"""
def __init__(self, njit, degrees, num_inducing, wgen, use_old_Z=False):
"""
njit: number of initial jittered ponts
degrees: degree range of jittered joints
num_inducing: number of inducing points for sparse GP models
wgen: weigted threshold for generating new models
"""
self.solar = []
self.pub_solar = rospy.Publisher('solarGP',
LocalGP,
queue_size=10,
latch=True)
self.njit = njit
self.degrees = degrees
self.num_inducing = num_inducing
self.wgen = wgen
self.use_old_Z = use_old_Z
self.joint_names = []
self.TrainData = []
self.rate = []
self.stop = False
self.pub_traintime = rospy.Publisher('traintime',
Float64,
queue_size=10)
self.x_topic = ""
self.y_topic = ""
rospy.wait_for_service('set_neutral')
self.set_neutral = rospy.ServiceProxy('set_neutral', SetNeutral)
rospy.wait_for_service('jitter')
self.jitter_init = rospy.ServiceProxy('jitter', Jitter)
def initialize(self):
R = rospy.get_param('~train_pub_rate', 100)
self.rate = rospy.Rate(R)
self.buffer_duration = rospy.get_param('~buffer_duration', 0.1)
self.buffer_size = rospy.get_param('~buffer_size', 500)
# Robot-specific setup implemented by derived class
self.setup_robot()
# Jitter robot initially
XI, YI = self.jitter_robot()
num_joints = np.size(YI, 1)
# Create and initialize SOLAR_GP model
self.solar = LocalModels(self.num_inducing,
wgen=self.wgen,
xdim=3,
ndim=num_joints * 2)
self.solar.initializeF(XI, YI)
# Serialize SOLAR_GP model into custom message and publish
SolarMsg = self.constructMsg(self.solar)
self.pub_solar.publish(SolarMsg)
# Create Data buffer listening on training input-output topics
self.TrainData = DataBuffer(self.x_topic, self.y_topic,
self.joint_names, self.buffer_duration,
self.buffer_size)
def setup_robot(self):
print("Setup Robot not implemented")
return False
def jitter_robot(self):
XI = []
YI = []
print("Jitter Robot not implemented")
# Service based implementation
# self.set_neutral()
# self.TrainData = DataBuffer(self.x_topic, self.y_topic, self.joint_names, self.buffer_duration, self.buffer_size)
# self.jitter_init(self.njit, self.degrees)
#
# XI = np.asarray(self.TrainData.Xexp).reshape(len(self.TrainData.Xexp),3)
# YI = np.asarray(self.TrainData.Yexp).reshape(len(self.TrainData.Yexp),len(self.joint_names))
# rospy.loginfo("Number of initial points: %s", len(XI))
# self.TrainData.clear()
return XI, YI
def jitter(self, n, Y_init, deg=5):
"""
Randomly sample joint states within specified degree range from initial joint position
"""
max_rough = 0.0174533
pert = deg * max_rough * np.random.uniform(-1., 1.,
(n, np.size(Y_init, 1)))
Y_start = Y_init + pert
return Y_start
def constructMsg(self, local):
"""
Serializes SOLAR_GP object into custom ROS topic msg
"""
LocMsg = LocalGP()
L = []
for count, m in enumerate(local.Models):
GP = OSGPR_GP()
GP.kern_var = m.kern.variance[0]
GP.kern_lengthscale = np.array(m.kern.lengthscale).tolist()
GP.likelihood_var = m.likelihood.variance[0]
GP.xmean = local.LocalData[count][2][0].tolist()
GP.ymean = local.LocalData[count][3][0].tolist()
GP.numloc = local.LocalData[count][0]
Z = np.array(m.Z)
Z_old = np.array(m.Z_old)
mu_old = np.array(m.mu_old)
Su_old = np.array(m.Su_old)
Kaa_old = np.array(m.Kaa_old)
X_arr = []
Y_arr = []
Z_arr = []
Z_old_arr = []
mu_old_arr = []
Su_old_arr = []
Kaa_old_arr = []
for j in range(0, np.shape(m.X)[0]):
X_row = Arrays()
Y_row = Arrays()
X_row.array = np.array(m.X[j, :]).tolist()
Y_row.array = np.array(m.Y[j, :]).tolist()
X_arr.append(X_row)
Y_arr.append(Y_row)
for j in range(0, np.shape(Z)[0]):
Z_row = Arrays()
Z_row.array = Z[j, :].tolist()
Z_arr.append(Z_row)
for j in range(0, np.shape(Z_old)[0]):
Z_old_row = Arrays()
mu_old_row = Arrays()
Su_old_row = Arrays()
Kaa_old_row = Arrays()
Z_old_row.array = Z_old[j, :].tolist()
mu_old_row.array = mu_old[j, :].tolist()
Su_old_row.array = Su_old[j, :].tolist()
Kaa_old_row.array = Kaa_old[j, :].tolist()
Z_old_arr.append(Z_old_row)
mu_old_arr.append(mu_old_row)
Su_old_arr.append(Su_old_row)
Kaa_old_arr.append(Kaa_old_row)
GP.X = X_arr
GP.Y = Y_arr
GP.Z = Z_arr
GP.Z_old = Z_old_arr
GP.mu_old = mu_old_arr
GP.Su_old = Su_old_arr
GP.Kaa_old = Kaa_old_arr
L.append(GP)
LocMsg.localGPs = L
LocMsg.W = local.W.diagonal().tolist()
LocMsg.M = local.M
LocMsg.xdim = local.xdim
LocMsg.ndim = local.ndim
return LocMsg
def run(self):
while not rospy.is_shutdown() and not self.stop:
t1 = time.time()
# Skip training if data buffer is empty
if not self.TrainData.Xexp:
continue
else:
try:
# Grab training pairs from buffer
Xexp = np.asarray(self.TrainData.Xexp).reshape(
len(self.TrainData.Xexp), 3)
Y = np.asarray(self.TrainData.Yexp).reshape(
len(self.TrainData.Yexp), len(self.joint_names))
Yexp = self.solar.encode_ang(Y)
except:
continue
# Clear buffer
self.TrainData.clear()
try:
# Train drifting model and save trained hyperparameters
mdrift = self.solar.doOSGPR(Xexp,
Yexp,
self.solar.mdrift,
100,
use_old_Z=True,
driftZ=False)
mkl = []
for j in range(0, self.solar.xdim):
mkl.append(1 / (mdrift.kern.lengthscale[j]**2))
W = np.diag(mkl)
self.solar.W = W
self.solar.mdrift = mdrift
except:
pass
# Partition training pairs
self.solar.partition(Xexp.reshape(len(Xexp), self.solar.xdim),
Yexp.reshape(len(Yexp), self.solar.ndim))
try:
# Train SOLAR_GP model
self.solar.train()
except:
pass
# Construct and publish custom SOLAR_GP ROS topic
LocMsg = self.constructMsg(self.solar)
self.pub_solar.publish(LocMsg)
# Publish training time
t2 = time.time()
self.pub_traintime.publish(t2 - t1)
self.rate.sleep()
#Test = TestTrajectory(Trajectory.nPoly2D(n,4,r,x0,y0, arclength = -2*np.pi,flip =0), robot) #Test = TestTrajectory(Trajectory.Rect2D(n, width = 1, height = 1), robot) #Test = TestTrajectory(Trajectory.Star2D(n,5,r,x0,y0), robot) #Test = TestTrajectory(Trajectory.Spiral2D(n,1,0.5,x0,y0,6*np.pi), robot) workspace = Trajectory.Circle2D(n,np.sum(robot2D.dh_table[:,1]),0,0) # size of reachable workspace X_test = Test.xtest #Y_test = Test.ytest # if inverse kinematics is provided njit = 25 #number of jittered data points num_inducing = 10 # max number of induced/support points per model " Initialize Local Models" local = LocalModels(num_inducing, wgen =0.9, robot = Test.robot) local.initialize(njit,YStart, Test.robot) Xtot = local.XI # stack of experiences Ytot = local.YI "Timing" tc = np.zeros([len(X_test)-1,1]) "Axis settings (for circle trajectory)" k = 1 xmin = workspace.center_x - k*workspace.radius xmax = workspace.center_x + k*workspace.radius ymin = workspace.center_y - k*workspace.radius ymax = workspace.center_y + k*workspace.radius
def model_callback(self, LocMsg):
"""
Unserializes SOLAR_GP model messages passed across ROS Topic into SOLAR_GP Objects
"""
W = LocMsg.W
W = np.diag([W[0], W[1], W[2]])
M = LocMsg.M
LocalData = []
Models = []
xdim = LocMsg.xdim
ndim = LocMsg.ndim
for L in LocMsg.localGPs:
X_loc = []
X_loc.append(L.numloc)
X_loc.append(L.numloc)
X_loc.append(np.array(L.xmean).reshape(1, xdim))
X_loc.append(np.array(L.ymean).reshape(1, ndim))
X_loc.append(True)
LocalData.append(X_loc)
X = np.empty([0, xdim])
Y = np.empty([0, ndim])
Z = np.empty([0, xdim])
Z_old = np.empty([0, xdim])
mu_old = np.empty([0, ndim])
Su_old = np.empty([0, len(L.Z_old)])
Kaa_old = np.empty([0, len(L.Z_old)])
kern = GPy.kern.RBF(xdim, ARD=True)
kern.variance = L.kern_var
kern.lengthscale = L.kern_lengthscale
for x, y in zip(L.X, L.Y):
X = np.vstack((X, np.array(x.array).reshape(1, xdim)))
Y = np.vstack((Y, np.array(y.array).reshape(1, ndim)))
for z in L.Z:
Z = np.vstack((Z, np.array(z.array).reshape(1, xdim)))
for z, mu, su, ka in zip(L.Z_old, L.mu_old, L.Su_old, L.Kaa_old):
Z_old = np.vstack((Z_old, np.array(z.array).reshape(1, xdim)))
mu_old = np.vstack(
(mu_old, np.array(mu.array).reshape(1, ndim)))
Su_old = np.vstack(
(Su_old, np.array(su.array).reshape(1, len(L.Z_old))))
Kaa_old = np.vstack(
(Kaa_old, np.array(ka.array).reshape(1, len(L.Z_old))))
m = osgpr_GPy.OSGPR_VFE(X, Y, kern, mu_old, Su_old, Kaa_old, Z_old,
Z)
m.kern.variance = L.kern_var
m.kern.lengthscale = np.array(L.kern_lengthscale)
m.likelihood.variance = L.likelihood_var
Models.append(m)
local = LocalModels()
local.W = W
local.M = M
local.LocalData = LocalData
local.Models = Models
local.xdim = xdim
local.ndim = ndim
self.model = local
class SolarPredictor():
"""
This class creates the base Predictor module for SOLAR_GP. While running, requests are made from the
teleoperator service for the next goal pose and any relevant teleop commands. Predictions are published
on a topic for the controller
"""
def __init__(self, topic):
"""
topic: name of end effector state topic
"""
self.model = LocalModels()
self.pred_pub = rospy.Publisher('prediction', Arrays, queue_size=10)
self.curX = []
self.curPose = PoseStamped()
rospy.Subscriber('solarGP', LocalGP, self.model_callback)
rospy.Subscriber(topic, EndpointState, self.x_callback, queue_size=10)
rospy.wait_for_service('set_teleop_pose')
self.set_teleop_pose = rospy.ServiceProxy('set_teleop_pose', SetPose)
def decode_ang(self, q):
"""
Decode angles from sin/cos to radians
"""
d = int(np.size(q, 1) / 2)
decoding = np.arctan2(q[:, :d], q[:, d:]).reshape(np.size(q, 0), d)
return decoding
def x_callback(self, msg):
self.curPose.header = msg.header
self.curPose.header.frame_id = '/teleop'
self.curPose.pose = msg.pose
self.curX = np.array(
[msg.pose.position.x, msg.pose.position.y,
msg.pose.position.z]).reshape(1, 3)
def model_callback(self, LocMsg):
"""
Unserializes SOLAR_GP model messages passed across ROS Topic into SOLAR_GP Objects
"""
W = LocMsg.W
W = np.diag([W[0], W[1], W[2]])
M = LocMsg.M
LocalData = []
Models = []
xdim = LocMsg.xdim
ndim = LocMsg.ndim
for L in LocMsg.localGPs:
X_loc = []
X_loc.append(L.numloc)
X_loc.append(L.numloc)
X_loc.append(np.array(L.xmean).reshape(1, xdim))
X_loc.append(np.array(L.ymean).reshape(1, ndim))
X_loc.append(True)
LocalData.append(X_loc)
X = np.empty([0, xdim])
Y = np.empty([0, ndim])
Z = np.empty([0, xdim])
Z_old = np.empty([0, xdim])
mu_old = np.empty([0, ndim])
Su_old = np.empty([0, len(L.Z_old)])
Kaa_old = np.empty([0, len(L.Z_old)])
kern = GPy.kern.RBF(xdim, ARD=True)
kern.variance = L.kern_var
kern.lengthscale = L.kern_lengthscale
for x, y in zip(L.X, L.Y):
X = np.vstack((X, np.array(x.array).reshape(1, xdim)))
Y = np.vstack((Y, np.array(y.array).reshape(1, ndim)))
for z in L.Z:
Z = np.vstack((Z, np.array(z.array).reshape(1, xdim)))
for z, mu, su, ka in zip(L.Z_old, L.mu_old, L.Su_old, L.Kaa_old):
Z_old = np.vstack((Z_old, np.array(z.array).reshape(1, xdim)))
mu_old = np.vstack(
(mu_old, np.array(mu.array).reshape(1, ndim)))
Su_old = np.vstack(
(Su_old, np.array(su.array).reshape(1, len(L.Z_old))))
Kaa_old = np.vstack(
(Kaa_old, np.array(ka.array).reshape(1, len(L.Z_old))))
m = osgpr_GPy.OSGPR_VFE(X, Y, kern, mu_old, Su_old, Kaa_old, Z_old,
Z)
m.kern.variance = L.kern_var
m.kern.lengthscale = np.array(L.kern_lengthscale)
m.likelihood.variance = L.likelihood_var
Models.append(m)
local = LocalModels()
local.W = W
local.M = M
local.LocalData = LocalData
local.Models = Models
local.xdim = xdim
local.ndim = ndim
self.model = local
def on_teleop(self, state):
print("On Teleop not implemented")
return False
def run(self):
R = rospy.get_param('~predict_pub_rate', 100)
rate = rospy.Rate(R)
d = rospy.get_param('~max_distance', 1)
Yexp = []
rospy.wait_for_service('get_teleop')
get_teleop = rospy.ServiceProxy('get_teleop', GetTeleop)
teleop_state = GetTeleopResponse()
rospy.wait_for_message('solarGP', LocalGP)
while not rospy.is_shutdown():
# Get teleoperation state
teleop_state = get_teleop()
# Check for teleop actions
if not self.on_teleop(teleop_state):
continue
# Get next goal pose
data = teleop_state.pose
xnext = np.array([
data.pose.position.x, data.pose.position.y,
data.pose.position.z
]).reshape(1, 3)
# If the distance to the next goal point is larger than desired threshold, clip
v = xnext - self.curX
norm_v = np.linalg.norm(v)
if norm_v > d:
u = v / norm_v
xnext = self.curX + d * u
# Predict output joint angles (sin/cos)
try:
Ypred, _ = self.model.prediction(xnext, Y_prev=Yexp)
Yexp = Ypred
except:
warnings.warn("warning during prediction")
pass
if np.isnan(Yexp).any():
warnings.warn("warning has prediction Nan")
continue
# Publish joint angles (radians)
Y = self.decode_ang(Yexp).astype(float)
self.pred_pub.publish(Y[0, :].tolist())
rate.sleep()