using UnityEngine;

using System;

using System.Linq;

using uPLibrary.Networking.M2Mqtt;

using uPLibrary.Networking.M2Mqtt.Messages;

using Newtonsoft.Json;

using System.Collections.Concurrent;

​

public class MQTTSub : MonoBehaviour

{

    private MqttClient mqttClient;

    public string brokerAddress = "127.0.0.1";

    public int brokerPort = 1883;

    public string username = "karrtik";

    public string password = "123";

    public string[] topicsToSubscribe = new string[6] { "webots/joint1", "webots/joint2", "webots/joint3", "webots/joint4", "webots/joint5", "webots/joint6" };

    public string[] jointNames = new string[6] { "shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", "wrist_1_joint", "wrist_2_joint", "wrist_3_joint" };

    public ArticulationBody[] jointArticulationBodies;

​

    private ConcurrentQueue<(int jointIndex, JointData data)> jointUpdateQueue = new ConcurrentQueue<(int, JointData)>();

​

    [Serializable]

    private class JointData

    {

        public float position;

    }

​

    void Start()

    {

        ConnectToMqttBroker();

    }

​

    void ConnectToMqttBroker()

    {

        try

        {

            Debug.Log($"Attempting to connect to MQTT broker at {brokerAddress}:{brokerPort}");

            mqttClient = new MqttClient(brokerAddress, brokerPort, false, null, null, MqttSslProtocols.None);

            mqttClient.MqttMsgPublishReceived += OnMqttMessageReceived;

​

            string clientId = Guid.NewGuid().ToString();

            mqttClient.Connect(clientId, username, password);

​

            if (mqttClient.IsConnected)

            {

                Debug.Log("Connected to MQTT broker");

                SubscribeToTopics();

            }

            else

            {

                Debug.LogError("Failed to connect to MQTT broker");

            }

        }

        catch (Exception e)

        {

            Debug.LogError($"Error connecting to MQTT broker: {e.Message}");

            Debug.LogError($"Stack trace: {e.StackTrace}");

        }

    }

​

    void SubscribeToTopics()

    {

        if (mqttClient.IsConnected)

        {

            mqttClient.Subscribe(topicsToSubscribe, Enumerable.Repeat(MqttMsgBase.QOS_LEVEL_AT_LEAST_ONCE, topicsToSubscribe.Length).ToArray());

            Debug.Log("Subscribed to topics");

        }

    }

​

    void OnMqttMessageReceived(object sender, MqttMsgPublishEventArgs e)

    {

        try

        {

            string topic = e.Topic;

            string message = System.Text.Encoding.UTF8.GetString(e.Message);

            JointData jointData = JsonConvert.DeserializeObject<JointData>(message);

            int jointIndex = Array.IndexOf(topicsToSubscribe, topic);

            if (jointIndex != -1)

            {

                jointUpdateQueue.Enqueue((jointIndex, jointData));

            }

            Debug.Log($"Received message on topic {topic}: Position = {jointData.position}");

        }

        catch (Exception ex)

        {

            Debug.LogError($"Error processing MQTT message: {ex.Message}");

        }

    }

​

    void FixedUpdate()

    {

        while (jointUpdateQueue.TryDequeue(out var jointUpdate))

        {

            UpdateJoint(jointUpdate.jointIndex, jointUpdate.data);

        }

    }

​

    void UpdateJoint(int jointIndex, JointData data)

    {

        if (jointIndex < jointArticulationBodies.Length)

        {

            ArticulationBody joint = jointArticulationBodies[jointIndex];

            ArticulationDrive drive = joint.xDrive;

            float targetRotation = data.position;

​

            // Invert rotation for all joints except shoulder_lift_joint

            if (jointNames[jointIndex] != "shoulder_lift_joint")

            {

                targetRotation = -targetRotation;

            }

​

            float currentRotation = drive.target;

            float newRotation = Mathf.Lerp(currentRotation, targetRotation * Mathf.Rad2Deg, Time.fixedDeltaTime * 10f);

​

            drive.target = newRotation;

            joint.xDrive = drive;

        }

    }

​

    void OnApplicationQuit()

    {

        if (mqttClient != null && mqttClient.IsConnected)

        {

            mqttClient.Disconnect();

        }

    }

}

from controller import Robot

import paho.mqtt.client as mqtt

import json

import math

import time

​

# Initialize the robot

robot = Robot()

timestep = int(robot.getBasicTimeStep())

​

# Set up joints

joint_names = ["shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", "wrist_1_joint"]

joints = []

​

for name in joint_names:

    joint = robot.getDevice(name)

    if joint:

        # Print current joint limits

        min_pos = joint.getMinPosition()

        max_pos = joint.getMaxPosition()

        print(f"{joint.getName()} limits: [{min_pos}, {max_pos}]")

        joint.setPosition(float('inf'))

        joint.setVelocity(0.0)

        joint.setAvailableTorque(100)  # Adjust this value as needed

        joint.setVelocity(joint.getMaxVelocity())

        # Enable position sensor

        sensor = joint.getPositionSensor()

        if sensor:

            sensor.enable(timestep)

        joints.append(joint)

​

# Function to move joints to home position

def move_to_home_position():

    print("Moving to home position...")

    for joint in joints:

        joint.setPosition(0)  # Set each joint to 0 radians (0 degrees)

    # Wait for joints to reach home position

    timeout = 5  # 5 seconds timeout

    start_time = time.time()

    while time.time() - start_time < timeout:

        all_at_home = True

        for joint in joints:

            current_pos = joint.getPositionSensor().getValue()

            if abs(current_pos) > 0.01:  # Allow small tolerance

                all_at_home = False

                break

        if all_at_home:

            break

        robot.step(timestep)

    print("Home position reached")

​

# Move to home position before starting MQTT communication

move_to_home_position()

​

# MQTT setup

broker_address = "127.0.0.1"

broker_port = 1883

topics = [f"unity/joint{i+1}" for i in range(len(joints))]

​

def on_connect(client, userdata, flags, rc):

    print(f"Connected with result code {rc}")

    for topic in topics:

        client.subscribe(topic)

        print(f"Subscribed to {topic}")

​

def on_message(client, userdata, message):

    try:

        topic = message.topic

        payload = json.loads(message.payload.decode())

        joint_index = topics.index(topic)

        rotation = math.radians(payload['rotation'])

        # Invert rotation for all joints except shoulder_lift_joint

        if joint_names[joint_index] != "shoulder_lift_joint":

            rotation = -rotation

        joints[joint_index].setPosition(rotation)

        print(f"Set joint {joint_names[joint_index]} to rotation {math.degrees(rotation):.2f} degrees")

    except Exception as e:

        print(f"Error processing message: {e}")

​

mqtt_client = mqtt.Client()

mqtt_client.on_connect = on_connect

mqtt_client.on_message = on_message

​

# Set username and password

mqtt_client.username_pw_set("karrtik", "123")

​

mqtt_client.connect(broker_address, broker_port, 60)

mqtt_client.loop_start()

​

# Main control loop

while robot.step(timestep) != -1:

    # Print current positions

    for i, joint in enumerate(joints):

        sensor = joint.getPositionSensor()

        if sensor:

            current_position = sensor.getValue()

            print(f"Joint {joint_names[i]} current position: {math.degrees(current_position):.2f} degrees")

​

mqtt_client.loop_stop()

mqtt_client.disconnect()