Commit 4d159967 authored by Lorenz Halt's avatar Lorenz Halt 🔀
Browse files

new measurement script and filename decoding

use literal '{rospkg ilc}' for path to ilc
use literal '{time %F} for current date (cf. std::sftime)
parent 96ed8141
...@@ -17,6 +17,7 @@ find_package(Eigen REQUIRED) ...@@ -17,6 +17,7 @@ find_package(Eigen REQUIRED)
add_service_files( add_service_files(
FILES FILES
TriggerILC.srv TriggerILC.srv
TriggerILCMeas.srv
RetimeILC.srv RetimeILC.srv
RetimeIfILC.srv RetimeIfILC.srv
) )
......
...@@ -54,8 +54,25 @@ ...@@ -54,8 +54,25 @@
</data> </data>
</type> </type>
<type id="script_trigger_ilc_meas" prototype="script_trigger_ilc">
<meta>
<member id="description">Triggers a ros script to generate a file including ilc feedforward (file_out) based on given measeurement loggings (file_in) and the setpoint</member>
<member id="implementation">
<clone prototype="orocos">
<member id="package">ilc_scripts</member>
<member id="component">ScriptTriggerILCMeas</member>
</clone>
</member>
</meta>
<data>
<type id="setpoint" prototype="list:float_parameter">
<member id="description">The constant setpoints for the respective measurement</member>
</type>
</data>
</type>
<type id="script_retime_ilc" prototype="script_service_caller"> <type id="script_retime_ilc" prototype="script_service_caller">
<meta> <meta>
<member id="description">Commands a ilc_proxy to change the duration of the setpoint trajectory to the given length.</member> <member id="description">Commands a ilc_proxy to change the duration of the setpoint trajectory to the given length.</member>
<member id="implementation"> <member id="implementation">
...@@ -96,7 +113,7 @@ ...@@ -96,7 +113,7 @@
<type id="retime_factor" prototype="float_parameter"> <type id="retime_factor" prototype="float_parameter">
<member id="description">Wanted duration reshaping to duration * [0..1]</member> <member id="description">Wanted duration reshaping to duration * [0..1]</member>
</type> </type>
<type id="convergence_threshold" prototype="float_parameter"> <type id="convergence_threshold" prototype="float_parameter">
<member id="description">Apply reshaping if convergence is below this parameter</member> <member id="description">Apply reshaping if convergence is below this parameter</member>
</type> </type>
......
#!/usr/bin/env python #!/usr/bin/env python
from __future__ import print_function from __future__ import print_function
from ilc.srv import TriggerILC, TriggerILCResponse, RetimeILC, RetimeILCResponse, RetimeIfILC, RetimeIfILCResponse from ilc.srv import *
import rospy import rospy
import numpy as np import numpy as np
import ilc_lib import ilc_lib
import os import os
import argparse import re
import rospkg
import datetime
class Ilc_serviceproxy: class Ilc_serviceproxy:
...@@ -22,59 +24,71 @@ class Ilc_serviceproxy: ...@@ -22,59 +24,71 @@ class Ilc_serviceproxy:
self.fs = 1.0/(self.T * 0.001) # sampling frequency self.fs = 1.0/(self.T * 0.001) # sampling frequency
rospy.Service('ilc_service', TriggerILC, self.callback_ilc_calc) rospy.Service('ilc_service', TriggerILC, self.callback_ilc_calc)
rospy.Service('ilc_service_meas', TriggerILCMeas, self.callback_ilc_calc_meas)
rospy.Service('ilc_retime', RetimeILC, self.callback_ilc_retime) rospy.Service('ilc_retime', RetimeILC, self.callback_ilc_retime)
rospy.Service('ilc_retime_if', RetimeIfILC, self.callback_ilc_retime_if) rospy.Service('ilc_retime_if', RetimeIfILC, self.callback_ilc_retime_if)
self.folder = rospy.get_param('~folder', None) self.folder = rospy.get_param('~folder', None)
def callback_ilc_calc(self, req):
try:
### Read Error data
data_org, header = ilc_lib.read(req.file_in)
### Resample def decode_filename(self, filename):
data_rs = ilc_lib.resample(data_org, self.T) name = filename
# Find timestamp and replace according to formating
for matchObj in re.finditer("{(\w*) ([^\}]*)}", name):
if matchObj.group(1) == "time":
if matchObj.group(2) != "":# found custom formating
# Replace timestamp according to input parameters
name = name.replace(matchObj.group(), "{}".format(datetime.datetime.now().strftime(matchObj.group(2))))
else:
# Replace timestamp according to predefined (general) parameters
name = name.replace(matchObj.group(),
"{}".format(
datetime.datetime.now().strftime(
"%Y-%m-%d_%H:%M:%S")))
elif matchObj.group(1) == "rospkg":
rospack = rospkg.RosPack()
name = name.replace(matchObj.group(), "{}".format(rospack.get_path(matchObj.group(2))))
print("Decoded Name: {} -> {}".format(filename, name))
return name
def verify_file_path(self, name):
# Check if folder exists, otherwise create it
path = os.path.dirname(name)
if path and not os.path.isdir(path):
print("Creating new folder: {}".format(path))
os.makedirs(path)
def callback_ilc_calc_meas(self, req):
try:
path = self.decode_filename(req.file_in)
### Trajectory generation ### Read measurement data
if self.decay_time is None: data_org, header = ilc_lib.read(path)
self.decay_time = data_rs[-1, 0]
trajectory = ilc_lib.traj_handle(data_rs, "cos", decay_time=self.decay_time)
### Relative Error: data-trajectory ### Create error from measurement - setpoint
self.data_diff = ilc_lib.combine_data(data_rs, trajectory, "diff") data_org[:, 1:] = data_org[:, 1:] - req.setpoint
### ILC Gain ### Continue as if nothing happend
data_ff = ilc_lib.ilc_calculations(self.data_diff, req.L) message = self.ilc_calc(req, data_org, header)
### Combine with last iteration, if existing return TriggerILCMeasResponse(success=True, message=message)
if self.initialized and os.path.exists(req.file_out):
last_ff, _ = ilc_lib.read(req.file_out)
# Reshape last feedforward if wanted except Exception as e:
if self.reshape: message = str(e)
last_ff = self.reshape_ff(last_ff, self.last_decay_time, self.decay_time) return TriggerILCMeasResponse(success=False, message=message)
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: # initialized == False -> cannot combine with previous csv
data_ilc = data_ff
self.initialized = True
#Filter data def callback_ilc_calc(self, req):
data_ilc_q = ilc_lib.butter_lowpass_filter(data_ilc, req.cutoff_freq, self.fs, 5) try:
path = self.decode_filename(req.file_in)
#Write ### Read Error data
ilc_lib.write(req.file_out, data_ilc_q, header) data_org, header = ilc_lib.read(path)
message = "file written sucessfully"
if self.folder is not None: self.ilc_calc(req, data_org, header)
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", "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)
return TriggerILCResponse(success=True, message=message) return TriggerILCResponse(success=True, message=message)
...@@ -82,6 +96,57 @@ class Ilc_serviceproxy: ...@@ -82,6 +96,57 @@ class Ilc_serviceproxy:
message = str(e) message = str(e)
return TriggerILCResponse(success=False, message=message) return TriggerILCResponse(success=False, message=message)
def ilc_calc(self, req, data_org, header):
### 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)
### Relative Error: data-trajectory
self.data_diff = ilc_lib.combine_data(data_rs, trajectory, "diff")
### ILC Gain
data_ff = ilc_lib.ilc_calculations(self.data_diff, 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):
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
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
#Filter data
data_ilc_q = ilc_lib.butter_lowpass_filter(data_ilc, req.cutoff_freq, self.fs, 5)
#Write
self.verify_file_path(path_out)
ilc_lib.write(path_out, data_ilc_q, header)
message = "file written sucessfully"
if self.folder is not None:
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", "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)
return message
def callback_ilc_retime(self, req): def callback_ilc_retime(self, req):
self.last_decay_time = self.decay_time self.last_decay_time = self.decay_time
self.decay_time = req.setpoint_duration_ms self.decay_time = req.setpoint_duration_ms
......
#include "cppitasc/coordination/script_rosservice.hpp" #include "cppitasc/coordination/script_rosservice.hpp"
#include <ilc/TriggerILC.h> #include <ilc/TriggerILC.h>
#include <ilc/TriggerILCMeas.h>
#include <ilc/RetimeILC.h> #include <ilc/RetimeILC.h>
#include <ilc/RetimeIfILC.h> #include <ilc/RetimeIfILC.h>
...@@ -50,6 +51,55 @@ private: ...@@ -50,6 +51,55 @@ private:
}; };
class ScriptTriggerILCMeas : public ScriptRosService<ilc::TriggerILCMeas, ilc::TriggerILCMeas::Request, ilc::TriggerILCMeas::Response>
{
public:
ScriptTriggerILCMeas(const string& name)
: ScriptRosService<ilc::TriggerILCMeas, ilc::TriggerILCMeas::Request, ilc::TriggerILCMeas::Response>(
name)
{
}
bool init(Dict& params) override {
extract(params["file_in"], fin_);
extract(params["setpoint"], setpoint_);
extract(params["file_out"], fout_);
extract(params["file_traj"], trajout_);
extract(params["traj_type"], trajtype_);
extract(params["L"], Lgain_);
extract(params["cutoff_freq"], cutoff_);
service_.request.file_in = fin_;
service_.request.setpoint = setpoint_;
service_.request.file_out = fout_;
service_.request.file_traj = trajout_;
service_.request.traj_type = trajtype_;
service_.request.L = Lgain_;
service_.request.cutoff_freq = cutoff_;
return ScriptRosService::init(params);
}
bool call() override {
bool result = ScriptRosService<ilc::TriggerILCMeas, ilc::TriggerILCMeas::Request, ilc::TriggerILCMeas::Response>::call();
if (!result) {
pi_error("Message: {}", service_.response.message);
}
return service_.response.success;
}
private:
string fin_;
vector<double> setpoint_;
string fout_;
string trajout_;
string trajtype_;
double Lgain_;
double cutoff_;
};
class ScriptRetimeILC : public ScriptRosService<ilc::RetimeILC, ilc::RetimeILC::Request, ilc::RetimeILC::Response> class ScriptRetimeILC : public ScriptRosService<ilc::RetimeILC, ilc::RetimeILC::Request, ilc::RetimeILC::Response>
{ {
public: public:
...@@ -81,7 +131,7 @@ public: ...@@ -81,7 +131,7 @@ public:
private: private:
int setpoint_duration_ms_; int setpoint_duration_ms_;
bool reuse_ff_; bool reuse_ff_;
int reuse_scheme_; int reuse_scheme_;
}; };
...@@ -120,9 +170,10 @@ public: ...@@ -120,9 +170,10 @@ public:
private: private:
double retime_factor_; double retime_factor_;
double convergence_threshold_; double convergence_threshold_;
bool reuse_ff_; bool reuse_ff_;
int reuse_scheme_; int reuse_scheme_;
}; };
RUNTIME_COMPONENT(ScriptTriggerILC) RUNTIME_COMPONENT(ScriptTriggerILC)
RUNTIME_COMPONENT(ScriptTriggerILCMeas)
RUNTIME_COMPONENT(ScriptRetimeILC) RUNTIME_COMPONENT(ScriptRetimeILC)
RUNTIME_COMPONENT(ScriptRetimeIfILC) RUNTIME_COMPONENT(ScriptRetimeIfILC)
string file_in
float64[] setpoint
string file_out
string file_traj
string traj_type
float64 L
float64 cutoff_freq
----------------------
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