import rtde_control
import rtde_receive
import rospy
from std_msgs.msg import Float32MultiArray, Bool
import csv
import torch
def spark_angle_lightning(data):
global spark_angle
global offset
spark_angle = list(data.data)
home_offset = [0.0, -torch.pi / 2, 0.0, -torch.pi / 2, 0.0, 0.0, 0.0]
for i in range(len(spark_angle)):
spark_angle[i] += home_offset[i]
spark_angle[0] += offset
def spark_enable_thunder(data):
global enable
enable = data.data
def sign(num):
return -1 if num < 0 else 1
def main():
global spark_angle
global enable
global offset
enable = False
spark_angle = None
offset = 0.0
rospy.init_node("ur5e_force_control")
rate = rospy.Rate(100)
# torch.set_grad_enabled(True)
torch.set_grad_enabled(False)
ip = "192.168.0.102"
control = rtde_control.RTDEControlInterface(ip, 500)
receive = rtde_receive.RTDEReceiveInterface(ip)
off = 0.30
home = (torch.tensor((-180, -50, -130, -0, 90, +0))/180*3.14159).numpy()
control.moveJ(home, 0.5, 0.5, False)
control.stopJ(0.5)
rospy.sleep(1)
print("Homed")
ur_time = 0.001
ur_lookahead_time = 0.05
ur_gain = 200
ur5e_DH = [
[0.0, 0.0, 0.1625, 3.14159/2],
[0.0, -0.425, 0.0, 0.0],
[0.0, -0.3922, 0.0, 0.0],
[0.0, 0.0, 0.1333, 3.14159/2],
[0.0, 0.0, 0.0997, -3.14159/2],
[0.0, 0.0, 0.0996, 0.0]
]
data = []
starting_q = receive.getActualQ()
print(starting_q)
angle = starting_q
rospy.Subscriber("/Spark_angle/lightning", Float32MultiArray, spark_angle_lightning)
rospy.Subscriber("/Spark_enable/thunder", Bool, spark_enable_thunder)
print("Waiting for Spark")
while spark_angle is None and not rospy.is_shutdown():
rospy.sleep(0.1)
print("Spark Found")
diff_joint_0 = spark_angle[0] - starting_q[0]
if diff_joint_0 > 3.14159:
offset -= 2 * 3.14159
print("Offset -2pi")
elif diff_joint_0 < -3.14159:
offset += 2 * 3.14159
print("Offset +2pi")
else:
print("Offset 0")
for _ in range(600000):
if rospy.is_shutdown():
break
theta = torch.tensor(receive.getActualQ(), requires_grad=True)
total_torque_loss = torch.zeros_like(theta, requires_grad=True)
ik_loss = torch.zeros(1, requires_grad=True)
raw_torques = control.getJointTorques()
# Filter out the torques less than 5 Nm
torques = raw_torques
min_torque = 5
torques = [t-(min_torque*sign(t)) if abs(t) > min_torque else 0 for t in torques]
targets = torch.tensor(torques)/10000
targets += theta
# Loss Calculation
with torch.enable_grad():
total_torque_loss = torch.sum(torch.abs(theta-targets)**2)
T = torch.eye(4, requires_grad=True)
for j in range(len(theta)):
a = torch.tensor(ur5e_DH[j][1], requires_grad=True)
d = torch.tensor(ur5e_DH[j][2], requires_grad=True)
alpha = torch.tensor(ur5e_DH[j][3], requires_grad=True)
angle = theta[j]
A = torch.stack([
torch.stack([torch.cos(angle), -torch.sin(angle) * torch.cos(alpha), torch.sin(angle) * torch.sin(alpha), a * torch.cos(angle)]),
torch.stack([torch.sin(angle), torch.cos(angle) * torch.cos(alpha), -torch.cos(angle) * torch.sin(alpha), a * torch.sin(angle)]),
torch.stack([torch.tensor(0.0), torch.sin(alpha), torch.cos(alpha), d]),
torch.stack([torch.tensor(0.0), torch.tensor(0.0), torch.tensor(0.0), torch.tensor(1.0)])
])
T = T @ A
ik_loss = torch.abs(T[2, 3] - 0.6) ** 2 *100
if enable:
spark = torch.tensor(spark_angle, requires_grad=True)
total_spark_loss = torch.sum(torch.abs(theta-spark[:6])**2)*
else:
total_spark_loss = torch.tensor(0.0, requires_grad=Tru
# loss fraction, if a joint torque is between 8-15 nm, the spark and torque will be mapped with 0% torque and 100% spark and vice versa
max_torque = 60
min_torque = 8
maximum_joint_torque = torch.max(torch.abs(torch.tensor(raw_torques)))
torque = torch.clamp((maximum_joint_torque - min_torque) / (max_torque - min_torque), 0, 1)
spark = 1 - torque
print(f"Torque: {torque} Spark: {spark}")
print(f"Raw Torques: {raw_torques}")
loss = total_spark_loss*spark + total_torque_loss*torqu
if enable
with torch.no_grad():
loss.backward()
grad = theta.grad
grad = (-grad * 1).numpy().tolist()
control.speedJ(grad, 5, 0.005)
else:
control.speedStop()
rate.sleep()
if __name__ == "__main__":
main()
while(true):
theta = torch.tensor(receive.getActualQ(), requires_grad=True)
spark = torch.tensor(spark_angle, requires_grad=True)
total_spark_loss = torch.sum(torch.abs(theta-spark[:6])**2)*2
loss = total_spark_loss*spark + total_torque_loss*torque
loss.backward()
grad = theta.grad
grad = (-grad * 1).numpy().tolist()
control.speedJ(grad, 5, 0.005)
The theta and spark variables represent the angles of both devices. Calculating total_spark_loss is the sum over all joints of the square of the absolute difference. The loss coefficients are determined by the amount of force in the system. At zero force SPARK has 100% control, at 60N on any joint, the force controller has 100% control. The gradient is taken with respect to the loss The velocity is set based on the gradient (Minimize loss → Optimize theta for total_spark_loss → Gradient descent is literally moving joints → Hardware parameter update). SpeedJ controls the UR5e. The loop starts again with reading the updated theta from the arms.
while(true):
theta = torch.tensor(receive.getActualQ(), requires_grad=True)
total_torque_loss = torch.zeros_like(theta, requires_grad=True)
raw_torques = control.getJointTorques()
torques = raw_torques
# Filter out the torques less than 5 Nm
min_torque = 5
torques = [t-(min_torque*sign(t)) if abs(t) > min_torque else 0 for t in torques]
targets = torch.tensor(torques)/10000
targets += theta
total_torque_loss = torch.sum(torch.abs(theta-targets)**2)
max_torque = 60
min_torque = 8
maximum_joint_torque = torch.max(torch.abs(torch.tensor(raw_torques)))
torque = torch.clamp((maximum_joint_torque - min_torque) / (max_torque - min_torque), 0, 1)
spark = 1 - torque
loss = total_spark_loss*spark + total_torque_loss*torque
loss.backward()
grad = theta.grad
grad = (-grad * 1).numpy().tolist()
control.speedJ(grad, 5, 0.005)
The theta and torques variables represent the angle of SPARK and the torque on each joint in Nm (Newton meter). E-stops happen at 70 Nm. total_torque_loss is the overall goal to minimize. Torque readings below 5 Nm are mostly noise, filtered out. Very problematic without. The readings are scaled to represent very small movements away from the direction of force (possibly not required, but only version to work so far). The small offsets are added to the value of theta to represent a goal joint configuration that minimizes the torque on the system. The square of the absolute torques is summed for all joints to form the loss function. Updated Loss function: The maximum_joint_torque variable represents the largest torque in the system The min and max torque give a mapping between completely in SPARK’s control vs completely in force mode. At 8 Nm or less: 100% SPARK — 0% Force Mode At 34 Nm : 50% SPARK — 50% Force Mode At 60 Nm : 0% SPARK — 100% Force Mode This means that the further the user moves the SPARK from the UR’s position, the force increases. Similarly to SPARK, the gradient is taken, and the loss is physically backpropagated.
ur5e_DH = [
[0.0, 0.0, 0.1625, 3.14159/2],
[0.0, -0.425, 0.0, 0.0],
[0.0, -0.3922, 0.0, 0.0],
[0.0, 0.0, 0.1333, 3.14159/2],
[0.0, 0.0, 0.0997, -3.14159/2],
[0.0, 0.0, 0.0996, 0.0]
]
while(true):
theta = torch.tensor(receive.getActualQ(), requires_grad=True)
ik_loss = torch.zeros(1, requires_grad=True)
T = torch.eye(4, requires_grad=True)
for j in range(len(theta)):
a = torch.tensor(ur5e_DH[j][1], requires_grad=True)
d = torch.tensor(ur5e_DH[j][2], requires_grad=True)
alpha = torch.tensor(ur5e_DH[j][3], requires_grad=True)
angle = theta[j]
A = torch.stack([
torch.stack([torch.cos(angle), -torch.sin(angle) * torch.cos(alpha), torch.sin(angle) * torch.sin(alpha), a * torch.cos(angle)]),
torch.stack([torch.sin(angle), torch.cos(angle) * torch.cos(alpha), -torch.cos(angle) * torch.sin(alpha), a * torch.sin(angle)]),
torch.stack([torch.tensor(0.0), torch.sin(alpha), torch.cos(alpha), d]),
torch.stack([torch.tensor(0.0), torch.tensor(0.0), torch.tensor(0.0), torch.tensor(1.0)])
])
T = T @ A
ik_loss = torch.abs(T[2, 3] - 0.6) ** 2 *100
loss = ik_loss*spark + total_torque_loss*torque
loss.backward()
grad = theta.grad
grad = (-grad * 1).numpy().tolist()
control.speedJ(grad, 5, 0.005)
ik_loss is the distance metric between the FK of the EEF and the target position. The DH parameters for the UR5e are hard-coded. The Transform T starts as the identity matrix. The forward kinematics for each joint are calculated using theta and the DH parameters. The final transform is the EEF in Cartesian. The loss is used to optimize joint angles (theta).