def __init__(self):
iprgt = '10.2.0.50'
iplft = '10.2.0.51'
logging.basicConfig()
self.__rgtarm = None
# self.__rgtarm = Robot(iprgt)
# self.__rgtarm.set_tcp((0, 0, 0, 0, 0, 0))
# self.__rgtarm.set_payload(1.28)
self.__lftarm = Robot(iplft, use_rt=True)
self.__lftarm.set_tcp((0, 0, 0, 0, 0, 0))
self.__lftarm.set_payload(.86)
self.__lftarmbase = [0, 235.00, 965.00]
self.__rgtarmbase = [0, -235.00, 965.00]
self.__sqrt2o2 = math.sqrt(2.0) / 2.0
def connect(robot_ip="192.168.2.102"):
connected = False
while not connected:
try:
robot = Robot(robot_ip)
connected = True
print "Robot", robot_ip, "is connected"
except:
pass
return robot
def __init__(self):
iprgt = '10.2.0.50'
iplft = '10.2.0.51'
logging.basicConfig()
self.__rgtarm = None
# self.__rgtarm = Robot(iprgt)
# self.__rgtarm.set_tcp((0, 0, 0, 0, 0, 0))
# self.__rgtarm.set_payload(1.28)
self.__lftarm = Robot(iplft, use_rt = True)
self.__lftarm.set_tcp((0, 0, 0, 0, 0, 0))
self.__lftarm.set_payload(.86)
self.__lftarmbase = [0, 235.00, 965.00]
self.__rgtarmbase = [0, -235.00, 965.00]
self.__sqrt2o2 = math.sqrt(2.0)/2.0
def do_arm_draw(doodle_commands: List[ArmDoodleCommand], robot_multiple):
os.system('say "I will now commence drawing. Humans, please stand back."')
from urx import Robot
rob = Robot("169.254.241.97")
robot_pose = rob.get_pose()
start_position = robot_pose.get_pos().copy()
start_orientation = robot_pose.get_orient().copy()
last_robot_x, last_robot_y = start_position[0], start_position[1]
pen_was_down = False
for d in doodle_commands:
robot_dx, robot_dy = d.dx * robot_multiple, d.dy * robot_multiple
increased_x, increased_y = last_robot_x + robot_dx, last_robot_y + robot_dy
if not d.pen_down:
if pen_was_down:
say_from_phrases([
'Yes! Very nice.', 'This is coming along well.',
'This may be my best creation yet.', 'This will be great.'
])
robot_pose.set_pos(
[increased_x, increased_y, start_position[2] + 0.01])
rob.set_pose(robot_pose, 0.1, 0.1)
else:
robot_pose.set_pos([increased_x, increased_y, start_position[2]])
rob.set_pose(robot_pose, 0.1, 0.1)
# Increment position, update cross-step state
last_robot_x, last_robot_y = increased_x, increased_y
pen_was_down = d.pen_down
# Go back to home
robot_pose.set_pos(
[start_position[0], start_position[1], start_position[2] + 0.01])
robot_pose.set_pos(
[start_position[0], start_position[1], start_position[2]])
rob.set_pose(robot_pose, 0.1, 0.1)
say_from_phrases(["Human! How do you like my doodle?"])
class Ur5DualUrx():
"""
urx 50, right arm 51, left arm 52
author: weiwei
date: 201670411
"""
def __init__(self):
iprgt = '10.2.0.50'
iplft = '10.2.0.51'
logging.basicConfig()
self.__rgtarm = None
# self.__rgtarm = Robot(iprgt)
# self.__rgtarm.set_tcp((0, 0, 0, 0, 0, 0))
# self.__rgtarm.set_payload(1.28)
self.__lftarm = Robot(iplft, use_rt=True)
self.__lftarm.set_tcp((0, 0, 0, 0, 0, 0))
self.__lftarm.set_payload(.86)
self.__lftarmbase = [0, 235.00, 965.00]
self.__rgtarmbase = [0, -235.00, 965.00]
self.__sqrt2o2 = math.sqrt(2.0) / 2.0
@property
def rgtarm(self):
# read-only property
return self.__rgtarm
def movejntssgl(self, joints, armid='rgt'):
"""
:param joints: a 1-by-6 vector in degree
:param armid:
:return:
author: weiwei
date: 20170411
"""
targetarm = self.__rgtarm
if armid == 'lft':
targetarm = self.__lftarm
jointsrad = [math.radians(angdeg) for angdeg in joints]
targetarm.movej(jointsrad, acc=0.2, vel=0.2, wait=True)
def movejntsall(self, joints):
"""
move all joints of the ur5 dual-arm robot
NOTE that the two arms are moved sequentially
use wait=False for simultaneous motion
:param joints: a 1-by-12 vector in degree, 6 for right, 6 for left
:return: bool
author: weiwei
date: 20170411
"""
jointsrad = [math.radians(angdeg) for angdeg in joints[0:6]]
self.__rgtarm.movej(jointsrad)
jointsrad = [math.radians(angdeg) for angdeg in joints[6:12]]
self.__lftarm.movej(jointsrad)
def gettcp(self, armid='rgt'):
"""
get the tcp (x, y, z, rx, ry, rz) of the specified arm (in dual arm coordinate system)
:param armid:
:return:
author: weiwei
date: 20170413
"""
targetarm = self.__rgtarm
targetarmbase = self.__rgtarmbase
if armid == 'lft':
targetarm = self.__lftarm
targetarmbase = self.__lftarmbase
tpos = targetarm.getl(wait=True)
print tpos
rtpos = [0, 0, 0, 0, 0, 0]
rtpos[0] = -tpos[0] * 1000.0
rtpos[1] = (-tpos[2] + tpos[1]) * self.__sqrt2o2 * 1000.0
rtpos[2] = (-tpos[2] - tpos[1]) * self.__sqrt2o2 * 1000.0
if armid == 'lft':
rtpos[0] = -rtpos[0]
rtpos[1] = -rtpos[1]
rtpos[2] = rtpos[2]
rtpos[1] = rtpos[1] + targetarmbase[1]
rtpos[2] = rtpos[2] + targetarmbase[2]
return rtpos
def getforcevec(self, armid='rgt'):
"""
return force tcp in world coordinate system
:return:
author: weiwei
date: 20170412
"""
targetarm = self.__rgtarm
if armid == 'lft':
targetarm = self.__lftarm
return targetarm.get_tcp_force()
from urx import Robot
import time
import numpy as np
robot = Robot("192.168.2.102")
acc = 0.05
vel = 0.06
trans0 = robot.get_pose()
# print "Go to drone takeoff position..."
# trans0.pos.x = 0.37846
# trans0.pos.y = -0.17486
# trans0.pos.z = 0.50439
# trans0.orient = np.array([[ 0.04920185, -0.87325427, 0.48477434],
# [-0.03945265, -0.48668261, -0.87268753],
# [ 0.99800935, 0.02381221, -0.05839788]])
# robot.set_pose(trans0, acc, vel)
# time.sleep(2)
print "Go to landing pad release position..."
trans_release_pad = trans0
trans_release_pad.pos.x = 0.25152
trans_release_pad.pos.y = -0.51420
trans_release_pad.pos.z = 0.50441
trans_release_pad.orient = np.array([[0.02487802, -0.99955103, 0.01669785],
[-0.05793992, -0.01811662, -0.99815568],
[0.99801005, 0.02386466, -0.05836461]])
robot.set_pose(trans_release_pad, acc, vel)
time.sleep(1)
print "Go a bit back..."
def __init__(self, ip="192.168.2.102"):
Robot.__init__(self, ip)
self.ip = ip
self.ee_pose = np.zeros(3)
self.ee_orient = np.zeros(3)
#!/usr/bin/env python
import sys
import logging
from math import pi
from IPython import embed
from urx import Robot
import math3d
if __name__ == "__main__":
if len(sys.argv) > 1:
host = sys.argv[1]
else:
host = 'localhost'
try:
robot = Robot(
host) #, logLevel=logging.DEBUG, parserLogLevel=logging.DEBUG)
r = robot
embed()
finally:
if "robot" in dir():
robot.cleanup()
class IceRobot(hms.Message, hms.GenericRobot, hms.Agent, _LightHolon):
def __init__(self, name, ip):
_LightHolon.__init__(self, name)
self.robot = Robot(ip)
self._ilog("Robot: ", name, " created", level=1)
def movel(self, pose, vel, acc, current=None):
self.robot.movel(pose, acc, vel)
def movej(self, pose, vel, acc, current=None):
self.robot.movej(pose, acc, vel)
def getl(self, current=None):
return self.robot.getl()
def getj(self, current=None):
return self.robot.getj()
def setTCP(self, tcp, current=None):
self.robot.set_tcp(tcp)
def grasp(self, current=None):
self.robot.set_digital_out(4, 1)
self.robot.set_digital_out(3, 0)
def release(self, current=None):
self.robot.set_digital_out(4, 0)
self.robot.set_digital_out(3, 1)
def cleanup(self):
self.robot.cleanup()
mutex.acquire()
while ('cs_rasc_tcp' in rigid_body_dict) == False:
print("no data")
time.sleep(0.01)
opti_pose = rigid_body_dict['cs_rasc_tcp']
mutex.release()
return tcp_pose, opti_pose
if __name__ == '__main__':
signal.signal(signal.SIGINT, signal_handler)
signal.signal(signal.SIGTERM, signal_handler)
arm = Robot("192.168.1.22")
streamingClient = NatNetClient()
streamingClient.newFrameListener = receiveNewFrame
streamingClient.rigidBodyPackageListener = receiveRigidBodyPackageFrame
# run optitrack thread
streamingClient.run()
time.sleep(2)
trans = math3d.Transform()
start_pos = arm.get_pose()
# x error
x_error = []
count = test_count
except:
pass
dt = 20 * abs(dy)
time.sleep(dt)
def move_dz(robot, dz):
try:
robot.z_t += dz # move robot in tool z axis [m]
except:
pass
dt = 20 * abs(dz)
time.sleep(dt)
robot = Robot("192.168.2.102")
# print 'Position:', robot.x, robot.y, robot.z
# print 'Orientation:', robot.get_orientation()
# trans = robot.get_pose()
# print trans.pos
robot.set_orientation([0, 0, np.pi])
# time.sleep(5)
# move_dx(robot, -0.1)
# move_dy(robot, 0.05)
# move_dz(robot, -0.1)
# try:
# trans = robot.get_pose() # get current transformation matrix (tool to base)
it = 500
#Radius of the pipe
r = 230
#Starting position
start = [-0.7550, 0.00384, 0.57738, math.pi, 0, 0]
start_circle = [-0.7550 - 0.150, -0.00384, 0.57738, math.pi, 0, 0]
#Arcmovements
spiral_move = spiralweld(x, y, z, it, r, start)
spiral_move_back = spiralweldback(x, y, z, it, r, start)
if __name__ == "__main__":
robot = Robot("192.168.0.20", True)
try:
print('Moving to start position')
robot.movel(start)
print('Moving 150 mm in y direction')
robot.movel([-0.150, 0, 0, 0, 0, 0], relative=True)
print('Weld is not detected')
quartercircle(230, start_circle, 'iterated')
class Ur5DualUrx():
"""
urx 50, right arm 51, left arm 52
author: weiwei
date: 201670411
"""
def __init__(self):
iprgt = '10.2.0.50'
iplft = '10.2.0.51'
logging.basicConfig()
self.__rgtarm = None
# self.__rgtarm = Robot(iprgt)
# self.__rgtarm.set_tcp((0, 0, 0, 0, 0, 0))
# self.__rgtarm.set_payload(1.28)
self.__lftarm = Robot(iplft, use_rt = True)
self.__lftarm.set_tcp((0, 0, 0, 0, 0, 0))
self.__lftarm.set_payload(.86)
self.__lftarmbase = [0, 235.00, 965.00]
self.__rgtarmbase = [0, -235.00, 965.00]
self.__sqrt2o2 = math.sqrt(2.0)/2.0
@property
def rgtarm(self):
# read-only property
return self.__rgtarm
def movejntssgl(self, joints, armid='rgt'):
"""
:param joints: a 1-by-6 vector in degree
:param armid:
:return:
author: weiwei
date: 20170411
"""
targetarm = self.__rgtarm
if armid == 'lft':
targetarm = self.__lftarm
jointsrad = [math.radians(angdeg) for angdeg in joints]
targetarm.movej(jointsrad, acc = 0.2, vel = 0.2, wait = True)
def movejntsall(self, joints):
"""
move all joints of the ur5 dual-arm robot
NOTE that the two arms are moved sequentially
use wait=False for simultaneous motion
:param joints: a 1-by-12 vector in degree, 6 for right, 6 for left
:return: bool
author: weiwei
date: 20170411
"""
jointsrad = [math.radians(angdeg) for angdeg in joints[0:6]]
self.__rgtarm.movej(jointsrad)
jointsrad = [math.radians(angdeg) for angdeg in joints[6:12]]
self.__lftarm.movej(jointsrad)
def gettcp(self, armid = 'rgt'):
"""
get the tcp (x, y, z, rx, ry, rz) of the specified arm (in dual arm coordinate system)
:param armid:
:return:
author: weiwei
date: 20170413
"""
targetarm = self.__rgtarm
targetarmbase = self.__rgtarmbase
if armid == 'lft':
targetarm = self.__lftarm
targetarmbase = self.__lftarmbase
tpos = targetarm.getl(wait = True)
print tpos
rtpos = [0,0,0,0,0,0]
rtpos[0] = -tpos[0]*1000.0
rtpos[1] = (-tpos[2]+tpos[1])*self.__sqrt2o2*1000.0
rtpos[2] = (-tpos[2]-tpos[1])*self.__sqrt2o2*1000.0
if armid == 'lft':
rtpos[0] = - rtpos[0]
rtpos[1] = -rtpos[1]
rtpos[2] = rtpos[2]
rtpos[1] = rtpos[1] + targetarmbase[1]
rtpos[2] = rtpos[2] + targetarmbase[2]
return rtpos
def getforcevec(self, armid='rgt'):
"""
return force tcp in world coordinate system
:return:
author: weiwei
date: 20170412
"""
targetarm = self.__rgtarm
if armid == 'lft':
targetarm = self.__lftarm
return targetarm.get_tcp_force()
oo#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Created on Fri Dec 13 18:02:54 2019
@author: wouter
"""
import numpy as np
import math
from urx import Robot
robot = Robot("192.168.0.20", True)
def spiralweld(x_p, y, z, it, r, pos):
def teta(x,y):
if y == 0:
return 0
teta = math.atan(x/y)
return teta
Xref, Yref, Zref = [0,r,0]
Xmes, Ymes, Zmes = [x_p,y,z]
Xref_cil = (Xref**2+Yref**2)**0.5
Yref_cil = teta(Xref, Yref)
Zref_cil = Zref
Xmes_cil = (Xmes**2+Ymes**2)**0.5
Ymes_cil = teta(Xmes, Ymes)
Zmes_cil = Zmes
from urx import Robot
import time
import numpy as np
robot = Robot("192.168.2.102")
acc = 0.05
vel = 0.06
print "Go to drone takeoff position..."
trans0 = robot.get_pose()
trans0.pos.x = 0.37846
trans0.pos.y = -0.17486
trans0.pos.z = 0.50439
trans0.orient = np.array([[0.04920185, -0.87325427, 0.48477434],
[-0.03945265, -0.48668261, -0.87268753],
[0.99800935, 0.02381221, -0.05839788]])
robot.set_pose(trans0, acc, vel)
time.sleep(2)
robot.close()
#!/usr/bin/env python
import sys
import logging
from math import pi
from IPython import embed
from urx import Robot
import math3d
if __name__ == "__main__":
if len(sys.argv) > 1:
host = sys.argv[1]
else:
host = 'localhost'
try:
robot = Robot(host)
r = robot
embed()
finally:
if "robot" in dir():
robot.close()
# -*- coding: utf-8 -*-
"""
Created on Mon Jun 11 17:16:28 2018
@author: Ye Tian
"""
import sys
import json
import signal
import math3d
import numpy as np
from urx import Robot
import paho.mqtt.client as mqtt
arm = Robot("192.168.1.22")
def trace(*args):
print("".join(map(str, args)))
def exit(signum, frame):
print('UR bridge exit...')
arm.stop()
arm.close()
mqtt_client.disconnect()
def on_mqtt_connect(client, userdata, flags, rc):
print("Connected with result code " + str(rc))
trans_matrix.set_orient(quaternion.orientation)
obj_to_global_T = trans_matrix
temp_matrix = global_to_arm_T.matrix * obj_to_global_T.matrix * tcp_to_target_T.matrix
tcp_to_arm_T = math3d.Transform()
tcp_to_arm_T.set_pos(temp_matrix[:3, 3].A1)
orient = math3d.Orientation(temp_matrix[:3, :3].A1)
tcp_to_arm_T.set_orient(orient)
trace("obj_to_global_T", obj_to_global_T.matrix)
if __name__ == '__main__':
# arm = Robot("192.168.0.133")
arm = Robot("192.168.0.200")
tcp_to_arm_T = arm.get_pose()
# init optitrack stream
streamingClient = NatNetClient()
# setup callback
streamingClient.newFrameListener = receiveNewFrame
streamingClient.rigidBodyListener = receiveRigidBodyFrame
# run optitrack thread
streamingClient.run()
try:
while True:
print("set tcp pose\n", tcp_to_arm_T.matrix)
arm.set_pose(tcp_to_arm_T, acc=0.2, vel=0.2)
from urx import Robot
from libs.modded_gripper import Modded_Gripper
import os
rob = Robot("169.254.241.97")
gripper = Modded_Gripper(rob)
gripper.gripper_and_move_action(80)
robot_start_pose = rob.get_pose()
start_position = robot_start_pose.get_pos().copy()
start_orientation = robot_start_pose.get_orient().copy()
new_orientation = robot_start_pose.get_orient().copy()
# new_orientation.set_axis_angle("X")
robot_start_pose.set_pos(
[start_position[0], start_position[1], start_position[2] + 0.01])
robot_start_pose.set_orient(new_orientation)
rob.set_pose(robot_start_pose, 0.1, 0.1) #, True, "movej")
# robot_start_pose.set_pos(start_position)
# robot_start_pose.set_orient(new_orientation)
# rob.set_pose(robot_start_pose, 0.1, 0.1)
os.system('say "movement completed"')
#!/usr/bin/env python
import sys
import logging
from math import pi
from urx import Robot
import math3d
if __name__ == "__main__":
if len(sys.argv) > 1:
host = sys.argv[1]
else:
host = 'localhost'
try:
robot = Robot( host )#, logLevel=logging.DEBUG, parserLogLevel=logging.DEBUG)
r = robot
from IPython.frontend.terminal.embed import InteractiveShellEmbed
ipshell = InteractiveShellEmbed( banner1="\nStarting IPython shell, robot object is available\n")
ipshell(local_ns=locals())
finally:
if "robot" in dir():
robot.cleanup()
def __init__(self, name, ip):
_LightHolon.__init__(self, name)
self.robot = Robot(ip)
self._ilog("Robot: ", name, " created", level=1)
from urx import Robot
rob = Robot("172.22.22.2")
print(rob.x) # returns current x
print(rob.y) # returns current x
print(rob.z) # returns current x
rob.set_tcp((0, 0, 0.1, 0, 0, 0))
rob.set_payload(2, (0, 0, 0.1))
sleep(0.2) #leave some time to robot to process the setup commands
rob.movej((1, 2, 3, 4, 5, 6), a, v)
rob.movel((x, y, z, rx, ry, rz), a, v)
print "Current tool pose is: ", rob.getl()
rob.movel((0.1, 0, 0, 0, 0, 0), a, v, relative=true) # move relative to current pose
rob.translate((0.1, 0, 0), a, v) #move tool and keep orientation
rob.stopj(a)
robot.movel(x, y, z, rx, ry, rz), wait=False)
while True :
sleep(0.1) #sleep first since the robot may not have processed the command yet
if robot.is_program_running():
break
robot.movel(x, y, z, rx, ry, rz), wait=False)
while.robot.getForce() < 50:
sleep(0.01)
if not robot.is_program_running():
break
robot.stopl()
try:
robot.movel((0,0,0.1,0,0,0), relative=True)
except RobotError, ex:
ap = argparse.ArgumentParser()
ap.add_argument('-d',
'--data_folder',
type=str,
default='data_' + datetime.datetime.now().isoformat()[:19])
ap.add_argument('robot_host', type=str)
# Lab-trondheim '192.168.0.90'
# Lab Giørtz '10.0.0.100'
args = ap.parse_args()
os.makedirs(args.data_folder, exist_ok=True)
pcg = PCG()
rob = Robot(args.robot_host)
rob.set_tcp(6 * [0])
rob.set_payload(2.0)
i = 0
while True:
key = input(
'Pose robot for sample {}, press [Ret] when ready. Press "e" and [Ret] for stopping'
.format(i))
if key == 'e':
break
p = rob.get_pose().pose_vector
np.savetxt(os.path.join(args.data_folder, fibtmpl.format(i)), p)
npc = pcg.get_pc()
pc = pcl.PointCloud(npc)
pcl.save(pc, os.path.join(args.data_folder, scenetmpl.format(i)))
target_to_tcp_T.set_orient(orient)
# trace("object_to_global_T", object_to_global_T.matrix)
def signal_handler(signum, frame):
print("signal hit")
streamingClient.stop()
sys.exit()
if __name__ == '__main__':
signal.signal(signal.SIGINT, signal_handler)
signal.signal(signal.SIGTERM, signal_handler)
arm = Robot("192.168.1.22")
streamingClient = NatNetClient()
streamingClient.newFrameListener = receiveNewFrame
streamingClient.rigidBodyPackageListener = receiveRigidBodyPackageFrame
# run optitrack thread
streamingClient.run()
while True:
print("set tcp pose\n", target_to_tcp_T.matrix)
# b = arm.get_pose()
# target_to_tcp_T.pos.x = 0
# target_to_tcp_T.pos.y = 0
# target_to_tcp_T.pos.z = 0
arm.movex_tool("movep",