Skip to content
GitLab
Commit 9e8852ee authored by Lorenz Halt's avatar Lorenz Halt 🔀
Browse files

Merge branch 'master' into 'master' 

new measurement script and filename decoding

See merge request ipa325_public/pitasc_components/ilc!7
parents 96ed8141 f4678aa3
Expand all Hide whitespace changes
Inline Side-by-side
.gitignore
View file @ 9e8852ee
......@@ -139,3 +139,5 @@ cython_debug/
*.log
*.pyc
logs/
\ No newline at end of file
CMakeLists.txt
View file @ 9e8852ee
......@@ -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}
......
README.md
View file @ 9e8852ee
# 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)
apps/approach.xml deleted 100644 → 0
View file @ 96ed8141
<?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/lin_log_ff-ctrl.xml deleted 100644 → 0
View file @ 96ed8141
<?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 id="ilc_skill" prototype="skill_lin">
<member id="tool_frame">ee_link</member>
<member id="target_frame">intermediate_position_low</member>
<member id="tasks.tracking.setpoint_generators">
<clone id="setpoint" prototype="constant_setpoint">
<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"/>
</clone>
</member>
<member id="tasks.tracking.controllers">
<clone id="ilc_controller" prototype="p_controller_ilc">
<member id="filename_in">/home/ipa325/logs/lin_ff_2.txt</member>
<member id="data_source">
<reference reference_id="collections.target_to_tool.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="limit" reference_id="max_linear_velocity"/>
<member id="gain" reference_id="linear_controller_gain"/>
</clone>
<!-- orientation controller -->
<clone prototype="tanh_controller">
<member id="max_output" reference_id="max_angular_velocity"/>
<member id="P_lin" reference_id="angular_controller_gain"/>
<member id="data_source">
<reference reference_id="collections.target_to_tool.chains[0]"/>
</member>
<member id="setpoint_generator">
<reference reference_id="setpoint_generators.setpoint"/>
</member>
<member id="coordinates">a, b, c</member>
<member id="prefix" reference_id="prefix"/>
</clone>
</member>
<member id="scripts">
<clone prototype="script_error_logger">
<member id="controllers">
<!-- Controller must be declared before referencing -->
<reference reference_id="tasks.tracking.controllers.ilc_controller"/>
</member>
<member id="file_name">/home/ipa325/logs/logger_0.csv</member>
<!-- <member id="filter">target_to_tool/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">filename_in</member>
<member id="file_out">filename_out</member>
</clone>
</member>
<member id="transitions">
<clone prototype="transition">
<member id="event">succeeded</member>
<member id="target">ilc_skill</member>
</clone>
</member>
</clone>
</member>
</clone>
</member>
</clone>
</pitasc>
apps/force.xml → apps/rob_ilc_force.xml
View file @ 9e8852ee
<?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="pitasc_library" file="models/skills.xml"/> -->
<include package="ilc" file="models/ilc_controllers.xml"/>
<include package="ilc" file="models/ilc_scripts.xml"/>
<include package="ilc" file="models/ilc_setpoint_gen.xml"/>
<include package="pitasc_library" file="universal_robots/ur.xml"/>
<!-- <include package="ilc" file="models/skills.xml"/> -->
<include package="snapshot_tools" file="models/scripts_gstreamer.xml"/>
</models>
<!-- Create a project -->
......@@ -19,7 +25,9 @@
<member id="environment">
<!-- Add a UR5 -->
<clone prototype="robot_ur5">
<member id="robot_driver.max_velocity">10</member>
<!-- <member id="robot_driver.max_velocity">0.02</member>
<member id="robot_driver.max_acceleration">1</member>
<member id="robot_driver.max_velocity">10</member> -->
<member id="robot_driver.max_acceleration">10</member>
<member id="components">
<clone prototype="force_sensor">
......@@ -36,49 +44,83 @@
<!-- Use the UR5 for this skill (and for its subskill) -->
<member id="robot" reference_id="environment.robot_ur5"/>
<!-- Add subskill(s) -->
<member id="skills">
<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>
<!-- Move to target1 (lin) -->
<!-- <clone prototype="skill_lin_ilc"> -->
<clone prototype="skill_lin">
<member id="tool_frame">ee_link</member>
<member id="target_frame">start_position</member>
</clone>
<clone prototype="skill_ptp">
<member id="joint_names">elbow_joint, shoulder_lift_joint, shoulder_pan_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint</member>
<member id="target_joint_state">-1.9502881209002894, -1.6445744673358362, -1.508630100880758, -1.117523495350973, 1.5609349012374878, -0.743263069783346</member>
</clone>
<clone prototype="skill_lin">
<member id="tool_frame">ee_link</member>
<member id="target_frame">intermediate_position_low</member>
<member id="target_frame">1_above_contact</member>
<member id="linear_controller_gain">0.5</member>
<member id="max_linear_velocity">5</member>
</clone>
<clone prototype="skill_guarded_approach">
<!-- <clone prototype="skill_guarded_approach">
<member id="tool_frame">ee_link</member>
<member id="control_frame">target2</member>
<member id="control_frame">kms40</member>
<member id="axes">z</member>
<member id="velocities">0.02</member>
<member id="max_force">20.0</member>
</clone>
<member id="velocities">0.005</member>
<member id="max_forces">2.0</member>
</clone> -->
<clone prototype="start_video_recorder" />
<clone id="ilc_push_settle" prototype="skill_push_settle">
<clone prototype="skill_idle_duration">
<member id ="duration"> 1 </member>
</clone>
<clone id="ilc_push_settle" prototype="skill_push_settle">
<member id="tool_frame">ee_link</member>
<member id="force.force_frame">target2</member>
<member id="force.force_frame">kms40</member>
<member id="force.axes">z</member>
<member id="force.target_forces">100</member>
<member id="force.target_forces">10.0</member>
<member id="force.compliance">0.0005</member>
<member id="force.corner_frequency">7</member>
<clone id="max_linear_velocity" prototype="float_parameter">10</clone>
<clone id="max_angular_velocity" prototype="float_parameter">10</clone>
<clone id="trajectory_duration" prototype="float_parameter">60</clone>
<clone id="retime_factor" prototype="float_parameter">0.9</clone>
<member id="force.tasks.apply_force.setpoint_generators">
<clone id="setpoint_ilc" prototype="cosine_setpoint_ILC">
<member id="data_source">
<reference reference_id="collections.force_chain.chains[0]"/>
</member>
<member id="desired" reference_id="target_forces"/>
<member id="coordinates" reference_id="coordinates"/>
<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">
<reference reference_id="max_linear_velocity"/>
<reference reference_id="max_angular_velocity"/>
</member>
<member id="symbol_groups">
<clone id="position_coordinates" prototype="string_csv">x, y, z</clone>
<clone id="orientation_coordinates" prototype="string_csv">a, b, c</clone>
</member>
</clone>
</member>
<member id="force.tasks.apply_force.controllers">
<clone id="ilc_controller" prototype="first_order_impedance_controller_ilc">
<member id="filename_in">/home/victor/ilc_ws/src/ilc/Logs/force_ff_2.txt</member>
<member id="filename_ff">{rospkg ilc}/logs/{time %F}/force_ff_2.txt</member>
<member id="data_source">
<reference reference_id="collections.force_chain.chains[0]"/>
</member>
<member id="setpoint_generator">
<reference reference_id="setpoint_generators.setpoint"/>
<reference reference_id="setpoint_generators.setpoint_ilc"/>
</member>
<member id="coordinates">x, y, z</member>
<member id="prefix" reference_id="prefix"/>
......@@ -88,30 +130,59 @@
<member id="scripts">
<clone prototype="script_error_logger">
<member id="controllers">
<!-- Controller must be declared before referencing -->
<reference reference_id="force.tasks.apply_force.controllers.ilc_controller"/>
<reference reference_id="force.tasks.apply_force.controllers.first_order_impedance_controller"/>
</member>
<member id="file_name">/home/victor/ilc_ws/src/ilc/Logs/force_logger_0.csv</member>
<member id="file_name">{rospkg ilc}/logs/{time %F}/force_logger_0.csv</member>
<member id="filter">force_chain/x,force_chain/y,force_chain/z</member>
</clone>
<clone prototype="script_measurement_logger">
<member id="provider">
<reference reference_id="force.tasks.apply_force.controllers.ilc_controller.data_source"/>
</member>
<member id="file_name">/home/ipa325/Documents/ilc_Meas/force_meas_{time %F_%H_%M_%S}.csv</member>
</clone>
</member>
</clone>
<clone prototype="skill_idle_duration">
<member id ="duration"> 1 </member>
</clone>
<clone prototype="stop_video_recorder" />
<clone prototype="skill_lin">
<member id="tool_frame">ee_link</member>
<member id="target_frame">start_position</member>
<member id="target_frame">1_above_contact</member>
<member id="scripts">
<clone prototype="script_trigger_ilc">
<member id="namespace">ilc_rosproxy</member>
<member id="service_name">ilc_service</member>
<member id="file_in">/home/victor/ilc_ws/src/ilc/Logs/force_logger_0.csv</member>
<member id="file_out">/home/victor/ilc_ws/src/ilc/Logs/force_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="filename_error">{rospkg ilc}/logs/{time %F}/force_logger_0.csv</member>
<member id="filename_ff">{rospkg ilc}/logs/{time %F}/force_ff_2.txt</member>
<member id="traj_type">e</member>
<member id="L">0.00005</member>
<member id="cutoff_freq">0.9</member>
<member id="time_shift">25</member>
</clone>
<clone prototype="script_retime_if_ilc">
<member id="namespace">ilc_rosproxy</member>
<member id="service_name">ilc_retime_if</member>
<member id="on_start">false</member>
<member id="retime_factor">0.9</member>
<member id="convergence_threshold">150</member>
<member id="reuse_ff">True</member>
<member id="reuse_scheme">1</member>
</clone>
</member>
<member id="transitions">
......@@ -123,10 +194,9 @@
</clone>
</member>
</clone>
</member>
</clone>
</pitasc>
apps/lin_1D_retime.xml → apps/rob_ilc_lin_contact.xml
View file @ 9e8852ee
......@@ -4,10 +4,15 @@
<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"/>
<include package="ilc" file="models/ilc_setpoint_gen.xml"/>
<include package="pitasc_library" file="universal_robots/ur.xml"/>
<include package="snapshot_tools" file="models/scripts_gstreamer.xml"/>
</models>
<!-- Create a project -->
......@@ -38,45 +43,76 @@
<!-- Use the UR5 for this skill (and for its subskill) -->
<member id="robot" reference_id="environment.robot_ur5"/>
<!-- Add subskill(s) -->
<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>
<!-- Move to target1 (lin) -->
<!-- <clone prototype="skill_lin_ilc"> -->
<clone prototype="skill_ptp">
<member id="joint_names">elbow_joint, shoulder_lift_joint, shoulder_pan_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint</member>