diff --git a/src/ar3_config/config/ar3_joy.yaml b/src/ar3_config/config/ar3_joy.yaml
index 12ae0db..a2ecc43 100644
--- a/src/ar3_config/config/ar3_joy.yaml
+++ b/src/ar3_config/config/ar3_joy.yaml
@@ -4,7 +4,7 @@
 use_gazebo: false # Whether the robot is started in a Gazebo simulation environment
 
 ## Properties of incoming commands
-command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s
+command_in_type: "unitless" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s
 scale:
   # Scale parameters are only used if command_in_type=="unitless"
   linear:  0.4 # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands.
diff --git a/src/ar3_config/launch/dev.launch.py b/src/ar3_config/launch/dev.launch.py
index 872ceb2..ad650d4 100644
--- a/src/ar3_config/launch/dev.launch.py
+++ b/src/ar3_config/launch/dev.launch.py
@@ -123,5 +123,6 @@ def generate_launch_description():
             package='robot_state_publisher',
             executable='robot_state_publisher',
             output='screen',
-            parameters=[robot_description]),
+            parameters=[robot_description]
+        ),
     ])
diff --git a/src/ar3_description/ar3_description/state_publisher.py b/src/ar3_description/ar3_description/state_publisher.py
index 88b9136..36e23ed 100644
--- a/src/ar3_description/ar3_description/state_publisher.py
+++ b/src/ar3_description/ar3_description/state_publisher.py
@@ -47,7 +47,7 @@ class StatePublisher(Node):
                 # joint_state.position = [random.random() * pi / 2] * 6
                 # joint_state.position = [angle * 16, pi/2., -25 / 180 * pi, 0., 0., 0.]
                 # joint_state.position = [angle * 0.01 * random.random()] * 6
-                joint_state.position = [0., 0., 0., 0., 0., 0.]
+                # joint_state.position = [0., 0., 0., 0., 0., 0.]
 
                 # update transform
                 # (moving in a circle with radius=2)
diff --git a/src/ar3_hardware/src/ar3_system.cpp b/src/ar3_hardware/src/ar3_system.cpp
index b4aff4d..0c831a6 100644
--- a/src/ar3_hardware/src/ar3_system.cpp
+++ b/src/ar3_hardware/src/ar3_system.cpp
@@ -176,7 +176,7 @@ void Ar3SystemHardware::update_j6_cb(std_msgs::msg::Float32::SharedPtr msg) {
 
 hardware_interface::return_type Ar3SystemHardware::read()
 {
-  //RCLCPP_INFO(rclcpp::get_logger("Ar3SystemHardware"), "Reading...");
+  // RCLCPP_INFO(rclcpp::get_logger("Ar3SystemHardware"), "Reading...");
 
   /*
   for (uint i = 0; i < hw_commands_.size(); i++)
diff --git a/src/cobot_platform/cobot_platform/move.py b/src/cobot_platform/cobot_platform/move.py
index eaef00f..bdba73b 100644
--- a/src/cobot_platform/cobot_platform/move.py
+++ b/src/cobot_platform/cobot_platform/move.py
@@ -3,20 +3,12 @@ from rclpy.node import Node
 from rclpy.qos import QoSProfile
 
 from sensor_msgs.msg import JointState
+from std_msgs.msg import Float32
 
 from math import pi
 
-from std_msgs.msg import Float32
-
 from serial import Serial
 
-from enum import Enum
-
-class Servos(Enum):
-    J1 = 'A'
-    J2 = 'B'
-    J3 = 'C'
-
 class CobotMove(Node):
     def __init__(self):
         super().__init__('cobot_move')
@@ -39,12 +31,12 @@ class CobotMove(Node):
         self.create_subscription(Float32, base_topic+'j6', self._move_j6_cb, rclpy.qos.qos_profile_sensor_data)
 
         publish_base_topic = 'update/'
-        self.s_j1 = self.create_publisher(Float32, publish_base_topic+'j1', rclpy.qos.qos_profile_sensor_data)
-        self.s_j2 = self.create_publisher(Float32, publish_base_topic+'j2', rclpy.qos.qos_profile_sensor_data)
-        self.s_j3 = self.create_publisher(Float32, publish_base_topic+'j3', rclpy.qos.qos_profile_sensor_data)
-        self.s_j4 = self.create_publisher(Float32, publish_base_topic+'j4', rclpy.qos.qos_profile_sensor_data)
-        self.s_j5 = self.create_publisher(Float32, publish_base_topic+'j5', rclpy.qos.qos_profile_sensor_data)
-        self.s_j6 = self.create_publisher(Float32, publish_base_topic+'j6', rclpy.qos.qos_profile_sensor_data)
+        self.s_j1 = self.create_publisher(Float32, publish_base_topic+'j1', 10)
+        self.s_j2 = self.create_publisher(Float32, publish_base_topic+'j2', 10)
+        self.s_j3 = self.create_publisher(Float32, publish_base_topic+'j3', 10)
+        self.s_j4 = self.create_publisher(Float32, publish_base_topic+'j4', 10)
+        self.s_j5 = self.create_publisher(Float32, publish_base_topic+'j5', 10)
+        self.s_j6 = self.create_publisher(Float32, publish_base_topic+'j6', 10)
 
         qos = QoSProfile(depth=10)
         self._joint_pub = self.create_publisher(JointState, 'joint_states', qos)
@@ -80,7 +72,6 @@ class CobotMove(Node):
     def _move_servo(self):
         self._serial.write(f'MTA{self._cur[0]}B{self._cur[1]}C{self._cur[2]}D{self._cur[3]}E{self._cur[4]}F{self._cur[5]}\n'.encode('UTF-8'))
 
-
         line = self._serial.readline().decode('UTF-8')
 
         blocks = re.findall(r'[ABCDEF]-*\d+', line)
@@ -103,7 +94,6 @@ class CobotMove(Node):
         self.s_j4.publish(Float32(data=d))
         self.s_j5.publish(Float32(data=e))
         self.s_j6.publish(Float32(data=f))
-
     #    self._logger.info(f'published {a} {b} {c} {d} {e} {f}')
     # def _push ?
 
diff --git a/src/joystickmove/joystickmove/move.py b/src/joystickmove/joystickmove/move.py
index e396337..94f69d4 100644
--- a/src/joystickmove/joystickmove/move.py
+++ b/src/joystickmove/joystickmove/move.py
@@ -43,16 +43,16 @@ class JoyMove(Node):
         self._logger.info('joystick init')
 
         JOY_TOPIC = '/joy'
-        # TWIST_TOPIC = '/servo_server/delta_twist_cmds'
+        TWIST_TOPIC = '/servo_server/delta_twist_cmds'
         JOINT_TOPIC = '/servo_server/delta_joint_cmds'
-        # EE = 'joint_6'
-        # BASE = 'base_link'
+        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.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.collision_pub = self.create_publisher(PlanningScene, "/planning_scene", 10);
 
         self.servo_client = self.create_client(Trigger, '/servo_server/start_servo')
         # Check if the a service is available
@@ -74,35 +74,40 @@ class JoyMove(Node):
         axes = msg.axes
         buttons = msg.buttons
 
-        # ts = TwistStamped()
-        jj = JointJog()
+        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()
 
-        jj.joint_names = ['joint_2']
-        # jj.displacements = [axes[0]]
-        jj.velocities = [axes[0]]
-        # jj.duration = 1.0
-        # ts.twist.linear.z = axes[Axes.RIGHT_Y.value]
-        # ts.twist.linear.y = axes[Axes.RIGHT_X.value]
+            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 = 0.1
 
-        # lin_x_right = -0.5 * axes[Axes.R1.value] # - AXIS_DEFAULTS.at(R1));
-        # lin_x_left = 0.5 * axes[Axes.L1.value] # - AXIS_DEFAULTS.at(L1));
-        # ts.twist.linear.x = lin_x_right + lin_x_left
+            joint.header.stamp = self.get_clock().now().to_msg()
+            joint.header.frame_id = "link_3"
+            self.joint_pub.publish(joint)
+            return
 
-        # ts.twist.angular.y = axes[Axes.LEFT_Y.value]
-        # ts.twist.angular.x = axes[Axes.LEFT_X.value]
+        ts = TwistStamped()
 
-        # roll_positive = buttons[Buttons.R2.value]
-        # roll_negative = -1 * buttons[Buttons.L2.value]
-        # ts.twist.angular.z = float(roll_positive + roll_negative)
+        ts.twist.linear.z = axes[Axes.RIGHT_Y.value]
+        ts.twist.linear.y = axes[Axes.RIGHT_X.value]
 
-        # ts.header.stamp = self.get_clock().now().to_msg()
-        # ts.header.frame_id = "base_link"
+        lin_x_right = -0.5 * axes[Axes.R1.value] # - AXIS_DEFAULTS.at(R1));
+        lin_x_left = 0.5 * axes[Axes.L1.value] # - AXIS_DEFAULTS.at(L1));
+        ts.twist.linear.x = lin_x_right + lin_x_left
 
-        jj.header.stamp = self.get_clock().now().to_msg()
-        jj.header.frame_id = "base_link"
+        ts.twist.angular.y = axes[Axes.LEFT_Y.value]
+        ts.twist.angular.x = axes[Axes.LEFT_X.value]
 
-        # self.twist_pub.publish(ts)
-        self.joint_pub.publish(jj)
+        roll_positive = buttons[Buttons.R2.value]
+        roll_negative = -1 * buttons[Buttons.L2.value]
+        ts.twist.angular.z = float(roll_positive + roll_negative)
+
+        ts.header.stamp = self.get_clock().now().to_msg()
+        ts.header.frame_id = "base_link"
+
+        self.twist_pub.publish(ts)
 
 
 def main(args=None):