diff --git a/src/roarm_cpp/README.md b/src/roarm_cpp/README.md new file mode 100644 index 0000000..10eb280 --- /dev/null +++ b/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 +ament_cmake +rclcpp +std_msgs +geometry_msgs +sensor_msgs +``` + +- Update your package.xml in the python workspace with the following. +#### Example package.xml: +```xml +rclpy +sensor_msgs +geometry_msgs +rclpy +sensor_msgs +geometry_msgs +``` +- Build the workspace and run the nodes using ROS2 commands. + diff --git a/src/roarm_cpp/pub_feedback.py b/src/roarm_cpp/pub_feedback.py new file mode 100644 index 0000000..1578dc5 --- /dev/null +++ b/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() diff --git a/src/roarm_cpp/pub_fk.cpp b/src/roarm_cpp/pub_fk.cpp new file mode 100644 index 0000000..3a72d65 --- /dev/null +++ b/src/roarm_cpp/pub_fk.cpp @@ -0,0 +1,58 @@ +#include +#include +#include +#include +#include + +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( + "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::SharedPtr publisher_; + rclcpp::Subscription::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()); + rclcpp::shutdown(); + return 0; +} diff --git a/src/roarm_cpp/pub_ik.cpp b/src/roarm_cpp/pub_ik.cpp new file mode 100644 index 0000000..2e37483 --- /dev/null +++ b/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 +#include +#include +#include +#include +#include + +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("joint_states", 10); + subscription_ = this->create_subscription( + "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> 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 end_effector_pos = {x, y, z - 0.03796, 1.0}; + std::vector 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::SharedPtr publisher_; + rclcpp::Subscription::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()); + rclcpp::shutdown(); + return 0; +}