All contributions from master's thesis

README.md

@@ -109,6 +109,23 @@ then finally run the smach example
 roslaunch etasl_ros_control_examples example_smach.launch
 ```
 
+## SMACH Peg-In-Hole Example
+To be able to run the peg-in-hole example, do
+```bash
+cd ./src/
+git clone [email protected]:daglofthus/kuka_lab_setup.git
+```
+Build and source the workspace again:
+```bash
+cd ..
+catkin_make --cmake-args -DCMAKE_BUILD_TYPE=Release
+source ./devel/setup.bash
+```
+then finally run the smach example
+```
+roslaunch etasl_ros_control_examples smach_peg.launch
+```
+
 ## Acknowledgements
 
 - The expressiongraph and eTaSL projects have been developed by KU Leuven.

etasl_ros_control.rosinstall

@@ -1,7 +1,7 @@
 - git:
     local-name: etasl_ros_control
-    uri: https://github.com/tingelst/etasl_ros_control.git
-    version: master
+    uri: https://github.com/daglofthus/etasl_ros_control.git
+    version: develope
 - git:
     local-name: expressiongraph
     uri: https://github.com/eaertbel/expressiongraph.git
@@ -48,5 +48,33 @@
     version: f4e309fd
 - git:
     local-name: kuka_experimental
-    uri: https://github.com/ros-industrial/kuka_experimental.git
+    uri: https://github.com/ra-mtp-ntnu/kuka_experimental.git
+    version: velocity-interface
+- git:
+    local-name: executive_smach
+    uri: https://github.com/ros/executive_smach.git
+    version: indigo-devel
+- git:
+    local-name: executive_smach_visualization
+    uri: https://github.com/ros-visualization/executive_smach_visualization.git
     version: indigo-devel
+- git:
+    local-name: robotiq
+    uri: https://github.com/ros-industrial/robotiq.git
+    version: kinetic-devel
+- git:
+    local-name: netft
+    uri: https://github.com/fsuarez6/netft.git
+    version: master
+- git:
+    local-name: kuka_lab_setup
+    uri: https://github.com/daglofthus/kuka_lab_setup.git
+    version: master
+- git:
+    local-name: forcesensor
+    uri: https://github.com/daglofthus/forcesensor.git
+    version: master
+- git:
+    local-name: workspace
+    uri: https://github.com/daglofthus/workspace.git
+    version: master

etasl_ros_control_cmd/CMakeLists.txt

@@ -0,0 +1,21 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(etasl_ros_control_cmd)
+
+find_package(catkin)
+
+catkin_package()
+
+## Installation
+install(DIRECTORY launch config scripts
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+catkin_install_python(
+  PROGRAMS 
+  scripts/etasl_pose_publisher.py
+  scripts/jointstate_grapher.py
+  scripts/smach_task.py
+  scripts/speed_pub.py
+  scripts/zivid_publisher.py
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)

etasl_ros_control_cmd/config/rqt_vel.perspective

@@ -0,0 +1,87 @@
+{
+  "keys": {},
+  "groups": {
+    "pluginmanager": {
+      "keys": {
+        "running-plugins": {
+          "type": "repr",
+          "repr": "{u'rqt_plot/Plot': [1]}"
+        }
+      },
+      "groups": {
+        "plugin__rqt_plot__Plot__1": {
+          "keys": {},
+          "groups": {
+            "dock_widget__DataPlotWidget": {
+              "keys": {
+                "dockable": {
+                  "type": "repr",
+                  "repr": "True"
+                },
+                "parent": {
+                  "type": "repr",
+                  "repr": "None"
+                },
+                "dock_widget_title": {
+                  "type": "repr",
+                  "repr": "u'MatPlot'"
+                }
+              },
+              "groups": {}
+            },
+            "plugin": {
+              "keys": {
+                "autoscroll": {
+                  "type": "repr",
+                  "repr": "True"
+                },
+                "plot_type": {
+                  "type": "repr",
+                  "repr": "1"
+                },
+                "topics": {
+                  "type": "repr",
+                  "repr": "[u'/joint_states/velocity[3]', u'/joint_states/velocity[1]', u'/joint_states/velocity[0]', u'/joint_states/velocity[5]', u'/joint_states/velocity[4]', u'/joint_states/velocity[2]']"
+                },
+                "y_limits": {
+                  "type": "repr",
+                  "repr": "[-0.7529437659566103, 0.6887592827145149]"
+                },
+                "x_limits": {
+                  "type": "repr",
+                  "repr": "[201.98214020163167, 217.72157382965088]"
+                }
+              },
+              "groups": {}
+            }
+          }
+        }
+      }
+    },
+    "mainwindow": {
+      "keys": {
+        "geometry": {
+          "type": "repr(QByteArray.hex)",
+          "repr(QByteArray.hex)": "QtCore.QByteArray('01d9d0cb00020000000000fa0000004b0000051400000318000000fa00000067000005140000031800000000000000000690')",
+          "pretty-print": "       K       g         "
+        },
+        "state": {
+          "type": "repr(QByteArray.hex)",
+          "repr(QByteArray.hex)": "QtCore.QByteArray('000000ff00000000fd00000001000000020000041b0000029cfc0100000001fb00000042007200710074005f0070006c006f0074005f005f0050006c006f0074005f005f0031005f005f00440061007400610050006c006f007400570069006400670065007401000000000000041b0000017800ffffff0000041b0000000000000004000000040000000800000008fc00000001000000030000000100000036004d0069006e0069006d0069007a006500640044006f0063006b00570069006400670065007400730054006f006f006c0062006100720000000000ffffffff0000000000000000')",
+          "pretty-print": "                 Brqt_plot__Plot__1__DataPlotWidget                            6MinimizedDockWidgetsToolbar        "
+        }
+      },
+      "groups": {
+        "toolbar_areas": {
+          "keys": {
+            "MinimizedDockWidgetsToolbar": {
+              "type": "repr",
+              "repr": "8"
+            }
+          },
+          "groups": {}
+        }
+      }
+    }
+  }
+}

etasl_ros_control_cmd/config/rqt_wrench.perspective

@@ -0,0 +1,87 @@
+{
+  "keys": {},
+  "groups": {
+    "pluginmanager": {
+      "keys": {
+        "running-plugins": {
+          "type": "repr",
+          "repr": "{u'rqt_plot/Plot': [1]}"
+        }
+      },
+      "groups": {
+        "plugin__rqt_plot__Plot__1": {
+          "keys": {},
+          "groups": {
+            "dock_widget__DataPlotWidget": {
+              "keys": {
+                "dockable": {
+                  "type": "repr",
+                  "repr": "True"
+                },
+                "parent": {
+                  "type": "repr",
+                  "repr": "None"
+                },
+                "dock_widget_title": {
+                  "type": "repr",
+                  "repr": "u'MatPlot'"
+                }
+              },
+              "groups": {}
+            },
+            "plugin": {
+              "keys": {
+                "autoscroll": {
+                  "type": "repr",
+                  "repr": "True"
+                },
+                "plot_type": {
+                  "type": "repr",
+                  "repr": "1"
+                },
+                "topics": {
+                  "type": "repr",
+                  "repr": "[u'/etasl_controller_cmd/netft_transform/torque/y', u'/etasl_controller_cmd/netft_transform/torque/x', u'/etasl_controller_cmd/netft_transform/torque/z', u'/etasl_controller_cmd/netft_transform/force/y', u'/etasl_controller_cmd/netft_transform/force/x', u'/etasl_controller_cmd/netft_transform/force/z']"
+                },
+                "y_limits": {
+                  "type": "repr",
+                  "repr": "[-93.24041739797687, 83.25357648077065]"
+                },
+                "x_limits": {
+                  "type": "repr",
+                  "repr": "[174.19292413186733, 184.04304599761963]"
+                }
+              },
+              "groups": {}
+            }
+          }
+        }
+      }
+    },
+    "mainwindow": {
+      "keys": {
+        "geometry": {
+          "type": "repr(QByteArray.hex)",
+          "repr(QByteArray.hex)": "QtCore.QByteArray('01d9d0cb00020000000000fa0000004b0000051400000318000000fa00000067000005140000031800000000000000000690')",
+          "pretty-print": "       K       g         "
+        },
+        "state": {
+          "type": "repr(QByteArray.hex)",
+          "repr(QByteArray.hex)": "QtCore.QByteArray('000000ff00000000fd00000001000000020000041b0000029cfc0100000001fb00000042007200710074005f0070006c006f0074005f005f0050006c006f0074005f005f0031005f005f00440061007400610050006c006f007400570069006400670065007401000000000000041b0000017800ffffff0000041b0000000000000004000000040000000800000008fc00000001000000030000000100000036004d0069006e0069006d0069007a006500640044006f0063006b00570069006400670065007400730054006f006f006c0062006100720000000000ffffffff0000000000000000')",
+          "pretty-print": "                 Brqt_plot__Plot__1__DataPlotWidget                            6MinimizedDockWidgetsToolbar        "
+        }
+      },
+      "groups": {
+        "toolbar_areas": {
+          "keys": {
+            "MinimizedDockWidgetsToolbar": {
+              "type": "repr",
+              "repr": "8"
+            }
+          },
+          "groups": {}
+        }
+      }
+    }
+  }
+}

etasl_ros_control_cmd/config/smach_peg.yaml

@@ -0,0 +1,55 @@
+#Publish all joint states
+joint_state_controller:
+  type: joint_state_controller/JointStateController
+  publish_rate: 50
+  extra_joints:
+    - name: 'finger_joint'
+      position: 0.65
+
+# eTaSl example controller
+etasl_controller_cmd:
+  type: "etasl_ros_controllers/EtaslController"
+  joints:
+    - joint_a1
+    - joint_a2
+    - joint_a3
+    - joint_a4
+    - joint_a5
+    - joint_a6
+  input:
+    names:
+      - "speed"
+      - "peg1_frame"
+      - "peg2_frame"
+      - "peg3_frame"
+      - "peg4_frame"
+      - "peg5_frame"
+      - "block_frame"
+      - "netft_data"
+    types:
+      - "Scalar"
+      - "Frame"
+      - "Frame"
+      - "Frame"
+      - "Frame"
+      - "Frame"
+      - "Frame"
+      - "Wrench"
+  output:
+    names:
+      - "ee_frame"
+      - "peg_hole_1"
+      - "peg_hole_2"
+      - "peg_hole_3"
+      - "peg_hole_4"
+      - "peg_hole_5"
+      - "netft_transform"
+    types:
+      - "Frame"
+      - "Frame"
+      - "Frame"
+      - "Frame"
+      - "Frame"
+      - "Frame"
+      - "Wrench"
+  task_specification: "$(find etasl_ros_control_cmd)/scripts/peg_in_hole.lua"

etasl_ros_control_cmd/launch/rqt_plot.launch

@@ -0,0 +1,7 @@
+<?xml version="1.0" encoding="utf-8"?>
+<launch>
+    <node name="rqt_vel" pkg="rqt_gui" type="rqt_gui" respawn="false" 
+    output="screen" args="--perspective-file $(find etasl_ros_control_cmd)/config/rqt_vel.perspective"/>
+    <node name="rqt_wrench" pkg="rqt_gui" type="rqt_gui" respawn="false" 
+    output="screen" args="--perspective-file $(find etasl_ros_control_cmd)/config/rqt_wrench.perspective"/>
+</launch>

etasl_ros_control_cmd/launch/smach_peg.launch

@@ -0,0 +1,45 @@
+<?xml version="1.0" encoding="utf-8"?>
+<launch>
+    <arg name="connected" default="false"/><!-- Are gripper and force sensor connected? -->
+    <arg name="sim" default="true"/><!-- Are you running the physical robot? -->
+    <arg name="speed" default="1.0"/><!-- Mulitplier for K in eTaSL (K*speed) -->
+    <arg name="plot" default="false"/><!-- Do ypu want to plot a graph of joint states? -->
+    <arg name="ctx" default="false"/><!-- Do you want to print the eTaSL context to terminal? -->
+    <param name="connection" value="$(arg connected)"/>
+    <param name="print_ctx" value="$(arg ctx)"/>
+    <!-- Robot Model -->
+    <include file="$(find kuka_lab_setup)/launch/load_kukalabsetup.launch"/>
+    <!-- Hardware -->
+    <rosparam file="$(find kuka_rsi_hw_interface)/config/controller_joint_names.yaml" command="load"/>
+    <rosparam file="$(find kuka_rsi_hw_interface)/config/joint_limits.yaml" command="load"/>
+    <rosparam file="$(find kuka_rsi_hw_interface)/test/test_params_sim.yaml" command="load" if="$(arg sim)"/>
+    <rosparam file="$(find kuka_rsi_hw_interface)/test/test_params.yaml" command="load" unless="$(arg sim)"/>
+    <node name="kuka_hardware_interface" pkg="kuka_rsi_hw_interface" type="kuka_hardware_interface_node" respawn="false" output="screen" required="true"/>
+    <!-- Controllers -->
+    <rosparam file="$(find etasl_ros_control_cmd)/config/smach_peg.yaml" command="load" subst_value="true"/>
+    <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_state_controller"/>
+    <node name="controller_spawner_etasl" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="--stopped etasl_controller_cmd"/>
+    <!-- Robot states -->
+    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
+    <!--<node name="rqt_gui" pkg="rqt_gui" type="rqt_gui"/>-->
+    <!-- Visualization -->
+    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find kuka_lab_setup)/config/zero_state_robotiq.rviz" required="true"/>
+    <node name="kuka_rsi_simulator" pkg="kuka_rsi_simulator" type="kuka_rsi_simulator" args="129.241.65.242 49152" if="$(arg sim)"/>    
+    <node name="smach_viz" pkg="smach_viewer" type="smach_viewer.py" required="true"/>
+    <!-- Peg-In-Hole task specific nodes -->
+    <node name="gripper_controller" pkg="robotiq_2f_gripper_control" type="Robotiq2FGripperTcpNode.py" args="192.168.250.11" required="true" if="$(arg connected)"/>
+    <node name="netft_publisher" pkg="netft_control" type="netft_node" args="--rate 250 --wrench --address 192.168.1.13" required="true"  if="$(arg connected)">
+        <remap from="netft_data" to="etasl_controller_cmd/netft_data"/>
+    </node>
+    <node name="zivid_publisher" pkg="etasl_ros_control_cmd" type="zivid_publisher.py" required="true"/>
+    <node name="etasl_pose_publisher" pkg="etasl_ros_control_cmd" type="etasl_pose_publisher.py" required="true"/>
+    <node name="speed_pub" pkg="etasl_ros_control_cmd" type="speed_pub.py" required="true"> 
+        <param name="gain_multiplier" value="$(arg speed)"/>
+    </node>
+    <!-- State Machine -->
+    <node name="smach_peg" pkg="etasl_ros_control_cmd" type="smach_task.py" required="true"/>
+
+    <node name="wrench_publisher" pkg="etasl_ros_control_examples" type="force_publisher.py" required="true" unless="$(arg connected)">
+        <remap from="/etasl_controller_lissajous/netft_data" to="/etasl_controller_cmd/netft_data"/>
+    </node>
+</launch>