Kinda works... ish

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: "unitless" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s
+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
 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.
@@ -14,7 +14,7 @@ scale:
 
 ## Properties of outgoing commands
 publish_period: 0.034  # 1/Nominal publish rate [seconds]
-low_latency_mode: false  # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored)
+low_latency_mode: true  # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored)
 
 # What type of topic does your robot driver expect?
 # Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory
@@ -37,14 +37,14 @@ ee_frame_name: link_6  # The name of the end effector link, used to return the E
 robot_link_command_frame: base_link  # 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
+incoming_command_timeout:  10.1  # Stop servoing if X seconds elapse without a new command
 # If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish.
 # Important because ROS may drop some messages and we need the robot to halt reliably.
 num_outgoing_halt_msgs_to_publish: 4
 
 ## Configure handling of singularities and joint limits
-lower_singularity_threshold: 17.0 # Start decelerating when the condition number hits this (close to singularity)
+lower_singularity_threshold: 170000.0 # Start decelerating when the condition number hits this (close to singularity)
-hard_stop_singularity_threshold: 30.0 # Stop when the condition number hits this
+hard_stop_singularity_threshold: 300000.0 # Stop when the condition number hits this
 joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger.
 
 ## Topic names
@@ -53,9 +53,10 @@ joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle comm
 joint_topic: /joint_states
 status_topic: ~/status # Publish status to this topic
 command_out_topic: /joint_trajectory_controller/joint_trajectory # Publish outgoing commands here
+# command_out_topic: /gaycunts # Publish outgoing commands here
 
 ## Collision checking for the entire robot body
-check_collisions: true # Check collisions?
+check_collisions: false # Check collisions?
 collision_check_rate: 10.0 # [Hz] Collision-checking can easily bog down a CPU if done too often.
 # Two collision check algorithms are available:
 # "threshold_distance" begins slowing down when nearer than a specified distance. Good if you want to tune collision thresholds manually.

src/ar3_config/launch/dev.launch.py

@@ -63,6 +63,7 @@ def generate_launch_description():
                         {'moveit_servo': get_yaml(join(config_root, 'ar3_joy.yaml'))},
                         robot_description,
                         robot_description_semantic,
+                        robot_description_kinematics,
                     ],
                     extra_arguments=[{"use_intra_process_comms": True}],
                 ),

src/cobot_platform/cobot_platform/move.py

@@ -54,11 +54,11 @@ class CobotMove(Node):
         print(self._serial.readline())
 
     def _move_j1_cb(self, msg):
-        self._cur[0] = int(msg.data / pi / 2. * 8000)
+        self._cur[0] = int(msg.data / pi / 2. * 8000 * 4)
         self._move_servo()
 
     def _move_j2_cb(self, msg):
-        self._cur[1] = int(msg.data / pi / 2. * 10000)
+        self._cur[1] = int(msg.data / pi / 2. * 10000 * 4)
         self._move_servo()
 
     def _move_j3_cb(self, msg):
@@ -84,8 +84,8 @@ class CobotMove(Node):
         line = self._serial.readline().decode('UTF-8')
 
         blocks = re.findall(r'[ABCDEF]-*\d+', line)
-        a = float(blocks[0][1:]) / 8000. * pi * 2.
+        a = float(blocks[0][1:]) / 8000. / 4. * pi * 2.
-        b = float(blocks[1][1:]) / 10000 * pi * 2.
+        b = float(blocks[1][1:]) / 10000 / 4. * pi * 2.
         c = float(blocks[2][1:]) / 10000. * pi * 2.
         d = float(blocks[3][1:]) / 2000. * pi * 2.
         e = float(blocks[4][1:]) / 2000. * pi * 2.

src/joystickmove/joystickmove/move.py

@@ -6,6 +6,7 @@ from std_srvs.srv import Trigger
 from std_msgs.msg import Float32
 from sensor_msgs.msg import Joy
 from geometry_msgs.msg import TwistStamped
+from control_msgs.msg import JointJog
 from moveit_msgs.msg import PlanningScene, PlanningSceneWorld
 
 from enum import Enum
@@ -42,15 +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'
+        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.twist_pub = self.create_publisher(TwistStamped, TWIST_TOPIC, 10);
-        self.collision_pub = self.create_publisher(PlanningScene, "/planning_scene", 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')
         # Check if the a service is available
@@ -72,26 +74,35 @@ class JoyMove(Node):
         axes = msg.axes
         buttons = msg.buttons
 
-        ts = TwistStamped()
+        # ts = TwistStamped()
+        jj = JointJog()
 
-        ts.twist.linear.z = axes[Axes.RIGHT_Y.value]
+        jj.joint_names = ['joint_2']
-        ts.twist.linear.y = axes[Axes.RIGHT_X.value]
+        # 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]
 
-        lin_x_right = -0.5 * axes[Axes.R1.value] # - AXIS_DEFAULTS.at(R1));
+        # 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));
+        # lin_x_left = 0.5 * axes[Axes.L1.value] # - AXIS_DEFAULTS.at(L1));
-        ts.twist.linear.x = lin_x_right + lin_x_left
+        # ts.twist.linear.x = lin_x_right + lin_x_left
 
-        ts.twist.angular.y = axes[Axes.LEFT_Y.value]
+        # ts.twist.angular.y = axes[Axes.LEFT_Y.value]
-        ts.twist.angular.x = axes[Axes.LEFT_X.value]
+        # ts.twist.angular.x = axes[Axes.LEFT_X.value]
 
-        roll_positive = buttons[Buttons.R2.value]
+        # roll_positive = buttons[Buttons.R2.value]
-        roll_negative = -1 * buttons[Buttons.L2.value]
+        # roll_negative = -1 * buttons[Buttons.L2.value]
-        ts.twist.angular.z = float(roll_positive + roll_negative)
+        # ts.twist.angular.z = float(roll_positive + roll_negative)
 
-        ts.header.stamp = self.get_clock().now().to_msg()
+        # ts.header.stamp = self.get_clock().now().to_msg()
-        ts.header.frame_id = "base_link"
+        # ts.header.frame_id = "base_link"
 
-        self.twist_pub.publish(ts)
+        jj.header.stamp = self.get_clock().now().to_msg()
+        jj.header.frame_id = "base_link"
+
+        # self.twist_pub.publish(ts)
+        self.joint_pub.publish(jj)
 
 
 def main(args=None):