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

cythonize ilc_rosproxy

parent 7f36e78d
#!/usr/bin/env python
from __future__ import print_function
from ilc.srv import TriggerILC, TriggerILCResponse, RetimeILC, RetimeILCResponse, RetimeIfILC, RetimeIfILCResponse
import rospy
import numpy as np
from ilc import ilc_lib
import os
import argparse
class Ilc_serviceproxy:
def __init__(self):
self.decay_time = None
self.last_decay_time = None
self.initialized = False
self.data_diff = None
self.reshape = False
self.T = rospy.get_param('~T', 8) # sampling time in ms
self.fs = 1.0/(self.T * 0.001) # sampling frequency
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)
def callback_ilc_calc(self, req):
try:
### Read Error data
data_org, header = ilc_lib.read(req.file_in)
### 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
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(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
ilc_lib.write(req.file_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 TriggerILCResponse(success=True, message=message)
except Exception as e:
message = str(e)
return TriggerILCResponse(success=False, message=message)
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
if req.reuse_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
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 req.reuse_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')
Ilc_serviceproxy()
print("Ready to serve.")
rospy.spin()
#!/usr/bin/env python
from __future__ import print_function
from ilc.srv import TriggerILC, TriggerILCResponse, RetimeILC, RetimeILCResponse, RetimeIfILC, RetimeIfILCResponse
import rospy
import numpy as np
from ilc import ilc_lib
import os
import argparse
class Ilc_serviceproxy:
def __init__(self):
self.decay_time = None
self.last_decay_time = None
self.initialized = False
self.data_diff = None
self.reshape = False
self.T = rospy.get_param('~T', 8) # sampling time in ms
self.fs = 1.0/(self.T * 0.001) # sampling frequency
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)
def callback_ilc_calc(self, req):
try:
### Read Error data
data_org, header = ilc_lib.read(req.file_in)
### 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
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(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
ilc_lib.write(req.file_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 TriggerILCResponse(success=True, message=message)
except Exception as e:
message = str(e)
return TriggerILCResponse(success=False, message=message)
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
if req.reuse_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
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 req.reuse_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))
from ilc import ilc_rosproxy
if __name__ == '__main__':
rospy.init_node('ilc_proxy')
Ilc_serviceproxy()
print("Ready to serve.")
ilc_rosproxy.Ilc_serviceproxy()
print("ILC: Ready to serve.")
rospy.spin()
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