Skip to content
GitLab
Commit 7eadc0c3 authored by victor's avatar victor
Browse files

add a possible timeshift to the summed feedforward

parent 00f49334
Hide whitespace changes
Inline Side-by-side
apps/approach.xml deleted 100644 → 0
View file @ 00f49334
<?xml version="1.0" encoding="UTF-8"?>
<pitasc>
<models>
<include package="pitasc_library" file="models/pitasc.xml"/>
<include package="pitasc_library" file="universal_robots/ur.xml"/>
<include package="ilc" file="models/ilc_controllers.xml"/>
<include package="ilc" file="models/ilc_scripts.xml"/>
</models>
<!-- Create a project -->
<clone prototype="project">
<member id="configuration">
<!-- Use the default configuration with recommended settings -->
<clone id="configuration" prototype="default_configuration"/>
</member>
<member id="environment">
<!-- Add a UR5 -->
<clone prototype="robot_ur5">
<member id="robot_driver.max_velocity">10</member>
<member id="robot_driver.max_acceleration">10</member>
<member id="components">
<clone prototype="force_sensor">
<member id="wrench_topic">wrench</member>
</clone>
</member>
</clone>
</member>
<member id="applications">
<!-- Use a skill sequence as a container -->
<clone prototype="skill_sequence">
<!-- Use the UR5 for this skill (and for its subskill) -->
<member id="robot" reference_id="environment.robot_ur5"/>
<member id="skills">
<!-- Move ptp -->
<clone prototype="skill_ptp">
<member id="joint_names">elbow_joint, shoulder_pan_joint, wrist_3_joint, wrist_1_joint, shoulder_lift_joint, wrist_2_joint</member>
<member id="target_joint_state">1.5111723007545828, 1.6092279039888917, -3.141917966357802, 2.9014219134662995, -1.3423824706872163, -3.1797213696262983</member>
</clone>
<clone prototype="skill_lin">
<member id="tool_frame">ee_link</member>
<member id="target_frame">start_position</member>
</clone>
<clone prototype="skill_lin">
<member id="tool_frame">ee_link</member>
<member id="target_frame">intermediate_position_low</member>
</clone>
<clone id="approach_ilc" prototype="skill_guarded_approach">
<member id="control_frame">target2</member>
<member id="tool_frame">ee_link</member>
<member id="axes">z</member>
<member id="velocities">0.005</member>
<member id="max_force">20.0</member>
<member id="move_skill.tasks.velocity_feedforward.controllers">
<clone prototype="velocity_controller_ilc">
<member id="filename_in">/home/victor/ilc_ws/src/ilc/Logs/vel_ff_2.txt</member>
<member id="data_source">
<reference reference_id="collections.target_to_eef.chains[0]"/>
</member>
<member id="setpoint_generator">
<reference reference_id="setpoint_generators.setpoint"/>
</member>
<member id="coordinates">x, y, z</member>
<member id="prefix" reference_id="prefix"/>
<member id="velocities" reference_id="velocities"/>
</clone>
</member>
<member id="scripts">
<clone prototype="script_measurement_logger">
<member id="provider" reference_id="force_skill.collections.force_chain.chains"/>
<member id="file_name">/home/victor/ilc_ws/src/ilc/Logs/approach_force_0.csv</member>
<member id="filter">force_chain/z</member>
</clone>
</member>
</clone>
<clone prototype="skill_lin">
<member id="tool_frame">ee_link</member>
<member id="target_frame">start_position</member>
<member id="scripts">
<clone prototype="script_trigger_ilc">
<member id="service_name">ilc_service</member>
<member id="file_in">/home/victor/ilc_ws/src/ilc/Logs/approach_force_0.csv</member>
<member id="file_out">/home/victor/ilc_ws/src/ilc/Logs/vel_ff_2.txt</member>
<member id="file_traj">/home/victor/ilc_ws/src/ilc/Logs/trajectory.txt</member>
<member id="traj_type">cos</member>
<member id="L">0.0001</member>
<member id="cutoff_freq">0.9</member>
</clone>
</member>
<member id="transitions">
<clone prototype="transition">
<member id="event">succeeded</member>
<member id="target">skill_ptp</member>
</clone>
</member>
</clone>
</member>
</clone>
</member>
</clone>
</pitasc>
apps/rob_ilc_lin_test.xml
View file @ 7eadc0c3
......@@ -84,7 +84,7 @@
<member id="tasks.tracking.controllers">
<clone id="ilc_controller" prototype="p_controller_ilc">
<member id="filename_in">/home/ipa325/Documents/ilc_Logs/lin_ff_2.txt</member>
<member id="filename_in">/home/victor/Documents/ilc_Logs/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/ipa325/Documents/ilc_Logs/logger_0.csv</member>
<member id="file_name">/home/victor/Documents/ilc_Logs/logger_0.csv</member>
</clone>
</member>
</clone>
......@@ -132,12 +132,13 @@
<member id="scripts">
<clone prototype="script_trigger_ilc">
<member id="service_name">ilc_service</member>
<member id="file_in">/home/ipa325/Documents/ilc_Logs/logger_0.csv</member>
<member id="file_out">/home/ipa325/Documents/ilc_Logs/lin_ff_2.txt</member>
<member id="file_traj">/home/ipa325/Documents/ilc_Logs/trajectory.txt</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="traj_type">cos</member>
<member id="L">0.6</member>
<member id="cutoff_freq">0.5</member>
<member id="time_shift">50</member>
</clone>
<clone prototype="script_retime_if_ilc">
......
launch/bringup_microsim.launch
View file @ 7eadc0c3
......@@ -70,6 +70,7 @@
<node name="goal_pos_publisher" pkg="tf" type="static_transform_publisher"
args="-0.05 0.6 0.45 1 0 0 0 world goal_position 100"/>
<!-- args="-0.05 0.6 0.45 1 0 0 0 world goal_position 100"/> -->
<node name="intermediate_pos_v_high_publisher" pkg="tf" type="static_transform_publisher"
args="-0.05 0.6 0.3 1 0 0 0 world intermediate_position_v_high 100"/>
......@@ -89,7 +90,7 @@
<!-- <node name="pitasc_http_server" pkg="pitasc_http" type="http_server.py" /> -->
<node name="ilc_rosproxy" pkg="ilc" type="ilc_rosproxy.py" output="screen" >
<rosparam param="T">8</rosparam>
<!-- <rosparam param="folder">/tmp/ilc_folder</rosparam> -->
<!-- <rosparam param="folder">/home/$User/test</rosparam> -->
</node>
</launch>
scripts/ilc_lib/ilc_lib.py
View file @ 7eadc0c3
......@@ -60,7 +60,6 @@ def read(file):
skip_header=1)
return data, header_str
def ilc_calculations(data, L):
data_out = data*L
data_out[:,0] = data[:,0] # restore timestamp
......@@ -113,6 +112,22 @@ def pad_data(a, b, val=0):
a[:,0] = b[:,0] # restore timestamp
return a, b
def ilc_shift(data, timestep, T):
out = []
for i, num in enumerate( data[:,0] ):
l = []
if i + timestep < len(data[:,0]):
l.append(num)
for n, val in enumerate(data[i+timestep,1:len(data[0])]):
l.insert(n+1,val)
#elif i + timestep >= len(data[:,0]):
# l.append( (i + timestep)*T )
# for n, val in enumerate(data[0,1:len(data[0])]):
# l.insert(n+1,0)
out.append(l)
out = np.array(out)
return out
def reshape(data, last_duration, new_duration):
y = copy.deepcopy(data)
......
scripts/ilc_lib/ilc_rosproxy.py
View file @ 7eadc0c3
......@@ -63,13 +63,14 @@ class Ilc_serviceproxy:
def callback_ilc_calc_meas(self, req):
try:
print
path = self.decode_filename(req.file_in)
### Read measurement data
data_org, header = ilc_lib.read(path)
### Create error from measurement - setpoint
data_org[:, 1:] = data_org[:, 1:] - req.setpoint
data_org[:, 1:] = req.setpoint - data_org[:, 1:]
### Continue as if nothing happend
message = self.ilc_calc(req, data_org, header)
......@@ -87,12 +88,12 @@ class Ilc_serviceproxy:
### Read Error data
data_org, header = ilc_lib.read(path)
message = self.ilc_calc(req, data_org, header)
return TriggerILCResponse(success=True, message=message)
except Exception as e:
print("except")
message = str(e)
return TriggerILCResponse(success=False, message=message)
......@@ -100,7 +101,6 @@ class Ilc_serviceproxy:
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]
......@@ -109,9 +109,15 @@ class Ilc_serviceproxy:
### 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)
### 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)
### 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):
......@@ -140,6 +146,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", "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)
......
srv/TriggerILC.srv
View file @ 7eadc0c3
......@@ -4,6 +4,7 @@ string file_traj
string traj_type
float64 L
float64 cutoff_freq
uint8 timeshift
----------------------
bool success
string message
srv/TriggerILCMeas.srv
View file @ 7eadc0c3
......@@ -5,6 +5,7 @@ string file_traj
string traj_type
float64 L
float64 cutoff_freq
uint8 timeshift
----------------------
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

Datenschutz und Impressum