02-16-2016, 12:28 AM
Good evening
Hello , I am trying to use the provided conveyor belt, but I'm missing to understand how it works...
I see there are three python programs "commanding" it.
"Run conveyor" simulates object movement on top of the conveyor right ?
"Wait for part" makes a part detector on "get conveyor" reference correct ?
And Simulate gripper just well... does what it says on the tin.
the thing is when I look at the python output of the "wait for part " I can only see it detects part 2 ( using the conveyor model from the online library)
Is there any way I can stop the conveyor or rather the objects moving on top of it ?
I have tried merging the two python programs so I can stop the conveyor but with no sucess. What am I doing wrong ?
What am I missing ?
Best Regards
Hello , I am trying to use the provided conveyor belt, but I'm missing to understand how it works...
I see there are three python programs "commanding" it.
"Run conveyor" simulates object movement on top of the conveyor right ?
"Wait for part" makes a part detector on "get conveyor" reference correct ?
And Simulate gripper just well... does what it says on the tin.
the thing is when I look at the python output of the "wait for part " I can only see it detects part 2 ( using the conveyor model from the online library)
Is there any way I can stop the conveyor or rather the objects moving on top of it ?
I have tried merging the two python programs so I can stop the conveyor but with no sucess. What am I doing wrong ?
Code:
# Type help("robolink") or help("robodk") for more information
# Press F5 to run the script
# Note: you do not need to keep a copy of this file, your python script is saved with the station
from robolink import * # API to communicate with RoboDK
from robodk import * # basic matrix operations
RL = Robolink()
# This script simulates a conveyor belt
CONVEYOR_NAME = 'Conveyor'
PICKABLE_OBJECTS_KEYWORD = 'Part'
# Speed of movement in MM/S with respect to the conveyor coordinates:
MOVE_SPEED_MMS = [0,50,0]
REFRESH_RATE = 0.005
# Define workspace of the conveyor to pick the objects:
CONV_SZ_X_MIN = 0
CONV_SZ_X_MAX = 440
CONV_SZ_Y_MIN = 0
CONV_SZ_Y_MAX = 2000
CONV_SZ_Z_MIN = -200
CONV_SZ_Z_MAX = +500
# Move objects that reached the end of the conveyor and were not picked:
FALLEN_OBJECTS = [0,-1500,0]
# Get the conveyor item and reference for work space:
conv = RL.Item(CONVEYOR_NAME)
conv_reference = conv.Parent()
poseconv = conv_reference.PoseAbs()
# One second in real life means 1 second of simulation. The simulation speed is taken from the station
SIMULATION_SPEED = 1
def is_inside_conveyor(pose):
"""Checks if a pose is inside the conveyor workspace"""
pos = pose.Pos()
if pos[0] > CONV_SZ_X_MIN and pos[0] < CONV_SZ_X_MAX and pos[1] > CONV_SZ_Y_MIN and pos[1] < CONV_SZ_Y_MAX and pos[2] > CONV_SZ_Z_MIN and pos[2] < CONV_SZ_Z_MAX:
return True
return False
def conveyor_move_object(pose, delta_time):
"""Moves the object pose through the conveyor depending on the time and speed"""
delta_mm = mult3(MOVE_SPEED_MMS, delta_time)
newpose = transl(delta_mm)*pose
return newpose
# Get all objects (string list)
all_objects = RL.ItemList(ITEM_TYPE_OBJECT)
# Convert object list into item pointers (faster)
# Also filter the list to take into account pickable objects only
objects = []
objects_name = []
objects_active = []
for i in range(len(all_objects)):
if all_objects[i].count(PICKABLE_OBJECTS_KEYWORD) > 0:
objects.append(RL.Item(all_objects[i]))
objects_name.append(all_objects[i])
objects_active.append(False)
# The number of objects that can go in the conveyor
nobjects = len(objects)
# Infinite loop to simulate the conveyor behavior
current_time = 0
tic()
time_last = toc()
while True:
# First: Look for objects that are not in the conveyor but are in the conveyor workspace
for i in range(nobjects):
obj_i = objects[i]
# Skip if the object is already in the conveyor
if objects_active[i]:
continue
# Check if the object has already been taken by a tool. If so, do not take it in the conveyor
if obj_i.Parent().Type() == ITEM_TYPE_TOOL:
continue
# Check if the object is within the conveyor work area
posei = obj_i.PoseAbs()
poseirel = invH(poseconv)*posei
if is_inside_conveyor(poseirel):
#德e the object
obj_i.setParentStatic(conv)
print('Adding object %s to the conveyor' % objects_name[i])
objects_active[i] = True
# Second step: Update the position of every object in the conveyor
SIMULATION_SPEED = RL.SimulationSpeed()
time_current = toc()
time_delta = time_current - time_last
time_last = time_current
current_time = current_time + time_delta*SIMULATION_SPEED
#做一个列表的对象与他们匹配的假设ions to update
obj_items = []
obj_poses_abs = []
for i in range(nobjects):
obj_i = objects[i]
# Check if the object has been picked from the conveyor
if objects_active[i] and obj_i.Parent() != conv:
objects_active[i] = False
print('Object %s was picked from the conveyor' % objects_name[i])
continue
# Skip update for objects that are not in the conveyor
if not objects_active[i]:
continue
# Update the position of the object
posei = invH(poseconv)*obj_i.PoseAbs()
newposei = conveyor_move_object(posei, time_delta*SIMULATION_SPEED)
if not is_inside_conveyor(newposei):
print('Warning!! Object %s fell from the conveyor at %.1f seconds after simulation started' % (objects_name[i], current_time))
#raise Exception('Object %s was not picked from the conveyor!' % objects_name[i])
newposei = transl(FALLEN_OBJECTS)*newposei
objects_active[i] = False
#obj_i.setPose(newposei) # this will provoke a refresh (can be slow)
obj_items.append(obj_i)
obj_poses_abs.append(poseconv*newposei)
# Update the object positions
RL.setPosesAbs(obj_items, obj_poses_abs)
# Take a break...
pause(REFRESH_RATE)
# Use a target from the station as a reference plane:
TARGET_NAME = 'Get Conveyor'
LASER_PLANE =(0,1,0) #正常的飞机
# Activate within this tolerance
TOLERANCE_CHECK_MM = 10
# Look for parts with the keyword "Part"
PICKABLE_OBJECTS_KEYWORD = 'Part'
# Update status every 1 ms
RECHECK_PERIOD = 0.001
# Get the target and the detection plane
target = RL.Item(TARGET_NAME)
targetpose = target.PoseAbs()
DETECT_PLANE_POINT = targetpose.Pos()
DETECT_PLANE_VECTOR = LASER_PLANE
# Define a function to detect a target
def part_detected(pos):
"""Check if a part is in the detection area (proximity of a point to a plane)"""
pos_proj = proj_pt_2_plane(pos, DETECT_PLANE_POINT, DETECT_PLANE_VECTOR)
distance2plane = norm(subs3(pos,pos_proj))
if distance2plane < TOLERANCE_CHECK_MM:
return True
return False
if part_detected:
MOVE_SPEED_MMS = [0,0,0]
What am I missing ?
Best Regards
