Add logic for sausage detection

CustomRobots/robot_arms/config/ur3_controllers.yaml

@@ -0,0 +1,86 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 100  # Hz
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+    joint_trajectory_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+    gripper_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+joint_state_broadcaster:
+  ros__parameters:
+    joints:
+      - shoulder_pan_joint
+      - shoulder_lift_joint
+      - elbow_joint
+      - wrist_1_joint
+      - wrist_2_joint
+      - wrist_3_joint
+      - robotiq_85_left_knuckle_joint
+
+    interfaces:
+      - position
+      - velocity
+      - effort
+
+joint_trajectory_controller:
+  ros__parameters:
+    joints:
+      - shoulder_pan_joint
+      - shoulder_lift_joint
+      - elbow_joint
+      - wrist_1_joint
+      - wrist_2_joint
+      - wrist_3_joint
+
+    command_interfaces:
+      - position
+
+    state_interfaces:
+      - position
+      - velocity
+
+    state_publish_rate: 100.0
+    action_monitor_rate: 20.0
+    allow_partial_joints_goal: false
+    allow_nonzero_velocity_at_trajectory_end: true
+    allow_integration_in_goal_trajectories: true
+
+    constraints:
+      stopped_velocity_tolerance: 0.01
+      goal_time: 0.0
+
+gripper_controller:
+  ros__parameters:
+    joints:
+      - robotiq_85_left_knuckle_joint
+
+    command_interfaces:
+      - position
+
+    state_interfaces:
+      - position
+      - velocity
+
+    state_publish_rate: 100.0
+    action_monitor_rate: 20.0
+    allow_partial_joints_goal: false
+    allow_integration_in_goal_trajectories: true
+
+    constraints:
+      stopped_velocity_tolerance: 0.01
+      goal_time: 0.0
+      robotiq_85_left_knuckle_joint:
+        trajectory: 0.5
+        goal: 0.5
+
+    gains:
+      robotiq_85_left_knuckle_joint:
+        p: 100.0
+        d: 1.0
+        i: 0.01
+        i_clamp: 1.0

CustomRobots/robot_arms/config/ur5/joint_limits.yaml

@@ -15,19 +15,19 @@ joint_limits:
     has_position_limits: true
     has_velocity_limits: true
     max_effort: 150.0
-    max_position: !degrees  360.0
+    max_position: !degrees  180.0
     max_velocity: !degrees  180.0
-    min_position: !degrees -360.0
+    min_position: !degrees -180.0
   shoulder_lift_joint:
     # acceleration limits are not publicly available
     has_acceleration_limits: false
     has_effort_limits: true
     has_position_limits: true
     has_velocity_limits: true
     max_effort: 150.0
-    max_position: !degrees  360.0
+    max_position: !degrees  180.0
     max_velocity: !degrees  180.0
-    min_position: !degrees -360.0
+    min_position: !degrees -180.0
   elbow_joint:
     # acceleration limits are not publicly available
     has_acceleration_limits: false
@@ -55,26 +55,26 @@ joint_limits:
     has_position_limits: true
     has_velocity_limits: true
     max_effort: 28.0
-    max_position: !degrees  360.0
+    max_position: !degrees  180.0
     max_velocity: !degrees  180.0
-    min_position: !degrees -360.0
+    min_position: !degrees -180.0
   wrist_2_joint:
     # acceleration limits are not publicly available
     has_acceleration_limits: false
     has_effort_limits: true
     has_position_limits: true
     has_velocity_limits: true
     max_effort: 28.0
-    max_position: !degrees  360.0
+    max_position: !degrees  180.0
     max_velocity: !degrees  180.0
-    min_position: !degrees -360.0
+    min_position: !degrees -180.0
   wrist_3_joint:
     # acceleration limits are not publicly available
     has_acceleration_limits: false
     has_effort_limits: true
     has_position_limits: true
     has_velocity_limits: true
     max_effort: 28.0
-    max_position: !degrees  360.0
+    max_position: !degrees  180.0
     max_velocity: !degrees  180.0
-    min_position: !degrees -360.0
+    min_position: !degrees -180.0