class MoveDorna:
def __init__(self):
path = os.path.dirname(os.path.abspath(sys.argv[0]))
path = path + "/config/dorna.yaml"
# Objekt der Dorna-Klasse instanziieren
self.robot = Dorna(path)
# Verbindung mit dem Roboter herstellen
self.robot.connect()
# Homing-Prozess
def home(self):
proceed = False
while not proceed:
_input = input("Home Dorna? (y/n)")
if _input == "y":
proceed = True
elif _input == "n":
self.terminate()
break
if proceed:
# Dorna in die Home-Position fahren
self.robot.home(["j0", "j1", "j2", "j3"])
print(self.robot.homed())
print(self.robot.position())
proceed = False
while not proceed:
_input = input("j3 und j4 auf 0 setzen? (y/n)")
if _input == "y":
proceed = True
elif _input == "n":
break
if proceed:
# Gelenke des Endeffektors nullen
self.set_joint_j3_0()
def set_joint_j3_0(self):
self.robot.set_joint({"j3": 0})
self.robot.set_joint({"j4": 0})
# Endeffektor zu einer bestimmten Koordinate fahren
def move_to_position(self):
# Koordinate eingeben
i = Input()
output = i.input_values()
x = output.get("x")
y = output.get("y")
z = output.get("z")
# Koordinate berechnen
position = JointValue(x, y, z)
position_result = position.calc_joint_values()
position_array = self.get_joints(position_result)
print(position_array)
proceed = True
while proceed:
_input = input("Bewegung ausführen? (y/n)")
if _input == "n":
proceed = False
elif _input == "y":
break
if proceed:
# Bewegung ausführen
self.move_dorna(position_array)
# Ablaufsteuerung zum Bälle einsammeln ausführen
def collect_balls(self):
n = eval(input("Anzahl der Bälle: "))
balls_array = []
out_of_range = False
# Koordinaten der Bälle eingeben
for j in range(n):
print("Position des", j + 1, ". Balls:")
i = Input()
output = i.input_values()
x = output.get("x")
y = output.get("y")
z = output.get("z")
# Werte der Gelenke berechnen
seven_above_ball = JointValue(x, y, z + 7) # 7cm über dem Ball
five_above_ball = JointValue(x, y, z + 5) # 5cm über dem Ball
one_above_ball = JointValue(x, y, z + 1) # 1cm über dem Ball
seven_above_ball_result = seven_above_ball.calc_joint_values()
five_above_ball_result = five_above_ball.calc_joint_values()
one_above_ball_result = one_above_ball.calc_joint_values()
if five_above_ball == 1 or one_above_ball == 1:
out_of_range = True
break
seven_above_ball_array = self.get_joints(seven_above_ball_result)
five_above_ball_array = self.get_joints(five_above_ball_result)
one_above_ball_array = self.get_joints(one_above_ball_result)
balls_dict = {"1.": five_above_ball_array, "2.": one_above_ball_array, "3.": seven_above_ball_array}
balls_array.append(balls_dict)
print("Position des Behälters: ")
# Koordinate des Behälters angeben
i = Input()
output = i.input_values()
x = output.get("x")
y = output.get("y")
z = output.get("z")
# Werte der Gelenke berechnen
fifteen_above_container = JointValue(x, y, z + 15) # 15cm über dem Behälter
fifteen_above_container_result = fifteen_above_container.calc_joint_values()
if five_above_ball == 1 or one_above_ball == 1:
out_of_range = True
fifteen_above_container_array = self.get_joints(fifteen_above_container_result)
print(balls_array)
balls_array_len = len(balls_array)
if not out_of_range:
for x in range(balls_array_len):
print(balls_array[x])
print(fifteen_above_container_array)
proceed = True
while proceed:
_input = input("Bewegung ausführen? (y/n)")
if _input == "n":
proceed = False
elif _input == "y":
break
if proceed:
for i in range(n):
first = balls_array[i].get("1.")
second = balls_array[i].get("2.")
third = balls_array[i].get("3.")
# Ablaufsteuerung
self.open_gripper()
self.move_dorna(first)
self.move_dorna(second)
self.close_gripper()
self.move_dorna(third)
self.move_dorna(fifteen_above_container_array)
self.open_gripper()
print("Bewegung ausgeführt.")
else:
print("Bewegung konnte nicht ausgeführt werden")
def terminate(self):
self.robot.terminate()
def close_gripper(self):
self.robot.set_io({"servo": 700})
def open_gripper(self):
self.robot.set_io({"servo": 0})
def get_joints(self, height):
j0 = height.get("j0")
j1 = height.get("j1")
j2 = height.get("j2")
j3 = height.get("j3")
return [j0, j1, j2, j3, 0]
def move_dorna(self, position):
self.robot.move({"movement": 0, "path": "joint", "joint": position})
# Nullposition
def zero(self):
self.robot.move({"movement": 0, "path": "joint", "joint": [0, 0, 0, 0, 0]})
class Subscriber:
def __init__(self):
rospy.init_node("coordinates_subscriber")
self.coordinates_sub = rospy.Subscriber("/coordinates", Coordinates,
self.callback)
self.coordinates_msg = Coordinates()
path = os.path.dirname(os.path.abspath(sys.argv[0]))
path = path + "/config/dorna.yaml"
self.robot = Dorna(path)
self.robot.connect()
def home(self):
proceed = False
while not proceed:
_input = input("Home Dorna? (y/n)")
if _input == "y":
proceed = True
elif _input == "n":
self.terminate()
break
if proceed:
self.robot.home(["j0", "j1", "j2", "j3"])
proceed = False
while not proceed:
_input = input("j3 auf 0 setzen? (y/n)")
if _input == "y":
proceed = True
elif _input == "n":
break
if proceed:
self.set_joint_j3_0()
def set_joint_j3_0(self):
self.robot.set_joint({"j3": 0})
self.robot.set_joint({"j4": 0})
def callback(self, msg):
self.coordinates_msg = msg
def collect_balls(self):
container_coordinate = self.coordinates_msg.container_coordinates
if len(container_coordinate) > 0:
x_c = container_coordinate[0]
y_c = container_coordinate[1]
z_c = 0
print("Behälterposition: ", x_c, y_c)
ball_coordinate = self.coordinates_msg.ball_coordinates
balls_array = []
if len(ball_coordinate) > 0:
for i in range(0, len(ball_coordinate), 2):
x_b, y_b = ball_coordinate[i], ball_coordinate[i + 1]
z_b = 0
print("Ballposition: ", x_b, y_b)
five_above_ball = JointValue(x_b, y_b, z_b + 5)
one_above_ball = JointValue(x_b, y_b, z_b + 1)
try:
five_above_ball_result = five_above_ball.calc_joint_values(
)
one_above_ball_result = one_above_ball.calc_joint_values()
except ValueError:
print("Ball außerhalb des Bereiches")
continue
five_above_ball_array = self.get_joints(five_above_ball_result)
one_above_ball_array = self.get_joints(one_above_ball_result)
balls_dict = {
"1.": five_above_ball_array,
"2.": one_above_ball_array,
"3.": five_above_ball_array
}
balls_array.append(balls_dict)
ten_above_container = JointValue(x_c, y_c, z_c + 10)
try:
ten_above_container_result = ten_above_container.calc_joint_values(
)
except ValueError:
print("Behälter außerhalb des Bereiches")
ten_above_container_array = self.get_joints(
ten_above_container_result)
for i in balls_array:
first = i.get("1.")
second = i.get("2.")
third = i.get("3.")
self.open_gripper()
self.move_dorna(first)
self.move_dorna(second)
self.close_gripper()
self.move_dorna(third)
self.move_dorna(ten_above_container_array)
self.open_gripper()
else:
print("Konnte keine Bälle orten, versuche erneut")
rospy.sleep(1)
self.collect_balls()
def terminate(self):
self.robot.terminate()
def close_gripper(self):
self.robot.set_io({"servo": 675})
def open_gripper(self):
self.robot.set_io({"servo": 0})
def move_dorna(self, position):
self.robot.move({"movement": 0, "path": "joint", "joint": position})
def get_joints(self, height):
j0 = height.get("j0")
j1 = height.get("j1")
j2 = height.get("j2")
j3 = height.get("j3")
return [j0, j1, j2, j3, 0]