def __init__(self, rs, sizeOfTarget, saveTraj, thetaFile):
'''
Initializes the parameters used to run the functions below
Inputs:
-arm, armModel, class object
-rs, readSetup, class object
-sizeOfTarget, size of the target, float
-Ukf, unscented kalman filter, class object
-saveTraj, Boolean: true = Data are saved, false = data are not saved
'''
self.arm = Arm()
self.arm.setDT(rs.dt)
self.controller = initRBFNController(rs,thetaFile)
#load the controller, i.e. the vector of parameters theta
theta = self.controller.loadTheta(thetaFile+".theta")
#put theta to a one dimension numpy array, ie row vector form
#theta = matrixToVector(theta)
self.rs = rs
self.sizeOfTarget = sizeOfTarget
#6 is the dimension of the state for the filter, 4 is the dimension of the observation for the filter, 25 is the delay used
self.stateEstimator = StateEstimator(rs.inputDim,rs.outputDim, rs.delayUKF, self.arm)
self.saveTraj = saveTraj
def compavg(clients):
signal.signal(signal.SIGINT, signal.SIG_DFL)
y = np.zeros(shape=(6,8)) # observation matrix
m = 8
n = 8
dg = DataGenerator(n=n, m=m);
se = StateEstimator(dg.EstimationMatrix)
while True:
global globcount
totals = [0]*8
averages = [0]*8 # [avg1, avg2, avg3,..., avg8]
for c in clients:
line = c.getLine().strip().split(",")
if len(line) != 8:
line = [0.0, time.time(), 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
for i in range(len(line)):
if i == 1:
continue
if i > 1:
#print line[i]
totals[i-1] += float(line[i])
else:
totals[0] += float(line[0])
averages = map(lambda x: x / len(clients), totals)
#with comp_state_est_cv:
with cv:
y[globcount,:]=averages
globcount += 1
if globcount == 6:
times = y[:,1].reshape(-1, 1)
count = 0
timeno = 0
observation = sp.zeros((48, 3))
for t, row in zip(times, y):
for id, val in enumerate(row):
if timeno == 1:
timeno += 1
continue
observation[count, :] = sp.array([id, t[0] , val ] ).reshape(1, -1)
count += 1
timeno += 1
xhat = sp.zeros( (6, 8) )
T = 6
stepsize = 1
for t in range( 0, T + 1 - stepsize, stepsize ):
newstate,t0 = se.estimate( observation[t*m:(t+stepsize)*m,:] )
xhat[t:(t+stepsize),:] = se.interpolate(newstate,t0,times[t:(t+stepsize),:])
global state
state = xhat.reshape(-1).tolist()
globcount=0
cv.notifyAll()
print "interrupted"
def __init__(self,
rs,
sizeOfTarget,
saveTraj,
thetaFile=None,
estim="Inv"):
'''
Initializes the parameters used to run the functions below
Inputs:
-arm, armModel, class object
-rs, readSetup, class object
-sizeOfTarget, size of the target, float
-Ukf, unscented kalman filter, class object
-saveTraj, Boolean: true = Data are saved, false = data are not saved
'''
self.arm = ArmType[rs.arm]()
self.arm.setDT(rs.dt)
if (not rs.det and rs.noise != None):
self.arm.setNoise(rs.noise)
self.controller = initController(rs, thetaFile)
if (rs.costClass == None):
self.trajCost = CostCMAES(rs)
else:
self.trajCost = rs.costClass(rs)
self.costU12 = 0
#put theta to a one dimension numpy array, ie row vector form
#theta = matrixToVector(theta)
self.rs = rs
self.sizeOfTarget = sizeOfTarget
#6 is the dimension of the state for the filter, 4 is the dimension of the observation for the filter, 25 is the delay used
if estim == "Inv":
self.stateEstimator = StateEstimator(rs.inputDim, rs.outputDim,
rs.delayUKF, self.arm)
elif estim == "Reg":
self.stateEstimator = StateEstimatorRegression(
rs.inputDim, rs.outputDim, rs.delayUKF, self.arm)
elif estim == "Hyb":
self.stateEstimator = StateEstimatorHyb(rs.inputDim, rs.outputDim,
rs.delayUKF, self.arm)
elif estim == "No":
self.stateEstimator = StateEstimatorNoFeedBack(
rs.inputDim, rs.outputDim, rs.delayUKF, self.arm)
else:
raise TypeError("This Estimator do not exist")
self.saveTraj = saveTraj
class TrajMaker:
def __init__(self, rs, sizeOfTarget, saveTraj, thetaFile):
'''
Initializes the parameters used to run the functions below
Inputs:
-arm, armModel, class object
-rs, readSetup, class object
-sizeOfTarget, size of the target, float
-Ukf, unscented kalman filter, class object
-saveTraj, Boolean: true = Data are saved, false = data are not saved
'''
self.arm = Arm()
self.arm.setDT(rs.dt)
self.controller = initRBFNController(rs,thetaFile)
#load the controller, i.e. the vector of parameters theta
theta = self.controller.loadTheta(thetaFile+".theta")
#put theta to a one dimension numpy array, ie row vector form
#theta = matrixToVector(theta)
self.rs = rs
self.sizeOfTarget = sizeOfTarget
#6 is the dimension of the state for the filter, 4 is the dimension of the observation for the filter, 25 is the delay used
self.stateEstimator = StateEstimator(rs.inputDim,rs.outputDim, rs.delayUKF, self.arm)
self.saveTraj = saveTraj
#Initializes variables used to save trajectory
def setTheta(self, theta):
self.controller.setTheta(theta)
def computeManipulabilityCost(self):
'''
Computes the manipulability cost on one step of the trajectory
Input: -cost: cost at time t, float
Output: -cost: cost at time t+1, float
'''
dotq, q = getDotQAndQFromStateVector(self.arm.getState())
manip = self.arm.directionalManipulability(q,self.cartTarget)
return 1-manip
def computeStateTransitionCost(self, U):
'''
Computes the cost on one step of the trajectory
Input: -cost: cost at time t, float
-U: muscular activation vector, numpy array (6,1)
-t: time, float
Output: -cost: cost at time t+1, float
'''
#compute the square of the norm of the muscular activation vector
norme = np.linalg.norm(U)
mvtCost = norme*norme
#compute the cost following the law of the model
#return np.exp(-t/self.rs.gammaCF)*(-self.rs.upsCF*mvtCost)
return -self.rs.upsCF*mvtCost
def computePerpendCost(self):
dotq, q = getDotQAndQFromStateVector(self.arm.getState())
J = self.arm.jacobian(q)
xi = np.dot(J,dotq)
xi = xi/np.linalg.norm(xi)
return 500-1000*xi[0]*xi[0]
def computeFinalReward(self, t, coordHand):
cost = self.computePerpendCost()
'''
Computes the cost on final step if the target is reached
Input: -cost: cost at the end of the trajectory, float
-t: time, float
Output: -cost: final cost if the target is reached
'''
#check if the target is reached and give the reward if yes
if coordHand[1] >= self.rs.YTarget:
#print "main X:", coordHand[0]
if coordHand[0] >= -self.sizeOfTarget/2 and coordHand[0] <= self.sizeOfTarget/2:
cost += np.exp(-t/self.rs.gammaCF)*self.rs.rhoCF
else:
cost += -500-500000*(coordHand[0]*coordHand[0])
else:
cost += -4000
return cost
def runTrajectory(self, x, y, foldername):
'''
Generates trajectory from the initial position (x, y)
Inputs: -x: abscissa of the initial position, float
-y: ordinate of the initial position, float
Output: -cost: the cost of the trajectory, float
'''
#computes the articular position q1, q2 from the initial coordinates (x, y)
q1, q2 = self.arm.mgi(x, y)
#creates the state vector [dotq1, dotq2, q1, q2]
q = createVector(q1,q2)
state = np.array([0., 0., q1, q2])
#print("start state --------------: ",state)
#computes the coordinates of the hand and the elbow from the position vector
coordElbow, coordHand = self.arm.mgdFull(q)
#assert(coordHand[0]==x and coordHand[1]==y), "Erreur de MGD" does not work because of rounding effects
#initializes parameters for the trajectory
i, t, cost = 0, 0, 0
self.stateEstimator.initStore(state)
self.arm.setState(state)
estimState = state
dataStore = []
qtarget1, qtarget2 = self.arm.mgi(self.rs.XTarget, self.rs.YTarget)
vectarget = [0.0, 0.0, qtarget1, qtarget2]
#loop to generate next position until the target is reached
while coordHand[1] < self.rs.YTarget and i < self.rs.numMaxIter:
stepStore = []
#computation of the next muscular activation vector using the controller theta
#print ("state :",self.arm.getState())
U = self.controller.computeOutput(estimState)
if det:
Unoisy = muscleFilter(U)
else:
Unoisy = getNoisyCommand(U,self.arm.musclesP.knoiseU)
Unoisy = muscleFilter(Unoisy)
#computation of the arm state
realNextState = self.arm.computeNextState(Unoisy, self.arm.getState())
#computation of the approximated state
tmpState = self.arm.getState()
if det:
estimNextState = realNextState
else:
U = muscleFilter(U)
estimNextState = self.stateEstimator.getEstimState(tmpState,U)
#print estimNextState
self.arm.setState(realNextState)
#computation of the cost
cost += self.computeStateTransitionCost(Unoisy)
'''
print "U =", U
print "Unoisy =", Unoisy
print "estimstate =", estimState
#print "theta =", self.controller.theta
if math.isnan(cost):
print "NAN : U =", U
print "NAN : Unoisy =", Unoisy
print "NAN : estimstate =", estimState
#print "NAN : theta =", self.controller.theta
sys.exit()
'''
#get dotq and q from the state vector
dotq, q = getDotQAndQFromStateVector(tmpState)
coordElbow, coordHand = self.arm.mgdFull(q)
#print ("dotq :",dotq)
#computation of the coordinates to check if the target is reach or not
#code to save data of the trajectory
#Note : these structures might be much improved
if self.saveTraj == True:
stepStore.append(vectarget)
stepStore.append(estimState)
stepStore.append(tmpState)
stepStore.append(Unoisy)
stepStore.append(np.array(U))
stepStore.append(estimNextState)
stepStore.append(realNextState)
stepStore.append([coordElbow[0], coordElbow[1]])
stepStore.append([coordHand[0], coordHand[1]])
#print ("before",stepStore)
tmpstore = np.array(stepStore).flatten()
row = [item for sub in tmpstore for item in sub]
#print ("store",row)
dataStore.append(row)
estimState = estimNextState
i += 1
t += self.rs.dt
cost += self.computeFinalReward(t,coordHand)
if self.saveTraj == True:
filename = findFilename(foldername+"Log/","traj",".log")
np.savetxt(filename,dataStore)
'''
if coordHand[0] >= -self.sizeOfTarget/2 and coordHand[0] <= self.sizeOfTarget/2 and coordHand[1] >= self.rs.YTarget and i<230:
foldername = pathDataFolder + "TrajRepository/"
name = findFilename(foldername,"Traj",".traj")
np.savetxt(name,dataStore)
'''
lastX = -1000 #used to ignore dispersion when the target line is not crossed
if coordHand[1] >= self.rs.YTarget:
lastX = coordHand[0]
#print "end of trajectory"
return cost, t, lastX
# args.PORT = 8000 # The port used on server
# Set up server
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.bind(('', args.PORT))
s.listen(5)
# s.settimeout(3.0)
state_estimation_server = StateEstimationServer(args.HOST, args.PORT,
pmu_list, state_list)
# Initialize data generator and state estimator
dg = DataGenerator(n=args.n,
m=args.m,
error_std=args.error_std,
randseed=args.randseed)
se = StateEstimator(dg.EstimationMatrix)
# Start uploading thread
last_timestamp = -1
work_queue = Queue()
uploader = Uploader(state_estimation_server, work_queue)
uploader.start()
# Start State Estimator Thread
se_thread = StateEstimatorThread(state_estimation_server, args.start_time,
args.time_period, work_queue)
se_thread.start()
''' Connect to GMS '''
gms_s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
gms_s.connect((args.GMS_HOST, args.GMS_PORT))
# s2.settimeout(3.0)