diff --git a/kits/arms/ar_control_sm.py b/kits/arms/ar_control_sm.py
index 6732ad0..011632a 100644
--- a/kits/arms/ar_control_sm.py
+++ b/kits/arms/ar_control_sm.py
@@ -1,17 +1,17 @@
-
 from enum import Enum, auto
 from dataclasses import dataclass, field
 from time import time, sleep
 import os
 import numpy as np
 from scipy.spatial.transform import Rotation as R
+import argparse
 
 import hebi
 from hebi.util import create_mobile_io
 
 import typing
 if typing.TYPE_CHECKING:
-    from typing import Callable
+    from typing import Sequence, Callable
     import numpy.typing as npt
     from hebi._internal.mobile_io import MobileIO
     from hebi.arm import Arm, Gripper
@@ -29,7 +29,7 @@ class ArmControlState(Enum):
 class ArmMobileIOInputs:
     phone_pos: 'npt.NDArray[np.float32]' = field(default_factory=lambda: np.array([0., 0., 0.]))
     phone_rot: 'npt.NDArray[np.float64]' = field(default_factory=lambda: np.eye(3))
-    ar_scaling: float = 1.0
+    xyz_scaling: float = 1.0
     lock_toggle: bool = False
     locked: bool = True
     gripper_closed: bool = False
@@ -37,36 +37,33 @@ class ArmMobileIOInputs:
 
 
 class ArmMobileIOControl:
-    def __init__(self, arm: 'Arm', gripper: 'Gripper | None' = None, homing_time=5.0, traj_duration=1.0, xyz_scale: 'npt.NDArray[np.float64]' = np.ones(3)):
+    def __init__(self, arm: 'Arm', gripper: 'Gripper | None' = None, home_pose: 'Sequence[float] | npt.NDArray[np.float64] | None' = None, ik_seed: 'Sequence[float] | npt.NDArray[np.float64] | None' = None, homing_time: float = 5.0, traj_duration: float = 0.25, xyz_scale: 'npt.NDArray[np.float64]' = np.ones(3)):
         self.namespace = ''
 
         self.state = ArmControlState.STARTUP
         self.arm = arm
         self.gripper = gripper
 
-        # 5 DoF home
-        # self.arm_xyz_home = [0.34, 0.0, 0.23]
-        # 6 DoF home
-        self.arm_xyz_home = [0.5, 0.0, 0.0]
-        self.arm_rot_home: 'npt.NDArray[np.float64]' = (R.from_euler(
-            'z', np.pi / 2) * R.from_euler('x', np.pi)).as_matrix()
+        self.arm_xyz_home = [0.5, 0.0, 0.15]
+        self.arm_rot_home: 'npt.NDArray[np.float64]' = R.from_euler('x', np.pi).as_matrix()
         self.homing_time = homing_time
         self.traj_duration = traj_duration
 
-        # 5 DoF seed
-        # self.arm_seed_ik = np.array([0.25, -1.0, 0, -0.75, 0])
-        # 6 DoF seed
-        self.arm_seed_ik = np.array([0.3, 1.2, 2.2, 2.9, -1.57, 0])
-        self.arm_home = self.arm.ik_target_xyz_so3(
+        # Set the IK seed for the arm
+        self.arm_seed_ik = ik_seed if ik_seed is not None else np.array([0.01, 1.4, 1.8, 2.0, -1.57, 0.01])[:arm.size]
+        assert len(self.arm_seed_ik) == arm.size, \
+            f"IK seed must have the same size as the arm ({arm.size}), got {self.arm_seed_ik.shape[0]} elements."
+        # Set the home position for the arm
+        self.arm_home = home_pose if home_pose is not None else self.arm.ik_target_xyz_so3(
             self.arm_seed_ik,
             self.arm_xyz_home,
             self.arm_rot_home)
 
+        self.xyz_scale_init = xyz_scale
         self.xyz_scale = xyz_scale
 
         self.last_locked_xyz = self.arm_xyz_home.copy()
         self.last_locked_rot = self.arm_rot_home.copy()
-        self.last_locked_seed = self.arm_home.copy()
 
         self.locked = True
         self._transition_handlers: 'list[Callable[[ArmMobileIOControl, ArmControlState], None]]' = [
@@ -82,7 +79,7 @@ def send(self):
         if self.gripper:
             self.gripper.send()
 
-    def update(self, t_now: float, arm_input: 'ArmMobileIOInputs | None'):
+    def update(self, t_now: float, arm_input: 'ArmMobileIOInputs | None' = None):
         self.arm.update()
 
         if self.state is self.state.EXIT:
@@ -97,7 +94,6 @@ def update(self, t_now: float, arm_input: 'ArmMobileIOInputs | None'):
         # Reset the timeout
         self.mobile_last_fbk_t = t_now
         # Transition to teleop if mobileIO is reconnected
-        last_pos = self.arm.last_feedback.position
         if self.state is self.state.DISCONNECTED:
             self.mobile_last_fbk_t = t_now
             print(self.namespace + 'Controller reconnected, demo continued.')
@@ -112,18 +108,18 @@ def update(self, t_now: float, arm_input: 'ArmMobileIOInputs | None'):
         # If homing is complete, transition to teleop
         elif self.state is self.state.HOMING:
             if self.arm.at_goal:
-                self.phone_xyz_home = arm_input.phone_pos
-                self.phone_rot_home = arm_input.phone_rot
+                self.phone_xyz_init = arm_input.phone_pos.copy()
+                self.phone_rot_init = arm_input.phone_rot.copy()
 
-                self.last_locked_seed = last_pos.copy()
                 xyz = np.empty(3)
                 orientation = np.empty((3, 3))
-                self.arm.FK(last_pos,
+                self.arm.FK(self.arm.last_feedback.position,
                             xyz_out=xyz,
                             orientation_out=orientation)
 
-                self.last_locked_xyz = xyz
-                self.last_locked_rot = orientation
+                self.last_locked_xyz = xyz.copy()
+                self.last_locked_rot = orientation.copy()
+                self.locked = True  # Lock the arm after reaching home
 
                 self.transition_to(t_now, self.state.TELEOP)
 
@@ -135,26 +131,26 @@ def update(self, t_now: float, arm_input: 'ArmMobileIOInputs | None'):
 
             if arm_input.lock_toggle:
                 self.locked = arm_input.locked
+                if not self.locked:
+                    self.phone_xyz_init = arm_input.phone_pos.copy()
+                    self.phone_rot_init = arm_input.phone_rot.copy()
+
+                    xyz = np.zeros(3)
+                    orientation = np.zeros((3, 3))
+                    self.arm.FK(self.arm.last_feedback.position,
+                                xyz_out=xyz,
+                                orientation_out=orientation)
+
+                    self.last_locked_xyz = xyz.copy()
+                    self.last_locked_rot = orientation.copy()
 
             if not self.locked:
                 arm_goal = self.compute_arm_goal(arm_input)
-                if arm_goal is not None:
-                    self.arm.set_goal(arm_goal)
+                self.arm.set_goal(arm_goal)
             else:
-                self.phone_xyz_home = arm_input.phone_pos
-                self.phone_rot_home = arm_input.phone_rot
+                self.xyz_scale = self.xyz_scale_init * arm_input.xyz_scaling
 
-                self.last_locked_seed = last_pos.copy()
-                xyz = np.zeros(3)
-                orientation = np.zeros((3, 3))
-                self.arm.FK(last_pos,
-                            xyz_out=xyz,
-                            orientation_out=orientation)
-
-                self.last_locked_xyz = xyz
-                self.last_locked_rot = orientation
-
-            if self.gripper is not None:
+            if self.gripper:
                 gripper_closed = self.gripper.state == 1.0
                 if arm_input.gripper_closed and not gripper_closed:
                     print('Gripper Close')
@@ -186,20 +182,19 @@ def transition_to(self, t_now: float, state: ArmControlState):
         self.state = state
 
     def compute_arm_goal(self, arm_input: ArmMobileIOInputs):
-        phone_offset = arm_input.phone_pos - self.phone_xyz_home
-        rot_mat = self.phone_rot_home
-        arm_xyz_target = self.last_locked_xyz + arm_input.ar_scaling * \
-            self.xyz_scale * (rot_mat.T @ phone_offset)
-        arm_rot_target = rot_mat.T @ arm_input.phone_rot @ self.last_locked_rot
+        phone_offset = arm_input.phone_pos - self.phone_xyz_init
+        arm_xyz_target = self.last_locked_xyz + self.xyz_scale * \
+            (self.phone_rot_init.T @ phone_offset)
+        arm_rot_target = (arm_input.phone_rot @ self.phone_rot_init.T) @ self.last_locked_rot
 
         # if ar scaling is 0, move the home AR pose to current pose
         # this keeps the arm from driving to some weird offset when scaling
         # is turned back up by the user in the future
-        if arm_input.ar_scaling == 0.0:
+        if arm_input.xyz_scaling == 0.0:
             self.phone_xyz_home = arm_input.phone_pos
 
         joint_target = self.arm.ik_target_xyz_so3(
-            self.last_locked_seed,
+            self.arm.last_feedback.position,
             arm_xyz_target,
             arm_rot_target)
 
@@ -217,15 +212,33 @@ def stop(self):
 
 
 def setup_mobile_io(m: 'MobileIO'):
-    m.set_button_label(1, '⟲')
-    m.set_button_label(5, 'lock')
+    m.resetUI()
+    m.set_button_label(1, '⟲', blocking=False)
+    m.set_button_label(2, '', blocking=False)
+    m.set_button_label(3, '', blocking=False)
+    m.set_button_label(4, '', blocking=False)
+    m.set_button_label(5, 'Lock', blocking=False)
     m.set_button_mode(5, 1)
-    m.set_button_label(7, 'grip')
+    m.set_button_label(6, '', blocking=False)
+    m.set_button_label(7, 'Gripper', blocking=False)
     m.set_button_mode(7, 1)
-    m.set_button_label(8, '❌')
-
-
-def parse_mobile_feedback(m: 'MobileIO'):
+    m.set_button_label(8, '❌', blocking=False)
+
+    m.set_axis_label(1, '', blocking=False)
+    m.set_axis_label(2, '', blocking=False)
+    m.set_axis_label(3, '', blocking=False)
+    m.set_axis_label(4, 'XYZ\nScale', blocking=False)
+    m.set_axis_label(5, '', blocking=False)
+    m.set_axis_label(6, '', blocking=False)
+    m.set_axis_label(7, '', blocking=False)
+    m.set_axis_label(8, '', blocking=False)
+
+    m.set_snap(4, np.nan)
+    global xyz_scaling
+    xyz_scaling = 1.0
+    m.set_axis_value(4, xyz_scaling)
+
+def parse_mobile_io_feedback(m: 'MobileIO', locked: bool):
     if not m.update(0.0):
         return False, None
 
@@ -243,40 +256,82 @@ def parse_mobile_feedback(m: 'MobileIO'):
         print(f'Error getting orientation as matrix: {e}\n{m.orientation}')
         rotation = np.eye(3)
 
+    global xyz_scaling
+    # Disable xyz scaling if the arm is unlocked
+    if locked:
+        xyz_scaling = (m.get_axis_state(4) + 1.0) / 2.0
+    if not locked:
+        m.set_axis_value(4, (2 * xyz_scaling) - 1.0)
+
     arm_input = ArmMobileIOInputs(
         phone_pos=np.copy(m.position),
         phone_rot=rotation,
-        lock_toggle=m.get_button_state(5),
-        locked=m.get_button_state(7))
+        lock_toggle=(m.get_button_diff(5) != 0),
+        locked=(not m.get_button_state(5)),
+        gripper_closed=m.get_button_state(7),
+        xyz_scaling=(m.get_axis_state(4) + 1.0) / 2.0
+    )
 
     return False, arm_input
 
 
 if __name__ == "__main__":
+    # Parse command line arguments
+    parser = argparse.ArgumentParser(description='Control a HEBI arm with AR input')
+    parser.add_argument('--enable-logging', action='store_true', help='Enable logging')
+    args = parser.parse_args()
+
     lookup = hebi.Lookup()
     sleep(2)
 
     # Arm setup
-    cfg = hebi.config.load_config('./config/A-2580-06.cfg.yaml')
+    root_dir = os.path.abspath(os.path.dirname(__file__))
+    cfg_file = os.path.join(root_dir, "config", "A-2580-06G.cfg.yaml")
+    cfg = hebi.config.load_config(cfg_file)
+    family = cfg.families[0]
+
+    # Create Arm object
     arm = hebi.arm.create_from_config(cfg, lookup)
 
-    arm_control = ArmMobileIOControl(arm)
+    enable_logging = args.enable_logging
+    if enable_logging:
+        arm.group.start_log('dir', 'logs', mkdirs=True)
 
-    arm_family = cfg.families[0]
+    # Check if the gripper is disabled
+    use_gripper = cfg.user_data.get('has_gripper', False)
+    gripper = None
+    if use_gripper:
+        gripper_family = family
+        gripper_name = 'gripperSpool'
+
+        gripper_group = lookup.get_group_from_names([gripper_family], [gripper_name])
+        while gripper_group is None:
+            print(f"Looking for gripper module {gripper_family} / {gripper_name} ...")
+            sleep(1)
+            gripper_group = lookup.get_group_from_names([gripper_family], [gripper_name])
+
+        print("Gripper module found.")
+        gripper = hebi.arm.Gripper(gripper_group, -5, 1)
+        gripper_gains = os.path.join(root_dir, "config/gains/gripper_spool_gains.xml")
+        gripper.load_gains(gripper_gains)
+
+    arm_control = ArmMobileIOControl(arm,
+                                     gripper,
+                                     ik_seed=cfg.user_data.get('ik_seed_pos', None)
+                                     )
 
     # Setup MobileIO
     print('Looking for mobileIO device...')
-    m = create_mobile_io(lookup, arm_family)
+    m = create_mobile_io(lookup, family)
     while m is None:
         try:
             print('Waiting for mobileIO device to come online...')
             sleep(1)
-            m = create_mobile_io(lookup, arm_family)
+            m = create_mobile_io(lookup, family)
         except KeyboardInterrupt as e:
             exit()
 
     print("mobileIO device found.")
-    m.resetUI()
     m.update()
     setup_mobile_io(m)
 
@@ -287,7 +342,7 @@ def parse_mobile_feedback(m: 'MobileIO'):
     while arm_control.running:
         t = time()
         try:
-            quit, arm_input = parse_mobile_feedback(m)
+            quit, arm_input = parse_mobile_io_feedback(m, arm_control.locked)
             if quit:
                 break
             arm_control.update(t, arm_input)
@@ -296,3 +351,5 @@ def parse_mobile_feedback(m: 'MobileIO'):
             break
 
     arm_control.stop()
+    if enable_logging:
+        hebi_log = arm.group.stop_log()
diff --git a/kits/arms/joystick_control_sm.py b/kits/arms/joystick_control_sm.py
index 8505515..0dfaed3 100644
--- a/kits/arms/joystick_control_sm.py
+++ b/kits/arms/joystick_control_sm.py
@@ -1,8 +1,9 @@
-import os
 from enum import Enum, auto
+from dataclasses import dataclass, field
+from time import time, sleep
+import os
 import numpy as np
 from scipy.spatial.transform import Rotation as R
-from time import time, sleep
 import argparse
 
 import hebi
@@ -10,9 +11,10 @@
 
 import typing
 if typing.TYPE_CHECKING:
-    from typing import Sequence, Optional
+    from typing import Sequence, Callable
     import numpy.typing as npt
     from hebi._internal.mobile_io import MobileIO
+    from hebi.arm import Arm, Gripper
 
 
 class ArmControlState(Enum):
@@ -23,21 +25,37 @@ class ArmControlState(Enum):
     EXIT = auto()
 
 
+@dataclass(kw_only=True)
 class ArmJoystickInputs:
-    def __init__(self, home: bool, delta_xyz, delta_rot_xyz, gripper_closed: bool):
-        self.home = home
-        self.delta_xyz: 'npt.NDArray[np.float64]' = np.array(delta_xyz, dtype=np.float64)
-        self.delta_rot_xyz: 'npt.NDArray[np.float64]' = np.array(delta_rot_xyz, dtype=np.float64)
-        self.gripper_closed = gripper_closed
+    delta_xyz: 'npt.NDArray[np.float32]' = field(default_factory=lambda: np.array([0., 0., 0.]))
+    delta_rot: 'npt.NDArray[np.float64]' = field(default_factory=lambda: np.eye(3))
+    gripper_closed: bool = False
+    home: bool = False
 
 
 class ArmJoystickControl:
-    def __init__(self, arm: hebi.arm.Arm, home_pose: 'Sequence[float] | npt.NDArray[np.float64]', homing_time: float = 5.0, shoulder_flip_angle=-np.pi / 2, joint_limits=None, use_gripper=False):
+    def __init__(self, arm: 'Arm', gripper: 'Gripper | None' = None, home_pose: 'Sequence[float] | npt.NDArray[np.float64] | None' = None, ik_seed: 'Sequence[float] | npt.NDArray[np.float64] | None' = None, homing_time: float = 5.0, traj_duration: float = 0.25, shoulder_flip_angle=-np.pi / 2, joint_limits=None):
+        self.namespace = ''
+
         self.state = ArmControlState.STARTUP
         self.arm = arm
-        self.use_gripper = use_gripper
+        self.gripper = gripper
+
+        self.arm_xyz_home = [0.5, 0.0, 0.15]
+        self.arm_rot_home: 'npt.NDArray[np.float64]' = R.from_euler('x', np.pi).as_matrix()
+        self.homing_time = homing_time
+        self.traj_duration = traj_duration
+
+        # Set the IK seed for the arm
+        self.arm_seed_ik = ik_seed if ik_seed is not None else np.array([0.01, 1.4, 1.8, 2.0, -1.57, 0.01])[:arm.size]
+        assert len(self.arm_seed_ik) == arm.size, \
+            f"IK seed must have the same size as the arm ({arm.size}), got {self.arm_seed_ik.shape[0]} elements."
+        # Set the home position for the arm
+        self.arm_home = home_pose if home_pose is not None else self.arm.ik_target_xyz_so3(
+            self.arm_seed_ik,
+            self.arm_xyz_home,
+            self.arm_rot_home)
 
-        self.arm_home = home_pose
         self.homing_time = homing_time
         # TODO: GENERALIZE THIS
         self.shoulder_flip_angle = shoulder_flip_angle
@@ -51,6 +69,7 @@ def __init__(self, arm: hebi.arm.Arm, home_pose: 'Sequence[float] | npt.NDArray[
         self.xyz_curr = np.empty(3)
         self.rot_curr = np.empty((3, 3))
         self.joint_target = arm.last_feedback.position_command
+        self.mobile_last_fbk_t = time()
 
     @property
     def running(self):
@@ -58,47 +77,56 @@ def running(self):
 
     def send(self):
         self.arm.send()
+        if self.gripper:
+            self.gripper.send()
 
-    def update(self, t_now: float, demo_input: 'Optional[ArmJoystickInputs]' = None):
+    def update(self, t_now: float, arm_input: 'ArmJoystickInputs | None' = None):
         self.arm.update()
 
         if self.state is self.state.EXIT:
             return
 
-        if demo_input is None:
-            if t_now - self.mobile_last_fbk_t > 1.0 and self.state is not self.state.DISCONNECTED:
-                print("mobileIO timeout, disabling motion")
+        if arm_input is None:
+            if self.state is not self.state.DISCONNECTED and t_now - self.mobile_last_fbk_t > 1.0:
+                print(self.namespace + "mobileIO timeout, disabling motion")
                 self.transition_to(t_now, self.state.DISCONNECTED)
             return
 
+        # Reset the timeout
         self.mobile_last_fbk_t = t_now
-
+        # Transition to teleop if mobileIO is reconnected
         if self.state is self.state.DISCONNECTED:
             self.mobile_last_fbk_t = t_now
+            print(self.namespace + 'Controller reconnected, demo continued.')
             self.transition_to(t_now, self.state.TELEOP)
 
+        # After startup, transition to homing
+        elif self.state is self.state.STARTUP:
+            self.transition_to(t_now, self.state.HOMING)
+
+        # If homing is complete, transition to teleop
         elif self.state is self.state.HOMING:
             if self.arm.at_goal:
                 self.joint_target = self.arm.last_feedback.position_command
                 self.transition_to(t_now, self.state.TELEOP)
 
+        # Teleop mode
         elif self.state is self.state.TELEOP:
-            if demo_input.home:
+            if arm_input.home:
                 self.transition_to(t_now, self.state.HOMING)
                 return
 
-            arm_goal = self.compute_arm_goal(demo_input)
+            arm_goal = self.compute_arm_goal(arm_input)
             self.arm.set_goal(arm_goal)
 
-            if self.use_gripper:
-                gripper_closed = self.arm.end_effector.state == 1.0
-                if demo_input.gripper_closed and not gripper_closed:
-                    self.arm.end_effector.close()
-                elif not demo_input.gripper_closed and gripper_closed:
-                    self.arm.end_effector.open()
-
-        elif self.state is self.state.STARTUP:
-            self.transition_to(t_now, self.state.HOMING)
+            if self.gripper:
+                gripper_closed = self.gripper.state == 1.0
+                if arm_input.gripper_closed and not gripper_closed:
+                    print('Gripper Close')
+                    self.gripper.close()
+                elif not arm_input.gripper_closed and gripper_closed:
+                    print('Gripper Open')
+                    self.gripper.open()
 
     def transition_to(self, t_now: float, state: ArmControlState):
         # self transitions are noop
@@ -106,24 +134,21 @@ def transition_to(self, t_now: float, state: ArmControlState):
             return
 
         if state is self.state.HOMING:
-            print("TRANSITIONING TO HOMING")
-            g = hebi.arm.Goal(self.arm.size)
-            g.add_waypoint(t=self.homing_time, position=self.arm_home)
-            self.arm.set_goal(g)
+            print(self.namespace + "TRANSITIONING TO HOMING")
+            self.home(self.homing_time)
 
         elif state is self.state.TELEOP:
-            print("TRANSITIONING TO TELEOP")
+            print(self.namespace + "TRANSITIONING TO TELEOP")
 
         elif state is self.state.DISCONNECTED:
-            print("TRANSITIONING TO DISCONNECTED")
+            print(self.namespace + "mobileIO timeout, disabling motion")
 
         elif state is self.state.EXIT:
-            print("TRANSITIONING TO EXIT")
+            print(self.namespace + "TRANSITIONING TO EXIT")
 
         self.state = state
 
     def compute_arm_goal(self, arm_inputs: ArmJoystickInputs):
-        arm_goal = hebi.arm.Goal(self.arm.size)
         pos_curr = self.arm.last_feedback.position_command
 
         if np.any(np.isnan(pos_curr)):
@@ -138,11 +163,11 @@ def compute_arm_goal(self, arm_inputs: ArmJoystickInputs):
 
         arm_xyz_target = self.xyz_curr + arm_inputs.delta_xyz
 
-        r_x = R.from_euler('x', arm_inputs.delta_rot_xyz[0])
-        r_y = R.from_euler('y', arm_inputs.delta_rot_xyz[1])
-        r_z = R.from_euler('z', arm_inputs.delta_rot_xyz[2])
+        r_x = R.from_euler('x', arm_inputs.delta_rot[0])
+        r_y = R.from_euler('y', arm_inputs.delta_rot[1])
+        r_z = R.from_euler('z', arm_inputs.delta_rot[2])
         wrist_rot = R.from_euler('z', -pos_curr[5])
-        arm_rot_target = R.from_matrix(self.rot_curr) * wrist_rot * r_x * r_y * r_z * wrist_rot.inv()
+        arm_rot_target = (R.from_matrix(self.rot_curr) * wrist_rot * r_x * r_y * r_z * wrist_rot.inv()).as_matrix()
 
         #curr_seed_ik = pos_curr
         #curr_seed_ik[2] = abs(curr_seed_ik[2])
@@ -154,7 +179,7 @@ def compute_arm_goal(self, arm_inputs: ArmJoystickInputs):
         joint_target = self.arm.ik_target_xyz_so3(
             self.joint_target,
             arm_xyz_target,
-            arm_rot_target.as_matrix())
+            arm_rot_target)
 
         out_of_bounds = False
         for idx, joint in enumerate(joint_target):
@@ -164,94 +189,108 @@ def compute_arm_goal(self, arm_inputs: ArmJoystickInputs):
         if not out_of_bounds:
             self.joint_target = joint_target
 
-        arm_goal.add_waypoint(position=self.joint_target)
+        arm_goal = hebi.arm.Goal(self.arm.size)
+        arm_goal.add_waypoint(t=self.traj_duration, position=self.joint_target)
         return arm_goal
 
+    def home(self, duration):
+        g = hebi.arm.Goal(self.arm.size)
+        g.add_waypoint(t=duration, position=self.arm_home)
+        self.arm.set_goal(g)
+
+    def stop(self):
+        self.transition_to(time(), self.state.EXIT)
+
 
 def setup_mobile_io(m: 'MobileIO'):
     m.resetUI()
     m.set_button_label(1, '⟲', blocking=False)
     m.set_button_label(2, '', blocking=False)
-    m.set_button_label(3, '', blocking=False)
-    m.set_button_label(4, 'Quit', blocking=False)
-    m.set_button_label(6, '\u21E7', blocking=False)
-    m.set_button_label(7, 'grip', blocking=False)
+    m.set_button_label(3, 'Quit', blocking=False)
+    m.set_button_label(4, '', blocking=False)
+    m.set_button_label(5, 'grip', blocking=False)
+    m.set_button_label(6, '\u2191', blocking=False)
+    m.set_button_label(7, '', blocking=False)
     m.set_button_mode(7, 1)
-    m.set_button_label(8, '\u21E9', blocking=False)
+    m.set_button_label(8, '\u2193', blocking=False)
 
-    m.set_axis_label(3, '', blocking=False)
-    m.set_axis_label(4, '', blocking=False)
-    m.set_axis_label(5, '', blocking=False)
+    m.set_axis_label(1, '', blocking=False)
+    m.set_axis_label(2, 'rotate', blocking=False)
+    m.set_axis_label(3, 'wrist', blocking=False)
+    m.set_axis_label(4, 'XYZ\nScale', blocking=False)
+    m.set_axis_label(5, 'Rot\nScale', blocking=False)
     m.set_axis_label(6, '', blocking=False)
+    m.set_axis_label(7, '', blocking=False)
+    m.set_axis_label(8, 'translate', blocking=False)
 
-    m.set_axis_label(1, '')
-    m.set_axis_label(7, '')
-    m.set_axis_label(2, 'rotate')
-    m.set_axis_label(8, 'translate')
-    m.set_axis_label(3, 'wrist', blocking=False)
     m.set_snap(3, 0)
+    m.set_snap(4, np.nan)
+    m.set_snap(5, np.nan)
+    m.set_axis_value(4, 0.0)
+    m.set_axis_value(5, 0.0)
 
-
-def parse_mobile_feedback(m: 'MobileIO'):
+def parse_mobile_io_feedback(m: 'MobileIO'):
     if not m.update(0.0):
-        return None
+        return False, None
+
+    if m.get_button_state(3):
+        return True, None
 
-    home = m.get_button_state(1)
+    if m.get_button_state(1):
+        return False, ArmJoystickInputs(home=True)
+    
+    xyz_scale = (m.get_axis_state(4) + 1.0) / 30.0
+    rot_scale = (m.get_axis_state(5) + 1.0) / 10.0
 
-    arm_dx = 0.25 * m.get_axis_state(8)
-    arm_dy = -0.25 * m.get_axis_state(7)
+    arm_dx = xyz_scale * m.get_axis_state(8)
+    arm_dy = -xyz_scale * m.get_axis_state(7)
 
     arm_dz = 0.0
     if m.get_button_state(6):
-        arm_dz = 0.1
+        arm_dz = xyz_scale
     elif m.get_button_state(8):
-        arm_dz = -0.1
+        arm_dz = -xyz_scale
 
-    arm_drx = 0.5 * m.get_axis_state(1)
-    arm_dry = -0.5 * m.get_axis_state(2)
-    arm_drz = 0.75 * m.get_axis_state(3)
+    arm_drx = -rot_scale * m.get_axis_state(2)
+    arm_dry = -rot_scale * m.get_axis_state(1)
+    arm_drz = -rot_scale * m.get_axis_state(3)
 
-    gripper_closed = m.get_button_state(7)
+    arm_input = ArmJoystickInputs(
+        delta_xyz=[arm_dx, arm_dy, arm_dz],
+        delta_rot=[arm_drx, arm_dry, arm_drz],
+        gripper_closed=m.get_button_state(5),
+    )
 
-    arm_inputs = ArmJoystickInputs(
-        home,
-        [arm_dx, arm_dy, arm_dz],
-        [arm_drx, arm_dry, arm_drz],
-        gripper_closed=gripper_closed)
-
-    return arm_inputs
+    return False, arm_input
 
 
 if __name__ == "__main__":
     # Parse command line arguments
-    parser = argparse.ArgumentParser(description='Control a HEBI arm with joystick inputs')
-    parser.add_argument('--no-gripper', action='store_true', help='Disable gripper control')
+    parser = argparse.ArgumentParser(description='Control a HEBI arm with joystick input')
+    parser.add_argument('--enable-logging', action='store_true', help='Enable logging')
     args = parser.parse_args()
 
     lookup = hebi.Lookup()
     sleep(2)
 
     # Arm setup
-    arm_family = "Arm"
-    module_names = ['J1_base', 'J2_shoulder', 'J3_elbow', 'J4_wrist1', 'J5_wrist2', 'J6_wrist3']
-    hrdf_file = "config/hrdf/A-2580-06.hrdf"
-    gains_file = "config/gains/A-2580-06.xml"
-
     root_dir = os.path.abspath(os.path.dirname(__file__))
-    hrdf_file = os.path.join(root_dir, hrdf_file)
-    gains_file = os.path.join(root_dir, gains_file)
+    cfg_file = os.path.join(root_dir, "config", "A-2580-06G.cfg.yaml")
+    cfg = hebi.config.load_config(cfg_file)
+    family = cfg.families[0]
 
     # Create Arm object
-    arm = hebi.arm.create([arm_family],
-                          names=module_names,
-                          hrdf_file=hrdf_file,
-                          lookup=lookup)
-    arm.load_gains(gains_file)
+    arm = hebi.arm.create_from_config(cfg, lookup)
+
+    enable_logging = args.enable_logging
+    if enable_logging:
+        arm.group.start_log('dir', 'logs', mkdirs=True)
 
     # Check if the gripper is disabled
-    use_gripper = not args.no_gripper
+    use_gripper = cfg.user_data.get('has_gripper', False)
+    gripper = None
     if use_gripper:
-        gripper_family = arm_family
+        gripper_family = family
         gripper_name = 'gripperSpool'
 
         gripper_group = lookup.get_group_from_names([gripper_family], [gripper_name])
@@ -260,10 +299,10 @@ def parse_mobile_feedback(m: 'MobileIO'):
             sleep(1)
             gripper_group = lookup.get_group_from_names([gripper_family], [gripper_name])
 
+        print("Gripper module found.")
         gripper = hebi.arm.Gripper(gripper_group, -5, 1)
         gripper_gains = os.path.join(root_dir, "config/gains/gripper_spool_gains.xml")
         gripper.load_gains(gripper_gains)
-        arm.set_end_effector(gripper)
 
     joint_limits = np.empty((7, 2))
     joint_limits[:, 0] = -np.inf
@@ -275,19 +314,23 @@ def parse_mobile_feedback(m: 'MobileIO'):
     # joint_limits[1, 0] = -2.0
 
     arm_control = ArmJoystickControl(arm,
-                                     [0.0, 2.09, 2.09, 0.0, 1.57, 0.0],
-                                     homing_time=3.0,
-                                     joint_limits=joint_limits,
-                                     use_gripper=use_gripper)
+                                     gripper,
+                                     ik_seed=cfg.user_data.get('ik_seed_pos', None),
+                                     joint_limits=joint_limits
+                                     )
 
     # Setup MobileIO
-    print('Looking for Mobile IO...')
-    m = create_mobile_io(lookup, arm_family)
+    print('Looking for mobileIO device...')
+    m = create_mobile_io(lookup, family)
     while m is None:
-        print('Waiting for Mobile IO device to come online...')
-        sleep(1)
-        m = create_mobile_io(lookup, arm_family)
+        try:
+            print('Waiting for mobileIO device to come online...')
+            sleep(1)
+            m = create_mobile_io(lookup, family)
+        except KeyboardInterrupt as e:
+            exit()
 
+    print("mobileIO device found.")
     m.update()
     setup_mobile_io(m)
 
@@ -298,14 +341,14 @@ def parse_mobile_feedback(m: 'MobileIO'):
     while arm_control.running:
         t = time()
         try:
-            arm_inputs = parse_mobile_feedback(m)
-            arm_control.update(t, arm_inputs)
-
+            quit, arm_input = parse_mobile_io_feedback(m)
+            if quit:
+                break
+            arm_control.update(t, arm_input)
             arm_control.send()
-        except KeyboardInterrupt:
-            arm_control.transition_to(t, ArmControlState.EXIT)
-            m.set_led_color('red')
+        except KeyboardInterrupt as e:
+            break
 
-        if m.get_button_state(4):
-            arm_control.transition_to(t, ArmControlState.EXIT)
-            m.set_led_color('red')
+    arm_control.stop()
+    if enable_logging:
+        hebi_log = arm.group.stop_log()
diff --git a/kits/rosie/rosie_arm_io_control.py b/kits/rosie/rosie_arm_io_control.py
index fec8f94..a13e1e9 100644
--- a/kits/rosie/rosie_arm_io_control.py
+++ b/kits/rosie/rosie_arm_io_control.py
@@ -178,7 +178,7 @@ def parse_mobile_io_feedback(m: 'MobileIO'):
 
     lookup = hebi.Lookup()
 
-    root_dir = os.path.dirname(__file__)
+    root_dir = os.path.abspath(os.path.dirname(__file__))
     cfg_file = os.path.join(root_dir, 'config', 'rosie-t.cfg.yaml')
     cfg = hebi.config.load_config(cfg_file)
     family = cfg.families[0]