diff --git a/src/ar3_config/config/ar3_joy.yaml b/src/ar3_config/config/ar3_joy.yaml
index 5e7bfa1..ef893d3 100644
--- a/src/ar3_config/config/ar3_joy.yaml
+++ b/src/ar3_config/config/ar3_joy.yaml
@@ -34,7 +34,7 @@ planning_frame: base_link  # The MoveIt planning frame. Often 'base_link' or 'wo
 
 ## Other frames
 ee_frame_name: link_6  # The name of the end effector link, used to return the EE pose
-robot_link_command_frame: base_link  # commands must be given in the frame of a robot link. Usually either the base or end effector
+robot_link_command_frame: link_6  # commands must be given in the frame of a robot link. Usually either the base or end effector
 
 ## Stopping behaviour
 incoming_command_timeout:  0.1  # Stop servoing if X seconds elapse without a new command
diff --git a/src/joystickmove/joystickmove/move.py b/src/joystickmove/joystickmove/move.py
index f305752..54a3b3b 100644
--- a/src/joystickmove/joystickmove/move.py
+++ b/src/joystickmove/joystickmove/move.py
@@ -7,6 +7,7 @@ from std_msgs.msg import Float32
 from sensor_msgs.msg import Joy
 from geometry_msgs.msg import TwistStamped
 from moveit_msgs.msg import PlanningScene, PlanningSceneWorld
+from control_msgs.msg import JointJog
 
 from enum import Enum
 
@@ -43,13 +44,14 @@ class JoyMove(Node):
 
         JOY_TOPIC = '/joy'
         TWIST_TOPIC = '/servo_server/delta_twist_cmds'
-        # JOINT_TOPIC = '/servo_server/delta_joint_cmds'
-        # EE = 'joint_6'
-        # BASE = 'base_link'
+        JOINT_TOPIC = '/servo_server/delta_joint_cmds'
+        EE = 'joint_6'
+        BASE = 'base_link'
 
         self.create_subscription(Joy, '/joy', self._joy_cb, rclpy.qos.qos_profile_sensor_data)
 
         self.twist_pub = self.create_publisher(TwistStamped, TWIST_TOPIC, 10);
+        self.joint_pub = self.create_publisher(JointJog, JOINT_TOPIC, 10);
         self.collision_pub = self.create_publisher(PlanningScene, "/planning_scene", 10);
 
         self.servo_client = self.create_client(Trigger, '/servo_server/start_servo')
@@ -72,6 +74,21 @@ class JoyMove(Node):
         axes = msg.axes
         buttons = msg.buttons
 
+        if buttons[Buttons.CROSS.value] or buttons[Buttons.CIRCLE.value] or buttons[Buttons.TRIANGLE.value] or buttons[Buttons.SQUARE.value] or axes[Axes.DPAD_UPDOWN.value] or axes[Axes.DPAD_LEFTRIGHT.value]:
+
+            joint = JointJog()
+
+            joint.joint_names = ['joint_1', 'joint_2',
+                                 'joint_6', 'joint_5']
+            joint.velocities = [float(axes[Axes.DPAD_UPDOWN.value]), float(axes[Axes.DPAD_LEFTRIGHT.value]),
+                                float(buttons[Buttons.CIRCLE.value] - buttons[Buttons.CROSS.value]), float(buttons[Buttons.SQUARE.value] - buttons[Buttons.TRIANGLE.value])]
+            joint.duration = 1.0
+
+            joint.header.stamp = self.get_clock().now().to_msg()
+            joint.header.frame_id = "link_3"
+            self.joint_pub.publish(joint)
+            return
+
         ts = TwistStamped()
 
         ts.twist.linear.z = axes[Axes.RIGHT_Y.value]
@@ -89,7 +106,7 @@ class JoyMove(Node):
         ts.twist.angular.z = float(roll_positive + roll_negative)
 
         ts.header.stamp = self.get_clock().now().to_msg()
-        ts.header.frame_id = "base_link"
+        ts.header.frame_id = "link_6"
 
         self.twist_pub.publish(ts)