diff --git a/c300/c300_bringup/config/c300_isaac_controllers.yaml b/c300/c300_bringup/config/c300_isaac_controllers.yaml
index 6a00c12..fa39a5f 100644
--- a/c300/c300_bringup/config/c300_isaac_controllers.yaml
+++ b/c300/c300_bringup/config/c300_isaac_controllers.yaml
@@ -33,7 +33,7 @@ diff_drive_base_controller:
 
     cmd_vel_timeout: 0.5
     #publish_limited_velocity: true
-    use_stamped_vel: false
+    use_stamped_vel: true
     #velocity_rolling_window_size: 10
 
     # Velocity and acceleration limits
@@ -43,7 +43,7 @@ diff_drive_base_controller:
     linear.x.has_jerk_limits: false
     linear.x.max_velocity: 2.0
     linear.x.min_velocity: -2.0
-    linear.x.max_acceleration: 0.5
+    linear.x.max_acceleration: 2.5
     linear.x.max_jerk: 0.0
     linear.x.min_jerk: 0.0
 
@@ -52,7 +52,7 @@ diff_drive_base_controller:
     angular.z.has_jerk_limits: false
     angular.z.max_velocity: 2.0
     angular.z.min_velocity: -2.0
-    angular.z.max_acceleration: 1.0
-    angular.z.min_acceleration: -1.0
+    angular.z.max_acceleration: 3.2
+    angular.z.min_acceleration: -3.2
     angular.z.max_jerk: 0.0
     angular.z.min_jerk: 0.0
diff --git a/c300/c300_bringup/launch/isaac_c300.launch.py b/c300/c300_bringup/launch/isaac_c300.launch.py
index 1d1d9ae..f62a83d 100644
--- a/c300/c300_bringup/launch/isaac_c300.launch.py
+++ b/c300/c300_bringup/launch/isaac_c300.launch.py
@@ -58,7 +58,7 @@ def generate_launch_description():
     declared_arguments.append(
         DeclareLaunchArgument(
             "description_file",
-            default_value="c300_base.urdf",
+            default_value="c300.urdf.xacro",
             description="URDF/XACRO description file with the robot.",
         )
     )
@@ -145,7 +145,7 @@ def generate_launch_description():
             robot_controllers,
         ],
         remappings=[
-            ("/diff_drive_base_controller/cmd_vel_unstamped", "/cmd_vel"),
+            ("/diff_drive_base_controller/cmd_vel", "/cmd_vel_nav"),
             ("/diff_drive_base_controller/odom", "/odom"),
         ],
         output="both",
diff --git a/c300/c300_bringup/package.xml b/c300/c300_bringup/package.xml
index 136c5f3..033fdb9 100644
--- a/c300/c300_bringup/package.xml
+++ b/c300/c300_bringup/package.xml
@@ -18,6 +18,7 @@
   <exec_depend>ros_gz_sim</exec_depend>
   <exec_depend>joint_state_broadcaster</exec_depend>
   <exec_depend>diff_drive_controller</exec_depend>
+  <exec_depend>joint_state_topic_hardware_interface</exec_depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
diff --git a/c300/c300_bringup/worlds/depot.sdf b/c300/c300_bringup/worlds/depot.sdf
index 09bc5cf..f54ac39 100644
--- a/c300/c300_bringup/worlds/depot.sdf
+++ b/c300/c300_bringup/worlds/depot.sdf
@@ -20,6 +20,10 @@
       filename="gz-sim-scene-broadcaster-system"
       name="gz::sim::systems::SceneBroadcaster">
     </plugin>
+    <plugin
+       filename='gz-sim-contact-system'
+       name='gz::sim::systems::Contact'>
+    </plugin>
 
     <include>
       <uri>
diff --git a/c300/c300_description/urdf/c300.ros2_control.xacro b/c300/c300_description/urdf/c300.ros2_control.xacro
index 4f39c14..e7e6d33 100644
--- a/c300/c300_description/urdf/c300.ros2_control.xacro
+++ b/c300/c300_description/urdf/c300.ros2_control.xacro
@@ -15,15 +15,17 @@
           <plugin>gz_ros2_control/GazeboSimSystem</plugin>
         </xacro:if>
         <xacro:if value="${sim_isaac}">
-          <plugin>topic_based_ros2_control/TopicBasedSystem</plugin>
+          <plugin>joint_state_topic_hardware_interface/JointStateTopicSystem</plugin>
           <param name="joint_commands_topic">${isaac_joint_commands}</param>
           <param name="joint_states_topic">${isaac_joint_states}</param>
           <param name="sum_wrapped_joint_states">true</param>
+          <param name="trigger_joint_command_threshold">-1</param> <!-- always publish commands on the topic -->
         </xacro:if>
         <xacro:if value="${mock_hardware}">
           <plugin>mock_components/GenericSystem</plugin>
           <param name="fake_sensor_commands">false</param>
           <param name="state_following_offset">0.0</param>
+          <param name="calculate_dynamics">true</param>
         </xacro:if>
       </hardware>
       <joint name="drivewhl_l_joint">
diff --git a/c300/c300_description/urdf/c300_macro.xacro b/c300/c300_description/urdf/c300_macro.xacro
index 85b7d9f..18459ba 100644
--- a/c300/c300_description/urdf/c300_macro.xacro
+++ b/c300/c300_description/urdf/c300_macro.xacro
@@ -5,7 +5,7 @@
     prefix
     sim_isaac:=false
     sim_gazebo:=false
-    isaac_joint_commands:=/isaac_joint_commands
+    isaac_joint_commands:=/base_joint_command
     isaac_joint_states:=/isaac_joint_states
     mock_hardware:=false">
 
diff --git a/c300/c300_description/usd/UAM-05LP.usd b/c300/c300_description/usd/UAM-05LP.usd
index 862e9b1..97ff5c9 100644
Binary files a/c300/c300_description/usd/UAM-05LP.usd and b/c300/c300_description/usd/UAM-05LP.usd differ
diff --git a/c300/c300_description/usd/c300.usd b/c300/c300_description/usd/c300.usd
index 170c1b5..9602ef5 100644
Binary files a/c300/c300_description/usd/c300.usd and b/c300/c300_description/usd/c300.usd differ
diff --git a/c300/c300_description/usd/c300_no_js.usd b/c300/c300_description/usd/c300_no_js.usd
new file mode 100644
index 0000000..fdfeebb
Binary files /dev/null and b/c300/c300_description/usd/c300_no_js.usd differ
diff --git a/c300/c300_description/usd/python.sh b/c300/c300_description/usd/python.sh
deleted file mode 100755
index cc7f842..0000000
--- a/c300/c300_description/usd/python.sh
+++ /dev/null
@@ -1,18 +0,0 @@
-#!/bin/bash
-
-# Script to call the isaac launch system (see python.sh in the ISAAC_SCRIPT_DIR)
-
-ISAAC_SCRIPT_DIR="$HOME/.local/share/ov/pkg/isaac_sim-2022.2.1"
-
-# Prepend the path to all arguments passed in and also pass the current directory
-# to allow the launch script to find the USD files located here.
-CUR_SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
-NEW_ARGS=""
-for arg in "$@"
-do
-    NEW_ARGS="${NEW_ARGS} ${CUR_SCRIPT_DIR}/${arg}"
-done
-
-pushd ${ISAAC_SCRIPT_DIR}
-./python.sh $NEW_ARGS $CUR_SCRIPT_DIR
-popd
diff --git a/c300/c300_navigation/params/nav2_params_mppi.yaml b/c300/c300_navigation/params/nav2_params_mppi.yaml
index 21c893d..1dddb57 100644
--- a/c300/c300_navigation/params/nav2_params_mppi.yaml
+++ b/c300/c300_navigation/params/nav2_params_mppi.yaml
@@ -354,13 +354,13 @@ velocity_smoother:
     smoothing_frequency: 20.0
     scale_velocities: False
     feedback: "CLOSED_LOOP"
-    max_velocity: [1.0, 0.0, 1.0] # [x, y, theta]
-    min_velocity: [-1.0, 0.0, -1.0] # [x, y, theta]
+    max_velocity: [2.0, 0.0, 2.0] # [x, y, theta]
+    min_velocity: [-2.0, 0.0, -2.0] # [x, y, theta]
     max_accel: [2.5, 0.0, 3.2]
     max_decel: [-2.5, 0.0, -3.2]
     odom_topic: "odom"
     odom_duration: 0.1
-    deadband_velocity: [0.0, 0.0, 0.0]
+    deadband_velocity: [0.05, 0.0, 0.15]
     velocity_timeout: 1.0
     enable_stamped_cmd_vel: True
 
diff --git a/c3pzero/c3pzero_bringup/config/c3pzero_gz_controllers.yaml b/c3pzero/c3pzero_bringup/config/c3pzero_gz_controllers.yaml
index 903a9ea..6070d3d 100644
--- a/c3pzero/c3pzero_bringup/config/c3pzero_gz_controllers.yaml
+++ b/c3pzero/c3pzero_bringup/config/c3pzero_gz_controllers.yaml
@@ -114,7 +114,7 @@ left_arm_jtc:
 right_arm_gripper_controller:
   ros__parameters:
     action_monitor_rate: 20.0
-    allow_stalling: false
+    allow_stalling: true
     goal_tolerance: 0.01
     joint: right_arm_gripper_joint
     stall_timeout: 1.0
@@ -123,7 +123,7 @@ right_arm_gripper_controller:
 left_arm_gripper_controller:
   ros__parameters:
     action_monitor_rate: 20.0
-    allow_stalling: false
+    allow_stalling: true
     goal_tolerance: 0.01
     joint: left_arm_gripper_joint
     stall_timeout: 1.0
diff --git a/c3pzero/c3pzero_bringup/config/c3pzero_hardware_controllers.yaml b/c3pzero/c3pzero_bringup/config/c3pzero_hardware_controllers.yaml
index 1c8d3bc..0461741 100644
--- a/c3pzero/c3pzero_bringup/config/c3pzero_hardware_controllers.yaml
+++ b/c3pzero/c3pzero_bringup/config/c3pzero_hardware_controllers.yaml
@@ -45,7 +45,7 @@ diff_drive_base_controller:
 
     cmd_vel_timeout: 0.5
     #publish_limited_velocity: true
-    use_stamped_vel: false
+    use_stamped_vel: true
     #velocity_rolling_window_size: 10
 
     # Velocity and acceleration limits
@@ -86,6 +86,7 @@ right_arm_jtc:
     state_publish_rate: 100.0
     action_monitor_rate: 20.0
     allow_partial_joints_goal: false
+    allow_nonzero_velocity_at_trajectory_end: true
     constraints:
       stopped_velocity_tolerance: 0.0
       goal_time: 0.0
@@ -107,6 +108,7 @@ left_arm_jtc:
     state_publish_rate: 100.0
     action_monitor_rate: 20.0
     allow_partial_joints_goal: false
+    allow_nonzero_velocity_at_trajectory_end: true
     constraints:
       stopped_velocity_tolerance: 0.0
       goal_time: 0.0
diff --git a/c3pzero/c3pzero_bringup/config/c3pzero_isaac_controllers.yaml b/c3pzero/c3pzero_bringup/config/c3pzero_isaac_controllers.yaml
index cf8e712..2ced992 100644
--- a/c3pzero/c3pzero_bringup/config/c3pzero_isaac_controllers.yaml
+++ b/c3pzero/c3pzero_bringup/config/c3pzero_isaac_controllers.yaml
@@ -45,7 +45,7 @@ diff_drive_base_controller:
 
     cmd_vel_timeout: 0.5
     #publish_limited_velocity: true
-    use_stamped_vel: false
+    use_stamped_vel: true
     #velocity_rolling_window_size: 10
 
     # Velocity and acceleration limits
@@ -55,7 +55,7 @@ diff_drive_base_controller:
     linear.x.has_jerk_limits: false
     linear.x.max_velocity: 2.0
     linear.x.min_velocity: -2.0
-    linear.x.max_acceleration: 0.5
+    linear.x.max_acceleration: 2.5
     linear.x.max_jerk: 0.0
     linear.x.min_jerk: 0.0
 
diff --git a/c3pzero/c3pzero_bringup/launch/isaac_c3pzero.launch.py b/c3pzero/c3pzero_bringup/launch/isaac_c3pzero.launch.py
index cc766e1..4d6944f 100644
--- a/c3pzero/c3pzero_bringup/launch/isaac_c3pzero.launch.py
+++ b/c3pzero/c3pzero_bringup/launch/isaac_c3pzero.launch.py
@@ -78,11 +78,12 @@ def generate_launch_description():
         package="controller_manager",
         executable="ros2_control_node",
         parameters=[
-            {"use_sim_time": True, "robot_description": robot_description_content},
+            {"use_sim_time": True},
             robot_controllers,
         ],
         remappings=[
-            ("/diff_drive_base_controller/cmd_vel_unstamped", "/cmd_vel"),
+            ("~/robot_description", "/robot_description"),
+            ("/diff_drive_base_controller/cmd_vel", "/cmd_vel_nav"),
             ("/diff_drive_base_controller/odom", "/odom"),
         ],
         output="both",
@@ -102,10 +103,6 @@ def generate_launch_description():
             "--controller-manager",
             "/controller_manager",
         ],
-        remappings=[
-            ("/diff_drive_base_controller/cmd_vel_unstamped", "/cmd_vel"),
-            ("/diff_drive_base_controller/odom", "/odom"),
-        ],
         output="both",
     )
 
diff --git a/c3pzero/c3pzero_bringup/rviz/c3pzero.rviz b/c3pzero/c3pzero_bringup/rviz/c3pzero.rviz
index 866eb98..a786b69 100644
--- a/c3pzero/c3pzero_bringup/rviz/c3pzero.rviz
+++ b/c3pzero/c3pzero_bringup/rviz/c3pzero.rviz
@@ -5,8 +5,8 @@ Panels:
     Property Tree Widget:
       Expanded:
         - /LaserScan1/Topic1
-      Splitter Ratio: 0.36942070722579956
-    Tree Height: 182
+      Splitter Ratio: 0.6189376711845398
+    Tree Height: 1022
 Visualization Manager:
   Class: ""
   Displays:
@@ -147,6 +147,7 @@ Visualization Manager:
           Alpha: 1
           Show Axes: false
           Show Trail: false
+          Value: true
         left_arm_wrist_camera_color_frame:
           Alpha: 1
           Show Axes: false
@@ -263,6 +264,7 @@ Visualization Manager:
           Alpha: 1
           Show Axes: false
           Show Trail: false
+          Value: true
         right_arm_wrist_camera_color_frame:
           Alpha: 1
           Show Axes: false
@@ -366,20 +368,20 @@ Visualization Manager:
       Axis: Z
       Channel Name: intensity
       Class: rviz_default_plugins/LaserScan
-      Color: 255; 255; 255
-      Color Transformer: Intensity
+      Color: 255; 0; 0
+      Color Transformer: FlatColor
       Decay Time: 0
       Enabled: true
       Invert Rainbow: false
       Max Color: 255; 255; 255
-      Max Intensity: 0
+      Max Intensity: 17
       Min Color: 0; 0; 0
-      Min Intensity: 0
+      Min Intensity: 1
       Name: LaserScan
       Position Transformer: XYZ
       Selectable: true
       Size (Pixels): 3
-      Size (m): 0.009999999776482582
+      Size (m): 0.004999999888241291
       Style: Flat Squares
       Topic:
         Depth: 5
@@ -392,7 +394,7 @@ Visualization Manager:
       Use rainbow: true
       Value: true
     - Class: rviz_default_plugins/Camera
-      Enabled: true
+      Enabled: false
       Far Plane Distance: 100
       Image Rendering: background and overlay
       Name: RightWristCamera
@@ -403,7 +405,7 @@ Visualization Manager:
         History Policy: Keep Last
         Reliability Policy: Reliable
         Value: /right_arm_wrist_camera/color/image_raw
-      Value: true
+      Value: false
       Visibility:
         Grid: true
         LaserScan: true
@@ -414,7 +416,7 @@ Visualization Manager:
         Value: true
       Zoom Factor: 1
     - Class: rviz_default_plugins/Camera
-      Enabled: true
+      Enabled: false
       Far Plane Distance: 100
       Image Rendering: background and overlay
       Name: LeftWristCamera
@@ -425,7 +427,7 @@ Visualization Manager:
         History Policy: Keep Last
         Reliability Policy: Reliable
         Value: /left_arm_wrist_camera/color/image_raw
-      Value: true
+      Value: false
       Visibility:
         Grid: true
         LaserScan: true
@@ -456,25 +458,25 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 2.973403215408325
+      Distance: 3.0082173347473145
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: -0.18479067087173462
-        Y: -0.09187475591897964
-        Z: 0.46009013056755066
+        X: 0.2740049660205841
+        Y: -0.2658533453941345
+        Z: 0.6486567258834839
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.5347971320152283
+      Pitch: 0.5897971391677856
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 2.175384283065796
+      Yaw: 3.3403968811035156
     Saved: ~
 Window Geometry:
   Displays:
@@ -484,9 +486,9 @@ Window Geometry:
   Hide Right Dock: true
   LeftWristCamera:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd0000000100000000000003330000042dfc0200000003fb000000100044006900730070006c0061007900730100000044000000f7000000eb00fffffffb00000020005200690067006800740057007200690073007400430061006d0065007200610100000142000001940000001600fffffffb0000001e004c0065006600740057007200690073007400430061006d00650072006101000002dd000001940000001600ffffff000004460000042d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000100000000000001b300000439fc0200000003fb000000100044006900730070006c006100790073010000003b00000439000000c700fffffffb00000020005200690067006800740057007200690073007400430061006d006500720061000000013b0000019a0000001600fffffffb0000001e004c0065006600740057007200690073007400430061006d00650072006100000001d90000029b0000001600ffffff000005c70000043900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   RightWristCamera:
     collapsed: false
   Width: 1920
-  X: 588
-  Y: 141
+  X: 3200
+  Y: 166
diff --git a/c3pzero/c3pzero_description/urdf/c3pzero.urdf.xacro b/c3pzero/c3pzero_description/urdf/c3pzero.urdf.xacro
index 4ef2d3b..7a59d80 100644
--- a/c3pzero/c3pzero_description/urdf/c3pzero.urdf.xacro
+++ b/c3pzero/c3pzero_description/urdf/c3pzero.urdf.xacro
@@ -31,7 +31,7 @@
       mock_hardware="$(arg mock_hardware)"
       sim_gazebo="$(arg sim_gazebo)"
       sim_isaac="$(arg sim_isaac)"
-      isaac_joint_commands="/mobile_base_joint_commands"
+      isaac_joint_commands="/base_joint_command"
       isaac_joint_states="/isaac_joint_states"/>
 
     <!-- Right Arm -->
@@ -53,6 +53,13 @@
       <origin xyz="0 0 0" rpy="0 0 0" />
     </xacro:load_piper>
 
+    <link name="$(arg prefix)right_arm_grasp_link"/>
+    <joint name="$(arg prefix)right_arm_grasp_joint" type="fixed">
+      <parent link="$(arg prefix)right_arm_gripper_base_link"/>
+      <child link="$(arg prefix)right_arm_grasp_link"/>
+      <origin xyz="0.0 0.0 0.1" rpy="0 0 0"/>
+    </joint>
+
     <xacro:wrist_camera
         parent="$(arg prefix)right_arm_gripper_aux_mount"
         prefix="$(arg prefix)right_arm_"
@@ -78,6 +85,13 @@
       <origin xyz="0 0 0" rpy="0 0 0" />
     </xacro:load_piper>
 
+    <link name="$(arg prefix)left_arm_grasp_link"/>
+    <joint name="$(arg prefix)left_arm_grasp_joint" type="fixed">
+      <parent link="$(arg prefix)left_arm_gripper_base_link"/>
+      <child link="$(arg prefix)left_arm_grasp_link"/>
+      <origin xyz="0.0 0.0 0.1" rpy="0 0 0"/>
+    </joint>
+
     <xacro:wrist_camera
         parent="$(arg prefix)left_arm_gripper_aux_mount"
         prefix="$(arg prefix)left_arm_"
diff --git a/c3pzero/c3pzero_description/usd/c3pzero.usd b/c3pzero/c3pzero_description/usd/c3pzero.usd
index c62a175..d6ba546 100644
Binary files a/c3pzero/c3pzero_description/usd/c3pzero.usd and b/c3pzero/c3pzero_description/usd/c3pzero.usd differ
diff --git a/c3pzero/c3pzero_description/usd/c3pzero_single_arm.usd b/c3pzero/c3pzero_description/usd/c3pzero_single_arm.usd
new file mode 100644
index 0000000..6362630
Binary files /dev/null and b/c3pzero/c3pzero_description/usd/c3pzero_single_arm.usd differ
diff --git a/c3pzero/c3pzero_description/usd/right_arm.usd b/c3pzero/c3pzero_description/usd/right_arm.usd
index 5728e94..00eedeb 100644
Binary files a/c3pzero/c3pzero_description/usd/right_arm.usd and b/c3pzero/c3pzero_description/usd/right_arm.usd differ
diff --git a/c3pzero/c3pzero_moveit_config/config/c3pzero.srdf.xacro b/c3pzero/c3pzero_moveit_config/config/c3pzero.srdf.xacro
index 5b8862f..84ca756 100644
--- a/c3pzero/c3pzero_moveit_config/config/c3pzero.srdf.xacro
+++ b/c3pzero/c3pzero_moveit_config/config/c3pzero.srdf.xacro
@@ -80,6 +80,12 @@
     <group_state name="closed" group="right_gripper">
         <joint name="right_arm_gripper_joint" value="0"/>
     </group_state>
+    <group_state name="open" group="left_gripper">
+        <joint name="left_arm_gripper_joint" value="0.038"/>
+    </group_state>
+    <group_state name="closed" group="left_gripper">
+        <joint name="left_arm_gripper_joint" value="0"/>
+    </group_state>
     <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
     <end_effector name="right_eff" parent_link="right_arm_tool_0" group="right_gripper" parent_group="right_manipulator"/>
     <end_effector name="left_eff" parent_link="left_arm_tool_0" group="left_gripper" parent_group="left_manipulator"/>
diff --git a/c3pzero/c3pzero_moveit_config/config/kinematics.yaml b/c3pzero/c3pzero_moveit_config/config/kinematics.yaml
index 01417a3..392e166 100644
--- a/c3pzero/c3pzero_moveit_config/config/kinematics.yaml
+++ b/c3pzero/c3pzero_moveit_config/config/kinematics.yaml
@@ -2,10 +2,10 @@
   ros__parameters:
     robot_description_kinematics:
       left_manipulator:
-        kinematics_solver: cached_ik_kinematics_plugin/CachedKDLKinematicsPlugin
-        kinematics_solver_search_resolution: 0.0050000000000000001
-        kinematics_solver_timeout: 0.0050000000000000001
+        kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
+        kinematics_solver_search_resolution: 0.005
+        kinematics_solver_timeout: 0.005
       right_manipulator:
-        kinematics_solver: cached_ik_kinematics_plugin/CachedKDLKinematicsPlugin
-        kinematics_solver_search_resolution: 0.0050000000000000001
-        kinematics_solver_timeout: 0.0050000000000000001
+        kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
+        kinematics_solver_search_resolution: 0.005
+        kinematics_solver_timeout: 0.005
diff --git a/c3pzero/c3pzero_moveit_config/config/moveit.rviz b/c3pzero/c3pzero_moveit_config/config/moveit.rviz
index db93b92..44b321c 100644
--- a/c3pzero/c3pzero_moveit_config/config/moveit.rviz
+++ b/c3pzero/c3pzero_moveit_config/config/moveit.rviz
@@ -5,7 +5,7 @@ Panels:
     Property Tree Widget:
       Expanded: ~
       Splitter Ratio: 0.5
-    Tree Height: 275
+    Tree Height: 485
   - Class: rviz_common/Help
     Name: Help
   - Class: rviz_common/Views
@@ -13,6 +13,29 @@ Panels:
       - /Current View1
     Name: Views
     Splitter Ratio: 0.5
+  - Class: moveit_task_constructor/Motion Planning Tasks
+    Global Settings:
+      Task View Settings:
+        Old task handling: Keep
+        Show Computation Times: true
+        Task Expansion: All Expanded
+    Name: Motion Planning Tasks
+    Tasks View:
+      property_splitter:
+        - 316
+        - 107
+      solution_sorting:
+        column: 0
+        order: 1
+      solutions_splitter:
+        - 424
+        - 212
+      solutions_view_columns: ~
+      tasks_view_columns:
+        - 335
+        - 26
+        - 26
+        - 35
 Visualization Manager:
   Class: ""
   Displays:
@@ -102,6 +125,10 @@ Visualization Manager:
             Show Axes: false
             Show Trail: false
             Value: true
+          left_arm_grasp_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
           left_arm_gripper_aux_mount:
             Alpha: 1
             Show Axes: false
@@ -166,6 +193,7 @@ Visualization Manager:
             Alpha: 1
             Show Axes: false
             Show Trail: false
+            Value: true
           left_arm_wrist_camera_color_frame:
             Alpha: 1
             Show Axes: false
@@ -218,6 +246,10 @@ Visualization Manager:
             Show Axes: false
             Show Trail: false
             Value: true
+          right_arm_grasp_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
           right_arm_gripper_aux_mount:
             Alpha: 1
             Show Axes: false
@@ -282,6 +314,7 @@ Visualization Manager:
             Alpha: 1
             Show Axes: false
             Show Trail: false
+            Value: true
           right_arm_wrist_camera_color_frame:
             Alpha: 1
             Show Axes: false
@@ -412,6 +445,10 @@ Visualization Manager:
             Show Axes: false
             Show Trail: false
             Value: true
+          left_arm_grasp_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
           left_arm_gripper_aux_mount:
             Alpha: 1
             Show Axes: false
@@ -476,6 +513,7 @@ Visualization Manager:
             Alpha: 1
             Show Axes: false
             Show Trail: false
+            Value: true
           left_arm_wrist_camera_color_frame:
             Alpha: 1
             Show Axes: false
@@ -528,6 +566,10 @@ Visualization Manager:
             Show Axes: false
             Show Trail: false
             Value: true
+          right_arm_grasp_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
           right_arm_gripper_aux_mount:
             Alpha: 1
             Show Axes: false
@@ -592,6 +634,7 @@ Visualization Manager:
             Alpha: 1
             Show Axes: false
             Show Trail: false
+            Value: true
           right_arm_wrist_camera_color_frame:
             Alpha: 1
             Show Axes: false
@@ -644,6 +687,101 @@ Visualization Manager:
         Show Robot Visual: true
       Value: true
       Velocity_Scaling_Factor: 1
+    - Class: moveit_task_constructor/Motion Planning Tasks
+      Enabled: false
+      Interrupt Display: false
+      Loop Animation: false
+      Markers:
+        All at once?: false
+        Value: true
+      Name: Motion Planning Tasks
+      Robot:
+        Fixed Robot Color: 150; 50; 150
+        Links:
+          All Links Enabled: true
+          Expand Joint Details: false
+          Expand Link Details: false
+          Expand Tree: false
+          Link Tree Style: Links in Alphabetic Order
+        Robot Alpha: 0.5
+        Show Robot Collision: false
+        Show Robot Visual: true
+        Use Fixed Robot Color: false
+        Value: ""
+      Robot Description: robot_description
+      Scene:
+        Attached Body Color: 150; 50; 150
+        Scene Alpha: 0.8999999761581421
+        Scene Color: 50; 230; 50
+        Value: true
+        Voxel Coloring: Z-Axis
+        Voxel Rendering: Occupied Voxels
+      Show Trail: false
+      State Display Time: 0.05 s
+      Task Solution Topic: /solution
+      Tasks:
+        {}
+      Trail Step Size: 1
+      Value: false
+    - Class: moveit_rviz_plugin/PlanningScene
+      Enabled: false
+      Move Group Namespace: ""
+      Name: PlanningScene
+      Planning Scene Topic: /monitored_planning_scene
+      Robot Description: robot_description
+      Scene Geometry:
+        Scene Alpha: 0.8999999761581421
+        Scene Color: 50; 230; 50
+        Scene Display Time: 0.009999999776482582
+        Show Scene Geometry: true
+        Voxel Coloring: Z-Axis
+        Voxel Rendering: Occupied Voxels
+      Scene Robot:
+        Attached Body Color: 150; 50; 150
+        Links:
+          All Links Enabled: true
+          Expand Joint Details: false
+          Expand Link Details: false
+          Expand Tree: false
+          Link Tree Style: Links in Alphabetic Order
+        Robot Alpha: 1
+        Show Robot Collision: false
+        Show Robot Visual: true
+      Value: false
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/LaserScan
+      Color: 255; 0; 0
+      Color Transformer: FlatColor
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 17
+      Min Color: 0; 0; 0
+      Min Intensity: 1
+      Name: LaserScan
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /scan
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -662,41 +800,45 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 2.100095510482788
+      Distance: 3.4742915630340576
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: -0.062461890280246735
-        Y: 0.15434633195400238
-        Z: 0.4571358561515808
+        X: 0.24386073648929596
+        Y: -0.9881587624549866
+        Z: 0.5298995971679688
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.37500035762786865
+      Pitch: 0.25000059604644775
       Target Frame: base_link
       Value: Orbit (rviz_default_plugins)
-      Yaw: 5.465185642242432
+      Yaw: 5.060180187225342
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 1369
+  Height: 1403
   Help:
     collapsed: false
   Hide Left Dock: false
   Hide Right Dock: false
+  Motion Planning Tasks:
+    collapsed: false
+  Motion Planning Tasks - Slider:
+    collapsed: false
   MotionPlanning:
     collapsed: false
   MotionPlanning - Trajectory Slider:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd0000000100000000000002b4000004f7fc0200000005fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720100000044000000470000004700fffffffb000000100044006900730070006c006100790073010000009200000154000000eb00fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001ed0000024b000001a100fffffffb0000000800480065006c0070000000029a0000006e0000008100fffffffb0000000a00560069006500770073010000043f000000fc000000b900ffffff000006ff000004f700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000100000000000002b400000525fc0200000007fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c0069006400650072000000003b0000004a0000003f00fffffffb000000100044006900730070006c006100790073010000003b00000220000000c700fffffffc00000261000002ff000001840100001afa000000010100000002fb0000002a004d006f00740069006f006e00200050006c0061006e006e0069006e00670020005400610073006b00730100000000ffffffff000000b800fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000000000002b4000001c900fffffffb0000000800480065006c0070000000029a0000006e0000006e00fffffffb0000000a005600690065007700730000000344000000c0000000a000fffffffb000000120020002d00200053006c00690064006500720000000000ffffffff0000000000000000fb0000003c004d006f00740069006f006e00200050006c0061006e006e0069006e00670020005400610073006b00730020002d00200053006c006900640065007200000004f0000000550000003f00ffffff000002460000052500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Views:
     collapsed: false
-  Width: 2490
-  X: 70
-  Y: 34
+  Width: 1280
+  X: 3840
+  Y: 0
diff --git a/c3pzero/c3pzero_moveit_config/config/moveit_controllers.yaml b/c3pzero/c3pzero_moveit_config/config/moveit_controllers.yaml
index 2a060a7..07f0ab9 100644
--- a/c3pzero/c3pzero_moveit_config/config/moveit_controllers.yaml
+++ b/c3pzero/c3pzero_moveit_config/config/moveit_controllers.yaml
@@ -34,17 +34,17 @@
           - left_arm_joint_4
           - left_arm_joint_5
           - left_arm_joint_6
-      # ParallelGripperCommand is not yet supported.
-      # see https://github.com/moveit/moveit2/issues/3017
       right_arm_gripper_controller:
         type: ParallelGripperCommand
         joints:
           - right_arm_gripper_joint
         action_ns: gripper_cmd
+        allow_stalling: true
         default: true
       left_arm_gripper_controller:
         type: ParallelGripperCommand
         joints:
           - left_arm_gripper_joint
         action_ns: gripper_cmd
+        allow_stalling: true
         default: true
diff --git a/c3pzero/c3pzero_moveit_config/launch/demo.launch.xml b/c3pzero/c3pzero_moveit_config/launch/demo.launch.xml
index 2208051..f56c505 100644
--- a/c3pzero/c3pzero_moveit_config/launch/demo.launch.xml
+++ b/c3pzero/c3pzero_moveit_config/launch/demo.launch.xml
@@ -21,11 +21,13 @@
         <node pkg="controller_manager" exec="ros2_control_node" output="both">
             <remap from="~/robot_description" to="/robot_description" />
             <param from="$(find-pkg-share c3pzero_bringup)/config/c3pzero_hardware_controllers.yaml" />
+            <remap from="/diff_drive_base_controller/cmd_vel" to="/cmd_vel_nav" />
+            <remap from="/diff_drive_base_controller/odom" to="/odom" />
         </node>
 
         <!-- ros2_control spawners -->
         <node pkg="controller_manager" exec="spawner"
-            args="right_arm_jtc left_arm_jtc right_arm_gripper_controller left_arm_gripper_controller joint_state_broadcaster -c /controller_manager" />
+            args="right_arm_jtc left_arm_jtc right_arm_gripper_controller left_arm_gripper_controller diff_drive_base_controller joint_state_broadcaster -c /controller_manager" />
     </group>
 
     <!-- MoveGroup -->
@@ -35,7 +37,7 @@
         <param from="$(find-pkg-share c3pzero_moveit_config)/config/kinematics.yaml" />
         <param from="$(find-pkg-share c3pzero_moveit_config)/config/joint_limits.yaml" />
         <param from="$(find-pkg-share c3pzero_moveit_config)/config/moveit_controllers.yaml" />
-        <!-- <param name="capabilities" value="move_group/ExecuteTaskSolutionCapability" /> only needed with MTC-->
+        <param name="capabilities" value="move_group/ExecuteTaskSolutionCapability" />
         <param name="publish_robot_description_semantic" value="true" />
         <param name="use_sim_time" value="$(var use_sim_time)" />
     </node>
@@ -50,4 +52,11 @@
             <param name="use_sim_time" value="$(var use_sim_time)" />
         </node>
     </group>
+
+    <node pkg="btcpp_ros2_samples" exec="sample_bt_executor" output="screen">
+        <param from="$(find-pkg-share c3pzero_moveit_config)/config/ompl_planning.yaml" />
+        <param from="$(find-pkg-share c3pzero_moveit_config)/config/kinematics.yaml" />
+        <param from="$(find-pkg-share c3pzero_moveit_config)/config/joint_limits.yaml" />
+        <param from="$(find-pkg-share task_bot_bringup)/config/task_bot_executor.yaml"/>
+    </node>
 </launch>
diff --git a/c3pzero/c3pzero_moveit_config/package.xml b/c3pzero/c3pzero_moveit_config/package.xml
index 443b449..35e34d2 100644
--- a/c3pzero/c3pzero_moveit_config/package.xml
+++ b/c3pzero/c3pzero_moveit_config/package.xml
@@ -21,13 +21,11 @@
   <exec_depend>moveit_simple_controller_manager</exec_depend>
   <exec_depend>moveit_ros_move_group</exec_depend>
   <exec_depend>moveit_ros_visualization</exec_depend>
-  <exec_depend>moveit_ros_warehouse</exec_depend>
   <exec_depend>robot_state_publisher</exec_depend>
   <exec_depend>rviz2</exec_depend>
   <exec_depend>rviz_common</exec_depend>
   <exec_depend>rviz_default_plugins</exec_depend>
   <exec_depend>tf2_ros</exec_depend>
-  <exec_depend>warehouse_ros_mongo</exec_depend>
   <exec_depend>xacro</exec_depend>