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

fix absolute pathes, renaming, cleanup

parent c1d4509b
...@@ -139,3 +139,5 @@ cython_debug/ ...@@ -139,3 +139,5 @@ cython_debug/
*.log *.log
*.pyc *.pyc
logs/
\ No newline at end of file
...@@ -3,6 +3,7 @@ project(ilc CXX) ...@@ -3,6 +3,7 @@ project(ilc CXX)
find_package(catkin REQUIRED COMPONENTS find_package(catkin REQUIRED COMPONENTS
rospy rospy
roslib
roscpp roscpp
std_msgs std_msgs
message_generation message_generation
......
...@@ -115,7 +115,7 @@ ...@@ -115,7 +115,7 @@
<member id="force.tasks.apply_force.controllers"> <member id="force.tasks.apply_force.controllers">
<clone id="ilc_controller" prototype="first_order_impedance_controller_ilc"> <clone id="ilc_controller" prototype="first_order_impedance_controller_ilc">
<member id="filename_in">/home/ipa325/Documents/ilc_Logs/force_ff_2.txt</member> <member id="filename_ff">/home/ipa325/Documents/ilc_Logs/force_ff_2.txt</member>
<member id="data_source"> <member id="data_source">
<reference reference_id="collections.force_chain.chains[0]"/> <reference reference_id="collections.force_chain.chains[0]"/>
</member> </member>
...@@ -165,8 +165,8 @@ ...@@ -165,8 +165,8 @@
<clone prototype="script_trigger_ilc"> <clone prototype="script_trigger_ilc">
<member id="namespace">ilc_rosproxy</member> <member id="namespace">ilc_rosproxy</member>
<member id="service_name">ilc_service</member> <member id="service_name">ilc_service</member>
<member id="file_in">/home/ipa325/Documents/ilc_Logs/force_logger_0.csv</member> <member id="filename_error">/home/ipa325/Documents/ilc_Logs/force_logger_0.csv</member>
<member id="file_out">/home/ipa325/Documents/ilc_Logs/force_ff_2.txt</member> <member id="filename_ff">/home/ipa325/Documents/ilc_Logs/force_ff_2.txt</member>
<member id="traj_type">e</member> <member id="traj_type">e</member>
<member id="L">0.00005</member> <member id="L">0.00005</member>
<member id="cutoff_freq">0.9</member> <member id="cutoff_freq">0.9</member>
......
...@@ -72,7 +72,7 @@ ...@@ -72,7 +72,7 @@
<member id="trajectory_duration">10</member> <member id="trajectory_duration">10</member>
<clone id="retime_factor" prototype="float_parameter">0.9</clone> <clone id="retime_factor" prototype="float_parameter">0.9</clone>
<member id="positioning_monitor.distances">0.0, 0.0, 0.0, 0.0, 0.0, 0.0</member> <member id="positioning_monitor.distances">0.0, 0.0, 0.0, 0.0, 0.0, 0.0</member>
<member id="monitors"> <member id="monitors">
<clone prototype="monitor_duration"> <clone prototype="monitor_duration">
...@@ -106,7 +106,7 @@ ...@@ -106,7 +106,7 @@
<member id="tasks.tracking.controllers"> <member id="tasks.tracking.controllers">
<clone id="ilc_controller" prototype="p_controller_ilc"> <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_ff">/home/ipa325/Documents/ilc_Logs/lin_ff_2.txt</member>
<member id="data_source"> <member id="data_source">
<reference reference_id="collections.target_to_tool.chains[0]"/> <reference reference_id="collections.target_to_tool.chains[0]"/>
...@@ -172,9 +172,8 @@ ...@@ -172,9 +172,8 @@
<clone prototype="script_trigger_ilc"> <clone prototype="script_trigger_ilc">
<member id="namespace">ilc_rosproxy1</member> <member id="namespace">ilc_rosproxy1</member>
<member id="service_name">ilc_service</member> <member id="service_name">ilc_service</member>
<member id="file_in">/home/ipa325/Documents/ilc_Logs/logger_0.csv</member> <member id="filename_error">/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="filename_ff">/home/ipa325/Documents/ilc_Logs/lin_ff_2.txt</member>
<member id="file_traj">/home/ipa325/Documents/ilc_Logs/trajectory.txt</member>
<member id="traj_type">const</member> <member id="traj_type">const</member>
<member id="L">0.6</member> <member id="L">0.6</member>
<member id="cutoff_freq">0.5</member> <member id="cutoff_freq">0.5</member>
......
...@@ -81,7 +81,7 @@ ...@@ -81,7 +81,7 @@
<member id="tasks.tracking.controllers"> <member id="tasks.tracking.controllers">
<clone id="ilc_controller" prototype="p_controller_ilc"> <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_ff">/home/ipa325/Documents/ilc_Logs/lin_ff_2.txt</member>
<member id="data_source"> <member id="data_source">
<reference reference_id="collections.target_to_tool.chains[0]"/> <reference reference_id="collections.target_to_tool.chains[0]"/>
</member> </member>
...@@ -134,7 +134,7 @@ ...@@ -134,7 +134,7 @@
<member id="force.tasks.apply_force.controllers"> <member id="force.tasks.apply_force.controllers">
<clone id="ilc_controller" prototype="first_order_impedance_controller_ilc"> <clone id="ilc_controller" prototype="first_order_impedance_controller_ilc">
<member id="filename_in">/home/ipa325/Documents/ilc_Logs/force_ff_2.txt</member> <member id="filename_ff">/home/ipa325/Documents/ilc_Logs/force_ff_2.txt</member>
<member id="data_source"> <member id="data_source">
<reference reference_id="collections.force_chain.chains[0]"/> <reference reference_id="collections.force_chain.chains[0]"/>
</member> </member>
...@@ -174,9 +174,8 @@ ...@@ -174,9 +174,8 @@
<clone prototype="script_trigger_ilc"> <clone prototype="script_trigger_ilc">
<member id="namespace">ilc_rosproxy</member> <member id="namespace">ilc_rosproxy</member>
<member id="service_name">ilc_service</member> <member id="service_name">ilc_service</member>
<member id="file_in">/home/ipa325/Documents/ilc_Logs/logger_0.csv</member> <member id="filename_error">/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="filename_ff">/home/ipa325/Documents/ilc_Logs/lin_ff_2.txt</member>
<member id="file_traj">/home/ipa325/Documents/ilc_Logs/trajectory.txt</member>
<member id="traj_type">cos</member> <member id="traj_type">cos</member>
<member id="L">0.6</member> <member id="L">0.6</member>
<member id="cutoff_freq">0.5</member> <member id="cutoff_freq">0.5</member>
...@@ -195,8 +194,8 @@ ...@@ -195,8 +194,8 @@
<clone prototype="script_trigger_ilc"> <clone prototype="script_trigger_ilc">
<member id="namespace">ilc_rosproxy2</member> <member id="namespace">ilc_rosproxy2</member>
<member id="service_name">ilc_service</member> <member id="service_name">ilc_service</member>
<member id="file_in">/home/ipa325/Documents/ilc_Logs/force_logger_0.csv</member> <member id="filename_error">/home/ipa325/Documents/ilc_Logs/force_logger_0.csv</member>
<member id="file_out">/home/ipa325/Documents/ilc_Logs/force_ff_2.txt</member> <member id="filename_ff">/home/ipa325/Documents/ilc_Logs/force_ff_2.txt</member>
<member id="traj_type">cos</member> <member id="traj_type">cos</member>
<member id="L">0.00005</member> <member id="L">0.00005</member>
<member id="cutoff_freq">0.9</member> <member id="cutoff_freq">0.9</member>
......
...@@ -75,24 +75,18 @@ ...@@ -75,24 +75,18 @@
<member id="tasks.tracking.setpoint_generators"> <member id="tasks.tracking.setpoint_generators">
<!-- <clone id="setpoint_cos" prototype="cosine_setpoint_ILC"> <!-- Replace setpoint generator with rosservice callable ilc cosine setpoint_gen -->
<member id="data_source">
<reference reference_id="collections.target_to_tool.chains[0]"/>
</member>
<member id="desired" reference_id="target_offsets"/>
<member id="coordinates" reference_id="coordinates"/>
<member id="prefix" reference_id="prefix"/>
<member id="duration" reference_id="trajectory_duration"/>
</clone> -->
<clone id="setpoint_ilc" prototype="cosine_setpoint_ILC"> <clone id="setpoint_ilc" prototype="cosine_setpoint_ILC">
<member id="data_source"> <member id="data_source">
<reference reference_id="collections.target_to_tool.chains[0]"/> <reference reference_id="collections.target_to_tool.chains[0]"/>
</member> </member>
<member id="duration" reference_id="trajectory_duration"/> <!-- use parameter from above for usability -->
<member id="retime_factor" reference_id="retime_factor"/> <!-- use parameter from above for usability -->
<member id="desired" reference_id="target_offsets"/> <member id="desired" reference_id="target_offsets"/>
<member id="coordinates" reference_id="coordinates"/> <member id="coordinates" reference_id="coordinates"/>
<member id="prefix" reference_id="prefix"/> <member id="prefix" reference_id="prefix"/>
<member id="duration" reference_id="trajectory_duration"/>
<member id="retime_factor" reference_id="retime_factor"/>
<member id="max_vel_groups"> <member id="max_vel_groups">
<reference reference_id="max_linear_velocity"/> <reference reference_id="max_linear_velocity"/>
<reference reference_id="max_angular_velocity"/> <reference reference_id="max_angular_velocity"/>
...@@ -106,14 +100,14 @@ ...@@ -106,14 +100,14 @@
</member> </member>
<member id="tasks.tracking.controllers"> <member id="tasks.tracking.controllers">
<!-- Replace controller with ilc controller adaption taking feedforward into account -->
<clone id="ilc_controller" prototype="p_controller_ilc"> <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_ff">{rospkg ilc}/logs/{time %F}/ilc_ff.csv</member>
<member id="data_source"> <member id="data_source">
<reference reference_id="collections.target_to_tool.chains[0]"/> <reference reference_id="collections.target_to_tool.chains[0]"/>
</member> </member>
<member id="setpoint_generator"> <member id="setpoint_generator">
<reference reference_id="setpoint_generators.setpoint_ilc"/> <reference reference_id="setpoint_generators.setpoint_ilc"/>
<!-- <reference reference_id="setpoint_generators[0]"/> -->
</member> </member>
<member id="coordinates">x, y, z</member> <member id="coordinates">x, y, z</member>
<member id="prefix" reference_id="prefix"/> <member id="prefix" reference_id="prefix"/>
...@@ -121,7 +115,7 @@ ...@@ -121,7 +115,7 @@
<member id="gain" reference_id="linear_controller_gain"/> <member id="gain" reference_id="linear_controller_gain"/>
</clone> </clone>
<!-- orientation controller --> <!-- orientation controller, not replaced yet referencing ilc setpoint_gen -->
<clone prototype="tanh_controller"> <clone prototype="tanh_controller">
<member id="max_output" reference_id="max_angular_velocity"/> <member id="max_output" reference_id="max_angular_velocity"/>
<member id="P_lin" reference_id="angular_controller_gain"/> <member id="P_lin" reference_id="angular_controller_gain"/>
...@@ -130,7 +124,6 @@ ...@@ -130,7 +124,6 @@
</member> </member>
<member id="setpoint_generator"> <member id="setpoint_generator">
<reference reference_id="setpoint_generators.setpoint_ilc"/> <reference reference_id="setpoint_generators.setpoint_ilc"/>
<!-- <reference reference_id="setpoint_generators[0]"/> -->
</member> </member>
<member id="coordinates">a, b, c</member> <member id="coordinates">a, b, c</member>
<member id="prefix" reference_id="prefix"/> <member id="prefix" reference_id="prefix"/>
...@@ -138,19 +131,19 @@ ...@@ -138,19 +131,19 @@
</member> </member>
<member id="scripts"> <member id="scripts">
<!-- Log the control errors for ilc calculations -->
<clone prototype="script_error_logger"> <clone prototype="script_error_logger">
<member id="controllers"> <member id="controllers">
<reference reference_id="tasks.tracking.controllers.ilc_controller"/> <reference reference_id="tasks.tracking.controllers.ilc_controller"/>
</member> </member>
<member id="file_name">/home/ipa325/Documents/ilc_Logs/logger_0.csv</member> <member id="file_name">{rospkg ilc}/logs/{time %F}/error.csv</member>
</clone> </clone>
<clone prototype="script_measurement_logger"> <clone prototype="script_measurement_logger">
<member id="provider"> <member id="provider">
<reference reference_id="collections.target_to_tool.chains[0]"/> <reference reference_id="collections.target_to_tool.chains[0]"/>
</member> </member>
<member id="file_name">/home/ipa325/Documents/ilc_Meas/meas_logger_{time %F_%H_%M_%S}.csv</member> <member id="file_name">{rospkg ilc}/logs/{time %F}/meas_lin/target_to_tool_{time %F_%H_%M_%S}.csv</member>
</clone> </clone>
</member> </member>
...@@ -166,9 +159,8 @@ ...@@ -166,9 +159,8 @@
<clone prototype="script_trigger_ilc"> <clone prototype="script_trigger_ilc">
<member id="namespace">ilc_rosproxy</member> <member id="namespace">ilc_rosproxy</member>
<member id="service_name">ilc_service</member> <member id="service_name">ilc_service</member>
<member id="file_in">/home/ipa325/Documents/ilc_Logs/logger_0.csv</member> <member id="filename_error">{rospkg ilc}/logs/{time %F}/error.csv</member>
<member id="file_out">/home/ipa325/Documents/ilc_Logs/lin_ff_2.txt</member> <member id="filename_ff">{rospkg ilc}/logs/{time %F}/ilc_ff.csv</member>
<member id="file_traj">/home/ipa325/Documents/ilc_Logs/trajectory.txt</member>
<member id="traj_type">const</member> <member id="traj_type">const</member>
<member id="L">0.6</member> <member id="L">0.6</member>
<member id="cutoff_freq">0.5</member> <member id="cutoff_freq">0.5</member>
......
...@@ -130,7 +130,7 @@ ...@@ -130,7 +130,7 @@
<member id="force.tasks.apply_force.controllers"> <member id="force.tasks.apply_force.controllers">
<clone id="ilc_controller" prototype="first_order_impedance_controller_ilc"> <clone id="ilc_controller" prototype="first_order_impedance_controller_ilc">
<member id="filename_in">/home/ipa325/Documents/ilc_Logs/force_ff_in.txt</member> <member id="filename_ff">/home/ipa325/Documents/ilc_Logs/force_ff_in.txt</member>
<member id="data_source"> <member id="data_source">
<reference reference_id="collections.force_chain.chains[0]"/> <reference reference_id="collections.force_chain.chains[0]"/>
</member> </member>
...@@ -209,7 +209,7 @@ ...@@ -209,7 +209,7 @@
<member id="tasks.tracking.controllers"> <member id="tasks.tracking.controllers">
<clone id="ilc_controller" prototype="p_controller_ilc"> <clone id="ilc_controller" prototype="p_controller_ilc">
<member id="filename_in">/home/ipa325/Documents/ilc_Logs/lin_ff_out.txt</member> <member id="filename_ff">/home/ipa325/Documents/ilc_Logs/lin_ff_out.txt</member>
<member id="data_source"> <member id="data_source">
<reference reference_id="collections.target_to_tool.chains[0]"/> <reference reference_id="collections.target_to_tool.chains[0]"/>
...@@ -289,7 +289,7 @@ ...@@ -289,7 +289,7 @@
<member id="tasks.tracking.controllers"> <member id="tasks.tracking.controllers">
<clone id="ilc_controller" prototype="p_controller_ilc"> <clone id="ilc_controller" prototype="p_controller_ilc">
<member id="filename_in">/home/ipa325/Documents/ilc_Logs/lin_ff_left.txt</member> <member id="filename_ff">/home/ipa325/Documents/ilc_Logs/lin_ff_left.txt</member>
<member id="data_source"> <member id="data_source">
<reference reference_id="collections.target_to_tool.chains[0]"/> <reference reference_id="collections.target_to_tool.chains[0]"/>
...@@ -375,7 +375,7 @@ ...@@ -375,7 +375,7 @@
<member id="tasks.tracking.controllers"> <member id="tasks.tracking.controllers">
<clone id="ilc_controller" prototype="p_controller_ilc"> <clone id="ilc_controller" prototype="p_controller_ilc">
<member id="filename_in">/home/ipa325/Documents/ilc_Logs/lin_ff_up.txt</member> <member id="filename_ff">/home/ipa325/Documents/ilc_Logs/lin_ff_up.txt</member>
<member id="data_source"> <member id="data_source">
<reference reference_id="collections.target_to_tool.chains[0]"/> <reference reference_id="collections.target_to_tool.chains[0]"/>
...@@ -463,7 +463,7 @@ ...@@ -463,7 +463,7 @@
<member id="tasks.tracking.controllers"> <member id="tasks.tracking.controllers">
<clone id="ilc_controller" prototype="p_controller_ilc"> <clone id="ilc_controller" prototype="p_controller_ilc">
<member id="filename_in">/home/ipa325/Documents/ilc_Logs/lin_ff_down.txt</member> <member id="filename_ff">/home/ipa325/Documents/ilc_Logs/lin_ff_down.txt</member>
<member id="data_source"> <member id="data_source">
<reference reference_id="collections.target_to_tool.chains[0]"/> <reference reference_id="collections.target_to_tool.chains[0]"/>
...@@ -543,7 +543,7 @@ ...@@ -543,7 +543,7 @@
<member id="tasks.tracking.controllers"> <member id="tasks.tracking.controllers">
<clone id="ilc_controller" prototype="p_controller_ilc"> <clone id="ilc_controller" prototype="p_controller_ilc">
<member id="filename_in">/home/ipa325/Documents/ilc_Logs/lin_ff_right.txt</member> <member id="filename_ff">/home/ipa325/Documents/ilc_Logs/lin_ff_right.txt</member>
<member id="data_source"> <member id="data_source">
<reference reference_id="collections.target_to_tool.chains[0]"/> <reference reference_id="collections.target_to_tool.chains[0]"/>
...@@ -608,8 +608,8 @@ ...@@ -608,8 +608,8 @@
<clone prototype="script_trigger_ilc"> <clone prototype="script_trigger_ilc">
<member id="namespace">ilc_rosproxy1</member> <member id="namespace">ilc_rosproxy1</member>
<member id="service_name">ilc_service</member> <member id="service_name">ilc_service</member>
<member id="file_in">/home/ipa325/Documents/ilc_Logs/force_logger_in.csv</member> <member id="filename_error">/home/ipa325/Documents/ilc_Logs/force_logger_in.csv</member>
<member id="file_out">/home/ipa325/Documents/ilc_Logs/force_ff_in.txt</member> <member id="filename_ff">/home/ipa325/Documents/ilc_Logs/force_ff_in.txt</member>
<member id="traj_type">e</member> <member id="traj_type">e</member>
<member id="L">0.00005</member> <member id="L">0.00005</member>
<member id="cutoff_freq">0.9</member> <member id="cutoff_freq">0.9</member>
...@@ -629,8 +629,8 @@ ...@@ -629,8 +629,8 @@
<clone prototype="script_trigger_ilc"> <clone prototype="script_trigger_ilc">
<member id="namespace">ilc_rosproxy2</member> <member id="namespace">ilc_rosproxy2</member>
<member id="service_name">ilc_service</member> <member id="service_name">ilc_service</member>
<member id="file_in">/home/ipa325/Documents/ilc_Logs/logger_lin_out.csv</member> <member id="filename_error">/home/ipa325/Documents/ilc_Logs/logger_lin_out.csv</member>
<member id="file_out">/home/ipa325/Documents/ilc_Logs/lin_ff_out.txt</member> <member id="filename_ff">/home/ipa325/Documents/ilc_Logs/lin_ff_out.txt</member>
<member id="traj_type">e</member> <member id="traj_type">e</member>
<member id="L">0.6</member> <member id="L">0.6</member>
<member id="cutoff_freq">0.5</member> <member id="cutoff_freq">0.5</member>
...@@ -649,8 +649,8 @@ ...@@ -649,8 +649,8 @@
<clone prototype="script_trigger_ilc"> <clone prototype="script_trigger_ilc">
<member id="namespace">ilc_rosproxy3</member> <member id="namespace">ilc_rosproxy3</member>
<member id="service_name">ilc_service</member> <member id="service_name">ilc_service</member>
<member id="file_in">/home/ipa325/Documents/ilc_Logs/logger_lin_left.csv</member> <member id="filename_error">/home/ipa325/Documents/ilc_Logs/logger_lin_left.csv</member>
<member id="file_out">/home/ipa325/Documents/ilc_Logs/lin_ff_left.txt</member> <member id="filename_ff">/home/ipa325/Documents/ilc_Logs/lin_ff_left.txt</member>
<member id="traj_type">e</member> <member id="traj_type">e</member>
<member id="L">0.6</member> <member id="L">0.6</member>
<member id="cutoff_freq">0.5</member> <member id="cutoff_freq">0.5</member>
...@@ -669,8 +669,8 @@ ...@@ -669,8 +669,8 @@
<!-- <clone prototype="script_trigger_ilc"> <!-- <clone prototype="script_trigger_ilc">
<member id="namespace">ilc_rosproxy4</member> <member id="namespace">ilc_rosproxy4</member>
<member id="service_name">ilc_service</member> <member id="service_name">ilc_service</member>
<member id="file_in">/home/ipa325/Documents/ilc_Logs/lin_logger_up.csv</member> <member id="filename_error">/home/ipa325/Documents/ilc_Logs/lin_logger_up.csv</member>
<member id="file_out">/home/ipa325/Documents/ilc_Logs/lin_ff_up.txt</member> <member id="filename_ff">/home/ipa325/Documents/ilc_Logs/lin_ff_up.txt</member>
<member id="traj_type">e</member> <member id="traj_type">e</member>
<member id="L">0.6</member> <member id="L">0.6</member>
<member id="cutoff_freq">0.9</member> <member id="cutoff_freq">0.9</member>
...@@ -689,8 +689,8 @@ ...@@ -689,8 +689,8 @@
<clone prototype="script_trigger_ilc"> <clone prototype="script_trigger_ilc">
<member id="namespace">ilc_rosproxy5</member> <member id="namespace">ilc_rosproxy5</member>
<member id="service_name">ilc_service</member> <member id="service_name">ilc_service</member>
<member id="file_in">/home/ipa325/Documents/ilc_Logs/lin_logger_down.csv</member> <member id="filename_error">/home/ipa325/Documents/ilc_Logs/lin_logger_down.csv</member>
<member id="file_out">/home/ipa325/Documents/ilc_Logs/lin_ff_down.txt</member> <member id="filename_ff">/home/ipa325/Documents/ilc_Logs/lin_ff_down.txt</member>
<member id="traj_type">e</member> <member id="traj_type">e</member>
<member id="L">0.6</member> <member id="L">0.6</member>
<member id="cutoff_freq">0.9</member> <member id="cutoff_freq">0.9</member>
...@@ -709,8 +709,8 @@ ...@@ -709,8 +709,8 @@
<clone prototype="script_trigger_ilc"> <clone prototype="script_trigger_ilc">
<member id="namespace">ilc_rosproxy6</member> <member id="namespace">ilc_rosproxy6</member>
<member id="service_name">ilc_service</member> <member id="service_name">ilc_service</member>
<member id="file_in">/home/ipa325/Documents/ilc_Logs/logger_lin_right.csv</member> <member id="filename_error">/home/ipa325/Documents/ilc_Logs/logger_lin_right.csv</member>
<member id="file_out">/home/ipa325/Documents/ilc_Logs/lin_ff_right.txt</member> <member id="filename_ff">/home/ipa325/Documents/ilc_Logs/lin_ff_right.txt</member>
<member id="traj_type">e</member> <member id="traj_type">e</member>
<member id="L">0.6</member> <member id="L">0.6</member>
<member id="cutoff_freq">0.5</member> <member id="cutoff_freq">0.5</member>
......
...@@ -6,6 +6,9 @@ ...@@ -6,6 +6,9 @@
#include <iostream> #include <iostream>
#include <fstream> #include <fstream>
#include <ros/package.h>
#include <regex>
template<class T> template<class T>
class ILC : public T class ILC : public T
{ {
...@@ -13,13 +16,14 @@ class ILC : public T ...@@ -13,13 +16,14 @@ class ILC : public T
public: public:
ILC(const string& name) ILC(const string& name)
: T(name), : T(name),
filename_in_{""} filename_ff_{""}
{ {
} }
bool init(Dict& params) override { bool init(Dict& params) override {
extract(params["filename_in"], filename_in_); extract(params["filename_ff"], filename_ff_);
filename_ff_ = decode_filename(filename_ff_);
return T::init(params); return T::init(params);
} }
...@@ -28,9 +32,9 @@ public: ...@@ -28,9 +32,9 @@ public:
feedforward_.clear(); feedforward_.clear();
ff_coordinates_.clear(); ff_coordinates_.clear();
std::ifstream fin(filename_in_, std::ios::out); std::ifstream fin(filename_ff_, std::ios::out);
if (!fin.is_open()){ if (!fin.is_open()){
pi_error("Unable to load file: {}", filename_in_); pi_error("Unable to load file: {}", filename_ff_);
return true; return true;
} }
string line; string line;
...@@ -106,7 +110,7 @@ protected: ...@@ -106,7 +110,7 @@ protected:
coordinate, coordinate,
res, res,
it->second, it->second,
res_ff); res_ff);
res = res_ff; res = res_ff;
} }
} else { } else {
...@@ -115,8 +119,47 @@ protected: ...@@ -115,8 +119,47 @@ protected:
return res; return res;
} }
string decode_filename(const string file_name)
{
/*
* replace '{time|rospkg (...)}' accordingly
*/
std::regex time_regex("\\{(\\w*) ([^\\}]*)\\}");
// Get time now, that it does not differ within matches
auto t = std::time(nullptr);
auto tm = *std::localtime(&t);
std::string out;
std::string::const_iterator it = file_name.cbegin(), end = file_name.cend();
for (std::smatch match; std::regex_search(it, end, match, time_regex); it = match[0].second)
{
out += match.prefix();
if (match[1].str().compare("time") == 0) {
std::ostringstream oss;
string match_timeformat = match[2].str();
oss << std::put_time(&tm, match_timeformat.c_str());
auto time_replace = oss.str();
out += time_replace; // replace here
}
else if (match[1].str().compare("rospkg") == 0) {
out += ros::package::getPath(match[2].str()); // replace with rospackage path
}
else {
pi_warn_named("controller_ilc", "Replacement for '{}' in '{}' not defined!", match[1].str(), match.str());
out += match.str(); // no replace here
}
}
out.append(it, end);
return out;
}
protected: protected:
string filename_in_; string filename_ff_;
Tick start_; Tick start_;
std::map<int, std::map<string, double>> feedforward_; // maps tick in ms -> object that maps coordinate -> feedforward value std::map<int, std::map<string, double>> feedforward_; // maps tick in ms -> object that maps coordinate -> feedforward value
......
...@@ -33,6 +33,9 @@ ...@@ -33,6 +33,9 @@
<rosparam param="wrench_noise_std">0</rosparam> <rosparam param="wrench_noise_std">0</rosparam>
</node> </node>
<!-- republish the joint velocities from a C++ node (https://gitlab.cc-asp.fraunhofer.de/pitasc/pitasc/-/issues/367) -->
<node name="joint_vel_republisher" pkg="microsim" type="joint_vel_republisher_node" output="screen" />
<!-- <param name="robot_description" command="$(find xacro)/xacro - -inorder '$(find peg_in_hole)/urdf/ur10_robotiq2F.urdf.xacro'" /> --> <!-- <param name="robot_description" command="$(find xacro)/xacro - -inorder '$(find peg_in_hole)/urdf/ur10_robotiq2F.urdf.xacro'" /> -->
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find ur_description)/urdf/ur5_robot.urdf.xacro'" /> <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find ur_description)/urdf/ur5_robot.urdf.xacro'" />
......
...@@ -11,7 +11,7 @@ ...@@ -11,7 +11,7 @@