diff --git a/initial_guess.yaml b/initial_guess.yaml
new file mode 100644
index 00000000..b3903370
--- /dev/null
+++ b/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
\ No newline at end of file
diff --git a/registered_pose.yaml b/registered_pose.yaml
new file mode 100644
index 00000000..ce462187
--- /dev/null
+++ b/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
\ No newline at end of file
diff --git a/src/fanuc_sim/config/config.yaml b/src/fanuc_sim/config/config.yaml
index be3aff45..c1609571 100644
--- a/src/fanuc_sim/config/config.yaml
+++ b/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=[]]
diff --git a/src/fanuc_sim/config/control/picknik_fanuc.ros2_control.yaml b/src/fanuc_sim/config/control/picknik_fanuc.ros2_control.yaml
index 68aedf87..c8111bdd 100644
--- a/src/fanuc_sim/config/control/picknik_fanuc.ros2_control.yaml
+++ b/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:
diff --git a/src/fanuc_sim/config/moveit/picknik_fanuc.srdf b/src/fanuc_sim/config/moveit/picknik_fanuc.srdf
index 3fb19407..81eb1627 100644
--- a/src/fanuc_sim/config/moveit/picknik_fanuc.srdf
+++ b/src/fanuc_sim/config/moveit/picknik_fanuc.srdf
@@ -19,6 +19,7 @@
+
diff --git a/src/fanuc_sim/description/box-welds (copy).xml b/src/fanuc_sim/description/box-welds (copy).xml
new file mode 100644
index 00000000..6592fb80
--- /dev/null
+++ b/src/fanuc_sim/description/box-welds (copy).xml
@@ -0,0 +1,514 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/fanuc_sim/description/box-welds.xml b/src/fanuc_sim/description/box-welds.xml
new file mode 100644
index 00000000..6592fb80
--- /dev/null
+++ b/src/fanuc_sim/description/box-welds.xml
@@ -0,0 +1,514 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/fanuc_sim/description/box-welds.xml.bak b/src/fanuc_sim/description/box-welds.xml.bak
new file mode 100644
index 00000000..e2d34158
--- /dev/null
+++ b/src/fanuc_sim/description/box-welds.xml.bak
@@ -0,0 +1,528 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/fanuc_sim/description/fanuc_lrmate200id.ros2_control.xacro b/src/fanuc_sim/description/fanuc_lrmate200id.ros2_control.xacro
index e926a49e..a0788da1 100644
--- a/src/fanuc_sim/description/fanuc_lrmate200id.ros2_control.xacro
+++ b/src/fanuc_sim/description/fanuc_lrmate200id.ros2_control.xacro
@@ -5,6 +5,7 @@
params="name initial_positions_file hardware_interface mujoco_model"
>
+
picknik_mujoco_ros/MujocoSystem
$(arg mujoco_model)
fanuc_sim
+ $(arg mujoco_viewer)
10
60
10
diff --git a/src/fanuc_sim/description/picknik_fanuc.xacro b/src/fanuc_sim/description/picknik_fanuc.xacro
index 885c3fe4..28d063b3 100644
--- a/src/fanuc_sim/description/picknik_fanuc.xacro
+++ b/src/fanuc_sim/description/picknik_fanuc.xacro
@@ -13,12 +13,63 @@
filename="$(find fanuc_sim)/description/fanuc_lrmate200id.ros2_control.xacro"
/>
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -65,6 +66,10 @@
+
+
+
+
@@ -75,50 +80,224 @@
type="plane"
material="groundplane"
/>
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
-
-
-
+
+
+
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/fanuc_sim/description/tool.urdf b/src/fanuc_sim/description/tool.urdf
new file mode 100644
index 00000000..055ee7aa
--- /dev/null
+++ b/src/fanuc_sim/description/tool.urdf
@@ -0,0 +1,42 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/fanuc_sim/description/tool.xml b/src/fanuc_sim/description/tool.xml
new file mode 100644
index 00000000..b6eacb7b
--- /dev/null
+++ b/src/fanuc_sim/description/tool.xml
@@ -0,0 +1,84 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/fanuc_sim/objectives/activate_vacuum.xml b/src/fanuc_sim/objectives/activate_vacuum.xml
new file mode 100644
index 00000000..b8563917
--- /dev/null
+++ b/src/fanuc_sim/objectives/activate_vacuum.xml
@@ -0,0 +1,10 @@
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/fanuc_sim/objectives/clean_convex.xml b/src/fanuc_sim/objectives/clean_convex.xml
new file mode 100644
index 00000000..bdd38812
--- /dev/null
+++ b/src/fanuc_sim/objectives/clean_convex.xml
@@ -0,0 +1,90 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/fanuc_sim/objectives/close_gripper.xml b/src/fanuc_sim/objectives/close_gripper.xml
deleted file mode 100644
index ac4fe639..00000000
--- a/src/fanuc_sim/objectives/close_gripper.xml
+++ /dev/null
@@ -1,7 +0,0 @@
-
-
-
-
-
-
-
diff --git a/src/fanuc_sim/objectives/deactivate_vacuum.xml b/src/fanuc_sim/objectives/deactivate_vacuum.xml
new file mode 100644
index 00000000..c60626a5
--- /dev/null
+++ b/src/fanuc_sim/objectives/deactivate_vacuum.xml
@@ -0,0 +1,11 @@
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/fanuc_sim/objectives/load_mesh_as_colored_pointcloud.xml b/src/fanuc_sim/objectives/load_mesh_as_colored_pointcloud.xml
new file mode 100644
index 00000000..5bbad485
--- /dev/null
+++ b/src/fanuc_sim/objectives/load_mesh_as_colored_pointcloud.xml
@@ -0,0 +1,44 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/fanuc_sim/objectives/load_mesh_as_green_poincloud.xml b/src/fanuc_sim/objectives/load_mesh_as_green_poincloud.xml
new file mode 100644
index 00000000..cce854ba
--- /dev/null
+++ b/src/fanuc_sim/objectives/load_mesh_as_green_poincloud.xml
@@ -0,0 +1,39 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/fanuc_sim/objectives/load_mesh_as_red_pointcloud.xml b/src/fanuc_sim/objectives/load_mesh_as_red_pointcloud.xml
new file mode 100644
index 00000000..e7492b15
--- /dev/null
+++ b/src/fanuc_sim/objectives/load_mesh_as_red_pointcloud.xml
@@ -0,0 +1,39 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/fanuc_sim/objectives/move_to_pose.xml b/src/fanuc_sim/objectives/move_to_pose.xml
index 1e58a85e..753dc569 100644
--- a/src/fanuc_sim/objectives/move_to_pose.xml
+++ b/src/fanuc_sim/objectives/move_to_pose.xml
@@ -1,4 +1,4 @@
-
+
-
+
diff --git a/src/fanuc_sim/objectives/open_gripper.xml b/src/fanuc_sim/objectives/open_gripper.xml
deleted file mode 100644
index 9b25c0c2..00000000
--- a/src/fanuc_sim/objectives/open_gripper.xml
+++ /dev/null
@@ -1,8 +0,0 @@
-
-
-
-
-
-
-
-
diff --git a/src/fanuc_sim/objectives/pick_up_block_with_tool.xml b/src/fanuc_sim/objectives/pick_up_block_with_tool.xml
new file mode 100644
index 00000000..a8b5bbcb
--- /dev/null
+++ b/src/fanuc_sim/objectives/pick_up_block_with_tool.xml
@@ -0,0 +1,48 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/fanuc_sim/objectives/pick_up_tool.xml b/src/fanuc_sim/objectives/pick_up_tool.xml
new file mode 100644
index 00000000..38682ccd
--- /dev/null
+++ b/src/fanuc_sim/objectives/pick_up_tool.xml
@@ -0,0 +1,64 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/fanuc_sim/objectives/place_tool_in_tool_holder.xml b/src/fanuc_sim/objectives/place_tool_in_tool_holder.xml
new file mode 100644
index 00000000..c4b7c47a
--- /dev/null
+++ b/src/fanuc_sim/objectives/place_tool_in_tool_holder.xml
@@ -0,0 +1,50 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/fanuc_sim/objectives/precondition_test.xml b/src/fanuc_sim/objectives/precondition_test.xml
new file mode 100644
index 00000000..6e56f5e2
--- /dev/null
+++ b/src/fanuc_sim/objectives/precondition_test.xml
@@ -0,0 +1,35 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/fanuc_sim/objectives/precondition_test_2.xml b/src/fanuc_sim/objectives/precondition_test_2.xml
new file mode 100644
index 00000000..4045ad5b
--- /dev/null
+++ b/src/fanuc_sim/objectives/precondition_test_2.xml
@@ -0,0 +1,28 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/fanuc_sim/objectives/precondition_test_3.xml b/src/fanuc_sim/objectives/precondition_test_3.xml
new file mode 100644
index 00000000..342268e5
--- /dev/null
+++ b/src/fanuc_sim/objectives/precondition_test_3.xml
@@ -0,0 +1,37 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/fanuc_sim/objectives/precondition_test_4.xml b/src/fanuc_sim/objectives/precondition_test_4.xml
new file mode 100644
index 00000000..55ae0b32
--- /dev/null
+++ b/src/fanuc_sim/objectives/precondition_test_4.xml
@@ -0,0 +1,36 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/fanuc_sim/objectives/register_cad_part.xml b/src/fanuc_sim/objectives/register_cad_part.xml
new file mode 100644
index 00000000..e0d66b7b
--- /dev/null
+++ b/src/fanuc_sim/objectives/register_cad_part.xml
@@ -0,0 +1,66 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/fanuc_sim/objectives/register_convex.xml b/src/fanuc_sim/objectives/register_convex.xml
new file mode 100644
index 00000000..06ecda9d
--- /dev/null
+++ b/src/fanuc_sim/objectives/register_convex.xml
@@ -0,0 +1,107 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/fanuc_sim/objectives/register_convex.xml.bak b/src/fanuc_sim/objectives/register_convex.xml.bak
new file mode 100644
index 00000000..010a0136
--- /dev/null
+++ b/src/fanuc_sim/objectives/register_convex.xml.bak
@@ -0,0 +1,41 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/fanuc_sim/objectives/request_teleoperation.xml b/src/fanuc_sim/objectives/request_teleoperation.xml
index e4c00eaa..3dc0b490 100644
--- a/src/fanuc_sim/objectives/request_teleoperation.xml
+++ b/src/fanuc_sim/objectives/request_teleoperation.xml
@@ -1,3 +1,4 @@
+
-
+
-
+
@@ -146,4 +147,7 @@
+
+
+
diff --git a/src/fanuc_sim/objectives/retract.xml b/src/fanuc_sim/objectives/retract.xml
new file mode 100644
index 00000000..225598dd
--- /dev/null
+++ b/src/fanuc_sim/objectives/retract.xml
@@ -0,0 +1,33 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/fanuc_sim/objectives/setup_planning_scene.xml b/src/fanuc_sim/objectives/setup_planning_scene.xml
new file mode 100644
index 00000000..8176c5e2
--- /dev/null
+++ b/src/fanuc_sim/objectives/setup_planning_scene.xml
@@ -0,0 +1,21 @@
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/fanuc_sim/objectives/take_snapshot.xml b/src/fanuc_sim/objectives/take_snapshot.xml
index 5d823a6a..265262c4 100644
--- a/src/fanuc_sim/objectives/take_snapshot.xml
+++ b/src/fanuc_sim/objectives/take_snapshot.xml
@@ -1,15 +1,17 @@
+
@@ -20,4 +22,9 @@
/>
+
+
+
+
+
diff --git a/src/fanuc_sim/objectives/tool_attachment_example.xml b/src/fanuc_sim/objectives/tool_attachment_example.xml
new file mode 100644
index 00000000..6e9d5179
--- /dev/null
+++ b/src/fanuc_sim/objectives/tool_attachment_example.xml
@@ -0,0 +1,16 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/fanuc_sim/waypoints/waypoints.yaml b/src/fanuc_sim/waypoints/waypoints.yaml
index fe51488c..187b2536 100644
--- a/src/fanuc_sim/waypoints/waypoints.yaml
+++ b/src/fanuc_sim/waypoints/waypoints.yaml
@@ -1 +1,190 @@
-[]
+- description: ''
+ favorite: true
+ joint_group_names:
+ - gripper
+ - manipulator
+ joint_state:
+ effort: []
+ header:
+ frame_id: ''
+ stamp:
+ nanosec: 0
+ sec: 0
+ name:
+ - joint_1
+ - joint_2
+ - joint_3
+ - joint_4
+ - joint_5
+ - joint_6
+ position:
+ - 0.07619213524736071
+ - -0.7105125089645793
+ - -0.39623479079949836
+ - 0.1310527185218626
+ - -0.42624024512758213
+ - -0.09171837818641333
+ velocity: []
+ multi_dof_joint_state:
+ header:
+ frame_id: ''
+ stamp:
+ nanosec: 0
+ sec: 0
+ joint_names: []
+ transforms: []
+ twist: []
+ wrench: []
+ name: Registration
+- description: ''
+ favorite: false
+ joint_group_names:
+ - gripper
+ - manipulator
+ joint_state:
+ effort: []
+ header:
+ frame_id: ''
+ stamp:
+ nanosec: 0
+ sec: 0
+ name:
+ - joint_1
+ - joint_2
+ - joint_3
+ - joint_4
+ - joint_5
+ - joint_6
+ position:
+ - 2.0238548295251704
+ - 1.012548784310725
+ - 0.16732230853201366
+ - 0.48572896407227517
+ - -0.3925271000306122
+ - -0.8617334001254201
+ velocity: []
+ multi_dof_joint_state:
+ header:
+ frame_id: ''
+ stamp:
+ nanosec: 0
+ sec: 0
+ joint_names: []
+ transforms: []
+ twist: []
+ wrench: []
+ name: Left bin
+- description: ''
+ favorite: true
+ joint_group_names:
+ - gripper
+ - manipulator
+ joint_state:
+ effort: []
+ header:
+ frame_id: ''
+ stamp:
+ nanosec: 0
+ sec: 0
+ name:
+ - joint_1
+ - joint_2
+ - joint_3
+ - joint_4
+ - joint_5
+ - joint_6
+ position:
+ - 4.101780649531268e-37
+ - -0.4688951300482917
+ - -0.4842548345795775
+ - 6.3512581794154385e-19
+ - -0.002355461329286324
+ - 6.351037779836293e-19
+ velocity: []
+ multi_dof_joint_state:
+ header:
+ frame_id: ''
+ stamp:
+ nanosec: 0
+ sec: 0
+ joint_names: []
+ transforms: []
+ twist: []
+ wrench: []
+ name: View Concave
+- description: ''
+ favorite: true
+ joint_group_names:
+ - gripper
+ - manipulator
+ joint_state:
+ effort: []
+ header:
+ frame_id: ''
+ stamp:
+ nanosec: 0
+ sec: 0
+ name:
+ - joint_1
+ - joint_2
+ - joint_3
+ - joint_4
+ - joint_5
+ - joint_6
+ position:
+ - -4.531861149816098e-05
+ - -0.12266538508312397
+ - -0.701862360395226
+ - -3.0254405894626604e-05
+ - -0.00378749392859631
+ - -4.690750471590086e-05
+ velocity: []
+ multi_dof_joint_state:
+ header:
+ frame_id: ''
+ stamp:
+ nanosec: 0
+ sec: 0
+ joint_names: []
+ transforms: []
+ twist: []
+ wrench: []
+ name: View Convex
+- description: ''
+ favorite: false
+ joint_group_names:
+ - gripper
+ - manipulator
+ joint_state:
+ effort: []
+ header:
+ frame_id: ''
+ stamp:
+ nanosec: 0
+ sec: 0
+ name:
+ - joint_1
+ - joint_2
+ - joint_3
+ - joint_4
+ - joint_5
+ - joint_6
+ position:
+ - 1.580174692118959
+ - -0.3174498466822826
+ - -0.5574400907076587
+ - 0.0030420865446560085
+ - -0.5759784636128362
+ - -0.0008934379999721851
+ velocity: []
+ multi_dof_joint_state:
+ header:
+ frame_id: ''
+ stamp:
+ nanosec: 0
+ sec: 0
+ joint_names: []
+ transforms: []
+ twist: []
+ wrench: []
+ name: Home
diff --git a/src/lab_sim/objectives/logger_test.xml b/src/lab_sim/objectives/logger_test.xml
new file mode 100644
index 00000000..ae460f40
--- /dev/null
+++ b/src/lab_sim/objectives/logger_test.xml
@@ -0,0 +1,11 @@
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/lab_sim/objectives/register_cad_part.xml b/src/lab_sim/objectives/register_cad_part.xml
index 4c56b729..89601321 100644
--- a/src/lab_sim/objectives/register_cad_part.xml
+++ b/src/lab_sim/objectives/register_cad_part.xml
@@ -1,3 +1,4 @@
+
-
+
+
-
-
+
+
+
+
diff --git a/src/picknik_accessories b/src/picknik_accessories
index 6ddf6570..62dd8d35 160000
--- a/src/picknik_accessories
+++ b/src/picknik_accessories
@@ -1 +1 @@
-Subproject commit 6ddf657050f678b9eac282ade0940d827a8ea166
+Subproject commit 62dd8d359f265910b676da5515bdde1543fb2937