diff --git a/src/custom-ur-descriptions/ur5e_moveit_configs/ur_standalone_moveit_config/config/initial_positions.yaml b/src/custom-ur-descriptions/ur5e_moveit_configs/ur_standalone_moveit_config/config/initial_positions.yaml
index 44b3f2c5..8502235a 100644
--- a/src/custom-ur-descriptions/ur5e_moveit_configs/ur_standalone_moveit_config/config/initial_positions.yaml
+++ b/src/custom-ur-descriptions/ur5e_moveit_configs/ur_standalone_moveit_config/config/initial_positions.yaml
@@ -1,9 +1,8 @@
-# Default initial positions for ur's ros2_control fake system
-
-initial_positions:
-  elbow_joint: 0
-  shoulder_lift_joint: 0
-  shoulder_pan_joint: 0
-  wrist_1_joint: 0
-  wrist_2_joint: 0
-  wrist_3_joint: 0
\ No newline at end of file
+# Initial positions for fake hardware simulation
+# Joint angles: [79.72, -69.5, -81.91, -117.92, -268.05, -157.15] degrees
+shoulder_pan_joint: 1.3914
+shoulder_lift_joint: -1.2132
+elbow_joint: -1.4297
+wrist_1_joint: -2.0584
+wrist_2_joint: -4.6783
+wrist_3_joint: -2.7430
diff --git a/src/custom-ur-descriptions/ur5e_moveit_configs/ur_standalone_moveit_config/config/joint_limits.yaml b/src/custom-ur-descriptions/ur5e_moveit_configs/ur_standalone_moveit_config/config/joint_limits.yaml
index 130053c3..b4361bdf 100644
--- a/src/custom-ur-descriptions/ur5e_moveit_configs/ur_standalone_moveit_config/config/joint_limits.yaml
+++ b/src/custom-ur-descriptions/ur5e_moveit_configs/ur_standalone_moveit_config/config/joint_limits.yaml
@@ -11,30 +11,30 @@ joint_limits:
   elbow_joint:
     has_velocity_limits: true
     max_velocity: 3.1415926535897931
-    has_acceleration_limits: false
-    max_acceleration: 0
+    has_acceleration_limits: true
+    max_acceleration: 5.0
   shoulder_lift_joint:
     has_velocity_limits: true
     max_velocity: 3.1415926535897931
-    has_acceleration_limits: false
-    max_acceleration: 0
+    has_acceleration_limits: true
+    max_acceleration: 5.0
   shoulder_pan_joint:
     has_velocity_limits: true
     max_velocity: 3.1415926535897931
-    has_acceleration_limits: false
-    max_acceleration: 0
+    has_acceleration_limits: true
+    max_acceleration: 5.0
   wrist_1_joint:
     has_velocity_limits: true
     max_velocity: 3.1415926535897931
-    has_acceleration_limits: false
-    max_acceleration: 0
+    has_acceleration_limits: true
+    max_acceleration: 5.0
   wrist_2_joint:
     has_velocity_limits: true
     max_velocity: 3.1415926535897931
-    has_acceleration_limits: false
-    max_acceleration: 0
+    has_acceleration_limits: true
+    max_acceleration: 5.0
   wrist_3_joint:
     has_velocity_limits: true
     max_velocity: 3.1415926535897931
-    has_acceleration_limits: false
-    max_acceleration: 0
\ No newline at end of file
+    has_acceleration_limits: true
+    max_acceleration: 5.0
\ No newline at end of file
diff --git a/src/custom-ur-descriptions/ur5e_moveit_configs/ur_standalone_moveit_config/config/ompl_planning.yaml b/src/custom-ur-descriptions/ur5e_moveit_configs/ur_standalone_moveit_config/config/ompl_planning.yaml
index 97ae75fb..2247fc31 100644
--- a/src/custom-ur-descriptions/ur5e_moveit_configs/ur_standalone_moveit_config/config/ompl_planning.yaml
+++ b/src/custom-ur-descriptions/ur5e_moveit_configs/ur_standalone_moveit_config/config/ompl_planning.yaml
@@ -5,6 +5,7 @@ planner_configs:
   RRTConnectkConfigDefault:
     type: geometric::RRTConnect
     range: 0.0
+    goal_bias: 0.15  # Higher = more direct paths (default 0.05)
     # goal_tolerance: 0.01
     # num_planning_attempts: 10
     # planning_time: 10.0
diff --git a/src/custom-ur-descriptions/ur5e_moveit_configs/ur_standalone_moveit_config/config/pilz_industrial_motion_planner_planning.yaml b/src/custom-ur-descriptions/ur5e_moveit_configs/ur_standalone_moveit_config/config/pilz_industrial_motion_planner_planning.yaml
new file mode 100644
index 00000000..6402ab93
--- /dev/null
+++ b/src/custom-ur-descriptions/ur5e_moveit_configs/ur_standalone_moveit_config/config/pilz_industrial_motion_planner_planning.yaml
@@ -0,0 +1,10 @@
+planning_plugin: pilz_industrial_motion_planner/CommandPlanner
+request_adapters: >-
+  default_planner_request_adapters/FixWorkspaceBounds
+  default_planner_request_adapters/FixStartStateBounds
+  default_planner_request_adapters/FixStartStateCollision
+  default_planner_request_adapters/FixStartStatePathConstraints
+default_planner_config: PTP
+capabilities: >-
+    pilz_industrial_motion_planner/MoveGroupSequenceAction
+    pilz_industrial_motion_planner/MoveGroupSequenceService
diff --git a/src/custom-ur-descriptions/ur5e_moveit_configs/ur_standalone_moveit_config/config/ur.srdf b/src/custom-ur-descriptions/ur5e_moveit_configs/ur_standalone_moveit_config/config/ur.srdf
index 391dd098..c22f4901 100644
--- a/src/custom-ur-descriptions/ur5e_moveit_configs/ur_standalone_moveit_config/config/ur.srdf
+++ b/src/custom-ur-descriptions/ur5e_moveit_configs/ur_standalone_moveit_config/config/ur.srdf
@@ -19,7 +19,7 @@
         <joint name="shoulder_pan_joint" value="0"/>
         <joint name="wrist_1_joint" value="0"/>
         <joint name="wrist_2_joint" value="0"/>
-        <joint name="wrist_3_joint" value="0"/>
+        <joint name="wrist_3_joint" value="-3.14159"/>
     </group_state>
     <!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
     <virtual_joint name="virtual_joint" type="fixed" parent_frame="map" child_link="map"/>
diff --git a/src/custom-ur-descriptions/ur5e_moveit_configs/ur_standalone_moveit_config/launch/robot_bringup.launch.py b/src/custom-ur-descriptions/ur5e_moveit_configs/ur_standalone_moveit_config/launch/robot_bringup.launch.py
index 2e35fc55..a8095d2e 100644
--- a/src/custom-ur-descriptions/ur5e_moveit_configs/ur_standalone_moveit_config/launch/robot_bringup.launch.py
+++ b/src/custom-ur-descriptions/ur5e_moveit_configs/ur_standalone_moveit_config/launch/robot_bringup.launch.py
@@ -52,7 +52,7 @@ def generate_launch_description():
         .planning_scene_monitor(
             publish_robot_description=True, publish_robot_description_semantic=True
         )
-        .planning_pipelines(pipelines=["ompl"])
+        .planning_pipelines(pipelines=["ompl", "pilz_industrial_motion_planner"])
         .to_moveit_configs()
     )
     # Load  ExecuteTaskSolutionCapability so we can execute found solutions in simulation
diff --git a/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_epick_moveit_config/config/initial_positions.yaml b/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_epick_moveit_config/config/initial_positions.yaml
index ea0b51dd..f9b76921 100644
--- a/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_epick_moveit_config/config/initial_positions.yaml
+++ b/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_epick_moveit_config/config/initial_positions.yaml
@@ -1,10 +1,9 @@
-# Default initial positions for ur's ros2_control fake system
-
-initial_positions:
-  elbow_joint: 0
-  gripper: 0
-  shoulder_lift_joint: 0
-  shoulder_pan_joint: 0
-  wrist_1_joint: 0
-  wrist_2_joint: 0
-  wrist_3_joint: 0
\ No newline at end of file
+# Initial positions for fake hardware simulation
+# Joint angles: [79.72, -69.5, -81.91, -117.92, -268.05, -157.15] degrees
+shoulder_pan_joint: 1.3914
+shoulder_lift_joint: -1.2132
+elbow_joint: -1.4297
+wrist_1_joint: -2.0584
+wrist_2_joint: -4.6783
+wrist_3_joint: -2.7430
+gripper: 0.0
diff --git a/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_epick_moveit_config/config/joint_limits.yaml b/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_epick_moveit_config/config/joint_limits.yaml
index 845ac0ce..200f9c0b 100644
--- a/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_epick_moveit_config/config/joint_limits.yaml
+++ b/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_epick_moveit_config/config/joint_limits.yaml
@@ -11,35 +11,35 @@ joint_limits:
   elbow_joint:
     has_velocity_limits: true
     max_velocity: 3.1415926535897931
-    has_acceleration_limits: false
-    max_acceleration: 0
+    has_acceleration_limits: true
+    max_acceleration: 5.0
   gripper:
     has_velocity_limits: true
     max_velocity: 1.0
-    has_acceleration_limits: false
-    max_acceleration: 0.0
+    has_acceleration_limits: true
+    max_acceleration: 5.0
   shoulder_lift_joint:
     has_velocity_limits: true
     max_velocity: 3.1415926535897931
-    has_acceleration_limits: false
-    max_acceleration: 0
+    has_acceleration_limits: true
+    max_acceleration: 5.0
   shoulder_pan_joint:
     has_velocity_limits: true
     max_velocity: 3.1415926535897931
-    has_acceleration_limits: false
-    max_acceleration: 0
+    has_acceleration_limits: true
+    max_acceleration: 5.0
   wrist_1_joint:
     has_velocity_limits: true
     max_velocity: 3.1415926535897931
-    has_acceleration_limits: false
-    max_acceleration: 0
+    has_acceleration_limits: true
+    max_acceleration: 5.0
   wrist_2_joint:
     has_velocity_limits: true
     max_velocity: 3.1415926535897931
-    has_acceleration_limits: false
-    max_acceleration: 0
+    has_acceleration_limits: true
+    max_acceleration: 5.0
   wrist_3_joint:
     has_velocity_limits: true
     max_velocity: 3.1415926535897931
-    has_acceleration_limits: false
-    max_acceleration: 0
\ No newline at end of file
+    has_acceleration_limits: true
+    max_acceleration: 5.0
\ No newline at end of file
diff --git a/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_epick_moveit_config/config/ompl_planning.yaml b/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_epick_moveit_config/config/ompl_planning.yaml
index 9b3a451d..01554348 100644
--- a/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_epick_moveit_config/config/ompl_planning.yaml
+++ b/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_epick_moveit_config/config/ompl_planning.yaml
@@ -5,6 +5,7 @@ planner_configs:
   RRTConnectkConfigDefault:
     type: geometric::RRTConnect
     range: 0.0
+    goal_bias: 0.15  # Higher = more direct paths (default 0.05)
   RRTstar:
     type: geometric::RRTstar
     range: 1.0
diff --git a/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_epick_moveit_config/config/pilz_industrial_motion_planner_planning.yaml b/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_epick_moveit_config/config/pilz_industrial_motion_planner_planning.yaml
new file mode 100644
index 00000000..6402ab93
--- /dev/null
+++ b/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_epick_moveit_config/config/pilz_industrial_motion_planner_planning.yaml
@@ -0,0 +1,10 @@
+planning_plugin: pilz_industrial_motion_planner/CommandPlanner
+request_adapters: >-
+  default_planner_request_adapters/FixWorkspaceBounds
+  default_planner_request_adapters/FixStartStateBounds
+  default_planner_request_adapters/FixStartStateCollision
+  default_planner_request_adapters/FixStartStatePathConstraints
+default_planner_config: PTP
+capabilities: >-
+    pilz_industrial_motion_planner/MoveGroupSequenceAction
+    pilz_industrial_motion_planner/MoveGroupSequenceService
diff --git a/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_epick_moveit_config/config/ur.srdf b/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_epick_moveit_config/config/ur.srdf
index bbbf9fc3..a20d267c 100644
--- a/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_epick_moveit_config/config/ur.srdf
+++ b/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_epick_moveit_config/config/ur.srdf
@@ -28,7 +28,7 @@
         <joint name="shoulder_pan_joint" value="0"/>
         <joint name="wrist_1_joint" value="0"/>
         <joint name="wrist_2_joint" value="0"/>
-        <joint name="wrist_3_joint" value="0"/>
+        <joint name="wrist_3_joint" value="-3.14159"/>
     </group_state>
     <group_state name="vacuum_on" group="epick_gripper">
         <joint name="gripper" value="1"/>
diff --git a/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_epick_moveit_config/launch/robot_bringup.launch.py b/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_epick_moveit_config/launch/robot_bringup.launch.py
index e4ec514c..9b50c72f 100644
--- a/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_epick_moveit_config/launch/robot_bringup.launch.py
+++ b/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_epick_moveit_config/launch/robot_bringup.launch.py
@@ -36,7 +36,7 @@ def generate_launch_description():
             "description_file": LaunchConfiguration("description_file"),
             "controllers_file": LaunchConfiguration("controllers_file"),
             "kinematics_params_file": os.path.join(get_package_share_directory("ur5e_robot_description"), "config", "ur5e_calibration.yaml"),
-            "use_tool_communication": "true",  # Enable to make tool_voltage parameter work
+            "use_tool_communication": "false",  # We launch our own tool_communication node with delay
             "tool_voltage": "24",
         }.items()
     )
@@ -51,7 +51,7 @@ def generate_launch_description():
         .planning_scene_monitor(
             publish_robot_description=True, publish_robot_description_semantic=True
         )
-        .planning_pipelines(pipelines=["ompl"])
+        .planning_pipelines(pipelines=["ompl", "pilz_industrial_motion_planner"])
         .to_moveit_configs()
     )
     # Load  ExecuteTaskSolutionCapability so we can execute found solutions in simulation
diff --git a/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_hande_moveit_config/config/initial_positions.yaml b/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_hande_moveit_config/config/initial_positions.yaml
index dd2b3c10..c26956f0 100644
--- a/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_hande_moveit_config/config/initial_positions.yaml
+++ b/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_hande_moveit_config/config/initial_positions.yaml
@@ -1,10 +1,9 @@
-# Default initial positions for ur's ros2_control fake system
-
-initial_positions:
-  elbow_joint: 0
-  robotiq_hande_left_finger_joint: 0
-  shoulder_lift_joint: 0
-  shoulder_pan_joint: 0
-  wrist_1_joint: 0
-  wrist_2_joint: 0
-  wrist_3_joint: 0
\ No newline at end of file
+# Initial positions for fake hardware simulation
+# Joint angles: [79.72, -69.5, -81.91, -117.92, -268.05, -157.15] degrees
+shoulder_pan_joint: 1.3914
+shoulder_lift_joint: -1.2132
+elbow_joint: -1.4297
+wrist_1_joint: -2.0584
+wrist_2_joint: -4.6783
+wrist_3_joint: -2.7430
+robotiq_hande_left_finger_joint: 0.0
diff --git a/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_hande_moveit_config/config/joint_limits.yaml b/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_hande_moveit_config/config/joint_limits.yaml
index fe40adc0..79eb3469 100644
--- a/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_hande_moveit_config/config/joint_limits.yaml
+++ b/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_hande_moveit_config/config/joint_limits.yaml
@@ -11,40 +11,40 @@ joint_limits:
   elbow_joint:
     has_velocity_limits: true
     max_velocity: 3.1415926535897931
-    has_acceleration_limits: false
-    max_acceleration: 0
+    has_acceleration_limits: true
+    max_acceleration: 5.0
   robotiq_hande_left_finger_joint:
     has_velocity_limits: true
     max_velocity: 0.14999999999999999
-    has_acceleration_limits: false
-    max_acceleration: 0
+    has_acceleration_limits: true
+    max_acceleration: 5.0
   robotiq_hande_right_finger_joint:
     has_velocity_limits: true
     max_velocity: 0.14999999999999999
-    has_acceleration_limits: false
-    max_acceleration: 0
+    has_acceleration_limits: true
+    max_acceleration: 5.0
   shoulder_lift_joint:
     has_velocity_limits: true
     max_velocity: 3.1415926535897931
-    has_acceleration_limits: false
-    max_acceleration: 0
+    has_acceleration_limits: true
+    max_acceleration: 5.0
   shoulder_pan_joint:
     has_velocity_limits: true
     max_velocity: 3.1415926535897931
-    has_acceleration_limits: false
-    max_acceleration: 0
+    has_acceleration_limits: true
+    max_acceleration: 5.0
   wrist_1_joint:
     has_velocity_limits: true
     max_velocity: 3.1415926535897931
-    has_acceleration_limits: false
-    max_acceleration: 0
+    has_acceleration_limits: true
+    max_acceleration: 5.0
   wrist_2_joint:
     has_velocity_limits: true
     max_velocity: 3.1415926535897931
-    has_acceleration_limits: false
-    max_acceleration: 0
+    has_acceleration_limits: true
+    max_acceleration: 5.0
   wrist_3_joint:
     has_velocity_limits: true
     max_velocity: 3.1415926535897931
-    has_acceleration_limits: false
-    max_acceleration: 0
\ No newline at end of file
+    has_acceleration_limits: true
+    max_acceleration: 5.0
\ No newline at end of file
diff --git a/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_hande_moveit_config/config/ompl_planning.yaml b/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_hande_moveit_config/config/ompl_planning.yaml
index 9b3a451d..01554348 100644
--- a/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_hande_moveit_config/config/ompl_planning.yaml
+++ b/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_hande_moveit_config/config/ompl_planning.yaml
@@ -5,6 +5,7 @@ planner_configs:
   RRTConnectkConfigDefault:
     type: geometric::RRTConnect
     range: 0.0
+    goal_bias: 0.15  # Higher = more direct paths (default 0.05)
   RRTstar:
     type: geometric::RRTstar
     range: 1.0
diff --git a/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_hande_moveit_config/config/pilz_industrial_motion_planner_planning.yaml b/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_hande_moveit_config/config/pilz_industrial_motion_planner_planning.yaml
new file mode 100644
index 00000000..6402ab93
--- /dev/null
+++ b/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_hande_moveit_config/config/pilz_industrial_motion_planner_planning.yaml
@@ -0,0 +1,10 @@
+planning_plugin: pilz_industrial_motion_planner/CommandPlanner
+request_adapters: >-
+  default_planner_request_adapters/FixWorkspaceBounds
+  default_planner_request_adapters/FixStartStateBounds
+  default_planner_request_adapters/FixStartStateCollision
+  default_planner_request_adapters/FixStartStatePathConstraints
+default_planner_config: PTP
+capabilities: >-
+    pilz_industrial_motion_planner/MoveGroupSequenceAction
+    pilz_industrial_motion_planner/MoveGroupSequenceService
diff --git a/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_hande_moveit_config/config/ur.srdf b/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_hande_moveit_config/config/ur.srdf
index 61f7ff7a..9be18b13 100644
--- a/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_hande_moveit_config/config/ur.srdf
+++ b/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_hande_moveit_config/config/ur.srdf
@@ -28,7 +28,7 @@
         <joint name="shoulder_pan_joint" value="0"/>
         <joint name="wrist_1_joint" value="0"/>
         <joint name="wrist_2_joint" value="0"/>
-        <joint name="wrist_3_joint" value="0"/>
+        <joint name="wrist_3_joint" value="-3.14159"/>
     </group_state>
     <group_state name="hande_open" group="hande_gripper">
         <joint name="robotiq_hande_left_finger_joint" value="0.025"/>
diff --git a/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_hande_moveit_config/launch/robot_bringup.launch.py b/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_hande_moveit_config/launch/robot_bringup.launch.py
index 88f9f57a..78179d06 100644
--- a/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_hande_moveit_config/launch/robot_bringup.launch.py
+++ b/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_hande_moveit_config/launch/robot_bringup.launch.py
@@ -57,7 +57,7 @@ def generate_launch_description():
         .planning_scene_monitor(
             publish_robot_description=True, publish_robot_description_semantic=True
         )
-        .planning_pipelines(pipelines=["ompl"])
+        .planning_pipelines(pipelines=["ompl", "pilz_industrial_motion_planner"])
         .to_moveit_configs()
     )
     # Load  ExecuteTaskSolutionCapability so we can execute found solutions in simulation
diff --git a/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_pipettor_moveit_config/config/initial_positions.yaml b/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_pipettor_moveit_config/config/initial_positions.yaml
index d8bbb6f6..8502235a 100644
--- a/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_pipettor_moveit_config/config/initial_positions.yaml
+++ b/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_pipettor_moveit_config/config/initial_positions.yaml
@@ -1,10 +1,8 @@
-# Default initial positions for ur's ros2_control fake system
-
-initial_positions:
-  elbow_joint: 0
-  # Pipettor has no movable joints controlled by MoveIt
-  shoulder_lift_joint: 0
-  shoulder_pan_joint: 0
-  wrist_1_joint: 0
-  wrist_2_joint: 0
-  wrist_3_joint: 0
\ No newline at end of file
+# Initial positions for fake hardware simulation
+# Joint angles: [79.72, -69.5, -81.91, -117.92, -268.05, -157.15] degrees
+shoulder_pan_joint: 1.3914
+shoulder_lift_joint: -1.2132
+elbow_joint: -1.4297
+wrist_1_joint: -2.0584
+wrist_2_joint: -4.6783
+wrist_3_joint: -2.7430
diff --git a/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_pipettor_moveit_config/config/joint_limits.yaml b/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_pipettor_moveit_config/config/joint_limits.yaml
index c4ef86a9..e9414891 100644
--- a/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_pipettor_moveit_config/config/joint_limits.yaml
+++ b/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_pipettor_moveit_config/config/joint_limits.yaml
@@ -11,31 +11,31 @@ joint_limits:
   elbow_joint:
     has_velocity_limits: true
     max_velocity: 3.1415926535897931
-    has_acceleration_limits: false
-    max_acceleration: 0
+    has_acceleration_limits: true
+    max_acceleration: 5.0
   # Pipettor has no movable joints controlled by MoveIt
   shoulder_lift_joint:
     has_velocity_limits: true
     max_velocity: 3.1415926535897931
-    has_acceleration_limits: false
-    max_acceleration: 0
+    has_acceleration_limits: true
+    max_acceleration: 5.0
   shoulder_pan_joint:
     has_velocity_limits: true
     max_velocity: 3.1415926535897931
-    has_acceleration_limits: false
-    max_acceleration: 0
+    has_acceleration_limits: true
+    max_acceleration: 5.0
   wrist_1_joint:
     has_velocity_limits: true
     max_velocity: 3.1415926535897931
-    has_acceleration_limits: false
-    max_acceleration: 0
+    has_acceleration_limits: true
+    max_acceleration: 5.0
   wrist_2_joint:
     has_velocity_limits: true
     max_velocity: 3.1415926535897931
-    has_acceleration_limits: false
-    max_acceleration: 0
+    has_acceleration_limits: true
+    max_acceleration: 5.0
   wrist_3_joint:
     has_velocity_limits: true
     max_velocity: 3.1415926535897931
-    has_acceleration_limits: false
-    max_acceleration: 0
\ No newline at end of file
+    has_acceleration_limits: true
+    max_acceleration: 5.0
\ No newline at end of file
diff --git a/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_pipettor_moveit_config/config/ompl_planning.yaml b/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_pipettor_moveit_config/config/ompl_planning.yaml
index 9b3a451d..01554348 100644
--- a/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_pipettor_moveit_config/config/ompl_planning.yaml
+++ b/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_pipettor_moveit_config/config/ompl_planning.yaml
@@ -5,6 +5,7 @@ planner_configs:
   RRTConnectkConfigDefault:
     type: geometric::RRTConnect
     range: 0.0
+    goal_bias: 0.15  # Higher = more direct paths (default 0.05)
   RRTstar:
     type: geometric::RRTstar
     range: 1.0
diff --git a/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_pipettor_moveit_config/config/pilz_industrial_motion_planner_planning.yaml b/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_pipettor_moveit_config/config/pilz_industrial_motion_planner_planning.yaml
new file mode 100644
index 00000000..6402ab93
--- /dev/null
+++ b/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_pipettor_moveit_config/config/pilz_industrial_motion_planner_planning.yaml
@@ -0,0 +1,10 @@
+planning_plugin: pilz_industrial_motion_planner/CommandPlanner
+request_adapters: >-
+  default_planner_request_adapters/FixWorkspaceBounds
+  default_planner_request_adapters/FixStartStateBounds
+  default_planner_request_adapters/FixStartStateCollision
+  default_planner_request_adapters/FixStartStatePathConstraints
+default_planner_config: PTP
+capabilities: >-
+    pilz_industrial_motion_planner/MoveGroupSequenceAction
+    pilz_industrial_motion_planner/MoveGroupSequenceService
diff --git a/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_pipettor_moveit_config/config/ur.srdf b/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_pipettor_moveit_config/config/ur.srdf
index 20c97291..0b4d6312 100644
--- a/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_pipettor_moveit_config/config/ur.srdf
+++ b/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_pipettor_moveit_config/config/ur.srdf
@@ -24,7 +24,7 @@
         <joint name="shoulder_pan_joint" value="0"/>
         <joint name="wrist_1_joint" value="0"/>
         <joint name="wrist_2_joint" value="0"/>
-        <joint name="wrist_3_joint" value="0"/>
+        <joint name="wrist_3_joint" value="-3.14159"/>
     </group_state>
 
     <!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
diff --git a/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_pipettor_moveit_config/launch/robot_bringup.launch.py b/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_pipettor_moveit_config/launch/robot_bringup.launch.py
index a35061bc..41ee00c1 100644
--- a/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_pipettor_moveit_config/launch/robot_bringup.launch.py
+++ b/src/custom-ur-descriptions/ur5e_moveit_configs/ur_zivid_pipettor_moveit_config/launch/robot_bringup.launch.py
@@ -52,7 +52,7 @@ def generate_launch_description():
         .planning_scene_monitor(
             publish_robot_description=True, publish_robot_description_semantic=True
         )
-        .planning_pipelines(pipelines=["ompl"])
+        .planning_pipelines(pipelines=["ompl", "pilz_industrial_motion_planner"])
         .to_moveit_configs()
     )
     # Load  ExecuteTaskSolutionCapability so we can execute found solutions in simulation
diff --git a/src/custom-ur-descriptions/ur5e_robot_description/config/hand_eye_calibration.yaml b/src/custom-ur-descriptions/ur5e_robot_description/config/hand_eye_calibration.yaml
new file mode 100644
index 00000000..967f7f87
--- /dev/null
+++ b/src/custom-ur-descriptions/ur5e_robot_description/config/hand_eye_calibration.yaml
@@ -0,0 +1,7 @@
+__version__: 1
+FloatMatrix:
+  Data: [
+    [-0.9990297, -0.0061767, 0.0436062, 56.75],
+    [0.0064507, -0.9999603, 0.0061441, 103.22],
+    [0.0435665, 0.0064194, 0.9990299, 54.89],
+    [0, 0, 0, 1]]
diff --git a/src/custom-ur-descriptions/ur5e_robot_description/meshes/zivid/zivid_onarm_mount.stl b/src/custom-ur-descriptions/ur5e_robot_description/meshes/zivid/zivid_onarm_mount.stl
new file mode 100644
index 00000000..6024a560
Binary files /dev/null and b/src/custom-ur-descriptions/ur5e_robot_description/meshes/zivid/zivid_onarm_mount.stl differ
diff --git a/src/custom-ur-descriptions/ur5e_robot_description/urdf/ur_standalone.xacro b/src/custom-ur-descriptions/ur5e_robot_description/urdf/ur_standalone.xacro
index 7c15b087..e5925ecf 100644
--- a/src/custom-ur-descriptions/ur5e_robot_description/urdf/ur_standalone.xacro
+++ b/src/custom-ur-descriptions/ur5e_robot_description/urdf/ur_standalone.xacro
@@ -57,7 +57,7 @@
    <xacro:arg name="mock_hardware" default="true" />
 
    <!-- initial position for simulations (Fake Hardware, Gazebo, Ignition) -->
-   <xacro:arg name="initial_positions_file" default="$(find ur_description)/config/initial_positions.yaml"/>
+   <xacro:arg name="initial_positions_file" default="$(find ur_standalone_moveit_config)/config/initial_positions.yaml"/>
 
    <!-- convert to property to use substitution in function -->
    <xacro:property name="initial_positions_file" default="$(arg initial_positions_file)"/>
@@ -112,7 +112,7 @@
   <xacro:zivid_camera_mount
     parent_link="$(arg tf_prefix)tool0"
     xyz="0 0 0.005"
-    rpy="0 0 3.14159"
+    rpy="0 0 0"
     zivid_model="ZIVID_2_PLUS_MR60"/>
 
   <!-- Tool exchanger robotside attached to arm mount (not camera) -->
diff --git a/src/custom-ur-descriptions/ur5e_robot_description/urdf/ur_with_zivid_epick.xacro b/src/custom-ur-descriptions/ur5e_robot_description/urdf/ur_with_zivid_epick.xacro
index 92066007..353cd976 100644
--- a/src/custom-ur-descriptions/ur5e_robot_description/urdf/ur_with_zivid_epick.xacro
+++ b/src/custom-ur-descriptions/ur5e_robot_description/urdf/ur_with_zivid_epick.xacro
@@ -61,7 +61,7 @@
   <xacro:arg name="mock_hardware" default="true" />
 
   <!-- initial position for simulations (Fake Hardware, Gazebo, Ignition) -->
-  <xacro:arg name="initial_positions_file" default="$(find ur_description)/config/initial_positions.yaml"/>
+  <xacro:arg name="initial_positions_file" default="$(find ur_zivid_epick_moveit_config)/config/initial_positions.yaml"/>
   <xacro:property name="initial_positions_file" default="$(arg initial_positions_file)"/>
 
   <!-- create link fixed to the "map" -->
@@ -114,7 +114,7 @@
   <xacro:zivid_camera_mount
     parent_link="$(arg tf_prefix)tool0"
     xyz="0 0 0.005"
-    rpy="0 0 3.14159"
+    rpy="0 0 0"
     zivid_model="ZIVID_2_PLUS_MR60"/>
 
   <!-- Tool block attached to the Zivid arm mount (not the camera) -->
diff --git a/src/custom-ur-descriptions/ur5e_robot_description/urdf/ur_with_zivid_hande.xacro b/src/custom-ur-descriptions/ur5e_robot_description/urdf/ur_with_zivid_hande.xacro
index 138ea070..b85a35b7 100644
--- a/src/custom-ur-descriptions/ur5e_robot_description/urdf/ur_with_zivid_hande.xacro
+++ b/src/custom-ur-descriptions/ur5e_robot_description/urdf/ur_with_zivid_hande.xacro
@@ -61,7 +61,7 @@
   <xacro:arg name="mock_hardware" default="true" />
 
   <!-- initial position for simulations (Fake Hardware, Gazebo, Ignition) -->
-  <xacro:arg name="initial_positions_file" default="$(find ur_description)/config/initial_positions.yaml"/>
+  <xacro:arg name="initial_positions_file" default="$(find ur_zivid_hande_moveit_config)/config/initial_positions.yaml"/>
   <xacro:property name="initial_positions_file" default="$(arg initial_positions_file)"/>
 
   <!-- create link fixed to the "map" -->
@@ -113,8 +113,8 @@
   <!-- Using tool0 instead of flange - Zivid calibration uses tool0 frame -->
   <xacro:zivid_camera_mount
     parent_link="$(arg tf_prefix)tool0"
-    xyz="0.00000 0.00000 0.00500"
-    rpy="0.00000 0.00000 3.14159"
+    xyz="0 0 0.005"
+    rpy="0 0 0"
     zivid_model="ZIVID_2_PLUS_MR60"/>
 
   <!-- Tool block attached to the Zivid arm mount (not the camera) -->
diff --git a/src/custom-ur-descriptions/ur5e_robot_description/urdf/ur_with_zivid_pipettor.xacro b/src/custom-ur-descriptions/ur5e_robot_description/urdf/ur_with_zivid_pipettor.xacro
index a0c60586..bb54bc7b 100644
--- a/src/custom-ur-descriptions/ur5e_robot_description/urdf/ur_with_zivid_pipettor.xacro
+++ b/src/custom-ur-descriptions/ur5e_robot_description/urdf/ur_with_zivid_pipettor.xacro
@@ -61,7 +61,7 @@
   <xacro:arg name="mock_hardware" default="true" />
 
   <!-- initial position for simulations (Fake Hardware, Gazebo, Ignition) -->
-  <xacro:arg name="initial_positions_file" default="$(find ur_description)/config/initial_positions.yaml"/>
+  <xacro:arg name="initial_positions_file" default="$(find ur_zivid_pipettor_moveit_config)/config/initial_positions.yaml"/>
   <xacro:property name="initial_positions_file" default="$(arg initial_positions_file)"/>
 
   <!-- create link fixed to the "map" -->
@@ -114,7 +114,7 @@
   <xacro:zivid_camera_mount
     parent_link="$(arg tf_prefix)tool0"
     xyz="0 0 0.005"
-    rpy="0 0 3.14159"
+    rpy="0 0 0"
     zivid_model="ZIVID_2_PLUS_MR60"/>
 
   <!-- Tool block attached to the Zivid arm mount (not the camera) -->
diff --git a/src/custom-ur-descriptions/ur5e_robot_description/urdf/zivid_camera_mount.xacro b/src/custom-ur-descriptions/ur5e_robot_description/urdf/zivid_camera_mount.xacro
index 1f4c3605..f59ec932 100644
--- a/src/custom-ur-descriptions/ur5e_robot_description/urdf/zivid_camera_mount.xacro
+++ b/src/custom-ur-descriptions/ur5e_robot_description/urdf/zivid_camera_mount.xacro
@@ -1,19 +1,60 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://ros.org/wiki/xacro" name="zivid_camera_mount">
 
-  <!-- Import official Zivid camera from zivid-ros -->
-  <xacro:include filename="$(find zivid_description)/urdf/macros/zivid_camera.xacro"/>
+  <!--
+    Zivid Camera Mount with Direct Hand-Eye Calibration
+
+    Frame chain:
+      parent_link (tool0) -> zivid_optical_frame (CALIBRATED - point cloud origin)
+      parent_link (tool0) -> zivid_arm_mount -> zivid_base_link (visual mesh only)
+
+    The hand-eye calibration gives us: tool0 -> optical_frame directly.
+    We DON'T use the zivid_camera macro's uncalibrated optical joint.
+  -->
 
   <xacro:macro name="zivid_camera_mount" params="parent_link xyz:='0 0 0' rpy:='0 0 0' zivid_model:='ZIVID_2_PLUS_MR60'">
 
-    <!-- ARM MOUNT LINK -->
+    <!-- ============================================================ -->
+    <!-- CALIBRATED OPTICAL FRAME (for point cloud data)              -->
+    <!-- This is where the Zivid point cloud is referenced            -->
+    <!-- Hand-eye calibration result: tool0 -> zivid_optical_frame    -->
+    <!-- ============================================================ -->
+
+    <link name="zivid_optical_frame"/>
+
+    <joint name="tool0_to_zivid_optical" type="fixed">
+      <parent link="${parent_link}"/>
+      <child link="zivid_optical_frame"/>
+      <!-- Hand-eye calibration: tool0 -> optical_frame -->
+      <!-- Rotation: Euler XYZ extrinsic, radians | Translation: meters -->
+      <!-- Calibration (2026-01-15): Residuals: rot < 0.22°, trans < 0.47mm -->
+      <origin xyz="0.05675 0.10322 0.05489"
+              rpy="-0.00615 0.04362 3.13541"/>
+      <!-- TEST (2026-01-22): Zero roll/pitch - did NOT help Y variance
+      <origin xyz="0.05675 0.10322 0.05489"
+              rpy="0 0 3.14159"/> -->
+      <!-- Calibration (2025-12-17):
+      <origin xyz="0.05659 0.10548 0.05660"
+              rpy="-0.01432 0.04829 3.13430"/> -->
+      <!-- Calibration (2026-01-13) - had offset issues:
+      <origin xyz="0.05646 0.10182 0.05680"
+              rpy="-0.03542 0.04745 3.13222"/> -->
+      <!-- Old calibration (2025-10-09):
+      <origin xyz="0.02803 0.07664 0.0"
+              rpy="0.53964 -1.53712 -2.13794"/> -->
+    </joint>
+
+    <!-- ============================================================ -->
+    <!-- ARM MOUNT (for attaching grippers via tool_block)            -->
+    <!-- ============================================================ -->
+
     <link name="zivid_arm_mount">
       <visual>
         <origin xyz="0 0 0" rpy="0 0 0"/>
         <geometry>
-          <!-- MOUNT MESH - Updated with custom mount from Zivid -->
-          <mesh filename="package://ur5e_robot_description/meshes/zivid/zivid_custom_mount.dae"
-                scale="1.0 1.0 1.0"/>
+          <!-- Official Zivid On-Arm Mount (from zivid.com/downloads) -->
+          <mesh filename="package://ur5e_robot_description/meshes/zivid/zivid_onarm_mount.stl"
+                scale="0.001 0.001 0.001"/>
         </geometry>
         <material name="mount_dark">
           <color rgba="0.2 0.2 0.2 1.0"/>
@@ -22,8 +63,8 @@
       <collision>
         <origin xyz="0 0 0" rpy="0 0 0"/>
         <geometry>
-          <mesh filename="package://ur5e_robot_description/meshes/zivid/zivid_custom_mount.dae"
-                scale="1.0 1.0 1.0"/>
+          <mesh filename="package://ur5e_robot_description/meshes/zivid/zivid_onarm_mount.stl"
+                scale="0.001 0.001 0.001"/>
         </geometry>
       </collision>
       <inertial>
@@ -35,26 +76,45 @@
       </inertial>
     </link>
 
-    <!-- Joint: Flange -> Arm Mount -->
     <joint name="zivid_arm_mount_joint" type="fixed">
       <parent link="${parent_link}"/>
       <child link="zivid_arm_mount"/>
       <origin xyz="${xyz}" rpy="${rpy}"/>
     </joint>
 
-    <!-- ZIVID CAMERA (From official zivid-ros)       -->
-    <xacro:zivid_camera
-      prefix="zivid_"
-      model="${zivid_model}"
-      field_of_view="false"/>
+    <!-- ============================================================ -->
+    <!-- CAMERA VISUAL MESH (for RViz visualization)                  -->
+    <!-- Positioned relative to optical frame using inverse offset    -->
+    <!-- ============================================================ -->
+
+    <link name="zivid_base_link">
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 ${0.5*pi}"/>
+        <geometry>
+          <mesh filename="package://zivid_description/meshes/visual/zivid_2_plus.stl"/>
+        </geometry>
+        <material name="zivid_gray">
+          <color rgba="0.3 0.3 0.3 1.0"/>
+        </material>
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 ${0.5*pi}"/>
+        <geometry>
+          <mesh filename="package://zivid_description/meshes/collision/zivid_2_plus_convex_hull.stl"/>
+        </geometry>
+      </collision>
+    </link>
 
-    <!-- Joint: Arm Mount -> Camera Base Link -->
-    <!-- Zivid calibration uses tool0 frame per: https://github.com/zivid/zivid-ros/issues/124 -->
-    <joint name="mount_to_camera_joint" type="fixed">
-      <parent link="zivid_arm_mount"/>
+    <!-- Position visual mesh relative to optical frame -->
+    <!-- INVERSE of Zivid's factory transform (base_link → optical_frame) -->
+    <!-- Source: zivid_description/urdf/macros/zivid_camera.xacro (ZIVID_2_PLUS_MR60) -->
+    <!-- Forward: xyz=(0.049, 0.03202, 0.0295), rpy=(-90°, 0°, -92.5°) -->
+    <!-- Inverse: T_inv = [R^T | -R^T * t] computed mathematically -->
+    <joint name="optical_to_base_link" type="fixed">
+      <parent link="zivid_optical_frame"/>
       <child link="zivid_base_link"/>
-      <origin xyz="0.02803 0.07664 -0.04577"
-              rpy="0.53964 -1.53712 -2.13794"/>
+      <origin xyz="0.03413 0.02950 -0.04756"
+              rpy="-1.57080 -1.52716 3.14159"/>
     </joint>
 
   </xacro:macro>