Commit db4f4adb authored by Lorenz Halt's avatar Lorenz Halt 🔀
Browse files

reshaping function

parent 5cb3efd5
from __future__ import division
import numpy as np
import os
import math
......@@ -113,6 +114,11 @@ def pad_data(a, b, val=0):
return a, b
def reshape(data, last_duration, new_duration):
y = copy.deepcopy(data)
y[:,0] = (new_duration/last_duration) * y[:,0]
return y
def e_traj(t,t_end):
if t<t_end:
s=1.2/200 *1000/t_end
......
......@@ -12,7 +12,10 @@ class Ilc_serviceproxy:
def __init__(self):
self.decay_time = None
self.initialize = True
self.last_decay_time = None
self.initialized = False
self.reshape = False
self.T = rospy.get_param('~T', 8) # sampling time in ms
self.fs = 1.0/(self.T * 0.001) # sampling frequency
......@@ -27,10 +30,8 @@ class Ilc_serviceproxy:
### Read Error data
data_org, header = ilc_lib.read(req.file_in)
#ilc_lib.external_data_store("/tmp", "filtered_ilc_data.xxl", data_org, header)
### Resample
data_rs = ilc_lib.resample(data_org, 8)
data_rs = ilc_lib.resample(data_org, self.T)
### Trajectory generation
if self.decay_time is None:
......@@ -44,13 +45,19 @@ class Ilc_serviceproxy:
data_ff = ilc_lib.ilc_calculations(data_diff, req.L)
### Combine with last iteration, if existing
if not self.initialize and os.path.exists(req.file_out):
if self.initialized and os.path.exists(req.file_out):
last_ff, _ = ilc_lib.read(req.file_out)
# Reshape last feedforward if wanted
if self.reshape:
last_ff = self.reshape_ff(self.last_decay_time, last_ff, self.decay_time)
self.reshape = False
data_ff, last_ff = ilc_lib.pad_data(data_ff, last_ff)
data_ilc = ilc_lib.combine_data(data_ff, last_ff, "sum")
else:
else: # initialized == False -> cannot combine with previous csv
data_ilc = data_ff
self.initialize = False
self.initialized = True
#Filter data
data_ilc_q = ilc_lib.butter_lowpass_filter(data_ilc, req.cutoff_freq, self.fs, 5)
......@@ -74,10 +81,21 @@ class Ilc_serviceproxy:
return TriggerILCResponse(success=False, message=message)
def callback_ilc_retime(self, req):
print(req)
#self.decay_time = req.setpoint_duration_ms
self.last_decay_time = self.decay_time
self.decay_time = req.setpoint_duration_ms
self.initialized = req.reuse_ff
if req.retime_scheme != 0:
self.reshape = True
print("Retiming Service received:\n{}".format(req))
return RetimeILCResponse(success=True, message="done")
def reshape_ff(self, last_ff, last_decay_time, new_decay_time):
y = ilc_lib.reshape(last_ff, last_decay_time, new_decay_time)
y = ilc_lib.resample(y, self.T)
return y
if __name__ == '__main__':
rospy.init_node('ilc_proxy')
......
%% Cell type:code id: tags:
``` python
import numpy as np
import ilc_lib
import os
from ilc_lib import cos_traj,e_traj
```
%% Cell type:code id: tags:
``` python
import matplotlib
import numpy as np
import matplotlib.pyplot as plt
%matplotlib inline
```
%% Cell type:code id: tags:
``` python
## Parameter Definitions ##
ws_path = "/home/ipa325/logs"
error_path = ws_path + "/logger_0.csv"
ff_path = ws_path + "/lin_ff_2.txt"
traj_path = ws_path + "/server/trajectory.txt"
out_path = ws_path + "/lin_ff_out.txt"
traj_type = "cos"
L = 2.0
cutoff_freq = 0.5
T = 8 #ms
```
%% Cell type:code id: tags:
``` python
## Test Reading of csv error-file ##
data_org, header = ilc_lib.read(error_path)
#remove first row with negative tick
#data = data[1:]
#plt.plot(data[:,0], data[:,1])
#plt.plot(data[:,0], data[:,2])
plt.plot(data_org[:,0], data_org[:,3], '.-')
```
%% Cell type:code id: tags:
``` python
## Test resampling error-file to T-th timesteps
data = ilc_lib.resample(data_org, T)
plt.plot(data[:,0], data[:,3], '-o')
plt.plot(data_org[:,0], data_org[:,3], 'x')
```
%% Cell type:code id: tags:
``` python
## Test setpoint-trajectory generation ##
trajectory = ilc_lib.traj_handle(data, "cos", decay_time=None)
#print(trajectory)
#plt.plot(trajectory[:,0], trajectory[:,1])
#plt.plot(trajectory[:,0], trajectory[:,2])
plt.plot(trajectory[:,0], trajectory[:,3], '-o')
```
%% Cell type:code id: tags:
``` python
## Test time series combination functions ##
plt.plot(data[:,0], data[:,3], label="error")
plt.plot(trajectory[:,0], trajectory[:,3], label="trajectory")
plt.legend()
plt.show()
## Test difference (error to desired trajectory) ##
data_diff = ilc_lib.combine_data(data, trajectory, "diff")
plt.plot(data_diff[:,0], data_diff[:,3], label='diff')
## Test addition (update ilc feedforward with last feedfoward) ##
data_sum = ilc_lib.combine_data(data, trajectory, "sum")
plt.plot(data_sum[:,0], data_sum[:,3], label='sum')
plt.legend()
```
%% Cell type:code id: tags:
``` python
## Test multiplication (ilc calculation) with factor and vector ##
test = data[:,0:2]
test[:,1] = np.arange(0, data[-1,0]+1, T) / (data[-1,0]+1) * L # linear vanishing factor vector
#plt.plot(test[:,0], test[:,1])
#plt.show()
data_ff = ilc_lib.ilc_calculations(data_diff, L)
data_ff_vec = ilc_lib.ilc_calculations(data_diff[:,0:2], test)
plt.plot(data_diff[:,0], data_diff[:,1], label='diff')
plt.plot(data_ff[:,0], data_ff[:,1], label="np.mult(L)")
plt.plot(data_ff_vec[:,0], data_ff_vec[:,1], label="np.mult(vec)")
plt.legend()
```
%% Cell type:code id: tags:
``` python
## Test updateing ilc feedforward (sum) ##
old_ff = ilc_lib.ilc_calculations(data_diff, 5)
ilc_data = ilc_lib.combine_data(data_ff, old_ff, "sum")
plt.plot(old_ff[:,0], old_ff[:,3], label="last_ff")
plt.plot(data_ff[:,0], data_ff[:,3], label="data_ff")
plt.plot(ilc_data[:,0], ilc_data[:,3], label="addition")
plt.legend()
```
%% Cell type:code id: tags:
``` python
## Test updating including padding if file is too short ##
old_ff, _ = ilc_lib.read(ff_path)
try:
ilc_data = ilc_lib.combine_data(data_ff, old_ff, "sum")
except:
data_ff, old_ff = ilc_lib.pad_data(data_ff, old_ff)
ilc_data = ilc_lib.combine_data(data_ff, old_ff, "sum")
plt.plot(old_ff[:,0], old_ff[:,3], label="last_ff")
plt.plot(data_ff[:,0], data_ff[:,3], label="data_ff")
plt.plot(ilc_data[:,0], ilc_data[:,3], label="addition")
plt.legend()
```
%% Cell type:code id: tags:
``` python
## Test reshaping of last feedforward file ##
plt.plot(data_ff[:,0], data_ff[:,3], label="data_ff")
reshaped_ff = ilc_lib.reshape(data_ff, data_ff[-1,0], 3000)
plt.plot(reshaped_ff[:,0], reshaped_ff[:,3], label="reshaped_ff")
#print(reshaped_ff[1:10, :])
reshaped_ff = ilc_lib.resample(reshaped_ff, T)
plt.plot(reshaped_ff[:,0], reshaped_ff[:,3], '.', label="reshaped_resampled_ff")
#print(reshaped_ff[1:10, :])
```
%% Cell type:code id: tags:
``` python
## Test butterworth Q-Filter ##
plt.plot(ilc_data[:,0], ilc_data[:,3], label="ilc_data")
ilc_data_t = ilc_lib.butter_lowpass_filter(ilc_data, cutoff_freq, 1/(T*0.001), 5)
plt.plot(ilc_data_t[:,0], ilc_data_t[:,3], label="ilc_data_t")
plt.legend()
```
%% Cell type:code id: tags:
``` python
## Test writing of result csv ##
ilc_lib.write(out_path, ilc_data_t, header)
data_new, header = ilc_lib.read(out_path)
plt.plot(ilc_data_t[:,0], ilc_data_t[:,3], 'x')
plt.plot(data_new[:,0], data_new[:,3], '-')
```
%% Cell type:code id: tags:
``` python
## Test debug output storage ##
ilc_lib.external_data_store(ws_path + "/read_error", "read_error.csv", data, header)
```
%% Cell type:code id: tags:
``` python
```
%% Cell type:code id: tags:
``` python
```
%% Cell type:code id: tags:
``` python
```
%% Cell type:code id: tags:
``` python
```
%% Cell type:code id: tags:
``` python
```
%% Cell type:code id: tags:
``` python
```
%% Cell type:code id: tags:
``` python
```
%% Cell type:code id: tags:
``` python
```
%% Cell type:code id: tags:
``` python
```
%% Cell type:code id: tags:
``` python
```
%% Cell type:code id: tags:
``` python
```
%% Cell type:code id: tags:
``` python
```
%% Cell type:code id: tags:
``` python
```
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment