def initialize(rate, jspub, cmd, GPpub):
njit = rospy.get_param('~njit')
YStart = rospy.get_param('~YStart2')
# rospy.loginfo(YStart)
YStart = np.array(YStart).reshape(1, 2)
num_inducing = rospy.get_param('~num_inducing')
w_gen = rospy.get_param('~wgen')
d = rospy.get_param('~drift')
local = LocalModels(num_inducing, wgen=w_gen, drift=d, ndim=2)
YI = local.jitROS(njit, YStart)
print(YI)
XI = np.empty([0, 2])
for y in YI:
cmd.position = [y[0], y[1]]
jspub.publish(cmd)
data = rospy.wait_for_message('experience', Point)
XI = np.vstack((XI, np.array([data.x, data.y]).reshape(1, 2)))
rate.sleep()
local.initROS(XI, YI)
LocMsg = constructMsg(local)
GPpub.publish(LocMsg)
return local
def initGP(resp):
"ROS Settings"
rospy.init_node('initGP_node')
R = rospy.get_param('~train_pub_rate')
cmd = JointState()
cmd.name = ['joint1', 'joint2']
"Initialize Local Models"
njit = rospy.get_param('~njit')
YStart = rospy.get_param('~YStart')
YStart = np.array(np.deg2rad(YStart)).reshape(1,2)
num_inducing = rospy.get_param('~num_inducing')
w_gen = rospy.get_param('~wgen')
d = rospy.get_param('~drift')
local = LocalModels(num_inducing, wgen = w_gen, drift = d, ndim = 2)
YI = local.jitROS(njit,YStart)
XI = np.empty([0,2])
for y in YI:
cmd.position = [y[0], y[1]]
jspub.publish(cmd)
rate.sleep()
data = rospy.wait_for_message('experience',Point)
XI = np.vstack((XI,np.array([data.x,data.y]).reshape(1,2)))
local.initROS(XI,YI)
LocSrv = constructSrv(local)
return LocSrv
def deconstructMsg(LocMsg):
W = LocMsg.W
W = np.diag([1 / (W[0]**2), 1 / (W[1]**2)])
M = LocMsg.M
LocalData = []
Models = []
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, 2))
X_loc.append(np.array(L.ymean).reshape(1, 2))
X_loc.append(True)
LocalData.append(X_loc)
# kern_var = L.kern_var
# kern_lengthscale = L.kern_lengthscale
X = np.empty([0, 2])
Y = np.empty([0, 2])
Z = np.empty([0, 2])
Z_old = np.empty([0, 2])
mu_old = np.empty([0, 2])
Su_old = np.empty([0, len(L.Z_old)])
Kaa_old = np.empty([0, len(L.Z_old)])
kern = GPy.kern.RBF(2, 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, 2)))
Y = np.vstack((Y, np.array(y.array).reshape(1, 2)))
for z in L.Z:
Z = np.vstack((Z, np.array(z.array).reshape(1, 2)))
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, 2)))
mu_old = np.vstack((mu_old, np.array(mu.array).reshape(1, 2)))
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))))
# X = np.array(L.X).reshape(1,2)
# Y = np.array(L.Y).reshape(1,2)
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
return local
def __init__(self):
self.local = LocalModels()
# self.publisher =
# self.subscriber
self.count = 0
def train():
"ROS Settings"
rospy.init_node('train_node')
R = rospy.get_param('~train_pub_rate')
rate = rospy.Rate(R)
traintime = rospy.Publisher('traintime', Float64, queue_size=10)
jspub = rospy.Publisher('joint_states', JointState, queue_size=10)
GPpub = rospy.Publisher('localGP', LocalGP, queue_size=10)
cmd = JointState()
cmd.name = ['joint1', 'joint2']
"Initialize Local Models"
# local,Xtot,Ytot = initialize(rate,jspub,cmd, GPpub)
njit = rospy.get_param('~njit')
njit = 15
YStart = rospy.get_param('~YStart')
YStart = np.array(np.deg2rad(YStart)).reshape(1, 2)
num_inducing = rospy.get_param('~num_inducing')
w_gen = rospy.get_param('~wgen')
d = rospy.get_param('~drift')
local = LocalModels(num_inducing, wgen=w_gen, drift=d, ndim=2)
YI = local.jitROS(njit, YStart)
XI = np.empty([0, 2])
print(YI)
# print(np.rad2deg(YI))
twolink = RobotModels.nLink2D(Links=[1, 1])
Xtr = twolink.fkin(YI)
# print(twolink.fkin(YI))
j = 0
# while j < len(YI):
rospy.Rate(0.5).sleep()
for y in YI:
cmd.header.stamp = rospy.Time.now()
cmd.position = [y[0], y[1]]
# prediction.publish([y[0],y[1]])
# cmd.position = [YI[j][0],YI[j][1]]
jspub.publish(cmd)
# rate.sleep()
rospy.Rate(5).sleep()
print("get experience")
data = rospy.wait_for_message('experience', Point)
# rospy.Subscriber('experience',Point,callback)
XI = np.vstack((XI, np.array([data.x, data.y]).reshape(1, 2)))
# XI = np.vstack((XI,np.array([2,0]).reshape(1,2)))
# rospy.Rate(1).sleep()
# XI = np.vstack((XI,np.array([Xexp[0],Xexp[1]]).reshape(1,2)))
print(y)
print(data)
print(Xtr[j])
j += 1
# rospy.Rate(100).sleep()
local.initROS(XI, YI)
# print(local.Models)
LocMsg = constructMsg(local)
GPpub.publish(LocMsg)
Xtot = local.XI
Ytot = local.YI
i = 1
"Main Loop"
while not rospy.is_shutdown():
print("count: " + str(i))
"Get Yexp"
# print("get Yexp")
# data = rospy.wait_for_message('prediction',Arrays)
# Yexp = np.array(data.array).reshape(1,2)
data = rospy.wait_for_message('joint_states', JointState)
Yexp = np.array(data.position).reshape(1, 2)
print("Yexp: " + str(Yexp))
"Get Xexp"
print("get Xexp")
data = rospy.wait_for_message('experience', Point)
Xexp = np.array([data.x, data.y]).reshape(1, 2)
print("Xexp: " + str(Xexp))
t1 = time.time()
"Training"
if Xtot.shape[0] > 50:
Xtot = np.delete(Xtot, 0, 0)
Ytot = np.delete(Ytot, 0, 0)
Xtot = np.vstack((Xtot, Xexp))
Ytot = np.vstack((Ytot, Yexp))
if i % local.drift == 0:
mdrift = local.doOSGPR(Xtot[-local.drift:],
Ytot[-local.drift:],
local.mdrift,
local.num_inducing,
use_old_Z=False,
driftZ=False)
W = np.diag([
1 / (mdrift.kern.lengthscale[0]**2),
1 / (mdrift.kern.lengthscale[1]**2)
])
local.W = W
local.mdrift = mdrift
local.partition(Xexp.reshape(len(Xexp), 2), Yexp.reshape(len(Yexp), 2))
try:
local.train()
except:
pass
LocMsg = constructMsg(local)
GPpub.publish(LocMsg)
t2 = time.time()
traintime.publish(t2 - t1)
"Next"
i += 1
rate.sleep()
def sender():
"ROS Settings"
jspub = rospy.Publisher('joint_states', JointState, queue_size=10)
rospy.init_node('controller_node')
count = rospy.Publisher('count', Int32, queue_size=10)
numModels = rospy.Publisher('numModels', Int32, queue_size=10)
looptime = rospy.Publisher('looptime', Float32, queue_size=10)
# listener = tf.TransformListener()
# R = rospy.get_param('~controller_pub_rate')
rate = rospy.Rate(100)
# T = rospy.get_param('~period')
# r = 0.2
# [X_test,N] = getCircle()
# [X_test,N] = getRectangle()
# [X_test, N] = getSpiral()
cmd = JointState()
cmd.name = ['joint1', 'joint2']
# cmd.name = ['joint1', 'joint2','joint3','joint4']
# cmd.name = ['joint1', 'joint2','joint3','joint4','joint5','joint6']
"Robot Settings"
twolink = RobotModels.nLink2D(Links=[1, 1])
# twolink = RobotModels.nLink2D(Links = [1,1,1,1,1,1])
#twolink = RobotModels.nLink2D(Links = [0.5,0.5,0.5,0.5,0.5,0.5])
"Test Settings"
data = teleop_client(0)
XStart = np.array([data.x, data.y]).reshape(1, 2)
# YStart = np.array(np.deg2rad([-90,90,30,0,30,60])).reshape(1,6)
YStart = twolink.inkin(XStart)
# YStart = np.array(np.deg2rad([0,90])).reshape(1,2)
# YStart = np.array(np.deg2rad([0,90,0,90])).reshape(1,twolink.num_links)
#YStart = np.array(np.deg2rad([90,90,30])).reshape(1,3)
# XStart = twolink.fkin(YStart)
n = 250
r = 2
# x0 = 0.25
# y0 = 1
x0 = XStart[0][0] - r
y0 = XStart[0][1]
# Test = TestTrajectory(Trajectory.Circle2D(n,r,x0,y0,arclength = 2*np.pi), twolink)
# Test = TestTrajectory(Trajectory.nPoly2D(n,4,r,x0,y0), twolink)
#Test = TestTrajectory(Trajectory.Rect2D(n, width = 1, height = 1), twolink)
Test = TestTrajectory(Trajectory.Star2D(n, 5, r, x0, y0), twolink)
# Test = TestTrajectory(Trajectory.Spiral2D(n,0.6,0.1,x0,y0,6*np.pi), twolink)
X_test = Test.xtest
#Y_test = YStart
# Y_test = Test.ytest
"Algorithm Settings"
njit = 10
num_inducing = 25
drift = 1
" Initialize Local Models"
local = LocalModels(num_inducing,
wgen=0.95,
ndim=Test.robot.num_links,
robot=Test.robot)
# local.initialize(njit,Y_test[0], Test.robot)
local.initialize(njit, YStart, Test.robot)
Xtot = local.XI
Ytot = local.YI
Yexp = YStart
#Yexp = Ytot[-1]
i = 1
"Main Loop"
while not rospy.is_shutdown():
t1 = time.time()
cmd.header.stamp = rospy.Time.now()
# t = rospy.get_time()
count.publish(i)
numModels.publish(local.M)
# if i > len(X_test)-1:
# i = 0
# cmd.name = ['baseHinge', 'interArm']
"Grab Teleoperator command"
data = teleop_client(i)
xnext = np.array([data.x, data.y])
print(i)
"Prediction"
Ypred, _ = local.prediction(xnext.reshape(1, 2), Y_prev=Ytot[-1:])
# Ypred = local.prediction(X_test[i].reshape(1,2),Y_prev = Ytot[-1:])
Ypost = np.vstack((Ytot[-1:], Ypred))
Ypost = np.unwrap(Ypost, axis=0)
Yexp = Ypost[-1].reshape(1, Test.robot.num_links)
"Actuation"
# cmd.position = [Ypost[0][0], Ypost[0][1]]
cmd.position = [Yexp[0][0], Yexp[0][1]]
# cmd.position = Yexp.tolist()
# cmd.position= [Yexp]
# cmd.position = [Ypost[0][0], Ypost[0][1],Ypost[0][2], Ypost[0][3],Ypost[0][4], Ypost[0][5]]
jspub.publish(cmd)
"Sensing"
# Xexp = Test.robot.fkin(Yexp)
data = rospy.wait_for_message('experience', Point)
Xexp = np.array([data.x, data.y]).reshape(1, 2)
"Training"
if Xtot.shape[0] > 50:
Xtot = np.delete(Xtot, 0, 0)
Ytot = np.delete(Ytot, 0, 0)
Xtot = np.vstack((Xtot, Xexp))
Ytot = np.vstack((Ytot, Yexp))
if i % drift == 0:
mdrift = local.doOSGPR(Xtot[i + njit - drift:i + njit],
Ytot[i + njit - drift:i + njit],
local.mdrift,
num_inducing,
use_old_Z=False,
driftZ=False)
# mdrift = local.doOSGPR(Xexp, Yexp, local.mdrift,num_inducing,use_old_Z=False, driftZ = False)
W = np.diag([
1 / (mdrift.kern.lengthscale[0]**2),
1 / (mdrift.kern.lengthscale[1]**2)
])
local.W = W
local.mdrift = mdrift
local.partition(Xexp.reshape(len(Xexp), 2),
Yexp.reshape(len(Yexp), Test.robot.num_links))
try:
local.train()
except:
pass
t3 = time.time()
looptime.publish(t3 - t1)
"Next"
i += 1
rate.sleep()