Commit 144eac24 authored by Lorenz Halt's avatar Lorenz Halt 🔀
Browse files

retime loop in "rob_ilc_lin_test" using setpoint gen and ilc_proxy

parent 1d935321
......@@ -66,6 +66,8 @@
<clone id="ilc_skill" prototype="skill_lin">
<member id="trajectory_duration">10</member>
<clone id="retime_factor" prototype="float_parameter">0.9</clone>
<member id="tool_frame">ee_link</member>
<member id="target_frame">start_position</member>
......@@ -91,7 +93,7 @@
<member id="coordinates" reference_id="coordinates"/>
<member id="prefix" reference_id="prefix"/>
<member id="duration" reference_id="trajectory_duration"/>
<member id="retime_factor">0.9</member>
<member id="retime_factor" reference_id="retime_factor"/>
<member id="max_vel_groups">
<reference reference_id="max_linear_velocity"/>
<reference reference_id="max_angular_velocity"/>
......@@ -160,7 +162,7 @@
<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="traj_type">cos</member>
<member id="traj_type">const</member>
<member id="L">0.6</member>
<member id="cutoff_freq">0.5</member>
<member id="time_shift">0</member>
......@@ -169,8 +171,9 @@
<clone prototype="script_retime_if_ilc">
<member id="namespace">ilc_rosproxy</member>
<member id="service_name">ilc_retime_if</member>
<member id="retime_srv">retime_setpoint_ilc</member> <!-- "retime_" + <id of cosine_setpoint_ILC prototype> -->
<member id="on_start">false</member>
<member id="retime_factor">0.9</member>
<member id="retime_factor" reference_id="ilc_skill.retime_factor"/>
<member id="convergence_threshold">0.02</member>
<member id="reuse_ff">True</member>
<member id="reuse_scheme">1</member>
......
......@@ -130,6 +130,11 @@
<member id="description">Selects different reshaping schemes. 0: No reshaping</member>
<data>0</data>
</type>
<type id="retime_srv" prototype="string_parameter">
<member id="description">Service name of the setpoint generator to retime the trajectory</member>
<data>retime_setpoint_ilc</data>
</type>
</data>
</type>
</models>
......
......@@ -75,7 +75,7 @@ def traj_handle(data, traj_type, decay_time=None):
elif traj_type == "const":
ref = [ 0 for t in data[:,0] ]
else:
print("Unknown reference shape: '{}'".format(traj_type))
print("Unknown reference traj_type: '{}'".format(traj_type))
ref = [ 0 for t in data[:,0] ]
trajectory = data[:, 0]
......
......@@ -10,6 +10,8 @@ import re
import rospkg
import datetime
from std_srvs.srv import Empty, EmptyResponse
class Ilc_serviceproxy:
def __init__(self):
......@@ -30,7 +32,6 @@ class Ilc_serviceproxy:
self.folder = rospy.get_param('~folder', None)
def decode_filename(self, filename):
name = filename
# Find timestamp and replace according to formating
......@@ -104,7 +105,7 @@ class Ilc_serviceproxy:
### 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)
trajectory = ilc_lib.traj_handle(data_rs, req.traj_type, decay_time=self.decay_time)
### Relative Error: data-trajectory
self.data_diff = ilc_lib.combine_data(data_rs, trajectory, "diff")
......@@ -182,12 +183,20 @@ class Ilc_serviceproxy:
if req.reuse_scheme != 0:
self.reshape = True
# Call service to setpoint_ilc (retime factor is start parameter, not in service but must be in sync!)
print(req)
resp = rospy.ServiceProxy(req.retime_srv, Empty)()
print("-------------------------------------------")
print("Retimed from {}ms to {}ms".format(self.last_decay_time, self.decay_time))
print("-------------------------------------------")
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:
print(str(e))
return RetimeIfILCResponse(success=False, message=str(e))
......
......@@ -147,6 +147,7 @@ public:
bool init(Dict& params) override {
extract(params["retime_factor"], retime_factor_);
extract(params["retime_srv"], retime_srv_);
extract(params["convergence_threshold"], convergence_threshold_);
extract(params["reuse_ff"], reuse_ff_);
extract(params["reuse_scheme"], reuse_scheme_);
......@@ -155,6 +156,7 @@ public:
service_.request.convergence_threshold = convergence_threshold_;
service_.request.reuse_ff = reuse_ff_;
service_.request.reuse_scheme = reuse_scheme_;
service_.request.retime_srv = retime_srv_;
return ScriptRosService::init(params);
}
......@@ -172,6 +174,7 @@ private:
double convergence_threshold_;
bool reuse_ff_;
int reuse_scheme_;
string retime_srv_;
};
RUNTIME_COMPONENT(ScriptTriggerILC)
RUNTIME_COMPONENT(ScriptTriggerILCMeas)
......
......@@ -2,6 +2,7 @@ float32 retime_factor
float32 convergence_threshold
bool reuse_ff
uint8 reuse_scheme
string retime_srv
----------------------
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