import torch
import MLP_Model
import math
import numpy as np
import rclpy
from rclpy.node import Node
from tf2_msgs.msg import TFMessage
from std_msgs.msg import Float32
import time
L = 1
d = 0.5
# load model using dict
FILE = "model.pth"
loaded_model = MLP_Model.MLP()
loaded_model.load_state_dict(torch.load(FILE))
loaded_model.eval()
def euler_from_quaternion(x, y, z, w):
t3 = +2.0 * (w * z + x * y)
t4 = +1.0 - 2.0 * (y * y + z * z)
yaw_z = math.atan2(t3, t4)
return yaw_z # in radians
class MinimalPublisher(Node):
def __init__(self):
def timer_callback(self):
" Calculate Mx1, My1, ...... Mx6, My6 "
#Initialize Phi's
if self.t == 0:
self.Phix1 = 0 # 1x1
self.Phiy1 = 0 # 1x1
self.Phix2 = 0 # 1x1
self.Phiy2 = 0 # 1x1
self.t += 1
Mx1 = self.x2 - self.x1
My1 = self.y2 - self.y1
Mx2 = self.x1 - self.x2
My2 = self.y1 - self.y2
" Use MLP to Predict control inputs "
relative_pose_1 = [Mx1, My1, self.Phix1,
self.Phiy1] # tensor data for MLP model
relative_pose_2 = [Mx2, My2, self.Phix2,
self.Phiy2] # tensor data for MLP model
u1_predicted = MLP_Model.predict(
relative_pose_1, loaded_model) # predict control input u1, tensor
u2_predicted = MLP_Model.predict(
relative_pose_2, loaded_model) # predict control input u2, tensor
print(u1_predicted)
self.Phix1 = u2_predicted[0][0] # 1x1
self.Phiy1 = u2_predicted[0][1] # 1x1
self.Phix2 = u1_predicted[0][0] # 1x1
self.Phiy2 = u1_predicted[0][1] # 1x1
u1_predicted_np = np.array([[u1_predicted[0][0]], [
u1_predicted[0][1]
]]) # from tensor to numpy array for calculation
u2_predicted_np = np.array([[u2_predicted[0][0]], [
u2_predicted[0][1]
]]) # from tensor to numpy array for calculation
" Calculate V1/W1, V2/W2, V3/W3, V4/W4, V5/W5, V6/W6 "
S1 = np.array([[self.v1], [self.w1]]) #2x1
G1 = np.array([[1, 0], [0, 1 / L]]) #2x2
R1 = np.array([[math.cos(self.Theta1),
math.sin(self.Theta1)],
[-math.sin(self.Theta1),
math.cos(self.Theta1)]]) #2x2
S1 = np.dot(np.dot(G1, R1), u1_predicted_np) #2x1
S2 = np.array([[self.v2], [self.w2]]) #2x1
G2 = np.array([[1, 0], [0, 1 / L]]) #2x2
R2 = np.array([[math.cos(self.Theta2),
math.sin(self.Theta2)],
[-math.sin(self.Theta2),
math.cos(self.Theta2)]]) #2x2
S2 = np.dot(np.dot(G2, R2), u2_predicted_np) # 2x1
" Calculate VL1/VR1, VL2/VR2, VL3/VR3, VL4/VR4, VL5/VR5, VL6/VR6 "
D = np.array([[1 / 2, 1 / 2], [-1 / (2 * d), 1 / (2 * d)]]) #2x2
Di = np.linalg.inv(D) #2x2
Speed_L1 = np.array([[self.vL1],
[self.vR1]]) # Vector 2x1 for Speed of Robot 1
Speed_L2 = np.array([[self.vL2],
[self.vR2]]) # Vector 2x1 for Speed of Robot 2
M1 = np.array([[S1[0]], [S1[1]]]).reshape(2, 1) #2x1
M2 = np.array([[S2[0]], [S2[1]]]).reshape(2, 1) #2x1
Speed_L1 = np.dot(Di, M1) # 2x1 (VL1, VR1)
Speed_L2 = np.dot(Di, M2) # 2x1 (VL2, VR2)
VL1 = float(Speed_L1[0])
VR1 = float(Speed_L1[1])
VL2 = float(Speed_L2[0])
VR2 = float(Speed_L2[1])
" Publish Speed Commands to Robot 1 "
msgl1 = Float32()
msgr1 = Float32()
msgl1.data = VL1
msgr1.data = VR1
self.publisher_l1.publish(msgl1)
self.publisher_r1.publish(msgr1)
#self.get_logger().info('Publishing R1: "%s"' % msgr1.data)
" Publish Speed Commands to Robot 2 "
msgl2 = Float32()
msgr2 = Float32()
msgl2.data = VL2
msgr2.data = VR2
self.publisher_l2.publish(msgl2)
self.publisher_r2.publish(msgr2)
self.i += 1
def timer_callback(self):
" Calculate Mx1, My1, ...... Mx6, My6 "
# Initialize Phi's
if self.t == 0:
self.Phix3 = 0 # 1x1
self.Phiy3 = 0 # 1x1
self.Phix4 = 0 # 1x1
self.Phiy4 = 0 # 1x1
self.t += 1
Mx3 = self.x4 - self.x3
My3 = self.y4 - self.y3
Mx4 = self.x3 - self.x4
My4 = self.y3 - self.y4
" Use MLP to Predict control inputs "
relative_pose_3 = [Mx3, My3, self.Phix3,
self.Phiy3] # tensor data for MLP model
relative_pose_4 = [Mx4, My4, self.Phix4,
self.Phiy4] # tensor data for MLP model
u3_predicted = MLP_Model.predict(
relative_pose_3, loaded_model) # predict control input u1, tensor
u4_predicted = MLP_Model.predict(
relative_pose_4, loaded_model) # predict control input u2, tensor
self.Phix3 = u4_predicted[0][0] # 1x1
self.Phiy3 = u4_predicted[0][1] # 1x1
self.Phix4 = u3_predicted[0][0] # 1x1
self.Phiy4 = u3_predicted[0][1] # 1x1
u3_predicted_np = np.array([[u3_predicted[0][0]], [
u3_predicted[0][1]
]]) # from tensor to numpy array for calculation
u4_predicted_np = np.array([[u4_predicted[0][0]], [
u4_predicted[0][1]
]]) # from tensor to numpy array for calculation
" Calculate V1/W1, V2/W2, V3/W3, V4/W4, V5/W5, V6/W6 "
S3 = np.array([[self.v3], [self.w3]]) #2x1
G3 = np.array([[1, 0], [0, 1 / L]]) #2x2
R3 = np.array([[math.cos(self.Theta3),
math.sin(self.Theta3)],
[-math.sin(self.Theta3),
math.cos(self.Theta3)]]) #2x2
S3 = np.dot(np.dot(G3, R3), u3_predicted_np) #2x1
S4 = np.array([[self.v4], [self.w4]]) #2x1
G4 = np.array([[1, 0], [0, 1 / L]]) #2x2
R4 = np.array([[math.cos(self.Theta4),
math.sin(self.Theta4)],
[-math.sin(self.Theta4),
math.cos(self.Theta4)]]) #2x2
S4 = np.dot(np.dot(G4, R4), u4_predicted_np) #2x1
" Calculate VL1/VR1, VL2/VR2, VL3/VR3, VL4/VR4, VL5/VR5, VL6/VR6 "
D = np.array([[1 / 2, 1 / 2], [-1 / (2 * d), 1 / (2 * d)]]) #2x2
Di = np.linalg.inv(D) #2x2
Speed_L3 = np.array([[self.vL3],
[self.vR3]]) # Vector 2x1 for Speed of Robot 3
Speed_L4 = np.array([[self.vL4],
[self.vR4]]) # Vector 2x1 for Speed of Robot 4
M3 = np.array([[S3[0]], [S3[1]]]).reshape(2, 1) #2x1
M4 = np.array([[S4[0]], [S4[1]]]).reshape(2, 1) #2x1
Speed_L3 = np.dot(Di, M3) # 2x1 (VL3, VR3)
Speed_L4 = np.dot(Di, M4) # 2x1 (VL4, VR4)
VL3 = float(Speed_L3[0])
VR3 = float(Speed_L3[1])
VL4 = float(Speed_L4[0])
VR4 = float(Speed_L4[1])
" Publish Speed Commands to Robot 3 "
msgl3 = Float32()
msgr3 = Float32()
msgl3.data = VL3
msgr3.data = VR3
self.publisher_l3.publish(msgl3)
self.publisher_r3.publish(msgr3)
" Publish Speed Commands to Robot 4 "
msgl4 = Float32()
msgr4 = Float32()
msgl4.data = VL4
msgr4.data = VR4
self.publisher_l4.publish(msgl4)
self.publisher_r4.publish(msgr4)
self.i += 1
def timer_callback(self):
" Calculate Mx1, My1, ...... Mx6, My6 "
Mx1 = ((self.x2 - self.x1) + (self.x3 - self.x1) +
(self.x4 - self.x1) + (self.x5 - self.x1) +
(self.x6 - self.x1)) / 5 # 1x1
My1 = ((self.y2 - self.y1) + (self.y3 - self.y1) +
(self.y4 - self.y1) + (self.y5 - self.y1) +
(self.y6 - self.y1)) / 5 # 1x1
Mx2 = ((self.x1 - self.x2) + (self.x3 - self.x2) +
(self.x4 - self.x2) + (self.x5 - self.x2) +
(self.x6 - self.x2)) / 5 # 1x1
My2 = ((self.y1 - self.y2) + (self.y3 - self.y2) +
(self.y4 - self.y2) + (self.y5 - self.y2) +
(self.y6 - self.y2)) / 5 # 1x1
Mx3 = ((self.x1 - self.x3) + (self.x2 - self.x3) +
(self.x4 - self.x3) + (self.x5 - self.x3) +
(self.x6 - self.x3)) / 5 # 1x1
My3 = ((self.y1 - self.y3) + (self.y2 - self.y3) +
(self.y4 - self.y3) + (self.y5 - self.y3) +
(self.y6 - self.y3)) / 5 # 1x1
Mx4 = ((self.x1 - self.x4) + (self.x2 - self.x4) +
(self.x3 - self.x4) + (self.x5 - self.x4) +
(self.x6 - self.x4)) / 5 # 1x1
My4 = ((self.y1 - self.y4) + (self.y2 - self.y4) +
(self.y3 - self.y4) + (self.y5 - self.y4) +
(self.y6 - self.y4)) / 5 # 1x1
Mx5 = ((self.x1 - self.x5) + (self.x2 - self.x5) +
(self.x3 - self.x5) + (self.x4 - self.x5) +
(self.x6 - self.x5)) / 5 # 1x1
My5 = ((self.y1 - self.y5) + (self.y2 - self.y5) +
(self.y3 - self.y5) + (self.y4 - self.y5) +
(self.y6 - self.y5)) / 5 # 1x1
Mx6 = ((self.x1 - self.x6) + (self.x2 - self.x6) +
(self.x3 - self.x6) + (self.x4 - self.x6) +
(self.x5 - self.x6)) / 5 # 1x1
My6 = ((self.y1 - self.y6) + (self.y2 - self.y6) +
(self.y3 - self.y6) + (self.y4 - self.y6) +
(self.y5 - self.y6)) / 5 # 1x1
" Use MLP to Predict control inputs "
relative_pose_1 = [Mx1, My1] # tensor data for MLP model
relative_pose_2 = [Mx2, My2] # tensor data for MLP model
relative_pose_3 = [Mx3, My3] # tensor data for MLP model
relative_pose_4 = [Mx4, My4] # tensor data for MLP model
relative_pose_5 = [Mx5, My5] # tensor data for MLP model
relative_pose_6 = [Mx6, My6] # tensor data for MLP model
u1_predicted = MLP_Model.predict(
relative_pose_1, loaded_model) # predict control input u1, tensor
u2_predicted = MLP_Model.predict(
relative_pose_2, loaded_model) # predict control input u2, tensor
u3_predicted = MLP_Model.predict(
relative_pose_3, loaded_model) # predict control input u3, tensor
u4_predicted = MLP_Model.predict(
relative_pose_4, loaded_model) # predict control input u4, tensor
u5_predicted = MLP_Model.predict(
relative_pose_5, loaded_model) # predict control input u5, tensor
u6_predicted = MLP_Model.predict(
relative_pose_6, loaded_model) # predict control input u6, tensor
u1_predicted_np = np.array([[u1_predicted[0][0]], [
u1_predicted[0][1]
]]) # from tensor to numpy array for calculation
u2_predicted_np = np.array([[u2_predicted[0][0]], [
u2_predicted[0][1]
]]) # from tensor to numpy array for calculation
u3_predicted_np = np.array([[u3_predicted[0][0]], [
u3_predicted[0][1]
]]) # from tensor to numpy array for calculation
u4_predicted_np = np.array([[u4_predicted[0][0]], [
u4_predicted[0][1]
]]) # from tensor to numpy array for calculation
u5_predicted_np = np.array([[u5_predicted[0][0]], [
u5_predicted[0][1]
]]) # from tensor to numpy array for calculation
u6_predicted_np = np.array([[u6_predicted[0][0]], [
u6_predicted[0][1]
]]) # from tensor to numpy array for calculation
" Calculate V1/W1, V2/W2, V3/W3, V4/W4, V5/W5, V6/W6 "
S1 = np.array([[self.v1], [self.w1]]) #2x1
G1 = np.array([[1, 0], [0, 1 / L]]) #2x2
R1 = np.array([[math.cos(self.Theta1),
math.sin(self.Theta1)],
[-math.sin(self.Theta1),
math.cos(self.Theta1)]]) #2x2
S1 = np.dot(np.dot(G1, R1), u1_predicted_np) #2x1
S2 = np.array([[self.v2], [self.w2]]) #2x1
G2 = np.array([[1, 0], [0, 1 / L]]) #2x2
R2 = np.array([[math.cos(self.Theta2),
math.sin(self.Theta2)],
[-math.sin(self.Theta2),
math.cos(self.Theta2)]]) #2x2
S2 = np.dot(np.dot(G2, R2), u2_predicted_np) # 2x1
S3 = np.array([[self.v3], [self.w3]]) #2x1
G3 = np.array([[1, 0], [0, 1 / L]]) #2x2
R3 = np.array([[math.cos(self.Theta3),
math.sin(self.Theta3)],
[-math.sin(self.Theta3),
math.cos(self.Theta3)]]) #2x2
S3 = np.dot(np.dot(G3, R3), u3_predicted_np) #2x1
S4 = np.array([[self.v4], [self.w4]]) #2x1
G4 = np.array([[1, 0], [0, 1 / L]]) #2x2
R4 = np.array([[math.cos(self.Theta4),
math.sin(self.Theta4)],
[-math.sin(self.Theta4),
math.cos(self.Theta4)]]) #2x2
S4 = np.dot(np.dot(G4, R4), u4_predicted_np) #2x1
S5 = np.array([[self.v5], [self.w5]]) #2x1
G5 = np.array([[1, 0], [0, 1 / L]]) #2x2
R5 = np.array([[math.cos(self.Theta5),
math.sin(self.Theta5)],
[-math.sin(self.Theta5),
math.cos(self.Theta5)]]) #2x2
S5 = np.dot(np.dot(G5, R5), u5_predicted_np) #2x1
S6 = np.array([[self.v6], [self.w6]]) #2x1
G6 = np.array([[1, 0], [0, 1 / L]]) #2x2
R6 = np.array([[math.cos(self.Theta6),
math.sin(self.Theta6)],
[-math.sin(self.Theta6),
math.cos(self.Theta6)]]) #2x2
S6 = np.dot(np.dot(G6, R6), u6_predicted_np) #2x1
" Calculate VL1/VR1, VL2/VR2, VL3/VR3, VL4/VR4, VL5/VR5, VL6/VR6 "
D = np.array([[1 / 2, 1 / 2], [-1 / (2 * d), 1 / (2 * d)]]) #2x2
Di = np.linalg.inv(D) #2x2
Speed_L1 = np.array([[self.vL1],
[self.vR1]]) # Vector 2x1 for Speed of Robot 1
Speed_L2 = np.array([[self.vL2],
[self.vR2]]) # Vector 2x1 for Speed of Robot 2
Speed_L3 = np.array([[self.vL3],
[self.vR3]]) # Vector 2x1 for Speed of Robot 3
Speed_L4 = np.array([[self.vL4],
[self.vR4]]) # Vector 2x1 for Speed of Robot 4
Speed_L5 = np.array([[self.vL5],
[self.vR5]]) # Vector 2x1 for Speed of Robot 5
Speed_L6 = np.array([[self.vL6],
[self.vR6]]) # Vector 2x1 for Speed of Robot 6
M1 = np.array([[S1[0]], [S1[1]]]).reshape(2, 1) #2x1
M2 = np.array([[S2[0]], [S2[1]]]).reshape(2, 1) #2x1
M3 = np.array([[S3[0]], [S3[1]]]).reshape(2, 1) #2x1
M4 = np.array([[S4[0]], [S4[1]]]).reshape(2, 1) #2x1
M5 = np.array([[S5[0]], [S5[1]]]).reshape(2, 1) #2x1
M6 = np.array([[S6[0]], [S6[1]]]).reshape(2, 1) #2x1
Speed_L1 = np.dot(Di, M1) # 2x1 (VL1, VR1)
Speed_L2 = np.dot(Di, M2) # 2x1 (VL2, VR2)
Speed_L3 = np.dot(Di, M3) # 2x1 (VL3, VR3)
Speed_L4 = np.dot(Di, M4) # 2x1 (VL4, VR4)
Speed_L5 = np.dot(Di, M5) # 2x1 (VL5, VR5)
Speed_L6 = np.dot(Di, M6) # 2x1 (VL6, VR6)
VL1 = float(Speed_L1[0])
VR1 = float(Speed_L1[1])
VL2 = float(Speed_L2[0])
VR2 = float(Speed_L2[1])
VL3 = float(Speed_L3[0])
VR3 = float(Speed_L3[1])
VL4 = float(Speed_L4[0])
VR4 = float(Speed_L4[1])
VL5 = float(Speed_L5[0])
VR5 = float(Speed_L5[1])
VL6 = float(Speed_L6[0])
VR6 = float(Speed_L6[1])
" Publish Speed Commands to Robot 1 "
msgl1 = Float32()
msgr1 = Float32()
msgl1.data = VL1
msgr1.data = VR1
self.publisher_l1.publish(msgl1)
self.publisher_r1.publish(msgr1)
" Publish Speed Commands to Robot 2 "
msgl2 = Float32()
msgr2 = Float32()
msgl2.data = VL2
msgr2.data = VR2
self.publisher_l2.publish(msgl2)
self.publisher_r2.publish(msgr2)
" Publish Speed Commands to Robot 3 "
msgl3 = Float32()
msgr3 = Float32()
msgl3.data = VL3
msgr3.data = VR3
self.publisher_l3.publish(msgl3)
self.publisher_r3.publish(msgr3)
" Publish Speed Commands to Robot 4 "
msgl4 = Float32()
msgr4 = Float32()
msgl4.data = VL4
msgr4.data = VR4
self.publisher_l4.publish(msgl4)
self.publisher_r4.publish(msgr4)
" Publish Speed Commands to Robot 5 "
msgl5 = Float32()
msgr5 = Float32()
msgl5.data = VL5
msgr5.data = VR5
self.publisher_l5.publish(msgl5)
self.publisher_r5.publish(msgr5)
" Publish Speed Commands to Robot 6 "
msgl6 = Float32()
msgr6 = Float32()
msgl6.data = VL6
msgr6.data = VR6
self.publisher_l6.publish(msgl6)
self.publisher_r6.publish(msgr6)
self.i += 1
def timer_callback(self):
" Calculate Mx1, My1, ...... Mx6, My6 "
# Initialize Phi's
if self.t == 0:
self.Phix1 = 0 # 1x1
self.Phiy1 = 0 # 1x1
self.Phix2 = 0 # 1x1
self.Phiy2 = 0 # 1x1
self.Phix3 = 0 # 1x1
self.Phiy3 = 0 # 1x1
self.t += 1
Mx1 = (self.x2 - self.x1)
My1 = (self.y2 - self.y1)
Mx2 = ((self.x1 - self.x2) + (self.x3 - self.x2)) / 2
My2 = ((self.y1 - self.y2) + (self.y3 - self.y2)) / 2
Mx3 = (self.x2 - self.x3)
My3 = (self.y2 - self.y3)
" Use MLP to Predict control inputs "
relative_pose_1 = [Mx1, My1, self.Phix1,
self.Phiy1] # tensor data for MLP model
relative_pose_2 = [Mx2, My2, self.Phix2,
self.Phiy2] # tensor data for MLP model
relative_pose_3 = [Mx3, My3, self.Phix3,
self.Phiy3] # tensor data for MLP model
u1_predicted = MLP_Model.predict(
relative_pose_1, loaded_model) # predict control input u1, tensor
u2_predicted = MLP_Model.predict(
relative_pose_2, loaded_model) # predict control input u2, tensor
u3_predicted = MLP_Model.predict(
relative_pose_3, loaded_model) # predict control input u3, tensor
self.Phix1 = u2_predicted[0][0] # 1x1
self.Phiy1 = u2_predicted[0][1] # 1x1
self.Phix2 = (u1_predicted[0][0] + u3_predicted[0][0]) / 2 # 1x1
self.Phiy2 = (u1_predicted[0][1] + u3_predicted[0][1]) / 2 # 1x1
self.Phix3 = u2_predicted[0][0] # 1x1
self.Phiy3 = u2_predicted[0][1] # 1x1
u1_predicted_np = np.array([[u1_predicted[0][0]], [
u1_predicted[0][1]
]]) # from tensor to numpy array for calculation
u2_predicted_np = np.array([[u2_predicted[0][0]], [
u2_predicted[0][1]
]]) # from tensor to numpy array for calculation
u3_predicted_np = np.array([[u3_predicted[0][0]], [
u3_predicted[0][1]
]]) # from tensor to numpy array for calculation
" Calculate V1/W1, V2/W2, V3/W3, V4/W4, V5/W5, V6/W6 "
S1 = np.array([[self.v1], [self.w1]]) #2x1
G1 = np.array([[1, 0], [0, 1 / L]]) #2x2
R1 = np.array([[math.cos(self.Theta1),
math.sin(self.Theta1)],
[-math.sin(self.Theta1),
math.cos(self.Theta1)]]) #2x2
S1 = np.dot(np.dot(G1, R1), u1_predicted_np) #2x1
S2 = np.array([[self.v2], [self.w2]]) #2x1
G2 = np.array([[1, 0], [0, 1 / L]]) #2x2
R2 = np.array([[math.cos(self.Theta2),
math.sin(self.Theta2)],
[-math.sin(self.Theta2),
math.cos(self.Theta2)]]) #2x2
S2 = np.dot(np.dot(G2, R2), u2_predicted_np) # 2x1
S3 = np.array([[self.v3], [self.w3]]) #2x1
G3 = np.array([[1, 0], [0, 1 / L]]) #2x2
R3 = np.array([[math.cos(self.Theta3),
math.sin(self.Theta3)],
[-math.sin(self.Theta3),
math.cos(self.Theta3)]]) #2x2
S3 = np.dot(np.dot(G3, R3), u3_predicted_np) # 2x1
" Calculate VL1/VR1, VL2/VR2, VL3/VR3, VL4/VR4, VL5/VR5, VL6/VR6 "
D = np.array([[1 / 2, 1 / 2], [-1 / (2 * d), 1 / (2 * d)]]) #2x2
Di = np.linalg.inv(D) #2x2
Speed_L1 = np.array([[self.vL1],
[self.vR1]]) # Vector 2x1 for Speed of Robot 1
Speed_L2 = np.array([[self.vL2],
[self.vR2]]) # Vector 2x1 for Speed of Robot 2
Speed_L3 = np.array([[self.vL3],
[self.vR3]]) # Vector 2x1 for Speed of Robot 3
M1 = np.array([[S1[0]], [S1[1]]]).reshape(2, 1) #2x1
M2 = np.array([[S2[0]], [S2[1]]]).reshape(2, 1) #2x1
M3 = np.array([[S3[0]], [S3[1]]]).reshape(2, 1) #2x1
Speed_L1 = np.dot(Di, M1) # 2x1 (VL1, VR1)
Speed_L2 = np.dot(Di, M2) # 2x1 (VL2, VR2)
Speed_L3 = np.dot(Di, M3) # 2x1 (VL1, VR1)
VL1 = float(Speed_L1[0])
VR1 = float(Speed_L1[1])
VL2 = float(Speed_L2[0])
VR2 = float(Speed_L2[1])
VL3 = float(Speed_L3[0])
VR3 = float(Speed_L3[1])
" Publish Speed Commands to Robot 1 "
msgl1 = Float32()
msgr1 = Float32()
msgl1.data = VL1
msgr1.data = VR1
self.publisher_l1.publish(msgl1)
self.publisher_r1.publish(msgr1)
" Publish Speed Commands to Robot 2 "
msgl2 = Float32()
msgr2 = Float32()
msgl2.data = VL2
msgr2.data = VR2
self.publisher_l2.publish(msgl2)
self.publisher_r2.publish(msgr2)
" Publish Speed Commands to Robot 3 "
msgl3 = Float32()
msgr3 = Float32()
msgl3.data = VL3
msgr3.data = VR3
self.publisher_l3.publish(msgl3)
self.publisher_r3.publish(msgr3)
self.i += 1
""" Code for loading MLP parameters @author: hussein """ import torch import MLP_Model # load model using dict FILE = "model.pth" loaded_model = MLP_Model.ModelE() loaded_model.load_state_dict(torch.load(FILE)) loaded_model.eval() # for param in loaded_model.parameters(): # print(param) Input = [ 0, 0, 0, 0 ] u = MLP_Model.predict(Input, loaded_model) print(u)
import rclpy
from rclpy.node import Node
from tf2_msgs.msg import TFMessage
from std_msgs.msg import Float32
import time
L = 1
d = 0.5
A = np.ones(6) - np.identity(6) # Adjancency Matrix fully connected case 6x6
#A = np.array([ [0,1,0,0,0,0], [1,0,1,0,0,0], [0,1,0,1,0,0], [0,0,1,0,1,0], [0,0,0,1,0,1], [0,0,0,0,1,0] ]) # Line Graph 6x6 for communication
#A = np.array([ [0,1,1,0,0,0], [1,0,1,0,0,0], [1,1,0,0,0,0], [0,0,0,0,1,1], [0,0,0,1,0,1], [0,0,0,1,1,0] ])
#A = np.array([ [0,1,1,0,0,0], [1,0,1,0,0,0], [1,1,0,1,0,0], [0,0,1,0,1,1], [0,0,0,1,0,1], [0,0,0,1,1,0] ])
# load model using dict
FILE = "model.pth"
loaded_model = MLP_Model.ModelE()
loaded_model.load_state_dict(torch.load(FILE))
loaded_model.eval()
def euler_from_quaternion(x, y, z, w):
t3 = +2.0 * (w * z + x * y)
t4 = +1.0 - 2.0 * (y * y + z * z)
yaw_z = math.atan2(t3, t4)
return yaw_z # in radians
class MinimalPublisher(Node):
def __init__(self):
def timer_callback(self):
" Calculate Mx1, My1, ...... Mx6, My6 "
# Initialize Phi's
if self.t == 0:
self.Phix1 = 0 # 1x1
self.Phiy1 = 0 # 1x1
self.Phix2 = 0 # 1x1
self.Phiy2 = 0 # 1x1
self.Phix3 = 0 # 1x1
self.Phiy3 = 0 # 1x1
self.Phix4 = 0 # 1x1
self.Phiy4 = 0 # 1x1
self.Phix5 = 0 # 1x1
self.Phiy5 = 0 # 1x1
self.Phix6 = 0 # 1x1
self.Phiy6 = 0 # 1x1
self.t += 1
" Use Adjacency Matrix to find Mxy and Phi's "
#A = np.ones(6) - np.identity(6) # Adjancency Matrix
self.X = np.array([[self.x1], [self.x2], [self.x3], [self.x4],
[self.x5], [self.x6]]) #6x1
self.Y = np.array([[self.y1], [self.y2], [self.y3], [self.y4],
[self.y5], [self.y6]]) #6x1
Mx = np.zeros((6, 1)) # 6x1
My = np.zeros((6, 1)) # 6x1
for i in range(1, 7):
for j in range(1, 7):
Mx[i - 1] += (A[i - 1][j - 1]) * (self.X[j - 1] - self.X[i - 1]
) # 1x1 each
My[i - 1] += (A[i - 1][j - 1]) * (self.Y[j - 1] - self.Y[i - 1]
) # 1x1 each
Mx1 = float(Mx[0]) / 5 # 1x1
My1 = float(My[0]) / 5 # 1x1
Mx2 = float(Mx[1]) / 5 # 1x1
My2 = float(My[1]) / 5 # 1x1
Mx3 = float(Mx[2]) / 5 # 1x1
My3 = float(My[2]) / 5 # 1x1
Mx4 = float(Mx[3]) / 5 # 1x1
My4 = float(My[3]) / 5 # 1x1
Mx5 = float(Mx[4]) / 5 # 1x1
My5 = float(My[4]) / 5 # 1x1
Mx6 = float(Mx[5]) / 5 # 1x1
My6 = float(My[5]) / 5 # 1x1
# Mx1 = ( (self.x2-self.x1)+(self.x3-self.x1)+(self.x4-self.x1)+(self.x5-self.x1)+(self.x6-self.x1) ) / 5 # 1x1
# My1 = ( (self.y2-self.y1)+(self.y3-self.y1)+(self.y4-self.y1)+(self.y5-self.y1)+(self.y6-self.y1) ) / 5 # 1x1
# Mx2 = ( (self.x1-self.x2)+(self.x3-self.x2)+(self.x4-self.x2)+(self.x5-self.x2)+(self.x6-self.x2) ) / 5 # 1x1
# My2 = ( (self.y1-self.y2)+(self.y3-self.y2)+(self.y4-self.y2)+(self.y5-self.y2)+(self.y6-self.y2) ) / 5 # 1x1
# Mx3 = ( (self.x1-self.x3)+(self.x2-self.x3)+(self.x4-self.x3)+(self.x5-self.x3)+(self.x6-self.x3) ) / 5 # 1x1
# My3 = ( (self.y1-self.y3)+(self.y2-self.y3)+(self.y4-self.y3)+(self.y5-self.y3)+(self.y6-self.y3) ) / 5 # 1x1
# Mx4 = ( (self.x1-self.x4)+(self.x2-self.x4)+(self.x3-self.x4)+(self.x5-self.x4)+(self.x6-self.x4) ) / 5 # 1x1
# My4 = ( (self.y1-self.y4)+(self.y2-self.y4)+(self.y3-self.y4)+(self.y5-self.y4)+(self.y6-self.y4) ) / 5 # 1x1
# Mx5 = ( (self.x1-self.x5)+(self.x2-self.x5)+(self.x3-self.x5)+(self.x4-self.x5)+(self.x6-self.x5) ) / 5 # 1x1
# My5 = ( (self.y1-self.y5)+(self.y2-self.y5)+(self.y3-self.y5)+(self.y4-self.y5)+(self.y6-self.y5) ) / 5 # 1x1
# Mx6 = ( (self.x1-self.x6)+(self.x2-self.x6)+(self.x3-self.x6)+(self.x4-self.x6)+(self.x5-self.x6) ) / 5 # 1x1
# My6 = ( (self.y1-self.y6)+(self.y2-self.y6)+(self.y3-self.y6)+(self.y4-self.y6)+(self.y5-self.y6) ) / 5 # 1x1
" Use MLP to Predict control inputs "
relative_pose_1 = [Mx1, My1, self.Phix1,
self.Phiy1] # tensor data for MLP model
relative_pose_2 = [Mx2, My2, self.Phix2,
self.Phiy2] # tensor data for MLP model
relative_pose_3 = [Mx3, My3, self.Phix3,
self.Phiy3] # tensor data for MLP model
relative_pose_4 = [Mx4, My4, self.Phix4,
self.Phiy4] # tensor data for MLP model
relative_pose_5 = [Mx5, My5, self.Phix5,
self.Phiy5] # tensor data for MLP model
relative_pose_6 = [Mx6, My6, self.Phix6,
self.Phiy6] # tensor data for MLP model
u1_predicted = MLP_Model.predict(
relative_pose_1, loaded_model) # predict control input u1, tensor
u2_predicted = MLP_Model.predict(
relative_pose_2, loaded_model) # predict control input u2, tensor
u3_predicted = MLP_Model.predict(
relative_pose_3, loaded_model) # predict control input u3, tensor
u4_predicted = MLP_Model.predict(
relative_pose_4, loaded_model) # predict control input u4, tensor
u5_predicted = MLP_Model.predict(
relative_pose_5, loaded_model) # predict control input u5, tensor
u6_predicted = MLP_Model.predict(
relative_pose_6, loaded_model) # predict control input u6, tensor
Phix = np.zeros((6, 1)) # 6x1
Phiy = np.zeros((6, 1)) # 6x1
for i in range(1, 7):
for j in range(1, 7):
Phix[i - 1] += (A[i - 1][j - 1]) * (Mx[j - 1]) # 1x1 each
Phiy[i - 1] += (A[i - 1][j - 1]) * (My[j - 1]) # 1x1 each
self.Phix1 = float(Phix[0]) / 5 # 1x1
self.Phiy1 = float(Phiy[0]) / 5 # 1x1
self.Phix2 = float(Phix[1]) / 5 # 1x1
self.Phiy2 = float(Phiy[1]) / 5 # 1x1
self.Phix3 = float(Phix[2]) / 5 # 1x1
self.Phiy3 = float(Phiy[2]) / 5 # 1x1
self.Phix4 = float(Phix[3]) / 5 # 1x1
self.Phiy4 = float(Phiy[3]) / 5 # 1x1
self.Phix5 = float(Phix[4]) / 5 # 1x1
self.Phiy5 = float(Phiy[4]) / 5 # 1x1
self.Phix6 = float(Phix[5]) / 5 # 1x1
self.Phiy6 = float(Phiy[5]) / 5 # 1x1
# self.Phix1 = ( Mx2 + Mx3 + Mx4 + Mx5 + Mx6 ) / 5 # 1x1
# self.Phiy1 = ( My2 + My3 + My4 + My5 + My6 ) / 5 # 1x1
# self.Phix2 = ( Mx1 + Mx3 + Mx4 + Mx5 + Mx6 ) / 5 # 1x1
# self.Phiy2 = ( My1 + My3 + My4 + My5 + My6 ) / 5 # 1x1
# self.Phix3 = ( Mx1 + Mx2 + Mx4 + Mx5 + Mx6 ) / 5 # 1x1
# self.Phiy3 = ( My1 + My2 + My4 + My5 + My6 ) / 5 # 1x1
# self.Phix4 = ( Mx1 + Mx2 + Mx3 + Mx5 + Mx6 ) / 5 # 1x1
# self.Phiy4 = ( My1 + My2 + My3 + My5 + My6 ) / 5 # 1x1
# self.Phix5 = ( Mx1 + Mx2 + Mx3 + Mx4 + Mx6 ) / 5 # 1x1
# self.Phiy5 = ( My1 + My2 + My3 + My4 + My6 ) / 5 # 1x1
# self.Phix6 = ( Mx1 + Mx2 + Mx3 + Mx4 + Mx5 ) / 5 # 1x1
# self.Phiy6 = ( My1 + My2 + My3 + My4 + My5 ) / 5 # 1x1
u1_predicted_np = np.array([[u1_predicted[0][0]], [
u1_predicted[0][1]
]]) # from tensor to numpy array for calculation
u2_predicted_np = np.array([[u2_predicted[0][0]], [
u2_predicted[0][1]
]]) # from tensor to numpy array for calculation
u3_predicted_np = np.array([[u3_predicted[0][0]], [
u3_predicted[0][1]
]]) # from tensor to numpy array for calculation
u4_predicted_np = np.array([[u4_predicted[0][0]], [
u4_predicted[0][1]
]]) # from tensor to numpy array for calculation
u5_predicted_np = np.array([[u5_predicted[0][0]], [
u5_predicted[0][1]
]]) # from tensor to numpy array for calculation
u6_predicted_np = np.array([[u6_predicted[0][0]], [
u6_predicted[0][1]
]]) # from tensor to numpy array for calculation
" Calculate V1/W1, V2/W2, V3/W3, V4/W4, V5/W5, V6/W6 "
S1 = np.array([[self.v1], [self.w1]]) #2x1
G1 = np.array([[1, 0], [0, 1 / L]]) #2x2
R1 = np.array([[math.cos(self.Theta1),
math.sin(self.Theta1)],
[-math.sin(self.Theta1),
math.cos(self.Theta1)]]) #2x2
S1 = np.dot(np.dot(G1, R1), u1_predicted_np) #2x1
S2 = np.array([[self.v2], [self.w2]]) #2x1
G2 = np.array([[1, 0], [0, 1 / L]]) #2x2
R2 = np.array([[math.cos(self.Theta2),
math.sin(self.Theta2)],
[-math.sin(self.Theta2),
math.cos(self.Theta2)]]) #2x2
S2 = np.dot(np.dot(G2, R2), u2_predicted_np) # 2x1
S3 = np.array([[self.v3], [self.w3]]) #2x1
G3 = np.array([[1, 0], [0, 1 / L]]) #2x2
R3 = np.array([[math.cos(self.Theta3),
math.sin(self.Theta3)],
[-math.sin(self.Theta3),
math.cos(self.Theta3)]]) #2x2
S3 = np.dot(np.dot(G3, R3), u3_predicted_np) #2x1
S4 = np.array([[self.v4], [self.w4]]) #2x1
G4 = np.array([[1, 0], [0, 1 / L]]) #2x2
R4 = np.array([[math.cos(self.Theta4),
math.sin(self.Theta4)],
[-math.sin(self.Theta4),
math.cos(self.Theta4)]]) #2x2
S4 = np.dot(np.dot(G4, R4), u4_predicted_np) #2x1
S5 = np.array([[self.v5], [self.w5]]) #2x1
G5 = np.array([[1, 0], [0, 1 / L]]) #2x2
R5 = np.array([[math.cos(self.Theta5),
math.sin(self.Theta5)],
[-math.sin(self.Theta5),
math.cos(self.Theta5)]]) #2x2
S5 = np.dot(np.dot(G5, R5), u5_predicted_np) #2x1
S6 = np.array([[self.v6], [self.w6]]) #2x1
G6 = np.array([[1, 0], [0, 1 / L]]) #2x2
R6 = np.array([[math.cos(self.Theta6),
math.sin(self.Theta6)],
[-math.sin(self.Theta6),
math.cos(self.Theta6)]]) #2x2
S6 = np.dot(np.dot(G6, R6), u6_predicted_np) #2x1
" Calculate VL1/VR1, VL2/VR2, VL3/VR3, VL4/VR4, VL5/VR5, VL6/VR6 "
D = np.array([[1 / 2, 1 / 2], [-1 / (2 * d), 1 / (2 * d)]]) #2x2
Di = np.linalg.inv(D) #2x2
Speed_L1 = np.array([[self.vL1],
[self.vR1]]) # Vector 2x1 for Speed of Robot 1
Speed_L2 = np.array([[self.vL2],
[self.vR2]]) # Vector 2x1 for Speed of Robot 2
Speed_L3 = np.array([[self.vL3],
[self.vR3]]) # Vector 2x1 for Speed of Robot 3
Speed_L4 = np.array([[self.vL4],
[self.vR4]]) # Vector 2x1 for Speed of Robot 4
Speed_L5 = np.array([[self.vL5],
[self.vR5]]) # Vector 2x1 for Speed of Robot 5
Speed_L6 = np.array([[self.vL6],
[self.vR6]]) # Vector 2x1 for Speed of Robot 6
M1 = np.array([[S1[0]], [S1[1]]]).reshape(2, 1) #2x1
M2 = np.array([[S2[0]], [S2[1]]]).reshape(2, 1) #2x1
M3 = np.array([[S3[0]], [S3[1]]]).reshape(2, 1) #2x1
M4 = np.array([[S4[0]], [S4[1]]]).reshape(2, 1) #2x1
M5 = np.array([[S5[0]], [S5[1]]]).reshape(2, 1) #2x1
M6 = np.array([[S6[0]], [S6[1]]]).reshape(2, 1) #2x1
Speed_L1 = np.dot(Di, M1) # 2x1 (VL1, VR1)
Speed_L2 = np.dot(Di, M2) # 2x1 (VL2, VR2)
Speed_L3 = np.dot(Di, M3) # 2x1 (VL3, VR3)
Speed_L4 = np.dot(Di, M4) # 2x1 (VL4, VR4)
Speed_L5 = np.dot(Di, M5) # 2x1 (VL5, VR5)
Speed_L6 = np.dot(Di, M6) # 2x1 (VL6, VR6)
VL1 = float(Speed_L1[0])
VR1 = float(Speed_L1[1])
VL2 = float(Speed_L2[0])
VR2 = float(Speed_L2[1])
VL3 = float(Speed_L3[0])
VR3 = float(Speed_L3[1])
VL4 = float(Speed_L4[0])
VR4 = float(Speed_L4[1])
VL5 = float(Speed_L5[0])
VR5 = float(Speed_L5[1])
VL6 = float(Speed_L6[0])
VR6 = float(Speed_L6[1])
" Publish Speed Commands to Robot 1 "
msgl1 = Float32()
msgr1 = Float32()
msgl1.data = VL1
msgr1.data = VR1
self.publisher_l1.publish(msgl1)
self.publisher_r1.publish(msgr1)
" Publish Speed Commands to Robot 2 "
msgl2 = Float32()
msgr2 = Float32()
msgl2.data = VL2
msgr2.data = VR2
self.publisher_l2.publish(msgl2)
self.publisher_r2.publish(msgr2)
" Publish Speed Commands to Robot 3 "
msgl3 = Float32()
msgr3 = Float32()
msgl3.data = VL3
msgr3.data = VR3
self.publisher_l3.publish(msgl3)
self.publisher_r3.publish(msgr3)
" Publish Speed Commands to Robot 4 "
msgl4 = Float32()
msgr4 = Float32()
msgl4.data = VL4
msgr4.data = VR4
self.publisher_l4.publish(msgl4)
self.publisher_r4.publish(msgr4)
" Publish Speed Commands to Robot 5 "
msgl5 = Float32()
msgr5 = Float32()
msgl5.data = VL5
msgr5.data = VR5
self.publisher_l5.publish(msgl5)
self.publisher_r5.publish(msgr5)
" Publish Speed Commands to Robot 6 "
msgl6 = Float32()
msgr6 = Float32()
msgl6.data = VL6
msgr6.data = VR6
self.publisher_l6.publish(msgl6)
self.publisher_r6.publish(msgr6)
self.i += 1
def listener_callback(self, msg):
if msg.transforms[0].child_frame_id == 'robot1' :
self.x1 = msg.transforms[0].transform.translation.x
self.y1 = msg.transforms[0].transform.translation.y
self.xr1 = msg.transforms[0].transform.rotation.x
self.yr1 = msg.transforms[0].transform.rotation.y
self.zr1 = msg.transforms[0].transform.rotation.z
self.wr1 = msg.transforms[0].transform.rotation.w
self.Theta1 = euler_from_quaternion(self.xr1,self.yr1,self.zr1,self.wr1)
if msg.transforms[0].child_frame_id == 'robot2' :
self.x2 = msg.transforms[0].transform.translation.x
self.y2 = msg.transforms[0].transform.translation.y
self.xr2 = msg.transforms[0].transform.rotation.x
self.yr2 = msg.transforms[0].transform.rotation.y
self.zr2 = msg.transforms[0].transform.rotation.z
self.wr2 = msg.transforms[0].transform.rotation.w
self.Theta2 = euler_from_quaternion(self.xr2,self.yr2,self.zr2,self.wr2)
if msg.transforms[0].child_frame_id == 'robot3' :
self.x3 = msg.transforms[0].transform.translation.x
self.y3 = msg.transforms[0].transform.translation.y
self.xr3 = msg.transforms[0].transform.rotation.x
self.yr3 = msg.transforms[0].transform.rotation.y
self.zr3 = msg.transforms[0].transform.rotation.z
self.wr3 = msg.transforms[0].transform.rotation.w
self.Theta3 = euler_from_quaternion(self.xr3,self.yr3,self.zr3,self.wr3)
if msg.transforms[0].child_frame_id == 'robot4' :
self.x4 = msg.transforms[0].transform.translation.x
self.y4 = msg.transforms[0].transform.translation.y
self.xr4 = msg.transforms[0].transform.rotation.x
self.yr4 = msg.transforms[0].transform.rotation.y
self.zr4 = msg.transforms[0].transform.rotation.z
self.wr4 = msg.transforms[0].transform.rotation.w
self.Theta4 = euler_from_quaternion(self.xr4,self.yr4,self.zr4,self.wr4)
if msg.transforms[0].child_frame_id == 'robot5' :
self.x5 = msg.transforms[0].transform.translation.x
self.y5 = msg.transforms[0].transform.translation.y
self.xr5 = msg.transforms[0].transform.rotation.x
self.yr5 = msg.transforms[0].transform.rotation.y
self.zr5 = msg.transforms[0].transform.rotation.z
self.wr5 = msg.transforms[0].transform.rotation.w
self.Theta5 = euler_from_quaternion(self.xr5,self.yr5,self.zr5,self.wr5)
if msg.transforms[0].child_frame_id == 'robot6' :
self.x6 = msg.transforms[0].transform.translation.x
self.y6 = msg.transforms[0].transform.translation.y
self.xr6 = msg.transforms[0].transform.rotation.x
self.yr6 = msg.transforms[0].transform.rotation.y
self.zr6 = msg.transforms[0].transform.rotation.z
self.wr6 = msg.transforms[0].transform.rotation.w
self.Theta6 = euler_from_quaternion(self.xr6,self.yr6,self.zr6,self.wr6)
# Initialize Phi's and Mxy's
if self.i ==0:
self.Phix1 = 0 # 1x1
self.Phiy1 = 0 # 1x1
self.Phix2 = 0 # 1x1
self.Phiy2 = 0 # 1x1
self.Phix3 = 0 # 1x1
self.Phiy3 = 0 # 1x1
self.Phix4 = 0 # 1x1
self.Phiy4 = 0 # 1x1
self.Phix5 = 0 # 1x1
self.Phiy5 = 0 # 1x1
self.Phix6 = 0 # 1x1
self.Phiy6 = 0 # 1x1
self.Mx1 = 0
self.My1 = 0
self.Mx2 = 0
self.My2 = 0
self.Mx3 = 0
self.My3 = 0
self.Mx4 = 0
self.My4 = 0
self.Mx5 = 0
self.My5 = 0
self.Mx6 = 0
self.My6 = 0
self.i = 1
" Use MLP to Predict control inputs "
# Calculate Mx's and My's
self.Mx1 = self.x2 - self.x1 # 1x1
self.My1 = self.y2 - self.y1 # 1x1
self.Mx2 = ( ( self.x1 - self.x2 ) + ( self.x3 - self.x2 ) ) / 2 # 1x1
self.My2 = ( ( self.y1 - self.y2 ) + ( self.y3 - self.y2 ) ) / 2 # 1x1
self.Mx3 = ( ( self.x2 - self.x3 ) + ( self.x4 - self.x3 ) ) / 2 # 1x1
self.My3 = ( ( self.y2 - self.y3 ) + ( self.y4 - self.y3 ) ) / 2 # 1x1
self.Mx4 = ( ( self.x3 - self.x4 ) + ( self.x5 - self.x4 ) ) / 2 # 1x1
self.My4 = ( ( self.y3 - self.y4 ) + ( self.y5 - self.y4 ) ) / 2 # 1x1
self.Mx5 = ( ( self.x4 - self.x5 ) + ( self.x6 - self.x5 ) ) / 2 # 1x1
self.My5 = ( ( self.y4 - self.y5 ) + ( self.y6 - self.y5 ) ) / 2 # 1x1
self.Mx6 = self.x5 - self.x6 # 1x1
self.My6 = self.y5 - self.y6 # 1x1
# Calculate input vector for MLP Model
relative_pose_1 = [ self.Mx1, self.My1, self.Phix1, self.Phiy1 ] # tensor data for MLP model
relative_pose_2 = [ self.Mx2, self.My2, self.Phix2, self.Phiy2 ] # tensor data for MLP model
relative_pose_3 = [ self.Mx3, self.My3, self.Phix3, self.Phiy3 ] # tensor data for MLP model
relative_pose_4 = [ self.Mx4, self.My4, self.Phix4, self.Phiy4 ] # tensor data for MLP model
relative_pose_5 = [ self.Mx5, self.My5, self.Phix5, self.Phiy5 ] # tensor data for MLP model
relative_pose_6 = [ self.Mx6, self.My6, self.Phix6, self.Phiy6 ] # tensor data for MLP model
# Predict control input's ui's using MLP
u1_predicted = MLP_Model.predict(relative_pose_1, loaded_model) # predict control input u1, tensor
u2_predicted = MLP_Model.predict(relative_pose_2, loaded_model) # predict control input u2, tensor
u3_predicted = MLP_Model.predict(relative_pose_3, loaded_model) # predict control input u3, tensor
u4_predicted = MLP_Model.predict(relative_pose_4, loaded_model) # predict control input u4, tensor
u5_predicted = MLP_Model.predict(relative_pose_5, loaded_model) # predict control input u5, tensor
u6_predicted = MLP_Model.predict(relative_pose_6, loaded_model) # predict control input u6, tensor
# Calculate Phix's and Phiy's from output of MLP
self.Phix1 = u2_predicted[0][0] # 1x1
self.Phiy1 = u2_predicted[0][1] # 1x1
self.Phix2 = ( u1_predicted[0][0] + u3_predicted[0][0] )/2 # 1x1
self.Phiy2 = ( u1_predicted[0][1] + u3_predicted[0][1] )/2 # 1x1
self.Phix3 = ( u2_predicted[0][0] + u4_predicted[0][0] )/2 # 1x1
self.Phiy3 = ( u2_predicted[0][1] + u4_predicted[0][1] )/2 # 1x1
self.Phix4 = ( u3_predicted[0][0] + u5_predicted[0][0] )/2 # 1x1
self.Phiy4 = ( u3_predicted[0][1] + u5_predicted[0][1] )/2 # 1x1
self.Phix5 = ( u4_predicted[0][0] + u6_predicted[0][0] )/2 # 1x1
self.Phiy5 = ( u4_predicted[0][1] + u6_predicted[0][1] )/2 # 1x1
self.Phix6 = u5_predicted[0][0] # 1x1
self.Phiy6 = u5_predicted[0][1] # 1x1
# Transform control inputs from tensor to numpy array for transforming unicycle to differential drive
u1_predicted_np = np.array([[ u1_predicted[0][0] ], [ u1_predicted[0][1] ]]) # from tensor to numpy array for calculation
u2_predicted_np = np.array([[ u2_predicted[0][0] ], [ u2_predicted[0][1] ]]) # from tensor to numpy array for calculation
u3_predicted_np = np.array([[ u3_predicted[0][0] ], [ u3_predicted[0][1] ]]) # from tensor to numpy array for calculation
u4_predicted_np = np.array([[ u4_predicted[0][0] ], [ u4_predicted[0][1] ]]) # from tensor to numpy array for calculation
u5_predicted_np = np.array([[ u5_predicted[0][0] ], [ u5_predicted[0][1] ]]) # from tensor to numpy array for calculation
u6_predicted_np = np.array([[ u6_predicted[0][0] ], [ u6_predicted[0][1] ]]) # from tensor to numpy array for calculation
" Calculate V1/W1, V2/W2, V3/W3, V4/W4, V5/W5, V6/W6 "
S1 = np.array([[self.v1], [self.w1]]) #2x1
G1 = np.array([[1,0], [0,1/L]]) #2x2
R1 = np.array([[math.cos(self.Theta1),math.sin(self.Theta1)],[-math.sin(self.Theta1),math.cos(self.Theta1)]]) #2x2
S1 = np.dot(np.dot(G1, R1), u1_predicted_np) #2x1
S2 = np.array([[self.v2], [self.w2]]) #2x1
G2 = np.array([[1,0], [0,1/L]]) #2x2
R2 = np.array([[math.cos(self.Theta2),math.sin(self.Theta2)],[-math.sin(self.Theta2),math.cos(self.Theta2)]]) #2x2
S2 = np.dot(np.dot(G2, R2), u2_predicted_np) # 2x1
S3 = np.array([[self.v3], [self.w3]]) #2x1
G3 = np.array([[1,0], [0,1/L]]) #2x2
R3 = np.array([[math.cos(self.Theta3),math.sin(self.Theta3)],[-math.sin(self.Theta3),math.cos(self.Theta3)]]) #2x2
S3 = np.dot(np.dot(G3, R3), u3_predicted_np) #2x1
S4 = np.array([[self.v4], [self.w4]]) #2x1
G4 = np.array([[1,0], [0,1/L]]) #2x2
R4 = np.array([[math.cos(self.Theta4),math.sin(self.Theta4)],[-math.sin(self.Theta4),math.cos(self.Theta4)]]) #2x2
S4 = np.dot(np.dot(G4, R4), u4_predicted_np) #2x1
S5 = np.array([[self.v5], [self.w5]]) #2x1
G5 = np.array([[1,0], [0,1/L]]) #2x2
R5 = np.array([[math.cos(self.Theta5),math.sin(self.Theta5)],[-math.sin(self.Theta5),math.cos(self.Theta5)]]) #2x2
S5 = np.dot(np.dot(G5, R5), u5_predicted_np) #2x1
S6 = np.array([[self.v6], [self.w6]]) #2x1
G6 = np.array([[1,0], [0,1/L]]) #2x2
R6 = np.array([[math.cos(self.Theta6),math.sin(self.Theta6)],[-math.sin(self.Theta6),math.cos(self.Theta6)]]) #2x2
S6 = np.dot(np.dot(G6, R6), u6_predicted_np) #2x1
" Calculate VL1/VR1, VL2/VR2, VL3/VR3, VL4/VR4, VL5/VR5, VL6/VR6 "
D = np.array([[1/2,1/2],[-1/(2*d),1/(2*d)]]) #2x2
Di = np.linalg.inv(D) #2x2
Speed_L1 = np.array([[self.vL1], [self.vR1]]) # Vector 2x1 for Speed of Robot 1
Speed_L2 = np.array([[self.vL2], [self.vR2]]) # Vector 2x1 for Speed of Robot 2
Speed_L3 = np.array([[self.vL3], [self.vR3]]) # Vector 2x1 for Speed of Robot 3
Speed_L4 = np.array([[self.vL4], [self.vR4]]) # Vector 2x1 for Speed of Robot 4
Speed_L5 = np.array([[self.vL5], [self.vR5]]) # Vector 2x1 for Speed of Robot 5
Speed_L6 = np.array([[self.vL6], [self.vR6]]) # Vector 2x1 for Speed of Robot 6
M1 = np.array([[S1[0]],[S1[1]]]).reshape(2,1) #2x1
M2 = np.array([[S2[0]],[S2[1]]]).reshape(2,1) #2x1
M3 = np.array([[S3[0]],[S3[1]]]).reshape(2,1) #2x1
M4 = np.array([[S4[0]],[S4[1]]]).reshape(2,1) #2x1
M5 = np.array([[S5[0]],[S5[1]]]).reshape(2,1) #2x1
M6 = np.array([[S6[0]],[S6[1]]]).reshape(2,1) #2x1
Speed_L1 = np.dot(Di, M1) # 2x1 (VL1, VR1)
Speed_L2 = np.dot(Di, M2) # 2x1 (VL2, VR2)
Speed_L3 = np.dot(Di, M3) # 2x1 (VL3, VR3)
Speed_L4 = np.dot(Di, M4) # 2x1 (VL4, VR4)
Speed_L5 = np.dot(Di, M5) # 2x1 (VL5, VR5)
Speed_L6 = np.dot(Di, M6) # 2x1 (VL6, VR6)
VL1 = float(Speed_L1[0])
VR1 = float(Speed_L1[1])
VL2 = float(Speed_L2[0])
VR2 = float(Speed_L2[1])
VL3 = float(Speed_L3[0])
VR3 = float(Speed_L3[1])
VL4 = float(Speed_L4[0])
VR4 = float(Speed_L4[1])
VL5 = float(Speed_L5[0])
VR5 = float(Speed_L5[1])
VL6 = float(Speed_L6[0])
VR6 = float(Speed_L6[1])
" Publish Speed Commands to Robot 1 "
msgl1 = Float32()
msgr1 = Float32()
msgl1.data = VL1
msgr1.data = VR1
self.publisher_l1.publish(msgl1)
self.publisher_r1.publish(msgr1)
#self.get_logger().info('Publishing R1: "%s"' % msgr1.data)
" Publish Speed Commands to Robot 2 "
msgl2 = Float32()
msgr2 = Float32()
msgl2.data = VL2
msgr2.data = VR2
self.publisher_l2.publish(msgl2)
self.publisher_r2.publish(msgr2)
" Publish Speed Commands to Robot 3 "
msgl3 = Float32()
msgr3 = Float32()
msgl3.data = VL3
msgr3.data = VR3
self.publisher_l3.publish(msgl3)
self.publisher_r3.publish(msgr3)
" Publish Speed Commands to Robot 4 "
msgl4 = Float32()
msgr4 = Float32()
msgl4.data = VL4
msgr4.data = VR4
self.publisher_l4.publish(msgl4)
self.publisher_r4.publish(msgr4)
" Publish Speed Commands to Robot 5 "
msgl5 = Float32()
msgr5 = Float32()
msgl5.data = VL5
msgr5.data = VR5
self.publisher_l5.publish(msgl5)
self.publisher_r5.publish(msgr5)
" Publish Speed Commands to Robot 6 "
msgl6 = Float32()
msgr6 = Float32()
msgl6.data = VL6
msgr6.data = VR6
self.publisher_l6.publish(msgl6)
self.publisher_r6.publish(msgr6)
def listener_callback(self, msg):
if msg.transforms[0].child_frame_id == 'robot1':
self.x1 = msg.transforms[0].transform.translation.x
self.y1 = msg.transforms[0].transform.translation.y
self.xr1 = msg.transforms[0].transform.rotation.x
self.yr1 = msg.transforms[0].transform.rotation.y
self.zr1 = msg.transforms[0].transform.rotation.z
self.wr1 = msg.transforms[0].transform.rotation.w
self.Theta1 = euler_from_quaternion(self.xr1, self.yr1, self.zr1,
self.wr1)
if msg.transforms[0].child_frame_id == 'robot2':
self.x2 = msg.transforms[0].transform.translation.x
self.y2 = msg.transforms[0].transform.translation.y
self.xr2 = msg.transforms[0].transform.rotation.x
self.yr2 = msg.transforms[0].transform.rotation.y
self.zr2 = msg.transforms[0].transform.rotation.z
self.wr2 = msg.transforms[0].transform.rotation.w
self.Theta2 = euler_from_quaternion(self.xr2, self.yr2, self.zr2,
self.wr2)
self.distance = abs(self.x1 - self.x2) + abs(self.y1 - self.y2)
if self.counter == 0:
self.t0 = time.process_time()
#print(t0)
self.counter = 1
#time.sleep(3)
self.elapsed_time = time.process_time() - self.t0
print(self.elapsed_time)
#print(self.distance)
" Calculate the Pose of Robot 2 w.r.t Robot 1 and Control input U1 "
self.X1 = self.x2 - self.x1 # Relative Pose of Robot 2 wrt Robot 1 in Global frame for X coordinate of dimension 1x1
self.Y1 = self.y2 - self.y1 # Relative Pose of Robot 2 wrt Robot 1 in Global frame for Y coordinate of dimension 1x1
#self.U1 = u1 # Control input U1 in Global frame for robot 1 of dimension 2x1
" Calculate the Pose of Robot 1 w.r.t Robot 2 and Control input U2 "
self.X2 = self.x1 - self.x2 # Relative Pose of Robot 1 wrt Robot 2 in Global frame for X coordinate of dimension 1x1
self.Y2 = self.y1 - self.y2 # Relative Pose of Robot 1 wrt Robot 2 in Global frame for Y coordinate of dimension 1x1
#self.U2 = u2 # Control input U2 in Global frame for robot 2 of dimension 2x1
" Transform Relative Pose from Global to Local Reference Frame "
R1 = np.array([[math.cos(self.Theta1),
math.sin(self.Theta1)],
[-math.sin(self.Theta1),
math.cos(self.Theta1)]]) #2x2
R2 = np.array([[math.cos(self.Theta2),
math.sin(self.Theta2)],
[-math.sin(self.Theta2),
math.cos(self.Theta2)]]) #2x2
PoseG1 = np.array(
[[self.X1], [self.Y1]]
) # Relative Pose of Robot 2 wrt Robot 1 in Global Frame of dimension 2x1
PoseL1 = np.dot(
R1, PoseG1
) # Relative Pose of Robot 2 wrt Robot 2 in Local Frame of dimension 2x1
PoseG2 = np.array(
[[self.X2], [self.Y2]]
) # Relative Pose of Robot 1 wrt Robot 1 in Global Frame of dimension 2x1
PoseL2 = np.dot(
R2, PoseG2
) # Relative Pose of Robot 1 wrt Robot 2 in Local Frame of dimension 2x1
" Calculate Speed inputs V1/2 using MLP "
relative_pose_1 = [PoseL1[0][0],
PoseL1[1][0]] # tensor data for MLP model
relative_pose_2 = [PoseL2[0][0],
PoseL2[1][0]] # tensor data for MLP model
V1_predicted = MLP_Model.predict(
relative_pose_1,
loaded_model) # predict local control input u1, tensor
V2_predicted = MLP_Model.predict(
relative_pose_2,
loaded_model) # predict local control input u2, tensor
V1_predicted_np = np.array([[V1_predicted[0][0]], [
V1_predicted[0][1]
]]) # from tensor to numpy array for calculation
V2_predicted_np = np.array([[V2_predicted[0][0]], [
V2_predicted[0][1]
]]) # from tensor to numpy array for calculation
print(V1_predicted_np)
print(V2_predicted_np)
VL1 = float(V1_predicted[0][0])
VR1 = float(V1_predicted[0][1])
VL2 = float(V2_predicted[0][0])
VR2 = float(V2_predicted[0][1])
" Publish Speed Commands to Robot 1"
msgl1 = Float32()
msgr1 = Float32()
msgl1.data = VL1
msgr1.data = VR1
self.publisher_l1.publish(msgl1)
self.publisher_r1.publish(msgr1)
#self.get_logger().info('Publishing R1: "%s"' % msgr1.data)
" Publish Speed Commands to Robot 2"
msgl2 = Float32()
msgr2 = Float32()
msgl2.data = VL2
msgr2.data = VR2
self.publisher_l2.publish(msgl2)
self.publisher_r2.publish(msgr2)
#distance = self.x2 - self.x1
#print(distance)
" Write Values to CSV file "
if self.count % 2 == 0:
with open('plot_data.csv', 'a', newline='') as f:
fieldnames = ['Delta_time', 'Distance']
thewriter = csv.DictWriter(f, fieldnames=fieldnames)
if self.i2 == 0: # write header value once
thewriter.writeheader()
self.i2 = 1
if self.j2 != 0:
thewriter.writerow({
'Delta_time': self.elapsed_time,
'Distance': self.distance
})
if self.j2 == 0: # skip first value because it's noisy
self.j2 = 1
self.count += 1 # Counter to skip values while saving to csv file
def listener_callback(self, msg):
if msg.transforms[0].child_frame_id == 'robot1':
self.x1 = msg.transforms[0].transform.translation.x
self.y1 = msg.transforms[0].transform.translation.y
self.xr1 = msg.transforms[0].transform.rotation.x
self.yr1 = msg.transforms[0].transform.rotation.y
self.zr1 = msg.transforms[0].transform.rotation.z
self.wr1 = msg.transforms[0].transform.rotation.w
self.Theta1 = euler_from_quaternion(self.xr1, self.yr1, self.zr1,
self.wr1)
if msg.transforms[0].child_frame_id == 'robot2':
self.x2 = msg.transforms[0].transform.translation.x
self.y2 = msg.transforms[0].transform.translation.y
self.xr2 = msg.transforms[0].transform.rotation.x
self.yr2 = msg.transforms[0].transform.rotation.y
self.zr2 = msg.transforms[0].transform.rotation.z
self.wr2 = msg.transforms[0].transform.rotation.w
self.Theta2 = euler_from_quaternion(self.xr2, self.yr2, self.zr2,
self.wr2)
" Calculate the Pose of Robot 2 w.r.t Robot 1 and Control input U1 "
self.X1 = self.x2 - self.x1 # Relative Pose of Robot 2 wrt Robot 1 in Global frame for X coordinate of dimension 1x1
self.Y1 = self.y2 - self.y1 # Relative Pose of Robot 2 wrt Robot 1 in Global frame for Y coordinate of dimension 1x1
#self.U1 = u1 # Control input U1 in Global frame for robot 1 of dimension 2x1
" Calculate the Pose of Robot 1 w.r.t Robot 2 and Control input U2 "
self.X2 = self.x1 - self.x2 # Relative Pose of Robot 1 wrt Robot 2 in Global frame for X coordinate of dimension 1x1
self.Y2 = self.y1 - self.y2 # Relative Pose of Robot 1 wrt Robot 2 in Global frame for Y coordinate of dimension 1x1
#self.U2 = u2 # Control input U2 in Global frame for robot 2 of dimension 2x1
" Transform Relative Pose from Global to Local Reference Frame "
R1 = np.array([[math.cos(self.Theta1),
math.sin(self.Theta1)],
[-math.sin(self.Theta1),
math.cos(self.Theta1)]]) #2x2
R2 = np.array([[math.cos(self.Theta2),
math.sin(self.Theta2)],
[-math.sin(self.Theta2),
math.cos(self.Theta2)]]) #2x2
PoseG1 = np.array(
[[self.X1], [self.Y1]]
) # Relative Pose of Robot 2 wrt Robot 1 in Global Frame of dimension 2x1
PoseL1 = np.dot(
R1, PoseG1
) # Relative Pose of Robot 2 wrt Robot 2 in Local Frame of dimension 2x1
PoseG2 = np.array(
[[self.X2], [self.Y2]]
) # Relative Pose of Robot 1 wrt Robot 1 in Global Frame of dimension 2x1
PoseL2 = np.dot(
R2, PoseG2
) # Relative Pose of Robot 1 wrt Robot 2 in Local Frame of dimension 2x1
" Calculate Control inputs u1 and u2 using MLP "
relative_pose_1 = [PoseL1[0][0],
PoseL1[1][0]] # tensor data for MLP model
relative_pose_2 = [PoseL2[0][0],
PoseL2[1][0]] # tensor data for MLP model
u1_predicted = MLP_Model.predict(
relative_pose_1,
loaded_model) # predict local control input u1, tensor
u2_predicted = MLP_Model.predict(
relative_pose_2,
loaded_model) # predict local control input u2, tensor
#u1 = np.array([[ self.k*(self.x2-self.x1)],[self.k*(self.y2-self.y1)]]) # 2x1
#u2 = np.array([[ self.k*(self.x1-self.x2)],[self.k*(self.y1-self.y2)]]) # 2x1
" Calculate V1/W1 and V2/W2 "
u1_predicted_np = np.array([[u1_predicted[0][0]], [
u1_predicted[0][1]
]]) # from tensor to numpy array for calculation
u2_predicted_np = np.array([[u2_predicted[0][0]], [
u2_predicted[0][1]
]]) # from tensor to numpy array for calculation
print(u1_predicted_np)
print(u2_predicted_np)
S1 = np.array([[self.v1], [self.w1]]) #2x1
G1 = np.array([[1, 0], [0, 1 / L]]) #2x2
R1 = np.array([[math.cos(0), math.sin(0)], [-math.sin(0),
math.cos(0)]]) #2x2
S1 = np.dot(np.dot(G1, R1), u1_predicted_np) #2x1
S2 = np.array([[self.v2], [self.w2]]) #2x1
G2 = np.array([[1, 0], [0, 1 / L]]) #2x2
R2 = np.array([[math.cos(0), math.sin(0)], [-math.sin(0),
math.cos(0)]]) #2x2
S2 = np.dot(np.dot(G2, R2), u2_predicted_np) # 2x1
" Calculate VL1/VR1 and VL2/VR2 "
D = np.array([[1 / 2, 1 / 2], [-1 / (2 * d), 1 / (2 * d)]]) #2x2
Di = np.linalg.inv(D) #2x2
Speed_L1 = np.array([[self.vL1],
[self.vR1]]) # Vector 2x1 for Speed of Robot 1
Speed_L2 = np.array([[self.vL2],
[self.vR2]]) # Vector 2x1 for Speed of Robot 2
M1 = np.array([[S1[0]], [S1[1]]]).reshape(2, 1) #2x1
M2 = np.array([[S2[0]], [S2[1]]]).reshape(2, 1) #2x1
Speed_L1 = np.dot(Di, M1) # 2x1 (VL1, VR1)
Speed_L2 = np.dot(Di, M2) # 2x1 (VL2, VR2)
VL1 = float(Speed_L1[0])
VR1 = float(Speed_L1[1])
VL2 = float(Speed_L2[0])
VR2 = float(Speed_L2[1])
" Transform Control Input U1 from Global to Local Reference Frame "
#U1L = np.dot(R1, self.U1) # Control input of Robot 1 in Local Frame of dimension 2x1
#U2L = np.dot(R2, self.U2) # Control input of Robot 2 in Local Frame of dimension 2x1
" Publish Speed Commands to Robot 1"
msgl1 = Float32()
msgr1 = Float32()
msgl1.data = VL1
msgr1.data = VR1
self.publisher_l1.publish(msgl1)
self.publisher_r1.publish(msgr1)
#self.get_logger().info('Publishing R1: "%s"' % msgr1.data)
" Publish Speed Commands to Robot 2"
msgl2 = Float32()
msgr2 = Float32()
msgl2.data = VL2
msgr2.data = VR2
self.publisher_l2.publish(msgl2)
self.publisher_r2.publish(msgr2)
def timer_callback(self):
" Calculate Mx1, My1, ...... Mx6, My6 "
Mx1 = self.x3 - self.x1
My1 = self.y3 - self.y1
Mx3 = self.x1 - self.x3
My3 = self.y1 - self.y3
" Use MLP to Predict control inputs "
relative_pose_1 = [Mx1, My1] # tensor data for MLP model
relative_pose_3 = [Mx3, My3] # tensor data for MLP model
u1_predicted = MLP_Model.predict(
relative_pose_1, loaded_model) # predict control input u1, tensor
u3_predicted = MLP_Model.predict(
relative_pose_3, loaded_model) # predict control input u2, tensor
u1_predicted_np = np.array([[u1_predicted[0][0]], [
u1_predicted[0][1]
]]) # from tensor to numpy array for calculation
u3_predicted_np = np.array([[u3_predicted[0][0]], [
u3_predicted[0][1]
]]) # from tensor to numpy array for calculation
" Calculate V1/W1, V2/W2, V3/W3, V4/W4, V5/W5, V6/W6 "
S1 = np.array([[self.v1], [self.w1]]) #2x1
G1 = np.array([[1, 0], [0, 1 / L]]) #2x2
R1 = np.array([[math.cos(self.Theta1),
math.sin(self.Theta1)],
[-math.sin(self.Theta1),
math.cos(self.Theta1)]]) #2x2
S1 = np.dot(np.dot(G1, R1), u1_predicted_np) #2x1
S3 = np.array([[self.v3], [self.w3]]) #2x1
G3 = np.array([[1, 0], [0, 1 / L]]) #2x2
R3 = np.array([[math.cos(self.Theta3),
math.sin(self.Theta3)],
[-math.sin(self.Theta3),
math.cos(self.Theta3)]]) #2x2
S3 = np.dot(np.dot(G3, R3), u3_predicted_np) # 2x1
" Calculate VL1/VR1, VL2/VR2, VL3/VR3, VL4/VR4, VL5/VR5, VL6/VR6 "
D = np.array([[1 / 2, 1 / 2], [-1 / (2 * d), 1 / (2 * d)]]) #2x2
Di = np.linalg.inv(D) #2x2
Speed_L1 = np.array([[self.vL1],
[self.vR1]]) # Vector 2x1 for Speed of Robot 1
Speed_L3 = np.array([[self.vL3],
[self.vR3]]) # Vector 2x1 for Speed of Robot 3
M1 = np.array([[S1[0]], [S1[1]]]).reshape(2, 1) #2x1
M3 = np.array([[S3[0]], [S3[1]]]).reshape(2, 1) #2x1
Speed_L1 = np.dot(Di, M1) # 2x1 (VL1, VR1)
Speed_L3 = np.dot(Di, M3) # 2x1 (VL1, VR1)
VL1 = float(Speed_L1[0])
VR1 = float(Speed_L1[1])
VL3 = float(Speed_L3[0])
VR3 = float(Speed_L3[1])
" Publish Speed Commands to Robot 1 "
msgl1 = Float32()
msgr1 = Float32()
msgl1.data = VL1
msgr1.data = VR1
self.publisher_l1.publish(msgl1)
self.publisher_r1.publish(msgr1)
" Publish Speed Commands to Robot 3 "
msgl3 = Float32()
msgr3 = Float32()
msgl3.data = VL3
msgr3.data = VR3
self.publisher_l3.publish(msgl3)
self.publisher_r3.publish(msgr3)
self.i += 1
"""
Code for loading MLP parameters
@author: hussein
"""
import torch
import MLP_Model
# load model using dict
FILE = "model.pth"
loaded_model = MLP_Model.MLP()
loaded_model.load_state_dict(torch.load(FILE))
loaded_model.eval()
for param in loaded_model.parameters():
print(param)
row = [
-0.373219668865204, 2.17479133605957, 4.73384788632393, -8.16998362541199
]
print(row)
u1_predicted = MLP_Model.predict(row, loaded_model)
print(u1_predicted)
#2.49452987313271
#4.87876439094543
