Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Sign in
Toggle navigation
Menu
Open sidebar
Lorenz Halt
ilc
Commits
7eadc0c3
Commit
7eadc0c3
authored
Feb 15, 2021
by
victor
Browse files
add a possible timeshift to the summed feedforward
parent
00f49334
Changes
7
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
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment