diff --git a/.gitignore b/.gitignore index 11a7c77b043451b5d368e0aaf5a59120457d5404..dd3cc25e0aaae699c8565f2b0a5285afdf469219 100644 --- a/.gitignore +++ b/.gitignore @@ -139,3 +139,5 @@ cython_debug/ *.log *.pyc + +logs/ \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index 7cd59fbdb211219793662485146b3ef9dd49b3f6..651a47aa685a894649ab0bfc1ef197bb2a50956b 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,10 +3,12 @@ project(ilc CXX) find_package(catkin REQUIRED COMPONENTS rospy + roslib roscpp std_msgs message_generation cppitasc + pitasc pitasc_library ) @@ -17,6 +19,7 @@ find_package(Eigen REQUIRED) add_service_files( FILES TriggerILC.srv + TriggerILCMeas.srv RetimeILC.srv RetimeIfILC.srv ) @@ -34,7 +37,7 @@ generate_messages( catkin_package( CATKIN_DEPENDS roscpp cppitasc message_runtime pitasc_library std_msgs INCLUDE_DIRS include - LIBRARIES ilc_controllers ilc_scripts + LIBRARIES ilc_controllers ilc_scripts ilc_setpoint_generators # DEPENDS system_lib ) @@ -67,10 +70,19 @@ target_link_libraries(ilc_scripts ${catkin_LIBRARIES} ) +add_library(ilc_setpoint_generators + src/ilc/raising_cosine_setpoint_generator_ilc.cpp +) +add_dependencies(ilc_setpoint_generators ${catkin_EXPORTED_TARGETS}) +add_dependencies(ilc_setpoint_generators ${${PROJECT_NAME}_EXPORTED_TARGETS}) +target_link_libraries(ilc_setpoint_generators + ${catkin_LIBRARIES} +) install(TARGETS ilc_controllers ilc_scripts + ilc_setpoint_generators ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} diff --git a/README.md b/README.md index 6ca8f073d5e962604d144ede69e454631a30b13d..10d50972a62395904b0b337a2e2e4c6cc942bd11 100644 --- a/README.md +++ b/README.md @@ -1,12 +1,62 @@ # ilc +#P +#Paths which need to be adapted are: +#1: ilc_rosproxy.py data_path in bringup-****.launch +#2: Paths in ilc_skills and ilc_rosproxy calls in *****.xml + +#---------------------Microsim---------------------------- +#To launch a Test-run with the micosimulator: roslaunch ilc bringup_microsim.launch -rosrun ilc ilc_server.py --folder /home/victor/Data_Storage +rosrun cppitasc file_reader ilc apps/rob_ilc_lin_test.xml + +#Data can be plotted with scripts/data_visualizer.ipynb. +# Available Data is: measurements (logged by measurement_logger to path - see rob_usb_ilc.xml for path, each ilc_skill has it's own measurement_logger with respective path) +# ilc_data such as error, ff, resampled_ff, filtered_ff,.. (logged by ilc_rosproxy to path - bringup_microsim.launch for path) + + + +#----------------------Real Robot----------------------------------- +#To launch an Linear-Movement or Push Application with ilc adaption on the real robot: +roscore +#load ilc_rqt.yaml into rqt (necessary for frames) +rqt + +#launches frames and 2 ilc_rosproxy (and other stuff) +roslaunch ilc bringup_ur5.launch +#Alternative (to simulate): +roslaunch ilc bringup_microsim.launch + +#Lin: linear movement downwards until contact with ground (foam) is nearly reached: +rosrun cppitasc file_reader ilc apps/rob_ilc_lin_contact.xml + +#Push: linear movement downwards startin 1cm above foam until 10 Newton are reached: +rosrun cppitasc file_reader ilc apps/rob_ilc_lin_contact.xml + +#Lin and Push: Lin and Push in sequential execution (TODO: not yet with setpoint_gen version- skills need to be adapted) +rosrun cppitasc file_reader ilc apps/rob_ilc_lin_force.xml + +#Data can be plotted with scripts/data_visualizer.ipynb. +# Available Data is: measurements (logged by measurement_logger to path - see rob_usb_ilc.xml for path, each ilc_skill has it's own measurement_logger with respective path) +# ilc_data such as error, ff, resampled_ff, filtered_ff,.. (logged by ilc_rosproxy to path - bringup_microsim.launch for path) + + +#------------------------USB-------------------------------- +#To launch the USB-Application with ilc: +roscore +#load ilc_rqt.yaml into rqt (necessary for frames) +rqt +#launches frames and 2 ilc_rosproxy (and other stuff) +roslaunch ilc bringup_ur5.launch -#adjust paths -rosrun cppitasc file_reader ilc apps/v_lin_log_ff-ctrl_single_axis.xml +#USB_application with ilc: +rosrun cppitasc file_reader ilc apps/rob_usb_ilc.xml +#USB_application without ilc: +rosrun cppitasc file_reader ilc apps/rob_usb_no_ilc.xml -#jupyter notebooks in scripts +#Data can be plotted with scripts/data_visualizer.ipynb. +#Available Data is: measurements (logged by measurement_logger to path - see rob_usb_ilc.xml for path, each ilc_skill has it's own measurement_logger with respective path) +#ilc_data such as error, ff, resampled_ff, filtered_ff,.. (logged by ilc_rosproxy to path - bringup_microsim.launch for path) diff --git a/apps/approach.xml b/apps/approach.xml deleted file mode 100644 index 4a6a2759dc0aa5f3e3c5e5cc63d6e63d1b5a956e..0000000000000000000000000000000000000000 --- a/apps/approach.xml +++ /dev/null @@ -1,122 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - 10 - 10 - - - wrench - - - - - - - - - - - - - - - - elbow_joint, shoulder_pan_joint, wrist_3_joint, wrist_1_joint, shoulder_lift_joint, wrist_2_joint - 1.5111723007545828, 1.6092279039888917, -3.141917966357802, 2.9014219134662995, -1.3423824706872163, -3.1797213696262983 - - - - ee_link - start_position - - - - ee_link - intermediate_position_low - - - - - target2 - ee_link - z - 0.005 - 20.0 - - - - - /home/victor/ilc_ws/src/ilc/Logs/vel_ff_2.txt - - - - - - - x, y, z - - - - - - - - - - /home/victor/ilc_ws/src/ilc/Logs/approach_force_0.csv - force_chain/z - - - - - - - - ee_link - start_position - - - - ilc_service - /home/victor/ilc_ws/src/ilc/Logs/approach_force_0.csv - /home/victor/ilc_ws/src/ilc/Logs/vel_ff_2.txt - /home/victor/ilc_ws/src/ilc/Logs/trajectory.txt - cos - 0.0001 - 0.9 - - - - - - succeeded - skill_ptp - - - - - - - - - diff --git a/apps/force.xml b/apps/force.xml deleted file mode 100644 index 6a6ab05c9732c2673fdd155603aa4e4f51d7fc5c..0000000000000000000000000000000000000000 --- a/apps/force.xml +++ /dev/null @@ -1,132 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - 10 - 10 - - - wrench - - - - - - - - - - - - - - - - elbow_joint, shoulder_pan_joint, wrist_3_joint, wrist_1_joint, shoulder_lift_joint, wrist_2_joint - 1.5111723007545828, 1.6092279039888917, -3.141917966357802, 2.9014219134662995, -1.3423824706872163, -3.1797213696262983 - - - - ee_link - start_position - - - - ee_link - intermediate_position_low - - - - ee_link - target2 - z - 0.02 - 20.0 - - - - - - ee_link - target2 - z - 100 - 0.0005 - 7 - - - - /home/victor/ilc_ws/src/ilc/Logs/force_ff_2.txt - - - - - - - x, y, z - - - - - - - - - - - /home/victor/ilc_ws/src/ilc/Logs/force_logger_0.csv - force_chain/x,force_chain/y,force_chain/z - - - - - - - ee_link - start_position - - - - ilc_service - /home/victor/ilc_ws/src/ilc/Logs/force_logger_0.csv - /home/victor/ilc_ws/src/ilc/Logs/force_ff_2.txt - /home/victor/ilc_ws/src/ilc/Logs/trajectory.txt - cos - 0.00005 - 0.9 - - - - - - succeeded - skill_ptp - - - - - - - - - - - - diff --git a/apps/lin.xml b/apps/lin.xml deleted file mode 100644 index 522e6625153f5013a005fd0604359cf02480797b..0000000000000000000000000000000000000000 --- a/apps/lin.xml +++ /dev/null @@ -1,133 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - 10 - 10 - - - wrench - - - - - - - - - - - - - - - - - - - - - ee_link - start_position - - 5 - 15 - - - - 20 - - - - - - - - - 0.5 - 50 - - - - /home/victor/ilc_ws/src/ilc/etc/vel_ff.txt - /home/victor/ilc_ws/src/ilc/Logs/error_lin.csv - - - - - - - x, y, z - - - - - - - - - - - - - - - - a, b, c - - - - - ee_link - intermediate_position_low - 200 - - - - 20 - - - - - - - - - ee_link - start_position - - - - - 20 - - - - - - - - - - diff --git a/apps/lin_1D.xml b/apps/lin_1D.xml deleted file mode 100644 index d3f13d18f2d2b36ff88feedd09841fa5e18f8d66..0000000000000000000000000000000000000000 --- a/apps/lin_1D.xml +++ /dev/null @@ -1,142 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - 10 - 10 - - - wrench - - - - - - - - - - - - - - - - elbow_joint, shoulder_pan_joint, wrist_3_joint, wrist_1_joint, shoulder_lift_joint, wrist_2_joint - 1.5111723007545828, 1.6092279039888917, -3.141917966357802, 2.9014219134662995, -1.3423824706872163, -3.1797213696262983 - - - - ee_link - start_position - - - - ee_link - intermediate_position_low - - 5 - 5 - - - - - - - - - - - - - - - /home/victor/ilc_ws/src/ilc/Logs/lin_ff_2_1D.txt - - - - - - - - x, y, z - - - - - - - - - - - - - - - - a, b, c - - - - - - - - - - - /home/victor/ilc_ws/src/ilc/Logs/logger_0_1D.csv - target_to_tool/z - - - - - - ee_link - start_position - - - - ilc_service - /home/victor/ilc_ws/src/ilc/Logs/logger_0_1D.csv - /home/victor/ilc_ws/src/ilc/Logs/lin_ff_2_1D.txt - /home/victor/ilc_ws/src/ilc/Logs/trajectory_1D.txt - cos - 0.3 - 0.5 - - - - - - succeeded - skill_ptp - - - - - - - - - diff --git a/apps/lin_1D_retime.xml b/apps/lin_1D_retime.xml deleted file mode 100644 index 3e991bdda77d7d7ef6df27009ae89cd6cfe9b089..0000000000000000000000000000000000000000 --- a/apps/lin_1D_retime.xml +++ /dev/null @@ -1,158 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - 10 - 10 - - - wrench - - - - - - - - - - - - - - - - elbow_joint, shoulder_pan_joint, wrist_3_joint, wrist_1_joint, shoulder_lift_joint, wrist_2_joint - 1.5111723007545828, 1.6092279039888917, -3.141917966357802, 2.9014219134662995, -1.3423824706872163, -3.1797213696262983 - - - - ee_link - start_position - - - - ee_link - intermediate_position_low - - 5 - 5 - - - - - - - - - - - - - - - /home/victor/ilc_ws/src/ilc/Logs/lin_ff_2_1D.txt - - - - - - - - x, y, z - - - - - - - - - - - - - - - - a, b, c - - - - - - - - - - - /home/victor/ilc_ws/src/ilc/Logs/logger_0_1D.csv - target_to_tool/z - - - - - - ee_link - start_position - - - - ilc_service - /home/victor/ilc_ws/src/ilc/Logs/logger_0_1D.csv - /home/victor/ilc_ws/src/ilc/Logs/lin_ff_2_1D.txt - /home/victor/ilc_ws/src/ilc/Logs/trajectory_1D.txt - cos - 0.3 - 0.5 - - - - - ilc_retime_if - false - 0.9 - 0.5 - True - 1 - - - - - - - - succeeded - skill_ptp - - - - - - - - - diff --git a/apps/lin_log_ff-ctrl.xml b/apps/lin_log_ff-ctrl.xml deleted file mode 100644 index 9610653aea85eaebd319b4457cba6b65efda51d8..0000000000000000000000000000000000000000 --- a/apps/lin_log_ff-ctrl.xml +++ /dev/null @@ -1,135 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - 10 - 10 - - - wrench - - - - - - - - - - - - - - - - - elbow_joint, shoulder_pan_joint, wrist_3_joint, wrist_1_joint, shoulder_lift_joint, wrist_2_joint - 1.5111723007545828, 1.6092279039888917, -3.141917966357802, 2.9014219134662995, -1.3423824706872163, -3.1797213696262983 - - - - ee_link - start_position - - - - ee_link - intermediate_position_low - - - - - - - - - - - - - - - /home/ipa325/logs/lin_ff_2.txt - - - - - - - x, y, z - - - - - - - - - - - - - - - - a, b, c - - - - - - - - - - - /home/ipa325/logs/logger_0.csv - - - - - - - ee_link - start_position - - - - ilc_service - filename_in - filename_out - - - - - - succeeded - ilc_skill - - - - - - - - - diff --git a/apps/rob_ilc_force.xml b/apps/rob_ilc_force.xml new file mode 100644 index 0000000000000000000000000000000000000000..52954c20b6616b79780873ce37805c93feea6a4f --- /dev/null +++ b/apps/rob_ilc_force.xml @@ -0,0 +1,202 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 10 + + + wrench + + + + + + + + + + + + + + + + + + + + + elbow_joint, shoulder_lift_joint, shoulder_pan_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint + -1.9502881209002894, -1.6445744673358362, -1.508630100880758, -1.117523495350973, 1.5609349012374878, -0.743263069783346 + + + + ee_link + 1_above_contact + + 0.5 + 5 + + + + + + + + 1 + + + + ee_link + kms40 + z + 10.0 + 0.0005 + 7 + + 10 + 10 + 60 + 0.9 + + + + + + + + + + + + + + + + + x, y, z + a, b, c + + + + + + + + {rospkg ilc}/logs/{time %F}/force_ff_2.txt + + + + + + + x, y, z + + + + + + + + + + + {rospkg ilc}/logs/{time %F}/force_logger_0.csv + force_chain/x,force_chain/y,force_chain/z + + + + + + + /home/ipa325/Documents/ilc_Meas/force_meas_{time %F_%H_%M_%S}.csv + + + + + + + + 1 + + + + + + + + + ee_link + 1_above_contact + + + + ilc_rosproxy + ilc_service + {rospkg ilc}/logs/{time %F}/force_logger_0.csv + {rospkg ilc}/logs/{time %F}/force_ff_2.txt + e + 0.00005 + 0.9 + 25 + + + + ilc_rosproxy + ilc_retime_if + false + 0.9 + 150 + True + 1 + + + + + + + succeeded + skill_ptp + + + + + + + + + + + diff --git a/apps/rob_ilc_lin_contact.xml b/apps/rob_ilc_lin_contact.xml new file mode 100644 index 0000000000000000000000000000000000000000..a8995250a7e690c3fbf3d4ed149e0e5153e6a744 --- /dev/null +++ b/apps/rob_ilc_lin_contact.xml @@ -0,0 +1,210 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 10 + 10 + + + wrench + + + + + + + + + + + + + + + + + + + + elbow_joint, shoulder_lift_joint, shoulder_pan_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint + -1.8796876112567347, -1.6048505941974085, -1.50897723833193, -1.227774445210592, 1.5608389377593994, -0.742410961781637 + + + + ee_link + above_contact + + 0.5 + 10 + + + + + + + + ee_link + contact + + 10 + 0.9 + + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 + + + 15 + + + + 0.5 + 10 + + + + + + + + + + + + + + + + + x, y, z + a, b, c + + + + + + + {rospkg ilc}/logs/{time %F}/lin_ff_2.txt + + + + + + + + x, y, z + + + + + + + + + + + + + + + + a, b, c + + + + + + + + + + {rospkg ilc}/logs/{time %F}/logger_0.csv + + + + + + + /home/ipa325/Documents/ilc_Meas/lin_meas_{time %F_%H_%M_%S}.csv + + + + + + + 1 + + + + + + + + ee_link + above_contact + + 0.5 + 10 + + + + ilc_rosproxy1 + ilc_service + {rospkg ilc}/logs/{time %F}/logger_0.csv + {rospkg ilc}/logs/{time %F}/lin_ff_2.txt + const + 0.6 + 0.5 + 0 + + + + ilc_rosproxy1 + ilc_retime_if + retime_setpoint_ilc + false + + 0.02 + True + 1 + + + + + + succeeded + skill_ptp + + + + + + + + + + + + diff --git a/apps/rob_ilc_lin_force.xml b/apps/rob_ilc_lin_force.xml new file mode 100644 index 0000000000000000000000000000000000000000..e66d71fbb31bb981540d3d3a841ad9d482fb7378 --- /dev/null +++ b/apps/rob_ilc_lin_force.xml @@ -0,0 +1,230 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + 20 + 10 + + + wrench + + + + + + + + + + + + + + + + + + + + elbow_joint, shoulder_lift_joint, shoulder_pan_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint + -1.8836291472064417, -1.6066244284259241, -1.5089896360980433, -1.2221058050738733, 1.5608270168304443, -0.7423265616046351 + + + + ee_link + 5_above_contact + + 0.5 + 10 + + + + ee_link + contact + + 0.5 + 10 + + + + + + + + + + + + + + + + {rospkg ilc}/logs/{time %F}/lin_ff_2.txt + + + + + + + x, y, z + + + + + + + + + + + + + + + + a, b, c + + + + + + + + + + + {rospkg ilc}/logs/{time %F}/logger_0.csv + + + + + + 1 + + + + ee_link + kms40 + z + 10.0 + 0.0005 + 7 + + + + {rospkg ilc}/logs/{time %F}/force_ff_2.txt + + + + + + + x, y, z + + + + + + + + + + + {rospkg ilc}/logs/{time %F}/force_logger_0.csv + force_chain/x,force_chain/y,force_chain/z + + + + + + + 1 + + + + ee_link + 5_above_contact + + 0.5 + 10 + + + + ilc_rosproxy + ilc_service + {rospkg ilc}/logs/{time %F}/logger_0.csv + {rospkg ilc}/logs/{time %F}/lin_ff_2.txt + cos + 0.6 + 0.5 + 25 + + + ilc_rosproxy + ilc_retime_if + false + 0.9 + 0.03 + True + 1 + + + + ilc_rosproxy2 + ilc_service + {rospkg ilc}/logs/{time %F}/force_logger_0.csv + {rospkg ilc}/logs/{time %F}/force_ff_2.txt + cos + 0.00005 + 0.9 + 25 + + + ilc_rosproxy2 + ilc_retime_if + false + 0.9 + 15 + True + 1 + + + + + + succeeded + skill_ptp + + + + + + + + + + + + diff --git a/apps/v_lin_log_ff-ctrl.xml b/apps/rob_ilc_lin_test.xml similarity index 57% rename from apps/v_lin_log_ff-ctrl.xml rename to apps/rob_ilc_lin_test.xml index b97f317ee3fea9042bdcf16687b0813a10ba062b..66506775991f88a0e61246b844fd841c60c4d42e 100644 --- a/apps/v_lin_log_ff-ctrl.xml +++ b/apps/rob_ilc_lin_test.xml @@ -4,10 +4,14 @@ - - + + + + + + @@ -21,7 +25,7 @@ - 10 + 20 10 @@ -38,11 +42,15 @@ + + - - - elbow_joint, shoulder_pan_joint, wrist_3_joint, wrist_1_joint, shoulder_lift_joint, wrist_2_joint - 1.5111723007545828, 1.6092279039888917, -3.141917966357802, 2.9014219134662995, -1.3423824706872163, -3.1797213696262983 + + + + + elbow_joint, shoulder_lift_joint, shoulder_pan_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint + -0.8761704603778284, -1.705047909413473, -1.5110262076007288, -2.1497629324542444, 1.5639780759811401, -0.7346790472613733 @@ -50,29 +58,56 @@ start_position + + ee_link + goal_position + + + 10 + 0.9 + ee_link - intermediate_position_low + start_position + + 0.5 + 10 + - + + + + + + + + + + + + x, y, z + a, b, c + + + - /home/victor/ilc_ws/src/ilc/Logs/lin_ff_2.txt + {rospkg ilc}/logs/{time %F}/ilc_ff.csv - + x, y, z @@ -80,7 +115,7 @@ - + @@ -88,7 +123,7 @@ - + a, b, c @@ -96,50 +131,65 @@ + - - /home/victor/ilc_ws/src/ilc/Logs/logger_0.csv - + {rospkg ilc}/logs/{time %F}/error.csv + + + + + + + {rospkg ilc}/logs/{time %F}/meas_lin/target_to_tool_{time %F_%H_%M_%S}.csv + + + ee_link start_position + ilc_rosproxy ilc_service - /home/victor/ilc_ws/src/ilc/Logs/logger_0.csv - /home/victor/ilc_ws/src/ilc/Logs/lin_ff_2.txt - /home/victor/ilc_ws/src/ilc/Logs/trajectory.txt - cos - 0.3 + {rospkg ilc}/logs/{time %F}/error.csv + {rospkg ilc}/logs/{time %F}/ilc_ff.csv + const + 0.6 0.5 + 0 + ilc_rosproxy ilc_retime_if + retime_setpoint_ilc false - 0.9 - 0.2 + + 0 True 1 - + succeeded - ilc_skill + skill_ptp + + + diff --git a/apps/test_retime.xml b/apps/rob_simple_test.xml similarity index 71% rename from apps/test_retime.xml rename to apps/rob_simple_test.xml index 6c16202eae7dcc3b06221da651123b8c4e97bc09..b9fae9dbb80fbff37dd2ea7e9cba1624015901f1 100644 --- a/apps/test_retime.xml +++ b/apps/rob_simple_test.xml @@ -5,7 +5,6 @@ - @@ -23,7 +22,7 @@ - 10 + 0.5 10 @@ -46,18 +45,8 @@ ee_link - start_position - - - - ilc_retime - 100 - True - 0 - - + 1_above_contact - diff --git a/apps/rob_track_marker.xml b/apps/rob_track_marker.xml new file mode 100644 index 0000000000000000000000000000000000000000..489b043d81f387643b236960e6e511275cb2e085 --- /dev/null +++ b/apps/rob_track_marker.xml @@ -0,0 +1,73 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + 0.5 + 10 + + + wrench + + + + + + + + + + + + + + + + + + + elbow_joint, shoulder_lift_joint, shoulder_pan_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint + -1.9282563368426722, -1.6304424444781702, -1.5087736288653772, -1.1538098494159144, 1.560910940170288, -0.7429631392108362 + + + + + ee_link + above_ground + + + + ee_link + marker + + 0.5 + 5 + + + + + + + + diff --git a/apps/rob_usb_ilc.xml b/apps/rob_usb_ilc.xml new file mode 100644 index 0000000000000000000000000000000000000000..3478c81f359bb88d6ad06e2dc03945d924f8bf4c --- /dev/null +++ b/apps/rob_usb_ilc.xml @@ -0,0 +1,737 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0.1 + 10 + + + wrench + + + + + + + + + + + + + + + + + + + + + + + + + ee_link + above_usb_insert + + + + True + + + + ee_link + usb_in_pos_0 + + + + 10 + 50 + + + + 80 + + + + ee_link + usb_pre_in_pos_0 + + + + + + 1 + + + + ee_link + kms40_low + y + 10 + 0.0005 + 7 + + 1.0 + 50 + + 10 + 10 + 30 + 0.9 + + + + + + + + + + + + + + + + + x, y, z + a, b, c + + + + + + + + {rospkg ilc}/logs/{time %F}/force_ff_in.txt + + + + + + + x, y, z + + + + + + + + + + + {rospkg ilc}/logs/{time %F}/force_logger_in.csv + force_chain/x,force_chain/y,force_chain/z + + + + + + + /home/ipa325/Documents/ilc_Meas/force_in_meas_{time %F_%H_%M_%S}.csv + + + + + + + kms40_low + ee_link + z + 0.00015 + absolute_greater + + + + + + 80 + + + + + ee_link + usb_pre_in_pos_0 + 10 + 0.9 + + 0.5 + 10 + + + + + + + + + + + + + + + + + x, y, z + a, b, c + + + + + + + {rospkg ilc}/logs/{time %F}/lin_ff_out.txt + + + + + + + + x, y, z + + + + + + + + + + + + + + + + a, b, c + + + + + + + + + + {rospkg ilc}/logs/{time %F}/logger_lin_out.csv + + + + + + + /home/ipa325/Documents/ilc_Meas/lin_out_meas_{time %F_%H_%M_%S}.csv + + + + + + + ee_link + usb_pre_in_pos_0_left + + 10 + 0.9 + + 0.5 + 10 + + + + + + + + + + + + + + + + + x, y, z + a, b, c + + + + + + + {rospkg ilc}/logs/{time %F}/lin_ff_left.txt + + + + + + + + x, y, z + + + + + + + + + + + + + + + + a, b, c + + + + + + + + + + {rospkg ilc}/logs/{time %F}/logger_lin_left.csv + + + + + + + /home/ipa325/Documents/ilc_Meas/lin_left_meas_{time %F_%H_%M_%S}.csv + + + + + + + + + + ee_link + usb_pre_in_pos_0 + + 10 + 0.9 + + 0.5 + 10 + + + + + + + + + + + + + + + + + x, y, z + a, b, c + + + + + + + {rospkg ilc}/logs/{time %F}/lin_ff_right.txt + + + + + + + + x, y, z + + + + + + + + + + + + + + + + a, b, c + + + + + + + + + + {rospkg ilc}/logs/{time %F}/logger_lin_right.csv + + + + + + + /home/ipa325/Documents/ilc_Meas/lin_right_meas_{time %F_%H_%M_%S}.csv + + + + + + + 1 + + + + + + 0.1 + + + succeeded + start_skill + + + + + ilc_rosproxy1 + ilc_service + {rospkg ilc}/logs/{time %F}/force_logger_in.csv + {rospkg ilc}/logs/{time %F}/force_ff_in.txt + e + 0.00005 + 0.9 + 25 + + + ilc_rosproxy1 + ilc_retime_if + false + 0.9 + 150 + True + 1 + + + + + ilc_rosproxy2 + ilc_service + {rospkg ilc}/logs/{time %F}/logger_lin_out.csv + {rospkg ilc}/logs/{time %F}/lin_ff_out.txt + e + 0.6 + 0.5 + 5 + + + ilc_rosproxy2 + ilc_retime_if + false + 0.9 + 0.03 + True + 1 + + + + ilc_rosproxy3 + ilc_service + {rospkg ilc}/logs/{time %F}/logger_lin_left.csv + {rospkg ilc}/logs/{time %F}/lin_ff_left.txt + e + 0.6 + 0.5 + 5 + + + ilc_rosproxy3 + ilc_retime_if + false + 0.9 + 0.03 + True + 1 + + + + + + ilc_rosproxy6 + ilc_service + {rospkg ilc}/logs/{time %F}/logger_lin_right.csv + {rospkg ilc}/logs/{time %F}/lin_ff_right.txt + e + 0.6 + 0.5 + 5 + + + ilc_rosproxy6 + ilc_retime_if + false + 0.9 + 0.03 + True + 1 + + + + + + + + + + + diff --git a/apps/rob_usb_no_ilc.xml b/apps/rob_usb_no_ilc.xml new file mode 100644 index 0000000000000000000000000000000000000000..f88159597f1ae39597305d04c55b79c901de4ce7 --- /dev/null +++ b/apps/rob_usb_no_ilc.xml @@ -0,0 +1,162 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 10 + 10 + + + wrench + + + + + + + + + + + + + + + + + + + + + + + + + + False + + + + 13 + 50 + + + + 100 + + + + ee_link + usb_pre_in_pos_0 + + + + ee_link + kms40_low + y + 10 + 0.0005 + 7 + + 1.0 + 20 + + + + kms40_low + ee_link + z + 0.00015 + absolute_greater + + + + + + + ee_link + kms40_low + y + 0 + 0.0005 + 7 + + 1.0 + 20 + + + kms40_low + ee_link + z + 0.00015 + absolute_greater + + + + + + ee_link + usb_pre_in_pos_0_left + + + + ee_link + usb_pre_in_pos_0_high + + + + ee_link + usb_pre_in_pos_0_left + + succeeded + start_skill + + + + + + + + + + diff --git a/apps/v_lin_log_ff-ctrl_single_axis.xml b/apps/v_lin_log_ff-ctrl_single_axis.xml deleted file mode 100644 index 28f3278624ee4142ccc1c6580c0191f7f90d0cab..0000000000000000000000000000000000000000 --- a/apps/v_lin_log_ff-ctrl_single_axis.xml +++ /dev/null @@ -1,142 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - 10 - 10 - - - wrench - - - - - - - - - - - - - - - - elbow_joint, shoulder_pan_joint, wrist_3_joint, wrist_1_joint, shoulder_lift_joint, wrist_2_joint - 1.5111723007545828, 1.6092279039888917, -3.141917966357802, 2.9014219134662995, -1.3423824706872163, -3.1797213696262983 - - - - ee_link - start_position - - - - ee_link - intermediate_position_low - - 5 - 5 - - - - - - - - - - - - - - - /home/victor/ilc_ws/src/ilc/Logs/lin_ff_2_1D.txt - - - - - - - - x, y, z - - - - - - - - - - - - - - - - a, b, c - - - - - - - - - - - /home/victor/ilc_ws/src/ilc/Logs/logger_0_1D.csv - target_to_tool/z - - - - - - ee_link - start_position - - - - ilc_service - /home/victor/ilc_ws/src/ilc/Logs/logger_0_1D.csv - /home/victor/ilc_ws/src/ilc/Logs/lin_ff_2_1D.txt - /home/victor/ilc_ws/src/ilc/Logs/trajectory_1D.txt - cos - 0.3 - 0.5 - - - - - - succeeded - ilc_skill - - - - - - - - - diff --git a/include/controller_ilc.hpp b/include/controller_ilc.hpp index 7954921151f048c25b852e83ce1b464d3a70a52e..0b011a81bcc3489c99ea7da7ddf35a065f09f7c6 100644 --- a/include/controller_ilc.hpp +++ b/include/controller_ilc.hpp @@ -6,6 +6,9 @@ #include #include +#include +#include + template class ILC : public T { @@ -13,13 +16,14 @@ class ILC : public T public: ILC(const string& name) : T(name), - filename_in_{""} + filename_ff_{""} { } 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); } @@ -28,9 +32,9 @@ public: feedforward_.clear(); ff_coordinates_.clear(); - std::ifstream fin(filename_in_, std::ios::out); + std::ifstream fin(filename_ff_, std::ios::out); if (!fin.is_open()){ - pi_error("Unable to load file: {}", filename_in_); + pi_error("Unable to load file: {}", filename_ff_); return true; } string line; @@ -106,7 +110,7 @@ protected: coordinate, res, it->second, - res_ff); + res_ff); res = res_ff; } } else { @@ -115,8 +119,47 @@ protected: 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: - string filename_in_; + string filename_ff_; Tick start_; std::map> feedforward_; // maps tick in ms -> object that maps coordinate -> feedforward value diff --git a/launch/bringup_microsim.launch b/launch/bringup_microsim.launch index 6fb88173daf7011b06cbd6f5ec1eca20ae4f4076..e8ece4f1b79d2d50600067e1c7485dd9135b0522 100644 --- a/launch/bringup_microsim.launch +++ b/launch/bringup_microsim.launch @@ -9,32 +9,34 @@ --> - - - - - - - - - - - - shoulder_pan_joint: 0.0 - shoulder_lift_joint: -1.57 - elbow_joint: 1.57 - wrist_1_joint: -1.57 - wrist_2_joint: -1.57 - wrist_3_joint: -1.57 - - 0 - 0 - 0 - - - - + + + + + + + + + + + + shoulder_pan_joint: 0.0 + shoulder_lift_joint: -1.57 + elbow_joint: 1.57 + wrist_1_joint: -1.57 + wrist_2_joint: -1.57 + wrist_3_joint: -1.57 + + 0 + 0 + 0 + + + + + + - + - - - - + + + + args="-0.05 0.6 0.3 1 0 0 0 world intermediate_position_v_high 100"/> + args="-0.05 0.6 0.2 1 0 0 0 world intermediate_position_high2 100"/> + args="-0.05 0.6 0.03 1 0 0 0 world intermediate_position_high 100"/> @@ -86,10 +89,13 @@ - - - 8 - - + + + + + + diff --git a/launch/bringup_ur5.launch b/launch/bringup_ur5.launch index 0c901d73e676f916c89dccc6c6218440d549f212..921ecdbf1a39643f048f4758ede6fd04a8d39202 100644 --- a/launch/bringup_ur5.launch +++ b/launch/bringup_ur5.launch @@ -20,16 +20,42 @@ - + - 8 - + /home/ipa325/Documents/ilc_Datas + + + + 192.168.0.30 + 1000 + kms40_low + + + + + + + + + + + 8 + /home/ipa325/Documents/test/Lin + + + + 8 + /home/ipa325/Documents/test/Force + + diff --git a/launch/bringup_ur5_usb.launch b/launch/bringup_ur5_usb.launch new file mode 100644 index 0000000000000000000000000000000000000000..80a2da1af2010147b114e3265ec4c9e4e1ff575e --- /dev/null +++ b/launch/bringup_ur5_usb.launch @@ -0,0 +1,83 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 8 + /home/ipa325/Documents/ilc_Datas + + + + 192.168.0.30 + 1000 + kms40_low + + + + + + + + + + + + + + + 8 + /home/ipa325/Documents/USB/push_in + + + + 8 + /home/ipa325/Documents/USB/lin_out + + + + 8 + /home/ipa325/Documents/USB/lin_left + + + + 8 + /home/ipa325/Documents/USB/lin_up + + + + 8 + /home/ipa325/Documents/USB/lin_down + + + + 8 + /home/ipa325/Documents/USB/lin_right + + + diff --git a/models/ilc_controllers.xml b/models/ilc_controllers.xml index 230b740a2854512080338ef6bd6dc3de0d616f7f..4951ac9924a9a1e50dc60cb921a40a297da39fc3 100755 --- a/models/ilc_controllers.xml +++ b/models/ilc_controllers.xml @@ -11,7 +11,7 @@ ilc_controllers PControllerILC - gain, limit, filename_in, data_source, setpoint_generator, coordinates, prefix + gain, limit, filename_ff, data_source, setpoint_generator, coordinates, prefix @@ -24,7 +24,7 @@ Limit 1.0 - + Filename to be loaded for feedforward @@ -50,13 +50,13 @@ ilc_controllers VelocityControllerILC - filename_in, data_source, setpoint_generator, coordinates, prefix, velocities + filename_ff, data_source, setpoint_generator, coordinates, prefix, velocities - + Filename to be loaded for feedforward @@ -84,13 +84,13 @@ ilc_controllers FirstOrderImpedanceILC - filename_in, compliance, f0_hz, update_rate, data_source, setpoint_generator, coordinates, prefix + filename_ff, compliance, f0_hz, update_rate, data_source, setpoint_generator, coordinates, prefix - + Filename to be loaded for feedforward @@ -123,6 +123,49 @@ + + + Controller with smoothly saturated output. The controller transfer function is realized using a tanh. + The approx. linear steepness of the tanh is parametrized by defining the gain factor 'P_lin' at 90% of 'max_error'. + + + ilc_controllers + TanHControllerILC + filename_ff, max_output, P_lin, update_rate, data_source, setpoint_generator, coordinates, prefix + + + + + + Filename to be loaded for feedforward + + + + + + + + + + + + + Upper bound on the output value (or L2 norm). + + + + + Error (L2 norm) at which 90% of max_output is returned. + + + + + Equivalent proportional controller in linear region. + + + + + diff --git a/models/ilc_scripts.xml b/models/ilc_scripts.xml index d976d3f7dcf709115777b30f2b84e2a78e6c5b69..42721d774a9101e2a17cd97369e7f0e93b9155d8 100755 --- a/models/ilc_scripts.xml +++ b/models/ilc_scripts.xml @@ -6,7 +6,7 @@ - Triggers a ros script to generate a file including ilc feedforward (file_out) based on given error loggings (file_in) + Triggers a ros script to generate a file including ilc feedforward (filename_ff) based on given error loggings (filename_error) ilc_scripts @@ -15,18 +15,14 @@ - + Filename to be loaded for feedforward - + Filename to be generated for feedforward - - Filename to be generated/loaded for trajectory - - trajectory type to be used @@ -39,6 +35,9 @@ cutoff_freq to be used in lowpass filter + + time_shift +