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",