RoArm-M2-S executables

src/roarm_cpp/README.md

@@ -0,0 +1,68 @@
+# RoArm
+
+This folder contains the code for the RoArm-M2-S robotic manipulator. It includes implementations for Inverse Kinematics (IK), Forward Kinematics (FK) analytically without using extra packages like MoveIt2 to significantly save the computational resources, and a script to get the servo feedback which can be useful in various aspects like implementing different control strategies.
+
+## Project Overview
+
+RoArm-M2-S is a robotic manipulator designed for various tasks. This repository includes:
+- `pub_ik.cpp`: A C++ file to compute the Inverse Kinematics (IK) of the manipulator.
+- `pub_fk.cpp`: A C++ file to compute the Forward Kinematics (FK) of the manipulator.
+- `servo_feedback.py`: A Python file to get the servo feedback from the manipulator.
+
+## Installation
+
+### Prerequisites
+
+Ensure you have the following software and libraries installed:
+- C++ compiler (e.g., `g++`)
+- Python 3.x
+- ROS2 Humble (Robot Operating System)
+- All the required packages to use RoArm-M2-S
+
+## Usage 
+
+- Clone the cpp and python packages seperately in the respective ROS2 workspaces. 
+- Update your CMakeLists.txt and package.xml files in the cpp workspace with the following.
+
+#### Example for CMakeLists.txt:
+```cmake
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+```
+```cmake
+add_executable(pub_ik src/pub_ik.cpp)
+ament_target_dependencies(pub_ik rclcpp std_msgs geometry_msgs sensor_msgs)
+
+add_executable(pub_fk src/pub_fk.cpp)
+ament_target_dependencies(pub_fk rclcpp std_msgs geometry_msgs sensor_msgs)
+
+install(TARGETS
+  pub_fk
+  pub_ik
+  DESTINATION lib/${PROJECT_NAME})
+```
+#### Example packages.xml:
+```xml
+<build_type>ament_cmake</build_type>
+<depend>rclcpp</depend>
+<depend>std_msgs</depend>
+<depend>geometry_msgs</depend>
+<depend>sensor_msgs</depend>
+```
+
+- Update your package.xml in the python workspace with the following. 
+#### Example package.xml:
+```xml 
+<build_depend>rclpy</build_depend>
+<build_depend>sensor_msgs</build_depend>
+<build_depend>geometry_msgs</build_depend>
+<exec_depend>rclpy</exec_depend>
+<exec_depend>sensor_msgs</exec_depend>
+<exec_depend>geometry_msgs</exec_depend>
+```
+- Build the workspace and run the nodes using ROS2 commands. 
+

src/roarm_cpp/pub_feedback.py

@@ -0,0 +1,116 @@
+# This node gives feedback of the servo motors on the hardware and publishes it to a topic called 'feedback' as a Point data structure.
+
+import rclpy
+from rclpy.node import Node
+import array
+import math
+
+from sensor_msgs.msg import JointState
+from geometry_msgs.msg import Point
+from std_msgs.msg import Float64
+from std_msgs.msg import Float64MultiArray
+
+import json
+import serial
+
+ser = serial.Serial("/dev/ttyUSB0", 115200, timeout=1)  # Added timeout for non-blocking read
+global x, y, z, b, s, e, t, t_b, t_s, t_e, t_t, t1, t2, t3, l1, l2
+x, y, z, b, s, e1, t, t_b, t_s, t_e, t_t, t1, t2, t3 = 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
+l1 = math.sqrt(0.23682**2 + 0.03**2)
+l2 = 0.21599
+
+class MinimalSubscriber(Node):
+
+    def __init__(self):
+        super().__init__('feedback')
+        self.position = []
+        self.publisher_ = self.create_publisher(Float64MultiArray, "feedback", 10)
+
+        self.joint_data = Float64MultiArray()
+        self.joint_data.data = [0.0] * 11
+
+        timer_period = 0.05 # seconds
+        self.timer = self.create_timer(timer_period, self.timer_callback)
+
+    def posGet(self, radInput, direcInput, multiInput):
+        if radInput == 0:
+            return 2047
+        else:
+            getPos = int(2047 + (direcInput * radInput / 3.1415926 * 2048 * multiInput) + 0.5)
+            return getPos
+
+    def timer_callback(self):
+        global x, y, z, b, s, e1, t, t_b, t_s, t_e, t_t, t1, t2, t3, l1, l2
+
+        # a = msg.position
+        data = json.dumps({"T":105}) + "\n"
+        ser.write(data.encode())
+
+        if ser.in_waiting > 0:
+            response = ser.read(ser.in_waiting)  # Read all available data
+            response_str = response.decode('utf-8')  # Decode bytes to string
+
+            # Split the response by newline to handle multiple JSON objects
+            responses = response_str.strip().split('\n')
+            # print(responses)
+            # Assuming the third response (responses[2]) contains the required dictionary
+            for res in responses:
+                try:
+                    req_dictionary = json.loads(res)
+                    b = req_dictionary.get("b", b)
+                    s = req_dictionary.get("s", s)
+                    e1 = req_dictionary.get("e", e1)
+
+                    t1 = b
+                    t2 = (math.pi/2) - (s + math.atan2(0.03, 0.23682))
+                    t3 = e1 - math.atan2(0.03, 0.23682)
+
+                    x = (l1 * math.cos(t2) + l2 * math.cos(t2 - t3)) * math.cos(t1)
+                    y = (l1 * math.cos(t2) + l2 * math.cos(t2 - t3)) * math.sin(t1)
+                    z = (l1 * math.sin(t2) + l2 * math.sin(t2 - t3)) + 0.03796
+
+                    t = req_dictionary.get("t", t)
+                    t_b = req_dictionary.get("torB", t_b)
+                    t_s = req_dictionary.get("torS", t_s)
+                    t_e = req_dictionary.get("torE", t_e)
+                    t_t = req_dictionary.get("torH", t_t)
+
+                    # print(f"Parsed JSON: {req_dictionary}")  # Debug: print each parsed dictionary
+                except json.JSONDecodeError as e:
+                    # print(f"Failed to decode JSON response: {e}")
+                    pass
+                except KeyError as e:
+                    # print(f"Missing expected key in response: {e}")
+                    pass
+
+        else:
+            print("No data available to read")
+        self.joint_data.data[0] = float(x)
+        self.joint_data.data[1] = float(y)
+        self.joint_data.data[2] = float(z)
+        self.joint_data.data[3] = float(b)
+        self.joint_data.data[4] = float(s)
+        self.joint_data.data[5] = float(e1)
+        self.joint_data.data[6] = float(t)
+        self.joint_data.data[7] = float(t_b)
+        self.joint_data.data[8] = float(t_s)
+        self.joint_data.data[9] = float(t_e)
+        self.joint_data.data[10] = float(t_t)
+        self.publisher_.publish(self.joint_data)
+
+def main(args=None):
+    rclpy.init(args=args)
+    minimal_subscriber = MinimalSubscriber()
+    print("Starting spin")
+    rclpy.spin(minimal_subscriber)
+
+    # Destroy the node explicitly
+    # (optional - otherwise it will be done automatically
+    # when the garbage collector destroys the node object)
+    minimal_subscriber.destroy_node()
+    rclpy.shutdown()
+    print("Shut down")
+
+
+if __name__ == '__main__':
+    main()

src/roarm_cpp/pub_fk.cpp

@@ -0,0 +1,58 @@
+#include <rclcpp/rclcpp.hpp>
+#include <geometry_msgs/msg/point.hpp>
+#include <sensor_msgs/msg/joint_state.hpp>
+#include <cmath>
+#include <vector>
+
+using std::placeholders::_1;
+
+double t1 = 0.0;
+double t2 = 0.0;
+double t3 = 0.0;
+
+double l1 = 0.23682;
+double l2 = 0.28384;
+
+double x = 0.0;
+double y = 0.0;
+double z = 0.0;
+double y_ = 0.0;
+
+class EndStatePublisher : public rclcpp::Node
+{
+public:
+    EndStatePublisher()
+    : Node("pub_end_state")
+    {
+        subscription_ = this->create_subscription<sensor_msgs::msg::JointState>(
+            "joint_states", 10, std::bind(&EndStatePublisher::xyz_callback, this, _1));
+    }
+
+private:
+    void xyz_callback(const sensor_msgs::msg::JointState::SharedPtr msg)
+    {
+        t1 = msg->position[0];
+        t2 = (M_PI / 2) - msg->position[1];
+        t3 = msg->position[2];
+
+        x = (l1 * std::cos(t2) + l2 * std::cos(t2 - t3)) * std::cos(t1);
+        y = (l1 * std::cos(t2) + l2 * std::cos(t2 - t3)) * std::sin(t1);
+        z = (l1 * std::sin(t2) + l2 * std::sin(t2 - t3)) + 0.03796;
+        RCLCPP_INFO(this->get_logger(),
+                    "x = %f, y = %f, z = %f",
+                    x, y, z);
+
+    }
+    // rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr publisher_;
+    rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr subscription_;
+    // rclcpp::TimerBase::SharedPtr timer_;
+    // sensor_msgs::msg::JointState joint_state_msg_;
+};
+
+int main(int argc, char * argv[])
+{
+    rclcpp::init(argc, argv);
+    rclcpp::spin(std::make_shared<EndStatePublisher>());
+    rclcpp::shutdown();
+    return 0;
+}

src/roarm_cpp/pub_ik.cpp

@@ -0,0 +1,140 @@
+// Node to publish the joint angles by calculating the ik. Run this with serial_ctrl node to update both simulation and the hardware.
+// The msg can be sent from the command line or using another node over the topic "coordinates_cpp"  
+
+#include <rclcpp/rclcpp.hpp>
+#include <geometry_msgs/msg/point.hpp>
+#include <sensor_msgs/msg/joint_state.hpp>
+#include <std_msgs/msg/header.hpp>
+#include <cmath>
+#include <vector>
+
+using std::placeholders::_1;
+
+double x = 0.0;
+double y = 0.0;
+double z = 0.49075;
+
+class JointStatePublisher : public rclcpp::Node
+{
+public:
+    JointStatePublisher()
+    : Node("pub_joint_state")
+    {
+        publisher_ = this->create_publisher<sensor_msgs::msg::JointState>("joint_states", 10);
+        subscription_ = this->create_subscription<geometry_msgs::msg::Point>(
+            "coordinates_cpp", 10, std::bind(&JointStatePublisher::xyz_callback, this, _1));
+
+        timer_ = this->create_wall_timer(
+            std::chrono::milliseconds(50), std::bind(&JointStatePublisher::timer_callback, this));
+
+        joint_state_msg_ = sensor_msgs::msg::JointState();
+    }
+
+private:
+    void xyz_callback(const geometry_msgs::msg::Point::SharedPtr msg)
+    {
+        if (0.0 <= msg->x && msg->x <= 0.4 && -0.4 <= msg->y && msg->y <= 0.4 && 
+            -0.17803 <= msg->z && msg->z <= 0.49 && std::sqrt(msg->x * msg->x + msg->y * msg->y + msg->z * msg->z) <= 0.49)
+        {
+            x = msg->x;
+            y = msg->y;
+            z = msg->z;
+            RCLCPP_INFO(this->get_logger(), "Valid set of points");
+        }
+        else
+        {
+            RCLCPP_INFO(this->get_logger(), "The given coordinates are out of the workspace");
+        }
+    }
+
+    void timer_callback()
+    {
+        double theta1 = std::atan2(y, x);
+
+        double t1 = theta1;
+
+        // Transformation matrix for joint 1
+        std::vector<std::vector<double>> T1 = {
+            {std::cos(t1), std::sin(t1), 0, 0},
+            {-std::sin(t1), std::cos(t1), 0, 0},
+            {0, 0, 1, 0},
+            {0, 0, 0, 1}
+        };
+
+        // Apply transformation matrix T1 to the end-effector position (x, y, z)
+        std::vector<double> end_effector_pos = {x, y, z - 0.03796, 1.0};
+        std::vector<double> transformed_pos(4, 0);
+        for (size_t i = 0; i < 4; ++i)
+        {
+            for (size_t j = 0; j < 4; ++j)
+            {
+                transformed_pos[i] += T1[i][j] * end_effector_pos[j];
+            }
+        }
+
+        double x_transformed = transformed_pos[0];
+        double z_transformed = transformed_pos[2];
+
+        auto inverse_kinematics = [](double x_1, double y_1, double l1, double l2) {
+            double cos_theta3 = (x_1 * x_1 + y_1 * y_1 - l1 * l1 - l2 * l2) / (2 * l1 * l2);
+            double sin_theta3_1 = std::sqrt(1 - cos_theta3 * cos_theta3);
+            double sin_theta3_2 = -std::sqrt(1 - cos_theta3 * cos_theta3);
+
+            double theta2_1 = std::atan2(y_1, x_1) - std::atan2(l2 * sin_theta3_1, l1 + l2 * cos_theta3);
+            double theta2_2 = std::atan2(y_1, x_1) - std::atan2(l2 * sin_theta3_2, l1 + l2 * cos_theta3);
+
+            double theta3_1 = std::atan2(sin_theta3_1, cos_theta3);
+            double theta3_2 = std::atan2(sin_theta3_2, cos_theta3);
+
+            return std::make_tuple(theta2_1, theta3_1, theta2_2, theta3_2);
+        };
+
+        // Constants
+        double l1 = std::sqrt(0.23682*0.23682 + 0.03*0.03);
+        double l2 = 0.21599;
+        double x_1 = x_transformed;
+        double y_1 = z_transformed;
+
+        auto [theta2_1, theta3_1, theta2_2, theta3_2] = inverse_kinematics(x_1, y_1, l1, l2);
+
+        double theta2[] = {M_PI / 2 - (theta2_1 + std::atan2(0.03, 0.23682)), M_PI / 2 - (theta2_2 + std::atan2(0.03, 0.23682))};
+        double theta3[] = {-(theta3_1 - std::atan2(0.03, 0.23682)), -(theta3_2 - std::atan2(0.03, 0.23682))};
+
+        double theta_final_2 = 0.0;
+        double theta_final_3 = 0.0;
+        double theta_4 = 0.0;
+        for (int i = 0; i < 2; ++i) {
+          if (theta2[i] < -1.570796 || theta2[i] > 1.570796)
+            continue;
+          else
+            theta_final_2 = theta2[i];
+
+          if (theta3[i] < -0.78539815 || theta3[i] > 3.1415926)
+            continue;
+          else
+            theta_final_3 = theta3[i];
+        }
+        theta_4 = -theta_final_2 - theta_final_3;
+
+        joint_state_msg_.header.stamp = this->now();
+        joint_state_msg_.name = {"base_to_L1", "L1_to_L2", "L2_to_L3", "L3_to_L4"};
+        joint_state_msg_.position = {theta1, theta_final_2, theta_final_3, theta_4};
+        joint_state_msg_.velocity = {0.0, 0.0, 0.0, 0.0};
+        joint_state_msg_.effort = {0.0, 0.0, 0.0, 0.0};
+
+        publisher_->publish(joint_state_msg_);
+    }
+
+    rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr publisher_;
+    rclcpp::Subscription<geometry_msgs::msg::Point>::SharedPtr subscription_;
+    rclcpp::TimerBase::SharedPtr timer_;
+    sensor_msgs::msg::JointState joint_state_msg_;
+};
+
+int main(int argc, char * argv[])
+{
+    rclcpp::init(argc, argv);
+    rclcpp::spin(std::make_shared<JointStatePublisher>());
+    rclcpp::shutdown();
+    return 0;
+}