ilc_rosproxy.py 7.46 KB
Newer Older
Lorenz Halt's avatar
Lorenz Halt committed
1
2
3
#!/usr/bin/env python

from __future__ import print_function
4
from ilc.srv import *
Lorenz Halt's avatar
Lorenz Halt committed
5
6
import rospy
import numpy as np
Lorenz Halt's avatar
Lorenz Halt committed
7
import ilc_lib
Lorenz Halt's avatar
Lorenz Halt committed
8
import os
9
10
11
import re
import rospkg
import datetime
Lorenz Halt's avatar
Lorenz Halt committed
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26

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)
27
        rospy.Service('ilc_service_meas', TriggerILCMeas, self.callback_ilc_calc_meas)
Lorenz Halt's avatar
Lorenz Halt committed
28
29
30
31
32
33
        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)


34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
    def decode_filename(self, filename):
        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:
66
            print
67
            path = self.decode_filename(req.file_in)
Lorenz Halt's avatar
Lorenz Halt committed
68

69
70
            ### Read measurement data
            data_org, header = ilc_lib.read(path)
Lorenz Halt's avatar
Lorenz Halt committed
71

72
            ### Create error from measurement - setpoint
73
            data_org[:, 1:] =  req.setpoint - data_org[:, 1:]
Lorenz Halt's avatar
Lorenz Halt committed
74

75
76
            ### Continue as if nothing happend
            message = self.ilc_calc(req, data_org, header)
Lorenz Halt's avatar
Lorenz Halt committed
77

78
            return TriggerILCMeasResponse(success=True, message=message)
Lorenz Halt's avatar
Lorenz Halt committed
79

80
81
82
        except Exception as e:
            message = str(e)
            return TriggerILCMeasResponse(success=False, message=message)
Lorenz Halt's avatar
Lorenz Halt committed
83
84


85
86
87
    def callback_ilc_calc(self, req):
        try:
            path = self.decode_filename(req.file_in)
Lorenz Halt's avatar
Lorenz Halt committed
88

89
90
            ### Read Error data
            data_org, header = ilc_lib.read(path)
Lorenz Halt's avatar
Lorenz Halt committed
91
            message = self.ilc_calc(req, data_org, header)
Lorenz Halt's avatar
Lorenz Halt committed
92
93
94
95

            return TriggerILCResponse(success=True, message=message)

        except Exception as e:
96
            print("except")
Lorenz Halt's avatar
Lorenz Halt committed
97
98
99
            message = str(e)
            return TriggerILCResponse(success=False, message=message)

100
101
102
103
104
105
106
107
108
109
110
111

    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")

112
113
114
115
116
117
        ### 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)
118

119
120
        ### ILC Gain
        data_ff =  ilc_lib.ilc_calculations(data_ff_shift, req.L)
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
        ### 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)
149
            ilc_lib.external_data_store(self.folder + "/ff_data_shift",           "ff_data_shift", data_ff_shift, header)
150
151
152
153
154
155
156
            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


Lorenz Halt's avatar
Lorenz Halt committed
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
    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()