working ik!

robot/rospackages/src/arm_ik/arm_ik/AbsencControlSystem.py

@@ -0,0 +1,135 @@
+#!/usr/bin/env python3
+
+import rclpy
+from sensor_msgs.msg import Joy
+from rclpy.node import Node
+import math
+from rclpy.node import Node
+from rclpy.qos import QoSProfile
+from geometry_msgs.msg import Quaternion
+from std_msgs.msg import String, Header, Float32
+from sensor_msgs.msg import JointState
+from absenc_interface.msg import EncoderValues
+from arm_controller.msg import ArmMotorValues
+import threading
+
+
+
+class AbsencNode(Node):
+
+    def __init__(self):
+        node_name = 'absenc_node'
+        super().__init__(node_name)
+        self.get_logger().info('Initialized "'+node_name+'" node for functionality')
+
+        arm_topic = '/arm_command'
+        self.arm_publisher = self.create_publisher(String, arm_topic, 10)
+        self.get_logger().info('Created publisher for topic "'+arm_topic)
+
+        arm_2_topic = '/arm_values'
+        self.arm_2_publisher = self.create_publisher(ArmMotorValues, arm_2_topic, 10)
+        #self.get_logger().info('Created publisher for topic "'+arm_topic)
+
+        ik_topic = '/joint_states'
+        self.ik_sub = self.create_subscription(JointState, ik_topic, self.ik_callback, 10)
+        self.get_logger().info('Created subscriber for topic "'+ik_topic)
+
+        absenc_topic = '/absenc_values'
+        self.absenc_sub = self.create_subscription(EncoderValues, absenc_topic, self.absenc_callback, 10)
+        self.get_logger().info('Created subscriber for topic "' + absenc_topic)
+
+        # Angles reported from absenc
+        # Angles reported from IK
+        self.abs_angles = None
+        self.ik_angles = None
+        # self.ik_angles = [0.0, -30.0, 20.0, 10.0]
+
+        # Controls if motor sign is aligned with encoder direction
+        self.angle_signs = [1, -1, -1, -1]
+
+
+    def ik_callback(self, message:JointState):
+        # Receive values and convert to degrees
+        self.ik_angles = [math.degrees(x) for x in list(message.position)]
+
+    def absenc_callback(self, message: EncoderValues):
+        angle_1 = message.angle_1 if message.angle_1 < 180 else message.angle_1 - 360
+        angle_2 = message.angle_2
+        angle_3 = 360 + message.angle_3 if message.angle_3 < -180 else message.angle_3
+        self.abs_angles = [0, angle_1, angle_2, angle_3]
+        self.get_logger().info(f"Angles Fixed: {self.abs_angles}")
+
+    def publish_arm_command(self):
+        if self.ik_angles == None or self.abs_angles == None:
+            return
+
+        arm_command = "set_motor_speeds "
+        values_2 = []
+
+        for absenc_angle, ik_angle, angle_sign in zip(self.abs_angles, self.ik_angles, self.angle_signs):
+            # Here, the difference between the actual and desired angle is split into abs value and sign
+            # This is done to make mapping the range easier (it has a minimum value of 55, it can simply
+            # be added before the proper sign is applied)
+            absolute_difference = abs(absenc_angle - ik_angle)
+            difference_sign = 1 if absenc_angle - ik_angle >= 0 else -1
+
+            if absolute_difference <= 1:
+                arm_command += "0 "
+                values_2.append(0)
+            else:
+                # Map the angle difference (at most 180 deg) onto 0-255 range with a min of 150
+                # This implements a basic P controller, further from correct angle, faster it moves
+                value = ((absolute_difference) / 180.0) * 2000 
+                # Clamp value
+                value = 255 if value >= 255 else value
+                value *= difference_sign
+                value = int(value) * angle_sign
+                values_2.append(float(value))
+                arm_command += f"{value} "
+                # self.get_logger().info(f"Angle difference {}")
+        print(arm_command)
+        print(values_2)
+        # Add two for last two pwm motors this isn't controlling yet
+        arm_command += "0 0"
+        msg = String()
+        msg.data = arm_command
+
+        msg2 = ArmMotorValues()
+        msg2.val[0] = values_2[0]
+        msg2.val[1] = values_2[1]
+        msg2.val[2] = values_2[2]
+        msg2.val[3] = values_2[3]
+        msg2.val[4] = 0
+        msg2.val[5] = 0
+
+        # msg2.val[0] = values_2[0]
+        # msg2.val[1] = values_2[1]
+        # msg2.val[2] = values_2[2]
+        # msg2.val[3] = values_2[3]
+        # msg2.val[4] = values_2[4]
+        # msg2.val[5] = values_2[5]
+
+
+        self.arm_publisher.publish(msg)
+        self.arm_2_publisher.publish(msg2)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    absenc_node = AbsencNode()
+
+    # Spin in a separate thread
+    thread = threading.Thread(target=rclpy.spin, args=(absenc_node, ), daemon=True)
+    thread.start()
+
+    loop_rate = absenc_node.create_rate(30)
+    while rclpy.ok():
+        try:
+            absenc_node.publish_arm_command()
+            loop_rate.sleep()
+        except KeyboardInterrupt:
+            print("Node shutting down due to shutting down node.")
+            break
+
+    rclpy.shutdown()

robot/rospackages/src/arm_ik/arm_ik/IKNode.py

@@ -10,6 +10,7 @@
 from sensor_msgs.msg import JointState
 from std_msgs.msg import String
 import threading
+from absenc_interface.msg import EncoderValues
 
 # Publishes angles in radians for the motors
 
@@ -63,9 +64,6 @@ def __init__(self):
     # Convert them to radians
     self.mins = [math.radians(x)  for x in mins]
     self.maxes = [math.radians(x)  for x in maxes]
-    # Pitch is set not by IK but by the user, handled separately
-    self.pitch_min = self.mins[3]
-    self.pitch_max = self.maxes[3]
 
     # Sensitivity
     self.sensitivity = self.get_parameter('sensitivity').get_parameter_value().double_value
@@ -79,24 +77,20 @@ def __init__(self):
 
     # Get the initial angle values 
     absenc_topic = '/absenc_values'
-    self.absenc_sub = self.create_subscription(String, absenc_topic, self.absenc_callback, 10)
-    self.get_logger().info('Created publisher for topic "'+joy_topic)
     self.abs_angles = None
+    self.initialized = False
+    self.angles = None
+    self.absenc_sub = self.create_subscription(EncoderValues, absenc_topic, self.absenc_callback, 10)
+    self.get_logger().info('Created subscriber for topic "'+absenc_topic)
 
-    # Wait till get initial value from encoders
-    r = self.create_rate(30)
-    while self.abs_angles == None:
-      self.spin_once()
-      r.sleep()
-
-
-    # Get initial state from absolute encoders to initialize
-    # the angle values and coordinates
-    self.initialize_angles_coords()
+
+  def have_abs_angles(self):
+    return len(self.abs_angles) > 0
 
 
   def initialize_angles_coords(self):
     absenc_angles = self.abs_angles
+    self.get_logger().info(f"encoder angles: {absenc_angles}")
 
     # Start in cylindrical coords
     # The first angle swivel, it's the phi angle in the cylindrical coords system
@@ -106,16 +100,25 @@ def initialize_angles_coords(self):
     # The last three angles are flex, so add the displacement caused by these joints
     self.u, self.v = self.coords_from_flex(absenc_angles[1:])
 
-    # The pitch is the angle of the gripper (wrt the previous joint) 
-    # it must be set as well since this is an angle which is set directly
-    self.pitch = absenc_angles[-1]
+    # The pitch is the angle of the gripper (wrt to the vertical)
+    self.pitch = 0
+    # accumulate all angles
+    for i in absenc_angles:
+      self.pitch += i
+    # make pitch angle with respect to bottom
+    self.pitch = math.pi - self.pitch
 
     # Turn to cartesian (stored in self.x, y, z)
     self.calculate_cartesian()
+    self.get_logger().info(f"Initial coordinates {self.x} {self.y} {self.z}")
 
 
   def absenc_callback(self, message):
-    self.abs_angles = [math.radians(x) for x in message.value]
+    # self.get_logger().info(f"Callback for absenc value")
+    angle_1 = 360 - message.angle_1 if message.angle_1 > 180 else message.angle_1
+    angle_2 = message.angle_2
+    angle_3 = 360 + message.angle_3 if message.angle_3 < -180 else message.angle_3
+    self.abs_angles = [0, math.radians(angle_1), math.radians(angle_2), math.radians(angle_3)]
 
 
   def coords_from_flex(self, angles):
@@ -132,6 +135,11 @@ def coords_from_flex(self, angles):
 
 
   def publish_joint_state(self):
+    # If not initialized, initialize from abs enc values
+    if not self.angles and self.abs_angles and not self.initialized:
+      self.initialize_angles_coords()
+      self.initialized = True
+
     # If not initialized yet, don't publish
     if self.angles:
       joint_state = JointState()
@@ -152,7 +160,7 @@ def calculate_angles(self):
       cv = self.v + self.L3 * math.cos(self.pitch)
 
       if cu < 0 and cv < 0:
-        self.get_logger().warn(f"Point (xyz) {self.x} {self.y} {self.z} out of range")
+        self.get_logger().warn(f"Point (xyz) {self.x} {self.y} {self.z} out of range, cu {cu} cv {cv}")
         return False
 
       # In this context, cu and cv are lengths, so must be positive
@@ -188,7 +196,7 @@ def calculate_angles(self):
       if self.validAngles(angles):
         self.angles = angles
       else:
-        self.get_logger().warn(f"Outside joint limits")
+        self.get_logger().warn(f"Outside joint limits, angles: {angles}")
         return False
 
       # self.get_logger().info(f"angles: {self.angles}")
@@ -208,14 +216,14 @@ def joy_callback(self, message: Joy):
 
     # Pitch is the one angle which is directly set, so check bounds here
     new_pitch = self.pitch + (self.sensitivity * roll / 30000)
-    if self.validPitch(new_pitch):
-      self.pitch = new_pitch
+    self.pitch = new_pitch    
 
     self.calculate_cylindical()
 
     # if 2d, force y to be 0
     if self.mode == "2D":
       self.y = 0
+    self.get_logger().info(f"Pitch: {self.pitch}")
 
     # Perform IK. If out of range, restore point where it was
     if not self.calculate_angles():
@@ -255,9 +263,6 @@ def validAngles(self, angles):
         return False
 
     return True
-
-  def validPitch(self, new_pitch):
-    return self.pitch_min <= new_pitch and new_pitch <= self.pitch_max
 
 
 def main(args=None):
@@ -276,5 +281,6 @@ def main(args=None):
         loop_rate.sleep()
     except KeyboardInterrupt:
         print("Node shutting down due to shutting down node.")
+        break
 
   rclpy.shutdown()

robot/rospackages/src/arm_ik/setup.py

@@ -30,6 +30,7 @@
     entry_points={
         'console_scripts': [
             'IKNode = arm_ik.IKNode:main',
+            'AbsencControlSystem = arm_ik.AbsencControlSystem:main',
             'CadMouseJoyNode = arm_ik.CadMouseJoyNode:main',
             'state_publisher = arm_ik.state_publisher:main'
         ],

robot/rospackages/src/mcu_control_python/mcu_control_python/CommsNode.py

@@ -70,6 +70,7 @@ def get_handler(commandId, selectedDevice):
 SW_PINS = [29,31,33] # GPIO01, GPIO11, GPIO13 - https://developer.nvidia.com/embedded/learn/jetson-orin-nano-devkit-user-guide/hardware_spec.html
 
 
+local_mode = False
 ser = None
 # Hack since science teensy is USB
 ser_hardware = None
@@ -105,7 +106,7 @@ def main(args=None):
 
     # rate = comms_node.create_rate(10)
 
-    print('wacl')
+    print('waasdasdscl')
     try:
         while rclpy.ok():
             print('spin')
@@ -203,20 +204,12 @@ def send_command(command_name, args, deviceToSendTo):
             gpio.output(SW_PINS, TX2)
 
         ser.write(commandID.to_bytes(1, 'big'))
-
-        if commandID != 10:
-            ser.write(get_arg_bytes(command).to_bytes(1, 'big'))
+        ser.write(get_arg_bytes(command).to_bytes(1, 'big'))
+        print(commandID.to_bytes(1, 'big'))
+        print(get_arg_bytes(command).to_bytes(1, 'big'))
 
-        #arg_length = len(command[2]).to_bytes(1,'big')
-        #ser.write(arg_length)
         data_types = [element[0] for element in command[2]]
 
-        # MEGA STUPID WORKAROUND, DIRE SITUATION, PLEASE DON'T REUSE THIS
-        if commandID == 10:
-            ser.write((12).to_bytes(1, 'big'))
-            args, data_types = move_wheels_dumb_workaround(args)
-            print(f'args: {args} | dataTypes: {data_types}')
-
         for argument in zip(args, data_types):
             data = argument[0]
             data_type = argument[1]
@@ -229,7 +222,7 @@ def send_command(command_name, args, deviceToSendTo):
                 ser.write(arg_int.to_bytes(1,'big'))
 
             elif data_type == dt.ARG_FLOAT32_ID:
-                ser.write(bytearray(struct.pack(">f", data))) # This is likely correct now, will need to consult
+                ser.write(bytearray(struct.pack(">f", data)))
 
         ser.write(STOP_BYTE.to_bytes(1, 'big'))
         ser.flush()

robot/util/rosRoverStart/robot_ik.py

@@ -0,0 +1,50 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+def generate_launch_description():
+
+    use_sim_time = LaunchConfiguration('use_sim_time', default='false')
+
+    urdf_file_name = 'astro_arm.urdf'
+    urdf = os.path.join(
+        get_package_share_directory('arm_ik'),
+        urdf_file_name)
+    with open(urdf, 'r') as infp:
+        robot_desc = infp.read()
+
+    return LaunchDescription([
+        Node(
+            package='absenc_interface',
+            executable='absenc_node',
+            name='absenc_node',
+            output='screen'
+        ),
+        Node(
+            package='arm_ik',
+            executable='AbsencControlSystem',
+            name='absenc_control_system',
+            output='screen'
+        ),
+        Node(
+            package='arm_ik',
+            executable='IKNode',
+            name='ik_node',
+            output='screen',
+            parameters=[
+                {'joint_lengths': [1.354, 1.333, 1.250]},
+                {'joint_angle_mins': [-180.0, -90.0, -170.0, -150.0]},
+                {'joint_angle_maxes': [180, 90.0, 170.0, 150.0]},
+                {'sensitivity': 1.5},
+                {'mode': '2D'}
+            ]
+                        ),
+        Node(
+            package='arm_ik',
+            executable='CadMouseJoyNode',
+            name='cad_mouse_joy_node',
+            output='screen'),
+    ])