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 DornaRobot:
def __init__(self):
self._config_path = os.path.dirname(
os.path.realpath(__file__)) + str("/config.yaml")
self.params = list(yaml.safe_load(open(self._config_path, 'r')).keys())
self.joint_names = ['j0', 'j1', 'j2', 'j3', 'j4']
self.axis_names = ['x', 'y', 'z', 'a', 'b']
self._robot = Dorna(self._config_path)
self._default_home = dict(zip(self.joint_names, [0, 145, -90, 0, 0]))
self.connection_status = None
self._elevator_demo = False
self._hand = True
self.triggered = False
def init(self, home=True):
'''
Connect to the robot and then home or set the stepper motors
'''
try:
self.connect()
except ConnectionException:
logger.error(
"ConnectionError [dorna_robot]: There was a connection issue. Check Robot and try again"
)
raise
else:
if home:
self.home()
else:
self.set_joint()
logger.info("Robot startup success")
################################
#### Command calls to robot ####
################################
def calibrate(self):
'''
Set angle offsets for homeing
'''
joints = self.get_joint_angles()
self._robot.calibrate(joints)
self._robot.save_config(save_path=self._config_path)
self.set_joint()
def connect(self, usb=None):
'''
Connects to Dorna\n
Note: Make sure to setup dorna udev rule
'''
logger.info('Connecting to Dorna...')
if not usb:
usb = "/dev/actuators/dorna"
response = json.loads(self._robot.connect(port_name=usb))
self.connection_status = self.get_connection_status()
while response["connection"] == DornaConnection.CONNECTING:
time.sleep(0.5)
response = json.loads(self._robot.device())
# if response["connection"] == DornaConnection.CONNECTED:
if response["connection"] == DornaConnection.DISCONNECTED:
logger.error("ConnectionError")
logger.info(
"There was a connection issue. Check Robot and try again")
raise ConnectionException()
def disconnect(self):
'''
Disconects from Arduino
'''
self._robot.disconnect()
def halt(self):
'''
Stops Dorna and delets queue
'''
self._robot.halt()
def force_fulfill(self, joint, desired_pose):
'''
Waits until joint angle equal to desired pose\n
Note: only works for single joint and angle
'''
# TODO: Make this better
not_in_position = True
while not_in_position:
joint_angles = self.get_joint_angles()
if int(joint_angles[joint]) == desired_pose:
not_in_position = False
def force_fulfill_joints(self, desired):
not_in_position = True
while not_in_position and not self.triggered:
current = self.get_joint_angles()
check = np.linalg.norm(
np.diff(np.row_stack((current, desired)), axis=0))
not_in_position = check >= 0.1
def force_fulfill_xyzab(self, desired):
not_in_position = True
while not_in_position and not self.triggered:
current = self.get_cartesian_position()
check = np.linalg.norm(
np.diff(np.row_stack((current, desired)), axis=0))
not_in_position = check >= 0.1
def elevator_demo_safe_home(self, i):
'''
Turn the base 90 degrees before calibrating next motor\n
Then move base back to origin
'''
if i == 1:
self.move_joints(path="joint",
movement=DornaMovement.GLOBAL.value,
speed=DornaMovement.DEFAULT_JOINT_SPEED.value,
joint_angles={'j0': -90})
self.force_fulfill(0, -90)
elif i == 2:
self.move_joints(path="joint",
movement=DornaMovement.GLOBAL.value,
speed=DornaMovement.DEFAULT_JOINT_SPEED.value,
joint_angles={'j0': 0})
self.force_fulfill(0, 0)
def home(self):
'''
Calibrate stepper motors
'''
logger.info('Homing Robot...')
try:
## Only homing joints 0-2 and setting joints 3-4 so that tool head does not colide with robot
if self._hand:
n = 3
self.set_joint({"j3": 0, "j4": 0})
else:
n = len(self.joint_names)
for i in range(n):
if self._elevator_demo:
self.elevator_demo_safe_home(i)
motor_name = "j" + str(i)
response = json.loads(self._robot.home(motor_name))
if response[motor_name] != 1:
raise HomingException()
logger.info('Finished Homing Robot...')
except HomingException:
logger.error(
'HomingError [dorna_robot]: Motor {} was unable to home'.
format(i))
def jog(self, path, cmd_dict):
'''
Jogs single motor or robot in single direction\n
path = 'joint' or 'line'\n
cmd_dict = dictionary of joint_name or axis_name as keys and angle or position as values
'''
logger.info('Jogging...')
if path == "joint":
speed = DornaMovement.DEFAULT_JOINT_SPEED
elif path == "line":
speed = DornaMovement.DEFAULT_XYZ_SPEED
if list(cmd_dict.keys())[0] in self.joint_names:
self.move_joints(path=path,
movement=DornaMovement.RELATIVE,
speed=speed,
joint_angles=cmd_dict)
elif list(cmd_dict.keys())[0] in self.axis_names:
self.move_xyzab(path=path,
movement=DornaMovement.RELATIVE,
speed=speed,
xyzab=cmd_dict)
def move_xyzab(self, path, movement, speed, xyzab, fulfill=True):
'''
path = 'joint' or 'line'\n
movement = relative or global\n
speed = units/second\n
xyzab = can be a list of all positions or a dict of single direction
'''
try:
if type(xyzab) == list and len(xyzab) == 5:
cmd_dict = dict(zip(self.axis_names, xyzab))
elif type(xyzab) == dict:
cmd_dict = xyzab
pass
else:
raise TypeError
except:
raise
parameters = dorna_cmd_utils.get_move_msg(path=path,
movement=movement,
speed=speed,
cmd_dict=cmd_dict)
cmd = dorna_cmd_utils.get_play_msg(command="move",
parameters=parameters,
fulfill=fulfill)
logger.info(cmd)
self.play(cmd)
def move_joints(self, path, movement, speed, joint_angles, fulfill=True):
'''
path = 'joint' or 'line'\n
movement = relative or global\n
speed = units/second\n
joint_angles = can be a list of all angles or a dict of some angles
'''
try:
if type(joint_angles) == list and len(joint_angles) == 5:
cmd_dict = dict(zip(self.joint_names, joint_angles))
# print(cmd_dict)
elif type(joint_angles) == dict:
cmd_dict = joint_angles
else:
raise TypeError
except:
raise
parameters = dorna_cmd_utils.get_move_msg(path=path,
movement=movement,
speed=speed,
cmd_dict=cmd_dict)
cmd = dorna_cmd_utils.get_play_msg(command="move",
parameters=parameters,
fulfill=fulfill)
logger.info(cmd)
self.play(cmd)
def move_to_home(self):
'''
Moves Dorna to default home position at default speed
'''
self.move_joints(path="joint",
movement=DornaMovement.GLOBAL,
speed=DornaMovement.DEFAULT_JOINT_SPEED,
joint_angles=self._default_home)
def move_to_zeropose(self):
'''
Moves Dorna to joint angles all 0
'''
cmd_dict = dict.fromkeys(self.joint_names, 0)
self.move_joints(path="joint",
movement=DornaMovement.GLOBAL,
speed=DornaMovement.DEFAULT_JOINT_SPEED,
joint_angles=cmd_dict)
def pause(self):
'''
Pauses Dorna command queue
'''
self._robot.pause()
def play(self, path):
'''
Adds path to command queue
'''
logger.info('Playing Path...')
try:
self._robot.play(path, append=True)
except KeyboardInterrupt:
self.halt()
pass
def set_joint(self, joint_dict=None):
'''
Sets values for motor angle
'''
if joint_dict:
self._robot.set_joint(joint_dict)
else:
logger.debug(self._default_home)
self._robot.set_joint(self._default_home)
def terminate(self):
'''
Kills threads in Dorna
'''
self._robot.terminate()
def xyz_to_joint(self, xyzab):
'''
Checks if xyz position is in workspace\n
If true then returns inverse kinematics
'''
if self._robot._config["unit"]["length"] == "mm":
xyzab[0:3] = [self._robot._mm_to_inch(xyz) for xyz in xyzab[0:3]]
joint_response_dict = self._robot._xyz_to_joint(np.asarray(xyzab))
if joint_response_dict['status'] == 0:
logger.info("Can achieve xyz position and safe")
return joint_response_dict['joint']
elif joint_response_dict['status'] == 1:
logger.info(
"Can achieve xyz position but NOT safe, USE WITH CAUTION")
return joint_response_dict['joint']
elif joint_response_dict['status'] == 2:
logger.error("Cannot achieve xyz position")
return None
####################################
#### Get information from robot ####
####################################
def get_cartesian_position(self):
'''
Get cartesian postion of end effector\n
Return in units[mm, inch] set by params
'''
pos = json.loads(self._robot.position("xyz"))
return pos
def get_connection_status(self):
'''
Get connection statues
'''
status = json.loads(self._robot.device())
return status["connection"] == DornaConnection.CONNECTED
def get_homed_status(self):
'''
Returns if stepper motors calibrated
'''
homed = json.loads(self._robot.homed())
return homed
def get_joint_angles(self, units='deg'):
'''
Returns motor joint angles
'''
joints = json.loads(self._robot.position("joint"))
if units == 'rad':
joints = [joint * np.pi / 180. for joint in joints]
return joints
def get_state(self):
'''
Returns if robot is moving
'''
state = json.loads(self._robot.device())['state']
return state
def get_param(self, param_name):
'''
Gets value of param from param_list
'''
if param_name in self.params:
if param_name == 'calibrate':
logger.warn('Dorna param calibrate is only a setting function')
return
if hasattr(self._robot, param_name):
call = getattr(self._robot, param_name, None)
response = json.loads(call())
# logger.info("Get robot param: "+str(response))
return response
else:
logger.warn(
'Dorna does not have method: {}'.format(param_name))
else:
logger.warn('[{}] not in param list'.format(param_name))
def set_param(self, param_name, value):
'''
Sets value of param from param_list
'''
if param_name in self.params:
attr = 'set_' + param_name
if hasattr(self._robot, attr):
call = getattr(self._robot, attr, None)
response = call(value)
logger.info("Set robot param: " + str(response))
else:
logger.warn('Dorna does not have method: {}'.format(attr))
else:
logger.warn("[{}] not in param list".format(param_name))
class Arm(object):
def __init__(self):
self.robot = Dorna("./config.yaml")
def go(self, position=HOME):
print("Go to ", position)
tmpcmd = {"command": "move", "prm": position}
self.robot.play(tmpcmd)
"""============================================================
CONNECTION AND HOMING THE ROBOT
===========================================================
"""
def connect(self):
return self.robot.connect()
def dis(self):
return self.robot.disconnect()
def connectAndHomeIfneed(self):
self.robot.connect()
self.homeIfneed()
def connectAndHomes(self):
self.robot.connect()
self.robot.home("j0")
self.robot.home("j1")
self.robot.home("j2")
self.robot.home("j3")
def isConnected(self):
device = json.loads(r.robot.device())
if device['connection'] == 2:
return True
else:
return False
def isHomed(self):
if not self.isConnected():
print("Robot is not connected")
return False
homed = c
for key in homed:
if homed[key] == 1:
print(key, " is homed")
else:
print(key, " is not homed yet")
def homes(self):
self.robot.home("j0")
self.robot.home("j1")
self.robot.home("j2")
self.robot.home("j3")
def homeIfneed(self):
if not self.isConnected():
print("Robot is not connected")
return False
homed = json.loads(self.robot.homed())
for key in homed:
if homed[key] == 1:
print(key, " is homed")
else:
print("Homing ", key)
self.robot.home(key)
"""===================================================================
GET CURRENT POSITION AS XYZ SYSTEM
==================================================================
"""
def posXYZ(self):
return json.loads(self.robot.position('xyz'))
"""===================================================================
GET CURRENT POSITION AS JOINTS SYSTEM
==================================================================
"""
def pos(self):
return json.loads(self.robot.position())
"""==============================================
ADJUST the joints
=================================================
"""
#pass in joint and value as parameter ex:adjust("j0",10)
def adjust(self, joint, degrees: float, relatively=True):
# creates a position-movement description that can be fed to go().
tempPath = deepcopy(PRM_TEMPLATE)
for j in JOINTS:
if j == joint:
tempPath[j] = degrees
break
tempPath["movement"] = 1 if relatively else 0
self.go(tempPath)
return tempPath
#pass in joints object as parameter ex:adjust({"j1":0,"j2":0})
def adjustJoints(self, joints, relatively=True):
# creates a position-movement description that can be fed to go().
tempPath = deepcopy(PRM_TEMPLATE)
print(joints)
for key in joints:
for validJoint in JOINTS:
if key == validJoint:
tempPath[validJoint] = joints[key]
tempPath["movement"] = 1 if relatively else 0
self.go(tempPath)
return tempPath
"""==================================================
MOVE with x,y,z Coordinate System
=================================================
"""
#pass in axis and value as parameter
#ex:moveToPosition('z',10)
def adjustAxis(self, axis, degrees, relatively=True):
tempPath = deepcopy(PRM_TEMPLATE)
tempPath['path'] = 'line'
for c in COORDINATE:
if c == axis:
tempPath[c] = degrees
break
tempPath["movement"] = 1 if relatively else 0
self.go(tempPath)
return tempPath
"""==================================================
MOVE with xyz Coordinate System
=================================================
"""
#pass in coordinate object as parameter
#ex:moveToCoordinate({'x':10,'y':20,'z':10})
def adjustCoordinate(self, coor, relatively=True):
tempPath = deepcopy(PRM_TEMPLATE)
tempPath['path'] = 'line'
for key in coor:
for validJoint in COORDINATE:
if key == validJoint:
tempPath[validJoint] = coor[key]
tempPath["movement"] = 1 if relatively else 0
self.go(tempPath)
return tempPath
"""==================================================
wait
=================================================
"""
def wait(self, value):
time.sleep(value)
def goHome(self):
self.go(HOME)
def goFlat(self):
self.go(FLAT)
"""==================================================
MOVE tools head relatively to current angle
=================================================
"""
def adjustHead(self, angle):
self.adjust('j3', angle)
return self.pos()[3]
"""==================================================
MOVE tools head to specific Angle
=================================================
"""
def setHeadAngle(self, angle):
self.adjust('j3', angle, False)
return self.pos()[3]
"""==================================================
MOVE with specific directin
=================================================
"""
def move(self, direct: Direction.FORWARD, value: float):
if direct == Direction.FORWARD:
self.adjustAxis('x', value)
if direct == Direction.BACKWARD:
moveAxis('x', value * -1)
if direct == Direction.UP:
self.adjustAxis('z', value)
if direct == Direction.DOWN:
self.adjustAxis('z', value * -1)
if direct == Direction.LEFT:
self.adjustAxis('y', value)
if direct == Direction.RIGHT:
self.adjustAxis('y', value * -1)
"""==================================================
MOVE move to a coordiation
ex:moveTo((1,2,6)) x=1,y=2,z6
=================================================
"""
def moveTo(self, xyz):
self.adjustCoordinate({'x': xyz[0], 'y': xyz[1], 'z': xyz[2]}, False)
"""==================================================
Collection of Motion
"""
def pickIt(self):
self.adjustJoints({'j1': 45, 'j2': -90, 'j3': 45, 'j4': 0}, False)
self.adjustCoordinate({'x': 15, 'y': 0, 'z': 3.5}, False)
self.wait(10)
self.adjustCoordinate({'x': 16, 'y': 0, 'z': 10}, False)
self.adjust('j0', 180)
self.adjustCoordinate({'x': 4, 'y': 0, 'z': -5})
self.adjustCoordinate({'x': 0, 'y': 0, 'z': -3})
self.wait(10)
self.adjustCoordinate({'x': -5, 'y': 0, 'z': 8})
self.adjust('j0', -180)
self.adjustCoordinate({'x': 15, 'y': 0, 'z': 3.5}, False)
self.wait(10)
self.go()
def preHit(self):
self.adjustJoints({
'j0': 0,
'j1': 45,
'j2': -123,
'j3': 78,
'j4': 0
}, False)
self.adjustCoordinate({'x': -10, 'y': 0, 'z': 0})
self.adjustCoordinate({'x': 60, 'y': 0, 'z': 0})
def hit(self):
self.adjustCoordinate({'x': 1, 'y': 0, 'z': 0})
for i in [0, 1, 2, 3, 4]:
self.adjustCoordinate({'x': -60, 'y': 0, 'z': 0})
self.adjustCoordinate({'x': 60, 'y': 0, 'z': 0})
self.wait(10)
self.adjustCoordinate({'x': -10, 'y': 0, 'z': 0})
self.adjustCoordinate({'x': -10, 'y': 0, 'z': 0})
def preHitTop(self):
self.adjustJoints({
'j0': 0,
'j1': 45,
'j2': -123,
'j3': 78,
'j4': 0
}, False)
self.adjustCoordinate({'x': 0, 'y': 0, 'z': 80})
self.adjustCoordinate({'x': 80, 'y': 0, 'z': 0, 'a': -90})
self.adjustCoordinate({'x': 0, 'y': 0, 'z': 14})
def hitTop(self):
self.adjustCoordinate({'x': 0, 'y': 0, 'z': -1})
for i in [0, 1, 2, 3, 4]:
self.adjustCoordinate({'x': 0, 'y': 0, 'z': 20})
self.adjustCoordinate({'x': 0, 'y': 0, 'z': -20})
self.wait(6)
def prRight(self):
self.adjustJoints({
'j0': 0,
'j1': 45,
'j2': -123,
'j3': 78,
'j4': 0
}, False)
def hitRight(self):
self.adjustCoordinate({'x': 0, 'y': 1, 'z': 0})
for i in [0, 1, 2, 3, 4]:
self.adjustCoordinate({'x': 0, 'y': -20, 'z': 0})
self.adjustCoordinate({'x': 0, 'y': 20, 'z': 0})
self.wait(6)
from dorna import Dorna
import json
import time
# creat Dorna object and connect to the robot
robot = Dorna('./my_config_new.yaml')
robot.connect()
print(robot.homed())
print(robot.position())
print(robot.home("j0"))
print(robot.homed())
print(robot.position())
print(robot.home("j1"))
print(robot.homed())
print(robot.position())
print(robot.home("j2"))
print(robot.homed())
print(robot.position())
print(robot.home("j3"))
print(robot.homed())
print(robot.position())
# pose: pre shelf [-60, 120, -120, -10, 0]
# open servo robot.play({"command": "set_io", "prm": { "servo": 500 }})
# servo 0 fully opened
# pose: at shelf [-63, 85, -95, -10, 0]