Hzk

.gitignore

@@ -6,6 +6,7 @@
 build/
 devel/
 logs/
+afp_logs/
 data/
 
 MUJOCO_LOG.TXT
@@ -25,4 +26,13 @@ src/afp_mjc/env/flange_model/urdf/*.STL
 
 src/afp_mjc/env/mujoco_ur5e/assets/
 
-src/il_capture/data/
+src/il_capture/data/
+
+src/il_capture/calibrate/raw_data
+
+
+src/ur5e_control/src/ur5e_control/_pycache__
+
+
+src/sensor/script/__pycache__
+src/sensor/src/sensor/__pycache__

src/afp_integration/CMakeLists.txt

@@ -11,6 +11,7 @@ find_package(catkin REQUIRED COMPONENTS
   motor_msgs
   rospy
   std_msgs
+  std_srvs
 )
 
 ## System dependencies are found with CMake's conventions
@@ -159,10 +160,14 @@ include_directories(
 
 ## Mark executable scripts (Python etc.) for installation
 ## in contrast to setup.py, you can choose the destination
-# catkin_install_python(PROGRAMS
-#   scripts/my_python_script
-#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
+catkin_install_python(PROGRAMS
+  scripts/afp_integration_node.py
+  scripts/afp_motor2_resend_node.py
+  scripts/afp_feedforward_demo.py
+  scripts/afp_experiment_logger.py
+  scripts/afp_plot_experiment.py
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
 
 ## Mark executables for installation
 ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
@@ -186,11 +191,12 @@ include_directories(
 # )
 
 ## Mark other files for installation (e.g. launch and bag files, etc.)
-# install(FILES
-#   # myfile1
-#   # myfile2
-#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-# )
+install(FILES
+  launch/afp_bringup.launch
+  launch/afp_five_nodes.launch
+  launch/afp_experiment.launch
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
+)
 
 #############
 ## Testing ##

src/afp_integration/launch/afp_bringup.launch

@@ -0,0 +1,15 @@
+<launch>
+  <arg name="tension_port" default="/dev/ttyUSB0"/>
+  <arg name="tension_baudrate" default="9600"/>
+  <arg name="tension_timeout" default="0.02"/>
+  <arg name="tension_read_interval" default="0.02"/>
+
+  <node pkg="sensor" type="tension_rs485.py" name="tension_sensor_node" output="screen">
+    <param name="port" value="$(arg tension_port)"/>
+    <param name="baudrate" value="$(arg tension_baudrate)"/>
+    <param name="timeout" value="$(arg tension_timeout)"/>
+    <param name="read_interval" value="$(arg tension_read_interval)"/>
+  </node>
+
+  <node pkg="motor_control" type="motor_can.py" name="servo_motor_ros_interface" output="screen"/>
+</launch>

src/afp_integration/launch/afp_experiment.launch

@@ -0,0 +1,56 @@
+<launch>
+  <arg name="target_tension" default="0.05"/>
+  <arg name="speed_gain" default="1.0"/>
+  <arg name="accumulate_limit" default="800.0"/>
+  <arg name="leak" default="0.999"/>
+  <arg name="sensor_timeout" default="0.2"/>
+  <arg name="deadband" default="0.02"/>
+  <arg name="tension_control_rate" default="50.0"/>
+
+  <arg name="publish_rate" default="200.0"/>
+  <arg name="motion_mode" default="sine_positive"/>
+  <arg name="linear_speed" default="0.05"/>
+  <arg name="max_linear_speed" default="0.05"/>
+  <arg name="sine_period" default="6.0"/>
+  <arg name="roller_diameter" default="0.05"/>
+  <arg name="run_duration" default="18.0"/>
+  <arg name="start_angle" default="0.0"/>
+  <arg name="direction" default="1.0"/>
+  <arg name="time_mode" default="fixed"/>
+
+  <arg name="logger_sample_rate" default="200.0"/>
+  <arg name="logger_output_dir" default="/home/hzk/AFPController/afp_logs"/>
+  <arg name="logger_file_prefix" default="paper_periodic"/>
+  <arg name="logger_file_name" default=""/>
+
+  <node pkg="afp_smoother" type="tension_pid_node.py" name="tension_pid_node" output="screen">
+    <param name="target_tension" value="$(arg target_tension)"/>
+    <param name="speed_gain" value="$(arg speed_gain)"/>
+    <param name="accumulate_limit" value="$(arg accumulate_limit)"/>
+    <param name="leak" value="$(arg leak)"/>
+    <param name="sensor_timeout" value="$(arg sensor_timeout)"/>
+    <param name="deadband" value="$(arg deadband)"/>
+    <param name="control_rate" value="$(arg tension_control_rate)"/>
+  </node>
+
+  <node pkg="afp_integration" type="afp_feedforward_demo.py" name="afp_feedforward_demo" output="screen">
+    <param name="publish_rate" value="$(arg publish_rate)"/>
+    <param name="motion_mode" value="$(arg motion_mode)"/>
+    <param name="linear_speed" value="$(arg linear_speed)"/>
+    <param name="max_linear_speed" value="$(arg max_linear_speed)"/>
+    <param name="sine_period" value="$(arg sine_period)"/>
+    <param name="roller_diameter" value="$(arg roller_diameter)"/>
+    <param name="run_duration" value="$(arg run_duration)"/>
+    <param name="start_angle" value="$(arg start_angle)"/>
+    <param name="direction" value="$(arg direction)"/>
+    <param name="time_mode" value="$(arg time_mode)"/>
+  </node>
+
+  <node pkg="afp_integration" type="afp_experiment_logger.py" name="afp_experiment_logger" output="screen">
+    <param name="sample_rate" value="$(arg logger_sample_rate)"/>
+    <param name="run_duration" value="$(arg run_duration)"/>
+    <param name="output_dir" value="$(arg logger_output_dir)"/>
+    <param name="file_prefix" value="$(arg logger_file_prefix)"/>
+    <param name="file_name" value="$(arg logger_file_name)"/>
+  </node>
+</launch>

src/afp_integration/launch/afp_five_nodes.launch

@@ -0,0 +1,88 @@
+<launch>
+  <!-- AFP bringup: start 6 nodes -->
+
+  <!-- tension sensor (RS485) -->
+  <arg name="tension_port" default="/dev/ttyUSB0"/>
+  <arg name="tension_baudrate" default="9600"/>
+  <arg name="tension_timeout" default="0.02"/>
+  <arg name="tension_read_interval" default="0.02"/>
+
+  <!-- tension PID -->
+  <arg name="target_tension" default="0.05"/>
+  <arg name="speed_gain" default="1.0"/>
+  <arg name="accumulate_limit" default="800.0"/>
+  <arg name="leak" default="0.999"/>
+  <arg name="sensor_timeout" default="0.2"/>
+  <arg name="deadband" default="0.02"/>
+  <arg name="tension_control_rate" default="50.0"/>
+
+  <!-- integration node -->
+  <arg name="integration_rate" default="200.0"/>
+
+  <!-- UR to AFP TF bridge -->
+  <arg name="base_frame" default="base"/>
+  <arg name="tcp_frame" default="tool0_controller"/>
+  <arg name="bridge_rate" default="200.0"/>
+  <arg name="roller_diameter" default="0.057"/>
+  <arg name="reduction_ratio" default="36.0"/>
+  <arg name="feedforward_gain" default="1.0"/>
+
+  <!-- motor2 resend -->
+  <arg name="motor2_resend_length_m" default="0.09"/>
+  <arg name="motor2_roller_diameter_m" default="0.02"/>
+  <arg name="motor2_direction_sign" default="1.0"/>
+  <arg name="motor2_command_rate_hz" default="20.0"/>
+  <arg name="motor2_ramp_speed_deg_s" default="90.0"/>
+  <arg name="motor2_hold_after_done" default="false"/>
+  <arg name="motor2_kp" default="800.0"/>
+  <arg name="motor2_ki" default="0.0"/>
+  <arg name="motor2_kd" default="8.0"/>
+
+  <!-- 1) sensor + 2) motor CAN -->
+  <include file="$(find afp_integration)/launch/afp_bringup.launch">
+    <arg name="tension_port" value="$(arg tension_port)"/>
+    <arg name="tension_baudrate" value="$(arg tension_baudrate)"/>
+    <arg name="tension_timeout" value="$(arg tension_timeout)"/>
+    <arg name="tension_read_interval" value="$(arg tension_read_interval)"/>
+  </include>
+
+  <!-- 3) tension PID -->
+  <node pkg="afp_smoother" type="tension_pid_node.py" name="tension_pid_node" output="screen">
+    <param name="target_tension" value="$(arg target_tension)"/>
+    <param name="speed_gain" value="$(arg speed_gain)"/>
+    <param name="accumulate_limit" value="$(arg accumulate_limit)"/>
+    <param name="leak" value="$(arg leak)"/>
+    <param name="sensor_timeout" value="$(arg sensor_timeout)"/>
+    <param name="deadband" value="$(arg deadband)"/>
+    <param name="control_rate" value="$(arg tension_control_rate)"/>
+  </node>
+
+  <!-- 4) integration layer -->
+  <node pkg="afp_integration" type="afp_integration_node.py" name="afp_integration_node" output="screen">
+    <param name="control_rate" value="$(arg integration_rate)"/>
+  </node>
+
+  <!-- 5) UR TCP displacement to motor target angle -->
+  <node pkg="afp_integration" type="ur5e2afphead_bridge.py" name="afp_motor_sync_node" output="screen">
+    <param name="base_frame" value="$(arg base_frame)"/>
+    <param name="tcp_frame" value="$(arg tcp_frame)"/>
+    <param name="update_rate" value="$(arg bridge_rate)"/>
+
+    <param name="roller_diameter" value="$(arg roller_diameter)"/>
+    <param name="reduction_ratio" value="$(arg reduction_ratio)"/>
+    <param name="feedforward_gain" value="$(arg feedforward_gain)"/>
+  </node>
+
+  <!-- 6) motor2 resend standby node -->
+  <node pkg="afp_integration" type="afp_motor2_resend_node.py" name="afp_motor2_resend_node" output="screen">
+    <param name="resend_length_m" value="$(arg motor2_resend_length_m)"/>
+    <param name="roller_diameter_m" value="$(arg motor2_roller_diameter_m)"/>
+    <param name="direction_sign" value="$(arg motor2_direction_sign)"/>
+    <param name="command_rate_hz" value="$(arg motor2_command_rate_hz)"/>
+    <param name="ramp_speed_deg_s" value="$(arg motor2_ramp_speed_deg_s)"/>
+    <param name="hold_after_done" value="$(arg motor2_hold_after_done)"/>
+    <param name="kp" value="$(arg motor2_kp)"/>
+    <param name="ki" value="$(arg motor2_ki)"/>
+    <param name="kd" value="$(arg motor2_kd)"/>
+  </node>
+</launch>

src/afp_integration/package.xml

@@ -52,12 +52,18 @@
   <build_depend>motor_msgs</build_depend>
   <build_depend>rospy</build_depend>
   <build_depend>std_msgs</build_depend>
+  <build_depend>std_srvs</build_depend>
   <build_export_depend>motor_msgs</build_export_depend>
   <build_export_depend>rospy</build_export_depend>
   <build_export_depend>std_msgs</build_export_depend>
+  <build_export_depend>std_srvs</build_export_depend>
   <exec_depend>motor_msgs</exec_depend>
   <exec_depend>rospy</exec_depend>
   <exec_depend>std_msgs</exec_depend>
+  <exec_depend>std_srvs</exec_depend>
+  <exec_depend>sensor</exec_depend>
+  <exec_depend>motor_control</exec_depend>
+  <exec_depend>afp_smoother</exec_depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->