3D Object Mapping using Force Sensor & Servo Cart (Python SDK)
This article contains code for mapping and visualizing an object from probing the target
**PLEASE NOTE THIS CODE IS MEANT AS AN EXAMPLE/TEMPLATE AND SHOULD NOT BE RAN!!! USE THIS TO CREATE MODIFICATIONS TO ENSURE SAFETY AND COMPATABILITY WITH YOUR ROBOT**
(The working repository of this will be attached as a .zip file)
Requirements to run:
- Fairino robot (This code was written with an FR5; joint values and cartesian positions will vary with model)
- XJC Force/Torque sensor
- Any end of arm tool with precision end effector for accurate probing
- Python packages: numpy, matplotlib, time, Fairino SDK (available at Fairino Documentation Page)
The main code base below (Grid_test.py) is meant to accomplish the following:
- Given a starting joint position, size, and number of loops (number of probes per row and column) move to a hover point above an object (offsets determined by BASE COORDINATE SYSTEM)
- From the hover point, use servoCart() movements to slowly approach the target until a resistance force is met (controlled touch to the object)
- Record the point of detection and send that data to a .txt file
The grapher (joint_torque_grapher.py) is meant to accomplish the following:
- Read and parse values from the .txt files
- Display results as a 3D heatmap model to show rough shape of object probed (accuracy correlates with number of loops and size of map) shown below
Source Code for main probing program (written for software version 3.8.2):
from fairino import Robot
import time
START_POS = [29.74836540222168, -134.0897674560547, -89.1602554321289, -46.7747802734375, 89.99434661865234, 29.713558197021484]
START_CART = [459.6044006347656, 145.47499084472656, -23.17245864868164, 179.98126220703125, -0.017205478623509407, -89.9651870727539]
length = 200 # Length/width of a square grid in mm
def get_z_force(robot: Robot.RPC):
e, ft = robot.FT_GetForceTorqueRCS()
if e != 0:
print("Error getting force/torque:", e)
return False
return ft[2] # Z-axis force
def make_grid(robot: Robot.RPC, loops=20):
file_object = open(f"{loops}x{loops}_data.txt", "w") # open graph data file
robot.SetSpeed(40) # Set speed to 40%
# Loop over specified grid size and probe for depth
for i in range(loops):
for j in range(loops):
robot.MoveL(START_CART, tool=1, user=0, vel=100, offset_flag=1, offset_pos=[(i * -(length / loops)), (j * -(length / loops)), 0, 0, 0, 0]) # Go to probe prep
time.sleep(0.5)
# Use servo cart to slowly lower the tool until force is detected
for _ in range(400):
robot.ServoCart(1, [0, 0,-0.15, 0, 0, 0], vel=100)
time.sleep(0.008)
fz = get_z_force(robot)
if( fz is not False):
if(abs(fz) > 0.5):
coord = robot.GetActualTCPPose()[1][:3] # Get current TCP position of contact point
file_object.write(f"{[i for i in coord]} \n".replace('[', '').replace(']', '')) # Write to file
print(f"Force detected: {fz} at position {coord}")
break
else:
return False
robot.MoveL(robot.GetActualTCPPose()[1], tool=1, user=0, vel=100, offset_flag=1, offset_pos=[0, 0, 50, 0, 0, 0]) # Retract 50mm
def main():
robot = Robot.RPC('192.168.58.2')
robot.SetSpeed(20) # Set speed to 20%
robot.FT_SetZero(1) # Set force/torque sensor zero
robot.MoveJ(START_POS, tool=0, user=0, vel=50) # Move to start position
make_grid(robot, loops=10)
if __name__ == "__main__":
main()
Source code for object grapher (joint_torque_grapher.py):
import matplotlib.pyplot as plt
import numpy as np
data = np.loadtxt('10x10_data.txt', delimiter=',')
x = data[:, 0]
y = data[:, 1]
z = data[:, 2]
print(max(z) - min(z))
fig = plt.figure(figsize=(10, 8))
ax = plt.axes(122, projection='3d')
plt.xlim(min(x) - 100, max(x) + 100)
plt.ylim(min(y) - 100, max(y) + 100)
ax.set_xlabel('X Position')
ax.set_ylabel('Y Position')
ax.set_zlabel('Z Position')
ax_dims = max([max(x) - min(x), max(y) - min(y), max(z) - min(z)])
ax.set(zlim=(min(z), min(z) + ax_dims))
ax.set(xlim=(min(x), min(x) + ax_dims))
ax.set(ylim=(min(y), min(y) + ax_dims))
plt.title('3D Probe Data')
ax.plot_trisurf(x, y, z, linewidth=0.2, antialiased=False, cmap='coolwarm')
############################################
data = np.loadtxt('20x20_data.txt', delimiter=',')
x = data[:, 0]
y = data[:, 1]
z = data[:, 2]
ax2 = plt.axes(121, projection='3d')
plt.xlim(min(x) - 100, max(x) + 100)
plt.ylim(min(y) - 100, max(y) + 100)
ax2.set_xlabel('X Position')
ax2.set_ylabel('Y Position')
ax2.set_zlabel('Z Position')
ax_dims = max([max(x) - min(x), max(y) - min(y), max(z) - min(z)])
ax2.set(zlim=(min(z), min(z) + ax_dims))
ax2.set(xlim=(min(x), min(x) + ax_dims))
ax2.set(ylim=(min(y), min(y) + ax_dims))
plt.title('3D Probe Data')
ax2.plot_trisurf(x, y, z, linewidth=0.2, antialiased=False, cmap='coolwarm')
plt.show()
Related Articles
Fairino Servo Commands (SDK)
Servo commands, while being intimidating and complex at first, are a low level way to control your robot while getting precise movements and are ideal for low level control and accuracy. When using these servo commands with the SDK, you may notice a ...Connecting an XJC Force/Torque Sensor to Fairino Cobot
This article will cover the steps needed to connect your XJC force/torque sensor to the end of a Fairino Cobot arm Hardware required: - 1 XJC force sensor - 1 Fairino Cobot - 1 computer with ethernet connection to Control box/button box OR 1 teach ...Fairino FAQ
Q: How do I connect my DH gripper to my Fairino? A: Follow the instructions linked here ...How to update Fairino Robot Software
Step 1) - Navigate to the Fairino documentation page here - Use the version select button in the bottom right to select the target software version you wish to migrate to (please note the compatibility table below): Current Version Maximum Upgradable ...Setting Collision Level
Fairino Collision detection sensitivity The Fairino robotic arms are Cobots, meaning they are meant to work around people safely. A key to accomplishing this is the "Collision detection" setting. This article will cover how to set the collision level ...