diff --git a/src/moveit_pro_franka_configs/franka_dual_arm_sim/config/config.yaml b/src/moveit_pro_franka_configs/franka_dual_arm_sim/config/config.yaml
index 9142ad56..9994a274 100644
--- a/src/moveit_pro_franka_configs/franka_dual_arm_sim/config/config.yaml
+++ b/src/moveit_pro_franka_configs/franka_dual_arm_sim/config/config.yaml
@@ -33,6 +33,7 @@ hardware:
     # [Optional]
     urdf_params:
       - mujoco_model: "description/mujoco/scene.xml"
+      - mujoco_viewer: false
     simulated_hardware_launch_file:
       package: "moveit_studio_agent"
       path: "launch/blank.launch.py"
@@ -129,6 +130,8 @@ ros2_control:
   # for running the application.
   # [Optional, default=[]]
   controllers_active_at_startup:
+    - "left_gripper_controller"
+    - "right_gripper_controller"
     - "joint_state_broadcaster"
     - "joint_trajectory_controller"
   # Load but do not start these controllers so they can be activated later if needed.
diff --git a/src/moveit_pro_franka_configs/franka_dual_arm_sim/config/control/franka_ros2_control.yaml b/src/moveit_pro_franka_configs/franka_dual_arm_sim/config/control/franka_ros2_control.yaml
index a1b8187f..389c2f68 100644
--- a/src/moveit_pro_franka_configs/franka_dual_arm_sim/config/control/franka_ros2_control.yaml
+++ b/src/moveit_pro_franka_configs/franka_dual_arm_sim/config/control/franka_ros2_control.yaml
@@ -15,6 +15,10 @@ controller_manager:
       type: velocity_force_controller/VelocityForceController
     right_velocity_force_controller:
       type: velocity_force_controller/VelocityForceController
+    left_gripper_controller:
+      type: position_controllers/GripperActionController
+    right_gripper_controller:
+      type: position_controllers/GripperActionController
 
 joint_trajectory_controller:
   ros__parameters:
@@ -38,6 +42,7 @@ joint_trajectory_controller:
       - right_fr3_joint5
       - right_fr3_joint6
       - right_fr3_joint7
+    allow_partial_joints_goal: true
     gains:
       left_fr3_joint1: { p: 600., d: 30., i: 0., i_clamp: 1. }
       left_fr3_joint2: { p: 600., d: 30., i: 0., i_clamp: 1. }
@@ -54,6 +59,22 @@ joint_trajectory_controller:
       right_fr3_joint6: { p: 150., d: 10., i: 0., i_clamp: 1. }
       right_fr3_joint7: { p: 50., d: 5., i: 0., i_clamp: 1. }
 
+left_gripper_controller:
+  ros__parameters:
+    default: true
+    joint: left_fr3_finger_joint1
+    allow_stalling: true
+    stall_timeout: 0.1
+    goal_tolerance: 0.005
+
+right_gripper_controller:
+  ros__parameters:
+    default: true
+    joint: right_fr3_finger_joint1
+    allow_stalling: true
+    stall_timeout: 0.1
+    goal_tolerance: 0.005
+
 left_joint_trajectory_controller:
   ros__parameters:
     command_interfaces:
@@ -154,7 +175,7 @@ left_velocity_force_controller:
     # Base frame of the kinematic chain, for computation of arm kinematics.
     base_frame: left_fr3_link0
     # The tip link of the kinematic chain, i.e. the frame that will be controlled.
-    ee_frame: left_fr3_link8
+    ee_frame: left_grasp_link
     # The frame where the force / torque sensor reading is given.
     # (Needs to exist in the kinematic chain).
     sensor_frame: left_fr3_link8
@@ -229,7 +250,7 @@ right_velocity_force_controller:
     # Base frame of the kinematic chain, for computation of arm kinematics.
     base_frame: right_fr3_link0
     # The tip link of the kinematic chain, i.e. the frame that will be controlled.
-    ee_frame: right_fr3_link8
+    ee_frame: right_grasp_link
     # The frame where the force / torque sensor reading is given.
     # (Needs to exist in the kinematic chain).
     sensor_frame: right_fr3_link8
diff --git a/src/moveit_pro_franka_configs/franka_dual_arm_sim/config/moveit/franka.srdf b/src/moveit_pro_franka_configs/franka_dual_arm_sim/config/moveit/franka.srdf
index 7e246a8a..9c14a7b5 100644
--- a/src/moveit_pro_franka_configs/franka_dual_arm_sim/config/moveit/franka.srdf
+++ b/src/moveit_pro_franka_configs/franka_dual_arm_sim/config/moveit/franka.srdf
@@ -1,48 +1,49 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <robot name="franka">
   <!-- Planning groups -->
-  <!-- Right arm -->
-  <group name="right_arm">
-    <chain base_link="right_fr3_link0" tip_link="right_fr3_link8"/>
-  </group>
-  <group name="right_hand">
-    <link name="right_fr3_hand"/>
+
+  <!-- All right gripper links, excluding arm. -->
+  <group name="right_gripper">
+    <link name="right_fr3_hand_tcp"/>  <!-- root of gripper -->
     <link name="right_fr3_leftfinger"/>
     <link name="right_fr3_rightfinger"/>
-    <link name="right_fr3_hand_tcp"/>
-    <!--<link name="right_grasp_link"/>-->
+    <link name="right_grasp_link"/>
     <joint name="right_fr3_finger_joint1"/>
     <passive_joint name="right_fr3_finger_joint2"/>
   </group>
+
+  <!-- All right robot links, including gripper. -->
   <group name="right_manipulator">
-    <chain base_link="right_fr3_link0" tip_link="right_fr3_link8"/>
+    <chain base_link="right_fr3_link0" tip_link="right_grasp_link"/>
   </group>
-  <!-- Left arm -->
-  <group name="left_arm">
-    <chain base_link="left_fr3_link0" tip_link="left_fr3_link8"/>
-  </group>
-  <group name="left_hand">
-    <link name="left_fr3_hand"/>
+
+  <!-- Right end effectors -->
+  <end_effector name="right_hand_tcp" parent_link="right_grasp_link" group="right_gripper" parent_group="right_manipulator"/>
+
+  <!-- All left gripper links, excluding arm. -->
+  <group name="left_gripper">
+    <link name="left_fr3_hand_tcp"/>
     <link name="left_fr3_leftfinger"/>
     <link name="left_fr3_rightfinger"/>
-    <link name="left_fr3_hand_tcp"/>
-    <!--<link name="left_grasp_link"/>-->
+    <link name="left_grasp_link"/>
     <joint name="left_fr3_finger_joint1"/>
     <passive_joint name="left_fr3_finger_joint2"/>
   </group>
+  
+  <!-- All left robot links, including gripper. -->
   <group name="left_manipulator">
-    <chain base_link="left_fr3_link0" tip_link="left_fr3_link8"/>
+    <chain base_link="left_fr3_link0" tip_link="left_grasp_link"/>
   </group>
 
+  <!-- Left end effectors -->
+  <end_effector name="left_hand_tcp" parent_link="left_grasp_link" group="left_gripper" parent_group="left_manipulator"/>
+  
   <!-- Group containing both arms -->
   <group name="manipulator">
     <group name="left_manipulator"/>
     <group name="right_manipulator"/>
   </group>
 
-  <end_effector name="left_hand_tcp" parent_link="left_fr3_link8" group="left_hand" parent_group="left_arm"/>
-  <end_effector name="right_hand_tcp" parent_link="right_fr3_link8" group="right_hand" parent_group="right_arm"/>
-
   <!-- Arm collisions -->
   <virtual_joint name="right_virtual_joint" type="fixed" parent_frame="right_mount" child_link="right_fr3_link0"/>
   <virtual_joint name="left_virtual_joint" type="fixed" parent_frame="left_mount" child_link="left_fr3_link0"/>
diff --git a/src/moveit_pro_franka_configs/franka_dual_arm_sim/config/moveit/pose_ik_distance.yaml b/src/moveit_pro_franka_configs/franka_dual_arm_sim/config/moveit/pose_ik_distance.yaml
index 3b50e2d7..b26894ac 100644
--- a/src/moveit_pro_franka_configs/franka_dual_arm_sim/config/moveit/pose_ik_distance.yaml
+++ b/src/moveit_pro_franka_configs/franka_dual_arm_sim/config/moveit/pose_ik_distance.yaml
@@ -1,18 +1,9 @@
-left_arm:
-  kinematics_solver: pose_ik_plugin/PoseIKPlugin
-  target_tolerance: 0.001
-  solve_mode: "optimize_distance"
-  optimization_timeout: 0.005
 left_manipulator:
   kinematics_solver: pose_ik_plugin/PoseIKPlugin
   target_tolerance: 0.001
   solve_mode: "optimize_distance"
   optimization_timeout: 0.005
-right_arm:
-  kinematics_solver: pose_ik_plugin/PoseIKPlugin
-  target_tolerance: 0.001
-  solve_mode: "optimize_distance"
-  optimization_timeout: 0.005
+
 right_manipulator:
   kinematics_solver: pose_ik_plugin/PoseIKPlugin
   target_tolerance: 0.001
diff --git a/src/moveit_pro_franka_configs/franka_dual_arm_sim/config/moveit/pose_ik_speed.yaml b/src/moveit_pro_franka_configs/franka_dual_arm_sim/config/moveit/pose_ik_speed.yaml
index 005fb007..3dd5c3fe 100644
--- a/src/moveit_pro_franka_configs/franka_dual_arm_sim/config/moveit/pose_ik_speed.yaml
+++ b/src/moveit_pro_franka_configs/franka_dual_arm_sim/config/moveit/pose_ik_speed.yaml
@@ -1,16 +1,9 @@
-right_arm:
+left_manipulator:
   kinematics_solver: pose_ik_plugin/PoseIKPlugin
   target_tolerance: 0.001
   solve_mode: "first_found"
+
 right_manipulator:
   kinematics_solver: pose_ik_plugin/PoseIKPlugin
   target_tolerance: 0.001
   solve_mode: "first_found"
-left_arm:
-  kinematics_solver: pose_ik_plugin/PoseIKPlugin
-  target_tolerance: 0.001
-  solve_mode: "first_found"
-left_manipulator:
-  kinematics_solver: pose_ik_plugin/PoseIKPlugin
-  target_tolerance: 0.001
-  solve_mode: "first_found"
diff --git a/src/moveit_pro_franka_configs/franka_dual_arm_sim/description/franka.urdf.xacro b/src/moveit_pro_franka_configs/franka_dual_arm_sim/description/franka.urdf.xacro
index 70e5170a..72556275 100644
--- a/src/moveit_pro_franka_configs/franka_dual_arm_sim/description/franka.urdf.xacro
+++ b/src/moveit_pro_franka_configs/franka_dual_arm_sim/description/franka.urdf.xacro
@@ -30,9 +30,11 @@
                       arm_prefix="right"
                       connected_to= "right_mount">
   </xacro:franka_robot>
+
+  <!-- This link aligns the axes of the grasp frame to our required convention for motion planning tasks. -->
   <link name="right_grasp_link" />
   <joint name="right_grasp_link_joint" type="fixed">
-     <parent link="right_fr3_link8"/>
+     <parent link="right_fr3_hand_tcp"/>
      <child link = "right_grasp_link"/>
      <origin xyz="0 0 0" rpy="0 0 -1.5708"/>
   </joint>
@@ -42,6 +44,7 @@
      <child link = "left_mount"/>
     <origin xyz="0.035 0.050681 0.356" rpy="-0.8936 -0.1746 -0.4636"/>
   </joint>
+
   <xacro:franka_robot arm_id="fr3"
                       joint_limits="${xacro.load_yaml('$(find franka_description)/robots/fr3/joint_limits.yaml')}"
                       inertials="${xacro.load_yaml('$(find franka_description)/robots/fr3/inertials.yaml')}"
@@ -61,10 +64,10 @@
                       connected_to= "left_mount">
   </xacro:franka_robot>
   
-    <!-- This link aligns the axes of the grasp frame to our required convention for motion planning tasks. -->
+   <!-- This link aligns the axes of the grasp frame to our required convention for motion planning tasks. -->
    <link name="left_grasp_link" />
    <joint name="left_grasp_link_joint" type="fixed">
-      <parent link="world"/>
+      <parent link="left_fr3_hand_tcp"/>
       <child link = "left_grasp_link"/>
       <origin xyz="0 0 0" rpy="0 0 -1.5708"/>
     </joint>
@@ -147,6 +150,17 @@
         <state_interface name="velocity"/>
         <state_interface name="effort"/>
       </joint>
+      <joint name="right_fr3_finger_joint1">
+        <command_interface name="position">
+          <param name="min">-0.0</param>
+          <param name="max">0.04</param>
+        </command_interface>
+        <state_interface name="position">
+          <param name="initial_value">0.035</param>
+        </state_interface>
+        <state_interface name="velocity"/>
+        <state_interface name="effort"/>
+      </joint>
       <joint name="left_fr3_joint1">
         <command_interface name="position">
           <param name="min">-2.8973</param>
@@ -224,14 +238,25 @@
         <state_interface name="velocity"/>
         <state_interface name="effort"/>
       </joint>
+      <joint name="left_fr3_finger_joint1">
+        <command_interface name="position">
+          <param name="min">-0.0</param>
+          <param name="max">0.04</param>
+        </command_interface>
+        <state_interface name="position">
+          <param name="initial_value">0.035</param>
+        </state_interface>
+        <state_interface name="velocity"/>
+        <state_interface name="effort"/>
+      </joint>
       <hardware>
         <plugin>picknik_mujoco_ros/MujocoSystem</plugin>
         <param name="mujoco_model">$(arg mujoco_model)</param>
         <param name="mujoco_model_package">franka_dual_arm_sim</param>
         <param name="render_publish_rate">10</param>
         <param name="tf_publish_rate">60</param>
-        <param name="mujoco_viewer">false</param>
         <param name="mujoco_keyframe">home</param>
+        <param name="mujoco_viewer">$(arg mujoco_viewer)</param>
       </hardware>
     </ros2_control>
 </robot>
diff --git a/src/moveit_pro_franka_configs/franka_dual_arm_sim/description/mujoco/dual_franka3.xml b/src/moveit_pro_franka_configs/franka_dual_arm_sim/description/mujoco/dual_franka3.xml
deleted file mode 100644
index 817aa44b..00000000
--- a/src/moveit_pro_franka_configs/franka_dual_arm_sim/description/mujoco/dual_franka3.xml
+++ /dev/null
@@ -1,853 +0,0 @@
-<mujoco model="fr3">
-  <option integrator="implicitfast" timestep="0.005" />
-
-  <default>
-    <default class="fr3">
-      <joint armature="0.1" damping="1" />
-      <position inheritrange="1" />
-      <default class="finger">
-        <joint axis="0 1 0" type="slide" range="0 0.04" />
-      </default>
-      <default class="visual">
-        <geom type="mesh" group="2" contype="0" conaffinity="0" />
-      </default>
-      <default class="collision">
-        <geom type="mesh" group="3" mass="0" density="0" />
-      </default>
-      <site size="0.001" rgba="0.5 0.5 0.5 0.3" group="4" />
-    </default>
-    <default class="fingertip_pad_collision_1">
-      <geom type="box" size="0.0085 0.004 0.0085" pos="0 0.0055 0.0445" />
-    </default>
-    <default class="fingertip_pad_collision_2">
-      <geom type="box" size="0.003 0.002 0.003" pos="0.0055 0.002 0.05" />
-    </default>
-    <default class="fingertip_pad_collision_3">
-      <geom type="box" size="0.003 0.002 0.003" pos="-0.0055 0.002 0.05" />
-    </default>
-    <default class="fingertip_pad_collision_4">
-      <geom type="box" size="0.003 0.002 0.0035" pos="0.0055 0.002 0.0395" />
-    </default>
-    <default class="fingertip_pad_collision_5">
-      <geom type="box" size="0.003 0.002 0.0035" pos="-0.0055 0.002 0.0395" />
-    </default>
-  </default>
-
-  <asset>
-    <material name="black" rgba=".2 .2 .2 1" />
-    <material name="white" rgba="1 1 1 1" />
-    <material name="off_white" rgba="0.901961 0.921569 0.929412 1" />
-    <material name="red" rgba="1 0.072272 0.039546 1" />
-    <material name="gray" rgba="0.863156 0.863156 0.863157 1" />
-    <material name="button_green" rgba="0.102241 0.571125 0.102242 1" />
-    <material name="button_red" rgba="0.520996 0.008023 0.013702 1" />
-    <material name="button_blue" rgba="0.024157 0.445201 0.737911 1" />
-
-    <mesh file="assets/link0_0.obj" />
-    <mesh file="assets/link0_1.obj" />
-    <mesh file="assets/link0_2.obj" />
-    <mesh file="assets/link0_3.obj" />
-    <mesh file="assets/link0_4.obj" />
-    <mesh file="assets/link0_5.obj" />
-    <mesh file="assets/link0_6.obj" />
-    <mesh file="assets/link1.obj" />
-    <mesh file="assets/link2.obj" />
-    <mesh file="assets/link3_0.obj" />
-    <mesh file="assets/link3_1.obj" />
-    <mesh file="assets/link4_0.obj" />
-    <mesh file="assets/link4_1.obj" />
-    <mesh file="assets/link5_0.obj" />
-    <mesh file="assets/link5_1.obj" />
-    <mesh file="assets/link5_2.obj" />
-    <mesh file="assets/link6_0.obj" />
-    <mesh file="assets/link6_1.obj" />
-    <mesh file="assets/link6_2.obj" />
-    <mesh file="assets/link6_3.obj" />
-    <mesh file="assets/link6_4.obj" />
-    <mesh file="assets/link6_5.obj" />
-    <mesh file="assets/link6_6.obj" />
-    <mesh file="assets/link6_7.obj" />
-    <mesh file="assets/link7_0.obj" />
-    <mesh file="assets/link7_1.obj" />
-    <mesh file="assets/link7_2.obj" />
-    <mesh file="assets/link7_3.obj" />
-
-    <mesh name="link0_coll" file="assets/link0.stl" />
-    <mesh name="link1_coll" file="assets/link1.stl" />
-    <mesh name="link2_coll" file="assets/link2.stl" />
-    <mesh name="link3_coll" file="assets/link3.stl" />
-    <mesh name="link4_coll" file="assets/link4.stl" />
-    <mesh name="link5_coll" file="assets/link5.stl" />
-    <mesh name="link6_coll" file="assets/link6.stl" />
-    <mesh name="link7_coll" file="assets/link7.stl" />
-    <mesh name="hand_coll" file="assets/hand.stl" />
-
-    <mesh file="assets/hand_0.obj" />
-    <mesh file="assets/hand_1.obj" />
-    <mesh file="assets/hand_2.obj" />
-    <mesh file="assets/hand_3.obj" />
-    <mesh file="assets/hand_4.obj" />
-    <mesh file="assets/finger_0.obj" />
-    <mesh file="assets/finger_1.obj" />
-  </asset>
-
-  <worldbody>
-    <body name="dual_arm_setup">
-      <!-- Left Arm -->
-
-      <body
-        name="left_arm"
-        pos="0 0.06 0.342"
-        quat="0.883098 -0.400776 -0.175322 -0.169642"
-      >
-        <body name="left_base" childclass="fr3">
-          <body name="left_fr3_link0">
-            <geom mesh="link0_0" material="black" class="visual" />
-            <geom mesh="link0_1" material="white" class="visual" />
-            <geom mesh="link0_2" material="white" class="visual" />
-            <geom mesh="link0_3" material="white" class="visual" />
-            <geom mesh="link0_4" material="white" class="visual" />
-            <geom mesh="link0_5" material="red" class="visual" />
-            <geom mesh="link0_6" material="black" class="visual" />
-            <geom
-              name="left_fr3_link0_collision"
-              mesh="link0_coll"
-              class="collision"
-            />
-            <body name="left_fr3_link1" pos="0 0 0.333">
-              <inertial
-                pos="4.128e-07 -0.0181251 -0.0386036"
-                quat="0.998098 -0.0605364 0.00380499 0.0110109"
-                mass="2.92747"
-                diaginertia="0.0239286 0.0227246 0.00610634"
-              />
-              <joint
-                name="left_fr3_joint1"
-                axis="0 0 1"
-                range="-2.7437 2.7437"
-                actuatorfrcrange="-87 87"
-              />
-              <geom
-                name="left_fr3_link1_collision"
-                class="collision"
-                mesh="link1_coll"
-              />
-              <geom material="white" mesh="link1" class="visual" />
-              <body name="left_fr3_link2" quat="1 -1 0 0">
-                <inertial
-                  pos="0.00318289 -0.0743222 0.00881461"
-                  quat="0.502599 0.584437 -0.465998 0.434366"
-                  mass="2.93554"
-                  diaginertia="0.0629567 0.0411924 0.0246371"
-                />
-                <joint
-                  name="left_fr3_joint2"
-                  axis="0 0 1"
-                  range="-1.7837 1.7837"
-                  actuatorfrcrange="-87 87"
-                />
-                <geom material="white" mesh="link2" class="visual" />
-                <geom
-                  name="left_fr3_link2_collision"
-                  class="collision"
-                  mesh="link2_coll"
-                />
-                <body name="left_fr3_link3" pos="0 -0.316 0" quat="1 1 0 0">
-                  <inertial
-                    pos="0.0407016 -0.00482006 -0.0289731"
-                    quat="0.921025 -0.244161 0.155272 0.260745"
-                    mass="2.2449"
-                    diaginertia="0.0267409 0.0189869 0.0171587"
-                  />
-                  <joint
-                    name="left_fr3_joint3"
-                    axis="0 0 1"
-                    range="-2.9007 2.9007"
-                    actuatorfrcrange="-87 87"
-                  />
-                  <geom mesh="link3_0" material="white" class="visual" />
-                  <geom mesh="link3_1" material="black" class="visual" />
-                  <geom
-                    name="left_fr3_link3_collision"
-                    class="collision"
-                    mesh="link3_coll"
-                  />
-                  <body name="left_fr3_link4" pos="0.0825 0 0" quat="1 1 0 0">
-                    <inertial
-                      pos="-0.0459101 0.0630493 -0.00851879"
-                      quat="0.438018 0.803311 0.00937812 0.403414"
-                      mass="2.6156"
-                      diaginertia="0.05139 0.0372717 0.0160047"
-                    />
-                    <joint
-                      name="left_fr3_joint4"
-                      axis="0 0 1"
-                      range="-3.0421 -0.1518"
-                      actuatorfrcrange="-87 87"
-                    />
-                    <geom mesh="link4_0" material="white" class="visual" />
-                    <geom mesh="link4_1" material="black" class="visual" />
-                    <geom
-                      name="left_fr3_link4_collision"
-                      class="collision"
-                      mesh="link4_coll"
-                    />
-                    <body
-                      name="left_fr3_link5"
-                      pos="-0.0825 0.384 0"
-                      quat="1 -1 0 0"
-                    >
-                      <inertial
-                        pos="-0.00160396 0.0292536 -0.0972966"
-                        quat="0.919031 0.125604 0.0751531 -0.366003"
-                        mass="2.32712"
-                        diaginertia="0.0579335 0.0449144 0.0130634"
-                      />
-                      <joint
-                        name="left_fr3_joint5"
-                        axis="0 0 1"
-                        range="-2.8065 2.8065"
-                        actuatorfrcrange="-12 12"
-                      />
-                      <geom mesh="link5_0" material="white" class="visual" />
-                      <geom mesh="link5_1" material="white" class="visual" />
-                      <geom mesh="link5_2" material="black" class="visual" />
-                      <geom
-                        name="left_fr3_link5_collision"
-                        class="collision"
-                        mesh="link5_coll"
-                      />
-                      <body name="left_fr3_link6" quat="1 1 0 0">
-                        <inertial
-                          pos="0.0597131 -0.0410295 -0.0101693"
-                          quat="0.621301 0.552665 0.510011 0.220081"
-                          mass="1.81704"
-                          diaginertia="0.0175039 0.0161123 0.00193529"
-                        />
-                        <joint
-                          name="left_fr3_joint6"
-                          axis="0 0 1"
-                          range="0.5445 4.5169"
-                          actuatorfrcrange="-12 12"
-                        />
-                        <geom
-                          mesh="link6_0"
-                          material="button_green"
-                          class="visual"
-                        />
-                        <geom mesh="link6_1" material="white" class="visual" />
-                        <geom mesh="link6_2" material="white" class="visual" />
-                        <geom mesh="link6_3" material="gray" class="visual" />
-                        <geom
-                          mesh="link6_4"
-                          material="button_red"
-                          class="visual"
-                        />
-                        <geom mesh="link6_5" material="white" class="visual" />
-                        <geom mesh="link6_6" material="black" class="visual" />
-                        <geom
-                          mesh="link6_7"
-                          material="button_blue"
-                          class="visual"
-                        />
-                        <geom
-                          name="left_fr3_link6_collision"
-                          class="collision"
-                          mesh="link6_coll"
-                        />
-                        <body
-                          name="left_fr3_link7"
-                          pos="0.088 0 0"
-                          quat="1 1 0 0"
-                        >
-                          <inertial
-                            pos="0.00452258 0.00862619 -0.0161633"
-                            quat="0.727579 0.0978688 -0.24906 0.63168"
-                            mass="0.627143"
-                            diaginertia="0.000223836 0.000223642 5.64132e-07"
-                          />
-                          <joint
-                            name="left_fr3_joint7"
-                            axis="0 0 1"
-                            range="-3.0159 3.0159"
-                            actuatorfrcrange="-12 12"
-                          />
-                          <geom
-                            mesh="link7_0"
-                            material="black"
-                            class="visual"
-                          />
-                          <geom
-                            mesh="link7_1"
-                            material="white"
-                            class="visual"
-                          />
-                          <geom
-                            mesh="link7_2"
-                            material="white"
-                            class="visual"
-                          />
-                          <geom
-                            mesh="link7_3"
-                            material="black"
-                            class="visual"
-                          />
-                          <geom
-                            name="left_fr3_link7_collision"
-                            class="collision"
-                            mesh="link7_coll"
-                          />
-                          <site name="left_attachment_site" pos="0 0 0.107" />
-                          <body
-                            name="left_hand"
-                            pos="0 0 0.107"
-                            quat="0.9238795 0 0 -0.3826834"
-                          >
-                            <inertial
-                              mass="0.73"
-                              pos="-0.01 0 0.03"
-                              diaginertia="0.001 0.0025 0.0017"
-                            />
-                            <geom
-                              mesh="hand_0"
-                              material="off_white"
-                              class="visual"
-                            />
-                            <geom
-                              mesh="hand_1"
-                              material="black"
-                              class="visual"
-                            />
-                            <geom
-                              mesh="hand_2"
-                              material="black"
-                              class="visual"
-                            />
-                            <geom
-                              mesh="hand_3"
-                              material="white"
-                              class="visual"
-                            />
-                            <geom
-                              mesh="hand_4"
-                              material="off_white"
-                              class="visual"
-                            />
-                            <geom mesh="hand_coll" class="collision" />
-                            <body name="left_left_finger" pos="0 0 0.0584">
-                              <inertial
-                                mass="0.015"
-                                pos="0 0 0"
-                                diaginertia="2.375e-6 2.375e-6 7.5e-7"
-                              />
-                              <joint
-                                name="left_fr3_finger_joint1"
-                                class="finger"
-                              />
-                              <geom
-                                mesh="finger_0"
-                                material="off_white"
-                                class="visual"
-                              />
-                              <geom
-                                mesh="finger_1"
-                                material="black"
-                                class="visual"
-                              />
-                              <geom mesh="finger_0" class="collision" />
-                              <geom class="fingertip_pad_collision_1" />
-                              <geom class="fingertip_pad_collision_2" />
-                              <geom class="fingertip_pad_collision_3" />
-                              <geom class="fingertip_pad_collision_4" />
-                              <geom class="fingertip_pad_collision_5" />
-                            </body>
-                            <body
-                              name="left_right_finger"
-                              pos="0 0 0.0584"
-                              quat="0 0 0 1"
-                            >
-                              <inertial
-                                mass="0.015"
-                                pos="0 0 0"
-                                diaginertia="2.375e-6 2.375e-6 7.5e-7"
-                              />
-                              <joint
-                                name="left_fr3_finger_joint2"
-                                class="finger"
-                              />
-                              <geom
-                                mesh="finger_0"
-                                material="off_white"
-                                class="visual"
-                              />
-                              <geom
-                                mesh="finger_1"
-                                material="black"
-                                class="visual"
-                              />
-                              <geom mesh="finger_0" class="collision" />
-                              <geom class="fingertip_pad_collision_1" />
-                              <geom class="fingertip_pad_collision_2" />
-                              <geom class="fingertip_pad_collision_3" />
-                              <geom class="fingertip_pad_collision_4" />
-                              <geom class="fingertip_pad_collision_5" />
-                            </body>
-                          </body>
-                        </body>
-                      </body>
-                    </body>
-                  </body>
-                </body>
-              </body>
-            </body>
-          </body>
-        </body>
-      </body>
-
-      <!-- Right Arm -->
-
-      <body
-        name="right_arm"
-        pos="0 -0.06 0.342"
-        quat="0.883098 0.400776 -0.175322 0.169642"
-      >
-        <body name="right_base" childclass="fr3">
-          <body name="right_fr3_link0">
-            <geom mesh="link0_0" material="black" class="visual" />
-            <geom mesh="link0_1" material="white" class="visual" />
-            <geom mesh="link0_2" material="white" class="visual" />
-            <geom mesh="link0_3" material="white" class="visual" />
-            <geom mesh="link0_4" material="white" class="visual" />
-            <geom mesh="link0_5" material="red" class="visual" />
-            <geom mesh="link0_6" material="black" class="visual" />
-            <geom
-              name="right_fr3_link0_collision"
-              mesh="link0_coll"
-              class="collision"
-            />
-            <body name="right_fr3_link1" pos="0 0 0.333">
-              <inertial
-                pos="4.128e-07 -0.0181251 -0.0386036"
-                quat="0.998098 -0.0605364 0.00380499 0.0110109"
-                mass="2.92747"
-                diaginertia="0.0239286 0.0227246 0.00610634"
-              />
-              <joint
-                name="right_fr3_joint1"
-                axis="0 0 1"
-                range="-2.7437 2.7437"
-                actuatorfrcrange="-87 87"
-              />
-              <geom
-                name="right_fr3_link1_collision"
-                class="collision"
-                mesh="link1_coll"
-              />
-              <geom material="white" mesh="link1" class="visual" />
-              <body name="right_fr3_link2" quat="1 -1 0 0">
-                <inertial
-                  pos="0.00318289 -0.0743222 0.00881461"
-                  quat="0.502599 0.584437 -0.465998 0.434366"
-                  mass="2.93554"
-                  diaginertia="0.0629567 0.0411924 0.0246371"
-                />
-                <joint
-                  name="right_fr3_joint2"
-                  axis="0 0 1"
-                  range="-1.7837 1.7837"
-                  actuatorfrcrange="-87 87"
-                />
-                <geom material="white" mesh="link2" class="visual" />
-                <geom
-                  name="right_fr3_link2_collision"
-                  class="collision"
-                  mesh="link2_coll"
-                />
-                <body name="right_fr3_link3" pos="0 -0.316 0" quat="1 1 0 0">
-                  <inertial
-                    pos="0.0407016 -0.00482006 -0.0289731"
-                    quat="0.921025 -0.244161 0.155272 0.260745"
-                    mass="2.2449"
-                    diaginertia="0.0267409 0.0189869 0.0171587"
-                  />
-                  <joint
-                    name="right_fr3_joint3"
-                    axis="0 0 1"
-                    range="-2.9007 2.9007"
-                    actuatorfrcrange="-87 87"
-                  />
-                  <geom mesh="link3_0" material="white" class="visual" />
-                  <geom mesh="link3_1" material="black" class="visual" />
-                  <geom
-                    name="right_fr3_link3_collision"
-                    class="collision"
-                    mesh="link3_coll"
-                  />
-                  <body name="right_fr3_link4" pos="0.0825 0 0" quat="1 1 0 0">
-                    <inertial
-                      pos="-0.0459101 0.0630493 -0.00851879"
-                      quat="0.438018 0.803311 0.00937812 0.403414"
-                      mass="2.6156"
-                      diaginertia="0.05139 0.0372717 0.0160047"
-                    />
-                    <joint
-                      name="right_fr3_joint4"
-                      axis="0 0 1"
-                      range="-3.0421 -0.1518"
-                      actuatorfrcrange="-87 87"
-                    />
-                    <geom mesh="link4_0" material="white" class="visual" />
-                    <geom mesh="link4_1" material="black" class="visual" />
-                    <geom
-                      name="right_fr3_link4_collision"
-                      class="collision"
-                      mesh="link4_coll"
-                    />
-                    <body
-                      name="right_fr3_link5"
-                      pos="-0.0825 0.384 0"
-                      quat="1 -1 0 0"
-                    >
-                      <inertial
-                        pos="-0.00160396 0.0292536 -0.0972966"
-                        quat="0.919031 0.125604 0.0751531 -0.366003"
-                        mass="2.32712"
-                        diaginertia="0.0579335 0.0449144 0.0130634"
-                      />
-                      <joint
-                        name="right_fr3_joint5"
-                        axis="0 0 1"
-                        range="-2.8065 2.8065"
-                        actuatorfrcrange="-12 12"
-                      />
-                      <geom mesh="link5_0" material="white" class="visual" />
-                      <geom mesh="link5_1" material="white" class="visual" />
-                      <geom mesh="link5_2" material="black" class="visual" />
-                      <geom
-                        name="right_fr3_link5_collision"
-                        class="collision"
-                        mesh="link5_coll"
-                      />
-                      <body name="right_fr3_link6" quat="1 1 0 0">
-                        <inertial
-                          pos="0.0597131 -0.0410295 -0.0101693"
-                          quat="0.621301 0.552665 0.510011 0.220081"
-                          mass="1.81704"
-                          diaginertia="0.0175039 0.0161123 0.00193529"
-                        />
-                        <joint
-                          name="right_fr3_joint6"
-                          axis="0 0 1"
-                          range="0.5445 4.5169"
-                          actuatorfrcrange="-12 12"
-                        />
-                        <geom
-                          mesh="link6_0"
-                          material="button_green"
-                          class="visual"
-                        />
-                        <geom mesh="link6_1" material="white" class="visual" />
-                        <geom mesh="link6_2" material="white" class="visual" />
-                        <geom mesh="link6_3" material="gray" class="visual" />
-                        <geom
-                          mesh="link6_4"
-                          material="button_red"
-                          class="visual"
-                        />
-                        <geom mesh="link6_5" material="white" class="visual" />
-                        <geom mesh="link6_6" material="black" class="visual" />
-                        <geom
-                          mesh="link6_7"
-                          material="button_blue"
-                          class="visual"
-                        />
-                        <geom
-                          name="right_fr3_link6_collision"
-                          class="collision"
-                          mesh="link6_coll"
-                        />
-                        <body
-                          name="right_fr3_link7"
-                          pos="0.088 0 0"
-                          quat="1 1 0 0"
-                        >
-                          <inertial
-                            pos="0.00452258 0.00862619 -0.0161633"
-                            quat="0.727579 0.0978688 -0.24906 0.63168"
-                            mass="0.627143"
-                            diaginertia="0.000223836 0.000223642 5.64132e-07"
-                          />
-                          <joint
-                            name="right_fr3_joint7"
-                            axis="0 0 1"
-                            range="-3.0159 3.0159"
-                            actuatorfrcrange="-12 12"
-                          />
-                          <geom
-                            mesh="link7_0"
-                            material="black"
-                            class="visual"
-                          />
-                          <geom
-                            mesh="link7_1"
-                            material="white"
-                            class="visual"
-                          />
-                          <geom
-                            mesh="link7_2"
-                            material="white"
-                            class="visual"
-                          />
-                          <geom
-                            mesh="link7_3"
-                            material="black"
-                            class="visual"
-                          />
-                          <geom
-                            name="right_fr3_link7_collision"
-                            class="collision"
-                            mesh="link7_coll"
-                          />
-                          <site name="right_attachment_site" pos="0 0 0.107" />
-                          <body
-                            name="right_hand"
-                            pos="0 0 0.107"
-                            quat="0.9238795 0 0 -0.3826834"
-                          >
-                            <inertial
-                              mass="0.73"
-                              pos="-0.01 0 0.03"
-                              diaginertia="0.001 0.0025 0.0017"
-                            />
-                            <geom
-                              mesh="hand_0"
-                              material="off_white"
-                              class="visual"
-                            />
-                            <geom
-                              mesh="hand_1"
-                              material="black"
-                              class="visual"
-                            />
-                            <geom
-                              mesh="hand_2"
-                              material="black"
-                              class="visual"
-                            />
-                            <geom
-                              mesh="hand_3"
-                              material="white"
-                              class="visual"
-                            />
-                            <geom
-                              mesh="hand_4"
-                              material="off_white"
-                              class="visual"
-                            />
-                            <geom mesh="hand_coll" class="collision" />
-                            <body name="right_left_finger" pos="0 0 0.0584">
-                              <inertial
-                                mass="0.015"
-                                pos="0 0 0"
-                                diaginertia="2.375e-6 2.375e-6 7.5e-7"
-                              />
-                              <joint
-                                name="right_fr3_finger_joint1"
-                                class="finger"
-                              />
-                              <geom
-                                mesh="finger_0"
-                                material="off_white"
-                                class="visual"
-                              />
-                              <geom
-                                mesh="finger_1"
-                                material="black"
-                                class="visual"
-                              />
-                              <geom mesh="finger_0" class="collision" />
-                              <geom class="fingertip_pad_collision_1" />
-                              <geom class="fingertip_pad_collision_2" />
-                              <geom class="fingertip_pad_collision_3" />
-                              <geom class="fingertip_pad_collision_4" />
-                              <geom class="fingertip_pad_collision_5" />
-                            </body>
-                            <body
-                              name="right_right_finger"
-                              pos="0 0 0.0584"
-                              quat="0 0 0 1"
-                            >
-                              <inertial
-                                mass="0.015"
-                                pos="0 0 0"
-                                diaginertia="2.375e-6 2.375e-6 7.5e-7"
-                              />
-                              <joint
-                                name="right_fr3_finger_joint2"
-                                class="finger"
-                              />
-                              <geom
-                                mesh="finger_0"
-                                material="off_white"
-                                class="visual"
-                              />
-                              <geom
-                                mesh="finger_1"
-                                material="black"
-                                class="visual"
-                              />
-                              <geom mesh="finger_0" class="collision" />
-                              <geom class="fingertip_pad_collision_1" />
-                              <geom class="fingertip_pad_collision_2" />
-                              <geom class="fingertip_pad_collision_3" />
-                              <geom class="fingertip_pad_collision_4" />
-                              <geom class="fingertip_pad_collision_5" />
-                            </body>
-                          </body>
-                        </body>
-                      </body>
-                    </body>
-                  </body>
-                </body>
-              </body>
-            </body>
-          </body>
-        </body>
-      </body>
-    </body>
-  </worldbody>
-
-  <actuator>
-    <!-- left arm -->
-
-    <position
-      class="fr3"
-      name="left_fr3_joint1"
-      joint="left_fr3_joint1"
-      kp="4500"
-      kv="450"
-    />
-    <position
-      class="fr3"
-      name="left_fr3_joint2"
-      joint="left_fr3_joint2"
-      kp="4500"
-      kv="450"
-    />
-    <position
-      class="fr3"
-      name="left_fr3_joint3"
-      joint="left_fr3_joint3"
-      kp="3500"
-      kv="350"
-    />
-    <position
-      class="fr3"
-      name="left_fr3_joint4"
-      joint="left_fr3_joint4"
-      kp="3500"
-      kv="350"
-    />
-    <position
-      class="fr3"
-      name="left_fr3_joint5"
-      joint="left_fr3_joint5"
-      kp="2000"
-      kv="200"
-    />
-    <position
-      class="fr3"
-      name="left_fr3_joint6"
-      joint="left_fr3_joint6"
-      kp="2000"
-      kv="200"
-    />
-    <position
-      class="fr3"
-      name="left_fr3_joint7"
-      joint="left_fr3_joint7"
-      kp="2000"
-      kv="200"
-    />
-    <position
-      class="fr3"
-      name="left_fr3_finger_joint1"
-      joint="left_fr3_finger_joint1"
-      kp="1000"
-      kv="100"
-    />
-    <position
-      class="fr3"
-      name="left_fr3_finger_joint2"
-      joint="left_fr3_finger_joint2"
-      kp="1000"
-      kv="100"
-    />
-
-    <!-- Right arm -->
-
-    <position
-      class="fr3"
-      name="right_fr3_joint1"
-      joint="right_fr3_joint1"
-      kp="4500"
-      kv="450"
-    />
-    <position
-      class="fr3"
-      name="right_fr3_joint2"
-      joint="right_fr3_joint2"
-      kp="4500"
-      kv="450"
-    />
-    <position
-      class="fr3"
-      name="right_fr3_joint3"
-      joint="right_fr3_joint3"
-      kp="3500"
-      kv="350"
-    />
-    <position
-      class="fr3"
-      name="right_fr3_joint4"
-      joint="right_fr3_joint4"
-      kp="3500"
-      kv="350"
-    />
-    <position
-      class="fr3"
-      name="right_fr3_joint5"
-      joint="right_fr3_joint5"
-      kp="2000"
-      kv="200"
-    />
-    <position
-      class="fr3"
-      name="right_fr3_joint6"
-      joint="right_fr3_joint6"
-      kp="2000"
-      kv="200"
-    />
-    <position
-      class="fr3"
-      name="right_fr3_joint7"
-      joint="right_fr3_joint7"
-      kp="2000"
-      kv="200"
-    />
-    <position
-      class="fr3"
-      name="right_fr3_finger_joint1"
-      joint="right_fr3_finger_joint1"
-      kp="1000"
-      kv="100"
-    />
-    <position
-      class="fr3"
-      name="right_fr3_finger_joint2"
-      joint="right_fr3_finger_joint2"
-      kp="1000"
-      kv="100"
-    />
-  </actuator>
-</mujoco>
diff --git a/src/moveit_pro_franka_configs/franka_dual_arm_sim/description/mujoco/franka3.xml b/src/moveit_pro_franka_configs/franka_dual_arm_sim/description/mujoco/franka3.xml
index ff7a2d88..acb31f15 100644
--- a/src/moveit_pro_franka_configs/franka_dual_arm_sim/description/mujoco/franka3.xml
+++ b/src/moveit_pro_franka_configs/franka_dual_arm_sim/description/mujoco/franka3.xml
@@ -775,15 +775,8 @@
       class="fr3"
       name="left_fr3_finger_joint1"
       joint="left_fr3_finger_joint1"
-      kp="1000"
-      kv="100"
-    />
-    <position
-      class="fr3"
-      name="left_fr3_finger_joint2"
-      joint="left_fr3_finger_joint2"
-      kp="1000"
-      kv="100"
+      kp="5000"
+      kv="500"
     />
 
     <!-- Right arm -->
@@ -841,15 +834,24 @@
       class="fr3"
       name="right_fr3_finger_joint1"
       joint="right_fr3_finger_joint1"
-      kp="1000"
-      kv="100"
-    />
-    <position
-      class="fr3"
-      name="right_fr3_finger_joint2"
-      joint="right_fr3_finger_joint2"
-      kp="1000"
-      kv="100"
+      kp="5000"
+      kv="500"
     />
   </actuator>
+
+  <equality>
+    <joint
+      joint1="left_fr3_finger_joint1"
+      joint2="left_fr3_finger_joint2"
+      polycoef="0 1 0 0 0"
+    />
+  </equality>
+
+  <equality>
+    <joint
+      joint1="right_fr3_finger_joint1"
+      joint2="right_fr3_finger_joint2"
+      polycoef="0 1 0 0 0"
+    />
+  </equality>
 </mujoco>
diff --git a/src/moveit_pro_franka_configs/franka_dual_arm_sim/description/mujoco/scene.xml b/src/moveit_pro_franka_configs/franka_dual_arm_sim/description/mujoco/scene.xml
index 0add24f2..3b7cab19 100644
--- a/src/moveit_pro_franka_configs/franka_dual_arm_sim/description/mujoco/scene.xml
+++ b/src/moveit_pro_franka_configs/franka_dual_arm_sim/description/mujoco/scene.xml
@@ -57,14 +57,72 @@
   </worldbody>
 
   <keyframe>
+    <!-- Home position for FR3 robot with gripper
+       qpos: 
+         right_fr3_joint1
+         right_fr3_joint2
+         right_fr3_joint3
+         right_fr3_joint4
+         right_fr3_joint5
+         right_fr3_joint6
+         right_fr3_joint7
+         right_fr3_finger1
+         right_fr3_finger2
+         left_fr3_joint1
+         left_fr3_joint2
+         left_fr3_joint3
+         left_fr3_joint4
+         left_fr3_joint5
+         left_fr3_joint6
+         left_fr3_joint7
+         left_fr3_finger1
+         left_fr3_finger2
+       qvel:
+         right_fr3_joint1
+         right_fr3_joint2
+         right_fr3_joint3
+         right_fr3_joint4
+         right_fr3_joint5
+         right_fr3_joint6
+         right_fr3_joint7
+         right_fr3_finger1
+         right_fr3_finger2
+         left_fr3_joint1
+         left_fr3_joint2
+         left_fr3_joint3
+         left_fr3_joint4
+         left_fr3_joint5
+         left_fr3_joint6
+         left_fr3_joint7
+         left_fr3_finger1
+         left_fr3_finger2
+       ctrl: 
+         right_fr3_joint1
+         right_fr3_joint2
+         right_fr3_joint3
+         right_fr3_joint4
+         right_fr3_joint5
+         right_fr3_joint6
+         right_fr3_joint7
+         right_fr3_finger1 - We control only one finger.
+         left_fr3_joint1
+         left_fr3_joint2
+         left_fr3_joint3
+         left_fr3_joint4
+         left_fr3_joint5
+         left_fr3_joint6
+         left_fr3_joint7
+         left_fr3_finger1 - We control only one finger.
+
+    -->
     <key
       name="home"
       qpos="0 -0.7854 0 -2.3562 0 1.5707 0.7853 0 0
             0 -0.7854 0 -2.3562 0 1.5707 0.7853 0 0"
       qvel="0 0 0 0 0 0 0 0 0
             0 0 0 0 0 0 0 0 0"
-      ctrl="0 -0.7854 0 -2.3562 0 1.5707 0.7853 0 0
-            0 -0.7854 0 -2.3562 0 1.5707 0.7853 0 0"
+      ctrl="0 -0.7854 0 -2.3562 0 1.5707 0.7853 0
+            0 -0.7854 0 -2.3562 0 1.5707 0.7853 0"
     />
   </keyframe>
 </mujoco>
diff --git a/src/moveit_pro_franka_configs/franka_dual_arm_sim/objectives/close_gripper.xml b/src/moveit_pro_franka_configs/franka_dual_arm_sim/objectives/close_gripper.xml
new file mode 100644
index 00000000..5740aa21
--- /dev/null
+++ b/src/moveit_pro_franka_configs/franka_dual_arm_sim/objectives/close_gripper.xml
@@ -0,0 +1,26 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<root BTCPP_format="4" main_tree_to_execute="Close Gripper">
+  <!--//////////-->
+  <BehaviorTree
+    ID="Close Gripper"
+    _description="Close the right gripper by default"
+    _favorite="true"
+  >
+    <Control ID="Sequence" name="TopLevelSequence">
+      <Action
+        ID="MoveGripperAction"
+        gripper_command_action_name="/right_gripper_controller/gripper_cmd"
+        position="0"
+        timeout="10.000000"
+      />
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Close Gripper">
+      <MetadataFields>
+        <Metadata runnable="true" />
+        <Metadata subcategory="Grasping" />
+      </MetadataFields>
+    </SubTree>
+  </TreeNodesModel>
+</root>
diff --git a/src/moveit_pro_franka_configs/franka_dual_arm_sim/objectives/close_left_gripper.xml b/src/moveit_pro_franka_configs/franka_dual_arm_sim/objectives/close_left_gripper.xml
new file mode 100644
index 00000000..cbb9a12c
--- /dev/null
+++ b/src/moveit_pro_franka_configs/franka_dual_arm_sim/objectives/close_left_gripper.xml
@@ -0,0 +1,26 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<root BTCPP_format="4" main_tree_to_execute="Close Left Gripper">
+  <!--//////////-->
+  <BehaviorTree
+    ID="Close Left Gripper"
+    _description="Close the left gripper"
+    _favorite="true"
+  >
+    <Control ID="Sequence" name="TopLevelSequence">
+      <Action
+        ID="MoveGripperAction"
+        gripper_command_action_name="/left_gripper_controller/gripper_cmd"
+        position="0"
+        timeout="10.000000"
+      />
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Close Left Gripper">
+      <MetadataFields>
+        <Metadata runnable="true" />
+        <Metadata subcategory="Grasping" />
+      </MetadataFields>
+    </SubTree>
+  </TreeNodesModel>
+</root>
diff --git a/src/moveit_pro_franka_configs/franka_dual_arm_sim/objectives/close_right_gripper.xml b/src/moveit_pro_franka_configs/franka_dual_arm_sim/objectives/close_right_gripper.xml
new file mode 100644
index 00000000..a84cf62c
--- /dev/null
+++ b/src/moveit_pro_franka_configs/franka_dual_arm_sim/objectives/close_right_gripper.xml
@@ -0,0 +1,26 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<root BTCPP_format="4" main_tree_to_execute="Close Right Gripper">
+  <!--//////////-->
+  <BehaviorTree
+    ID="Close Right Gripper"
+    _description="Close the right gripper"
+    _favorite="true"
+  >
+    <Control ID="Sequence" name="TopLevelSequence">
+      <Action
+        ID="MoveGripperAction"
+        gripper_command_action_name="/right_gripper_controller/gripper_cmd"
+        position="0"
+        timeout="10.000000"
+      />
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Close Right Gripper">
+      <MetadataFields>
+        <Metadata runnable="true" />
+        <Metadata subcategory="Grasping" />
+      </MetadataFields>
+    </SubTree>
+  </TreeNodesModel>
+</root>
diff --git a/src/moveit_pro_franka_configs/franka_dual_arm_sim/objectives/draw.xml b/src/moveit_pro_franka_configs/franka_dual_arm_sim/objectives/draw.xml
index fb340af1..d9d31011 100644
--- a/src/moveit_pro_franka_configs/franka_dual_arm_sim/objectives/draw.xml
+++ b/src/moveit_pro_franka_configs/franka_dual_arm_sim/objectives/draw.xml
@@ -15,7 +15,16 @@
         publish_rate="50"
       />
       <Control ID="Sequence" name="TopLevelSequence">
-        <Action ID="ActivateControllers" controller_names="{controller_name}" />
+        <Action
+          ID="SwitchController"
+          activate_asap="true"
+          automatic_deactivation="true"
+          controller_list_action_name="/controller_manager/list_controllers"
+          controller_switch_action_name="/controller_manager/switch_controller"
+          strictness="1"
+          timeout="-1.000000"
+          activate_controllers="{controller_name}"
+        />
         <Action
           ID="LoadPoseStampedVectorFromYaml"
           output="{pose_stamped_msgs}"
@@ -25,6 +34,9 @@
           ID="VisualizePath"
           marker_lifetime="0.000000"
           path="{pose_stamped_msgs}"
+          marker_name="path"
+          pose_marker_size="0.100000"
+          show_poses="false"
         />
         <Action
           ID="PlanCartesianPath"
@@ -48,6 +60,7 @@
           goal_position_tolerance="0.000000"
           goal_time_tolerance="0.000000"
           goal_duration_tolerance="-1.000000"
+          trajectory_remainder="{joint_trajectory_remainder}"
         />
       </Control>
     </Control>
diff --git a/src/moveit_pro_franka_configs/franka_dual_arm_sim/objectives/draw_and_wipe.xml b/src/moveit_pro_franka_configs/franka_dual_arm_sim/objectives/draw_and_wipe.xml
index acd1279c..75f083c5 100644
--- a/src/moveit_pro_franka_configs/franka_dual_arm_sim/objectives/draw_and_wipe.xml
+++ b/src/moveit_pro_franka_configs/franka_dual_arm_sim/objectives/draw_and_wipe.xml
@@ -14,52 +14,24 @@
         file_path="{file_path}"
         controller_name="right_joint_trajectory_controller"
         execute_follow_joint_trajectory_action_name="/right_joint_trajectory_controller/follow_joint_trajectory"
-        planning_group_name="right_arm"
-        tip_link="right_fr3_link8"
+        planning_group_name="right_manipulator"
+        tip_link="right_grasp_link"
         center="{center}"
         name="Write a text"
       />
-      <SubTree
-        ID="Move to Waypoint"
-        _collapsed="true"
-        acceleration_scale_factor="1.0"
-        controller_action_server="/right_joint_trajectory_controller/follow_joint_trajectory"
-        controller_names="right_joint_trajectory_controller"
-        joint_group_name="right_arm"
-        keep_orientation="false"
-        keep_orientation_tolerance="0.05"
-        link_padding="0.01"
-        seed="0"
-        velocity_scale_factor="1.0"
-        waypoint_name="Right Home"
-        name="Move right arm to home position"
-      />
+      <SubTree ID="Move Right Home" _collapsed="true" />
       <SubTree
         ID="Draw"
         _collapsed="true"
         file_path="wipe_zig_zag.yaml"
         controller_name="left_joint_trajectory_controller"
         execute_follow_joint_trajectory_action_name="/left_joint_trajectory_controller/follow_joint_trajectory"
-        planning_group_name="left_arm"
-        tip_link="left_fr3_link8"
+        planning_group_name="left_manipulator"
+        tip_link="left_grasp_link"
         center="{center}"
         name="Wipe out the text"
       />
-      <SubTree
-        ID="Move to Waypoint"
-        _collapsed="true"
-        acceleration_scale_factor="1.0"
-        controller_action_server="/left_joint_trajectory_controller/follow_joint_trajectory"
-        controller_names="left_joint_trajectory_controller"
-        joint_group_name="left_arm"
-        keep_orientation="false"
-        keep_orientation_tolerance="0.05"
-        link_padding="0.01"
-        seed="0"
-        velocity_scale_factor="1.0"
-        waypoint_name="Left Home"
-        name="Move left arm to home position"
-      />
+      <SubTree ID="Move Left Home" _collapsed="true" />
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
diff --git a/src/moveit_pro_franka_configs/franka_dual_arm_sim/objectives/move_left_home.xml b/src/moveit_pro_franka_configs/franka_dual_arm_sim/objectives/move_left_home.xml
new file mode 100644
index 00000000..5079562c
--- /dev/null
+++ b/src/moveit_pro_franka_configs/franka_dual_arm_sim/objectives/move_left_home.xml
@@ -0,0 +1,34 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<root BTCPP_format="4" main_tree_to_execute="Move Left Home">
+  <!--//////////-->
+  <BehaviorTree
+    ID="Move Left Home"
+    _description="Move the left arm to the home position"
+    _favorite="false"
+  >
+    <Control ID="Sequence" name="TopLevelSequence">
+      <SubTree
+        ID="Move to Waypoint"
+        _collapsed="true"
+        acceleration_scale_factor="1.0"
+        controller_action_server="/left_joint_trajectory_controller/follow_joint_trajectory"
+        controller_names="left_joint_trajectory_controller"
+        joint_group_name="left_manipulator"
+        keep_orientation="false"
+        keep_orientation_tolerance="0.05"
+        link_padding="0.01"
+        seed="0"
+        velocity_scale_factor="1.0"
+        waypoint_name="Left Home"
+      />
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Move Left Home">
+      <MetadataFields>
+        <Metadata runnable="true" />
+        <Metadata subcategory="Motion - Execute" />
+      </MetadataFields>
+    </SubTree>
+  </TreeNodesModel>
+</root>
diff --git a/src/moveit_pro_franka_configs/franka_dual_arm_sim/objectives/move_right_home.xml b/src/moveit_pro_franka_configs/franka_dual_arm_sim/objectives/move_right_home.xml
new file mode 100644
index 00000000..15225c32
--- /dev/null
+++ b/src/moveit_pro_franka_configs/franka_dual_arm_sim/objectives/move_right_home.xml
@@ -0,0 +1,34 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<root BTCPP_format="4" main_tree_to_execute="Move Right Home">
+  <!--//////////-->
+  <BehaviorTree
+    ID="Move Right Home"
+    _description="Move the right arm to the home position"
+    _favorite="false"
+  >
+    <Control ID="Sequence" name="TopLevelSequence">
+      <SubTree
+        ID="Move to Waypoint"
+        _collapsed="true"
+        acceleration_scale_factor="1.0"
+        controller_action_server="/right_joint_trajectory_controller/follow_joint_trajectory"
+        controller_names="right_joint_trajectory_controller"
+        joint_group_name="right_manipulator"
+        keep_orientation="false"
+        keep_orientation_tolerance="0.05"
+        link_padding="0.01"
+        seed="0"
+        velocity_scale_factor="1.0"
+        waypoint_name="Right Home"
+      />
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Move Right Home">
+      <MetadataFields>
+        <Metadata runnable="true" />
+        <Metadata subcategory="Motion - Execute" />
+      </MetadataFields>
+    </SubTree>
+  </TreeNodesModel>
+</root>
diff --git a/src/moveit_pro_franka_configs/franka_dual_arm_sim/objectives/open_gripper.xml b/src/moveit_pro_franka_configs/franka_dual_arm_sim/objectives/open_gripper.xml
new file mode 100644
index 00000000..1c929867
--- /dev/null
+++ b/src/moveit_pro_franka_configs/franka_dual_arm_sim/objectives/open_gripper.xml
@@ -0,0 +1,26 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<root BTCPP_format="4" main_tree_to_execute="Open Gripper">
+  <!--//////////-->
+  <BehaviorTree
+    ID="Open Gripper"
+    _description="Open the right gripper by default"
+    _favorite="true"
+  >
+    <Control ID="Sequence" name="TopLevelSequence">
+      <Action
+        ID="MoveGripperAction"
+        gripper_command_action_name="/right_gripper_controller/gripper_cmd"
+        position="0.04"
+        timeout="10.000000"
+      />
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Open Gripper">
+      <MetadataFields>
+        <Metadata runnable="true" />
+        <Metadata subcategory="Grasping" />
+      </MetadataFields>
+    </SubTree>
+  </TreeNodesModel>
+</root>
diff --git a/src/moveit_pro_franka_configs/franka_dual_arm_sim/objectives/open_left_gripper.xml b/src/moveit_pro_franka_configs/franka_dual_arm_sim/objectives/open_left_gripper.xml
new file mode 100644
index 00000000..4ed072d1
--- /dev/null
+++ b/src/moveit_pro_franka_configs/franka_dual_arm_sim/objectives/open_left_gripper.xml
@@ -0,0 +1,26 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<root BTCPP_format="4" main_tree_to_execute="Open Left Gripper">
+  <!--//////////-->
+  <BehaviorTree
+    ID="Open Left Gripper"
+    _description="Open the left gripper"
+    _favorite="true"
+  >
+    <Control ID="Sequence" name="TopLevelSequence">
+      <Action
+        ID="MoveGripperAction"
+        gripper_command_action_name="/left_gripper_controller/gripper_cmd"
+        position="0.04"
+        timeout="10.000000"
+      />
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Open Left Gripper">
+      <MetadataFields>
+        <Metadata runnable="true" />
+        <Metadata subcategory="Grasping" />
+      </MetadataFields>
+    </SubTree>
+  </TreeNodesModel>
+</root>
diff --git a/src/moveit_pro_franka_configs/franka_dual_arm_sim/objectives/open_right_gripper.xml b/src/moveit_pro_franka_configs/franka_dual_arm_sim/objectives/open_right_gripper.xml
new file mode 100644
index 00000000..72dcbef2
--- /dev/null
+++ b/src/moveit_pro_franka_configs/franka_dual_arm_sim/objectives/open_right_gripper.xml
@@ -0,0 +1,26 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<root BTCPP_format="4" main_tree_to_execute="Open Right Gripper">
+  <!--//////////-->
+  <BehaviorTree
+    ID="Open Right Gripper"
+    _description="Open the right gripper"
+    _favorite="true"
+  >
+    <Control ID="Sequence" name="TopLevelSequence">
+      <Action
+        ID="MoveGripperAction"
+        gripper_command_action_name="/right_gripper_controller/gripper_cmd"
+        position="0.04"
+        timeout="10.000000"
+      />
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Open Right Gripper">
+      <MetadataFields>
+        <Metadata runnable="true" />
+        <Metadata subcategory="Grasping" />
+      </MetadataFields>
+    </SubTree>
+  </TreeNodesModel>
+</root>
diff --git a/src/moveit_pro_franka_configs/franka_dual_arm_sim/objectives/writing_demo.xml b/src/moveit_pro_franka_configs/franka_dual_arm_sim/objectives/writing_demo.xml
index d187db3c..1bfc667e 100644
--- a/src/moveit_pro_franka_configs/franka_dual_arm_sim/objectives/writing_demo.xml
+++ b/src/moveit_pro_franka_configs/franka_dual_arm_sim/objectives/writing_demo.xml
@@ -26,7 +26,7 @@
         reference_frame="world"
         stamped_pose="{stamped_pose}"
         orientation_xyzw="-0.7073883;0.7068252;0.0000327;0.0000328"
-        position_xyz="0.4;0.0;0.20"
+        position_xyz="0.4;0.0;0.02"
         name="Create center pose"
       />
       <Action
@@ -40,7 +40,7 @@
         <Control ID="Sequence" name="TopLevelSequence">
           <SubTree
             ID="Draw and Wipe"
-            _collapsed="true"
+            _collapsed="false"
             file_path="moveit_pro.yaml"
             center="{stamped_pose}"
           />
diff --git a/src/moveit_pro_franka_configs/franka_dual_arm_sim/waypoints/waypoints.yaml b/src/moveit_pro_franka_configs/franka_dual_arm_sim/waypoints/waypoints.yaml
index 0ad13dc0..28d6c46d 100644
--- a/src/moveit_pro_franka_configs/franka_dual_arm_sim/waypoints/waypoints.yaml
+++ b/src/moveit_pro_franka_configs/franka_dual_arm_sim/waypoints/waypoints.yaml
@@ -1,6 +1,6 @@
 - name: Left Home
   joint_group_names:
-    - left_arm
+    - left_manipulator
   joint_state:
     header:
       frame_id: world
@@ -13,7 +13,7 @@
     effort: []
 - name: Right Home
   joint_group_names:
-    - right_arm
+    - right_manipulator
   joint_state:
     header:
       frame_id: world