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

first try

how to sync ilc_calc and retime_srv like this?
parent 7eadc0c3
......@@ -84,7 +84,7 @@
<member id="tasks.tracking.controllers">
<clone id="ilc_controller" prototype="p_controller_ilc">
<member id="filename_in">/home/victor/Documents/ilc_Logs/lin_ff_2.txt</member>
<member id="filename_in">/tmp/ilc/lin_ff_2.txt</member>
<member id="data_source">
<reference reference_id="collections.target_to_tool.chains[0]"/>
......@@ -119,7 +119,7 @@
<!-- Controller must be declared before referencing -->
<reference reference_id="tasks.tracking.controllers.ilc_controller"/>
</member>
<member id="file_name">/home/victor/Documents/ilc_Logs/logger_0.csv</member>
<member id="file_name">/tmp/ilc/logger_0.csv</member>
</clone>
</member>
</clone>
......@@ -132,9 +132,9 @@
<member id="scripts">
<clone prototype="script_trigger_ilc">
<member id="service_name">ilc_service</member>
<member id="file_in">/home/victor/Documents/ilc_Logs/logger_0.csv</member>
<member id="file_out">/home/victor/Documents/ilc_Logs/lin_ff_2.txt</member>
<member id="file_traj">/home/victor/Documents/ilc_Logs/trajectory.txt</member>
<member id="file_in">/tmp/ilc/logger_0.csv</member>
<member id="file_out">/tmp/ilc/lin_ff_2.txt</member>
<member id="file_traj">/tmp/ilc/trajectory.txt</member>
<member id="traj_type">cos</member>
<member id="L">0.6</member>
<member id="cutoff_freq">0.5</member>
......
......@@ -12,13 +12,16 @@ import datetime
class Ilc_serviceproxy:
def __init__(self):
self.decay_time = None
self.last_decay_time = None
self.initialized = False
self.data_diff = None
def _initialize(self):
d = {"decay_time": None,
"last_decay_time" : None,
"initialized": False,
"data_diff" : None,
"reshape": False}
return d
self.reshape = False
def __init__(self):
self.data = {}
self.T = rospy.get_param('~T', 8) # sampling time in ms
self.fs = 1.0/(self.T * 0.001) # sampling frequency
......@@ -93,46 +96,52 @@ class Ilc_serviceproxy:
return TriggerILCResponse(success=True, message=message)
except Exception as e:
print("except")
message = str(e)
print("Exception: '{}'".format(message))
return TriggerILCResponse(success=False, message=message)
def ilc_calc(self, req, data_org, header):
# Get name of script
# Create entry if not existing
if (req.id not in self.data):
self.data[req.id] = self._initialize()
print("Init new set@'{}'".format(req.id))
### Resample
data_rs = ilc_lib.resample(data_org, self.T)
### Trajectory generation
if self.decay_time is None:
self.decay_time = data_rs[-1, 0]
trajectory = ilc_lib.traj_handle(data_rs, "cos", decay_time=self.decay_time)
if self.data[req.id]["decay_time"] is None:
self.data[req.id]["decay_time"] = data_rs[-1, 0]
trajectory = ilc_lib.traj_handle(data_rs, "cos", decay_time=self.data[req.id]["decay_time"])
### Relative Error: data-trajectory
self.data_diff = ilc_lib.combine_data(data_rs, trajectory, "diff")
self.data[req.id]["data_diff"] = ilc_lib.combine_data(data_rs, trajectory, "diff")
### Shift Error by timestep
if req.timeshift == None:
timeshift = 0
else:
timeshift = req.timeshift
data_ff_shift = ilc_lib.ilc_shift(self.data_diff, timeshift, self.T)
data_ff_shift = ilc_lib.ilc_shift(self.data[req.id]["data_diff"], timeshift, self.T)
### ILC Gain
data_ff = ilc_lib.ilc_calculations(data_ff_shift, req.L)
### Combine with last iteration, if existing
path_out = self.decode_filename(req.file_out)
if self.initialized and os.path.exists(path_out):
if self.data[req.id]["initialized"] and os.path.exists(path_out):
last_ff, _ = ilc_lib.read(path_out)
# Reshape last feedforward if wanted
if self.reshape:
last_ff = self.reshape_ff(last_ff, self.last_decay_time, self.decay_time)
self.reshape = False
if self.data[req.id]["reshape"]:
last_ff = self.reshape_ff(last_ff, self.data[req.id]["last_decay_time"], self.data[req.id]["decay_time)"])
self.data[req.id]["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: # initialized == False -> cannot combine with previous csv
data_ilc = data_ff
self.initialized = True
self.data[req.id]["initialized"] = True
#Filter data
data_ilc_q = ilc_lib.butter_lowpass_filter(data_ilc, req.cutoff_freq, self.fs, 5)
......@@ -146,7 +155,7 @@ class Ilc_serviceproxy:
ilc_lib.external_data_store(self.folder + "/read_error", "read_error", data_org, header)
ilc_lib.external_data_store(self.folder + "/resampled_error", "resampled_error", data_rs ,header)
ilc_lib.external_data_store(self.folder + "/trajectory", "trajectory", trajectory, header)
ilc_lib.external_data_store(self.folder + "/ff_data_shift", "ff_data_shift", data_ff_shift, header)
ilc_lib.external_data_store(self.folder + "/ff_data_shift", "ff_data_shift", data_ff_shift, header)
ilc_lib.external_data_store(self.folder + "/ff_data", "ff_data", data_ff, header)
ilc_lib.external_data_store(self.folder + "/ilc_data", "ilc_data", data_ilc, header)
ilc_lib.external_data_store(self.folder + "/filtered_ilc_data", "filtered_ilc_data", data_ilc_q, header)
......@@ -155,12 +164,12 @@ class Ilc_serviceproxy:
def callback_ilc_retime(self, req):
self.last_decay_time = self.decay_time
self.decay_time = req.setpoint_duration_ms
self.initialized = req.reuse_ff
self.data[req.id]["last_decay_time"] = self.data[req.id]["decay_time"]
self.data[req.id]["decay_time"] = req.setpoint_duration_ms
self.data[req.id]["initialized"] = req.reuse_ff
if req.reuse_scheme != 0:
self.reshape = True
self.data[req.id]["reshape"] = True
print("Retiming Service received:\n{}".format(req))
return RetimeILCResponse(success=True, message="done")
......@@ -173,18 +182,18 @@ class Ilc_serviceproxy:
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):
print("Reducing trajectory time to {}".format(self.decay_time))
self.last_decay_time = self.decay_time
self.decay_time = self.decay_time * req.retime_factor
self.initialized = req.reuse_ff
if (np.max(ilc_lib.error_convergence(self.data[req.id]["data_diff"])) < req.convergence_threshold):
print("Reducing trajectory time to {}}".format(self.data[req.id]["decay_time"]))
self.data[req.id]["last_decay_time"] = self.data[req.id]["decay_time"]
self.data[req.id]["decay_time"] *= req.retime_factor
self.data[req.id]["initialized"] = req.reuse_ff
if req.reuse_scheme != 0:
self.reshape = True
self.data[req.id]["reshape"] = True
return RetimeIfILCResponse(success=True, message="reshaped")
else:
print("Convergence: {} > {}".format(np.max(ilc_lib.error_convergence(self.data_diff)), req.convergence_threshold))
print("Convergence: {} > {}".format(np.max(ilc_lib.error_convergence(self.data[req.id]["data_diff"])), req.convergence_threshold))
return RetimeIfILCResponse(success=True, message="skipped")
except Exception as e:
......
......@@ -12,6 +12,7 @@ public:
: ScriptRosService<ilc::TriggerILC, ilc::TriggerILC::Request, ilc::TriggerILC::Response>(
name)
{
name_ = name;
}
bool init(Dict& params) override {
......@@ -22,6 +23,7 @@ public:
extract(params["L"], Lgain_);
extract(params["cutoff_freq"], cutoff_);
service_.request.id = name_;
service_.request.file_in = fin_;
service_.request.file_out = fout_;
service_.request.file_traj = trajout_;
......@@ -42,6 +44,7 @@ public:
}
private:
string name_;
string fin_;
string fout_;
string trajout_;
......@@ -58,6 +61,7 @@ public:
: ScriptRosService<ilc::TriggerILCMeas, ilc::TriggerILCMeas::Request, ilc::TriggerILCMeas::Response>(
name)
{
name_ = name;
}
bool init(Dict& params) override {
......@@ -69,6 +73,7 @@ public:
extract(params["L"], Lgain_);
extract(params["cutoff_freq"], cutoff_);
service_.request.id = name_;
service_.request.file_in = fin_;
service_.request.setpoint = setpoint_;
service_.request.file_out = fout_;
......@@ -90,6 +95,7 @@ public:
}
private:
string name_;
string fin_;
vector<double> setpoint_;
string fout_;
......@@ -107,6 +113,7 @@ public:
: ScriptRosService<ilc::RetimeILC, ilc::RetimeILC::Request, ilc::RetimeILC::Response>(
name)
{
name_ = name;
}
bool init(Dict& params) override {
......@@ -114,6 +121,7 @@ public:
extract(params["reuse_ff"], reuse_ff_);
extract(params["reuse_scheme"], reuse_scheme_);
service_.request.id = name_;
service_.request.setpoint_duration_ms = setpoint_duration_ms_;
service_.request.reuse_ff = reuse_ff_;
service_.request.reuse_scheme = reuse_scheme_;
......@@ -130,6 +138,7 @@ public:
}
private:
string name_;
int setpoint_duration_ms_;
bool reuse_ff_;
int reuse_scheme_;
......@@ -143,6 +152,7 @@ public:
: ScriptRosService<ilc::RetimeIfILC, ilc::RetimeIfILC::Request, ilc::RetimeIfILC::Response>(
name)
{
name_ = name;
}
bool init(Dict& params) override {
......@@ -151,6 +161,7 @@ public:
extract(params["reuse_ff"], reuse_ff_);
extract(params["reuse_scheme"], reuse_scheme_);
service_.request.id = name_;
service_.request.retime_factor = retime_factor_;
service_.request.convergence_threshold = convergence_threshold_;
service_.request.reuse_ff = reuse_ff_;
......@@ -168,6 +179,7 @@ public:
}
private:
string name_;
double retime_factor_;
double convergence_threshold_;
bool reuse_ff_;
......
string id
uint32 setpoint_duration_ms
bool reuse_ff
uint8 reuse_scheme
......
string id
float32 retime_factor
float32 convergence_threshold
bool reuse_ff
......
string id
string file_in
string file_out
string file_traj
......
string id
string file_in
float64[] setpoint
string file_out
......
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