WIP: Orientation/Motion Overhaul PR

conntact/CMakeLists.txt

@@ -0,0 +1,8 @@
+cmake_minimum_required(VERSION 3.0)
+project(conntact)
+
+find_package(catkin REQUIRED COMPONENTS)
+
+catkin_package()
+
+catkin_python_setup()

ReadMe.md → conntact/ReadMe.md

@@ -42,6 +42,15 @@ Development of framework was done under Ubuntu Focal (20.04) using [ROS Noetic](
   - Install ROS package dependencies: `rosdep install --rosdistro noetic --ignore-src --from-paths .`
   - Source ROS and build the workspace: `. /opt/ros/noetic/setup.bash`, `catkin build`
 
+It may be required to uninstall the ROS-default `ros-noetic-ur-client-library`, which uses old debs which lack major performance improvements. Even when running on a highly-performant computer, the RTDE buffer may overflow, giving `Pipeline Producer overflowed! <RTDE Pipeline>` errors. This causes major instability in robot performance. 
+
+Fix this by building the more recent versions from source:
+
+- `sudo apt remove ros-noetic-ur-client-library`
+- `sudo apt install ccache`
+- `git clone https://github.com/UniversalRobots/Universal_Robots_Client_Library.git`
+- `catkin config -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -DCMAKE_BUILD_TYPE=RelWithDebInfo -DCMAKE_CXX_COMPILER_LAUNCHER=ccache -DCMAKE_C_COMPILER_LAUNCHER=ccache`
+
 ## Structure
 
 ![Detailed block diagram](resource/Conntact_Detail_Diagram.png)

conntact/config/ur10e_controllers_template.yaml

@@ -0,0 +1,117 @@
+# Settings for ros_control control loop
+hardware_control_loop:
+   loop_hz: 500
+
+# Settings for ros_control hardware interface
+ur_hardware_interface:
+   joints: &robot_joints
+     - shoulder_pan_joint
+     - shoulder_lift_joint
+     - elbow_joint
+     - wrist_1_joint
+     - wrist_2_joint
+     - wrist_3_joint
+
+# Publish all joint states ----------------------------------
+joint_state_controller:
+   type:         joint_state_controller/JointStateController
+   publish_rate: 200
+
+# Publish wrench ----------------------------------
+force_torque_sensor_controller:
+   type:         force_torque_sensor_controller/ForceTorqueSensorController
+   publish_rate: 200
+
+# Publish speed_scaling factor
+speed_scaling_state_controller:
+   type:         ur_controllers/SpeedScalingStateController
+   publish_rate: 200
+
+# Joint Trajectory Controller - position based -------------------------------
+# For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller
+scaled_pos_joint_traj_controller:
+   type: position_controllers/ScaledJointTrajectoryController
+   joints: *robot_joints
+   constraints:
+      goal_time: 0.6
+      stopped_velocity_tolerance: 0.05
+      shoulder_pan_joint: {trajectory: 0.2, goal: 0.1}
+      shoulder_lift_joint: {trajectory: 0.2, goal: 0.1}
+      elbow_joint: {trajectory: 0.2, goal: 0.1}
+      wrist_1_joint: {trajectory: 0.2, goal: 0.1}
+      wrist_2_joint: {trajectory: 0.2, goal: 0.1}
+      wrist_3_joint: {trajectory: 0.2, goal: 0.1}
+   stop_trajectory_duration: 0.5
+   state_publish_rate: 500
+   action_monitor_rate: 10
+
+pos_joint_traj_controller:
+   type: position_controllers/JointTrajectoryController
+   joints: *robot_joints
+   constraints:
+      goal_time: 0.6
+      stopped_velocity_tolerance: 0.05
+      shoulder_pan_joint: {trajectory: 0.2, goal: 0.1}
+      shoulder_lift_joint: {trajectory: 0.2, goal: 0.1}
+      elbow_joint: {trajectory: 0.2, goal: 0.1}
+      wrist_1_joint: {trajectory: 0.2, goal: 0.1}
+      wrist_2_joint: {trajectory: 0.2, goal: 0.1}
+      wrist_3_joint: {trajectory: 0.2, goal: 0.1}
+   stop_trajectory_duration: 0.5
+   state_publish_rate: 500
+   action_monitor_rate: 10
+
+joint_group_position_controller:
+   type: position_controllers/JointGroupPositionController
+   joints: *robot_joints
+
+cartesian_force_controller:
+    type: "position_controllers/CartesianForceController"
+    end_effector_link: "tool0"
+    robot_base_link: "base_link"
+    ft_sensor_ref_link: "tool0"
+    joints: *robot_joints
+    pd_gains:
+        trans_x: {p: 0.006}
+        trans_y: {p: 0.006}
+        trans_z: {p: 0.001}
+        rot_x: {p: 0.09}
+        rot_y: {p: 0.09}
+        rot_z: {p: 0.09}
+
+cartesian_compliance_controller:
+   type: "position_controllers/CartesianComplianceController"
+   end_effector_link: "tool0"
+   robot_base_link: "base_link"
+   ft_sensor_ref_link: "tool0"
+   compliance_ref_link: "tool0"
+   target_frame_topic: "target_frame"
+   joints: *robot_joints
+   stiffness:
+      trans_x: 1200
+      trans_y: 1200
+      trans_z: 1200
+      rot_x: 150
+      rot_y: 150
+      rot_z: 150
+   pd_gains:
+      trans_x: {p: 0.0025}
+      trans_y: {p: 0.0025}
+      trans_z: {p: 0.0025}
+      rot_x: {p: 0.06}
+      rot_y: {p: 0.06}
+      rot_z: {p: 0.06}
+
+# cartesian_force_controller:
+#     type: "position_controllers/CartesianForceController"
+#     end_effector_link: "tool0"
+#     robot_base_link: "base_link"
+#     ft_sensor_ref_link: "tool0"
+#     joints: *robot_joints
+#     pd_gains:
+#         trans_x: {p: 0.05}
+#         trans_y: {p: 0.05}
+#         trans_z: {p: 0.05}
+#         rot_x: {p: 0.01}
+#         rot_y: {p: 0.01}
+#         rot_z: {p: 0.01}