Fanuc bin picking

initial_guess.yaml

@@ -0,0 +1,15 @@
+header:
+  stamp:
+    sec: 1736790590
+    nanosec: 769190399
+  frame_id: world
+pose:
+  position:
+    x: 0.75
+    y: 0
+    z: 0.59999999999999998
+  orientation:
+    x: 0
+    y: 0
+    z: 0.70710678118654746
+    w: 0.70710678118654746

registered_pose.yaml

@@ -0,0 +1,15 @@
+header:
+  stamp:
+    sec: 1736110686
+    nanosec: 32573000
+  frame_id: world
+pose:
+  position:
+    x: -0.18573307991027832
+    y: 0.0037446753121912479
+    z: 0.36070221662521362
+  orientation:
+    x: 0.00019979173269462342
+    y: 0.20791358031915227
+    z: -0.0018835711780756951
+    w: 0.97814727367841525

src/fanuc_sim/config/config.yaml

@@ -43,6 +43,7 @@ hardware:
     urdf_params:
         # Use "mock", "mujoco", or "real"
       - hardware_interface: "mujoco"
+      - mujoco_viewer: "true"
 
 # Sets ROS global params for launch.
 # [Optional]
@@ -112,6 +113,8 @@ ros2_control:
     - "joint_trajectory_controller"
     - "servo_controller"
     - "joint_state_broadcaster"
+    - "tool_attach_controller"
+    - "suction_cup_controller"
     # - "servo_controller"
   # Load but do not start these controllers so they can be activated later if needed.
   # [Optional, default=[]]

src/fanuc_sim/config/control/picknik_fanuc.ros2_control.yaml

@@ -7,6 +7,29 @@ controller_manager:
       type: joint_trajectory_controller/JointTrajectoryController
     servo_controller:
       type: joint_trajectory_controller/JointTrajectoryController
+    # A controller to enable/disable the tool attachment interface at the robot flange.
+    # In sim, this is modeled as a weld constraint in Mujoco.
+    # In a real robot, this controller would activate a mechanism to attach/detach the tool at the robot flange.
+    tool_attach_controller:
+      type: position_controllers/GripperActionController
+    # A controller to enable/disable the suction cup.
+    # In sim, this is modeled as a weld constraint in Mujoco.
+    # In a real robot, this controller would activate / deactivate a suction cup mechanism.
+    suction_cup_controller:
+      type: position_controllers/GripperActionController
+
+
+tool_attach_controller:
+  ros__parameters:
+    default: true
+    joint: suction_cup_tool_interface
+    allow_stalling: true
+
+suction_cup_controller:
+  ros__parameters:
+    default: true
+    joint: suction_cup_tool_tip
+    allow_stalling: true
 
 joint_state_broadcaster:
   ros__parameters:

src/fanuc_sim/config/moveit/picknik_fanuc.srdf

@@ -19,6 +19,7 @@
     <end_effector name="fanuc_ee" parent_link="tool0" group="gripper"/>    
 
     <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
+    <disable_collisions link1="arm_pedestal" link2="base_link" reason="Adjacent"/>
     <disable_collisions link1="base_link" link2="link_1" reason="Adjacent"/>
     <disable_collisions link1="base_link" link2="link_2" reason="Never"/>
     <disable_collisions link1="base_link" link2="link_3" reason="Never"/>