def get_Tc2r(self):
rtde_c = rtde_control.RTDEControlInterface("192.168.178.15")
rtde_r = rtde_receive.RTDEReceiveInterface("192.168.178.15")
print("[INFO] AQUIRING ROBOT TO CAMERA TRANSFORMATION")
print("[INFO] 1. move robot to idle position (jointq)")
print("[INFO] 2. aquire an image with 3 aruco markers on it.")
rtde_c.moveJ(joint_q)
TCPpose = rtde_r.getActualTCPPose()
print("[INFO] 1. move robot to idle position (jointq)")
print("[INFO] starting camera")
self._setup_camera()
warmup_counter = 0
while True:
self.frame = self.wait_for_frames()
if warmup_counter > self.warmup_length:
align = rs.align(rs.stream.color)
aligned = align.process(frames)
c_frames = aligned.get_color_frame()
img = np.asanyarray(c_frames.get_data())
img, rvec, tvec =
"""
jog ur5
https://www.universal-robots.com/articles/ur/programming/how-to-jog-the-robot/
teach mode for freedrive start and stop
https://sdurobotics.gitlab.io/ur_rtde/api/api.html
"""
pass
def main():
ur_ctrl = rtde_control.RTDEControlInterface(rsd_conf.UR_IP)
ur_ctrl.moveJ(q.IDLE)
ur_modbus_client = ModbusTcpClient(rsd_conf.UR_IP)
ur_modbus_client.write_coil(16 + rsd_conf.IO_GRIPPER, False)
pi_modbus_client = ModbusTcpClient(rsd_conf.PI_IP, rsd_conf.PI_MODBUS_PORT)
def gripper_grasp(grasp_q, pre_post_q):
ur_ctrl.moveJ(pre_post_q)
ur_ctrl.moveJ(grasp_q)
ur_modbus_client.write_coil(16 + rsd_conf.IO_GRIPPER, True)
ur_ctrl.moveJ(pre_post_q)
def gripper_release():
ur_modbus_client.write_coil(16 + rsd_conf.IO_GRIPPER, False)
order = {"blue": 0, "red": 0, "yellow": 20}
for brick_color_id in range(len(rsd_conf.BRICK_COLORS)):
brick_color = rsd_conf.BRICK_COLORS[brick_color_id]
q_grasp_brick, q_above_brick = q.GRASP_BRICKS[
brick_color_id], q.ABOVE_BRICKS[brick_color_id]
count = order[brick_color]
for _ in range(count):
gripper_grasp(q_grasp_brick, q_above_brick)
ur_ctrl.moveJ(q.ABOVE_CAMERA)
pi_modbus_client.read_input_registers(0, 1)
ur_ctrl.moveJ(q.BRICK_DROP_ORDER_BOX)
gripper_release()
ur_ctrl.moveJ(q.IDLE)
def __init__(self, IP="192.168.92.99", direction=False):
self.__rtde_r = rtde_receive.RTDEReceiveInterface(IP)
self.__rtde_c = rtde_control.RTDEControlInterface(IP)
self.__direction = direction
self.__initialPos = [137.57 / 180 * np.pi, -114.70 / 180 * np.pi, 124.21 / 180 * np.pi,
-100.04 / 180 * np.pi, 91.38 / 180 * np.pi, 23.93 / 180 * np.pi]
self.angle = 0
self.track = LineTrack(video=0, mode=1)
self.tool = Tool(dev="/dev/ttyUSB1")
def main():
print("Starting Control... <" + str(os.getpid()) + ">")
global rtde_c
rtde_c = rtde_control.RTDEControlInterface("192.168.2.66")
rtde_r = rtde_receive.RTDEReceiveInterface("192.168.2.66")
rtde_c.setTcp(TCP_FRAME)
rtde_c.setPayload(0.5, [0,0,0])
while(True):
cook, meat = getNextSteak()
if cook != None and meat != None:
getSteak(meat)
place_time = time.time()
placeSteak()
delta_time = time.time() - place_time
thickness = measureThickness(rtde_c, rtde_r)
print("THICKNESS: " + str(thickness))
first_time, second_time = getCookTimes(thickness, cook)
print("TIMES: < " + str(first_time) + ", " + str(second_time) + " >")
rtde_c.moveJ(HOME)
if first_time > 0 and second_time > 0:
turn1 = threading.Timer(first_time/2.0 - delta_time, turnSteak)
turn1.start()
flip = threading.Timer(first_time - delta_time, flipSteak)
flip.start()
turn2 = threading.Timer(first_time + second_time/2.0 - delta_time, turnSteak)
turn2.start()
finish = threading.Timer(first_time + second_time - delta_time, finishSteak)
finish.start()
bar = progressbar.ProgressBar(max_value=1.0)
while(True):
if turn1.is_alive() or turn2.is_alive() or finish.is_alive() or flip.is_alive():
bar.update( min( ( time.time() - place_time ) / ( first_time + second_time ) , 1.0 ) )
time.sleep(1)
else:
print("MmmmMmMMMMMmmm")
break
returnSteak()
else:
print("Measurement Error")
os.remove("steak.yaml")
def main():
parser = argparse.ArgumentParser()
parser.add_argument('--ur-ip', type=str, required=True)
parser.add_argument('--n', type=int, default=3)
parser.add_argument('--grid-size', type=float, default=0.05)
parser.add_argument('--tcp-tool-offset', type=float, default=0.012)
args = parser.parse_args()
ur_ip = args.ur_ip
n = args.n
grid_size = args.grid_size
tcp_tool_offset = args.tcp_tool_offset
rtde_c = rtde_control.RTDEControlInterface(ur_ip)
rtde_r = rtde_receive.RTDEReceiveInterface(ur_ip)
rtde_c.teachMode()
table_poses = []
input(
'Insert the calibration tool close to the table origin with aligned tool axes'
)
table_poses.append(get_stable_table_pose(rtde_c, rtde_r))
table_offset_x = (int(input('How many holes to the table origin (x)?')) +
0.5) * grid_size
table_offset_y = (int(input('How many holes to the table origin (y)?')) +
0.5) * grid_size
for i in range(1, n):
input(
'Insert the calibration tool at another position. ({}/{})'.format(
i + 1, n))
table_poses.append(get_stable_table_pose(rtde_c, rtde_r))
pts_base = np.array([T @ (0, 0, tcp_tool_offset) for T in table_poses])
# estimate table_T_base just from the first table pose
table_T_base = table_poses[0] @ Transform(
t=(-table_offset_x, -table_offset_y, tcp_tool_offset))
base_T_table = table_T_base.inv()
# find the acutal positions in the table-coordinates
pts_table = np.array([
base_T_table @ pose @ (0, 0, tcp_tool_offset) for pose in table_poses
])
# nearest possible table positions
pts_table[:, 2] = 0
pts_table[:, :2] = np.round(((pts_table[:, :2] - grid_size / 2) /
grid_size)) * grid_size + grid_size / 2
base_T_table = kabsch(pts_base, pts_table)
print(base_T_table)
def __init__(self, ur_ipaddress=None, speed_ms=0.25, accel_mss=0.5):
if ur_ipaddress == None:
self.ur_ipaddress = "10.3.15.240"
else:
self.ur_ipaddress = ur_ipaddress
self.urrobot_r = rtde_receive.RTDEReceiveInterface(self.ur_ipaddress)
self.r_control = rtde_control.RTDEControlInterface(self.ur_ipaddress)
self.HOST = self.ur_ipaddress # The UR IP address
self.PORT = 30003 # UR secondary client
self.s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.s.connect((self.HOST, self.PORT))
self.speed_ms = speed_ms
self.accel_mss = accel_mss
self.path = []
def main():
print("Starting Control...")
rtde_c = rtde_control.RTDEControlInterface("192.168.2.66")
rtde_c.setTcp([0.0, 0.0, 0.05, 0.0, 0.0, 0.0])
rtde_c.setPayload(0.1, [0, 0, 0])
while (True):
updateSettings()
if state:
rtde_c.moveJ([-1.5707, -2.26893, 2.26893, -3.22886, -1.5707, 0])
for p in traps:
if do_traps and state:
makeMove(rtde_c, p)
else:
break
if do_traps and state:
makeMove(rtde_c, p, True)
else:
break
for p in lats:
if do_lats and state:
makeMove(rtde_c, p)
else:
break
if do_lats and state:
makeMove(rtde_c, p, True)
else:
break
for p in erects_tehe:
if do_erector and state:
makeMove(rtde_c, p)
else:
break
if do_erector and state:
makeMove(rtde_c, p, True)
else:
break
else:
rtde_c.moveJ([-1.5707, -3.14, 0.0, -3.22886, -1.5707, 0])
print("Done")
def main():
rtde_c = rtde_control.RTDEControlInterface(args.IP)
rtde_r = rtde_receive.RTDEReceiveInterface(args.IP)
im = Image.open(args.Image)
px = im.convert('RGB')
pose_to_pixel = max(im.width / args.Width, im.height / args.Height)
print(pose_to_pixel)
led = RGBLED(23, 24, 25)
rtde_c.setTcp([0, 0, 0, 0, 3.14159, 0])
image_complete = False
current_target = None
move_target = None
row_num = 0
while (True):
current_pose = rtde_r.getActualTCPPose()
updatePixel(current_pose, pose_to_pixel, px, led,
[args.Width / 2.0, args.Height / 2.0])
if movementComplete(move_target, current_pose):
current_target, image_complete, row_num = nextMove(
rtde_c, current_target, args.Width, args.Height, args.Spacing,
args.Overhang, row_num)
if not image_complete:
move_target = rtde_c.poseTrans([
-args.Width / 2, -args.Distance, args.Height / 2, 0, 0,
-1.5707
], current_target)
rtde_c.moveL(move_target, args.Speed, 0.5, True)
else:
break
time.sleep(1.0 / args.Rate)
rtde_c.stopScript()
import rtde_control
import rtde_receive
import time
from std_msgs.msg import String
from copy import deepcopy
UR_IP = "192.168.131.40"
rtde_c = rtde_control.RTDEControlInterface(
UR_IP, rtde_control.RTDEControlInterface.FLAG_VERBOSE
| rtde_control.RTDEControlInterface.FLAG_USE_EXT_UR_CAP)
rtde_r = rtde_receive.RTDEReceiveInterface(UR_IP)
inp = input("Fold the robot? y/n: ")[0]
if (inp == 'y'):
folded_joints = [1.602, -2.869, 2.683, -2.869, -1.584, -0.001]
rtde_c.moveJ(folded_joints, 0.1, 0.01)
else:
print("Skipping")
rst = rtde_c.zeroFtSensor()
print(rst, "=== Force Reset")
for i in range(5):
contact = rtde_r.getActualTCPForce()
print("Current Force Values ", contact)
# print("Forces #",i," ->",contact,rtde_c.isSteady())
time.sleep(0.5)
# while (contact[2] > 20):
# contact = rtde_r.getActualTCPForce()
# stopRobt()
#####################################################
# UR COMM CODE
HOST = "10.0.0.6" # The remote host
PORT = 30002
rtde_c = rtde_control.RTDEControlInterface(HOST)
rtde_r = rtde_receive.RTDEReceiveInterface(HOST)
###################################################
v = 2
a = 0.3
blend =0.00
print("Connection: ",rtde_c.isConnected())
# coordinates
move_joints_1 = [-0.5,-0.22,0.3,2.89,-1.29,0.126]
move_joints_2 = [-0.5,-0.22,0.4,2.89,-1.29,0.126]
drop = [ 0.145, -0.623, 0.0479, 3.09, 0.0169, -0.041]
placing = [ 0.116, -0.559, 0.4300, 3.10, 0.0169, -0.050]
# placing1 = [-0.252, -0.511, 0.4300, 2.95, -0.9491, -0.063]
#!/usr/bin/env python import rtde_control import time ip = "127.0.0.1" # ip = "172.16.101.225" rtde_c = rtde_control.RTDEControlInterface(ip) dt = 1.0 / 500 # 2ms joint_q = [0, -1.57, 1.57, 0, 1.57, 3.14] # Move to initial joint position with a regular moveJ rtde_c.moveJ(joint_q, 0.2, 0.2, False) rtde_c.stopScript()
import math
import numpy as np
import rtde_control
import rtde_receive
rtde_r = rtde_receive.RTDEReceiveInterface("192.168.178.15")
actual_q = np.array(rtde_r.getActualQ()) * 180 / (2 * math.pi)
rtde_c = rtde_control.RTDEControlInterface("192.168.178.15")
modified_q = actual_q
modified_q[0] = 0
from rtde_control import RTDEControlInterface as RTDEControl
rtde_c = RTDEControl("192.168.178.15", RTDEControl.FLAG_USE_EXT_UR_CAP)
rtde_c.moveL(modified_q, 0.1, 0.1)
print(actual_q)
from math import pi
import rtde_control
import time
JOINT_NAMES = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']
Q1 = [2.2,-0,-1.57,0,0,0]
Q2 = [1.5,0,-1.57,0,0,0]
Q3 = [1.5,-0.2,-1.57,0,0,0]
Q4 = [0,-pi/2,0,-pi/2,0,0]
QNum = 60
TNUM = 5
Q5 = np.zeros((6,QNum))
client = None
rtde_c = rtde_control.RTDEControlInterface("127.0.0.1")
# Parameters
acceleration = 0.5
dt = 1.0/500 # 2ms
joint_q = [-1.54, -1.83, -2.28, -0.59, 1.60, 0.023]
joint_speed = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
# Move to initial joint position with a regular moveJ
rtde_c.moveJ(joint_q)
# Move to initial joint position with a regular moveJ
rtde_c.moveJ(joint_q)
def OnceMove():
g = FollowJointTrajectoryGoal()
g.trajectory = JointTrajectory()
g.trajectory.joint_names = JOINT_NAMES
import rtde_control from rsd import conf from rsd.robot import q import time ur_ctrl = rtde_control.RTDEControlInterface(conf.UR_IP) ur_ctrl.moveJ(q.IDLE) time.sleep(5)
def resrart(self, ip):
self.__rtde_r = rtde_receive.RTDEReceiveInterface(ip)
self.__rtde_c = rtde_control.RTDEControlInterface(ip)
def __init__(self, robot_ip):
self.vel = 0.15
self.acc = 0.5
self.stop_acc = 0.3
self.cmd_velocity_vector = []
self.move_vel = False
self.item_height = 0.11
self.robot_c = rtde_control.RTDEControlInterface(
robot_ip) #urx.Robot(robot_ip, True)
self.robot_r = rtde_receive.RTDEReceiveInterface(
robot_ip) #urx.Robot(robot_ip, True)
self.robot_io = rtde_io.RTDEIOInterface(
robot_ip) #urx.Robot(robot_ip, True)
self.my_tcp = m3d.Transform() # create a matrix for our tool tcp
self.current_TCP = 'TCP'
self.set_TCP('pressure_ft')
# self.robot.set_payload(4.25)
# self.robot.set_gravity([0, 0, 0.15])
time.sleep(0.2)
self.ros_node = rospy.init_node('ur10_node', anonymous=True)
self.pose_publisher = rospy.Publisher('tcp_pose',
PoseStamped,
queue_size=1)
self.velocity_publisher = rospy.Publisher('/dvs/vel',
Twist,
queue_size=1)
self.speed_publisher = rospy.Publisher('/dvs/spd',
Float64,
queue_size=1)
self.cam_pose_publisher = rospy.Publisher('/dvs/pose',
PoseStamped,
queue_size=1)
self.cmd_vel_subs = rospy.Subscriber("ur_cmd_vel",
Twist,
self.move_robot_callback,
queue_size=1)
self.cmd_pose_subs = rospy.Subscriber("ur_cmd_pose", Pose,
self.move_pose_callback)
self.cmd_adjust_pose_subs = rospy.Subscriber("ur_cmd_adjust_pose",
Pose,
self.adjust_pose_callback)
self.rotate_ee_cmd = rospy.Subscriber("ur_rotate_ee", Float64,
self.angle_callback)
self.rotate_ee_cmd = rospy.Subscriber("ur_rotate_ee_x", Float64,
self.angle_callback_x)
self.pressure_movement_subs = rospy.Subscriber("move_pressure_to_cam",
Bool,
self.move_PF_to_cam)
self.pickup_service = rospy.Service("ur_pickup", Empty, self.pick_item)
self.set_tcp_service = rospy.Service("set_TCP", desiredTCP,
self.set_TCP_cb)
self.move_service = rospy.Service('move_ur', moveUR, self.moveUR_cb)
self.move_service = rospy.Service('fire_drill', fireDrill,
self.fire_drill_cb)
self.rate = rospy.Rate(100)
#TF
self.tfBuffer = tf2_ros.Buffer()
self.tl = tf2_ros.TransformListener(self.tfBuffer)
self.br = tf2_ros.TransformBroadcaster()
self.brs = tf2_ros.StaticTransformBroadcaster()
self.robot_pose = PoseStamped()
self.camera_pose = PoseStamped()
self.prev_camera_pose = PoseStamped()
self.pressure_ft_pose = PoseStamped()
self.cam_vel = Twist()
self.cam_speed = Float64()
self.seq = 1
self.pose = []
self.initial_pose = []
self.center_pose = []
self.static_transform_list = []
self.setup_tf()
def __init__(self, address="172.31.1.25", debug = 1, enable_force = 1, file_name=None, folder_name="futek_data"):
self._debug = debug
self._enable_force = enable_force
self.rob_c = rtde_control.RTDEControlInterface(address)
manager = Manager()
self.glob = manager.Namespace()
# shared memory for receiving data from manupulator
self.X_cur_shared = Array('d',[0,0,0,0,0,0])
self.dX_cur_shared = Array('d',[0,0,0,0,0,0])
p = Process(target=self.__read_data_from_manip,args=(address,))
p.start()
# shared memory
self.Xd_shared = Array('d',[0,0,0])
self.dXd_shared = Array('d',[0,0,0])
self.__force_from_sensor = Value('d',0.0)
self.term_process = Value('i',0)
# shared memory
self.Xg_shared = Array('d', [0, 0, 0])
self.dXg_shared = Array('d', [0, 0, 0])
self.Fd_ideal_shared = Value('d', 0.0)
self.Fd_real_shared = Value('d', 0.0)
self.start_vel_control = Value('i', 0)
# For plotting
self.glob.x_act = []
self.glob.x_des = []
self.glob.time = []
self.glob.force_data = []
# vel and acc constraints
self.__max_operational_acc = 0.4
self.__max_lin_acc = 0.5
self.__max_lin_vel = 0.5
# modification trajectory tune params
self.__vir_stiff = 400000
self.__freq_mod_traj = 500
# vel control parameters
self.__freq_log = 500
self.__k = 0.99
# sensor parameters
self.force_threshold = 1
# todo
self._common_orient=[3.14,0.1,0]
self.starting_pose = [-0.6284734457983073, 0.04110124901844167, 0.24322792682955954, 2.885542842994124, -0.09630215284222861, -0.8197553730344054]
if self._enable_force:
self.__start_futek_sensor(file_name,folder_name)
"Sample", "X", "Y", "Z", "Rx", "Ry", "Rz", "x1", "x2", "x3", "y1",
"y2", "y3", "z1", "z2", "z3", "Picked correctly"
]
for item in headers:
worksheet.write(row, column, item)
column += 1
rospy.init_node('test_results_picking_node')
base_frame = "base"
tfBuffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(tfBuffer)
broadcaster = tf2_ros.TransformBroadcaster()
static_broadcaster = tf2_ros.StaticTransformBroadcaster()
rtde_c = rtde_control.RTDEControlInterface("192.168.12.245")
rtde_r = rtde_receive.RTDEReceiveInterface("192.168.12.245")
tool_frame = "wrist_3_link"
tf_transform_base_component = tfBuffer.lookup_transform(
base_frame, "tool0", rospy.Time(), rospy.Duration(2.0))
t, rot_vector, y_axis_original = get_t_rotvector_component(
tf_transform_base_component=tf_transform_base_component)
t = [
tf_transform_base_component.transform.translation.x,
tf_transform_base_component.transform.translation.y,
tf_transform_base_component.transform.translation.z
]
quaternion = [
import rtde_control
rtde_c = rtde_control.RTDEControlInterface("192.168.0.100")
res = rtde_c.moveL([-0.143, -0.435, 0.10, 0, 0, 0], 0.5, 0.3)
res = rtde_c.moveL([-0.143, -0.435, 0.20, 0, 0, 0], 0.5, 0.3)
res = rtde_c.moveL([-0.143, -0.435, 0.30, 0, 0, 0], 0.5, 0.3)
print("result: " + str(res))
import rtde_receive
import rtde_control
robot_ip = "192.168.100.2"
rtde_r = rtde_receive.RTDEReceiveInterface(robot_ip)
rtde_c = rtde_control.RTDEControlInterface(robot_ip)
def go_to_next():
anw = input("Go to next [y]:")
if anw == "y":
return
else:
exit()
speed = 0.25
acc = 1
go_to_next()
fist_pose = [
-0.24126775044997928, -0.42075908202903856, 0.6586334885483427,
2.599420154499895, 0.09288735719897556, -0.6802083104833498
]
rtde_c.moveP(fist_pose, speed, acc, 0.0)
go_to_next()
overRockwool_pos = [
-0.08203968530815126, -0.5326926026406975, 0.3, -3.0392028741044914,
def setup(self):
"""
Connects Arm Movement to the UR5 and the Server.
Starts up the rest of the functionality for the class.
Args:
None.
Returns:
None.
"""
connected = False
conn = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
rtde_c = 0
rtde_r = 0
#Connect to arm
while not (connected):
try:
rtde_c = rtde_control.RTDEControlInterface(self.armIP)
rtde_r = rtde_receive.RTDEReceiveInterface(self.armIP)
connected = True
except Exception as e:
print(e)
finally:
time.sleep(1)
#Move arm to home position
# Connect to server.
connected = False
while not (connected):
try:
conn.connect((self.serverIP, self.serverPORT))
connected = True
except Exception as e:
print(e)
finally:
time.sleep(1)
q = Queue()
t1 = threading.Thread(target=self.moniterUserInput, args=(
q,
conn,
))
t2 = threading.Thread(target=self.TCPwrapper,
args=(
q,
conn,
rtde_c,
rtde_r,
))
t1.start()
t2.start()
t1.join()
t2.join()
