[AbdullahShafique27]

I have further modified my code and now there are no errors when doing the quick program check. However when I set the minimum distance to larger than 760 I get a weird response as seen in the video attached. I am sure this has to do with the fact that my TVP is offset from the flange by 760mm, and the targets for the movement are based around the TCP changing angles around the parts reference frame which will change if the minimum distance is larger than the initial offset of 760mm.

I am unsure of if this is a problem with my code or I am going to have to use another way other than targets to define my path as it will change depending on the minimum distance I set and the scanning program I have made. My code is below if there are any obvious problems with it but I cant seem to see any.

Code:

from robolink import *
from robodk import *


# Connect to RoboDK
RDK = Robolink()


# Get the robot and tool and object
robot = RDK.Item('KUKA KR 6 R900-2')
tool = RDK.Item('Kinect')
object = RDK.Item('Piston deform')

# Set the minimum distance constraint
min_distance = 800 # Set the desired minimum distance in millimeters


while True:
# Get the pose of the robot, tool, and object
robot_pose = robot.Pose()
tool_pose = tool.PoseTool()
object_pose = object.Pose()

# Calculate the position of the tool in the global reference frame
tool_pose_global = robot_pose*tool_pose

# Calculate the distance between the tool and object
dist = subs3(tool_pose_global.Pos(), object_pose.Pos())
newdist = norm(dist)

# If the distance is less than the minimum distance, move the robot away from the object
if newdist < min_distance:
displacement = min_distance - newdist
displacement = int(displacement)
direction = mult3(dist, 1.0/newdist)
new_pos = robot_pose * transl(displacement * direction)
robot.MoveJ(new_pos)