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

Retiming loop


fix
parent db4f4adb
......@@ -18,6 +18,7 @@ add_service_files(
FILES
TriggerILC.srv
RetimeILC.srv
RetimeIfILC.srv
)
## Uncomment this if the package has a setup.py. This macro ensures
......
......@@ -121,6 +121,15 @@
<member id="L">0.3</member>
<member id="cutoff_freq">0.5</member>
</clone>
<clone prototype="script_retime_if_ilc">
<member id="service_name">ilc_retime_if</member>
<member id="on_start">false</member>
<member id="retime_factor">0.9</member>
<member id="convergence_threshold">0.2</member>
<member id="reuse_ff">True</member>
<member id="reuse_scheme">1</member>
</clone>
</member>
<member id="transitions">
......
......@@ -81,6 +81,37 @@
</type>
</data>
</type>
<type id="script_retime_if_ilc" prototype="script_service_caller">
<meta>
<member id="description">Commands a ilc_proxy to change the duration of the setpoint trajectory to the given length.</member>
<member id="implementation">
<clone prototype="orocos">
<member id="package">ilc_scripts</member>
<member id="component">ScriptRetimeIfILC</member>
</clone>
</member>
</meta>
<data>
<type id="retime_factor" prototype="float_parameter">
<member id="description">Wanted duration reshaping to duration * [0..1]</member>
</type>
<type id="convergence_threshold" prototype="float_parameter">
<member id="description">Apply reshaping if convergence is below this parameter</member>
</type>
<type id="reuse_ff" prototype="bool_parameter">
<member id="description">Reshape and reuse previous learned feedforwards</member>
<data>True</data>
</type>
<type id="reuse_scheme" prototype="int_parameter">
<member id="description">Selects different reshaping schemes. 0: No reshaping</member>
<data>0</data>
</type>
</data>
</type>
</models>
</pitasc>
......@@ -119,6 +119,12 @@ def reshape(data, last_duration, new_duration):
y[:,0] = (new_duration/last_duration) * y[:,0]
return y
def error_convergence(data):
y = np.linalg.norm(data[:,1:], ord=2, axis=0)
return y
def e_traj(t,t_end):
if t<t_end:
s=1.2/200 *1000/t_end
......
#!/usr/bin/env python
from __future__ import print_function
from ilc.srv import TriggerILC, TriggerILCResponse, RetimeILC, RetimeILCResponse
from ilc.srv import TriggerILC, TriggerILCResponse, RetimeILC, RetimeILCResponse, RetimeIfILC, RetimeIfILCResponse
import rospy
import numpy as np
import ilc_lib
......@@ -14,6 +14,7 @@ class Ilc_serviceproxy:
self.decay_time = None
self.last_decay_time = None
self.initialized = False
self.data_diff = None
self.reshape = False
......@@ -22,6 +23,7 @@ class Ilc_serviceproxy:
rospy.Service('ilc_service', TriggerILC, self.callback_ilc_calc)
rospy.Service('ilc_retime', RetimeILC, self.callback_ilc_retime)
rospy.Service('ilc_retime_if', RetimeIfILC, self.callback_ilc_retime_if)
self.folder = rospy.get_param('~folder', None)
......@@ -39,10 +41,10 @@ class Ilc_serviceproxy:
trajectory = ilc_lib.traj_handle(data_rs, "cos", decay_time=self.decay_time)
### Relative Error: data-trajectory
data_diff = ilc_lib.combine_data(data_rs, trajectory, "diff")
self.data_diff = ilc_lib.combine_data(data_rs, trajectory, "diff")
### ILC Gain
data_ff = ilc_lib.ilc_calculations(data_diff, req.L)
data_ff = ilc_lib.ilc_calculations(self.data_diff, req.L)
### Combine with last iteration, if existing
if self.initialized and os.path.exists(req.file_out):
......@@ -96,6 +98,26 @@ class Ilc_serviceproxy:
y = ilc_lib.resample(y, self.T)
return y
def callback_ilc_retime_if(self, req):
#print("Retiming Service received:\n{}".format(req))
try:
if (np.max(ilc_lib.error_convergence(self.data_diff)) < req.convergence_threshold):
self.last_decay_time = self.decay_time
self.decay_time = self.decay_time * req.retime_factor
self.initialized = req.reuse_ff
if req.retime_scheme != 0:
self.reshape = True
return RetimeIfILCResponse(success=True, message="reshaped")
else:
print("Convergence: {} > {}".format(np.max(ilc_lib.error_convergence(self.data_diff)), req.convergence_threshold))
return RetimeIfILCResponse(success=True, message="skipped")
except Exception as e:
return RetimeIfILCResponse(success=False, message=str(e))
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 ##
## Test convergence ##
conv = ilc_lib.error_convergence(data_diff)
print(conv)
print(np.max(conv))
```
%% 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
```
......
......@@ -2,6 +2,7 @@
#include <ilc/TriggerILC.h>
#include <ilc/RetimeILC.h>
#include <ilc/RetimeIfILC.h>
class ScriptTriggerILC : public ScriptRosService<ilc::TriggerILC, ilc::TriggerILC::Request, ilc::TriggerILC::Response>
{
......@@ -84,5 +85,44 @@ private:
int reuse_scheme_;
};
class ScriptRetimeIfILC : public ScriptRosService<ilc::RetimeIfILC, ilc::RetimeIfILC::Request, ilc::RetimeIfILC::Response>
{
public:
ScriptRetimeIfILC(const string& name)
: ScriptRosService<ilc::RetimeIfILC, ilc::RetimeIfILC::Request, ilc::RetimeIfILC::Response>(
name)
{
}
bool init(Dict& params) override {
extract(params["retime_factor"], retime_factor_);
extract(params["convergence_threshold"], convergence_threshold_);
extract(params["reuse_ff"], reuse_ff_);
extract(params["reuse_scheme"], reuse_scheme_);
service_.request.retime_factor = retime_factor_;
service_.request.convergence_threshold = convergence_threshold_;
service_.request.reuse_ff = reuse_ff_;
service_.request.reuse_scheme = reuse_scheme_;
return ScriptRosService::init(params);
}
bool call() override {
bool result = ScriptRosService<ilc::RetimeIfILC, ilc::RetimeIfILC::Request, ilc::RetimeIfILC::Response>::call();
if (!result) {
pi_error("Message: {}", service_.response.message);
}
return service_.response.success;
}
private:
double retime_factor_;
double convergence_threshold_;
bool reuse_ff_;
int reuse_scheme_;
};
RUNTIME_COMPONENT(ScriptTriggerILC)
RUNTIME_COMPONENT(ScriptRetimeILC)
RUNTIME_COMPONENT(ScriptRetimeIfILC)
float32 retime_factor
float32 convergence_threshold
bool reuse_ff
uint8 reuse_scheme
----------------------
bool success
string message
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